Sync main to 7.0

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/_scan_scene.xml

@@ -3,7 +3,7 @@
   <!--//////////-->
   <BehaviorTree
     ID="_Scan Scene"
-    _description="Move robot our of scene to enable full snapshot of scene."
+    _description="Move robot out of scene to enable full snapshot of scene."
     _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">

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/create_pose_vector.xml

@@ -1,17 +1,20 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Create Pose Vector">
   <!--//////////-->
   <BehaviorTree
     ID="Create Pose Vector"
     _favorite="false"
     _subtreeOnly="true"
     _description=""
+    target_poses="{target_poses}"
   >
     <Control ID="Sequence">
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
-        position_xyz="1;0.5;1"
+        position_xyz="0.75;0.5;0.9"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -23,6 +26,31 @@
         orientation_xyzw="0;-1;0;0"
         position_xyz="0.5;0.5;0.8"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0;-1;0;0"
+        position_xyz="0.25;0.5;0.7"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0;-1;0;0"
+        position_xyz="0;0.5;0.6"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -34,6 +62,31 @@
         orientation_xyzw="0;-1;0;0"
         position_xyz="-0.25;0.5;0.7"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0;-1;0;0"
+        position_xyz="-0.5;0.5;0.8"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        vector="{target_poses}"
+        input="{stamped_pose}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        orientation_xyzw="0;-1;0;0"
+        position_xyz="-0.75;0.5;0.9"
+        stamped_pose="{stamped_pose}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -45,6 +98,7 @@
         orientation_xyzw="0;-1;0;0"
         position_xyz="-1;0.5;1"
         stamped_pose="{stamped_pose}"
+        reference_frame="world"
       />
       <Action
         ID="AddPoseStampedToVector"
@@ -58,6 +112,7 @@
       <output_port name="target_poses" default="{target_poses}" />
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="false" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>

src/lab_sim/objectives/joint_diagnostic.xml

@@ -11,8 +11,8 @@
         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"
+        controller_names="/joint_trajectory_controller"
+        link_padding="0.0"
       />
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
@@ -21,15 +21,15 @@
             ID="Move to Waypoint"
             waypoint_name="Wrist 2 Max"
             joint_group_name="manipulator"
-            controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-            planner_interface="moveit_default"
+            controller_names="/joint_trajectory_controller"
+            link_padding="0.0"
           />
           <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"
+            controller_names="/joint_trajectory_controller"
+            link_padding="0.0"
           />
         </Control>
       </Decorator>

src/lab_sim/objectives/load_and_execute_joint_trajectory.xml

@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Load and Execute Joint Trajectory">
   <!--//////////-->
   <BehaviorTree
@@ -7,26 +8,54 @@
     _subtreeOnly="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <!-- Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path -->
+      <!--Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path-->
       <SubTree
         ID="Move to Waypoint"
         _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"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        velocity_scale_factor="1.0"
       />
       <Action
         ID="LoadJointTrajectoryFromYaml"
         file_path="joint_trajectory.yaml"
+        output="{joint_trajectory_msg}"
+      />
+      <!--Validate the loaded trajectory with the current planning scene to check for collisions before executing-->
+      <Action
+        ID="GetCurrentPlanningScene"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="ValidateTrajectory"
+        cartesian_space_step="0.020000"
+        joint_space_step="0.100000"
+        joint_trajectory_msg="{joint_trajectory_msg}"
+        planning_group_name="manipulator"
+        planning_scene_msg="{planning_scene}"
+        debug_solution="{debug_solution}"
+      />
+      <Action
+        ID="ExecuteFollowJointTrajectory"
+        execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
+        goal_duration_tolerance="-1.000000"
+        goal_position_tolerance="0.000000"
+        goal_time_tolerance="0.000000"
+        joint_trajectory_msg="{joint_trajectory_msg}"
       />
-      <Action ID="ExecuteFollowJointTrajectory" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Load and Execute Joint Trajectory">
       <MetadataFields>
         <Metadata subcategory="Application - Basic Examples" />
+        <Metadata runnable="true" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>