Ecole Nationale des sciences Appliquées de Fès

Département Génie Industriel

Robotics

Chapter 3
Forward and Inverse Kinematics
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

1. Forward kinematics introduction

• Challenge: given all the joint parameters of a manipulator, determine

the position and orientation of the tool frame
– Tool frame: coordinate frame attached to the most distal link of the
manipulator
– Inertial frame: fixed (immobile) coordinate system fixed to the most proximal
link of a manipulator
• Therefore, we want a mapping between the tool frame and the inertial
frame
– This will be a function of all joint parameters and the physical geometry of
the manipulator
• Purely geometric: we do not worry about joint torques or dynamics
– (yet!)
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

2. Convention
• A n-DOF manipulator will have n joints (either revolute or prismatic) and
n+1 links (since each joint connects two links)
– We assume that each joint only has one DOF. Although this may seem like
it does not include things like spherical or universal joints, we can think of
multi-DOF joints as a combination of 1DOF joints with zero length between
them
– The o0 frame is the inertial frame
– on is the tool frame
– Joint i connects links i-1 and i
– The oi is connected to link i
• Joint variables, qi
 if joint i is revolute
qi =  i
d i if joint i is prismatic
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

2. Convention
• We said that a homogeneous transformation allowed us to express the
position and orientation of oj with respect to oi
– what we want is the position and orientation of the tool frame with respect to
the inertial frame
– An intermediate step is to determine the transformation matrix that gives
position and orientation of oi with respect to oi-1: Ai
– Now we can define the transformation oj to oi as:

 Ai +1 Ai + 2 ... A j −i A j if i  j

T ji =  I if i = j

 ( )
T j
i −1
if j  i
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

2. Convention
• Finally, the position and orientation of the tool frame with respect to the
inertial frame is given by one homogeneous transformation matrix:
– For a n-DOF manipulator

Rn0 on0 
H=  = Tn = A1(q1 )A2 (q2 )    An (qn )
0

0 1
• Thus, to fully define the forward kinematics for any serial manipulator,
all we need to do is create the Ai transformations and perform matrix
multiplication

• But there are shortcuts…

 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

3. The Denavit-Hartenberg (DH) Convention

• Representing each individual homogeneous transformation as the
product of four basic transformations:
Ai = Rot z, i Trans z,d i Trans x,ai Rot x, i
c i − s i 0 0  1 0 0 0  1 0 0 ai  1 0 0 0
s c i 0 0 0 1 0 0  0 1 0 0  0 c i − s i 0
=  i
0 0 1 0 0 0 1 d i  0 0 1 0  0 s i c i 0
    
0 0 0 1 0 0 0 1  0 0 0 1  0 0 0 1
c i − s i c i s i s i ai c i 
s c i c i − c i s i ai s i 
=  i
0 s i c i di 
 
0 0 0 1 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

3. The Denavit-Hartenberg (DH) Convention

• Four DH parameters:
– ai: link length
– i: link twist
– di: link offset
– i: joint angle
• Since each Ai is a function of only one variable, three of these will be
constant for each link
– di will be variable for prismatic joints and i will be variable for revolute joints

• But we said any rigid body needs 6 parameters to describe its position
and orientation
– Three angles (Euler angles, for example) and a 3x1 position vector
– So how can there be just 4 DH parameters?...
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

4. Existence and uniqueness

• When can we represent a homogeneous transformation using the 4 DH
parameters?
• For example, consider two coordinate frames o0 and o1
– There is a unique homogeneous transformation between these two frames
• Now assume that the following holds:
1. DH1: xˆ1 ⊥ zˆ0
2. DH2: xˆ1  zˆ0
• If these hold, we claim that there
exists a unique transformation A:

A = Rot z, Trans z,d Trans x,aRot x,

R10 o10 
= 
0 1
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

4. Existence and uniqueness

• Proof:
1. We assume that R10 has the form: R1 = Rz, R x ,
0

2. Use DH1 to verify the form of R10

xˆ1 ⊥ zˆ0  x10  z00 = 0
r11 r12 r13 
T
r11  0
 r21  0 = r = 0 R10 = r21 r22 r23 
  31
