From b00b271dba1605751e027d848b761a55571c015a Mon Sep 17 00:00:00 2001
From: Michael Wiznitzer <michael.wiznitzer@resquared.com>
Date: Thu, 26 Jan 2023 15:23:00 -0500
Subject: [PATCH] don't wrap continuous joint angles

---
 include/bio_ik/robot_info.h |  3 +++
 src/kinematics_plugin.cpp   | 14 +++++++++-----
 2 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/include/bio_ik/robot_info.h b/include/bio_ik/robot_info.h
index a6139c14..94171f3c 100644
--- a/include/bio_ik/robot_info.h
+++ b/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; }
 };
diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp
index 1fc28b96..f780e079 100644
--- a/src/kinematics_plugin.cpp
+++ b/src/kinematics_plugin.cpp
@@ -528,8 +528,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) {
@@ -555,10 +555,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
     {