Pneuman's Arm

FS[a___] := FullSimplify[a]

TF[a___] := TraditionalForm[a]

DH Parameters

dh  =  {{0, 0, 0, θ1, 0}, {π/2, 0, 0, θ2, 
          0}, {-π/2, 0, d3, θ3, 0}, {π/2, 0, 0, θ4, 
          0}, {-π/2, 0, 0, θ5, 0}}

Tori  =  {{R11,  R12,  R12,  PX}, {R21,  R22,  R23,  
            PY}, {R31,  R32,  R33,  PZ}, {0, 0, 0, 1}};

Tori // TF

T05  =  T[dh,  0,  5];  T05  //  TF

Solve for θ2

eq2a  =  Tori[[1, 4]]^2  ==  T05[[1, 4]]^2; eq2a // FS // 
      TF

eq2b  =  
      Tori[[2, 4]]^2 == T05[[2, 4]]^2; eq2b // FullSimplify // 
      TF

eq2c  =  Tori[[3, 4]] == T05[[3, 4]]; eq2c // FS // 
      TF

So, θ2 = Atan2(+-(Sqrt[(Px^2) + (Py^2)]), Pz)

Solve for θ1

eq1a  =  Tori[[1, 4]] == T05[[1, 4]]; eq1a  // FS // 
      TF

eq1b  =  Tori[[2, 4]] == T05[[2, 4]]; eq1b // FS // 
      TF

So, θ1 = Atan2(-Py, -Px)

Solve for θ3

Premultiply...

T01  =  T[dh, 0, 1]; T01 // FS // TF

T12  =  T[dh, 1, 2]; T12 // FS // TF

T10  =  Inverse[T01]; T10 // FS // TF

T21  =  Inverse[T12]; T21 // FS // TF

T25a  =  T21 . T10 . Tori; T25a // FS // TF

T25b  =  T[dh, 2, 5];; T25b // FS // TF

eq3a  =  T25a[[1, 3]] == T25b[[1, 3]]; eq3a // FS // 
      TF

eq3b  =  T25a[[3, 3]] == T25b[[3, 3]]; eq3b // FS // 
      TF

So, θ3 = Atan2(-R12 cos(θ1) cos(θ2) + R23 sin(θ1) cos(θ2) + R33 sin(θ2), R12 sin(θ1) - R23 cos(θ1))

Solve for θ4

eq4a  =  
      T25a[[2, 1]]^2 == T25b[[2, 1]]^2; eq4a // FS // 
      TF

eq4b  =  
      T25a[[2, 2]]^2 == T25b[[2, 2]]^2; eq4b // FS // 
      TF

eq4c  =  T25a[[2, 3]] == T25b[[2, 3]]; eq4c // FS // 
      TF

So, θ4 = Atan2(\(TraditionalForm\`\(+\(-\@\(\((R31\ \(cos(θ2)\)\  - \ \((R11\ \(cos(θ1)\)\  + \ R21\ \(sin(θ1)\))\)\ \(sin(θ2)\))\)\^2\  + \ \((R32\ \(cos(θ2)\)\  - \ \((R12\ \(cos(θ1)\)\  + \ R22\ \(sin(θ1)\))\)\ \(sin(θ2)\))\)\^2\)\)\)\), R33 cos(θ2) - R12 cos(θ1) sin(θ2) - R23 sin(θ1) sin(θ2))

Solve for θ5

Again, premultiply...

T23  =  T[dh, 2, 3]; T23 // FS // TF

T34  =  T[dh, 3, 4]; T34 // FS // TF

T32  =  Inverse[T23]; T32 // FS // TF

T43  =  Inverse[T34]; T43 // FS // TF

T45a  =  T43 . T32 . T21 . T10 . Tori; T45a // FS // 
      TF

T45b  =  T[dh, 4, 5]; T45b // FS // TF

eq5a  =  T45a[[3, 1]] == T45b[[3, 1]]; eq5a // FS // 
      TF

eq5b  =  T45a[[3, 2]] == T45b[[3, 2]]; eq5b // FS // 
      TF

Solve[eq5a,  Sin[θ5]] // FS // TF

Solve[eq5b,  Cos[θ5]] // FS // TF

So, θ5 = Atan2[(cos(θ3) (R21 cos(θ1) - R11 sin(θ1)) - (R11 cos(θ1) cos(θ2) + R21 sin(θ1) cos(θ2) + R31 sin(θ2)) sin(θ3)),
		 (cos(θ3) (R22 cos(θ1) - R12 sin(θ1)) - (R12 cos(θ1) cos(θ2) + R22 sin(θ1) cos(θ2) + R32 sin(θ2)) sin(θ3))]