r31  1  0 r32 r33 
r11 + r21 = 1
2 2
– Since the rows and columns of R10
must be unit vectors:
r32 + r33 = 1
2 2
– The remainder of R10 follows from the properties of
rotation matrices
– Therefore our assumption that there exists a unique 
and  that will give us R10 is correct given DH1
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

4. Existence and uniqueness

• Proof:
1. Use DH2 to determine the form of o10
– Since the two axes intersect, we can represent the line between the two
frames as a linear combination of the two axes (within the plane formed by
x1 and z0)

xˆ1  zˆ0  o10 = dz00 + ax10

0 c  ac 
 o10 = d 0 + a s  = as 
1  0   d 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

5. Physical basis for DH parameters

• ai: link length, distance between the z0 and z1 (along x1)
• i: link twist, angle between z0 and z1 (measured around x1)
• di: link offset, distance between o0 and intersection of z0 and x1 (along z0)
• i: joint angle, angle between x0 and x1 (measured around z0)

positive convention:
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

6. Assigning coordinate frames

• For any n-link manipulator, we can always choose coordinate frames
such that DH1 and DH2 are satisfied
– The choice is not unique, but the end result will always be the same
1. Choose zi as axis of rotation for joint i+1
– z0 is axis of rotation for joint 1, z1 is axis of rotation for joint 2, etc
– If joint i+1 is revolute, zi is the axis of rotation of joint i+1
– If joint i+1 is prismatic, zi is the axis of translation for joint i+1
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

6. Assigning coordinate frames

2. Assign base frame
– Can be any point along z0
3. Chose x0, y0 to follow the right-handed convention
4. Now start an iterative process to define frame i with respect to frame i-1
– Consider three cases for the relationship of zi-1 and zi:
i. zi-1 and zi are non-coplanar
ii. zi-1 and zi intersect
zi-1 and zi are coplanar
iii. zi-1 and zi are parallel
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

6. Assigning coordinate frames

i. zi-1 and zi are non-coplanar
– There is a unique shortest distance between the two axes
– Choose this line segment to be xi
• oi is at the intersection of zi and xi
– Choose yi by right-handed convention
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

6. Assigning coordinate frames

ii. zi-1 and zi intersect
– Choose xi to be normal to the plane defined by zi and zi-1
• oi is at the intersection of zi and xi
– Choose yi by right-handed convention
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

6. Assigning coordinate frames

iii. zi-1 and zi are parallel
– Infinitely many normals of equal length between zi and zi-1
– Free to choose oi anywhere along zi, however if we choose xi to be along
the normal that intersects at oi-1, the resulting di will be zero
– Choose yi by right-handed convention
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

7. Assigning tool frame

• The previous assignments are valid up to frame n-1
– The tool frame assignment is most often defined by the axes n, s, a:
• a is the approach direction
• s is the ‘sliding’ direction (direction along which the grippers open/close)
• n is the normal direction to a and s
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
1: two-link planar manipulator
• 2DOF: need to assign three coordinate frames
1. Choose z0 axis (axis of rotation for joint 1, base frame)
2. Choose z1 axis (axis of rotation for joint 2)
3. Choose z2 axis (tool frame)
• This is arbitrary for this case since we have described no wrist/gripper
• Instead, define z2 as parallel to z1 and z0 (for consistency)
4. Choose xi axes
• All zi’s are parallel
• Therefore choose xi to intersect oi-1
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
1: two-link planar manipulator
• Now define DH parameters
– First, define the constant parameters ai, i
– Second, define the variable parameters i, di
link ai  i di i
1 a1 0 0 1
2 a2 0 0 2
– The i terms are 0 because all zi are parallel
– Therefore only i are variable

c1 − s1 0 a1c1  c2 − s2 0 a2c 2  T10 = A1

s c1 0 a1s1  s c2 0 a2s2  c12 − s12 0 a1c1 + a2c12 
A1 =  1 , A2 =  2 s
0 0 1 0  0 0 1 0  c12 0 a1s1 + a2s12 
    T2 = A1A2 =  12
0

0 0 0 1  0 0 0 1  0 0 1 0 
 
