Ch.

3: Forward and Inverse Kinematics

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!)

2
 1. Kinematic Chains

• 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. For spherical or
universal joints, we can think of multi-DOF joints as a combination
of 1DOF joints with zero length between them
– o0 is the inertial frame
– on is the tool frame
– Joint i connects links i-1 and i
– oi is connected to link i

• Joint variables, qi
⎧θ if joint i is revolute
qi = ⎨ i
⎩ di if joint i is prismatic

1. Kinematic Chains

• 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
⎪ j −1
( )
⎪ Ti
⎩
if j > i

4
 1. Kinematic Chains

• The position and orientation of the tool frame with respect to the
inertial frame is given by one homogeneous transformation matrix:
⎡ Rn0 on0 ⎤
⎥ = Tn = A1 ( q1 ) A2 ( q2 ) ⋅⋅⋅ An ( qn )
0
H =⎢
⎣0 1⎦
⎡ Rii −1 oii −1 ⎤
Ai (qi ) = ⎢ ⎥
⎣ 0 1 ⎦
Rij = Rii+1 ⋅⋅⋅ R jj −1 , oij = oij −1 + R ij −1o jj −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

2. Forward Kinematics:
The Denavit-Hartenberg (DH) Convention
• Representing each individual homogeneous transformation as the
product of four basic transformations:
Ai = Rot z ,θi Trans z ,di Trans x,ai Rot x ,αi
⎡cθi − sθi 0 0 ⎤ ⎡1 0 0 0 ⎤ ⎡1 0 0 ai ⎤ ⎡1 0 0 0⎤
⎢ ⎥ ⎢ 0⎥
0 0 ⎥ ⎢0 1 0 0 ⎥ ⎢0 1 0 0 ⎥ ⎢0 cαi − sαi
= ⎢ θi ⎥
s cθi
⎢ ⎥⎢ ⎥
⎢0 0 ⎥
1 0 ⎢0 0 1 di ⎥ ⎢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 ⎥⎦

• Four DH parameters:
– ai: link length -
– αi: link twist
– di: link offset -
– θi: joint angle
Each Ai is a function of only one variable
-> di will be variable for prismatic joints
qi will be variable for revolute joints
Any rigid body needs 6 parameters to describe
its position and orientation <-> 4 DH parameters
6
 2. Forward Kinematics:
The Denavit-Hartenberg (DH) Convention
• Representing each individual homogeneous transformation as the
product of four basic transformations:
Ai = Rot z ,θi Trans z ,di Trans x ,ai Rot x ,αi
⎡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 ⎥⎦
• Four DH parameters:
– ai: link length - Each Ai is a function of only one variable
– αi: link twist -> di will be variable for prismatic joints
qi will be variable for revolute joints
– di: link offset - Any rigid body needs 6 parameters to describe
– θi: joint angle its position and orientation <-> 4 DH parameters
7

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 ,a Rot x ,α
⎡ R10 o10 ⎤
=⎢ ⎥
⎣0 1⎦
8
 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 , x1 (along z0)
• θi: joint angle, angle between x0 and x1 (measured around z0)

Positive convention:

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

10
 Assigning coordinate frames

2. Assign base frame

– Can be any point along z0
3. Chose x0, y0 to follow the right-handed convention
4. 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:

11

Assigning coordinate frames

i. If 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

ii. If 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

iii. If 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
12
 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

13

Example 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

14
 Example 1: two-link planar manipulator

• Now define DH parameters

link ai αi di θi
– Define the constant parameters ai, αi
1 a1 0 0 θ1
– Define the variable parameters θi, di
2 a2 0 0 θ2
– The αi = 0 because all zi are parallel
– Therefore only θi are variable

⎡c1 − s1 0 a1c1 ⎤ ⎡c2 − s2 0 a2c2 ⎤

