Merge pull request #76 from PickNikRobotics/use-pro-rrt-mtc-behaviors

Use new Pro RRT SetupMTCPlanTo_ behaviors
PickNikRobotics · Jan 9, 2025 · 7280163 · 7280163
2 parents 8aab45e + 91268bc
commit 7280163
Show file tree

Hide file tree

Showing 47 changed files with 62 additions and 171 deletions.
diff --git a/src/fanuc_sim/objectives/move_to_pose.xml b/src/fanuc_sim/objectives/move_to_pose.xml
@@ -14,12 +14,11 @@
       />
       <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
       <Action
-        ID="SetupMTCMoveToPose"
+        ID="SetupMTCPlanToPose"
         ik_frame="tool0"
         planning_group_name="manipulator"
         target_pose="{target_pose}"
         task="{move_to_pose_task}"
-        planner_interface="moveit_default"
       />
       <Action
         ID="PlanMTCTask"

diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
@@ -6,7 +6,6 @@
     _subtreeOnly="true"
     _description=""
     joint_group_name="manipulator"
-    planner_interface="moveit_default"
     mtc_task="{mtc_task}"
   >
     <Control ID="Sequence">
@@ -17,11 +16,10 @@
         joint_group_name="{joint_group_name}"
       />
       <Action
-        ID="SetupMTCMoveToJointState"
+        ID="SetupMTCPlanToJointState"
         joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
+        name="SetupMTCPlanToJointState"
         planning_group_name="{joint_group_name}"
-        planner_interface="{planner_interface}"
         task="{mtc_task}"
       />
       <Action
@@ -31,19 +29,17 @@
         joint_group_name="{joint_group_name}"
       />
       <Action
-        ID="SetupMTCMoveToJointState"
+        ID="SetupMTCPlanToJointState"
         joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
+        name="SetupMTCPlanToJointState"
         planning_group_name="{joint_group_name}"
-        planner_interface="{planner_interface}"
         task="{mtc_task}"
       />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Add Waypoints to MTC Task">
       <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="planner_interface" default="moveit_default" />
       <inout_port name="mtc_task" default="{mtc_task}" />
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />

diff --git a/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
@@ -10,17 +10,6 @@
     _description=""
   >
     <Control ID="Sequence">
-      <!--Create a special type of motion planning configuration that includes an upwards orientation requirement-->
-      <Action
-        ID="InitializeMotionConstraints"
-        constraints_name="gripper pointing down"
-        constraints="{constraints}"
-      />
-      <Action
-        ID="AppendOrientationConstraint"
-        config_file_name="my_constraints.yaml"
-        constraints="{constraints}"
-      />
       <!--Move to pick location-->
       <!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
       <Decorator ID="RetryUntilSuccessful" num_attempts="3">
@@ -30,7 +19,8 @@
           joint_group_name="manipulator"
           controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
           planner_interface="moveit_default"
-          constraints="{constraints}"
+          keep_orientation="true"
+          keep_orientation_tolerance="0.2"
         />
       </Decorator>
       <Decorator ID="RetryUntilSuccessful" num_attempts="3">
@@ -40,7 +30,8 @@
           joint_group_name="manipulator"
           controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
           planner_interface="moveit_default"
-          constraints="{constraints}"
+          keep_orientation="true"
+          keep_orientation_tolerance="0.2"
         />
       </Decorator>
       <!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
@@ -54,7 +45,8 @@
           controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
           joint_group_name="manipulator"
           planner_interface="moveit_default"
-          constraints="{constraints}"
+          keep_orientation="true"
+          keep_orientation_tolerance="0.2"
           _collapsed="true"
           waypoint_name="{pre_pick}"
         />
@@ -65,7 +57,8 @@
           controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
           joint_group_name="manipulator"
           planner_interface="moveit_default"
-          constraints="{constraints}"
+          keep_orientation="true"
+          keep_orientation_tolerance="0.2"
           _collapsed="true"
           waypoint_name="{pre_place}"
         />
@@ -76,7 +69,8 @@
           controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
           joint_group_name="manipulator"
           planner_interface="moveit_default"
-          constraints="{constraints}"
+          keep_orientation="true"
+          keep_orientation_tolerance="0.2"
           _collapsed="true"
           waypoint_name="{place}"
         />
@@ -88,7 +82,8 @@
           controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
           joint_group_name="manipulator"
           planner_interface="moveit_default"
