Bugfixes from testing with Kinova Gen3

src/ik_gradient.cpp

@@ -79,9 +79,13 @@ auto step(GradientIk& self, Robot const& robot, CostFn const& cost_fn, double st
     // (move along gradient direction by estimated step size)
     for (size_t i = 0; i < count; ++i) {
         auto const& var = robot.variables[i];
+        auto updated_value = self.local[i] - self.gradient[i] * joint_diff;
         if (var.bounded) {
-            self.working[i] =
-                std::clamp(self.local[i] - self.gradient[i] * joint_diff, var.min, var.max);
+            self.working[i] = std::clamp(updated_value, var.min, var.max);
+        } else {
+            self.working[i] = std::clamp(updated_value,
+                                         self.local[i] - var.span / 2.0,
+                                         self.local[i] + var.span / 2.0);
         }
     }
 

src/ik_memetic.cpp

@@ -158,6 +158,10 @@ void MemeticIk::reproduce(Robot const& robot, CostFn const& cost_fn) {
                 // Clamp to valid joint values
                 if (joint.bounded) {
                     gene = std::clamp(gene, joint.min, joint.max);
+                } else {
+                    gene = std::clamp(gene,
+                                      original_gene - joint.span / 2.0,
+                                      original_gene + joint.span / 2.0);
                 }
 
                 // Approximate gradient

src/robot.cpp

@@ -14,7 +14,7 @@
 #include <moveit/robot_state/robot_state.h>
 
 namespace {
-constexpr double kUnboundedVariableSpan = 1.0;
+constexpr double kUnboundedVariableSpan = 2.0 * M_PI;
 constexpr double kUnboundedJointSampleSpread = M_PI;
 }  // namespace