Merge pull request #82 from PickNikRobotics/remove-mtc-record-and-rep…

…lay-motion

Don't use MTC in Plan and Save Trajectory objective

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"