Add trajectory stitching demo objective

PickNikRobotics · Jan 14, 2025 · 2c92124 · 2c92124
1 parent 7280163
commit 2c92124
Show file tree

Hide file tree

Showing 3 changed files with 198 additions and 0 deletions.
diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/config/config.yaml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/config/config.yaml
@@ -161,6 +161,7 @@ objectives:
       - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
+      - "moveit_studio::behaviors::ConverterBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:

diff --git a/...ova_configs/kinova_gen3_mujoco_config/objectives/admittance_parameters_no_admittance.yaml b/...ova_configs/kinova_gen3_mujoco_config/objectives/admittance_parameters_no_admittance.yaml
@@ -0,0 +1,53 @@
+SetAdmittanceParameters:
+  admittance:
+    damping_ratio_position:
+      max: 1000
+      min: 0
+      x: 1000
+      y: 1000
+      z: 650
+    damping_ratio_rotation:
+      max: 1000
+      min: 0
+      x: 40
+      y: 40
+      z: 40
+    joint_damping:
+      max: 100
+      min: 0
+      value: 1
+    mass_position:
+      max: 100
+      min: 0.001
+      x: 40
+      y: 40
+      z: 10
+    mass_rotation:
+      max: 100
+      min: 0.001
+      x: 4
+      y: 4
+      z: 4
+    selected_axes:
+      rx: false
+      ry: false
+      rz: false
+      x: false
+      y: false
+      z: false
+    stiffness_position:
+      max: 1000
+      min: 0
+      x: 120
+      y: 120
+      z: 300
+    stiffness_rotation:
+      max: 1000
+      min: 0
+      x: 80
+      y: 80
+      z: 80
+  base_frame: base_link
+  control_frame: grasp_link
+  controller_name: ''
+  end_effector_frame: grasp_link
diff --git a/...eit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/demo_trajectory_stitching.xml b/...eit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/demo_trajectory_stitching.xml
@@ -0,0 +1,144 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Demo Trajectory Stitching">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Demo Trajectory Stitching"
+    _description="Take a pointcloud snapshot of the scene with a depth camera"
+    _favorite="false"
+    _subtreeOnly="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        joint_group_name="manipulator"
+        planner_interface="moveit_default"
+        controller_names="/joint_trajectory_controller"
+        waypoint_name="Home"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        velocity_scale_factor="1.0"
+      />
+      <Action
+        ID="GetCurrentPlanningScene"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="GetRobotJointState"
+        planning_group_name="manipulator"
+        planning_scene="{planning_scene}"
+        joint_state="{start_joint_state}"
+      />
+      <Action
+        ID="UnpackJointStateMessage"
+        joint_state="{start_joint_state}"
+        effort="{start_effort}"
+        header="{start_header}"
+        name_="{joint_names}"
+        position="{start_position}"
+        velocity="{start_velocity}"
+      />
+      <Action
+        ID="CreateJointState"
+        joint_state_msg="{start_state}"
+        positions="{start_position}"
+        joint_names="{joint_names}"
+        velocities="{start_velocity}"
+      />
+      <Action
+        ID="CreateJointState"
+        positions="0.5;0.51;-3.025;-2.31;0.2;1.159;1.37"
+        joint_names="{joint_names}"
+        joint_state_msg="{target_state}"
+        velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"
+      />
+      <Action
+        ID="GetRobotJointState"
+        planning_group_name="manipulator"
+        planning_scene="{planning_scene}"
+        joint_state="{joint_state}"
+      />
+      <Action
+        ID="CreateJointState"
+        positions="-0.5;-0.51;-3.025;-2.31;0.2;1.159;1.37"
+        joint_names="{joint_names}"
+        joint_state_msg="{stitch_target_state}"
+        velocities="0.0;0.0;0.0;0.0;0.0;0.0;0.0"
+      />
+      <Action
+        ID="GeneratePointToPointTrajectory"
+        jerk_scale_factor="0.5"
+        velocity_scale_factor="1.0"
+        acceleration_scale_factor="1.0"
+        planning_group_name="manipulator"
+        start_state="{start_state}"
+        start_time="0.000000"
+        target_state="{target_state}"
+        trajectory_sampling_rate="100"
+        joint_trajectory_msg="{joint_trajectory_msg}"
+      />
+      <Action
+        ID="GetTrajectoryStateAtTime"
+        time_from_reference="0.8"
+        joint_state="{stitch_state}"
+        from_start="true"
+        joint_trajectory_msg="{joint_trajectory_msg}"
+      />
+      <Action
+        ID="GeneratePointToPointTrajectory"
+        start_state="{stitch_state}"
+        target_state="{stitch_target_state}"
+        joint_trajectory_msg="{stitch_joint_trajectory_msg}"
+        velocity_scale_factor="1.0"
+        start_time="0.8"
+        acceleration_scale_factor="1.0"
+        jerk_scale_factor="0.5"
+        planning_group_name="manipulator"
+        trajectory_sampling_rate="100"
+      />
+      <Action
+        ID="SetAdmittanceParameters"
+        config_file_name="admittance_parameters_no_admittance.yaml"
+        admittance_parameters_msg="{admittance_parameters_msg}"
+      />
+      <Action
+        ID="ActivateControllers"
+        controller_names="/joint_trajectory_admittance_controller"
+      />
+      <Control ID="Parallel" failure_count="2" success_count="1">
+        <Action
+          ID="ExecuteTrajectoryWithAdmittance"
+          path_position_tolerance="0.2"
+          absolute_force_torque_threshold="45;45;45;10;10;10"
+          admittance_parameters_msg="{admittance_parameters_msg}"
+          controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
+          goal_duration_tolerance="-1.000000"
+          goal_position_tolerance="0.001000"
+          joint_trajectory_msg="{joint_trajectory_msg}"
+        />
+        <Decorator ID="Delay" delay_msec="400">
+          <Action
+            ID="ExecuteTrajectoryWithAdmittance"
+            joint_trajectory_msg="{stitch_joint_trajectory_msg}"
+            absolute_force_torque_threshold="45;45;45;10;10;10"
+            goal_position_tolerance="0.001000"
+            path_position_tolerance="0.2"
+            admittance_parameters_msg="{admittance_parameters_msg}"
+            controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
+            goal_duration_tolerance="-1.000000"
+          />
+        </Decorator>
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Test Trajectory Stitching">
+      <MetadataFields>
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>