Verbose trajectory stitching example objective

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:

src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/3_waypoint_pick_and_place.xml

@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
   <!--//////////-->
   <BehaviorTree
@@ -14,11 +15,18 @@
         waypoint_name="Table"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller"
+        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="MoveGripperAction"
         gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
         position="0"
+        timeout="10.000000"
       />
       <!--Run pick and place on loop-->
       <Decorator ID="KeepRunningUntilFailure">
@@ -30,34 +38,60 @@
               waypoint_name="Pick"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
+              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="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0.7929"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
+              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"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
               waypoint_name="Place"
+              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="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
+              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"
             />
           </Control>
           <!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
@@ -67,38 +101,67 @@
               waypoint_name="Place"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
+              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="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0.7929"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
+              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"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Pick"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
+              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="MoveGripperAction"
               gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
               position="0"
+              timeout="10.000000"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Table"
               joint_group_name="manipulator"
               controller_names="/joint_trajectory_controller"
+              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"
             />
           </Control>
         </Control>
       </Decorator>
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="3 Waypoints Pick and Place" />
+  </TreeNodesModel>
 </root>

src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/admittance_parameters.yaml

@@ -0,0 +1,53 @@
+SetAdmittanceParameters:
+  base_frame: "base_link"
+  controller_name: ""
+  end_effector_frame: "grasp_link"
+  control_frame: "grasp_link"
+  admittance:
+    selected_axes:
+      x: True
+      y: True
+      z: True
+      rx: True
+      ry: True
+      rz: True
+    mass_position:  # kg
+      x: 4.0
+      y: 4.0
+      z: 4.0
+      min: 0.001
+      max: 100.0
+    mass_rotation:  # kg*m^2
+      x: 4.0
+      y: 4.0
+      z: 4.0
+      min: 0.001
+      max: 100.0
+    stiffness_position:  # N/m
+      x: 80.0
+      y: 80.0
+      z: 80.0
+      min: 0.0
+      max: 1000.0
+    stiffness_rotation:  # N*m/rad
+      x: 80.0
+      y: 80.0
+      z: 80.0
+      min: 0.0
+      max: 1000.0
+    damping_ratio_position:  # unitless
+      x: 40.0
+      y: 40.0
+      z: 40.0
+      min: 0.0
+      max: 100.0
+    damping_ratio_rotation:  # unitless
+      x: 40.0
+      y: 40.0
+      z: 40.0
+      min: 0.0
+      max: 100.0
+    joint_damping:  # 1/s
+      value: 1.0
+      min: 0.0
+      max: 100.0

src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/demo_trajectory_stitching.xml

@@ -0,0 +1,100 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Demo Trajectory Stitching">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Demo Trajectory Stitching"
+    _description="Stitch two trajectories together. Extract a joint state at a give time from an existing trajectory, generate a new trajectory from that joint state, and then interrupt the first trajectory at the same time (and joint state) the stitch trajectory was generated from."
+    _favorite="false"
+    _subtreeOnly="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <!--For demo objectives, we start the robot at a known position-->
+      <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"
+      />
+      <!--Get the most recent information on the robot and its environment-->
+      <Action
+        ID="GetCurrentPlanningScene"
+        planning_scene_msg="{planning_scene}"
+      />
+      <!--Get the current values of the robot joints, and unpack them on to the blackboard-->
+      <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}"
+      />
+      <!--Create a RobotJointState for the current and final joint states-->
+      <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"
+      />
+      <!--Generate a trajectory from the robot start state to the end state-->
+      <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}"
+      />
+      <!--Execute the initial trajectory, stitching in a new trajectory to a new target 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"
+      />
+      <SubTree
+        ID="Stitch Joint Trajectory"
+        joint_names="{joint_names}"
+        joint_trajectory_msg="{joint_trajectory_msg}"
+        start_time="0.8"
+        stitch_target_state="{stitch_target_state}"
+        _collapsed="false"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Demo Trajectory Stitching">
+      <MetadataFields>
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>