From edb027e8564650760c30295f611e78eca8d09d9e Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Fri, 6 Mar 2015 15:26:26 +0100
Subject: [PATCH] crop ik solutions wrt joint_limits

---
 ur10_moveit_config/config/kinematics.yaml |  3 +-
 ur5_moveit_config/config/kinematics.yaml  |  3 +-
 ur_kinematics/src/ur_moveit_plugin.cpp    | 51 +++++++++++++++++++++--
 3 files changed, 49 insertions(+), 8 deletions(-)

diff --git a/ur10_moveit_config/config/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml
index c45b66490..e37d336c3 100644
--- a/ur10_moveit_config/config/kinematics.yaml
+++ b/ur10_moveit_config/config/kinematics.yaml
@@ -1,4 +1,3 @@
-## the UR10KinematicsPlugin only works with the unlimited UR10, i.e. limited:=false
 #manipulator:
 #  kinematics_solver: ur_kinematics/UR10KinematicsPlugin
 #  kinematics_solver_search_resolution: 0.005
@@ -8,4 +7,4 @@ manipulator:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
-  kinematics_solver_attempts: 3
\ No newline at end of file
+  kinematics_solver_attempts: 3
diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml
index 7b3d7afd7..e2fa9b157 100644
--- a/ur5_moveit_config/config/kinematics.yaml
+++ b/ur5_moveit_config/config/kinematics.yaml
@@ -1,4 +1,3 @@
-## the UR5KinematicsPlugin only works with the unlimited UR5, i.e. limited:=false
 #manipulator:
 #  kinematics_solver: ur_kinematics/UR5KinematicsPlugin
 #  kinematics_solver_search_resolution: 0.005
@@ -8,4 +7,4 @@ manipulator:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
-  kinematics_solver_attempts: 3
\ No newline at end of file
+  kinematics_solver_attempts: 3
diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp
index 56ca34fb6..160558603 100644
--- a/ur_kinematics/src/ur_moveit_plugin.cpp
+++ b/ur_kinematics/src/ur_moveit_plugin.cpp
@@ -645,14 +645,57 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
     // Do the analytic IK
     num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols, 
                        jnt_pos_test(ur_joint_inds_start_+5));
+    
+    
+    uint16_t num_valid_sols;
+    std::vector< std::vector<double> > q_ik_valid_sols;
+    for(uint16_t i=0; i<num_sols; i++)
+    {
+      bool valid = true;
+      std::vector< double > valid_solution;
+      valid_solution.assign(6,0.0);
+      
+      for(uint16_t j=0; j<6; j++)
+      {
+        if((q_ik_sols[i][j] <= ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j] >= ik_chain_info_.limits[j].min_position))
+        { 
+          valid_solution[j] = q_ik_sols[i][j];
+          valid = true;
+          continue;
+        }
+        else if ((q_ik_sols[i][j] > ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j]-2*M_PI > ik_chain_info_.limits[j].min_position))
+        {
+          valid_solution[j] = q_ik_sols[i][j]-2*M_PI;
+          valid = true;
+          continue;
+        }
+        else if ((q_ik_sols[i][j] < ik_chain_info_.limits[j].min_position) && (q_ik_sols[i][j]+2*M_PI < ik_chain_info_.limits[j].max_position))
+        {
+          valid_solution[j] = q_ik_sols[i][j]+2*M_PI;
+          valid = true;
+          continue;
+        }
+        else
+        {
+          valid = false;
+          break;
+        }
+      }
+      
+      if(valid)
+      {
+        q_ik_valid_sols.push_back(valid_solution);
+      }
+    }
+     
      
     // use weighted absolute deviations to determine the solution closest the seed state
     std::vector<idx_double> weighted_diffs;
-    for(uint16_t i=0; i<num_sols; i++) {
+    for(uint16_t i=0; i<q_ik_valid_sols.size(); i++) {
       double cur_weighted_diff = 0;
       for(uint16_t j=0; j<6; j++) {
         // solution violates the consistency_limits, throw it out
-        double abs_diff = std::fabs(ik_seed_state[ur_joint_inds_start_+j] - q_ik_sols[i][j]);
+        double abs_diff = std::fabs(ik_seed_state[ur_joint_inds_start_+j] - q_ik_valid_sols[i][j]);
         if(!consistency_limits.empty() && abs_diff > consistency_limits[ur_joint_inds_start_+j]) {
           cur_weighted_diff = std::numeric_limits<double>::infinity();
           break;
@@ -669,7 +712,7 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
     printf("                     q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", ik_seed_state[1], ik_seed_state[2], ik_seed_state[3], ik_seed_state[4], ik_seed_state[5], ik_seed_state[6]);
     for(uint16_t i=0; i<weighted_diffs.size(); i++) {
       int cur_idx = weighted_diffs[i].first;
-      printf("diff %f, i %d, q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", weighted_diffs[i].second, cur_idx, q_ik_sols[cur_idx][0], q_ik_sols[cur_idx][1], q_ik_sols[cur_idx][2], q_ik_sols[cur_idx][3], q_ik_sols[cur_idx][4], q_ik_sols[cur_idx][5]);
+      printf("diff %f, i %d, q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", weighted_diffs[i].second, cur_idx, q_ik_valid_sols[cur_idx][0], q_ik_valid_sols[cur_idx][1], q_ik_valid_sols[cur_idx][2], q_ik_valid_sols[cur_idx][3], q_ik_valid_sols[cur_idx][4], q_ik_valid_sols[cur_idx][5]);
     }
     printf("end\n");
 #endif
@@ -682,7 +725,7 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
 
       // copy the best solution to the output
       int cur_idx = weighted_diffs[i].first;
-      std::copy(q_ik_sols[cur_idx], q_ik_sols[cur_idx+1], solution.begin() + ur_joint_inds_start_);
+      solution = q_ik_valid_sols[cur_idx];
 
       // see if this solution passes the callback function test
       if(!solution_callback.empty())