Newton's Method for Robotic Arm with Constrained Motion

Question

My question is related to the system in Using a Grobner basis to calculate common roots of a system of polynomial equations, but I am trying to solve with Newton root-finding methods the inverse kinematics of a robot with 3 revolute joints and one prismatic arm, able to take lengths between 1 and 2. We assume all other lengths are 1.

Define a point $(a,b,θ)$ in configuration space to be a position and orientation of the end "hand", with $(a,b)∈\mathbb{R}^2$ the position of the hand, and $θ$ the angle the hand makes with the standard $x$ axis. As the hand is rigidly attached to length $l_4$ it follows that any orientation $θ$ is precisely the sum $θ_1 +θ_2 +θ_3$. For any such position in configuration space $(a,b,θ)$ we want to find whether there exists a point $(θ_1,θ_2,θ_3,l_4)$ in joint space mapping to $(a,b,θ)$ under the forward kinematic equation, i.e. a root of the function $f(\theta_1,\theta_2,\theta_3,l_4) = \begin{bmatrix} l_4 \cos(\theta_1 + \theta_2 + \theta_3) + l_3 \cos(\theta_1 + \theta_2) + l_2 \cos(\theta_1) - a \\ l_4 \sin(\theta_1 + \theta_2 + \theta_3) + l_3 \sin(\theta_1 + \theta_2) + l_2 \sin(\theta_1) - b\\ \theta_1 + \theta_2 + \theta_3 - \theta \end{bmatrix}$

in the region $1\leq l_4\leq 2$.

