wip

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

@@ -1,53 +1,53 @@
 SetAdmittanceParameters:
+  base_frame: "base_link"
+  controller_name: ""
+  end_effector_frame: "grasp_link"
+  control_frame: "grasp_link"
   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
+    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
-      x: 40
-      y: 40
-      z: 10
-    mass_rotation:
-      max: 100
+      max: 100.0
+    mass_rotation:  # kg*m^2
+      x: 4.0
+      y: 4.0
+      z: 4.0
       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
+      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

@@ -1,13 +1,14 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?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"
+    _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"
@@ -22,10 +23,12 @@
         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"
@@ -41,6 +44,7 @@
         position="{start_position}"
         velocity="{start_velocity}"
       />
+      <!--Create a RobotJointState for the current and final joint states-->
       <Action
         ID="CreateJointState"
         joint_state_msg="{start_state}"
@@ -55,19 +59,7 @@
         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"
-      />
+      <!--Generate a trajectory from the robot start state to the end state-->
       <Action
         ID="GeneratePointToPointTrajectory"
         jerk_scale_factor="0.5"
@@ -80,62 +72,26 @@
         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="GetTrajectoryStateAtTime"
-        time_from_reference="0.8"
-        joint_state="{stitch_state}"
-        from_start="true"
-        joint_trajectory_msg="{joint_trajectory_msg}"
+        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"
-        start_state="{stitch_state}"
-        target_state="{stitch_target_state}"
-        joint_trajectory_msg="{stitch_joint_trajectory_msg}"
-        velocity_scale_factor="1.0"
+      <SubTree
+        ID="Stitch Joint Trajectory"
+        joint_names="{joint_names}"
+        joint_trajectory_msg="{joint_trajectory_msg}"
         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.yaml"
-        admittance_parameters_msg="{admittance_parameters_msg}"
-      />
-      <Action
-        ID="ActivateControllers"
-        controller_names="/joint_trajectory_admittance_controller"
+        stitch_target_state="{stitch_target_state}"
+        _collapsed="false"
       />
-      <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="450;450;450;100;100;100"
-            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">
+    <SubTree ID="Demo Trajectory Stitching">
       <MetadataFields>
         <Metadata runnable="true" />
       </MetadataFields>