Inverse kinematic solution: θ4, θ5, θ6: The home stretch

The following matrix is now known completely:

T36b // d

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

What can we do next?

Here's what we got on the other side 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

When s5 ≠ 0, elements (1, 3) and (3, 3) offer solution to θ4:

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

-c4 s5c1 c23 r13 + c23 r23 s1 - r33 s23

eq2 = T36a[[3, 3]] == T36b[[3, 3]] ; eq2 // d

s4 s5c1 r23 - r13 s1

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

Next, consider T46:

The following matrix is completely known:

T46b = T[dh, 4, 3] . T36b ; <br />T46b //Transpose //d

( c4 (c1 c23 r11 + c23 r21 s1 - r31 s23) - (c1 r21 - r11 s1) s4 - ... pz s23) s4 -d4 - c23 pz - c1 px s23 - py s1 s23 + a2 s3 1

What can we do next?

T46a = T45 . T56 ; T46a // Transpose //d

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

Using elements (1, 3) and (3, 3), solve for θ5:

eq1 = T46a[[1, 3]] == T46b[[1, 3]] ; eq1 // d

-s5c4 (c1 c23 r13 + c23 r23 s1 - r33 s23) - (c1 r23 - r13 s1) s4

Solve[eq1 /. short, s5] // Flatten// TF

{s5 -c1 c23 c4 r13 - s1 s4 r13 - c23 c4 r23 s1 + c4 r33 s23 + c1 r23 s4}

eq2 = T46a[[3, 3]] == T46b[[3, 3]] ; eq2 // d

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

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)

Next, consider T56:

The following matrix is completely known:

T56b = T[dh, 5, 4] . T46b ; T56b //d

( c5 (c4 (c1 c23 r11 + c23 r21 s1 - r31 s23) - (c1 r21 - r11 s1) s4) + (-c23 r31 - c1 ... 1

What can we do next?

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

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

Using elements (1, 1) and (3, 1), solve for θ6:

eq1 = T56a[[1, 1]] == T56b[[1, 1]] ; eq1 // d

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

eq2 = T56a[[3, 1]] == T56b[[3, 1]] ; eq2 // d

-s6c4 (c1 r21 - r11 s1) + (c1 c23 r11 + c23 r21 s1 - r31 s23) s4

Solve[eq2 /. short, s6] // Flatten// TF

{s6 -c1 c4 r21 - c23 s1 s4 r21 + c4 r11 s1 - c1 c23 r11 s4 + r31 s23 s4}

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)

Created by Mathematica  (September 29, 2003)