Don't use MTC in Plan and Save Trajectory objective

The MTC plan's computational complexity increases exponentially when
several MoveToPose stages, each with multiple solutions are stacked on
top of each other.

Using PlanCartesianPath with a validation step to ensure no collisions
is a much less computationally complex way to navigate through poses.

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/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,55 @@
     _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>

src/lab_sim/objectives/plan_and_save_trajectory.xml

@@ -8,7 +8,7 @@
     _subtreeOnly="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
-      <!-- Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position -->
+      <!--Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position-->
       <SubTree
         ID="Move to Waypoint"
         _collapsed="true"
@@ -23,45 +23,30 @@
         link_padding="0.01"
         velocity_scale_factor="1.0"
       />
-      <!-- Create a vector of poses so we can add them all to the MTC task at once -->
+      <!--Create a vector of poses we want to move through-->
       <SubTree
         ID="Create Pose Vector"
         _collapsed="false"
         target_poses="{target_poses}"
       />
-      <!-- Add the cartesian poses we created in the previous step -->
-      <SubTree
-        ID="Add Poses to MTC Task"
-        _collapsed="true"
-        target_poses="{target_poses}"
-        controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
-        planning_group_name="manipulator"
-        ik_frame="grasp_link"
-        mtc_task="{mtc_task}"
-      />
-      <!-- Add joint-state waypoints to the MTC task as well -->
-      <SubTree
-        ID="Add Waypoints to MTC Task"
-        _collapsed="false"
-        mtc_task="{mtc_task}"
-        waypoint_names="Pick Cube;Place Cube"
-        joint_group_name="manipulator"
-        planner_interface="moveit_default"
-      />
-      <!-- Perform the computationally intensive calculation now -->
-      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
-      <!-- Preview the trajectory for approval -->
-      <Action ID="WaitForUserTrajectoryApproval" solution="{mtc_solution}" />
-      <!-- The MTC solution does not contain time parameterization, so we can use TOTG to convert it to a JointTrajectory. The source code for the following behavior can be found in the example_behaviors package -->
+      <!--Plan a cartesian path through the desired poses-->
       <Action
-        ID="ConvertMtcSolutionToJointTrajectory"
-        joint_trajectory="{joint_trajectory_msg}"
-        acceleration_scaling_factor="1.000000"
-        joint_group="manipulator"
-        sampling_rate="100"
-        solution="{mtc_solution}"
-        velocity_scaling_factor="1.000000"
+        ID="PlanCartesianPath"
+        acceleration_scale_factor="1.000000"
+        blending_radius="0.020000"
+        ik_cartesian_space_density="0.010000"
+        ik_joint_space_density="0.100000"
+        path="{target_poses}"
+        planning_group_name="manipulator"
+        position_only="false"
+        tip_link="grasp_link"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
+        debug_solution="{debug_solution}"
+        joint_trajectory_msg="{joint_trajectory_msg}"
       />
+      <!--Preview the trajectory for approval and save if approved-->
+      <Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
       <Action
         ID="SaveJointTrajectoryToYaml"
         yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"