Merge pull request #1 from mechwiz/master

implement PR TAMS-Group#30

Author: dudasdavid

include/bio_ik/robot_info.h

@@ -56,6 +56,7 @@ class RobotInfo
     std::vector<VariableInfo> variables;
     std::vector<size_t> activeVariables;
     std::vector<moveit::core::JointModel::JointType> variable_joint_types;
+    std::vector<bool> has_bounds;
     moveit::core::RobotModelConstPtr robot_model;
 
     __attribute__((always_inline)) static inline double clamp2(double v, double lo, double hi)
@@ -97,6 +98,7 @@ class RobotInfo
             info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0;
 
             variables.push_back(info);
+            has_bounds.push_back(bounded);
         }
 
         for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++)
@@ -120,6 +122,7 @@ class RobotInfo
 
     inline bool isRevolute(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::REVOLUTE; }
     inline bool isPrismatic(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::PRISMATIC; }
+    inline bool hasBounds(size_t variable_index) const { return has_bounds[variable_index]; }
     inline double getMaxVelocity(size_t i) const { return variables[i].max_velocity; }
     inline double getMaxVelocityRcp(size_t i) const { return variables[i].max_velocity_rcp; }
 };

src/kinematics_plugin.cpp

@@ -582,8 +582,8 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
       if (robot_info.isRevolute(ivar) &&
           robot_model->getMimicJointModels().empty()) {
         auto r = problem.initial_guess[ivar];
-        auto lo = robot_info.getMin(ivar);
-        auto hi = robot_info.getMax(ivar);
+        auto lo = robot_info.getClipMin(ivar);
+        auto hi = robot_info.getClipMax(ivar);
 
         // move close to initial guess
         if (r < v - M_PI || r > v + M_PI) {
@@ -609,10 +609,14 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
           v = hi;
       }
       state[ivar] = v;
-    }
 
-    // wrap angles
-    robot_model->enforcePositionBounds(state.data());
+      // wrap angles that are bounded
+      if(robot_info.hasBounds(ivar))
+      {
+        auto* joint_model = robot_model->getJointOfVariable(ivar);
+        joint_model->enforcePositionBounds(state.data() + ivar, joint_model->getVariableBounds());
+      }
+    }
 
     // map result to jointgroup variables
     {