Merge pull request #63 from PickNikRobotics/pro-rrt-default

Use Pro-RRT directly under `MoveToJointState` and `MoveToWaypoint`
PickNikRobotics · Dec 30, 2024 · 17eb85f · 17eb85f
2 parents c20d519 + 3084b0e
commit 17eb85f
Show file tree

Hide file tree

Showing 24 changed files with 491 additions and 500 deletions.
diff --git a/src/fanuc_sim/objectives/move_to_joint_state.xml b/src/fanuc_sim/objectives/move_to_joint_state.xml
@@ -1,37 +1,45 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
   <BehaviorTree
     ID="Move to Joint State"
     _description="Move to a specified joint state"
     _subtreeOnly="true"
   >
     <Control ID="Sequence">
+      <Action ID="ActivateControllers" controller_names="{controller_names}" />
       <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_joint_state"
-        controller_names="/joint_trajectory_controller"
-        task="{move_to_waypoint_task}"
+        ID="PlanToJointGoal"
+        joint_goal="{target_joint_state}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        link_padding="{link_padding}"
+        keep_orientation="{keep_orientation}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        velocity_scale_factor="{velocity_scale_factor}"
+        planning_group_name="{joint_group_name}"
       />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
       <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState_First"
-        planning_group_name="manipulator"
-        task="{move_to_waypoint_task}"
-        planner_interface="moveit_default"
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="{controller_action_server}"
       />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
       <input_port name="target_joint_state" default="{target_joint_state}" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
+      <input_port
+        name="controller_names"
+        default="joint_trajectory_controller"
+      />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>

diff --git a/src/fanuc_sim/objectives/move_to_waypoint.xml b/src/fanuc_sim/objectives/move_to_waypoint.xml
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
   <BehaviorTree
     ID="Move to Waypoint"
@@ -11,42 +12,38 @@
         waypoint_name="{waypoint_name}"
         joint_group_name="{joint_group_name}"
       />
-      <Action
-        ID="InitializeMTCTask"
-        task_id="move_to_named_pose"
+      <SubTree
+        ID="Move to Joint State"
+        _collapsed="true"
+        velocity_scale_factor="{velocity_scale_factor}"
+        acceleration_scale_factor="{acceleration_scale_factor}"
+        link_padding="{link_padding}"
+        keep_orientation_tolerance="{keep_orientation_tolerance}"
+        keep_orientation="{keep_orientation}"
+        target_joint_state="{target_joint_state}"
         controller_names="{controller_names}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{target_joint_state}"
-        name="SetupMTCMoveToJointState"
-        planning_group_name="{joint_group_name}"
-        planner_interface="{planner_interface}"
-        constraints="{constraints}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{move_to_waypoint_solution}"
-        task="{move_to_waypoint_task}"
-      />
-      <Action
-        ID="BlockingExecuteMTCTask"
-        solution="{move_to_waypoint_solution}"
+        joint_group_name="{joint_group_name}"
+        controller_action_server="{controller_action_server}"
       />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Move to Waypoint">
-      <input_port name="waypoint_name" default="Point A" />
+      <input_port name="waypoint_name" default="" />
       <input_port name="joint_group_name" default="manipulator" />
-      <input_port name="planner_interface" default="moveit_default" />
+      <input_port name="link_padding" default="0.01" />
+      <input_port name="keep_orientation_tolerance" default="0.05" />
+      <input_port name="keep_orientation" default="false" />
+      <input_port name="velocity_scale_factor" default="1.0" />
+      <input_port name="acceleration_scale_factor" default="1.0" />
       <input_port
         name="controller_names"
         default="/joint_trajectory_controller"
       />
+      <input_port
+        name="controller_action_server"
+        default="/joint_trajectory_controller/follow_joint_trajectory"
+      />
       <MetadataFields>
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>

diff --git a/src/lab_sim/config/moveit/picknik_ur.srdf b/src/lab_sim/config/moveit/picknik_ur.srdf
@@ -195,4 +195,7 @@
     <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>
     <disable_collisions link1="wrist_2_link" link2="wrist_mounted_camera_link" reason="Never"/>
     <disable_collisions link1="wrist_3_link" link2="wrist_mounted_camera_link" reason="Never"/>
+    <disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
+    <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>
+    <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Never"/>
 </robot>
diff --git a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
@@ -13,9 +13,7 @@
         ID="Move to Waypoint"
         waypoint_name="Look at Table"
         joint_group_name="manipulator"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planner_interface="moveit_default"
-        constraints="{constraints}"
+        controller_names="/joint_trajectory_controller"
       />
       <Action
         ID="MoveGripperAction"
@@ -31,9 +29,7 @@
               ID="Move to Waypoint"
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -44,16 +40,12 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
               waypoint_name="Place Cube"
             />
             <Action
@@ -65,9 +57,7 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
           </Control>
           <!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
@@ -76,9 +66,7 @@
               ID="Move to Waypoint"
               waypoint_name="Place Cube"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -89,17 +77,13 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <SubTree
               ID="Move to Waypoint"
               waypoint_name="Pick Cube"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
             <Action
               ID="MoveGripperAction"
@@ -110,9 +94,7 @@
               ID="Move to Waypoint"
               waypoint_name="Look at Table"
               joint_group_name="manipulator"
-              controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-              planner_interface="moveit_default"
-              constraints="{constraints}"
+              controller_names="/joint_trajectory_controller"
             />
           </Control>
         </Control>