05-Inverse manipulator

kinematics
Doan The Thao

1-1
 Direct Kinematics

Joint Space Task Space

dimensions n dimensions m

q   q1 q2 . . . qn  X  f (q) X   x1 x2 . . . xm 
T T

1-2
 Inverse Kinematics  q1 
q 
 2
.
q 
.
.
 
 qn 
B
W T  T(q1 , q2 ,..., qn )
0
n
X p 
or: X     f (q)
 Xr 

Inverse problem:
Given:
B
W T or: X find: q
1-3
 Inverse Kinematics
1
Finding: q f ( X)
or
Solving:
T(q1 , q2 ,..., qn )  T
0
n
B
W

12 equations 
 
 6 unknowns 

1-4
Multiplicity of Solutions

Selection of a solution
Criterion: Joint distance
 C1  q ( B )  q ( A)

Cw  min(C1 , C2 )
1

C2  q ( B2 )  q ( A)

Weighted Joint distance

moving smaller joints 1-5
 Number of Solutions
It depends on:
• Number of Joints
• Link Parameters
e.g. 6-revelute-joint manipulator
If all ai  0 Number solutions  16
If a1=a3=a5=0 Number solutions  4
• Range of Motion
Range of Motion is a measurement of
movement around a joint.
General Mechanism with 6 d.o.f
Number of solutions  16
1-6
 Methods of Solutions

In general the IK problem can be solved by various

methods such as:
• Dual Matrices (Denavit 1956)
• Iterative (Uicker 1964)
• Screw Algebra (Kohli 1975)
• Inverse Transform (Paul, 1981)
• Geometric approach (Lee 1984)
• Decoupling of position and orientation (Pieper 1968)

1-7
 Inverse Transform (Paul, 1981)
Elbow manipulator C1 0 S1 0
S 0 C1 0 
 1
1T 
0
Link ai i di i 0 1 0 0
1 0 /2 0 1(*)  
0 0 0 1
2 a2 0 0 2(*)
3 a3 0 0 3(*) C2  S2 0 a2C2 
S a2 S2 
1
T   2 C2 0
2
0 0 1 0 
 
0 0 0 1 
C3  S3 0 a3C3 
S a3 S3 
2
T   3 C3 0
3
0 0 1 0 
 
0 0 0 1 

C1C2C3  C1S 2 S3 C1C2 S3  C1S 2C3 S1 a 3C1C2 C3 -a 3C1S2S3  a 2 C1C2 

S C C S S S  S1C2 S3  S1S 2C3 C1 a 3S1C2 C3 -a 3S1S2S3  a 2S1C2 
0
T   1 2 3 1 2 3
3
 S 2C3  C2 S3  S 2 S3  C2C3 0 a 3S2 C3  a 3C2S3  a 2S2 
 
 0 0 0 1  1-8
 Inverse Transform (Paul, 1981)
IK: Elbow manipulator
C1C2C3  C1S2 S3 C1C2 S3  C1S 2C3 S1 a 3C1C2 C3 -a 3C1S2S3  a 2C1C2 
S C C S S S S1C2 S3  S1S 2C3 C1 a 3S1C2 C3 -a 3S1S2S3  a 2S1C2 
0
T   1 2 3 1 2 3
3
 S2C3  C2 S3  S2 S3  C2C3 0 a 3S2C3  a 3C2S3  a 2S2 
 
 0 0 0 1 

Given:  r11 r12 r13 Px 

r r r23 Py 
0
Tknown   21 22
3
 r31 r32 r33 Pz 
 
 0 0 0 1 

find:
q  1  2 3 
T

1-9
 Inverse Transform (Paul, 1981)
IK: Elbow manipulator
Solving for 1 ( 01T) 1 03T  12T 23
C1r11  S1r21 C1r12  S1r22 C1r13  S1r23 C1 Px  S1Py  C2C3  S 2 S3 C2 S3  S 2C3 0 a3C2C3  a3 S 2 S3  a2C2 
 r31 r32 r33 Pz  S C  C S  S 2 S3  C2C3 0 a3 S 2C3  a3C2 S3  a2C2 
  2 3 2 3

 S1r11  C1r21 S1r12  C1r22 S1r13  C1r23 (S1Px  C1Py )   0 0 1 (0) 

   
 0 0 0 1   0 0 0 1 

