From dce561646a2996d41f2ff4a1245b7fc7a593dfbb Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Thu, 2 Jan 2025 10:05:18 -0700
Subject: [PATCH] Use trac ik again to resolve performance regression in
 `SetupMTCMoveToPose`

---
 .gitmodules                                   |  3 +
 src/external_dependencies/trac_ik             |  1 +
 src/lab_sim/objectives/create_pose_vector.xml | 55 +++++++++++++++++++
 .../picknik_ur_base_config/config/config.yaml |  2 +-
 .../moveit/trac_ik_kinematics_distance.yaml   |  6 ++
 .../picknik_ur_base_config/package.xml        |  1 +
 6 files changed, 67 insertions(+), 1 deletion(-)
 create mode 160000 src/external_dependencies/trac_ik
 create mode 100644 src/moveit_pro_ur_configs/picknik_ur_base_config/config/moveit/trac_ik_kinematics_distance.yaml

diff --git a/.gitmodules b/.gitmodules
index 80236521..da03f5c4 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,6 @@
 [submodule "src/picknik_accessories"]
 	path = src/picknik_accessories
 	url = https://github.com/PickNikRobotics/picknik_accessories.git
+[submodule "src/external_dependencies/trac_ik"]
+	path = src/external_dependencies/trac_ik
+	url = git@bitbucket.org:traclabs/trac_ik.git
diff --git a/src/external_dependencies/trac_ik b/src/external_dependencies/trac_ik
new file mode 160000
index 00000000..45e82f69
--- /dev/null
+++ b/src/external_dependencies/trac_ik
@@ -0,0 +1 @@
+Subproject commit 45e82f69c58bd0f1ee9b1f8542d5ae98253a5e94
diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
index 661b04b3..fe951010 100644
--- a/src/lab_sim/objectives/create_pose_vector.xml
+++ b/src/lab_sim/objectives/create_pose_vector.xml
@@ -18,6 +18,17 @@
         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"
@@ -29,6 +40,28 @@
         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"
@@ -40,6 +73,28 @@
         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"
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/config/config.yaml b/src/moveit_pro_ur_configs/picknik_ur_base_config/config/config.yaml
index 46450fe1..88294f0d 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/config/config.yaml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/config/config.yaml
@@ -102,7 +102,7 @@ moveit_params:
     path: "config/moveit/pilz_industrial_motion_planner_planning.yaml"
   kinematics:
     package: "picknik_ur_base_config"
-    path: "config/moveit/pose_ik_distance.yaml"
+    path: "config/moveit/trac_ik_kinematics_distance.yaml"
   servo:
     package: "picknik_ur_base_config"
     path: "config/moveit/ur_servo.yaml"
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/config/moveit/trac_ik_kinematics_distance.yaml b/src/moveit_pro_ur_configs/picknik_ur_base_config/config/moveit/trac_ik_kinematics_distance.yaml
new file mode 100644
index 00000000..65f77d5c
--- /dev/null
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/config/moveit/trac_ik_kinematics_distance.yaml
@@ -0,0 +1,6 @@
+manipulator:
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3
+  epsilon: 0.0001
+  solve_type: "Distance"
diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml
index 6fe4d9e0..a69e0f8c 100644
--- a/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml
+++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/package.xml
@@ -25,6 +25,7 @@
   <exec_depend>realsense2_description</exec_depend>
   <exec_depend>robotiq_description</exec_depend>
   <exec_depend>robotiq_controllers</exec_depend>
+  <exec_depend>trac_ik_kinematics_plugin</exec_depend>
   <exec_depend>ur_description</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>