0 0 0 1 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
2: three-link cylindrical robot
• 3DOF: need to assign four coordinate frames
1. Choose z0 axis (axis of rotation for joint 1, base frame)
2. Choose z1 axis (axis of translation for joint 2)
3. Choose z2 axis (axis of translation for joint 3)
4. Choose z3 axis (tool frame)
• This is again arbitrary for this case since we have described no wrist/gripper
• Instead, define z3 as parallel to z2
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
2: three-link cylindrical robot
• Now define DH parameters link ai i di i
– First, define the constant parameters ai, i
1 0 0 d1 1
– Second, define the variable parameters i, di
2 0 -90 d2 0
c1 − s1 0 0 1 0 0 0 1 0 0 0 3 0 0 d3 0
s c1 0 0  0 0 1 0  0 1 0 0 
A1 =  1 , A2 =  , A3 = 
0 0 1 d1  0 − 1 0 d2  0 0 1 d3 
     
0 0 0 1 0 0 0 1 0 0 0 1

c1 0 − s1 − s1d 3 
s 0 c1 c1d 3 
T3 = A1A2 A3 =
0  1

 0 − 1 0 d1 + d 2 
 
0 0 0 1 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
3: spherical wrist
• 3DOF: need to assign four coordinate frames
– yaw, pitch, roll (4, 5, 6) all intersecting at one point o (wrist center)
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
3: spherical wrist
• Now define DH parameters link ai i di i
– First, define the constant parameters ai, i
4 0 -90 0 4
– Second, define the variable parameters i, di
5 0 90 0 5
c 4 0 − s4 0 c5 0 − s5 0 c6 − s6 0 0 6 0 0 d6 6
s 0 c4 0 s 0 c5 0 s c6 0 0 
A1 =  4 , A2 =  5 , A3 =  6
0 −1 0 0 0 −1 0 0 0 0 1 d6 
     
0 0 0 1 0 0 0 1 0 0 0 1

c 4c5c 6 − s4s6 − c 4c 5 s6 − s 4c 6 c 4 s5 c 4 s5 d 6 
s c c + c s − s 4 c 5 s6 + c 4 c 6 s 4 s5 s4s5d 6 
T6 = A4 A5 A6 =  4 5 6
3 4 6

 − s5 c 6 s5 c 6 c5 c5d 6 
 
 0 0 0 1 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
4: cylindrical robot with spherical wrist
• 6DOF: need to assign seven coordinate frames
– But we already did this for the previous two examples, so we can fill in the
table of DH parameters:

link ai i di i
1 0 0 d1 1
2 0 -90 d2 0
3 0 0 d3 0
4 0 -90 0 4
o3, o4, o5 are all at
the same point oc 5 0 90 0 5
6 0 0 d6 6
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
4: cylindrical robot with spherical wrist
• Note that z3 (axis for joint 4) is collinear with z2 (axis for joint 3), thus we
can make the following combination:
r11 r12 r13 dx  r11 = c1c 4c5c 6 − c1s4s6 + s1s5c 6
r r r23 d y  r21 = s1c 4c5c 6 − s1s4s6 − c1s5c 6
T6 = T3 T6 =  21 22
0 0 3
r31 = −s4c5c 6 − c 4s6
r31 r32 r33 dz 
  r12 = −c1c 4c5s6 − c1s4c 6 − s1s5c 6
0 0 0 1
r22 = −s1c 4c5s6 − s1s4s6 + c1s5c 6
r32 = s4c5c 6 − c 4c 6
r13 = c1c 4s5 − s1c5
r23 = s1c 4s5 + c1c5
r33 = −s4s5
d x = c1c 4s5d 6 − s1c5d 6 − s1d 3
d y = s1c 4s5d 6 + c1c5d 6 + c1d 3
d z = −s4s5d 6 + d1 + d 2
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
5: the Stanford manipulator
• 6DOF: need to assign seven coordinate frames:
1. Choose z0 axis (axis of rotation for joint 1, base frame)
2. Choose z1-z5 axes (axes of rotation/translation for joints 2-6)
3. Choose xi axes
4. Choose tool frame
5. Fill in table of DH parameters:

link ai i di i
1 0 -90 0 1
2 0 90 d2 2
3 0 0 d3 0
4 0 -90 0 4
5 0 90 0 5
6 0 0 d6 6
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
5: the Stanford manipulator
• Now determine the individual homogeneous transformations:

