Inverse Kinematics of a 3R Spatial Manipulator

I want to solve the make a module for the inverse kinematics of a 3R Spatial Manipulator. I am new to Mathematica and cant seem to figure out how to write it. The forward kinematics equations are :

x=cos(a1)*(l2cos(a2)+l3cos(a23))
y=sin(a1)(l2cos(a2)+l3cos(a23))
z=l1+l2sin(a2)+l3sin(a23)


a1,a2,a3 are the angles. l1,l2,l3 are the link lengths a23=a2+a3.. Can anyone please help I would like to avoid using the solve function.

forwardKinematics3R[l1_, l2_, l3_, {θ1_, θ2_, θ3_}, {x_, y_, z_}] := Module[
{sol}
, sol = {
x -> Cos[θ1]*(l2*Cos[θ2] +  l3*Cos[θ2 + θ3])
, y -> Sin[θ1]*(l2*Cos[θ2] +  l3*Cos[θ2 + θ3])
, z -> l1 + l2*Sin[θ2] + l3*Sin[θ2 + θ3]
}
; Return[sol];
];

• I know how to solve it analytically on paper. Squaring and adding first 2 equations to eliminate a1 then simultaneously solving the z equation and the squared equation will give me univariate equation which i can solve. But i cant seem to write the above code.. – Akshay Katpatal Jun 6 '17 at 12:01
• You should start with some basic Mathematica tutorials. Currently, your syntax is not compatible. – Yves Klett Jun 6 '17 at 12:13
• Can you tell me how to solve these 3 equations for finding a1,a2,a3 given x,y,z and the link lengths. This is not the actual code I have written for the forward kinematics. This is just to show you the equations – Akshay Katpatal Jun 6 '17 at 12:33
• Please write the actual code so we can cut-n-paste it. – MikeY Jun 6 '17 at 12:40
• theta look weird.I directly copied it from Mathematica. Cant seem to figure out how to write for inverse. – Akshay Katpatal Jun 6 '17 at 12:49

This code grinds out the inverse kinematic equations in analytic form (via Solve[], however):

eq1=x==Cos[a1](l2 Cos[a2]+l3 Cos[a2+a3]);
eq2=y==Sin[a1](l2 Cos[a2]+l3 Cos[a2+a3]);
eq3=z==l1+l2 Sin[a2]+l3 Sin[a2+a3];

(*  Let t=x^2+y^2, then:  *)
eq4=t==(l2 Cos[a2]+l3 Cos[a2+a3])^2;

(*  Now solve Eq.3 and Eq.4 for a2 and a3:  *)
invsol=Solve[{eq3,eq4},{a2,a3}]//Simplify;

(*  Note from Eq.1 and Eq.2 that a1 is just arctan(y/x)
(in MMA, ArcTan[x,y])  *)


It finds four solutions for {a2,a3}, corresponding, I guess to four distinct configuration spaces for the mechanism. The solutions differ only in a couple of pairs of signs, one w.r.t. another. Run time was about two minutes on my desktop. You need to maximize the notebook window to get a good view.

• This does work. But it uses solve. Anyway thanks for the help. – Akshay Katpatal Jun 7 '17 at 7:06