Inverse kinematic summary

Solution derived thus far:

θ1 = atan2 (px, py) -    atan2 (d3, ± ( px^2 +   py^2 - d3^2)^(1/2))

K =    (px^2 + py^2 + pz^2 - (a2^2 + a3^2 + d3^2 + d4^2))/(2a2) <br /> θ3 = atan2 (a3, d4) -    atan2 (K, ± ( a3^2 + d4^2 - K^2)^(1/2))

θ2  = atan2[-(a3+a2 c3) pz-(c1 px+py s1) (d4-a2 s3), -d4 pz+a2 s3 pz+(a3+a2 c3) (c1 px+py s1)] - θ3

θ4  = atan2(c1 r23-r13 s1, -c1 c23 r13- c23 r23 s1+r33 s23), sin(θ5) ≠0

c5 =-c23 r33-c1 r13 s23-r23 s1 s23

s5 =-c1 c23 c4 r13-s1 s4 r13-c23 c4 r23 s1+c4 r33 s23+c1 r23 s4

θ5 = atan2(s5, c5)

c6= c5 (c4 (c1 c23 r11+c23 r21 s1-r31 s23)-(c1 r21-r11 s1) s4)+(-c23 r31-c1 r11 s23-r21 s1 s23) s5

s6= -c1 c4 r21-c23 s1 s4 r21+c4 r11 s1-c1 c23 r11 s4+r31 s23 s4

θ6  = atan2(s6, c6)

Alternate solution:

θ4' = θ4 + π

θ5' = -θ5

θ6' = θ6 + π

Created by Mathematica  (September 29, 2003)