c1 0 − s1 0 c 2 0 s2 0 1 0 0 0
s 0 c1 0 s 0 − c20  0 1 0 0 
A1 =  1 , A2 =  2 , A3 = 
0 −1 0 0 0 1 0 d2  0 0 1 d3 
     
0 0 0 1 0 0 0 1 0 0 0 1
c 4 0 − s 4 0 c5 0 s5 0  c 6 − s6 0 0
s 0 c 4 0 s 0 − c5 0 s c6 0 0 
A4 =  4 , A5 =  5 , A6 =  6
0 − 1 0 0 0 − 1 0 0 0 0 1 d6 
     
0 0 0 1 0 0 0 1 0 0 0 1
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
5: the Stanford manipulator
• Finally, combine to give the complete description of the forward
kinematics:
r11 = c1c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6  − d 2 (s4c5c 6 + c 4s6 )
r21 = s1c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6  + c1 (s4c5c6 + c 4s6 )
r31 = −s2 (c 4c5c 6 − s4s6 ) − c 2s5c 6
r12 = c1− c 2 (c 4c5s6 + s4c 6 ) + s2s5s6  − s1(− s4c5s6 + c 4c 6 )
r11 r12 r13 dx  r22 = −s1− c 2 (c 4c5s6 − s4c 6 ) − s2s5s6  + c1 (− s4c5s6 + c 4s6 )
r d y 
r r23 r32 = s2 (c 4c5s6 + s4c6 ) + c 2s5s6
T6 = A1    A6 =  21 22
0
r31 r32 r33 dz  r13 = c1 (c 2c 4s5 + s2c5 ) − s1s4s5
 
0 0 0 1 r23 = s1 (c 2c 4s5 + s2c5 ) + c1s4s5
r33 = −s2c 4s5 + c 2c5
d x = c1s2d 3 − s1d 2 + d 6 (c1c 2c 4s5 + c1c5s2 − s1s4s5 )
d y = s1s2d 3 + c1d 2 + d 6 (c1s4s5 + c 2c 4s1s5 + c5s1s2 )
d z = c 2d 3 + d 6 (c 2c5 − c 4s2s5 )
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
6: the SCARA manipulator
• 4DOF: need to assign five coordinate frames:
1. Choose z0 axis (axis of rotation for joint 1, base frame)
2. Choose z1-z3 axes (axes of rotation/translation for joints 2-4)
3. Choose xi axes
4. Choose tool frame
5. Fill in table of DH parameters:

link ai i di i
1 a1 0 0 1
2 a2 180 0 2
3 0 0 d3 0
4 0 0 d4 4
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

8. Examples
6: the SCARA manipulator
• Now determine the individual homogeneous transformations:

c1 − s1 0 a1c1  c2 s2 0 a2c 2  1 0 0 0 c 4 − s4 0 0

s c1 0 a1s1  s − c2 0 a2s2  0 1 0 0  s c4 0 0 
A1 =  1 , A2 =  2 , A3 =  , A4 =  4
0 0 1 0  0 0 −1 0  0 0 1 d3  0 0 1 d4 
       
0 0 0 1  0 0 0 1  0 0 0 1 0 0 0 1

c12c 4 + s12s4 − c12s4 + s12c 4 0 a1c1 + a2c12 

s c − c s − s12s4 − c12c 4 0 a1s1 + a2s12 
T4 = A1    A4 =  12 4
0 12 4

 0 0 − 1 − d3 − d 4 
 
 0 0 0 1 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

9. Forward kinematics of parallel

manipulators

• Parallel manipulator: two or more series chains connect the end-

effector to the base (closed-chain)
• # of DOF for a parallel manipulator determined by taking the total DOFs
for all links and subtracting the number of constraints imposed by the
closed-chain configuration
• Gruebler’s formula (3D):
nj
# DOF = 6(nL − n j ) +  fi
i =1 #DOF for joint i

number of links* number of joints

*excluding ground
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

9. Forward kinematics of parallel

