ES159/259

Ch. 3: Forward and Inverse Kinematics

ES159/259
Recap: The Denavit-Hartenberg (DH) Convention
Representing each individual homogeneous transformation as the
product of four basic transformations:

(
(
(
(

=
(
(
(
(

(
(
(
(

(
(
(
(

(
(
(
(

=
=
1 0 0 0
0
1 0 0 0
0 0
0 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0 1 0
0 0 1
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0
0 0
, , , ,
i
i
i
i
i
x a x d z z i
d c s
s a s c c c s
c a s s c s c
c s
s c
a
d
c s
s c
A
i i
i i i i i i
i i i i i i
i i
i i i i
i i
i i i i
o o
u o u o u u
u o u o u u
o o
o o u u
u u
o u
Rot Trans Trans Rot
ES159/259
Recap: the physical basis for DH parameters
a
i
: link length, distance between the o
0
and o
1
(projected along x
1
)
o
i
: link twist, angle between z
0
and z
1
(measured around x
1
)
d
i
: link offset, distance between o
0
and o
1
(projected along z
0
)
u
i
: joint angle, angle between x
0
and x
1
(measured around z
0
)


ES159/259
General procedure for determining forward kinematics
1. Label joint axes as z
0
, , z
n-1
(axis z
i
is joint axis for joint i+1)
2. Choose base frame: set o
0
on z
0
and choose x
0
and y
0
using right-
handed convention
3. For i=1:n-1,
i. Place o
i
where the normal to z
i
and z
i-1
intersects z
i
. If z
i
intersects z
i-1
, put
o
i
at intersection. If z
i
and z
i-1
are parallel, place o
i
along z
i
such that d
i
=0
ii. x
i
is the common normal through o
i
, or normal to the plane formed by z
i-1

and z
i
if the two intersect
iii. Determine y
i
using right-handed convention
4. Place the tool frame: set z
n
parallel to z
n-1
5. For i=1:n, fill in the table of DH parameters
6. Form homogeneous transformation matrices, A
i
7. Create T
n
0
that gives the position and orientation of the end-effector in
the inertial frame
ES159/259
Example 2: three-link cylindrical robot
3DOF: need to assign four coordinate frames
1. Choose z
0
axis (axis of rotation for joint 1, base frame)
2. Choose z
1
axis (axis of translation for joint 2)
3. Choose z
2
axis (axis of translation for joint 3)
4. Choose z
3
axis (tool frame)
This is again arbitrary for this case since we have described no wrist/gripper
Instead, define z
3
as parallel to z
2

ES159/259
Example 2: three-link cylindrical robot
Now define DH parameters
First, define the constant parameters a
i
, o
i

Second, define the variable parameters u
i
, d
i






link a
i
o
i
d
i
u
i
1 0

0 d
1
u
1
2 0

-90 d
2
0

3 0 0 d
3
0
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0
0 1 0 0
0 0 0 1
1 0 0 0
1 0 0
0 0
0 0
3
3
2
2
1
1 1
1 1
1
d
A
d
A
d
c s
s c
A , ,

(
(
(
(

+

= =
1 0 0 0
0 1 0
0
0
2 1
3 1 1 1
3 1 1 1
3 2 1
0
3
d d
d c c s
d s s c
A A A T
ES159/259
Example 3: spherical wrist
3DOF: need to assign four coordinate frames
yaw, pitch, roll (u
4
, u
5
, u
6
) all intersecting at one point o (wrist center)
1. Choose z
3
axis (axis of rotation for joint 4)
2. Choose z
4
axis (axis of rotation for joint 5)
3. Choose z
5
axis (axis of rotation for joint 6)
4. Choose tool frame:
z
6
(a) is collinear with z
5
y
6
(s) is in the direction the gripper closes
x
6
(n) is chosen with a right-handed convention

ES159/259
Example 3: spherical wrist
link a
i
o
i
d
i
u
i
4 0

-90 0

u
4
5 0

90 0 u
5
6 0 0 d
6
u
6
Now define DH parameters
First, define the constant parameters a
i
, o
i

Second, define the variable parameters u
i
, d
i




(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
6
6 6
6 6
6
5 5
5 5
5
4 4
4 4
4
d
c s
s c
A
c s
s c
A
c s
s c
A , ,

(
(
(
(

+ +

= =
1 0 0 0
6 5 5 6 5 6 5
6 5 4 5 4 6 4 6 5 4 6 4 6 5 4
6 5 4 5 4 6 4 6 5 4 6 4 6 5 4
6 5 4
3
6
d c c c s c s
d s s s s c c s c s s c c c s
d s c s c c s s c c s s c c c
A A A T
ES159/259
Example 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 a
i
o
i
d
i
u
i
1 0 0 d
1
u
1
2 0 -90 d
2
0
3 0 0 d
3
0
4 0

-90 0

u
4
5 0

90 0 u
5
6 0 0 d
6
u
6
o
3
, o
4
, o
5
are all at
the same point o
c

ES159/259
Example 4: cylindrical robot with spherical wrist
Note that z
3
(axis for joint 4) is collinear with z
2
(axis for joint 3), thus we
can make the following combination:

(
(
(
(

= =
1 0 0 0
33 32 31
23 22 21
13 12 11
3
6
0
3
0
6
z
y
x
d r r r
d r r r
d r r r
T T T
2 1 6 5 4
3 1 6 5 1 6 5 4 1
3 1 6 5 1 6 5 4 1
5 4 33
5 1 5 4 1 23
5 1 5 4 1 13
6 4 6 5 4 32
6 5 1 6 4 1 6 5 4 1 22
6 5 1 6 4 1 6 5 4 1 12
6 4 6 5 4 31
6 5 1 6 4 1 6 5 4 1 21
6 5 1 6 4 1 6 5 4 1 11
d d d s s d
d c d c c d s c s d
d s d c s d s c c d
s s r
c c s c s r
c s s c c r
c c c c s r
c s c s s s s c c s r
c s s c s c s c c c r
s c c c s r
c s c s s s c c c s r
c s s s s c c c c c r
z
y
x
+ + =
+ + =
=
=
+ =
=
=
+ =
=
=
=
+ =
ES159/259
Example 5: the Stanford manipulator
6DOF: need to assign seven coordinate frames:
1. Choose z
0
axis (axis of rotation for joint 1, base frame)
2. Choose z
1
-z
5
axes (axes of rotation/translation for joints 2-6)
3. Choose x
i
axes
4. Choose tool frame
5. Fill in table of DH parameters:
link a
i
o
i
d
i
u
i
1 0 -90 0

u
1
2 0 90 d
2
u
2
3 0 0 d
3
0
4 0

-90 0

u
4
5 0

90 0 u
5
6 0 0 d
6
u
6
Suggested insertion:
photo of the Stanford
manipulator
ES159/259
Example 5: the Stanford manipulator
Now determine the individual homogeneous transformations:
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0
0 0
0 0
1 0 0 0
0 0 1 0
0 0
0 0
6
6 6
6 6
6
5 5
5 5
5
4 4
4 4
4
3
3
2
2 2
2 2
2
1 1
1 1
1
d
c s
s c
A
c s
s c
A
c s
s c
A
d
A
d
c s
s c
A
c s
s c
A
, ,
, ,
ES159/259
Example 5: the Stanford manipulator
Finally, combine to give the complete description of the forward
kinematics:

(
(
(
(

= =
1 0 0 0
33 32 31
23 22 21
13 12 11
6 1
0
6
z
y
x
d r r r
d r r r
d r r r
A A T
( ) | | ( )
( ) | | ( )
( )
( ) | | ( )
( ) | | ( )
( )
( )
( )
( )
( )
( )
5 2 4 5 2 6 3 2
2 1 5 5 1 4 2 5 4 1 6 2 1 3 2 1
5 4 1 2 5 1 5 4 2 1 6 2 1 3 2 1
5 2 5 4 2 33
5 4 1 5 2 5 4 2 1 23
5 4 1 5 2 5 4 2 1 13
6 5 2 6 4 6 5 4 2 32
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 22
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 12
6 5 2 6 4 6 5 4 2 31
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 21
6 4 6 5 4 2 6 5 2 6 4 6 5 4 2 1 11
s s c c c d d c d
s s c s s c c s s c d d c d s s d
s s s s c c s c c c d d s d s c d
c c s c s r
s s c c s s c c s r
s s s c s s c c c r
s s c c s s c c s r
s c s c s c s s s c s s c c c s r
c c s c s s s s s c s s c c c c r
c s c s s c c c s r
s c c c s c c s s s s c c c c s r
s c c c s d c s s s s c c c c c r
z
y
x
+ =
+ + + + =
+ + =
+ =
+ + =
+ =
+ + =
+ + =
+ + + =
=
+ + =
+ =
ES159/259
Example 6: the SCARA manipulator
4DOF: need to assign five coordinate frames:
1. Choose z
0
axis (axis of rotation for joint 1, base frame)
2. Choose z
1
-z
3
axes (axes of rotation/translation for joints 2-4)
3. Choose x
i
axes
4. Choose tool frame
5. Fill in table of DH parameters:
link a
i
o
i
d
i
u
i
1 a
1
0 0

u
1
2 a
2
180 0

u
2
3 0 0 d
3
0
4 0

0 d
4
u
4
Suggested
insertion: photo
of SCARA
manipulator
ES159/259
Example 6: the SCARA manipulator
Now determine the individual homogeneous transformations:
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0
0 0
0 0
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0
0
1 0 0 0
0 1 0 0
0
0
4
4 4
4 4
4
3
3
2 2 2 2
2 2 2 2
2
1 1 1 1
1 1 1 1
1
d
c s
s c
A
d
A
s a c s
c a s c
A
s a c s
c a s c
A , , ,

(
(
(
(

+
+ + +
= =
1 0 0 0
1 0 0
0
0
4 3
12 2 1 1 4 12 4 12 4 12 4 12
12 2 1 1 4 12 4 12 4 12 4 12
4 1
0
4
d d
s a s a c c s s s c c s
c a c a c s s c s s c c
A A T
ES159/259
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
Grueblers formula (3D):
( )

=
+ =
j
n
i
i j L
f n n
1
6 DOF #
number of links*
*excluding ground
number of joints
#DOF for joint i
ES159/259
Forward kinematics of parallel manipulators
Grueblers formula (2D):

Example (2D):
Planar four-bar, n
L
= 3, n
j
= 4, f
i
= 1(for all joints)
3(3-4)+4 = 1DOF
Forward kinematics:

( )

=
+ =
j
n
i
i j L
f n n
1
3 DOF #
( )
2
tan
2
2 2
cos
1
2 1
2
2
2
1
2
2
2
1
t
o
o
o o
u
|
|
.
|

\
|

+
|
|
.
|

\
|
+
+
=

L
L
L L
L
ES159/259
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:


Find all solutions to:

Noting that:

This gives 12 (nontrivial) equations with n unknowns


( ) 3
1 0
SE
o R
H e
(

=
( ) H q q T
n n
= ,...,
1
0
( ) ( ) ( )
n n n n
q A q A q q T =
1 1 1
0
,...,
ES159/259
For a given H:




Find u
1
, u
2
, d
3
, u
4
, u
5
, u
6
:






One solution: u
1
= t/2, u
2
= t/2, d
3
= 0.5, u
4
= t/2, u
5
= 0, u
6
= t/2

(
(
(
(

=
1 0 0 0
0 0 0 1
763 . 0 1 0 0
154 . 0 0 1 0
H
Example: the Stanford manipulator
( ) | | ( )
( ) | | ( )
( )
( ) | | ( )
( ) | | ( )
( )
( )
( )
( )
( )
( ) 0
763 . 0
154 . 0
0
1
0
0
0
1
1
0
0
5 2 4 5 2 6 3 2
2 1 5 5 1 4 2 5 4 1 6 2 1 3 2 1
5 4 1 2 5 1 5 4 2 1 6 2 1 3 2 1
5 2 5 4 2
5 4 1 5 2 5 4 2 1
5 4 1 5 2 5 4 2 1
6 5 2 6 4 6 5 4 2
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1
6 5 2 6 4 6 5 4 2
6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1
6 4 6 5 4 2 6 5 2 6 4 6 5 4 2 1
= +
= + + + +
= + +
= +
= + +
= +
= + +
= + +
= + + +
=
= + +
= +
s s c c c d d c
s s c s s c c s s c d d c d s s
s s s s c c s c c c d d s d s c
c c s c s
s s c c s s c c s
s s s c s s c c c
s s c c s s c c s
s c s c s c s s s c s s c c c s
c c s c s s s s s c s s c c c c
c s c s s c c c s
s c c c s c c s s s s c c c c s
s c c c s d c s s s s c c c c c
ES159/259
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!

ES159/259
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 o
c




Use the position of the wrist center to determine the first three joint
angles
( )
( ) o q q o
R q q R
=
=
6 1
0
6
6 1
0
6
,...,
,...,
ES159/259
Overview: kinematic decoupling
Now, origin of tool frame, o
6
, is a distance d
6
translated along z
5
(since
z
5
and z
6
are collinear)
Thus, the third column of R is the direction of z
6
(w/ respect to the base
frame) and we can write:


Rearranging:


Calling o = [o
x
o
y
o
z
]
T
, o
c
0
= [x
c
y
c
z
c
]
T
(
(
(

+ = =
1
0
0
6
0
6
R d o o o
o
c
(
(
(

=
1
0
0
6
R d o o
o
c
(
(
(

=
(
(
(

33 6
23 6
13 6
r d o
r d o
r d o
z
y
x
z
y
x
c
c
c
ES159/259
Overview: kinematic decoupling
Since [x
c
y
c
z
c
]
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 R
3
0
Note that:

To solve for the final three joint angles:


Since the last three joints for a
spherical wrist, we can use a set of
Euler angles to solve for them
3
6
0
3
R R R =
( ) ( ) R R R R R
T
0
3
1
0
3
3
6
= =

ES159/259
Inverse position
Now that we have [x
c
y
c
z
c
]
T
we need to find q
1
, q
2
, q
3

Solve for q
i
by projecting onto the x
i-1
, y
i-1
plane, solve trig problem
Two examples: elbow (RRR) and spherical (RRP) manipulators
For example, for an elbow manipulator, to solve for u
1
, project the arm onto
the x
0
, y
0
plane
ES159/259
Background: two argument atan
We use atan2() instead of atan() to account for the full range of
angular solutions
Called four-quadrant arctan
( )
( )

= =
= >
> > |
.
|

\
|
< > |
.
|

\
|

<
=
0 , 0
0 , 0
2
0 , 0
0 , 0
0 , 2
, 2
x y
x y
x y
x
y
x y
x
y
y x y
x y
undefined
atan
atan
atan
atan
t
t
ES159/259
Example: RRR manipulator
1. To solve for u
1
, project the arm onto the x
0
, y
0
plane










Can also have:
This will of course change the solutions for u
2
and u
3
( )
c c
y x , 2
1
atan = u
( )
c c
y x , 2
1
atan + = t u
ES159/259
If there is an offset, then we will
have two solutions for u
1
: left arm
and right arm
However, wrist centers cannot
intersect z
0
If x
c
=y
c
=0, u
1
is undefined
i.e. any value of u
1
will work

Caveats: singular configurations, offsets
ES159/259
Left arm: Right arm:
Left arm and right arm solutions
( )
|
.
|

\
|
+ =
=
=
d d y x
y x
c c
c c
, 2
, 2
2
2 2
1
atan
atan
-
o
|
o | u
( )
|
.
|

\
|
+ =
|
.
|

\
|
+ + =
=
+ =
d d y x
d d y x
y x
c c
c c
c c
, 2
, 2
, 2
2
2 2
2
2 2
1
atan
atan
atan
t |
o
| o u
ES159/259
Therefore there are in general two solutions for u
1
Finding u
2
and u
3
is identical to the planar two-link manipulator we have
seen previously:






Therefore we can find two solutions for u
3
:
Left arm and right arm solutions
( )
D
a a
a a d z d y x
d z s
d y x r
a a
a a s r
c c c
c
c c

+ +
=
=
+ =
+
=
3 2
2
3
2
2
2
1
2
2 2
3
1
2
2 2
2
3 2
2
3
2
2
2 2
3
2
cos
2
cos
u
u
( )
2
3
1 , 2 D D = atan u
ES159/259
The two solutions for u
3
correspond to the elbow-down and elbow-up
positions respectively
Now solve for u
2
:




Thus there are two solutions for the pair (u
2
, u
3
)

Left arm and right arm solutions
( ) ( )
( )
3 3 3 3 2 1
2
2 2
3 3 3 3 2 2
, 2 , 2
, 2 , 2
s a c a a d z d y x
s a c a a s r
c c c
+ |
.
|

\
|
+ =
+ =
atan atan
atan atan u
ES159/259
In general, there will be a maximum of four solutions to the inverse
position kinematics of an elbow manipulator
Ex: PUMA
RRR: Four total solutions
ES159/259
Spherical configuration
Solve for u
1
using same method as with RRR

Again, if there is an offset, there
will be left-arm and right-arm solutions
Solve for u
2
:




Solve for d
3
:


Example: RRP manipulator
( )
c c
y x , 2
1
atan = u
( )
1
2 2
2
2
, 2
d z s
y x r
r s
c
c c
=
+ =
= atan u
( )
2
1
2 2
2 2
3
d z y x
s r d
c c c
+ + =
+ =
ES159/259
Next class
Complete the discussion of inverse kinematics
Inverse orientation
Introduction to other methods
Introduction to velocity kinematics and the Jacobian