Reduce the poses used in PlanAndSaveTrajectory

`PlanMTC` has significantly reduced performance when called with multiple `SetupMTCMoveToPose` stages and can no longer plan for as many poses. Reducing the number of poses as a quick fix enables the `PlanAndSaveTrajectory` objective to work again.
PickNikRobotics · Dec 31, 2024 · c708a09 · c708a09
1 parent 17eb85f
commit c708a09
Showing 1 changed file with 0 additions and 55 deletions.
diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
@@ -18,17 +18,6 @@
         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}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
@@ -40,28 +29,6 @@
         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}"
-      />
-      <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}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
@@ -73,28 +40,6 @@
         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}"
-      />
-      <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}"
-      />
-      <Action
-        ID="AddPoseStampedToVector"
-        vector="{target_poses}"
-        input="{stamped_pose}"
-      />
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"