manipulators
• Gruebler’s formula (2D): nj
# DOF = 3(nL − n j ) +  fi
i =1
• Example (2D):
– Planar four-bar, nL = 3, nj = 4, fi = 1(for all joints)
• 3(3-4)+4 = 1DOF
– Forward kinematics:

  2 − 2 + 2L 2 
 = cos −1 2  + tan −1 L2  
 −
  L −
 2 (L1 −  ) + L2  1  2
2 2

 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

10. Inverse Kinematics

• Find the values of joint parameters that will put the tool frame at a
desired position and orientation (within the workspace)
– Given H:
R o 
H=   SE (3 )
 0 1

– Find all solutions to:

Tn0 (q1,..., qn ) = H
– Noting that:
Tn0 (q1,..., qn ) = A1(q1 )    An (qn )
– This gives 12 (nontrivial) equations with n unknowns
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

10. Inverse Kinematics

Example: the Stanford manipulator
• For a given H: c1c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6  − d 2 (s4c5c 6 + c 4s6 ) = 0
s1c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6  + c1 (s4c5c 6 + c 4s6 ) = 0
0 1 0 − 0.154
0 − s2 (c 4c5c 6 − s4s6 ) − c 2s5c 6 = 1
0 1 0.763 
H= c1− c 2 (c 4c5s6 + s4c 6 ) + s2s5s6  − s1 (− s4c5s6 + c 4c 6 ) = 1
1 0 0 0 
  − s1− c 2 (c 4c5s6 − s4c 6 ) − s2s5s6  + c1 (− s4c5s6 + c 4s6 ) = 0
0 0 0 1 
s2 (c 4c5s6 + s4c 6 ) + c 2s5s6 = 0
• Find 1, 2, d3, 4, 5, 6: c1(c 2c 4s5 + s2c5 ) − s1s4s5 = 0
s1 (c 2c 4s5 + s2c5 ) + c1s4s5 = 1
− s 2 c 4 s5 + c 2 c 5 = 0
c1s2d 3 − s1d 2 + d 6 (c1c 2c 4s5 + c1c5s2 − s1s4s5 ) = −0.154
s1s2d 3 + c1d 2 + d 6 (c1s4s5 + c 2c 4s1s5 + c5s1s2 ) = 0.763
c 2d 3 + d 6 (c 2c5 − c 4s2s5 ) = 0

• One solution: 1 = /2, 2 = /2, d3 = 0.5, 4 = /2, 5 = 0, 6 = /2

 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

10. Inverse Kinematics

• The previous example shows how difficult it would be to obtain a

closed-form solution to the 12 equations
• Instead, we develop systematic methods based upon the manipulator
configuration
• For the forward kinematics there is always a unique solution
– Potentially complex nonlinear functions
• The inverse kinematics may or may not have a solution
– Solutions may or may not be unique
– Solutions may violate joint limits
• Closed-form solutions are ideal!
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

11. Overview: kinematic decoupling

• Appropriate for systems that have an arm a wrist

– Such that the wrist joint axes are aligned at a point
• For such systems, we can split the inverse kinematics problem into two
parts:
1. Inverse position kinematics: position of the wrist center
2. Inverse orientation kinematics: orientation of the wrist
• First, assume 6DOF, the last three intersecting at oc
R60 (q1,..., q6 ) = R
o60 (q1,..., q6 ) = o

• Use the position of the wrist center to determine the first three joint
angles…
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

11. Overview: kinematic decoupling

• Now, origin of tool frame, o6, is a distance d6 translated along z5 (since

z5 and z6 are collinear)
– Thus, the third column of R is the direction of z6 (w/ respect to the base
frame) and we can write: 0
o = o60 = oco + d 6R 0
 1
– Rearranging: 0
oco = o − d 6R 0
 1
– Calling o = [ox oy oz]T, oc0 = [xc yc zc]T

 xc  o x − d 6 r13 
 y  = o − d r 
 c  y 6 23 

 zc  oz − d 6 r33 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

11. Overview: kinematic decoupling

• Since [xc yc zc]T are determined from the first three joint angles, our
forward kinematics expression now allows us to solve for the first three
joint angles decoupled from the final three.
– Thus we now have R30
– Note that:
R = R30R63

– To solve for the final three joint angles:

