From c708a097fb88ca061df1635035fc2867a43617f0 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Tue, 31 Dec 2024 13:33:46 -0700
Subject: [PATCH] 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.
---
 src/lab_sim/objectives/create_pose_vector.xml | 55 -------------------
 1 file changed, 55 deletions(-)

diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
index fe951010..661b04b3 100644
--- 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"