Equating the (3,4) elements from both sides of the equation have:

S1 Px - C1 Py = 0 To solve this equation, the trigonometric substitution

was made:

Px  R cos(); Py  R sin() where   A tan 2(Py , Px ); R  Px2  Py2

S1 cos( )  C1 sin( )  0; sin(1   )  0; cos(1   )  1

1  A tan 2( Py , Px )  A tan 2(0, 1)

1-10
 Inverse Transform (Paul, 1981)
IK: Elbow manipulator
Solving for 2 ( 12T) 1 ( 01T) 1 03Tknown  23T
 C2C1r11  C2 S1r21  S 2 r31 C2C1r12  C2 S1r22  S 2 r32 C2C1r13  C2 S1r23  S 2 r33 C2C1Px  C2 S1Py  S 2 Pz  a2 
S C r  S S r  C r  S 2C1r12  S 2 S1r22  C2 r32  S 2C1r13  S 2 S1r23  C2 r33 S 2C1Px  S 2 S1Py  C2 Pz 
 2 1 11 2 1 21 2 31

 S1r11  C1r21 S1r12  C1r22 S1r13  C1r23 S1 Px  C1 Py 
 
 0 0 0 1 
C3  S3 0 a3C3 
S
Equating the (1, 4) and (2,4) elements from both
C3 0 a3 S3 
 3 sides of the equation, we have:
0 0 1 0 
 
0 1 
C2C1 Px  C2 S1 Py  S 2 Pz  a2  a3C3
0 0


  S 2C1 Px  S 2 S1 Py  C2 Pz  a3 S3
To square these equations and add the resulting equations, we might have:

C12 Px2  S12 Py2  Pz2  2C1Px S1Py  a 22  a 32  2a 2 [S2 Pz  C 2 (C1Px  S1Py )]
1-11
 Inverse Transform (Paul, 1981)
IK: Elbow manipulator Solving for 2
C12 Px2  S12 Py2  Pz2  2C1 Px S1Py  a22  a32  2a2 [ S 2 Pz  C2 (C1Px  S1Py )]
C12 Px2  S12 Py2  Pz2  2C1 Px S1Py  a22  a32
S2 Pz  C2 (C1 Px  S1 Py ) 
2a2
AS 2  BC 2  K Where: A  Pz B  C1Px  S1Py

C12 Px2  S12 Py2  Pz2  2C1Px S1Py  a 22  a 32

