5
$\begingroup$

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?

$\endgroup$
  • $\begingroup$ I did a similar simulation using SolidWorks: youtube.com/watch?v=crJXUlzJ918 $\endgroup$ – LCarvalho Feb 19 '18 at 19:47
  • $\begingroup$ Very interesting and instructive. Thanks for sharing your knowledge. In the end, I was able to find a Mathematica solution to this problem, but not using the interior point method. Instead, a change of variables allowed me to use the Standard Newton's method without having to worry about constraining motion. $\endgroup$ – An Coileanach Feb 20 '18 at 19:18
  • $\begingroup$ I'm glad you liked my video. Now I am looking for the solution of all the movement that the software provided through the mathematical base $\endgroup$ – LCarvalho Feb 21 '18 at 18:33
1
$\begingroup$

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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.