Use new Pro RRT SetupMTCPlanTo_ behaviors

These new behaviors expose more planning parameters for the Pro RRT
Connect planner.

Replace:
* `SetupMTCMoveToPose` -> `SetupMTCPlanToPose`
* `SetupMTCMoveToJointState` -> `SetupMTCPlanToJointState`

Update planning configuration ports:
* Remove deprecated `planner_interface` port.
* use `keep_orientation` and `keep_orientation_tolerance` ports instead
  of `constraints` port
* Lower `link_padding` where it causes self collisions


Former-commit-id: 91268bc

Author: dyackzan

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"

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" />

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}"
         />

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" />

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>

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"

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"

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>

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">

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}"