Inverse kinematic solution: θ2: Premultiply by T30

One side of the equation

T36a = T34 . T45 . T56 ; T36a // d

( c4 c5 c6 - s4 s6 -c6 s4 - c4 c5 s6 -c4 s5 a3 ᡝ ... s4 s5 0 0 0 0 1

The other side of the equation

T30 = T[dh, 3, 0] // FullSimplify ; T30 // d

( c1 c23 c23 s1 -s23 -a2 c3 ) -c1 s23 -s1 s23 -c23 a2 s3 -s1 c1 0 -d3 0 0 0 1

T36b = T30 . Trp ; T36b // d

( c1 c23 r11 + c23 r21 s1 - r31 s23 c1 c23 r12 + c23 r22 s1 - r32 s23 ... 0 0 1

Both sides of the equation

T36a // d

( c4 c5 c6 - s4 s6 -c6 s4 - c4 c5 s6 -c4 s5 a3 ᡝ ... s4 s5 0 0 0 0 1

T36b // d

( c1 c23 r11 + c23 r21 s1 - r31 s23 c1 c23 r12 + c23 r22 s1 - r32 s23 ... 0 0 1

Equate elements (1, 4) and (2, 4)

eq1 = T36a[[1, 4]] == T36b[[1, 4]] ; eq1 // d

a3 -a2 c3 + c1 c23 px + c23 py s1 - pz s23

eq2 = T36a[[2, 4]] == T36b[[2, 4]] ; eq2 // d

d4 -c23 pz - c1 px s23 - py s1 s23 + a2 s3

Solve for c23 and s23:

sol = Solve[{eq1 /. short, eq2 /. short}, {c23, s23}] // FullSimplify ; sol // Flatten // TF

{c23 (-d4 pz + a2 s3 pz + (a3 + a2 c3) (c1 px + py s1))/(pz^2 + (c1 px + py s1)^2), s23 -((a3 + a2 c3) pz + (c1 px + py s1) (d4 - a2 s3))/(pz^2 + (c1 px + py s1)^2)}

c23 = (-d4 pz + a2 s3 pz + (a3 + a2 c3) (c1 px + py s1))/(pz^2 + (c1 px + py s1)^2) <br /><br ... + py s1) (d4 - a2 s3))/(pz^2 + (c1 px + py s1)^2)         

Solve for θ2:

θ23  = atan2(s23, c23)

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

θ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

Created by Mathematica  (September 29, 2003)