⎢s c1 0 a1s1 ⎥ ⎢s c2 0 a2 s2 ⎥
A1 = ⎢ 1 ⎥ , A2 = ⎢ 2 ⎥
⎢0 0 1 0 ⎥ ⎢0 0 1 0 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣0 0 0 1 ⎦

⎡c12 − s12 0 a1c1 + a2c12 ⎤

⎢s c12 0 a1s1 + a2 s12 ⎥
T2 = A1 A2 = ⎢ 12
0 ⎥
⎢0 0 1 0 ⎥
⎢ ⎥
⎣0 0 0 1 ⎦
15

Example 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

16
 Example 2: three-link cylindrical robot
link ai αi di θi
• Now define DH parameters 1 0 0 d1 θ1
– Define the constant parameters ai, αi 2 0 -90 d2 0
– Define the variable parameters θi, di 3 0 0 d3 0

⎡c1 − s1 0 0⎤ ⎡1 0 0 0⎤
⎢s c1 0 0⎥ ⎢0 0 1 0⎥
A1 = ⎢ 1 ⎥ , A2 = ⎢ ⎥
⎢0 0 1 d1 ⎥ ⎢ 0 −1 0 d2 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 0 0 1⎦

⎡1 0 0 0⎤ ⎡ c1 0 − s1 − s1d3 ⎤
⎢0 1 0 0 ⎥ ⎢s 0 c1 c1d3 ⎥
A3 = ⎢ ⎥ , T3 = ⎢
0 1 ⎥
⎢0 0 1 d3 ⎥ ⎢ 0 −1 0 d1 + d 2 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 0 0 1 ⎦
17

Example 3: spherical wrist

• 3DOF: need to assign four coordinate frames

– yaw, pitch, roll (θ4, θ5, θ6)
all intersecting at one
point o (wrist center) d6
1. Choose z3 axis (joint 4)
2. Choose z4 axis (joint 5)
3. Choose z5 axis (joint 6)
4. Choose tool frame:
• z6 (a): collinear with z5
• y6 (s): in the direction
the gripper closes
• x6 (n): chosen with a
right-handed convention

18
 Example 3: spherical wrist
link ai αi di θi
• Now define DH parameters 4 0 -90 0 θ4
– Define the constant parameters ai, αi 5 0 90 0 θ5
– Define the variable parameters θi, di 6 0 0 d6 θ6

⎡c4 0 − s4 0⎤ ⎡c5 0 − s5 0⎤ ⎡c6 − s6 0 0⎤

⎢s 0 c4 0 ⎥ ⎢s 0 c5 0 ⎥ ⎢s c6 0 0⎥ -90 + θ5
A4 = ⎢ 4 ⎥ , A5 = ⎢ 5 ⎥ , A6 = ⎢ 6 ⎥
⎢0 −1 0 0⎥ ⎢0 −1 0 0⎥ ⎢0 0 1 d6 ⎥ c5 = s5
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 0 0 1⎦ ⎣0 0 0 1⎦ s5 = -c5

T63 = A4 A5 A6
⎡ c4c5c6 − s4 s6 −c4c5 s6 − s4c6 c4 s5 c4 s5d 6 ⎤
⎢s c c + c s − s4c5 s6 + c4c6 s4 s5 s4 s5d 6 ⎥
=⎢ 4 5 6 4 6 ⎥
⎢ − s5c6 s5c6 c5 c5d 6 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦

19

Example 3: spherical wrist

link ai αi di θi link ai αi di θi
4 0 -90 0 θ4 4 0 -90 0 θ4
5 0 90 0 θ5 5 0 90 0 -90 +θ5
6 0 0 d6 θ6 6 0 0 d6 θ6

3
20
 General procedure for determining
