Use trac ik again to resolve performance regression in `SetupMTCMoveT…

…oPose`

.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 = https://bitbucket.org/traclabs/trac_ik.git

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"

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"

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"

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>