( )
R63 = R30
−1
( )T
R = R30 R

– Since the last three joints for a

spherical wrist, we can use a set of
Euler angles to solve for them
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

12. Inverse position

• Now that we have [xc yc zc]T we need to find q1, q2, q3

– Solve for qi by projecting onto the xi-1, yi-1 plane, solve trig problem
– Two examples: elbow (RRR) and spherical (RRP) manipulators
– For example, for an elbow manipulator, to solve for 1, project the arm onto
the x0, y0 plane
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Background: two argument atan

• We use atan2(·) instead of atan(·) to account for the full range of

angular solutions
– Called ‘four-quadrant’ arctan

− atan2(− y , x ) y 0
  y
 − atan −  y  0, x  0
  x

atan2(y , x ) =  atan y  y  0, x  0
 x
 
 y  0, x = 0
2
 undefined y = 0, x = 0

 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: RRR manipulator

1. To solve for 1, project the arm onto the x0, y0 plane
1 = atan2(xc , y c )

– Can also have: 1 =  + atan2(xc , y c )

• This will of course change the solutions for 2 and 3
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Caveats: singular configurations, offsets

• If xc=yc=0, 1 is undefined • If there is an offset, then we will

– i.e. any value of 1 will work have two solutions for 1: left arm
and right arm
– However, wrist centers cannot
intersect z0
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Left arm and right arm solutions

• Left arm: • Right arm:

1 =  + 
1 =  - 
 = atan2(xc , y c )
 = atan2(xc , y c )
 =  + atan2 xc + y c − d 2 , d 
2 2

 = atan2 xc + y c − d 2 , d   
2 2

 
= atan2 − xc + y c − d 2 ,−d 
2 2

 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Left arm and right arm solutions

• Therefore there are in general two solutions for 1

• Finding 2 and 3 is identical to the planar two-link manipulator we have
seen previously:
r 2 + s 2 − a2 − a3
2 2
cos 3 =
2a2a3
r 2 = xc + y c − d 2
2 2

s = zc − d1
xc + y c − d 2 + (zc − d1 ) − a2 − a3
2 2 2 2 2
 cos 3 = D
2a2a3

• Therefore we can find two solutions for 3:

(
 3 = atan2 D, 1 − D 2 )
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Left arm and right arm solutions

• The two solutions for 3 correspond to the elbow-down and elbow-up

positions respectively
• Now solve for 2:

 2 = atan2(r , s ) − atan2(a2 + a3c3 , a3s3 )

= atan2 xc + y c − d 2 , zc − d1  − atan2(a2 + a3c3 , a3s3 )
2 2

 

• Thus there are two solutions for the pair (2, 3)
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

RRR: Four total solutions

• In general, there will be a maximum of four solutions to the inverse

position kinematics of an elbow manipulator
– Ex: PUMA
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: RRP manipulator

• Spherical configuration
– Solve for 1 using same method as with RRR
1 = atan2(xc , y c )
– Again, if there is an offset, there
will be left-arm and right-arm solutions
– Solve for 2:
 2 = atan2(s, r )
r 2 = xc + y c
2 2

s = zc − d1

– Solve for d3:

d3 = r 2 + s 2
= xc + y c + (zc − d1 )
2 2 2
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Next class…

• Complete the discussion of inverse kinematics

– Inverse orientation
– Introduction to other methods
• Introduction to velocity kinematics and the Jacobian
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Inverse orientation kinematics

• Now that we can solve for the position of the wrist center (given
kinematic decoupling), we can use the desired orientation of the end
effector to solve for the last three joint angles
– Finding a set of Euler angles corresponding to a desired rotation matrix R
– We want the final three joint angles that give the orientation of the tool
frame with respect to o3 (i.e. R63)
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Inverse orientation: spherical wrist

• Previously, we said that the forward kinematics of the spherical wrist
were identical to a ZYZ Euler angle transformation:
c 4c5c 6 − s4s6 − c 4c 5 s6 − s 4c 6 c 4 s5 c 4 s5 d 6 
s c c + c s − s 4 c 5 s6 + c 4 c 6 s 4 s5 s4s5d 6 
T6 = A4 A5 A6 =  4 5 6
3 4 6

 − s5 c 6 s5 c 6 c5 c5d 6 
 
 0 0 0 1 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Inverse orientation: spherical wrist