forward kinematics
1. Label joint axes as z0, …, zn-1 (axis zi is joint axis for joint i+1)
2. Choose base frame: set o0 on z0 and choose x0 and y0 using right-
handed convention
3. For i = 1 : n-1,
i. Place oi where the normal to zi and zi-1 intersects zi. If zi intersects zi-1,
put oi at intersection. If zi and zi-1 are parallel, place oi along zi such
that di=0
ii. xi is the common normal through oi, or normal to the plane formed by
zi-1 and zi if the two intersect
iii. Determine yi using right-handed convention
4. Place the tool frame: set zn parallel to zn-1
5. For i = 1 : n, fill in the table of DH parameters
6. Form homogeneous transformation matrices, Ai
7. Create Tn0 that gives the position and orientation of the end-effector
in the inertial frame
21

Example 4: cylindrical robot with spherical wrist

• 6DOF: need to assign seven coordinate frames

– But we already did this for the previous two examples (Ex 3
and Ex 4), so we can fill in the table of DH parameters:
z6 ≡ a
x6 ≡ n
z3,z5
link ai αi di θi
1 0 0 d1 θ1
x3,x5 y3,z4 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

22
Example 4: cylindrical robot with spherical wrist

• Note that z3 (axis for joint 4) is collinear r11 = c1c4c5c6 − c1s4 s6 + s1s5c6
with z2 (axis for joint 3), thus we can r21 = s1c4 c5c6 − s1s4 s6 − c1s5c6
make the following combination:
r31 = − s4 c5c6 − c4 s6
z6 ≡ a r12 = −c1c4 c5 s6 − c1s4 c6 − s1s5c6
x6 ≡ n
z3,z5 r22 = − s1c4 c5 s6 − s1s4 s6 + c1s5c6
r32 = s4 c5c6 − c4c6
x3,x5 y3,z4 r13 = c1c4 s5 − s1c5
r23 = s1c4 s5 + c1c5
⎡ r11 r12 r13 dx ⎤ r33 = − s4 s5
⎢r dy ⎥ d x = c1c4 s5 d 6 − s1c5 d 6 − s1d3
21 r22 r23
T6 = T3 T6 = ⎢
0 0 3 ⎥
⎢ r31 r32 r33 dz ⎥ d y = s1c4 s5 d 6 + c1c5 d 6 + c1d3
⎢ ⎥
⎣0 0 0 1⎦ d z = − s4 s5 d 6 + d1 + d 2

23

Example 5: the Stanford manipulator

• 6DOF: need to assign seven

link ai αi di θi
coordinate frames:
1 0 -90 0 θ1
1. Choose z0 axis (axis of
rotation for joint 1, base 2 0 90 d2 θ2
frame) 3 0 0 d3 0
2. Choose z1-z5 axes (axes 4 0 -90 0 θ4
of rotation/translation for 5 0 90 0 θ5
joints 2-6)
6 0 0 d6 θ6
3. Choose xi axes
4. Choose tool frame
5. Fill in table of DH
parameters:

24
 Example 5: the Stanford manipulator

• 6DOF: need to assign seven

link ai αi di θi
coordinate frames:
1 0 -90 d1 θ1
1. Choose z0 axis (axis of
rotation for joint 1, base 2 0 90 d2 θ2
frame) 3 0 0 d3 0
2. Choose z1-z5 axes (axes 4 0 -90 0 θ4
of rotation/translation for 5 0 90 0 θ5
joints 2-6)
6 0 0 d6 θ6
3. Choose xi axes
4. Choose tool frame
5. Fill in table of DH
parameters:

25

Example 5: the Stanford manipulator

• Now determine the individual homogeneous transformations:

⎡ c1 0 − s1 0⎤ ⎡ c2 0 s2 0⎤ ⎡1 0 0 0⎤
⎢s 0 c1 0⎥ ⎢s 0 −c2 0⎥ ⎢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⎦
⎡ c4 0 − s4 0 ⎤ ⎡ c5 0 s5 0 ⎤ ⎡ c6 − s6 0 0⎤
⎢s 0 c4 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⎦
⎡ r11 r12 r13 dx ⎤
⎢r dy ⎥
21 r22 r23
T6 = A1 ⋅⋅⋅ A6 = ⎢
0 ⎥
⎢ r31 r32 r33 dz ⎥
⎢ ⎥
⎣0 0 0 1⎦
26
 Example 5: the Stanford manipulator
