add angle wrapping to IK

jhavl · jhavl · commit d6cf1567e8f1 · 2022-06-29T13:55:18.000+10:00
diff --git a/roboticstoolbox/core/ik.cpp b/roboticstoolbox/core/ik.cpp
@@ -91,6 +91,12 @@ extern "C"
                 {
                     // We have arrived
 
+                    // wrap q to +- pi
+                    for (int i = 0; i < ets->n; i++)
+                    {
+                        q(i) = std::fmod(q(i), PI);
+                    }
+
                     // Check for joint limit violation
                     if (reject_jl)
                     {
@@ -107,9 +113,6 @@ extern "C"
                 // Jacobian Matric J
                 _ETS_jacob0(ets, q.data(), (double *)NULL, J);
 
-                // g = J.T @ We @ e
-                // robot.q += np.linalg.inv(J.T @ We @ J) @ g
-
                 g = J.transpose() * We * e;
                 Jw = J.transpose() * We * J;
 
@@ -222,6 +225,12 @@ extern "C"
                 {
                     // We have arrived
 
+                    // wrap q to +- pi
+                    for (int i = 0; i < ets->n; i++)
+                    {
+                        q(i) = std::fmod(q(i), PI);
+                    }
+
                     // Check for joint limit violation
                     if (reject_jl)
                     {
@@ -339,6 +348,12 @@ extern "C"
                 {
                     // We have arrived
 
+                    // wrap q to +- pi
+                    for (int i = 0; i < ets->n; i++)
+                    {
+                        q(i) = std::fmod(q(i), PI);
+                    }
+
                     // Check for joint limit violation
                     if (reject_jl)
                     {
@@ -447,6 +462,12 @@ extern "C"
                 {
                     // We have arrived
 
+                    // wrap q to +- pi
+                    for (int i = 0; i < ets->n; i++)
+                    {
+                        q(i) = std::fmod(q(i), PI);
+                    }
+
                     // Check for joint limit violation
                     if (reject_jl)
                     {
@@ -553,6 +574,12 @@ extern "C"
                 {
                     // We have arrived
 
+                    // wrap q to +- pi
+                    for (int i = 0; i < ets->n; i++)
+                    {
+                        q(i) = std::fmod(q(i), PI);
+                    }
+
                     // Check for joint limit violation
                     if (reject_jl)
                     {