• The inverse orientation problem reduces to finding a set of Euler angles
(4, 5, 6) that satisfy:
c4 c5c6 − s4 s6 − c4 c5 s6 − s4 c6 c4 s5 
R =  s4 c5c6 + c4 s6
3
6 − s4 c5 s6 + c4 c6 s4 s5 
 − s5c6 s5c6 c5 
• to solve this, take two cases:
1. Both r13 and r23 are not zero (i.e. 5 ≠ 0)… nonsingular
2. 5 = 0, thus r13 = r23 = 0… singular
• Nonsingular case
– If 5 ≠ 0, then r33 ≠ ±1 and:

c5 = r33 , s5 =  1 − r33
2

 5 = atan2 r33 , 1 − r33 

2

 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Inverse orientation: spherical wrist

• Thus there are two values for 5. Using the first (s5 > 0):
 4 = atan2(r13 , r23 )
 6 = atan2(− r31, r32 )
• Using the second value for 5 (s5 < 0):
 4 = atan2(− r13 ,−r23 )
 6 = atan2(r31,−r32 )
• Thus for the nonsingular case, there are two solutions for the inverse
orientation kinematics
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Inverse orientation: spherical wrist

• In the singular case, 5 = 0 thus s5 = 0 and r13 = r23 = r31 = r32 = 0
• Therefore, R63 has the form:
c 4c 6 − s4s6 − c 4s6 − s 4c 6 0 c 46 − s46 0 r11 r12 0
R = s4c 6 + c 4s6
3
6 − s 4s6 + c 4c 6 0 = s46 c 46 0 = r21 r22 0
 0 0 1  0 0 1  0 0 1

• So we can find the sum 4 + 6 as follows:

 4 +  6 = atan2(r11, r21 ) = atan2(r11,−r12 )
• Since we can only find the sum, there is an infinite number of solutions
(singular configuration)
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Inverse Kinematics: general procedure

1. Find q1, q2, q3 such that the position of the wrist center is:
0
inverse position
oco = o − d 6R 0 kinematics
 1

2. Using q1, q2, q3, determine R30

3. Find Euler angles corresponding to the rotation matrix:
( )
R63 = R30
−1
( )
R = R30 R
T inverse orientation
kinematics
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: RRR arm with spherical wrist

• For the DH parameters below, we can derive R30 from the forward
kinematics: c1c 23 − c1s23 s1 
R30 = s1c 23 − s1s23 − c1 
 s23 c 23 0 
• We know that R63 is given as follows:
c 4c5c 6 − s4s6 − c 4c 5s6 − s 4c 6 c 4 s5 
R63 = s4c5c 6 + c 4s6 − s 4c 5 s6 + c 4c 6 s4s5 
 − s5c 6 s5 c 6 c5 

• To solve the inverse orientation kinematics:

i i
( ) T link ai di
R63 = R30 R
– For a given desired R 1 0 90 d1 1
2 a2 0 0 2
3 a3 0 0 3
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: RRR arm with spherical wrist

• Euler angle solutions can be applied. Taking the third column of (R30)TR
c 4s5 = c1c23 r13 + s1c 23 r23 + s23 r33
s4s5 = −c1s23 r13 − s1s23 r23 + c23 r33
c5 = s1r13 − c1r23

• Again, if 5 ≠ 0, we can solve for 5:

 5 = atan2 s1r13 − c1r23 , 1 − (s1r13 − c1r23 ) 
2

 
• Finally, we can solve for the two remaining angles as follows:
 4 = atan2(c1c23r13 + s1c23r23 + s23r33 ,−c1s23r13 − s1s23r23 + c23r33 )
 6 = atan2(− s1r11 + c1r21, s1r12 − c1r22 )
• For the singular configuration (5 = 0), we can only find 4 + 6 thus it is
common to arbitrarily set 4 and solve for 6
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: elbow manipulator with spherical wrist

• Derive complete inverse kinematics solution