r11 = c1 ⎡⎣c2 ( c4c5c6 − s4 s6 ) − s2 s5c6 ⎤⎦ − d 2 ( s4 c5c6 + c4 s6 )
r21 = s1 ⎡⎣ c2 ( c4c5c6 − s4 s6 ) − s2 s5c6 ⎤⎦ + c1 ( s4c5c6 + c4 s6 )
r31 = − s2 ( c4c5c6 − s4 s6 ) − c2 s5c6
r12 = c1 ⎡⎣ −c2 ( c4c5 s6 + s4c6 ) + s2 s5 s6 ⎤⎦ − s1 ( − s4 c5 s6 + c4c6 )
r22 = − s1 ⎡⎣ −c2 ( c4 c5 s6 − s4c6 ) − s2 s5 s6 ⎤⎦ + c1 ( − s4c5 s6 + c4 s6 )
r32 = s2 ( c4 c5 s6 + s4 c6 ) + c2 s5 s6
r13 = c1 ( c2c4 s5 + s2c5 ) − s1s4 s5
r23 = s1 ( c2c4 s5 + s2 c5 ) + c1s4 s5
r33 = − s2c4 s5 + c2c5
d x = c1s2 d3 − s1d 2 + d 6 ( c1c2 c4 s5 + c1c5 s2 − s1s4 s5 )
d y = s1s2 d3 + c1d 2 + d 6 ( c1s4 s5 + c2c4 s1s5 + c5 s1s2 )
d z = c2 d3 + d 6 ( c2 c5 − c4 s2 s5 )
27

Example 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
0
0
4. Choose tool frame
5. DH parameters: 0

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
28
 Example 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. DH parameters:

link ai αi di θi
1 a1 0 d1 θ1 0
0
2 a2 180 0 θ2
0
3 0 0 d3 0
4 0 0 d4 θ4
29

Example 6: the SCARA manipulator

• Now determine the individual homogeneous transformations:

⎡c1 − s1 0 a1c1 ⎤ ⎡c2 s2 0 a2 c2 ⎤
⎢s c1 0 a1s1 ⎥ ⎢ s −c2 0 a2 s2 ⎥
A1 = ⎢ 1 ⎥ , A2 = ⎢ 2 ⎥
⎢0 0 1 0 ⎥ ⎢0 0 −1 0 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣0 0 0 1 ⎦
⎡1 0 0 0 ⎤ ⎡c4 − s4 0 0⎤
⎢0 1 0 0 ⎥ ⎢s c4 0 0⎥
A3 = ⎢ ⎥ , A4 = ⎢ 4 ⎥
⎢ 0 0 1 d3 ⎥ ⎢0 0 1 d4 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣0 0 0 1⎦
⎡c12c4 + s12 s4 −c12 s4 + s12 c4 0 a1c1 + a2 c12 ⎤
⎢ s c − c s −s s − c c 0 a1s1 + a2 s12 ⎥
T4 = A1 ⋅⋅⋅ A4 = ⎢ 12 4 12 4
0 12 4 12 4 ⎥
⎢ 0 0 −1 − d3 − d 4 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
30
Example 7: PUMA 560

31

Example 7: PUMA 560

link ai αi di θi
90 + θ1 1 0 -90 0 θ1
2 a2 90 d2 θ2
90 + θ3 3 a3 90 0 θ3
4 0 -90 d4 θ4
5 0 90 0 θ5
6 0 0 d6 θ6

a3 < 0

32
 Example 7: PUMA 560

link ai αi di θi
1 0 -90 0 θ1
2 a2 0 d2 θ2
3 a3 90 0 θ3
4 0 -90 d4 θ4
5 0 90 0 θ5
6 0 0 d6 θ6 33

Example 7: PUMA 560

• Now determine the individual homogeneous transformations:

⎡c1 0 − s1 0⎤ ⎡c2 − s2
0 a2 c2 ⎤
⎢s 0 c1 0⎥ ⎢s c2 0 a2 s2 ⎥
A1 = ⎢ 1 ⎥ , A2 = ⎢ 2 ⎥
⎢ 0 −1 0 0⎥ ⎢0 0 0 d2 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 0 0 1 ⎦
⎡ c3 0 s3 a3c3 ⎤ ⎡c4 0 − s4 0 ⎤
⎢s 0 −c3 a3 s3 ⎥ ⎢s 0 c4 0⎥
A3 = ⎢ 3 ⎥, A4 = ⎢ 4 ⎥
⎢0 1 0 0 ⎥ ⎢0 −1 0 d 4 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣0 0 0 1⎦
⎡ c5 0 s5 0⎤ ⎡c6 − s6 0 0⎤
⎢s 0 −c5 0⎥ ⎢s c6 0 0⎥
A5 = ⎢ 5 ⎥, A6 = ⎢ 6 ⎥
⎢0 1 0 0⎥ ⎢0 0 1 d6 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 0 0 1⎦
34
 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

35

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δ + 2 L2 2 ⎟
−1 ⎜
θ = cos
⎜⎜ 2 ⎟
⎝ 2 ( L1 − δ ) + L2 ⎠
2
⎟

⎛ L ⎞ π
+ tan −1 ⎜ 2 ⎟−
⎝ L1 − δ ⎠ 2
36
 3. 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

37

3. Inverse Kinematics

• Ex: Stanford manipulator c1 ⎡⎣c2 ( c4c5c6 − s4s6 ) − s2s5c6 ⎤⎦ − d2 ( s4c5c6 + c4s6 ) = 0

– For a given H: s1 ⎡⎣c2 ( c4c5c6 − s4s6 ) − s2s5c6 ⎤⎦ + c1 ( s4c5c6 + c4s6 ) = 0

⎡0 1 0 −0.154 ⎤ −s2 ( c4c5c6 − s4s6 ) − c2s5c6 = 1

⎢0 0 1 0.763 ⎥ c1 ⎡⎣−c2 ( c4c5s6 + s4c6 ) + s2s5s6 ⎤⎦ − s1 ( −s4c5s6 + c4c6 ) = 1
H =⎢ ⎥
⎢1 0 0 0 ⎥ −s1 ⎡⎣−c2 ( c4c5s6 − s4c6 ) − s2s5s6 ⎤⎦ + c1 ( −s4c5s6 + c4s6 ) = 0
⎢ ⎥
⎣0 0 0 1 ⎦ s2 ( c4c5s6 + s4c6 ) + c2s5s6 = 0
c1 ( c2c4s5 + s2c5 ) − s1s4s5 = 0
– Find θ1, θ2, d3, θ4, θ5, θ6: s1 ( c2c4s5 + s2c5 ) + c1s4s5 = 1
−s2c4s5 + c2c5 = 0
c1s2d3 − s1d2 + d6 ( c1c2c4s5 + c1c5s2 − s1s4s5 ) = −0.154
s1s2d3 + c1d2 + d6 ( c1s4s5 + c2c4s1s5 + c5s1s2 ) = 0.763
c2d3 + d6 ( c2c5 − c4s2s5 ) = 0

• One solution: θ1 = π/2, θ2 = π/2, d3 = 0.5, θ4 = π/2, θ5 = 0, θ6 = π/2

38
 3. Inverse Kinematics

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

closed-form solution to the 12 equations
• Instead, we develop methods based on 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, may violate joint limits
• Closed-form solutions are ideal!

39

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…

40
 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 (with respect
to the base frame) and we can write:
⎡0 ⎤ ⎡0 ⎤
o = o60 = oco + d 6 R ⎢0 ⎥ ⇒ oco = o − d 6 R ⎢0 ⎥
⎢ ⎥ ⎢ ⎥
⎢⎣1 ⎥⎦ ⎢⎣1 ⎥⎦

