Inverse kinematic setup

Puma DH parameters (degrees)

dhDegrees = {{0, 0, 0, θ1}, {-90, 0, 0, θ2}, {0, a2, d3, θ3}, {-90, a3, d4, θ4}, {90, 0, 0, θ5}, {-90, 0, 0, θ6}} ; dhDegrees // TraditionalForm

( 0 0 0 θ1 ) -90 0 0 ... 90 0 0 θ5 -90 0 0 θ6

Puma DH parameters (radians)

dh = ConvertFromDegrees[dhDegrees] ; dh // TraditionalForm

( 0 0 0 θ1 ) -90 ° 0 0 ... 90 ° 0 0 θ5 -90 ° 0 0 θ6

Short-hand display notation

short = MakeRulesList[dh, {{2, 3}}] ; TF := TraditionalForm ; d[m_] := m /. short // TraditionalForm ;

What we know:

In the inverse kinematics problem, we assume that the matrix below is known; that is, we assume that we have a desired position/orientation of the end-effector with respect to the base, and we want to solve for the joint angles.

Trp = RAndPToT[{{r11, r12, r13}, {r21, r22, r23}, {r31, r32, r33}}, {px, py, pz}] ; Trp // TF

( r11 r12 r13 px ) r21 r22 r23 py r31 r32 r33 pz 0 0 0 1

Link transforms

T01 = T[dh, 0, 1] ; T01 // d

( c1 -s1 0 0 ) s1 c1 0 0 0 0 1 0 0 0 0 1

T13 = T[dh, 1, 3] // FullSimplify ; T13 // d

( c23 -s23 0 a2 c2 ) 0 0 1 d3 -s23 -c23 0 -a2 s2 0 0 0 1

T34 = T[dh, 3, 4] ; T34 // d

( c4 -s4 0 a3 ) 0 0 1 d4 -s4 -c4 0 0 0 0 0 1

T45 = T[dh, 4, 5] ; T45 // d

( c5 -s5 0 0 ) 0 0 -1 0 s5 c5 0 0 0 0 0 1

T56 = T[dh, 5, 6] ; T56 // d

( c6 -s6 0 0 ) 0 0 1 0 -s6 -c6 0 0 0 0 0 1

Overall transform

Below, we now have 12 possible equations in terms of the joint angles; namely Trp = T06...

T06 = T01 . T13 . T34 . T45 . T56 ; T06 //Transpose // d

( c6 (c5 (c1 c23 c4 + s1 s4) - c1 s23 s5) - (c1 c23 s4 - c4 s1) s6 c6 (c5 (c23 c4 s ... 3 -c23 d4 - a2 s2 - a3 s23 1

Trp // Transpose // d

( r11 r21 r31 0 ) r12 r22 r32 0 r13 r23 r33 0 px py pz 1

Created by Mathematica  (September 29, 2003)