To do this, I tried to use the interior point method (https://en.wikipedia.org/wiki/Interior_point_method), where the barrier function used is $B(\theta_1,\theta_2,\theta_3,l_4,\mu)= ||f||^{2} - \mu\Big(\log(l_4-1)+\log(2-l_4)\Big)$

I ordered the variables and defined the gradient of $B$ as BB in what follows

T := {A1, A2, A3, l4, mu}; (*Place some order on the variables*)
BB[A1_, A2_, A3_, l4_, mu_] := Grad[B[A1, A2, A3, l4, mu], {A1, A2, A3, l4, mu}]
VJB[v_, m_] := N[BB[A1, A2, A3, l4, mu] /. A1 -> v[[1]] /. A2 -> v[[2]] /.A3 -> v[[3]] /. l4 -> v[[4]] /. mu -> m] (*Evaluates BB at a given configuration, where v = {A1,A2,A3,l4}*)
JB[A1_, A2_, A3_, l4_, mu_] := D[BB[A1, A2, A3, l4, mu], {T}] 
(*Jacobian matrix of BB wrt A1, A2, A3, l4, mu*)
NJB[v_, m_] := N[JB[A1, A2, A3, l4, mu]] /. A1 -> v[[1]] /. A2 -> v[[2]] /. A3 -> v[[3]] /. l4 -> v[[4]] /. mu -> m (*Evaluate the Jacobian at a given configuration, where v = {A1,A2,A3,l4}*)

The idea then was to use a Newton's method to root-find here. I coded this as follows:

check[p0_, tol_, iter_, maxiter_] := (p0 > tol && iter < maxiter) (*Checks conditions within loop are valid*)
Config := Function[v0, Block[(*Use to declare local variables*)


{  p0 = 1  (*Some number larger than the initial tolerance to start off the loop*),
    v = v0(*v is a 4-vector which of the form (Subscript[\[Theta], 1], Subscript[\[Theta], 2], Subscript[\[Theta], 3], Subscript[l, 4]) *),
    m = 0.75(*\[Mu] in the interior point method description*),
    iter = 1 (*Iter counts the number of iterations*),
    iter1 = 1,
    maxiter = 50 (*Max number of iterations*), 
    tol = 10^-8 (*Tolerance*)
    },
   While[check[p0, tol, iter, maxiter] == True,
    {
     p = -PseudoInverse[NJB[v, m]].VJB[v, m] (*Plays the role of Subscript[\[CapitalDelta], k]in the usual Newton method for multi-variables*),
     v[[1 ;; 3]] = Mod[v[[1 ;; 3]] + p[[1 ;; 3]], 2 Pi], (*First four elements of p correspond to v*)(*First three elements are the angles - work (mod 2\[Pi])*)
     v[[4]] = v[[4]] + p[[4]] (*Fourth element of p corresponds to Subscript[l, 4]*),
     m = m + Abs[p[[5]]], (*Last two elements of p correspond to \[Mu]*)
     p0 = Norm[p[[1 ;; 4]]], (*We only want to ensure that the tolerance is related to the convergence of v-v^(Fixed point) to 0, and is not related to \[Mu]*)
     iter = iter + 1
     }
    ];
   v
   ]
  ]

Unfortunately, there are two immediate problems with this method:

1. The vector v outputted by the config function is more often than not complex-valued, which doesn't make sense, since it should correspond to our angles $\theta_1,\theta_2,\theta_3$ and our length $l_4$.
2. The output of config doesn't give a zero for the function $f$ as defined above.

As can be seen above, I am a fairly novice programmer, and I suspect that the issue lies with the code I've written. Can anyone offer any insights into why this is going so badly wrong?

Answer 1

So the trick came from making the change of variables $l_4: \mathbb{R}\to (1,2)$ by $l_4(t) = 1+ \frac{1}{1+\exp{(-t+1)}}$. In Mathematica, define l4[t_] := 1 + 1/(1 + Exp[-t + 1]); The inverse to this function is defined to be

invl4[m_] := 1 - Log[1/(m - 1) - 1]

My forward equation for a desired configuration $\{a,b,\theta\}$ was given by

f[A1_, A2_, A3_, t_, a_, b_, \[Theta]_] := {
                       l4[t]  Cos[A1 + A2 + A3] + l3 Cos[A1 + A2] + l2 Cos[A1] - a
                    , l4[t] Sin[A1 + A2 + A3] + l3 Sin[A1 + A2] + l2 Sin[A1] - b
                    , A1 + A2 + A3 - \[Theta]
                    }; (*Underlying function at position a, b, \[Theta]*)
l3 = 1; 
l2 = 1;

where $A_1, A_2, A_3$ correspond to the angles for the individual joints. The next step was to calculate the Jacobian and be able to evaluate it for a given vector, so $F$ is defined to be the Jacobian and $FF$ is the Jacobian evaluated on some vector of the form $\{A_1, A_2, A_3, t,a,b,\theta\}$. We also define $ff$ to be the same as the function, but taking a vector as its input to be consistent with $FF$

T := {A1, A2, A3, t};
F[A1_, A2_, A3_, t_, a_, b_, \[Theta]_] := D[f[A1, A2, A3, t, a, b, \[Theta]], {T}]
FF[v_] :=  N[F[A1, A2, A3, t, a, b, \[Theta]] /. {A1 -> v[[1]], A2 -> v[[2]], A3 -> v[[3]], t -> v[[4]]}] 
ff[v_] := N[f[A1, A2, A3, t, a, b, \[Theta]] /. {A1 -> v[[1]], A2 -> v[[2]], A3 -> v[[3]], t -> v[[4]]}] 

Finally, before creating the loop for the Newton method, I wanted a function which would break the loop:

check[p0_, tol_, iter_,   maxiter_] := (p0 > tol && iter < maxiter) (*Checks conditions within loop are valid*)

Finally, we can define the function which returns a configuration for some initial estimated input:

Config :=  Function[{v0, a, b, \[Theta]}, 
Block[(*Use to declare local variables*)
{  p0 = 1  (*Some number larger than the initial tolerance to start off the loop*),
    v = Join[v0[[1 ;; 3]], {invl4[v0[[4]]]}]    (*v is a 4-vector which is of the form (A_1, A_2, A_3, t) *),
    iter = 1 (*Iter counts the number of iterations*),
    maxiter = 50000 (*Max number of iterations*), 
    tol = 10^-10 (*Tolerance*),
    vlist = {v0}
    },
While[check[p0, tol, iter, maxiter] == True,
    {
     p = (-PseudoInverse[FF[v]].ff[v]) (*Plays the role of Subscript\[CapitalDelta], k] in the usual Newton method for multi-variables*),
     v = v + p, (*First four elements of p correspond to v*)(*First three elements are the angles*) (* Fourth element of p corresponds to t*), 
     p0 = Norm[p], (*We want to ensure that the tolerance is related to the convergence of v-v^(Fixed point) to 0)
     iter = iter + 1
     }
    ];
   (*v is outputted from this loop as a vector (A_1, A_2, A_3, t), so we need to convert it back to the form (A_1, A_2, A_3, l_4[t])*)
   Join[Mod[Re[v][[1 ;; 3]], 2 Pi], {Re[l4[v[[4]]]]}]
   ]
  ]

I'm sure there's a cleaner way to do this, but this way at least worked up to a point, although convergence became an issue for $l_4\to 2$, as this corresponded to $t\to \infty$.