– Calling o is the desired position of

the tool frame:
o = [ox oy oz]T, oc0 = [xc yc zc]T
⎡ xc ⎤ ⎡ ox − d 6 r13 ⎤
⎢ y ⎥ = ⎢o − d r ⎥
⎢ c⎥ ⎢ y 6 23 ⎥
⎢⎣ zc ⎥⎦ ⎢⎣ oz − d 6 r33 ⎥⎦
41

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 = R30 R63
– To solve for the final three
joint angles:
( ) ( )
−1 T
R63 = R30 R = R30 R

– Since the last three joints for

a spherical wrist, we can use
a set of Euler angles to solve
for them
42
 Inverse position

• 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.
– 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

⎧−atan2 ( − y, x ) y<0
• We use atan2(·) instead ⎪
of atan(·) to account for ⎪π − atan ⎛ − y ⎞ y ≥ 0, x < 0
⎪ ⎜ ⎟
the full range of angular ⎝ x⎠
⎪
solutions: ⎪
atan2 ( y, x ) = ⎨ atan ⎛⎜ y ⎞⎟ y ≥ 0, x ≥ 0
⎪ ⎝x⎠
⎪ π
⎪ y > 0, x = 0
⎪ 2
⎪⎩ undefined y = 0, x = 0
43

Inverse position
Example: RRR manipulator
• To solve for θ1, project the arm onto the x0, y0 plane

(1) : θ1 = atan2 ( xc , yc )
(2) : θ1 = π + atan2 ( xc , yc )

– Value of θ1 in (1) or (2) will change the solutions for θ2 and θ3

44
 RRR: singular configurations, offsets

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

– i.e. any value of θ1 will two solutions for θ1: left arm and right
work arm. However, wrist centers cannot
intersect z0

45

Inverse position
RRR: Left arm and right arm solutions
• Left arm: • Right arm:
θ1 = ϕ − α θ1 = α + β
ϕ = atan2 ( xc , yc ) α = atan2 ( xc , yc )

α = atan2 ( xc 2 + yc 2 − d 2 , d ) β = π + atan2 ( xc 2 + yc 2 − d 2 , d )
(
= atan2 − xc 2 + yc 2 − d 2 , − d )

46
 Inverse position
RRR: 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 2 − a32
cos θ3 =
2a2 a3
r 2 = xc 2 + yc 2 − d 2
s = zc − d1
xc 2 + yc 2 − d 2 + ( zc − d1 ) − a2 2 − a32
2
cos θ3 =
2a2 a3
:= D
• Therefore we can find two solutions for θ3:

(
θ3 = atan2 D, ± 1 − D 2 )
47

Inverse position
RRR: 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 , a3 s3 )

= atan2 ( xc 2 + yc 2 − d 2 , zc − d1 )
−atan2 ( a2 + a3c3 , a3 s3 )

• Thus there are two solutions for the

pair (θ2, θ3)

48
 Inverse position
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

49

Inverse position
Example: RRP manipulator
• Spherical configuration
– Solve for θ1 using same method as with RRR
θ1 = atan2 ( xc , yc )
– If there is an offset,
there will be left-arm
and right-arm solutions
– Solve for θ2:
θ 2 = atan2 ( s, r )
r 2 = xc 2 + yc 2
s = zc − d1

– Solve for d3:

d3 = r 2 + s 2 = xc 2 + yc 2 + ( zc − d1 )
2

50
 Inverse orientation

• 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)

51

Inverse orientation: spherical wrist

• Previously, we said that the forward kinematics of the spherical

wrist were identical to a ZYZ Euler angle transformation:
⎡ c4 c5c6 − s4 s6 −c4 c5 s6 − s4c6 c4 s5 c4 s5 d 6 ⎤
⎢s c c + c s − s4 c5 s6 + c4 c6 s4 s5 s4 s5 d 6 ⎥
T6 = A4 A5 A6 = ⎢ 4 5 6 4 6
3 ⎥
⎢ − s5c6 s5c6 c5 c5 d 6 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦

