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:
- 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$.
- 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?