K
2a 2
A  R ' cos(); B  R ' sin() where   A tan 2(B, A); R '  A 2  B 2
K
R' S2 cos()  R' sin()C 2  K  S 2 cos( )  sin( )C 2 
2
R'
K K K
sin( 2   )  ;  2    A tan 2( , 1  2 )
R' R' R'
K2
cos( 2  )   1  2  2  A tan 2( K , R ' 2  K 2 )  A tan 2( B, A )
R' 1-12
 Inverse Transform (Paul, 1981)
IK: Elbow manipulator Solving for 3
 C2C1r11  C2 S1r21  S2 r31 C2C1r12  C2 S1r22  S2 r32 C2C1r13  C2 S1r23  S2 r33 C2C1 Px  C2 S1 Py  S2 Pz  a2 
S C r  S S r  C r  S2C1r12  S2 S1r22  C2 r32 S2C1r13  S2 S1r23  C2 r33 S2C1Px  S 2 S1Py  C2 Pz 
 2 1 11 2 1 21 2 31

 S1r11  C1r21 S1r12  C1r22 S1r13  C1r23 S1 Px  C1 Py 
 
 0 0 0 1 
C3  S3 0 a3C3 
S C3 0 a3 S3 
 3
0 0 1 0  Equating the (1,4) and (2,4) elements from both sides of
  the equation have:
0 0 0 1 

 C2C1 Px  C2 S1 Py  S 2 Pz  a2
  C3
C2C1 Px  C2 S1 Py  S 2 Pz  a2  a3C3  a

3

  S 2C1 Px  S 2 S1 Py  C2 Pz  a3 S3   S 2C1 Px  S 2 S1 Py  C2 Pz  S
 a3
3

 S2C1 Px  S 2 S1 Py  C2 Pz C2C1 Px  C2 S1Py  S 2 Pz  a2

3  A tan 2( , )
a3 a3
1-13
 Inverse Transform (Paul, 1981)
IK: Elbow manipulator Check the simulation and
kinematics equations:

Kinematics
check for
a3=150mm

1-14
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

Given:  r11 r12 r13 xc  find:

r yc 
q  1  2 3 
 r22 r23 T

0 known 21
T
3
 r31 r32 r33 zc 
 
 0 0 0 1 

d1

1-15
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

a3 1  atan2( yc , xc )
a2 zc
𝜃2 𝜃3 Note that a second valid
solution for 1 is
s
z0 1    atan2( yc , xc ) (*)
y0
x0 we can apply the law of
r d1 yc
𝜃1 cosines to obtain

r 2  s 2  a22  a32  2a2 a3 cos(  3 )

xc
r 2  s 2  a22  a32 xc2  yc2  ( zc  d1 )2  a22  a32
cos 3   D
2a2 a3 2a2 a3

sin 3   1  D 2 ; 3  atan2( 1  D 2 , D) 1-16

 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

a a3
2 zc
𝜃2 𝜃3
s
z0
y0
x0
r d1 yc
𝜃1

xc
 2  atan2( s, r )  atan2(a3 sin3 , a2  a3cos3 )

 2  atan2( zc  d1 , xc2  yc2 )  atan2(a3 sin 3 , a2  a3 cos 3 )

1-17
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

if xc  yc  0
The manipulator is in a singular configuration

If there is an offset d0 as

shown in the figure then the
wrist center cannot intersect
z0.

Elbow manipulator
with shoulder offset 1-18
Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

PUMA manipulator

1-19
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

PUMA manipulator
We will have d2 = d. In general, there will be only two
solutions for 1. These correspond to the so-called left arm
and right arm configurations.

Right arm configuration 1-20

Left arm configuration
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

Left arm configurations. PUMA manipulator

1    
1  atan2( yc , xc ) 
 atan2(d , r 2  d 2 )

1  atan2( yc , xc ) 
 atan2(d , xc2  yc2  d 2 )

1-21
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

Right arm configurations. PUMA manipulator

1      atan2( yc , xc )  
      atan2(d , r 2  d 2 )  
sin(   )   sin  ; cos(   )   cos 
  atan2(d ,  r 2  d 2 )

1    
1  atan2( yc , xc )  atan2(d ,  r  d ) 2 2

1-22
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

PUMA manipulator
a2 zc
𝜃2 a3
𝜃3
s
a
d1 z0 d
d1
r
xc yc

x0
a 2  s 2  a22  a32  2a2 a3 cos(  3 )
xc2  yc2  d 2  ( zc  d1 ) 2  a22  a32  2a2 a3 cos 3
1-23
Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

PUMA manipulator
a2 zc
𝜃2 a3
𝜃3
s
a
d1 z0 d
d1
r
xc yc

x0 xc2  yc2  d 2  ( zc  d1 ) 2  a22  a32

cos 3  D
2a2 a3
sin 3   1  D 2 ; 3  a tan 2( 1  D 2 , D) 1-24
 Geometric approach (Lee 1984)
IK: Elbow manipulator (RRR) (Articulated Configuration)

PUMA manipulator
a2 zc
𝜃2 a3
𝜃3
s
a
d1 z0 d
d1
r
xc yc

x0
 2  a tan 2( s, a)  a tan 2(a3 s3 , a2  a3c3 )

 2  a tan 2( zc  d1 , xc2  yc2  d 2 )  a tan 2(a3 s3 , a2  a3c3 )

1-25
 Geometric approach (Lee 1984)
IK: Spherical Manipulator (RRP)
Given:  r11 r12 r13 xc  find:
r r r y 

q  1  2 d3 
0 known
  21 22 23 c
T
3T
 r31 r32 r33 zc 
 
 0 0 0 1 

Spherical Serial Manipulator

pdfs.semanticscholar.org 1-26
 Geometric approach (Lee 1984)
IK: Spherical Manipulator (RRP)
1  atan2( yc , xc ) Or:
1    atan2( yc , xc )
 2  a tan 2( s, r )

 2  a tan 2( zc  d1 , xc2  yc2 )

d3  s 2  r 2

d3  ( zc  d1 ) 2  xc2  yc2

• If both xc and yc are zero, the configuration is singular

• If there is an offset then there will be left and right
arm configurations as in the case of the elbow
manipulator 1-27
 Geometric approach (Lee 1984)
IK: Cylindrical Robot (RPP)
Given:  r11 r12 r13 xc  find:
r r r y 

q   d3 
0 known
  21 22 23 c
T
3T
 r31 r32 r33 zc  d2
  1
 0 0 0 1 

1-28
 Geometric approach (Lee 1984)
IK: Cylindrical Robot (RPP)

yc

1 d3
xc

x0

d1 1  a tan 2( yc , xc )
d3  xc2  yc2
d 2  zc  d1
1-29
 Geometric approach (Lee 1984)
IK: Scara Robot (RRPR)
Given:  r11 r12 r13 xc 
r r r23 yc 
0
Tknown   21 22
4
 r31 r32 r33 zc  z0
  z1
 0 0 0 1 
find: z2
a1
a2

q  1  2 d3  4 
T z3

d4
z4

1-30
 Geometric approach (Lee 1984)
IK: Scara Robot (RRPR)
r 2  a12  a22  2a1a2c 2 ;
xc2  yc2  a12  a22
c 2  D
z1
2a1a2
s 2   1  D 2
z0
z2

z3
 2  a tan 2( 1  D 2 , D)

1    
yc z4

β  a1
a2 1  a tan 2( yc , xc ) 
1
2  a tan 2(a2 s 2 , a1  a2c 2 )
xc
1-31
 Geometric approach (Lee 1984)
IK: Scara Robot (RRPR)
zc   d 3  d 4
z1
d3*
d 3   zc  d 4
z0
z2
c12 c4  s12 s4 s12c4  c12 s4 0 a1c1  a2c12 
d4 s c  c s
 12 4 12 4 c12c4  s12 s4 0 a1s1  a2 s12 
4T  
0
z3
 0 0 1 d3  d 4 
 
d1  0 0 0 1 
yc z4
c s 0 0   a tan 2(r12 , r11 )
 s c 0 0 
r 4T 
0 
0 0 1 0
  1   2   4
a2  
β  a1
 0 0 0 1 
1
2

xc

(*)
 4  1   2  a tan 2(r12 , r11 )
d3   zc  d 4  d1 1-32
 Inverse Orientation
Spherical Wrist: Z-Y-Z Euler angles
• The spherical wrist simplify the robot
kinematics. It allow the decoupling of the
position and orientation in the Inverse Kinematic
analysis.
 c4 c5c6  s4 s6 c4 c5 s6  s4 c6 c4 s5 c4 s5 d 6 
s c c  c s  s4 c5 s6  c4 c6 s4 s5 s4 s5 d 6 
3
T   4 5 6 4 6
6
  s5c6 s5 s6 c5 c5 d 6 
 
 0 0 0 1 

c c c  s s c c s  s c c s 
A
R ( ,  ,  )   s c c  c s  s c s  c c s s 
B Z 'Y ' Z ' 
  s c s s c 
1-33
 Inverse Orientation
Spherical Wrist:

• The rotation matrix obtained for the spherical

wrist has the same form as the rotation matrix
for the Euler angles.
• Therefore, we can use the method developed
in the inverse section of Z-Y-Z Euler angles
rotation matrix to solve for the three joint
angles of the spherical wrist.
• In particular, we solve for the three Euler
angles,, β, γ and then use the mapping:

 4   ; 5   ;  6  
1-34
 Kinematic Decoupling:
If the manipulator has:
• Six joints (DOF = 6).
• The last 3 joint axes intersecting in one point
(Spherical Wrist).
then the problem is decoupled into two sub-
problems:
• Inverse position kinematics.
• Inverse orientation kinematics

1-35
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist

O0c
O06
d1
Given the desired
homogeneous transformation T
Link ai i di i
 r '11 r '12 r '13 Ox 
1 0 /2 d1 1(*)  r '23 Oy 
 R O   r '21 r '22
T   r '
2 a2 0 0 2(*)
 0 1   31 r '32 r '33 Oz 

3 a3 0 0 3(*)  0 0 0 1 

solve for q1, . . . , q6.

1-36
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist

• Inverse Position: Solve the IK for the position of Oc

.
3
• Inverse Rotation: Solve the IK for the orientation 6 R

O 0c 0
O 6

0 3
How to compute O c and 6 R? 1-37
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
0
Calculation of the wrist center O c
The position of the end-effector
center, O6, is obtained by a
translation of distance d6 along z5
O0c
O06 from Oc. Since z5 and z6 are on the
same axis, we can choose the third
R O 
column of the desired rotation 06 R  R
T  as the direction of z6 and z5 w.r.t.
 0 1  base frame.

 xc   Ox  d 6 r '13 
O  O 0c  d 6 0 z 5  O 0c   yc   Oy  d 6 r '23 
 zc   Oz  d 6 r '33 
1-38
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
3
Calculation of the orientation matrix 6 R

Determine the orientation of the end-effector

relative to the frame O3x3y3z3 from the expression

1
R  R R  R  ( R) R  ( R) R
0
3
3
6
3
6
0
3
0
3
T

0
R is given and R can be calculated once
3
the first three joint variables are known.

1-39
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Position Problem

1  atan2( yc , xc )

xc2  yc2  ( zc  d1 ) 2  a22  a32

D
2a2 a3

sin 3   1  D2 ; 3  atan2( 1  D2 , D)

 2  atan2( zc  d1 , xc2  yc2 )  atan2(a3 sin 3 , a2  a3 cos 3 )

1-40
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Orientation Problem C1 0 S1 0  C2  S2 0 a2C2 
S S 0 a2 S2 
 1 0 C1 0  1
  2 C2
1T 
0 T
Link ai i di i 0
2
0 0 
1 0 d1  
0 1

1 0 /2 d1 1(*)   0 0 0 1 
0 0 0 1
2 a2 0 0 2(*)
C3  S3 0 a3C3 
3 a3 0 0 3(*) S C3 0 a3 S3 
 3
3T 
2
0 0 1 0 
 
0 0 0 1 

c23c1  s23c1 s1 c1 (a3c23  a2c2 ) 

c s  s23 s1 c1 s1 (a3c23  a2 c2 ) 
0
T   23 1
C1C2C3  C1S2 S3 C1C2 S3  C1S 2C3 S1 a 3C1C2 C3 -a 3C1S2S3  a 2C1C2 
3
 s23 c23 0 d1  a3 s23  a2 s2 
 S C C  S S S S C S  S S C C a S C C -a S S S  a S C 
 1 2 3 1 2 3 2 1 2   
 0 0 0 1 

0 1 2 3 1 2 3 1 3 1 2 3 3 1 2 3
T
3
 S2C3  C2 S3  S2 S3  C2C3 0 a 3S2C3  a 3C2S3  a 2S2 
 
 0 0 0 1 

T03 =[ cos(q2 + q3)*cos(q1), -sin(q2 + q3)*cos(q1), sin(q1), cos(q1)*(a3*cos(q2 + q3) + a2*cos(q2))]

[ cos(q2 + q3)*sin(q1), -sin(q2 + q3)*sin(q1), -cos(q1), sin(q1)*(a3*cos(q2 + q3) + a2*cos(q2))]
[ sin(q2 + q3), cos(q2 + q3), 0, d1 + a3*sin(q2 + q3) + a2*sin(q2)] 1-41
[ 0, 0, 0, 1]
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Orientation Problem
c23c1  s23c1 s1 c1 (a3c23  a2c2 )  c23c1  s23c1 s1 
c s s1 (a3c23  a2c2 )  c s
0
T   23 1  s23 s1 c1 0
3 R   23 1  s23 s1 c1 
3
 s23 c23 0 d1  a3 s23  a2 s2 
   s23 c23 0 
 0 0 0 1 
Spherical Wrist:  c4 c5c6  s4 s6 c4c5 s6  s4c6 c4 s5 c4 s5d 6 
s c c  c s  s4c5 s6  c4c6 s4 s5 s4 s5 d 6 

6T 
3 4 5 6 4 6

  s5c6 s5 s6 c5 c5 d 6 
 
 0 0 0 1 

 c4 c5c6  s4 s6 c4 c5 s6  s4c6 c4 s5 

3
R  s c c  c s  s4 c5 s6  c4 c6 s4 s5 
6  4 5 6 4 6
  s5c6 s5 s6 c5  1-42
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Orientation Problem
3
6 R  ( 03 R )T R
 c4 c5c6  s4 s6 c4c5 s6  s4c6  s23c1
T
c4 s5  c23c1 s1   r '11 r '12 r '13 
s c c  c s  s4 c5 s6  c4c6 s4 s5   c23 s1  s23 s1 c1  r ' r '22 r '23 
 4 5 6 4 6  21
  s5c6 s5 s6 c5   s23 c23 0   r '31 r '32 r '33 
 c4 c5c6  s4 s6 c4c5 s6  s4c6 c4 s5   c23c1 c23 s1 s23   r '11 r '12 r '13 
s c c  c s  s4 c5 s6  c4 c6 s4 s5     s23c1  s23 s1 c23   r '21 r '22 r '23 
 4 5 6 4 6
  s5c6 s5 s6 c5   s1 c1 0   r '31 r '32 r '33 
 c4 c5c6  s4 s6 c4c5 s6  s4c6 c4 s5   r11 r12 r13 
s c c  c s  s4 c5 s6  c4c6 s4 s5    r21 r22 r23 
 4 5 6 4 6
  s5c6 s5 s6 c5   r31 r32 r33 
The Euler angle solution can be applied to this equation 1-43
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Orientation Problem  4   ; 5   ;  6  

  atan2( r312  r322 , r33 )  23 1

c c c23 s1 s23   r '11 r '12 r '13 
s c  s23 s1 c23   r '21 r '22 r '23 
 23 1
 s1 c1 0   r '31 r '32 r '33 

r31  s1r '11  c1r '21 ; r32  s1r '12  c1r '22 ; r33  s1r '13  c1r '23 ;
r13  c23c1r '13  c23 s1r '23  s23r '33
r23   s23c1r '13   s23 s1r '23  c23r '33

5  a tan 2( ( s1r '11  c1r '21 ) 2  ( s1r '12  c1r '22 ) 2 , s1r '13  c1r '23 )
1-44
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Orientation Problem
If s   0   atan2(r23 / s  , r13 / s  )
  atan2(r32 / s  , r31 / s  )
r31  s1r '11  c1r '21 ; r32  s1r '12  c1r '22 ; r33  s1r '13  c1r '23 ;
r13  c23c1r '13  c23 s1r '23  s23r '33
r23   s23c1r '13   s23 s1r '23  c23r '33
 s23c1r '13   s23 s1r '23  c23r '33 c23c1r '13  c23 s1r '23  s23r '33
 4  a tan 2( , )
s5 s5
s1r '12  c1r '22 s1r '11  c1r '21
 6  a tan 2( , )
s5 s5
1-45
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Orientation Problem
 0
If s5  0; 5  0 or 5  180 o

  atan2(r12 , r11 );   0o
5  0 ; s5  0; c5  1
o

One possible convention is to choose 4  0

  atan2(r12 , r11 );   180o  c23c1 c23 s1 s23   r '11 r '12 r '13 
s c  s23 s1 c23   r '21 r '22 r '23 
 23 1
r11  c23c1r '11  c23 s1r '21  s23r '31  s1 c1 0   r '31 r '32 r '33 
r12  c23c1r '12  c23 s1r '22  s23r '32

6  a tan 2((c23c1r '12  c23 s1r '22  s23r '32 ), (c23c1r '11  c23 s1r '21  s23r '31 ))
1-46
 Kinematic Decoupling:
IK: Elbow manipulator (RRR) with Spherical Wrist
Inverse Orientation Problem
 0
If s5  0; 5  0 or 5  180 o
  atan2(r12 , r11 );   180o
5  180 ; s5  0; c5  1
o

One possible convention is to choose 4  0

  atan2(r12 , r11 );   180o  c23c1 c23 s1 s23   r '11 r '12 r '13 
s c  s23 s1 c23   r '21 r '22 r '23 
 23 1
r11  c23c1r '11  c23 s1r '21  s23r '31  s1 c1 0   r '31 r '32 r '33 
r12  c23c1r '12  c23 s1r '22  s23r '32

6  a tan 2(c23c1r '12  c23 s1r '22  s23r '32 , (c23c1r '11  c23 s1r '21  s23r '31 ))
1-47