52
 Inverse orientation: spherical wrist

• The inverse orientation problem reduces to finding a set of Euler

angles (θ4, θ5, θ6) that satisfy:

⎡ c4c5c6 − s4 s6 −c4c5 s6 − s4 c6 c4 s5 ⎤
R63 = ⎢ s4 c5c6 + c4 s6 − s4c5 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

53

Inverse orientation: spherical wrist

• Nonsingular case
– If θ5 ≠ 0, then r33 ≠ ±1 and:
(
θ5 = atan2 r33 , ± 1 − r332 )
– 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

54
 Inverse orientation: spherical wrist

• Singular case
– θ5 = 0 thus s5 = 0 and r13 = r23 = r31 = r32 = 0
– Therefore, R63 has the form:
⎡ c4c6 − s4 s6 −c4 s6 − s4 c6 0 ⎤ ⎡ c46 − s46 0 ⎤ ⎡ r11 r12 0⎤
R63 = ⎢ s4 c6 + c4 s6 − s4 s6 + c4c6 0 ⎥ = ⎢ s46 c46 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)

55

Example: RRR arm with spherical wrist

• For the DH parameters below, we can

link ai αi di θi
derive R30 from the forward kinematics:
−c1s23 1 0 90 d1 θ1
⎡c1c23 s1 ⎤
R3 = ⎢ s1c23
0
− s1s23 −c1 ⎥ 2 a2 0 0 θ2
⎢ ⎥
⎢⎣ s23 c23 0 ⎥⎦ 3 a3 0 0 θ3

• We know that R63 is given:

⎡ c4c5c6 − s4 s6 −c4c5 s6 − s4c6 c4 s5 ⎤
R63 = ⎢ s4c5c6 + c4 s6 − s4c5 s6 + c4c6 s4 s5 ⎥
⎢ ⎥
⎣⎢ − s5c6 s5c6 c5 ⎦⎥

• Solve inverse orientation:

( )
T
R63 = R30 R

56
 Example: RRR arm with spherical wrist

• Euler angle solutions can be applied. Taking the third column of

(R30)TR:
c4 s5 = c1c23r13 + s1c23r23 + s23r33
s4 s5 = −c1s23r13 − s1s23r23 + c23r33
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

57

Inverse Kinematics: general procedure

1. Find q1, q2, q3 such that the position of the

wrist center is:
⎡0 ⎤ inverse position
oco = o − d6 R ⎢0 ⎥ kinematics
⎢ ⎥
⎢⎣1 ⎥⎦

2. Using q1, q2, q3, determine R30

3. Find Euler angles corresponding to the

rotation matrix: inverse orientation

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

58
 Example: SCARA manipulator

• 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

⎡ R o⎤
T40 = ⎢ ⎥
⎣ 0 1⎦
⎡ c12 c4 + s12 s4 s12c4 − c12 s4 0 a1c1 + a2 c12 ⎤
⎢s c − c s −c12c4 − s12 s4 0 a1s1 + a2 s12 ⎥
= ⎢ 12 4 12 4 ⎥
⎢ 0 0 −1 −d3 − d 4 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦ 59

Example: SCARA manipulator

• Thus, given the form of T40, R must have the following form:
⎡cα sα 0⎤
R = ⎢ sα −cα 0 ⎥ α = θ1 + θ 2 − θ 4 = atan2 ( r11 , r12 )
⎢ ⎥
⎢⎣ 0 0 −1⎥⎦
• To solve for θ1 and θ2 we project the manipulator onto the x0-y0 plane:
ox 2 + o y 2 − a12 − a2 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 , o y − atan2 ( a1 + a2c2 , a2 s2 )
• θ4 is now give as: θ 4 = θ1 + θ 2 − atan2 ( r11 , r12 )
• Finally, it is trivial to see that d3 = -(oz + d4 )
60