-1
$\begingroup$

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];
];
|improve this question
$\endgroup$
  • $\begingroup$ 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.. $\endgroup$ – Akshay Katpatal Jun 6 '17 at 12:01
  • $\begingroup$ You should start with some basic Mathematica tutorials. Currently, your syntax is not compatible. $\endgroup$ – Yves Klett Jun 6 '17 at 12:13
  • $\begingroup$ 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 $\endgroup$ – Akshay Katpatal Jun 6 '17 at 12:33
  • $\begingroup$ Please write the actual code so we can cut-n-paste it. $\endgroup$ – MikeY Jun 6 '17 at 12:40
  • $\begingroup$ theta look weird.I directly copied it from Mathematica. Cant seem to figure out how to write for inverse. $\endgroup$ – Akshay Katpatal Jun 6 '17 at 12:49
1
$\begingroup$

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;

invsol//TraditionalForm

(*  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.

|improve this answer
$\endgroup$
  • $\begingroup$ This does work. But it uses solve. Anyway thanks for the help. $\endgroup$ – Akshay Katpatal Jun 7 '17 at 7:06

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.