Giao Trinh Robot Park-Lynch

INTRODUCTION TO ROBOTICS
MECHANICS, PLANNING, AND CONTROL
F. C. Park and K. M. Lynch
Contents
1 Preview
2 Configuration Space
2.1 Configuration Space of a Rigid Body . . . . .
2.2 Configuration Space of a Robot . . . . . . . .
2.2.1 Degrees of Freedom of a Robot . . . .
2.2.2 Parametrizing the Configuration Space
2.2.3 Pfaffian Constraints . . . . . . . . . .
2.2.4 Task Space . . . . . . . . . . . . . . .
2.3 Summary . . . . . . . . . . . . . . . . . . . .
2.4 Exercises . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9
10
12
13
21
23
25
28
30
3 Rigid-Body Motions
3.1 A Motivating Example . . . . . .
3.2 Rotations . . . . . . . . . . . . .
3.2.1 Definition . . . . . . . . .
3.2.2 Properties . . . . . . . . .
3.2.3 Euler Angles . . . . . . .
3.2.4 Roll-Pitch-Yaw Angles . .
3.2.5 Exponential Coordinates .
3.2.6 Unit Quaternions . . . . .
3.3 Rigid-Body Motions . . . . . . .
3.3.1 Definition . . . . . . . . .
3.3.2 Properties . . . . . . . . .
3.3.3 Screw Motions . . . . . .
3.4 Velocities and Forces . . . . . . .
3.4.1 Angular Velocities . . . .
3.4.2 Spatial Velocities . . . . .
3.4.3 Spatial Forces . . . . . . .
3.5 Summary . . . . . . . . . . . . .
3.6 Notes and References . . . . . . .
3.7 Exercises . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
39
40
44
44
46
48
51
53
61
63
63
64
69
76
76
78
81
83
88
89
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
CONTENTS
4 Forward Kinematics
4.1 Denavit-Hartenberg Representation . . . . . . . . . . . . . . .
4.1.1 Assigning Link Frames . . . . . . . . . . . . . . . . . .
4.1.2 Why Four Parameters are Sufficient . . . . . . . . . .
4.1.3 Manipulator Forward Kinematics . . . . . . . . . . . .
4.1.4 Examples . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Product of Exponentials Formula . . . . . . . . . . . . . . . .
4.2.1 First Formulation . . . . . . . . . . . . . . . . . . . . .
4.2.2 Examples . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.3 Relation with the Denavit-Hartenberg Representation
4.2.4 Second Formulation . . . . . . . . . . . . . . . . . . .
4.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.4 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
99
101
102
105
106
107
109
110
112
115
116
117
119
5 Velocity Kinematics and Statics

5.1 Manipulator Jacobian . . . . . . . . . . . . . . .
5.1.1 Space Jacobian . . . . . . . . . . . . . . .
5.1.2 Body Jacobian . . . . . . . . . . . . . . .
5.1.3 Relationship between the Space and Body
5.1.4 Alternative Notions of the Jacobian . . .
5.2 Statics of Open Chains . . . . . . . . . . . . . . .
5.3 Singularity Analysis . . . . . . . . . . . . . . . .
5.4 Manipulability . . . . . . . . . . . . . . . . . . .
5.5 Summary . . . . . . . . . . . . . . . . . . . . . .
5.6 Notes and References . . . . . . . . . . . . . . . .
5.7 Exercises . . . . . . . . . . . . . . . . . . . . . .
. . . . . .
. . . . . .
. . . . . .
Jacobian
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
125
129
130
134
136
137
138
140
145
147
149
150
6 Inverse Kinematics
6.1 Analytic Inverse Kinematics . . . . . . . .
6.1.1 6R PUMA-Type Arm . . . . . . .
6.1.2 Generalized 6R PUMA-Type Arms
6.1.3 Stanford-Type Arms . . . . . . . .
6.2 Numerical Inverse Kinematics . . . . . . .
6.3 Inverse Velocity Kinematics . . . . . . . .
6.4 A Note on Closed Loops . . . . . . . . . .
6.5 Summary . . . . . . . . . . . . . . . . . .
6.6 Notes and References . . . . . . . . . . . .
6.7 Exercises . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
169
171
171
174
179
181
183
184
184
185
186
7 Kinematics of Closed Chains

7.1 Inverse and Forward Kinematics . . . . . .
7.1.1 3RPR Planar Parallel Mechanism .
7.1.2 Stewart-Gough Platform . . . . . . .
7.1.3 General Parallel Mechanisms . . . .
7.2 Differential Kinematics . . . . . . . . . . . .
7.2.1 Stewart-Gough Platform . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
189
192
192
194
195
196
196
Park and Lynch
16:18 December 10, 2014
ii
CONTENTS
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
198
201
204
206
8 Dynamics of Open Chains

8.1 Lagrangian Formulation . . . . . . . . . . . . . . . .
8.1.1 Basic Concepts and Motivating Example . . .
8.1.2 General Formulation . . . . . . . . . . . . . .
8.2 Dynamics of a Single Rigid Body . . . . . . . . . . .
8.2.1 Classical Formulation . . . . . . . . . . . . .
8.2.2 Twist-Wrench Formulation . . . . . . . . . .
8.3 Inverse Dynamics of Open Chains . . . . . . . . . .
8.3.1 Overview of Newton-Euler Inverse Dynamics
8.3.2 Details of Newton-Euler Inverse Dynamics . .
8.4 Dynamic Equations in Closed Form . . . . . . . . . .
8.5 Forward Dynamics of Open Chains . . . . . . . . . .
8.6 Dynamics in Task Space Coordinates . . . . . . . . .
8.7 Summary . . . . . . . . . . . . . . . . . . . . . . . .
8.8 Notes and References . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
209
210
210
212
213
213
216
218
218
219
223
227
229
230
230
9 Trajectory Generation
9.1 Definitions . . . . . . . . . . . . . . . . . .
9.2 Point-to-Point Trajectories . . . . . . . .
9.2.1 Straight-Line Paths . . . . . . . .
9.2.2 Time Scaling a Straight-Line Path
9.3 Polynomial Via Point Trajectories . . . .
9.4 Time-Optimal Time Scaling . . . . . . . .
9.4.1 The (s, s)
Phase Plane . . . . . . .
9.4.2 The Time-Scaling Algorithm . . .
9.4.3 Assumptions and Caveats . . . . .
9.5 Summary . . . . . . . . . . . . . . . . . .
9.6 Exercises . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
231
231
232
232
234
238
240
243
244
246
248
250
10 Motion Planning
10.1 Overview of Motion Planning . . . . . . . . . . . . .
10.1.1 Types of Motion Planning Problems . . . . .
10.1.2 Properties of Motion Planners . . . . . . . .
10.1.3 Motion Planning Methods . . . . . . . . . . .
10.2 Foundations . . . . . . . . . . . . . . . . . . . . . . .
10.2.1 Configuration Space Obstacles . . . . . . . .
10.2.2 Distance to Obstacles and Collision Detection
10.2.3 Graphs and Trees . . . . . . . . . . . . . . . .
10.3 Complete Path Planners . . . . . . . . . . . . . . . .
10.3.1 A Search . . . . . . . . . . . . . . . . . . . .
10.3.2 Other Search Methods . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
255
255
256
257
258
259
259
263
264
265
267
268
7.3
7.4
7.5
7.2.2 General Parallel Mechanisms

Singularities . . . . . . . . . . . . . .
Summary . . . . . . . . . . . . . . .
Exercises . . . . . . . . . . . . . . .
Park and Lynch
iii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
16:18 December 10, 2014
CONTENTS
10.4 Grid Methods . . . . . . . . . . . . . . . . . . .
10.4.1 Multi-Resolution Grid Representation .
10.4.2 Grid Methods with Motion Constraints
10.5 Sampling Methods . . . . . . . . . . . . . . . .
10.5.1 The RRT . . . . . . . . . . . . . . . . .
10.5.2 The PRM . . . . . . . . . . . . . . . . .
10.6 Virtual Potential Fields . . . . . . . . . . . . .
10.6.1 A Point in C-space . . . . . . . . . . . .
10.6.2 Navigation Functions . . . . . . . . . . .
10.6.3 Workspace Potential . . . . . . . . . . .
10.6.4 Wheeled Mobile Robots . . . . . . . . .
10.6.5 Use of Potential Fields in Planners . . .
10.7 Nonlinear Optimization . . . . . . . . . . . . .
10.8 Smoothing . . . . . . . . . . . . . . . . . . . . .
10.9 Summary . . . . . . . . . . . . . . . . . . . . .
10.10Exercises . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
269
271
272
277
278
283
284
285
286
288
289
289
289
291
291
294
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
297
298
299
300
313
315
317
319
319
321
323
324
325
325
326
329
12 Grasping and Manipulation

12.1 Contact Kinematics . . . . . . . . . . . . . . . . . . . . . .
12.1.1 First-Order Analysis of a Single Contact . . . . . . .
12.1.2 Contact Types: Rolling, Sliding, and Breaking Free .
12.1.3 Multiple Contacts . . . . . . . . . . . . . . . . . . .
12.1.4 Collections of Parts . . . . . . . . . . . . . . . . . .
12.1.5 Other Types of Contacts . . . . . . . . . . . . . . . .
12.1.6 Planar Graphical Methods . . . . . . . . . . . . . . .
12.1.7 Form Closure . . . . . . . . . . . . . . . . . . . . . .
12.2 Contact Forces and Friction . . . . . . . . . . . . . . . . . .
12.2.1 Friction . . . . . . . . . . . . . . . . . . . . . . . . .
12.2.2 Planar Graphical Methods . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
337
338
338
339
342
344
345
346
350
356
356
359
11 Robot Control
11.1 Control System Overview . . . . . . . . .
11.2 Motion Control . . . . . . . . . . . . . . .
11.2.1 Motion Control of a Single Joint .
11.2.2 Multi-Joint Motion Control . . . .
11.2.3 Task Space Motion Control . . . .
11.3 Force Control . . . . . . . . . . . . . . . .
11.4 Hybrid Motion-Force Control . . . . . . .
11.4.1 Natural and Artificial Constraints
11.4.2 A Hybrid Controller . . . . . . . .
11.5 Impedance Control . . . . . . . . . . . . .
11.5.1 Impedance Control Algorithm . . .
11.5.2 Admittance Control Algorithm . .
11.6 Other Topics . . . . . . . . . . . . . . . .
11.7 Summary . . . . . . . . . . . . . . . . . .
11.8 Exercises . . . . . . . . . . . . . . . . . .
Park and Lynch
iv
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
16:18 December 10, 2014
CONTENTS
12.2.3 Force Closure . . . . . . . . . . . . . .
12.2.4 Duality of Force and Motion Freedoms
12.3 Manipulation . . . . . . . . . . . . . . . . . .
12.4 Exercises . . . . . . . . . . . . . . . . . . . .
Bibliography
Park and Lynch
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
360
364
365
371
379
16:18 December 10, 2014
CONTENTS
Park and Lynch
vi
16:18 December 10, 2014
Chapter 1
Preview
As an academic discipline, robotics is a relatively young field with highly ambitious goals, the ultimate one being the creation of machines that behave and
think like humans. This attempt to create intelligent machines naturally leads
us to first examine ourselvesto ask, for example, why our bodies are designed
the way they are, how our limbs are coordinated, and how we learn and refine complex motions. The sense that the fundamental questions in robotics
are ultimately questions about ourselves is part of what makes robotics such a
fascinating and engaging endeavor.
In contrast to the lofty goals set by robotics researchers, the aims of this
textbook are more modest. Our focus will be on the mechanics, planning and
control of robot mechanisms. Robot arms are one familiar example. So are
wheeled platforms, as are robot arms mounted on wheeled platforms. Basically,
a mechanism is constructed by connecting rigid bodies, called links, together
with joints, so that relative motion between adjacent links becomes possible.
Actuation of the joints, typically by electric motors, then causes the robot to
move and exert forces in desired ways.
The links of a robot mechanism can be arranged in serial fashion, like the
familiar serial-chain arm shown in Figure 1.1(a). Robot mechanisms can also
have closed loops, such as the Stewart-Gough platform shown in Figure 1.1(b).
In the case of a serial chain, all of the joints are actuated, while in the case of
mechanisms with closed loops only a subset of the joints may be actuated.
Let us examine more closely the current technology behind robot mechanisms. The links are moved by actuators, which are typically electrically driven
(e.g., DC or AC servo motors, stepper motors, even shape memory alloys), or
by pneumatic or hydraulic cylinders, or even by internal combustion engines. In
the case of rotating electric motors, they should ideally be lightweight, operate
at relatively low rotational speeds (e.g., in the range of hundreds of RPM) and
be able to generate large forces and torques. Since most currently available motors operate in the range of thousands of RPM, speed reduction devices with low
slippage and backlash are often required. Belts, sprockets, and spur gears are
usually not well-suited for this purpose; instead, specially designed low backlash
1
Preview
gears, harmonic drives, and ball screws are used to simultaneously reduce speed
and amplify the delivered torque. Brakes may also be attached to quickly stop
the robot or to maintain a stationary posture.
Robots are also equipped with sensors to measure the position and velocity at
the joints. For both revolute and prismatic joints, optical encoders measure the
displacement, while tachometers measure their velocity. Forces at the links or at
the tip can be measured using various types of force-torque sensors. Additional
sensors may be used depending on the nature of the task, e.g., cameras, sonar
and laser range finders to locate and measure the position and orientation of
objects.
This textbook is about the mechanics, motion planning, and control of such
robots. We now provide a preview of the later chapters.
Chapter 2: Configuration Space
(a) An open chain industrial manipulator.
(b) Stewart-Gough platform.
Figure 1.1: Open chain and closed chain robot mechanisms.

At its most basic level, a robot consists of rigid bodies connected by joints,
with the joints driven by actuators. In practice the links may not be completely
rigid, and the joints may be affected by factors such as elasticity, backlash,
friction, and hysteresis. In this book we ignore these effects and assume all links
are rigid. The most commonly found joints are revolute joints (allowing for
rotation about the joint axis) and prismatic joints (allowing for linear translation
along the joint axis). Revolute and prismatic joints have one degree of freedom
(either rotation or translation); other joints, such as the spherical joint (also
called the ball-in-socket joint), have higher degrees of freedom.
In the case of a serial chain robot such as the industrial manipulator of Figure 1.1(a), all of the joints are independently actuated. This is the essential idea
3
behind the degrees of freedom of a robot: it is the sum of all the independently actuated degrees of freedom of the joints. For serial chains the degrees of
freedom is obtained simply by adding up all the degrees of freedom associated
with the joints.
For closed chains like the Stewart-Gough platform shown in Figure 1.1(b),
the situation is somewhat more complicated. First, joints with multiple degrees
of freedom like the spherical joint are quite common. Second, it is usually
not possible to independently actuate all of the jointsfixing a certain set of
joints to prescribed values automatically determines the values of the remaining
joints. For even more complicated closed chains with multiple loops and different
joint types, determining the degrees of freedom may not be straightforward or
intuitive.
A more abstract but equivalent definition of the degrees of freedom of a robot
begins with the notion of its configuration space: a robots configuration
is a complete specification of the positions and orientations of each link of a
robot, and its configuration space is the set of all possible configurations of the
robot. The degrees of freedom, then, is the minimum number of independent
parameters required to specify the position and orientation of each of the links.
Based on this definition we obtain a formulaGr
ublers formulathat relates
the number of links and joints (including the degrees of freedom of each joint)
comprising a robot with its degrees of freedom.
Robot motion planning and control both begin by choosing coordinates that
parametrize the robots configuration space. Often the coordinates of choice
are the joint variables, and the configuration space can be parametrized either
explicitly or implicitly in terms of these joint variables. Also, to grasp and
manipulate objects, a robot is typically equipped with an end-effector, e.g., a
mechanical hand or gripper. The task space, also called the workspace, is the
configuration space of a frame attached to the end-effector. In this chapter we
study the various ways in which the configuration and task spaces of a robot
can be parametrized.
Chapter 3: Rigid-Body Motions

This chapter addresses the problem of how to mathematically describe the motion of a rigid body moving in three-dimensional physical space. One convenient
way is to attach a reference frame to the rigid body, and to develop a way to
quantitatively describe the frames position and orientation as it moves. As
a first step, we cover some preliminaries on the analysis of velocities and accelerations of particles with respect to moving frames. We then introduce the
3 3 matrix representation for describing a frames orientation; such a matrix
is referred to as a rotation matrix. Two well-known three-parameter representations for rotation matrices, the Euler angles and roll-pitch-yaw angles, are
described.
We then introduce the exponential representation for rotations. This representation, which can also be identified with the familiar angle-axis representation for rotations, is derived in a somewhat roundabout way as the solution
Preview
to a certain linear vector differential equation. Doing so allows us to, among

other things, proceed directly to the exponential description of general rigid
body motions, which forms the cornerstone for our later kinematic analysis of
serial chains.
The exponential description of rigid body motions can also be identified with
classical screw theory. In addition to the basic rules for the matrix representation and manipulation of rigid body motions, we also cover in detail the linear
algebraic constructs of screw theory, including the unified description of linear
and angular velocities as six-dimensional spatial velocities. Analogously, it
is also natural to combine three-dimensional forces and moments into a sixdimensional spatial force.
Chapter 4: Forward Kinematics

For an open chain, the position and orientation of the end-effector are uniquely
determined from the joint positions. This is precisely the forward kinematics
problem for a robot: given a set of input joint values, find the output position
and orientation of the reference frame attached to the end-effector. In this chapter we study two methods for describing the forward kinematics of open chains:
the Denavit-Hartenberg (D-H) representation and the product-of-exponentials
(PoE) formula. The D-H representation uses a fewer number of parameters,
but requires that reference frames be attached to each link. The PoE formula
requires more parameters, but there is no need to attach reference frames to
each link. Instead it relies solely on information about the location of each joint
axisessentially, a line in spacemaking this the preferred forward kinematic
representation for our subsequent analysis in the later chapters.
Chapter 5: Velocity Kinematics and Statics

Velocity kinematics refers to the relationship between joint rates and the linear
and angular velocities of the end-effector frame. Central to velocity kinematics is
the Jacobian of the forward kinematics. By multiplying the vector of joint rates
by this matrix, the linear and angular velocities of the end-effector frame can be
obtained for any given robot configuration. Kinematic singularities, which
are configurations in which the end-effector frame loses the ability to move or
rotate in one or more directionsimagine, for example, a two-link planar chain
with its two links folded over each othercorrespond to those configurations at
which the Jacobian matrix fails to have maximal rank. The closely related and
more general notion of the manipulability ellipsoid, whose shape indicates
the ease with which the robot can move in various directions, is also derived
from the Jacobian.
Finally, the Jacobian is also central to static force analysis. In static equilibrium settings, the Jacobian is used to determine what forces and torques need
to be exerted at the input joints in order for the end-effector to apply a certain
force or moment. In this chapter we show how to obtain the Jacobian for general
serial chains, and its many practical uses in the above and other settings.
Chapter 6: Inverse Kinematics

The inverse kinematics problem is to determine the set of joint positions
that achieves a desired end-effector configuration. For serial chain robots, the
inverse kinematics is in general more involved than the forward kinematics: for
a given set of joint values there usually exists a unique end-effector position and
orientation, but for a particular end-effector position and orientation, there may
exist multiple solutions, or even none at all.
In this chapter we first examine a special class of six-dof serial chain structures whose inverse kinematics admits a closed-form analytic solution. Iterative
numerical algorithms are then derived for solving the inverse kinematics of general six-dof serial chains. We also examine the inverse kinematics of redundant
serial chains (that is, those with more than seven degrees of freedom) in the context of tracking a desired end-effector trajectory. For this problem, we present
a solution for obtaining the corresponding input joint rates that relies on the
generalized inverse of the forward kinematics Jacobian.
Chapter 7: Kinematics of Closed Chains

While serial chains have unique forward kinematics solutions, closed chains often
have multiple forward kinematics solutions. Also, because closed chains possess
both actuated and passive joints, the kinematic singularity analysis of closed
chains presents unusual subtleties not encountered in serial chains. In this
chapter we study the basic concepts and tools for the kinematic analysis of
closed chains. We begin with a detailed case study of mechanisms like the
planar five-bar linkage and the Stewart-Gough Platform. These results are then
generalized into a systematic methodology for the kinematic analysis of more
general closed chains.
Chapter 8: Dynamics of Open Chains

This chapter derives the dynamic equations for serial chains. As a first step, the
dynamic equations for a single rigid body are derived in terms of spatial velocities, accelerations, and forces. The dynamics for a serial chain robot are then
derived by applying the single body equation to each link of the robot. Analogous to the notions of forward and inverse kinematics, the forward dynamics
problem involves determining the resulting joint trajectory for a given input joint
torque profile. Similarly, the inverse dynamics problem is concerned with determining the input joint torque profile for a desired joint trajectory. The main
result of this chapter is a set of recursive algorithms for the forward and inverse
dynamics problem. Unlike traditional methods that rely on a separate analysis
of the linear and angular components of the dynamic equations, our algorithms
are formulated entirely in terms of six-dimensional spatial quantities, and rely
upon the product-of-exponentials formula to model the kinematics.
Preview
Chapter 9: Trajectory Generation

What sets a robot apart from an automated machine is that it should be easily
reprogrammable for different tasks. Different tasks require different motions,
and it would be unreasonable to expect the user to specify the entire time-history
of each joint for every task; clearly it would be desirable for the computer to
fill in the details from a small set of task input data.
This chapter is concerned with the automatic generation of joint trajectories
from this set of task input data. Often this input data is given in the form of an
ordered set of joint values, called control points, together with a corresponding
set of control times. Based on this data the trajectory generation algorithm
produces a smooth trajectory for each joint that satisfies various user-supplied
conditions.
We study some popular algorithms for trajectory generation that were originally developed for computer-aided curve design applications. Algorithms that
are of particular interest are cubic splines and Bezier curves. Versions of these
algorithms are offered for the generation of trajectories in both joint space and
end-effector space. In the latter case, algorithms are presented for interpolating
through an ordered set of end-effector reference frames.
Chapter 10: Motion Planning

This chapter addresses the problem of finding a collision-free path for a robot
through a cluttered workspace. The most intuitive approach is to work in configuration space, which because of the workspace obstacles have forbidden regions.
We begin with the most basic of planning algorithms involving grid search. A
convenient way to navigate through such cluttered regions is through the use
of artificial potential fields. Briefly, in these methods the obstacles generate
artificial forces that repel the robot should it venture too near. When they
work, these methods have the advantage of being able to generate collisionfree trajectories in real-time. There are a number of subtleties associated with
these methods, however, e.g., the possibility of getting stuck in local equilibria without reaching the goal configuration, and we discuss some methods
to overcome these difficulties. We also cover a basic randomized algorithm for
planning collision-free paths, based on rapidly-exploring random trees.
Chapter 11: Robot Control

A robot arm can exhibit a number of different behaviors depending on the task
and its environment. It can act as a source of programmed motions for tasks
such as moving an object from one place to another, or tracing a trajectory for
manufacturing applications. It can act as a source of forces, for example when
grinding or polishing a workpiece. In tasks such as writing on a chalkboard, it
must control forces in some directions (the force pressing the chalk against the
board) and motions in others (motion in the plane of the board). In certain
applications, e.g., haptic displays, we may want the robot to act like a spring,
7
damper, or mass, controlling its position, velocity, or acceleration in response
to forces applied to it.
In each of these cases, it is the job of the robot controller to convert the
task specification to forces and torques at the actuators. Control strategies
to achieve the behaviors described above are known as motion (or position)
control, force control, hybrid motion-force control, and impedance control. Which of these behaviors is appropriate depends on both the task and
the environment. For example, a force control goal makes sense when the endeffector is in contact with something, but not when it is moving in free space. We
also have a fundamental constraint imposed by mechanics, irrespective of the
environment: the robot cannot independently control both motions and forces
in the same direction. If the robot imposes a motion, then the environment
determines the force, and vice versa.
Most robots are driven by actuators that apply a force or torque to each
joint. Hence, to precisely control a robot would require an understanding of
the relationship between joint forces and torques and the motion of the robot;
this is the domain of dynamics. Even for simple robots, however, the dynamic
equations are usually very complex, Also, to accurately derive the dynamics
requires, among other things, precise knowledge of the mass and inertia of each
link, which may not be readily available. Even if they were, the dynamic equations would still not reflect physical phenomena like friction, elasticity, backlash,
and hysteresis.
Most practical control schemes compensate for these errors by using feedback. One effective method of industrial robot control is to neglect the robots
dynamics, and instead model each actuator as a scalar second-order linear system. As such we first introduce basic concepts from linear control, and show
how they can be used to effectively control complex multi-dof robots.
This chapter also introduces some basic robot control techniques that assume a dynamic model of the robot is available; such feedforward control
techniques use the dynamic model of the robot and its environment to determine actuator control inputs that achieve the desired task. Because of modeling
and other errors, feedforward control is rarely used by itself, but is often used
in conjunction with feedback control. After considering feedback and forward
strategies for model-based motion control, we then examine force control, hybrid
motion-force control, and impedance control.
Chapter 12: Grasping and Manipulation

The focus of the previous chapters has been mostly on the internal characterization of the robotits kinematics and dynamics, as well as methods for motion
planning and control. In this chapter we now explicitly consider physical interactions between the robot and its environment. The first order of business is to
characterize the nature of contacts between the robot and objects, or more generally, contact constraints between rigid bodies. As such it becomes necessary
to consider friction, which we considered earlier in the chapter on force closure,
at least in the case for point contacts. We examine the equations of motion for
Preview
rigid body mechanics with friction, formulate the general problem of manipulation plannning and grasping, and examine simplifications and assumptions that
lead to certain basic solutions.
Chapter 13: Wheeled Mobile Robots

This chapter addresses the kinematics, motion planning, and control of wheeled
robots that are subject to no-slip rolling constraints. Such constraints are fundamentally different from the loop closure constraints found in closed chainsthe
former are holonomic, the latter nonholonomicand as such we begin with
a discussion of nonholonomic constraints. We then examine the kinematics
of some popular wheeled robots: car-like, differential drive, Dubins, and omnidirectional robots. The controllability problem of determining whether a
wheeled robot is able to move from a given initial configuration to an arbitrary
final configuration is then examined. The chapter concludes with a discussion
of motion planning and control algorithms for wheeled robots, including the
problem of characterizing and finding optimal paths, and feedback control of
wheeled robots.
Chapter 2
Configuration Space
A typical robot is mechanically constructed from several bodies, or links, that
are connected by various types of joints. The robot moves when certain joints
are driven by actuators (such as electric motors) that deliver forces or torques
to the joints. Usually an end-effector, such as a gripper or hand for grasping
and manipulating objects, is attached to some link of the robot. All of the robots
considered in this book will have links that can be modeled as rigid bodies.
Given a particular robot, perhaps the most fundamental question one can
ask is where is the robot?1 The answer to this question is the robots configurationa specification of the positions of all points of the robot. Since
the robots links are rigid and of known shape, only a few numbers are needed
to represent the configuration.2 For example, to represent the configuration of
a door, we need only one number, the angle that the door rotates about its
hinge. The configuration of a point on a plane can be described by two coordinates, (x, y). To represent the configuration of a coin lying on a flat table,
we need three coordinates: two specifying the location (x, y) on the table of a
particular point on the coin, and one specifying the coins orientation, . (See
Figure 2.1.)
The minimum number of real-valued coordinates needed to represent the
configuration is the degrees of freedom (dof) of the robot. Thus a coin
(viewed as a robot) lying on a table has three degrees of freedom. If the coin
could lie heads up or tails up, the configuration space still has only three degrees
of freedom, since the fourth variable, representing which side of the coin is
up, can only take values in the discrete set {heads, tails}; it does not take a
continuous range of real values as required by our definition.
Definition 2.1. The configuration of a robot is a complete specification of
the positions of every point of the robot. The minimum number n of realvalued coordinates needed to represent the configuration is the degrees of
freedom (dof) of the robot. The n-dimensional space containing all possible
1 In
the sense of where are the links of the robot situated?

with trying to represent the configuration of a pillow.
2 Compare
10
Configuration Space
y^
y^
(x, y)
(x, y)
x^
(a)
(b)
x^
(c)
Figure 2.1: (a) The configuration of a door is described by its angle . (b) The
configuration of a point in a plane is described by coordinates (x, y). (c) The
configuration of a penny on a table is described by (x, y, ).
configurations of the robot is called the configuration space.
In this chapter we study the configuration space and degrees of freedom of
general robots. Since our robots are constructed of rigid bodies, we first examine
the configuration space and degrees of freedom of rigid bodies. We then examine
these concepts for general robots. The chapter concludes with a discussion of
the configuration space of a robots end-effector, or its task space. In the next
chapter we study in more detail the various mathematical representations for
the configuration space of rigid bodies.
2.1
Configuration Space of a Rigid Body
Continuing with the example of the coin lying on the table, choose three points
A, B, and C fixed to the coin (Figure 2.2(a)). The positions of these points
in the plane are written (xA , yA ), (xB , yB ), and (xC , yC ). If these points could
be placed independently anywhere in the plane, we would have six degrees of
freedomtwo for each of the three points. However, a rigid body is defined as
one where all points on the body maintain constant distances from each other,
i.e.,
p
d(A, B) = (xA xB )2 + (yA yB )2
p
d(B, C) = (xB xC )2 + (yB yC )2
p
d(A, C) = (xA xC )2 + (yA yC )2
cannot change no matter how the coin moves. Lets call these constant distances
dAB , dBC , and dAC .
To determine the degrees of freedom of the coin, we first choose the position
of point A in the plane (Figure 2.2(b)). We may choose it to be anything we
want, so we have two degrees of freedom to specify, (xA , yA ). Once this is
specified, however, the constraint d(A, B) = dAB restricts the choice of (xB , yB )
to those points on the circle of radius dAB centered at A. Thus the two apparent
dAC
dAB
B
dBC
^x
2.1. Configuration Space of a Rigid Body
C
A
11
z^
y^
dAC
A
dBC
dAB
B
^x
(a)
(b)
y^
x^
(c)
Figure 2.2: (a) Choosing three points fixed to the penny. (b) Once the location
of A is chosen, B must lie on a circle of radius dAB centered at A. Once the
location of B is chosen, C must lie at the intersection of circles centered at
A and B. Only one of these two intersections corresponds to the heads up
configuration. (c) The configuration of a penny in three-dimensional space is
given by the three coordinates of A, two angles to the point B on the sphere of
radius dAB centered at y^A, andA one angle to the point C on the circle defined by
dAC at A and a sphere
the intersection of the a sphere centered
at B.
A
y^ centered
dAB
dBC
dAB
dAC
C
freedoms (xB and yB ) are reduced
by one constraint, for a total of one actual
B
dBC
freedom in choosing the location of B. This freedom is the angleB specifying the
location of B on the circle centered at A; lets
call this angle AB , and define it
x^
^x
to be the angle that the vector AB makes with the x
-axis.
Finally, once we have chosen the location of point B, the two apparent
freedoms to place C (xC and yC ) are eliminated by the two constraints d(A, C) =
dAC and d(B, C) = dBC . In other words, once we have placed A and B, the
location of C is fixed. The coin has exactly three degrees of freedom in the
plane, which can be specified by (xA , yA , AB ).
Suppose we were to choose an additional point on the coin, D, thus introducing three additional constraints: d(A, D) = dAD , d(B, D) = dBD , and
d(C, D) = dCD . Although these constraints may appear to be independent,
they are in fact redundantonly two of the three constraints are independent.
To see why, the constraint d(A, D) = dAD restricts D to lie on a circle of radius
dAD centered at A. Similarly, the constraint d(B, D) = dBD restricts D to lie
on a circle of radius dBD centered at B. These two constraints together thus
uniquely specify D to be the point of intersection of these two circles. The third
constraint dCD = dCD is therefore not needed; points C, D, and in fact all other
points on the coin, do not add any more freedoms or constraints to the coins
configuration space.
We have been applying the following general rule for determining the number
of degrees of freedom of a system:
Degrees of freedom = (Sum of freedoms of the points)

(Number of independent constraints).
(2.1)
12
Configuration Space
This rule can also be expressed in terms of the number of variables and independent equations that describe the system:
Degrees of freedom = (Number of variables)
(Number of independent equations).
(2.2)
We can use this general rule to determine the number of freedoms of a rigid
body in three dimensions as well. For example, assume our coin is no longer
confined to the table (Figure 2.2(c)). The three points A, B, and C are now
defined by (xA , yA , zA ), (xB , yB , zB ), and (xC , yC , zC ). Point A can be placed
freely (three degrees of freedom). The location of point B is subject to the
constraint d(A, B) = dAB , meaning it must lie on the sphere of radius dAB
centered at A. Thus we have 3 1 = 2 freedoms to specify, which can be
expressed as a latitude and longitude for a point on the sphere. Finally, the
location of point C must lie at the intersection of spheres centered at A and B
of radius dAC and dBC , respectively. The intersection of two spheres is a circle,
and thus the location of point C can be described by an angle that parametrizes
this circle. Point C therefore adds 3 2 = 1 freedom. Once the position of point
C is chosen, the coin is fixed in space.
In summary, a rigid body in three-dimensional space has six freedoms, which
can be described by the three coordinates parametrizing point A, the two angles parametrizing point B, and one angle parametrizing point C. Other sixparameter representations for the six freedoms of a rigid body are discussed in
Chapter 3.
We have just estabilshed that a rigid body moving in three-dimensional
space, which we henceforth call a spatial rigid body, has six degrees of freedom, three of which are linear and three angular. Similarly, a rigid body moving
in a two-dimensional plane, which we henceforth call a planar rigid body, has
two linear freedoms and one angular freedom. This latter result can also be obtained by considering the planar rigid body to be a spatial rigid body with six degrees of freedom, but with the three independent constraints zA = zB = zC = 0
imposed.
Since our robots will be constructed of rigid bodies, we express Equation (2.1)
as follows:
Degrees of freedom = (Sum of freedoms of the bodies)
(Number of independent constraints).
(2.3)
Equation (2.3) forms the basis for determining the degrees of freedom of general
robots, which is the topic of the next section.
2.2
Configuration Space of a Robot
Consider once again the door example of Figure 2.1(a), consisting of a single
rigid body connected to the wall by a hinge joint. From the previous section we
know that the door has only one degree of freedom, conveniently parametrized
2.2. Configuration Space of a Robot
13
by the hinge joint angle . The reasoning goes as follows. Without the hinge
joint, the door is free to move in three-dimensional space and has six degrees of
freedom. By connecting the door to the wall via the hinge joint, five independent
constraints are imposed on the motion of the door; from Equation (2.3) we
conclude that the door has one degree of freedom. Alternatively, we can view
the door from above and regard it as a planar body, which has three degrees of
freedom. The hinge joint then imposes two independent constraints, and again
from Equation (2.3) we conclude that the door has one degree of freedom. Its
configuration space is parametrized by some range in the interval [0, 2] over
which is allowed to vary.
In both cases we see that joints have the effect of constraining the motion
of the rigid body, and thus reducing the overall degrees of freedom. It seems
plausible that a formula can be obtained for determining the degrees of freedom
of a robot, simply by counting the number of rigid bodies and joints. This is in
fact the case, and in this section we derive Gr
ublers formula for determining the
degrees of freedom of planar and spatial robots. We then examine mathematical
representations of a robots configuration space.
2.2.1
Degrees of Freedom of a Robot
Robot Joints
Figure 2.3 illustrates the basic joints found in typical robots. Every joint connects exactly two links; we do not allow joints that simultaneously connect three
or more links. The revolute joint (R), also called a hinge joint, allows for rotational motion about the joint axis. The prismatic joint (P), also called a
sliding or linear joint, allows for translational (or rectilinear) motion along the
direction of the joint axis. Both revolute and prismatic joints are one-dof joints;
for the revolute joint, its degree of freedom is parameterized by the angle of
rotation about the joint axis, while the prismatic joints degree of freedom is
parameterized by the translation distance along the joint axis. The screw joint
(H), also called a helical joint, is another one-dof joint whose motion consists of
simultaneous rotation and translation.
Joints can also have multiple degrees of freedom. The cylindrical joint
(C) is a two-dof joint that allows for independent translations and rotations
about a single fixed joint axis. The universal joint (U) is another two-dof
joint constructed by serially connecting a pair of revolute joints so that their
joint axes are orthogonal. The spherical joint (S), also called a ball-and-socket
joint, has three degrees of freedom and functions much like our shoulder joint.
Gr
ublers Formula for Planar Robots
Gr
ublers formula allows one to calculate the mobility of a robot based on the
number of links, and the number and type of joints. The formula is in fact a
more formal expression of Equation 2.3, and in this section we state this formula
for planar robots, i.e., robots whose links all move in a single fixed plane. First,
14
Configuration Space
Figure 2.3: Typical robot joints.
Figure 2.4: Coordinates for a planar rigid body.

consider a single rigid body in the plane (Figure 2.4). Choose two points A
and B whose (x, y) coordinates with respect to some fixed reference frame are
(xA , yA ) and (xB , yB ), respectively. Of the four coordinates, we know that only
three of these coordinates can be chosen independently, because of the rigid
body constraint that the distance between A and B is a fixed constant, i.e.,
p
dAB = (xA xB )2 + (yA yB )2
(2.4)
is constant. Define AB to be the angle between the x-axis of the fixed frame
and the vector AB. Then (xA , yA , AB ) serve as a set of three independent
coordinates that parametrizes the configuration of the planar rigid body. In
15
fact, (xB , yB ) can be determined to be

xB
xA + dAB cos AB
yB
yA + dAB sin AB .
Now consider another planar rigid body with two points C and D chosen.
Let their coordinates with respect to the same fixed frame be (xC , yC ) and
(xD , yD ), respectively, subject to the distance constraint
p
(2.5)
dCD = (xC xD )2 + (yC yD )2
is constant. Defining angle CD in a similar fashion, it then follows that
xD
= xC + dCD cos CD
yD
= yC + dCD sin CD .
Suppose point B of the first body and point C of the second body are connected by a revolute joint. Then in addition to the distance constraints (2.4)
and (2.5), the revolute joint connecting the two bodies imposes an additional
two independent constraints:
xB
= xC
(2.6)
yB
= yC
(2.7)
Thus, we have eight coordinates (xA , yA , . . . , xD , yD ) and four constraint equations, leading to 8 4 = 4 degrees of freedom for this system. If we define the
revolute joint angle by = CD AB , then (xA , yA , AB , ) serve as a convenient set of four independent coordinates that parametrize the configuration
space of this system.
If points B and C are instead connected by a prismatic joint, allowing for
translation inqsome fixed direction (tx , ty ) (assume this direction vector is a unit
vector, i.e.,
replaced by
t2x + t2y = 1), then the two constraints (2.6) and (2.7) are now
xC
= xB + d t x
(2.8)
yC
= yB + d t y ,
(2.9)
where the scalar d now represents the translation distance in the direction
(tx , ty ). Again we have four constraints imposed on eight variables, leading to
8 4 = 4 degrees of freedom for this system. The coordinates (xA , yA , AB , d)
serve as a set of four independent coordinates that parametrize the configuration
space of this system.
Now suppose the two bodies are instead connected by a two-dof joint, obtained as the serial connection of the above R and P joints. In this case deriving
the additional constraints on the eight point coordinates {(xA , yA ), . . . , (xD , yD )}
is not as straightforward as in the two previous cases. It would seem, for example, that the constraints will depend on the sequence in which the R and P
16
Configuration Space
Figure 2.5: (a) Four-bar linkage (b) slider-crank mechanism.

joints are connected, e.g., whether it is an RP joint or a P R joint. What is clear,
however, is that knowledge of the two joint values and d completely specifies
coordinates (xC , yC ) and (xD , yD ) on the second body. Equivalently, the eight
coordinates for points {A, B, C, D} can be uniquely determined from knowledge
of the five variables (xA , yA , AB , , d). The two-dof joint thus imposes a single
constraint on the six independent variables (xA , yA , AB , xC , yC , CD ), leading
to five degrees of freedom.
In summary, a one-dof joint reduces the degrees of freedom of the system
by two, while a two-dof joint reduces the degrees of freedom by one. This
observation is in fact exactly what is expressed by (2.3). We now state Gr
ublers
formula for planar robots:

Proposition 2.1. Consider a planar robot consisting of N links, where the
ground is also regarded as a link. Let J denote the total number of joints, and
let fi be the degrees of freedom of the i-th joint. The degrees of freedom (dof )
of the robotic mechanism can then be evaluated from the formula
dof
3(N 1)
J
X
(3 fi )
i=1
3(N 1 J) +
J
X
fi
(2.10)
i=1
Example 2.1. Four-bar linkage and slider-crank mechanism

Consider the four-bar linkage shown in Figure 2.5(a). This planar mechanism
consists of four links (one of them ground) connected in a single closed loop by
four revolute joints. Subsituting N = 4, J = 4, and fi = 1, i = 1, . . . , 4 into the
planar version of Gr
ublers formula, we determine that the four-bar linkage has
one degree of freedom.
The slider-crank mechanism of Figure 2.5(b) can be analyzed in two ways: (i)
the mechanism consists of three revolute joints and one prismatic joint (J = 4,
and each fi = 1), and four links (N = 4, including the ground link), or (ii) the
mechanism consists of two revolute joints (fi = 1) and one RP joint (fi = 2;
the RP joint is a concatenation of a revolute and prismatic joint), and three
links (N = 3; remember that each joint connects precisely two bodies). In both
cases the the mechanism has one degree of freedom.
17
Figure 2.6: (a) k-link planar serial chain. (b) Five-bar planar linkage. (c)
Stephenson six-bar linkage. (d) Watt six-bar linkage.
Example 2.2. Some classical planar mechanisms
Let us now apply Gr
ublers formula to several classical planar mechanisms as
shown in Figure 2.6. For the k-link planar serial chain in (a), N = k + 1 (k
links plus ground), J = k, and since all the joints are revolute, each fi = 1.
Therefore,
dof = 3((k + 1) 1 k) + k = k
as expected. For the five-bar linkage of (b), N = 5 (4 links plus ground), J = 5,
and since all joints are revolute, each fi = 1. Therefore,
dof = 3(5 1 5) + 5 = 2.
For the Stephenson six-bar linkage in (c), we have N = 6, J = 7, and fi = 1 for
all i, so that
dof = (3(6 1 7) + 7 = 1.
Finally, for the Watt six-bar linkage in (d), we have N = 6, J = 7, and fi = 1
for all i, so that like the Stephenson six-bar linkage,
dof = (3(6 1 7) + 7 = 1.
Example 2.3. A planar mechanism with overlapping joints
Consider the planar mechanism illustrated in Figure 2.7. Again, there are a
18
Configuration Space
Figure 2.7: A planar mechanism with two overlapping joints.
Figure 2.8: (a) A parallelogram linkage; (b) The five-bar linkage in a regular
and singular configuration.
number of ways to derive its mobility using Gr
ublers formula. If all the joints
are regarded as either revolute or prismatic, then the mechanism consists of
eight links (N = 8), eight revolute joints, and one prismatic joint. Note that
three bodies meet at a single point on the right. Recalling that a joint connects
exactly two links, the joint at this intersection point is not a single revolute
joint, but in fact two revolute joints overlapping each other. Substituting into
Gr
ublers formula,
dof = 3(8 1 9) + 9(1) = 3.
Alternatively, the revolute-prismatic pair connected to this intersection point
can be regarded as a single two-dof joint. In this case the number of links is
N = 7, with seven revolute joints and a single two-dof revolute-prismatic pair.
Gr
ublers formula identically yields
dof = 3(7 1 8) + 7(1) + 1(2) = 3.
Example 2.4. Gr
ublers formula and singularities
Consider the parallelogram linkage of Figure 2.8(a). Here N = 5, J = 6, and
fi = 1 for each link; from Gr
ublers formula, the degrees of freedom is given by
3(5 1 6) + 6 = 0. A mechanism with zero degrees of freedom is by definition
a rigid structure. However, if the three parallel links are of the same length
and the two horizontal rows of joints are collinear as implied by the figure, the
mechanism can in fact move, with one degree of freedom.
19
A similar situation occurs for the five-bar linkage of Figure 2.8(b). If the
two joints connected to ground are fixed, then the five-bar linkage should, given
that it has two degrees of freedom, become a rigid structure. Note however that
if the two middle links overlap each other as shown in the figure, then these
links are able to rotate freely about the two overlapping joints. Of course, the
link lengths need to be chosen in such a way that this configuration is feasible.
If a different pair of joints is fixed, then the mechanism does become a rigid
structure as expected.
Gr
ublers formula is unable to distinguish singular cases like those just described. These phenomena are examples of configuration space singularities
arising in closed chains, which is discussed in detail in the chapter on closed
chain kinematics.
Gr
ublers Formula for Spatial Robots
Having established that a minimum of six independent parameters is required
to describe the position and orientation of a spatial rigid body, we now proceed
as we did in the derivation of the planar version of Gr
ublers formula. If two
spatial rigid bodies are connected by a k-dof joint, then only 12 (6 k) = 6 + k
independent parameters are required. Based on this observation, Gr
ublers
formula for spatial robots can be stated as follows:
Proposition 2.2. Consider a spatial robot consisting of N links (including
ground), J joints (labelled from from 1 to J), and denote by fi the degrees
of freedom of joint i. The degrees of freedom (dof ) of the robot can then be
evaluated from the formula
dof
6(N 1)
J
X
(6 fi )
i=1
6(N 1 J) +
J
X
fi .
(2.11)
i=1
We remind the reader that the spatial version of this formula should not
be applied to planar mechanisms, and vice versa. We now consider some wellknown examples of spatial robots.
Example 2.5. Stewart-Gough Platform
The Stewart-Gough platform shown in Figure 2.9 consists of two platforms
the lower one stationary, the upper one mobileconnected by six serial chain
structures, or legs. Each leg consists of a universal-prismatic-spherical (UPS)
joint arrangement. The total number of links (including the fixed lower platform,
which is regarded as the ground link) is N = 14. There are six universal joints
(each with two degrees of freedom, fi = 2), six prismatic joints (each with
a single degree of freedom, fi = 1), and six spherical joints (each with three
20
Configuration Space
Figure 2.9: The Stewart-Gough Platform.

degrees of freedom, fi = 3). The total number of joints is 18. Substituting
these values into the spatial version of Gr
ublers formula,
dof = 6(14 1 18) + 6(1) + 6(2) + 6(3) = 6.
In some versions of the Stewart-Gough platform the six universal joints are
replaced by spherical joints. Gr
ublers formula then indicates that this mechanism has twelve degrees of freedom. This result may seem surprising but is
correct; replacing each universal joint by a spherical joint introduces an extra
degree of mobility in each leg, by allowing torsional rotations about the leg
axis. Note however that this torsional rotation has no effect on the motion of
the mobile platform.
Example 2.6. 3 U P U platform
The next example, shown in Figure 2.10 is also a platform structure, in which
the two platforms are connected by three U P U serial chain structures. The total
number of links is N = 8. There are six universal joints, and three prismatic
joints, making for a total of J = 9 joints. Substituting these values into Gr
ublers
formula leads to
dof = 6(8 1 9) + 3(1) + 6(2) = 3.
The 3 U P U mechanism has three degrees of freedom according to Gr
ublers
formula, but constructed prototypes of this mechanism reveal extra degrees of
freedom not predicted by Gr
ublers formula. These and other subtle aspects of
the 3 U P U platform are further discussed in Chapter 7.
21
Figure 2.10: The 3 U P U platform.
Figure 2.11: The configuration space of the 2R planar serial chain.
2.2.2
Parametrizing the Configuration Space
Recall that by definition a robots configuration is a complete specification of

the positions of every point on the robot, and that the set of all configurations
constitutes the robots configuration space. For a single planar rigid body, its
configuration can be completely characterized by specifying three independent
coordinates: (x, y, ), where (x, y) R2 and [0, 2). The configuration
space is thus three-dimensional, and the coordinates (x, y, ) are an explicit
parametrization. Alternatively, we could choose to parametrize its configura-
22
Configuration Space
Figure 2.12: The four-bar linkage.

tion by the coordinates of two points A and B on the body, denoted (xA , yA )
and (xB , yB ), respectively. Of course, the rigid body constraint
p
(xA xB )2 + (yA yB )2 = dAB
(2.12)
where dAB is constant, means these four coordinates are not independent. Thus
the equality (2.12) is an implicit parametrization of the configuration space.
The configuration spaces of general robots usually admit both explicit and
implicit parametrizations. In the case of open chains, an explicit parametrization for the configuration space is offered by the joint variables. For the planar
2R open chain of Figure 2.11, specifying values for the two revolute joints (q1 , q2 )
completely specifies the position and orientation of the two links. The configuration space in this case is [0, 2] [0, 2], which can be visualized as the
two-dimensional torus (the doughnut-like surface) shown in Figure 2.11. Note
that by cutting the torus along the two solid lines indicated in the figure, the
torus can be flattened into the rectangle on the right (called the flat torus).
For closed chains (that is, robots containing one or more closed loops), usually an implicit parametrization is more easily obtained; obtaining an explicit
parametrization will typically involve solving a set of nonlinear equations. As
an example, consider the planar four-bar linkage of Figure 2.12, which has one
degree of freedom. The fact that the four links always form a closed loop can
be expressed in the form of the following three equations:
L1 cos 1 + L2 cos(1 + 2 ) + . . . + L4 cos(1 + . . . + 4 )
L1 sin 1 + L2 sin(1 + 2 ) + . . . + L4 sin(1 + . . . + 4 )
1 + 2 + 3 + 4 +
0.
These equations are obtained by viewing the four-bar linkage as a serial chain
with four revolute joints, in which (i) the tip of link L4 always coincides with
the origin, and (ii) the orientation of link L4 is always horizontal.
These equations are sometimes referred to as the loop closure equations. For
the four four-bar linkage they are given by a set of three equations in four unknowns. The set of all solutions forms a curve in the four-dimensional joint space
23
parametrized by the coordinates (1 , . . . , 4 ), and constitutes the configuration

space.
For general robots containing one or more closed loops, the configuration
space can be implicitly parametrized by loop closure equations of the form
g1 (1 , . . . , n )
..
(2.13)
g() =
= 0,
.
gk (1 , . . . , n )
where Rn denotes the vector of joint variables, and g : Rn Rk is a set of k
independent equations, with k n. Such constraints are known as holonomic
constraints. The configuration space can thus be viewed as a multidimensional
surface of dimension n k embedded in Rn .
2.2.3
Pfaffian Constraints
Suppose a closed chain robot with loop closure equations g() = 0, g : Rn Rk ,

is in motion, following the time trajectory (t). Then differentiating both sides
of g((t)) = 0 with respect to t, we obtain
g1
+ . . . +
()n
n
..
=
.
gk
gk
1 ()1 + . . . + n ()n
g1
g1
1
1 ()
n ()
..
..
..
..
=
.
.
.
.
gk
n
() gk ()
d
g((t))
dt
g1
1 ()1
g
=
()
= 0.
(2.14)
kn
Here i denotes the time derivative of i with respect to time t, g
,
() R
n
and , R . From the above we see that the joint velocity vector Rn
cannot be arbitrary, but must always satisfy
g
() = 0.
(2.15)
A() = 0,
(2.16)
Constraints on of the form
where A() Rkn , are called Pfaffian constraints. We saw earlier that
differentiating the loop closure equation g((t)) = 0 with respect to t leads to
such a constraint. Conversely, one could regard g() as being the integral of
24
Configuration Space
Figure 2.13: A coin rolling on a plane without slipping.

g
();
for this reason constraints of the form g() = 0 are also called integrable
constraints, or holonomic constraints.
We now consider another class of Pfaffian constraint that is fundamentally
different from the holonomic type. To illustrate with a concrete example, consider the coin of radius r rolling on the plane as shown in Figure 2.13. A
body-fixed frame is attached to the center of the coin, in such a way that the
axis of rotation is always directed along the x
-axis. Since this frame is attached
to the coin, the y- and z-axes rotate about the x
-axis at the same rate as the
disk. The coin is assumed to always remain normal to the plane (that is, the
x
-axis of the coin reference frame always remains parallel to the plane).
Under these assumptions, a minimum of four parameters are needed to completely describe the configuration of the coin. A convenient set is given by the
contact point coordinates (x, y), the steering angle , and the angle of rotation
(see Figure 2.13). The configuration space of the coin is therefore R2 T 2 ,
where T 2 is the two-dimensional torus parametrized by the angles and . This
configuration space is four-dimensional.
Let us now express, in mathematical form, the fact that the coin rolls without
slipping. The coin must always roll in the direction indicated by (cos , sin ),
with forward speed r:

x
cos
= r
.
(2.17)
y
sin
Collecting the four configuration space coordinates into a single vector q =
(x, y, , ) = (q1 , q2 , q3 , q4 ) R2 T 2 , the above no-slip rolling constraint can
then be expressed in the form

1 0 0 r cos q3
q = 0.
(2.18)
0 1 0 r sin q3
This is also a differential constraint of the form A(q)q = 0, A(q) R24 .
This constraint is not integrable; that is, for the A(q) given in (2.18), there
does not exist any differentiable g : R4 R2 such that g
q = A(q). To see why,
25
there would have to exist a differentiable g1 (q) that satisfied the following four
equalities:
g1
q1
g1
q2
g1
q3
g1
q4
1 g1 (q) = q1 + h1 (q2 , q3 , q4 )
0 g1 (q) = h2 (q1 , q3 , q4 )
0 g1 (q) = h3 (q1 , q2 , q4 )
= r cos q3 g1 (q) = rq4 cos q3 + h4 (q1 , q2 , q3 ),
for some hi , i = 1, . . . , 4 differentiable in each of its variables, and by inspection it should be clear that no such g1 (q) exists. It can be similarly be shown
that g2 (q) also does not exist, so that the constraint (2.18) is nonintegrable. A
Pfaffian constraint that is nonintegrable is also called a nonholonomic constraint. Such constraints arise in a number of robotics contexts that involve
rolling without slipping, e.g., wheeled vehicle kinematics and grasp contact kinematics. We examine nonholonomic constraints in greater detail later in the
chapter on wheeled vehicles.
2.2.4
Task Space
We introduce one more piece of terminology via an example involving the planar
2R open chain (see Figure 2.14(a)). Suppose we take the Cartesian tip (a point)
to be the end-effector. The region in the plane that the tip can reach is then
called the task space, or the workspace, of the chain. The task space can be
regarded as the configuration space of the end-effector. It is distinct from the
configuration space of the robot in that any point in task space is not required
to specify the full configuration of the robot; there may be a multitude or even
infinity of robot configurations that map to the same task space point.
Two mechanisms with different configuration spaces may also have the same
task space. For example, considering the Cartesian tip of the robot to be the
end-effector and ignoring orientations, the planar 2R open chain with links of
equal length three, and the planar 3R open chain with links of equal length
two (see Figure 2.14(b)), will have the same task space despite having different
configuration spaces.
Two mechanisms with the same configuration space may also have different
task spaces. For example, taking the Cartesian tip to be the end-effector and
ignoring orientations, the 2R open chain of Figure 2.14(a) has a planar disk as
its task space, while the 2R open chain of Figure 2.14(c) has a sphere as its
task space. Similarly, the 3R open chain of Figure 2.14(b) has a planar disk as
its task space, while the 3R open chain of Figure 2.14(d) has the space of all
possible orientations as its task space (this can be seen by noting that the three
joint axes always intersect at the tip, so that the tip remains fixedif a gripper
26
Configuration Space
Figure 2.14: Examples of task spaces for various robots: (a) a planar 2R open
chain; (b) a planar 3R open chain; (c) a spherical 2R open chain; (d) a 3R
orienting mechanism.
were attached at the tip, then this gripper would be able to change orientations
only).
Example 2.7. The SCARA robot of Figure 2.15 is an RRRP open chain that
is widely used for tabletop pick-and-place tasks. The end-effector configuration is completely described by the four parameters (x, y, z, ), where (x, y, z)
denotes the Cartesian position of the end-effector center point, and denotes
the orientation of the end-effector in the x-y plane. Its task space is therefore
R3 [0, 2].
Example 2.8. A standard 6R industrial manipulator can be adapted to spray
painting applications as shown in Figure 2.16. The paint spray nozzle attached
to the tip can be regarded as the end-effector. What is important is the Cartesian position of the spray nozzle, together with the direction in which the spray
nozzle is pointing; rotations about the nozzle axis (which points in the direction
in which paint is being sprayed) do not matter. The nozzle configuration can
therefore be described by five coordinates: (x, y, z) for the Cartesian position of
the nozzle, and spherical coordinates (, ) to describe the direction in which the
nozzle is pointing. Since (, ) also parametrizes the unit sphere in R3 , the task
27
Figure 2.15: SCARA robot.
Figure 2.16: A spray-painting robot.

space as described is given by R3 S 2 , where S 2 denotes the two-dimensional
unit sphere in R3 .
28
Configuration Space
2.3
Summary
A robot is mechanically constructed from links that are connected by

various types of joints. The links are usually modelled as rigid bodies.
An end-effector such as a gripper is attached to some link of the robot.
Actuators deliver forces and torques to the joints, thereby causing motion
of the robot.
The most widely used one-dof joints are the revolute joint, which allows
for rotation about the joint axis, and the prismatic joint, which allows
for translation in the direction of the joint axis. Some common two-dof
joints include the cylindrical joint, which is constructed by serially connecting a revolute and prismatic joint, and the universal joint, which is
constructed by orthogonally connecting two revolute joints. The spherical joint, also known as ball-in-socket joint, is a three-dof joint whose
function is similar to the human shoulder joint.
The configuration of a rigid body is a specification of all of its points. For
a rigid body moving in the plane, three independent parameters taking
on a continuous range of values are needed to specify its configuration
. For a rigid body moving in three-dimensional sapce, six independent
parameters taking on a continuous range of values are needed to specify
its configuration.
The configuration of a robot is a specification of the configuration of all of
its links. The robots configuration space is the set of all possible robot
configurations. The degrees of freedom of a robot is to the number of
continuous parameters required to specify a robots configuration.
A robots degrees of freedom can be calculated using Gr
ublers formula.
For planar robots, i.e., robots whose links all move in a single fixed plane,
the planar version of Gr
ublers formula is given by
dof = 3(N 1 J) +
J
X
fi ,
i=1
where N is the number of links (including the ground link), J is the

number of joints (the joints are labelled from 1 to J), and fi is the degrees
of freedom of joint i. For general spatial robots, i.e., robots whose links
move in three-dimensional space, the spatial version of Gr
ublers Formula
is given by
J
X
M = 6(N 1 J) +
fi .
i=1
A robots configuration space can be parametrized explicitly or implicitly. For a robot with n degrees of freedom, an explicit parametrization requires n coordinatesusually taken to be the joint variables
2.3. Summary
29
with each coordinate taking on values in a continuous range. An implicit parametrization involves m coordinates with m n, with the
m coordinates subject to m n constraint equations. With the implicit
parametrization, a robots configuration space can be viewed as a multidimensional surface of dimension n embedded in a space of higher dimension
m.
The configuration space of an n-dof robot whose structure contains one
or more closed loops can be implicitly parametrized by loop closure
equations of the form g() = 0, where Rm and g : Rm Rmn ,
m n. Such a constraint equation is called a holonomic constraint.
Assuming (t) varies with time t, the holonomic constraint g((t)) = 0
can be differentiated with respect to t to yield
g
() = 0,
where
g
()
is a (m n) m matrix.
A robots motion can also be subject to velocity constraints of the form

A() = 0,
where A() is a k m that cannot be expressed as the differential of some
function g(), i.e., there does not exist any g(), g : Rm Rk such that
A() =
g
().
Such constraints are said to be nonholonomic constraints, or nonintegrable constraints. Wheeled vehicles, and other applications in which
robots are subject to rolling contact without slip, are subject to nonholonomic constraints.
A robots task space is the configuration space of its end-effector.
Notes and References

In the classical kinematics literature, structures that consist of links connected
by joints are called mechanisms or linkages. The of the degrees of freedom of
mechanisms is treated in most texts on mechanism analysis and design, e.g., [9].
In general, a robots kinematic configuration space has the mathematical structure of a differentiable manifold. Some accessible introductions to differential
geometry are [22], [8]. Configuration spaces are further examined in a motion
planning context in [14], [5].
30
2.4
Configuration Space
Exercises
1. Use the planar version of Gr

ublers formula to determine the mobility of the
mechanisms shown in Figure 2.17.
(a)
(b)
(c)
(d)
(e)
(f)
Figure 2.17: A collection of planar mechanisms.
2. (a) Use an appropriate version of Gr

ublers formula to determine the mobility
of the 6 RU S platform of Figure 2.18.
(b) What happens to the mobility if the universal joints are replaced by spherical
joints?
2.4. Exercises
31
Figure 2.18: 6 RU S platform.
3. Use Gr
ublers formula to determine the mobility of the six parallel mechanisms shown in Figure 2.19.
4. Three identical open chain SRS arms are grasping an object as shown in
Figure 2.20.
(a) Find the mobility of this system of cooperating robots.
(b) Suppose there are now a total of n 7-dof open chain arms grasping the
object. What is the mobility of this system of robots?
(c) Suppose the spherical wrist joint in each of the n seven degree of freedom
open chains is replaced by a universal joint. What is the mobility of the overall
system?
5. Determine the mobility of the chain formed by the human arm and robot
arm as shown in Figure 2.21.
6. Consider a spatial parallel mechanism consisting of a moving plate connected
to the fixed plate by n identical open chains. In order for the moving plate to
have mobility six, how many kinematic degrees of freedom should each leg have,
as a function of n? For example, if n = 3, then the moving plate and fixed
plate are connected by three open chains; how many degrees of freedom should
each open chain have in order for the moving plate to move with six degrees of
freedom? Solve the above for arbitrary n.
7. Consider the parallel mechanism shown in Figure 2.22, in which six legs
of identical length are connected to a fixed and moving platform via spherical
joints. Determine the mobility of this mechanism using Gr
ublers formula, and
32
Configuration Space
(a)
(b)
(c)
(d)
(e)
(f)
Figure 2.19: A collection of spatial parallel mechanisms.

illustrate all possible motion types of the upper platform.
8. The mobile manipulator of Figure 2.23 consists of a 6R arm and multifin-
2.4. Exercises
33
Figure 2.20: Three cooperating open chain arms grasping a common object.
Figure 2.21: Human-robot cooperating device.

gered hand mounted on a mobile base with a single-wheel. The wheel rotates
without slip, and its axis of rotation is always parallel to the ground.
(a) Ignoring the multifingered hand, describe the configuration space of the
mobile manipulator.
(b) Now suppose the robot hand rigidly grasps the refrigerator door handle, and
opens the door using only its arm. Assume the wheel is locked in place, so that
the mobile base does not move. After the door has been opened, what is the
mobility of the mechanism formed by the arm and open door?
(c) A second identical mobile manipulator comes along, and after parking its
mobile base, also rigidly grasps the refrigerator door handle. What is the mobility of the mechanism formed by the two arms and the open referigerator
door?
9. In this exercise we consider the configuration space of the four-bar linkage
of Figure 2.24, this time projected onto the space of the two joints attached to
the ground link. Label the joints and link lengths, and attach a fixed reference
34
Configuration Space
Figure 2.22: A 6 SS platform.
Figure 2.23: Mobile manipulator for Exercise 9.

frame as shown in the figure. It follows that
A()
(a cos , a sin )T
B()
(g + b cos , b sin )T
From kA() B()k2 = h2 for all values of and , we get

b2 + g 2 + 2gb cos + a2 2(a cos (g + b cos ) + ab sin sin ) = h2
Grouping the coefficients of cos and sin , the above equation can be expressed
2.4. Exercises
35
Figure 2.24: Planar four-bar linkage.

in the form
() cos + () sin = (),
where
2gb 2ab cos
()
()
= 2ab sin
()
= h2 g 2 b2 a2 + 2ag cos .
We wish to obtain
p as a function of . To do so, first divide both sides of
Equation 9 by 2 + 2 to obtain
p
cos + p
sin =
2 + B 2
2 + 2
2 + 2
Now, referring to Figure 2.24-(b), the angle is given by = tan1 (

), and
Equation 9 becomes
.
cos( ) = p
2
+ 2
Therefore,
1
= tan
( ) cos1
p
2 + 2
!
.
(a) Note that a solution exists only if 2 2 + 2 . What are the physical
implications if this constraint is not satisfied?
(b) Note that for each value of input angle , there exists two possible values of
the output angle . What do these two solutions look like?
(c) Draw the configuration space of the mechanism in - space for the following
link length values: a = b = g = h = 1.
(d) Repeat (c) for the following link length values: a = 1, b = 2, h = 5, g =

2.
(e) Repeat (c) for the following link length values: a = 1, b = 1, h = 1, g = 3.
36
Configuration Space
Figure 2.25: The 3 P U P platform.
10. Figure 2.25 shows a 3 P U P platform, in which three identical P U P legs

connect a fixed base to a moving platform. The P joints on both the fixed
base and moving platform are arranged symmetrically as shown. Recalling that
the U joint consists of two revolute joints connected orthogonally, each R joint
connected to the moving platform has its joint axis aligned in the same direction
as the platform P joint. The R joint connected to the fixed base has its joint
axis orthogonal to the base P joint.
(a) Use an appropriate version of Gr
ublers formula to find the mobility.
(b) Reasoning intuitively, determine whether the mobility you obtained in part
(a) is correct. If not, try to explain the discrepancy without resorting to a
detailed kinematic analysis.
11. Figure 2.26 shows a spherical four-bar linkage, in which four links (one
of the links is the ground link) are connected by four revolute joints to form a
single loop closed chain. The four revolute joints are arranged so that they lie
on a sphere, and such that their joint axes intersect at a common point.
(a) Use appropriate version of Gr
ublers formula to find the mobility.
(b) Describe the configuration space.
(c) Assuming a reference frame is attached to the center link, describe its task
space.
12. Consider the two-link planar open chain of Figure 2.27, whose tip coordinates are given by
x = 2 cos 1 + cos(1 + 2 )
y = 2 sin 1 + sin(1 + 2 ).
2.4. Exercises
37
Figure 2.26: The spherical four-bar linkage.
Figure 2.27: Two-link planar RR open chain.

(a) What is its configuration space?
(b) What is the set of all points the tip can reach?
(c) Suppose there is an infinitely long wall at x = 1 and x = 1. What is its
free space?
13. Consider the slider-crank mechanism of Figure ??. A rotational motion at
the revolute joint fixed to ground (the crank) causes a translational motion
at the prismatic joint (the slider). Suppose the two links connected to the
crank and slider are of equal length. Determine the configuration space of this
38
Configuration Space
mechanism, and draw its projected version on the space defined by the crank
and slider joint variables.
14. (a) Consider a planar 3R open chain with link lengths (starting from the
fixed base joint) 5, 2, and 1, respectively. Considering only the Cartesian point
of the tip, draw its task space.
(b) Now consider a planar 3R open chain with link lengths (starting from the
fixed base joint) 1, 2, and 5, respectively. Considering only the Cartesian point
of the tip, draw its task space. Which of these two chains has a larger task
space?
(c) A not so clever designer claims that he can make the task space of any planar
open chain larger simply by increasing the length of the last link. Explain the
fallacy behind this claim.
15. Determine if the following differential constraints are holonomic or nonholonomic:
(a)
(1 + cos q1 )q1 + (1 + cos q2 )q2 + (cos q1 + cos q2 + 4)q3 = 0.
(b)
q1 cos q2 + q3 sin(q1 + q2 ) q4 cos(q1 + q2 )
q3 sin q1 q4 cos q1
0.
16. (a) Determine the task space for a robot arm writing on a blackboard.
(b) Determine the task space for a robot arm twirling a baton.
Chapter 3
Rigid-Body Motions
In the previous chapter, we saw that a minimum of six numbers are needed
to specify the position and orientation of a rigid body in three-dimensional
physical space. We established this by selecting three points on a rigid body,
and arguing that the distances between any pair of these three points must
always be preserved regardless of where the rigid body is located. This led
to three constraints, which when imposed on the nine Cartesian coordinates
(x, y, z) coordinates for each of the three pointsled us to conclude that only
six of these nine coordinates could be independently chosen.
In this chapter we will develop a more systematic way to describe the position
and orientation of a rigid body. Rather than choosing three points on a body,
we instead attach a reference frame to the body, and develop ways to describe
this reference frame with respect to some fixed reference frame in space (we
know of course that this can be done using as few as six coordinates). This is
the descriptive aspect of the rigid body motions.
There is also a prescriptive aspect to the rigid body motions. Suppose a
rigid body is moved from one configuration in physical space to another. Once
a reference frame and length scale for physical space have been chosen, the
displacement of this rigid body can then described by a transformation from
R3 to R3 . It turns out that the same set of mathematical representations can
be used for both the descriptive and prescriptive interpretations of rigid body
motions.
To illustrate the simultaneous descriptive-prescriptive features of rigid body
motions, and also to provide a synopsis of the main concepts and tools that we
will learn in this chapter, we begin with a motivating planar example. Before
doing so, we make some remarks about vector notation.
A Word about Vector Notation
Recall that a vector is a geometric quantity with a length and direction. A
vector will be denoted by a regular text symbol, e.g., v. If a reference frame
and length scale have been chosen for the underlying space in which the vector
39
40
Rigid-Body Motions
Figure 3.1: Rigid body motion in the plane.

v lies, then this vector can be represented as a column vector with respect to
this reference frame (imagine moving the base of v to the reference frame origin
while maintaining its direction; such a notion of vector is sometimes referred to
as a free vector). The column vector representation of v will be denoted in
italics by v Rn . Note that if a different reference frame and length scale are
chosen, then the column vector representation v will change.
A point p in physical space can also be represented as a vector. Given a
choice of reference frame and length scale for physical space, the point p can
be represented as a vector from the reference frame origin to p; its column
vector representation will be denoted in italics by p Rn . Here as before, a
different choice of reference frame and length scale for physical space will lead to
a different column vector representation p Rn for the same point p in physical
space.
3.1
A Motivating Example
Consider the planar body of Figure 3.1(a), whose motion is confined to the
plane. Suppose a length scale and a fixed reference frame have been chosen as
shown. We will call the fixed reference frame the fixed frame, or the space
frame, denoted {s}, and label its unit axes x
s and y
s . Similarly, we attach a
reference frame with unit axes x
b and y
b to the planar body. Because this frame
moves with the body, it will be called the moving frame, or body frame, and
denoted {b}.
To describe the configuration of the planar body, only the position and orientation of the body frame with respect to the fixed frame needs to be specified.
The vector from the fixed frame origin to the body frame origin, denoted p, can
3.1. A Motivating Example
41
be expressed in terms of the fixed frame unit axes as

p = px x
s + py y
s .
(3.1)
You are probably more accustomed to writing this vector as simply (px , py );
this is fine when there is no possibility of ambiguity about reference frames, but
when expressing the same vector in terms of multiple reference frames, writing p
as in Equation (3.1) clearly indicates the reference frame with respect to which
(px , py ) is defined.
The simplest way to describe the orientation of the body frame {b} relative
to the fixed frame {s} is by specifying the angle as shown in the figure. Another
(admittedly less simple) way is to specify the directions of the unit axes x
b and
y
b of the body frame, in the form
x
b
cos x
s + sin y
s ,
(3.2)
y
b
sin x
s + cos y
s .
(3.3)
At first sight this seems a rather inefficient way to represent the body frame
orientation. However, imagine if now the body were to move arbitrarily in threedimensional space; a single angle alone clearly would not suffice to describe
the orientation of the displaced reference frame. We would in fact need three
angles, but it is not yet clear how to define an appropriate set of three angles.
On the other hand, expressing the displaced body frames unit axes in terms of
the fixed frame, as we have done above for the planar case, is straightforward.
Assuming we agree to express everything in terms of the fixed frame coordinates, then just as the vector p of Equation (3.1) can be represented as a column
vector p R2 of the form

px
p=
,
(3.4)
py
Equations (3.2)-(3.3) can also be packaged into the following 2 2 matrix P :

cos sin
P =
,
(3.5)
sin
cos
Observe that the first column of P corresponds to x
b , and the second column
to y
b . It can be easily verified that that P T P = I, and that P 1 = P T . The
matrix P as constructed here is an example of a rotation matrix, and the pair
(P, p) provides a description of the orientation and position of the body frame
relative to the fixed frame. Of the six entries involvedtwo for p and four for
P only three are independent. In fact, the condition P T P = I implies three
equality constraints, so that of the four entries that make up P , only one can be
chosen independently. The three coordinates (, px , py ) would seem to offer an
intuitive minimal representation for the configuration of a planar rigid body. In
any event, the rotation matrix-vector pair (P, p) serves as a description of the
configuration of the rigid body as seen from the fixed frame.
Now referring to Figure 3.1(b), suppose the body at {b} is displaced to the
configuration at {c}. Repeating the above analysis for the position vector r and
42
Rigid-Body Motions
the unit axes of frame {c}, we can write

r
= rx x
s + ry y
s
(3.6)
x
c
cos x
s + sin y
s ,
(3.7)
y
c
sin x
s + cos y
s .
(3.8)
The above can be arranged into the following column vector r R2 and rotation
matrix R R22 :

rx
cos sin
r=
, R=
.
(3.9)
ry
sin
cos
We could also repeat the above to describe frame {c} as seen from frame {b}
(that is, pretend {b} is now the fixed frame, and repeat the previous analysis
for {c}). Letting q denote the vector from the {b}-frame origin to the {c}-frame
origin, we get
q
qx x
b + qy y
b ,
(3.10)
x
c
cos x
b + sin y
b ,
(3.11)
y
c
sin x
b + cos y
b ,
(3.12)
where = . As before, the vector q R2 and rotation matrix Q R22

can be defined as follows:

qx
cos sin
q=
, Q=
.
(3.13)
qy
sin
cos
Here q is the representation of q in frame {b} coordinates. Similarly, the rotation
matrix Q describes the orientation of frame {c} relative to frame {b}.
Now, if we imagine the body is displaced from frame {b} to frame {c}, then
clearly the point on the rigid body corresponding to p is displaced to r. We can
then ask, how is the column vector representation p transformed into r? The
answer is given by
r = p + P q.
(3.14)
To see why, from Figure 3.1(b) it can be seen that r is the vector sum of p and
q. Since p and r are respectively the column vector representations of p and r
in frame {s} coordinates, to obtain r as a function of p and q we first need to
represent q in frame {s} coordinates before adding this to p. This is precisely
P q (verify this for yourself), and Equation (3.14) now follows accordingly. Since
+ = , it can also be verified that
R = P Q.
(3.15)
The pair (Q, q) thus describes how the rigid body is displaced from {b}
to {c}: given any point p on the rigid body at configuration {b}, represented
by the vector p R3 , it is then transformed to the point in physical space
corresponding to the vector r = p + P q R3 . Also, the rotation matrix P
3.1. A Motivating Example
43
Figure 3.2: Mathematical description of position and orientation.

at configuration {b} is now transformed to R = P Q at configuration {c} as a
result of this displacement. For this reason we call the rotation matrix-vector
pair (Q, q) a rigid body displacement, or more commonly a rigid body
motion.
We thus see that a rotation matrix-vector pair can serve as a description
of a rigid bodys configuration (the descriptive interpretation as illustrated by
(P, p)), or as a prescription of a rigid bodys displacement in physical space (the
prescriptive interpretation as illustrated by (Q, q)).
We make one final observation. Once again referring to Figure 3.1(b), note
that the displacement from configuration {b} to {c} can be obtained by rotating
the planar body at {b} about the point s by an angle . The displacement can
therefore be parametrized by the three coordinates (, sx , sy ), where (sx , sy )
denote the coordinates for point s in fixed frame coordinates. This alternative
three-parameter representation of a rigid body motion is a (planar) example
of a screw motion. If the displacement is a pure translationthat is, the
orientation does not change, so that = then s is assumed to lie at infinity.
In the remainder of this chapter we will generalize the above concepts to
three-dimensional rigid body motions. For this purpose consider a rigid body
occupying three-dimensional physical space as shown in Figure 3.2. Assume
that a length scale for physical space has been chosen, and that both the fixed
frame {s} and body frame {b} have been chosen as shown. Throughout this
book all reference frames will be right-handed, i.e., the unit axes {
x, y
, z} always
satisfy x
y
= z. Denote the unit axes of the fixed frame by {
xs , y
s , zs }, and
the unit axes of the body frame by {
xb , y
b , zb }. Let p denote the vector from
the fixed frame origin to the body frame origin. In terms of the fixed frame
coordinates, p can be expressed as
p = p1 x
s + p2 y
s + p3zs
(3.16)
44
Rigid-Body Motions
The axes of the body frame can also be expressed as

x
b
= r11 x
s + r21 y
s + r31zs
(3.17)
y
b
= r12 x
s + r22 y
s + r32zs
(3.18)
zb
= r13 x
s + r23 y
s + r33zs .
(3.19)
Defining p R3 and R R33 by
p1
r11
p = p2 , R = r21
p3
r31
r12
r22
r32
r13
r23 ,
r33
(3.20)
the twelve parameters given by (R, p) then provide a description of the position
and orientation of the rigid body relative to the fixed frame.
Since a minimum of six parameters are required to describe the configuration of a rigid body, if we agree to keep the three parameters in p as they
are, then of the nine parameters in R, only three can be chosen independently.
We begin by examining some basic three-parameter representations for rotation matrices: the Euler angles and the related roll-pitch-yaw angles, the
exponential coordinates, and the unit quaternions. We then examine sixparameter representations for the combined position and orientation of a rigid
body. Augmenting the three-parameter representation for R with p R3 is
one obvious and natural way to do this. Another relies on the Chasles-Mozzi
Theorem, which states that every rigid body displacement can be described as
a screw motion about some fixed axis in space.
We conclude with a discussion of linear and angular velocities, and also
of forces and moments. Rather than treat these quantities as separate threedimensional quantities, we shall merge the linear and angular velocity vectors
into a single six-dimensional spatial velocity, and also the moment and force
vectors into a six-dimensional spatial force. These six-dimensional quantities,
and the rules for manipulating them, will form the basis for the kinematic and
dynamic analyses in the subsequent chapters.
3.2
Rotations
3.2.1
Definition
We argued earlier that of the nine entries in the rotation matrix R, only three can
be chosen independently. We begin by deriving a set of six explicit constraints
on the entries of R. Note that the three columns of R correspond to the body
frames unit axes {
x, y
, z}. The following conditions must therefore be satisfied:
(i) Unit norm condition: x
, y
, and z are all of unit norm, or
2
2
2
r11
+ r21
+ r31
2
r12
2
r13
1.
+
+
2
r22
2
r23
+
+
2
r32
2
r33
(3.21)
3.2. Rotations
45
(ii) Orthogonality condition: x

y
=x
z = y
z = 0 (here denotes the inner
product), or
r11 r12 + r21 r22 + r31 r32
r12 r13 + r22 r23 + r32 r33
r11 r13 + r21 r23 + r31 r33
0.
(3.22)
These six constraints can be expressed more compactly as a single constraint

on the matrix R:
RT R = I,
(3.23)
where RT denotes the transpose of R, and I denotes the 3 3 identity.
There is still the small matter of accounting for the fact that the frame is
y
=z
, where denotes the cross-product) rather than
right-handed (i.e., x
y
=
left-handed (i.e., x
z); our six equality constraints above do not distinguish between right- and left-handed frames. We recall the following formula
for evaluating the determinant of a 3 3 matrix M : denoting the three columns
of M by a, b, c, respectively, its determinant is given by
det M = aT (b c) = cT (a b) = bT (c a).
(3.24)
Substituting the columns for R into this formula then leads to the constraint
det R = 1.
(3.25)
Note that if the frame had been left-handed, we would have det R = 1. In
summary, the six equality constraints represented by (3.23) imply that det R =
1; imposing the additional constraint det R = 1 means that only right-handed
frames are allowed. Thus, the constraint det R = 1 does not change the number
of independent continuous variables needed to parametrize R.
The set of 3 3 rotation matrices forms the Special Orthogonal Group
SO(3), which we now formally define:
Definition 3.1. The Special Orthogonal Group SO(3), also known as the
group1 of rotation matrices, is the set of all 3 3 real matrices R that satisfy
(i) RT R = I, and (ii) det R = 1.
The set of 2 2 rotation matrices is a subgroup of SO(3), and denoted
SO(2).
Definition 3.2. The Special Orthogonal Group SO(2) is the set of all 2 2
real matrices R that satisfy (i) RT R = I, and (ii) det R = 1.
From the definition it follows that every R SO(2) is of the form

cos sin
R=
,
sin
cos
where [0, 2]. In what follows, all properties derived for SO(3) also apply
to SO(2).
1 The
formal algebraic notion of a group is discussed in the exercises.
46
3.2.2
Rigid-Body Motions
Properties
We now list some basic properties of rotation matrices. First, the identity I is
a trivial example of a rotation matrix. The inverse of a rotation matrix is also
a rotation matrix:
Proposition 3.1. The inverse of a rotation matrix R SO(3) always exists
and is given by its transpose, RT . RT is also a rotation matrix.
Proof. The condition RT R = I implies that R1 = RT and RRT = I. Since
det RT = det R = 1, RT is also a rotation matrix.
Proposition 3.2. The product of two rotation matrices is a rotation matrix.
Proof. Given R1 , R2 SO(3), it readily follows that their product R1 R2 satisfies
(R1 R2 )T (R1 R2 ) = I and det R1 R2 = det R1 det R2 = 1.
Proposition 3.3. For any vector x R3 and R SO(3), the vector y = Rx
is of the same length as x.
Proof. This follows from kyk2 = y T y = xT RT Rx = xT x = kxk2 .
The next property provides a descriptive interpretation for the product of
two rotation matrices. We first introduce some notation. Given two reference
frames {a} and {b}, the orientation of frame {b} as seen from frame {a} will be
represented by the rotation matrix Rab ; that is, the three columns of Rab are
just vector representations of the x
-, y
-, and z-axes of frame {b} expressed in
terms of coordinates for frame {a}. From this definition it follows readily that
Raa = I.
Proposition 3.4. Rab Rbc = Rac .
Proof. To prove this result, introduce a third reference frame {c}, and define the
unit axes of frames {a}, {b}, and {c} by the triplet of orthogonal unit vectors
{
xa , y
a , za }, {
xb , y
b , zb }, and {
xc , y
c , zc }, respectively. Suppose that the unit
axes of frame {b} can be expressed in terms of the unit axes of frame {a} by
x
b
y
b
zb
= r11 x
a + r21 y
a + r31za
= r12 x
a + r22 y
a + r32za
= r13 x
a + r23 y
a + r33za .
Similarly, suppose the unit axes of frame {c} can be expressed in terms of the
unit axes of frame {b} by
x
c
y
c
zc
=
=
=
s11 x
b + s21 y
b + s31zb
s12 x
b + s22 y
b + s32zb
s13 x
b + s23 y
b + s33zb .
3.2. Rotations
47
The above equations can also be expressed as
x
b
y
b
x
c
y
c
zb
zc
x
a
y
a
x
b
y
b
r11 r12 r13

za r21 r22 r23
r31 r32 r33
s11 s12 s13

zb s21 s22 s23 .
s31 s32 s33
(3.26)
(3.27)
Note that the 33 matrix of Equation (3.26) is Rab , while that of Equation (3.27)
is Rbc . Substituting (3.26) into (3.27),
s11 s12 s13

r11 r12 r13
x
c y
c zc = x
a y
a za r21 r22 r23 s21 s22 s23 .
r31 r32 r33
s31 s32 s33
From the above it follows that Rab Rbc = Rac .
Proposition 3.5.
1
Rab
= Rba .
(3.28)
Proof. This result follows immediately by choosing {c} to be the same as {a}
in the previous proposition Rab Rbc = Rac , and recalling that Raa = I.
For the next property, consider a free vector v in physical space with a
defined direction and magnitude. Given two reference frames {a} and {b} in
physical space, let va , vb R3 denote representations of v with respect to these
two frames; that is, va and vb are obtained by placing the base of v at the
origin of frames {a} and {b}, respectively, and expressing v in terms of the
given reference frame.
Proposition 3.6.
Rba va
vb
(3.29)
Rab vb
va .
(3.30)
Proof. In terms of the unit axes of {a} and {b}, v can be written as follows:
v
= va1 x
a + va 2 y
a + va3 za
(3.31)
= vb1 x
b + vb2 y
b + vb3 zb ,
(3.32)
or, letting va = (va1 , va2 , va3 ) , vb = (vb1 , vb2 , vb3 ) ,

x
a y
a za va
v =

x
b y
b zb vb .
=
(3.33)
(3.34)
From the previous proposition the unit axes of {a} and {b} are related by

x
a y
a za = x
b y
b zb Rba ,
(3.35)
from which it follows that Rba va = vb . The equality Rab vb = va follows similarly.
48
Rigid-Body Motions
z
y
{0}
x
z
{1}
y
x
z {3}
x
x {2}
Figure 3.3: Wrist mechanism illustrating the ZYX Euler angles.
3.2.3
Euler Angles
As we established above, a rotation matrix can be parametrized by three independent coordinates. Here we introduce one popular three-parameter representation of rotations, the ZYX Euler angles. One way to visualize these
angles is through the wrist mechanism shown in Figure 3.3. The ZYX Euler
angles (, , ) refer to the angle of rotation about the three joint axes of this
mechanism. In the figure the wrist mechanism is shown in its zero position, i.e.,
when all three joints are set to zero.
Four reference frames are defined as follows: frame {0} is the fixed frame,
while frames {1}, {2}, and {3} are attached to the three links of the wrist mechanism as shown. When the wrist is in the zero position, all four reference frames
have the same orientation. We now consider the relative frame orientations R01 ,
R12 , and R23 . First, it can be seen that R01 depends only on the angle : rotating about the z-axis of frame {0} by an angle (a positive rotation about
an axis is taken by aligning the thumb of the right hand along the axis, and
rotating in the direction of the fingers curling about the axis), it can be seen
that
cos sin 0
R01 = sin cos 0 = Rot(z, ).
(3.36)
0
0
1
The notation Rot(z, ) describes a rotation about the z-axis by angle . Simi-
3.2. Rotations
49
larly, R12 depends only on , and is
cos
0
R12 =
sin
given by
0 sin
1
0 = Rot(
y, ),
0 cos
(3.37)
where the notation Rot(

y, ) describes a rotation about the y
-axis by angle .
Finally, R23 depends only on , and is given by
1
0
0
R23 = 0 cos sin = Rot(
x, ),
(3.38)
0 sin
cos
where the notation Rot(
x, ) describes a rotation about the x
-axis by angle .
R03 = R01 R12 R23 is therefore given by
c c c s s s c c s c + s s
R03 = s c s s s + c c s s c c s ,
(3.39)
s
c s
c c
where s is shorthand for sin , c for cos , etc.
We now ask the following question: given an arbitrary rotation matrix R,
does there exist (, , ) such that Equation (3.39) is satisfied? If the answer is
yes, then the wrist mechanism of Figure 3.3 can reach any orientation. This is
indeed the case, and we prove this fact constructively as follows. Let rij be the
2
2
ij-th element of R. Then from Equation (3.39) we know that r11
+ r21
= cos2 ;
as long as cos 6= 0, or equivalently 6= 90 , we have

!

r31
sin
1
1
p
= tan
.
= tan
2 + r2
cos
r11
21
We now define the atan2 function, which is a two-argument function implemented in a variety of computer languages for computing the arctangent. Specifically, the function atan2(y, x) evaluates tan1 (y/x) by taking into account the
signs of x and y. For example, atan2(1, 1) = /4, while atan2(1, 1) = 3/4.
Using atan2, the possible values for can be expressed as

q
2
2
= atan2 r31 , r11 + r21
and

q
2 + r2
= atan2 r31 , r11
21 .
In the first case will lie in the range [90 , 90 ], while in the second case
lies in the range [90 , 270 ]. Assuming the obtained above is not 90 , and
can then be determined from the following relations:
atan2(r21 , r11 )
atan2(r32 , r33 )
50
Rigid-Body Motions
=90
Figure 3.4: Configuration corresponding to = 90 for ZYX Euler angles.

In the event that = 90 , there exists a one-parameter family of solutions
for and . This is most easily seen from Figure 3.4. If = 90 , then and
represent rotations (in the opposite direction) about the same vertical axis.
Hence, if (, , ) = (
, 90 , ) is a solution for a given rotation R, then any
0
0
triple (
, 90 , ) where
0 0 =
is also a solution.
Algorithm for Computing the ZYX Euler Angles
Given R SO(3), we wish to find angles , [0, 2) and [/2, /2)
that satisfy
R = s c s s s + c c s s c c s ,
(3.40)
s
c s
c c
where s is shorthand for sin , c for cos , etc. Denote by rij the ij-th entry
of R.
(i) If r31 6= 1, set

2
r11
2
r21
atan2 r31 ,
atan2(r21 , r11 )
(3.42)
atan2(r32 , r33 ),
(3.43)
where the square root is taken to be positive.
(3.41)
3.2. Rotations
51
(ii) If r31 = 1, then = /2, and a one-parameter family of solutions for

and exists. One possible solution is = 0 and = atan2(r12 , r22 ).
(iii) If r31 = 1, then = /2, and a one-parameter family of solutions for
and exists. One possible solution is = 0 and = atan2(r12 , r22 ).
From the earlier wrist mechanism illustration of the ZYX Euler angles it
should be evident that the choice of zero position for is, in some sense, arbitrary. That is, we could just as easily have defined the home position of
the wrist mechanism to be as in Figure 3.4; this would then lead to another
three-parameter representation (, , ) for SO(3). Figure 3.4 is precisely the
definition of the ZYZ Euler angles. The resulting rotation matrix can be
obtained via the following sequence of rotations:
R(, , )
Rot(z, ) Rot(
y, ) Rot(z, )
c s 0
c 0 s
c s
1 0 s c
= s c 0 0
0
0
1
s 0 c
0
0
c c c s s c c s s c c s
= s c c + c s s c s + c c s s .
s c
s s
c
=
0
0
1
(3.44)
Just as before, we can show that for every rotation R SO(3), there exists
a triple (, , ) that satisfies R = R(, , ) for R(, , ) as given in Equation (3.44). (Of course, the resulting formulas will differ from those for the ZYX
Euler angles.)
From the wrist mechanism interpretation of the ZYX and ZYZ Euler angles,
it should be evident that for Euler angle parametrizations of SO(3), what really
matters is that rotation axis 1 is orthogonal to rotation axis 2, and that rotation
axis 2 is orthogonal to rotation axis 3 (axis 1 and axis 3 need not necessarily be
orthogonal to each other). Specifically, any sequence of rotations of the form
Rot(axis1, ) Rot(axis2, ) Rot(axis3, ),
(3.45)
where axis1 is orthogonal to axis2, and axis2 is orthogonal to axis3, can serve
as a valid three-parameter representation for SO(3). Later in this chapter we
shall see how to express a rotation about an arbitrary axis that is not a unit
axis of the reference frame.
3.2.4
Roll-Pitch-Yaw Angles
Earlier in the chapter we asserted that a rotation matrix can also be used
to describe a transformation of a rigid body from one orientation to another.
Here we will use this prescriptive viewpoint to derive another three-parameter
representation for rotation matrices, the roll-pitch-yaw angles. Referring to
Figure 3.5, given a frame in the identity configuration (that is, R = I), we first
rotate this frame by an angle about the x
-axis of the fixed frame, followed by
52
Rigid-Body Motions
Figure 3.5: Illustration of XYZ roll-pitch-yaw angles.

an angle about the y
-axis of the fixed frame, and finally by an angle about
the z-axis of the fixed frame.
Let us derive the explicit form of a vector v R3 (expressed as a column
vector using fixed frame coordinates) that is rotated about the fixed frame x
-axis
by an angle . The rotated vector, denoted v 0 , will be
1
0
0
v 0 = 0 cos sin v.
(3.46)
0 sin
cos
If v 0 is now rotated about the fixed frame y
-axis by an angle , then the rotated
vector v 00 can be expressed in fixed frame coordinates as
cos 0 sin
0
1
0 v0 .
(3.47)
v 00 =
sin 0 cos
Finally, rotating v 00 about the fixed frame z-axis by an angle yields the vector
cos sin 0
v 000 = sin cos 0 v 00 .
(3.48)
0
0
1
If we now take v to successively be the three unit axes of the reference frame
in the identity orientation R = I, then after applying the above sequence of
rotations to the three axes of the reference frame, its final orientation will be
R(, , )
=
=
Rot(z, )Rot(
y, ) Rot(
x, ) I
c s 0
c 0 s
1 0
s c 0 0
1 0 0 c
0
0
1
s 0 c
0 s
s c s s s + c c s s c c s .
s
c s
c c
0
s I
c
(3.49)
3.2. Rotations
53
This product of three rotations is exactly the same as that for the ZYX Euler
angles given in (3.40). We see that the same product of three rotations admits
two different physical interpretations: as a sequence of rotations with respect
to the body frame (ZYX Euler angles), or, reversing the order of rotations, as
a sequence of rotations with respect to the fixed frame (the XYZ roll-pitch-yaw
angles).
The terms roll, pitch, and yaw are often used to describe the rotational motion of a ship or aircraft. In the case of a typical fixed-wing aircraft, for example,
suppose a body frame is attached such that the x
-axis is in the direction of forward motion, the z-axis is the vertical axis pointing downward toward ground
(assuming the aircraft is flying level with respect to ground), and the y
-axis
extends in the direction of the wing. The roll, pitch, and yaw motions are then
defined according to the XYZ roll-pitch-yaw angles (, , ) of Equation (3.49).
In fact, for any sequence of rotations of the form (3.45) in which consecutive
axes are orthogonal, a similar descriptive-prescriptive interpretation exists for
the corresponding Euler angle formula. Euler angle formulas can be defined in
a number of ways depending on the choice and order of the rotation axes, but
their common features are:
The angles represent three successive rotations taken about the axes of
either the body frame or the fixed frame.
The first axis must be orthogonal to the second axis, and the second axis
must be orthogonal to the third axis.
The angle of rotation for the first and third rotations ranges in value over
a 2 interval, while that of the second rotation ranges in value over an
interval of length .
3.2.5
Exponential Coordinates
We now introduce another three-parameter representation for rotations, the exponential coordinates. In the Euler angle representation, a rotation matrix
is expressed as a product of three rotations, each depending on a single parameter. The exponential coordinates parametrize a rotation matrix in terms of a
single rotation axis (represented by a vector of unit length), together with
an angle of rotation about that axis; The vector r = R3 then serves as
a three-parameter representation of the rotation. This representation is most
naturally introduced in the setting of linear differential equations, whose main
results we now review.
3.2.5.1
Some Basic Results from Linear Differential Equations
Let us begin with the simplest scalar linear differential equation

x(t)
= ax(t),
(3.50)
54
Rigid-Body Motions
where x(t) R, a R is constant, and initial condition x(0) = x0 is assumed

given (x(t)
denotes the derivative of x(t) with respect to t). Equation (3.50)

has solution
x(t) = eat x0 .
It is also useful to remember the series expansion of the exponential function:
eat = 1 + at +
(at)3
(at)2
+
+ ...
2!
3!
Now consider the vector linear differential equation

x(t)
= Ax(t)
(3.51)
where x(t) Rn , A Rnn is constant, and initial condition x(0) = x0 is given.

From the earlier scalar result, one can conjecture a solution of the form
x(t) = eAt x0
(3.52)
where the matrix exponential eAt now needs to be defined in a meaningful way.
Again mimicking the scalar case, we define the matrix exponential to be
eAt = I + At +
(At)2
(At)3
+
+ ...
2!
3!
(3.53)
The first question to be addressed is under what conditions this series converges,
so that the matrix exponential is well-defined. It can be shown that if A is
constant and finite, this series is always guaranteed to converge to a finite limit;
the proof can be found in most texts on ordinary linear differential equations
and will not be covered here.
The second question is whether (3.52) is indeed a solution to (3.51). Taking
the time derivative of x(t) = eAt x0 ,

d At
x(t)
=
e
x0
dt

d
A3 t3
A2 t 2
=
+
+ . . . x0
I + At +
dt
2!
3!

A3 t2
+ . . . x0
(3.54)
=
A + A2 t +
2!
= AeAt x0
= Ax(t),
which proves that x(t) = eAt x0 is indeed a solution. That this is a unique
solution follows from the basic existence and uniqueness result for linear ordinary
differential equations, which we will just invoke here without proof.
Note that in the fourth line of (3.54), A could also have been factored to the
right, i.e.,
x(t)
= eAt Ax0 .
3.2. Rotations
55
While for arbitrary square matrices A and B we have in general that AB =

6 BA,
it is always true that
AeAt = eAt A
(3.55)
for any square A and scalar t (you can verify this directly using the series
expansion for the matrix exponential).
While the matrix exponential eAt is defined as an infinite series, closedform expressions are often available. For example, if A can be expressed as
A = P DP 1 for some D Rnn and invertible P Rnn , then
eAt
(At)2
+ ...
2!
I + At +
I + (P DP 1 )t + (P DP 1 )(P DP 1 )
P (I + Dt +
P eDt P 1 .
t2
+ ...
2!
(Dt)2
+ . . .)P 1
2!
(3.56)
If moreover D is diagonal, i.e., D = diag{d1 , d2 , . . . , dn }, then its matrix exponential is particularly simple to evaluate:
d1 t
e
0
0
0
ed2 t
0
eDt = .
(3.57)
..
.. .
..
..
.
.
.
0
edn t
We summarize the above results in the following proposition:

Proposition 3.7. The linear differential equation x(t)
= Ax(t) with initial

condition x(0) = x0 , where A Rnn is constant and x(t) Rn , has solution
x(t) = eAt x0
where
eAt = I + tA +
t2 2 t3 3
A + A + ....
2!
3!
(3.58)
(3.59)
The matrix exponential eAt further satisifies the following properties:

(i)
d At
dt e
= AeAt = eAt A.
(ii) If A = P DP 1 for some D Rnn and invertible P Rnn , then

eAt = P eDt P 1 .
(iii) If AB = BA, then eA eB = eA+B .
(iv) (eA )1 = eA .
56
Rigid-Body Motions
Figure 3.6: The vector p(0) is rotated by an angle about the axis , to p().
The third property can be established by expanding the exponentials and
comparing terms. The fourth property follows by setting B = A in the third
property.
A final linear algebraic result useful in finding closed-form expressions for
eAt is the Cayley-Hamilton Theorem, which we state here without proof:
Proposition 3.8. Let A Rnn be constant, with characteristic polynomial
p(s) = det(Is A) = sn + cn1 sn1 + . . . + c1 s + c0 .
Then
p(A) = An + cn1 An1 + . . . + c1 A + c0 I = 0.
3.2.5.2
Exponential Coordinates for Rotations
In the exponential coordinate representation for rotations, a rotation is represented by a single axis of rotation together with a rotation angle about the axis.
Referring to Figure 3.6, suppose a three-dimensional vector p(0) is rotated by
an angle about the fixed rotation axis to p(); here we assume all quantities
are expressed in fixed frame coordinates and kk = 1. This rotation can be
achieved by imagining that p(0) is subject to a rotation about at a constant
rate of 1 rad/sec, from time t = 0 to t = . Let p(t) denote this path. The
velocity of p(t), denoted p,
is then given by
p = p.
(3.60)
To see why this is true, let be the angle between p(t) and . Observe that p
traces a circle of radius kpk sin about the -axis. Then p = p is tangent
to the path with magnitude kpk sin , which is exactly (3.60).
We now propose an alternative way of expressing the cross-product between
two vectors. To do this we introduce the following notation:
3.2. Rotations
57
Definition 3.3. Given a vector R3 , define
0 3
2
0 1 .
[] = 3
2
1
0
(3.61)
The matrix [] as defined above is skew-symmetric; that is,

[] = []T .
With this representation it is a simple calculation to verify the following property:
Proposition 3.9. Given two vectors x, y R3 , their cross product x y can
be obtained as
x y = [x]y,
(3.62)
where [x] is the skew-symmetric matrix representation of x. Also,
[x y] = [x][y] [y][x].
(3.63)
Another useful property involving rotations and skew-symmetric matrices is

the following:
Proposition 3.10. Given any R3 and R SO(3), the following always
holds:
R[]RT = [R].
(3.64)
Proof. Letting riT be the ith row of R,
T
r1 ( r1 ) r1T ( r2 ) r1T ( r3 )
R[]RT = r2T ( r1 ) r2T ( r2 ) r2T ( r3 )
T
T
T
r3 ( r1 ) T r3 ( T r2 ) r3 ( r3 )
0
r3 r2
0
r1T
= r3T
T
T
r2 r1
0
= [R],
(3.65)
where the second line makes use of the determinant formula for 3 3 matrices,
i.e., if M is a 3 3 matrix with columns {a, b, c}, then det M = aT (b c) =
cT (a b) = bT (c a).
With the above results, the differential equation (3.60) can be expressed as
p = []p,
(3.66)
with initial condition p(0) = 0. This is a linear differential equation of the form
x = Ax that we have studied earlier; its solution is given by
p(t) = e[]t p(0).
58
Rigid-Body Motions
Since t and are interchangeable as a result of assuming that p(t) rotates at a

constant rate of 1 rad/sec (after t seconds, p(t) will have rotated t radians), the
above can also be written
p() = e[] p(0).
We now derive a closed-form expression for e[] . Here we make use of the
Cayley-Hamilton Theorem for []. First, the characteristic polynomial associated with the 3 3 matrix [] is given by
p(s) = det(Is []) = s3 + s.
The Cayley-Hamilton Theorem then implies []3 + [] = 0, or
[]3 = [].
Let us now expand the matrix exponential e[] in series form. Replacing []3
by [], []4 by []2 , []5 by []3 = [], and so on, we obtain
e[]
=
=
3
2
I + [] + []2 + []3 + . . .
2!
3!
2

5
4
6
3
+
[] +
+
[]2 .
I+
3!
5!
2!
4!
6!
Now recall the series expansions for sin and cos :

3
5
+
...
3!
5!
2
4
1
+
...
2!
4!
sin
cos
The exponential e[] therefore simplifies to the following:

Proposition 3.11. Given a vector R3 such that kk = 1, and any scalar
R,
e[] = I + sin [] + (1 cos )[]2 .
(3.67)
This formula is also known as the Rodrigues formula for rotations.
3.2.5.3
Matrix Logarithm of a Rotation Matrix
So far we have shown that multiplying a vector v R3 by the exponential matrix

e[] above amounts to rotating v about the axis by an angle . Our next
task is to show that for any given rotation matrix R SO(3), one can always
find a unit vector and scalar such that R = e[] . Defining r = R3 , we
shall call the skew-symmetric matrix [r] the matrix logarithm of R. In this
way we can assert that r provides another three-parameter representation for
rotations. Expanding each of the entries for e[] in (3.67) leads to the following
expression:
c + 12 (1 c )
1 2 (1 c ) 3 s 1 3 (1 c ) + 2 s
1 2 (1 c ) + 3 s
c + 22 (1 c )
2 3 (1 c ) 1 s , (3.68)
1 3 (1 c ) 2 s 2 3 (1 c ) + 1 s
c + 32 (1 c )
3.2. Rotations
59
where (1 , 2 , 3 ) are the elements of , and we use the shorthand notation

s = sin , c = cos . Setting the above equal to the given R SO(3) and
subtracting the transpose from both sides leads to the following:
r32 r23
21 sin
r13 r31
22 sin
r21 r12
23 sin .
Therefore as long as sin 6= 0 (or equivalently, is not an integer multiple of

), we can write
1
1
(r32 r23 )
2 sin
1
(r13 r31 )
2 sin
1
(r21 r12 ).
2 sin
The above equations can also be expressed in skew-symmetric matrix form as
0
3 2

1
0
1 =
[] = 3
R RT .
(3.69)
2 sin
2 1
0
Recall that represents the axis of rotation for the given R. Because of the
sin term in the denominator, [] is not well-defined if is an integer multiple
of . We address this situation next, but for now let us assume this is not the
case and find an expression for . Setting R equal to (3.68) and taking the trace
of both sides (recall that the trace of a matrix is the sum of its diagonals),
tr R = r11 + r22 + r33 = 1 + 2 cos .
(3.70)
The above follows since 12 + 22 + 32 = 1. For any satisfying 1 + 2 cos = tr R

such that is not an integer multiple of , R can be expressed as the exponential
e[] with [] as given in (3.69).
Let us now return to the case = k, where k is some integer. When k
is an even integer (corresponding to = 0, 2, 4, . . .) we have tr R = 3,
or equivalently R = I, and it follows straightforwardly that = 0 is the only
possible solution. When k is an odd integer (corresponding to = , 3, . . .,
which in turn implies tr R = 1), the exponential formula (3.67) simplifies to
R = e[] = I + 2[]2 .
The three diagonal terms of (3.71) can be manipulated to
r
rii + 1
, i = 1, 2, 3.
i =
2
(3.71)
(3.72)
60
Rigid-Body Motions
These may not always lead to a feasible unit-norm solution . The off-diagonal
terms lead to the following three equations:
21 2
= r12
22 3
= r23
21 3
= r13 ,
(3.73)
From (3.71) we also know that R must be symmetric: r12 = r21 , r23 = r32 ,
r13 = r31 . Both (3.72) and (3.73) may be necessary to obtain a feasible solution.
Once a solution has been found, then R = e[]k , k = , 3, . . ..
From the above it can be seen that solutions for exist at 2 intervals. If
we restrict to the interval [0, ], then the following algorithm can be used to
compute the matrix logarithm of the rotation matrix R SO(3):
Algorithm: Given R SO(3), find r = R3 , where [0, ] and R3 ,
kk = 1, such that
R = e[r] = e[] = I + sin [] + (1 cos )[]2 .
(3.74)
The skew-symmetric matrix [r] R33 is then said to be a matrix logarithm

of R.
(i) If R = I, then = 0 and [r] = 0.
(ii) If tr R = 1, then = , and set to any of the three following vectors
that is a feasible solution:
r13
1
r23
(3.75)
=p
2(1 + r33 ) 1 + r
33
or
r12
1
1 + r22
=p
2(1 + r22 )
r32
or
1 + r11
1
r21 .
=p
2(1 + r11 )
r31

(iii) Otherwise = cos1 tr R1
[0, ) and
2
(3.76)
[] =
1
(R RT ).
2 sin
(3.77)
(3.78)
The formula for the logarithm suggests a picture of the rotation group SO(3)
as a solid ball of radius (see Figure 3.7): given a point r R3 in this solid ball,
let = r/krk and = krk, so that r = . The rotation matrix corresponding
3.2. Rotations
61
Figure 3.7: SO(3) as a solid ball of radius .

to r can then be regarded as a rotation about the axis by an angle . For any
R SO(3) such that tr R 6= 1, there exists a unique r in the interior of the
solid ball such that e[r] = R. In the event that tr R = 1, log R is given by two
antipodal points on the surface of this solid ball. That is, if there exists some
r such that R = e[r] , then krk = , and R = e[r] also holds; both r and r
correspond to the same rotation R.
3.2.6
Unit Quaternions
One disadvantage with the exponential coordinates on SO(3) is that because of

the division by sin in the logarithm formula, the logarithm can be numerically
sensitive to small rotation angles . The unit quaternions are an alternative
representation of rotations that alleviates some of these numerical difficulties,
but at the cost of introducing an additional fourth parameter. We now illustrate
the definition and use of these coordinates.
Let R SO(3) have exponential coordinate representation R = e[] , where
as usual kk = 1 and [0, ]. The unit quaternion representation of R is
constructed as follows. Define q R4 according to
q0

q1
cos 2
=
q=
R4 .
(3.79)
q2
sin 2
q3
q as defined clearly satifies kqk = 1. Geometrically, q is a point lying on the
three-dimensional unit sphere in R4 , and for this reason the unit quaternions
are also identified with the three-sphere, denoted S 3 . Naturally among the
62
Rigid-Body Motions
four coordinates of q, only three can be chosen independently. Recalling that

1 + 2 cos = tr R, and using the cosine double angle formula, i.e., cos 2 =
2 cos2 1, the elements of q can be obtained directly from the entries of R as
follows:
q0 =
q1
q2 =
q3
1
1 + r11 + r22 + r33
2
r r23
1 32
r13 r31 .
4q0
r21 212
(3.80)
(3.81)
Going the other way, given a unit quaternion (q0 , q1 , q2 , q3 ), the corresponding rotation matrix R is obtained as a rotation about the unit axis in the direction of (q1 , q2 , q3 ), by an angle 2 cos1 q0 . Explicitly,
2
2(q1 q2 q0 q3 )
2(q0 q2 + q1 q3 )
q0 + q12 q22 q32
q02 q12 + q22 q32
2(q2 q3 q0 q1 ) . (3.82)
R = 2(q0 q3 + q1 q2 )
2
2(q1 q3 q0 q2 )
2(q0 q1 + q2 q3 )
q0 q12 q22 + q32
From the above explicit formula it should be apparent that both q S 3 and its
antipodal point q S 3 produce the same rotation matrix R. For every rotation
matrix there exists two unit quaternion representations that are antipodal to
each other.
The final property of the unit quaternions concerns the product of two rotations. Let Rq , Rp SO(3) denote two rotation matrices, with unit quaternion
representations q, p S 3 , respectively. The unit quaternion representation
for the product Rq Rp can then be obtained by first arranging the elements of q
and p in the form of the following 2 2 complex matrices:

q0 + iq1 q2 + ip3
p0 + ip1 p2 + ip3
Q=
, P =
,
(3.83)
q2 + iq3 q0 iq1
p2 + ip3 p0 ip1
where i denotes the imaginary unit. Now take the product N = QP , where the
entries of N are written

n0 + in1 n2 + in3
N=
.
(3.84)
n2 + in3 n0 in1
The unit quaternion for the product Rq Rp is then given by (n0 , n1 , n2 , n3 )
obtained from the entries of N :
n0
q0 p0 q1 p1 q2 p2 q3 p3
n1 q0 p1 + p0 q1 + q2 p3 q3 p2
(3.85)
n2 = q0 p2 + p0 q2 q1 p3 + q3 p1 .
n3
q0 p3 + p0 q3 + q1 p2 q2 p1
Verification of this formula is left as an exercise at the end of this chapter.
3.3. Rigid-Body Motions
3.3
3.3.1
63
Rigid-Body Motions
Definition
We now consider representations for the combined orientation and position of

a rigid body. This would seem to be fairly straightforward: once fixed and
moving frames are defined, the orientation can be described by a rotation matrix
R SO(3), and the position by a vector p R3 . Rather than identifying R and
p separately, we shall combine them into a single matrix as follows.
Definition 3.4. The Special Euclidean Group SE(3), also known as the
group of rigid body motions or homogeneous transformations in R3 , is
the set of all 4 4 real matrices T of the form

R p
T =
,
(3.86)
0 1
where R SO(3), p R3 , and 0 denotes the three-dimensional row vector of
zeros.
An element T SE(3) will also sometimes be denoted more compactly as
T = (R, p). We begin this section by establishing some basic properties of
SE(3), and explaining why we package R and p into this somewhat unusual
matrix form.
From the definition it should be apparent that six coordinates are needed
to parametrize SE(3). The most obvious choice of coordinates would be to use
any of the earlier three-parameter representations for SO(3) (e.g., Euler angles,
exponential coordinates) to parametrize the orientation R, and the usual three
Cartesian coordinates in R3 to parametrize the position p. Instead, we shall
derive a six-dimensional version of exponential coordinates on SE(3) that turns
out to have several advantages over these other parametrizations.
Many of the robotic mechanisms we have encountered thus far are planar.
With planar rigid-body motions in mind, we make the following definition:
Definition 3.5. The Special Euclidean Group SE(2) is the set of all 3 3
real matrices T of the form

R p
T =
,
(3.87)
0 1
where R SO(2), p R2 , and 0 denotes the two-dimensional row vector of
zeros.
A matrix T SE(2) will always be of the form
cos sin px
cos py ,
T = sin
0
0
1
where [0, 2] and (px , py ) R2 .
64
Rigid-Body Motions
3.3.2
Properties
The following two properties of SE(3) can be verified by direct calculation.

Proposition 3.12. The product of two SE(3) matrices is also an SE(3) matrix.
Proposition 3.13. The inverse of an SE(3) matrix always exists, and has the
following explicit form:

R
0
p
1
1

=
RT
0
RT p
1

.
(3.88)
Before stating the next proposition, we introduce the following notation:

Definition 3.6. Given T = (R, p) SE(3) and x R3 , the product T x R3
is then defined to be
T x = Rx + p.
(3.89)
The above is a slight abuse of notation, but is motivated by the fact that if
x R3 is turned into a four-dimensional vector by appending a 1, then

R p
x
Rx + p
=
.
(3.90)
0 1
1
1
The four-dimensional vector obtained by appending a 1 to x is an example of
homogeneous coordinates; the transformation T SE(3) is also accordingly
called a homogeneous transformation.
With the above definition of T x, the next proposition can also be verified
by direct calculation:
Proposition 3.14. Given T = (R, p) SE(3) and x, y R3 , the following
hold:
(i) kT x T yk = kx yk, where k k denotes the standard Euclidean norm in
R3 , i.e., kxk = xT x.
(ii) hT x T z, T y T zi = hx z, y zi for all z R3 , where h, i denotes the
standard Euclidean inner product in R3 , i.e., hx, yi = xT y.
In the above proposition, T is regarded as a transformation on points in R3 ,
i.e., T transforms a point x to T x. The first property then asserts that T preserves distances, while the second asserts that T preserves angles. Specifically,
if x, y, z R3 represent the three vertices of a triangle, then the triangle formed
by the transformed vertices {T x, T y, T z} has the same set of lengths and angles
as those of the triangle {x, y, z} (or, the two triangles are said to be isometric).
One can easily imagine taking x to be the points on a rigid body, in which case
T x results in a displaced version of the rigid body. It is in this sense that SE(3)
can be identified with the rigid body motions.
The remaining properties describe the different physical contexts in which
rigid-body motions are used. Like rotations, both descriptive and prescriptive
65
Figure 3.8: Three reference frames in space.

interpretations for rigid body motions are possible, and we begin with the former. Referring to Figure 3.8, consider three reference frames {a}, {b}, and {c}
in physical space. Define u to be the vector from the origin of frame {a} to
the origin of frame {b}; v and w are also defined as indicated in the figure.
Denote by Tab SE(3) the position and orientation of frame {b} as seen from
frame {a}; that is, Rab denotes the orientation of frame {b} expressed in frame
{a} coordinates, and pab R3 is the vector representation of u in {a} frame
coordinates. Tbc and Tac are defined in a similar fashion, with pac R3 the
vector representation of w in {a} frame coordinates, and pbc R3 the vector
representation of v in {b} frame coordinates.
Proposition 3.15. Given three reference frames {a}, {b}, {c} in physical space,
let Tab , Tbc , Tac SE(3) denote the relative displacements between these frames.
Then Tab Tbc = Tac .
Proof. Let Tab , Tbc , Tac SE(3) be given by

Rab pab
Rbc pbc
Rac
Tab =
, Tbc =
, Tac =
0
1
0
1
0
pac
1

.
From a previously derived property of rotation matrices we know that Rab Rbc =
Rac . Referring to Figure 3.8, to express the vector relation u+v = w in {a} frame
coordinates, we first need to find an expression for v in {a} frame coordinates.
From Proposition 3.6, this is simply Rab pbc . It now follows that
pac = Rab pbc + pab .
The above can be combined into the following matrix equation:

Rab pab
Rbc pbc
Rac pac
=
,
0
1
0
1
0
1
or equivalently, Tab Tbc = Tac .
(3.91)
(3.92)
66
Rigid-Body Motions
Figure 3.9: Assignment of reference frames.

The Tab notation is a very convenient way of keeping track of the relationships between multiple reference frames. The following is a direct consequence
of the previously established property:
1
Proposition 3.16. Taa = I, and Tab
= Tba .
Referring again to Figure 3.8, we now change our perspective slightly, and
let q denote the point in physical space corresponding to the {c} frame origin.
Let qb R3 be the vector representation of the point q in {b} frame coordinates;
that is, qb = pbc . Similarly, let qa R3 be the vector representation of the same
point q in {a} frame coordinates; that is, qa = pac . Since pac and pbc are related
by Equation 3.91, it follows that the same relationship also exists between qa
and qb :

qa
Rab pab
qb
=
,
(3.93)
1
0
1
1
or in homogeneous coordinate notation,
qa = Tab qb .
(3.94)
We have in fact just proven the following:

Proposition 3.17. Given a point q in physical space, let qa R3 and qb R3
denote its coordinates in terms of reference frames {a} and {b}, respectively.
Then
qa = Tab qb ,
(3.95)
with the product Tab qb interpreted in the sense of the right-hand side of Equation (3.93).
67
Example
Figure 3.9 shows a robot arm mounted on a wheeled mobile platform, and a
camera fixed to the ceiling. Frames {b} and {c} are respectively attached to
the wheeled platform and the end-effector of the robot arm, and frame {d} is
attached to the camera. A fixed frame {a} has been established, and the robot
must pick up the object with body frame {e}. Suppose that the transformations
Tdb and Tde can be calculated from measurements obtained with the camera.
The transformation Tbc can be determined by evaluating the forward kinematics
for the current joint measurements. The transformation Tad is assumed to be
known in advance. Suppose these known transformations are given as follows:
0
0 1
250
0 1
0 150
Tdb =
1
0
0
200
0
0
0
1
0
0 1 300
0 1
0 100
Tde =
1
0
0 120
0
0
0
1
0
0 1 400
0 1
0 50
Tad =
1
0
0 300
0
0
0
1
0 1/2 1/2
30
0
1/ 2 1/ 2 40
.
Tbc =
1
0
0
25
0
0
0
1
In order for the robot arm to pick up the object, Tce must be determined. We
know that
Tab Tbc Tce = Tad Tde ,
where the only quantity besides Tce not given to us directly is Tab . However,
since Tab = Tad Tde , we can determine Tce as follows:
1
Tce = (Tad Tdb Tbc )
Tad Tde .
68
Rigid-Body Motions
Figure 3.10: Displacement of a rigid body from an initial to final configuration.

From the given transformations,
Tad Tde
Tad Tdb Tbc
(Tad Tdb Tbc )
1
0
=
0
0
0
0
=
1
0
0 280
0 50
1
0
0
1
1/ 2 1/2
1/ 2 1/ 2
0
0
0
0
0
1
0
0
0
1/ 2
=
1/ 2
0
from which Tce is evaluated to be
0
1/ 2
Tce =
1/ 2
0
0
1/ 2
1/ 2
0
0
1/ 2
1/ 2
0
230
160
75
1
1
75
0 70/ 2
0 390/ 2
0
1
1
75
0 260/ 2
.
0 130/ 2
0
1
We conclude this section with an examination of the prescriptive interpretation of a rigid body motion. Figure 3.10 shows a rigid body that has been
displaced from some initial configuration to a new configuration. {a} is the fixed
frame, and the rigid body motion is represented by the SE(3) matrix Tba (we
shall specify in a moment how to locate the {b} frame from the two given configurations of the rigid body). Let p denote a point on the rigid body; pa R3
is then the coordinates for p in the initial configuration, and pb R3 is the
coordinates for p in the displaced configuration. In terms of Tba we then have
the relation
pb = Tba pa .
(3.96)
69
Figure 3.11: A rigid body displacement expressed as a screw motion.

The frame {b} is now drawn such that its location relative to the initial rigid
body configuration is the same as the location of frame {a} relative to the
displaced rigid body configuration (see Figure 3.10). For the given example, Tba
is given by
1
0
0
0
0 1/ 2 1/ 2 18
.
Tba =
(3.97)
0 1/ 2 1/ 2
0
0
0
0
1
3.3.3
Screw Motions
3.3.3.1
Mathematical Representation
In the planar example given at the beginning of this chapter, we saw that any
planar rigid body displacement can be achieved by rotating the rigid body about
some fixed point in the plane (for a pure translation, this point lies at infinity).
A similar result also exists for spatial rigid body displacements: called the
Chasles-Mozzi Theorem, it states that every rigid body displacement can
be expressed as a rotation about some fixed axis in space, followed by a pure
translation parallel to that axis. In fact, switching the order of the rotation and
translation will still result in the same displacement. One can therefore imagine
the rotation and translation occuring simultaneously, resulting in the familiar
motion of a screw.
In this section we shall develop a mathematical representation for screw
motions. Figure 3.11 shows a rigid body undergoing a displacement in threedimensional space; all vectors in the figure are expressed in terms of the fixed
{0} frame coordinates. The initial and final configurations of the rigid body are
labelled by frames {1} and {2}, respectively. According to the Chasles-Mozzi
Theorem, there exists a screw axisrepresented by the line passing through the
point q and in the direction of the unit vector such that the displacement can
be characterized as a screw motion about this axis. The screw motion consists of
70
Rigid-Body Motions
a rotation about the screw axis by some angle , and a translation parallel to the
screw axis by a distance d. As mentioned earlier, the order of the rotation and
translation are interchangeable. We now derive the homogeneous transformation
corresponding to this screw motion. Suppose the relative displacements T01 and
T02 are given by

R p
T01 =
(3.98)
0 1
0

R p0
T02 =
.
(3.99)
0 1
The screw pitch, denoted h, is a scalar quantity defined as
h=
d
.
(3.100)
From the figure we can express R0 and p0 as

R0
p
= e[] R
(3.101)
= q + e[] (p q) + h.
(3.102)
The first equation is a consequence of the fact that the orientation of frame {2}
is obtained by rotating frame {1} about the axis by an angle . The second
equation follows by verifying that p0 is the vectorial sum of the three vectors q,
e[] (p q), and h as indicated in the figure. The above two equations can
be combined into the following matrix equation:
0
[]

R p0
R p
e
I e[] q + h
=
.
(3.103)
0 1
0 1
0
1
The SE(3) matrix

e[]
0

I e[] q + h
1
(3.104)
is the homogeneous transformation representation of the screw motion. In the

remainder of this chapter and also the next chapter on kinematics, we shall
be making the case that it is better to express this matrix as a pure matrix
exponential, in the same way that a rotation matrix can be expressed as the
exponential of a skew-symmetric matrix.
In the meantime we first introduce some notation.
Definition 3.7. Given , v R3 , let

S=
R6 ,
v
which we also write more
vector S = (, v) is called
matrix:

[]
[S] =
0
(3.105)
compactly as S = (, v) R6 . The six-dimensional

a twist. Define [S] R44 to be the following 4 4

0
3 2
v
0
1 ,
, [] = 3
(3.106)
0
2 1
0
71
where the bottom row of [S] consists of all zeros.

With the above notation, let us now derive a closed-form expression for the
matrix exponential e[S] , where S = (, v) with R3 satisfying kk = 1.
By an appropriate choice of v, the exponential e[S] can be made equal to the
matrix of Equation (3.104); our immediate task is to find this v. Expanding the
matrix exponential in series form leads to
e[S]
=
=
2
3
I + [S] + [S]2 + [S]3 + . . .
2!
3!
[]
3
2
e
G()v
, G() = I + [] + []2 + . . . (3.107)
0
1
2!
3!
Noting the similarity between G() and the series definition for e[] , it is tempting to write I + G()[] = e[] , and to conclude that G() = (e[] I)[]1 .
This is wrong: []1 does not exist (try computing det[]).
Instead we make use of the result []3 = [] that was obtained from the
Cayley-Hamilton Theorem. In this case G() can be simplified to
G()
2
3
= I + [] + []2 + . . .
3!
22!

3

4
6
5
7
= I +
+
. . . [] +
+
. . . []2
2!
4!
6!
3!
5!
7!
= I + (1 cos )[] + ( sin )[]2 .
(3.108)
Putting everything together,

Proposition 3.18. Let , v R3 and define S = (, v). If satisfies kk = 1,
then for any R,
[]

e
I + (1 cos )[] + ( sin )[]2 v
[S]
e
=
.
(3.109)
0
1
If = 0 so that S = (0, v), then
e
[S]

=
I
0
v
1

.
(3.110)
The latter result of the propostion can be verified directly from the series
expansion of e[S] with set to zero.
We now answer the question of what choice of v results in (3.109) equaling
(3.104). The answer is
v = []q + h.
(3.111)
This can be verified by substituting this v into Equation (3.109) and making
use of the identities [] = 0 and []3 = []. With this choice of v, the pitch
h of the screw motion can then be expressed as
h = T v.
(3.112)
72
Rigid-Body Motions
3.3.3.2
Matrix Logarithm of a Homogeneous Transformation
The above derivation essentially provides a constructive proof of the ChaslesMozzi Theorem. That is, given an arbitrary (R, p) SE(3), one can always
find some S = (, v) and a scalar such that

R p
[S]
e
=
.
(3.113)
0 1
In the simplest case, if R = I, then = 0, and the preferred choice for v is
v = p/kpk (this would make = kpk the translation distance). If R is not the
identity matrix and tr R 6= 1, one solution is given by
[]
v
1
(R RT )
2 sin
= G1 ()p,
=
(3.114)
(3.115)
where satisfies 1 + 2 cos = tr R. We leave as an exercise the verification of

the following formula for G1 ():
G1 () =
1
1
1 1
I [] + ( cot )[]2 .
2
2
2
(3.116)
Finally, if tr R = 1, we choose = , and [] can be obtained via the matrix

logarithm formula on SO(3). Once [] and have been determined, v can then
be obtained as v = G1 ()p.
Algorithm: Given (R, p) written as T SE(3), we seek S = (, v) R6 and
[0, ] such that e[S] = T .
(i) If R = I, then set = 0, v = p/kpk, and = kpk.
(ii) If tr R = 1, then set = , and [] = log R as determined by the matrix
logarithm formula on SO(3) for the case tr R = 1. v is then given by
v = G1 ()p.

(iii) Otherwise set = cos1 tr R1
[0, ) and
2
[]
1
(R RT )
2 sin
G1 ()p,
(3.117)
(3.118)
where G1 () is given by Equation (3.116).
Example
As an example, we consider the special case of planar rigid body motions and
examine the matrix logarithm formula on SE(2). Referring once again to Fig-
73
Figure 3.12: Transformation of the twist vector for a screw motion under a
change of reference frames.
ure 3.1(b), suppose the initial and final configurations
tively represented by the SE(2) matrices
cos 30 sin 30 1
sin 30
cos 30 2
Tsb =
0
0
1
cos 60 sin 60 2
cos 60 1
Tsc = sin 60
0
0
1
of the body are respec
For this example, the rigid body displacement occurs in the x-y plane. The
corresponding screw motion will therefore have its screw axis in the direction of
the z-axis, and be of zero pitch. The twist vector (, v) representing the screw
motion will be of the form
(0, 0, 3 )
(v1 , v2 , 0).
Using this reduced form, we seek the screw motion that displaces the frame at
Tsb to Tsc , i.e., Tsc = e[S] Tsb , or
1
Tsc Tsb
= e[S] ,
where
0
[S] = 3
0
3
0
0
v1
v2 .
0
1
We can apply the matrix logarithm algorithm directly to Tsc Tsb
to obtain [S]
and as follows:
0 1
3
0 3 .
[S] = 1
0
0
0
74
Rigid-Body Motions
Alternatively, we can observe that the displacement is not a pure translation

Tsb and Tsc have rotation components that differ by an angle of 30 and quickly
determine that = 30 and 3 = 1. We can also graphically determine the
point q = (qx , qy ) on the x-y plane that the screw axis must pass through; for
our example this point is given by q = (3, 3). Since the screw motion has zero
pitch (h = 0), using the relation v = q + h leads to
(vx , vy ) = (qy , qx ) = (3, 3).
3.3.3.3
Transformation Under Change of Reference Frames
We now examine how the twist vector for a screw motion transforms under a
change of reference frames. For this purpose consider the general screw motion
of pitch h (see Figure 3.12). Pick an arbitrary point q on this screw axis, and
denote its coordinates in terms of reference frames {a} and {b} respectively by
qa , qb R3 . Let the twist for this screw motion as seen from frame {a} be given
by Sa = (a , va ), where va = a qa +ha . Similarly, let the twist for this same
screw motion as seen from frame {b} be Sb = (b , vb ), where vb = b qb +hb .
Given the homogeneous transformation Tba = (Rba , pba ) SE(3), we now try
to express (b , vb ) in terms of (a , va ) and (Rba , pba ).
From our previous results we know that b = Rba a and qb = Rba qa + pba .
It follows that
vb
= Rba a (Rba qa + pba ) + hRba a

= [Rba a ]Rba qa [Rba a ]pba + hRba a
T
= Rba [a ]Rba
Rba qa + [pba ]Rba a + hRba a
= Rba [a ]qa + Rba ha + [pba ]Rba a

= Rba ([a ]qa + ha ) + [pba ]Rba a
= Rba va + [pba ]Rba a ,
(3.119)
where we have made use of the properties u v = [u]v and R[u]RT = [Ru] for
any u, v R3 and R SO(3). These equations for b and vb can be written in
the equivalent matrix form

T

T
[b ] vb
Rba pba
[a ] va
Rba Rba
pba
=
.
(3.120)
0
0
0
1
0
0
0
1
1
The above can be written as [Sb ] = Tba [Sa ]Tba
, which can be also expressed in
vector form as

b
Rba
0
a
=
.
(3.121)
vb
[pba ] Rba Rba
va
We introduce the following transformation to express the above relation:

Definition 3.8. Given S = (, v) R6 , S 0 = ( 0 , v 0 ) R6 , T = (R, p)
SE(3), the Adjoint map S 0 = AdT (S) is defined as follows:
0

R
0
S0 =
=
= AdT (S).
(3.122)
v0
[p] R R
v
75
The notation [AdT ] is used to denote the 6 6 matrix representation of the

linear transformation AdT :

R
0
[AdT ] =
.
(3.123)
[p] R R
Using this notation Equation (3.122) can also be expressed as follows:
S 0 = [AdT ]S.
(3.124)
The adjoint map can also be equivalently expressed in the following matrix form:
[S 0 ] =

[ 0 ] v 0
=
0
1
T [S]T 1

R[]RT
0
[p]R + Rv
0

.
(3.125)
The adjoint map satisfies the following properties, all verifiable by direct
calculation:
Proposition 3.19. Let T1 , T2 SE(3), and S = (, v). Then
AdT1 (AdT2 (S)) = AdT1 T2 (S).
(3.126)
Also, for any T SE(3) the following holds:

Ad1
T = AdT 1 ,
(3.127)
The second property follows from the first by choosing T1 = T 1 and T2 = T ,

so that
AdT 1 (AdT (S)) = AdT 1 T (S) = AdI (S) = S.
(3.128)
The following proposition states the previously derived transformation rule
for the twist of a screw motion under a change of reference frames:
Proposition 3.20. Suppose a screw motion is described in terms of reference
frame {a} by the twist Sa = (a , va ), and in terms of reference frame {b} by the
twist Sb = (b , vb ). Sa and Sb are then related by
Sb
= AdTba (Sa )
(3.129)
Sa
= AdTab (Sb ).
(3.130)
We close this section by comparing the prescriptive and descriptive interpretations of a screw motion. Referring once again to Figure 3.11, the relationship
between T01 and T02 can be expressed as follows:
T02
1
T01 T12 T01
=
=
e[S] T01
[S]
(3.131)
(3.132)
Here e[S] can be thought as a transformation that displaces frame T01 to T02 ;
this is the prescriptive interpretation of the screw motion e[S] . T12 can also be
76
Rigid-Body Motions
.
Figure 3.13: (a) The instantaneous angular velocity vector. (b) Calculating x
0
expressed as the matrix exponential T12 = e[S ] for some S 0 = ( 0 , v 0 ); here the
0
screw motion e[S ] is a descriptive representation for frame {2} as seen from
1
frame {1}. Applying the general matrix identity P eA P 1 = eP AP , we get
e[S]
1
= T01 e[S ] T01
1
0
= e(T01 [S ]T01 ) .
(3.133)
(3.134)
In terms of the adjoint notation, the relation between the twists S and S 0 can
now be expressed as
S = AdT01 (S 0 ).
(3.135)
Here S is the twist representation of the screw motion expressed in frame {0}
coordinates, while S 0 is the twist representation of the same screw motion in
frame {1} coordinates, which agrees with the previous proposition.
3.4
3.4.1
Velocities and Forces

Angular Velocities
We first examine the angular velocity of a rigid body. Referring to Figure 3.13(a),
suppose a body frame with unit axes {
x, y
, z} is attached to the rotating body.
Let us determine the time derivatives of these unit axes. Beginning with x
, first
note that x
is of unit length; only the direction of x
can vary with time (the same
goes for y
and z). If we examine the body frame at times t and t + tsince
whats relevant for us is the orientation of the body frame, for better visualization we draw the frame at the two instants with coinciding originsthe change
in frame orientation from t to t + t can be described as a rotation of angle
about some unit axis w
passing through the origin.
In the limit as t approaches zero, the ratio
t becomes the rate of rotation
and w
,
can similarly be regarded as the instantaneous axis of rotation. In fact,
3.4. Velocities and Forces
77
w
and can be put together to define the angular velocity vector w as follows:
w=w
.
(3.136)
It is important to note here that both the rate of rotation and the rotation
axis direction w
can vary with time. Referring to Figure 3.13(b), it should be
evident that
x
wx
(3.137)
= wy
z = w z.
(3.138)
(3.139)
Let us now express these relations explicitly in terms of fixed frame coordinates. Let R(t) be the rotation matrix describing the orientation of the body
frame with respect to the fixed frame. Then the first column of R(t), denoted
r1 (t), describes x
in fixed frame coordinates; similarly, r2 (t) and r3 (t) respectively describe y
and z in fixed frame coordinates. Let s R3 be the angular
velocity w expressed in fixed frame coordinates. The previous three velocity
relations expressed in fixed frame coordinates become
ri = s ri = [s ]ri , i = 1, 2, 3.
The above three equations can be rearranged into the following single matrix
equation:
R = [s ]R.
(3.140)
Since R1 = RT , the above can also be written
T = [s ].
RR
(3.141)
T always skew-symmetric, but also admits

This result shows that not only is RR
the physical interpretation as the angular velocity in fixed-frame coordinates.
Now let b be w expressed in body frame coordinates. To see how to obtain
b from s and vice versa, let us momentarily return to our earlier notational
practice of writing R as Rsb (recall that Rsb means the orientation of the body
frame {b} as seem from the fixed frame {s}). Then s and b are two different
vector representations of the same angular velocity vector w, with s = Rsb b
1
1
and b = Rsb
s . Since Rsb
= RT , we have
b = R T s .
(3.142)
Let us now express the above relation in skew-symmetric matrix form. Recall
that for any R3 and R SO(3), R[]RT = [R] always holds. With this
result, we now express both sides of Equation (3.142) in skew-symmetric matrix
form:
[b ] = [RT s ]
= RT [s ]R
(3.143)
T )R
= RT (RR
= RT R
In summary, we have the following two equations that relate the rotation matrix
R to the angular velocity w:
78
Rigid-Body Motions
Proposition 3.21. Let R(t) denote the orientation of the rotating frame as
seen from the fixed frame. Denote by w the angular velocity of the rotating
frame. Then
1
RR
R1 R
[s ]
(3.144)
[b ],
(3.145)
where s R3 is the fixed frame vector representation of w, and b R3 is the

moving frame vector representation of w.
3.4.2
Spatial Velocities
We now consider both the linear and angular velocity of the moving frame. As
before, denote by {s} and {b} the fixed (space) and moving (body) frames,
respectively, and let

R(t) p(t)
Tsb (t) = T (t) =
(3.146)
0
1
denote the homogeneous transformation of {b} as seen from {s} (to keep the
notation uncluttered, for the time being we shall write T instead of the usual
Tsb ).
In the previous section we discovered that pre- or post-multiplying R by
1
R results in (a skew-symmetric representation of) the angular velocity vector,
either in fixed or moving frame coordinates. One might reasonably ask if a
similar property carries over to T , i.e., whether T 1 T and T T 1 carry similar
physical interpretations.
Let us first see what happens when we pre-multiply T by T 1 :
T

R
RT p
R p
T 1 T =
0
1
0 0
T

T
R R R p
=
(3.147)
0
0

[b ] vb
=
.
0
0
Recall that RT R = [b ] is just the (skew-symmetric matrix representation of)
the angular velocity expressed in moving frame coordinates. Also, p is the linear
velocity of the moving frame origin expressed in fixed frame coordinates, and
RT p = vb is this linear velocity expressed in moving frame coordinates. Putting
these two observations together, we can conclude that T 1 T represents the
linear and angular velocity of the moving frame in terms of the moving frame
coordinates.
The previous calculation of T 1 T suggests that it is reasonable to merge b
and vb into a single six-dimensional velocity vector. This is exactly what we
shall do. Define

b
[b ] vb
Vb =
, [Vb ] =
= T 1 T .
(3.148)
vb
0
0
79
Figure 3.14: Physical interpretation of vs . The initial (solid line) and displaced
(dotted line) configurations of a rigid body.
We shall also use the more compact notation Vb = (b , vb ), so that [Vb ] is the
4 4 matrix representation of Vb . We shall call Vb the spatial velocity in the
moving (or body) frame.
Now that we have a physical interpretation for T 1 T , let us evaluate T T 1 :

T

R
RT p
R p
T T 1 =
0
1
0 0

T
T
RR
p RR p
=
(3.149)
0
0

[s ] vs
=
.
0
0
T is the angular velocity
Observe that the skew-symmetric matrix [s ] = RR
T p is not the linear
expressed in fixed frame coordinates, but that vs = p RR
velocity of the moving frame origin expressed in the fixed frame (that quantity
would simply be p).
On the other hand, if we write vs as
vs = p s p = p + s (p),
(3.150)
the physical meaning of vs can now be inferred: imagining the moving frame is
attached to an infinitely large rigid body, vs is the instantaneous velocity of the
point on this body corresponding to the fixed frame origin (see Figure 3.14).
As we did for b and vb , we also merge s and vs into a six-dimensional
velocity:

s
[s ] vs
Vs =
, [Vs ] =
= T T 1 .
(3.151)
vs
0
0
80
Rigid-Body Motions
We shall also use the more compact notation Vs = (s , vs ), so that [Vs ] is the
4 4 matrix representation of Vs . We call Vs the spatial velocity in the fixed
(or space) frame.
If we regard the fixed and moving bodies as being infinitely large, there is in
fact an appealing and natural symmetry between Vs = (s , vs ) and Vb = (b , vb ):
(i) b is the angular velocity in moving frame coordinates;
(ii) s is the angular velocity in fixed frame coordinates;
(iii) vb is the linear velocity of the moving frame origin, in moving frame
coordinates;
(iv) vs is the linear velocity of the fixed frame origin (regarded as a point
on the moving rigid body), in fixed frame coordinates.
Vb can be obtained from Vs as follows:
[Vb ]
= T 1 T
= T 1 (T T 1 )T
= T 1 [Vs ] T.
(3.152)
Going the other way,

[Vs ] = T [Vb ] T 1 .
(3.153)
The reader may recognize that the relation between Vs and Vb is given precisely by the transformation rule for a twist vector of a screw motion under a
change of reference frames. In fact, using the adjoint mapping AdT the above
can be expressed as Vs = AdT (Vb ), Vb = AdT 1 (Vs ). This transformation rule
can be more easily remembered if we write T = Tsb , in which case
Vs
AdTsb (Vb )
(3.154)
Vb
AdTbs (Vs ).
(3.155)
In expanded form, the above becomes

s
R
0
b
=
vs
[p] R R
vb

T
b
R
0
s
=
.
vb
vs
RT [p] RT
(3.156)
(3.157)
The main results on spatial velocities derived thus far are summarized in the
following proposition:
Proposition 3.22. Given a fixed (space) frame {s} and moving (body) frame
{b}, let Tsb (t) SE(3) be differentiable, where

R(t) p(t)
Tsb (t) =
.
(3.158)
0
1
81
Then
1
Tsb
Tsb

= [Vb ] =
[b ] vb
0
0
is the spatial velocity in body coordinates, and

[s ] vs
1
Tsb Tsb
= [Vs ] =
0
0
(3.159)
(3.160)
is the spatial velocity in space coordinates. Vs and Vb are related by

s
R
0
b
Vs =
=
= AdTsb (Vb )
(3.161)
vs
[p] R R
vb

b
RT
0
s
Vb =
=
= AdTbs (Vs ).
(3.162)
vb
vs
RT [p] RT
3.4.3
Spatial Forces
Consider a force f acting on a rigid body. If the rigid bodys center of mass does
not lie on the line of action of the force, then the force will cause the rigid body
to rotate; this rotation is caused by a moment. More precisely, the moment m
generated by f about some reference point p in physical space is defined to be
the cross product
m = r f,
(3.163)
where r is the vector from p to the point on the rigid body at which the force is
applied. For a rigid body subject to a collection of forces and moments, if both
the sum of the forces and sum of the moments are zero, then the body will be
stationary, and is said to be in static equilibrium. When summing moments
it is important that each of the moments be expressed with respect to the same
reference point p.
Just as we found it advantageous to merge angular and linear velocities into
a single six-dimensional spatial velocity vector, for the same reasons it will be
useful to analogously merge forces and moments into a single six-dimensional
spatial force. Toward this end, suppose a force f is being applied to a point
p on a rigid body. Given some reference frame {a}, let fa R3 denote the
vector representation of f in frame {a} coordinates. This force then generates
a moment with respect to the {a} frame origin; in {a} frame coordinates, this
moment is
ma = ra fa ,
(3.164)
where ra R3 is the vector from the {a} frame origin to p, expressed in {a}
frame coordinates. We pair the force fa and moment ma into a single sixdimensional spatial force Fa = (ma , fa ), and refer to it as the spatial force in
frame {a} coordinates. Adopting the terminology from classical screw theory, a
spatial force will also be referred to as a wrench.
Suppose we now wish to express the force and moment in terms of another
reference frame {b}. Let fb R3 denote the vector representation of f in {b}
82
Rigid-Body Motions
ra
{a}
Figure 3.15: Relation between Fa and Fb .

frame coordinates. The moment generated by this force with respect to the {b}
frame origin is, again expressed in {b} frame coordinates,
mb = rb fb ,
(3.165)
where rb R3 is the {b} frame representation of the vector from the {b} frame
origin to p. As we did for Fa , let us also bundle mb and fb into the sixdimensional spatial force Fb = (mb , fb ), and refer to it as the spatial force in
{b} frame coordinates.
We now determine the relation between Fa = (ma , fa ) and Fb = (mb , fb ).
Referring to Figure 3.15, denote the transformation Tab by

Rab pab
Tab =
.
0
1
Clearly fb = Rba fa , which with the benefit of hindsight, and remembering that
T
Rba = Rab
, we shall write as
T
fb = Rab
fa .
(3.166)
The moment mb is given by rb fb , where rb = Rba (ra pab ); this follows from
the fact that the ra pab is expressed in {a} frame coordinates, and must be
transformed to {b} frame coordinates via multiplication by Rba . Again with
hindsight, we shall write
T
rb = Rab
(ra pab ).
The moment mb = rb fb can now be written in terms of fa and ma as
mb
T
T
= Rab
(ra pab ) Rab
fa
T
T
T
T
[Rab
ra ]Rab
fa [Rab
pab ]Rab
fa
T
T
= Rab
[ra ]fa Rab
[pab ]fa
T
T
= Rab
ma + Rab
[pab ]T fa ,
(3.167)
3.5. Summary
83
where in the last line we make use of the fact that [pab ]T = [pab ]. Writing both
mb and fb in terms of ma and fa we have, from Equations (3.166) and (3.167),

mb
fb

=
Rab
[pab ] Rab
0
Rab
T
ma
fa

,
(3.168)
or in terms of spatial forces and the adjoint map,

Fb = AdTTab (Fa ) = [AdTab ]T Fa .
(3.169)
The above shows that under a change of reference frames, spatial velocities
transform via the adjoint map, whereas spatial forces transform via the adjoint
transpose map. The following proposition formally states this result.
Proposition 3.23. Given a force f, let m be the moment generated by f with
respect to some point p in physical space. Given a reference frame {a}, let
fa R3 and ma = ra fa R3 be representations of f and m in frame {a}
coordinates, where ra R3 is the vector from the {a} frame origin to p, also
expressed in {a} frame coordinates. Similarly, given another reference frame
{b}, let fb R3 and mb = rb fb R3 be representations of f and m in frame
{b} coordinates, where rb R3 is the vector from the {b} frame origin to p, also
expressed in {b} frame coordinates. Defining the spatial forces Fa = (ra fa , fa )
and Fb = (rb fb , fb ), Fa and Fb are related by
Fb
Fa
3.5
AdTTab (Fa ) = [AdTab ]T Fa
(3.170)
AdTTba (Fb )
(3.171)
= [AdTba ] Fb .
Summary
Rigid bodies occupy a three-dimensional physical space. Assuming a

length scale for physical space has been chosen, and given a fixed righthanded reference frame (the fixed frame), and a right-handed reference
frame (the body frame) attached to the body, the orientation of the
body frame with respect to the fixed frame is described by a 3 3 rotation matrix, while the position of the body frame origin is described by a
three-dimensional vector.
The set of all 3 3 rotation matrices is called the Special Orthogonal
group in three dimensions, and denoted SO(3). SO(3) matrices are orthogonal, e.g., R SO(3) satisfies RT R = I, and further satisfy det R = 1.
Some other properties of R are R1 = RT and kRxk = kxk for all x R3 .
Define Rab SO(3) to be the relative orientation of frame {b} as seen
from frame {a}. Then given three frames {a}, {b}, {c}, Rab , Rbc , and Rac
always satisfy Rab Rbc = Rac . A consequence of this result is Raa = I and
1
Rab
= Rba .
84
Rigid-Body Motions
The ZYX Euler angles are a three-parameter representation for rotations. Given , [0, 2] and [/2, /2], the corresponding rotation
matrix R is given by
c s 0
c 0 s
1 0
0
1 0 0 c s .
R(, , ) s c 0 0
0
0
1
s 0 c
0 s c
Given R SO(3), closed-form formulas for determining (, , ) are also
available. More generally, Euler angles physically correspond to a sequence
of rotations about the axes of the moving frame (e.g., in the case of ZYX
Euelr angles, a rotation about the z-axis of the moving frame by , followed
by a rotation about the y
-axis of the moving frame by , followed by a
rotation about the x
-axis of the moving frame by ).
Roll-Pitch-Yaw angles can be interpreted as a sequence of rotations
about the axes of the fixed frame (e.g., in the case of XYZ roll-pitch-yaw
angles, as a rotation about the x
-axis of the fixed frame by , followed by
a rotation about the y
-axis of the fixed frame by , followed by a rotation
about the z-axis of the fixed frame by ). The formulas for XYZ rollpitch-yaw angles are identical to the formulas for the ZYX Euler angles.
A similar relationship holds between the physical interpretations for all
Euler angle representations and their corresponding roll-pitch-yaw angle
representations.
Euler angles can be defined about any three axes, in which the first and
second axes are orthonormal, and the second and third axes are orthonormal (e.g., ZYZ, ZXZ Euler angles).
The exponential coordinates parametrize a rotation matrix R in terms
of a unit vector R3 indicating the direction of the axis of rotation,
and a rotation angle [0, ]. The corresponding rotation is given by
e[] = I + sin [] + (1 cos )[]2 .
Going the other way, given R SO(3), closed-form formulas for and
exist.
The exponential coordinates allow us to visualize SO(3) as a solid ball of
radius : a point p inside this solid ball represents a rotation about the
axis = p/kpk, by an angle kpk. If kpk = , then p and p represent the
same rotation.
The unit quaternions are a four-parameter representation for rotations.
Given R SO(3) with exponential representation e[] , the corrresponding unit quaternion representation for R is given by the following four-
3.5. Summary
85
dimensional vector q of unit length:
1
q0
2 1 + r11 + r22 + r33
1
q1
4q0 (r32 r23 )
=
1
q2
4q0 (r13 r31 )

1
q3
4q0 (r21 212 )
For every rotation R SO(3), there exist two unit quaternion representations q and q for R corresponding to antipodal points on the unit
three-sphere S 3 .
Let R(t) SO(3) describe the orientation of a moving (or body) reference
frame as seen from the fixed (or space) frame, and w be the instantaneous
angular velocity of the moving frame. let s R3 be w expressed in
fixed frame coordinates, and b R3 be w expressed in the body frame
coordinates. Then s and b can be obtained from R(t) via
T
= RR
[b ] = RT R.
[s ]
where [s ] and [b ] denote the 33 skew-symmetric matrix representations

of s and b , respectively. s and b are related by
s
= Rsb b = Rb
= Rbs s = RT s .
A rigid body motion is described by an element of the Special Euclidean

group SE(3):

R p
T =
, R SO(3), p R3 .
0 1
Elements of SE(3) are also represented more compactly as (R, p).
T = (R, p) SE(3) can be interpreted as a description of the position
and orientation of some reference frame relative to another. In this case, if
Tab SE(3) denotes the relative position and orientation of frame {b} as
seen from frame {a}, and Tbc , Tac are similarly defined, then Tab Tbc = Tac .
T = (R, p) SE(3) also relates the coordinates for a point in space
in terms of two different reference frames. That is, if qa R3 are the
coordinates of a given point q in physical space with respect to frame {a},
and qb R3 are the coordinates for the same point q in physical space
with respect to another frame {b}, then qa = Tab qb and qb = Tba qa .
T = (R, p) SE(3) can also be interpreted as a transformation T :
R3 R3 that maps a point x R3 in physical space to another point
T x R3 in physical space, where T x = Rx + p. The transformation T
86
Rigid-Body Motions
is a rigid body motion (i.e., a displacement of a rigid body in physical
space) in the following sense: (i) kT x T yk = kx yk for all x, y R3 ;
(ii) hT x T z, T y T zi = hx z, y zi for all x, y, z R3 , where h, i
denotes the standard Euclidean inner product in R3 , i.e., hx, yi = xT y.
The former condition implies that distances are preserved, while the latter
implies that angles are preserved.
The Chasles-Mozzi Theorem asserts that every rigid body motion T
SE(3) admits a representation as a screw motion, i.e., a rotation about
some fixed axis in space (the screw axis), followed by a translation parallel
to that axis.
For a general screw motion that is not a pure translation, the screw axis
is described by a unit vector R3 , kk = 1 indicating the direction of
the screw axis, a point q R3 on the axis, and a nonnegative scalar h
indicating the pitch of the screw (that is, the amount of translation per
rotation ). h is zero for a pure rotation. Defining v = q + h, the
rigid body motion T SE(3) corresponding to a rotation about the the
screw S = (, v) is given by
[]

e
I + (1 cos )[] + ( sin )[]2 v
e[S] =
.
0
1
where the notation [S] indicates

[S] =
[]
0
v
0

.
Given T SE(3), closed form formulas are available for finding S = (, v)

and R such that T = e[S] .
For a pure translation h is taken to be infinite, is taken to be zero, and
v is taken to be a unit vector in the direction of the translation.
Given a screw vector Sa = (a , va ) and Tba = (Rba , pba ) SE(3), the
Adjoint map AdTba is defined as follows:

Rba
0
a
AdTba (Sa ) =
.
[pba ] Rba Rba
va
Denoting AdTba (Sa ) = Sb , then Sa can be physically interpreted to be
the screw vector description in terms of reference frame {a}, while Sb is
the screw vector description of the same screw motion in terms of another
reference frame {b}.
Given a space (fixed) frame {s} attached to ground, and a body (moving)
frame {b} attached to a rigid body moving in physical space, let

R(t) p(t)
Tsb (t) = T (t) =
SE(3)
0
1
3.5. Summary
87
denote the spatial displacement of {b} as seen from {s}. Then

T p RR
Tp
[s ] vs
RR
T T 1 =
=
= [Vs ]
0
0
0
0
T

[b ] vb
R R RT p
T 1 T =
=
= [Vb ].
0
0
0
0
Vs = (s , vs ) R6 is called the spatial velocity in space frame coordinates, while Vb = (b , vb ) R6 is the spatial velocity in body
frame coordinates. Here s R3 and b R3 respectively denote the
angular velocity of the body frame expressed in space and body frame
coordinates. vb R3 denotes the linear velocity of the body frame origin,
expressed in body frame coordinates. vs R3 denotes the linear velocity
of the space frame origin (regarded as a point on the moving rigid body),
expressed in space frame coordinates. Vs and Vb are related by
[Vs ]
T [Vb ]T 1
[Vb ]
T 1 [Vs ]T,
or, alternatively,

Vs
Vb

b
= AdTsb (Vb )
vb

b
RT
0
s
=
=
= AdTbs (Vs ).
vb
vs
RT [p] RT
s
vs
R
[p] R
0
R
The matrix [AdT ] R66 is defined to be

R
0
[AdT ] =
.
[p] R R
Given a rigid body with a body frame {b} attached, and a fixed frame
{s} chosen for physical space. Suppose a force f is applied to some point
p on the rigid body. The moment m generated by f with respect to the
fixed frame origin is ms = rs f, where rs is the vector from the fixed
frame origin to p. Let ms R3 and fs R3 be vector representations of
ms and f in the fixed {s} frame coordinates. The spatial force in space
coordinates Fs R6 is then defined to be Fs = (ms , fs ).
Let fb be the {b} frame representation of the force f. Further define
mb = rb f to be the moment generated by f with respect to the body
frame origin, where rb is the vector from the {b} frame origin to the point
p. Let mb R3 be the b frame vector representation of mb . The spatial
force in body coordinates Fb R6 is defined to be Fb = (mb , fb ). Fb
and Fs are related by
Fb
AdTTsb (Fs ) = [AdTsb ]T Fs
Fs
AdTTbs (Fb ) = [AdTbs ]T Fb .
88
Rigid-Body Motions
Adopting terminology from classical screw theory, a spatial force is also
referred to as a wrench.
3.6
More detailed coverage of the various parametrizations of SO(3) can be found in,
e.g., [32] and the references cited there. The treatment of the matrix exponential
representation for screw motions is based on the work of Brockett [4], and a more
mathematically detailed discussion can be found in [25]. Classical screw theory
is presented in its original form in R. Balls treatise [2]. More modern (algebraic
and geometric) treatments can be found in, e.g., Bottema and Roth [3], Angeles
[1], and McCarthy [20].
3.7. Exercises
3.7
89
Exercises
Y , Z}
for physical space, let p be a point whose co1. Given a fixed frame {X,
1
1
1
ordinates are ( 3 , 6 , 2 ). Suppose p is rotated about the fixed frame X-axis

by 30 degrees, then about the fixed frame Y -axis by 135 degrees, and finally
about the fixed frame Z-axis

by -120 degrees.
(a) What are the coordinates of the point p following these three rotations?
(b) Find the rotation matrix R such that Rp are the coordinates you obtained
in (a).
2. (a) Derive a procedure for finding the ZXZ Euler angles of a rotation matrix.
(b) Using the results of (a), find the ZXZ Euler angles for the following rotation
matrix:
12 12
0
1 1 1
.
2
2
2
1
2
1
2
1
2
3. Show that rotation matrices of the form
r11 r12 0
r21 r22 r23
r31 r32 r33
can be parametrized using just two parameters and as follows:
cos
sin
0
sin cos cos cos sin .
sin sin cos sin cos
What should the range of values be for and ?
4. (a) Show that the three eigenvalues of a rotation matrix R SO(3) each have
unit magnitude, and conclude that they can always be written {+i, i, 1},
where 2 + 2 = 1.
(b) Show that a rotation matrix R SO(3) can always be factored in the form
0
R = A 0 A1 ,
0 0 1
where A SO(3) and 2 + 2 = 1. (Hint: Denote the eigenvector associated
with the eigenvalue + i by x + iy, x, y R3 , and the eigenvector associated
with the eigenvalue 1 by z R3 . For the purposes of this problem you may
90
Rigid-Body Motions
assume that the set of vectors {x, y, z} can always be chosen to be linearly
independent.)
5. (a) Find the general solution to the differential equation x = Ax, where

2 1
A=
.
0 1
What happens to the solution x(t) as t ?
(b) Do the same for

2 1
A=
.
1
2
What happens to the solution x(t) as t ?
6. Let x R2 , A R22 , and consider the linear differential equation x(t)
=
Ax(t). Suppose that

e3t
x(t) =
3e3t
is a solution for the initial condition x(0) = (1, 3), and
t
e
x(t) =
.
et
is a solution for the initial condition x(0) = (1, 1). Find A and eAt .
7. Given a differential equation of the form x = Ax + f (t), where x Rn and
f (t) is a given differentiable function of t. Show that the general solution can
be written
Z
t
x(t) = eAt x(0) =
eA(ts) f (s) ds.
(Hint: Define z(t) = eAt x(t), and evaluate z(t).)
8. (a) Prove the matrix identity M eA M 1 = eM AM .

(b) Under what conditions on A, B Rnn is the following matrix identity
true?
eA eB = eA+B .
9. Consider a wrist mechanism with two revolute joints 1 and 2 , in which the
end-effector frame orientation R SO(3) is given by
R = e[w1 ]1 e[w2 ]2 ,
with 1 = (0, 0, 1) and 2 = (0, 12 , 12 ). Determine whether the following
orientation is reachable (that is, find, if it exists, a solution (1 , 2 ) for the
3.7. Exercises
91
following R):
1
2
R= 0
1
2
0 12
1
0
1
0
2
Figure 3.16: A three degree of freedom wrist mechanism.
10. Figure 3.16 shows a three degree of freedom wrist mechanism in its zero
position (that is, with all its joints set to zero).
(a) Express the tool frame orientation R03 = R(, , ) as a product of three
rotation matrices.
(b) Find all possible angles (, , ) for the two values of R03 given below. If
no solution exists, explain why in terms of the analogy between SO(3) and the
solid ball of radius .
0 1
0
0 .
(i) R03 = 1 0
0 0 1
(ii) R03 = e[w] 2 , where w = (0, 15 , 25 ).
11. (a) Suppose we seek the logarithm of a rotation matrix R whose trace is
-1. From the exponential formula
e[] = I + sin [] + (1 cos )[]2 , kk = 1,
92
Rigid-Body Motions
and recalling that tr R = 1 implies = , the above equation simplifies to
1 2(22 + 32 )
21 2
21 3
21 2
1 2(12 + 32 )
22 3
R = I + 2[]2 =
2
2
21 2
22 3
1 2(1 + 2 )
Using the fact that 12 + 22 + 32 = 1, is it correct to conclude that
r
r
r
r11 + 1
r22 + 1
r33 + 1
,
2 =
,
3 =
.
1 =
2
2
2
is also a solution?
(c) Using the fact that []3 = [], the identity R = I + 2[]2 can also be
written in the alternative form
RI
2[]2
[] (R I)
2 [] = 2 []
[] (R + I)
0.
The resulting equation is a system of three linear equations in (1 , 2 , 3 ). What

is the relation between the solution to this linear system and the logarithm of
R?
12. (a) Given a rotation matrix A = Rot(
z , ), where Rot(
z , ) indicates a
rotation about the z-axis by an angle , find all rotation matrices R SO(3)
that satisfy AR = RA.
(b) Given rotation matrices A = Rot(
z , ) and B = Rot(
z , ), with 6= , find
all rotation matrices R SO(3) that satisfy AR = RB.
(c) Given arbitrary rotation matrices A, B SO(3), find all solutions R SO(3)
to the equation AR = RB.
13. (a) Exploiting all of the known properties of rotation matrices, determine
the minimum number of arithmetic operations (multiplication and division, addition and subtraction) required to multiply two rotation matrices.
(b) Due to finite arithmetic precision, the numerically obtained product of two
rotation matrices is not necessarily a rotation matrix; that is, the resulting
rotation A may not exactly satisfy AT A = I as desired. Devise an iterative
numerical procedure that takes an arbitrary matrix A R33 , and produces a
matrix R SO(3) that minimizes
kA Rk2 = tr (A R)(A R)T .
14. (a) Verify the formula for obtaining the unit quaternion representation of
a rotation R SO(3).
(b) Verify the formula for obtaining the rotation matrix R given a unit quaternion q S 3 .
3.7. Exercises
93
(c) Verify the product rule for two unit quaternions; that is, given two unit
quaternions q, p S 3 corresponding respectively to the rotations R, Q SO(3),
find a formula for the unit quaternion representation of the product RQ
SO(3).
(d) Compare the number of arithmetic operations for multiplying two rotation
matrices versus two unit quaternions. Which requires fewer arithmetic operations?
Figure 3.17: Four reference frames defined in a robots work environment.
15. Consider the robot of Figure 3.17, in which four reference frames are
depicted: the fixed frame {a}, the end-effector frame effector {b}, camera frame
{c}, and workpiece frame {d}.
(a) Find Tad and Tcd in terms of the dimensions given in the figure.
(b) Find Tab given that
1 0 0 4
0 1 0 0
Tbc =
0 0 1 0 .
0 0 0 1
16. Consider a robot arm mounted on a spacecraft as shown in Figure 3.18,
in which frames are attached to the earth {e}, satellite {s}, the spacecraft {a},
and the robot arm {r}, respectively.
(a) Given Tea , Tar , and Tes , find Trs .
94
Rigid-Body Motions
Figure 3.18: A robot arm mounted on a spacecraft.

(b) Suppose the frame {s} origin as seen from
more that
1 0
0
0 1
0
Ter =
0 0 1
0 0
0
{e} is (1, 1, 1). Suppose further

1
1
.
1
1
Write down the coordinates of the frame {s} origin as seen from frame {r}.
Figure 3.19: A classical bicycle with a larger front wheel.
17. Consider the classical bicycle of Figure 3.19, in which the diameter of the
front wheel is twice that of the rear wheel. Frames {a} and {b} are attached to
the centers of each wheel, and frame {c} is attached to the top of the front wheel.
Assuming the bike moves forward in the y direction, find Tac as a function of
the front wheels rotation angle (assume = 0 at the instant shown in the
figure).
3.7. Exercises
95
Figure 3.20: A laser tracking a moving target.
18. A target moves along a circular path at constant angular velocity rad/sec
as shown in Figure 3.20. The target is tracked by a laser mounted on a moving
platform, rising vertically at constant speed v. Assume the laser and the platform start at L1 at t = 0, while the target starts at frame T1 .
(a) Derive frames T01 , T12 , T03 as a function of t.
(b) Using your results from part (a), derive T23 as a function of t.
Figure 3.21: Spacecraft and space station.
19. Suppose the space station of Figure 18 is in circular orbit around the earth,
and at the same time rotates about an axis always pointing toward the north
star. A spacecraft heading toward the space station is unable to locate the
docking port due to an instrument malfunction. An earth-based ground station
96
Rigid-Body Motions
sends the following information
0 1
1
0
Tab =
0
0
0
0
to the spacecraft:
0 100
0
0
300
, pa = 800 ,
1
500
0
0
1
where pa is the vector p~ expressed in {a} frame coordinates.

(a) From the given information, find rb , the vector ~r expressed in {b} frame
coordinates.
(b) Determine Tbc at the instant shown in the figure. Assume here that the y
and z axes of the {a} and {c} frames are coplanar with the docking port.
Figure 3.22: Two toy cars on a round table.
20. Two toy cars are moving on a round table as shown in Figure 3.22. Car 1
moves at a constant speed v1 along the circumference of the table, while car 2
moves at a constant speed v2 along a radius; the positions of the two vehicles
at t = 0 are shown in the figure.
(a) Find T01 , T02 as a function of t.
(b) Find T12 as a function of t.
21. Figure 3.23 shows the configuration, at t = 0, of a robot arm whose first
joint is a screw joint of pitch h = 2. The arms link lengths are L1 = 10,
L2 = L3 = 5, and L4 = 3. Suppose all joint angular velocities are constant,
with values 1 = 4 , 2 = 8 , w3 = 4 rad/sec. Find Tsb (4) SE(3), i.e., the
end-effector frame {b} SE(3) relative to the fixed frame {s}, at time t = 4.
22. A cube undergoes two different screw motions from frame {1} to frame {2}
as shown in Figure 3.24. In both cases (a) and (b), the initial configuration of
the cube is
1 0 0 0
0 1 0 1
T01 =
0 0 1 0 .
0 0 0 1
3.7. Exercises
97
Figure 3.23: A robot arm with a screw joint.
(a) a
(b) b
Figure 3.24: A cube undergoing two different screw motions.

(a) For each case (a) and (b), find the screw parameter S = (, v) such that
T02 = e[S] T01 , where no constraints are placed on or v.
(b) Repeat (a), this time with the constraint that kk [, ].
23. A particle starts from the origin, and undergoes a radially increasing circular spiral motion in the y direction as illustrated in Figure 3.25. For every
translation of 10 units in the y direction, the particle completes one revolution.
Attaching a moving frame to the particle, the motion of this moving frame can
98
Rigid-Body Motions
Figure 3.25: A particle undergoing a screw motion.

be expressed in the form
T () = e[Sa ] e[Sb ] ,
for some Sa = (a , va ), Sb = (a , vb ). Find Sa and Sb .
24. Given R3 , kk = 1, and R, show that
I + (1 cos )[] + (1 sin )[]

2 1

= I []+ 1 (sec + cot ) []2 .

2
2
Under what conditions, if any, will the inverse fail to exist? (Hint: Express the
inverse as a quadratic matrix polynomial in [], and determine the coefficients.
The quadratic polynomial assumption can be justified via the identity []3 =
[].)
25. Given two reference frames {a} and {b} in physical space, and a fixed frame
{o}, define the distance between frames {a} and {b} as
p
dist(Toa , Tob ) 2 + ||pab ||2
where Rab = e[] . Suppose the fixed frame is displaced to another frame {o0 },
and that To0 a = SToa , To0 b = STo0 b for some constant S = (Rs , ps ) SE(3).
(a) Evaluate dist(To0 a , To0 b ) using the above distance formula.
(b) Under what conditions on S does dist(Toa , Tob ) = dist(To0 a , To0 b )?
26. Two frames {a} and {b} are attached to a moving rigid body. Show that
the spatial velocity of {a} in space frame coordinates is the same as the spatial
velocity of {b} in space frame coordinates.
Chapter 4
Forward Kinematics
The forward kinematics of a robot refers to the calculation of the position and
orientation of its end-effector frame from its joint values. Figure 4.1 illustrates
the forward kinematics problem for the 3R planar open chain. Starting from
the base link, the link lengths are L1 , L2 , and L3 . Choose a fixed frame {0}
with origin located at the base joint as shown, and assume an end-effector frame
{4} has been attached to the tip of the third link. The Cartesian position (x, y)
and orientation of the end-effector frame as a function of the joint angles
(1 , 2 , 3 ) are then given by
x =
L1 cos 1 + L2 cos(1 + 2 ) + L3 cos(1 + 2 + 3 )
(4.1)
L1 sin 1 + L2 sin(1 + 2 ) + L3 sin(1 + 2 + 3 )
(4.2)
1 + 2 + 3 .
(4.3)
If one is only interested in the (x, y) position of the end-effector, the robots
task space is then taken to be the x-y plane, and the forward kinematics would
then consist of Equations (4.1)-(4.2) only. If the end-effectors position and
orientation both matter, the forward kinematics would then consist of the three
equations (4.1)-(4.3).
While the above analysis can be done using only basic trigonometry, it is
not difficult to imagine that for more general spatial chains, the analysis can
become considerably more complicated. A more systematic method of deriving
the forward kinematics would be to first attach reference frames to each of the
links; in Figure 4.1 the three link reference frames are respectively labelled {1},
{2}, and {3}. The forward kinematics can then be written as a product of four
SE(2) matrices:
T04 = T01 T12 T23 T34 ,
99
100
Forward Kinematics
Figure 4.1: Forward kinematics of a 3R planar open chain.

where
cos 1 sin 1 0
cos 2 sin 2 L1
cos 1 0 , T12 = sin 2
cos 2
0
T01 = sin 1
0
0 1
0
0
1
cos 3 sin 3 L2
1 0 L3
cos 3
0 ,
0 .
T23 = sin 3
T34 = 0 1
0
0
1
0 0
1
(4.4)
Observe that T34 is constant, and that each remaining Ti1,i depends only on
the joint variable i .
Alternatively, if we regard each joint axis as being the axis of a zero-pitch
screw motion, then observe that joint axis three can be thought of as applying
a screw motion to link three. Assuming joints 1 and 2 are both held constant
at zero, from the matrix exponential representation for screw motions covered
in the previous chapter, we can write
T04 = e[S3 ]3 M,
where
1 0
M = 0 1
0 0
L1 + L2 + L3
,
0
1
(4.5)
(4.6)
describes the position and orientation of frame {4} when all joints are set to
zero, and
0 1
0
0 (L1 + L2 ) .
[S3 ] = 1
(4.7)
0
0
0
4.1. Denavit-Hartenberg Representation
101
Similarly, joint axis two can be viewed as applying a screw motion to the link
two-link three pair; assuming joint 1 is held constant at zero, we can write
T04 = e[S2 ]2 e[S3 ]3 M,
where [S3 ] and M are as defined previously, and
0 1
0
0 L1 .
[S2 ] = 1
0
0
0
(4.8)
(4.9)
Finally, joint axis one can be viewed as applying a screw motion to the entire
three-link assembly; for arbitrary values of (1 , 2 , 3 ) we can therefore write
T04 = e[S1 ]1 e[S2 ]2 e[S3 ]3 M,
where [S2 ], [S3 ], and M are as defined previously, and
0 1 0
0 0 .
[S1 ] = 1
0
0 0
(4.10)
(4.11)
The forward kinematics can thus be expressed as a product of matrix exponentials, each corresponding to a screw motion. Note that this latter derivation of
the forward kinematics does not make use of any link reference frames.
In this chapter we shall consider the forward kinematics of general open
chains, taking the task space to be the position and orientation of the endeffector frame in the most general case. Two widely used forward kinematic
representations for open chains will be considered: the homogeneous transformation representation based on the Denavit-Hartenberg (D-H) parameters,
which corresponds to Equation (4.4), and the screw-theoretic formulation based
on the Product of Exponentials (PoE) formula, which corresponds to Equation (4.11). The advantage of the D-H representation is that it is a minimal
representation, in the sense of requiring the smallest number of parameters to
describe the robots kinematic structure. The PoE representation is not minimal, but as we show later, has so many other advantages over the D-H representation (e.g., no link frames are necessary) that with few exceptions it will be
our preferred choice of forward kinematics representation. We include the D-H
representation mostly for completeness.
4.1
Denavit-Hartenberg Representation
The basic idea underlying the Denavit-Hartenberg approach to forward kinematics is to attach reference frames to each link of the open chain, and to derive the
forward kinematics based on knowledge of the relative displacements between
adjacent link frames. Assume that a fixed reference frame has been established,
and that a reference frame (the end-effector frame) has been attached to some
102
Forward Kinematics
Figure 4.2: Illustration of Denavit-Hartenberg parameters.

point on the last link of the open chain. For a chain consisting of n one degree
of freedom joints, the links are numbered sequentially from 0 to n, in which
the ground link is labelled 0, and the end-effector frame is attached to some
point on link n. Reference frames attached to the links are also correspondingly
labelled from {0} (the fixed frame) to {n} (the end-effector frame). The joint
variable corresponding to the i-th joint is denoted i . The forward kinematics
of the n-link open chain can then be expressed as
T0n (1 , . . . , n ) = T01 (1 )T12 (2 ) Tn1,n (n ),
(4.12)
where Ti,i1 SE(3) denotes the relative displacement between link frames
{i 1} and {i}. Depending on how the link reference frames have been chosen,
for open chains each Ti1,i can be obtained in a straightforward fashion.
4.1.1
Assigning Link Frames
Rather than attaching reference frames to each link in some arbitrary fashion,
in the Denavit-Hartenberg convention a set of rules for assigning link frames
is observed. Figure 4.2 illustrates the frame assignment convention for two
adjacent revolute joints i 1 and i that are connected by link i 1.
The first rule is that the z-axis coincides with joint axis i, and zi1 coincides
with joint axis i 1. The direction of each link frame z-axis is determined via
the right-hand rule, i.e., such that positive rotations are counterclockwise about
the z-axis.
Once the z-axis direction has been assigned, the next rule determines the
origin of the link reference frame. First, find the line segment that orthogonally
intersects both joint axes zi1 and zi . For now let us assume that this line
103
segment is unique; the case where it is not unique (i.e., when the two joint
axes are parallel), or fails to exist (i.e., when the two joint axes intersect), is
addressed later. Connecting joint axes i 1 and i by a mutually perpendicular
line, the origin of frame {i 1} is then located at the point where this line
intersects joint axis i 1.
Determining the remaining x
- and y
-axes of each link reference frame is now
straightforward: the x
axis is chosen to be in the direction of the mutually
perpendicular line pointing from the i 1 axis to the i axis. The y
-axis is then
uniquely determined from the cross-product x
y
= z. Figure 4.2 depicts the
link frames i and i 1 chosen according to this convention.
Having assigned reference frames in this fashion for links i and i 1, we now
define four parameters that exactly specify Ti1,i :
The length of the mutually perpendicular line, denoted by the scalar ai1 ,
is called the link length of link i 1. Despite its name, this link length
does not necessarily correspond to the actual length of the physical link.
The link twist i1 is the angle from zi1 to zi , measured about x
i1 .
The link offset di is the distance from the intersection of x
i1 and zi to
the link i frame origin (the positive direction is defined to be along the zi
axis).
The joint angle i is the angle from x
i1 to x
i , measured about the
zi -axis in the right-hand sense.
These parameters constitute the Denavit-Hartenberg parameters. For an
open chain with n one degree-of-freedom joints, the 4n Denavit-Hartenberg
parameters are sufficient to completely describe the forward kinematics. In the
case of an open chain with all joints revolute, the link lengths ai1 , twists i1 ,
and offset parameters di are all constant, while the joint angle parameters i
act as the joint variables.
We now consider the case where the mutually perpendicular line is undefined
or fails to be unique, as well as when some of the joints are prismatic, and finally,
how to choose the ground and end-effector frames.
When Adjacent Revolute Joint Axes Intersect
If two adjacent revolute joint axes intersect each other, then the mutually perpendicular line between the joint axes fails to exist. In this case the link length
is set to zero, and we choose x
i1 to be perpendicular to the plane spanned
by zi1 and zi . There are two possibilities here, both of which are acceptable:
one leads to a positive value of the twist angle i1 , while the other leads to a
negative value.
When Adjacent Revolute Joint Axes are Parallel
The second special case occurs when two adjacent revolute joint axes are parallel. In this case there exist many possibilities for a mutually perpendicular
104
Forward Kinematics
Figure 4.3: Link frame assignment convention for prismatic joints. Joint i 1
is a revolute joint, while joint i is a prismatic joint.
line, all of which are valid (more precisely, a one-parameter family of mutual
perpendicular lines is said to exist). Again, it is important to detail precisely
how the link frames are assigned. A useful guide is to try to choose the mutually
perpendicular line that is the most physically intuitive, and simplifies as many
Denavit-Hartenberg parameters as possible (e.g., such that their values become
zero).
Prismatic Joints
For prismatic joints, the z-direction of the link reference frame is chosen to be
along the positive direction of translation. This convention is consistent with
that for revolute joints, in which the z-axis indicates the positive axis of rotation.
With this choice the link offset parameter di now becomes the joint variable (see
Figure 4.3). The procedure for choosing the link frame origin, as well as the
remaining x
and y
-axes, remains the same as for revolute joints.
Assigning the Ground and End-Effector Frames
Our frame assignment procedure described thus far does not specify how to
choose the ground and final link frames. Here as before, a useful guideline is to
choose initial and final frames that are the most physically intuitive, and simplify
as many Denavit-Hartenberg parameters as possible. This usually implies that
the ground frame is chosen to coincide with the link 1 frame in its zero (rest)
position; in the event that the joint is revolute, this choice forces a0 = 0 =
d1 = 0, while for a prismatic joint we have a0 = 0 = 1 = 0. The end-effector
frame is typically attached to some reference point on the end-effector, usually
at a location that makes the description of the task intuitive and natural, and
also simplifies as many of the Denavit-Hartenberg parameters as possible (e.g.,
their values become zero).
105
Figure 4.4: An example of three frames {a}, {b}, and {c}, in which the transformations Tab and Tac cannot be described by any set of Denavit-Hartenberg
parameters.
It is important to realize that arbitrary choices of the ground and endeffector frames may not always be possible, since there may not exist a valid set
of Denavit-Hartenberg parameters to describe the relative transformation; we
elaborate on this point below.
4.1.2
Why Four Parameters are Sufficient
In our earlier study of spatial displacements, we argued that a minimum of

six independent parameters were required to describe the relative displacement
between two frames in space: three for the orientation, and three for the position. Based on this result, it would seem that for an n-link arm, a total of
6n parameters would be required to completely describe the forward kinematics
(each Ti1,i in the above equation would require six parameters). Surprisingly,
in the Denavit-Hartenberg parameter representation only four parameters are
required for each transformation Ti1,i . Although this result may at first appear
to contradict our earlier results, this reduction in the number of parameters is
accomplished by the carefully stipulated rules for assigning link reference frames.
If the link reference frames had been assigned in arbitrary fashion, then more
parameters would have been required.
Consider, for example, the link frames shown in Figure 4.4. The transformation from frame {a} to frame {b} is a pure translation along the y
-axis
of frame {a}. If one were to try to express the transformation Tab in terms
of the Denavit-Hartenberg parameters (, a, d, ) as prescribed above, it should
become apparent that no such set of parameter values exist. Similarly, the transformation Tac also does not admit a description in terms of Denavit-Hartenberg
parameters, as only rotations about the x
- and z- axes are permissible. Under
our Denavit-Hartenberg convention, only rotations and translations along the
x
and z axes are allowed, and no combination of such motions can achieve the
transformations shown in Figure 4.4.
106
Forward Kinematics
Given that the Denavit-Hartenberg convention uses exactly four parameters

to describe the transformation between link frames, one might naturally wonder
if the number of parameters can be reduced even further, by an even more clever
set of link frame assignment rules. Denavit and Hartenberg show that this is
not possible, and that four is the minimum number of parameters [7].
We end this section with a reminder that there are alternative conventions
for assigning link frames. Whereas we chose the z-axis to coincide with the joint
axis, some authors choose the x
-axis, and reserve the z-axis to be the direction
of the mutually perpendicular line. To avoid ambiguities in the interpretation
of the Denavit-Hartenberg parameters, it is essential to include a concise description of the link frames together with the parameter values.
4.1.3
Manipulator Forward Kinematics
Once all the transformations Ti1,i between adjacent link frames are known
in terms of their Denavit-Hartenberg parameters, the forward kinematics is
obtained by sequentially multiplying these link transformations. Each link frame
transformation is of the form
Ti1,i
Rot(
x, i1 ) Trans(
x, ai1 ) Trans(z, di ) Rot(z, i )
cos i
sin i
0
ai1
sin i cos i1 cos i cos i1 sin i1 di sin i1
=
sin i sin i1 cos i sin i1
cos i1
di cos i1
0
0
0
1
=
where
Rot(
x, i1 )
Trans(
x, ai1 )
Trans(z, di )
Rot(z, i )
1
0
0
0 cos i1 sin i1
0 sin i1 cos i1
0
0
0
1 0 0 ai1
0 1 0
0
0 0 1
0
0 0 0
1
1 0 0 0
0 1 0 0
0 0 1 di
0 0 0 1
cos i1 sin i1 0
sin i1 cos i1 0
0
0
1
0
0
0
0
0
0
1
(4.13)
(4.14)
(4.15)
0
0
.
0
1
(4.16)
A useful way to visualize Ti,i1 is to transport frame {i 1} to frame {i} via

the following sequence of four transformations:
107
Figure 4.5: A 3R spatial open chain.

(i) Rotate frame {i 1} about its x
axis by an angle i1 .
(ii) Translate this new frame along its x
axis by a distance ai1 .
(iii) Translate this new frame along its z axis by a distance di .
(iv) Rotate this new frame about its z axis by an angle i .
Note that switching the order of the first and second steps will not change the
final form of Ti1,i . Similarly, the order of the third and fourth steps can also
be switched without affecting Ti1,i .
4.1.4
Examples
We now derive the Denavit-Hartenberg parameters for some common spatial

open chain structures.
Example: A 3R Spatial Open Chain
Consider the 3R spatial open chain of Figure 4.5, shown in its zero position
(i.e., with all its joint variables set to zero). The assigned link reference frames
are shown in the figure, and the corresponding Denavit-Hartenberg parameters
listed in the following table:
i
1
2
3
i1
0
90
90
ai1
0
L1
L2
di
0
0
0
i
1
2 90
3
108
Forward Kinematics
Figure 4.6: An RRRP spatial open chain.

Note that frames {1} and {2} are uniquely specified from our frame assignment convention, but that we have some latitude in choosing frames {0} and
{3}. Here we choose the ground frame {0} to coincide with frame {1} (resulting
in 0 = a0 = d1 = 0), and frame {3} such that x
3 = x
2 (resulting in no offset
to the joint angle 3 ).
Example: A Spatial RRRP Open Chain
The next example we consider is the four degree-of-freedom RRRP spatial open
chain of Figure 4.6, here shown in its zero position. The link frame assignments
are as shown, and the corresponding Denavit-Hartenberg parameters are listed
in the following table:
i
1
2
3
4
i1
0
90
0
90
ai1
0
0
L2
0
di
0
0
0
4
i
1
2
3 + 90
0
The four joint variables are (1 , 2 , 3 , 4 ), where 4 is the displacement of

the prismatic joint. As in the previous example, the ground frame {0} and final
link frame {4} have been chosen to make as many of the Denavit-Hartenberg
parameters zero.
Example: A Spatial 6R Open Chain
The final example we consider is a widely used six 6R robot arm (Figure 4.7).
This open chain has six rotational joints: the first three joints function as a
Cartesian positioning device, while the last three joints act as a ZYZ type
4.2. Product of Exponentials Formula
109
Figure 4.7: A 6R spatial open chain.

wrist. The link frames are shown in the figure, and the corresponding DenavitHartenberg parameters are listed in the following table:
i
1
2
3
4
5
6
4.2
i1
0
90
0
90
90
90
ai1
0
0
L1
0
0
0
di
0
0
0
L2
0
0
i
1
2
3 + 90
4 + 180
5 + 180
6
Product of Exponentials Formula
In this section we present an alternative representation of the forward kinematics, called the product of exponentials (PoE) formula, that is based on the
matrix exponential representation for screw motions introduced in the previous
chapter. We begin by recalling the Chasles-Mozzi theorem: Any displacement
of a rigid body can be expressed as a finite rotation about some fixed line in
space (the screw axis), followed by a finite translation parallel to the fixed line.
Since both the rotation and translation are taken about the same fixed line,
reversing their order still results in the same displacement.
To state things more precisely, assume a fixed frame has been chosen, and
a body-fixed frame attached to the rigid body. If the body is displaced from
some initial configuration M SE(3) to another configuration T SE(3), the
displacement can then be expressed as

[] v
T = e[S] M, [S] =
,
(4.17)
0 0
110
Forward Kinematics
Figure 4.8: Illustration of the PoE formula for an n-link spatial open chain.
where S = (, v) denotes the six-dimensional twist representation of the screw
motion and [S] its 4 4 matrix representation, and R is the joint displacement. Recall that for displacements that are not pure translations, R3 is a
unit vector in the direction of the screw axis and [] R33 its skew-symmetric
matrix representation, and v = q+h, where q R3 is a point on the screw
axis and h R is the screw pitch. If the displacement is a pure translation,
is zero, and v R3 is a unit vector in the direction of positive translation.
The two limiting cases for a general screw motion are a pure rotation (corresponding to zero pitch h = 0) and a pure translation (corresponding to infinite
pitch h = ). These two motions also happen to correspond to the two most
prevalent joint types found in robots, the revolute joint and prismatic joint. The
PoE formula takes advantage of this identification, by expressing the motions of
revolute and prismatic joints as screw motions. We now derive the PoE formula
for general spatial open chains.
4.2.1
First Formulation
The key concept behind the PoE formula is to regard each joint as applying a
screw motion to all the outward links. To illustrate, consider a general spatial
open chain like the one shown in Figure 4.8, consisting of n single dof joints
that are connected serially. Choose a fixed frame, and also an end-effector frame
attached to the last linkunlike the Denavit-Hartenberg convention, there are
no restrictions placed on the choice of fixed and end-effector frames. Place the
robot in its zero position by setting all joint values to zero, with the direction
of positive displacement (rotation for revolute joints, translation for prismatic
joints) for each joint specified.
111
Let M SE(3) denote the configuration of the end-effector frame when the
robot is in its zero position. Suppose only joint n is displaced to some joint
value n . The end-effector frame M then undergoes a screw motion of the form
T = e[Sn ]n M,
(4.18)
where T SE(3) is the newly displaced configuration of the end-effector frame,

and Sn = (n , vn ) is the twist vector describing the motion for joint n. For
example, if joint n is revolute (corresponding to a screw motion of zero pitch),
then n R3 is a unit vector in the positive direction of joint axis n, and
vn = n qn , with qn any arbitrary point on joint axis n. If joint n is
prismatic, then n = 0, and v R3 is a unit vector in the direction of positive
translation. We remind the reader that all vector quantities here are expressed
in terms of the fixed frame.
If we assume joint n 1 is also allowed to vary, then this has the effect of
applying a screw motion to link n 1 (and by extension to link n, since link n
is connected to link n 1 via joint n). The end-effector frame thus undergoes a
screw motion of the form

T = e[Sn1 ]n1 e[Sn ]n M .
(4.19)
Continuing with this reasoning and now allowing all the joints (1 , . . . , n ) to
vary, it follows that
T = e[S1 ]1 e[Sn1 ]n1 e[Sn ]n M.
(4.20)
This is the product of exponentials formula describing the forward kinematics of

an n-dof open chain. Specifically, we will call Equation (4.20) the space form
of the product of exponentials formula. We note the following features of the
product of exponentials formula:
No link reference frames need to be assigned. Only the fixed and endeffector frames need to be chosen (note that there are no restrictions on
how these frames are chosen), together with a definition of the robots zero
position, and the direction of positive displacement for each joint (rotation
in the case of a revolute joint, translation in the case of a prismatic joint).
Placing the robot in its zero position, if joint i is revolute, then i R3 is
a unit vector in the (positive) direction of the joint axis, and v = i qi ,
where qi R3 is an arbitrary point on joint axis i. If joint i is prismatic,
then i = 0 and vi R3 is a unit vector in the direction of (positive)
translation. Unlike the Denavit-Hartenberg parameters, where the joint
variable involves either i or di depending on whether the joint is revolute
or prismatic, in the product of exponentials formula the joint variable is
always labelled in a consistent fashion by i .
Compared to the Denavit-Hartenberg parameters, the fact that no link reference
frames are used, and that the fixed and end-effector frames can be assigned
without any restrictions, considerably simplies the forward kinematic analysis.
Further advantages will come to light when we examine the velocity kinematics
in the next chapter.
112
4.2.2
Forward Kinematics
Examples
We now derive the forward kinematics for some common spatial open structures
using the product of exponentials formula.
Example: 3R Spatial Open Chain
We return to the previous 3R open chain example of Figure 4.5. Choose the
fixed frame {0} and end-effector frame {3} as indicated in the figure, and express
all vectors and homogeneous transformations in terms of the fixed frame. The
forward kinematics will be of the form
T = e[S1 ]1 e[S2 ]2 e[S3 ]3 M,
where M SE(3) is the end-effector frame configuration when the robot is in
its zero position. By inspection M can be obtained as
0 0 1 L1
0 1 0
0
M =
1 0 0 L2 .
0 0 0
1
The twist S1 = (1 , v1 ) for joint axis 1 is then given by 1 = (0, 0, 1) and
v1 = (0, 0, 0) (the fixed frame origin (0,0,0) is a convenient choice for the point
q1 lying on joint axis 1). To determine the twist S2 for joint axis 2, observe that
joint axis 2 points in the
y0 axis direction, so that 2 = (0, 1, 0). Choose
q2 = (L1 , 0, 0), in which case v2 = 2 q2 = (0, 0, L1 ). Finally, to determine
the twist S3 for joint axis, note that 3 = (1, 0, 0). Choosing q3 = (0, 0, L2 ),
it follows that v3 = 3 q3 = (0, L2 , 0).
In summary, we have the following 4 4 matrix representations for the three
joint twist vectors S1 , S2 , and S3 :
0 1 0 0
1
0 0 0
[S1 ] =
0
0 0 0
0
0 0 1
0 0 1
0
0 0
0
0
[S2 ] =
1 0
0 L1
0 0
0
1
0 0
0 0
0 0 1 L2
.
[S3 ] =
0 1
0 0
0 0
0 1
It will be more convenient to list the twist vectors in the following tabular form:
113
z
y
y
x
{S}
{T}
Figure 4.9: POE forward kinematics for the 6R open chain.

i
1
2
3
i
(0, 0, 1)
(0, -1, 0)
(1, 0, 0)
vi
(0, 0, 0)
(0, 0, L1 )
(0, L2 , 0)

We now derive the forward kinematics of the 6R open chain of Figure 4.9. The
zero position and the direction of positive rotation for each joint axis are as
shown in the figure. A fixed frame {0} and end-effector frame {6} are also
assigned as shown. The end-effector frame M in the zero position is then
1 0 0 0
0 1 0 3L
(4.21)
M =
0 0 1 0
0 0 0 1
The screw axis for joint 1 is in the direction 1 = (0, 0, 1). The most convenient
choice for point q1 lying on joint axis 1 is the origin, so that v1 = (0, 0, 0). The
screw axis for joint 2 is in the y
direction of the fixed frame, so 2 = (0, 1, 0).
Choosing q2 = (0, 0, 0), we have v2 = (0, 0, 0). The screw axis for joint 3 is in the
direction 3 = (1, 0, 0). Choosing q3 = (0, 0, 0) leads to v3 (0, 0, 0). The screw
axis for joint 4 is in the direction 4 = (1, 0, 0). Choosing q4 = (0, L, 0) leads
to v4 = (0, 0, L). The screw axis for joint 5 is in the direction 5 = (1, 0, 0);
choosing q5 = (0, 2L, 0) leads to v5 = (0, 0, 2L). The screw axes for joint 6 is
in the direction 6 = (0, 1, 0); choose q6 = (0, 0, 0) leads to v6 = (0, 0, 0). In
summary, the twists Si = (i , vi ), i = 1, . . . 6 are as follows:
114
Forward Kinematics
Figure 4.10: The RRPRRR spatial open chain.

i
1
2
3
4
5
6
i
( 0, 0,
( 0, 1,
(-1, 0,
(-1, 0,
(-1, 0,
( 0, 1,
1)
0)
0)
0)
0)
0)
vi
(0, 0, 0)
(0, 0, 0)
(0, 0, 0)
(0, 0, L)
(0, 0, 2L)
(0, 0, 0)
Example: An RRPRRR Spatial Open Chain

In this example we consider the six degree-of-freedom RRPRRR spatial open
chain of Figure 4.10. The end-effector frame in the zero position is given by
1 0 0
0
0 1 0 L1 + L2
.
M =
0 0 1
0
0 0 0
1
The values of the screw twists Si = (i , vi ) are listed in the following table:
i
1
2
3
4
5
6
(
(
(
(
(
(
i
0, 0,
1, 0,
0, 0,
0, 1,
1, 0,
0, 1,
1)
0)
0)
0)
0)
0)
vi
(0, 0, 0)
(0, 0, 0)
(0, 1, 0)
(0, 0, 0)
(0, 0, L1 )
(0, 0, 0)
Note that the third joint is prismatic, so that 3 = 0 and v3 is a unit vector in
the direction of positive translation.
4.2.3
115
Relation with the Denavit-Hartenberg Representation
The product of exponentials formula can also be derived directly from the
Denavit-Hartenberg parameter-based representation of the forward kinematics.
As before, denote the relative displacement between adjacent link frames by
Ti1,i = Rot(
x, i1 ) Trans(
x, ai1 ) Trans(z, di ) Rot(z, i ).
If joint i is revolute, the first three matrices can be regarded as constant, and
i becomes the revolute joint variable. Define i = i , and
Mi = Rot(
x, i1 ) Trans(
x, ai1 ) Trans(z, di ),
and write Rot(z, i ) as the following matrix exponential:
0 1 0
1
0 0
Rot(z, i ) = e[Ai ]i , [Ai ] =
0
0 0
0
0 0
0
0
.
0
0
(4.22)
(4.23)
With the above definitions we can write Ti1,i = Mi e[Ai ]i .

If joint i is prismatic, then di becomes the joint variable, i is a constant
parameter, and the order of Trans(z, di ) and Rot(z, i ) in Ti1,i can be reversed
(recall that reversing translations and rotations taken along the same axis still
results in the same motion). In this case we can still write Ti1,i = Mi e[Ai ]i ,
where i = di and
Mi
Rot(
x, i1 )Trans(
x, ai1 )Rot(z, i )
0 0 0 0
0 0 0 0
=
0 0 0 1 .
0 0 0 0
[Ai ]
(4.24)
(4.25)
Based on the above, for an n-link open chain containing both revolute and
prismatic joints, the forward kinematics can be written
T0,n = M1 e[A1 ]1 M2 e[A2 ]2 Mn e[An ]n
(4.26)
where i denotes joint variable i, and [Ai ] is either of the form (4.23) or (4.25)
depending on whether joint i is revolute or prismatic.
1
We now make use of the matrix identity M eP M 1 = eM P M , which holds
for any nonsingular M Rnn and arbitrary P Rnn . The above can also be
1
rearranged as M eP = eM P M M . Beginning from the left of Equation (4.26),
if we repeatedly apply this identity, after n iterations we obtain the product of
exponentials formula as originally derived:
T0n
1 (M1 M2 )[A2 ](M1 M2 )1 2
eM1 [A1 ]M1
eM1 [A1 ]M1
[S1 ]1
(M1 M2 )e[A2 ]2 e[An ]n

e
[Sn ]n
M,
(M1 M2 M3 )e[A3 ]3 e[An ]n

(4.27)
116
Forward Kinematics
where
[Si ]
(M1 Mi1 )[Ai ](M1 Mi1 )1 , i = 1, . . . , n
(4.28)
M1 M2 Mn .
(4.29)
We now re-examine the physical meaning of the Si by recalling how a screw

twist transforms under a change of reference frames. If Sa represents the screw
twist for a given screw motion with respect to frame {a}, and Sb represents the
screw twist for the same physical screw motion but this time with respect to
frame {b}, then recall that Sa and Sb are related by
1
[Sb ] = Tba [Sa ]Tba
,
(4.30)
or using the adjoint notation AdTba ,

Sb = AdTba (Sa ).
(4.31)
Seen from the perspective of this transformation rule, Equation (4.29) suggests
that Ai is the screw twist for joint axis i as seen from link frame {i}, while Si
is the screw twist for joint axis i as seen from the fixed frame {0}.
4.2.4
Second Formulation
1
The matrix identity eM P M = M 1 eP M can also be expressed as M eM P M =

eP M . Beginning with the right-most term of the previously derived product of
exponentials formula, if we repeatedly apply this identity, after n iterations we
obtain
T
e[S1 ]1 e[Sn ]n M
=
=
e[S1 ]1 M eM [Sn ]M n
1
1
e[S1 ]1 M eM [Sn1 ]M n1 eM [Sn ]M n
M e[B1 ]1 e[B2 ]2 e[Bn ]n ,
(4.32)
where each [Bi ] = M 1 [Si ]M , i = 1, . . . , n. This is an alternative form of the

product of exponentials formula. Note that M 1 is the relative displacement of
the fixed frame as seen from the end-effector frame; since each
Bi = AdM 1 (Si ),
(4.33)
Bi is the screw twist for joint i expressed in terms of the end-effector frame at
the zero position. We will call Equation (4.32) the body form of the product
of exponentials formula.
We now express the forward kinematics for the same 6R open chain of Figure 4.9
in the second form
T = M e[B1 ]1 e[B2 ]2 e[B6 ]6 .
4.3. Summary
117
Assume the same fixed and end-effector frames and zero position as the previous
example. M is still the same as Equation (4.21), obtained as the end-effector
frame as seen from the fixed frame with the chain in its zero position. The
screw twists for each joint axes, however, are now expressed with respect to the
end-effector frame in its zero position:
i
1
2
3
4
5
6
4.3
i
( 0, 0,
( 0, 1,
(-1, 0,
(-1, 0,
(-1, 0,
( 0, 1,
1)
0)
0)
0)
0)
0)
vi
(3L, 0, 0)
(0, 0, 0)
(0, 0, 3L)
(0, 0, 2L)
(0, 0, L)
(0, 0, 0)
Summary
Given an open chain with a reference frame attached to some point on

its last linkthis frame is denoted the end-effector framethe forward
kinematics is the mapping from the joint values to the position and orientation of the end-effector frame.
In the Denavit-Hartenberg representation, the forward kinematics of
an open chain is described in terms of relative displacements between
reference frames attached to each link. If the link frames are sequentially
labelled {0}, . . . , {n}, where {0} is the fixed frame and {n} is the endeffector frame, then the forward kinematics is expressed as
T0n = T01 (1 ) Tn1,n (n )
where i denotes the joint i variable.
The Denavit-Hartenberg convention requires that reference frames assigned to each link obey a strict convention. Following this convention,
the link frame transformation Ti1,i between link frames {i 1} and {i}
can be parametrized using a minimum of four parameters (the DenavitHartenberg parameters): the link twist i1 , link length ai1 , link
offset di , and joint angle i .
The forward kinematics can also be expressed as the following product
of exponentials (the space form):
T0n = e[S1 ]1 e[Sn ]n M,
where Si = (i , vi ) denotes the screw twist associated with joint i expressed in fixed frame coordinates, i is the joint i variable, and M
SE(3) denotes the position and orientation of the end-effector frame when
the robot is in its zero position. A choice of fixed frame and end-effector
118
Forward Kinematics
frame, together with a specification of the robots zero position and direction of positive rotation or translation of each of the robots joints, then
completely specifies the product of exponentials formula.
The product of exponentials formula can also be written in the equivalent
body form:
T0n = M e[B1 ]1 e[Bn ]n ,
where [B]i = M 1 [Si ]M , i = 1, . . . , n. Bi = (i , vi ) is the screw twist
corresponding to joint axis i, expressed in terms of the end-effector frame
with the robot in its zero position.

In the original paper by Denavit and Hartenberg [7], the four parameters are
defined without appealing to any reference frames, e.g., the link length is defined
to be length of the mutually perpendicular line between the joint axes, the
twist angle is obtained by first projecting the two adjacent axes to the plane
perpendicular to the common normal and measuring the angle between the axes,
etc. The convention of choosing the z-axis to be along the joint axis and the x
axis to be along the common normal is not standard; other authors, for example,
choose the joint axis to be in the x
-direction. The product of exponentials
formula is first presented by Brockett in [4]. Computational aspects of the
product of exponentials formula are also discussed in [27].
4.4. Exercises
4.4
119
Exercises
1. (a) For each given T SE(3)(3) below, find, if it exists, (, a, d, ) that

satisfies
T = Rot(
x, )Trans(
x, a)Trans(
z , d)Rot(
z , ).
0 1 1 3
1 0 0 0
(a) T =
0 1 0 1 .
0 0 0 1
cos
sin
0
1
sin cos 0
0
.
(b) T =
0
0
1 2
0
0
0
1
0 1 0 1
0 0 1 0
.
(c) T =
1 0
0
2
0 0
0
1
2. Let T1 , T2 SE(3), and suppose T1 = e[A1 ] , T2 = e[A2 ] for some A1 =
(1 , v1 ), A2 = (2 , v2 ). Under what conditions on A1 and A2 does T1 T2 = T2 T1 ?
Figure 4.11: A RRRP robot for performing pick-and-place operations.
3. The RRRP spatial open chain of Figure 4.11 is shown in its zero position.
(a) Choose appropriate link reference frames, and derive the corresponding
120
Forward Kinematics
Denvait-Hartenberg parameters (use the given fixed frame.)

(b) Find T04 .
Figure 4.12: A RRP P RR open chain robot.
4. The RRP P RR spatial open chain of Figure 4.12 is shown in its zero position.
Using the fixed reference frame given in the figure, choose appropriate link
reference frames and derive the Denavit-Hartenberg parameters.
5. The robot with a screw joint in Figure 4.13 is shown in its zero position.
Using the given fixed frame, choose appropriate link reference frames and derive
the Denavit-Hartenberg parameters.
6. The spatial RP RRR open chain of Figure 4.14 is shown in its zero position.
Using the given fixed reference frame, choose appropriate link reference frames
and derive the Denavit-Hartenberg parameters.
7. The RRP RRR spatial open chain of Figure 4.15 is shown in its zero position
(all joints lie on the same plane).
(a) Using the given fixed frame, choose appropriate link reference frames and
derive the Denavit-Hartenberg parameters.
(b) Set 5 = and all the other joint variables to zero, and find T60 .
8. The P RRRRR spatial open chain of Figure 4.16 is shown in its home
position. Using the given fixed and end-effector frames, and the direction of
positive rotation or translation for each joint, derive its forward kinematics in
the following product of exponentials form:
e[S1 ]1 e[S2 ]2 e[S6 ]6 M
4.4. Exercises
121
Figure 4.13: A robot with a screw joint.
Figure 4.14: An RP RRR spatial open chain.
122
Forward Kinematics
Figure 4.15: An RRP RRR spatial open chain.
Figure 4.16: A P RRRRR spatial open chain.
9. For the same open chain of Figure 4.16, express the forward kinematics in
the following product of exponentials form:
M e[B ]1 e[B2 ]2 e[B6 ]6 .
10. The spatial RRP P RR open chain of Figure 4.17 is shown in its zero
position.
(a) Derive its forward kinematics in the form e[S1 ]1 e[S2 ]2 e[S6 ]6 M .
(b) Derive its forward kinematics in the form M e[B ]1 e[B2 ]2 e[B6 ]6 .
11. The spatial RRRP RR open chain of Figure 4.18 is shown in its zero position.
(a) Choose appropriate link frames and derive the corresponding Denavit-Hartenberg
parameters.
(b) Express its forward kinematics in the form e[S1 ]1 e[S2 ]2 e[S6 ]6 M .
4.4. Exercises
123
Figure 4.17: A spatial RRP P RR open chain with prescribed fixed and endeffector frames.
Figure 4.18: A spatial RRP RRR open chain with prescribed fixed and endeffector frames.
12. The spatial RRRRP R open chain of Figure 4.19 is shown in its zero position, with fixed and end-effector frames chosen as shown.
(a) Choose appropriate link frames and derive the corresponding Denavit-Hartenberg
parameters.
(b) Express its forward kinematics in the form e[S1 ]1 e[S2 ]2 e[S6 ]6 M .
124
Forward Kinematics
Figure 4.19: A spatial RRRRP R open chain.
Chapter 5
Velocity Kinematics and

Statics
In the previous chapter we saw how to calculate the robot end-effector frames
position and orientation for a given set of joint positions. In this chapter we
examine the related problem of calculating the end-effector frames angular and
linear velocity from a given set of joint positions and velocities. To illustrate
the main concepts covered in this chapter, consider once again the 2R planar
open chain with forward kinematics given by
x1
cos 1 + cos(1 + 2 )
x2
sin 1 + sin(1 + 2 ).
Differentiating both sides with respect to time t results in

x 1
x 2
1 sin 1 1 sin(1 + 2 ) 2 sin(1 + 2 )

1 cos 1 + 1 cos(1 + 2 ) + 2 cos(1 + 2 ),
which can be rearranged into a linear equation of the form x = J():

x 1
sin 1 sin(1 + 2 ) sin(1 + 2 )
1
=
.
x 2
cos 1 + cos(1 + 2 )
cos(1 + 2 )
2
(5.1)
The 2 2 matrix J() is referred to as the Jacobian. Note that J() depends
on , and that x is linearly related to via J().
Denote the two columns of J() by J1 () and J2 (), and the tip velocity x
by vtip . Using this notation Equation (5.1) can be written
vtip = J1 ()1 + J2 ()2 .
As long as J1 () and J2 () are not collinear, it is possible to generate a tip
velocity vtip in any arbitrary direction in the x1 -x2 plane simply by choosing
125
126
Velocity Kinematics and Statics
Figure 5.1: Obtaining the magnitude and direction of the maximum tip force
for the planar 2R open chain.
appropriate joint velocities 1 and 2 . Since J1 () and J2 () depend on the
joint values 1 and 2 , one may ask if there are any configurations at which
J1 () and J2 () become collinear. For our example the answer is yes: if 2
is 0 or 180 the two links completely overlap each other in this casethen
regardless of the value of 1 , J1 () and J2 () will be collinear, and the Jacobian
J() becomes a singular matrix. Such configurations are called singularities;
they are characterized by the robot tip being unable to generate velocities in
certain directions.
The Jacobian also plays a central role in static analysis. Suppose some
external force is applied to the robot tip; what are the joint torques required
to resist this external tip force and maintain static equilibrium (that is, to
keep all links of the robot stationary)? This question can be answered via a
conservation of power argument. For mechanical systems, power is the time
derivative of work, and can be obtained as the product of the force on an object
and the objects velocity, or as the product of a torque on a rotating shaft and
the shafts angular velocity. Assuming there is no power loss due to friction
or other effects, under static equilibrium the power generated at the robots
tip should equal the power generated at the joints. Denoting the external tip
force vector by ftip and the joint torque vector by , conservation of power then
requires that
T
ftip
vtip = T ,
Since vtip = J(),
the equality
for all arbitrary joint velocities .
T
ftip
J() = T
This can only be true if

must hold for all possible .
= J T ()ftip .
(5.2)
The joint torque needed to resist the external tip force ftip while maintaining
static equilibrium is calculated from the above equation.
127
Figure 5.2: Obtaining the magnitude and direction of the maximum tip velocity
vector for the planar 2R open chain.
For our two-link planar chain example J() is a square matrix dependent on
. If the configuration is not a singularity, then both J() and its tranpose
are invertible, and Equation (5.2) can be inverted as follows:
ftip = J T ().
(5.3)
Using the above equation one can now determine, under the same static equilibrium assumption, what input torques are needed to generate a desired tip force,
e.g., to determine the joint torques needed for the robot tip to push against
a wall with a specified normal force. Robot actuators typically have limits on
the maximum torques that can be generated. For a given posture one can
ask what is the direction and magnitude of the maximum force that can be
generated at the tip. For example, if joint torques 1 and 2 are limited to the
range
1 Nm 1 1 Nm
1 Nm 2 1 Nm,
then using Equation (5.3) we can generate the set of all possible tip forces as
shown in Figure 5.1, and accordingly determine the magnitude and direction of
the maximum tip force.
and acSince the Jacobian J() maps to the tip velocity vtip = J(),
tuators may also have limits on the maximum rotational speedthese can be
represented as bounds on the

magnitude and direction of the maximum tip
velocities can also be obtained as shown in Figure 5.2. Instead of maximum tip
velocities, however, one could alternatively seek a quantitative characterization
of a robots dexterity, i.e., its ability to move the tip in arbitrary directions. For
example, since the tip loses the ability to move in certain directions at singularities, it is useful to place the robot in a configuration as far away as possible from
a singularity. One useful geometric characterization can be obtained by mapping
= 1, via
the set of all joint velocities of unit magnitude, i.e., all satisfying kk
The
the Jacobian, and plotting the corresponding tip velocities vtip = J().
128
Figure 5.3: Manipulability ellipsoids for two different postures of the 2R planar
open chain.
results are shown in Figure 5.3 for two different postures of the robot arm. For
both postures the unit circle in the 1 -2 plane is mapped to an ellipsoid in
the space of tip velocities; this ellipsoid is referred to as the manipulability
ellipsoid. At a singularity the ellipsoid collapses to a line. Similarly, a force
ellipsoid can be drawn by mapping a unit circle in the 1 -2 plane to an ellipsoid in the f1 -f2 tip force plane via the Jacobian transpose inverse J T () (see
Figure 5.4).
Using the manipulability ellipsoid, one can quantify in a number of ways
how close a given posture is to a singularity. For example, one way is to compare the lengths of the major and minor principal axes of the manipulability
ellipsoid, respectively denoted max and min . The closer the ellipsoid is to a
circlemeaning the ratio max /min is close to 1 (note that for nonsingular
configurations this ratio cannot be smaller than 1)the more easily the tip can
move in arbitrary directions, and thus the more removed it is from a singularity. The ratio max /min is known as the condition number, and offers a
quantitative measure of how close a robot configuration is to a singularity.
In this chapter we present methods for deriving various forms of the Jacobian
for general open chains, and discuss both analytical and geometric characterizations of the Jacobian. We also examine how the Jacobian can be used for
velocity and static analysis, including the identification of kinematic singularities and determining the manipulability and force ellipsoids. Later chapters on
5.1. Manipulator Jacobian
129
Figure 5.4: Force ellipsoids for two different postures of the 2R planar open
chain.
inverse kinematics, motion planning, dynamics and control make extensive use
of the Jacobian and related notions introduced in this chapter.
5.1
Manipulator Jacobian
In the 2R planar open chain example, we saw that for any joint configuration ,
the tip velocity vector vtip and joint velocity vector are linearly related via the
The tip velocity vtip depends on the
Jacobian matrix J(), i.e., vtip = J().
coordinates of interest for the tip, which in turns determines the specific form
of the Jacobian. For example, in the most general case vtip can be taken to
be the six-dimensional spatial velocity of the end-effector frame, while for pure
orienting devices like a wrist vtip is usually taken to be the angular velocity
of the end-effector frame. Other choices for vtip lead to different formulations
for the Jacobian, and we examine some of these Jacobian formulations below.
We begin with the general case where vtip is taken to be the six-dimensional
end-effector spatial velocity expressed in the fixed frame.
130
5.1.1
Space Jacobian
In this section we derive the relationship between an open chains joint velocity
vector and the end-effectors spatial velocity Vs . We first recall a few basic
properties from linear algebra and linear differential equations: (i) if A, B
Rnn are both invertible, then (AB)1 = B 1 A1 ; (ii) if A Rnn is constant
d A
(iii) (eA )1 =
e = AeA = eA A;
and (t) is a scalar function of t, then dt
A
e
.
Consider an n-link open chain whose forward kinematics is expressed in the
following product of exponentials form:
T (1 , . . . , n ) = e[S1 ]1 e[S2 ]2 e[Sn ]n M.
(5.4)
The spatial velocity of the end-effector frame with respect to the fixed frame,
Vs , is given by [Vs ] = T T 1 , where
T
=
=
d [S1 ]1
d
e
) e[Sn ]n M + e[S1 ]1 ( e[S2 ]2 ) e[Sn ]n M + . . .
dt
dt
[S1 ]1 e[S1 ]1 e[Sn ]n M + e[S1 ]1 [S2 ]2 e[S2 ]2 e[Sn ]n M + . . .
Also,
T 1 = M 1 e[Sn ]n e[S1 ]1 .
Calculating T T 1 ,
[Vs ] = [S1 ]1 + e[S1 ]1 [S2 ]e[S1 ]1 2 + e[S1 ]1 e[S2 ]2 [S3 ]e[S2 ]2 e[S1 ]1 3 + . . . .
The above can also be expressed in vector form by means of the adjoint mapping:
Vs = S1 1 + Ade[S1 ]1 (S2 ) 2 + Ade[S1 ]1 e[S2 ]2 (S3 ) 3 + . . .
|{z}
|
{z
}
{z
}
|
Vs1
Vs2
(5.5)
Vs3
Observe that Vs is a sum of n spatial velocities of the form

Vs = Vs1 ()1 + . . . + Vsn ()n ,
(5.6)
where each Vsi () = (si (), vsi ()) depends explictly on the joint values Rn
for i = 2, . . . , n. In matrix form,
1

Vs1 () Vs2 () Vsn () ...

Vs =
(5.7)
n
= Js ().
The matrix Js () is said to be the Jacobian in fixed (space) frame coordinates,
or more simply the space Jacobian.
131
Figure 5.5: Physical interpretation of the screw adjoint transformation AdTba :

(a) describing the same screw motion in terms of two different reference frames
{a} and {b}; (b) An initial screw axis displaced by a transformation Tba .
Definition 5.1. Let the forward kinematics of an n-link open chain be expressed
in the following product of exponentials form:
T = e[S1 ]1 e[Sn ]n M.
(5.8)
The space Jacobian Js () R6n relates the joint rate vector Rn to the
The i-th column of Js () is
end-effector spatial velocity Vs via Vs = Js ().
Vsi () = Ade[S1 ]1 e[Si1 ]i1 (Si ),
(5.9)
for i = 2, . . . , n, with the first column Vs1 () = S1 .

To understand the physical meaning behind the columns of Js (), recall
from Chapter 3 that if Sa = (a , va ) is the vector describing a screw motion in
frame {a} coordinates, and Sb = (b , vb ) is a vector describing the same screw
motion in frame {b} coordinates, then Sb and Sa are related by Sb = AdTba (Sa )
(see Figure 5.5(a)). Another physical interpretation of this transformation is
from the perspective of reference frame {a} only. Referring to Figure 5.5(b),
suppose the vector Sa describes the initial screw axis with respect to frame {a},
and the vector Sb describes the screw axis after it has undergone a rigid body
displacement Tba . It follows that
b = Rba a .
(5.10)
The point q on the screw axis is now displaced from its initial location qa to
qb = Tba qa = Rba qa + pba .
(5.11)
Then from the definition vb = b qb + hb b , where hb = ha (the screw pitch

is a scalar quantity, and hence independent of the choice of reference frames),
132
Figure 5.6: Space Jacobian for a spatial RRRP chain.

we have
vb
=
=
=
=
b qb + hb b
Rba a (Rba qa + pba ) + hb Rba
a
T
Rba ([a ]qa + ha a ) Rba [a ]Rba
pba
Rba va + [pba ]Rba a ,
where in the last two lines we have again made use of the matrix identity
R[]RT = [R] for R SO(3) and R3 . Equations (5.10) and (5.12)
can be combined in the form

b
Rba
0
a
=
,
(5.12)
vb
[pba ] Rba Rba
va
which is precisely Sb = AdTba (Sa ).
Returning now to the equation for the space Jacobian (5.5), observe that
the i-th term of the right-hand side of (5.5) is of the form AdTi1 (Si ), where
Ti1 = e[S1 ]1 e[Si1 ]i1 ; recall here that Si is the screw vector describing
the i-th joint axis in terms of the fixed frame with the robot in its zero position.
AdTi1 (Si ) can therefore be viewed as the screw vector describing the i-th joint
axis after it undergoes the rigid body displacement Ti1 . But physically this is
the same as moving the first i 1 joints from their zero position to the current
values 1 , . . . , i1 . Therefore, the i-th column Vsi () of Js () is simply the
screw vector describing joint axis i, expressed in fixed frame coordinates as a
function of the joint variables 1 , . . . , i1 .
In summary, the procedure for determining the columns of Js () is similar to
that for deriving the Si in the product of exponentials formula e[S1 ]1 e[Sn ]n M :
each column Vsi () is the screw vector describing joint axis i, expressed in fixed
frame coordinates, but for arbitrary rather than = 0.
Example: Space Jacobian for a Spatial RRRP Chain
We now illustrate the procedure for finding the space Jacobian for the spatial
RRRP chain of Figure 5.6. Denote the i-th column of Js () by Vi = (i , vi ).

1
133
4
3
q1
qw
L2
L1
Figure 5.7: Space Jacobian for the spatial RRP RRR chain.
-direction: 1 = (0, 0, 1). Picking
Observe that 1 is constant and in the z
q1 to be the origin, v1 = (0, 0, 0).
-direction, so 2 = (0, 0, 1). Pick q2 to be the
2 is also constant in the z
point (L1 c1 , L1 s1 , 0), where c1 = cos 1 , s1 = sin 1 . Then v2 = 2 q2 =
(L1 s1 , L1 c1 , 0).
-direction regardless of the
The direction of 3 is always fixed in the z
values of 1 and 2 , so 3 = (0, 0, 1). Picking q3 = (L1 c1 + L2 c12 , L1 s1 +
L2 s12 , 0), where c12 = cos(1 + 2 ), s12 = sin(1 + 2 ), it follows that
v3 = (L1 s1 + L2 s12 , L1 c1 L2 c12 , 0).
Since the final joint is prismatic, 4 = (0, 0, 0), and the joint axis direction
is given by v4 = (0, 0, 1).
The space Jacobian is therefore
0
0
0
0
1
1
Js () =
0 L1 s1
0 L1 c1
0
0
0
0
1
L1 s1 + L2 s12
L1 c1 L2 c12
0
0
0
0
0
0
1
Example: Space Jacobian for Spatial RRPRRR Chain

We now derive the space Jacobian for the spatial RRP RRR chain of Figure 5.7.
The base frame is chosen as shown in the figure.
The first joint axis is in the direction 1 = (0, 0, 1). Picking q1 = (0, 0, L1 ),
we get v1 = 1 q1 = (0, 0, 0).
The second joint axis is in the direction 2 = (c1 , s1 , 0). Picking q2 =
(0, 0, L1 ), we get v2 = 2 q2 = (L1 s1 , L1 c1 , 0).
134

The third joint is prismatic, so 3 = (0, 0, 0). The direction of the prismatic joint axis is given by
0
s1 c2
v3 = Rot(
z, 1 )Rot(
x, 2 ) 1 = c1 c2 .
0
s2
Now consider the wrist portion of the chain. The wrist center is located
at the point
0
0
(L2 + 3 )s1 c2
qw = 0 +Rot(
z, 1 )Rot(
x, 2 ) L1 + 3 = (L2 + 3 )c1 c2 .
L1
0
L1 (L2 + 3 )s2
Observe that the directions of the wrist axes depend on 1 , 2 , and the
preceding wrist axes. These are
0
s1 s2
4 = Rot(
z, 1 )Rot(
x, 2 ) 0 = c1 s2
1
c2
1
c1 c4 + s1 c2 s4
5 = Rot(
z, 1 )Rot(
x, 2 )Rot(
z, 4 ) 0 = s1 c4 c1 c2 s4
0
s2 s4
0
6 = Rot(
z, 1 )Rot(
x, 2 )Rot(
z, 4 )Rot(
x, 5 ) 1
0
c5 (s1 c2 c4 + c1 s4 ) + s1 s2 s5
= c5 (c1 c2 c4 s1 s4 ) c1 s2 s5 .
s2 c4 c5 c2 s5
The space Jacobian can now be computed and written in matrix form as follows:

1
2
0
4
5
6
Js () =
.
0 2 q2 v3 4 qw 5 qw 6 qw
Note that we were able to obtain the entire Jacobian directly, without having
to explicitly differentiate the forward kinematic map.
5.1.2
Body Jacobian
In the previous section we derived the relationship between the joint rates and
[Vs ] = T T 1 , the end-effectors spatial velocity expressed in fixed frame coordinates. Here we derive the relationship between the joint rates and [Vb ] = T 1 T ,
the end-effector spatial velocity in end-effector frame coordinates. For this purpose it will be more convenient to express the forward kinematics in the alternate
product of exponentials form:
T (q) = M e[B1 ]1 e[B2 ]2 e[Bn ]n .
(5.13)
135
Computing T ,
T
d [Bn ]n
d
) + M e[B1 ]1 ( e[Bn1 ]n1 )e[Bn ]n + . . .
e
dt
dt
= M e[B1 ]1 e[Bn ]n [Bn ]n + M e[B1 ]1 e[Bn1 ]n1 [Bn1 ]e[Bn ]n n1 + . . .
+M e[B1 ]1 [B1 ]e[B2 ]2 e[Bn ]n 1 .
M e[B1 ]1 e[Bn1 ]n1 (
Also,
T 1 = e[Bn ]n e[B1 ]1 M 1 .
Evaluating T 1 T ,
[Vb ]
[Bn ]n + e[Bn ]n [Bn1 ]e[Bn ]n n1 + . . .

+e[Bn ]n e[B2 ]2 [B1 ]e[B2 ]2 e[Bn ]n 1 ,
or in vector form,
Vb = Bn n + Ade[Bn ]n (Bn1 ) n1 + . . . + Ade[Bn ]n e[B2 ]2 (B1 ) 1 . (5.14)
|{z}
|
{z
}
|
{z
}
Vbn
Vb,n1
Vb1
Vb can therefore be expressed as a sum of n spatial velocities, i.e.,

Vb = Vb1 ()1 + . . . + Vbn ()n ,
(5.15)
where each Vbi () = (bi (), vbi ()) depends explictly on the joint values for
i = 1, . . . , n 1. In matrix form,
1

Vb1 () Vb2 () Vbn () ...

Vb =
(5.16)
n
= Jb ().
The matrix Jb (q) is the Jacobian in the end-effector (or body) frame coordinates,
or more simply the body Jacobian.
Definition 5.2. Let the forward kinematics of an n-link open chain be expressed
in the following product of exponentials form:
T = M e[B1 ]1 e[Bn ]n .
(5.17)
The body Jacobian Jb () R6n relates the joint rate vector Rn to the
end-effector spatial velocity Vb = (b , vb ) via
Vb = Jb ().
(5.18)
The i-th column of Jb () is given by

Vb,i () = Ade[Bn ]n e[Bi+1 ]i+1 (Bi ),
for i = n, n 1, . . . , 2, with Vbn () = Bn .
(5.19)
136
Analogous to the columns of the space Jacobian, a similar physical interpretation can be given to the columns of Jb (): each column Vbi () = (bi (), vbi ())
of Jb () is the screw vector for joint axis i, expressed in coordinates of the endeffector frame rather than the fixed frame. The procedure for determining the
columns of Jb () is similar to the procedure for deriving the forward kinematics in the product of exponentials form M e[B1 ]1 e[Bn ]n , the only difference
being that each of the joint screws are derived for arbitrary rather than = 0.
5.1.3
Relationship between the Space and Body Jacobian
Denoting the fixed frame by {s} and the end-effector frame by {b}, the forward
kinematics can be written Tsb (). The spatial velocity of the end-effector frame
can be written in terms of the fixed and end-effector frame coordinates as
[Vs ]
[Vb ]
1
Tsb Tsb
T 1 Tsb ,
sb
with Vs and Vb related by Vs = AdTsb (Vb ) and Vb = AdTbs (Vs ). Vs and Vb are
also related to their respective Jacobians via
= Js ()
= Jb ().
Vs
Vb
(5.20)
(5.21)
Equation (5.20) can therefore be written
AdTsb (Vb ) = Js ().
(5.22)
Applying AdTbs to both sides of (??), and using the general property AdX
AdY = AdXY of the adjoint map, we obtain

AdTbs (AdTsb (Vb )) = AdTbs Tsb (Vb ) = Vb = AdTbs Js (q) .
it follows that Js () and Jb () are
Since we also have Vb = Jb () for all ,
related by
Jb ()
AdTbs (Js ())
(5.23)
[AdTbs ]Js ()
(5.24)
where [AdTsb ] denotes the 6 6 matrix representation of the linear transformation AdTsb (recall that if T SE(3) is of the form

R p
T =
,
0 1
then

[AdT ] =
R
[p] R
0
R

).
137
The space Jacobian can in turn be obtained from the body Jacobian via
Js () = AdTsb (Jb ()) = [AdTsb ]Jb ().
(5.25)
One of the important implications of (5.25) is that Jb () and Js () always have

the same rank; this is shown explicitly in the later section on singularity analysis.
5.1.4
Alternative Notions of the Jacobian
The space and body Jacobians derived above are matrices that relate joint rates
to the linear and angular velocities of the end-effector. There exist alternative
notions of the Jacobian that are based on an explicit choice of coordinates for the
end-effector orientation, e.g., ZYZ Euler angles (1 , 2 , 3 ); the Jacobian then
relates the joint rates not to the angular velocity, but to the time derivatives of
the coordinates ( 1 , 2 , 3 ). Such an alternative notion of Jacobian is sometimes
referred to as an analytic Jacobian.
Example: Analytic Jacobian for ZYZ Euler Angles
Consider an n-revolute joint open chain with body Jacobian
Vb = Jb ()
The angular and linear velocity components of Vb = (b , vb ) can be written
separately as
b
vb
J ()
Jv ().
Suppose we seek the relation between the joint rates and the ZYZ Euler angle
rates ( 1 , 2 , 3 ) rather than the angular velocity s . From the ZYZ Euler
angle formula,
R()
Rot(
z, 1 ) Rot(
y, 2 ) Rot(
z, 3 )
= e[z]1 e[y]2 e[z]3 ,

where z = (0, 0, 1) and y = (0, 1, 0). The angular velocity [b ] = RT R can then
be obtained in vector form as
b
or
= z 3 + e[z]3 y 2 + e[z]3 e[y]2 z 1
2 sin 3 + 1 sin 2 cos 3

= 2 cos 3 1 sin 2 sin 3 ,
3 + 1 cos 2
sin 2 cos 3
s = sin 2 sin 3
cos 2
sin 3
cos 3
0
0
1
0 2 .
1
3
(5.26)
138
The matrix
sin 2 cos 3
A() = sin 2 sin 3
cos 2
sin 3
cos 3
0
0
0
1
is invertible for 2 6= 0, . Therefore when A1 () exists we can write
= A1 ()b = A1 ()J ().

The corresponding analytic Jacobian Ja () is therefore obtained from Jb () as
1

A () 0
Ja () =
Jb ().
(5.27)
0
I
The corresponding analytic Jacobian can be obtained from the space Jacobian
Js () in a similar fashion.
Example: Exponential Coordinates
As a second example, suppose we seek the analytic Jacobian corresponding
to the exponential coordinate representation for rotations. Let r R3 be the
exponential coordinate parametrization for a rotation matrix R; if r is expressed
as r = R3 , where R3 is a unit vector and [0, ], then recall that
R(r) = I + sin [] + (1 cos )[]2 .
can be obtained in vector
The angular velocity in the body frame, [b ] = RT R,
form as
b
= A(r)r
krk sin krk 2
1 cos krk
[r] +
[r] .
A(r) = I
krk2
krk3
The derivation of this formula is explored in the exercises. Provided the matrix
A(r) is invertible, r can be obtained from b as
r = A1 (r)b = A1 (r)J ()
and the corresponding analytic Jacobian is obtained as in Equation (5.27). The
analytic Jacobian can be obtained from the space Jacobian Js () in a similar
fashion.
5.2
Statics of Open Chains
Recall from the earlier chapter on rigid body motions that the moment m generated by a force f acting on a rigid body, expressed with respect to some reference
point p in physical space, is defined to be the cross product
m = r f,
(5.28)
5.2. Statics of Open Chains
139
where r is the vector from p to the point on the rigid body at which the force is
applied. For a rigid body subject to a collection of forces and moments, if both
the sum of the forces and sum of the moments are zero, then the body will be
stationary, and is said to be in static equilibrium. When summing moments
it is important that each of the moments be expressed with respect to the same
reference point p.
A rigid body is in static equilibrium if it is motionless and the net forces and
moments applied to the body are all zero. Similarly, a robot arm is said to be
in static equilibrium if all of its links are in static equilibrium. In this section
we shall examine, for robot arms that are in static equilibrium, the relationship
between any external forces and moments applied at the end-effector, and the
forces and torques experienced at each of the joints. Such a situation arises,
for example, when a six degree-of-freedom arm is pushing against an immobile
wall.
To state our main result, consider an n-link robot arm assumed to be in
static equilibrium, and suppose some external force and moment are applied to
the end-effector. For now all vector quantities are defined in terms of the endeffector (body) frame: over some time interval [t0 , t1 ], let vb and b respectively
be the linear and angular velocity of the end-effector frame, and Vb = (b , vb )
be the spatial velocity of the end-effector. Further let fb and mb respectively
be the external force and moment applied to the end-effector (the moment is
defined with respect to the end-effector frame origin), and Fb = (mb , fb ) be the
spatial force representation. The net work done by the robot is then given by
Z t1
Z t1
Work =
fbT vb + mTb b dt =
FbT Vb dt.
(5.29)
t0
t0
Assuming there is no power loss due to friction or othe effects, this work should
be the same as that produced by any torques applied at the joints:
Z t1
Z t1
FbT Vb dt =
T dt,
Work =
t0
t0
where Rn is the vector of joint velocities, and Rn is the vector of joint

we have
torques. Since Vb = Jb (),
Z t0 +t
Z t0 +t
FbT Jb () dt =
T dt.
t0
t0
Since this equality must hold over all intervals [t0 , t1 ], the integrands must be
equal:
FbT Jb () = T .
Moreover, since the above equality must hold for all it follows that
= JbT ()Fb .
(5.30)
Let us replicate the derivation of = JbT ()Fb , but this time expressing
all quantities in terms of the fixed (space) frame. Let Vs = (s , vs ) denote
140
the spatial velocity of the end-effector, and Fs = (ms , fs ) the spatial force
applied at the end-effector frame origin, all expressed in fixed frame coordinates.
Here fs is the external applied force expressed in fixed frame coordinates, while
ms is the external applied moment about the fixed frame origin. Recalling
from the transformation rule for spatial forces that Fb and Fs are related by
Fb = [AdTs b ]T Fs , and that Jb () and Js () are further related by Jb () =
[AdTbs ]Js (), Equation (5.30) can be rewritten
= JbT ()Fb = ([AdTbs ]Js ()) [AdTs b ]T Fs

T
= JsT () ([AdTsb ][AdTbs ]) Fs
T
= Js ()Fs .
(5.31)
where we have made use of the adjoint identity AdTab AdTbc = AdTac , or
equivalently
[AdTab ][AdTbc ] = [AdTac ].
We can therefore write the statics relation in the general form
= J T ()F,
(5.32)
with the understanding that J() and F are expressed in terms of the same
frame. Often in robotics one is interested in determining what joint torques are
necessary, under static equilibrium conditions, to produce a given desired F;
the static relation provides an explicit answer to this question.
One could also ask the opposite question, namely, what is the spatial force
at the tip generated by a given joint torque? If J T is a square invertible matrix,
then clearly F = J T () . However, if the dimension of the joint torque vector
(n) is greater than the dimension of F (six), then the inverse of J T does not exist.
When the number of columns of the Jacobian J exceeds the number of rows,
physically what this implies is that the robot arm has extra degrees of freedom,
or is kinematically redundant. Because of the extra degrees of freedom, even
when the end-effector is made completely stationary, any applied joint torques
may cause internal motions of the links, so that the static equilibrium condition
is no longer satisfied.
5.3
Singularity Analysis
The forward kinematics Jacobian further allows us to identify postures at which

the robots end-effector loses the ability to move instantaneously in a certain
direction, or rotate instantaneously about certain axes; such a posture is called
a kinematic singularity, or simply a singularity. Mathematically a singular
posture or joint configuration of a robot arm is one in which the Jacobian J()
fails to be of maximal rank. To understand why, consider the body Jacobian
5.3. Singularity Analysis
141
Jb (), whose columns are denoted Vbi , i = 1, . . . , n. Then
Vb
1
Vbn () ...
n
Vb1 ()1 + . . . + Vbn ()n .
Vb1 () Vb2 ()
Thus, the set of all possible instantaneous spatial velocities of the tip frame
is given by a linear combination of the Vbi . As long as n 6, the maximum
rank that Jb () can attain is six. Singular postures correspond to those values
of at which the rank of Jb () drops below the maximum possible value; at
such postures the tip frame loses to the ability to generate instantaneous spatial
velocities in in one or more dimensions.
The mathematical definition of a kinematic singularity is independent of the
choice of body or space Jacobian. To see why, recall the relationship between
Js () and Jb (): Js (q) = [AdTsb (Jb ()) = [AdTsb ]Jb (), or more explicitly,

Rsb
0
Js () =
Jb ().
[psb ] Rsb Rsb
Then the rank of Js () is equal to the rank of [AdTsb ]Jb (). We now claim that
the matrix [AdTsb ] is always invertible. This can be established by examining
the linear equation

Rsb
0
x
= 0.
[psb ] Rsb Rsb
y
Its unique solution is x = y = 0, implying that the matrix [AdTsb ] is invertible.
Since multiplying any matrix by an invertible matrix does not change its rank,
it follows that
rank Js () = rank Jb (),
as claimed; singularities of the space and body Jacobian are one and the same.
Kinematic singularities are also independent of the choice of fixed frame. In
some sense this is rather obviouschoosing a different fixed frame is equivalent
to simply relocating the robot arm, which should have absolutely no effect on
whether a particular posture is singular or not. This obvious fact can be verified
by referring to Figure 5.8-(a). The forward kinematics with respect to the
original fixed frame is denoted T (), while the forward kinematics with respect
to the relocated fixed frame is denoted T 0 () = P T (), where P SE(3) is
constant. Then the body Jacobian of T 0 (), denoted Jb0 (), is obtained from
T 01 T 0 . A simple calculation reveals that
T 01 T 0 = (T 1 P 1 )(P T ) = T 1 T ,
i.e., Jb0 () = Jb (), so that the singularities of the original and relocated robot
arms are the same.
Somewhat less obvious is the fact that kinematic singularities are also independent of the choice of end-effector frame. Referring to Figure 5.8-(b), suppose
142
Figure 5.8: Kinematic singularities are invariant with respect to choice of fixed
and end-effector frames. (a) Choosing a different fixed frame, which is equivalent
to relocating the base of the robot arm; (b) Choosing a different end-effector
frame.
the forward kinematics for the original end-effector frame is given by T (), while
the forward kinematics for the relocated end-effector frame is T 0 () = T ()Q,
where Q SE(3) is constant. This time looking at the space Jacobianrecall
that singularities of Jb () coincide with those of Js ()let Js0 () denote the
space Jacobian of T 0 (). A simple calculation reveals that
T 0 T 01 = (T Q)(Q1 T 1 ) = T T 1 ,
i.e., Js0 () = Js (), so that kinematic singularities are invariant with respect to
choice of end-effector frame.
In the remainder of this section we shall consider some common kinematic
singularities that occur in six-dof open chains with revolute and prismatic joints.
We now know that either the space or body Jacobian can be used for our
analysis; we shall use the space Jacobian in the examples below.
Case I: Two Collinear Revolute Joint Axes
The first case we consider is one in which two revolute joint axes are collinear
(see Figure 5.9). Without loss of generality these joint axes can be labelled 1
and 2. The corresponding columns of the Jacobian are

1
2
Vs1 () =
, Vs2 () =
1 q1
2 q2
Since the two joint axes are collinear, we must have 1 = 2 ; let us assume the
positive sign. Also, i (q1 q2 ) = 0 for i = 1, 2. Then Vs1 = Vs2 , which implies
that Vs1 and Vs2 lie on the same line in six-dimensional space. Therefore, the
set {Vs1 , Vs2 , . . . , Vs6 } cannot be linearly independent, and the rank of Js ()
must be less than six.
5.3. Singularity Analysis
143
Figure 5.9: A kinematic singularity in which two joint axes are collinear.
z
y
x
q1
q2
q3
Figure 5.10: A kinematic singularity in which three revolute joint axes are
parallel and coplanar.
Case II: Three Coplanar and Parallel Revolute Joint Axes
The second case we consider is one in which three revolute joint axes are parallel,
and also lie on the same plane (three coplanar axessee Figure 5.10). Without
loss of generality we label these as joint axes 1, 2, and 3. In this case we choose
the fixed frame as shown in the figure; then

1
1
1
Js () =
0 1 q2 1 q3
and since q2 and q3 are points on the same unit axis, it is not difficult to verify
that the above three vectors cannot be linearly independent.
144
A1
A2
A3
A4
Figure 5.11: A kinematic singularity in which four revolute joint axes intersect
at a common point.
Case III: Four Revolute Joint Axes Intersecting at a Common Point
Here we consider the case where four revolute joint axes intersect at a common
point (Figure 5.11). Again, without loss of generality label these axes from 1
to 4. In this case we choose the fixed frame origin to be the common point of
intersection, so that q1 = . . . = q4 = 0. In this case

1 2 3 4
Js () =
.
0
0
0
0
The first four columns clearly cannot be linearly independent; one can be written as a linear combination of the other three. Such a singularity occurs, for
example, when the wrist center of an elbow-type robot arm is directly above
the shoulder.
Case IV: Four Coplanar Revolute Joints
Here we consider the case in which four revolute joint axes are coplanar. Again,
without loss of generality label these axes from 1 to 4. Choose a fixed frame
such that the joint axes all lie on the x-y plane; in this case the unit vector
i R3 in the direction of joint axis i is of the form
ix
i = iy .
0
Similarly, any reference point qi R3 lying on joint axis i is of the form
qix
qi = qiy ,
0
5.4. Manipulability
145
and subsequently
0
.
0
vi = i qi =
iy qix ix qiy
The first four columns of the space Jacobian Js () are
1x
2x
3x
3y
1y
2y
0
0
0
0
0
0
0
0
0
1y q1x 1x q1y 2y q2x 2x q2y 3y q3x 3x q3y
4x
4y
0
0
0
4y q4x 4x q4y
which clearly cannot be linearly independent.

Case V: Six Revolute Joints Intersecting a Common Line
The final case we consider is six revolute joint axes intersecting a common line.
Choose a fixed frame such that the common line lies along the z-axis, and select
the intersection between this common line and joint axis i as the reference point
qi R3 for axis i; each qi is thus of the form qi = (0, 0, qiz ), and
vi = i qi = (iy qiz , ix qiz , 0)
i = 1, . . . , 6. The space Jacobian Js () thus becomes
1x
2x
3x
4x
5x
5y
1y
2y
3y
4y
5z
1z
2z
3z
4z
Js () =
1y q1z
q5z
2y
2z
3y
3z
4y
4z
5y
1x q1z 2x q2z 3x q3z 4x q4z 5x q5z

0
0
0
0
0
6x
6y
6z
6y q6z
6x q6z
0
which is clearly singular.
5.4
Manipulability
In the previous section we saw that at a kinematic singularity, a robots endeffector loses the ability to move or rotate in one or more directions. A kinematic
singularity is a binary propositiona particular configuration is either kinematically singular, or it isntand it is reasonable to ask whether it is possible to
quantify the proximity of a particular configuration to a singularity. The answer
is yes; in fact, one can even do better and quantify not only the proximity to a
singularity, but also determine the directions in which the end-effectors ability
to move is diminished, and to what extent. The manipulabiity ellipsoid allows one to geometrically visualize the directions in which the end-effector can
146
move with the least effort (in a sense to be made precise below); directions
that are orthogonal to these directions in contrast require the greatest effort.
We illustrate the notion of manipulability ellipsoids through the planar 2R
open chain example of Figure ??. Considering only the Cartesian position of
the tip, the velocity forward kinematics is of the form x = J():

x 1
L1 cos 1 L1 cos 1 + L2 cos(1 + 2 )
1
=
.
(5.33)
x 2
L1 sin 1 L1 sin 1 + L2 sin(1 + 2 )
2
Suppose the configuration is nonsingular, so that J() is invertible. Since J()
is a linear transformation that maps joint velocities to tip velocities, one can
conjecture that a unit circle in the space of joint velocities maps to an ellipsoid
in the space of tip velocities. To see why, the unit circle is parametrized by the
2 = 1; the same constraint expressed in terms of tip velocities is
constraint kk
1
2
kJ ()xk
= 1. Denoting the elements of J 1 () by

a b
J 1 () =
,
c d
the constraint on tip velocities becomes
x 21 + x 1 x 2 + x 22 = 1,
where = a2 + b2 , = 2(ab + cd), = b2 + d2 . As is well known this is the
equation for an ellipse centered at the origin (provided 2 4 < 0, which it
is).
The major axes of the ellipse indicate the directions in which the tip can
move (or more technically, generate velocities) with the least amount of effort
(effort here corresponding to input velocities). By the same reasoning, the minor
axes indicate the directions of motion for which the greatest amount of effort
is required. As the arm configuration approaches a kinematic singularity, the
ellipsoid eventually collapses to a line directed along the direction of allowable
motion. The proximity of a particular configuration to a singularity can be measured in several ways, e.g., by the ratio of the lengths between the major and
minor axes, with a minimum value of 1 indicating that the tip can move uniformly easily in all directionssuch configurations are also sometimes referred
to as isotropic configurations. In the absence of any preferred directions of the
end-effector, such an isotropic configuration would be a reasonable choice for a
robot performing generic tasks.
The above formulation can be generalized to spatial open chains more or less
straightforwardly; without working out the details of the derivation (which require some further results from linear algebra and finite dimensional optimization
some of these details are examined in the exercises), we now formulate the
manipulability ellipsoid for an n degree of freedom open chain (n 6). Let
Jb () R6n be the body Jacobian (the choice of body Jacobian is arbitrary
the space Jacobian could just as easily have been chosen), which can be partitioned into its angular and linear velocity components J () R3n and
5.5. Summary
147
Jv () R3n as follows:
b
vb
J ()
Jv ().
(5.34)
(5.35)
At this point one may ask why we choose to partition the Jacobian in this way.
The reason is that the notion of a manipulability ellipsoid in the six-dimensioal
space of spatial velocities (, v) makes little if any sensethe physical units for
angular velocities are different from those for linear velocities. Any ellipsoid
that merges these physically different quantities will depend, ultimately, on the
choice of length scale for physical space, which as is well known is arbitrary. On
the other hand, ellipsoids restricted to the space of Cartesian velocities vb R3
are quite meaningful (as are its counterpart ellipsoids restricted to the space of
angular velocities R3 ).
We now formulate the Cartesian velocity manipulability ellipsoid associated
with Jv (); the angular velocity manipulability ellipsoid can be formulated in
an identical fashion. Assuming Jv () is nonsingular at the configuration ,
2 = 1, to a threeJv () then maps a unit sphere in Rn , parametrized as kk
3
dimensional ellipsoid in R . The principal axes of this ellipsoid can be obtained
as the eigenvectors of Jv JvT R33 , with the length of each principal axis given
by the corresponding eigenvalue.
A three-dimensional ellipsoid can also be drawn in the space of joint velocities
as follows. First, observe that JvT Jv is n n, but with rank 3 (as a result of our
assumption that Jv () is of maximal rank 3). Consequently only three of its
eigenvalues are nonzero; they are, in fact, the three eigenvalues of Jv JvT . The
three eigenvectors corresponding to these nonzero eigenvalues are then precisely
the joint velocity vectors that map to the three principal axes of the ellipsoid in
the space of Cartesian velocities.
In the absence of any preferred directions of the end-effector, one can argue
that an ellipsoid that most closely resembles a sphere is the most desirable.
Configurations in which the ellipsoid is spherical are called isotropic configurations, and are marked by the eigenvalueswhich are proportional to the lengths
of the ellipsoids principal axeshaving identical value.
5.5
Summary
Let the forward kinematics of an n-link open chain be expressed in the

T = e[S1 ]1 e[Sn ]n M.
The space Jacobian Js () R6n relates the joint rate vector
The i-th
to the end-effector spatial velocity Vs via Vs = Js ().
of Js () is
Vsi () = Ade[S1 ]1 e[Si1 ]i1 (Si ),
Rn
column
(5.36)
148

for i = 2, . . . , n, with the first column Vs1 () = S1 . Vsi is the screw vector
for joint i expressed in space frame coordinates, with the joint values
assumed to be arbitrary rather than zero.
Let the forward kinematics of an n-link open chain be expressed in the
T = M e[B1 ]1 e[Bn ]n .
(5.37)
The body Jacobian Jb () R6n relates the joint rate vector Rn to

the end-effector spatial velocity Vb = (b , vb ) via
Vb = Jb ().
(5.38)
The i-th column of Jb () is given by

Vb,i () = Ade[Bn ]n e[Bi+1 ]i+1 (Bi ),
(5.39)
for i = n, n 1, . . . , 2, with Vbn () = Bn . Vbi is the screw vector for joint

i expressed in body frame coordinates, with the joint values assumed to
be arbitrary rather than zero. The body Jacobian is related to the space
Jacobian via the relation
Js ()
[AdT sb]Jb ()
Jb ()
[AdT bs]Js ()
where Tsb = T .
Consider a force f applied to some point p on a rigid body. The moment
m generated by f with respect to the {s} frame origin is m = r f, where
r is the vector from p to the {s} frame origin. Let ms R3 and fs R3 be
vector representations of m and f in {s} frame coordinates. The spatial
force in space coordinates Fs R6 is defined to be Fs = (ms , fs ).
Consider a rigid body with a body frame {b} attached, and a force f
applied to some point p on the rigid body. The moment m generated by
f with respect to the {b} frame origin is then m = r f, where r is now
the vector from p to the {b} frame origin. Let mb R3 and fb R3 be
vector representations of m and f in {b} frame coordinates. The spatial
force in body coordinates Fb R6 is defined to be Fb = (mb , fb ). Fb
and Fs are related by
Fb
AdTTsb (Fs ) = [AdTsb ]T Fs
Fs
AdTTbs (Fb ) = [AdTbs ]T Fb .
Consider a spatial open chain with n one degree of freedom joints that is
also assumed to be in static equilibrium. Let Rn denote the vector of
joint torques and forces, and F R6 be the spatial force applied at the
end-effector, in either space or body frame coordinates. Then and F are
related by
= JbT ()Fb = JsT ()Fs .
5.6. Notes and References
149
A kinematically singular configuration for an open chain, or more simply

a kinematic singularity, is any configuration Rn at which the rank
of the Jacobian (either Js () or Jb ()) is not maximal. For spatial open
chains of mobility six consisting of revolute and prismatic joints, some
common singularities include (i) two collinear revolute joint axes; (ii) three
coplanar and parallel revolute joint axes; (iii) four revolute joint axes
intersecting at a common point; (iv) four coplanar revolute joints, and (v)
six revolute joints intersecting a common line.
5.6
The terms spatial velocity and spatial force were first coined by Roy Featherstone [10], and are also referred to in the literature as twists and wrenches,
respectively. There is a well developed calculus of twists and wrenches that is
covered in treatments of classical screw theory, e.g., [3], [2]. Singularities of
closed chains are discussed in the later chapter on closed chain kinematics. Manipulability ellipsoids and their dual, force ellipsoids, are discussed in greater
detail in [30].
150
Figure 5.12: Polar coordinates.
5.7
Exercises
1. Given a particle moving in the plane, define a moving reference frame {

er , e }
such that its origin is fixed to the origin of the fixed frame, and its er axis always
points toward the particle (Figure 5.12). Let (r, ) be the polar coordinates for
the particle position, i.e., r is the distance of the particle from the origin, and
is the angle from the horizontal line to the er axis. The particle position p~ can
then be written
p~ = r
er ,
and its velocity ~v is given by
~v = r
er + re r .
The acceleration ~v is the time derivative of ~v .
(a) Express e r in terms of er and e .
(b) Show that ~v and ~a are given by
~v
~a =
e
r
er + r
e .
(
r r2 )
er + (r + 2r )
J}
denote the unit axes of the fixed frame, and let
2. Let {I,
p~ = X(t)I + Y (t)J
denote the position of a particle moving in the plane (see Figure 5.13). Suppose
the path traced by the particle has nonzero curvature everywhere, so that for
every point on the path there exists some circle tangent to the path; the center
of this circle is called the center of curvature, while its radius is the radius
of curvature. Clearly both the center and radius of curvature vary along the
path, and are well-defined only at points of nonzero curvature (or, at points
where the curvature is zero, the center of curvature can be regarded to lie at
infinity).
5.7. Exercises
151
Figure 5.13: Tangential-normal coordinates.

Now attach a moving reference frame {
et , en } to the particle, in such a way
that that et always points in the same direction as the velocity vector; en then
points toward the center of curvature. Since the speed of the particle is given
by
p
v = X 2 + Y 2 .
and et always points in the direction of the velocity vector, it follows that the
velocity vector ~v of the particle can be written
~v = v
et ,
while its acceleration is given by
~a = v
et + v e t .
(a) Show that e t = ke t k
en , or
en =
e t
,
ke t k
and that consequently the acceleration ~a is

~a = v
et + vke t k
en .
(b) The radius of curvature can be found from the following formula:
=
=
v3
Y Y X
X
(X 2 + Y 2 )3/2
.
X Y Y X
152
Figure 5.14: A cannon mounted on a 2R rotating platform.

Using the formula, show that the acceleration ~a can be written
~a = v
et +
v2
en .
3. In standard treatments of particle kinematics using moving frames, two

moving particles, A and B, are assumed, with a moving frame {
x, y, z} attached
to particle A. Writing the position of particle B as
p~B = p~A + p~B|A ,
where p~B|A denotes the vector from A to B, the following formulas for the
velocity and acceleration of B are usually provided:

~vB = ~vA + p~B|A
+
~ p~B|A
xyz

~aB = ~aA + p~B|A
+ 2~
p~B|A
+
~ p~B|A +
~ (~
p~B|A ),
xyz
xyz
where
~ and
~ respectively denote theangular
velocity

and angular acceler
ation vector of the moving frame, and p~B|A

p~B|A
are certain time
xyz
xyz
derivatives of p~B|A . Writing p~A and p~B|A in terms of fixed and moving frame
coordinates, i.e.,
p~A
p~B|A
+ Y Y + Z Z
= XX
= x
x + y y + z z,
derive the above

for
~vB and
formulas

~aB . Be sure to explicitly identify all terms,
in particular p~B|A
and p~B|A
.
xyz
xyz
5.7. Exercises
153
Figure 5.15: A circular plate of radius R.
4. Figure 5.14 depicts a cannon mounted on a 2R rotating platform at time

t = 0. The platform rotates at constant angular velocities 1 and 2 radians/sec
as shown. The axis of the cannon barrel is displaced at a distance d from the
origin of the fixed frame. Assume that a cannonball is fired at t = 0 from the
same height as the I axis, at a constant speed v0 along the axis of the barrel.
(a) Choose a moving frame and describe how the frame moves.
(b) Determine the velocity of the cannonball at t = 0 in terms of the moving
frame chosen in part (a).
5. As shown in Figure 5.15, a revolving circular plate of radius R, rotating at
a constant angular velocity of 2 radians/sec, is mounted on a wheeled mobile
base that moves periodically back and forth along the I axis according to
x(t) = a sin 1 t.
(a) Assuming t = 0 at the instant shown in the figure, determine the velocity
of point A as a function of t in fixed frame coordinates.
(b) Determine the acceleration of point A as a function of t in fixed frame
coordinates.
axis at a constant
6. The circular pipe of Figure 5.16 is rotating about the X
rate 1 radians/sec, while a marble D is circling the pipe at a constant speed
u.
(a) Find the angle at which the magnitude of the velocity of D is maximal.
What is the maximal velocity magnitude at this angle?
(b) Find the angle at which the magnitude of the acceleration of D is maximal.
What is the maximal acceleration magnitude at this angle?
154
Figure 5.16: A marble traversing a rotating circular pipe.
Figure 5.17: A satellite with a rotating panel.
axis at a
7. The satellite of Figure 5.17 is rotating about its own vertical K
constant rate 1 radians/sec, while at the same time its solar panel rotates at
a constant rate 2 radians/sec as shown.
(a) Determine the velocity of point A when 1 = 0.5, 2 = 0.25, l = 2m,
r = 0.5m, and = 30 .
(b) Determine the acceleration of point A under the same conditions as part
(a).
8. The two revolute joints in the spherical 2R open chain of Figure 5.18 rotating at constant angular velocities 1 radians/sec and 2 radians/sec as shown.
Denote by r the length of link AB, while is the angle between link AB and
the x-y plane.
(a) Choose a moving frame and explain how the frame moves.
(b) Determine the angular velocity and angular of link AB in terms of your
moving frame coordinates chosen in part (a).
(c) Determine the velocity of point B in terms of the chosen moving frame coordinates.
5.7. Exercises
155
Figure 5.18: A spherical 2R open chain.
Figure 5.19: A rotating disk.

(d) Determine the acceleration of point B in terms of the chosen moving frame
coordinates.
(e) Setting 1 = 0.1, 2 = 0.2, and r = 100mm, determine the velocity and
acceleration of point B in terms of the fixed frame coordinates when = /6.
9. As shown in Figure 5.19, a disk of radius r spins at a constant angular
156
Figure 5.20: A toroidal 2R open chain.

velocity of 2 radians/sec about its horizontal axis, while at the same time the
disk assembly rotates about the vertical axis at a constant angular velocity of
1 radians/sec.
(a) Determine the angular velocity and the angular acceleration of the disk in
terms of fixed frame coordinates.
(b) Determine the velocity and the acceleration of point P as a function of the
angle .
10. As shown in Figure 5.20, the two revolute joints of the toroidal 2R open
chain are rotating at a constant angular velocity 1 = 0.6 radians/sec about the
Y axis, and 2 = 0.45 radians/sec about the horizontal axis through C. When
= 120 , determine the following in terms of fixed frame coordinates:
(a) the angular acceleration of link CD.
(b) the velocity of point D.
(c) the acceleration of point D.
11. Figure 5.21 shows an RRP open chain at t = 0. The revolute joints rotate
at constant angular velocities 1 and 2 radians/sec. Suppose the vertical position of point B is given by x(t) = sin t. Determine the following quantities in
terms of fixed frame coordinates.
(a) The velocity of point B at t = 0.
(b) The acceleration of point B at t = 0.
12. The square plate of Figure 5.22 rotates about axis I with angular velocity
2 = 0.5 radians/sec and angular acceleration 2 = 0.01 radians/sec2 , while
the circular disk attached to the square plate rotates about the axis normal
5.7. Exercises
157
Figure 5.21: An RRP open chain.
Figure 5.22: A rotating square plate.

to the plate with angular velocity 1 = 1 radians/sec and angular acceleration
1 = 0.5 radians/sec2 . The radius of the circular disk is R = 5m, while the
length of each side of the square plate is 2R = 10m. The distance from the
center of the circular disk to the small circular knob is d = 3m. Assume that
both the disk and the square plate have zero thickness. Setting 1 = 0 and
2 = 45 , find the following in terms of fixed frame coordinates:
(a) The velocity of the circular knob.
(b) The acceleration of the circular knob.
13. A person is riding the 2R gyro swing of Figure 5.23. Joint oscillates
periodically according to (t) = cos t, and the circular plate connected to the
axis of the gyro swing rotates with constant angular velocity 2 radians/sec. At
t = 0, the person on the circular plate is at the maximal height as shown in the
figure. Setting l = 1, r = 1, and 2 = 1 radian/sec, determine the velocity of
the person in terms of the given fixed frame coordinates when t = 2 .
158
Figure 5.23: A 2R gyro swing.
Figure 5.24: A meteorite approaching the earth.
14. As shown in Figure 5.24, a meteorite is approaching a rotating asteroid

along the meteorites J axis with velocity v1 = 100 m/sec. Suppose the radius
of the asteroid is R = 1000m, and the distance of the meteorite from the asteroid
is initially D = 107 m. The asteroid takes 6 hours to complete a full revolution.
An astronaut stands at the point antipodal to the expected point of collision,
and unwittingly starts walking north along a longitudinal arc at a velocity of
v2 = 1 m/sec. After three hours, determine the velocity of the astronaut in terms
of the moving frame coordinates attached to the meteorite. of the moving frame
at the meteorite.
15. As shown in Figure 5.25, a clock of radius r is mounted on a gimbal
assembly as shown. The angles 1 , 2 , and 3 are adjustable to arbitrary values;
in the figure the angles are all assumed to be set to zero. A moving frame {T }
is attached to the tip of the clocks second hand, with its x
-axis aligned along
the tip of the hand as shown. Setting r = 1m , a = 3m, b = 7m, answer the
5.7. Exercises
159
Figure 5.25: A clock mounted on a gimbal assembly.

following:
(a) Assuming the second hand starts at 12 at t = 0, when the second hand
reaches 10, find TST SE(3) as a function of the angles (1 , 2 , 3 ).
(b) Setting 1 = 90 , 2 = 0, 3 = 90 , find the the velocity of the tip of the
second hand at the moment it passes 10.
16. In this exercise we derive the analytic Jacobian for an n-link open chain
corresponding to the exponential coordinates on SO(3).
(a) Given an n n matrix A(t) parametrized by t that is also differentiable with
respect to t, its exponential X(t) = eA(t) is then an n n matrix that is always
nonsingular. Prove the following:
Z 1
1
A(t)s
X X =
eA(t)s A(t)e
ds
0
1
XX
Z
=
1
A(t)s
eA(t)s A(t)e
ds.
(b) Using the above result to show that for r(t) R3 and R(t) = e[r(t)] , the
160
Figure 5.26: A 3R planar open chain.
Figure 5.27: A planar 4R open chain.

angular velocity in the body frame, [b ] RT R is related to r by
b
= A(r)r
1 cos krk
krk sin krk 2
A(r) = I
[r] +
[r] .
krk2
krk3
(c) Derive the corresponding formula relating the angular velocity in the space
T , with r.
frame, [s ] RR
17. The 3R planar open chain of Figure 5.26 is shown in its zero position.
(a) Suppose the tip must apply a force of 5N in the x
-direction. What torques
should be applied at each of the joints?
(b) Suppose the tip must now apply a force of 5N in the y-direction. What
torques should be applied at each of the joints?
5.7. Exercises
161
Figure 5.28: A rigid body rotating in the plane.
18. Answer the following questions for the 4R planar open chain of Figure 5.27.
(a) Derive the forward kinematics in the form
T () = e[S1 ]1 e[S2 ]2 e[S3 ]3 e[S4 ]4 M.
where each Si R3 and M SE(2).
(b) Derive the body Jacobian.
(c) Suppose the chain is in static equilibrium at the configuration 1 = 2 =
0, 3 = 2 , 4 = 2 , and a force f = (10, 10, 0) and moment m = (0, 0, 10) are
applied to the tip (both f and m are expressed with respect to the fixed frame).
What are the torques experienced at each of the joints?
(d) Under the same conditions as (c), suppose that a force f = (10, 10, 0)
and moment m = (0, 0, 10) are applied to the tip. What are the torques
experienced at each of the joints?
(e) Find all kinematic singularities for this chain.
19. Referring to Figure 5.28, the rigid body rotates about the point (L, L) with
angular velocity = 1.
(a) Find the position of point P on the moving body with respect to the fixed
reference frame in terms of .
(b) Find the velocity of point P in terms of the fixed frame.
(c) What is Tf b , the displacement of frame {b} as seen from the fixed frame
{f }?
(d) Find the spatial velocity of Tf b in body coordinates.
(e) Find the spatial velocity of Tf b in space coordinates.
(f) What is the relation between the spatial velocities obtained in (d) and (e)?
(g) What is the relation between the spatial velocity obtained in (d) and P
obtained in (b)?
162

3
4
z
y
{T}
2
L
1
z
y
x
{S}
Figure 5.29: An RRRP spatial open chain.
L
z
y
x
{ b}
2L
z
{S}
Figure 5.30: A spatial 3R open chain.

(h) What is the relation between the spatial velocity obtained in (e) and P
obtained in (b)?
20. The RRRP chain of Figure 5.29 is shown in its zero position.
(a) Determine the body Jacobian Jb () when 1 = 2 = 0, 3 = /2, 4 = L.
(b) Determine the linear velocity of the end-effector frame, in fixed frame coordinates, when 1 = 2 = 0, 3 = /2, 4 = L and 1 = 2 = 3 = 4 = 1.
21. The spatial 3R open chain of Figure 5.30 is shown in its zero position.
(a) In its zero position, suppose we wish to make the end-effector move with
linear velocity vtip = (10, 0, 0), where vtip is expressed with respect to the space
frame {s}. What are the necessary input joint velocities 1 , 2 , 3 ?
(b) Suppose the robot is in the configuration 1 = 0, 2 = 45 , 3 = 45 .
Assuming static equilibrium, suppose we wish to generate an end-effector force
fb = (10, 0, 0), where fb is expressed with respect to the end-effector frame {b}.
What are the necessary input joint torques 1 , 2 , 3 ?
5.7. Exercises
163
s
s
Z
X
W
[
s
\
]
Figure 5.31: A spatial P RRRRP open chain.

(c) Under the same conditions as in (b), suppose we now seek to generate an
end-effector moment mb = (10, 0, 0), where mb is expressed with respect to the
end-effector frame {b}. What are the necessary input joint torques 1 , 2 , 3 ?
(d) Suppose the maximum allowable torques for each joint motor are
k1 k 10, k2 k 20, k3 k 5.
In the home position, what is the maximum force that can be applied by the
tip in the end-effector frame x-direction?
22. The spatial P RRRRP open chain of Figure 5.31 is shown in its zero
position.
(a) At the zero position, find the first three columns of the space Jacobian.
(b) Find all configurations at which the first three columns of the space Jacobian
become linearly dependent.
(c) Suppose the chain is in the configuration 1 = 2 = 3 = 5 = 6 =
0, 4 = 90o . Assuming static equilibrium, suppose a pure force fb = (10, 0, 10)
is applied to the origin of the end-effector frame, where fb is expressed in terms
of the end-effector frame. Find the joint torques 1 , 2 , 3 experienced at the
first three joints.
23. Consider the P RRRRR spatial open chain of Figure 5.32 shown in its zero
position. The distance from the origin of the fixed frame to the origin of the
end-effector frame at the home position is L.
(a) Determine the first three columns of the space Jacobian Js .
(b) Determine the last two columns of the body Jacobian Jb .
(c) For what value of L is the home position a singularity?
164

2
z {S}
{T}
y
y
x
100N
6
L
Figure 5.32: A P RRRRR spatial open chain.

Prismatic joint axis
Revolute joint axis
Figure 5.33: A kinematic singularity involving prismatic and revolute joints.

(d) In the zero position, what joint torques must be applied in order to generate
a pure end-effector force of 100N in the -
z direction?
24. Find all kinematic singularities of the 3R wrist with the following forward
kinematics:
R = e[1 ]1 e[2 ]2 e[3 ]3
where 1 = (0, 0, 1), 2 = (1/ 2, 0, 1/ 2), and 3 = (1, 0, 0).

25. Show that a six degree of freedom spatial open chain is in a kinematic
singularity when any two of its revolute joint axes are parallel, and any prismatic
joint axis is normal to the plane spanned by the two parallel revolute joint axes
(see Figure 5.33).
26. (a) Determine the space Jacobian Js () of the 6R spatial open chain of
Figure 5.34.
5.7. Exercises
165
L
z
y
y
x
{S}
{T}
Figure 5.34: Singularities of an elbow-type 6R open chain.

1
4
z
Y
X
{S}
1
5
{T}
45o
2
1
1
Figure 5.35: A spatial P RRRRP open chain with a skewed joint axis.
(b) Find the kinematic singularities of the given chain. Explain each singularity
in terms of the alignment of the joint screws, and the directions in which the
end-effector loses one or more degrees of freedom of motion.
27. The spatial P RRRRP open chain of Figure 5.35 is shown in its zero
position.
(a) Determine the first 4 columns of the space Jacobian Js ().
(b) Determine whether the zero position is a kinematic singularity.
(c) Calculate the joint torques required for the tip to apply the following endeffector spatial forces:
(i) Fs = (0, 1, 1, 1, 0, 0)T
(ii) Fs = (1, 1, 0, 1, 0, 1)T .
28.
The spatial RRP RRR open chain of Figure 5.36 is shown in its zero
166

L1
L2
4
3
5
L0
{t}
L1
z
z
{ 0}
{0}
Figure 5.36: A spatial RRP RRR open chain.
Figure 5.37: A rollercoaster undergoing a screw motion.

position.
(a) For the fixed frame {0} and tool frame {t} as shown, express the forward
kinematics in the following product of exponentials form:
T () = e[S1 ]1 e[S2 ]2 e[S3 ]3 e[S4 ]4 e[S5 ]5 e[S6 ]6 M.
(b) Find the first three columns of the space Jacobian Js ().
(c) Suppose that the fixed frame {0} is moved to another location {00 } as shown
in the figure. Find the first three columns of the space Jacobian Js () with
respect to this new fixed frame.
(d) Determine if the zero position is a kinematic singularity, and if so, provide
a geometric description in terms of the joint screw axes.
29. The rollercoaster of Figure 5.37 undergoes a screw motion as shown: point
A traces a circle of radius R, and the rollercoaster moves in screw-like fashion at
a distance r from this larger circle. The roller coaster completes one revolution
about this larger circle when point A traverses 30 along the larger circle.
5.7. Exercises
167
(a) Find T12 , the relative displacement of the rollercoaster frame {2} as seen
from the fixed frame {1}, in terms of the angle as indicated in the figure.
(b) Derive the space Jacobian for T12 ().
30. Two frames {a} and {b} are attached to a moving rigid body. Show that
the spatial velocity of {a} in space frame coordinates is the same as the spatial
velocity of {b} in space frame coordinates.
31. Consider an n-link open chain, with reference frames attached to each link.
Let
T0k = e[S1 ]1 e[Sk ]k Mk , k = 1, . . . , n
be the forward kinematics up to link frame {k}. Let Js () be the space Jacobian
for T0n . The columns of Js ()are denoted

Js () = Vs1 () Vsn () .
T 1 be the spatial velocity of link frame {k} in frame {k} coorLet [Vk ] = T0k
0k
dinates.
(a) Derive explicit expressions for V2 and V3 .
(b) Based on your results from (a), derive a recursive formula for Vk+1 in terms
of Vk , Vs1 , . . . , Vs,k+1 , and .
168
Chapter 6
Inverse Kinematics
For a general n degree of freedom open chain with forward kinematics T (),
Rn , the inverse kinematics problem can be stated as follows: given a homogeneous transform X SE(3), find solutions that satisfy T () = X. To
highlight the main features of the inverse kinematics problem, let us consider
the two-link planar open chain of Figure 6.1-(a) as a motivational example.
Considering only the position of the end-effector and ignoring its orientation,
the forward kinematics can be expressed as

x
L1 cos 1 + L2 cos(1 + 2 )
=
.
(6.1)
y
L1 sin 1 + L2 sin(1 + 2 )
Assuming L1 > L2 , the set of reachable points, or the workspace, is an annulus
of innner radius L1 L2 and outer radius L1 + L2 . Given some end-effector
position (x, y), it is not hard to see that there will be either zero, one, or two
solutions depending on whether (x, y) lies in the exterior, boundary, or interior
of this annulus (the case of two solutions is given by the familiar elbow-up and
elbow-down configurations as illustrated in Figure ??).
Finding an explicit solution (1 , 2 ) for a given (x, y) is also not difficult. Referring this time to Figure 6.1-(b), assume that (x, y) lies in the first quadrant,
i.e., both x and y are positive (solutions for the other quadrants follow straightforwardly). Angle , restricted to lie in the interval [0, ], can be determined
from the law of cosines:
L21 + L22 2L1 L2 cos = x2 + y 2 ,
from which it follows that
cos
= .
L21 + L22 x2 y 2
2L1 L2
Also from the law of cosines,

= cos1
x2 + y 2 + L21 L22
p
2L1 x2 + y 2
169
!
.
170
Inverse Kinematics
(a) Workspace and elbow-up

elbow-down configurations.
and
(b) Geometric solution.
Figure 6.1: 2R planar open chain inverse kinematics.

and since tan(1 + ) = y/x, it follows that
1 = tan1
y
.
x
The above values for 1 and 2 correspond to the elbow-down solution. The
elbow-up solution is given by
1 = tan1
y
+ , 2 = + .
x
If x2 +y 2 lies outside the range [L1 L2 , L1 +L2 ], then no solution exists. Again,
the case when (x, y) lies in other quadrants follows straightforwardly from the
above solution for the first quadrant.
This simple motivational example illustrates that for open chains, the inverse
kinematics problem may have multiple solutions; this is in contrast to the forward kinematics, where a unique end-effector displacement T exists for a given
joint value . In fact, three-link planar open chains have an infinite number
of solutions for points (x, y) lying in the interior of the workspace; in this case
the chain possesses an extra degree of freedom, and is said to be kinematically
redundant.
This chapter begins by considering the inverse kinematics of spatial open
chains with six degrees of freedom. A finite number of solutions exists in this
case, and we first make some simplifying assumptions about the kinematic structure that lead to analytic solutions. As we shall see, these assumptions are not
restrictive, as they accommodate the most commonly used six degree of freedom
open chains. We then specialize the Newton-Raphson method for nonlinear root
finding to the inverse kinematics problem. The result is an iterative numerical
algorithm that, provided the initial guess is sufficiently close to the true solution,
eventually converges to the solution. The chapter concludes with a discussion
of pseudo-inverse based inverse kinematics solutions for redundant open chains.
6.1. Analytic Inverse Kinematics
171
Figure 6.2: Inverse position kinematics of a 6R PUMA-type arm.
(a) Elbow arm with

offset.
(b) Kinematic diagram.
Figure 6.3: A 6R PUMA-type arm with a shoulder offset.
6.1
Analytic Inverse Kinematics
We begin by writing the forward kinematics of a spatial six degree of freedom

open chain in the following product of exponentials form:
T () = e[S1 ]1 e[S2 ]2 e[S3 ]3 e[S4 ]4 e[S5 ]5 e[S6 ]6 M.
Given some X SE(3), the inverse kinematics problem is concerned with
finding all solutions R6 that satisfy T () = Y . In the following subsections
we shall make some simplifying assumptions about the kinematic structure of
the open chain that will lead to analytic solutions for the inverse kinematics.
6.1.1
6R PUMA-Type Arm
We first consider a 6R arm of the PUMA type (see Figure 6.2). Such arms
are characterized by (i) the last three joint axes intersecting orthogonally at
a common point (the wrist center) to form an orthogonal wrist; (ii) the first
two axes intersecting orthogonally to form a shoulder joint; and (iii) the elbow
172
Inverse Kinematics
Figure 6.4: Singular configuration of the zero-offset 6R PUMA-type arm.

joint axis being parallel with the horizontal shoulder joint axis. Such arms may
also have an offset at the shoulder (see Figure 6.3). The inverse kinematics
problem for PUMA-type arms can be decoupled into an inverse position and
inverse orientation subproblem, which we now discuss.
We first consider the simple case of a zero-offset PUMA-type arm. Referring
to Figure 6.2 and expressing all vectors in terms of fixed frame coordinates,
denote components of the wrist center p R3 by p = (px , py , pz ). Projecting p
onto the x-y plane, it can be seen that
1 = tan1
py
,
px
where the atan2() function can be used instead of tan1 . Note that a second
valid solution for 1 is given by
1 = tan1
py
+ ,
px
provided that the original solution for 2 is replaced by 2 . As long as

px , py 6= 0, both these solutions are valid. When px = py = 0 the arm is in a
singular configuration (see Figure 6.4), and there are infinitely many possible
solutions for 1 .
If there is an offset d1 6= 0 as shown in Figure 6.3, then in general there will be
two solutions for 1 , denoted the right and left arm solutions (Figure
6.3). As
p
seen from the figure, 1 = , where = tan1 ( pxy ) and = tan1 (

2 2 2
p +p d
tan1 ( x d1 y 1 ). The second solution is given by
py
1 = tan1 ( ) tan1 (
px
p2x + p2y d21

d1
r 2 d21
)
d1
Determining angles 2 and 3 for the PUMA-type arm now reduces to the inverse
173
Figure 6.5: Four possible inverse kinematics solutions for the 6R PUMA type
arm with shoulder offset.
kinematics problem for a planar two-link chain:
cos 3
=
=
r2 + s2 a22 a23
2a2 a3
p2x + p2y + (p2z d1 )2 a22 a23
2a2 a3
If we let cos 3 = D, then 3 is given by
3 = tan1 (
1 D2
)
D
2 can be obtained in a similar fashion as

2
s
a3 s3
tan1 ( ) tan1 (
)
r
a2 + a3 c3
a3 s3
pz d1
= tan1 ( q
) tan1 (
)
a2 + a3 c3
p2x + p2y
The two solutions for 3 correspond to the well-known elbow up and elbow
down configurations for the two-link planar arm. In general, a PUMA-type arm
with an offset will have four solutions to the inverse position problem, as shown
in Figure 6.5; the upper postures are called left-arm solutions (elbow-up and
elbow-down), while the lower postures are called right-arm solutions (elbow-up
and elbow-down).
174
Inverse Kinematics
Figure 6.6: A 6R spatial open chain of the generalized PUMA type.

We now solve the inverse orientation problem, i.e., finding values for (4 , 5 , 6 )
given the end-effector frame orientation. This is completely straightforward:
since the final three joints form a 3R wrist with orthogonal axes, the joint values can be determined via an appropriate set of Euler angles as discussed in
Chapter 3 (e.g., ZYX, ZYZ, depending on how the final three joint axes are
aligned when in the zero position).
6.1.2
Generalized 6R PUMA-Type Arms
We now relax some of the assumptions made for the 6R PUMA-type arm: a
generalized 6R PUMA-type arm is characterized by (i) the first two joint
axes intersecting orthogonally, and (ii) the last three joint axes intersecting
orthogonally at a common point. Referring to Figure 6.6, place the fixed frame
origin at the intersection of joint axes 1 and 2, and let rw R3 be the fixed
frame representation of the point of intersection of the final three axes. The
assumptions about the joint axes then lead to the following relations among the
joint screws Si = (i , i ri ), i = 1, . . . , 6, where ri denotes a reference point
on axis i:
1T 2 = 0;
4T 5 = 0 and 5T 6 = 0.
The inverse kinematics problem can now be stated as finding solutions to
e[S1 ]1 e[S2 ]2 e[S3 ]3 e[S4 ]4 e[S5 ]5 e[S6 ]6 = XM 1 ,
where
S1
(1 , 0)
S2
(2 , 0)
S3
(3 , 3 r3 )
S4
(4 , 4 rw )
S5
(5 , 5 rw )
S6
(6 , 6 rw ),
(6.2)
175
Figure 6.7: Solving ke[S] pk = c for .

and the right-hand side XM 1 is given; denote this known quantity by X1 =
XM 1 . Solving the inverse kinematics then proceeds in three steps:
Step 1: Solve for 3
We first multiply both sides of Equation (6.2) by rw (here multiplication of a
vector by a homogeneous transformation is understood in the usual sense, i.e.,

R p
T rw = Rrw + p, T =
.
0 1
The net effect is to consecutively apply the screw motions defined by e[S6 ]6 ,
e[S5 ]5 , and e[S4 ]4 to rw . However, since all three of these screw motions are
zero pitch (all joints are revolute), and rw is a point lying on all three screw
axes, it follows that
e[S4 ]4 e[S5 ]5 e[S6 ]6 rw = rw .
We are thus left with
e[S1 ]1 e[S2 ]2 e[S3 ]3 rw = X1 rw = p1 ,
(6.3)
where the vector p1 = X1 rw is known.

Now take the norm of both sides of (6.3). Since both e[S1 ]1 and e[S2 ]2 are
pure rotations, and the general identity kRvk = kvk holds for any rotation R
and vector v, Equation (6.3) becomes
ke[S3 ]3 rw k = kp1 k.
176
Inverse Kinematics
This is a problem of the general form

ke[S] pk = c,
where S = (, r), p R3 , and the scalar c > 0 are known, and the
objective is to find all values of [0, 2] satisfying this equality. solve this
problem. Referring to Figure 6.7, define the vectors w, u, v R3 according to
z
= e[S] p
u = pr
v
= w r,
with kzk = c given. We shall now project the vectors p, u, v, r, and z onto the
plane normal to the screw axis and containing p: define these respectively to be
pproj
uproj
=
=
p (pT )
u (uT )
vproj
v (v T )
rproj
r (rT )
zproj
z (z T ).
Note that uproj and qproj are known a priori. From the figure it can be seen
that
k(z T )k = k(pT )k = |pT |.
Since zproj = z (z T ) and kzk2 = c2 , it follows that
kzproj k2 = c2 (pT )2 ,
which is also known a priori. Let us first find the angle = + , where is
defined as indicated in the figure. Since
uTproj (qproj )
uproj (qproj )
= kuproj k kqproj )k cos( + )
(6.4)
=
(kuproj k kqproj k sin( + )) ,
(6.5)
from the latter equality it follows that

T (uproj (qproj )) = kuproj kkqproj k sin( + ).
(6.6)
From Equations (6.4) and (6.6) we have

1
= tan
T (uproj qproj )
uTproj qproj
!
.
(6.7)
We now determine from the law of cosines: referring to the inset of Figure 6.7,
krproj k2 + kvproj k2 2krproj k kvproj k cos = kzproj k2 .
177
Figure 6.8: Solving e[1 ]1 e[2 ]2 p2 = p1 for 1 and 2 .

Since kzproj k2 = c2 (pT )2 is known, and kvproj k = kuproj k is also known,
can be determined as follows:

krproj k2 + kvproj k2 kzproj k2
1
= cos
.
(6.8)
2krproj k kvproj k
From the figure it should be apparent that there can be up to two solutions for
:
= .
If = 0, the two solutions collapse to a single solution, while a solution does
not exist in the event that does not exist.
Step 2: Solve for 1 and 2
Having solved for 3 , Equation (6.3) can be written
e[S1 ]1 e[S2 ]2 p2 = p1 ,
(6.9)
where p2 = e[S3 ]3 rw and p1 are both known. Observe that S1 = (1 , 0) and

S2 = (2 , 0) are both pure rotations, and 1 and 2 are orthogonal to each
other. Thus, only the rotation component of (6.9) needs to be considered:
e[1 ]1 e[2 ]2 p2 = p1 .
(6.10)
Referring to Figure ??, clearly a necessary condition for a solution (1 , 2 ) to

exist is that kp1 k = kp2 k. Assuming this is the case, the solutions are then
178
Inverse Kinematics
marked by the intersection of the two circles indicated in Figure 6.8. In the
general case there can be up to two solutions, with one or no solution also a
possibility.
Assuming a solution exists, let z R3 be the vector p2 rotated about
2
by angle 2 . z can also be obtained by rotating p1 about 1 by an angle 1 .
Mathematically,
z = e[2 ]2 p2 = e[1 ]1 p1 .
Clearly {1 , 2 , 1 2 } forms an orthonormal basis for R3 . Further observe
that the 1 component of z is the same as the 1 component of p1 , and the
2 component of z is the same as the 2 component of p2 ). z can therefore be
expressed in terms of this orthonormal basis as
z = (pT1 1 )1 + (pT2 2 )2 c(1 2 )
for some scalar constant c 0. The length kzk is then, straightforwardly,
kzk = (pT1 1 )2 + (pT2 2 )2 + c2 .
Since z is also a rotated version of p2 (and also of p1 ), it follows that kzk =
kp2 k = kp1 k. Solving the above for c2 , z can now be written
q
z = (pT1 1 )1 + (pT2 2 )2 kp2 k2 (pT1 1 )2 (pT2 2 )(1 2 ).
If c = 0 then there exists a unique solution (1 , 2 ), while if c does not exist,
then no solution (1 , 2 ) exists.
Having found up to two possible solutions for z, what remains is to find 1
and 2 for each z. This is relatively straightforward: letting u1 and z1 respectively be the projections of p1 and z onto circle 1, and u2 and z2 respectively
be the projections of p2 and z onto circle 2, it follows that
1
cos1 (uT1 z1 )
cos1 (uT2 z2 ).
Step 3: Solve for 4 , 5 , and 6

Having found 1 , 2 , and 3 , it remains to solve for 4 , 5 , and 6 . We have
e[S4 ]4 e[S5 ]5 e[S6 ]6
e[S3 ]3 e[S2 ]2 e[S1 ]1 XM 1
X2 ,
(6.11)
where the right-side X2 is now known. Recall that 4T 5 = 0 and 5T 6 = 0;

this implies that 4 and 6 are either orthogonal or parallel. Assume for the
time being that 6 is orthogonal to 4 , or more precisely, 6 = 4 5 (the
parallel case will be considered later). Define the transformation

Rw rw
Tw =
, Rw = 6 5 4 SO(3).
0
1
179
Multiplying both sides of (6.11) by Tw1 leads to

Tw1 e[S4 ]4 e[S5 ]5 e[S6 ]6
e
1
Tw
[S4 ]Tw 4
1
Tw
[S5 ]Tw 5
1
Tw
[S6 ]Tw 6
Tw1 X2
= Tw1 X2 Tw .
Noting that Tw1 [Si ]Tw is the 44 matrix representation of the adjoint mapping
AdTw1 (Si ), it can be verified by calculation that
0 0
0 0
0 0 1 0
AdTw1 (S6 ) =
0 1
0 0
0 0
0 0
0 0 1 0
0 0 0 0
AdTw1 (S5 ) =
1 0 0 0
0 0 0 0
0 1 0 0
1
0 0 0
.
AdTw1 (S4 ) =
0
0 0 0
0
0 0 0
As long as the translational component of Tw1 X1 Tw is zero, the solution (4 , 5 , 6 )
can now be obtained from the following:
Rot(
z , 1 ) Rot(
y , 2 ) Rot(
x, 3 ) = R,
is the rotational component of Tw1 X1 Tw . The solution (4 , 5 , 6 )
where R
therefore corresponds to the ZY X Euler angles that parametrize R:

q
2 )
5 = atan2(
r31 , r1 12 + r21
4
atan2(
r21 , r11 )
atan2(
r32 , r33 ),
Recall that there are two solutions

where rij denotes the ij-th entry of R.
determined by the choice of sign for 5 .
The above derivation can be repeated for the case when 6 and 4 are
parallel, in which case the solutions (4 , 5 , 6 ) will now correspond to the ZYZ
Euler angles.
Like the earlier 6R PUMA-type arm, since there are up to two possible
solutions for 3 , and also for (1 , 2 ) and (4 , 5 , 6 ), for the generalized PUMAtype robot arm one can expect up to 2 2 2 = 8 possible inverse kinematic
solutions.
6.1.3
Stanford-Type Arms
If the elbow joint in a 6R PUMA-type arm is replaced by a prismatic joint

as shown in Figure 6.9, we then have a Stanford-type arm. Here we consider
180
Inverse Kinematics
Figure 6.9: The first three joints of a Stanford-type arm.

the inverse position kinematics for the arm of Figure 6.9; the inverse orientation
kinematics is identical to that for the PUMA-type arm and is not repeated here.
The first joint variable 1 an be found in a similar fashion to the PUMA-type
p
arm: 1 = tan1 ( pxy ) (provided that px and py are not both zero). 2 is then
found from Figure 6.9 to be
s
2 = tan1 ( )
r
where r2 = p2x + p2y and s = pz d1 . Similar to the PUMA-type arm, a second
solution for 1 , 2 is given by
1
2
py
)
px
py
= tan1 ( )
px
= + tan1 (
The translation distance d3 is found from the relation

(d3 + a2 )2 = r2 + s2
as
d3
=
=
p
r2 + s2
q
p2x + p2y + (pz d1 )2 a2
Ignoring the negative square root solution for d3 , we obtain two solutions to
the inverse position kinematics as long as the wrist center p does not intersect
the z-axis of the fixed frame. If there is an offset, then as in the case of the
PUMA-type arm there will be a left and right arm solution.
6.2. Numerical Inverse Kinematics
181
If the elbow joint in the generalized 6R PUMA-type arm is replaced by a

prismatic joint, the resulting arm is then referred to as a generalized Stanfordtype arm. The inverse kinematics proceeds in the same way as for the generalized PUMA-type arm; the only difference occurs in the first step (obtaining 3 ).
The screw vector for the third joint now becomes S3 = (0, v3 ), with kv3 k = 1,
and 3 is found by solving the equation
ke[S3 ]3 pk = c,
for some given p R3 and nonnegative positive scalar c. The above equation
reduces to solving the following quadratic in 3 :
32 + 2(pT v3 )3 + (kpk2 c2 ) = 0.
Imaginary, as well as negative solutions, naturally should be excluded.
6.2
Numerical Inverse Kinematics
In cases where the inverse kinematics equations do not admit analytic solutions,
one must resort to numerical methods. Even in cases where an analytic solution
exists, numerical methods are often used to improve the accuracy of these solutions. For example, in a generalized PUMA-type arm, the last three axes may
not exactly intersect at a common point, and the shoulder joint axes may not be
exactly orthogonal. In such cases, rather than throw away any analytic inverse
kinematic solutions that are available, such solutions can be used as the initial
point in an iterative procedure for numerically solving the inverse kinematics.
There exist a variety of iterative methods for finding the roots of a nonlinear
equation, and our aim is not to discuss these in detailany text on numerical
analysis will cover these methods in depthbut rather to develop ways in which
to transform the inverse kinematics equations such that they are amenable to
existing numerical methods.
However, it is useful to review one fundamental approach to nonlinear rootfinding, the Newton-Raphson method. Suppose we express the end-effector
frame using a coordinate vector x governed by the forward kinematics x = f (),
a nonlinear vector equation mapping the n joint coordinates to the m endeffector coordinates. Assume f : Rn Rm is twice differentiable, and let xd be
the desired end-effector coordinates. The goal is to find the joint coordinates d
such that
xd f (d ) = 0.
Solving this equation for d is a nonlinear root-finding problem.
Given an initial guess 0 which is close by the solution d , the kinematics
can be approximated by the Taylor expansion truncated at first-order

f
xd = f (d ) f (0 ) +
(d 0 ),
(6.12)
x 0 | {z }
| {z }
J(0 )
182
Inverse Kinematics
xd _ f()
1 d
Figure 6.10: The Newton-Raphson method for nonlinear root-finding.

where J(0 ) Rmn is the coordinate Jacobian evaluated at 0 . Assuming
J(0 ) is square (m = n) and invertible, we can solve for as
= (J(0 ))1 (xd f (0 )) .
(6.13)
If the kinematics were linear, then the new guess 1 = 0 + would exactly
satisfy xd = f (1 ). If not, the new guess 1 should be closer to the root than
0 , and the process is then repeated (Figure 6.10).
If J is not square, then J 1 does not exist. The Moore-Penrose pseudoinverse
J is used in place of J 1 , so Equation (6.13) becomes

= (J(0 )) (xd f (0 )) ,
(6.14)
where
J = J T (JJ T )1
if n > m and JJ T is invertible
J = (J T J)1 J T
if n < m and J T J is invertible.
If n < m, then there may not exist a exactly solving Equation (6.14). In
this case, the pseudoinverse returns the solution that best satisfies the equation
in the least-squares sense. If n > m, then the robot may be redundant, meaning
that there may exist an (n m)-dimensional set of solutions. Inthis case, the
pseudoinverse returns the solution that minimizes the two-norm T .
Equation (6.14) suggests the Newton-Raphson iterative algorithm for finding
d :
(i) Initialization: Given xd Rm and an initial guess 0 Rn . Set i = 0.
(ii) While kxd f (i )k > for some small do:
6.3. Inverse Velocity Kinematics
183
Set i+1 = i + (J(i )) (xd f (i )).

Increment i.
To modify this algorithm to work with a desired end-effector configuration
represented as Tsd SE(3) instead of as a coordinate vector xd , we can replace the coordinate Jacobian J with the end-effector body Jacobian Jb . Note,
however, that the vector xd f (i ), representing the direction from the current
guess (evaluated through the forward kinematics) to the desired end-effector
configuration, cannot simply be replaced with Tsd Tsb (i ), which is not even
an element of SE(3). Instead we should think of xd f (i ) as a velocity vector
which, if followed for unit time, would cause a motion from f (i ) to xd . Similarly, we should look for a body velocity Vb se(3) which, if followed for unit
time, would cause a motion from Tsb (i ) to the desired configuration Tsd .
To find this Vb , we first calculate the desired configuration represented in
the body frame,
1
Tbd (i ) = Tsb
(i )Tsd = Tbs (i )Tsd .
Then Vb is given by the matrix logarithm,
[Vb ] = log Tbd (i ).
This leads to the following inverse kinematics algorithm, analogous to the
coordinate vector algorithm:
(i) Initialization: Given Tsd and an initial guess 0 Rn . Set i = 0.
(ii) While kTsd Tsb (i )k > for some small do:

1
Set [Vb ] = log Tsb
(i )Tsd .
Set i+1 = i + (Jb (i )) Vb .
Increment i.
An equivalent form can be derived in the space frame, using the space Jacobian Js () and the space velocity Vs = [AdTsb ]Vb .
For this numerical inverse kinematics method to converge, the initial guess
0 must be sufficiently close to the solution d . This condition can be satisfied
by starting the robot from an initial home configuration where both the actual
end-effector configuration and the joint angles are known, and ensuring that
the requested end-effector position Tsd change slowly relative to the frequency
of the calculation of the inverse kinematics. Then, for the rest of the robots
run, the calculated d at the previous timestep serves as the initial guess 0 for
the new Tsd at the next timestep.
6.3
Inverse Velocity Kinematics
To control a robot to follow a desired end-effector trajectory Tsd (t), one solution
is to calculate the inverse kinematics d (kt) at each discrete time step k, then
184
Inverse Kinematics
control the joint velocities to be

= (d (kt) ((k 1)t))/t
during the time interval [(k 1)t, kt].
Another option is to calculate the joint velocities directly from the relationship J = Vd , where Vd and J are expressed with respect to the same frame,
without calculating the inverse kinematics:
= J Vd .
(6.15)
In this case, Vd should be chosen to keep the end-effector following Tsd (t).
In the case of a redundant robot with n > 6 joints, of the (n6)-dimensional
set of solutions satisfying Equation (6.15), the p
Moore-Penrose pseudoinverse
This is appealing; the
returns the minimizing the two-norm kk2 = T .

motion is the minimum joint velocity motion that achieves the desired endeffector velocity.
Another option is to give the individual joint velocities different weighting.
For example, as we will see later, the kinetic energy of a robot can be written
1 T
M (),
2
where M () is the configuration-dependent inertia matrix of the robot. In this
case, we might wish to choose the joint velocities to minimize the velocities of the joints moving a lot of mass, while still statisfying the desired endeffector velocity. To do this, we can choose to use the weighted Moore-Penrose
pseudoniverse
J = M J T (JM J T )1 ,
p
which yields a solution minimizing the weighted norm T M .
6.4
A Note on Closed Loops
A desired end-effector trajectory over a time-interval [0, tf ] is a closed loop if

Tsd (0) = Tsd (tf ). It should be noted that numerical methods for calculating
inverse kinematics for redundant robots, at either the configuration or velocity
levels, are likely to yield motions that are not closed loops in the joint space,
i.e., (0) 6= (tf ). If closed-loop motions in joint space are required, an extra
set of conditions on the inverse kinematics must be satisfied.
6.5
Summary
Given a spatial open chain with forward kinematics T (), Rn , in the

inverse kinematics problem one seeks to find, for some given X SE(3),
solutions that satisfy X = T (). Unlike the forward kinematics, the
6.6. Notes and References
185
inverse kinematics problem can possess multiple solutions, or no solutions

in the event that X lies outside the task space. For a spatial open chain,
n 6 typically leads to a finite number of inverse kinematic solutions,
while n > 6 leads to an infinite number of solutions.
The inverse kinematics can be solved analytically for a large class of open
chains that possess some degree of structure. One important class is the
generalized 6R PUMA-type arm; such an arm consists of a 3R orthogonal
axis wrist connected to a 2R orthogonal axis shoulder by an elbow joint.
Geometric algorithms have been developed for this class of arms that
exploits the product of exponentials forward kinematics representation.
Further assumptions on the joint axes, e.g., joint axes 2 and 3 are always
parallel, lead to simpler analytic forms for the inverse kinematics.
Another class of open chains that admit analytic inverse kinematic solutions are Stanford-type arms; these arms are obtained by replacing the
elbow joint in the generalized 6R PUMA-type arm by a prismatic joint.
Geometric inverse kinematic algorithms similar to those for PUMA-type
arms have also been developed.
Iterative numerical methods are used in cases where analytic inverse kinematics solutions are not available. These typically involve solving the inverse kinematics equations through an iterative procedure like the NewtonRaphson method, and require an initial point to be supplied. The performance of the iterative procedure depends to a large extent on the quality of
the initial point, and only one inverse kinematic solution is produced per
iteration. An iterative procedure based on the Jacobian of the forward
kinematics has been developed for general six degree-of-freedom spatial
open chains.
6.6
The inverse kinematics of the most general 6R open chain is known to have
up to 16 solutions; this result was first proved by Lee and Liang [15], and also
by Raghavan and Roth [33]. Iterative numerical procedures for finding all sixteen solutions of a general 6R open chain are reported in [18]. A summary
of inverse kinematics methods for kinematically redundant robot arms are discussed in [36]. The repeatability conditions for kinematically redundant inverse
kinematics schemes are examined in [35].
186
Inverse Kinematics
Figure 6.11: A 3R wrist.
6.7
Exercises
1. The 3R orthogonal axis wrist mechanism of Figure 6.11 is shown in its zero
position, with joint axes 1 and 3 collinear.
(a) Ignoring position and considering only the orientation of the end-effector
frame, find all kinematic singularities of the wrist mechanism.
(b) Given a desired wrist orientation R SO(3), derive an iterative numerical
procedure for solving its inverse kinematics.
(c) Assuming static equilibrium, suppose that at the zero position we wish to
generate a moment (0, 1, 0) (in fixed frame coordinates) at the end-effector.
What joint torques should be applied?
2. The 3R nonorthogonal chain of Figure 6.12 is shown in its zero position.
(a) Derive a numerical procedure for solving the inverse position kinematics
numerically; that is, given some end-effector position p as indicated in the figure,
find (1 , 2 , 3 ).
(b) Given an end-effector orientation R SO(3), find all inverse kinematic
solutions (1 , 2 , 3 ).
3. The RRP open chain of Figure 6.13 is shown in its zero position. Joint axes
1 and 2 intersect at the fixed frame origin, and the end-effector frame origin p
is located at (0, 1, 0) when the robot is is its zero position.
6.7. Exercises
187
Figure 6.12: A 3R nonorthogonal chain.
Figure 6.13: An RRP open chain.

(a) Suppose
1 = 0. Solve for 2 and 3 when the end-effector frame origin p is
at (6, 5, 3).
(b) If joint 1 is not fixed to zero but instead allowed to vary, find all inverse
kinematic solutions (1 , 2 , 3 ) for the same p given in part (a).
188
Inverse Kinematics
Figure 6.14: A 6R open chain of the elbow type.
Figure 6.15: An inverse elbow type robot arm.
4. Find the inverse kinematics solutions when tool frame {T } of the 6R open
chain of the elbow type shown in Figure 6.14 is set to {T 0 } as shown. The
orientation of {T } at the zero position is the same as that of the fixed frame
{S}, and T 0 is the result of a pure translation of T along the y-axis.
5. (a) Solve the inverse position kinematics (you do not need to solve the orientation kinematics) of the inverse elbow type robot arm shown in Figure 6.15.
(b) Determine the spatial Jacobian Js (theta) of the inverse elbow robot arm.
(c) Find as many kinematic singularities of the inverse elbow arm that you can,
and for each singularity, describe the directions in which the end-effector loses
degrees of freedom of motion.
Chapter 7
Kinematics of Closed
Chains
Any kinematic chain that contains one or more loops is called a closed chain.
Several examples of closed chains were encountered in Chapter 2, from the planar four-bar linkage to spatial mechanisms like the Stewart-Gough platform. In
this chapter we shall analyze the kinematics of closed chains, paying special attention to a class of closed chains that we shall refer to as parallel mechanisms;
these are closed chains consisting of a fixed and moving platform connected by
a set of legs; these legs are mostly open chains, but sometimes can themselves
be closed chains.
Figures 7.1-7.3 depict some well-known parallel mechanisms. The StewartGough Platform is a six degree of freedom mechanism, used widely as both
a motion simulator and six-axis force-torque sensor. It is typically realized
as either a 6 U P S or 6 SP S platform; note that the additional torsional
rotations of each of the six legs in the 6 SP S platform have no effect on the
moving platform. When used as a force-torque sensor, the six prismatic joints
experience internal linear forces whenever any external force is applied to the
moving platform; by measuring these internal linear forces one can estimate the
applied external force. The Delta robot is a three degree of freedom mechanism
that has the unusual feature of the moving platform always remaining parallel
to the fixed platform. Because the three actuators are all attached to the three
revolute joints of the fixed upper platform, the moving parts are relatively light;
this allows the Delta to achieve very fast motions. The Eclipse mechanism is
another six degree of freedom parallel mechanism whose moving platform is
capable of 90 orientations with respect to ground, and also of rotating 360
about the vertical axis.
Closed chains admit a much greater variety of designs than open chains,
and not surprisingly their kinematic analysis is considerably more complicated.
This can be traced to two defining features of closed chains: (i) the configuration space is curved (e.g., a multidimensional surface embedded in a higher-
189
190
Kinematics of Closed Chains
{T}
bi
Bi
di
{F}
ai
Ai
Figure 7.1: The Stewart-Gough platform.
Figure 7.2: The Delta robot.

dimensional vector space), and (ii) not all of the joints are actuated. The presence of such non-actuated, or passive joints, together with the fact that the
number of actuated joints may deliberately exceed the mechanisms kinematic
191
Figure 7.3: The Eclipse mechanism.

degrees of freedomsuch mechanisms are said to be redundantly actuated
makes not only the position and differential kinematics analysis more challenging, but also introduces new types of singularities not witnessed in open chains.
Recall also that for open chains, the kinematic analysis proceeds in a more
or less straightforward fashion with the formulation of the forward kinematics
(e.g., via the product of exponentials formalism) followed by that of the inverse
kinematics. For general closed chains it is usually difficult to obtain an explicit set of equations for the forward kinematics in the form X = T (), where
X SE(3) is the end-effector frame and Rn are the joint coordinates.
The most effective approaches for closed chain kinematic analysis are based on
a collection of tools and methodologies that exploit as much as possible any
kinematic symmetries and other special features of the mechanism.
For this reason we shall proceed in this chapter with a series of case studies involving some well-known parallel mechanisms, and eventually build up a
repetoire of kinematic analysis tools and methodologies that can be synthesized
to handle more general closed chains. We shall consider only parallel mechanisms that are exactly actuated, i.e., the number of actuated degrees of freedom
is equal to the mechanisms kinematic mobility. Methods for the forward and
inverse position kinematics of parallel mechanisms are discussed, followed by
the characterization and derivation of the constraint Jacobian, and the Jacobians of both the inverse and forward kinematics. The chapter concludes with
an examination of the various kinematic singularities that can arise in closed
chains.
192
Figure 7.4: A three degree-of-freedom 3 RP R planar parallel mechanism.
7.1
Inverse and Forward Kinematics
This section examines methods for the inverse and forward kinematics of closed
chains. Rather than attempt to develop a general methodology applicable to all
types of closed chains, we consider two case studies, the 3RP R planar parallel
mechanism, and its spatial counterpart, the 3 SP S Stewart-Gough platform.
The analysis of these two mechanisms draws upon some reduction techniques
that result in a reduced form of the governing kinematic equations. We briefly
describe how these methods can be generalized to the analysis of more general
parallel mechanisms.
7.1.1
3RPR Planar Parallel Mechanism
The first example we consider is the planar 3 RP R parallel mechanism shown

in Figure 7.4. It is easily verified from the planar version of Grueblers formula
that this mechanism has mobility three. Assign a fixed frame {s} and endeffector frame {b} as shown. Typically the three prismatic joints are actuated;
denote the lengths of each of the three legs by si , i = 1, 2, 3. The forward
kinematics problem is to determine, from given values of s = (s1 , s2 , s3 ), the
end-effector frames position and orientation.
Let p~ be the vector from the origin of the {s} frame to the origin of the {b}
frame. Let denote the angle measured from the x
axis of the {s} frame to
the {x} axis of the {b} frame. Further define the vectors ~ai , ~bi , d~i , i = 1, 2, 3 as
shown in the figure. From these definitions, clearly
d~i = p~ + ~bi ~ai ,
(7.1)
7.1. Inverse and Forward Kinematics
193
for i = 1, 2, 3. Let

px
py
aix
aiy
dix
diy
= d~i in {s} frame coordinates
bix
biy
= ~bi in {b} frame coordinates.
= p~ in {s} frame coordinates

= ~ai in {s} frame coordinates
Note that the vectors (aix , aiy ), (bix , biy ), i = 1, 2, 3 are constant, and that
with the exception of (bix , biy ), all other vectors are expressed in {s} frame
coordinates. To express Equation (7.1) in terms of {s} frame coordinates, it
is first necessary to find the {s} frame representation of the vector ~bi . This is
straightforward: defining

cos sin
Rsb =
,
sin cos
it now follows that

dix
px
cos sin
bix
aix
=
+
,
diy
py
sin cos
biy
aiy
for i = 1, 2, 3. Also, since s2i = d2ix + d2iy , we have
s2i
(px + bix cos biy sin aix )2

+(py + bix sin + biy cos aiy )2 ,
for i = 1, 2, 3.
Formulated as above, the inverse kinematics is trivial to compute: given
values for (px , py , ), the leg lengths (s1 , s2 , s3 ) can be directly calculated from
the above equations (negative values of si in most cases will not be physically
realizable, and can be ignored). The forward kinematics problem, in contrast, is
not trivial: here the objective is to determine, for given values of (s1 , s2 , s3 ), the
end-effector frames position and orientation (px , py , ). The following tangent
half-angle substitution, widely used in kinematic analysis, transforms the above
three equations into a system of polynomials in the newly defined scalar variable
t:
t
sin =
cos =
2
2t
1 + t2
1 t2
.
1 + t2
tan
194
After considerable algebraic manipulation, this system of polynomials can eventually be reduced to a single sixth-order polynomial in t, which effectively shows
that the 3 RP R mechanism may have up to six forward kinematics solutions
(showing that six real solutions are possible requires further verification, which
we do not pursue here).
7.1.2
Stewart-Gough Platform
We now examine the inverse and forward kinematics of the 6 SP S StewartGough platform of Figure 7.1. In this design, the fixed and moving platforms
are connected by six serial SP S structures, with the spherical joints passive,
and the prismatic joints actuated. The derivation of the kinematic equations
closely parallels that of our earlier planar 3 RP R mechanism. Let {s} and
{b} denote the fixed and end-effector frames, respectively, and let d~i be the
vector directed from joint Ai to joint Bi . Referring to Figure 7.1, we make the
following definitions:
p R3 = p~ in {s} frame coordinates;
ai R3 = ~ai in {s} frame coordinates;
bi R3 = ~bi in {b} frame coordinates;
di R3 = d~i in {s} frame coordinates.
R SO(3) = orientation of {b} as seen from {s}.
In order to derive the kinematic constraint equations, note that vectorially,
d~i = p~ + ~bi ~ai , i = 1, . . . , 6.
Writing the above equations explicitly in {s} frame coordinates,
di = p + Rbi ai , i = 1, . . . , 6.
Denoting the length of leg i by si , we have
s2i = dTi di = (p + Rbi ai )T (p + Rbi ai ),
for i = 1, . . . , 6. Observe that ai and bi as defined above are all known constant vectors. Having written the constraint equations in this form, the inverse
kinematics now becomes straightforward: given p and R, the six leg lengths si ,
i = 1, . . . , 6 can be evaluated directly from the above equations (negative values
of si in most cases will not be physically realizable, and can be ignored).
The forward kinematics is not as straightforward. Here we are given each of
the leg lengths si , i = 1, . . . , 6, and must solve for p R3 and R SO(3). The
six constraint equations, together with the rotation matrix constraint RT R = I,
constitute a set of twelve equations in twelve unknowns. Several methods exist
for finding all solutions to such a set of polynomial equations, e.g., methods
7.1. Inverse and Forward Kinematics
195
Figure 7.5: A general spatial parallel mechanism.

based on dialytic elimination, Grobner bases, etc. Of particular note is the work
of Raghavan and Roth [?], who show that there can be at most forty solutions to
the forward kinematics, and Husty [?], who develops a computational algorithm
for finding all forty solutions analytically.
7.1.3
General Parallel Mechanisms
For both the 3 RP R mechanism and Stewart-Gough Platform, we were able

to exploit certain features of the mechanism that resulted in a reduced set of
equations; for example, in the case of the Stewart-Gough Platform, the fact that
each of the legs can be modelled as straight lines considerably simplified the
analysis. In this brief section we consider the more general case where the legs
have the structure of an arbitrary open chain.
Consider such a parallel mechanism as shown in Figure 7.5; here the fixed
and moving platforms are connected by three open chains. Denote the forward
kinematics of the three chains by T1 (), T2 (), and T3 (), respectively, where
Rm , Rn , and Rp . The loop closure conditions can be written
T1 ()
T2 ()
(7.2)
T2 ()
T3 ().
(7.3)
Equation 7.2 and 7.3 each consists of 12 equations (9 for the rotation component
and 3 for the position component), 6 of which are independent (recall that the
nine equations for the rotation component can be reduced to a set of three
196
independent equations from the rotation matrix constraint, i.e., RT R = I);

there are thus 24 equations, 12 of which are independent, with n + m + p
unknown variables, and the mobility of the mechanism is d = 12 (n + m + p).
In the forward kinematics problem, given values for d of the joint variables
(, , ), Equations 7.2 and 7.2 can be solved for the remaining joint variables;
note that multiple solutions will be likely. Once the joint values for any one of
the open chain legs are known, the forward kinematics of that leg can then be
evaluated to determine the forward kinematics of the closed chain.
In the inverse kinematics problem, we are given the end-effector frame displacement T SE(3). Setting T = T1 = T2 = T3 , the objective is to solve
Equations 7.2 and 7.2 for all the joint variables (, , ). As hinted by the case
studies, for most parallel mechanisms there are often features of the mechanism
that can be exploited to eliminate some of these equations, and to simplify them
into a reduced form.
7.2
Differential Kinematics
We now consider the differential kinematics of parallel mechanisms. Unlike differential kinematics for open chains, in which the objective was to relate the
input joint velocities to the spatial velocity of the end-effector frame, the analysis for closed chains is complicated by the fact that not all of the joints are
actuated. Only the actuated joints can be prescribed input velocities; the velocities of the remaining passive joints must then be determined from the kinematic
constraint equations. These passive joint velocities are usually required to eventually determine the spatial velocity of the closed chains end-effector frame.
For open chains, the Jacobian of the forward kinematics played a defining
role in both velocity and static analysis. For closed chains, in addition to the
forward kinematics Jacobian, the Jacobian defined by the kinematic constraint
equationsfor this reason we refer to this latter Jacobian as the constraint
Jacobianalso plays a central role in velocity and static analysis. Much like
the case for the inverse and forward kinematic analysis of parallel mechanisms,
often there are features of the mechanism that can be exploited to simplify
and reduce the procedure for obtaining the Jacobians. We therefore begin with
a case study of the Stewart-Gough platform, and show that the Jacobian of
the inverse kinematics can be obtained straightforwardly via static analysis.
Velocity analysis for more general parallel mechanisms is then detailed.
7.2.1
Stewart-Gough Platform
Earlier we saw that the inverse kinematics for the Stewart-Gough platform can
be solved analytically; that is, given the end-effector frame orientation R
SO(3) and position p R3 , the leg lengths s R6 can be obtained analytically in
the functional form s = g(R, p). In principle this equation can be differentiated
and manipulated to eventually produce a differential version, e.g.,
s = G(R, p)Vs ,
(7.4)
7.2. Differential Kinematics
197
where s R6 denotes the leg velocities, Vs R6 is the end-effectors spatial

velocity in fixed frame coordinates, and G(R, p) R66 is the Jacobian of the
inverse kinematics. This derivation, however, will likely involve considerable
algebraic manipulation.
Here we take a different approach based on static analysis. Based on the same
virtual work considerations that were used to determine the static relationship
for open chains, the static relationship for closed chains (expressed in the fixed
frame) is also given by = JsT Fs , where is the vector of input joint torques,
Fs is (the fixed frame representation of) the external spatial force applied at the
end-effector frame, and Js denotes the space Jacobian of the forward kinematics.
For the Stewart-Gough platform, note that the only forces being applied to
the moving platform occur at the spherical joints. Let
fi = i i
be the three-dimensional linear force applied by leg i, where i R3 is a unit
vector indicating the direction of the applied force, and i R is the magnitude
of the linear force; we emphasize that fi is expressed in terms of the fixed frame
coordinates. The moment generated by fi , denoted mi , is then given by
mi = ri fi ,
where ri R3 denotes the vector from the fixed frame origin to the point of
application of the force (spherical joint i in this case); again, both ri and mi
are expressed in fixed frame coordinates. It is not too difficult to see that this
same moment can also be expressed as
mi = qi fi ,
where qi R3 denotes the vector from the fixed frame origin to the base of leg
i, i.e., the joint connecting leg i to the fixed base. Expressing the moment as
qi fi is preferred, since qi as defined is constant.
Combining fi and mi into a six-dimensional spatial force Fi = (mi , fi ), the
resultant spatial force Fs on the moving platform is then given by

6
6
X
X
ri fi
Fs =
Fi =
i
i
i=1
i=1

1
1 q1 6 q6 .
=
.. .
1
6
6
Since earlier we asserted that the static relationship for the Stewart-Gough
platform is also of the form = JsT Fs , based on the previous derivation we can
conclude that the inverse Jacobian Js1 (or equivalently, the Jacobian of the
inverse kinematics) is given by

T
1 q1 6 q6
Js1 =
.
1
198
7.2.2
General Parallel Mechanisms
Because of its kinematic structure, the Stewart-Gough platform lends itself particularly well to a static analysis, as each of the six joint forces are directed along
their respective legs. The Jacobian (or more precisely, the inverse Jacobian) can
therefore be derived in terms of the screws associated with each line. In this
section we consider more general parallel mechanisms where a static analysis is
not as straightforward. Using the previous three-legged, three degree-of-freedom
spatial parallel mechanism of Figure 7.5 as an example, we illustrate a general
procedure for determining the forward kinematics Jacobian; generalizing this
method to arbitrary parallel mechanisms should be completely straightforward.
The mechanism of Figure 7.5 consists of two platforms connected by three
legs, with each leg a five degree of freedom open chain. For the given fixed
and end-effector frames as indicated in the figure, we first write the forward
kinematics for the three chains as follows:
T1 (1 , 2 , . . . , 5 )
= e[S1 ]1 e[S2 ]2 e[S5 ]5 M1
T2 (1 , 2 , . . . , 5 )
= e[P1 ]1 e[P2 ]2 e[P5 ]5 M2
T3 (1 , 2 , . . . , 5 )
= e[Q1 ]1 e[Q2 ]2 e[Q5 ]5 M3 .
The kinematic loop constraints can be expressed as

T1 ()
T2 ()
(7.5)
T2 ()
T3 ().
(7.6)
Taking right differentials of both sides of the above two equations, we have
T1 T11
T2 T 1
2
= T2 T21
= T3 T 1 .
3
(7.7)
(7.8)
Since Ti Ti1 = [Vi ], where Vi is the spatial velocity of chain is end-effector

frame, the above identities can also be expressed in terms of the forward kinematics Jacobian for each chain:
J1 ()
J2 ()
= J2 ()
= J3 (),
(7.9)
(7.10)
which can also be rearranged as

J1 () J2 ()
0
0
J2 () J3 ()
= 0.
(7.11)
At this point we now rearrange the fifteen joints into those that are actuated,
and those that are passive. Let us assume without loss of generality that the
three actuated joints are (1 , 1 , 1 ). Define the vector of actuated joints qa R3
7.2. Differential Kinematics
199
and passive joints qp R12 as
1
qa = 1 ,
1
qp = ... ,
5
and q = (qa , qp ) R15 . Equation (7.11) can now be rearranged into the form

qa
Ha (q) Hp (q)
= 0,
(7.12)
qp
or equivalently
Ha qa + Hp qp = 0,
where Ha R
123
and Hp R
1212
(7.13)
. If Hp is invertible, we have
qp = Hp1 Ha qa .
(7.14)
Assuming Hp is invertible, once the velocities of the actuated joints are given,
the velocities of the remaining passive joints can be obtained uniquely via Equation 7.14.
It still remains to derive the forward kinematics Jacobian with respect to
the actuated joints, i.e., to find Ja (q) R63 satisfying Vs = Ja (q)qa , where Vs
is the spatial velocity of the end-effector frame. For this purpose we can use the
forward kinematics for any of the three open chains; for example, in terms of
chain 1, J1 () = Vs , and from Equation (7.14) we can write
2
3
4
5
g2T qa
(7.15)
g3T qa
g4T qa
g5T qa
(7.16)
=
=
(7.17)
(7.18)
where each gi (q) R , i = 2, . . . , 5, can be obtained from Equation (7.14).

Defining the row vector eT1 = (1, 0, 0), the differential forward kinematics for
chain 1 can now be written
T
e1
g2T 1
T

Vs = J1 ()
(7.19)
g3T 1 .
g4 1
g5T
Since we are seeking Ja (q) in Vs = Ja (q)qa , and qaT
above it now follows that
eT1
g2 (q)T
T
Ja (q) = J1 (q1 , . . . , q5 )
g3 (q)T
g4 (q)
g5 (q)T
= (1 , 1 , 1 ), from the
(7.20)
200
Figure 7.6: A planar four-bar linkage and its joint configuration space.
Figure 7.7: A planar five-bar linkage.

The above could also have been derived using either chain 2 or chain 3.
Given values for the actuated joints qa , it still remains to solve for the passive
joints qp from the loop constraint equations. Eliminating as many elements
of qp a priori will obviously simplify the task. The second point to note is
that Hp (q) may become singular, in which case qp cannot be obtained from
qa . Configurations in which Hp (q) becomes singular correspond to actuator
singularities, which are discussed in the next section.
7.3. Singularities
201
Figure 7.8: Configuration space singularities of the planar five-bar linkage.
7.3
Singularities
In this final section we shall examine the fundamental properties of closed chain
singularities. Characterizing the singularities of closed chains involves many
more subtleties than for open chains. Rather than attempt any such comprehensive classification for general closed chains, we instead choose to highlight
the essential features of closed chain singularities via two planar examples: a
four-bar linkage (see Figure 7.6), and a five-bar linkage (see Figure 7.7). The
examples should also make clear how our approach to singularity analysis can
be generalized to more complex closed chains.
We begin with the four-bar linkage. Recall that its configuration space is a
curve embedded in a four-dimensional ambient space; even without appealing
to equations, one can see that the allowable joint values for (, ) of the four-bar
form a curve of the type shown in Figure 7.6. In terms of the input and output
angles and , the kinematic loop constraint equations can be expressed as
!
1
1
p
= tan
cos
,
(7.21)
2 + 2
where
2L3 L4 2L1 L3 cos
= 2L1 L3 sin
L22
L24
L23
(7.22)
(7.23)
L21
+ 2L1 L4 cos .
(7.24)
Obviously the existence and uniqueness of solutions depends on the link lengths
L1 , . . . , L4 ; in particular, a solution fails to exist if 2 2 + 2 . The figure
depicts the input-output graph for the choice of link lengths L1 = 4, L2 = 4,
L3 = 3, L4 = 2; in this case both and can range from 0 to 2.
One of the striking features of this graph is the bifurcation point P as
indicated in the figure. Here two branches of the curve meet, resulting in a
self-intersection of the curve with itself. If the four-bar is in the configuration
indicated by P , it has the choice of following either branch. At no other point
in the four-bars joint configuration space does such a phenomenon occur.
202
Figure 7.9: Actuator singularities of the planar five-bar linkage: the left is
nondegenerate, while the right is degenerate.
.
We now turn to the five-bar linkage. The kinematic loop constraint equations
can be written
L1 cos 1 + . . . + L4 cos(1 + 2 + 3 + 4 )
= L5
(7.25)
L1 sin 1 + . . . + L4 sin(1 + 2 + 3 + 4 )
(7.26)
where we have eliminated joint variable 5 a priori from the loop closure conditions. Writing these two equations in the form f (1 , . . . , 4 ) = 0, where
f : R4 R2 , the configuration space can be regarded as a two-dimensional surface in R4 . Like the bifurcation point in the four-bar linkage, self-intersections
of the surface can also occur. At such points the constraint Jacobian loses rank;
for the five-bar, any point at which

f
rank
() < 2
(7.27)
corresponds to what we call a configuration space singularity. Figure 7.8

illustrates the possible configuration space singularities of the five-bar. Notice
that thus far we have made no mention of which joints of the five-bar are actuated, or where the end-effector frame is placed; it is worth emphasizing that
the notion of configuration space singularity is completely independent of the
choice of actuated joints, or the end-effector frame.
We now consider the case when two joints of the five-bar are actuated. Referring to Figure 7.9, the actuated joints are indicated by filled circles. Under
normal operating conditions, the motions of the actuated joints can be independently controlled. Alternatively, locking the actuated joints should immobilize
the five-bar and turn it into a rigid structure.
For the nondegenerate actuator singularity shown on the left, rotating
the two actuated joints in opposite directions will clearly have catastrophic
consequences of the mechanism. For the degenerate actuator singularity
7.3. Singularities
203
shown on the right, we have the opposite case: even when the actuated joints
are locked in place, the inner two links are free to rotate.
The reason for classifying these singularities as actuator singularities is
that, by relocating the actuators to a different set of joints, such singularities can
be eliminated. For both the degenerate and nondegenerate actuator singularities
of the five-bar, relocating one of the actuators to one of the three passive joints
eliminates the singularity.
Intuitively visualizing the actuator singularities of the planar five-bar is
straightforward enough, but for more complex spaatial closed chains this may
be difficult. Actuator singularities can be characterized mathematically by the
rank of the constraint Jacobian. As before, write the kinematic loop constraints
in differential form:

qa
Ha (q) Hp (q)
= 0,
(7.28)
qp
where qa Ra is the vector of actuated joints, and qp Rp is the vector of
passive joints. It follows that the matrix

H(q) = Ha (q) Hp (q) Rp(a+p) ,
(7.29)
and that Hp (q) is a p p matrix.
With the above definitions, we have the following:
If rank Hp (q) < p, then q is an actuator singularity. Distinguishing between degenerate and nondegenerate singularities involves additional
mathematical subtleties, and relies on second-order derivative information
that we shall not pursue further here.
If rank H(q) < p, then q is a configuration space singularity. Note
that under this condition Hp (q) is also singular (the converse is not true,
however). The configuration space singularities can thus be regarded as
the intersection of all possible actuator singularities obtained over all possible combinations of actuated joints.
The final class of singularities involves the choice of an end-effector frame.
For the five-bar, we ignore the orientation of the end-effector frame, and focus
exclusively on its x-y location. Figure 7.10 shows the five-bar in an end-effector
singularity for the given choice of end-effector location. Note that velocities
along the indicated line are not possible in this configuration, similar to the case
for singularities for open chains. Note that end-effector singularities are entirely
independent of the choice of actuated joints (note that it was not necessary to
specify which, or even how many, of the joints are actuated).
End-effector singularities can be mathematically characterized as follows.
Choose any valid set of actuated joints qa such that the mechanism is not at an
actuator singularity. Write the forward kinematics in the form
f (qa ) = T
(7.30)
where T denotes the end-effector frame. One can then check for rank deficiencies
in the Jacobian of f , as was done for open chains, to determine the presence of
an end-effector singularity.
204
Figure 7.10: End-effector singularities of the planar five-bar linkage.

.
7.4
Summary
Any kinematic chain that contains one or more loops is called a closed
chain. Parallel mechanisms are a class of closed chain that are characterized by two platformsone moving and one stationaryconnected
by several legs; the legs are typically open chains, but can themselves be
closed chains. Compared to open chains, the kinematic analysis of closed
chains is complicated by the fact that the configuration space is often
curved, and only a subset of the joints are actuated.
For a parallel mechanism whose actuated degrees of freedom equals its
mobility, the inverse kinematics problem involves finding, from the given
position and orientation of the moving platform, the values of all the
actuated joints. For well-known parallel mechanisms like the planar 3
RP R and spatial Stewart-Gough platform, the inverse kinematics admits
unique solutions.
For a parallel mechanism whose actuated degrees of freedom equals its
mobility, the forward kinematics problem involves finding, given values for
all the actuated joints, the position and orientation of the moving platform.
For well-known parallel mechanisms like the 3 RP R and the spatial
Stewart-Gough platform, the forward kinematics usually admits multiple
solutions. In the case of the most general Stewart-Gough platform, a
maximum of 40 solutions are possible.
The differential kinematics of a closed chain relates velocities of the actuated joints to the linear and angular velocities of the moving platform. For
a closed chain consisting of n one degree of freedom joints, whose actuated
degrees of freedom also equals its mobility m, let a Rm denote the vector of actuated joints, and p Rnm denote the vector of passive joints.
7.4. Summary
205
The kinematic loop closure constraints are described by an equation of the

form h(a , p ) = 0, where g : Rn Rnm . The forward kinematics can be
expressed in the form f (a ) = T , where f : Rm SE(3). The differential
kinematics then involves derivatives of both f and g with respect to a
and p . For platforms like the Stewart-Gough platorm, the differential
kinematics can also be obtained from a static analysis, by exploiting the
fact that just as for closed chains, the external forces F at the end-effector
are related to the joint torques by = J T F.
Singularities for closed chains can be classifed into three types: (i) configuration space singularities occur at, e.g., self-intersections of the configuration space surface (or bifurcation points in the event that the configuration
space is a curve); (ii) nondegenerate actuator singularities when the actuated joints cannot be independently actuated, while degenerate actuator
singularities are characterized by the mechanism failing to become a rigid
structure even when all the actuated joints are locked in place; (iii) endeffector singularities occur when the end-effector loses one or more degrees
of freedom of mobility. Configuration space singularities are independent
of choice of actuated joints, while actuator singularities depend on which
joints are actuated. End-effector singularities depend on where the endeffector frame is placed, but do not depend on the choice of actuated
joints.
206
Figure 7.11: Two cooperating six degree of freedom arms grasping an object.
Figure 7.12: A 3 RP R planar parallel mechanism.
7.5
Exercises
1. Two six degree of freedom arms cooperate to move the disc as shown in
Figure 7.11. Given the position and orientation of the disc, how many inverse
kinematics solutions exist?
2. Consider the 3 RP R planar parallel mechanism of Figure 7.12, in which
the prismatic joints are actuated. Define OAi = ai R3 with respect to the
7.5. Exercises
207
Figure 7.13: A Delta robot.
fixed frame and P Bi = bi R3 with respect to the moving platform frame.

(a) Solve the inverse kinematics.
(b) Derive a procedure to solve the forward kinematics.
(c) Is the configuration shown an end-effector singularity? Explain your answer by examining the inverse kinematics Jacobian. Is this also an actuator
singularity?
3. The Delta robot of Figure 7.13 consists of a fixed base connected to a moving platform by three arms. Each arm consists of an upper armmade up of a
spatial parallelogram connected by spherical joints at the ends of each rodand
a lower arm connected to the fixed base by a revolute joint i , connected or
thogonally to the upper arm rod. OAi = ri , ri i = z, Ai Bi = ai , Bi Ci = bi ,
i ai , P Ci = hi are defined with respect to the moving platform frame. Derive

step-by-step procedures for solving the following:
(a) Solve the forward kinematics.
(b) Solve the inverse kinematics.
(c) Find the Jacobian Ja .
208
Chapter 8
Dynamics of Open Chains

In this chapter we study once again the motion of open chain robots, but this
time taking into account the forces and torques that cause it; this is the subject
of robot dynamics. The associated dynamic equationsalso referred to as
the equations of motionare a set of second-order differential equations of
the form
= M () + b(, ),
(8.1)
where Rn is the vector of joint variables, Rn is the vector of joint forces
and torques, M () Rnn is a symmetric and invertible matrix called the mass
Rn are bias forces that lump together centrifugal and
matrix, and b(, )
One
Coriolis, gravity, friction and other force terms that depend on and .
should not be deceived by the apparent simplicity of these equations; even for
simple open chains, e.g., those with joint axes either orthogonal or parallel to
can be extraordinarily complex.
each other, M () and b(, )
Just as a distinction was made between a robots forward and inverse kinematics, it is also customary to distinguish between a robots forward and inverse dynamics. From the perspective of generating and simulating entire motion trajectories, it is useful to regard the robot dynamics as an input-output
system, in which the inputs are torque trajectories (t), and the outputs are
joint trajectories (t). From this perspective, in the case of forward dynamics
the objective is to determine, from a given input joint torque trajectory (t)
the output joint trajectory
and appropriate boundary conditions on and ,
(t); this is usually done by numerically integrating Equation (8.1). In the case
of inverse dynamics, the objective is to determine the joint torque trajectory
(t) that generates some desired joint motion trajectory (t).
Slight variations in these interpretations of the forward and inverse dynamics
are possible. For the inverse dynamics, observe that the velocity and acceleration can be obtained by taking derivatives of the desired joint trajectory (t).
)
at time t, the joint torques can be obtained just
Thus, given values for (, ,
by algebraic evaluation of the right-hand side of (8.1). This evaluation is also
commonly referred to as the inverse dynamics. In the case of forward dynamics,
209
210
since M () is always invertible, Equation (8.1) can be rewritten

.
= M 1 () b(, )
(8.2)
This evaluation of from given values for , , and is also often referred
to as the forward dynamics. While this interpretation may seem somewhat
different from the previous one, in fact it is not: given an input torque trajectory
(t) together with initial values for and at t = t0 , forward integration of
Equation (8.2) from t = t0 then produces the complete output trajectory (t).
A robots dynamic equations are typically derived in one of two ways: by a
direct application of Newtons and Eulers dynamic equations for a rigid body
(often called the Newton-Euler formulation), or from the Lagrangian dynamics formulation. The Lagrangian formalism is conceptually elegant and
quite effective for robots with simple structures, e.g., with three or fewer degrees of freedom. However, the calculations can quickly become intractable for
robots with more degrees of freedom. For general open chains, the Newton-Euler
formulation leads to efficient recursive algorithms for both the inverse and forward dynamics that can also be assembled into closed-form analytic expressions
for, e.g., the mass matrix M () and other terms in the dynamics equation (8.1).
In this chapter we study both the Lagrangian and Newton-Euler dynamics
formulations for an open chain robot. We conclude with a formulation of the
dynamics in task space coordinates, or operational space dynamics.
8.1
8.1.1
Lagrangian Formulation
Basic Concepts and Motivating Example
The first step in the Lagrangian formulation of dynamics is to choose a set

of independent coordinates q Rn that describes the systems configuration,
similar to what was done in the analysis of a robots configuration space. The
coordinates q are called generalized coordinates. Once generalized coordinates have been chosen, these then define another set of coordinates f Rn
that are dual to q, called generalized forces. f and q are dual to each other
in the sense that their inner product f T q corresponds to work (equivalently,
f T q corresponds to power). A Lagrangian function L(q, q)
is then defined as
the overall systems kinetic energy minus the potential energy. The equations
of motion can now be expressed in terms of the Lagrangian as follows:
f=
d L L
,
dt q
q
(8.3)
These equations are also referred to as the Euler-Lagrange equations with

external forces.1
We illustrate the Lagrangian dynamics formulation through two examples.
In the first example, consider a particle of mass m constrained to move on
1 The
external force f is zero in the standard form of the Euler-Lagrange equations.
8.1. Lagrangian Formulation
211
a vertical line. The particles configuration space is this vertical line, and a
natural choice for generalized coordinate is the height of the particle, which we
denote by the scalar variable x R. Suppose the gravitational force mg acts
downward, and an external force f is applied upward. By Newtons second law,
the equation of motion for the particle is
f mg = m
x.
(8.4)
We now apply the Lagrangian formalism to this particle. The kinetic energy is
mx 2 /2, the potential energy is mgx, and the Lagrangian is
L(x, x)
=
1 2
mx mgx.
2
(8.5)
The equations of motion are then given by

f=
d L L
= m
x + mg,
dt x
x
(8.6)
which matches Equation (8.4).

We now derive the dynamic equations for a planar 2R open chain moving in
the presence of gravity. The chain moves in the x-y plane, with gravity acting
in the y direction. Before the dynamics can be derived, the mass and inertial
properties of all the links must be specified. To keep things simple the two links
are modeled as point masses m1 and m2 concentrated at the ends of each link.
The position and velocity of the mass of link 1 are then given by

x1
L1 cos 1
=
y1
L1 sin 1

x 1
L1 sin 1
=
1 ,
y 1
L1 cos 1
while that of the link 2 mass are given by

x2
L1 cos 1 + L2 cos(1 + 2 )
=
y2
L1 sin 1 + L2 sin(1 + 2 )

x 2
L1 sin 1 L2 sin(1 + 2 ) L2 sin(1 + 2 )
=
y 2
L1 cos 1 + L2 cos(1 + 2 )
L2 cos(1 + 2 )
1
2

.
Choose the joint coordinates = (1 , 2 ) to be the generalized coordinates. The

generalized forces = (1 , 2 ) then correspond to joint torques (since T
is of the form
must correspond to work). The Lagrangian L(, )
=
L(, )
2
X
i=1
Ki Pi ,
(8.7)
212
where the link kinetic energy terms K1 and K2 are

K1
K2
=
=
1
1
m1 (x 21 + y 12 ) = m1 L21 12
2
2
1
2
2
m2 (x + y 2 )
2 2

m2
(L21 + 2L1 L2 cos 2 + L22 )12 + 2(L22 + L1 L2 cos 2 )1 2 + L22 22 ,
2
and the link potential energy terms P1 and P2 are

P1
m1 gL1 sin 1
P2
m2 g(L1 sin 1 + L2 sin(1 + 2 )).
The Euler-Lagrange equations (8.3) for this example are of the form
i =
d L
L
, i = 1, 2.
dt i
i
(8.8)
The dynamic equations for the 2R planar chain follow from explicit evaluation
of the right-hand side of (8.8) (we omit the detailed calculations, which are
straightforward but tedious):

1 = (m1 + m2 )L2 + m2 (2L1 L2 cos 2 + L2 ) 1
1
+m2 (L1 L2 cos 2 +

2
L22 )2
2m2 L1 L2 1 2 sin 2 m2 L1 L2 22 sin 2
+(m1 + m2 )L1 g cos 1 + m2 gL2 cos(1 + 2 )

m2 (L1 L2 cos 2 + L2 )1 + m2 L2 2 + m2 L1 L2 2 sin 2
2
+m2 gL2 cos(1 + 2 ).

In the Lagrangian formulation of dynamics, once a set of generalized coordinates has been chosen, it is conceptually straightforward to formulate the
Lagrangian, and from there to arrive at the dynamic equations by taking partial
derivatives of the Lagrangian. In practice, however, the calculations can very
quickly become intractable, especially as the degrees of freedom increase.
8.1.2
General Formulation
We now describe the Lagrangian dynamics formulation for general n-link open
chains. The first step is to select a set of generalized coordinates Rn for
the configuration space of the system. For open chains all of whose joints are
actuated, it is convenient and always possible to choose to be the vector of
joint values. The generalized forces will then be denoted Rn . If i is a
revolute joint i will correspond to a torque, while if i is a prismatic joint i
will correspond to a force.
Once has been chosen and the generalized forces identified, the next step
as
is to formulate the Lagrangian L(, )
= K(, )
P(),
L(, )
(8.9)
8.2. Dynamics of a Single Rigid Body
213
is the kinetic energy and P() the potential energy of the overall
where K(, )
system. For rigid-link robots the kinetic energy can always be written in the
form
n
n
1 XX
1
K() =
mij ()i j = T M (),
(8.10)
2 i=1 j=1
2
where mij () is the (i, j) element of the n n mass matrix M (); a constructive
proof of this assertion is provided when we examine the Newton-Euler formulation in the next section. The dynamic equations are analytically obtained by
evaluating the right-hand side of
i =
L
d L
, i = 1, . . . , n.
dt i
i
(8.11)
With the kinetic energy expressed as in Equation (8.10), the dynamics can be
explicitly written as
i =
n
X
j=1
mij ()j +
n
n X
X
j=1 k=1
P
ijk ()j k +
, i = 1, . . . , n,
i
(8.12)
where the ijk (), known as the Christoffel symbols of the first kind, are
defined as

1 mij
mik
mjk
ijk () =
+
.
(8.13)
2 k
j
i
The Lagrangian formulation has traditionally been regarded as the most direct way of obtaining a set of closed-form analytical equations for the dynamics.
For open chain robots not only is this no longer true, but the formula for ijk ()
above and our earlier examples offer a strong hint of how intractable the calculations can become, especially for robots with higher degrees of freedom. The
Newton-Euler formulation on the other hand allows us to bypass the evaluation
of these partial derivatives. However, as we show later, the Lagrangian formulation offers important insights into the structure of the dynamics equations,
especially in the development of stable robot control laws.
8.2
8.2.1
Dynamics of a Single Rigid Body

Classical Formulation
We now consider the dynamic equations for a single rigid body. In most treatments of rigid body dynamics one begins with the equations of motion for a
single particle of mass m (essentially, f = ma), which is then generalized to a
system of particles. A rigid body is then assumed to be a system consisting of
an infinite numer of particles, with the constraint that the distances between
particles are always preserved (ensuring that the body is a rigid body). The
equations of motion for a system of particles are then applied to this infinite
collection of particles, resulting in the equations of motion for a rigid body.
214
In this section we shall omit the single particle dynamics formulation and
proceed directly to the single rigid body case. Suppose a rigid body of mass m
has a reference frame {b} with axes {
xb , y
b , zb } attached to its center of mass.
As the rigid body moves, the body frame also moves with linear velocity v and
angular velocity w. Further assume that the rigid body is subject to an external
force f. The external moment m generated by f with respect to the center of
mass is then m = r f, where r is the vector from the center of mass to the
point on the body at which f is applied. Let h denote the angular momentum
vector about the center of mass (well explain shortly how to calculate h). The
dynamic equations for the rigid body are then given by
f
d
d
(mv) = m v
dt
dt
d
h.
dt
(8.14)
(8.15)
We now express these dynamic equations explicitly in frame {b} coordinates.

First express the angular and linear velocity in {b}-frame coordinates by
w
= x x
b + y y
b + z zb
= vx x
b + vy y
b + vz zb ,
with the column vectors b = (x , y , z )T , vb = (vx , vy , vz )T defined accordingly. The linear acceleration a is then
a=
d
v = (v x x
b + v y y
b + v z zb ) + vx x
b + vy y
b + vz z b .
dt
(8.16)
Substituting the rotating frame identities x

b = w x
b , y
b = w y
b , z b = w zb
into (8.16) leads to
a = (v x x
b + v y y
b + v z zb ) + w v.
The {b}-frame vector representation ab R3 of the linear acceleration is therefore
ab = v b + (b vb ),
where v b = (v x , v y , v z )T . Equation (8.14) expressed in {b}-frame coordinates
now becomes
fb = m(v b + b vb ).
(8.17)
We next express the angular momentum h in {b}-frame coordinates. When
the {b} frame is attached to the bodys center of mass as we have done here,
the angular momentum assumes a particularly simple form. First, the 3 3
rotational inertia matrix of the rigid body is required; this can be obtained by
imagining the rigid body as a collection of an infinite number of particles of mass
mi , each with coordinates (xi , yi , zi ) with respect to the {b} frame. Denoting
215
the rotational inertia matrix by Ib R33 , Ib is obtained via the following

summation over all particles constituting the rigid body:
P
P
P
2
2
mP
mi xi yi
P mi xi zi
i (yi + zi ) P
2
2
mP
mi yi zi
Ib = P mi xi yi
i (xi + zi ) P
mi xi zi
mi yi zi
mi (x2i + yi2 )
Ixx Ixy Ixz

= Ixy Iyy Iyz .
Ixz Iyz Izz
The matrix Ib defined in this way is constant, symmetric, and positive-definite.
In the limit as the number of particles becomes infinite, the summations can be
replaced by volume integrals over the body B, with the particle masses mi now
replaced by a mass density function (x, y, z):
Z Z Z
Ixx =
(y 2 + z 2 )(x, y, z) dx dy dz
Z Z ZB
Iyy =
(x2 + z 2 )(x, y, z) dx dy dz
B
Z Z Z
Izz =
(x2 + y 2 )(x, y, z) dx dy dz
B
Z Z Z
Ixy = Iyx =
xy(x, y, z) dx dy dz
B
Z Z Z
Ixz = Izx =
xz(x, y, z) dx dy dz
Z Z ZB
Iyz = Izy =
yz(x, y, z) dx dy dz.
B
If the body has a uniform mass density throughout, Ib is then determined

exclusively by the shape of the rigid body. The calculation of Ib for some
standard link shapes are covered in the exercises at the end of this chapter.
Express h in terms of the unit axes of the {b} frame as
h = hx x
b + hy y
b + hz zb ,
T
(8.18)
and define hb = (hx , hy , hz ) R . hb is then obtained as

hb = Ib b .
(8.19)
Since Equation (8.15) calls for the derivative of h, differentiating (8.18) leads to
d
h = (h x x
b + h y y
b + h z zb ) + w h.
dt
The moment equation (8.15) expressed in {b} frame coordinates thus becomes
mb = Ib b + b Ib b .
(8.20)
where mb R3 is the moment vector m in frame {b} coordinates. Equations (8.17) and (8.20) together constitute the dynamic equations of motion for
the rigid body.
216
8.2.2
Twist-Wrench Formulation
Equations (8.17) and (8.20) can be written in the following combined form:

mb
Ib 0
b
[b ]
0
Ib 0
b
=
+
. (8.21)
fb
0 mI
v b
0
[b ]
0 mI
vb
With the benefit of hindsight, and also making use of the fact that [v]v = vv =
0 and [v]T = [v], we write (8.21) in the following equivalent form:

mb
Ib 0
b
[b ] [vb ]
Ib 0
b
=
+
fb
0 mI
v b
0
[b ]
0 mI
vb

T

Ib 0
b
[b ]
0
Ib 0
b
=
(8.22)
.
0 mI
v b
[vb ] [b ]
0 mI
vb
Written in this way, each of the terms can now be identified with sixdimensional spatial quantities as follows:
(i) (b , vb ) and (mb , fb ) can be respectively identified with the spatial velocity
(or twist) Vb and spatial force (or wrench) Fb :

b
mb
Vb =
, Fb =
.
(8.23)
vb
fb
(ii) The spatial inertia matrix Gb R66 is defined as follows:

Ib 0
Gb =
,
0 mI
(8.24)
where I denotes the 33 identity matrix. Note as an aside that the kinetic
energy of the rigid body can be expressed in terms of the spatial inertia
matrix as
Kinetic Energy =
1
1
1 T
Ib b + mvbT vb = VbT Gb Vb .
2 b
2
2
(iii) The spatial momentum Pb R6 is defined as

Ib b
Ib 0
b
Pb =
=
= Gb Vb .
mvb
0 mI
vb
(8.25)
(8.26)
Observe that the Pb term in Equation (8.22) is left-multiplied by the matrix

T
[b ]
0
.
(8.27)
[vb ] [b ]
We now explain the origin and geometric significance of this matrix. First,
recall that the cross product of two vectors 1 , 2 R3 can be calculated using
skew-symmetric matrix notation as follows:
[1 2 ] = [1 ][2 ] [2 ][1 ].
(8.28)
217
The matrix in (8.27) can be thought of as a generalization of the cross product

operation to six-dimensional twists. Specifically, given two twists V1 = (1 , v1 )
and V2 = (2 , v2 ), we perform a calculation analogous to (8.28):

[1 ] v1
[2 ] v2
[2 ] v2
[1 ] v1
0
0
0
0
0
0
0
0

0

[1 ][2 ] [2 ][1 ] [1 ]v2 [2 ]v1
[ ] v 0
=
,
=
0
0
0
0
which can be written more compactly in vector form as
0

[1 ]
0
2
=
.
v0
[v1 ] [1 ]
v2
This generalization of the cross product to two twists V1 and V2 will be called
the Lie bracket of V1 and V2 .
Definition 8.1. Given two twists V1 = (1 , v1 ) and V2 = (2 , v2 ), the Lie
bracket of V1 and V2 , denoted simultaneously by [V1 , V2 ] and adV1 (V2 ), is
defined as follows:

[1 ]
0
2
[V1 , V2 ] =
= adV1 (V2 ).
(8.29)
[v1 ] [1 ]
v2
Given V = (, v), we further define the following notation for the 6 6 matrix
representation [adV ]:

[] 0
[adV ] =
R66 .
(8.30)
[v] []
With this notation the Lie bracket [V1 , V2 ] can also be expressed as
[V1 , V2 ] = adV1 (V2 ) = [adV1 ]V2 .
(8.31)
Definition 8.2. Given a twist V = (, v) and wrench F = (m, f ), define the

mapping

T

[] 0
m
[]m [v]f
T
T
adV (F) = [adV ] F =
=
(8.32)
[v] []
f
[]f
Using the above notation and definitions the dynamic equations for a single
rigid body can now be written as follows:
Fb
=
=
Gb V b adTVb (Pb )
Gb V b [adV ]T Gb Vb .
b
(8.33)
(8.34)
Note the similarity between (8.34) and the moment equation for a rotating rigid
body:
mb = Ib b [b ]T Ib b .
(8.35)
Equation (8.35) is simply the rotational component of (8.34).
218
8.3
Inverse Dynamics of Open Chains
We now consider the inverse dynamics problem for an n-link open chain connected by one degree-of-freedom joints. Given the joint positions Rn , velocities Rn , and accelerations Rn , the objective is to calculate the
right-hand side of the dynamics equation
= M () + b(, ).
The main result will be a recursive inverse dynamics algorithm consisting of a
forward and backward iteration stage. In the forward iteration, the velocities
and accelerations of each link are propagated from the base to the tip, while in
the backward iteration, the forces and moments experienced by each link are
propagated from the tip to the base.
8.3.1
Overview of Newton-Euler Inverse Dynamics
Before getting into the details of the calculations, it is worth summarizing the
method in words.
Preliminaries: Attach a frame {0} to the base, frames {1} to {n} to the
centers of mass of links {1} to {n}, and a frame {n+1} at the end-effector,
fixed in the frame {n}.
Initialization: Set the velocity V0 of the base frame (typically zero), and
let V 0 be opposite the gravitational acceleration. (Upward acceleration of
the base is equivalent to gravity acting downward.) Fn+1 is a wrench that
the end-effector applies to the environment, expressed in frame {n + 1}.
Forward Iteration: For links i = 1 to n do
Calculate Ti1,i , the configuration of frame {i} expressed in frame
{i 1}. This is used in the next two calculations.
Calculate Vi , the velocity of link i expressed in frame {i}, as the sum
of the velocity of link i1 (but expressed in frame {i}) plus the extra
velocity due to the joint velocity i .
Calculate V i as the sum of the acceleration of link i1 (but expressed
in frame {i}) plus the extra acceleration due to the joint acceleration
i .
Backward Iteration: For links i = n to 1 do
The wrench Fi that must be applied to link i is the sum of the
wrench Fi+1 that must be provided to link i + 1 (but expressed in
frame {i}) plus the extra wrench Gi V i adTVi (Gi Vi ), from the rigidbody dynamics of link i. For link i = n, the wrench Fi+1 is just
the wrench the end-effector applies to the environment. For inboard
links i < n, the wrench Fi+1 includes also the wrenches needed to
support the outboard links.
8.3. Inverse Dynamics of Open Chains
219
The 6-vector wrench Fi calculated in the previous step is the sum of

the wrench provided by the motor at joint i plus the wrench provided
by the structure of joint i (e.g., the bearings). The motor itself
provides only a wrench that lives on a one-dimensional line in the
wrench space, typically a pure torque about the axis of a revolute
joint or a pure force along the axis of a prismatic joint. We get the
other five dimensions of Fi for free from the structure of the joint
that limits the motion to one degree-of-freedom. Thus the force or
torque i provided by the motor at joint i is simply the component
of Fi along the one-dimensional line of motor wrenches. The motor
effort i can be calculated by taking the projection (i.e., dot product)
of Fi onto the motor axis.
All that is needed to implement the Newton-Euler method is to introduce
some notation and derive the calculations above.
8.3.2
Details of Newton-Euler Inverse Dynamics
A body-fixed reference frame {i} is attached to the center of mass of each link i,
i = 1, . . . , n. The ground frame is denoted {0}, and a frame at the end-effector
is denoted {n + 1}. This frame is fixed in {n}.
When the manipulator is at the home position, with all joint variables zero,
we define Mi SE(3) to be the configuration of {i} in {0} and Mi1,i SE(3)
to be the configuration of {i} in {i 1}. Let Ai be the twist vector for joint i
(assuming i is set to zero) expressed in the {i} frame. Then the displacement
from frame {i 1} to {i}, denoted Ti1,i SE(3), is expressed in the following
form:
Ti1,i = Mi1,i e[Ai ]i .
(8.36)
If the forward kinematics is expressed in the space-frame product-of-exponentials,
then the forward kinematics up to each link frame {i} can be written
T0i
= e[S1 ]1 e[S2 ]2 e[Si ]i Mi
(8.37)
for i = 1, . . . , n. For each i = 1, . . . , n, the following can be established via

direct calculation:
Mi1,i
1
Mi1
Mi
(8.38)
Ai
AdM 1 (Si ).
(8.39)
We further define the following notation:

(i) Denote the velocity of link frame {i} expressed in frame {i} coordinates
1
by Vi = (i , vi ). Note that Vi is obtained from [Vi ] = T0i
T0i .
(ii) Let Gi R66 denote the 6 6 inertia matrix of link i, expressed relative
to link frame {i}. Since we assume here that all link frames are situated
220
Figure 8.1: Free body diagram illustrating the moments and forces exerted on
link i.
at the link center of mass, Gi has the block-diagonal form

Ii
0
Gi =
,
0 mi I
(8.40)
where Ii denotes the 3 3 rotational inertia matrix of link i, and mi is

the link mass.
(iii) Denote by Fi = (mi , fi ) the spatial force transmitted from link i 1 to
link i, expressed in frame {i} coordinates. Note that Fi is transmitted
entirely through joint i.
With the above notation and definitions, we now consider the free-body
diagram for link i as shown in Figure 8.1. Note that Fi+1 is the wrench applied
by link i to link i + 1, expressed in frame {i + 1} coordinates. What is needed
is the wrench applied by link i + 1 to link i, expressed in frame {i} coordinates;
using the wrench transformation rule under a change of reference frames, this
is given by
AdTTi+1,i (Fi+1 ) = AdTTi+1,i (Fi+1 ).
The equations of motion for link i can therefore be written
Gi V i = adTVi (Gi Vi ) + Fi AdTTi+1,i (Fi+1 ).
(8.41)
The joint torque i R at joint i is then the projection of the wrench Fi onto
the joint twist Ai :
i = FiT Ai .
(8.42)
We now derive the forward iteration of link velocities and accelerations from
the base to the tip. First note that
1
[V1 ] = T01
T01 = [A1 1 ]
(8.43)
8.3. Inverse Dynamics of Open Chains
221
and
[V2 ]
1
= T02
T02
1
1
1
= T12
(T01
T01 )T12 + T12
T12
1
= T [V1 ]T12 + [A2 2 ],

12
(8.44)
or equivalently, V2 = AdT21 (V1 ) + A2 2 . Repeating this procedure for the subsequent links, it can be established that
Vi = AdTi,i1 (Vi1 ) + Ai i , i = 1, . . . , n.
(8.45)
The accelerations V i can also be found recursively. Noting that

[V i ] =
d 1
d
1
1
Ti1,i [Vi ]Ti1,i
+ Ti1,i [V i ]Ti1,i
+ Ti1,i [Vi ] Ti1,i
+ [Ai ]i ,
dt
dt
and
d
Ti1,i
dt
d 1
T
dt i1,i
= Mi1,i [Ai ]e[Ai ]i i = Mi1,i e[Ai ]i [Ai ]i

1
1
Ti1,i Ti1,i
,
= Ti1,i
it can be shown that

V i = Ai i + AdTi,i1 (V i1 ) + [AdTi1,i (Vi1 ), Ai i ],
(8.46)

where AdTi1,i (Vi1 ), Ai denotes the Lie bracket of AdTi1,i (Vi1 ) with Ai .
Note that since [Ai , Ai ] = 0 and AdTi,i1 (Vi1 ) = Vi Ai i , one obtains the
alternative but equivalent formula
V i = Ai i + AdTi,i1 (V i1 ) + [Vi , Ai i ].
(8.47)
The above formulas for the velocities and accelerations, together with the
dynamic equations for any given link, can now be organized into a two-stage
forward-backward iterative algorithm for the inverse dynamics. Before doing
so, we examine how to include gravity in the dynamics. One way to simulate
the effects of gravity is to set the base frame to have an acceleration g, where
g R3 denotes the gravitational acceleration vector as expressed in base frame
coordinates. In this case it is important to remember that the link acceleration
calculated from the recursive algorithm is not its true acceleration, but rather
its true acceleration minus g.
The algorithm is initialized by providing initial values for V0 , V 0 , and Ftip ,
where V0 and V 0 are respectively the spatial velocity and spatial acceleration of
the base frame expressed in base frame coordinates, and Ftip is the spatial force
applied to to the environment by the final link, expressed in end-effector frame
are also assumed
coordinates. The joint trajectory (t) and its derivatives ,
given as input.
222
Newton-Euler Inverse Dynamics Algorithm Summary

Preliminaries: Each link frame {i} is assumed attached to the links
center of mass. The forward kinematics from the base frame {0} to link
frame {i} is of the form
T0i = e[S1 ]1 e[S2 ]2 e[Si ]i Mi , i = 1, . . . , n.
(8.48)
Define Mi1,i to be the displacement from link frame {i 1} to link

frame {i} when the manipulator is at the home position. Then Mi =
1
M01 M12 Mi1,i and Mi1,i = Mi1
Mi , i = 1, . . . , n. The displacement
between link frames {i 1} and {i} is
Ti1,i = Mi1,i e[Ai ]i ,
(8.49)
Ai = AdM 1 (Si ), i = 1, . . . , n.
(8.50)
where
i
With respect to the link frame attached at its center of mass, the 6 6
spatial inertia Gi of link i is defined as

Ii
0
Gi =
,
(8.51)
0 mi I
where Ii R33 is its rotational inertia matrix, and mi is the mass of
link i. Define the twist V0 = (0 , v0 ) to be the spatial velocity of the
base frame, expressed in base frame coordinates. Define g R3 to be
the gravity vector expressed in base frame {0} coordinates. Define Ftip =
(mtip , ftip ) to be the wrench applied applied to the environment by the
end-effector expressed in the end-effector frame {n + 1}, fixed in the frame
{n}.
Initialization: V0 = given, V 0 = (0, g), Fn+1 = Ftip .
Forward Iteration: For i = 1 to n do
Ti1,i
Vi
V i
= Mi1,i e[Ai ]i
=
=
AdTi,i1 (Vi1 ) + Ai i
AdT
(V i1 ) + [Vi , Ai ]i + Ai i
i,i1
(8.52)
(8.53)
(8.54)
Backward Iteration: For i = n to 1 do

Fi
AdTTi+1,i (Fi+1 ) + Gi V i adTVi (Gi Vi )
(8.55)
FiT Ai .
(8.56)
As noted earlier, the recursion formula Equation (8.54) for the acceleration V i
can also be replaced by the equivalent formula
V i = AdTi,i1 (V i1 ) + [AdTi1,i (Vi1 ), Ai ]i + Ai i .
(8.57)
8.4. Dynamic Equations in Closed Form
8.4
223
Dynamic Equations in Closed Form
In this section we show how the equations in the recursive inverse dynamics
algorithm can be organized into a closed-form set of dynamics equations of the
+ g().
form = M () + c(, )
Before doing so, we prove our earlier assertion that the total kinetic energy
We do so by noting that K
K of the robot can be expressed as K = 12 T M ().
can be expressed as the sum of the kinetic energies of each link:
n
K=
1X T
V Gi Vi ,
2 i=1 i
(8.58)
where Vi is the spatial velocity of link frame {i}, and Gi is the spatial inertia
matrix of link i as defined by Equation (8.51) (both are expressed in link frame
{i} coordinates). Let T0i (1 , . . . , i ) denote the forward kinematics from the base
frame {0} to link frame {i}, and let Jib () denote the body Jacobian obtained
1
T0i . Note that Jib as defined is a 6 i matrix; we turn it into a 6 n
from T0i
matrix by filling in all entries of the last n i columns with zeros. With this
definition of Jib , we can write
i = 1, . . . , n.
Vi = Jib (),
The kinetic energy can then be written
n
X
1
K = T
2
Jib () Gi Jib () .
T
(8.59)
i=1
The term inside the parentheses is precisely the mass matrix M ():
M () =
n
X
Jib ()T Gi Jib ().
(8.60)
i=1
Some of the exercises at the end of this chapter examine ways to recursively
compute the entries of M ().
We now return to the original task of deriving a closed-form set of dynamic
equations. We start by defining the following stacked vectors:
V1
V = ... R6n
(8.61)
Vn
F1
= ... R6n .
Fn
(8.62)
224
Further define the following matrices:
A1 0 0
0 A2 0
A = .
..
..
..
..
.
.
.
[adV ]
An
G1
0
..
.
0
G2
..
.
..
.
0
0
..
.
Gn
[adV1 ]
0
..
.
0

adA =
S()
[adA1 1 ]
0
..
.
0
R6nn
(8.63)
R6n6n
0
[adV2 ]
0
..
..
..
.
.
.
[adVn ]
R6n6n
0
0
[adA2 2 ]
..
..
..
.
.
.
[adAn n ]
0
0
0
[AdT21 ]
0
0
[AdT32 ]
..
..
.
.
0
0
..
.
(8.64)
0
0
0
..
.
(8.65)
R6n6n
0
0
0
..
.

AdTn,n1
0
(8.66)
R6n6n .(8.67)
We write S() to emphasize the dependence of S on . Finally, define the

following stacked vectors:
AdT10 (V0 )
Vbase =
(8.68)
R6n
..
.
0
AdT10 (V 0 )
V base =
(8.69)
R6n
..
.
0
Ftip
0
..
.
0
AdTTn+1,n (Fn+1 )
R6n .
(8.70)
Note that A R6nn and G R6n6n are constant block-diagonal matrices,
8.4. Dynamic Equations in Closed Form
225
in which A contains only the kinematic parameters, while G contains only the
mass and inertial parameters for each link.
With the above definitions, our earlier recursive inverse dynamics algorithm
can be assembled into the following set of matrix equations:
V
V
F
= S()V + A + Vbase
= S()V + A [adA ](S()V + Vbase ) + V base
= S T ()F + G V [adV ]T GV + Ftip
T
= A F.
(8.71)
(8.72)
(8.73)
(8.74)
S() has the property that S n () = 0 (such a matrix is said to be nilpotent

of order n), and one consequence verifiable through direct calculation is that
(I S())1 = I + S() + . . . + S n1 (). Defining L() = (I S())1 , it can
further be verified via direct calculation that
I
0
0
0
[AdT21 ]
I
0
0
[AdT31 ] [AdT32 ]
I
0
L() =
(8.75)
R6n6n .
..
..
..
.
.
.
.
. .
.
.
.
[AdTn1 ]
[AdTn2 ]
[AdTn3 ]
We write L() to emphasize the dependence of L on . The earlier

equations can now be reorganized as

V = L() A + Vbase

V = L() A + [adA ]S()V + [adA ]Vbase + V base

F = LT () G V [adV ]T GV + Ftip
AT F.
matrix
(8.76)
(8.77)
(8.78)
(8.79)
If an external wrench Ftip is applied at the tip, this can be included into the
following dynamics equation:
+ g() + J T ()Ftip ,
= M () + c(, )
(8.80)
where J() denotes the Jacobian of the forward kinematics expressed in the
same reference frame as Ftip , and
M () = AT LT ()GL()A

= AT LT () GL() [ad ]S() + [adV ]T G L()A
c(, )
A
g() = AT LT ()GL()V base .
(8.81)
(8.82)
(8.83)
represents the Coriolis

The g() term reflects gravitational forces, while c(, )
and centrifugal forces. Comparing these equations with the Lagrangian form of
226
the dynamics, i.e.,

i =
n
X
mij ()j +
j=1
n X
n
X
j=1 k=1
P
, i = 1, . . . , n,
ijk ()j k +
i
(8.84)
where the ijk () are

1
ijk () =
2
mij
mik
mjk
+
k
j
i

,
(8.85)
term can be identified with

we can see that elements of the c(, )
n X
n
X
ijk ()j k
j=1 k=1
and are thus quadratic in the i . Elements of the gravity term g() can be
P
. With the Newton-Euler formulation, the partial derivative
identified with
i
terms appearing in ijk () can be evaluated directly from (8.82) without taking
Rnn as
derivatives. Further, by defining the matrix C(, )

n
n
X
1 X mij
mik
mjk
k ,
(8.86)
ijk ()k =
cij (, ) =
2
k
j
i
k=1
k=1
it can be seen that c(, )

can be
where cij denotes the (i, j) entry of C(, ),
expressed as
= C(, )
.
c(, )
(8.87)
is called the Coriolis matrix. The following property, reThe matrix C(, )
ferred to as the passivity property, turns out to have important ramifications
in proving the stability of certain robot control laws.
Rnn , where M () Rnn
Proposition 8.1. The matrix M () 2C(, )
Rnn is the
is the mass matrix and M () its time derivative, and C(, )

Coriolis matrix as defined in (8.86), is skew-symmetric.
Proof. The (i, j) component of M 2C is
m
ij () 2cij (, )
=
=
n
X
mij
k=1
n
X
k=1
mij
mik
mkj
k
k
k +
k
k
k
j
i
mkj
mik
k
k .
i
j
By switching the indices i and j, it can be seen that

= (m
m
ji () 2cji (, )
ij () 2cij (, )),
thus proving that (M 2C)T = (M 2C) as claimed.
The passivity property will be used later in the chapter on robot control.
8.5. Forward Dynamics of Open Chains
8.5
227
Forward Dynamics of Open Chains
We now consider the forward dynamics problem, where a torque trajectory (t)
together with a set of initial conditions on and is assumed given, and the
to obtain
objective is to integrate the dynamic equations (t) = M () + b(, )
the joint trajectory (t). The simplest numerical scheme for integrating the
general first-order differential equation q = f (q, t), q Rn , is via the Euler
iteration
q(t + h) = q(t) + hf (q(t), t),
where the positive scalar h denotes the timestep. The dynamic equations can be
converted to a first-order differential equation by taking advantage of the fact
and q = (q1 , q2 ) R2n ,
that M () is always invertible: setting q1 = , q2 = ,
we can write
q1
= q2
q2
= M 1 (q1 ) ( (t) b(q1 , q2 )) ,
which is of the form q = f (q, t). The Euler integration scheme for this equation
is thus of the form
q1 (t + h)
= q1 (t) + hq2 (t)
q2 (t + h)

= q2 (t) + h M (q1 (t))1 ( (t) b(q1 (t), q2 (t))) .
Given a set of initial values for q1 (0) = (0) and q2 (0) = (0),
the above equations can then be iterated forward in time to numerically obtain the motion
(t) = q1 (t).
Note that the above iteration appears to require the evaluation of M 1 (),
which can be computationally expensive. In fact, it is possible to integrate
these equations without having to explicitly compute the inverse of M (). The
closed-form dynamic equations can be arranged as
M () = (t) b(, ).
(8.88)
Therefore by running the inverse

Setting to zero in (8.88) leads to = b(, ).
dynamics algorithm with (t) set to zero and ((t), (t))

set to their current
values, b((t), (t))

can now be determined. Subtracting this from the given
value of (t) then results in the right-hand side of (8.88). Therefore, with a
as the
means of evaluating M (), it is a straightforward matter to obtain (t)
solution to the linear equation Ax = c, where A = M ((t)) Rnn is assured
to be nonsingular, and c = (t) b((t), (t))

is known.
Once a numerical integration scheme has been chosen, solving the forward
dynamics then reduces to a procedure for evaluating from given values for ,
and . In the following algorithm we allow for the possibility of an external
,
spatial force Ftip applied to the final link.
228
, )
Algorithm for Calculating the Joint Acceleration: GetJointAccel(, ,
ext
Prerequisites: Algorithms for calculating the inverse dynamics, and the
mass matrix, are assumed available. An algorithm for solving the linear
system Ax = c for x Rn , with given c Rn and A Rnn nonsingular,
is also assumed available.
the input torque . If an external spatial
Inputs: Current values for , ,
tip force Ftip is also given, it is transformed via the static force-torque
relation to ext = J T Ftip , where the Jacobian J() is expressed in terms
of the same reference frame as Ftip .
Output: The joint acceleration .

Initialization: Assign temporary storage variables A Rnn , Rn ,
c Rn .
Inverse dynamics calculation: Calculate the inverse dynamics with
and set to their given values, and set to zero; store the output joint
torques in , and set c = ext .
Evaluation of mass matrix: Calculate the mass matrix for the given
, and store the result in A.
Calculation of joint acceleration: Solve the linear system Ax = c for
x; the resulting joint acceleration is then given by x.
With the above algorithm for calculating the joint acceleration, various numerical schemes for integrating the forward dynamics can be implemented; here
we present an algorithm for the most basic Euler method described above:
Euler Integration Algorithm for Forward Dynamics
, ext ) required.
Prerequisites: Function GetJointAccel(, ,
Inputs: Initial conditions (0) and (0),

input torques (t) and ext for
t [0, tf ], integration timestep h > 0.
Output: Joint trajectory values [k] = (hk), k = 0, . . . , N .
Initialization: Set N = tf /h.
Iteration: For k = 1 to N do
[k]
[k + 1]
GetJointAccel([k], [k],
[k], ext [k]);
[k] + h[k];
[](k + 1)
+ h[k];
[k]
Joint trajectory: [k] = (hk), k = 0, . . . , N .
8.6. Dynamics in Task Space Coordinates
8.6
229
Dynamics in Task Space Coordinates
In this section we consider how the dynamic equations change under a transformation to coordinates of the end-effector frame (task space coordinates). To
keep things simple we consider a six degree-of-freedom open chain with joint
space dynamics
R6 , R6 .
= M () + b(, ),
(8.89)
We also ignore for the time being any external spatial forces that may be applied.
The spatial velocity V = (, v) of the end-effector is related to the joint velocity
by
V = J(),
(8.90)
with the understanding that V and J() are always expressed in terms of the
same reference frame. The time derivative V is then
+ J().
V = J()
(8.91)
At configurations where J() is invertible, we have

= J 1 V
1 V.
= J 1 V J 1 JJ
(8.92)
(8.93)
d
(A1 A) =
The second term in (8.93) follows from the general matrix identity dt
d 1
1 d
A + A dt A for any invertible and differentiable matrix A(t). Substidt A
tuting for and in Equation (8.89) leads to

1 V + b(, ),
= M J 1 V J 1 JJ
(8.94)
where J T denotes (J 1 )T = (J T )1 . Pre-multiply both sides by J T to get

J T
1 V
J T M J 1 V J T M J 1 JJ
T
1
+J b(, J V).
(8.95)
Expressing J T as the spatial force F, the above can be written

F = ()V + (, V),
(8.96)
= J T M ()J 1
(8.97)
where
()
(, V)
= J
b(, J
1 V.
V) JJ
(8.98)
These are the dynamic equations expressed in end-effector frame coordinates. If

an external spatial force F is applied to the end-effector frame, then assuming
zero joint torques, the motion of the end-effector frame is governed by these
equations. Note the dependence of () and (, V) on . If were replaced by
its inverse kinematics solution = T 1 (X), then one would obtain a differential
equation strictly in terms of the end-effector frames displacement X SE(3)
and spatial velocity V. In practice, since X is usually obtained by measuring
and substituting into the forward kinematics, it is preferable to leave the
dependence on explicit.
230
8.7
Summary
8.8
Chapter 9
Trajectory Generation
During robot motion, the robot controller is provided with a steady stream of
goal positions and velocities to track. This specification of the robot position
as a function of time is called a trajectory. In some cases, the trajectory is
completely specified by the taskfor example, the end-effector may be required
to track a known moving object. In other cases, as when the task is simply to
move from one position to another in a given time, we have freedom to design the
trajectory to meet these constraints. This is the domain of trajectory planning.
The trajectory should be a sufficiently smooth function of time, and it should
respect any given limits on joint velocities, accelerations, or torques.
In this chapter we consider a trajectory as the combination of a path, a purely
geometric description of the configurations achieved by the robot, and a time
scaling, which specifies the times when those configurations are reached. We
consider three cases: point-to-point straight-line trajectories in both joint space
and task space; trajectories passing through a sequence of timed via points;
and minimum-time trajectories along specified paths. Finding paths that avoid
obstacles is left to Chapter 10.
9.1
Definitions
A path (s) maps a scalar path parameter s, assumed to be zero at the start
of the path and 1 at the end, to a point in the robots configuration space ,
: [0, 1] . As s increases from 0 to 1, the robot moves along the path.
Sometimes s is taken to be time, and is allowed to vary from time s = 0 to
the total motion time s = T , but it is often useful to separate the role of the
geometric path parameter s from the time parameter t. A time scaling s(t)
assigns a value s to each time t [0, T ], s : [0, T ] [0, 1].
Together a path and time scaling define a trajectory (s(t)), or (t) for short.
Using the chain rule, the velocity and acceleration along the trajectory can be
231
232
written as
d
s
=
ds
d
d2
=
s + 2 s 2 .
ds
ds
(9.1)
(9.2)
To ensure that the robots acceleration (and therefore dynamics) are well defined, each of (s) and s(t) must be twice differentiable.
9.2
Point-to-Point Trajectories
The simplest type of motion is from rest at one configuration to rest at another.
We call this a point-to-point motion. The simplest type of path for point-topoint motion is a straight line. Straight-line paths and their time scalings are
discussed below.
9.2.1
Straight-Line Paths
A straight line from a start configuration start to an end configuration end

could be defined in joint space or in task space. The advantage of a straight-line
path from start to end in joint space is simplicity: since joint limits typically
take the form i,min i i,max for each joint angle i , the allowable joint
configurations form a convex set free in joint space, so the straight line between
any two endpoints in free also lies in free . The straight line can be written
(s) = (1 s)start + send ,
s [0, 1]
(9.3)
with derivatives
d
= end start
ds
d2
= 0.
ds2
(9.4)
(9.5)
Straight lines in joint space generally do not yield straight-line motion of the
end-effector in Cartesian space. If Cartesian straight-line motions are desired,
the start and end configurations can be specified by Xstart and Xend in task
space. If Xstart and Xend are represented by a minimum set of coordinates, then
a straight line is defined as X(s) = (1 s)Xstart + sXend , s [0, 1]. Compared
to joint coordinates, however, the following are issues that must be addressed:
Inverse kinematics must be used to find joint coordinates for the robot
controller. If the robot is redundant, a redundancy resolution scheme must
be employed.
If the path passes near a kinematic singularity, joint velocities may become
unreasonably large for almost all time scalings of the path.
9.2. Point-to-Point Trajectories
233
2 (deg)
180
end
90
start
2
-90
90
180
1 (deg)
-90
2 (deg)
180
end
90
start
-90
90
180
1 (deg)
-90
Figure 9.1: (Left) A 2R robot with joint limits 0 1 180 , 0 2 150 .

(Top right) A straight-line path in joint space and the corresponding motion
of the end-effector in task space. The reachable configurations, subject to joint
limits, are indicated in grey. (Bottom right) A straight-line path in task space
would violate the joint limits.
Since the robots reachable task space may not be convex in X coordinates,
some points on a straight line between two reachable endpoints may not
be reachable (Figure 9.1).
In addition to the issues above, if Xstart and Xend are represented as elements
of SE(3) instead of as a minimum set of coordinates, then there is the question
of how to define a straight line in SE(3). A configuration of the form (1
s)Xstart + sXend does not generally lie in SE(3). Instead, to keep the Cartesian
motion decoupled from the rotational motion, we write X = (R, p) and we could
choose the straight-line path
p(s) = (1 s)pstart + spend
R(s) =
T
Rstart exp(log(Rstart
Rend )s).
(9.6)
(9.7)
234
s(t)
ds/dt
3
2T
Figure 9.2: Plots of s(t) and s(t)
for a third-order polynomial time scaling.
9.2.2
Time Scaling a Straight-Line Path
A time scaling s(t) of a path should ensure that the motion is appropriately
smooth and that any constraints on robot velocity and acceleration are satisfied.
For a straight-line path in joint space of the form Equation (9.3), the time-scaled
joint velocities and accelerations are = s(
end start ) and = s(end start ),
respectively. For a straight-line path in task space, parameterized by minimum
and by X, X,
and X.
Inverse
coordinates X Rn , simply replace , ,
kinematics, and possibly redundancy resolution, is then used to convert to an
equivalent representation in joint space.
9.2.2.1
Polynomial Time Scaling
Third-order Polynomials A convenient form for the time scaling s(t) is a

cubic polynomial of time,
s(t) = a0 + a1 t + a2 t2 + a3 t3 .
(9.8)
A point-to-point motion in time T imposes the initial constraints s(0) = s(0)
=0
and the terminal constraints s(T ) = 1 and s(T
) = 0. Evaluating (9.8) and its
derivative
s(t)
= a1 + 2a2 t + 3a3 t2
(9.9)
at t = 0 and t = T and solving the four constraints for a0 , . . . , a3 , we find
a0 = 0 a1 = 0 a2 =
3
T2
Plots of s(t) and s(t)
are shown in Figure 9.2.
a3 =
2
.
T3

ds/dt
15
8T
s(t)
1
235
d2s/dt 2
10
T 2 3
0
T t
T t
T t
Figure 9.3: Plots of s(t), s(t),
and s(t) for a fifth-order polynomial time scaling.

Plugging s = a2 t2 + a3 t3 into Equation (9.3) yields

2
2t3
3t
(end start )
(t) = start +
T2
T3

2
= 6t 6t (end start )
(t)
T2
T3

= 6 12t (end start ).
(t)
T2
T3
(9.10)
(9.11)
(9.12)
The maximum joint velocities are achieved at the halfway point of the motion
t = T /2:
3
(end start ).
max =
2T
The maximum joint accelerations and decelerations are achieved at t = 0 and
t = T:
6
max = 2 (end start ).
T
limit and maxIf there are known limits on the maximum joint velocities ||
limit , these bounds can be checked to see if the
imum joint accelerations ||
requested motion time T is feasible. Alternatively, T could be solved for to find
the minimum possible motion time that satisfies the most restrictive velocity or
acceleration constraint.
Fifth-order Polynomials Because the third-order time scaling does not constrain the endpoint path accelerations s(0) and s(T ) to be zero, the robot is
asked to achieve a discontinuous jump in acceleration at both t = 0 and t = T .
This implies infinite jerk, the derivative of acceleration, which may cause vibration of the robot.
One solution is to constrain the endpoint accelerations to s(0) = s(T ) = 0.
The addition of these two constraints requires the addition of two more design
freedoms in the polynomial, yielding a quintic polynomial of time, s(t) = a0 +
. . . + a5 t5 . We can use the six terminal position, velocity, and acceleration
constraints to solve uniquely for a0 . . . a5 (Exercise 5), which yields a smoother
motion with a higher maximum velocity than a cubic time scaling. A plot of
the time scaling is shown in Figure 9.3.
236
s(t)
ds/dt
v
slo
= -a
pe
pe
slo
=a
ta
T - ta
Figure 9.4: Plots of s(t) and s(t)
for a trapezoidal motion profile.

9.2.2.2
Trapezoidal Motion Profiles
Trapezoidal time scalings are quite common in motor control, particularly for
the motion of a single joint, and they get their name from their velocity profiles.
The point-to-point motion consists of a constant acceleration phase s = a of
time ta , followed by a constant velocity phase s = v of time tv = T 2ta ,
followed by a constant deceleration phase s = a of time ta . The resulting s
profile is a trapezoid and the s profile is the concatenation of a parabola, linear
segment, and parabola as a function of time (Figure 9.4).
The trapezoidal time scaling is not as smooth as the cubic time scaling,
but it has the advantage that if there are known limits on joint velocities limit
and joint accelerations limit , the trapezoidal motion using the largest v and a
satisfying
|v(end start )| limit
|a(end start )| limit
(9.13)
(9.14)
is the fastest straight-line motion possible. (See Exercise 8.)

If v 2 /a > 1, the robot never reaches the velocity v during the motion (Exercise ??). The three-phase accelerate-coast-decelerate motion becomes a twophase accelerate-decelerate motion, and the trapezoidal profile s(t)
in Figure 9.4
becomes a triangle.
Assuming that v 2 /a 1, the trapezoidal motion is fully specified by v, a,
ta , and T , but only two of these can be specified independently, since they
must satisfy s(T ) = 1 and v = ata . It is unlikely that we would specify ta
independently, so we can eliminate it from the equations of motion by the substitution ta = v/a. The motion profile during the three stages (acceleration,
237
coast, deceleration) can then be written in terms of v, a, and T as

0t
v
:
a
v
v
<tT
:
a
a
s(t) = a
(9.15)
s(t)
= at
1
s(t) = at2
2
(9.16)
s(t) = 0
(9.18)
(9.17)
s(t)
=v
(9.19)
2
s(t) = vt
v
<tT :
a
v
2a
(9.20)
s(t) = a
(9.21)
s(t)
= a(T t)
(9.22)
2
s(t) =
2avT 2v a (t T )
.
2a
(9.23)
Since only two of v, a, and T can be chosen independently, we have three

options:
Choose v and a such that v 2 /a 1, assuring a three-stage trapezoidal
profile, and solve s(T ) = 1 (Equation (9.23)) for T :
T =
a + v2
.
va
If v and a correspond to the highest possible joint velocities and accelerations, this is the minimum possible time for the motion.
Choose v and T such that 2 vT > 1, assuring a three-stage trapezoidal
profile and that the top speed v is sufficient to reach s = 1 in time T , and
solve s(T ) = 1 for a:
v2
a=
.
vT 1
Choose a and T such that aT 2 4, assuring that the motion is completed
in time, and solve s(T ) = 1 for v:

p
1
v=
aT a aT 2 4 .
2
9.2.2.3
S-Curve Time Scalings
Just as cubic polynomial time scalings lead to infinite jerk at the beginning and
end of the motion, trapezoidal motions cause discontinuous jumps in acceleration at t {0, ta , T ta , T }. A solution is a smoother S-curve time scaling, a
238
ds/dt
v
=
pe
slo
slo
pe
-a
7
T
Figure 9.5: Plot of s(t)

for an S-curve motion profile consisting of seven stages:
(1) constant positive jerk, (2) constant acceleration, (3) constant negative jerk,
(4) constant velocity, (5) constant negative jerk, (6) constant deceleration, and
(7) constant positive jerk.
popular motion profile in motor control because it avoids vibrations or oscillations induced by step changes in acceleration. An S-curve time scaling consists
of seven stages: (1) constant jerk s(3) = J until a desired acceleration s = a is
achieved; (2) constant acceleration until the desired s = v is being approached;
(3) constant negative jerk J until s equals zero exactly at the time s reaches
v; (4) coasting at constant v; (5) constant negative jerk J; (6) constant deceleration a; and (7) constant positive jerk J until s and s reach zero exactly at
the time s reaches 1.
The s(t)
profile for an S-curve is shown in Figure 9.5.

Given some subset of v, a, J, and the total motion time T , algebraic manipulation reveals the switching time between stages and conditions that ensure
that all seven stages are actually achieved, similar to the case of the trapezoidal
motion profile.
9.3
Polynomial Via Point Trajectories
If the goal is to have the robot joints pass through a series of via points at
specified times, without a strict specification on the shape of path, a simple
solution is to use polynomial interpolation to directly find joint histories (t)
without first specifying a path (s) and then a time scaling s(t) (Figure 9.6).
Let the trajectory be specified by k via points, with the start point occurring
at T1 = 0 and the final point at Tk = T . Since each joint history is interpolated
individually, we focus on a single joint angle and call it to avoid proliferation of
subscripts. At each via point i {1 . . . k}, the user specifies the desired position
i ) = i . The trajectory has j = k 1 segments,
(Ti ) = i and velocity (T
and the duration of segment j {1, . . . , k 1} is Tj = Tj+1 Tj . The joint
trajectory during segment j is expressed as the third-order polynomial
(t) = aj0 + aj1 t + aj2 t2 + aj3 t3
(9.24)
9.3. Polynomial Via Point Trajectories
via 2
239
via 3
via 3
via 2
start
end
(a)
start
x
(b)
end
Figure 9.6: Two paths in an (x, y) space corresponding to piecewise-cubic trajectories interpolating four via points, including one start point and one end
point. The velocities at the start and end are zero, and the velocities at vias 2
and 3 are indicated by dashed tangent vectors. The shape of the path depends
on the velocities specified at the via points.
in terms of the time t elapsed in segment j, where 0 t Tj . Segment j
is subject to the four constraints
(0) = j
(Tj ) = j+1
(0)
= j
(T
j ) = j+1 .
Solving these constraints for aj0 , . . . , aj3 yields

aj0 = j
aj1 = j
3j+1 3j 2 j Tj j+1 Tj
Tj2
2j + ( j + j+1 )Tj 2j+1
=
.
Tj3
(9.25)
(9.26)
aj2 =
(9.27)
aj3
(9.28)
Figure 9.7 shows the time histories for the interpolation of Figure 9.6(a). In
this 2D (x, y) coordinate space, the via points 1 . . . 4 occur at times T1 = 0,
T2 = 1, T3 = 2, and T4 = 3. The via points are at (0, 0), (0, 1), (1, 1), and (1, 0)
with velocities (0, 0), (1, 0), (0, 1), and (0, 0).
Two issues are worth mentioning:
The quality of the interpolated trajectories is improved by reasonable
combinations of via point times and via point velocities. For example,
if the user wants to specify a via point location and time, but not the
velocity, a heuristic could be used to choose a via velocity based on the
240
velocity
position
T1
T2
T3
T4
T1
T2
T3
T4
Figure 9.7: The coordinate time histories for the cubic via point interpolation
of Figure 9.6(a).
times and coordinate vectors to the via points before and after the via in
question. As an example, the trajectory of Figure 9.6(b) is smoother than
the trajectory of Figure 9.6(a).
Cubic via point interpolation ensures that velocities are continuous at via
points, but not accelerations. The approach is easily generalized to use
fifth-order polynomials and specifications of the accelerations at the via
points, at the cost of increased complexity of the solution.
If only two points are used (the start and end point), and the velocities at
each are zero, the resulting trajectory is identical to the point-to-point cubic
polynomial time-scaled trajectory discussed in Section 9.2.2.1.
There are many other methods for interpolating a set of via points. For
example, B-spline interpolation is popular. In B-spline interpolation, the path
may not pass exactly through the via points, but the path is guaranteed to be
confined to the convex hull of the via points, unlike the paths in Figure 9.6.
This can be important to ensure that joint limits or workspace obstacles are
respected.
9.4
Time-Optimal Time Scaling
In the case that the path (s) is fully specified by the task or an obstacleavoiding path planner (e.g., Figure 9.8), the trajectory planning problem reduces
to finding a time scaling s(t). One could choose the time scaling to minimize
energy consumed while meeting a time constraint, or to prevent spilling a glass
of water the robot is carrying. One of the most useful time scalings, however,
is the one that minimizes the time of motion along the path, subject to the
robots actuator limits. Such time-optimal trajectories maximize the robots
productivity.
9.4. Time-Optimal Time Scaling
s=1
241
(xc , yc)
s=0
R
y
x
Figure 9.8: A path planner has returned a semicircular path of radius R around
an obstacle in (x, y) space for a robot with two prismatic joints. The path can
be represented as a function of a path parameter s as x(s) = xc + R cos s and
y(s) = yc R sin s for s [0, 1]. For a 2R robot, inverse kinematics would be
used to express the path as a function of s in joint coordinates.
While the trapezoidal time scalings of Section 9.2.2.2 can yield time-optimal
trajectories, this is only under the assumption that the maximum acceleration
a and maximum coasting velocity v are constant. For most robots, because of
state-dependent joint actuator limits and the state-dependent dynamics
+ g() = ,
M () + C(, )
(9.29)
the maximum available velocities and accelerations change along the path.
In this section we consider the problem of finding the fastest possible time
scaling s(t) that respects the robots actuator limits. We write the limits on the
ith actuator as
i max (, ).
imin (, )
(9.30)
i
The available actuator torque is typically a function of the state of the actuator.
For example, for a given maximum voltage of a DC motor, the maximum torque
available from the motor drops linearly with the motors speed.
Before proceeding, we note that the Coriolis terms in Equation (9.29) can
be written equivalently as
= T (),
C(, )
where () is the three-dimensional tensor of Christoffel symbols constructed
of partial derivatives of components of the inertia matrix M () with respect to
. This form more clearly shows the Coriolis terms quadratic dependence on
velocities. Now beginning with Equation (9.29), replacing by (d/ds)s and
242
by (d/ds)
s + (d2 /ds2 )s 2 , and rearranging, we get
!
T

d2
d
d
d
s 2 +g((s)) = , (9.31)
s+ M ((s)) 2 +
((s))
M ((s))
| {z }
ds
ds
ds
ds
{z
} |
|
g(s)
{z
}
m(s)
c(s)
expressed more compactly as the vector equation

m(s)
s + c(s)s 2 + g(s) = ,
(9.32)
where m(s) is the effective inertia of the robot confined to the path (s), c(s)s 2
are the quadratic velocity terms, and g(s) is the gravitational torque.
Similarly, the actuation constraints (9.30) can be expressed as a function of
s:
imin (s, s)
i imax (s, s).
(9.33)
Plugging in the components of Equation (9.32), we get
imin (s, s)
mi (s)
s + ci (s)s 2 + gi (s) imax (s, s).
(9.34)
Let Li (s, s)
and Ui (s, s)
be the minimum and maximum accelerations s satisfying the ith component of Equation (9.34). Depending on the sign of mi (s),
we have three possibilities:
imin (s, s)
c(s)s 2 g(s)
mi (s)
max
(s, s)
c(s)s 2 g(s)
Ui (s) = i
mi (s)
max
(s, s)
c(s)s 2 g(s)
if mi (s) < 0 : Li (s) = i
mi (s)
imin (s, s)
c(s)s 2 g(s)
Ui (s) =
mi (s)
if mi (s) = 0 : zero-inertia point, discussed in Section 9.4.3
if mi (s) > 0 : Li (s) =
(9.35)
Defining
L(s, s)
= max Li (s, s)
and
U (s, s)
= min Ui (s, s),
the actuator limits (9.34) can be written as the state-dependent time-scaling

constraints
L(s, s)
s U (s, s).
(9.36)
The time-optimal time-scaling problem can now be stated:
Given a path (s), s [0, 1], an initial state (s0 , s 0 ) = (0, 0), and a final state
(sf , s f ) = (1, 0), find a monotonically increasing twice-differentiable time scaling
s : [0, T ] [0, 1] that
243
time scaling s(s)
Figure 9.9: A time scaling in the (s, s)

phase plane is a curve with s 0 at all
times connecting the initial path position and velocity (0, 0) to the final position
and velocity (1, 0).
(i) satisfies s(0) = s(0)
= s(T
) = 0 and s(T ) = 1, and
(ii) minimizes the total travel time T along the path while respecting the actuator constraints (9.36).
The problem formulation is easily generalized to the case of nonzero initial
and final velocity along the path, s(0)
> 0 and s(T

) > 0.
9.4.1
The (s, s)
Phase Plane
The problem is easily visualized in the (s, s)

phase plane of the path-constrained
robot, with s running from 0 to 1 on a horizontal axis and s on a vertical
axis. Since s(t) is monotonically increasing, s(t)
0 for all times t and for all

s [0, 1]. A time scaling of the path is any curve in the phase plane that moves
monotonically to the right from (0, 0) to (1, 0) (Figure 9.9). Not all such curves
satisfy the acceleration constraints (9.36), however.
To see the effect of the acceleration constraints, at each (s, s)
in the phase
plane, we can plot the limits L(s, s)
s U (s, s)
as a cone, as illustrated in
Figure 9.10(a). If L(s, s)
U (s, s),
the cone disappearsthere are no actuator
commands that can keep the robot on the path at this state. These inadmissible
states are indicated in grey in Figure 9.10(a). For any s, typically there is a
single limit velocity s lim (s) above which all velocities are inadmissible. The
function s lim (s) is called the velocity limit curve. On the velocity limit curve,
L(s, s)
= U (s, s),
and the cone reduces to a single vector.
For a time scaling to satisfy the acceleration constraints, the tangent of the
time-scaling curve must lie inside the feasible cone at all points on the curve.
This shows that the time scaling in Figure 9.10(b) is infeasible; it demands more
deceleration than the actuators can provide at the state indicated.
For a minimum-time motion, the velocity s must be as high as possible
at every s while still satisfying the acceleration constraints and the endpoint
constraints. This implies that the time scaling must always operate at the
limit U (s, s)
or L(s, s),
and our only choice is when to switch between these
244
velocity
limit curve
U(s,s)
L(s,s)
0
(a)
(b)
Figure 9.10: (a) Acceleration-limited motion cones at four different states. The
upper ray of the cone is the sum of U (s, s)
plotted in the vertical direction
(change in velocity) and s plotted in the horizontal direction (change in position). The lower ray of the cone is constructed from L(s, s)
and s.
Points in
grey, bounded by the velocity limit curve, have L(s, s)
U (s, s)the
state is
inadmissible. On the velocity limit curve, the cone is reduced to a single tangent
vector. (b) The proposed time scaling is infeasible because the tangent to the
curve is outside the motion cone at the state indicated.
limits. A common solution is a bang-bang trajectory: maximum acceleration
U (s, s)
followed by a switch to maximum deceleration L(s, s).
(This is similar
to the trapezoidal motion profile that never reaches the coasting velocity v in
Section 9.2.2.2.) In this case, the time scaling is calculated by numerically
integrating U (s, s)
forward in s from (0, 0), integrating L(s, s)
backward in s
from (1, 0), and finding the intersection of these curves (Figure 9.11(a)). The
switch between maximum acceleration and maximum deceleration occurs at the
intersection.
In some cases, however, the velocity limit curve prevents a single-switch
solution (Figure 9.11(b)). These cases require an algorithm to find multiple
switching points.
9.4.2
The Time-Scaling Algorithm
Finding the optimal time scaling is reduced to finding the switches between
maximum acceleration U (s, s)
and maximum deceleration L(s, s),
maximizing
the height of the curve in the (s, s)
phase plane.
Time-scaling algorithm.
1. Initialize an empty list of switches S = {} and a switch counter i = 0. Set
(si , s i ) = (0, 0).
245
optimal bang-bang
time scaling
U(s,s)
L(s,s)
non-optimal
0
s*
(a)
s
(b)
Figure 9.11: (a) A time-optimal bang-bang time scaling integrates U (s, s)

from
(0, 0) and switches to L(s, s)
at a switching point s . Also shown is a nonoptimal time scaling with a tangent inside a motion cone. (b) Sometimes the
velocity limit curve prevents a single-switch solution.
2. Integrate the equation s = L(s, s)
backward in time from (1, 0) until L(s, s)
>
U (s, s)
(the velocity limit curve is penetrated) or s = 0. Call this phase plane
curve F .
3. Integrate the equation s = U (s, s)
forward in time from (si , s i ) until it crosses
F or until U (s, s)
< L(s, s)
(the velocity limit curve is penetrated). Call this
curve Ai . If Ai crosses F , then increment i, set (si , s i ) to the (s, s)
value at
which the crossing occurs, and append si to the list of switches S. This is a
switch from maximum acceleration to maximum deceleration. The problem
is solved and S is the set of switches expressed in the path parameter. If
instead the velocity limit curve is penetrated, let (slim , s lim ) be the point of
penetration and proceed to the next step.
4. Perform a binary search on the velocity in the range [0, s lim ] to find the
velocity s 0 such that the curve integrating s = L(s, s)
forward from (slim , s 0 )
touches the velocity limit curve without penetrating it. Set s high = s lim and
s low = 0.
(a) Set the test velocity halfway between slow and shigh , s test = (s high +
s low )/2. The test point is (slim , s test ).
(b) If the curve from the test point penetrates the velocity limit curve, set
s high equal to s test . If instead the curve from the test point hits s = 0,
set s low equal to s test . Return to step 4a.
Continue the binary search until a specified tolerance. Let (stan , s tan ) equal
the point where the resulting curve just touches the velocity limit curve
tangentially. The motion cone at this point is reduced to a single tangent
vector (L(s, s)
= U (s, s)),
tangent to the velocity limit curve.
5. Integrate s = L(s, s)
backwards from (stan , s tan ) until it intersects Ai . Increment i, set (si , s i ) to the (s, s)
value at the intersection, and label as Ai the
246
curve segment from (si , s i ) to (stan , s tan ). Append si to the list of switches
S. This is a switch from maximum acceleration to maximum deceleration.
6. Increment i and set (si , s i ) to (stan , s tan ). Append si to the list of switches
S. This is a switch from maximum deceleration to maximum acceleration.
Go to step 3.
The algorithm is illustrated in Figure 9.12.
9.4.3
Assumptions and Caveats
The description above covers the major points of the optimal time-scaling algorithm. A few assumptions were glossed over, however; they are made explicit
below.
Static posture maintenance. The algorithm, as described, assumes that the
robot can maintain its configuration against gravity at any state (s, s = 0).
This ensures the existence of valid time scalings, namely, time scalings
that move the robot along the path arbirarily slowly. For some robots
and paths, this assumption may be violated due to weak actuators. For
example, some paths may require momentum to carry motion through
configurations the robot cannot maintain statically. The algorithm can be
modified to handle cases where this assumption is violated.
Inadmissible states. The algorithm assumes that at every s there is a
unique velocity limit s lim (s) > 0 such that all velocities s s lim (s) are
admissible and all velocities s > s lim (s) are inadmissible. For some models
of actuator dynamics or friction, this assumption may be violatedthere
may be isolated islands of inadmissible states. The algorithm can be
modified to handle this case.
Zero-inertia points. The algorithm assumes no zero-inertia points (Equation (9.35)). If mi (s) = 0 in (9.35), then the torque provided by actuator
i has no dependence on the acceleration s, and the ith actuator constraint
in (9.34) directly defines a velocity constraint on s.
At a point s with
one or more zero components in m(s), the velocity limit curve is defined
by the minimum of (a) the velocity constraints defined by the zero-inertia
components and (b) the s values satisfying Li (s, s)
= Ui (s, s)
for the other
components. For the algorithm as described, singular arcs of zero-inertia
points on the velocity limit curve may lead to rapid switching between
s = U (s, s)
and s = L(s, s).
In such cases, choosing an acceleration tangent to the velocity limit curve, and between U (s, s)
and L(s, s),
preserves
time optimality without chattering controls.
It is worth noting that the time-scaling algorithm generates trajectories with
discontinuous acceleration, which could lead to vibrations. Beyond this, inaccuracies in models of robot inertial properties and friction make direct application
of the time-scaling algorithm impractical. Finally, since a minimum-time time
247
(slim, slim )
velocity
limit curve
A0
(s0 , s0 )
0
Step 2: i = 0, S = {}
(s1 , s1 )
.
(stan , stan )
A1
F
A0
(stan , stan )
F
A0
s
s1
Step 4: i = 0, S = {}
Step 5: i = 1, S = {s1}
A1
(s2 , s2 )
s1
s2
Step 6: i = 2, S = {s1, s2}
(s3 , s3 )
A1
F
A0
0
Step 3: i = 0, S = {}
A2
A0
s
acc
0
s1
dec
s2
acc
F
s3
dec
1
Step 3: i = 3, S = {s1, s2, s3}
Figure 9.12: The time-scaling algorithm. (Step 2) Integrating s = L(s, s)

backward from (1, 0) until the velocity limit curve is reached. (Step 3) Integrating
s = U (s, s)
forward from (0, 0) to the intersection (slim , s lim ) with the velocity
limit curve. (Step 4) Binary search to find (slim , s 0 ) from which s = L(s, s),
integrated forward from (slim , s 0 ), touches the velocity limit curve tangentially.
(Step 5) Integrating backward along L(s, s)
from (stan , s tan ) to find the first
switch from acceleration to deceleration. (Step 6) The second switch, from deceleration to acceleration, is at (s2 , s 2 ) = (stan , s tan ). (Step 3) Integrating forward along U (s, s)
from (s2 , s 2 ) results in intersection with F at (s3 , s 3 ), where
a switch occurs from acceleration to deceleration. The optimal time scaling
consists of switches at S = {s1 , s2 , s3 }.
248
scaling always saturates at least one actuator, if the robot gets off the planned
trajectory, there may be no torque left for corrective action.
Despite these drawbacks, the time-scaling algorithm provides a deep understanding of the true maximum capabilities of a robot following a path.
9.5
Summary
A trajectory (t), : [0, T ] , can be written as (s(t)), the composition

of a path (s), : [0, 1] , and a time scaling s(t), s : [0, T ] [0, 1].
A straight-line path in joint space can be written (s) = (1s)start +send ,
s [0, 1]. A similar form holds for straight-line paths in a minimum
set of task space coordinates. A straight-line path in SE(3), where
X = (R, p), can be decoupled to a Cartesian path and a rotation path:
p(s) = (1 s)pstart + spend
R(s) =
T
Rstart exp(log(Rstart
Rend )s).
(9.37)
(9.38)
A cubic polynomial s(t) = a0 +a1 t+a2 t2 +a3 t3 can be used to time-scale a

point-to-point motion with zero initial and final velocity. The acceleration
undergoes a step change (infinite jerk) at t = 0 and t = T , however. An
impulse in jerk can cause vibration of the robot.
A quintic polynomial s(t) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4 + a5 t5 can be used
to time-scale a point-to-point motion with zero initial and final velocities
and accelerations. Jerk is finite at t = 0 and t = T .
The trapezoidal motion profile is a popular time scaling in point-to-point
control, particularly control of a single motor. The motion consists of
three phases: constant acceleration, constant velocity, and constant deceleration, resulting in a trapezoid in s(t).
Trapezoidal motion involves step

changes in acceleration.
The S-curve motion profile is also popular in point-to-point control of a
motor. It consists of seven phases: (1) constant positive jerk, (2) constant
acceleration, (3) constant negative jerk, (4) constant velocity, (5) constant
negative jerk, (6) constant deceleration, and (7) constant positive jerk.
Given a set of via points including a start state, a goal state, and other
via states through which the robots motion must pass, as well as the
times Ti these states should be reached, a series of cubic polynomial time
scalings can be used to generate a trajectory (t) interpolating the via
points. To prevent step changes in the acceleration at the via points, a
series of quintic polynomials can be used instead.
9.5. Summary
249
Given a robot path (s), the dynamics of the robot, and limits on the
actuator torques, the actuator constraints can be expressed in terms of
(s, s)
as the vector inequalities
L(s, s)
s s U (s, s).
The time-optimal time scaling is the s(t) such that the height of the
curve in the (s, s)
phase plane is maximized while satisfying s(0) = s(0)
=
s(T
) = 0, s(T ) = 1, and the actuator constraints. The optimal solution always operates at maximum acceleration U (s, s)
or maximum deceleration
L(s, s).
250
(2,1)
y
(0,0)
(4,0)
x
(2,-1)
Figure 9.13: An elliptical path.
9.6
Exercises
1. Consider an elliptical path in the (x, y) plane. The path starts at (0, 0)
and proceeds clockwise to (2, 1), (4, 0), (2, 1), and back to (0, 0) (Figure 9.13).
Write the path as a function of s [0, 1].
2. A cylindrical path in X = (x, y, z) is given by x = cos 2s, y = sin 2s,
z = 2s, s [0, 1], and its time scaling is s(t) = 41 t + 81 t2 , t [0, 2]. Write X and
X.
3. Plot the acceleration as a function of time t [0, T ] for a point-to-point
cubic time scaling.
4. Consider a straight-line path (s) = (1 s)start + send , s [0, 1] from
start = (0, 0) to end = (, /3). The feasible joint velocities are |1 |, |2 |
2 rad/s and the feasible joint accelerations are |1 |, |2 | 0.5 rad/s2 . Find the
fastest motion time T using a cubic time scaling that satisfies the joint velocity
and acceleration limits.
5. Find the fifth-order polynomial time scaling that satisfies s(T ) = 1 and
s(0) = s(0)
= s(0) = s(T
) = s(T ) = 0.
6. As a function of the total time of motion T , find the times at which the acceleration s of the fifth-order polynomial point-to-point time scaling is maximum
and minimum.
7. If you want to use a polynomial time scaling for point-to-point motion
with zero initial and final velocity, acceleration, and jerk, what would be the
minimum order of the polynomial?
8. Prove that the trapezoidal time scaling, using the maximum allowable ac-
9.6. Exercises
251
celeration a and velocity v, minimizes the time of motion T .

9. Plot by hand the acceleration profile s(t) for a trapezoidal time scaling.
10. If v and a are specified for a trapezoidal time scaling, prove that v 2 /a 1
is a necessary condition for the robot to reach the maximum velocity v during
the path.
11. If v and T are specified for a trapezoidal time scaling, prove that vT > 1
is a necessary condition for the motion to be able to complete in time T . Prove
that vT 2 is a necessary condition for a three-stage trapezoidal motion.
12. If a and T are specified for a trapezoidal time scaling, prove that aT 2 4
is a necessary condition to ensure that the motion completes in time.
13. Consider the case where the maximum velocity v is never reached in a
trapezoidal time scaling. The motion becomes a bang-bang motion: constant
acceleration a for time T /2 followed by constant deceleration a for time T /2.
Write the position s(t), velocity s(t),
and acceleration s(t) for both phases,

analogous to Equations (9.15)-(9.23).
14. Plot by hand the acceleration profile s(t) for an S-curve time scaling.
15. A seven-stage S-curve is fully specified by the time tJ (duration of constant
positive or negative jerk), the time ta (duration of constant positive or negative
acceleration), the time tv (duration of constant velocity), the total time T , the
jerk J, the acceleration a, and the velocity v. Of these seven quantities, how
many can be specified independently? You may assume that any inequality
constraints needed for a seven-stage profile are satisfied.
16. A nominal S-curve has seven stages, but it can have fewer if certain inequality constraints are not satisfied. Indicate which cases are possible with fewer
than seven stages. By hand, approximately draw the s(t)
velocity profiles for

these cases.
17. If the S-curve achieves all seven stages and uses jerk J, acceleration a, and
velocity v, what is the constant velocity coasting time tv in terms of v, a, J,
and the total motion time T ?
18. Write your own via point cubic polynomial interpolation trajectory generator program for a 2-DOF robot. A new position and velocity specification is
required for each joint at 1000 Hz. The user specifies a sequence of via point
positions, velocities, and times, and the program generates an array consisting
of the joint angles and velocities at every 1 ms from time t = 0 to time t = T ,
252
B
0
c
1
Figure 9.14: A, B, and C are candidate integral curves, originating from the
dots indicated, while a, b, and c are candidate motion cones at s = 0. Two of
the integral curves and two of the motion cones are incorrect.
the total duration of the movement. For a test case with at least three via
points (start and end at rest, and at least one more via), plot (a) the path in
the joint angle space (similar to Figure 9.6) and (b) the position and velocity of
each joint as a function of time (similar to Figure 9.7).
19. Via points with specified positions, velocities, and accelerations can be
interpolated using fifth-order polynomials of time. For a fifth-order polynomial
segment between via points j and j +1, of duration Tj , with j , j+1 , j , j+1 ,
j , and j+1 specified, solve for the coefficients of the fifth-order polynomial
(similar to Equations (9.25)(9.28)). A symbolic math solver will simplify the
problem.
20. By hand or by computer, plot a trapezoidal motion profile in the (s, s)
plane.
21. Figure 9.14 shows three candidate motion curves in the (s, s)
plane (A, B,
and C) and three candidate motion cones at s = 0 (a, b, and c). Two of the
three curves and two of the three motion cones cannot be correct for any robot
dynamics. Indicate which are incorrect and explain your reasoning. Explain
why the others are possibilities.
22. Under the assumptions of Section 9.4.3, explain why the time-scaling algorithm of Section 9.4.2 is correct. In particular, (a) explain why in the binary
search of Step 4, the curve integrated forward from (slim , s test ) must either hit
(or run tangent to) the velocity limit curve or hit the s = 0 axis (and does not
hit the curve F , for example); (b) explain why the final time scaling can only
touch the velocity limit curve tangentially; and (c) explain why the acceleration
switches from minimum to maximum at points where the time scaling touches
9.6. Exercises
253
the velocity limit curve.

23. Explain how the time-scaling algorithm should be modified, if at all, to
handle the case where the initial and final velocity, at s = 0 and s = 1, are
nonzero.
24. Explain how the time-scaling algorithm should be modified if the robots
actuators are too weak to hold it statically at some configurations of the path
(static posture maintenance assumption is violated), but the assumptions on
inadmissible states and zero-inertia points are satisfied. Valid time scalings
may no longer exist. Under what condition(s) should the algorithm terminate
and indicate that no valid time scaling exists? (Under the assumptions of Section 9.4.3, the original algorithm always finds a solution, and therefore does not
check for failure cases.) What do the motion cones look like at states (s, s = 0)
where the robot cannot hold itself statically?
25. Create a computer program that plots the motion cones in the (s, s)
plane
for a 2R robot in a horizontal plane. The path is a straight line in joint space
from (1 , 2 ) = (0, 0) to (/2, /2). First derive the dynamics of the arm, then
.
The actuators can
rewrite the dynamics in terms of s, s,
s instead of , ,
provide torques in the range i,limit bi i i,limit bi , where b > 0
indicates the velocity dependence of the torque. The cones should be drawn at
a grid of points in (s, s).
To keep the figure manageable, normalize each cone
ray to the same length.
254
Chapter 10
Motion Planning
Motion planning is the problem of finding a robot motion from a start state
to a goal state while avoiding obstacles in the environment, joint limits, and
torque limits. Motion planning is one of the most active subfields of robotics,
and it is the subject of entire books. The purpose of this chapter is to provide a
practical overview of a few common techniques, using robot arms and wheeled
mobile robots as the primary example systems (Figure 10.1).
The chapter begins with a brief overview of motion planning, followed by
foundational material including configuration space obstacles, and concludes
with summaries of several different planning methods.
10.1
Overview of Motion Planning
A key concept in motion planning is configuration space, or C-space for short.

Every point in the C-space C corresponds to a unique configuration q of the
robot, and every configuration of the robot can be represented as a point in
C-space. For example, the configuration of a robot arm with n joints can be
represented as a list of n joint angles, q = (1 , . . . , n ). The free C-space Cfree
Figure 10.1: (Left) A robot arm executing a motion plan. (Right) A car-like
mobile robot parallel parking.
255
256
Motion Planning
consists of the configurations where the robot does not penetrate any obstacle
nor violate a joint limit.
In this chapter, unless otherwise stated, we assume that q is an n-vector and
that C Rn . With some generalization, however, the concepts of this chapter
apply to non-Euclidean C-spaces like C = SE(3) (n = 6).
The control inputs available to drive the robot are written as an m-vector
u U Rm , where m = n for a typical robot arm. If the robot has secondorder dynamics, like a robot arm, and the control inputs are forces (equivalently,
accelerations), the state of the robot is its configuration and velocity, x = (q, v)
X . For q Rn , typically we write v = q.
If we can treat the control inputs as
velocities, the state x is simply the configuration q. The notation q(x) indicates
the configuration q corresponding to the state x, and Xfree = {x | q(x) Cfree }.
The equations of motion of the robot are written
x = f (x, u),
(10.1)
or, in integral form,

Z
x(T ) = x(0) +
f (x(t), u(t))dt.
(10.2)
10.1.1
Types of Motion Planning Problems
Given the definitions above, a fairly broad specification of the motion planning
problem is the following:
Given an initial state x(0) = xstart and a desired final state xgoal , find a
time T and a set of controls u : [0, T ] U such that the motion (10.2) satisfies
x(T ) = xgoal and q(x(t)) Cfree for all t [0, T ].
The goal state xgoal can be replaced by a set of acceptable states, Xgoal .
It is assumed that a feedback controller (Chapter 11) is available to ensure
that the planned motion x(t), t [0, T ], is followed closely. It is also assumed
that an accurate geometric model of the robot and environment is available to
evaluate Cfree during motion planning.
There are many variations of the basic problem; some are discussed below.
Path planning vs. motion planning. The path planning problem is a subproblem of the general motion planning problem. Path planning is the
purely geometric problem of finding a collision-free path q(s), s [0, 1],
from a start configuration q(0) = qstart to a goal configuration q(1) = qgoal ,
without concern for dynamics, the duration of motion, or constraints on
the motion or control inputs. It is assumed that the path returned by the
path planner can be time scaled to create a feasible trajectory (Chapter 9).
This problem is sometimes called the piano movers problem, emphasizing
the focus on the geometry of cluttered spaces.
10.1. Overview of Motion Planning
257
Control inputs: m = n vs. m < n. If there are fewer control inputs m than
degrees of freedom n, the robot is incapable of following many paths, even
if they are collision-free. For example, a car has n = 3 (position and
orientation of the chassis in the plane) but m = 2 (forward/backward
motion and steering)it cannot slide directly sideways into a parking
space.
Online vs. offline. A motion planning problem requiring an immediate result,
perhaps because obstacles appear, disappear, or move unpredictably, calls
for a fast, online planner. If the environment is static, an offline planner
may suffice.
Optimal vs. satisficing. In addition to reaching the goal state, we might want
the motion plan to minimize (or approximately minimize) a cost J, e.g.,
Z T
J=
L(x(t), u(t))dt.
0
For example, minimizing with L = 1 yields a time-optimal motion while

minimizing with L = uT (r)u(r) yields a minimum-effort motion.
Exact vs. approximate. We may be satisfied with a final state x(T ) that is
sufficiently close to xgoal , e.g., kx(T ) xgoal k < .
With or without obstacles. The motion planning problem can be challenging even in the absence of obstacles, particularly if m < n or optimality is
desired.
10.1.2
Properties of Motion Planners
Planners must conform to the properties of the motion planning problem as

outlined above. In addition, planners can be distinguished by the following
properties:
Multiple-query vs. single-query planning. If the environment is unchanging and the robot will be asked to solve a number of motion planning
problems in the environment, it may be worth spending the time to build
a data structure that accurately represents Cfree . This data structure can
then be searched to efficiently solve multiple planning queries. Singlequery planners solve each new problem from scratch.
Anytime planning. An anytime planner is one that continues to look for
better solutions after a first solution is found. The planner can be stopped
at any time, for example when a specified time limit has passed, and the
best solution is returned.
Completeness. A motion planner is said to be complete if it is guaranteed to
find a solution in finite time if one exists and to report failure if not. A
weaker notion is resolution completeness. A planner is resolution complete
258
Motion Planning
if it is guaranteed to find a solution if one exists at the resolution of a
discretized representation of the problem, such as the resolution of a grid
representation of Cfree . Finally, a planner is probabilistically complete if
the probability of finding a solution, if one exists, tends to 1 as planning
time goes to infinity.
Computational complexity. The computational complexity of a planner refers

to characterizations of the amount of time the planner takes to run or the
amount of memory it requires. These are measured in terms of the description of the planning problem, such as the dimension of the C-space
or the number of vertices in the representation of the robot and obstacles. For example, the time for a planner to run may be exponential in
n, the dimension of the C-space. The computational complexity may be
expressed in terms of the average case or the worst case. Some planning
algorithms lend themselves easily to computational complexity analysis,
while others do not.
10.1.3
Motion Planning Methods
There is no single planner applicable to all motion planning problems. Below

is a broad overview of some of the many motion planners available. Details are
left to the sections indicated.
Complete methods (Section 10.3). These methods focus on exact representations of the geometry or topology of Cfree , ensuring completeness.
For all but simple or low-degree-of-freedom problems, these representations are mathematically or computationally prohibitive to derive.
Grid methods (Section 10.4). These methods discretize Cfree into a grid and
search the grid for a motion from qstart to a grid point in the goal region.
Modifications of the approach may discretize the state space or control
space, or use multi-scale grids to refine the representation of Cfree near
obstacles. These methods are relatively easy to implement and can return
optimal solutions, but for a fixed resolution, the memory required to store
the grid, and the time to search it, grows exponentially with the number
of dimensions of the space. This limits the approach to low-dimensional
problems.
Sampling methods (Section 10.5). A generic sampling method relies on a
random or deterministic function to choose a sample from the C-space
or state space; a function to evaluate whether the sample is in Xfree ; a
function to determine the closest previous free-space sample; and a local planner to try to connect to, or move toward, the new sample. This
process builds up a graph or tree representing feasible motions of the
robot. Sampling methods are easy to implement, tend to be probabilistically complete, and can even solve high-degree-of-freedom motion planning
problems. The solutions tend to be satisficing, not optimal, and it can be
difficult to characterize the computational complexity.
10.2. Foundations
259
Virtual potential fields (Section 10.6). Virtual potential fields create forces
on the robot that pull it toward the goal and push it away from obstacles.
The approach is relatively easy to implement, even for high-degree-offreedom systems, and fast to evaluate, often allowing online implementation. The drawback is local minima in the potential function: the robot
may get stuck in configurations where the attractive and repulsive forces
cancel, but the robot is not at the goal state.
Nonlinear optimization (Section 10.7). The motion planning problem can
be converted to a nonlinear optimization problem by representing the path
or controls by a finite number of design parameters, such as the coefficients
of a polynomial or a Fourier series. The problem is to solve for the design
parameters that minimize a cost function while satisfying constraints on
the controls, obstacles, and goal. While these methods can produce nearoptimal solutions, they require an initial guess at the solution. Because
the objective function or feasible solution space are generally not convex,
the optimization process can get stuck far away from a solution, let alone
an optimal solution.
Smoothing (Section 10.8). Often the motions found by a planner are jerky.
A smoothing algorithm can be run on the result of the motion planner to
improve the smoothness.
The major trend in recent years has been toward sampling methods, which
are easy to implement and can handle high-dimensional problems.
10.2
Foundations
Before discussing motion planning algorithms, we establish concepts used in

most of them: configuration space obstacles, collision detection, and graphs.
10.2.1
Configuration Space Obstacles
Determining whether a robot at a configuration q is in collision with a known

environment generally requires a complex operation involving a CAD model of
the environment and robot. There are a number of free and commercial software
packages that can perform this operation, and we will not delve into them here.
For our purposes, it is enough to know that the workspace obstacles partition
the configuration space C into
S two sets, the free space Cfree and the obstacle
space Cobs , where C = Cfree Cobs . Joint limits are treated as obstacles in the
configuration space.
With the concepts of Cfree and Cobs , the path planning problem becomes
the problem of finding a path for a point robot among the obstacles Cobs . If
the obstacles break Cfree into disconnected connected components, and qstart and
qgoal do not lie in the same connected component, then there is no collision-free
path.
260
Motion Planning
2
end
end
A
2
B
A
B
start
2
1
A
A
start
0
Figure 10.2: (Left) The joint angles of a 2R robot arm. (Middle) The arm
navigating among obstacles. (Right) The same motion in C-space.
For technical reasons, a configuration that has the robot just barely touching
an obstacle, without penetrating, is typically considered part of the C-space
obstacle (or C-obstacle for short). In other words, C-obstacles are closed (they
contain their boundaries), while Cfree is open: from any point in Cfree , it is
possible to move in any direction, perhaps only infinitesimally, while remaining
in Cfree .
The explicit mathematical representation of a C-obstacle can be exceedingly
complex, and for that reason C-obstacles are rarely represented exactly. Despite
this, the concept of C-obstacles is very important for understanding motion
planning algorithms. The ideas are best illustrated by examples.
10.2.1.1
A 2R Planar Arm
Figure 10.2 shows a 2R planar robot arm, with configuration q = (1 , 2 ), among

obstacles A, B, and C in the workspace. The C-space of the robot is represented
by a portion of the plane with 0 1 < 2, 0 2 < 2. In fact, however, the
topology of the C-space is a torus (or doughnut), since the edge of the square
at 1 = 2 is connected to the edge 1 = 0; similarly, 2 = 2 is connected
to 2 = 0. The square region of R2 is obtained by slicing the surface of the
doughnut twice, at 1 = 0 and 2 = 0, and laying it flat on the plane.
The C-space in Figure 10.2 shows the workspace obstacles A, B, and C
represented as C-obstacles. Any configuration inside a C-obstacle corresponds
to penetration of the obstacle by the robot arm in the workspace. A free path for
the robot arm from one configuration to another is shown in both the workspace
and C-space. The path and obstacles illustrate the topology of the C-space.
Note that the obstacles break Cfree into three connected components.
Although the C-space is drawn as a subset of R2 , it is correctly described as
1
S S 1 , where S k is the k-dimensional sphere embedded in Rk+1 . Thus S 1
is a circle on a plane, representing a single angle, and S 2 is a sphere in 3-space
10.2. Foundations
(x,y)
261
(x,y)
(a)
(b)
Figure 10.3: (a) A circular mobile robot (white) and a workspace obstacle (grey).
The configuration of the robot is represented by (x, y), the center of the robot.
(b) In the C-space, the obstacle is grown by the radius of the robot and the
robot is treated as a point. Any (x, y) configuration outside the dark boundary
is collision-free.
(e.g., the surface of the Earth). A robot with n revolute joints with no joint
limits has a C-space S 1 . . . S 1 (n times), which is written more compactly
as T n , the n-dimensional torus in Rn+1 . Note that S 1 S 1 = T 2 6= S 2 ; the
topology of a 2-torus and a 2-sphere are different.
10.2.1.2
A Circular Planar Mobile Robot
Figure 10.3 shows a top view of a circular mobile robot whose configuration is
given by the (x, y) R2 location of its center. The robot translates in a plane
with a single obstacle. The corresponding C-obstacle is obtained by growing
the workspace obstacle by the radius of the mobile robot. Any point outside
this C-obstacle represents a free configuration of the robot. Figure 10.4 shows
the workspace and C-space for two obstacles, indicating that the mobile robot
cannot pass between the two obstacles.
10.2.1.3
A Polygonal Planar Mobile Robot
Figure 10.5 shows the C-obstacle for a polygonal mobile robot translating in
the presence of a polygonal obstacle. The C-obstacle is obtained by sliding the
robot along the boundary of the of the obstacle and tracing the position of the
robots reference point.
10.2.1.4
A Polygonal Planar Mobile Robot That Rotates
Figure 10.6 illustrates the C-obstacle for the workspace obstacle and triangular
mobile robot of Figure 10.5 if the robot is now allowed to rotate. The C-space
is now three-dimensional, given by (x, y, ) R2 S 1 . The three-dimensional
C-obstacle is the union of two-dimensional C-obstacle slices at angles [0, 2).
262
Motion Planning
Figure 10.4: The grown C-space obstacles corresponding to two workspace obstacles and a circular mobile robot. The overlapping boundaries mean that the
robot cannot move between the two obstacles.
(x,y)
x
(x,y)
x
(a)
(b)
Figure 10.5: (a) The configuration of a triangular mobile robot, which can
translate but not rotate, is represented by the (x, y) location of a reference
point. Also shown is a workspace obstacle in grey. (b) The corresponding
C-space obstacle is obtained by sliding the robot around the boundary of the
obstacle and tracing the position of the reference point.
Even for this relatively low-dimensional C-space, an exact representation of the
10.2. Foundations
263
(x,y)
Figure 10.6: (Top) A triangular mobile robot that can rotate and translate,
represented by the configuration (x, y, ). (Left) The C-space obstacle from
Figure 10.5(b) when the robot is restricted to = 0. (Right) The full 3-D
C-space obstacle shown in slices at 10 increments.
C-obstacle is quite complex. For this reason, C-obstacles are rarely described
exactly.
10.2.2
Distance to Obstacles and Collision Detection
Given a C-obstacle B and a configuration q, let d(q, B) be the distance between

the robot and the obstacle, where
d(q, B) > 0
no contact with the obstacle
d(q, B) = 0
contact
d(q, B) < 0
penetration.
The distance could be defined as the Euclidean distance between the two closest
points of the robot and the obstacle.
A distance-measurement algorithm is one that determines d(q, B) for a given
B. A collision-detection routine determines whether d(q, Bi ) 0 for any Cobstacle Bi . A collision-detection routine returns a binary result, and may or
may not utilize a distance-measurement algorithm at its core.
One popular distance-measurement algorithm is called the GJK (GilbertJohnson-Keerthi) algorithm, which efficiently computes the distance between
two convex bodies, possibly represented by triangular meshes. Any robot or
obstacle can be treated as the union of multiple convex bodies. Extensions of
this algorithm are used in many distance-measurement algorithms and collisiondetection routines for robotics, graphics, and game physics engines.
264
Motion Planning
Figure 10.7: A lamp represented by spheres. The approximation improves as

the number of spheres used to represent the lamp increases.
An even simpler approach is to approximate the robot and obstacles as
unions of overlapping spheres. Approximations must always be conservative
the approximation must cover all points of the objectso that if a collisiondetection routine indicates a free configuration q, we are guaranteed that the
actual geometry is collision-free. As the number of spheres in the representation
of the robot and obstacles increases, the closer the approximations come to the
actual geometry. An example is shown in Figure 10.7.
Given a robot at q represented by k spheres of radius Ri centered at ri (q),
i = 1 . . . k, and an obstacle B represented by ` spheres of radius Bj centered at
bj , j = 1 . . . `, the distance between the robot and the obstacle can be calculated
as
d(q, B) = min kri (q) bj k Ri Bj .
i,j
Apart from determining whether a particular configuration of the robot is

in collision, another useful operation is determining whether the robot collides
during a particular motion segment. While exact solutions have been developed
for particular object geometries and motion types, the general approach is to
sample the path at finely spaced points and to grow the robot to ensure that
if two consecutive configurations are collision-free for the grown robot, then
the swept volume of the actual robot between the two configurations is also
collision-free.
10.2.3
Graphs and Trees
Many motion planners explicitly or implicitly represent the C-space or state

space as a graph. A graph consists of a collection of nodes N and a collection
of edges E, where each edge e connects two nodes. In motion planning, a node
typically represents a configuration or state, while an edge between nodes n1 and
10.3. Complete Path Planners

2.5
b
1
4
e
3
c
265
1.1
8
e
5
c
d
e
root
d
g
h
(a)
(b)
(c)
Figure 10.8: (a) A weighted digraph. (b) A weighted undirected graph. (c) A
tree. Leaves are shaded grey.
n2 indicates the ability to move from n1 to n2 without penetrating an obstacle
or violating other constraints.
A graph can be either directed or undirected. In an undirected graph, each
edge is bidirectional: if the robot can travel from n1 to n2 , then it can also
travel from n2 to n1 . In a directed graph, or digraph for short, each edge allows
travel in only one direction.
Graphs can also be weighted or unweighted. In a weighted graph, each edge
has its own positive cost associated with traversing it. In an unweighted graph,
each edge has the same cost (e.g., 1). Thus the most general type of graph we
consider is a weighted digraph.
A tree is a digraph in which (1) there are no cycles and (2) each node has at
most one parent node (i.e., at most one edge leading to the node). A tree has
one root node with no parents and a number of leaf nodes with no children.
A digraph, undirected graph, and tree are illustrated in Figure 10.8.
Given N nodes, any graph can be represented by a matrix A RN N , where
element aij of the matrix represents the cost of the edge from node i to node j
(a zero indicates no edge between the nodes). A tree can be represented more
compactly as a list of nodes, each with a link (and perhaps a cost) to at most
one parent and a list of links (and costs) to its children.
10.3
Complete Path Planners
Complete path planners rely on an exact representation of Cfree . These techniques tend to be mathematically and algorithmically sophisticated, and impractical for many real systems, so we do not delve into them in any detail.
One approach to complete path planning, which we will see in modified form
in Section 10.5, is based on representing the complex, high-dimensional space
Cfree by a one-dimensional roadmap R with the following properties:
(i) Reachability. From every point q Cfree , a free path to a point q 0 R can
be found trivially (e.g., a straight-line path).
(ii) Connectivity. For each connected component of Cfree , there is one connected component of R.
266
Motion Planning
goal
start
(a)
(b)
(c)
(d)
(e)
(f)
Figure 10.9: (a) The start and goal configurations for a square mobile robot
(reference point shown) in an environment with a triangular and a rectangular
obstacle. (b) The grown C-obstacles. (c) The visibility graph roadmap R of
Cfree . (d) The full graph consists of R plus nodes at qstart and qgoal , along with
the links connecting these nodes to visible nodes of R. (e) Searching the graph
results in the shortest path in bold. (f) The robot traversing the path.
With such a roadmap, the planner can find a path between any two points in
the same connected component of Cfree by simply finding paths from qstart to
0
0
0
0
to qgoal
on
R, from a point qgoal
R to qgoal , and from qstart
a point qstart
the roadmap R. If a path can be found trivially between qstart and qgoal , the
roadmap may not even be used.
While constructing a roadmap of Cfree is complex in general, some problems
admit simple roadmaps. For example, consider a polygon translating among
polygonal obstacles in the plane. As we have seen in Figure 10.5, the C-obstacles
in this case are also polygons. A suitable roadmap is the weighted undirected
visibility graph, with nodes at the vertices of the C-obstacles and edges between
nodes that can see each other (i.e., the line segment between the vertices
does not intersect an obstacle). The weight associated with each edge is the
Euclidean distance between the nodes.
Not only is this a suitable roadmap R, but it allows us to find a shortest
path between any two configurations in the same connected component of Cfree ,
as the shortest path is guaranteed to either be a straight line from qstart to qgoal ,
0
or consist of a straight line from qstart to a node qstart
R, a straight line from
0
0
a node qgoal R to qgoal , and a path along the straight edges of R from qstart
to
0
qgoal (Figure 10.9). Note that the shortest path requires the robot to graze the
obstacles, so we implicitly treat Cfree as closed (i.e., including its boundary).
10.3. Complete Path Planners
10.3.1
267
A Search
As with many path planners, central to the visibility graph planner is a graph
search. A popular search algorithm is A (pronounced A star), which efficiently finds a minimum-cost path on a graph when the cost of the path is
simply the sum of positive edge costs.
Given a graph described by a set of nodes N = {1, . . . , N }, where node
1 is the start node, and a set of edges E, A makes use of the following data
structures:
a sorted list OPEN of the nodes to be explored from, and a list CLOSED of
nodes that have already been explored from;
an array past_cost[node] of the minimum cost found so far to reach
node node from the start node; and
a search tree defined by an array parent[node], which contains a link for
each node to the node preceding it in the shortest path found so far to
node.
To initialize the search, the list OPEN is initialized to the start node 1, the cost to
reach the start node (past_cost[1]) is initialized as 0, and past_cost[node]
for node {2, . . . , N } is initialized as infinity (or a large number), indicating
that we currently have no idea of the cost to reach those nodes.
At each step of the algorithm, the first node in OPEN is removed from OPEN
and called current. The node current is also added to CLOSED. The first node
in OPEN is one that minimizes the total estimated cost of the best path to the
goal that passes through that node, and it is calculated as
est_total_cost[node] = past_cost[node] +
heuristic_cost_to_go(node)
where heuristic_cost_to_go(node) 0 is an optimistic (underestimating)
estimate of the actual cost-to-go to the goal from node. For the visibility graph
example, an appropriate choice for the heuristic is the straight-line distance to
the goal, ignoring any obstacles.
Because OPEN is a sorted list according to the estimated total cost, inserting
a new node at the correct location in OPEN entails a small computational price.
If the node current is in the goal set, then the search is finished, and the
path is reconstructed from the parent links. If not, for each neighbor nbr of
current in the graph, which is not also in CLOSED, the tentative_past_cost
for nbr is calculated as past cost[current] + cost[current,nbr]. If
tentative_past_cost < past_cost[nbr], then nbr can be reached less expensively than previously thought, so past_cost[nbr] is set to tentative_past_cost
and parent[nbr] is set to current. The node nbr is then added (or moved) in
OPEN according to its estimated total cost.
The algorithm proceeds by returning to pop off of OPEN the node with the
lowest estimated total cost. If OPEN is empty, then there is no solution.
268
Motion Planning
The A algorithm is guaranteed to return a minimum-cost path, as nodes

are only checked for inclusion in the goal set when they have the minimum
total estimated cost of all nodes. If the node current is in the goal set, then
heuristic_cost_to_go(current) is zero, and since all edge costs are positive,
we know that any path found in the future must have a cost greater than
or equal to past_cost[current]. Therefore the path to current must be a
shortest path. (There may be other paths of the same cost.)
If the heuristic cost-to-go is calculated exactly, considering obstacles, then
A will expand the minimum number of nodes necessary to solve the problem.
Of course, calculating the cost-to-go exactly is equivalent to solving the path
planning problem, so this is impractical. Instead, the heuristic cost-to-go should
be calculated quickly and should be as close as possible to the actual cost-togo to ensure that the algorithm runs efficiently. Using an optimistic cost-to-go
ensures an optimal solution.
A is an example of the general class of best-first searches, which always
explore from the node currently deemed best by some measure. Not all types
of best-first searches are guaranteed to return a minimum-cost path, however.
The A search algorithm is described in pseudocode in Algorithm 1.
Algorithm 1 A search.
1: OPEN {1}
2: past_cost[1] 0, past_cost[node] infinity for node {2, . . . , N }
3: while OPEN is not empty do
4:
current first node in OPEN, remove from OPEN
5:
add current to CLOSED
6:
if current is in the goal set then
7:
return SUCCESS and the path to current
8:
end if
9:
for each nbr of current not in CLOSED do
10:
tentative_past_cost past_cost[current] + cost[current,nbr]
11:
if tentative past cost < past cost[nbr] then
12:
past_cost[nbr] tentative_past_cost
13:
parent[nbr] current
14:
put (or move) nbr in sorted list OPEN according to
est_total_cost[nbr] past_cost[nbr] +
heuristic_cost_to_go(nbr)
15:
end if
16:
end for
17: end while
18: return FAILURE
10.3.2
Other Search Methods
Suboptimal A search. If the heuristic cost-to-go is overestimated by multiplying the optimistic heuristic by a constant factor > 1, A search
10.4. Grid Methods
269
will be biased to explore from nodes closer to the goal rather than nodes
with a low past cost. This may cause a solution to be found more quickly,
but unlike the case of an optimistic cost-to-go heuristic, the solution will
not be guaranteed to be optimal. One possibility is to run A with an
inflated cost-to-go to find an initial solution, then re-run the search with
progressively smaller values of until the time allotted for the search has
expired or a solution is found with = 1.
Dijkstras method. If the heuristic cost-to-go is always estimated as zero,
then A always explores from the OPEN node that has been reached with
minimum past cost. This variant is called Dijkstras algorithm, which
preceded A historically. Dijkstras algorithm is also guaranteed to find a
minimum-cost path, but on many problems it runs slower than A due to
the lack of a heuristic look-ahead function to help guide the search.
Breadth-first search. If each edge in E has the same cost, Dijkstras algorithm reduces to breadth-first search. All nodes one edge away from the
start node are considered first, then all nodes two edges away, etc. This
also results in a minimum-cost path.
Depth-first search. Depth-first search primarily applies to trees. The
search follows one path through the tree as far as it can go, until it reaches
a goal node or hits a leaf. If it hits a non-goal leaf, the search backtracks
up the tree to the most recent decision that has never been taken, then
follows a path down the tree again to a new leaf. The search terminates
when all leaves have been explored (failure) or a node in the goal set has
been reached. Depth-first search is rarely used in motion planning due to
the likelihood of finding long paths before shorter ones.
10.4
Grid Methods
A search like A requires a discretization of the search space. The simplest

discretization of C-space is a grid. For example, if the configuration space is
n-dimensional and we desire k grid points along each dimension, the C-space is
represented by k n grid points.
The A algorithm can be used as a path planner for a C-space grid, with
the following minor modifications:
The definition of a neighbor of a grid point must be chosen: is the robot
constrained to move in axis-aligned directions in configuration space, or
can it move in multiple dimensions simultaneously? For example, for a
two-dimensional C-space, neighbors could be 4-connected (on the cardinal points of a compass: north, south, east, and west) or 8-connected
(diagonals allowed), as shown in Figure 10.10(a). If diagonal motions are
allowed, the cost to diagonal neighbors should be penalized appropriately.
For example, the cost to a N/S/E/W neighbor could be 1, while the cost
270
Motion Planning
2
4-connected
an
lide
Euc
goal
Manhattan
8-connected
(a)
0
(b)
start
0
(c)
Figure 10.10: (a) A 4-connected grid point and an 8-connected grid point for a
space n = 2. (b) Grid points spaced
at unit intervals. The Euclidean distance
between the two points indicated is 5 while the Manhattan distance is 3. (c) A
grid representation of the C-space and a minimum-length Manhattan-distance
path for the problem of Figure 10.2.
to a diagonal neighbor could be 2. If integers are desired, for efficiency

of the implementation, the approximate costs 10 and 14 could be used.
If only axis-aligned motions are used, the heuristic cost-to-go should be
based on the Manhattan distance, not the Euclidean distance. The Manhattan distance counts the number of city blocks that must be traveled, considering that diagonals through a block are not possible (Figure 10.10(b)).
A node nbr is only added to OPEN if the step from current to nbr is
collision-free. (The step may be considered collision-free if a grown version
of the robot at nbr does not intersect any obstacles.)
Other optimizations can be achieved due to the known regular structure
of the grid.
An A grid-based path planner is resolution-complete: it will find a solution
if one exists at the level of discretization of the C-space. The path will be a
shortest path subject to the allowed motions.
Figure 10.10(c) illustrates grid-based path planning for the 2R robot example
of Figure 10.2. The C-space is represented as a grid with k = 32, i.e., a resolution
of 360 /32 = 11.25 for each joint. This yields a total of 322 = 1024 grid points.
The grid-based planner, as described, is a single-query planner: it solves
each path planning query from scratch. On the other hand, if the same qgoal
will be used in the same environment for multiple path planning queries, it may
10.4. Grid Methods
271
Figure 10.11: A wavefront planner on a 2D grid. The goal configuration is

given a score of 0. Then all collision-free 4-neighbors are given a score of 1. The
process continues, breadth-first, with each free neighbor (that does not have a
score already) assigned the score of its parent plus 1. Once every grid cell in
the connected component of the goal configuration is assigned a score, planning
from any location in the connected component is trivial: at every step, the
robot simply moves downhill to a neighbor with a lower score. Grid points in
collision receive a high score.
be worth preprocessing the entire grid to enable fast path planning. This is the
wavefront planner, illustrated in Figure 10.11.
Although grid-based path planning is easy to implement, it is only appropriate for low-dimensional C-spaces. This is because the number of grid points,
and hence the computational complexity of the path planner, increases exponentially with the number of dimensions n. For instance, a resolution k = 100 in
a C-space with n = 3 dimensions leads to 1 million grid nodes, while n = 5 leads
to 10 billion grid nodes and n = 7 leads to 100 trillion nodes. An alternative
is to reduce the resolution k along each dimension, but this leads to a coarse
representation of C-space that may to miss free paths.
10.4.1
Multi-Resolution Grid Representation
One way to reduce the computational complexity of a grid-based planner is to

use a multi-resolution grid representation of Cfree . Conceptually, a grid point
is considered an obstacle if any part of the rectangular cell centered on the
grid point touches a C-obstacle. To refine the representation of the obstacle, an
obstacle cell can be subdivided into smaller cells. Each dimension of the original
cell is split in half, resulting in 2n sub-cells for an n-dimensional space. Any of
the cells that are still in contact with a C-obstacle are then subdivided further,
up to a specified maximum resolution.
The advantage of this representation is that only portions of C-space near
obstacles are refined to high resolution, while portions away from obstacles
272
Motion Planning
original cell
subdivision 1
subdivision 2
subdivision 3
original cell
subdivision 1
subdivision 2
subdivision 3
Figure 10.12: At the original C-space cell resolution, a small obstacle (indicated
by a dark square) causes the whole cell to be labeled an obstacle. Subdividing
the cell once shows that at least 3/4 of the cell is actually free. Three levels of
subdivision results in a representation using ten total cells: four at subdivision
level 3, three at subdivision level 2, and three at subdivision level 1. The cells
shaded grey are the obstacle cells in the final representation. The subdivision
of the original cell is also shown as a tree, where the leaves of the tree are the
final cells in the representation.
are represented by a coarse resolution. This allows the planner to find paths
using short steps through cluttered spaces while taking large steps through wide
open space. The idea is illustrated in Figure 10.12, which uses only 10 cells to
represent an obstacle at the same resolution as a fixed grid that uses 64 cells.
For n = 2, this multiresolution representation is called a quadtree, as each
obstacle cell subdivides into 2n = 4 cells. For n = 3, each obstacle cell subdivides
into 2n = 8 cells, and the representation is called an octree.
The multi-resolution representation of Cfree can be built in advance of the
search or incrementally as the search is being performed. In the latter case, if
the step from current to nbr is found to be in collision, the step size can be
halved until the step is free or the minimum step size is reached.
10.4.2
Grid Methods with Motion Constraints
The previous grid-based planners operate under the assumption that the robot
can go from a cell to any neighbor cell in a regular C-space grid. This may
not be possible for some robots. For example, a car cannot reach, in one step,
a neighbor cell that is to the side of it. Also, motions for a fast-moving
robot arm should be planned in the state space, not just C-space, to take the
arm dynamics into account. In the state space, the robot arm cannot move in
10.4. Grid Methods
273
Figure 10.13: Sample trajectories emanating from three initial states in the
phase space of a dynamic system with q R. If the initial state has q > 0,
the trajectory cannot move to the left (negative motion in q) instantaneously.
Similarly, if the initial state has q < 0, the trajectory cannot move to the right
instantaneously.
v
unicycle
v
diff-drive robot
v
car
Reeds-Shepp car
Figure 10.14: Discretizations of the control sets for unicycle, diff-drive, and
car-like robots.
certain directions (Figure 10.13).
Grid-based planners must be adapted to account for the motion constraints
of the particular robot. In particular, the constraints may result in a directed
grid graph, unlike the undirected graphs discussed so far. One approach is to
discretize the robot controls while still making use of a grid on the C-space or
state space, as appropriate. Details for a wheeled mobile robot and a dynamic
robot arm are described next.
10.4.2.1
Grid-Based Path Planning for a Wheeled Mobile Robot
In Chapter ??, we saw that the controls for simplified models of unicycle, diffdrive, and car-like robots are (v, ), the forward-backward linear velocity and
the angular velocity. The control sets for these mobile robots are shown in
Figure 10.14. Also shown are proposed discretizations of the controls, as dots.
Other discretizations could be chosen.
Using the control discretization, the A search can be modified slightly to a
Dijkstra algorithm (Algorithm 2).
Dubins car
274
Motion Planning
Algorithm 2 Grid-based Dijkstra planner for a wheeled mobile robot.

1: OPEN {qstart }
2: past_cost[qstart ] 0
3: counter 1
4: while OPEN is not empty and counter < MAXCOUNT do
5:
current first node in OPEN, remove from OPEN
6:
if current is in the goal set then
7:
return SUCCESS and the path to current
8:
end if
9:
if current is not in a previously occupied C-space grid cell then
10:
mark grid cell occupied
11:
counter counter + 1
12:
for each control in the discrete control set do
13:
integrate control forward a short time t from current to qnew
14:
if the path to qnew is collision-free then
15:
compute cost of the path to qnew
16:
place qnew in OPEN, sorted by cost
17:
parent[qnew ] current
18:
end if
19:
end for
20:
end if
21: end while
22: return FAILURE
The search expands from qstart by integrating forward each of the controls for
a time t, creating new nodes for the paths that are collision-free. Each node
keeps track of the control used to reach the node as well as the cost of the path
to the node. The cost of the path to a new node is the sum of the cost of the
previous node current plus the cost of the action, which is typically a constant
(due to the constant integration time t) plus perhaps a penalty if the action
changed from the one used to reach current. The constant term expresses the
preference for paths of short duration, while the penalty expresses a preference
for smooth paths minimizing control changes, particularly direction reversals.
Integration of the controls does not move the mobile robot to exact grid
points. Instead, the C-space grid comes into play in lines 9 and 10. When a
node is expanded, the grid cell it sits in is marked occupied. In future, any
node in this occupied cell will be pruned from the search. This prevents the
search from expanding nodes that are close by nodes reached with a lower cost.
If there is a cost for changing the control from one step to the next, then
we have to be careful about when we prune a node in an occupied cell, since
the path cost is no longer simply the sum of edge costs. One solution is to
maintain several copies of the C-space grid, one for each control in the discrete
set. Then a node current is only pruned from the search if its grid cell had
been previously occupied (explored from) by a node previous and if previous
had been reached by the same control step that current used.
10.4. Grid Methods
275
start
goal
Figure 10.15: (Left) A minimum-cost path for a car-like robot where each action
has identical cost, favoring a short path. (Right) A minimum-cost path where
reversals are heavily penalized.
No more than MAXCOUNT nodes, where MAXCOUNT is a value chosen by the
user, are considered during the search.
The time t should be chosen small enough that each motion step is small.
The size of the grid cells should be chosen as large as possible while ensuring
that integration of any control for time t will move the mobile robot outside
of its current grid cell.
The planner terminates when current lies inside the goal region, when there
are no more nodes left to expand (perhaps due to obstacles), or when MAXCOUNT
nodes have been considered. Any path found is optimal for the choice of cost
function and other parameters to the problem. The planner actually runs faster
in somewhat cluttered spaces, as the obstacles help guide the exploration.
Some examples of motion plans for a car are shown in Figure 10.15.
10.4.2.2
Grid-Based Motion Planning for a Robot Arm
One method for planning the motion for a robot arm is to decouple the problem
into a path planning problem followed by a time-scaling of the path:
(i) Apply a grid-based or other path planner to find an obstacle-free path in
C-space.
(ii) Time scale the path to find the fastest trajectory along the path that
respects the robots dynamics, as described in Chapter 9.4. Or use any
less aggressive time scaling.
Since the motion planning problem is broken into two steps (path planning plus
time scaling), the resultant motion will not be time-optimal in general.
Another approach is to plan directly in the state space. Given a state (q, q)
of the robot arm, let A(q, q)

represent the set of feasible accelerations based on
the limited joint torques. To discretize the controls, the set A(q, q)
is intersected
276
Motion Planning
..
q2
a1
a2
A(q,q )
..q
Figure 10.16: The instantaneously available acceleration set A(q, q)

for a twojoint robot, intersected with a grid spaced at a1 in q1 and a2 in q2 , gives the
discretized control actions shown in bold.
with a grid of points of the form
n
X
ci a i e i ,
i=1
where ci is an integer, ai > 0 is the acceleration step size in the qi direction,

and ei is a unit vector in the ith direction (Figure 10.16).
As the robot moves, the acceleration set A(q, q)
changes, but the grid remains
fixed. Because of this, and assuming a fixed integration time t at each step
in a motion plan, the reachable states of the robot (after any integral number of
steps) are confined to a grid in state space. To see this, consider a single joint
angle of the robot, q1 , and assume for simplicity zero initial velocity q1 (0) = 0.
The velocity at timestep k takes the form
q1 (k) = q1 (k 1) + c1 (k)a1 t,
where c1 (k) is chosen from a finite set of integers. By induction, the velocity at
any timestep must be of the form a1 kv t, where kv is an integer. The position
at timestep k takes the form
1
q1 (k) = q1 (k 1) + q1 (k 1)t + c1 (k)a1 (t)2 .
2
Plugging in the form of the velocity, we find that the position at any timestep
must be of the form a1 kp (t)2 /2 + q1 (0), where kp is an integer (Exercise ??).
To find a trajectory from a start node to a goal set, a breadth-first search
can be employed to create a search tree on the state space nodes. When a node
(q, q)
in the state space is explored from, the set A(q, q)
is evaluated to find the
discrete set of control actions. New nodes are created by integrating the control
10.5. Sampling Methods
277
actions for time t. A node is discarded if the path to it is in collision or if it

has been reached previously (i.e., by a trajectory taking the same or less time).
Because the joint angles and angular velocities are bounded, the state space
grid is finite, and therefore it can be searched in finite time. The planner
is resolution complete and returns a time-optimal trajectory, subject to the
resolution constraints in the control grid and timestep t.
The control grid stepsizes ai must be chosen small enough that A(q, q),
for
any feasible state (q, q),
contains a representative set of points of the control
grid. Choosing a finer grid for the controls, or a smaller timestep t, creates a
finer grid in the state space and a higher likelihood of finding a solution amidst
obstacles. It also allows choosing a smaller goal set while keeping points of the
state space grid inside the set.
Finer discretization comes at a computational cost, however. If the resolution of the control discretization is increased by a factor of r in each dimension
(i.e., each ai is reduced to ai /r), and the timestep size is divided by a factor of
, the computation time spent growing the search tree for a given robot motion
duration increases by a factor of rn , where n is the number of joints. For example, increasing the control grid resolution by a factor of r = 2 and decreasing
the timestep by a factor of = 4 for a three-joint robot results in a search that
is likely to take 234 = 4096 times longer to complete. The high computational
complexity of the planner makes it impractical beyond a few degrees of freedom.
The description above ignores one important issue: the feasible control set
A(q, q)
changes during a timestep, so the control chosen at the beginning of
the timestep may no longer be feasible by the end of the timestep. For that
q)
reason, a conservative approximation A(q,
A(q, q)
should be used instead.
This set should remain feasible over the duration of a timestep regardless of
which control action is chosen. How to determine a conservative approximation
q)
A(q,
is beyond the scope of this chapter, but it has to do with bounds on how
rapidly the arms inertia matrix M (q) changes with q and how fast the robot is
q)
moving. At low speeds q and short durations t, the conservative set A(q,
is
very close to A(q, q).
10.5
Sampling Methods
Each of the grid-based methods discussed above delivers optimal solutions subject to the chosen discretization. A drawback of the approaches is their high
computational complexity, making them unsuitable for systems of more than a
few degrees of freedom.
A different class of planners, known as sampling methods, relies on a random
or deterministic function to choose a sample from the C-space or state space;
a function to evaluate whether a sample or motion is in Xfree ; a function to
determine nearby previous free-space samples; and a simple local planner to try
to connect to, or move toward, the new sample. This process builds up a graph
or tree representing feasible motions of the robot.
Sampling methods generally give up on the resolution-optimal solutions of
278
Motion Planning
a grid search in exchange for the ability to find satisficing solutions quickly in
high-dimensional state spaces. The samples are chosen to form a roadmap or
search tree that quickly approximates the free space Xfree using fewer samples
than would typically be required by a fixed high-resolution grid, where the
number of grid points increases exponentially with the dimension of the search
space. Most sampling methods are probabilistically complete: the probability of
finding a solution, when one exists, approaches 100% as the number of samples
goes to infinity.
Two major classes of sampling methods are rapidly-exploring random trees
(RRTs) and probabilistic roadmaps (PRMs). RRTs use a tree representation for
single-query planning in either C-space or state space, while PRMs are primarily
C-space planners that create a roadmap graph for multiple-query planning.
10.5.1
The RRT
The RRT algorithm searches for a collision-free motion from an initial state
xstart to a goal set Xgoal . It applies to kinematic problems, where the state
x is simply the configuration q, as well as dynamic problems, where the state
includes the velocity. The basic RRT grows a single tree from xstart as outlined
in Algorithm 3.
Algorithm 3 RRT algorithm.
1: initialize search tree T with xstart
2: while T is less than the maximum tree size do
3:
xsamp sample from X
4:
xnearest nearest node in T to xsamp
5:
employ a local planner to find a motion from xnearest to xnew in
the direction of xsamp
6:
if the motion is collision-free then
7:
add xnew to T with an edge from xnearest to xnew
8:
if xnew is in Xgoal then
9:
return SUCCESS and the motion to xnew
10:
end if
11:
end if
12: end while
13: return FAILURE
In a typical implementation for a kinematic problem (where x is simply q),
the sampler in line 3 chooses xsamp randomly from an almost-uniform distribution over X , with a slight bias toward states in Xgoal . The closest node xnearest in
T (line 4) is the one minimizing the Euclidean distance to xsamp . The state xnew
(line 5) is chosen as the state a small distance d from xnearest on the straight line
to xsamp . Because d is small, a very simple local planner, e.g., one that returns
a straight line motion, will often find a motion connecting xnearest to xnew . If
the motion is collision-free, the new state xnew is added to T .
279
Figure 10.17: (Left) A tree generated by applying a uniformly-distributed random motion from a randomly chosen tree node results in a tree that does not
explore very far. (Right) A tree generated by the RRT algorithm using samples
drawn randomly from a uniform distribution. Both trees have 2000 nodes.
The net effect is that the nearly uniformly distributed samples pull the
tree toward them, causing the tree to rapidly explore Xfree . An example of the
effect of this pulling action on exploration is shown in Figure 10.17.
The basic algorithm leaves many choices: how to sample from X (line 3),
how to define the nearest node in T (line 4), and how to plan the motion
to make progress toward xsamp (line 5). Even a small change to the sampling
method, for example, can yield a dramatic change in the running time of the
planner. A wide variety of planners have been proposed in the literature based
on these choices and other variations. Some of these variations are described
below.
10.5.1.1
Line 3: The Sampler
The most obvious sampler is one that samples randomly from a uniform distribution over X . This is straightforward for Euclidean C-spaces Rn ; for n-joint
robot C-spaces T n = S 1 ... S 1 (n times), where we can choose a uniform
distribution over each joint angle; and for the C-space R2 S 1 for a mobile
robot in the plane, where we can choose a uniform distribution over R2 and
S 1 individually. The notion of a uniform distribution on some other curved
C-spaces, notably SO(3), are less straightforward.
For dynamic systems, a uniform distribution over the state space can be defined as the cross-product of a uniform distribution over C-space and a uniform
distribution over a bounded velocity set.
Although the name rapidly-exploring random trees gets its name from
the idea of a random sampling strategy, the samples need not be generated
randomly. For example, a deterministic sampling scheme that generates a progressively finer (multi-resolution) grid on X could be employed instead. To
reflect this more general view, the approach has been called rapidly-exploring
dense trees (RDTs), emphasizing the key point that the samples should even-
280
Motion Planning
Figure 10.18: Which of the three dashed configurations of the car is closest
to reaching the configuration in grey?
tually become dense in the state space (i.e., as the number of samples goes to
infinity, the samples become arbitrarily close to every point in X ).
10.5.1.2
Line 4: Defining the Nearest Node
Finding the nearest node depends on a definition of distance on X . For an

unconstrained kinematic robot on C = Rn , a natural choice for the distance
between two points is simply the Euclidean distance. For other spaces, the
choice is less obvious.
As an example, for a car-like robot with a C-space R2 S 1 , which configuration is closest to the configuration xsamp : one that is rotated twenty degrees
relative to xsamp , one that is 2 meters straight behind it, or one that is 1 meter
straight to the side of it (Figure 10.18)? Since the motion constraints prevent
spinning in place or moving directly sideways, the configuration that is 2 meters
straight behind is best positioned to make progress toward xsamp . Thus defining
a notion of distance requires
combining components of different units (e.g., degrees, meters, degrees/second,
meters/second) into a single distance measure; and
taking into account the motion constraints of the robot.
The closest node xnearest should perhaps be defined as the one that can reach
xsamp the fastest, but computing this is as hard as solving the motion planning
problem.
A simple choice of a distance measure from x to xsamp is the weighted sum of
the distances along the different components of xsamp x. The weights choose
the relative importance of the different components. If more is known about
the time-limited reachable sets of a motion-constrained robot from a state x,
this information can be used in determining the nearest node. In any case,
the nearest node should be computed quickly. Finding a nearest neighbor is a
common problem in computational geometry, and various algorithms, such as
kd trees and hashing, can be used to solve it efficiently.

10.5.1.3
281
Line 5: The Local Planner
The job of the local planner is to find a motion from xnearest to some point
xnew which is closer to xsamp . The planner should be simple and it should run
quickly. Two examples are:
Straight-line planner. This is for kinematic systems with no motion constraints. The plan is a straight line to xnew , which may be chosen at
xsamp or at a fixed distance d from xnearest on the straight line to xsamp .
Discretized controls. For systems with motion constraints, such as wheeled
mobile robots or dynamic systems, the controls can be discretized into a
discrete set {u1 , u2 , . . .}, as in the grid methods with motion constraints
(Section 10.4.2 and Figures 10.14 and 10.16). Each control is integrated
from xnearest for a fixed time t using x = f (x, u). The resulting state
that is closest to xsamp is chosen as xnew .
Wheeled robot planners. For a wheeled mobile robot, local plans can be
found using Reeds-Shepp curves or polynomial functions of time of the
differentially flat output, as described in Chapter ??.
Other robot-specific local planners can be designed.
10.5.1.4
Other RRT Variants
The performance of the basic RRT algorithm depends heavily on the choice
of the sampling method, the distance measure, and the local planner. Beyond
these choices, two other variants of the basic RRT are outlined below.
Bidirectional RRT. The bidirectional RRT grows two trees: one forward
from xstart and one backward from xgoal . The algorithm alternates between
growing the forward tree and the backward tree, and every so often attempts
to connect the two trees by choosing xsamp from the other tree. The advantage
of this approach is that a single goal state xgoal can be reached exactly, rather
than just a goal set Xgoal . Another advantage is that in many environments,
the two trees are likely to find each other much faster than a single forward
tree will find a goal set.
The major problem is that the local planner might not be able to connect
the two trees exactly. For example, the discretized controls planner of Section 10.5.1.3 is highly unlikely to create a motion exactly to a node in the other
tree. In this case, the two trees may be considered more-or-less connected when
points on each tree are sufficiently close. The broken discontinuous trajectory
can be returned and patched by a smoothing method (Section 10.8).
RRT . The basic RRT algorithm returns SUCCESS once a motion to Xgoal
is found. An alternative is to continue running the algorithm and to terminate
the search only when another termination condition is reached (e.g., maximum
running time or maximum tree size). Then the motion with the minimum cost
282
Motion Planning
Figure 10.19: (Left) The tree generated by an RRT after 20,000 nodes. The goal
region is the square at the top right corner, and the shortest path is indicated.
(Right) The tree generated by RRT after 20,000 nodes.
can be returned. In this way, the RRT solution may continue to improve as
time goes by. Because edges in the tree are never deleted or changed, however,
the RRT generally does not converge to an optimal solution.
RRT is a variation of the single-tree RRT that continually rewires the search
tree to ensure that it always encodes the shortest path from xstart to each node
in the tree. The basic approach works for C-space path planning with no motion
constraints, allowing exact paths from any node to any other node.
To modify the RRT to the RRT , line 7 of the RRT algorithm, which inserts
xnew in T with an edge from xnearest to xnew , is replaced by a test of all nodes
x Xnear in T that are sufficiently near to xnew . An edge to xnew is created
from the x Xnear that (1) has a collision-free motion by the local planner and
(2) minimizes the total cost of the path from xstart to xnew , not just the cost of
the added edge. The total cost is the cost to reach the candidate x Xnear plus
the cost of the new edge.
The next step is to consider each x Xnear to see if it could be reached by
lower cost by a motion through xnew . If so, the parent of x is changed to xnew .
In this way, the tree is incrementally rewired to eliminate high-cost motions in
favor of the minimum-cost motions available so far.
The definition of Xnear depends on the number of samples in the tree, details
of the sampling method, the dimension of the search space, and other factors.
Unlike the RRT, the solution provided by RRT approaches the optimal
solution as the number of sample nodes increases. Like the RRT, RRT is
probabilistically complete. The time to produce a rewired tree by the RRT
algorithm is within a constant factor of the time to produce a tree of the same
size by the RRT. Figure 10.19 demonstrates the rewiring behavior of RRT
compared to RRT for a simple example in C = R2 .
10.5.2
283
The PRM
The PRM uses sampling to build a roadmap representation of Cfree (Section 10.3)
before answering any specific queries. The roadmap is an undirected graph: the
robot can move in either direction along any edge exactly from one node to the
next. For this reason, PRMs primarily apply to kinematic problems for which
an exact local planner exists that can find a path (ignoring obstacles) from any
q1 to any q2 . The simplest example is a straight-line planner for a robot with
no kinematic constraints.
Once the roadmap is built, a particular start node qstart can be added to
the graph by attempting to connect it to the roadmap, starting with the closest
node. The same is done for the goal node qgoal . The graph is then searched for
a path, typically using A . Thus the query can be answered efficiently once the
roadmap has been built.
PRMs allow the possibility of building a roadmap quickly and efficiently
relative to constructing a roadmap using a high-resolution grid representation.
This is because the volume fraction of the C-space that is visible by the local
planner from a given configuration does not typically decrease exponentially
with increasing dimension of the C-space.
The algorithm to construct a roadmap R with N nodes is outlined in Algorithm 4 and illustrated in Figure 10.20.
Algorithm 4 PRM roadmap construction algorithm (undirected graph).
1: for i = 1 . . . N do
2:
qi sample from Cfree
3:
add qi to R
4: end for
5: for i = 1 . . . N do
6:
N (qi ) k closest neighbors of qi
7:
for each q N (qi ) do
8:
if there is a collision-free local path from q to qi and
there is not already an edge from q to qi then
9:
add an edge from q to qi to the roadmap R
10:
end if
11:
end for
12: end for
13: return R
A key choice in the PRM roadmap construction algorithm is how to sample
from Cfree . While the default might be sampling randomly from a uniform
distribution on C and eliminating configurations in collision, it has been shown
that sampling more densely near obstacles can improve the likelihood of finding
narrow passages, thus significantly reducing the number of samples needed to
properly represent the connectivity of Cfree . Another option is deterministic
multi-resolution sampling.
284
Motion Planning
Figure 10.20: An example PRM roadmap for a point robot in C = R2 . The

k = 3 closest neighbors are considered for connection to a sample node q. The
degree of a node can be greater than three since it may be a close neighbor of
many nodes.
10.6
Virtual Potential Fields
Virtual potential field methods are inspired by potential energy fields in nature,
such as gravitational and magnetic fields. From physics we know that a potential
field U (q) defined over C induces a force F = U/q that drives an object from
high to low potential. For example, if h is the height above the Earths surface in
a uniform gravitational potential field (g = 9.81 m/s2 ), then the potential energy
of a mass m is U (h) = mgh and the force acting on it is F = U/h = mg.
The force will cause the mass to fall to the Earths surface.
In robot motion control, the goal configuration qgoal is assigned a low virtual
potential energy and obstacles are assigned a high virtual potential. Applying a
force to the robot proportional to the negative gradient of the virtual potential
naturally pushes the robot toward the goal and away from the obstacles.
A virtual potential field is very different from the planners we have seen so
far. Typically the gradient of the field can be calculated quickly, so the motion
can be calculated in real time (reactive control) instead of planned in advance.
With appropriate sensors, the method can even handle obstacles that move or
appear unexpectedly. The drawback of the basic method is that the robot can
get stuck in local minima of the potential field, away from the goal, even when
a feasible motion to the goal exists. In certain cases it is possible to design the
potential to guarantee that the only local minimum is at the goal, eliminating
this problem.
10.6. Virtual Potential Fields
10.6.1
285
A Point in C-space
Lets begin by assuming a point robot in its C-space. A goal configuration qgoal
is typically encoded by a quadratic potential energy bowl with zero energy at
the goal,
1
Ugoal (q) = (q qgoal )T K(q qgoal ),
2
where K is a symmetric positive-definite weighting matrix (for example, the
identity matrix). The force induced by this potential is
Fgoal (q) =
Ugoal
= K(qgoal q),
q
an attractive force proportional to the distance from the goal.

The repulsive force induced by a C-obstacle B can be calculated from the
distance d(q, B) to the obstacle (Section 10.2.2):
UB (q) =
k
2d2 (q, B)
where k > 0 is a scaling factor. The potential is only properly defined for points
outside the obstacle, d(q, B) > 0. The force induced by the obstacle potential is
FB (q) =
d
UB
k
= 3
.
q
d (q, B) q
The total potential is obtained by summing the attractive goal potential and
the repulsive obstacle potentials,
X
U (q) = Ugoal (q) +
UBi (q),
i
yielding a total force

F (q) = Fgoal (q) +
FBi (q).
A bowl-like attractive potential and obstacle repulsive potentials are illustrated

for a point in R2 in *** Figure ***.
To actually control the robot using the calculated F (q), we have several
options, two of which are:
Apply the calculated force plus damping,
u = F (q) + B q,
(10.3)
where B is a positive semidefinite matrix. If B is positive definite, then

it dissipates energy for all q 6= 0, reducing oscillation and guaranteeing
that the robot will come to rest. If B = 0, the robot continues to move
while maintaining constant total energy, which is the sum of the initial
kinetic energy 21 qT (0)M (q(0))q(0)
plus the initial virtual potential energy

U (q(0)).
286
Motion Planning
Treat the calculated force as a commanded velocity instead:
q = F (q).
(10.4)
This automatically eliminates oscillations.

Figure *** shows the motion of point robots in the plane using the velocity
control law (10.4). It also shows a local minimum of the potential, which is the
main drawback of potential fields.
It is important to note that obstacle potentials go to infinity at the boundaries of the obstacles, resulting in steep gradients and therefore large FB (q) near
an obstacle boundary. A simple solution is to saturate FB (q) after calculating
it. Another issue is that even distant obstacles have a small effect on the motion
of the robot. To speed up evaluation of the repulsive terms, distant obstacles
could be ignored. We can define a range of influence of the obstacles drange > 0
so that the potential is zero for all d(q, B) drange :

2
k drange d(q,B)
if d(q, B) < drange

2 drange d(q,B)
UB (q) =
0
otherwise.
Another issue is that d(q, B) and its gradient is generally difficult to calculate.
An approach to dealing with this is described in Section ??.
10.6.2
Navigation Functions
A significant problem with the potential field method is local minima. While
potential fields may be appropriate for relatively uncluttered spaces, or for rapid
response to unexpected obstacles, they are likely to get the robot stuck in local
minima for many practical applications.
One exception is the wavefront planner of Figure 10.11. The wavefront
algorithm creates the equivalent of a potential function, with high potential at
grid points in obstacles and zero potential at the goal. If a solution exists to
the motion planning problem, then simply moving downhill at every step is
guaranteed to bring the robot to the goal.
Another approach to gradient-following planning is based on replacing the
virtual potential function with a navigation function. A navigation function
(q) is a type of virtual potential function that
(i) is smooth (or at least twice-differentiable) on q;
(ii) has a bounded maximum value (e.g., 1) on the boundaries of all obstacles;
(iii) has a single minimum at qgoal ; and
(iv) has a full-rank Hessian 2 /q 2 at all critical points q where /q = 0
(i.e., (q) is a Morse function).
10.6. Virtual Potential Fields
287
Figure 10.21: Examples of critical points in two dimensions: a maximum, a

saddle (minimum in some directions and maximum in others), and a minimum.
*** From Choset book, replace
Condition 1 ensures that the Hessian 2 /q 2 exists. Condition 2 puts an upper
bound on the virtual potential energy of the robot. The key conditions are 3
and 4. Condition 3 ensures that of the critical points of (q) (including minima,
maxima, and saddles, as shown in Figure 10.21), there is only one minimum, at
qgoal . This ensures that qgoal is at least locally attractive. However, there may
be saddle points which are minima along a subset of directions. Condition 4
ensures that the set of initial states that are attracted to any saddle point has
empty interior (zero measure), and therefore almost every initial state converges
to the unique minimum qgoal .
While constructing navigation potential functions with only a single minimum is non-trivial, Rimon and Koditschek showed how to construct them for
the particular case of an n-dimensional Cfree consisting of all points inside an
n-sphere of radius R and outside of smaller spherical obstacles Bi of radius ri
centered at qi , i.e., {q Rn | kqk R and kq qi k > ri for all i}. This is called
a sphere world. While a real C-space is unlikely to be a sphere world, Rimon
and Koditschek showed that the boundaries of the obstacles, and the associated
navigation function, can be deformed to a much broader class of star-shaped
obstacles. A star-shaped obstacle is one that has a center point from which
the line segment to any point on the obstacle boundary is contained completely
within the obstacle. A star world is a C-space which has star-shaped obstacles.
Thus finding a navigation function for an arbitrary star world reduces to finding a navigation function for a model sphere world that has centers at the
centers of the star-shaped obstacles, then stretching and deforming that navi-
288
Motion Planning
Figure 10.22: (Left) A model sphere world with five circular obstacles. The
contour plot of a navigation function is shown. The goal is at (0, 0). Note
that the obstacles induce saddle points near the obstacles, but no local minima.
(Right) A star world obtained by deforming the obstacles and the potential
while retaining a navigation function.
gation function to one that fits the star world. Rimon and Koditschek give a
systematic procedure for accomplishing this.
Figure 10.22 shows a deformation of a navigation function on a model sphere
world to a star world for the case C R2 .
10.6.3
Workspace Potential
A difficulty in calculating the repulsive force from an obstacle is calculating the

distance to the obstacle, d(q, B). One approach that avoids the exact calculation
is to represent the boundary of an obstacle as a set of point obstacles, and to
represent the robot by a small set of control points. Let the Cartesian location
of control point i on the robot be written fi (q) R3 and boundary point j of the
obstacle be cj R3 . Then the distance between the two points is kfi (q) cj k,
and the potential at the control point i due to the obstacle point j is
0
Uij
(q) =
k
,
2kfi (q) cj k2
yielding the repulsive force at the control point

Fij0 (q) =
k
Uij
=
(fi (q) cj ).
q
kfi (q) cj k4
To turn the linear force Fij0 (q) R3 into a generalized force Fij (q) Rn
acting on the robot arm or mobile robot, we first find the Jacobian Ji (q) relating
q to the linear velocity of the control point fi :
fi
fi =
q = Ji (q)q.
10.7. Nonlinear Optimization
289
By the principle of virtual work, the generalized force Fij (q) due to the repulsive
linear force Fij0 (q) is simply
Fij (q) = JiT (q)Fij0 (q).
Now the total force F (q) acting on the robot is the sum of the easily calculated
attractive force Fgoal (q) and the repulsive forces Fij (q) for all i and j.
10.6.4
Wheeled Mobile Robots
The preceding analysis assumes that a control force u = F (q) + B q (for control
law (10.3)) or a velocity q = F (q) (for control law (10.4)) can be applied in any
direction. If the robot is a wheeled mobile robot subject to rolling constraints
A(q)q = 0, however, the calculated F (q) must be projected to controls Fproj (q)
that move the robot tangent to the constraints. For a kinematic robot employing
the control law q = Fproj (q), a suitable projection is

1
Fproj (q) = I AT (q) A(q)AT (q)
A(q) F (q).
For a dynamic robot employing the control law u = Fproj (q)+B q,
the projection
is discussed in Chapter 11.4.2.
10.6.5
Use of Potential Fields in Planners
A potential field can be used in conjunction with a path planner. For example,
a best-first search like A can use the potential as an estimate of the cost-to-go.
Incorporating search prevents the planner from getting permanently stuck in
local minima.
10.7
Nonlinear Optimization
The motion planning problem can be expressed as a general nonlinear optimization with equality and inequality constraints, taking advantage of a number of
software packages to solve such problems. Nonlinear optimization problems can
be solved by gradient-based methods, like sequential quadratic programming
(SQP), and non-gradient methods, like simulated annealing, Nelder-Mead optimization, and genetic programming. Like many nonlinear optimization problems, however, these methods are not generally guaranteed to find a feasible
solution when one exists, let alone an optimal one. For methods that use gradients of the objective function and constraints, however, we can expect a locally
optimal solution if we start the process with a guess that is close to a solution.
290
Motion Planning
The general problem can be written
find
minimizing
subject to
u(t), q(t), T
(10.5)
J(u(t), q(t), T )
(10.6)
x(t)
= f (x(t), u(t))
t [0, T ]
(10.7)
u(t) U
t [0, T ]
(10.8)
q(t) Cfree
t [0, T ]
(10.9)
x(0) = xstart
(10.10)
x(T ) = xgoal .
(10.11)
To approximately solve this problem by nonlinear optimization, the control u(t), trajectory q(t), and equality and inequality constraints (10.7)(10.11)
must be discretized. This is typically done by ensuring that the constraints are
satisfied at a fixed number of points distributed evenly over the interval [0, T ],
and by choosing a finite-parameter representation of the position and/or control histories. We have three choices of how to parameterize the position and
controls:
(i) Parameterize the trajectory q(t). In this case, we solve for the parameterized trajectory directly. The control forces u(t) at any time are calculated
using the equations of motion. This approach does not apply to systems
with fewer controls than configuration variables, m < n.
(ii) Parameterize the control u(t). We solve for u(t) directly. Calculating the
state x(t) requires integrating the equations of motion.
(iii) Parameterize both q(t) and u(t). We have a larger number of variables,
since we are parameterizing both q(t) and u(t). Also, we have a larger
number of constraints, as q(t) and u must satisfy the dynamic equations
x = f (x, u) explicitly, typically at a fixed number of points distributed
evenly over the interval [0, T ]. We must be careful to choose the parameterizations of q(t) and u(t) to be consistent with each other, so that the
dynamic equations can be satisfied at these points.
A trajectory or control history can be parameterized in any number of ways.
The parameters can be the coefficients of a polynomial in time, the coefficients
of a truncated Fourier series, spline coefficients, wavelet coefficients, piecewise
constant acceleration or force segments, etc. For example, the control ui (t)
could be represented by p + 1 coefficients aj of a polynomial in time:
ui (t) =
p
X
aj tj .
j=0
In addition to the parameters for the state or control history, the total time
T may be another control parameter. The choice of parameterization has implications for the efficiency of the calculation of q(t) and u(t) at a given time t.
10.8. Smoothing
291
The choice of parameterization also determines the sensitivity of the state and
control to the parameters, and whether each parameter affects the profiles at all
times [0, T ] or just on a finite-time support base. These are important factors
in the stability and efficiency of the numerical optimization.
10.8
Smoothing
The axis-aligned motions of a grid planner and the randomized motions of sampling planners may lead to jerky motion of the robot. One approach to dealing
with this issue is to let the planner handle the work of searching globally for a
solution, then post-process the resulting motion to make it smoother.
There are many ways to do this; two possibilities are outlined below.
Nonlinear optimization. While gradient-based nonlinear optimization may
fail to find a solution if initialized with a random initial trajectory, it can make
an effective post-processing, since the plan initializes the optimization with a
reasonable solution. The initial motion must be converted to a parameterized
representation of the controls, and the cost J(u(t), q(t), T ) can be expressed as
a function of u(t) or q(t). For example,
Z
J=
u T (t)u(t)dt
penalizes the rate of control change. This has an analogy in human motor
control, where the smoothness of human arm motions has been attributed to
minimizing the rate of change of acceleration of the joints.
Subdivide and reconnect. A local planner can be used to attempt a connection between two distant points on a path. If this new connection is collisionfree, it replaces the original path segment. Since the local planner is designed
to produce short, smooth paths, the new path is likely shorter and smoother
than the original. This test-and-replace procedure can be applied iteratively to
randomly chosen points on the path. Another possibility is to use a recursive
procedure that subdivides the path first into two pieces and attempt to replace
each piece with a shorter path; then, if either portion could not be replaced by
a shorter path, subdivide again; etc.
10.9
Summary
A fairly general statement of the motion planning problem is: Given an

initial state x(0) = xstart and a desired final state xgoal , find a time T
and a set of controls u : [0, T ] U such that the motion (10.2) satisfies
x(T ) Xgoal and q(x(t)) Cfree for all t [0, T ].
292
Motion Planning
Motion planning problems can be classified in the following categories:
path planning vs. motion planning; fully actuated vs. constrained or underactuated; online vs. offline; optimal vs. satisficing; exact vs. approximate; with or without obstacles.
Motion planners can be characterized by the following properties: multiplequery vs. single-query; anytime planning or not; complete, resolution complete, probabilistically complete, or none of the above; and computational
complexity.
Obstacles partition
S the C-space into free C-space Cfree and obstacle space
Cobs , C = Cfree Cobs . Obstacles may split Cfree into multiple connected
components. There is no feasible path between configurations in different
connected components.
A conservative check of whether a configuration q is in collision uses a simplified grown representation of the robot and obstacles. If there is no
collision between the grown bodies, then the configuration is guaranteed
collision-free. Checking if a path is collision-free usually involves sampling
the path at finely spaced points, and ensuring that if the individual configurations are collision-free, then the swept volume of the robot path is
collision-free.
The C-space geometry is often represented by a graph consisting of nodes
and edges between the nodes, where edges represent feasible paths. The
graph can be undirected (edges flow both directions) or directed (edges
only flow one direction). Edges can be unweighted or weighted according
to their cost of traversal. A tree is a directed graph with no cycles and
where each node has at most one parent.
A roadmap path planner uses a graph representation of Cfree , and any path
planning problem can be solved using a simple path from qstart onto the
roadmap, a path along the roadmap, and a simple path from the roadmap
to qgoal .
A is a popular search method that finds minimum-cost paths on a graph.
It operates by always exploring from the node that is (1) unexplored and
(2) expected to be on a path with minimum estimated total cost. The
estimated total cost is the sum of edge weights to reach the node from the
start node plus an estimate of the cost-to-go to the goal. The cost-to-go
estimate should be optimistic to ensure that the search returns an optimal
solution.
A grid-based path planner discretizes the C-space into a graph consisting
of neighboring points on a regular grid. A multi-resolution grid can be
used to allow large steps in wide-open spaces while allowing smaller steps
near obstacle boundaries.
10.9. Summary
293
Discretizing the control set allows robots with motion constraints to take
advantage of grid-based methods. If integrating a control does not land
the robot exactly on a grid point, the new state may still be pruned if a
state in the same grid cell was already achieved with a lower cost.
The basic RRT algorithm grows a single search tree from xstart to find a
motion to Xgoal . It relies on a sampler to find a sample xsamp in X ; an
algorithm to find the closest node xnearest in the search tree; and a local
planner to find a motion from xnearest to a point closer to xsamp . The
sampling is chosen to cause the tree to explore Xfree quickly.
The bidirectional RRT grows a search tree from both xstart and xgoal and
attempts to join them up. RRT returns solutions that tend toward the
optimal as planning time goes to infinity.
The PRM builds a roadmap of Cfree for multiple-query planning. The
roadmap is built by sampling Cfree N times, then using a local planner to
attempt to connect each sample with several of its nearest neighbors. The
roadmap is searched for plans using A .
Virtual potential fields are inspired by potential energy fields such as gravitational and electromagnetic fields. The goal point creates an attractive
potential while obstacles create a repulsive potential. The total potential U (q) is the sum of these, and the virtual force applied to the robot
is F (q) = U/q. The robot is controlled by applying this force plus
damping, or by simulating first-order dynamics and driving the robot with
F (q) as a velocity. Potential field methods are conceptually simple but
tend to result in local minima where the robot gets stuck away from the
goal.
A navigation function is a potential function with no local minima. Navigation functions result in near-global convergence to qgoal . While navigation functions are difficult to design in general, they can be designed
systematically for certain systems.
Motion planning problems can be converted to general nonlinear optimization problems with equality and inequality constraints. While optimization methods can be used to find smooth, near-optimal motions, they tend
to get stuck in local minima in cluttered C-spaces. Optimization methods
typically require a good initial guess at a solution.
Motions returned by grid-based and sampling-based planners tend to be
jerky. Smoothing the plan using nonlinear optimization or subdivide-andreconnect can improve the quality of the motion.
294
Motion Planning
reference
point
Figure 10.23: Exercise 4.
10.10
Exercises
1. A path is homotopic to another if it can be continuously deformed into the

other without moving the endpoints. In other words, it can be stretched and
pulled like a rubber band, but it cannot be cut and pasted back together. For
the C-space of Figure 10.2, draw a path from the start to the goal that is not
homotopic to the one shown.
2. Label the connected components in Figure 10.2. For each connected component, draw a picture of the robot for one configuration in the connected
component.
3. Assume that 2 joint angles in the range [175 , 185 ] result in self-collision
for the robot of Figure 10.2. Draw the new joint limit C-obstacle on top of the
existing C-obstacles and label the resulting connected components of Cfree . For
each connected component, draw a picture of the robot for one configuration in
the connected component.
4. Draw the C-obstacle corresponding to the obstacle and translating planar
robot in Figure 10.23.
5. Write a program that accepts as input the coordinates of a polygonal robot
(relative to a reference point on the robot) and the coordinates of a polygonal
obstacle and produces as output a drawing of the corresponding C-space obstacle. In Mathematica, you may find the function ConvexHull useful. In Matlab,
try convhull.
6. Calculating a square root is typically computationally expensive. For a
robot and an obstacle represented as collections of spheres (Section 10.2.2),
provide a method for calculating the distance between the robot and obstacle
10.10. Exercises
295
goal
start
Figure 10.24: Planning problem for Exercise 8.

that minimizes the use of square roots.
7. For Figure 10.8(c), give the order that the nodes in the tree are visited in
a complete breadth-first search and a complete depth-first search. Assume that
leftmost branches are always followed first.
8. Draw the visibility roadmap for the C-obstacles and qstart and qgoal in Figure 10.24. Indicate the shortest path.
9. Not all edges of the visibility roadmap described in Section 10.3 are needed.
Prove that an edge between two vertices of C-obstacles need not be included in
the roadmap if either end of the edge does not hit the obstacle tangentially. In
other words, if the edge ends by colliding with an obstacle, it will never be
used in a shortest path.
10. You will implement an A path planner for a point robot in a plane with
obstacles. The planar region is a 100100 area. The program will generate a
graph consisting of N nodes and E edges, where N and E are chosen by the
user. After generating N randomly chosen nodes, the program should connect
randomly chosen nodes with edges until E unique edges have been generated.
The cost associated with each edge is the Euclidean distance between the nodes.
Finally, the program should display the graph, search the graph using A for the
shortest path between nodes 1 and N , and display the shortest path or indicate
FAILURE if no path exists. The heuristic cost-to-go is the Euclidean distance
to the goal.
11. Modify the A planner in Exercise 10 to use a heuristic cost-to-go equal
to ten times the distance to the goal node. Compare the running time to the
original A when they are run on the same graphs. Are the solutions found with
296
Motion Planning
the new heuristic optimal?

12. Modify the A algorithm from Exercise 10 to use Dijkstras algorithm
instead. Comment on the relative running time between A and Dijkstras
algorithm when each is run on the same graphs.
13. Write a program that accepts the vertices of polygonal obstacles from a
user, as well as the specification of a 2R robot arm, rooted at (x, y) = (0, 0), with
link lengths L1 and L2 . Each link is simply a line segment. Generate the C-space
obstacles for the robot by sampling the two joint angles at k-degree intervals
(e.g., k = 5) and checking for intersection between the line segments and the
polygon. Plot the obstacles in the workspace and the C-space grid using a black
square or dot for C-obstacles. (Hint: At the core of this program is a subroutine
to see if two line segments intersect. If the segments corresponding infinite lines
intersect, you can check if this intersection is within the line segments.)
14. Write an A grid path planner for the 2R robot with obstacles, and display
found paths on the C-space. (See Exercise 13 and Figure 10.10.)
Chapter 11
Robot Control
A robot arm can exhibit a number of different behaviors, depending on the task
and its environment. It can act as a source of programmed motions for tasks
such as moving an object from one place to another, or tracing a trajectory for
a spray paint gun. It can act as a source of forces, as when applying a polishing
wheel to a workpiece. In tasks such as writing on a chalkboard, it must control
forces in some directions (the force pressing the chalk against the board) and
motions in others (motion in the plane of the board). When the purpose of the
robot is to act as a haptic display, mimicking a virtual environment, we may
want it to act like a spring, damper, or mass, yielding in response to forces
applied to it.
In each of these cases, it is the job of the robot controller to convert the task
specification to forces and torques at the actuators. Control strategies to achieve
the behaviors described above are known as motion (or position) control, force
control, hybrid motion-force control, and impedance control. Which of these
behaviors is appropriate depends on both the task and the environment. For
example, a force control goal makes sense when the end-effector is in contact with
something, but not when it is moving in free space. We also have a fundamental
constraint imposed by mechanics, irrespective of the environment: the robot
cannot independently control both motions and forces. If the robot imposes a
motion, then the environment will determine the force, and vice-versa.
Once we have chosen a control goal consistent with the task and environment, we have a number of ways to achieve it. Feedback control uses position,
velocity, and force sensors to measure the actual behavior of the robot, compare
it to the desired behavior, and modulate the control signals sent to the actuators. Feedback is used in nearly all robot systems. Feedforward control uses a
model of the dynamics of the robot and its environment to determine actuator
signals that achieve the desired change in state. Because of modeling errors,
feedforward control is rarely used by itself, but it is often used in conjunction
with feedback control. Complementary control strategies include adaptive control, which continuously estimates properties of the dynamic system to improve
performance; robust control to guarantee some level of performance in the face
297
298
Robot Control
of an uncertain model of the system; and iterative learning control for repetitive tasks, where errors from previous executions of the same task are used to
generate more appropriate feedforward controls for future iterations.
In this chapter we focus on feedback and feedforward control for motion
control, force control, hybrid motion-force control, and impedance control.
11.1
Control System Overview
A typical control block diagram is shown in Figure 11.1(a). The controller is

often a PC or microcontroller. Sensors are typically potentiometers, encoders, or
resolvers for joint position/angle sensing; tachometers for joint velocity sensing;
strain gauge joint force/torque sensors; and/or multi-axis force-torque sensors
at the wrist between the end of the arm and the end-effector. The controller
samples the sensors and updates its control signals to the actuators at a rate
of hundreds to a few thousands of Hz. In most robotic applications, higher
control update rates are of limited benefit, given time constants associated with
the dynamics of the robot and environment. In our analysis, we will ignore
the nonzero sampling time and treat controllers as if they are implemented in
continuous time.
While tachometers can be used for direct velocity sensing, a common approach is to use a digital filter to numerically difference position signals at
successive time steps. A low-pass filter is often used in combination with the
differencing filter to reduce high frequency jitter due to quantization.
Actuators could be brushed DC motors, brushless DC motors, various types
of AC motors, hydraulic actuators, or pneumatic actuators, among others. Typically gears or other transmissions are used to lower the speed and increase the
force or torque of the actuator. Electric motors are coupled with a power amplifier that converts signals from the controller to high currents to drive the motor.
Local feedback of motor current or joint torque may be used in an inner control
loop to achieve the forces or torques requested by the controller.
For each robot joint, we will lump the amplifier, actuator, and transmission
together and treat them as a transformer from low-power control signals to forces
and torques. This assumption, along with the assumption of perfect sensors,
allows us to simplify the block diagram to the one shown in Figure 11.1(b), where
the controller produces forces and torques directly. The rest of this chapter deals
with the control algorithms that go inside the Controller box in Figure 11.1(b).
Real robot systems are subject to flexibility and vibrations in the joints
and links, backlash at gears and transmissions, actuator saturation limits, and
limited resolution of the sensors. These are significant issues in design and
control, but are beyond the scope of this chapter.
11.2. Motion Control
299
high
power
controls
low
power
controls
desired
behavior
controller
amplifiers
local
feedback
actuators
and
transmissions
forces
and
torques
dynamics of
arm and
environment
motions
and
forces
sensors
(a)
forces
and
torques
desired
behavior
controller
dynamics of
arm and
environment
motions
and
forces
(b)
Figure 11.1: (a) A typical robot control system. An inner control loop may be
used to help the amplifier and actuator achieve the desired force or torque. For
example, a DC motor amplifier in torque control mode may sense the current
actually flowing through the motor and implement a local controller to better
match the desired current, since the current is proportional to the torque produced by the motor. (b) A simplified model with ideal sensors and a controller
block that directly produces forces and torques. This assumes ideal behavior of
the amplifier and actuator blocks in part (a).
11.2
Motion Control
Typically a motion control task is to have the end-effector follow a desired

trajectory. If the robot is redundant, there may be more than one set of joint
variable histories that yield this end-effector trajectory. We begin by assuming
that inverse kinematics and its derivatives, possibly with redundancy resolution,
have already been applied to yield a unique set of target joint histories as a
function of time. Our goal is to construct controllers that drive the robot to track
this trajectory in joint space. We consider the case of a trajectory expressed in
the task space in Section 11.2.3.
The ideas are well illustrated by a robot with a single joint, so we begin
there, then generalize to a multi-joint robot.
300
Robot Control
mg
Figure 11.2: A single joint robot rotating in a gravity field.
11.2.1
Motion Control of a Single Joint
Consider a single motor attached to a single link, as shown in Figure 11.2. Let
be the motors torque and be the angle of the link. The dynamics can be
written as
= M + mgr cos ,
(11.1)
where M is the inertia of the link about the axis of rotation, m is the mass of
the link, r is the distance from the axis to the center of mass of the link, and
g 0 is gravitational acceleration.
According to the model (11.1), there is no dissipation: if the link is made
to spin and = 0, it would spin forever. This is unrealistic, of course; there
is friction at the various bearings, gears, and transmissions. Friction modeling
is an active research area, but a simple model of rotational friction is viscous
friction,
fric = b,
(11.2)
where b > 0. Adding the friction torque, our final model is
= M + mgr cos + b,
(11.3)
which we may write more compactly as
= M + h(, ),
(11.4)
where h contains all terms that depend only on the state, not the acceleration.
For concreteness in the following simulations, we set M = 0.5 kgm2 , m =
1 kg, r = 0.1 m, and b = 0.1 Nms/rad. In some examples, the link moves in
a horizontal plane, so g = 0. In other examples, the link moves in a vertical
2
plane, so g = 9.81 m/s .
11.2.1.1
Feedback Control: PID Control
The most common feedback control algorithm is linear PID (proportional-integralderivative) control. Defining the error between the desired angle d and actual
angle as
e = d ,
(11.5)
+
_
301
Kp
+
+
arm
dynamics
dt
d
dt
Ki
Kd
Figure 11.3: Block diagram of a PID controller.

the PID controller is simply
Z
= Kp e + Ki
e (t)dt + Kd e ,
(11.6)
where the control gains Kp , Ki , and Kd are nonnegative. The proportional

gain Kp acts as a virtual spring that tries to reduce the position error d ,
and the derivative gain Kd acts as a virtual damper that tries to reduce the
The integral gain, as we will see later, can be used to
velocity error d .
eliminate steady-state errors when the joint is at rest. See the block diagram in
Figure 11.3.
For now lets consider the case where Ki = 0. This is known as PD control.
(We can similarly define PI, P, I, and D control by setting other gains to zero.
PD and PI control are the most common variants of PID control.) Lets also
assume the robot moves in a horizontal plane, so g = 0. Plugging the control
law (11.6) into the dynamics (11.3), we get
M + b = Kp (d ) + Kd (d ).
(11.7)
If the goal state is rest at a constant d , then d = d = 0. This is called

and e = ,
Equation (11.7) can
setpoint control. Using e = d , e = ,
be rewritten as the linear mass-spring-damper error dynamics
M e + (b + Kd )e + Kp e = 0.
(11.8)
Stability Error dynamics, such as Equation (11.8), are an important concept

in the study of control systems. A minimum requirement is that the error
dynamics be stable, i.e., initial errors tend to zero exponentially with time. A
linear homogeneous ordinary differential equation of the form
an e(n) + an1 e(n1) + . . . + a2 e + a1 e + a0 e = 0
302
Robot Control
is stable if and only if all of the complex roots s1 , . . . , sn of its characteristic

equation
an sn + an1 sn1 + . . . + a2 s2 + a1 s + a0 = 0
have real components less than zero, i.e., Re(si ) < 0 for all i = 1 . . . n. A
necessary condition for stability, regardless of the order n of the dynamics, is
that ai > 0 for all i. This condition is also sufficient for second-order dynamics
such as (11.8). For third-order dynamics, it is also required that a2 a1 > a3 a0 .
PD Control and Second-Order Error Dynamics To study the secondorder error dynamics (11.8) more formally, we assume stability and rewrite in
the standard second-order form
Kp
b + Kd
e +
e = 0
e +
M
M
e + 2n e + n2 e = 0,
(11.9)
where the damping ratio and the natural frequency n are

r
Kp
b + Kd
p
,
n =
.
=
M
2 Kp M
The characteristic equation of (11.9) is
s2 + 2n s + n2 = 0,
(11.10)
with complex roots

s1,2 = n n
2 1.
There are three types of solutions to the differential equation (11.9), depending on whether the roots s1,2 are real and unequal ( > 1), real and equal
( = 1), or complex conjugates ( < 1):
Overdamped: > 1. The roots s1,2 are real and distinct, and the
solution is
e (t) = c1 exp(s1 t) + c2 exp(s2 t),
where c1 and c2 depend on the initial conditions. The response is the sum
of two decaying exponentials, with time constants 1,2 = 1/s1,2 , where
the time constant is the time it takes the exponential to decay to 37% of
its original value. The slower time constant
in the solution is given by
p
the less negative root, s1 = n + n 2 1.
Critically damped: = 1. The roots s1,2 = n are equal and real,
and the solution is
e (t) = exp(n t)(c1 + c2 t),
i.e., a decaying exponential multiplied by a linear function of time. The
time constant of the decaying exponential is = 1/(n ).
303
ess
0
overshoot
2% settling time
Figure 11.4: The error response to a step input for an underdamped secondorder system, showing steady-state error ess , overshoot, and 2% settling time.
Underdamped: < 1. The
p roots s1,2 are complex conjugates at s1,2 =
n d , where d = n 1 2 is the damped natural frequency. The
solution is
e (t) = exp(n t) (c1 cos(d t) + c2 sin(d t)) ,
i.e., a decaying exponential (time constant = 1/(n )) multiplied by a
sinusoid.
To see how to apply these solutions, imagine that the link is originally at rest
at = 0. At time t = 0, the desired position is suddenly changed from d = 0 to
d = 1. This is called a step input and the resulting motion of the system (t)
is called the step response. Our interest is in the error response e (t). We can
solve for c1,2 in the specific solution by solving e (0) = 1 (the error immediately
becomes 1) and e (0) = 0 (both d (0) and (0)

are zero).
The error response can be described by a transient response and a steadystate response (Figure 11.4). The steady-state response is characterized by the
steady-state error ess , which is the asymptotic error e (t) as t . For the
link in zero gravity with a stable PD controller, ess = 0. The transient response
is characterized by the overshoot and (2%) settling time. The 2% settling time
is the first time T such that |e (t) ess | 0.02(1 ess ) for all t T , and is
approximately equal to 4 , where is the slowest time constant in the solution.
Overshoot is defined as

e,min ess
100%
overshoot =
1 ess
304
Robot Control
Im(s)
increasing
oscillation
1.2
1
0.8
e
1
2
3
1
faster
response
Re(s)
unstable
0.6
0.4
0.2
0
0.2
0.1
0.2
0.3
0.4
0.5
0.6
time (s)
0.7
0.8
0.9
Figure 11.5: (Left) The complex roots of the characteristic equation of the PDcontrolled joint for a fixed Kd = 10 Nms/rad as Kp increases from zero. This is
known as a root locus plot. (Right) The response of the system to an initial
error e = 1, e = 0 is shown for overdamped ( = 1.5, roots at 1), critically
damped ( = 1, roots at 2), and underdamped ( = 0.5, roots at 3) cases.
where e,min is the least positive value achieved by the error. The overshoot can
be calculated to be
p
overshoot = exp(/ 1 2 ) 100%, 0 < 1.
A good transient response is characterized by a low settling time and little or
no overshoot.
Figure 11.5 shows the relationship of the location of the roots of (11.10) to
the transient response. For a fixed Kd and a small Kp , we have > 1, the system
is overdamped, and the response is sluggish due to the slow root. As Kp is
increased, the damping ratio decreases. The system is critically damped ( = 1)
at Kp = (b + Kd )2 /(4M ), and the two roots are coincident on the negative real
axis. This situation corresponds to a relatively fast response and no overshoot.
As Kp continues to increase, drops below 1, the roots move off the negative
real axis, and we begin to see overshoot and oscillation in the response. The
settling time is unaffected as Kp is increased beyond critical damping, as n
is unchanged.
Practical Bounds on Feedback Gains According to our simple model,
we could increase Kp and Kd without bound to make the real components
of the roots more and more negative, achieving arbitrarily fast response. In
practice, however, large gains lead to actuator saturation, rapid torque changes
(chattering), vibrations of the structure due to unmodeled flexibility in the joints
and links, and even instability due to the finite servo rate frequency. Thus there
are practical limits on the set of useful gains.
305
P term
1.6
desired configuration =
PID final configuration
1.4
D term
1.2
1
0.8
P term
PD control
0.6
I term
0.4
PID control
0.2
0
0.2
D term
0
time (s)
10
PD final configuration
g
initial configuration
controls
Figure 11.6: (Left) The tracking errors for a PD controller with Kd =

2 Nms/rad, Kp = 2.205 Nm/rad for critical damping, and a PID controller
with the same PD gains and Ki = 1 Nm/(rad s). The arm starts at (0) =
/2, (0)
= 0, with a goal state d = 0, d = 0. (Middle) The individual
contributions of the terms in the PD and PID control laws. (Right) The initial
and final configurations.
PID Control and Third-Order Error Dynamics Now consider the case
of setpoint control where the link moves in a vertical plane, i.e., g > 0. With
the PD control law above, the system can now be written
M e + (b + Kd )e + Kp e = mgr cos .
(11.11)
This implies that the system comes to rest at a configuration satisfying Kp e =

mgr cos , i.e., the final error e is not zero when d 6= 2 . This is because the
robot must provide a nonzero torque to hold the link at rest at 6= 2 , but the
PD control law only creates a nonzero torque at rest if e 6= 0. We can make
this steady-state error small by increasing the gain Kp , but as discussed above,
there are practical limits.
To eliminate the steady-state error, we return to the PID controller by setting
Ki > 0. This allows a nonzero steady-state torque even with zero position
error; only the integrated error must be nonzero. Figure 11.6 demonstrates the
addition of the integral term to the controller.
To see how this works, write the setpoint error dynamics
Z
M e + (b + Kd )e + Kp e + Ki e (t)dt = dist ,
(11.12)
where dist is a disturbance torque substituted for the gravity term mgr cos .
Taking derivatives of both sides, we get the third-order error dynamics
M e(3) + (b + Kd )e + Kp e + Ki e = dist .
(11.13)
If dist is constant, then the right-hand side of (11.13) is zero. If the PID
controller is stable, then (11.13) shows that e converges to zero. (While the
306
Robot Control
disturbance torque due to gravity is not constant as the link rotates, it approaches a constant as approaches zero, and therefore similar reasoning holds
close to the equilibrium.)
Integral control is useful to eliminate steady-state error in setpoint control,
but it may adversely affect the transient response. This is because integral
control essentially responds to delayed informationit takes time for the system
to respond to error as it integrates. It is well known in control theory that
delayed feedback can cause instability. To see this, consider the characteristic
equation of (11.13) when dist is constant:
M s3 + (b + Kd )s2 + Kp s + Ki = 0.
(11.14)
For all roots to have negative real part, we require b + Kd > 0 and Kp > 0 as
before, but there is also an upper bound on the new gain Ki (Figure 11.7):
0 Ki <
(b + Kd )Kp
.
M
Thus a reasonable design strategy is to choose Kp and Kd for a good transient

response, then choose Ki small so as not to adversely affect stability. In the
example of Figure 11.6, the relatively large Ki worsens the transient response,
giving significant overshoot. In practice, Ki = 0 for many robot controllers.
Pseudocode for the PID control algorithm is given in Figure 11.8.
While our analysis has focused on setpoint control, the PID controller applies
perfectly well to trajectory following, where d (t) 6= 0. Integral control will not
eliminate tracking error along arbitrary trajectories, however.
11.2.1.2
Feedforward Control
Another strategy for trajectory following is to use a model of the robots dynamics to proactively generate torques, instead of waiting for errors. Let the
controllers model of the dynamics be
c() + b
=M
h(, ),
(11.15)
= h(, ).
Note that
c() = M () and b
where the model is perfect if M
h(, )
c
the inertia model M () is written as a function of the configuration . While
the inertia of our simple one-joint robot is not a function of configuration, writing this way allows us to re-use Equation (11.15) for multi-joint systems in
Section 11.2.2.
Given d , d , and d from the trajectory generator, the commanded torque
is calculated as
c(d )d + b
=M
h(d , d ).
(11.16)
If the model of the robot dynamics is exact, and there are no initial state errors,
then the robot exactly follows the desired trajectory. This is called feedforward
control, as no feedback is used.
307
Im(s)
Re(s)
Figure 11.7: The three roots of (11.14) as Ki increases from zero. First a PD
controller is chosen with Kp and Kd yielding critical damping, giving rise to two
collocated roots on the negative real axis. Adding an infinitesimal gain Ki > 0
creates a third root at the origin. As we increase Ki , one of the two collocated
roots moves to the left on the negative real axis, while the other two roots move
toward each other, break away from the real axis, and move into the right-half
plane when Ki = (b + Kd )Kp /M . The system is unstable for larger values of
Ki .
A pseudocode implementation of feedforward control is given in Figure 11.9.
Figure 11.10 shows two examples of trajectory following for the link in gravity. Here, the controllers dynamic model is correct except that it has rb = 0.08 m,
when actually r = 0.1 m. In Task 1, the error stays small, as unmodeled gravity
effects provide a spring-like force to = /2, accelerating the robot at the beginning and decelerating it at the end. In Task 2, unmodeled gravity effects act
against the desired motion, resulting in a larger tracking error. Because there
are always modeling errors, feedforward control is always used in conjunction
with feedback, as discussed next.
11.2.1.3
Feedforward Plus Feedback Linearization
All practical controllers use feedback, as no model of robot and environment

dynamics will be perfect. Nonetheless, a good model can be used to improve
performance and simplify analysis.
c, b
Lets combine PID control with a model of the robot dynamics {M
h} to
achieve the error dynamics
Z
e + Kd e + Kp e + Ki e (t)dt = 0
(11.17)
308
Robot Control
time = 0
eint = 0
qprev = senseAngle
loop
[qd,qdotd] = trajectory(time)
// dt = cycle time
// error integral
// initial joint angle q
q = senseAngle
qdot = (q - qprev)/dt
qprev = q
// from trajectory generator

// sense actual joint angle
// simple velocity calculation
e = qd - q
edot = qdotd - qdot
eint = eint + e*dt
tau = Kp*e + Kd*edot + Ki*eint
commandTorque(tau)
time = time + dt
end loop
Figure 11.8: Pseudocode for PID control.
time = 0
// dt = cycle time
loop
[qd,qdotd,qdotdotd] = trajectory(time)
tau = Mhat(qd)*qdotdotd + hhat(qd,qdotd) // calculate dynamics
commandTorque(tau)
time = time + dt
end loop
Figure 11.9: Pseudocode for feedforward control.

along arbitrary trajectories, not just to a setpoint. The error dynamics (11.17)
and proper choice of PID gains ensure exponential decay of trajectory error.
to achieve the error dynamics (11.17), we choose the
Since e = d ,
robots commanded acceleration to be
com = d e
then plug in Equation (11.17) to get

Z
= d + Kd e + Kp e + Ki e (t)dt.
(11.18)
Plugging com into a model of the robot dynamics, we get the feedback linearizing
309
-/4
actual
Task 1
desired
-3/4
3/4
desired
Task 2
g
/4
actual
0
2
time (s)
Figure 11.10: Results of feedforward control with an incorrect model: r =

0.08 m, but r = 0.1 m. The desired trajectory in Task 1 is d (t) = /2 cos(t)
for 0 t . The desired trajectory for Task 2 is d (t) = /2cos(t), 0 t .
controller

Z
c() d + Kp e + Ki e (t)dt + Kd e + b
=M
h(, ).
(11.19)
This type of controller is called feedback linearizing because feedback of and
is used to transform the nonlinear control system to a linear one. The b

h(, )
c()
term cancels dynamics dependent only on the state, and the inertia model M
converts desired joint accelerations into joint torques, realizing the simple linear
error dynamics (11.17). This kind of controller is sometimes called a computed
torque controller.
A block diagram of the controller is shown in Figure 11.11. The gains Kp , Ki ,
and Kd are chosen to place the roots of the characteristic equation as desired
to achieve good transient response. Under the assumption of a perfect dynamic
model, we would choose Ki = 0.
Figure 11.12 shows typical behavior of feedback linearizing control relative
to feedforward and feedback only. Pseudocode is given in Figure 11.13.
310
Robot Control
..
d
.
d, d
..
fb
PID
controller
+
+
..
com
..
M() com
^
+
+
.
,
arm
dynamics
.
h(, )
Figure 11.11: Feedback linearizing control. The feedforward acceleration d is

added to the acceleration fb computed by the PID feedback controller to create
the commanded acceleration com .
fb
3/4
ff+fb
desired
/4
fb
ff+fb
2dt
ff
ff
0
2
time (s)
2
time (s)
Figure 11.12: Performance of feedforward only (ff), feedback only (fb), and
feedback linearizing control (ff+fb). PID gains are taken from Figure 11.6, and
the feedforward modeling error is taken from Figure 11.10. The desired motion
is Task 2 from Figure 11.10. The middle
R plot shows the tracking performance
of the three controllers. Also plotted is 2 (t)dt, a standard measure of control
effort, for each of the three controllers. These plots show typical behavior: the
feedback linearizing controller yields better tracking than either feedforward or
feedback alone, with less control effort than feedback alone.
11.2.1.4
A Closer Look at Friction
For simplicity, we have been assuming a viscous model of friction at bearings

and gears. In reality, friction is a complex phenomenon which is the subject
of considerable current research, and any friction model is a gross attempt to
capture average behavior of the micromechanics of contact. Friction forces may
be a function of the loading force at the contact, the time the contact has been at
rest, position (due to spring-like forces before sliding begins, or non-uniformity
in bearings), velocity, temperature, etc.
One noteworthy limitation of a viscous friction model is its implication of
zero friction force at zero velocity. In fact, common experience indicates that
friction forces can be large at zero velocity. For example, you can apply significant horizontal forces to a book on a table before it begins to slide. The
force resisting motion at rest is known as static friction and is not included in
time = 0
eint = 0
qprev = senseAngle
loop
q = senseAngle
qdot = (q - qprev)/dt
qprev = q
311
// dt = cycle time
// error integral
// initial joint angle q
// sense actual joint angle
// simple velocity calculation
e = qd - q
edot = qdotd - qdot
eint = eint + e*dt
tau = Mhat(q)*(qdotdotd + Kp*e + Kd*edot + Ki*eint) + hhat(q,qdot)
commandTorque(tau)
time = time + dt
end loop
Figure 11.13: Pseudocode for the feedback linearizing controller.

the viscous model. For a robot joint, nonzero static friction implies that some
torque can be applied to the joint at rest without causing any motion. The
discontinuity of friction force at zero velocity significantly complicates the problem of controlling low-speed motions, particularly motions involving switches
in direction. To address this issue, a more sophisticated model of joint friction
in feedcan be included in the nonlinear dynamics compensation term b
h(, )
forward or feedback linearizing control. See Figure 11.14 for some examples of
velocity-dependent friction. Other ways of dealing with friction include dithering (imposing a high-frequency zero-mean control signal on top of the nominal
control signal, to smooth the friction discontinuity at zero velocity) and using
larger PID gains in the neighborhood of zero velocity.
11.2.1.5
The Effect of Gearing
Until now we have been considering our actuator as a source of torque, without
considering how the actuator generates that torque. For example, if we choose
a DC motor with an appropriate power rating for our robot joint, we will likely
find that it is capable of producing high speed, up to 10,000 RPM or more, but
only low torque. Most robotic applications require significantly lower speeds and
higher torques. Therefore, gears, belts and pulleys, and other transmissions are
usually used to reduce speed by a gear ratio G > 1, while ideally increasing the
312
Robot Control
fricuf
fricf
.
v
(a)
fricf
.
qv
(b)
fricf
(c)
fricf
.
fric
.
qv
.
qv
qv
(d)
(e)
(f)
Figure 11.14: Examples of velocity-dependent friction models. (a) Viscous fric fric can take any value in [b, b] at
tion. (b) Coulomb friction, fric = b sgn().
+ bviscous .
(d)
zero velocity. (c) Static plus viscous friction, fric = bstatic sgn()
Static and kinetic friction, requiring fric |bstatic | to initiate motion, and then
during motion, where bstatic > bkinetic . (e) Static, kinetic,
fric = bkinetic sgn()
and viscous friction. (f) A friction law exhibiting the Stribeck effectat low
velocities, friction decreases as velocity increases.
available torque by a factor of G, thereby preserving power:
in
, out = Gin , Pout = out out = (Gin )(in /G) = Pin .
out =
G
In practice, however, some power is dissipated due to friction in the gearing,
and the torque available at the output is less than Gin . This effect tends to
increase with the gear ratio G. Typical choices of G are from single digits to
over one hundred.
Another option is to directly drive a robot joint with G = 1. Motors used in
direct-drive configurations typically have much higher power ratings than needed
for the application, so that they can provide sufficient torque without gearing.
These motors never approach their top-end speeds in typical applications.
Now consider the inertia M of our one-joint robot. It is actually a lumped
parameter capturing not only the inertia of the link, but also the inertia of the
motors rotor. Typically the motors inertia Imotor is much smaller than the
inertia of the link Ilink . When there is gearing G > 1, however, the motor spins
then the motor speed is
faster than the link; if the link angular velocity is ,
G. We can write the kinetic energy of the link-rotor system as

1
2 = 1 (Ilink + G2 Imotor ) 2 ,
K=
Ilink 2 + Imotor (G)
{z
}
2
2|
M
where G Imotor is the inertia of the motor seen from the gearing output shaft.
This is called the motors reflected inertia through the gearbox: the effective
inertia of the motor as seen at the output of the gearbox. The time derivative
of the kinetic energy K is the torque driving the link-rotor system multiplied
by the joint velocity, or
K = (Ilink + G2 Imotor ).
As an example, consider Ilink = 1 kgm2 and Imotor = 103 kgm2 . For G = 1,
99.9% of the total inertia is due to the link; only 0.1% of the acceleration torque
313
accelerates the motor. With a gear ratio G = 100, however, the effective inertia
of the geared motor is ten times thepinertia of the link.
When G is chosen so that G = Ilink /Imotor , half of the torque is used to
accelerate each of the motor and the link, and the system is said to be inertia
matched, maximizing the power transferred to the link (Exercise 9).
Summarizing, we can make two observations comparing direct-drive and
highly geared systems:
The behavior of geared systems is generally less sensitive to changes in
the link inertia, as when the arm carries a load, since the link inertia is a
smaller percentage of the total inertia due to the high reflected inertia of
geared motor.
Friction forces are larger in highly geared systems. In the limit where
friction forces dominate inertial forces, the joint dynamics may be closer
to a first-order viscous system than a second-order inertial system.
These properties play an important role in the analysis of control laws for
multi-joint systems, discussed next.
11.2.2
Multi-Joint Motion Control
The methods applied above for a single-joint robot carry over directly to n-joint
robots. The difference is that the dynamics (11.4) now take the more general
vector-valued form
= M () + h(, ),
where the n n positive-definite inertia matrix M is now a function of the
configuration . We will sometimes find it convenient to explicitly state the
components of the term h(, ):

+ g() + b(),
= M () + C(, )
(11.20)
are Coriolis and centripetal terms, g() are potential (e.g., gravwhere C(, )
are friction terms. In general, the dynamics (11.20) are
ity) terms, and b()
coupled the force or torque at a joint may be a function of the positions, velocities, and accelerations of other joints.
We distinguish between two types of control of multi-joint robots: decentralized control, where each joint is controlled separately with no sharing of
information between joints, and centralized control, where full state information for each of the n joints is available to calculate the controls for each joint.
11.2.2.1
Decentralized Multi-Joint Control
The simplest method for controlling a multi-joint robot, and one that is often
used, is to apply an independent controller at each joint. Decentralized control
is appropriate when the dynamics of the joints can be decoupled, or at least
approximately decoupled. The dynamics are decoupled when the acceleration
314
Robot Control
of each joint depends only on the torque applied at that joint. This occurs when
the inertia matrix is diagonal, as in Cartesian or gantry robots, where the first
three axes are prismatic and orthogonal along the x-y-z axes. This kind of robot
is equivalent to three single-joint systems.
Approximate decoupling is also achieved in highly geared robots in the absence of gravity. The inertia matrix M () is nearly diagonal, as it is dominated
by the reflected inertias of the motors themselves. Variations in M () due to
different joint configurations are small. Significant friction at the individual
joints also contributes to the decoupling of the dynamics.
11.2.2.2
Centralized Multi-Joint Control
When gravity forces and torques are significant and coupled, or when the inertia matrix M () is not well approximated by a diagonal matrix, decentralized
control may no longer yield acceptable performance. In this case, the feedback
linearizing control law (11.19) of Figure 11.11 can be generalized. The configurations and d and the error e = d are now n-vectors, and the positive
scalar gains become positive-definite matrices Kp , Ki , Kd :

Z
c() d + Kp e + Ki e (t)dt + Kd e + b
=M
h(, ).
(11.21)
Typically we choose the gain matrices as kp I, ki I, kd I, where I is the n n

identity matrix and kp , ki , and kd are nonnegative scalars. In the case of an
c and b
exact dynamics model M
h, the dynamics of each joint reduces to the
linear dynamics (11.17). The block diagram and pseudocode for this control
algorithm are found in Figures 11.11 and 11.13, respectively.
Implementing the control law (11.21) requires calculating potentially complex dynamics. We may not have a good model of these dynamics, or the
equations may be too computationally expensive to calculate at servo rate. In
this case, if the desired velocities and accelerations are small, an approximation
to (11.21) can be obtained using only PID control and gravity compensation:
Z
= K p e + K i
e (t)dt + Kd e + gb().
(11.22)
With zero friction, perfect gravity compensation, and PD setpoint control (Ki =
0 and d = d = 0), the controlled dynamics can be written
= Kp e Kd ,
M () + C(, )
(11.23)
,
and any viscous
where the Coriolis and centripetal terms are written C(, )
friction effects are included in Kd for simplicity. We can now define a virtual
315
error energy, the sum of an error potential energy stored in the virtual
spring and an error kinetic energy:
V (e , e ) =
1 T
1
Kp e + eT M ()e .
2 e
2
(11.24)
Since d = 0, this reduces to
= 1 T Kp e + 1 T M ().
V (e , )
2 e
2
(11.25)
Taking the time derivative and plugging in (11.23), we get

1
V = T Kp e + T M () + T M ()
2

+ 1 T M ().
= T Kp e + T Kp e Kd C(, )
2
(11.26)
Rearranging, and using the fact that M 2C is skew-symmetric, we get

1

V = T Kp e + T Kp e Kd + T M () 2C(, )
2
= T Kd 0.
(11.27)
This shows that the error energy is decreasing when 6= 0. If = 0 and
6= d , the virtual spring ensures that 6= 0, so e will again become nonzero
and more energy will be dissipated. Thus by the Krasovskii-LaSalle invariance
principle (Exercise 13), the total error energy decreases monotonically and the
robot converges to rest at d (e = 0) from any initial state.
11.2.3
Task Space Motion Control
In Section 11.2.2, we focused on motion control in joint space. This is convenient

because joint limits are easily expressed in this space, and the robot should be
able to execute any joint-space path respecting these limits. Trajectories are
naturally described by the joint variables, and there are no issues of singularities
or redundancy.
On the other hand, since the robot interacts with the external environment
and objects in it, it may be more convenient to express the motion as a trajectory
of the end-effector in task space. Let the end-effector trajectory be specified by
(X(t), V(t)), where X SE(3) or X Rn , for example, with V Rn the
velocity. Provided the corresponding trajectory in joint space is feasible and
does not pass through a dynamic singularity where M () loses rank, we now
have two options for control: (1) convert to a joint-space trajectory and proceed
with control as in Section 11.2.2 or (2) express the robot dynamics and control
law in the task space.
The first option is to convert the trajectory to joint space. The forward
where J() is the appropriate Jacobian
kinematics are X = f () and V = J(),
316
Robot Control
based on the chosen velocity representation V. Then the joint space trajectory
is obtained from the task space trajectory by
(inverse kinematics) (t) = f 1 (X(t))
= J 1 ((t))V(t)
(t)

= J 1 ((t)) V(t)
(t)
J((t))
(t)
.
(11.28)
(11.29)
(11.30)
If the robot is redundant, i.e., J() has more columns than rows, a redundancy
resolution scheme must be used to solve for f 1 and J 1 .
A drawback of this approach is that we must calculate the inverse kinematics,
which may require significant computing power. The second option is to express
the robots dynamics in task-space coordinates, as discussed in Chapter 8.6.
Recall the task-space dynamics
F = ()V + (, V) + ().
The joint forces and torques are related to the forces F expressed in the
end-effector frame by = J T ()F.
We can now write a control law in task coordinates inspired by the feedback
linearizing control law in joint coordinates (11.21),

Z
b
V d + Kp Xe + Ki Xe (t)dt + Kd Ve +
= J T () ()
b(, V) + b() ,
(11.31)
b
where V d is the desired acceleration and ,
b, and b represent the controllers
dynamics model.
The task-space control law (11.31) makes use of the configuration error Xe
and velocity error Ve . When X is expressed in a minimal set of coordinates
a natural choice is Xe = Xd X, Ve = Vd V. When
(X Rn ) and V = X,
X = (R, p) SE(3), however, there are a number of possible choices, including
the following:
V = Vb and J() = Jb () in the end-effector frame {b}. A natural choice
would be Xe = logSE(3) (X 1 Xd ) and Ve = AdX 1 Xd Vd V. The expression for Xe gives the body-fixed direction from the current configuration
X to the desired configuration Xd in the end-effector frame. The transform AdX 1 Xd transports the desired velocity Vd from Xd to a velocity
expressed in the end-effector frame at X.
V = Vs and J() = Js () in the space frame {s}. A natural choice would
be Xe = logSE(3) (Xd X 1 ) and Ve = Vd V.
V and J() chosen so that V = (, v), where is the angular velocity of
the end-effector relative to {s} and v = p.
A natural choice would be

logSO(3) (Rd RT )
Xe =
and Ve = Vd V.
pd p
11.3. Force Control
317
These choices lead to different behaviors of the robot. In particular, the last
choice decouples the rotational and linear corrective terms.
11.3
Force Control
When the goal is not to create motions at the end-effector, but to apply forces
and torques to the environment, the task requires force control. Pure force
control is only possible if the environment provides resistance forces in every
direction (e.g., when the end-effector is embedded in concrete or attached to
a spring-damper providing resistance in every motion direction). Pure force
control is a bit of an abstraction, as robots are usually able to move freely in at
least some direction. It is a useful abstraction, however. It also leads to hybrid
motion-force control in the next section.
In ideal force control, the force applied by the end-effector is unaffected by
disturbance motions applied to the end-effector. This is dual to the case of ideal
motion control, where the motion is unaffected by disturbance forces. Force
control is dual to motion control in the sense that forces are dual to velocities,
with their product being power, an intrinsic, coordinate-free concept.
Let Fapp be the force that the manipulator applies to the environment. The
manipulator dynamics can be written
+ g() + b()
+ J T ()Fapp = ,
M () + C(, )
(11.32)
Since the robot typically moves

where the Jacobian J() satisfies V = J().
slowly (or not at all) during a force control task, we ignore the acceleration and
velocity terms to get
g() + J T ()Fapp = .
(11.33)
In the absence of any direct measurements of the force-torque at the robot
end-effector, joint angle feedback alone can be used to implement the force
control law
= gb() + J T ()Fd ,
(11.34)
where gb() is the model of gravitational torques and Fd is the desired force. This
control law requires a good model for gravity compensation as well as precise
control of the torques produced at the robot joints. In the case of a direct-drive
joint, torque control can be achieved by current control of the motor. In the
case of a highly geared actuator, however, large friction torque in the gearing
degrades the quality of torque control achieved using only current control. In
this case, the output of the gearing can be instrumented with strain gauges to
directly measure the joint torque, which is fed back to a local controller that
modulates the motor current to achieve the desired output torque.
A more common solution is to equip the robot arm with a six-axis forcetorque sensor between the arm and the end-effector to directly measure the
end-effector forces Fapp (Figure 11.15). Force-torque measurements are often
noisy, so the time derivative of these measurements may not be meaningful.
In addition, the desired force Fd is typically constant or only slowly changing.
318
Robot Control
six-axis
force-torque
sensor
Figure 11.15: A six-axis force-torque sensor, highlighted in yellow, mounted

between the Barrett WAM robot arm and its end-effector.
These characteristics suggest a PI controller with a feedforward term and gravity
compensation,
T
= gb() + J () Fd + Kf p Fe + Kf i

Fe (t)dt ,
(11.35)
where Fe = Fd Fapp and Kf p and Kf i are positive-definite proportional and

integral gain matrices, respectively. In the case of perfect gravity modeling,
plugging the force controller (11.35) into the dynamics (11.33), we get the error
dynamics
Z
Kf p Fe + Kf i
Fe (t)dt = 0.
(11.36)
In the case of a constant force disturbance on the right-hand side of (11.36),

arising from an incorrect model of gb(), for example, we take the derivative to
get
Kf p F e + Kf i Fe = 0,
(11.37)
showing that Fe converges to zero for positive-definite Kf p and Kf i .
The control law (11.35) is simple and appealing, but potentially dangerous if
incorrectly applied. If there is nothing for the robot to push against, the robot
will accelerate in a failing attempt to create end-effector forces. Since a typical
force control task requires little motion, we can limit this acceleration by adding
velocity damping. This gives the modified control law

Z
= gb() + J T () Fd + Kf p Fe + Kf i Fe (t)dt Kdamp V ,
where Kdamp is positive definite.
(11.38)
11.4. Hybrid Motion-Force Control
11.4
319
Hybrid Motion-Force Control
Most tasks requiring the application of controlled forces also require the application of controlled motions. Control to achieve this is called hybrid motion-force
control. If the the task space is n-dimensional, then we are free to specify n
of the 2n forces and motions at any time t; the other n are determined by the
environment. Apart from this constraint, we also should not specify forces and
motions in the same direction, as they are not independent.
As an example, consider a two-dimensional environment modeled by a damper,
F = Benv V, where

2 1
Benv =
.
1 1
Defining the components of V and F as (V1 , V2 ) and (F1 , F2 ), we have F1 =
2V1 + V2 , F2 = V1 + V2 . We have n = 2 freedoms to choose among the 2n = 4
velocities and forces at any time. For example, we can specify both F1 and V1
independently, because Benv is not diagonal. Then V2 and F2 are determined
by Benv . We cannot independently control both F1 and 2V1 + V2 , however, as
these are in the same direction according to the damper.
11.4.1
Natural and Artificial Constraints
A particularly interesting case is when the environment is infinitely stiff (rigid

constraints) in k directions and unconstrained in n k directions. In this case,
we cannot choose which of the 2n motions and forces to specifythe contact
with the environment chooses the k directions in which the robot can freely
apply forces and the n k directions of free motion. As an example, consider
the task space to have the n = 6 dimensions of SE(3). Then a robot opening a
cabinet door has 6 k = 1 motion freedom, rotation about the cabinet hinges,
and therefore k = 5 force freedomsthe robot can apply any force and torque
that has zero moment about the axis of the hinges.
As another example, a robot writing on a chalkboard may freely control the
force into the board (k = 1), but it cannot penetrate the board; and it may
freely move with 6 k = 5 degrees of freedom (two specifying the motion of the
tip of the chalk in the plane of the board, and three describing the orientation
of the chalk), but it cannot independently control forces in these directions.
The chalk example comes with two caveats. The first is due to friction
the chalk-wielding robot can actually control forces tangent to the plane of the
board, provided the requested motion in the plane of the board is zero and the
requested tangential forces do not exceed the static friction limit determined by
the friction coefficient and the normal force into the board (see friction modeling
in Chapter 12). Within this regime, the robot has three motion freedoms and
three force freedoms. Second, the robot could decide to pull away from the
board. In this regime, the robot has six motion freedoms and no force freedoms.
Thus the configuration of the robot is not the only determinant in the directions
of motion and force freedoms. Nonetheless, in this section we consider the
simplified case where the motion and force freedoms are determined solely by the
320
Robot Control
zb
zs
yb
{b}
xb
xs
{s}
ys
Figure 11.16: The fixed space frame {s} attached to the chalkboard and the
body frame {b} attached to the eraser.
robots configuration, and all constraints are equality constraints. For example,
the inequality velocity constraint of the board (the chalk cannot penetrate the
board) is treated as an equality constraint (the robot also does not pull the
chalk away from the board).
As a final example, consider a robot erasing a chalkboard using an eraser
modeled as a rigid block (Figure 11.16). Let the configuration X(t) be expressed
in coordinates q = (, p) = (x , y , z , x, y, z), where are exponential coordinates for the orientation. The velocity is represented as V = q.
When the eraser
is in contact with the board, its configuration X(t) is subject to the constraints
x = 0
y = 0
z=c
where c is half the thickness of the eraser. These constraints can be expressed
differentially as
x = 0
y = 0
z = 0
In the language of Chapter 2, these constraints are holonomicthe differential
constraints can be integrated to give the configuration constraints.
These constraints are called natural constraints, specified by the environment. In light of the natural constraints, we can specify any motion of the
eraser satisfying these k = 3 velocity constraints, giving 6 k = 3 motion freedoms. We are also free to specify k = 3 forces, Fz , Fx , and Fy . These motion
and force specifications are called artificial constraints. Below is a typical set of
11.4. Hybrid Motion-Force Control
321
artificial constraints corresponding to the natural constraints:

natural constraints
x = 0
y = 0
Fz = 0
Fx = 0
Fy = 0
z = 0
artificial constraints
Fx = 0
Fy = 0
z = 0
x = k1
y = 0
F z = k2
The artificial constraints cause the eraser to move with an x-velocity k1 while
applying a constant force k2 against the board.
11.4.2
A Hybrid Controller
We now return to the problem of designing a hybrid motion-force controller. If

the environment is rigid, then we express the k natural constraints on velocity
in task space as
A(X)V = 0,
(11.39)
where A(X) Rkn . (Alternatively, these constraints could be written in a
minimal set of task coordinates as A(q)q = 0 or in joint coordinates as A() =
0.) This formulation includes holonomic and nonholonomic contact constraints
with the environment, as well as closure constraints in parallel mechanisms.
If the task-space dynamics of the robot, in the absence of constraints, are
F = ()V + (, V) + (),
then the constrained dynamics are
F = ()V + (, V) + () + AT (X),
| {z }
(11.40)
Fapp
where Rk are Lagrange multipliers and Fapp are forces that the robot applies
against the constraints. The requested force Fd must lie in the column space of
AT (X).
Since (11.39) must be satisfied at all times, we can replace (11.39) by the
time derivative
A(X)V + A(X)V
= 0.
(11.41)
plugging the result into (11.41), and solving for ,
Now, solving (11.40) for V,
we get
= (A1 AT )1 (A1 (F ) + AV)
= (A1 AT )1 (A1 (F ) AV),
(11.42)
322
Robot Control
by (11.41). With (11.42), we can calculate

where we have plugged in AV = AV
T
the forces Fapp = A (q) the robot applies against the constraints.
Plugging (11.42) into (11.40) and manipulating, the n equations of the constrained dynamics (11.40) can be expressed as the n k independent motion
equations
P (X)F = P (X)(()V + (, V) + ())
(11.43)
P = I AT (A1 AT )1 A1
(11.44)
where
and I is the identity matrix. The nn matrix P (X) has rank nk and projects
an arbitrary manipulator force F onto the subspace of forces that move the endeffector tangent to the constraints. The rank k matrix I P (X) projects an
arbitrary force F to the subspace of forces against the constraints. Thus P
partitions the n-dimensional force space into forces that address the motion
control task and forces that address the force control task.
Our hybrid motion-force controller is simply the sum of a task-space motion
controller, derived from the feedback linearizing control law (11.31), and a taskspace force controller (11.35), each projected to generate forces in its appropriate
subspace:

Z
T
b
= J () P (X) () Vd + Kp Xe + Ki Xe (t)dt + Kd Ve
{z
}
|
motion control

Z
+ (I P (X)) Fd + Kf p Fe + Kf i Fe (t)dt
|
{z
}
force control

+
b(, V) + b()
.
(11.45)
{z
}
|
nonlinear compensation
Because the dynamics of the two controllers are decoupled by the orthogonal
projections P and I P , the controller inherits the error dynamics and stability analyses of the individual force and motion controllers on their respective
subspaces.
A difficulty in implementing the hybrid control law (11.45) in rigid environments is knowing precisely the constraints A(X)V = 0 active at any time.
This is necessary to specify the desired motion and force and to calculate the
projections, but any model of the environment will have some uncertainty. One
approach to dealing with this issue is to use a real-time estimation algorithm to
identify the constraint directions based on force feedback. Another is to sacrifice some performance by choosing low feedback gains, which makes the motion
controller soft and the force controller more tolerant of force error. We can
also build passive compliance into the structure of the robot itself to achieve
a similar effect. In any case, some passive compliance is unavoidable, due to
flexibility in the joints and links.
11.5. Impedance Control
323
k
m
b
Figure 11.17: A robot creating a one-degree-of-freedom mass-spring-damper

virtual environment. The human hand applies a force f to the haptic interface.
11.5
Impedance Control
Ideal hybrid motion-force control in rigid environments demands extremes in

robot impedance, which characterizes the change in endpoint motion as a function of disturbance forces. Ideal motion control corresponds to high impedance
(little change in motion due to force disturbances) while ideal force control corresponds to low impedance (little change in force due to motion disturbances).
In practice, there are limits to a robots achievable impedance range.
In this section, we consider the problem of impedance control, where the
robot mimics particular mass, spring, and damper properties.1 For example,
a robot used as a haptic surgical simulator could be tasked with mimicking
the mass, stiffness, and damping properties of a virtual surgical instrument in
contact with virtual tissues.
A one-degree-of-freedom environment can be written
m
x + bx + kx = f,
(11.46)
where x is the position, m is the mass, b is the damping, and k is the stiffness,
and f is the force applied to the environment (Figure 11.17). Collectively, we
refer to the parameters {m, b, k} as the impedance. Loosely, we say that the
environment has high impedance if one or more of these parameters, usually
including b or k, is large. Similarly, we say that the impedance is low if all of
these parameters are small.
More formally, taking the Laplace transform of (11.46), we get
(ms2 + bs + k)X(s) = F (s),
(11.47)
and the impedance is defined by the transfer function from position perturbations to forces, Z(s) = F (s)/X(s). Thus impedance is frequency dependent, with low-frequency response dominated by the spring and high-frequency
response dominated by the mass. Admittance is the inverse of impedance,
Y (s) = Z 1 (s) = X(s)/F (s).
A good motion controller is characterized by high impedance (low admittance), since X = Y F . If the admittance Y is small, then force perturbations
1 A popular subcategory of impedance control is stiffness or compliance control, where the
robot mimics a virtual spring only.
324
Robot Control
F produce only small position perturbations X. Similarly, a good force controller is characterized by low impedance (high admittance), since F = ZX,
and small Z implies that motion perturbations produce only small force perturbations.
The goal of impedance control is to implement the task-space behavior
DV + BV + KX = Fext ,
(11.48)
where X Rn and V = X are the task-space position and velocity; D, B, and

K are the positive-definite virtual inertia, damping, and stiffness to be created
by the robot; and Fext is a force applied to the robot, perhaps by a user. The
values of D, B, and K may change depending on the location in the virtual
environment, to represent distinct objects for instance, but we will focus on the
case of constant values.
We can replace X and V in (11.48) by X(t) Xref (t) and V(t) Vref (t).
If Xref (t) is time-varying, it will pull the robot approximately along the trajectory. This allows impedance control to achieve motion control, much like
a task-space PD controller. Larger stiffness K and damping B achieve more
precise trajectory tracking.
There are at least two ways to achieve the behavior (11.48):
The robot senses motions and commands joint torques to create Fext , the
force to display to the user. Such a robot is called impedance controlled,
as it implements a transfer function Z(s) from motions to forces. These
robots tend to be lightweight and backdrivable, and are often cable-driven.
They are typically good at displaying low impedances, and not as good at
displaying stiff, high impedance environments.
The robot senses Fext using a wrist force-torque sensor and controls motions in response. Such a robot is called admittance controlled, as it
implements a transfer function Y (s) from forces to motions. These robots
tend to be highly geared. They are typically good at displaying stiff, high
impedance environments, and not as good at displaying low impedances.
11.5.1
Impedance Control Algorithm
In an impedance control algorithm, encoders, tachometers, and possibly accelerometers are used to estimate the joint and endpoint positions, velocities, and possibly accelerations. Often these robots are not equipped with a
force-torque sensor, and instead rely on their ability to precisely control joint
torques with little friction to display the appropriate end-effector force Fext
(from (11.48)) to the user. An ideal control law might be
V +
b(, V) + b() (DV + BV + KX) .
= J T () ()
|
{z
}
|
{z
}
arm dynamics compensation
Fext
(11.49)
11.6. Other Topics
325
Addition of an end-effector force-torque sensor allows the use of feedback terms

to more closely achieve the desired interaction force Fext .
V, and X are measured
In the control law (11.49), it is assumed that V,
directly. Measurement of the acceleration V is likely to be noisy, however, and

there is the problem of attempting to compensate for the robots inertia after
the acceleration has been sensed. Therefore, it is not uncommon to eliminate
b V and to set D = 0. The inertia of the arm
the inertial compensation term ()
will be apparent to the user, but impedance-controlled manipulators are often
designed to be lightweight.
Additionally, instability can arise when (11.49) is used to simulate stiff environments. Small changes in position, as measured by encoders for example, lead
to large changes in motor torques. This effective high gain, coupled with delays,
sensor quantization, and sensor errors, can lead to oscillatory behavior in stiff
virtual environments. On the other hand, the effective gains are low when emulating low impedance environments. A lightweight backdrivable manipulator
can excel at impedance control.
11.5.2
Admittance Control Algorithm
In an admittance control algorithm, the force Fext applied by the user is sensed
by the wrist load cell, and the robot responds with an end-effector acceleration
satisfying (11.48). A simple approach is to calculate the desired end-effector
acceleration V d according to
DV d + BV + KX = Fext ,
where (X, V) is the current state. Solving, we get
V d = D1 (Fext BV KX).
(11.50)
Given V d , V, and X, integration over the servo timestep t yields the desired
position and velocity Xd and Vd . The reference Xd , Vd , V d can be plugged into
the feedback linearizing control law in task coordinates (11.31). To make the
response smoother, the force readings can be low-pass filtered.
Simulating a low impedance environment is challenging for an admittance
control algorithm, as small forces produce large accelerations. The effective
large gains can produce instability. On the other hand, admittance control by
a highly geared robot can excel at emulating stiff environments.
11.6
Other Topics
Robust Control While all stable feedback controllers confer some amount of
robustness to uncertainty, the field of robust control deals with designing controllers that explicitly guarantee the performance of a robot subject to bounded
parametric uncertainties, such as in its inertial properties.
326
Robot Control
Adaptive Control Adaptive control of robots involves estimating the robots

inertia parameters during execution and updating the control law in real-time
to incorporate those estimates.
Iterative Learning Control Iterative learning control (ILC) generally focuses on repetitive tasks. For example, if a robot performs the same pick-andplace operation over and over, the trajectory errors from the previous execution
can be used to modify the feedforward control for the next execution. In this
way, the robot improves its performance over time, driving execution error toward zero. ILC differs from adaptive control in that the learned information
is generally nonparametric and focused on a single trajectory.
Passive Compliance and Flexible Manipulators All robots unavoidably
have some passive compliance. Models of this compliance could be as simple as a
single torsional spring at each revolute joint (e.g., to account for finite stiffness in
the flexsplines of harmonic drive gearing) or as complicated as treating links as
flexible beams. Two significant effects of flexibility are (1) a mismatch between
the motor angle reading, the true joint angle, and the endpoint of the attached
link, and (2) increased order of the dynamics of the robot. These issues raise
challenging problems in control, particularly when vibration modes are at low
frequencies.
Some robots are specifically designed for passive compliance, particularly
those meant for contact interactions with humans or the environment. Such
robots may sacrifice position control performance in favor of safety.
Variable Impedance Actuators The impedance of a joint is typically controlled using a feedback control law, as described in Section 11.5. There are limits to the bandwidth of this control, however; a joint that is actively controlled
to behave as a spring will only achieve spring-like behavior to low-frequency
perturbations.
A new class of actuators, called variable impedance actuators or variable stiffness actuators, aims to give actuators the desired passive mechanical impedance
without the bandwidth limitations of an active control law. As an example, a
variable stiffness actuator may comprise two motors, allowing the actuator to
independently control the mechanical stiffness of the joint (e.g., based on the
setpoint of an internal nonlinear spring) and the torque produced by the actuator.
11.7
Summary
A PID joint-space feedback controller takes the form

Z
= Kp e + Ki e (t)dt + Kd e ,
11.7. Summary
327
where e = d and d is the vector of desired joint angles. The proportional term acts like a virtual spring, to pull the robot joints to the
desired positions; the derivative term acts like a virtual damper, acting to
reduce velocity differences between the actual and desired velocities; and
the integral term can be used to eliminate steady-state error in setpoint
control.
The linear error dynamics
an e(n) + an1 e(n1) + . . . + a2 e + a1 e + a0 e = 0
are stable, i.e., initial errors converge exponentially to zero, if and only if
all of the complex roots s1 , . . . , sn of the characteristic equation
an sn + an1 sn1 + . . . + a2 s2 + a1 s + a0 = 0
have real components less than zero, i.e., Re(si ) < 0 for all i = 1 . . . n.
Stable second-order linear error dynamics can be written in the standard
form
e + 2n e + n2 e = 0,
where is the damping ratio and n is the natural frequency. The roots
of the characteristic equation are
p
s1,2 = n n 2 1.
The system is overdamped if > 1, critically damped if = 1, and
underdamped if < 1. The step response of a stable second-order system
is generally characterized by its overshoot, 2% settling time, and steadystate error.
Highly geared robots tend to have greater decoupling of their dynamics
due to high friction and large reflected motor inertias.
The joint-space feedback linearizing (or computed torque) controller is

Z
c
= M () d + Kp e + Ki e (t)dt + Kd e + b
h(, ).
This controller cancels nonlinear terms, uses feedforward control to anticipate the desired acceleration, and uses linear feedback control for stabilization.
Joint-space PD control plus gravity compensation
= Kp e + Kd e + gb()
yields global convergence to e = 0 by the Krasovskii-LaSalle invariance
principle.
328
Robot Control
The task-space feedback linearizing motion controller is

Z
T
b
= J () () Vd + Kp Xe + Ki Xe (t)dt + Kd Ve +
b(, V) + b() .
There are a number of possible choices for calculating Xe and Ve .
Task-space force control can be achieved by the controller

Z
= gb() + J T () Fd + Kf p Fe + Kf i Fe (t)dt Kdamp V ,
consisting of gravity compensation, feedforward force control, PI force
feedback, and damping to prevent fast motion.
In an n-dimensional task space (where typically n = 6), rigid constraints
specify n k free motion directions and k constraint directions in which
forces can be applied. These constraints can be represented as A(X)V = 0.
A force F can be partitioned as F = P (X)F + (I P (X))F, where P (X)
projects to forces that move the end-effector tangent to the constraints
and I P (X) projects to forces against the constraints. The projection
matrix P (X) is written in terms of the task-space inertia matrix () and
constraints A(X) as
P = I AT (A1 AT )1 A1 .
A hybrid motion-force controller using the projection P (X) can be written

Z
b
V d + Kp Xe + Ki Xe (t)dt + Kd Ve
= J T () P (X) ()
|
{z
}
motion control

Z
+ (I P (X)) Fd + Kf p Fe + Kf i Fe (t)dt
{z
}
|
force control

+
b(, V) + b()
.
{z
}
|
nonlinear compensation
An impedance controller measures end-effector motions and creates endpoint forces to mimic a mass-spring-damper system. An admittance controller measures end-effector forces and creates endpoint motions to achieve
the same purpose.
11.8. Exercises
11.8
329
Exercises
1. Classify the following robot tasks as motion control, force control, hybrid
motion-force control, impedance control, or some combination. Justify your
answer.
(i) Tightening a screw with a screwdriver.
(ii) Pushing a box on the floor.
(iii) Pouring a glass of water.
(iv) Shaking hands with a human.
(v) Throwing a baseball to hit a target.
(vi) Shoveling snow.
(vii) Digging a hole.
(viii) Giving a back massage.
(ix) Vacuuming the floor.
(x) Carrying a tray of glasses.
2. The 2% settling time of an underdamped second-order system is approximately t = 4/(n ), based on exp(n t) = 0.02. What is the 5% settling
time?
3. Solve for any constants and give the specific equation for an underdamped
second-order system with n = 4, = 0.2, e (0) = 1, and e (0) = 0 (a step
response). Calculate the damped natural frequency, approximate overshoot,
and 2% settling time. Plot the solution on a computer and measure the exact
overshoot and settling time.
4. Solve for any constants and give the specific equation for an underdamped
second-order system with n = 10, = 0.1, e (0) = 0, and e (0) = 1. Calculate
the damped natural frequency. Plot the solution on a computer.
5. Consider a pendulum in gravity g = 10 m/s2 . The pendulum consists of a
2 kg mass at the end of a 1 m massless rod. The pendulum joint has a viscous
friction coefficient of b = 0.1 Nms/rad.
(i) Write the equation of motion of the pendulum in terms of , where = 0
corresponds to the hanging down configuration.
330
Robot Control
(ii) Linearize the equation of motion about the stable hanging down equilibrium. To do this, replace any trigonometric terms in with the linear
term in the Taylor expansion. Give the effective mass and spring constants m and k in the linearized dynamics m + b + k = 0. At the stable
equilibrium, what is the damping ratio? Is the system underdamped, critically damped, or overdamped? If it is underdamped, what is the damped
natural frequency? What is the time constant of convergence to the equilibrium and the 2% settling time?
(iii) Now write the linearized equations of motion for = 0 at the balanced
upright configuration. What is the effective spring constant k?
(iv) You add a motor at the joint of the pendulum to stabilize the upright
position, and you choose a P controller = Kp . For what values of Kp
is the upright position stable?
6. You will develop a controller for a one-degree-of-freedom mass-spring-damper

system of the form m
x + bx + kx = f , where f is the control force and m = 4 kg,
b = 2 Ns/m, and k = 0.1 N/m.
(i) What is the damping ratio of the uncontrolled system? Is the uncontrolled system overdamped, underdamped, or critically damped? If it is
underdamped, what is the damped natural frequency? What is the time
constant of convergence to the origin?
(ii) Choose a P controller f = Kp xe , where xe = xd x is the position error
and xd = 0. What value of Kp yields critical damping?
(iii) Choose a D controller f = Kd x e , where x d = 0. What value of Kd yields
critical damping?
(iv) Choose a PD controller that yields critical damping and a 2% settling time
of 0.01 s.
(v) For the PD controller above, if xd = 1 and x d = x
d = 0, what is the
steady-state error xe (t) as t goes to infinity? What is the steady-state
control force?
(vi) Now plug a PID controller in for f . Assume xd 6= 0 and
d = 0.
R x d = x
Write the error dynamics in terms of x
e , x e , xe , and xe (t)dt on the
left-hand side and a constant forcing term on the right-hand side. (Hint:
You can write kx as k(xd x) + kxd .) Take the time derivative of this
equation and give the conditions on Kp , Ki , and Kd for stability. Show
that zero steady-state error is possible with a PID controller.
7. Simulation of a one-degree-of-freedom robot and robot controller.
11.8. Exercises
331
(i) Write a simulator for a one-joint robot consisting of a motor rotating a

link in gravity using the model parameters given in Section 11.2.1. The
simulator should consist of (1) a dynamics function that takes as input the
current state of the robot and the torque applied by the motor and gives as
output the acceleration of the robot; and (2) a numerical integrator that
uses the dynamics function to calculate the new state of the system over a
series of timesteps t. A first-order Euler integration method suffices for
+ 1) = (k)
this problem (e.g., (k + 1) = (k) + (k)t,

(k
+ (k)t).
Test the simulator in two ways: (a) starting the robot at rest at = /2
and applying a constant torque of 0.5 Nm and (b) starting the robot at
rest at = /4 and applying a constant torque of 0 Nm. For both
examples, plot the position as a function of time for sufficient duration to
see the basic behavior. Make sure the behavior makes sense. A reasonable
choice of t is 1 ms.
(ii) Add two more functions to your simulator: (1) a trajectory generator
function that takes the current time and returns the desired state and
acceleration of the robot, and (2) a control function that takes the current state of the robot and information from the trajectory generator and
returns a control torque. The simplest trajectory generator would return
= d1 and = = 0 for all time t < T , and = d2 6= d1 and = = 0
for all time t T . This trajectory is a step function in position. Use PD
feedback control for the control function and set Kp = 10 Nm/rad. For a
well-tuned choice of Kd , give Kd (including units) and plot the position as
a function of time over 2 seconds for an initial state at rest at = /2
and a step trajectory with d1 = /2 and d2 = 0. The step occurs at
T = 1 s.
(iii) Demonstrate two different choices of Kd that yield (1) overshoot and (2)
sluggish response with no overshoot. Give the gains and the position plots.
(iv) Add a nonzero Ki to your original well-tuned PD controller to eliminate
steady-state error. Give the PID gains and plot the results of the step
test.
8. Modify the simulation of the one-joint robot in Exercise 7 to model a flexible
transmission from the motor to the link with a stiffness of 500 Nm/rad. Tune a
PID controller to give a good step response from = /2 to = 0. Give the
gains and plot the response.
9. Consider a motor with rotor inertia Imotor connected through a gearhead of
gear ratio G to a load of inertia Ilink . The load and motor are said to be inertia
matched if, for any given torque m at the motor, the acceleration of the load
is maximized. The acceleration of the load can be written
=
Gm
.
Ilink + G2 Imotor
332
Robot Control
Solve for the inertia matching gear ratio

Show your work.
Ilink /Imotor by solving d/dG

= 0.
10. Simulation of a two-degree-of-freedom robot and robot controller (Figure 11.18).

(i) Dynamics. Derive the dynamics of a 2R robot in gravity (Figure 11.18).
The mass of link i is mi , the center of mass is a distance ri from the joint,
the inertia of link i about the joint is Ii , and the length of link i is Li .
There is no friction at the joints.
(ii) Direct drive. Assume each joint is directly driven by a DC motor with
no gearing. Each motor comes with specifications of the mass mstator
i
and inertia Iirotor of
and inertia Iistator of the stator and the mass mrotor
i
the rotor (the spinning portion). For the motor at joint i, the stator is
attached to link i 1 and the rotor is attached to link i. The links are
thin uniform-density rods of mass mi and length Li .
In terms of the quantities given above, for each link i {1, 2} give equations for the total inertia Ii about the joint, mass mi , and distance ri from
the joint to the center of mass. Think about how to assign the mass and
inertia of the motors to the different links.
(iii) Geared robot. Assume motor i has gearing with gear ratio Gi , and that the
gearing itself is massless. As in the part above, for each link i {1, 2}, give
equations for the total inertia Ii about the joint, mass mi , and distance
ri from the joint to the center of mass.
(iv) Simulation and control. As in Exercise 7, write a simulator with (at
least) four functions: a dynamics function, a numerical integrator, a trajectory generator, and a controller. Assume zero friction at the joints,
g = 9.81 m/s2 in the direction indicated, Li = 1 m, ri = 0.5 m, m1 = 3 kg,
m2 = 2 kg, I1 = 2 kgm2 , and I2 = 1 kgm2 . Write a PID controller, find
gains that give a good response, and plot the joint angles as a function of
time for a trajectory with a reference at rest at (1 , 2 ) = (/2, 0) for
t < 1 s and (1 , 2 ) = (0, /2) for t 1 s. The initial state of the robot
is at rest with (1 , 2 ) = (/2, 0).
(v) Torque limits. Real motors have limits on torque. While these limits are
generally velocity dependent, here we assume that each motors torque
limit is independent of velocity, i |imax |. Assume 1max = 100 Nm
and 2max = 20 Nm. If the control law requests greater torque, the actual torque is saturated to these values. Rerun the previous PID control
simulation and plot the torques as well as the position as a function of
time.
(vi) Friction. Add a viscous friction coefficient of bi = 1 Nms/rad to each joint
and rerun the previous PID control simulation.
11.8. Exercises
333
r1
g
1
m1
r2
m2
2
Figure 11.18: A two-link robot arm. The length of link i is Li and its inertia
about the joint is Ii . Gravity is g = 9.81 m/s2 .
11. For the two-joint robot of Exercise 10, write a more sophisticated trajectory
generator function. The trajectory generator should take as input
the desired initial position, velocity, and acceleration of each joint;
the desired final position, velocity, and acceleration; and
the total time of motion T .
A call of the form
returns the desired position, velocity, and acceleration of each joint at time
time. The trajectory generator should provide a trajectory that is a smooth
function of time.
As an example, each joint could follow a fifth-order polynomial trajectory of
the form
d (t) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4 + a5 t5 .
(11.51)
Given the desired positions, velocities, and accelerations of the joints at times
t = 0 and t = T , you can uniquely solve for the six coefficients a0 . . . a5 by
evaluating Equation (11.51) and its first and second derivatives at t = 0 and
t = T.
Tune a PID controller to track a fifth-order polynomial trajectory moving
from rest at (1 , 2 ) = (/2, 0) to rest at (1 , 2 ) = (0, /2) in T = 2 s. Give
334
Robot Control
the values of your gains and plot the reference position of both joints and the
actual position of both joints. You are free to ignore torque limits and friction.
12. For the two-joint robot of Exercise 10 and fifth-order polynomial trajectory
of Exercise 11, write a feedback linearizing controller to stabilize the trajectory.
The robot has no joint friction or torque limits. The model of the link masses
should be 20% greater than their actual values to create error in the feedforward
model. Give the PID gains and plot the reference and actual joint angles for
both the feedback linearizing controller as well as PID control only.
13. The Krasovskii-LaSalle invariance principle states the following: Given a
system x = f (x), x Rn such that f (0) = 0, and any energy-like function V (x)
such that
V (x) > 0 for all x 6= 0;
V (x) as x ;
V (0) = V (0) = 0; and
V (x) 0 along all trajectories of the system.
Let S be the largest set of Rn such that V (x) = 0 and trajectories beginning
in S remain in S for all time. Then if S contains only the origin, the origin is
globally asymptotically stableall trajectories converge to the origin.
Show how the Krasovskii-LaSalle principle is violated for centralized multijoint PD setpoint control with gravity compensation, using the energy function
V (x) from Equation (11.24), if Kp = 0 or Kd = 0. For a practical robot system,
is it possible to use the Krasovskii-LaSalle invariance principle to demonstrate
global asymptotic stability even if Kd = 0? Explain your answer.
14. The two-joint robot of Exercise 10 can be controlled in task space using
the endpoint task coordinates X = (x, y), as shown in Figure 11.18. The task
Give the Jacobian J() and the task-space dynamics
space velocity is V = X.
b
models (),
b(, V), and b() in the feedback linearizing control law (11.31).
15. Choose appropriate space and end-effector reference frames {s} and {b}
and express natural and artificial constraints, six each, that achieve the following
tasks: (a) opening a cabinet door; (b) turning a screw that advances linearly a
distance p for every revolution; and (c) drawing a circle on a chalkboard with a
piece of chalk.
16. Assume the end-effector of the two-joint robot in Figure 11.18 is constrained
to move on the line x y = 1. The robots link lengths are L1 = L2 = 1. (a)
(b) Write
Write the constraint as A(X)V = 0, where X = (x, y) and V = X.
11.8. Exercises
335
I measured
com
I com / com
I com
I error
current
sensor
PWM
PI
H-bridge
controller
measured
com
_
+
error
PWM
PI
H-bridge
controller
link
DC
motor
gear
G
torque
sensor
DC
motor
gear
G
link
strain
gauge
Figure 11.19: Two methods for controlling the torque at a joint driven by a
geared DC motor. (Top) The current to the motor is measured by measuring
the voltage across a small resistance in the current path. A PI controller works
to make the actual current better match the requested current Icom . (Bottom)
The actual torque delivered to the link is measured by a strain gauge.
the constraint as A() = 0.
17. Derive the constrained motion equations (11.43) and (11.44). Show all the
steps.
18. We have been assuming that each actuator delivers the torque requested by
the control law. In fact, there is typically an inner control loop running at each
actuator, typically at a higher servo rate than the outer loop, to try to track
the torque requested. Figure 11.19 shows two possibilities for a DC electric
motor, where the torque delivered by the motor is proportional to the current
I through the motor, = Kt I. The torque from the DC motor is amplified by
the gearhead with gear ratio G.
In one control scheme, the motor current is measured by a current sensor
and compared to the desired current Icom ; the error is passed through a PI controller which sets the duty cycle of a low-power pulse-width-modulation (PWM)
digital signal; and the PWM signal is sent to an H-bridge that generates the
actual motor current. In the second scheme, a strain gauge torque sensor is
inserted between the output of the motor gearing and the link, and the measured torque is compared directly to the requested torque com . Since a strain
gauge measures deflection, the element it is mounted on must have a finite torsional stiffness. Actuators called series elastic actuators are designed to have
particularly flexible torsional elements.
336
Robot Control
(i) For the current sensing scheme, what multiplicative factor should go in
the block labeled Icom /com ? Even if the PI current controller does its job
perfectly (Ierror = 0) and the torque constant Kt is perfectly known, what
effect may contribute to error in the generated torque?
(ii) For the strain gauge measurement method, explain the drawbacks, if any,
of having a flexible element between the gearhead and the link.
Chapter 12
Grasping and Manipulation

Most of the book so far has been concerned with kinematics, dynamics, motion
planning, and control of the robot itself. Only in Chapter 11, on the topics
of force control and impedance control, did the robot finally begin interacting
with an environment other than free space. This is when a robot really becomes
valuablewhen it can perform useful work on objects in the environment.
In this chapter our focus moves outward, from the robot itself to the interaction between the robot and its environment. The desired behavior of the robot
hand or end-effector, whether motion control, force control, hybrid motion-force
control, or impedance control, is assumed to be achieved perfectly using the
methods discussed so far. Our focus is on the contact interface between the
robot and objects, as well as contacts among objects and between objects and
constraints in the environment. In short, our focus is on manipulation rather
than the manipulator. Examples of manipulation include grasping, pushing,
rolling, throwing, catching, tapping, etc. To limit our scope, we will assume
that the manipulator, objects, and obstacles in the environment are rigid.
To simulate, plan, and control robotic manipulation tasks, we need an understanding of (at least) three elements: contact kinematics; forces applied through
contacts; and the dynamics of rigid bodies. Contact kinematics studies how rigid
bodies can move relative to each other without penetration, and classifies these
feasible motions according to whether the contacts are rolling, sliding, or separating. Contact force models address the normal and frictional forces that can
be transmitted through rolling and sliding contacts. Finally, the actual motions
of the bodies are those that simultaneously satisfy the kinematic constraints,
contact force model, and rigid-body dynamics. The environment is assumed to
consist of one or more rigid movable parts; perhaps stationary constraints such
as floors, walls, and fixtures; and one or more controlled manipulators (e.g., fingers) which could be under motion, force, hybrid motion-force, or impedance
control.
337
338
12.1
Contact Kinematics
Contact kinematics is the study of how two or more rigid bodies can move
relative to each other while respecting the impenetrability constraint. It also
classifies motion in contact as either rolling or sliding. Lets start by looking at
a single contact between two rigid bodies.
12.1.1
First-Order Analysis of a Single Contact
Consider two rigid bodies whose configuration is given by the local coordinate
column vectors q1 and q2 , respectively. Writing the composite configuration
as q = [q1T , q2T ]T , we define a distance function d(q) between the parts that is
positive when they are separated, zero when they are touching, and negative
when they are in penetration. When d(q) > 0, there are no constraints on the
motions of the parts; each is free to move with six degrees of freedom. When
d,
etc., to
the parts are in contact (d(q) = 0), we look at the time derivatives d,
determine if the parts stay in contact or break apart as they follow a particular
trajectory q(t). This can be determined by the following table of possibilities:
d
>0
<0
=0
=0
=0
=0
etc.
>0
<0
=0
=0
>0
<0
...
no contact
infeasible (penetration)
in contact, but breaking free
in contact, but breaking free
The contact is maintained only if all time derivatives are zero.

Now lets assume that the two bodies are initially in contact (d = 0) at a
single point. The first two time derivatives of d are written
d
q
d =
q
2d
d
d = qT 2 q +
q.
q
q
(12.1)
(12.2)
The terms d/q and 2 d/q 2 carry information about the local contact geometry. The gradient vector d/q corresponds to the separation direction in q
space associated with the contact normal (Figure 12.1). The matrix 2 d/q 2
encodes information about the relative curvature of the parts at the contact
point.
In this chapter we assume that only contact normal information d/q is
available at contacts. Other information about the local contact geometry, including the contact curvature 2 d/q 2 and higher derivatives, is unknown. With
12.1. Contact Kinematics
339
contact
tangent plane
n^
contact
normal
Figure 12.1: (Left) The parts A and B in single-point contact define a contact
tangent plane and a contact normal vector n
perpendicular to the tangent plane.
By default, the positive direction of the normal is chosen into body A. Since
contact curvature is not addressed in this chapter, the contact places the same
restrictions on the motions of the rigid bodies in the middle and right panels.
this assumption, we truncate our analysis at Equation (12.1) and assume bodies
remain in contact if d = 0. Since we deal only with the first-order contact derivative d/q, we refer to our analysis as a first-order analysis. By a first-order
analysis, the contacts in Figure 12.1 are treated identically.
As indicated in the table above, a second-order analysis incorporating contact curvature 2 d/q 2 may indicate that the contact is actually breaking or
penetrating even when d = d = 0. We will see examples of this, but a detailed
analysis of second-order contact conditions is beyond the scope of this chapter.
12.1.2
Contact Types: Rolling, Sliding, and Breaking Free
Given two bodies in single-point contact, they undergo a roll-slide motion

if the contact is maintained. The constraint that contact is maintained is a
holonomic constraint, as discussed in Chapter ??. A necessary condition for
maintaining contact is d = 0.
Lets write the velocity constraint d = 0 in a form that does not require an
explicit distance function, based on the contact normal (Figure 12.1). Let n
be
a unit vector aligned with the contact normal, expressed in a world frame. Let
pA be a representation of the point in contact on part A in the world frame,
and pB be the point in contact on part B. The condition d = 0 is written
n
T (pA pB ) = 0.
(12.3)
Since the contact normal is defined as into body A, the impenetrability constraint d 0 is written
n
T (pA pB ) 0.
(12.4)
Let us rewrite the constraint (12.4) in terms of the twists VA = (A , vA ) and
340
VB = (B , vB ) of parts A and B in a space frame, respectively.1 Note that

pA = vA + A pA = vA + [A ]pA
pB = vB + B pB = vB + [B ]pB .
We can also write the wrench F = (m, f ) corresponding to a unit force applied
along the contact normal:
F = (pA n
, n
) = ([pA ]
n, n
).
While it is not necessary to appeal to forces in a purely kinematic analysis of
rigid bodies, we will find it convenient to adopt this notation now in anticipation
of contact forces in Section 12.2.
With these expressions, the inequality constraint (12.4) can be written
(impenetrability constraint)
F T (VA VB ) 0.
(12.5)
(See Exercise 1). If

(active constraint)
F T (VA VB ) = 0,
(12.6)
then, to first order, the constraint is active and the parts remain in contact.
In the case that B is a stationary fixture, the impenetrability constraint
(12.5) simplifies to
F T VA 0.
(12.7)
If the condition (12.7) is satisfied, F and VA are said to be repelling. If
F T VA = 0, F and VA are said to be reciprocal and the constraint is active.
Twists VA and VB satisfying (12.6) are called first-order roll-slide motionsthe contact may be either sliding or rolling. Roll-slide contacts may
be further separated into rolling contacts and sliding contacts. The contact
is rolling if the parts have no motion relative to each other at the contact:
(rolling constraint)
pA = vA + [A ]pA = vB + [B ]pB = pB .
(12.8)
Note that rolling contacts include those where the two parts remain stationary
relative to each other, i.e., no relative rotation.
If the twists satisfy Equation (12.6) but not the rolling equations of (12.8),
then they are sliding.
We assign a rolling contact the contact label R, a sliding contact the label
S, and a contact that is breaking free (the impenetrability constraint (12.5) is
satisfied but not the active constraint (12.6)) the label B.
The distinction between rolling and sliding contacts becomes especially important when we consider friction forces in Section 12.2.
1 All
twists and wrenches are expressed in an inertial space frame in this chapter.
341
Az
z^
Az
breaking free
B
de
sli
ll-
ro
breaking free
B
ns
io
ot
m
y^
x^
pA = (1,2,0)
B
rolling
R
vAx
penetrating
vAy
penetrating
vAy
sliding
S
Az + vAy = 0
n^ = (0,1,0)
Figure 12.2: Example 12.1. (Left) The body B makes contact with A at pA =
pB = (1, 2, 0) with normal n
= (0, 1, 0). (Middle) The velocities VA and their
corresponding contact labels for B stationary and A confined to a plane. The
contact normal force F is mz = 1, fx = 0, fy = 1. (Right) Looking down the
vAx axis.
Example 12.1. Consider the contact shown in Figure 12.2. Parts A and B
are in contact at pA = pB = (1, 2, 0) with contact normal direction n
= (0, 1, 0).
The impenetrability constraint (12.5) is
F T (VA VB ) 0
([pA ]
n, n
)T (A B , vA vB ) 0
[0, 0, 1, 0, 1, 0] [Ax Bx , Ay By , Az Bz ,
vAx vBx , vAy vBy , vAz vBz ]T 0
Az Bz + vAy vBy 0,
and therefore roll-slide twists satisfy
Az Bz + vAy vBy = 0.
(12.9)
Equation (12.9) defines an 11-dimensional hyperplane in the 12-dimensional

space of twists (VA , VB ).
The rolling constraints (12.8) are
vAx + Az pAy Ay pAz = vBx + Bz pBy By pBz
vAy + Az pAx Ax pAz = vBy + Bz pBx Bx pBz
vAz + Ax pAy Ay pAx = vBz + Bx pBy By pBx ,
and plugging in for pA and pB , we get
vAx + 2Az = vBx + 2Bz
(12.10)
vAy + Az = vBy + Bz
(12.11)
vAz + 2Ax Ay = vBz + 2Bx By .
(12.12)
342
The constraint equations (12.10)(12.12) define a 9-dimensional hyperplane subset of the 11-dimensional hyperplane of roll-slide twists.
To visualize the constraints in a low-dimensional space, lets assume that
B is stationary (VB = 0) and A is confined to the z = 0 plane, i.e., VA =
(Ax , Ay , Az , vAx , vAy , vAz ) = (0, 0, Az , vAx , vAy , 0). The wrench F is written (mz , fx , fy ) = (1, 0, 1). The roll-slide constraint (12.9) reduces to
Az + vAy = 0,
while the rolling constraints simplify to
vAx + 2Az = 0
vAy + Az = 0
The single roll-slide constraint yields a plane in the (Az , vAx , vAy ) space, and
the two rolling constraints yield a line in that plane. Because VB = 0, the
constraint surfaces pass through the origin VA = 0. If VB 6= 0, this is no longer
the case in general.
Figure 12.2 graphically shows that nonpenetrating twists VA must have a
nonnegative dot product with the constraint wrench F when VB = 0.
12.1.3
Multiple Contacts
Now suppose that A is subject to several contacts, from B and perhaps other
parts. Each impenetrability constraint (12.5) constrains VA to a half-space of
its six-dimensional twist space bounded by a five-dimensional hyperplane of the
form F T VA = F T VB . Unioning the set of constraints from all the contacts, we
get a polyhedral convex set (polytope2 for short) V of feasible twists in the
VA space, written
V = {VA | FiT (VA Vi ) 0 i},
where Fi corresponds to the ith contact normal and Vi is the twist of the other
part in contact at contact i. A contact constraint i is redundant if the half-space
constraint contributed by Fi does not change the feasible twist polytope V . In
general, the feasible twist polytope for a part can consist of a six-dimensional
interior (where no contact constraint is active), five-dimensional faces where one
constraint is active, four-dimensional faces where two constraints are active, and
so on, down to one-dimensional edges and zero-dimensional points. A twist VA
on an n-dimensional facet of the polytope indicates that 6 n independent
(non-redundant) contact constraints are active.
If all of the bodies providing constraints are stationary, such as fixtures, then
each constraint hyperplane defined by (12.5) passes through the origin of the VA
space. We call such a constraint homogeneous. The feasible twist set becomes
2 We use the term polytope to refer generally to a convex set bounded by hyperplanes in
an arbitrary vector space. The set need not be finite; it could be a cone with infinite volume.
It could also be a point at the origin, or the null set if the constraints are incompatible with
the rigid-body assumption.
343
a cone rooted at the origin, called a (homogeneous) polyhedral convex cone.

Let Fi be the constraint wrench of stationary contact i. Then the feasible twist
cone V is
V = {VA | FiT VA 0 i}.
If the Fi positively span the six-dimensional wrench space, or, equivalently, the
convex hull of the Fi contains the origin in the interior, then the feasible twist
polytope reduces to a point at the origin, the stationary contacts completely
constrain the motion of the part, and we have form closure, discussed in more
detail in Section 12.1.7.
As mentioned in Section 12.1.2, each point contact i can be given a label
corresponding to the type of contact: B if the contact is breaking, R if the
contact is rolling, and S if the contact is sliding, i.e., (12.6) is satisfied but
(12.8) is not. The contact mode for the entire system can be written as the
concatenation of the contact labels at the contacts. Since we have three distinct
contact labels, a system of bodies with k contacts can have a maximum of 3k
contact labels. Some of these contact modes may not be feasible, however, as
their corresponding kinematic constraints may not be compatible.
Example 12.2. Figure 12.3 shows triangular fingers contacting a hexagonal
part A. To more easily visualize the contact constraints, the hexagon is restricted to translational motion in a plane only, so that its twist can be written
VA = (0, 0, 0, vAx , vAy , 0). In Figure 12.3(a), the single stationary finger creates
a contact wrench F1 that can be drawn in the VA space. All feasible twists
have a nonnegative component in the direction of F1 . Roll-slide twists satisfying F1T VA = 0 lie on the constraint line. Since no rotations are allowed, the
only twist yielding a rolling contact is VA = 0. In Figure 12.3(b), the union of
the constraints due to two stationary fingers creates a cone of feasible twists.
Figure 12.3(c) shows three fingers in contact, one of which is moving with twist
V3 . Because the moving finger has nonzero velocity, its constraint half-space is
displaced from the origin by V3 . The result is a closed polygon of feasible twists.
Example 12.3. Figure 12.4 shows the contact normals of three stationary
contacts with a planar part A, not shown. The part moves in a plane, so vAz =
Ax = Ay = 0. In this example we do not distinguish between rolling and
sliding motions, so the locations of the contacts along the normals are irrelevant.
The three contact wrenches, written (mz , fx , fy ), are F1 = (0, 1, 2), F2 =
(1, 0, 1), and F3 = (1, 0, 1), yielding the motion constraints
vAy 2Az 0
vAx + Az 0
vAx + Az 0.
These constraints describe a polyhedral convex cone of feasible twists rooted at
the origin, as illustrated in Figure 12.4.
344
V3
vAy
vAy
BBS
SB
R
B
vAx
RR
(a)
SBS
vAy
(b)
BB
BS
SBB
BSS
BBB
vAx
RRB
BSB
V3
vAx
(c)
Figure 12.3: Motion-controlled fingers contacting a hexagon that is constrained

to translate in a plane only (Example 12.2). (a) A single stationary finger provides a single half-space constraint on the hexagons twist VA . The feasible motion half-space is shaded gray. The two-dimensional set of twists corresponding
to breaking contact B, the one-dimensional set corresponding to sliding contact
S, and the zero-dimensional set corresponding to rolling (fixed) contact R are
shown. (b) The union of constraints from two stationary fingers creates a cone
of feasible twists. This cone corresponds to four possible contact modes: RR,
SB, BS, and BB. The contact label for the leftmost finger is given first. (c) Three
fingers, one of which is moving with a linear velocity V3 , create a closed polygon of feasible twists. There are seven possible contact modes corresponding to
the feasible twists: a two-dimensional set where all contacts are breaking, three
one-dimensional sets where one contact constraint is active, and three zerodimensional sets where two contact constraints are active. Note that rolling
contact at the moving finger is not feasible, since translation of the hexagon to
follow the moving finger, as indicated by the at the lower right of the figure,
would violate one of the impenetrability constraints. If the third finger were
stationary, the only feasible motion of the hexagon would be zero velocity, with
contact mode RRR.
12.1.4
Collections of Parts
The discussion above can be generalized to find the feasible twists of multiple
parts in contact. If parts i and j make contact at a point p, where n
points into
part i and F = ([p]
n, n
), then their spatial twists Vi and Vj must satisfy the
constraint
F T (Vi Vj ) 0
(12.13)
to avoid penetration. This is a homogeneous half-space constraint in the composite (Vi , Vj ) twist space. In an assembly of multiple parts, each pairwise
345
Az
contact 2
^y
contact 1
x^
contact 1
contact 3
contact 3
vAx
contact 2
vAy
Figure 12.4: Example 12.3. (Left) The lines of force corresponding to three
stationary contacts on a planar body. If we are only concerned with feasible
motions, and do not distinguish between rolling and sliding, contacts anywhere
along the lines, with the contact normals shown, are equivalent. (Right) The
three constraint half-spaces define a polyhedral convex cone of feasible twists.
In the figure, the cone is truncated at the plane vAy = 2. The outer faces of the
cone are shaded white and the inner faces are gray. Twists in the interior of the
Az
cone correspond to all contacts
breaking, while twists on the faces of the cone
correspond to an active constraint.
contact 1
contact contributes another constraint in the composite twist space, and the
result is a polyhedral convex cone of kinematically feasible twists rooted at the
contact for
3
origin of the composite
twist space. The contact mode
the entire assembly
vAx
vAy
is the concatenation of thecontact
contact
labels at each contact in the assembly.
2
If there are moving contacts whose motion is prescribed, e.g., robot fingers,
the constraints on the motion of the remaining parts are no longer homogeneous.
As a result, the polyhedral feasible twist space is no longer a cone rooted at the
origin.
12.1.5
Other Types of Contacts
We have been considering point contacts of the type shown in Figure 12.5(a),
where at least one of the bodies in contact uniquely defines the contact normal.
Figures 12.5(b)-(e) show other types of contact. The kinematic constraints provided by the convex-concave vertex, line, and plane contacts of Figures 12.5(b)(d) are, to first order, identical to those provided by finite collections of singlepoint contacts. Constraints provided by other points of contact are redundant.
The degenerate case in Figure 12.5(e) is ignored, as there is no unique definition
of a contact normal.
The impenetrability constraint (12.5) derives from the fact that arbitrarily
large contact forces can be applied in the normal direction to prevent penetration. In Section 12.2, we will see that tangential forces may also be applied
due to friction, and these forces may prevent slipping between two objects in
contact. Normal and tangential contact forces are subject to constraints: the
normal force must be pushing into a part, not pulling, and the maximum friction
346
(a)
(b)
(c)
(d)
(e)
Figure 12.5: (a) Vertex-face contact. (b) A convex vertex contacting a concave
vertex can be treated as multiple point contacts, one at each face adjacent to
the concave vertex. These faces define the contact normals. (c) A line contact
can be treated as two point contacts at either end of the line. (d) A plane
contact can be treated as point contacts at the corners of the convex hull of the
contact area. (e) Convex vertex-vertex contact. This case is degenerate and not
considered.
force is proportional to the normal force.
If we wish to apply a kinematic analysis that can approximate the effects
of friction without explicitly modeling forces, we can define three purely kinematic models of point contacts: a frictionless point contact, a hard-finger
contact (or point contact with friction), and a soft-finger contact. A frictionless point contact enforces only the roll-slide constraint (12.5). A hard-finger
contact also enforces the rolling constraints (12.8), implicitly modeling friction
sufficient to prevent slip at the contact. A soft-finger contact enforces the rolling
constraints (12.8) as well as one more constraint: the two bodies in contact may
not spin relative to each other about the contact normal axis. This models deformation and the resulting friction moment resisting spin due to the small but
nonzero contact area between the two bodies. For planar problems, a hard-finger
contact and a soft-finger contact are identical.
While these kinematic models are convenient for studying the kinematic
mobility of bodies in contact, we will not use them in the rest of this chapter.
12.1.6
Planar Graphical Methods
Planar problems allow the possibility of using graphical methods to visualize the
feasible motions for a single body, since the space of twists is three-dimensional.
An example planar twist cone is shown in Figure 12.4. Such a figure would be
very difficult to draw for a system with more than three degrees of freedom.
A convenient way to represent a planar twist V = (z , vx , vy ) is as a center
of rotation (CoR) at (vy /z , vx /z ) plus the angular velocity z . The CoR is
the point in the (projective) plane that remains stationary under the motion.3
In the case that the speed of motion is immaterial, we may simply label the
CoR with a +, , or 0 sign representing the direction of rotation (Figure 12.6).
3 Note
infinity.
that the case z = 0 must be treated with care, as it corresponds to a CoR at
347
+ CoR
Figure 12.6: Given the velocity of two points on the part, the lines normal to
the velocities intersect at the CoR. The CoR shown is labeled + corresponding
to the (counterclockwise) positive angular velocity of the part.
V
z
Figure 12.7: Mapping a planar twist V to a CoR. The ray containing a vector
V intersects either the plane of + CoRs at z = 1, the plane of CoRs at
z = 1, or the circle of translation directions.
The mapping from planar twists to CoRs is illustrated in Figure 12.7, which
shows that the space of CoRs consists of a plane of + CoRs (counterclockwise),
a plane of CoRs (clockwise), and a circle of translation directions.
Given two distinct twists V1 and V2 , the set of linear combinations of these
velocities k1 V1 + k2 V2 , where k1 , k2 R, maps to the line of CoRs containing
CoR(V1 ) and CoR(V2 ). Since k1 and k2 can have either sign, if either 1z or
2z is nonzero, the CoRs on this line can have either sign. If 1z = 2z = 0,
then this set corresponds to the set of all translation directions.
A more interesting case is when k1 , k2 0. Given two twists V1 and V2 , the
nonnegative linear combination of these two velocities is written
V = pos({V1 , V2 }) = {k1 V1 + k2 V2 | k1 , k2 0},
348

z
z
Figure 12.8: The intersection of a twist cone with the unit twist sphere, and the
representation of the cone as a set of CoRs.
+
+
+
_
+
(a)
(b)
+
+
(c)
(d)
Figure 12.9: (a) Positive linear combination of two CoRs labeled +. (b) Positive
linear combination of a + CoR and a CoR. (c) Positive linear combination of
three + CoRs. (d) Positive linear combination of two + CoRs and a CoR.
a polyhedral convex twist cone rooted at the origin, with V1 and V2 defining the
edges of the cone. If 1z and 2z have the same sign, then the CoRs of their
nonnegative linear combinations CoR(pos({V1 , V2 })) all have that sign, and lie
on the line segment between the two CoRs. If CoR(V1 ) and CoR(V2 ) are labeled
+ and , respectively, then CoR(pos({V1 , V2 })) consists of the line containing
the two CoRs, minus the segment between the CoRs. This set consists of a ray
of CoRs labeled + attached to CoR(V1 ), a ray of CoRs labeled attached to
CoR(V2 ), and a point at infinity labeled 0, corresponding to translation. This
collection should be considered as a single line segment (though one passing
through infinity), just like the first case. Figures 12.8 and 12.9 show examples
of CoR regions corresponding to positive linear combinations of planar twists.
The CoR representation of planar twists is particularly useful for representing the feasible motions of one movable body in contact with stationary bodies.
Since the constraints are stationary, as noted in Section 12.1.3, the feasible
twists form a polyhedral convex cone rooted at the origin. This cone can be
represented uniquely by a set of CoRs with +, , and 0 labels. A general twist
polytope, as in the case of moving constraints, cannot be uniquely represented
by a set of CoRs with +, , and 0 labels.
Given a contact between a stationary body and a movable body, we can plot
the CoRs that do not violate the impenetrability constraint. Label all points
349
+, Sr
, Sl
, R
+, B
+, Sl
, B
, Sr
Figure 12.10: The stationary triangle makes contact with a movable part. CoRs
to the left of the contact normal are labeled +, to the right are labeled , and
on the normal labeled . Also given are the contact types for the CoRs. For
points on the contact normal, the sign assigned to Sl and Sr CoRs switches at
the contact point.
on the contact normal , points to the left of the inward normal +, and points
to the right . All points labeled + can serve as CoRs with positive angular
velocity for the movable body, and all points labeled can serve as CoRs with
negative angular velocity, without violating the first-order contact constraint.
We can further assign contact labels to each CoR corresponding to the firstorder conditions for breaking contact B, sliding contact S, and rolling contact
R. For planar sliding, we subdivide the label S into two subclasses: Sr, where
the moving body slips right relative to the fixed constraint, and Sl, where the
moving body slips to the left. Figure 12.10 illustrates the labeling. If there is
more than one contact, we simply union the constraints and contact labels from
the individual contacts.
Example 12.4. Figure 12.11(a) shows a planar part standing on a table while
being contacted by a stationary robot finger. The finger defines an inequality
constraint on the parts motion and the table defines two more. The cone of
twists that do not violate the impenetrability constraints is represented by the
CoRs that are consistently labeled for each contact (Figure 12.11(b)). Each
feasible CoR is labeled with a contact mode that concatenates the labels for the
individual contacts (Figure 12.11(c)).
Now look more closely at the CoR labeled (+, SrBSr) in Figure 12.11(c).
Is this motion really possible? It should be apparent that it is, in fact, not
possible: the part would immediately penetrate the stationary finger. Our
incorrect conclusion is due to the fact that our first-order analysis ignores the
local contact curvature. A second-order analysis would show that this motion
is impossible. On the other hand, if the radius of curvature of the part at the
contact were sufficiently small, then the motion would be possible.
F
w1
F
w33
wF
22
w1 w
F
1 F22
350
Fw33
(a)
z
F
w1
F
w33
+
vx
Fw33
vy
(b)
sSlSlB
ls lb
at infinity
BSlB
bslb
(b)
srbsr
BBSr
bbsr SrBSr
sSlSlB
ls lb
at infinity
srbsr
BBSr
bbsr SrBSr
wF
22
(a)
(a)
BSlB
bslb
w1 w
F
1 F22
vy

vx
(b)
BBB
bbb
bbb
BBB
BBB
bbb
bslsl
BSlSl
bb
sl BBR
bbf BBSr
bbsr
BBSl
srbb
SrBB
fbb
RBB
slbb
SlBB
sSlSlB
lslb
at infinity
(c)
bs s
l l
Figure 12.11: Example 12.4. (a) ABSlSl
part
resting
bb
sl BBR
bbf on
bbsar table and the three contact
BBSr
BBSl
s
bb
SrBB
constraints.
(b)
The
feasible
twists
representated
as CoRs, shown in gray. Note
r
+
+
fbb
RBB
that the lines that extendbbb
off
to
the
left
and
to
the bottom wrap around
BBB
at infinity and come back in from the
right
and
the
top, respectively, so this
s
bb
SlBB
l
CoR region should be interpreted as a single connected convex region. (c) The
sSlSlB
lslb
contact modes assigned to each feasible
motion. The zero velocity contact mode
at infinity
is RRR.
(c)
(d)
Thus a first-order analysis of a contact indicating roll-slide motion might be
classified as penetrating or breaking by a second-order analysis. Similarly, if
our second-order analysis indicates a roll-slide motion, a third or higher-order
analysis may indicate penetration or breaking free. In any case, if an nth-order
analysis indicates that the contact is breaking or penetrating, then no analysis
of order greater than n will change the conclusion, because the higher-order
effects are smaller.
12.1.7
Form Closure
Form closure of an object is achieved if a set of stationary constraints prevents

all motion of the object. If these constraints are robot fingers, we call this a
form-closure grasp.
Before proceeding, we introduce a few definitions from linear algebra.
Definition 12.1. Given a set of j vectors A = a1 , . . . aj Rn , we define the
linear span, or the linear combinations, of the vectors to be
( j
)
X
span(A) =
ki ai | ki R ,
i=1
the nonnegative linear combinations, sometimes called the positive span,
(a)
351
(b)
(c)
(d)
Figure 12.12: (a) Three vectors in R2 , drawn as arrows from the origin. (b) The
linear span of the vectors is the entire plane. (c) The positive linear span is the
cone shaded gray. (d) The convex combination is the polygon and its interior.
to be
(
pos(A) =
j
X
)
ki ai | ki 0 ,
i=1
and the convex span to be

(
conv(A) =
j
X
)
ki ai | ki 0 and
i=1
ki = 1 .
Clearly conv(A) pos(A) span(A) (see Figure 12.12). The following

facts from linear algebra will also be useful.
1. The space Rn can be linearly spanned by n vectors, but no fewer.
2. The space Rn can be positively spanned by n + 1 vectors, but no fewer.
3. From any set A of vectors positively spanning Rn , a subset of no more than
2n vectors can be chosen that also positively spans Rn .
The first fact is implicit in our use of n coordinates to represent Rn . Fact
2 follows from the fact that for any choice of n vectors A = {a1 , . . . , an }, there
exists a vector c Rn such that aTi c 0 for all i. In other words, no nonnegative
combination of vectors in A can create a vector in the direction c. On the
n
other hand, if we choose
Pna1 , . . . , an to be orthogonal coordinate bases of R ,
then choose an+1 = i=1 ai , we see that this set of n + 1 vectors positively
spans Rn . Fact 3 is stated without proof, and all three facts are illustrated in
Figure 12.13.
These definitions and facts are useful in determining the number of contacts
needed to immobilize an object by form closure.
12.1.7.1
Number of Contacts Needed for First-Order Form Closure
Each stationary contact i provides a half-space twist constraint of the form

FiT V 0.
352
(a)
(b)
(c)
Figure 12.13: (a) Two vectors spanning the plane but not positively spanning
the plane. (b) Three vectors positively spanning the plane. (c) Four vectors
positively spanning the plane from which no three vectors can be chosen which
also positively span the plane.
Form closure holds if the only velocity V satisfying the constraints is the zero
velocity. For j contacts, this condition is equivalent to
pos({F1 , . . . , Fj }) = R6
for parts in three dimensions. Therefore, by Fact 2 above, at least 6 + 1 = 7
contacts are needed for first-order form closure of spatial parts. For planar
parts, the condition is
pos({F1 , . . . , Fj }) = R3 ,
and 3 + 1 = 4 contacts are needed for first-order form closure. These results are
summarized in the following theorem.
Theorem 12.1. For a planar part, at least four contacts are needed for firstorder form closure. For a spatial part, at least seven contacts are needed.
Now consider the problem of grasping a circular disk in the plane. It should
be clear that kinematically preventing motion of the disk is impossible regardless
of the number of contacts; it will always be able to spin about its center. Such
objects are called exceptionalthe positive span of the contact normal forces
at all points on the object is not equal to Rn , where n = 3 in the planar case
and n = 6 in the spatial case. Examples of such objects in three dimensions
include surfaces of revolution, such as spheres and ellipsoids.
Apart from these exceptional objects, we can directly use Fact 3 to show
that no more than six contacts are needed for planar form closure and no more
than twelve contacts for spatial form closure. With more detailed analysis, it is
possible to reduce the numbers further (to four and seven contacts for planar
and spatial cases, respectively), but we do not go into details here.
Figure 12.14 shows example planar grasps. The graphical methods of Section 12.1.6 indicate that the four contacts in Figure 12.14(a) immobilize the
object. Our first-order analysis indicates that the parts in Figures 12.14(b) and
12.14(c) can each rotate about their centers in the three-finger grasps, but in
(a)
(b)
353
(c)
(d)
(e)
(f)
Figure 12.14: (a) Four fingers yielding planar form closure. The first-order
analysis treats (b) and (c) identically, saying the triangle can rotate about its
center in each case. A second-order analysis shows this is not possible for (b).
The grasps in (d), (e), and (f) are identical by a first-order analysis, which says
that rotation about any center on the vertical line is possible. This is true for
(d), while only some of these centers are possible for (e). No motion is possible
in (f).
fact this is not possible for the part in Figure 12.14(b)a second-order analysis would tell us that this part is actually immobilized. Finally, the first-order
analysis tells us that the two-finger grasps in Figures 12.14(d)-(f) are identical,
but in fact the part in Figure 12.14(f) is immobilized by only two fingers due to
curvature effects.
To summarize, our first-order analysis always correctly labels breaking and
penetrating motions, but second- and higher-order effects may change firstorder roll-slide motions to breaking or penetrating. If an object is in form
closure by the first-order analysis, it is in form closure for any analysis; if only
roll-slide motions are feasible by the first-order analysis, the object could be in
form closure by a higher-order analysis; and otherwise the object is not in form
closure by any analysis.
12.1.7.2
A Linear Programming Test for First-Order Form Closure
Let F = [ F1 | F2 | . . . | Fj ] Rnj be a matrix whose columns are formed by

the j contact wrenches. For spatial parts, n = 6, and for planar parts, n = 3
with Fi = [miz , fix , fiy ]T . The contacts yield form closure if there exists a
vector of weights k Rj , k 0 such that F k = Fext for all Fext Rn .
Clearly the part is not in form closure if the rank of F is not full (rank(F ) <
n). If F is full rank, the form-closure condition above is equivalent to the
existence of strictly positive coefficients k > 0 such that F k = 0. We can
formulate this test as the following linear program:
find k
(12.14)
T
minimizing 1 k
such that F k = 0
k 1,
354

F4
y^
F1
F3
^x
F2
Figure 12.15: Two fingers grasping the interior of an object.

where 1 is a j-vector of ones. If F is full rank and there exists a solution k to
(12.14), the part is in first-order form closure. Otherwise it is not. Note that the
objective function 1T k is not strictly necessary to answer the binary question,
but it is included to make sure the problem is well posed, depending on the LP
solver.
Example 12.5. The planar object in Figure 12.15 has a hole in the center.
Two fingers each touch two different edges of the hole, creating four contact
normals. The matrix F = [ F1 | F2 | F3 | F4 ] is
0
0 1 2
0
1 0 .
F = 1
0 1
0 1
The matrix F is clearly rank 3. The linear program of (12.14) returns a solution
with k1 = k3 = 2, k2 = k4 = 1, so the grasp is form closure. If the circular
finger were moved to the bottom right corner of the hole, the new F matrix
0
0 0 2
0 1
0
F = 1
0 1 0 1
is still full rank, but there is no solution to the linear program. This grasp is
not form closure.
12.1.7.3
Measuring the Quality of a Form-Closure Grasp
Consider the two form-closure grasps shown in Figure 12.16. Which is a better
grasp?
Answering this question obviously requires a metric measuring the quality
of a grasp. A grasp metric takes the set of contacts {Fi } and returns a single
value Qual({Fi }), where Qual({Fi }) < 0 indicates that the grasp is not form
closure, and larger positive values indicate better grasps.
355
Figure 12.16: Both grasps are form closure, but which is better?
F2
F1
(a)
F3
F2
F1
(b)
F3
Figure 12.17: (a) A set of three wrenches in a two-dimensional wrench space,

the set of all wrenches that can be applied by the contacts, and the radius d
of the largest ball of wrenches centered at the origin that fits inside the wrench
polygon. (b) A different set of three wrenches yielding a larger inscribed ball.
There are many reasonable choices of grasp metric. As an example, suppose
that to avoid damaging the object, we require the magnitude of the force at
contact i be less than or equal to fi,max > 0, which may be set to 1. Then the
total set of contact wrenches that can be applied by the j contacts is given by
( j
)
X
CF =
fi Fi | fi [0, fi,max ] .
(12.15)
i=1
See Figure 12.17 for an example in two dimensions. This is the convex set of
wrenches that the contacts can apply to resist disturbance wrenches applied to
the part. If the grasp is form closure, the set includes the origin of the wrench
space.
Now the problem is to turn this polytope into a single number representing
the quality of the grasp. Ideally this process would use some idea of the distur-
356
bance wrenches the part can be expected to experience. A more common choice
is to set Qual({Fi }) to be the radius of the largest ball of wrenches, centered
at the origin of the wrench space, that fits inside the convex polytope. In evaluating this radius, two caveats should be considered: (1) moments and forces
have different units, so there is no obvious way to equate force and moment
magnitudes, and (2) the moments due to contact forces depend on the choice of
the location of the origin of the space frame. To address (1), it is common to
choose a characteristic length r of the grasped part and convert contat moments
m to forces m/r. To address (2), typically the origin is chosen somewhere near
the geometric center of the part, or at its center of mass.
Given the choice of the space frame and the characteristic length r, we
simply calculate the signed distance from the origin of the wrench space to
each hyperplane on the boundary of CF . The minimum of these distances is
Qual({Fi }) (Figure 12.17).
Returning to our original example in Figure 12.16, we can see that if each
finger is allowed to apply the same force, then the grasp on the left is likely
to be considered the better grasp, as the contacts can resist greater moments
about the center of the object.
12.1.7.4
Choosing Contacts for Form Closure
Many methods have been suggested for choosing form-closure contacts for fixturing or grasping. A typical approach is to sample candidate grasp points on
the surface of the object (four for planar parts or seven for spatial parts) until
a set is found yielding form closure. From there, the candidate grasp points
may be incrementally repositioned according to gradient ascent using the grasp
metric, i.e., Qual(p)/p, where p is a vector representing the grasp locations.
12.2
Contact Forces and Friction
12.2.1
Friction
A commonly-used model of friction in robotic manipulation is Coulomb friction. This experimental law states that the tangential friction force magnitude
ft is related to the normal force magnitude fn by ft fn , where is called
the friction coefficient. If the contact is sliding, or currently rolling but with
incipient slip (i.e., at the next instant the contacts are sliding), then ft = fn ,
and the friction force opposes the motion. The friction force is independent of
the speed of sliding.
Often two friction coefficients are defined, a static friction coefficient s and
a kinetic (or sliding) friction coefficient k , where s k . This implies that
a larger friction force is available to resist initial motion, but once motion has
begun, the resisting force is smaller. Many other friction models have been
developed with different functional dependences on factors such as the speed
of sliding and the duration of static contact before sliding (for example, see
Chapter 11.2.1). All of these are aggregate models of complex microscopic
12.2. Contact Forces and Friction
357
fz
fz
(b)
(c)
y
(a)
Figure 12.18: (a) A friction cone illustrating all possible forces that can be
transmitted through the contact. (b) A side view of the same friction cone
showing the friction coefficient and the friction angle = tan1 . (c) An
inscribed polyhedral convex cone approximation to the circular friction cone.
behavior. For simplicity, we use the simplest Coulomb friction model with a
single friction coefficient . This model is reasonable for hard, dry materials.
The friction coefficient depends on the two materials in contact, and typically
ranges from 0.1 to 1.
For a contact normal in the +z direction, the set of forces that can be
transmitted through the contact satisfies
q
fx2 + fy2 fz , fz 0.
(12.16)
Figure 12.18(a) shows that this set of forces forms a friction cone. The set
of forces that the finger can apply to the plane lies inside the cone shown.
Correspondingly, the equal and opposite force the plane applies to the finger is
inside the negative of the cone. Figure 12.18(b) shows the same cone from a
side view, illustrating the friction angle = tan1 , which is the half-angle
of the cone. If the finger slips to the right, the force it applies lies on the right
edge of the friction cone, with a magnitude determined by the normal force. If
the contact is rolling, the force may be anywhere inside the cone.
To allow linear formulations of contact mechanics problems, it is often convenient to represent the convex circular cone by a polyhedral convex cone. Figure 12.18(c) shows an inscribed four-sided pyramidal approximation of the friction cone, defined by the nonnegative linear combinations of the (fx , fy , fz )
cone edges (, 0, 1), (, 0, 1), (0, , 1), and (0, , 1). We can obtain a tighter
approximation to the circular cone by using more edges. An inscribed cone underapproximates the friction forces available, while a circumscribed cone overapproximates the friction forces. The choice of which to use depends on the
application. For example, if we want to ensure that a robot hand can grasp
an object, it is a good idea to underapproximate the friction forces available.
358

mz
F2
f
fx
(a)
(b)
F1
fy
mz
f1
f2
f3
(c)
f4
composite
wrench cone
F4
fx
(d)
F2
F3
fy
F1
Figure 12.19: (a) A planar friction cone with friction coefficient and corresponding friction angle = tan1 . (b) The corresponding wrench cone. (c)
Two friction cones. (d) The corresponding composite wrench cone.
For planar problems, no approximation is necessary; a friction cone is exactly
represented by the nonnegative linear combinations of the two edges of the cone,
similar to the side view illustrated in Figure 12.18(b).
Once we choose a coordinate frame, any contact force can be expressed as
a wrench F = ([p]f, f ), where p is the contact location. This turns the friction
cone into a wrench cone. A planar example is shown in Figure 12.19. The
two edges of the planar friction cone give two rays in the wrench space, and
the wrenches that can be transmitted to the part through the contact are all
nonnegative linear combinations of basis vectors along these edges. If F1 and
F2 are basis vectors for these wrench cone edges, we write the wrench cone as
WC = pos({F1 , F2 }).
If multiple contacts act on a part, then the total set of wrenches that can
be transmitted to the part through the contacts is the nonnegative linear com-
359
bination of all the individual wrench cones WC i ,

(
)
X
WC = pos({WC i }) =
ki Fi | Fi WC i , ki 0 .
i
This composite wrench cone is a convex cone rooted at the origin. An example
composite wrench cone is shown in Figure 12.19(d) for a planar object with the
two friction cones shown in Figure 12.19(c). For planar problems, the composite
wrench cone in the three-dimensional wrench space is polyhedral. For spatial
problems, wrench cones in the six-dimensional wrench space are not polyhedral,
unless the individual friction cones have been approximated by polyhedral cones,
as in Figure 12.18(c).
If a contact or set of contacts acting on a part is ideally force-controlled, the
wrench Fext specified by the controller must lie within the composite wrench
cone corresponding to those contacts. Because these force-controlled contacts
choose a nonzero wrench from this wrench cone, the set of wrenches that can
act on the part, including other non-force-controlled contacts, may no longer
be a homogeneous cone rooted at the origin. This is analogous to the case of
velocity-controlled contacts in Section 12.1.3, where moving constraints result
in feasible part twists which are not necessarily a cone rooted at the origin.
12.2.2
Planar Graphical Methods
Just as homogeneous twist cones for planar problems can be represented as

convex signed CoR regions in the plane, homogeneous wrench cones for planar
problems can be represented as convex signed regions in the plane. One method
for doing so is called moment labeling. Given a collection of lines of force
in the plane (e.g., the edges of friction cones from a set of point contacts), the
composite wrench cone can be represented by labeling each point in the plane
with either a + if all wrenches in the cone make nonnegative moment about
that point, a if all make nonpositive moment about that point, a if all
make zero moment about that point, and a blank label if there exist wrenches
making positive moment and wrenches making negative moment about that
point.
The idea is best illustrated by an example. In Figure 12.20(a), a single line
of force F1 is represented by labeling the points to the left of the line with a +
and points to the right of the line with a . Points on the line are labeled . In
Figure 12.20(b), another line of force is added, which could represent the other
edge of a planar friction cone. Only the points in the plane that are consistently
labeled for both lines of force retain their labels; inconsistently labeled points
lose their labels. Finally, a third line of force is added in Figure 12.20(c). The
result is a single region labeled +. A nonnegative linear combination of the
three lines of force can create any line of force in the plane that passes around
this region in a counterclockwise sense; no other forces can be created. This representation is equivalent to a homogeneous convex wrench cone representation
of pos({F1 , F2 , F3 }).
360
F1
_
+
_
+
F2
F1
_
_
+
(a)
F2
F3
(b)
F1
(c)
Figure 12.20: (a) Representing a line of force by moment labels. (b) Representing the nonnegative linear combinations of two lines of force by moment labels.
(c) Nonnegative linear combinations of three lines of force.
12.2.3
Force Closure
Consider a single movable object and a number of frictional contacts. We say

the contacts result in force closure if the composite wrench cone contains the
entire wrench spaceany external wrench Fext on the object can be balanced
by contact forces.
We can derive a simple linear test for force closure which is exact for planar
cases and approximate for spatial cases. Let Fi , i = 1 . . . j be the wrenches
corresponding to the edges of the friction cones for all the contacts. For planar
problems, each friction cone contributes two edges, and for spatial problems,
each friction cone contributes three or more edges (see Figure 12.18(d)), depending on the polyhedral approximation chosen. The columns of the n j
matrix F are the Fi , where n = 3 for planar problems and n = 6 for spatial
problems. As with the test for form closure in Section 12.1.7.2, the part is in
force closure if there is a vector of weights k Rj , k 0 such that F k = Fext
for all Fext Rn . In the case of = 0, each contact can provide forces only
along the normal direction, and force closure is equivalent to first-order form
closure.
12.2.3.1
Number of Contacts Needed for Force Closure
For planar problems, four contact wrenches are sufficient to positively span
the three-dimensional wrench space, which means that as few as two frictional
contacts (with two edges each) are sufficient for force closure. Using moment
labeling, we see that force closure is equivalent to having no consistent moment
labels. For example, if the two contacts can see each other by a line inside
both friction cones, we have force closure (Figure 12.21).
361
(a)
(b)
Figure 12.21: An equilateral triangle can be force-closure grasped by two fingers

on edges of the triangle if tan 30 0.577. (a) This grasp with = 0.25
is not force closure, as indicated by the consistently labeled moment-labeling
region. (b) This grasp is force closure with = 1.
It is important to note that force closure simply means that forces at the
contacts are capable of generating any wrench. It does not necessarily mean
that the object will not move, however. For the example of Figure 12.21(b),
whether the triangle falls under gravity or not depends on the internal forces
between the fingers. If the motors powering the fingers cannot provide sufficient
forces, or if they are restricted to only generate forces in certain directions, the
triangle may fall despite force closure.
Two frictional contacts are insufficient to yield force closure for spatial parts,
as there is no way to generate moment about the axis joining the two contacts.
A force-closure grasp can be obtained with as few as three frictional contacts,
however. A particularly simple and appealing result due to Li et al. [16] reduces
the force closure analysis of spatial frictional grasps into a planar force closure
problem. Referring to Figure 12.22, suppose a rigid body is constrained by
three point contacts with friction. If the three contact points happened to be
collinear, then obviously any moment applied about this line cannot be resisted
by the three contacts. We can therefore exclude this case, and assume that the
three contact points are not collinear. The three contacts then define a unique
plane S, and at each contact point, three possibilities arise (see Figure 12.23):
The friction cone intersects S in a planar cone;
The friction cone intersects S in a line;
The friction cone intersects S in a point.
The object is in force closure if and only if each of the friction cones intersects
S in a planar cone, and S is also in planar force closure.
Theorem 12.2. Given a spatial rigid body restrained by three point contacts
with friction, the body is in force closure if and only if the friction cone at each
of the contacts intersects the plane S of the contacts in a cone, and the plane S
is in planar force closure.
362
Figure 12.22: A spatial rigid body restrained by three point contacts with friction.
Figure 12.23: Three possibilities for the intersection between a friction cone and
a plane.
Proof. First, the necessity conditionif the spatial rigid body is in force closure,
then each of the friction cones intersects S in a planar cone and S is also in planar
force closureis easily verified: if the body is in spatial force closure, then S
(which is a part of the body) must also be in planar force closure. Moreover, if
even one friction cone intersects S in a line or point, then there will be external
moments (e.g., about the line between the remaining two contact points) that
cannot be resisted by the grasp.
To prove the sufficiency conditionif each of the friction cones intersects S
in a planar cone and S is also in planar force closure, then the spatial rigid body
is in force closurechoose a fixed reference frame such that S lies in the x-y
plane, and let ri R3 denote the vector from the fixed frame origin to contact
point i (see Figure 12.22). Denoting the contact force at i by fi R3 , the
contact spatial force Fi R6 is then of the form

fi
Fi =
,
(12.17)
mi
where each mi = ri fi , i = 1, 2, 3. Denote the arbitrary external spatial force
363
Fext R6 by

Fext =
fext
mext
R6 .
(12.18)
Force closure then requires that there exist contact spatial forces Fi , i = 1, 2, 3,
each lying inside its respective friction cone, such that for any external spatial
disturbance force Fext , the following equality is satisfied:
F1 + F2 + F3 + Fext = 0,
(12.19)
or equivalently,
f1 + f2 + f3 + fext
(12.20)
(r1 f1 ) + (r2 f2 ) + (r3 f3 ) + mext
0,
(12.21)
If each of the contact forces and moments, as well as the external force and moment, is orthogonally decomposed into components lying on the plane spanned
by S (corresponding to the x-y plane in our chosen reference frame) and its
normal subspace N (corresponding to the z-axis in our chosen reference frame),
then the previous force closure equality relations can be written
= fext,S
(12.22)
(r1 f1S ) + (r2 f2S ) + (r3 f3S )
= mext,S
(12.23)
f1N + f2N + f3N
= fext,N
(12.24)
= mext,N .
(12.25)
f1S + f2S + f3S
(r1 f1N ) + (r2 f2N ) + (r3 f3N )
In what follows we shall use S to refer both to the slice of the rigid body
corresponding to the x-y plane, as well as the x-y plane itself. N will always be
identified with the z-axis.
Proceeding with the proof of sufficiency, we now show that if S is in planar force closure, then the body is in spatial force closure. In terms of Equations (12.24)-(12.25) we wish to show that, for any arbitrary forces fext,S S,
fext,N N and arbitrary moments mext,S S, mext,N N , there exist contact
forces fiS S, fiN N , i = 1, 2, 3, that satisfy (12.24)-(12.25), and such that
for each i = 1, 2, 3, the contact force fi = fiS + fiN lies in friction cone i.
First consider the force closure equations (12.24)-(12.25) in the normal direction N . Given an arbitrary external force fext,N N and external moment
mext,S S, Equations (12.24)-(12.25) constitute a set of three linear equations
in three unknowns. From our hypothesis that the three contact points are never
collinear, these equations will always have a unique solution set {f1N
, f2N
, f3N
}
in N .
Since S is assumed to be in planar force closure, for any arbitrary fext,S S
and mext,N N , there will exist planar contact forces fiS S, i = 1, 2, 3, that
lie inside their respective planar friction cones and also satisfy Equations (12.22)(12.23). This solution set is not unique: one can always find a set of internal
forces i S, i = 1, 2, 3, each lying inside its respective friction cone, satisfying
1 + 2 + 3
(12.26)
(r1 1 ) + (r2 2 ) + (r3 3 )
0.
(12.27)
364
(To see why such i exist, recall that since S is assumed to be in planar force
closure, solutions to (12.22)-(12.23) must exist for fext,S = ext,N = 0; these
solutions are precisely the internal forces i ). Note that these two equations
constitute three linear equality constraints involving six variables, so that there
exists a three-dimensional linear subspace of solutions for {1 , 2 , 3 }.
Now if {f1S , f2S , f3S } satisfy (12.22)-(12.23), then so will {f1S + 1 , f2S +
2 , f3S + 3 }. The internal forces {1 , 2 , 3 } can in turn be chosen to have
sufficiently large magnitude so that the contact forces
f1
= f1N
+ f1S + 1
f2
f3
f2N
f3N
(12.28)
+ f2S + 2
(12.29)
+ f3S + 3
(12.30)
all lie inside their respective friction cone. This completes the proof of the
sufficiency condition.
12.2.3.2
Measuring the Quality of a Force-Closure Grasp
Friction forces are not always repeatable. Try putting a coin on a book and
tilting the book. The coin should begin to slide when the book is at an angle
= tan1 with respect to horizontal. If you do the experiment several times,
you may find a range of measured values of , however, due to effects that
are difficult to model. For that reason, when choosing between grasps, it is
reasonable to choose finger locations that minimize the friction coefficient needed
to achieve force closure.
12.2.4
Duality of Force and Motion Freedoms
Our discussion of kinematic constraints and friction should make apparent that,
for any point contact and contact label, the number of equality constraints on
the parts motion caused by that contact is equal to the number of wrench
freedoms it provides. For example, a breaking contact B provides zero equality
constraints on the part motion and also allows no contact force. A fixed contact
R provides 3 motion constraints (the motion of a point on the part is specified)
and 3 freedoms on the contact force: any wrench in the interior of the contact
wrench cone is consistent with the contact mode. Finally, a slipping contact
S provides 1 equality motion constraint (one equation on the parts motion
must be satisfied to maintain the contact), and for a given motion satisfying the
constraint, the contact wrench has only 1 freedom, the magnitude of the contact
wrench on the edge of the friction cone and opposite the slipping direction. In
the planar case, the motion constraints and wrench freedoms for B, S, and R
contacts are 0, 1, and 2, respectively.
12.3. Manipulation
12.3
365
Manipulation
So far we have studied the feasible twists and contact forces due to a set of
contacts. We have also considered two types of manipulation: form- and forceclosure grasping.
Manipulation consists of much more than just grasping, however. It includes
almost anything where manipulators impose motions or forces with the purpose
of achieving motion or restraint of objects. Examples include carrying glasses
on a tray without toppling them, pivoting a refrigerator about a foot, pushing a
sofa on the floor, throwing and catching a ball, transporting parts on a vibratory
conveyor, etc. Endowing a robot with methods of manipulation beyond graspand-carry allows it to manipulate parts that are too large to be grasped and too
heavy to be lifted, or to send objects outside the workspace of the end-effector.
To plan such manipulation tasks, we use the contact kinematic constraints
of Section 12.1, the Coulomb friction law of Section 12.2, and the dynamics of
rigid bodies. Restricting ourselves to a single rigid body and using the notation
of Chapter 8, the parts dynamics are written
X
Fext +
ki Fi = GV [adV ]T P, ki 0, Fi WC i ,
(12.31)
where V is the parts twist, P is its momentum, Fext is the external wrench
acting on the part due to gravity, etc., WCP
i is the set of possible wrenches
acting on the object due to contact i, and
ki Fi is the wrench due to the
contacts. Now, given a set of motion- or force-controlled contacts acting on the
part, and the initial state of the system, the general method for solving for the
motion of the part is the following:
(i) Enumerate the set of possible contact modes consisting of the contact
labels R, S, and B at each contact.
P
(ii) For each contact mode, determine if there exists a contact wrench
ki Fi
that is consistent with the contact mode and Coulombs law, and an acceleration V consistent with the kinematic constraints of the contact mode,
such that Equation (12.31) is satisfied. If so, this contact mode, contact
wrench, and part acceleration is a consistent solution to the rigid-body
dynamics.
For a given contact mode, the set of kinematically consistent accelerations is
Therefore, for planar problems
described by a set of linear constraints on V.
and spatial problems with approximate polyhedral friction cones, step (ii) above
is a linear constraint satisfaction problem (LCSP).
This kind of case analysis may sound unusual; we are not simply solving
a set of equations. It also seems to leave open the possibility that we could find
more than one consistent solution, or perhaps no consistent solution. This is,
in fact, the case: we can define problems with multiple solutions (ambiguous)
and problems with no solutions (inconsistent). This state of affairs is a bit
unsettling; surely there is exactly one solution to any real mechanics problem!
366
But this is the price we pay to use the assumptions of perfectly rigid bodies and
Coulomb friction. For many problems the method described above will yield a
unique motion.
A special case of the dynamics (12.31) are quasistatic problems, where the
velocities and accelerations of the parts are small so that inertial forces may be
ignored. Contact wrenches and external wrenches are always in force balance,
and Equation (12.31) reduces to
X
Fext +
ki Fi = 0, ki 0, Fi WC i .
(12.32)
The same case-enumeration method is used to solve such problems, except now
The parts
we often solve for the parts twist V instead of its acceleration V.
twist is relevant for determining whether individual contacts are sliding or not,
which places constraints on the contact forces that can be applied.
Below we illustrate the methods of this chapter with several examples.
Example 12.6. A block carried by two fingers.
Consider a planar block in gravity supported by two fingers, as in Figure 12.24(a).
The friction coefficient between one finger and the block is = 1, and the other
contact is frictionless. Thus the cone of wrenches that can be applied by the
fingers is pos({F1 , F2 , F3 }), as shown using moment labeling in Figure 12.24(a).
Our first question is whether the stationary fingers can keep the block at
rest. To do so, the fingers must provide a wrench F = (mz , fx , fy ) = (0, 0, mg)
to balance Fext = (0, 0, mg) due to gravity. As shown in Figure 12.24(b),
however, this wrench is not in the composite cone of possible contact wrenches.
Therefore the contact mode RR is not feasible, and the block will move relative
to the fingers.
Now consider the case where the fingers each accelerate to the left at 2g. In
this case, the contact mode RR requires that the block also accelerate to the left
at 2g. The wrench needed to cause this acceleration is (0, 2mg, 0). Therefore
the total wrench that the fingers must apply to the block is (0, 2mg, 0)Fext =
(0, 2mg, mg). As shown in Figures 12.24(c) and (d), this wrench lies inside the
composite wrench cone. Thus RR (the block stays stationary relative to the
fingers) is a solution as the fingers accelerate to the left at 2g.
This is called a dynamic graspinertial forces are used to keep the block
pressed against the fingers while the fingers move. If we plan to manipulate the
block using a dynamic grasp, we should make certain that no contact modes
other than RR are feasible, for completeness.
Moment labels are convenient for understanding this problem graphically,
but we can also solve it algebraically. Finger one contacts the block at (x, y) =
(3, 1) and finger 2 contacts the block at (1, 1). This gives the basis contact
12.3. Manipulation
367
y F3
F1
F2
mg
x
mg
(a)
(b)
k 1F1+ k 2F2
_
mg
+
k 3F3
acceleration
force
(c)
(d)
Figure 12.24: (a) A planar block in gravity supported by two robot fingers, one
with a friction cone with = 1 and one with = 0. (b) The composite wrench
cone that can be applied by the fingers represented using moment labels. To
balance the block against gravity, the fingers must apply the line of force shown.
This line does not make nonpositive moment with respect to all points labeled
, and therefore it cannot be generated by the two fingers. (c) For the block to
match the fingers acceleration to the left, the contacts must apply the vector
sum of the wrench to balance gravity plus the wrench needed to accelerate the
block to the left. This total wrench lies inside the composite wrench cone, as the
line of force makes positive moment with respect to the points labeled + and
negative moment with respect to the points labeled . (d) The total wrench
applied by the fingers in Figure (c) can be translated along the line of action
without changing the wrench. This allows us to easily visualize the components
k1 F1 + k2 F2 and k3 F3 provided by the fingers.
wrenches
1
F1 = (4, 1, 1)
2
1
F2 = (2, 1, 1)
2
F3 = (1, 1, 0).
Let the fingers acceleration in the x direction be written ax . Then, under
368
the assumption that the block stays fixed to the fingers (RR contact mode),
Equation (12.31) can be written
k1 F1 + k2 F2 + k3 F3 + (0, 0, mg) = (0, max , 0).
(12.33)
This yields three equations in the three unknowns, k1 , k2 , k3 . Solving, we get

blah blah. For the ki to be nonnegative, we need some range for ax . For
x-direction finger accelerations in this range, a dynamic grasp is a consistent
solution.
Example 12.7. The meter stick trick.
Try this experiment: Get a meter stick (or any similar long smooth stick) and
balance it horizontally on your two index fingers. Place your left finger near
the 10 cm mark and your right finger near the 60 cm mark. The center of mass
is closer to your right finger, but still between your fingers, so that the stick
is supported. Now, keeping your left finger stationary, slowly move your right
finger towards your left until they touch. What happens to the stick?
If you didnt try the experiment, you might guess that your right finger
passes the center of mass of the stick, at which point the stick falls. If you did
try the experiment, you saw something different. Lets see why.
Figure 12.25 shows the stick supported by two frictional fingers. Since all
motions are slow, we use the quasistatic approximation that the sticks acceleration is zero, and the net contact wrench must balance the gravitational wrench.
As the two fingers move together, the stick must slip on one or both fingers to
accommodate the motion. Figure 12.25 shows the moment-labeling representation of the composite wrench cone for three different contact modes where the
stick remains stationary on the left finger (R) or slips left relative to it (Sl) while
either remaining stationary on the right finger (R) or slipping right relative to
it (Sr). It is clear from the figure that only the SlR contact mode can provide a
wrench that balances the gravitational wrench. In other words, the right finger,
which supports more of the sticks weight, remains sticking, while the left finger
slides over the stick. Since the right finger is moving to the left in the world
frame, this means the center of mass is moving to the left at the same speed.
This continues until the center of mass is halfway between the fingers, at which
point the stick transitions to the SlSr contact mode, and the center of mass
stays centered between the fingers until they meet. The stick never falls.
Note that this analysis relies on the quasistatic assumption; it is easy to make
the stick fall if you move your right finger quickly, requiring large accelerations
of the stick to maintain the sticking contact. Also, in your experiment, you
might notice that when the center of mass is nearly centered, the stick does not
actually achieve the idealized SlSr contact mode, but instead switches rapidly
between the SlR and RSr contact modes. If there is a large difference between
the static and kinetic friction coefficients, the time between switches is increased.
Example 12.8. Stability of an assembly.
Consider the arch in Figure 12.26. Is it stable under gravity?
For a problem like this, graphical planar methods are difficult to use. Instead we test algebraically for consistency of the contact mode with all contacts
12.3. Manipulation
369
mg
+
R
Sr
mg
RSr
mg
_
+
+
Sl
R
mg
SlR
Sl
SlSr
Sr
Figure 12.25: Top left: Two frictional fingers supporting a meter stick in gravity.
The other three panels show the moment labels for the RSr, SlR, and SlSr
contact modes. Only the SlR contact mode yields force balance.
F8
2
F1
F9
F7
F6
F2 F3
F5
F4
F12
F13
F10
F11
F14 F15 F16
Figure 12.26: Left: An arch in gravity. Right: The friction cones at the contacts
of the stones 1 and 2.
labeled R. The friction cones are shown in Figure 12.26. With these labelings
of the friction cone edges, the arch remaining standing is a consistent solution
if there exist ki 0 for i = 1 . . . 16 satisfying the following nine wrench-balance
370
F1
F2
_
Figure 12.27: Left: A peg in two-point contact with a hole. Right: The wrench
F1 may cause the peg to jam, while the wrench F2 continues to push the peg
into the hole.
equations, three for each body:
8
X
ki Fi + Fext1 = 0
i=1
16
X
ki Fi + Fext2 = 0
i=9
12
X
ki Fi + Fext3 = 0.
i=5
The last set of equations comes from the fact that forces that body 1 applies
to body 3 are equal and opposite those that body 3 applies to body 1, and
similarly for bodies 2 and 3.
Example 12.9. Peg insertion.
Figure 12.27 shows a force-controlled planar peg in two-point contact with a
hole during insertion. Also shown are the contact friction cones acting on the
peg and the corresponding composite wrench cone, illustrated using moment
labels. If the force controller applies the wrench F1 to the peg, it may jam
the hole may generate contact forces that balance F1 . Therefore the peg may
get stuck in this position. If the force controller applies the wrench F2 , however,
the contacts cannot balance the wrench and insertion proceeds.
If the friction coefficients at the two contacts are large enough that the two
friction cones see each others base, the peg is in force closure, and the contacts
may be able to resist any wrench. The peg is said to be wedged.
12.4. Exercises
371
Figure 12.28: A 44 planar square restrained by five frictionless point contacts.
12.4
Exercises
1. Prove that the impenetrability constraint (12.4) is equivalent to the constraint (12.7).
2. note that any contact with the same normal provides the same interpenetrability constraints, no matter where it acts along the line of action
3. write SFC and PCwF contact velocity constraints in a coordinate frame at
the contact, with z-axis aligned with normal.
4. follow up on the examples at the end of the chapter. show that other contact
modes are not possible for the meter stick trick.
5. A finger toppling a block.
6. Vibratory transport.
7. pushing? 3 support points, CoRs of pushing and relative motion
8. holding a rod by over-under contacts. table supported by four legs. parallel jaw gripper. ambiguity and inconsistency example. waiters problem. 3d
example. LCSP solution. limit surface. two leg planar table.
372
Figure 12.29: A planar disk restrained by three frictionless point contacts.
9. (a) The planar grasp of Figure 12.28 consists of five frictionless point contacts. The square is of size 4 4. Show that this grasp is not force closure.
(b) The grasp of part (a) can be made force closure by adding one frictionless
point contact. Draw all the possible locations for this contact.
10. Assume all contacts shown in Figure 12.29 are frictionless point contacts.
Determine whether the grasp is force closure. If it is not, how many additional
frictionless point contacts are needed to construct a force closure grasp?
(a)
(b)
Figure 12.30: A planar rigid body with two square holes.
11. (a) In Figure 12.30-(a), the planar object with two rectangular holes is restrained from the interior by four frictionless point contacts. Determine whether
this grasp is force closure.
12.4. Exercises
373
(b) In Figure 12.30-(b), the same planar object with two rectangular holes is
now restrained from the interior by three frictionless point contacts. Determine
whether this grasp is force closure.
Figure 12.31: A planar wedge-like object with a hole.
12. The planar object of Figure 12.31 is restrained by four frictionless point
contacts.
(a) Determine if this grasp is force closure.
(b) Suppose the contacts A, B, C, D are now allowed to slide along the halfcircle (without crossing each other). Describe the set of all possible force closure
grasps.
13. (a) Determine whether the grasp of Figure 12.32-(a) is force closure. Assume all contacts are frictionless point contacts. If the grasp is not force closure,
slide the position of one of the contacts in order to construct a force closure
grasp.
(b) Now place two frictionless point contacts at the corners as shown in Figure 12.32-(b). Determine if this grasp is force closure.
(c) In the grasp of Figure 12.32-(c), contact A is a point contact with friction
(its friction cone is 90 as indicated in the figure), while contacts B and C are
frictionless point contacts. Determine whether this grasp is force closure.
14. Determine whether the grasp of Figure 12.33 is force closure. Assume all
contacts are point contacts with a friction coefficient = 1.
15. (a) In the planar triangle of Figure 12.34-(a), contacts A, B, and C are
374
(a)
(b)
(c)
Figure 12.32: A planar rigid body restrained by point contacts.

all frictionless point contacts. is this grasp force closure? If not, what type of
external force or moment would cause the object to slip out of the grasp?
(b) In the planar triangle of Figure 12.34-(a), suppose now that contact A is
a point contact with friction coefficient = 1, while contacts B and C are
frictionless point contacts. Determine whether the grasp shown is force closure.
(c) Now suppose contact point A can be moved to anywhere on the hypotenuse
of the triangle as shown in Figure 12.34-(b). Determine the range of all contact
points A for which the grasp is force closure.
(d) Contact points B and C are now moved as shown in Figure 12.35. Bill,
a clever student, argues that the two contacts B and C can be replaced by a
virtual point contact with friction (point D) with the given friction cone, and
Nguyens condition for force closure can now be applied to A and D, as shown
in the right Figure 12.35. Is Bill correct? Justify your answer with a derivation
of the appropriate force closure condition.
12.4. Exercises
375
Figure 12.33: A planar rigid body restrained by three point contacts with friction.
(a)
(b)
Figure 12.34: A planar triangle constrained by three point contacts.
16. Consider the L-shaped planar object of Figure 12.36.

(a) Suppose both contacts are point contacts with friction coefficient = 1.
Determine whether this grasp is force closure.
(b) Now suppose point contact 1 is a point contact with friction coefficient
= 1, while point contact 2 is frictionless. Determine whether this grasp is
force closure.
(c) Suppose now that the vertical position of contact 1 is allowed to vary; denote
its height by x. Find all positions x such that the grasp of part (b) is force
closure.
376
Figure 12.35: A planar triangle constrained by three point contacts.
Figure 12.36: An L-shaped planar object restrained by two point contacts with
friction.
17. A square is restrained by three point contacts as shown in Figure 12.37:

contact f1 is a point contact with friction coefficient , while contacts f2 and
f3 are frictionless point contacts. If c = 14 and h = 12 , find the range of values
of such that grasp is force closure.
18. In the grasp of Figure 12.38, contacts f1 and f2 on the left are frictionless
point contacts, while contact f3 on the right is a point contact with friction
coefficient = 0.2. Determine whether this grasp is force closure.
19. A single point contact with friction coefficient = 1 is applied to the left
side of the square doughnut as shown in Figure 12.39. A force closure grasp can
be constructed by adding another point contact with friction, also with = 1.
Draw all possible locations for this additional point contact.
12.4. Exercises
Figure 12.37: A square restrained by three point contacts.
Figure 12.38: A rectangle restrained by three point contacts.
377
378
Figure 12.39: A square doughnut.
Bibliography
[1] J. Angeles, Fundamentals of Robotic Mechanical Systems: Theory, Methods, and Algorithms. Springer, 2006.
[2] R. S. Ball, A Treatise on the Theory of Screws. Cambridge University Press,
1998.
[3] O. Bottema and B. Roth, Theoretical Kinematics. Dover Publications,
1990.
[4] R. W. Brockett, Robotic manipulators and the product of exponentials
formula, Proc. Int. Symp. Math. Theory of Networks and Systems, Beer
Sheba, Israel, 1983.
[5] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard,
L. E. Kavraki, S. Thrun, Principles of Robot Motion: Theory, Algorithms,
and Implementations. Cambridge, MA: MIT Press, 2005.
[6] J. Craig, Introduction to Robotics: Mechanics and Control. Upper Saddle
River, New Jersey: Prentice-Hall, 2004.
[7] J. Denavit and R.S. Hartenberg, A kinematic notation for lower-pair mechanisms based on matrices, Trans. ASME J. Appl. Mech, vol. 23, pp. 215221, 1955.
[8] M. Do Carmo, Differential Geometry of Curves and Surfaces. Upper Saddle
River, New Jersey: Prentice-Hall, 1976.
[9] A. G. Erdman and G. N. Sandor, Advanced Mechanism Design: Analysis
and Synthesis Vol. I and II. Upper Saddle River, New Jersey: Prentice-Hall,
1996.
[10] R. Featherstone, Rigid Body Dynamics Algorithms. Upper Saddle River,
New Jersey: Springer, 2008.
[11] D. T. Greenwood, Advanced Dynamics. Cambridge: Cambridge University
Press, 2003.
379
380
BIBLIOGRAPHY
[12] J. Hong, G. Lafferiere, B. Mishra, and X. Tan, Fine manipulation with

multifinger hands, Proc. IEEE Int. Conf. Robotics Autom., pp. 1568-1573,
1990.
[13] O. Khatib, A unified approach to motion and force control of robot manipulators: the operational space formulation, IEEE Int. J. Robotics Autom.,
vol. 3, no. 1, pp. 43-53, 1987.
[14] S. M. LaValle, Planning Algorithms. Cambridge: Cambridge University
Press, 2006.
[15] H.Y. Lee and C.G. Liang, A new vector theory for the analysis of spatial
mechanisms, Mech. Machine Theory, vol. 23, no. 3, pp. 209-217, 1988.
[16] J.-W. Li, H. Liu, and H.-G. Cai, On computing three-finger force-closure
graps of 2-D and 3-D objects, IEEE Trans. Robotics Autom., vol. 19, no. 1,
2003, pp. 155-161.
[17] K. M. Lynch and M. T. Mason, Pulling by pushing, slip with infinite
friction, and perfectly rough surfaces, Int. J. Robotics Res., vol. 14, no. 2,
pp. 174-183, 1995.
[18] D. Manocha and J. Canny, Real time inverse kinematics for general manipulators, Proc. IEEE Int. Conf. Robotics Automation, vol. 1, pp. 383-389,
1989.
[19] X. Markenscoff, L. Ni, and C. H. Papadimitriou, The geometry of grasping, Int. J. Robotics Res., vol. 9, no. 1, pp. 61-74, 1990.
[20] J. M. McCarthy, Introduction to Theoretical Kinematics. Cambridge: MIT
Press, 1990.
[21] J. P. Merlet, Parallel Robots. Berlin: Springer, 2006.
[22] R. S. Millman and G. D. Parker, Elements of Differential Geometry. Upper
Saddle River, New Jersey: Prentice-Hall, 1977.
[23] B. Mishra, J. T. Schwartz, and M. Sharir, On the existence and synthesis
of multifinger positive grips, Algorithmica, vol. 2, no. 4, pp. 541-558, 1987.
[24] M. T. Mason, Mechanics of Robot Manipulation. Cambridge: MIT Press,
2001.
[25] R. Murray, Z. Li, and S. Sastry, A Mathematical Introduction to Robotic
Manipulation. Boca Raton, Florida: CRC Press, 1994.
[26] V.-D. Nguyen, Constructing force-closure grasps, Int. J. Robotics Research, vol. 7, no. 3, pp. 3-16, 1988.
[27] F.C. Park, Computational aspects of the product of exponentials formula
for robot kinematics, IEEE Trans. Automatic Control, Vol. 39, No. 3,
pp. 643-647, 1994.
BIBLIOGRAPHY
381
[28] F.C. Park, J.E. Bobrow, and S.R. Ploen, A Lie group formulation of
robot dynamics, International Journal of Robotics Research, vol. 14, no. 6,
pp. 609-618, 1995.
[29] F. C. Park, Jihyeon Choi, and S. R. Ploen, Symbolic formulation of closed
chain dynamics in independent coordinates, Journal of Mechanism and
Machine Theory, vol. 34, no. 5, pp. 731-751, July 1999.
[30] F. C. Park and Jinwook Kim, Manipulability of closed kinematic chains,
ASME J. Mechanical Design, vol. 120, no. 4, pp. 542-548, 1998.
[31] F. C. Park and Jinwook Kim, Singularity analysis of closed kinematic
chains, ASME J. Mechanical Design, vol. 121, no. 1, pp. 32-38, 1999.
[32] F. C. Park and I. G. Kang, Cubic spline algorithms for orientation interpolation, Int. .J. Numerical Methods in Engineering, vol. 46, pp. 45-64,
1999.
[33] M. Raghavan and B. Roth, Kinematic analysis of the 6R manipulator of
general geometry, 5th Int. Symp. Robotics Research, 1990.
[34] E. Rimon and J.W. Burdick, Mobility of bodies in contact. I. A secondorder mobility index for multiple-finger grasps,a IEEE Trans. Robotics
Autom., vol. 14, no. 5, pp. 696-708, 1998.
[35] T. Shamir and Y. Yomdin, Repeatability of redundant manipulators:
mathematical solution to the problem, IEEE Trans. Automatic Control,
vol. 33, no. 11, pp. 1004-1009.
[36] B. Siciliano and O. Khatib, Eds., Springer Handbook of Robotics. BerlinHeidelberg: Springer, 2008.
[37] E.T. Whittaker, A Treatise on the Analytical Dynamics of Particles and
Rigid Bodies. Cambridge, Cambridge University Press, 1917.
382
BIBLIOGRAPHY

Giao Trinh Robot Park-Lynch

Uploaded by

Copyright:

Available Formats

Giao Trinh Robot Park-Lynch

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Giao Trinh Robot Park-Lynch

Uploaded by

Copyright:

Available Formats

What are some of the main topics covered in the book?

What are some of the main topics covered in the book?

How are robot configurations represented and analyzed?

How are robot configurations represented and analyzed?

INTRODUCTION TO ROBOTICS

MECHANICS, PLANNING, AND CONTROL

F. C. Park and K. M. Lynch

5 Velocity Kinematics and Statics

7 Kinematics of Closed Chains

Park and Lynch

16:18 December 10, 2014

8 Dynamics of Open Chains

7.2.2 General Parallel Mechanisms

Park and Lynch

16:18 December 10, 2014

12 Grasping and Manipulation

Park and Lynch

16:18 December 10, 2014

Park and Lynch

16:18 December 10, 2014

Park and Lynch

16:18 December 10, 2014

Chapter 2: Configuration Space

(a) An open chain industrial manipulator.

(b) Stewart-Gough platform.

Figure 1.1: Open chain and closed chain robot mechanisms.

Chapter 3: Rigid-Body Motions

to a certain linear vector differential equation. Doing so allows us to, among

Chapter 4: Forward Kinematics

Chapter 5: Velocity Kinematics and Statics

Chapter 6: Inverse Kinematics

Chapter 7: Kinematics of Closed Chains

Chapter 8: Dynamics of Open Chains

Chapter 9: Trajectory Generation

Chapter 10: Motion Planning

Chapter 11: Robot Control

Chapter 12: Grasping and Manipulation

Chapter 13: Wheeled Mobile Robots

the sense of where are the links of the robot situated?

Configuration Space of a Rigid Body

2.1. Configuration Space of a Rigid Body

Degrees of freedom = (Sum of freedoms of the points)

Configuration Space of a Robot

2.2. Configuration Space of a Robot

Degrees of Freedom of a Robot

Figure 2.3: Typical robot joints.

Figure 2.4: Coordinates for a planar rigid body.

2.2. Configuration Space of a Robot

fact, (xB , yB ) can be determined to be

Figure 2.5: (a) Four-bar linkage (b) slider-crank mechanism.

formula for planar robots:

Example 2.1. Four-bar linkage and slider-crank mechanism

2.2. Configuration Space of a Robot

Figure 2.7: A planar mechanism with two overlapping joints.

2.2. Configuration Space of a Robot

Figure 2.9: The Stewart-Gough Platform.

2.2. Configuration Space of a Robot

Figure 2.10: The 3 U P U platform.