-          constraints="{constraints}"
+          keep_orientation="true"
+          keep_orientation_tolerance="0.2"
           _collapsed="true"
           waypoint_name="{pre_place}"
         />

diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
@@ -11,14 +11,12 @@
       <SubTree
         ID="Move to Waypoint"
         _collapsed="true"
-        constraints="{constraints}"
         waypoint_name="Beaker 1a - Pre Pick"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller"
         velocity_scale_factor="1.0"
         acceleration_scale_factor="1.0"
         link_padding="0.01"
-        keep_orientation_tolerance="0.05"
         keep_orientation="false"
       />
       <SubTree ID="Open Gripper" />

diff --git a/src/lab_sim/objectives/joint_diagnostic.xml b/src/lab_sim/objectives/joint_diagnostic.xml
@@ -12,7 +12,6 @@
         waypoint_name="Look at Table"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
@@ -22,14 +21,12 @@
             waypoint_name="Wrist 2 Max"
             joint_group_name="manipulator"
             controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            planner_interface="moveit_default"
           />
           <SubTree
             ID="Move to Waypoint"
             waypoint_name="Wrist 2 Min"
             joint_group_name="manipulator"
             controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            planner_interface="moveit_default"
           />
         </Control>
       </Decorator>

diff --git a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
@@ -14,7 +14,6 @@
         _collapsed="true"
         waypoint_name="Workspace Right"
         joint_group_name="manipulator"
-        planner_interface="moveit_default"
         controller_names="/joint_trajectory_controller"
         acceleration_scale_factor="1.0"
         controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"

diff --git a/src/lab_sim/objectives/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml
@@ -15,12 +15,11 @@
       />
       <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
       <Action
-        ID="SetupMTCMoveToPose"
+        ID="SetupMTCPlanToPose"
         ik_frame="grasp_link"
         planning_group_name="manipulator"
         target_pose="{target_pose}"
         task="{move_to_pose_task}"
-        planner_interface="moveit_default"
       />
       <Action
         ID="PlanMTCTask"

diff --git a/src/lab_sim/objectives/plan_to_pose.xml b/src/lab_sim/objectives/plan_to_pose.xml
@@ -31,12 +31,11 @@
           />
           <Action ID="Script" code="pose_ctr += 1" />
           <Action
-            ID="SetupMTCMoveToPose"
+            ID="SetupMTCPlanToPose"
             ik_frame="{ik_frame}"
             planning_group_name="{planning_group_name}"
             target_pose="{target_pose}"
             task="{mtc_task}"
-            planner_interface="moveit_default"
           />
         </Control>
       </Decorator>

diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/description/robotiq_2f_85_macro.xacro
@@ -7,8 +7,6 @@
     fake_sensor_commands:=false
     sim_ignition:=false
     sim_isaac:=false
-    isaac_joint_commands:=/isaac_joint_commands
-    isaac_joint_states:=/isaac_joint_states
     use_internal_bus_gripper_comm:=false
     com_port:=/dev/ttyUSB0
     moveit_active:=false">

diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_pose.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_pose.xml
@@ -19,7 +19,7 @@
         />
         <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
         <Action
-          ID="SetupMTCMoveToPose"
+          ID="SetupMTCPlanToPose"
           ik_frame="grasp_link"
           planning_group_name="manipulator"
           target_pose="{target_pose}"

diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
@@ -188,7 +188,7 @@
         task="{mtc_task}"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <Action ID="SetupMTCMoveToPose" target_pose="{hole_object}" />
+      <Action ID="SetupMTCPlanToPose" target_pose="{hole_object}" />
       <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
       <Action ID="ExecuteMTCTask" solution="{mtc_solution}" />
       <Action

diff --git a/...eit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml b/...eit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
@@ -188,7 +188,7 @@
         task="{mtc_task}"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <Action ID="SetupMTCMoveToPose" target_pose="{hole_object}" />
+      <Action ID="SetupMTCPlanToPose" target_pose="{hole_object}" />
       <Action ID="PlanMTCTask" task="{mtc_task}" solution="{mtc_solution}" />
       <Action ID="ExecuteMTCTask" solution="{mtc_solution}" />
       <Action

diff --git a/...moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml b/...moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml
@@ -8,16 +8,6 @@
   >
     <Decorator ID="KeepRunningUntilFailure">
       <Control ID="Sequence">
