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?
