Example 3.4:: Determining the Forward Kinematics (Mathematica)

This example computes the forward kinematics for a 3R spatial manipulator.

Contents

Define a Generic A-Matrix Function

A[θ_, d_, a_, α_] := ({{Cos[θ], -Cos[α] Sin[θ], Sin[α] Sin[θ], a Cos[θ]}, {Sin[θ], Cos[α] Cos[θ], -Cos[θ] Sin[α], a Sin[θ]}, {0, Sin[α], Cos[α], d}, {0, 0, 0, 1}}) ;

DH Parameters

θ1=.; d1=-51; a1=178; α1=-90 Degree;
θ2=.; d2=0; a2=179; α2=-90 Degree;
θ3=.; d3=0; a3=153; α3=0 Degree;

A10: Transformation from the Link 1 Frame to the Link 0 Frame

A10 = A[θ1, d1, a1, α1];

A21: Transformation from the Link 2 Frame to the Link 1 Frame

A21 = A[θ2, d2, a2, α2];

A32: Transformation from the Link 3 Frame to the Link 2 Frame

A32 = A[θ3, d3, a3, α3];

U2: Transformation from the Link 3 Frame to the Link 1 Frame

U2 = Simplify[A21.A32];

MatrixForm[U2]

Create Local Variables

U2L = {{U211, U212, U2[[1, 3]], U214}, {U221, U222, U2[[2, 3]], U224}, {U2[[3, 1]], U2[[3, 2]], U2[[3, 3]], U234}, {0, 0, 0, 1}};

U1: Transformation from the Link 3 Frame to the Link 0 Frame (Use this to display with local variables.)

U1 = Simplify[A10.U2L];

MatrixForm[U1]

U1 = T30: Transformation from the Link 3 Frame to the Link 0 Frame (Use this for computation purposes.)

T30 = Simplify[A10.A21.A32];