-        <Action
-          ID="InitializeMotionConstraints"
-          constraints_name="gripper pointing down"
-          constraints="{constraints}"
-        />
-        <Action
-          ID="AppendOrientationConstraint"
-          config_file_name="my_constraints.yaml"
-          constraints="{constraints}"
-        />
         <Control ID="Sequence" name="TopLevelSequence">
           <SubTree ID="Open Gripper" />
           <Decorator ID="RetryUntilSuccessful" num_attempts="3">
@@ -35,8 +25,8 @@
               controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
               joint_group_name="manipulator"
               waypoint_name="Grasp Left"
-              planner_interface="moveit_all_planners"
-              constraints="{constraints}"
+              keep_orientation="true"
+              keep_orientation_tolerance="0.2"
             />
           </Decorator>
         </Control>

diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml
@@ -19,7 +19,7 @@
         />
         <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
         <Action
-          ID="SetupMTCMoveToPose"
+          ID="SetupMTCPlanToPose"
           ik_frame="grasp_link"
           planning_group_name="manipulator"
           target_pose="{target_pose}"

diff --git a/...manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml b/...manipulation/mobile_manipulation_config/objectives/00_solution_-_move_to_initial_pose.xml
@@ -15,7 +15,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
     </Control>
   </BehaviorTree>

diff --git a/..._manipulation/mobile_manipulation_config/objectives/20_solution_-_look_at_the_airfoil.xml b/..._manipulation/mobile_manipulation_config/objectives/20_solution_-_look_at_the_airfoil.xml
@@ -12,7 +12,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
     </Control>
   </BehaviorTree>

diff --git a/...ile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml b/...ile_manipulation/mobile_manipulation_config/objectives/21_solution_-_find_the_airfoil.xml
@@ -13,7 +13,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
       <SubTree
         ID="Estimate Object Pose"

diff --git a/...ipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml b/...ipulation/mobile_manipulation_config/objectives/22_solution_-_visualize_airfoil_frame.xml
@@ -16,7 +16,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
       <SubTree
         ID="Estimate Object Pose"

diff --git a/...ipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml b/...ipulation/mobile_manipulation_config/objectives/23_solution_-_airfoil_coverage_origin.xml
@@ -16,7 +16,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
       <SubTree
         ID="Estimate Object Pose"

diff --git a/...anipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml b/...anipulation/mobile_manipulation_config/objectives/24_solution_-_airfoil_coverage_path.xml
@@ -16,7 +16,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
       <SubTree
         ID="Estimate Object Pose"

diff --git a/...pulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml b/...pulation/mobile_manipulation_config/objectives/25_solution_-_execute_airfoil_coverage.xml
@@ -16,7 +16,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
       <SubTree
         ID="Estimate Object Pose"

diff --git a/...bile_manipulation/mobile_manipulation_config/objectives/26_solution_-_better_coverage.xml b/...bile_manipulation/mobile_manipulation_config/objectives/26_solution_-_better_coverage.xml
@@ -14,7 +14,6 @@
         waypoint_name="Look at Airfoil"
         joint_group_name="manipulator"
         controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
       />
       <SubTree
         ID="Estimate Object Pose"

diff --git a/...t_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml b/...t_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml
@@ -15,12 +15,11 @@
         />
         <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
         <Action
-          ID="SetupMTCMoveToPose"
+          ID="SetupMTCPlanToPose"
           ik_frame="grasp_link"
           planning_group_name="manipulator"
           target_pose="{target_pose}"
           task="{move_to_pose_task}"
-          planner_interface="moveit_default"
         />
         <Action
           ID="PlanMTCTask"

diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/config/moveit/picknik_ur.srdf b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/config/moveit/picknik_ur.srdf
@@ -230,4 +230,6 @@
     <disable_collisions link1="ur_arm_wrist_1_link" link2="ur_arm_wrist_2_link" reason="Adjacent"/>
     <disable_collisions link1="ur_arm_wrist_1_link" link2="ur_arm_wrist_3_link" reason="Never"/>
     <disable_collisions link1="ur_arm_wrist_2_link" link2="ur_arm_wrist_3_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link_inertia" link2="riser_link" reason="Never"/>
+    <disable_collisions link1="base_link_inertia" link2="chassis_link" reason="Never"/>
 </robot>