link ai i di i
1 0 90 d1 1
2 a2 0 0 2
3 a3 0 0 3
4 0 -90 0 4
5 0 0 0 5
6 0 0 d6 3
• we are given H = T60 such that: ox  r11 r12 r13 
o = oy , R = r21 r22 r23 
oz  r31 r32 r33 
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: elbow manipulator with spherical wrist

• First, we find the wrist center:

 xc  o x − d 6 r13 
 y  = o − d r 
 c  y 6 23 

 zc  oz − d 6 r33 

• Inverse position kinematics:

1 = atan2(xc , y c )
 2 = atan2 xc + y c − d 2 , zc − d1  − atan2(a2 + a3c3 , a3s3 )
2 2

 
(
 3 = atan2 D, 1 − D 2 )
• Where d is the shoulder offset (if any) and D is given by:
xc + y c − d 2 + (zc − d1 ) − a2 − a3
2 2 2 2 2
D=
2a2a3
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: elbow manipulator with spherical wrist

• Inverse orientation kinematics:

– Now that we know 1, 2, 3, we know R30. need to find R36:
( )
R63 = R30 R
T

• Solve for 4, 5, 6, Euler angles:

 4 = atan2(c1c23 r13 + s1c23 r23 + s23 r33 ,−c1s23r13 − s1s23 r23 + c23 r33 )
 5 = atan2 s1r13 − c1r23 , 1 − (s1r13 − c1r23 ) 
2

 
 6 = atan2(− s1r11 + c1r21, s1r12 − c1r22 )
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: inverse kinematics of SCARA manipulator

R o 
• We are given T40: T40 =  
 0 1
c12c 4 + s12s4 s12c 4 − c12s4 0 a1c1 + a2c12 
s c − c s − c12c 4 − s12s4 0 a1s1 + a2s12 
=  12 4 12 4

 0 0 − 1 − d3 − d 4 
 
 0 0 0 1 

link ai i di i
1 a1 0 0 1
2 a2 180 0 2
3 0 0 d3 0
4 0 0 d4 4
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: inverse kinematics of SCARA manipulator

• Thus, given the form of T40, R must have the following form:
c − s 0
R = s c 0
 0 0 1

• Where  is defined as:  = 1 + 2 −  4 = atan2(r11, r12 )

• To solve for 1 and 2 we project the manipulator onto the x0-y0 plane:
o x + oy − a1 − a2
2 2 2 2

c2 =
2a1a2
• This gives two solutions for 2:  2 = atan2 c2 , 1 − c2 2 
 
• Once 2 is known, we can solve for 1:
1 = atan2(ox , oy ) - atan2(a1 + a2c2 , a2s2 )
• 4 is now give as:  4 = 1 + 2 − atan2(r11, r12 )
• Finally, it is trivial to see that d3 = oz + d4
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: number of solutions

• How many solutions to the inverse position kinematics of a planar 3-link

arm?
– given a desired d=[dx dy]T, the forward kinematics can be written as:
d x = a1c1 + a2c12 + a3c123
d y = a1s1 + a2s12 + a3s123
– Therefore the inverse kinematics problem is under-constrained (two
equations and three unknowns)

∞ solutions is d is inside the workspace

1 solution if d is on the workspace boundary
0 solutions else
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Example: number of solutions

• What if now we describe the desired position and orientation of the end
effector?
– given a desired d=[dx dy]T, we can now call the position of o2 the ‘wrist
center’. This position is given as: w x = d x − a3 cos( d )
w y = d y − a3 sin( d )
– Now we have reduced the problem to finding the joint angles that will give
the desired position of the wrist center (we have done this for a 2D planar
manipulator).
– Finally, 3 is given as: 3 = d − (1 +  2 )

∞ solutions if the wrist center is on the origin

2 solutions if wrist center is inside the 2-link workspace
1 solution if wrist center is on the 2-link workspace boundary
0 solutions else
 Ecole Nationale des sciences Appliquées de Fès
Département Génie Industriel

Inverse kinematics: more general procedures

• Paden-Kahan subproblems
– An attempt to create a generalized formulation for solving inverse
kinematics based (again) upon some common configurations
– This typically involves rotating vectors about moving axes and projecting the
rotations onto orthogonal planes to solve for rotation angles
– See Murray, Li, Sastry, pp. 99-107