Get two dimensional Newton-Raphson in inverse kinematics of a plane robot?

Questions
, , , , ,
1

Hello,

I just started with Mathematica a week ago and tried to implement inverse kinematics of a plane robot.
Dependent of the length x1 and x2 the position (f1,f2) is described. The idea is to calculate (x1,x2) which will reach given point Xps. The vector X12l saves the initial and later on the iterating lengths of beams x1 and x2.

The trouble I have is that the loop is not working at all, and I am only getting weird therms.

Xps = {4, 3};
X12l = {3, 3};
a := 4;
i = 0;
f1[x1_, x2_] = x1 (a^2 + x1^2 - x2^2)/(2 a x1) ;
f2[x1_, x2_] = x1* Sqrt[1 - (((a^2 + x1^2 - x2^2)/(2 a x1))^2) ];
J[x1_, x2_] = ( {{D[f1[x1, x2], x1], D[f1[x1, x2], x2]},{D[f2[x1, x2], x1], D[f2[x1, x2], x2]} } )  // MatrixForm;
dx = N[Norm[Xps - {f1[X12l[[1]], X12l[[2]]], f2[X12l[[1]], X12l[[2]]]}], 5];

While[dx > 0.1, 
 	X12l = Inverse[J[X12l[[1]], X12l[[2]]]].(Xps - {f1[X12l[[1]], X12l[[2]]], f2[X12l[[1]], X12l[[2]]]});
 	dx = Norm[Xps - {f1[X12l[[1]], X12l[[2]]], f2[X12l[[1]], X12l[[2]]]}];
 	i++;
 ]
i
N[{f1[X12l[[1]], X12l[[2]]], f2[X12l[[1]], X12l[[2]]]}, 3]  (*Position of the TCP after dx<0.1*)
N[X12l, 3] (*the lengths needed to reach Xps*)

948120180203_035328.jpg
948120180203_035328.jpg1086×776 148 KB

2

It’s a stretch (literally, I suppose), to describe this as an inverse kinematics problem. It is really just solving for triangle lengths (doing motion planning from the initial to final configuration, using e.g. actuators to extend/reduce side lengths, is another matter). In any case, accepting the problem as worded, one might proceed as below.

There are a number of issues. One is that SetDelayed might be better to use than Set. A more subtle issue is that the Jacobian needs to take derivatives of a symbolic function, so those “variables” cannot be explicit numeric values. We can handle that with a scoping construct with substitution of actual values at the end (among other possibilities). A third issue is that a formatting function, MatrixForm, got mixed into a function definition. Another problem is that the Newton-Raphson update omitted its all-important starting term.

With all that in mind, here is the set up.

Xps = {4, 3};
X12l = {3., 3.};
a = 4;
i = 0;
f1[x1_, x2_] := x1 (a^2 + x1^2 - x2^2)/(2 a x1);
f2[x1_, x2_] := x1*Sqrt[1 - (((a^2 + x1^2 - x2^2)/(2 a x1))^2)];
J[x1val_, x2val_] := 
 Module[{x1, 
   x2}, ({{D[f1[x1, x2], x1], D[f1[x1, x2], x2]}, {D[f2[x1, x2], x1], 
      D[f2[x1, x2], x2]}}) /. Thread[{x1, x2} -> {x1val, x2val}]]
dx = N[Norm[
   Xps - {f1[X12l[[1]], X12l[[2]]], f2[X12l[[1]], X12l[[2]]]}]]

Might be a good idea to know the actual solution before we compute it iteratively.

In[397]:= FindRoot[Thread[{f1[x, y], f2[x, y]} == Xps], {x, 3}, {y, 3}]

Out[397]= {x -> 5., y -> 3.}

So that’s what we hope to get. On with the iterations. I tightened the epsilon, added an iteration limit, and also print values at each step.

i = 0; 
While[dx > 0.0001 && i < 100, 
 X12l = X12l + 
   Inverse[J[X12l[[1]], 
      X12l[[2]]]].(Xps - {f1[X12l[[1]], X12l[[2]]], 
       f2[X12l[[1]], X12l[[2]]]});
 dx = Norm[Xps - {f1[X12l[[1]], X12l[[2]]], f2[X12l[[1]], X12l[[2]]]}];
 i++;
 Print[{i, dx, X12l}];
 ]

(* {1,0.882231,{4.90273,2.23607}}

{2,0.0784914,{4.92159,2.95645}}

{3,0.000900627,{5.,2.99928}}

{4,8.11131*10^-8,{5.,3.}} *)
3

Thank you alot for your reply!