From b8bb9390ef6020674fff56d0e24af0bbcc546705 Mon Sep 17 00:00:00 2001
From: Dennis Hartmann <hartmanndennis@users.noreply.github.com>
Date: Sat, 19 Aug 2017 16:54:06 +0200
Subject: [PATCH] IKFastTemplate: searchPositionIK now returns collision-free
 solution which is nearest to seed state. (#585)

---
 .../ikfast61_moveit_plugin_template.cpp       | 66 ++++++++++++-------
 1 file changed, 44 insertions(+), 22 deletions(-)

diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp
index 51ab7c73bf..0c0ca098b2 100644
--- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp
+++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp
@@ -136,6 +136,18 @@ enum IkParameterizationType
   /// when serializing the ik parameterizations
 };
 
+// struct for storing and sorting solutions
+struct LimitObeyingSol
+{
+  std::vector<double> value;
+  double dist_from_seed;
+
+  bool operator<(const LimitObeyingSol& a) const
+  {
+    return dist_from_seed < a.dist_from_seed;
+  }
+};
+
 // Code generated by IKFast56/61
 #include "_ROBOT_NAME___GROUP_NAME__ikfast_solver.cpp"
 
@@ -797,31 +809,52 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose
   {
     ROS_DEBUG_STREAM_NAMED("ikfast", "No need to search since no free params/redundant joints");
 
-    // Find first IK solution, within joint limits
-    if (!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
+    std::vector<geometry_msgs::Pose> ik_poses(1, ik_pose);
+    std::vector<std::vector<double>> solutions;
+    kinematics::KinematicsResult kinematic_result;
+    // Find all IK solution within joint limits
+    if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
     {
       ROS_DEBUG_STREAM_NAMED("ikfast", "No solution whatsoever");
       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
       return false;
     }
 
-    // check for collisions if a callback is provided
-    if (!solution_callback.empty())
+    // sort solutions by their distance to the seed
+    std::vector<LimitObeyingSol> solutions_obey_limits;
+    for (std::size_t i = 0; i < solutions.size(); ++i)
     {
-      solution_callback(ik_pose, solution, error_code);
-      if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
+      double dist_from_seed = 0.0;
+      for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
       {
-        ROS_DEBUG_STREAM_NAMED("ikfast", "Solution passes callback");
-        return true;
+        dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
       }
-      else
+
+      solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
+    }
+    std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
+
+    // check for collisions if a callback is provided
+    if (!solution_callback.empty())
+    {
+      for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
       {
-        ROS_DEBUG_STREAM_NAMED("ikfast", "Solution has error code " << error_code);
-        return false;
+        solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
+        if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
+        {
+          solution = solutions_obey_limits[i].value;
+          ROS_DEBUG_STREAM_NAMED("ikfast", "Solution passes callback");
+          return true;
+        }
       }
+
+      ROS_DEBUG_STREAM_NAMED("ikfast", "Solution has error code " << error_code);
+      return false;
     }
     else
     {
+      solution = solutions_obey_limits[0].value;
+      error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
       return true;  // no collision check callback provided
     }
   }
@@ -1042,17 +1075,6 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, c
   int numsol = solve(frame, vfree, solutions);
   ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
 
-  // struct for storing and sorting solutions
-  struct LimitObeyingSol
-  {
-    std::vector<double> value;
-    double dist_from_seed;
-
-    bool operator<(const LimitObeyingSol& a) const
-    {
-      return dist_from_seed < a.dist_from_seed;
-    }
-  };
   std::vector<LimitObeyingSol> solutions_obey_limits;
 
   if (numsol)