add angle wrapping to IK
@@ -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)
{
