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.}} *)