Changed searchPositionIK to return the closest solution to the seed i…

…n the full joint range in the no freem paramater case.

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -820,6 +820,28 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose
       return false;
     }
 
+    // rotate joints by +/-360° where it is possible and useful
+    for (std::size_t i = 0; i < solutions.size(); i++)
+    {
+      for (std::size_t j = 0; j < num_joints_; ++j)
+      {
+        if (joint_has_limits_vector_[j])
+        {
+          double signed_distance = solutions[i][j] - ik_seed_state[j];
+          while (signed_distance > M_PI && solutions[i][j] - 2 * M_PI > (joint_min_vector_[j] - LIMIT_TOLERANCE))
+          {
+            signed_distance -= 2 * M_PI;
+            solutions[i][j] -= 2 * M_PI;
+          }
+          while (signed_distance < -M_PI && solutions[i][j] + 2 * M_PI < (joint_max_vector_[j] + LIMIT_TOLERANCE))
+          {
+            signed_distance += 2 * M_PI;
+            solutions[i][j] += 2 * M_PI;
+          }
+        }
+      }
+    }
+
     // sort solutions by their distance to the seed
     std::vector<LimitObeyingSol> solutions_obey_limits;
     for (std::size_t i = 0; i < solutions.size(); ++i)