Foundations of Robotics

Foundations of Robotics
Analysis and Control

Tsuneo Yoshikawa

The MIT Press

Cambridge, Massachusetts
London, England
© 1990 Massachusetts Institute of Technology. Based on Foundations of Robot Control,
published in Japan by Corona Publishing Co. Ltd. (1988).
All rights reserved. No part of this book may be reproduced in any form by any electronic
or mechanical means (i ncluding photocopying, recording, or in formation storage and
retrieval) without permission in writing from the publisher.

This book was set in Times Roman by Asco Trade Typesetting Ltd. in Hong Kong and was
printed and bound in the United States of America.

Library of Congress Cataloging-in-Publication Data

Yoshikawa, Tsuneo, 1941-

[Robotto seigyo kisoron. English]
Foundations of robotics: analysis and controlrrsuneo Yoshikawa.
p. cm.
Revised translation of: Robotto seigyo kisoron.
Includes bibliographical references.
ISBN 0-262-24028-9
1. Robotics. I. Title.
TJ211.Y6713 1990
629.8'92-dc20 89-29363
CIP
Contents

Preface IX

1 Overview of Robotic Mechanisms and Controller 1

1.1 Mechanisms

1.2 Controller 9
Exercises 11

References 12

2 Kinematics 13

2.1 Position and Orientation of Objects 13

2.1.1 Object Coordinate Frame 13
2.1.2 Rotation Matrix 14
21 3
. . Euler Angles 18
2.1.4 Roll, Pitch, and Yaw Angles 22

2.2 Coordinate Transformation 23

2.2 1. Homogeneous Transform 23
2.2.2 Product and Inverse of Homogeneous Transform 26

2.3 Joint Variables and Position of End Effector 28

2.3.1 General Relation 28
2.3.2 Link Parameters 31
2.3.3 Link Frames 33
2.3.4 Solution to Direct Kinematics Problem 39
2.4 Inverse Kinematics Problem 45
2.5 Jacobian Matrix 53
2.5.1 Translational and Rotational Velocity of Objects 53
2.5.2 Definition of the Jacobian Matrix 57
2.5.3 Link Velocities of a Manipulator 60
2.5.4 General Expression of the Jacobian Matrix Jv 63
2.5.5 Joint Velocity for Achieving Desired End-Effector
Velocity 66
2.5.6 Singular Configurations 67

2.6 Statics and Jacobian Matrices 71

2.6.1 Equivalent Forces Represented in Different Frames 71
2.6.2 Joint Driving Force. Equivalent to Force Applied to
Tip of Manipulator 74

Exercises 76
References 80
vi Contents

3 Dynamics 81

3.1 Lagrangian and Newton-Euler Formulations 81

3.2 Some Basics of Kinematics 82

3.2.1 Newton's Equation and Euler's Equation 82
3.2.2 Lagrange's Equation 86

3.3 Derivation of Dynamics Equations Based on Lagrangian

Formulation 88
3.3.1 Two-Link Manipulator 88
3.3.2 n-Link Manipulator 91
3.3.3 Parallel-Drive Two-Link Manipulator 98

3.4 Derivation of Dynamic Equations Based on Newton-Euler

Formulation 1 00
3.4.1 Basic Procedure of Newton-Euler Formulation 100
3.4.2 Link Accelerations of a Manipulator 102
3.4.3 n-Link Manipulator 103

3.5 Use of Dynamics Equations and Computational Load 108

3.5.1 Real-Time Control-Inverse Dynamics Problem 108
3.5.2 Simulation-Direct Dynamics Problem 110
3.6 Identification of Manipulator Dynamics 112
3.6.1 Identification Problem of Manipulators 112
3.6.2 Identification Scheme Based on Lagrangian
Formulation 113
3.6.3 Identification of Load 122

Exercises 123
References 1 25

4 Manipulability 127

4.1 Manipulability Ellipsoid and Manipulability Measure 127

4.2 Best Configurations of Robotic Mechanisms from
Manipulability Viewpoint 133
4.2.1 Two-Link Mechanism 133
4.2.2 SCARA-Type Robot Manipulator 135
4.2.3 PUMA-Type Robot Manipulator 137
4.2.4 Orthogonal-, Cylindrical-, and Polar-Coordinate
Manipulators 139
4.2.5 Four-Joint Robotic Finger 139
4.3 Various Indices of Manipulability 142
Contents vii

4.4 Dynamic Manipulability 143

4.4.1 Dynamic-Manipulability Ellipsoid and Dynamic-
Manipulability Measure 143
4.4.2 Two-Link Mechanism 148

Exercises 153

References 153

5 Position Control 155

5.1 Generating a Desired Trajectory 155

5.1.1 Joint-Variable Scheme 155
5.1.2 Scheme for Position Variables of End Effector 161
5.2 Linear Feedback Control 166
5.2.1 Effectiveness of Linear Feedback Control 166
5.2.2 Stability of Proportional and Differential Feedback
Control 168
5.3 Two-Stage Control by Linearization and Servo
Compensation 170
5.3.1 Basic Concept of Two-Stage Control 170
5.3.2 Structure of Control System 175
5.3.3 Parallel Processing Scheme 176

5.4 Design and Evaluation of Servo Compensation 182

5.4.1 Linear Servosystem Theory 182
5.4.2 Stability Margin and Sensitivity 187
5.5 Decoupling Control 192
5.5.1 Theory of Decoupling Control for Nonlinear
Systems 192
5.5.2 Application to Manipulators 196
5.5.3 Consideration of Actuator Dynamics 197
5.6 Adaptive Control 199
Exercises 207
References 209

6 Force Control 21 1

6.1 Impedance Control 211

6.1.1 Passive-Impedance Method 211
6.1.2 Active-Impedance Method-One- Degree-of-
Freedom Case 214
6.1.3 Active-Impedance Method-General Case 218
Vlll Contents

6.2 Hybrid Control 222

6.2.1 Hybrid Control via Feedback Compensation 222
6.2.2 Dynamic Hybrid Control 227

Exercises 240

References 242

7 Control of Redundant Manipulators 244

7.1 Redundant Manipulators 244

7.2 Task-Decomposition Approach 245

7.2. 1 Decomposing a Task into Subtasks with Priority Order 245
7.2.2 Basic Equations 245
7.2.3 Second Subtask Given by Desired Trajectory 247
7.2.4 Second Subtask Given by Criterion Function 248
7.2.5 Formulation as Instantaneous Optimization
Problem 249
7.3 Application to Avoiding Obstacles and Singularities 249
7.3.1 Avoiding Obstacles 249
7.3.2 Avoiding Singularities 253
7.4 Computational Method for Desired Joint Velocity 256
Exercises 257
References 258

Appendix 1 Function atan2 259

Appendix 2 Pseudo-Inverses 263

Appendix 3 Singular-Value Decomposition 268

Appendix 4 Lyapunov Stability Theory 272

Solutions to Selected Exercises 277

Index 283
Preface

Robots are now widely used in factories, and applications of robots in space,
the oceans, nuclear industries, and other fields are being actively developed.
Also, for the future, the use of robots in every facet of society, including the
home, is being seriously considered. To support the development of these
broad applications, robotics has evolved into a systematic approach to the
engineering of robots.
This book has been written with the objective of presenting some fun­
damental concepts and methodologies for the analysis, design, and control
of robot manipulators in an easily understandable way. I believe that this
knowledge is useful not only in robotics but also in the analysis and control
of other types of mechanical systems.
The book is based on class notes used at Kyoto University. It is intended
as a text or reference book on robotics, mainly for graduate students in
engineering. Junior and senior undergraduate students, however, should
also be able to understand the material without much difficulty. The
physical meanings of the concepts and equations used in the book are
explained as fully as possible, and the required background in kinetics,
linear algebra, and control theory is presented in an intuitively clear way
to spare the reader from having to refer to texts in those other fields.
The book is organized as follows. After an overview in chapter 1, chapters
2 through 4 cover the analysis of robot manipulator mechanisms. Based
on this analysis, chapters 5 through 7 discuss the control of robot mani­
pulators.
Chapter 1 introduces several typical robot manipulator mechanisms and
their controllers, in order to acquaint the reader with the kind of hardware
that will be dealt with. Chapter 2 covers the kinematics of robot manipula­
tors, studying geometrically the motion of manipulator links and objects
related to the manipulation task in terms of position, velocity, and acce­
leration. Chapter 3 deals with the dynamics of robot manipulators, looking
at how the manipulator'S motion is affected by its mass distribution and
applied forces. This chapter includes the derivation of the dynamic equa­
tions of motion, their use for control and simulation, and the identification
of inertial parameters. Chapter 4 develops the concept of manipulability
to analyze and evaluate quantitatively the manipulation ability of mani­
pulators. This concept is examined first from the viewpoint of kinematics
and then from the viewpoint of dynamics.
Chapter 5 covers various position-control algorithms that make the end
effector of a manipulator follow a desired position trajectory. Chapter 6
x Preface

discusses two typical force-control methods. These make the contact force
between the end effector and its environment follow a desired force trajec­
tory. Chapter 7 discusses, for manipulators with redundant degrees of
freedom, a way to develop control algorithms for active utilization of the
redundancy. The appendixes give compact reviews of the function atan2,
pseudo-inverses, singular-value decomposition, and Lyapunov stability
theory.
When writing a textbook in a changing field like robotics in which many
new achievements are being unveiled day by day, one usually has a difficult
time deciding what to include. I have attempted to include as many impor­
tant results as possible. At the same time, I have tried to present them
plainly with the help of many illustrative examples.
It is my pleasure to thank several people who have contributed in various
ways to the completion of this book. The content and the form of this book
are much influenced by Richard P. Paul's pioneering book Robot Mani­
pulators (MIT Press, 1981), which I translated into Japanese in 1984. I wish
to thank Yoshihiko Nakamura and Toshiharu Sugie, with whom I have
worked on research projects that some parts of this book are based on.
Yasuyoshi Yokokohji read the manuscript and provided many useful
comments. Discussions with Junichi Imura and Kiyoshi Maekawa proved
valuable in the writing of the section on adaptive control (section 5.6). Mike
Lipsett, who happened to be with me at Kyoto University at a critical stage
in the writing of the English version, contributed many improvements.
Takashi Hosoda, Jun Koreishi, Junichi Imura, and Osamu Suzuki helped
me with revisions and corrections. Masako Awakura performed the magic
of turning all my vague scribbling into a finely typed manuscript.
This book is mostly a translation of my Japanese book Robotto Seigyo
Kisoron (Foundations of Robot Control), published in Tokyo by Corona
Publishing Co. Ltd. in 1988. I am grateful to Corona for its cooperation
and support in publishing this English version. I also wish to thank Frank
P. Satlow and Teresa Ehling of The MIT Press for patiently awaiting my
manuscript. My wife, Sanae, supported and encouraged me at home during
the writing of this book.
Foundations of Robotics
1 Overview of Robotic Mechanisms and Controller

A robot system generally consists of three subsystems: a motion subsystem,

a recognition subsystem, and a control subsystem (figure 1.1). The motion
subsystem is the physical structure that carries out desired motions, corre­
sponding to human arms or legs. The recognition subsystem uses various
sensors to gather information about any objects being acted upon, about
the robot itself, and about the environment; it recognizes the robot's state,
the objects, and the environment from the gathered information. The
control subsystem influences the motion subsystem to achieve a given task
using the information from the recognition subsystem.
This book covers mainly the theoretical fundamentals of mechanism
analysis and control of robot manipulators which are necessary for design­
ing their mechanisms and their controller. For the motion subsystem
described above, manipulator mechanisms that function like human arms
will be the subject of this book. Their kinematics, dynamics, and perfor­
mance evaluation are discussed. For the control subsystem, position con­
trol of an end effector, control of the force applied by the end effector on
an object, and the control of redundant manipulators are discussed. The
recognition subsystem plays a key role in using robots outside of factories
and in making robots intelligent. However, this subsystem will not be
examined closely, since in this book a recognition subsystem need only
provide information about position, velocity, and force.
This chapter discusses several typical mechanisms and controllers of
robot manipulators in order to acquaint the reader with the hardware that
will be dealt with in the following chapters.

1.1 Mechanisms

Most robot manipulators can be regarded as open-loop link mechanisms

consisting of several links connected together by joints. Typical joints are
revolute joints and prismatic joints, which are represented by the symbols
show� in figure 1.2. Joint c in this figure is sometimes called the pivot joint
to distinguish it from joint b. The endpoint of the mechanism is moved by
driving these joints with appropriate actuators. A manipulator can usually
be divided into an arm portion, a wrist portion, and a hand portion. Several
typical mechanisms of the arm and wrist portions will be shown. The hand
is not addressed here, since its mechanism depends on the task to be
performed.
2 Chapter 1

Recognition subsystem
r--------------,
I
I I
Motion subsystem I
I :
I
I
: 0 I
I I
I
I
_J

Figure 1.1
Robot system.

T (a) (b)

---Gt- (el) (c2)

(c)

Figure 1.2
Symbols of joints (arrows show direction of motion). (a) Prismatic joint. (b) Revolute joint
1. (c) Revolute joint 2. (cl) Up-and-down rotation. (c2) Back-and-forth rotation.

Fig ure 1.3 shows several types of arm melO:hanisms: (a) the orthogonal­
coordinate type, (b) the cylindrical-coordinate type, (c) the polar-coordinate
type, (d) the vertical multi-joint type, and (e) the horizontal multi-joint type.
Type a is structurally simple and rigid, and so its positioning accuracy is
high. Types b-e are inferior to type a in positioning accuracy; however,
they need less floor area for a base, and they have broader reach. Every
mechanism in figure 1.3 has three degrees of freedom, which is the minimum
number of degrees of freedom needed for placing the endpoint of the arm
at an arbitrary point in three-dimensional space. Here the degree of freedom
is defined as the minimal number of position variables necessary for com­
pletely specifying the configuration of a mechanism.
The wrist is connected to the end of the arm portion. The main role of
the wrist is to change the orientation of the hand. Examples of wrist
mechanisms are shown in figure 1.4. Type a is similar to the human wrist;
Overview 3

� I

(c)
(a) (b)

(d) (e)

Figure 1.3
Arm mechanisms. (a) Orthogonal-coordinate type. (b) Cylindrical-coordinate type. (c)
Polar-coordinate type. (d) Vertical multijoint type. (e) Horizontal multijoint type.

To ann

End effector
(a) (h) (c)

Figure 1.4
Wrist mechanisms.
4 Chapter 1

Maximum attainable region

J1

J3 J3

(a) (b)

Figure I.S
Three Roll Wrist. (a) Drive mechanism. (b) Profile.

type b is often used in industrial manipulators. Type c is equivalent to the

Three Roll Wrist.1 This can also be regarded as a variation of type b in
which the axis of the middle joint is tilted a little. The Three Roll Wrist
elegantly uses bevel gears as shown in figure 1.5a to make the attainable
region of orientations as large as in figure 1.5b. These are all three-degrees­
of-freedom mechanisms; thus, they have enough degrees of freedom to let
the hand portion pose in an arbitrary orientation. However, each of these
wrists has some configurations in which it can no longer change the hand
orientation in a certain direction. Such a configuration is called a singular
configuration. For instance, the configurations shown in figures l.4b and
l.4c are singular ones because it is impossible to rotate the hand from side
to side on the page.
The wrist mechanisms shown in figures 1.6 and 1.7 have been developed
with the purpose of overcoming the degeneracy problem of singular con­
figurations. Figure 1.6 shows the ET (Elephant Trunk) wrist,2 a three­
degrees-of-freedom wrist with a special mechanism that relocates the singu­
lar configurations out of the main working domain. More specifically, as
figure 1.6a shows, it consists of five revolute joints, 11, 12, 13, 13', and 12',
with each element of the pairs {12,12'} and {13,13'} rotating through the
same angles. For instance, joints 12 and 12' rotate as in figure 1.6b. Figure
1.6c shows a hardware implementation of the mechanism using two uni­
versal joints connected by gears. The only singular configurations of the
ET wrist are those in which the approach direction ZH of the hand portion
and the outward direction Zw of the wrist base are in completely opposite
directions (for example, (}2 90° in figure 1.6b). Figure 1.7 shows a four­
=

degrees-of-freedom wrist,3 which can get away from the singular con­
figurations by actively using the mechanical redundancy.
Overview 5

J1

J2
_�_l
Mechanism for rotating through
Mg.
Zw

J3 I
I I
I I I
J 3' __ J I I
I I
I I
J2' I
I
, I
'../

(a) (b)

Connected to
ann portion

Connected to
hand portion

(c)

Figure 1.6
ET wrist. (a) Basic structure. (b) Example of motion. (c) Mechanism.

J1

J2

J3

J4

Figure 1.7
Four-degrees-of-freedom wrist.
6 Chapter 1

Figure 1.8
Combination of arm portion and wrist portion.

Figure 1.9
PUMA 260 (courtesy of Westinghouse Automation Division/Unimation Inc.).

Combining an arm and a wrist mechanism generally gives a m an ipu la to r

with six degrees of freedom, excl ud in g the degree of freedom in the hand .

For example, figu re 1.8 is the combination of fig ures 1.3d and l.4b, and the
design of the robot in figure 1.9 is based on this combination. Figure 1.10
is the combination of figures 1.3d and l.4c, and fig u re 1.11 show s an
example of this combination. Figure 1.12 is the combination of figures l.3c
and l.4b; fi gu re 1.13 shows an example of this kind of robot. Figure 1.14
is the combination of figure s l.3a and l.4b, and figure 1.15 shows an
example of this combination.
Depending on the purpose, there are often cases where six degrees of
freedom are not required, and there are many industrial manipulators on
the market that have only five degree s of freedom or fewer. One represen­
tative example is shown in figure 1.16, which is the combination of figure
Overview 7

Figure 1.10
Combination of arm portion and wrist portion.

--, .
Figure 1.11
T3-786 Robot (courtesy of Cincinnati Milacron, Japan Branch).

Figure 1.12
Combination of arm portion and wrist portion.
8 Chapter 1

Figure 1.13
Kawasaki Unimate (courtesy of Kawasaki Heavy Industries, Ltd.).

Figure 1.14
Combination of arm portion and wrist portion.

Figure 1.15
IBM 7565 Robotic System (1984) (courtesy of IBM Corp.).
Overview 9

Figure 1.16
SCARA-type robot.

1.3e and a wrist portion with just one joint rotating about a vertical axis.
This four-degrees-of-freedom manipulator is called a SCARA (Selective
Compliance Assembly Robot Arm).4 Manipulators with seven degrees of
freedom or more have also been developed.

1.2 Controller

The fundamental elements of tasks performed by robot manipulators are

(1) to move the end effector, with or without a load, along a desired
trajectory and (2) to exert a desired force on an object when the end effector
is in contact with it. The former is called position control (or trajectory
control) and the latter force control.
Figure 1.17 is a rough sketch of a typical controller for position control.
Joint positions and velocities are generally measured by joint sensors, such
as potentiometers, tachometer-generators, and/or encoders. Using these
data, the controller determines the inputs to the joint actuators so that the
end effector follows the desired trajectory as closely as possible.
The detailed structure of a position controller is shown in figure 1.18.
The outputs on the right of the figure are the joint positions of the manipu­
lator. The joint-trajectory generator determines the desired joint trajec­
tories from the desired trajectory for the end effector. The desired joint
trajectories are then given to the joint controllers. Each joint controller is
a servomechanism for a single joint position. Figure 1.19 shows a simple
servomechanism that uses the position and velocity feedback with constant
feedback gains. This kind of controller has often been used for general
industrial manipulators. Generally, changes in dynamics are due to changes
in the manipulator configuration; there is also interaction among the joints.
The above controller design implicitly assumes the possibility of coping
with changes in manipulator dynamics and joint interactions by regarding
10 Chapter 1

Desired trajectory Driving input

furendeffecror
r-------,
L-__�-..I
J----::::�--l=::::�> End-effector position

Joint displacement (velocity)

Figure 1.17
Rough sketch of position-control system.

r.====:::>End-effector position
Position (velocity) of
joint 1
Desired trajectory
for end effector
.
DeSlre d
. Position (velocity) of
Jomt
joint 2
trajectory
generator , l. l _r-==:::n==:--J
Position (velocity) of
j oint n

L------I Sensor for joint 1

Figure 1.18
Example of controller.

Position Sensor for joint;

Figure 1.19
Position-velocity feedback servosystem.
Overview 11

Figure 1.20
Four-joint wrist with distance between axes of joints 12 and 13 set to zero. (J2 and 13 are in
the horizontal plane and orthogonal to each other.)

them as disturbances. However, when there are severe demands for fast and
accurate positioning, the tracking performance of this type of controller is
no longer adequate. Eventually it would be necessary to return to figure
1.17 and to design the controller taking into consideration the interaction
among joints and the change of dynamics.
In force control, it is generally necessary to measure the forces driving
the joints or the contact force between the end effector and the object by
force sensors, and to feed these signals back to the controller.
In the following chapters, we will study the fundamentals of the analysis
and control of robot systems such as those described above.

Exercises

1.1 How many degrees of freedom do you think the human arm has
(except for the hand portion)? Draw a structural model of a manipulator
equivalent to the human arm using the symbols in figure 1.2.
1.2 Figure 1.20 shows a wrist mechanism obtained from the four deg rees - ­

of-freedom wrist shown in figu re 1.7 by setting the distance between the
axes of joints J2 and 13 to zero. Find the singular configurations of this
wrist. Is it possible to avoid these singular configurations without changing
the orientation of the end effector?
1.3 Consider the task of inserting a peg with a cir c ula r cross-section into
a cylindrical hole in a plate with arbitrary position and inclination. How
12 Chapter 1

many degrees of freedom should a manipulator have in order to perform

this task?

References

1. T. Stackhouse, "A New Concept in Robot Wrist Flexibility," in Proceedings of the Ninth
International Symposium on Industrial Robots (1979), pp. 589-599.

2. J. P. Trevelyan et aI., "ET -A Wrist Mechanism without Singular Positions," In terna tional
Journal of Robotics R esearch 4, no. 4 (1986): 71-85.

3. T. Yoshikawa and S. Kiriyama, "Four-Joint Redundant Wrist Mechanism and Its Con­
trol," ASME Journal of Dynam ic Systems. Measurement. and Control 11 I, no. 2 (1989):
200-204.

4. H. Makino and N. Furuya, "SCARA Robot and Its Family," in Proceedings of the
International Conference on Assembly Automation (1982), pp. 433-444.

The following are well-written textbooks on robotics. They affected the style of this book
in many ways.

R. P. Paul, Robot Manipulators: Mathematics. Programming. and Control (MIT Press, 1981).

H. Asada and J.-J. E. Siotine. Robot Analysis and Control (Wiley, 1986).
J. J. Craig, I ntroduction to Robotics (Addison-Wesley, 1986).
2 Kinematics

This chapter will be devoted to the kinematics of robot manipulators, which

means studying geometrically the motion of the manipulator links and/or
objects related to the manipulation task in terms of position, velocity, and
acceleration.
First, to express the position and orientation of a rigid object, the method
of assigning a coordinate frame to the object will be explained. Homogene­
ous transforms, which are convenient for describing the relations among
many objects, will be introduced. Then an expression for finding the end
effector's position in space from the description of the manipulator mecha­
nism and its joint displacements will be derived. Methods for obtaining the
j oint displacements that realize a given end-effector position will also be
discussed.
Next, the Jacobian matrix describing the relation between the joint's
velocity and the end effector's velocity will be introduced. We will consider
the problem of finding a joint velocity that achieves a given end effector
velocity and discuss singular configurations and an application of the
Jacobian matrix to some problems of statics.

2.1 Position and Orientation of Objects

2.1.1 Object Coordinate Frame

The first thing we have to do to analyze a manipulator is represent mathe­

matically the position and orientation of the manipulator itself, the tool it
holds, and the objects on which the robot works in three-dimensional space.
For this purpose we adopt a method generally used in mechanics l : we
attach an orthogonal coordinate frame to each object and express its
position and orientation by the position of the origin and the directions of

coordinate frame. The former is called the object frame and the latter the
the three axes of the attached frame relative to a given reference orthogonal

reference frame.

Consider the object shown in figure 2. 1 . The reference frame is denoted

by LA , its origin by OA ' and the three coordinate axes by XA, YA, and Z A ­
Similarly, the object frame is denoted by LB' its origin by O B ' and the three
axes by XB , Y B , and Z B ' The vector from OA to OB (i.e., the position vector
of O B relative to 0A )' expressed in LA , is denoted as ApB . * The unit vectors

* Vectors and matrices are denoted by boldface italic letters, the only exception being identity
matrices and zero matrices; those will be denoted by I and 0, respectively.
14 Chapter 2

z.

JL.------<:> y•

.\,

Figure 2.1
Reference frame and object frame.

in the directions of XB, YB, and ZB' expressed in kA' are denoted AXB, AyB,
and AZB . Then the position of the object is represented by ApB, and its
orientation is represented by {AXB' AyB, AZB}. The superscript A on the left
means that the vector is expressed in the frame kA• Hereafter, all frames
denoted by k are right-hand orthogonal coordinate frames.

2.1.2 Rotation Matrix

It was established in subsection 2.1.1 that the orientation of an object can

be specified by the three vectors {AXB' AYR, AZB}. It is often convenient,
however, to use a matrix defined by

(2.1 )
instead of the three vectors. This matrix ARB' which can be regarded as
describing the rotational part of the relative displacement of frame I:B from
frame I:A, is called the rotation matrix.

Example 2.1 Consider the problem of expressing the position and orien­

as shown in figure 2.2a. In words, the origin OH is taken to be at the

tation of a two-fingered hand. The hand frame I:H is attached to the hand

midpoint of the two fingers. The ZH axis is in the approach direction of the
hand. The YH axis is in one of the two directions in the plane including the
two fingers and normal to ZH' The XH axis is assigned so as to be normal
to ZH and YH, mak ing I:H a right-hand coordinate system. We assume that,

agrees with the reference frame I:A, the hand is rotated 90° around the axis
starting from the state (shown in figure 2.2b) where the hand frame kH

that goes through [0,0,2Y of kA and parallel to XA" The hand's position
Kinematics 15

(a) (b)

Figure 2.2
Representation of position and orientation of a hand. (The black dot on one finger
distinguishes it from the other finger.) (a) Hand frame �H. (b) Relation between � .. and �H.

and orientation after rotation are given by

and

ARH � [� � -no,
Some properties of the rotation matrix will now be given. Since AX B, AyB,
and AZB are unit vectors orthogonal to one another, they satisfy

(AXB ) T AXB
(AY B f AY B
= 1,

(AZ B ) T AZ B
= 1,

= 1,

(AX B )T AY B 0,
(2.2)

(AYB f AZB = 0,
=

(AZB) T AXB O.
Hence AR B satisfies
=

(2.3)

• An open square (0) denotes the end of an example.

16 Chapter 2

YB

XB

Figure 2.3
Interpretation of"" = AR/r.

where 13 is the 3 x 3 identity matrix. From equation 2.3, we have

ARB - I = (ARB)T, (2.4)

where R-1 denotes the inverse of a matrix R. Therefore, the rotation matrix
AR is an orthogonal matrix.
B
Consider the case where the origins of LA and LB are at the same point,
as shown in figure 2.3, and assume that a vector,. is described by
AI' = [ Ar"" Ary, ArzY
when expressed in LA' and by

when expressed in LB' Then we have

or

(2.5)
Replacing LA and LB yields

(2.6)

From equations 2.5 and 2.6 we have

(2.7)

Since equation 2.7 holds for any Ar, we obtain

Kinematics 17

(2.8)

or

(2.9)

Further, consider the case where the origin of a third frame, Lc, coi ncides
with the common ori gi n of LA and LB, and assume that r is described by
cr when expressed by Lc. Then

(2. 1 0)

(2.11 )

Hence, from equations 2.5, 2. 1 0, and 2. 1 1 , we h ave

(2. 1 2)

Since eq uation 2. 1 2 holds for any cr, we obtain the relation

ARc =
ARB BRc. (2. 1 3)

equa tion 2.13. First, from equation 2.13 we have

An e x pressio n of ARB different from equati on 2.1 can be obtained usi ng

(2.14)

Let XA, CYA, and cZA denote the un it vectors in the directions XA, �, and
c
ZA' and let cXB, CYB, and cZB denote those in the directions XB, YB, and ZB '

ARB =
[ (CXAfCXB (CXA)TCYB (CXA)TCZB
(CyAfCXB (CyA)TCYB (CyAfCZB .
]
both expressed in L . Then ARB can also be written as
c

(2. 1 5)
(CzAfCXB (CZA)TCYB (CZA)T CZB
Note that, for example, the (1,1) element, (CXA)TCXB, is t he cosine of the

elements. For this reason, the rota ti on matrix ARB is also called the direc­
angle between the two vectors cXA and cXB. The same holds for all other

tional cosine matrix. The value of ARB is, of course, independent of the choice

Although ARB has nine variables t he y al w ays satisfy equation 2.2. Hence,
ofLc·
,

the representa tion by ARB is redundant. When any two of t he three vectors
AXB, AyB, and AZB are g iven we can obtain the last vector using equation
,
18 Chapter 2

ZA
ZA
t\
Z,=ZA
ZA=Z.

\ e
e ZA·
\
\

I
I
\ Y.;= Yl'
I
FO-.--+--=---{> YA �'-------{'>
\ Y.
1
I
I
I

Figure 2.4
Euler angles (,p.O. "') .

2.2.* For instance, the pair {AZB, AXB}, which has six variables, can represent
the orientation. So long as the elements of the rotation matrix themselves
are used, there seems to be no way of representing the orientation by five
or fewer variables. On the other hand, intuitively speaking, since equation
2.2 provides six relations to nine variables, the orientation should be
describable by three variables, just as the position is. In fact, Euler angles
and roll-pitch-yaw angles are convenient established representations. Both
of them regard the object frame LB as a result of three sequential rotations
about some fixed axes from the reference frame LA , and represent the
orientation of LB by the set of three rotational angles.

2.1.3 Euler Angles

There are several definitions of Euler angles. A rather common definition,

illustrated in figure 2.4, is adopted here:

(i) The coordinate frame obtained from a rotation about the ZA axis by an
angle rP of a frame initially coincident with LA is named LAOA - XA , �,ZA ')'
(ii) The frame obtained from LA' by a rotation about YA, by an angle e is
named LA,, ( OA - XA " �" Z A" )'
(iii) The frame obtained from LA" by a rotation about ZA" by an angle '"
is named LB'

Thus, the orientation of LB with respect to LA can be represented by the

* Strictly speaking, determining the last vector requires, besides equation 2.2, the condition
that AXg. Ayg. and AZ8 form a right-hand system. See exercise 2.17.
Kinematics 19

set of three angles (fj), e,t/J), which are called te Euler angles. The relation
between the Euler angles and the rotation matrix ARB is as follows.

-sinfj) 0 ]
First, from figure 2.4a, the relation between I:A and I:A, is given by

o
cosfj) O . (2. 1 6)
1

Similarly, the rotation matrix A'RA" bet ween I:A , and I:A", and the rotation

[ cosO o Sine ]
matrix A"RB bet ween I:A " and I:B, are given by

o cosO
A'R " 0 1 0 (2.17)
A =

-sine

and

-sint/J
cost/J
0]
o . (2.1 8)
o 1

From eq u atio n 2. 1 3, the final rotation matrix ARB resulting from the three
rotations is obtained from

(2. 1 9)

which is

ARB =
[ cos¢> cosO cos'" - sin¢> sint/!
sin¢> cosO cost/! + cos¢> sin'"
- cos¢> cosO sin", - sin¢> cos'"
- sin¢> cosO sint/! + cos¢> cost/!
cos¢> Sino
sin¢> sinO .
]
-sinO cos'" sinO sint/! cosO
(2.20)

Let R(W,IX) denote the rotation matrix between a coordinate frame and the
frame obtained by its rotation about an axis W by an angle IX. Then
equation 2.20 can also be written as

( 2 .2 1 )

Hence w e have shown that the rotation matrix e q u ivalent to any Euler
angles (fj),e,t/J) is gi ven uniquely by equation 2.20.
Now consider the problem of obtaining the Euler angles for a given ARB'
Assume that
20 Chapter 2

(2.22)

is given. Equating each element of equation 2.20 and equation 2.22, we have

cos¢; cosO cost/! - sin¢; sint/! = R 11, (2.23a)

- cos¢; cosO sint/! - sin¢; cost/! = R 12 > (2.23b)

cos¢; sinO = R 13, (2.23c)

sin¢; cosO cost/! + cos¢; sint/! = R 2 1, (2.23d)

- sin¢; cosO sint/! + cos¢; cost/! = R 2Z , (2.23e)

sin¢; sinO = R 23 , (2.23f)

- sinO cost/! = R 31 , (2.23g)

sinO sint/! = R 32, (2.23h)

cosO = RH. (2.23i)

From equations 2.23c and 2.23f,

sinO = ± JR132 + R z/ . (2.24)

Hence, from equations 2.24 and 2.23i we obtain

0= atan2(± JR 13 2 + R 2 /, R 33 ). (2.25)

Either the top or the bottom symbol of ± or =+= should be used consistently
throughout this section. In equation 2.25, atan2 is a scalar function defined
by

atan2(a, b) = arg(b + ja), (2.26)

where j is the imaginary unit and arg(·) is the argument of a complex

number (see figure 2.5). The function atan2 is a kind of arc tangent function
that satisfies

o = atan2(sinO, cosO) (2.27)

and

o = atan2(k sinO, k cosO) (2.28)

Kinematics 21

1m

( b, a)

atan2(a, b)
--��-L-----DRe

Figure 2.S
Function atan2.

for any positive real number k. Several properties of the function atan2 are

Angles I/J and ljJ will be determined next. If sinO i= 0, we have

also summarized in appendix 1.

I/J = atan2(±R z3,±R 1 3) (2.29)

from equations 2.23c and 2.23f, and

ljJ = atan2( ± R 32,=t=R 31) (2.30)

from equations 2.23g and 2.23h. Hence, if R 13Z + R 2/ i= 0, the Euler

angles are obtained from equations 2.25, 2.29, and 2.30:

I/J = atan2 ( ± R2 3, ± R 1 3), (2.31a)

0 = atan2( ± JR 1 3
2
+ R 2/ , R 3 3), (2.3 1 b)

'" = atan2( ± R 3 2, =t=R 3d· (2.3 1c)

It is also straightforward to show that these solutions satisfy the other
equations in 2.23. Although there are two sets of Euler angles for a given
ARB' if we put the constraint 0 < 0 < 7r on 0 we have the following unique
Euler angles:

I/J = atan2(R z3, R 13), (2. 32a)

e = atan2(JR 1 3
2 Rz/ , R 33), (2.32b)
+

'" = atan2(R 3 2, - R31)' (2.3 2c)

On the other hand, if R 1 32 + R 2/ = 0, we have

I/J = arbitrary, (2.33a)

e = 90 0 - R 33 X 90 0, (2.33b)
 22 Chapter 2

(2. 33c)

combinations of t/J and !/I for just one orientation. So special care should
In other words, in the case of 8 0 or 8 n, there is an infinite number of
= =

be taken in treating this case. If we wish to keep uniqueness even in this

case, one way is to set t/J 0 and !/I atan2(R 2 1, R 22)'
= =

We can also find a general expression of the Euler angles for a given
rotation matrix which needs no distinction between the cases sin8 0 and =

sin8 -:f- 0; see exercise 2.2.

Example 2.2 The representation by Euler angles of the orientation in

example 2. 1 is

or

2.1.4 Roll, Pitch, and Yaw Angles

The concept of roll, pitch, and yaw angles is basically the same as that of
Euler angles except for a difference in the way we select the third rotational
axis. In the case of roll, pitch, and yaw angles (figure 2.6),

(i) the frame obtained from :EA by a rotation about ZA by t/J is named :EA"
as with Euler angles,

Z.,=Z;

'\,1"=-.\"1

Figure 2.6
Roll, pitch, and yaw angles (¢, 8, t/I).
Kinematics 23

(ii) the frame obtained from LA' by a rotation about YA, by () is named L A'"
again as with Euler angles, and
(iii) the frame obtained from LA" by a rotation about XA" by t/I is named
LB'
Then I/J, () and t/I are called the roll, pitch, and yaw angles, respectively. The
triple (I/J, (), t/I) is said to be the representation by the roll, pitch, and yaw
angles of the orientation of LB with respect to LA '
Example 2.3 The representation by roll, pitch, and yaw angles of the
orientation in example 2.1 is given by

or

(I/J, (), t/I) = ( 1 80°, 1 80°, -90°). 0

2.2 Coordinate Transformation

2.2.1 Homogeneous Transform

In figure 2.7 we assume that the relation between two coordinate frames
I:A and LB is given by the position vector 'PB and the rotation matrix A RB
of LB with respect to LA ' Then the relation between the expressions A,- and
B
y of a point in space by (respectively) LA and LB is

(2.34)

Figure 2.7
Frames 1:.. and 1:•.
24 Chapter 2

This equation can also be expressed as

(2.3 5)

B
In this new expression, the three-dimensional vectors Ay and r must be
represented by the four-dimensional vectors obtained by adding the ele­
ment 1 at the bottom of the original vectors. In return for this, the expression
obtained is simpler in the sense that the transformation from the expression
of a vector with respect to :EA to that with respect to :EB is d one by the
multiplication of j ust one matrix: ATB• The transformation represented by
A
the 4 x 4 matrix TB is called the homogeneous transform. The vector
[ArT, 1 Y may also be written as Ar when there is no confusion; for example,
equation 2.35 may also be written as

(2. 36)

Homogeneous transforms can be used for the following purposes:

(i) Changing the coordinate frame with respect to which a point in space is
expressed. This is what we did in equations 2.35 and 2.36 when we wished
B
to obtain the expression Ay with respect to :EA of a point expressed as r
with respect to :EB.
(ii) Describing the relation between two coordinate frames. Since ATB con­
tains both of the parameters (ApB and ARB) that represent the relation of
:EB to :EA, ATB can be regarded as describing this relation. Although ATB has
no merit over the pair {ApB' ARB} in real numerical calculation, it allows
for greater simplicity of expression.

l
Example 2.4 As is shown in figure 2.8, when :EB is obtained from :E by a
A
rotation about Z A for a , their relation is described by

-sina 0 0
cosa 0 0
o 1 0 . 0
o 0 1

Example 2.5 When:EB is obtained from :EA by a translation of 2 units

in Y,. and then by a translation of 1 unit in Z ' their relation is given
A
by
Kinematics 25

z.=z.

YB

�-"'---C>YA

Figure 2.8
Frames LA and LB'

-- ----
---- 2
J-1
-...
=::---_-:_� )30'
·-tr,
r:,
--
_-
- .-t
_

-- -

�=----<> YA

XA

[1 1 ]
Figure 2.9
Transfer of a point.

0 0 0

= 0 0
A
0 1 0 2
TB 1 . 0
0 0 0 1

(iii) Description of transfer of a point in space. We assume that a point in

space with a fixed coordinate frame is transferred in a given way. The
homogeneous transform can be used to describe the relation between the
original point location and the location after the transfer.

1:A. Let A'2 denote the new location of this point after rotating 30° about
Example 2.6 As shown in fi gure 2.9, we consider a point A'l expressed in

[
relation between A'I and ""2 is described by

]
Z A and then translating 2 units along r.c and - 1 units along Z A' The

COS300 -sin30° 0 0
..., _ s in30 ° cos30° 0 2 A
2- '1.0

o
0 o -1
o 0 1
26 Chapter 2

ZB

0"

Figure 2.10
Frames l" and I:".

These three uses of homogen eou s transformation are of course closely

related, as example 2.7 will show.
B
Example 2.7 When the transform given in e x a mple 2.6 is regarded as TA
describing the relation between two frames kA and kB' we have the change

origin all y at the sam e place as kA' first by tran s la t i ng it -2 units alo ng �
in frame s hown in figure 2.10. Note that kB is obtaine d from a fram e
and 1 unit a l o n g ZA an d then by rotati ng it - 30° about Z A' Al so note that
the point whose repres en t ati on in kA is Arl is represented as Arz in k8. 0

2.2.2 Product and Inverse of Homogeneous Transform

Consider three frames, kA' kB' and and assume tha t the relations
ke,
A B
between kA and k8 and between kB andke a re given by TB and Te
A
respectively. Then the relation Te between kA a nd ke is giv en by the
A
product of TB and BTc:

(2.37)

This is an extension of equation 2. 1 3.

The following two interpretations in terms of coordinate-frame t rans­
formations are poss i ble for equation 2.37:

to kA and then rotating the translated frame ( us i n g RB) about the origin
A
(i) The frame kB is obtained from kA by translating kA by pB wi th respect
A

of the translated frame itself. The frame ke is obtained from kB by trans­

B B
lating by Pe a n d then rotating the tra nsl a t ed frame (using Rc) about the
origin of the translated frame.
Kinematics 27

(ii) We denote as LB , the frame obtained by first rotating a frame initially

B
coincident with L A using BRe and then translating it by pe, both with
A
respect to LA' Further, we rotate LB' using R B and translate it by ApB, both
with respect to LA' Thus we obtain the frame Le.

In interpretation i, the product of two transforms is interpreted from the

B
left one ( ATB), and the second transformation ( Td is done with respect to
the new frame obtained from the left transformation. In contrast, in inter­
B
pretation ii the product is interpreted from the right transform ( Td, and
both transformations are done with respect to the original frame. The
difference of order between the translation and the rotation in interpreta­
tions i and ii could be understood by substituting into equation 2. 37 the
relation

(2.3 8)

and a similar relation for BTO where 0 denotes a vector or a matrix whose
elements are all zeros.
B
Example 2.8 Assume that ATB and Te are given by

� �l
- 1/2

o
vG/2
1 0
o o I

and

A
The two interpretations of Te are illustrated schematically in figure 2. 1 1 .
In this example the Z axis is always normal to the page and thus it is omitted
from the figure. The final location of Le is, of course, the same in both cases.
o

The inverse of ATB is, from equation 2.4,

28 Chapter 2

Yc

XB

Xc
Xc
I
J l
��-=--�- �-�_�_= ��______�XA
0. 2 X.

XB'
(a) (b)

Figure 2.11
Interpretation of ATe. (a) Interpretation by (i). (b) Interpretation by (ii).

(2.39)

2.3 Joint Variables and Position of End Effector

General Relation

In thi s subsection an ove rview is give n of the relation between joi nt dis­
2.3.1

p l ac emen ts (rotati onal disp l acements for revolute joints and linear dis­
pl acements for prisma tic joi n ts ) and end -effec tor position for a manipulator

In figure 2.12 the j oi nts are numbered 1, 2, ... , n, start in g from the base
wi t h n degrees of freedom.

of the manipulator. The displacement of j oint i is denoted qi and is called

the joint variable. The collection of joint va riable s

(2.40)

is called the joint vector. The pos ition of the end effector is deno ted by the
m- di men sio nal vector
(2.41)

position and ori entation in three-dimensional Euclidean space, we have

where m � n. For a general case where the end effector can take an ar b itrary
Kinematics 29

q2

Figure 2.12
n-link manipulator.

m 6. However, when the manipulator moves in a two-dimensional plane

=

set m = 2. If we are further concerned with the orientation of the endpoint

and we are concerned only with its endpoint position in the plane, we can

in the plane, we have to set m = 3.

The relation between rand q, determined by the manipulator mechanism,
generally is nonlinear. We assume that this relation is given by

r fr (q).
=
(2.42)

the joint v ector q is gi v en the corresponding r is determined uniquely and

This equation is called the kinematic equation of the manipulator. When
,

the calculation is rather simple. However, when some task is assigned to

the manipulator, what is given first is usually its end-effector position r or
a trajectory ofr. Thus we have to calculate a joint vector q which will realize
the required end-effector position r-that is, we have to obtain q satisfying
equation 2.42. This solution can be written formally as

(2.43)

Note, however, that q does not necessarily exist, and even when it does exist
it may not be unique. The problem of obtaining r for a given q is called the
direct kinematics problem, and that of obtaining q corresponding to a given
r is called the inverse kinematics problem. As we can see from the above
argument, the inverse kinematics problem is usually the more difficult of
the two.

Example 2.9 The relations 2.42 and 2.43 will be obtained for the two­

figure 2. 1 3. The joint vector in this case is q = [OI,02Y' the end-effector

degrees-of-freedom manipulator moving in the X Y plane as shown in
-

position is given by r = [x,YY, and the lengths of the links are 1 1 and 12,
The relation 2.42 is easily obtained:
30 Chapter 2

y
r=(x,y)'

_...l-_____-<>X

Figure 2.13
Two-link manipulator.

x = 11 eos8! + 1 2cos(81 + (2),

Y = 11sin8! + 12sin(81 + (2),

� X2 + y2 � ( I1 + 12)2
As for the relation 2.43, if r satisfies

(11 - /2 ) 2

then there exists at least one q corresponding to r. Further, if x2 + y2 # 0,

using the function atan2, from equations AU 1, Al.4, and A1.6 of appendix
1, we obtain

81 = atan2( y, x) + atan2(K, x2 + y2 + /1 2 - 1/)

and

where

in figure 2. 1 4. If, on the other hand, x2 + y2 (11 - 12)2 0, then

The two solutions given above correspond to the two arm configurations
= =

81 = arbitrary

and

that is, there are infinitely many solutions. 0

Kinematics 31

'--_--/f � x. y)

alan 2 (x. x! + y!-/,'-I,')

r-�..c.�-alan2 (x. x'+ y!+ I,'-I,!)

Figure 2.14
Solutions of inverse kinematics problem for two-link manipulator.

Joint II
Joint 2

~. .�
Link 1

Joint 1 0

Link 0 (basel

Figure 2.15
Link and j oint numbers for n-link manipulator.

As example 2.9 shows , it is rather easy to solve the direct kinematics

problem and the inverse kinematics problem for manipulators with around
two degrees of freedom. However, both problems become increasingly

with this diffic ul ty is to as si g n an ap p ropria te coordinate frame to each link

diffi cul t as the number of degrees of freedom increases. One way to cope

and to describe the relation among the links by the relation among these
frames. In this way, deri ving the function f.(q) becomes systematic and
obtaining f.-1(r) a nal yticall y or numerically becomes much easier. This
approach is developed in the following subsections.

2.3.2 Link Parameters

Consider a manipulator consisting of n links co n nected serially by n joints,

with one degree of freedom each, which may be either revolute or pri smatic.
As is shown in figure 2. 1 5, the links and the joints are numbered 1 , ... , n,
32 Chapter 2

Joint i

J:>
o Joint i + 1
/
Joint i-I
\

(a)

Joint i-I
\

(b)

Figure 2.16
Joint axes, joint variables, and link parameters: (a) when joint i is revolute; (b) when joint i
is prismatic.
Kinematics 33

starting from the base side. The base itself is called link 0; hence, link 0 is
connected to link 1 by joint 1. For each joint i, the joint axis i is defined as
the rotational axis in the case of a revolute joint or as an arbitrary straight
line parallel to the direction of translation in the case of a prismatic joint.
In figure 2.16, dashed lines show the joint axes in these two cases. The
common normal between joint axes i and i + 1 is considered to be the
mathematical model of link i. The common normals are shown as solid
lines in the figure. When the joint axes i and i + 1 are parallel, the common
normal is not unique, so we select one common normal arbitrarily as the
mathematical model of link i.
With these preparations, we can now describe the size and shape of link
i by two variables: the length ai of the common normal, and the angle IXi
between the orthogonal projections of joint axes i and i + 1 onto a plane
normal to the common normal. The variable ai is called the link length, and
IXi is the twist angle.

be described by the distance d; between the feet of two common normals

The relative positional relation between links i 1 and i at joint i can
-

on the joint axis i, and the angle 0i between the orthogonal projections of

dt is called thejoint length, and 0i is called the joint angle. Ifj oint i is revolute,
these common normals to a plane normal to the joint axis i. The variable

dj is constant and 0i expresses the rotational angle of the joint; if j oint i is

prismatic, 0i is constant and dj expresses the translational distance of the
joint. Hence, when joint i is revolute we adopt OJ as the joint variable q;,
and when joint i is prismatic we adopt di. The other three variables are
constant and are called link parameters. This way of describing link mecha­
nisms using ai' lXi' d;, and OJ is usually called the Denavit-Hartenberg
notation. 2

2.3.3 Link Frames

Now we will define coordinate frames, one fixed to each link. As is shown
in figure 2.17, the origin of coordinate frame Li of link i is set at the endpoint
of the m athematical model of link i on joint axis i. The Z axis of L;, denoted
Zj, is selected in such a way that it aligns with joint axis i in the direction
pointing toward the distal end of the m anipulator. When the direction
toward the distal end is not clear, the direction of Zj is arbitrary. The X
axis of Lj, Xi' is selected so that it is on the common normal and points
from joint i to joint i + 1. The Yaxis, y;, is selected in such a way that Lj
is a right-hand coordinate frame.
34 Chapter 2

Joint i
Joint i-I
\
!

Joint 2 Joint n
"­ /
' '-.,

Joint 1

Link 0

1
Figure 2.17
Link frame ki' (ko and kn are shown for the case when the joint variables are zero,)

By the above procedure, the link frames for links through n - 1 are
determined. For link 0, the link frame �o is defined to be equal to �I for
an arbitrarily selected reference configuration ofjoint 1. For link n, the joint

to be at the endpoint of the mathematical model oflink n - 1 on j oi nt axis

n is fixed at an arbitrarily selected configuration and the origin of �n is set

n. The axis Z. is aligned with the joint axis n with its direction pointed
toward the distal end of the manipulator when the direction toward the
distal end is clear. The axis Xn is aligned with Xn -I' and y" is determined
in such a way that �. is a right-hand frame.
Now that we have defined the frame �j, provided that the positive sense
of each variable is determined so that it harmonizes with �i> the four
variables introduced in the previous subsection can be expressed as follows:

the distance measured along the Xi axis from Zj to Zj + 1 ,

= the angle measured clockwise about the Xi axis fro m Zi to Zi+I'

aj =

(/.i

dj = the distance measured along the Zj axis from Xj_1 to Xi>

and

OJ = the angle measured clockwise about the Zj axis from Xi-I to Xj.

In other words, the frame �i can be obtained from �j_1 by the following
four transfo rmations:

(i) translation along Xj -I for a distance aj -I,

(ii) rotation about Xj_1 by an angle (/.i-l,
Kinematics 35

I Joint i + 1
Joint i-I
\

0'_1

Figure 2.18
An assignment of link frame i when joint i is prismatic (a; = O. di+ 1 = 0).

(iii) translation along Zj_1 after the rotation ii (which is ZJ for a distance
dj, and
(iv) rotation about Zi-I after the rotation ii (which is Zj) by an angle OJ'

�n-I and �n' wi th �o and �n defined earli er by se lec tin g the link parameters
Note that the above relation holds also between �o and �I' and between

as follows:

0 when j oint 1 is revolute,

ao = OCo0, =

d1 =

01 = 0 when joint 1 is prismatic,

0 when joint n is p ri sma ti c

dn = 0 when joint n is revolute,
On = .

It is desirable to make as many link p ara m eter s zero as we can, since this
will make the later analysis easier and will decrease the amount of computa­
tion necessary to solve the direct and inverse kinematics problems Re­ .

viewing the procedure of dete rmining the joint axes and the common
normals from this viewpoint makes it clear that when they are no t unique

When determining the joint axis i for a prismatic joint, we use the fact
the following scheme is a desirable one.

that its loc a ti o n is arbitrary while its direction is fixed. We can set aj_1 or
aj to zero, and d j 1 or dj+1 can also be set to zero. Hereafter we s et aj 0
_ =

and dj+1 0 by having the joint axis i pass through the endpoint of the
=

mathematical model of link i + 1 on the joint axis i + 1, as is shown in

figure 2.18. When the joint axes i and i + 1 are parallel, d; or di+1 can be
36 Chapter 2

!
Joint i Joint i + 1
I
Joint i-I
\

Z,

X,
Z,.,
X/-1

Figure 2.19
An assignment of link frame i when joint axes i and i + I are parallel (ddl = 0).

set to zero. Hereafter, as is shown in figure 2.19, we set di+l 0 by having =

the common normal pass through the endpoint of the mathematical model
of link i + 1 on the joint axis i + 1. In these two cases, if there exists any
arbitrariness in the location of the mathematical model of link i + 1, we
can further determine the link frames so as to set ai-I, di-I' etc. to zero.
Also note that in figures 2.18 and 2.19 t he Y axes of the link frames are not
shown because they are uniquely determined by the X and Z axes. This
convention will be used hereafter.
In summary, the link frames are determined as follows:

(i) Determine the joint axes.

(ii) Determine the common normals.
(iii) Determine the link frames.
(iv) For joint axes and common normals that have arbitrariness in their
location, return to step i or ii and modify their location to set as many
parameters to zero as possible.

of link frames. In those cases, we can just select one set of link frames
There are still cases where the above procedure does not yield a unique set

arbi trarily.

Example 2.10 The Stanford manipulator, 3 developed mainly for research

purposes, has the mechanism shown in figure 2.20. The link frames deter­
mined by the above procedure for this manipulator are shown in figure
2.2 1; the link parameters are given in table 2. 1. Note that figure 2.21 shows
a reference configuration for whi ch 8i 0 (i 1,2,4,5,6) and all the Xi axes
= =

are in the same direction. Also note that d3 is not taken to be zero in the
figure, because a configuration with d3 0 is unattainable. 0
=
Kinematics 37

JOint % '% Joint 4

/0-Joint 5
Joint 1

t
Link 3

'
Link 2

Link

Link 0

(a) (b)

Figure 2.20
Stanford manipulator. (a) Link structure. (b) Appearance.

Figure 2.21
Link frames for Stanford manipulator.
38 Chapter 2

Table 2.1
Link parameters for Stanford manipulator. (Parentheses indicate joint variables.)

ai-1 (Xj-l di 0,
0 0° 0 (0, )
2 0 -90° d2 (02)
90° (}o
3 0 (d3)
4 0 0° 0 (04)
5 0 -90° 0 (O
s)
6 0 90° 0 (06
)

Joint;
! Joint i + 1
/
Joint;-J
\

Joint n
Joint 2 /

'.

Figure 2.22
Another assignment of link frames.

The merit of the assignment scheme of link frames given above is that
the frame of each link has its origin on the joint axis closer to the base of

displacement directly causes the motion of that link. This assignment

the link. Also, the Z axis of the frame agrees with the joint axis whose

scheme appeared in Craig's Introduction to Robotics.4 Another scheme of

assigning link frames is shown in figure 2.22, where the origin of the frame
is on the distal joint axis of the corresponding link. Its Z axis is the joint
a xis for the motion of the next link at the distal end. A more detailed

description on this scheme can be found in Paul' s Robot Manipulators. 5

In principle, any set of n coordinate frames are feasible as link frames as
long as each link has a frame fixed to it. Any assignment that s�ts the origin
of kj on joint ax i s i (when j oint i is revolute) and aligns the Zj axis with
joint axis i has most of the merits of the Denavit-Hartenberg notation. The
assignment scheme described in this subsection has been selected simply
Kinematics 39

because it is intuitively easier to understand and because it gives a unique

set of link frames for most manipulators.

2.3.4 Solution to Direct Kinematics Problem

The hom ogeneous transform that describes the relation between Li and
Li-I is

(2.44)

o
where Tr(X,a) denotes a translation along the X axis for a distance a, and

o
TR(X,tx) denotes a rotatio n about the X axis by an angle a. From equation

o 1 r I
2.44 we obtain

f' �l
0 0 a,_, 0

i-1T-= 0 1 0 0 c � s ai - l -SJntxi_1
•
0 0 I 0 sJnai-1 cosai_l

I f
o 0 0 1 0 0 0

[f � �] 1
o
0 0 8, s inOi 0

x
-

0,
0sinOi
1 0 cosOi 0
0 1 d
oi 0 0
0 0 1 0 0 0

o
COSOi -sinOi 0 ai-I

=
C�Stxi_1 � nO
s i cosai_l cosOi -sinai_l -sinai-1di
(2.45)
.
slllai-lslllOi SinOti_lcoSOi COSOti_1 cosai_ldi
0 0 I

Let the homogeneous transform relating Ln to Lo be °Tn. Then we have

(2.46)

When the values of all link parameters are given, i-I I; is a function of qi
only Hence, ° Tn is a function of the joint vector q.
.

Let L E be the coordinate frame fixed to the end effector that is atta c hed
to link n, and let nTE be the homogeneous transform describing the relation
between LE and Ln. Let LR be the reference frame, and let RTO describe the
relation between LR and the base frame Lo. The transforms "TE and RTO are
constant ones determined by the way the end effector is attached to l ink n
and by the location of the base with respect to the reference frame. The
relation between the end effector and the reference frame is then
40 Chapter 2

�5

(a) (b)

Figure 2.23
PUMA-type manipulator. (a) PUMA robot (courtesy of Westinghouse Automation
Division/Unimation Inc.). (b) Link structure.

(2.47)

Since the position vector r of the end effector is determined uniquely from
RTE and since ° Tn is a function of q, we can obtain the function J. (q) of
equation 2.42 from equation 2.47. Thus, if each element of °Tn is obtained
as a function of q, the direct kinematics problem is basically solved.

Example 2.U Let us assign the link frames to the PUMA-type manipu-

rr ] r
lator shown in figure 2.23 and obtain °T6• Following the scheme of the

]
previous subsection, we obtain the link frames shown in figure 2.24 and the
link parameters in table 2.2. Hence, from equation 2.45,

- sin8l o 0 0
cos8l o 0 0 0 I. I,
oos8, oos8, - sin82
�
0 1 0 ' o '

rr r cos8,
Si
'r, lT2-
_

�
0 o 1 0 0 1
�

I,]
- s n82 - cos82 0

0
0 0 0 1 If

�]
cos8, -sin83 0 - sin84

0 1 o ' - cos84 0 o '

'r , � Si cos83 3T
4-
_

�
0 0 1 0 1
-s n84
0
Kinematics 41

�
Figure 2.24
Link frames for PUMA robot.

Table 2.2
Link parameters.

ai - 1 1%/ - , di °i

1 0 00 0 (O, l
2 0 00 Ib - Id (02 l
-9
0 0 ( 03 l
3 1, 0
0 If ( 04 l
4 Ie 0
-9
5 0 00 0 (Os l
9
6 0 00 0 ( 06 l
9
_
42 Chapter 2

4
Ts =
r COS (JS
0
sin(Js
- sin(Js
o
cos(Js
-1
o

0
0
0
0 '
] r s 7,6 -
-
COS(J6
0
' (J6
- SIn
- sin(J6
o
- COS(J6
0
1
0
0
0
0 .
]
o o o 1 0 o 0 1
( 2.4 8)

Now w e calculate °T6 . W e fi r s t find ° T3 for t h e arm portion a n d 3 T6 for t h e

wrist portion; then we multiply t h e m . Finding these two t ra n sform s i s just
a convenience for later use. The tran sform ° T3 is gi ven by*

- C1 S2 3
- SI S2 3
-
C2 3
o
3
The transform T6 is given by

- C4 CS S6 - S4 C6
- SS S6
(2.49)
S4 CS S6 - C4 C6
o

Thus, we finally obtain

[ ]
R" R12 R!3 P,
R2 1 R22 R23 Py
0 T6 =
(2.50)
'
R31 R32 R33 Pz
0 0 0 ]

where

RII Cl [C2 3 ( C4 CS C6 - S4 S6 ) - S2 3 SS C6 ]
=

+ Sl (S4 CS C6 + C4 S6 ),

RI2 =
C1 [ - C2 3 ( C4 CS S6 + S4 C6 ) + S2 3 SS S6 ]

u
- Sl (S4 CS S6 - C4 C6 ),

• Here we use the following notations: Cl cosOl , S l sinOl , C2 3 COS(02 + 03 ), and S2 3

= = = =

sin(02 + 03 ), This kind of notation will be s e d through out the book. For example, Sl 2 3
=

sin (61 + O2 + 03 ), and C2 34 = COS(02 + 03 + 04 ),

Kinematics 43

R 13 - Cl (C2 3 C4 SS + S2 3 CS) - Sl S4 SS '

R 2 1 = Sl [C2 3 ( C4 CS C6 - S4S6) - S2 3 SS C6 ]
=

- C1 (S4 CS C6 + C4S6), (2. 5 1 )

R22 = Sl [ - C2 3(C4 CS S6 + S4 C6) + S2 3 SS S6 ]

C1 (S4 CS S6 - C4 C6),

- Sl (C2 3 C4 SS + S2 3 CS)
+

R23 = + C1 S4 SS '

R3 1 = - S2 3 (C4 CS C6 - S4S6) - C2 3 SS C6 ,

R32 = S2 3 (C4 CS S6 + S4 C6 ) + C23 SS S6 ,

Px = C1 (lc C2 + le C2 3 - lf S2 3) - (lb - ld)SI ' (2.S2a)

Py = SI (lc Cl + le Cl 3 - lfS2 3) + ( lb - ld)C1 , (2.52b)

(2. S2c)

Example 2.1 2 For the PUM A-type manipulator in example 2. 1 1 , assume

that the reference frame LR and the end-effector frame LE are as illustrated
in fi gu re 2.25. As s u me also that the end-effector position vector r is

z"

Xo

Figure 2.25
Reference frame �R and end-effector frame �E'
44 Chapter 2

where rl ' '2 ' and ,3 are the X, Y, and Z coordinates of the origin of I:E and
where '4 ' '5 ' and '6 are the Euler angles of I: E with respect to I:R . We first
solve the direct kinematics problem; then we find the value r for the case
where the joint vector is

q* = [0°, _ 45°,0°,0°, - 45°,900 y.

First,

r� �l
° °
1 °
'T
, ° 1

l
�

° °

and

° ° °
1 ° °
° 1 Ig .
° ° 1

Hence, from equations 2.47 and 2.50,

From equations 2.32 and 2.33, the corresponding position vector r is given
by

'2 = Py + R 2 3 ig ,

'3 pz la + R33 1g,

=
= +

'4 atan2(R 2 3 , R 1 3 ),

'5 atan2 ( JR 1 3 2 + R 2 / , R 3 3 )'

{ + R2 /
=

2
atan2(R 3 2 , - R 3 1 ) if R 1 3 =F °
' 2
6 atan2 ( R 2 1 , R 2 2 ) - R 3 3 '4 if R 1 3 2 + R23 0.

q = q * we have
= =

Second, when
Kinematics 45

R1 1 0, R12 = 0, 1,

= 0,
=
R13 =

R21 = - 1, Rn = 0, R23

R3 1 0, R3 2 0,

(Ie + Ie
= = - 1, R33 =

p", = + 1/)/y'2,

Thus we obtain

2.4 Inverse Kinematics Problem

We will now consider the inverse kinematics problem -that is, the problem
of finding the joint vector q that realizes a given value of the end-effector
position vector , (in other words, the value of all or part of the elements of
° T.).
One approach to this problem is to find a closed-form solution by using
algebra or geometry. Another approach is to find a numerical solution by
some successive-approximation algorithm.6 Although the former approach
is generally more desirable in applying the solution to real-time control of
robots, it is not always possible to obtain closed-form solutions for manipu­
lators with an arbitrary mechanism. Rather, the class of manipulator
mechanisms for which the closed-form solutions are guaranteed is very
limited. Notice, however, that most of the manipulators in industrial use
today belong to this class. The algebraic approach to closed-form solutions
means finding q through various algebraic transformations of eq uation 2.42
or equation 2.47; the geometric approach means finding q by using geo­
metrical heuristics to take advantage of the special structure of the manipu­
lator.
It is sometimes advantageous to use both approaches together to solve
a problem. First we will solve a simple example using each of the two
approaches.

Example 2.13 Consider the three-degrees-of-freedom manipulator shown

[r;r;,ry ,rzY,
in figure 2.26a. Assume that the end position of the manipulator is expressed
by 0, = the position vector with respect to �o. The problem is
46 Chapter 2

x"
(a) (b)

Figure 2.26
Three-link manipulator. (a) Size. (b) Endpoint position.

ZII, Z .

z·

x,

Figure 2.27
Link frames.

to find the three j oint angles that realize any desired end position r. First,

OJ
in order to derive the kinematic equation, we determine the link frames as
shown in figure 2.27. The link parameters are then given as in table 2.3.
The transforms i- l Ti are given by
-S

r'
l o

° T, � � Cl
0
o
1
0
0 '
(2 . 5 3a)

0 o 1
C,
-S

l �l
0

T2 =
2

1
0 0C 1
-S - (2.53b)
2 2 0
0 0 0
Kinematics 47

Table 2.3
Link parameters.
a/ - t (Xi - l d, °i
0 0 0
2 0 - 90· 0
3 I. 0 0

and

r' �J
- S3 0
'T
, � ! C3
0
0
0

0
o
1
.
(2.5 3c)

�} 1
Therefore,

l
- C1 S2 -S
, I
C, C
0 T3 SI C2 - SI S2 C1
=
-S -
C2 0 o 3
2
0 0 0 1

From figures 2.26a and 2.27 we can easily show that the end position
relative to L 3 is given by 3, = [le,O, lbY. Hence,

(2. 55)

The kinematic equation is obtained from equations 2.54 and 2.55 as

rx = Ic(C1 C2 C3 - C1 S2 S3 ) - Ib SI + la CI C2 ,

ry = le(Sl C2 C3 - SI S2 S3 ) + Ib CI + laSI C2 , (2.56)

rz = IA - S2 C3 - C2 S3 ) - laS2 ·
Now we find the j oint angles f)i (i = 1, 2, 3) from equation 2.56 using an
48 Chapter 2

algebraic approach. Since equation 2.56 itself is not very suitable for this
purpose, let us consider the following equation instead:

oOJ rC3 o 0laJ

( O T2 f1 0r = 2 T3 3r. (2.57 )

Substituting equation 2.53 into equation 2.57, we have

o
°
=
S3 3
r
O r O 1 0 ,
1 0 o 1

or

C1 C2 rx S1 C2 ry - S2 r• = Ic C3 la' ( 2.58a)

C 1 S2 rx - Sl S2 ry - C2 rz
+ +

- lcS3 ' (2.58b)

C1 ry = lb·
=

- S1 rx + (2.58c)

If we further consider

(2.59)

we obtain

(2.60a)

(2.60b)

(2.60c)

Since the variables 0; are fai rly well separated in equations 2.58 and 2.60,
it is rather easy to find a way to solve these equations for (J; . For example,
we can obtain 01 from equation 2. 58c or 2.60b as follows:

(2 . 6 1 )

where we have used equation A U 7 of appendix 1 . Summing the squares

of equations 2.60a, 2.60b, and 2.60c, we have

(2.62)

Hence, 03 is given by

()3 :::; ± atan2(K, r/ + r/ + r/ - // - 1/ - 1/ ), (2.63)

Kinematics 49

where

K = { (r/ r/ r/ - Ib2 V [/ ) 2
2 [ ( r/ + r/ + r/ - lb2 ) 2 1/
+ + + +

-
+ + Ic4 ] } 1/2. (2.64)

As for the last variable O2 , from equations 2.58a and 2.58b we have

[r/ + (C1 rx + Sl ry f ] S2 = - rA1c C3 + la) - (C1 rx + Sl ry)lcS3 '

(2.65)
[r/ + (C1 rx + S l ry)2 ] C (C1 rx + Sl ry) (lcC3 + la) - rzlcS3 .
2
=

Thus we obtain

O2 = atan2 [ - rAlc C3 + la) - (C1 rx + Sl ryKS3 '

(C1 rx + Sl ry) (ic C3 + la) - rz 1c S3J. (2.66)

One value of O2 is determined for each combination of two values of 01 and

two values of 03 (given by equations 2.6 1 and 2.63, respectively). As is shown
above, the solution can be more easily found when a transformation such
as 2.57 and 2.59 is used rather than just 2.55. Next we will solve the same
inverse kinematics problem using a geometric approach. By projecting the
arm onto the Xo- Yo plane of Lo, we obtain figure 2.28. Thus we have

_------T'--c> Y"

(a)
(b)

Figure 2.28
Projection of manipulator on Xo- Yo plane (a) when the Xl axis and [rz,r, Y are in the same
direction; (b) when the Xl axis and [rz,r, Y are in opposite directions.
50 Chapter 2

and (/! is given by equation 2.6 1 . The "plus or minus" s ign in equation 2.6 1
is "plus" when the arm is posed as in figure 2.28a; it is "minus" when the
arm is posed as in figure 2.28b. Figure 2.29 i s another projection of the arm
shown in figure 2 26, this ti me onto the X! - Y! plane on : ! . From this figure,
.

as in example 2.9, we o b tain (/2 and (/3 ' That is, ()3 is gi ven by equation 2.63
and ()2 is given by

()2 atan2( - rz, Jr/ + r/ - 1/ )

=+= atan2(K, r/ + r/ + r/ - 1/ + 1/ - V)
=

(in the case o f figure 2.29a; the sign " ± is i n t h e same order a s i n equation
"

2.63) or
()2 atan2 ( - rZ ! - Jr/ + r/ - lb1 )
r/ + r/ 1/ - 1/ - 1/)
=+= atan2 ( K, r/ +
=

(in the case of figure 2.29b). The above expression for ()2 is different from
equation 2.66, w h ich was found by an alge b raic appro ac h but they can be
,

shown to be equi v alent Note that when we adopt a geometric approach,

.

we should be careful to examine every possible s olut i on . 0

In the above example we considered the case where only the three­
dimensional position of th e end effector is of concern. For a general
manipul at or with six degrees offreedom, however, the position and orienta-

Z,
Z,

X,

(a) (b)

Figure 2.29
Projection of manipulator on X I -ZI plane (solid and broken lines are two possible arm
configurations). (a) Case of figure 2.28a. (b) Case of figure 2.28b.
Kinematics 51

tion of the end effector should be considered in the inverse kinematics

problem. We will now look at two approaches that are often effective in
solving this problem. The first approach is algebraic, the second geometric.
(1) Modification of Kinematic Equation (ref. 5) As example 2. 1 3 showed,
transformation of the kinematic equation was effective in reaching the
solution. Here we will consider an extension of this approach. The original
kinematic equation, given by
(2.67)
is equivalent to twelve simultaneous algebraic equations with six unknown
variables ( Q l ,q 2 , . . . ,q 6 ) ' By transforming equation 2.67 into
[O T1 (Ql ) ' " i -lT;(Q j )] -l ° T6 [i1j+l(Qj+l ) ' " 5 T6(Q 6)] - 1
(2.68)
we obtain another equivalent set of twelve simultaneous algebraic equa­
tions for every (i,j) pair such that i < j. By selecting simpler ones among
these equations, we can very often obtain a solution rather easily.
(2) Pieper's Approach 7 Many industrial manipulators have a wrist with
three revolute joints whose axes intersect at one common point, as shown
in figure 2.30. In such cases, the origins of L4, L s , and L 6 are all located at

Joint 5
I

Joint 6
/'
Joint 4 ,-

Figure 2.30
Pieper's method,
52 Chapter 2

this point. Specifying the position and orientation of the end effector
determines the position of the origin of :E6, and therefore that of :E4. The
position of the origin of :E4, which is represented by three variables (i.e., X,
Y, Z coordinate variables ), is a function of three joint variables: q 1 ' q 2 ' and
q 3 . This means that we can generally determine the values of Q 1 ' Q2 ' and
Q 3 from the given position and orientation of the end effector. We can then
find the position and orientation of :E3 , and hence the orientation of :E6
with respect to :E 3 . Finally, from this orientation of :E6 we can determine
the joint variables Q4' Q 5 ' and Q 6 . Pieper showed a more general result: that
a closed-form solution can be obtained for a manipulator which has three
consecutive revolute joints whose joint axes intersect at a point.
For manipulators with no fewer than seven degrees of freedom, there is
generally an infinite number ofjoint solutions for a given realizable position
and orientation of the end effector, implying that the manipUlator has
redundancy.
Example 2.1 4 Consider the inverse kinematics problem for the PUMA­
type arm shown in figure 2.23. The problem can be reduced to that of
finding a q that satisfies equations 2.5 1 and 2.52 for given values of elements
Rij' Px, PY ' and pz of °T6 • Since the three distal joints satisfy Pieper's
condition, Px, PY ' and pz that represent the origin of :E4 are functions of only
(Jl ' (J2 ' and (J3 ' as is shown by equation 2.52. First we determine (J l ' (J2 ' and
(J3 from equation 2.52. By using a procedure similar to that in example 2. 1 3,
we get
(Jl atan2 ( - Px , py) ± atan2 (Jp/ + p/ - (lb - Id) 2 , Ib - Id), (2.69)
(J3 = atan2 ( - If, le ) ± atan2 (Jlc 2 (le + 1/) - "a 2 , "a ),
=

2 (2.70)
"a [ p/ + p/ + p/ - V - 1/ - 1/ - ( lb - Id)2]j2, (2.7 1 )
(J2 = atan2 [ - (le S3 + IfC3) (C1 px + Sl Py) - (leC3 - lfS3 + Ic) Pz '
=

(leC3 - IfS3 + U ( C1 Px + Sl Py) - (leS3 + IfC3)pz ]' (2.72)

We next find (J4' (J5 ' and (J6 from
(2.73)
Denoting the upper left 3 x 3 submatrix of the left-hand side of equation
2.73 as [Rij], where i, j 1 , 2, 3, we have
=
Kinematics 53

- C4CS S6 - S4 C6 - C4S5
- SSS6 Cs
]
S4CSS6 - C4C6 S4SS
(2.74)
Similarly to equations 2.3 1 and 2.33, the values of 84, 85 , and 86 that satisfy
equation 2.74 are given by
(J4 = atan2( ± R33 , + R1 3 ), (2.75a)
85 = atan2( ± JR132 + R 3 32 ,R2 3 )' (2.75b)

when R 1 32 + R 3 3 2 #- 0, and by
(J6 = atan2 ( + R2 2, ± R2 1 ) (2.75c)

84 arbitrary, (2.76a)
Os = 90°
=

- R23 X 90°, (2.76b)

86 atan2( - R31 , - R32 ) - 04R2 3 (2.76c)
when R 1 3 = R3 3 O. Therefore, there exist two values for (J1 , two values
=

82 , resulting in eight solutions {81 ,82 , ,86 } of the inverse kinematics

for 83, and two sets of values for {84,8s,fJ6 } for each combination of 81 and
• • .

problem. 0

2.5 Jacobian Matrix

2.5. 1 Translational and Rotational Velocity of Objects

In this section we will develop methods for expressing the velocity of an

orientation of an object in terms of the relation of the object frame 1:B
object moving in space. I n section 2. 1 we expressed the position and
(attached to the object) to the reference frame 1:A- Extending this idea, we
can express the velocity of an object in terms of the velocity of the object
frame with respect to the reference frame (that is, the translational velocity
and the rotational velocity of the object frame).
A representing the origin of
pB
The translation al velocity of the object frame can naturally be expressed
A A
1: B with respect to 1:A. This is den oted pB ; that is, pB d�B/dt, where t
by the time derivative of the pos ition vector
=

is the time variable. For expressing the rotational velocity of the o bject
frame, we can consider the following two methods:
54 Chapter 2

;,:=-----e> Y.

, x.

Figure 2.31
Angular velocity vector AWB•

Method I Select a vector AtPB consisting of three variables for expressing

its time derivative A ,j,B = d AtPBldt to express the rotational velocity of the
the orientation (e.g., Euler angles, or roll, pitch, and yaw angles), and use

object frame. (The superscript A and the subscript B of AtPB indicate that t/>
describes the orientation of :E B with respect to :EA ')
Method II Consider (figure 2.3 1 ) the third frame, :E B " which always has
the same origin as the reference frame :EA and which has the same orienta­
tion as the object frame :EB. Then the motion of :EB, with respect to :E at
A
each instant of time is a rotation about an axis passing through the origin.
This means that the rotational velocity of :EB, can be described by the vector
AWB, which has the same direction as the instantaneous axis of rotation and
a magnitude proportional to the rotational speed about this axis. The
vector AWB is called the angular velocity vector,

Since both of these methods will be used in this book, depending on the
situation, a brief description of the merits and demerits of each method
follows.
The integral of A,j,B in met hod I obviously corresponds to AtPB, which has
a clear physical meaning, whereas the integral of AwB in method II does not
have any clear meaning, as can be seen from the following example.

Example 2.15 Consider the following two cases. Case 1 has a time history
AWB given by
Kinematics 55

and case 2 has AWB given by

Aw.+H O�t� J

�. {n 1 <t � 2

Assume that :EA and :EB are coincident at t = o. The integral of AWB for both
cases is calculated to be

However, the value of rotation matrix ARB at time t = 2, which is denoted

as AbB, is

Ak. � � [ -!]
-1
1
0
0

[0 00
for case 1 and

AbB = 1
o 1
�]
for case 2. Thus, although the integrals of AWB are the same, the orientations
different orientations of an object at time t 2 corresponding to our cases
of :EB at time t 2 are quite different. Figure 2.32 illustrates the two
=

1 and 2, which started from the same orientation at time t o.

=

= 0

From the viewpoint of the physical meaning of the vector expressing the
velocity, AWB is superior to A,jJB. The three elements of AWB represent the
orthogonal angular velocity components about the X, Y, and Z axes of :EA
56 Chapter 2

z z

y y

1=0
X X
Z

Y Y

x x
z

-r---,t---c> Y Ik)�--<> Y
1=2

x
(a) (b)

Figure 2.32
Two motions with the same integral of AWB• (a) Case I. (b) Case 2.

as shown in figure 2. 3 3 a. In contrast, those of A�B generally represent

non-orthogonal components about the three axes of a skew coordinate
system whose coordinate axes vary depending on the present value of AtPB •
For example, when the Euler angles are used as AtPB , the relation between
A�B and AW B is given by

- sinifo
cosifo
cosifo Sino
sinifo sinO
] A�B ' (2.77)
o cosO

and we have the skew coordinate system shown in figure 2.33b.

sinO O.
The coefficient matrix on the right-hand side of equation 2.77 becomes
singular when = This means that although any rotational velocity

O. For example,
can be described by AWB, there are rotational velocities that cannot be

[cosifo, sinifo, OY
described by A�B when the orientation of 1:B satisfies sinO =

AWB = cannot be expressed by A�B ' Orientations of this

kind are called representational singularities of AtPB •
 57
Kinematics

/ _ - - - - : Angular velocity �ector

Z. '''i : correspondmg to rpH
z. I

,/\�\ J.,

I
\

j'---__-C> YA �'---Jc."..;-:;--C>, Y.
I ,' W!I
_ _ _ _ _ _ _ .6
Wx

(a) (b)

Figure 2.33
Two representations of angular velocity: (a) "wB; (b)
"J, B (case of Euler angles).

2.5.2 Definition of the Jacobian Matrix

=
ension al vector
= [� t ' �2 ' · · · , ek y and an I-dime
Suppo se that the follow ing relatio n holds betwe en a k-dim
C;
nsional vector " [f/ t , f/2 ' · · · , f/IY:

f/j = Jj(e 1 ' �2 '· . . , ed, j 1 , 2, . . . , I. (2.78)

Then the I x k matrix

=

af/ l af/ l a '1 1

a� l a�2 a �k

)
af/ 2 a'1 2 0 '1 2

Jq(C; = ac
a" (2.79 )
= a� l 0 �2 O �k

0'1 1 a '1 l 0'1 1

oe l 0�2 O et
�. Furth er, suppo se that
is called the Jacobian matrix8 of " with respect to
ing equat ion 2.78 with
� and " are funct ions of time. Then differentiat
s
respect to time and subst itutin g equa tion 2.79 yield
(2.80)

x is an exten sion of the

As is seen from equat ion 2.80, the Jacob ian matri
ble to the case of vecto rs.
deriv ative for a scala r funct ion of a scalar varia
Using the Jacob ian matri x, we can expre ss the relati on between the
manip ulato r in a comp act
end-effecto r veloc ity and the joint velocity of a
to time yields
form. Differentia ting equat ion 2.42 with respe ct
58 Chapter 2

l'

--GF��----� X

Figure 2.34
Three-link planar manipulator.

; = lr (q) q, (2.8 1)

where lr(q), the Jacobian matrix of r with respect to q, is given by

J,.(q)
or
= (2.82)
oq T '

The matrix lr(q) will henceforth be written as 1" for simplicity.

manipulator shown in figure 2.34. We let r [ x,y,(XY, where x and y

Example 2.1 6 Let us find the Jacobian matrix for the three-link planar

determine the position and (X determines the orientation of the endpoint.

=

Then

x = I I CI + 12 CI 2 + 13 C 1 2 3,
II SI 12S1 2 + 13 S1 23 ,
(}I (}2 (}3 '
Y = +

(X = + +

Differentiating these equations yields

x = - (lI Sl + 12 S1 2 + 13S1 2 3 )81 - (/2 S1 2 + 13S1 23 )82 - 13 S1 2 3 83 '

Y (11 C 1 + 1 2 C1 2 13 C1 23)81 (l 2 C1 2 + 13 C1 2 3)82 13 C1 2 3 83'
81 + 82 + 83 ,
= + + +

Ii =

Hence we obtain

- ( i 2 S1 2 + 13 S1 23)
(l2 C 1 2 + 13 C1 2 3)
1
Kinematics S9

When method I is used for expressing the rotational velocity of the end
effector, and when the vector r in equation 2.4 1 is defined by

[ I]JE J
r = R� , (2.83)

and R� are, respectively, the three­

I]JE
then the relation between the end-effector velocity and the joint velocity is
given by equation 2.8 1 . (Here
dimensional vectors expressing the position and orientation of the end­
effector frame LE with respect to the reference frame LR')
When method II is used to express the rotational velocity of the end
effector, the vector

(2.84)

expresses the total end-effector velocity. (Here RWE

is the angular velocity
vector of LE with respect to LR ') By defining a proper matrix Jv (q) which is
a function of q, we can obtain the following relation between " and q :

,, = Jv (q)q · (2.8 5)

Since the integral of RWE does not have any clear physical meaning, the

function .lj in equation 2.78. Because of the similarity of equations 2.85 and
coefficient matrix Jv (q) of equation 2.85 does not have any corresponding

2.8 1 , however, Jv (q) is also called the Jacobian matrix. The matrix Jv (q) will

When Euler angles are used as R� (see equation 2.77), the relation
also be written as Jv hereafter.

between J, and Jv is given by

Jv = T,. Jr (2.86a)

and

0 0 I 0 0 0
1 I
I 0 0 0
0 1 0 I

T =
I 0
0 0 I
0 0
- - - -- - �- - - - - - - - - - - - - - -
(2.86b)
I
,

0 0 0 I 0 - sintP costP sinO

I
I 0 costP sintP sinO
0 0 0 I
I 0 cosO
0 0 0

under the assumption that sinO #- O.

I
60 Chapter 2

two or three degrees of freedom even after fixing its position, Jr and Jv are
When a manipulator's end effector can make a rotational motion with

generally different. Ho wever, for a manipulator with fewer degrees of

can only rotate about a fixed a x i s , Jr and Jv are essentially the same. For
freedom whose end effector, after fixing its position, cannot move a t all or

example, in the case of the three-link manipulator in example 2. 16, if we

consider a Z axis such that a right-hand coordinate frame is formed, and
if we let

r = [x,y,O,O,O,ct]T,
then we have

[X,y,O,O,O,d]T.
Hence, Jv is a 6 x 3 m atri x obtained from Jr by just adding nine zero
i =

elements to it.
Using T, from equation 2.86b, we can write the relation between II and
i as II = T,i.

2.5.3 Link Velocities of a Manipulator

First of all, an important equation will be derived here for later use.
Consider two orthogonal coordinate frames � and �B ' and let AW B denote
A
the angular velocity of �B with respect to � . Then, for an arbitrary vector
B A
p expressed in � B ' the time derivative of ( ARB Bp) is given by

(2.87)

B B B B
By using the notations ARB = [ AX B AYB AZB ] and p = [ px' py , pz] T , we
can rewrite the second term in equation 2. 87 as

xB ) Px + dt ( AYB ) Py + / AZB) pz
d A B d A B d B d B
d t
( RB ) P =

d
/ d
= AWB X AXB p x + AWB X AYB py + AWB X AZB Bpz
B B

AWB X ( AXB Bpx + AYB Bpy + AZB Bp z)

AWB x ( ARB Bp) .
=

(2.88)

He re x denotes the vector product which is defined for any two vectors
=

Aa = [ax,ay,azY and Ab = [bx,by,bzY by

Kinematics 61

(2.89)

where

(2.90)

Consequently, from equations 2.87 and 2.88 we obtain

(2.9 1 )

This equation i s often used to derive relations among the link velocities
and the link accelerations of a manipulator.
Next we will establish a relation among relative velocities of three
moving frames, LA ' L B and Le. Take LA as the reference frame. As in figure
,

2.35, denote the vector from OA to OB expressed in LA as ApB, the vector

from 0A to Oe expressed in LA as Ape , and the vector from OB to Oe expressed
B
in L B as peB . Similarly, the angular velocities of L B and Le with respect to
LA are AWB and AWe, respectively, and the angular velocity of Le with respect
B
to LB is weB . Then the following relations hold:

(2.92)

(2.93)

Figure 2.35
Relative velocities among frames.
62 Chapter 2

Differentiating equation 2.92 with respect to time and substituting equation

2.9 1 yields

A' A ' + d ( AR B B )
Pc PB B PC
dt
=

A ' B + AR d ( B ) AOO ( ARB BPCB )' (2.94)

P B PCB + B X

where ApB d 'PB /d t . Thus, equation 2.93 is the relation between the rota­
d t
=

tional velocities of L B and L C with respect to LA ' and equation 2.94 is that
between the translational velocities.
Now we will use the equations obtained above to derive the relations
among the link velocities of a serial-link manipulator. We define the num­
bers of links and j oints as in subsection 2.3.2, and attach link frame Li to
each link as in subsection 2.3.3. As is shown in figure 2. 36, °P i denotes the

and i - 1Pi denotes the vector from 0i -1 to 0i, expressed in L i - 1 ' Note that
vector from the origin of L O , 00' to the origin of Li, 0i, expressed in L O ,

i 1P is given by
- i

translational velocities of link i with respect to the base as °OO i and °Pi =
from equation 2.45. Regarding velocities, we denote the rotational and

d( OpJ/dt, respectively. We further denote the upper left 3 x 3 submatrix of

Joint 1

Figure 2.36
Vectors 0p, and i - 1p,.
Kinematics 63

iTi as iR i because it is a rotation matrix. Then, by a correspondence of

:EA - :E o , :E B - :E i - 1 , and :EC - :E i between two sets of frames {:EA , :E B ,:E c}
and {:EO,:Ei ,:E;}, we obtain the following results:
-1

Revolute Joint If joint i is revolute, the angle ()i is the joint variable qi, and
i - 1Pi is a constant vector. Since i - 1W i. i_l ' the rotational velocity of :Ei with
respect to :Ei - 1 , is a vector in the direction of the Zi axis with magnitude 4i:

Hence, from equations 2.93 and 2.94 we obtain

(2.95)

(2.96)

where
[O,O, 1 Y. (2.9 7)

If joint i is prismatic, then the distance di is the joint

ez =

Prismatic Joint
variable q i ,

and
i -1Wi.i_l O.=

Thus we obtain
(2.98)

(2.99)

2.5.4 General Expression of the Jacobian Matrix Iv

Here we will derive a general expression of the Jacobian matrix Iv in

equation 2.85 based on the result of the previous subsection. Since the
end-effector frame :EE and the link frame :En are both fixed to link n, we
obtain the following expression of the end-effector velocity from equations
2.93 and 2.94 by considering the correspondence of :EA - :E o , :E B - :En, and
:EC - :EE:
(2. 100)

(2. 1 0 1)
64 Chapter 2

From equations 2.95-2. 1 0 1 and °wo Gp o 0, we can express OWE and OPE
= =

as linear functions of 4 1 , 4 2 ' . , 4n . From these linear functions we can

. .

obtain an expression for Iv ' For example, if all n joints are revolute, we
have from equations 2. 1 00, 2. 1 0 1 , 2.95, and 2.96

(2. 1 02)

(2. 1 03)

where npn + 1 = npE for notational convenience. Defining °Zi and °PE , j by

(2. 1 04)

and
n
° , _ " oR jA + oR i A
PE i j 'Pj +l - °PE . i+ 1 i 'Pi+ 1 , (2. 105)
_
L.
j=i
=

we obtain

°Z2 X °PE, 2 , . .
°
Zn X °P E , n ]
° 0 '
(2. 1 06)
Z2 . . •
Zn
As illustrated in figure 2.37, °Zi represents joint axis i, and °PE, i represents
the vector from a point on joint axis i to the end effector. Thus, the i th row
vector of matrix Iv implies a very natural result: that the translational
velocity of the end effector is given by ( OZi x °PE , ;)4i and the rotational

z"

Xll

Figure 2.37
Vectors 0:;, and oPE. ,.
Kinematics 65

velocity is given by °Z;tii' For more general manipulators with both revolute
and prismatic j oints, a similar argument leads to

(2 1 0 7)
.

where

l[ OZ O
i X PE , i
OZ
i
] if joint i is revolute,
(2. 1 08)

Jcl
�

["�'] ;f jo ; nt n, p,;,mat;c.

Hence, we can calculate Iv from 0 1; (i = 1 , 2, . . . , n + 1) by using the

following relations:

°P = O
"T,
E, i
+�-:�-:�-F�l
°
PE - Pi '
i
�
1, � . . ., n + 1, (2. 1 09)

(2. 1 10)

The first three elements of IV i can also be calculated from O( OPE)/Oqi'

Example 2.1 7 Let us find the Jacobian matrix for the PUMA-type ma­
nipulator in figure 2.23, assuming that the end-effector frame is assigned as
0
shown in figure 2.25. Using 1; obtained in example 2. 1 1 and nTE given in
example 2. 1 2, we can calculate the first three rows of Iv by o( °Ps)/oq T and
the remaining three rows by °Zi (i 1 , 2, , n). The result is as follows:
= . . .

- Py - R 2 3 1g
Px + R 1 3 19
o
o
o
1

C1 Pz + C1 R3 3 1g
Sl Pz + Sl R 3 3 1g
- lc C2 - le C2 3 + I,S2 3 + (C2 3 C2 SS + S2 3 Cs ) lg
- S1
C1
o
66 Chapter 2

- C1 (le S2 3 + lf C2 3) + C1 R 3 3 lg
- SI (/e S2 3 +lf C2 3) + SI R 3 3 l9
- le C2 3 + lfS2 3 + (C2 3 C4 SS + S2 3 Cs)lg
- SI
C1
o

(C1 C2 3 S4 SS - SI C4 Ss)lg
(SI C2 3 S4 SS - C1 C4 Ss)lg
- S2 3 S4 Ss / g
- C1 S2 3
- SI S2 3
- C2
3
- [C1 (C2 3 C4 CS - S2 3 SS ) + SI S4 Cs ] lg
- [SI (C2 3 C4 CS - S2 3 SS) + C1 S4 CS ] lg
(S2 3 C4 CS + C2 3 Ss)lg
C 1 C2 3 S4 - S I C4
SI C2 3 S4 + C1 C4
- S2 3 S4

0
0
0
. 0
R13
R2 3
R33

2.5.5 Joint Velocity for Achieving Desired End-Effector Velocity

We now know how to calculate the position of the end effector, r, for a
given j oint position q, and how to find the q that achieves a given r. We
also know that the end-effector velocity " for a given joint velocity q can
be calculated by equation 2.85. In this subsection we will consider the
problem of finding the joint velocity, q, that results in a given end-effector
velocity " (or r). This can be regarded as a problem of inverse kinematics
in a broader sense.
First, consider the case where n 6 and Jv is nonsingular. Then from
=

equation 2. 8 5 we have
Kinematics 67

q = IV - 1 V. (2. 1 1 1 )

Hence, in principle we can obtain q by calculating the inverse of Iv and

multiplying it by v. In practice, however, it requires less computation to
solve the algebraic equation 2.85 directly by some numerical algorithm
such as Gauss' elimination method. When an analytical solution to the
inverse kinematics problem is available, as in example 2. 14, it is also
possible to reduce the amount of computation by using the equation

We next consider the case when n � 7 and the rank of Iv 6. Then, using
derived from the differentiation of the analytical solution (ref. 5).
=

equation A2.22 in appendix 2, the general solution of equation 2.85 is given

by

q 1/ v (I 1/ Iv)k, (2. 1 1 2)
where Iv + is the pseudo-inverse of Iv and where k is an arbitrary n-dimen­
= + -

sional constant vector. This k implies that there are an infinite number of
solutions to equation 2.85. It also implies that the manipulator has some

end effector. However, once the value of k is given somehow, we can

redundancy in its configuration if the task is only to position and orient its

calculate q from equation 2. 1 12. Exploiting redundancy will be discussed

in detail in chapter 7.
We can also obtain the joint velocity q that realizes the given end-effector

obtain instead of equation 2. 1 1 1

velocity r by using J, instead of Iv ' For example, if I, is nonsingular, we

(2. 1 1 3)
We should be careful, however, about the fact that when the manipulator

cannot use equation 2. 1 1 3 because J, is singular.

is at the representational singularity described in subsection 2.5. 1, we

2.5.6 Singular Configurations

In section 1 . 1 it was pointed out that even for wrist mechanisms with three
degrees of freedom, there are configurations for which the end effector
cannot be rotated about a certain axis. Those configurations were called
singular configurations. Such situations are not limited to the end-effector
orientations; they also exist for end-effector positions. For example, con­

given by A in the figure, which is different from B (82 = 0°) and C (e2 =
sider the two-link manipulator shown in figure 2.38. If its configuration is
68 Chapter 2

Figure 2.38
Singular configurations of two-link manipulator.

1 80° ), then any desired velocity of the endpoint in the X Y plane is

-

realizable by a finite j oint velocity. However, at configurations B and C a

velocity that has a nonzero component in the direction from the origin 0
to the endpoint cannot be realized by any finite joint velocity. Generalizing
this situation, we define the singular configurations of a general n-link
manipulator as those for which end-effector velocities (translational veloci­
ties, rotational velocities, or their combinations) in a certain direction
cannot be realized by any finite joint velocity.
We will now define the above concept of singular configurations more
mathematically, using the Jacobian matrix. Let us consider an n-link ma­
nipulator and denote its Jacobian matrix as Iv' Let an integer n' be given by

n' = max ranklv(q). (2. 1 1 4)

q

If a configuration q = qs satisfies

ranklv(q) < n', (2. 1 1 5)

then we define this qs as the singular configuration. Equation 2. 1 1 4 means

that the manipulator under consideration has the ability to produce any
end-effector velocity vector in some n' -dimensional space at any configura­
tion other than an exceptional configuration. Equation 2. 1 1 5 implies that
we call these exceptional configurations singular. Note that if n > 6 we have
n' 6, and that if n � 6 we have n '
= n, unless the mechanism is very
=
Kinematics 69

special. Also note that if n' = n 6, from equa tion 2. 1 1 5 a necessary and
=

sufficient condition for q to be a s ingular c o nfigura tion i s

(2. 1 1 6)

where detJ denotes the determinant of a matrix J. Therefo re , q canno t be

calculated by equation 2. 1 1 1 or 2. 1 1 2 at a singular configuration. More­

if it is close to one, q calculated from e quati on 2. 1 1 1 or 2. 1 1 2 may become

over, even when the configuration is not right at a singular configuration,

so excessively large that this q c annot be achieved.

We can determine the singular configurations from Jr instead of Jv ' Let
n' be given by

n' = max r an kJ, (q). (2. 1 1 7)

q

Then the singular configurations always satisfy

ran k Jr (q) < n '. (2. 1 1 8)

real singul ar confi gu ra tions but also representational ones, we need to

However, since the set of q satisfying inequality 2. 1 1 8 includes not only the

check whether a singula r c onfiguratio n is a real one or not . When n = n' �

6 and Jr is square, a necessary condition for a q to be s ingul ar is given by

detJ, (q) = o. (2. 1 19)

Example 2.18 We will now check that the singular configurations of the

r = [X,yJ T and q = [81 , 82 Y; then the Jacobian Jr is

two-link manipulator in figure 2.38 are given by B and C in the figure. Let

Thus we have

detJr

Since there is obviously n o repre sentational singularity, we can conclude

= 1 1 /2 S2 •

from equation 2. 1 1 9 th a t the singular configurations are given by (}2 = 0°

or 82 = 1 80°. 0

For the two-link manipulator in e x am ple 2. 1 8, the hatched region in

figure 2.38 represents the zone of points which the m ani p ula t or end effector
70 Chapter 2

can reach in the X - Y plane. This is called the reachable region. In the case

reach a b le region is also of dimension m. Although the singular configura­

of a general manipulator with an m -d i mensional end-effector vector, the

tions in the above example are all on the boundary of the reachable region,
this is not always true. Singular configurations can also exist inside the
reachable region, as ex a mple 2. 1 9 shows.

m a nipul a tors in figure 2.23 w i th Ib Id Ie 0) , there are three kinds of

Example 2.19 For PUMA-type mani p ul a to rs with no offsets (i.e., for
= = =

singular configurations, shown in figure 2.39. Th o se shown in diagram a

are called shoulder singularities (the wrist position is righ t above the shoul­
der); those in diagram b are called elbow singularities (the elbow is stretched
straight); those in diagram c are called wrist singularities (the wrist is
stretched straight). 9 The singularities of diagram b are on the boundary of
the reachable region, but those of diagrams a and c can be inside the
reachable region. 0

There are two approache s for coping with the performance dete rio rati on
due to si ngul a r configurations. One is s i m p l y to avoid using the singular
con fi gura tions and their neighboring region by planning the gi ven task
carefully. Another is to change the mechanism design. We can, for example,
move the singular configurations to an u nim po r tant area of the reachable
region by some s o ph i sti cated mechanism, or we can add some extra degrees
of freedom to the manipulator so that the singular configurations can be
avoided by means of the added redundan cy . t o

(b) (el

Figure 2.39
Singular configurations. (a) Shoulder singularities. (b) Elbow singularities. (c) Wrist
singularities.
Kinematics 71

2.6 Statics and Jacobian Matrices

2.6.1 Equivalent Forces Represented in Different Frames

Suppose that t w o coordinate frames, k B and k e , are fixed t o a n object a s

shown in figure 2.40, and that the object is moving relative to a reference
frame, kA ' From equations 2.93 and 2.94, the relation between the velocities
of kB and ke, relative to kA and exp ressed in kA ' is given by

(2. 1 20)
ApB AWB A B
APe = + X ( RB peB ). (2. 1 2 1 )

Putting the above equations together we have ,

(2. 1 22)

where the notation [. x ] denotes for an arbitrary three-dimensional vector

,

a [ax,ay,az] T,

[ �z
=

[a x ]
- az
= o (2. 123)
-
ay

Note that, from the above definition, the following two equaliti es hold for

Figure 2.40
Frames !:B and !:c fixed to an object.
72 Chapter 2

two arbitrary three-dimensional vectors, a and b :

[a x ] b = a x b, (2. 1 24)

[a x y = - [a x ] . (2. 1 25)
B
Also note that, when we denote the vectors a and b expressed in 1:B as a
B
and b, respectively, we have from equation 2.90
B ARB ( Ba x Bb).
( ARB a) x ( ARB Bb) =

c
Now we denote the vectors ApC and Awc expressed in 1:B as PCA and
c B B
WCA , res pectively and define pBA and WBA similarly. Then, using the
,

rotation matrices ARB and ARc, we find

Thus we have

(2. 1 26)

where

(2. 1 27)

The matrix JCB is the Jacobian matrix relating the velocities of 1:B and 1:c
with respect to 1:A • This JCB is determined from the homogeneous transform
B B B
Tc (that is, Rc and pCB ) relating 1:B and 1:c, and it is independent of 1:A -
On the other hand, let AJc and Anc denote the force and moment acting
on the o rigin of 1:c, which are equivalent to the force AJB and moment AnB
acting on the origin of 1:B• Then

(2. 1 28)

(2. 1 29)

From these equations we obtain the following equations in a way similar

to the way we obtained equation 2. 1 26:

(2. 1 30)
Kinematics 73

Figure 2.41
Object frame I:B and force sensor frame I:e.

From equations 2. 1 30 and 2. 1 27, we finally o btain

(2. 1 3 1 )

Note the fact (observed from equations 2. 1 26 and 2. 1 3 1) that t he Jacobian

matrix JCB relates t he velocities of I:B and I:c, whereas the transpose of JCB
relates the forces and moments of I:c and I:B.

Example 2.20 Suppose that a hand is grasping an object as shown in

figure 2.4 1 , and that we can use the wrist force sensor to measure the force
and moment occurring at the wrist expressed in the sensor frame I:c . We
want to know the force and moment exerted on the tip of the object
expressed in the object frame I: B fixed to the tip from the sensor output.
We assume that the hand is not moving, and that the relative position of
I:c with respect to I:B is known. We also neglect the gravity force. This
t
calculate JCB T by using the relation
problem can be solved by the following procedure: We fi rs obtain BTc and

'T
e � [���'����+���1
tip of the object by substituting the sensor out p ut (Cfc and end into
B
Next we determine the force (BfB ) and the mom ent ( nB) exerted on the

eq u ation 2. 1 3 1 . For example, when the sensor output is given by cfc =

[30, - 1 0, - soy (in newtons) and nc [0.5,0.2,0] T (in newton-meters),
c
=

B
then Tc is given by
74 Chapter 2

B T.
1 /v'2 1 /J2 - 0 025 v'2 l
r
0
0 1 0
e-
_

- 1�J2 0
0
1 /J2
0
- 0.025 f - 0.06
.

BT
Hence,

Je
=

1 /J2 0 1 /J2 0 0 0
0 1 0 0 0 0
- 1 /J2 0 1 /J2 0 0 0
0 0.025 J2 + 0.06 0 1 /J2 0 1 /J2
- 0.05 - 0.03 J2 0 - 0.03 J2 0 1 0
0 - O.025 J2 0 - 1 /J2 0 1 /J2

Therefore, we find

BJB = [ - 1O/J2, - 1 0, - 40 J2Y (N),

B
n B = [ - 0.6, - 1 . 3 + 0.6 J2, Oy (Nm). 0

2.6.2 Joint Driving Force Equivalent to Force Applied to Tip of

Manipulator

driving force, T = [r 1 " 2, . , 'nY, which is equivalent to the force ryE and
For an n-link manipulator, let us consider the problem of finding the joint
. .

moment OnE applied to the origin of the end-effector frame LE. Here ' i is
the joint driving force exerted at j oint i between links i - 1 and i. When
joint i is revolute, Ti is a torque about the Zi axis; when joint i is prismatic,
' i is a force along the Zi axis.

By the correspondences LA ....... L o , LB ....... Li' and Le ....... LE, we obtain from
equations 2. 128 and 2. 129

(2.132)
(2.133)
The relation between 'i and the pair { <>.t;, onJ is
Kinematics 75

60 mm

Zo

Figure 2.42
Relation between rB and re.

°z/ OnE °ZiT ( OPE, i OfE)

X

°z/ OnE + ( OZi

= +

= °PE.J T OJE
X
(2. 1 34)

for revolute joints and

(2. 1 3 5)

for prismatic j oints. Hence, using Iv given by eq ua tion 2.107, we have

T = I/ [ Z: l (2 . 1 36 )

The m a i n results in this section, described by equations 2. 1 3 1 and 2. 1 36,

can also be derived using the principle of virtual work (see reference 1 ). For
example, equa ti on 2. 1 36 can be derived as follows: From eq u a tion 2.85, the
virtual displacement of the end effector, lJd, expressed by Lo corresponding
to \I and the virtual displacement of the j oints, lJq, satisfies
(2. 1 37)

On t he other hand, from the pri nciple of virtual work we have

(lJdf
(bq) T T =
[Z:J . (2. 1 38)

Hence, from equations 2. 1 3 7 and 2. 1 38 we obtain

76 Chapter 2

=
� J/[Z:l (2. 1 39)

Although we have discussed only positions and velocities in this chapter,

a similar argument can also be developed for accelerations of objects and
manipulator links. For example, by differentiating equation 2.8 1 we get the
following relation between the acceleration of the manipulator joint, ij, and
the acceleration of the end effector, r:

r = J,. (q) ij + i, (q)q. (2. 1 40)

When the end-effector velocity is expressed by v, by differentiating equation

2.85 we obtain

Ii = Jp (q)ij + ip(q) q. (2. 1 4 1)

Relations among the accelerations of manipulator links can also be derived

by arguments similar to those in subsection 2.5.3. This will be done in
subsection 3.4.2.

Exercises

2. 1 Derive the roll, pitch, and yaw angles equivalent to a given rotation
matrix (equation 2.22) in a form similar to equations 2.32 and 2.33.

2.2 Show that the Euler angles for a given rotation matrix (equation 2.22)
can be expressed as

rfJ = atan2( ± R 2 3 , ± R 1 3 ),

() = atan2(cosrfJR 1 3 + sinrfJR 2 3 , R 3 3 ),

IjJ = atan2( - sinrfJR l l + COSrfJR2 1 , - sinrfJ R 1 2 + cosrfJR 2 2 )·

2.3 The tetrahedron in figure 2.43 has an object frame �B attached to its
square corner. Give the position and orientation of the tetrahedron with
respect to the reference frame �A shown in figure 2.44. Use the rotation
matrix to describe the orientation.

of roll, pitch, and yaw angles to describe the orientation.

2.4 Solve exercise 2.3 using both the Euler-angles method and the method

2.5 Give the position and orientation of the tetrahedron in figure 2.43
when it is placed in �A as shown in figure 2.45. Use Euler angles to describe
Kinematics 77

Z.

",.--- Y.

x.

Figure 2.43
Object frame 1:8-

Z,

Z.

Figure 2.44
Object frame 1:8 and reference frame 1:,._

ZA

�
--�---::
O,�__--,13
�3�_-<> y,

: '0' x.
x,
Y. I

Z.

Figure 2.45
Expression of position and orienta tion of object. (Z8 and Z,. are parallel.)
78 Chapter 2

the orientation. Explain the cases when there are infinitely many Euler­
angle expressions of o rientation.

2.6 A frame I:B, which was initially coincident with another frame I:A , is

axis and 2 units along the ZA axis. Give the relation between the description
rotated about the YA axis by 30° and then translated 3 units along t he XA

Ar of an arbitrary point in I: and the description Br of the same point in

A
LB by a homogeneous transform.

tor Ar by a four-dimensional vector [ Ar T, I ] T, introduced in subsection 2.2. 1 ,

2.7 Generalizing the idea of describing a three-dimensional position vec­

let us regard the four-dimensional vector [x,y,z, kF as representing the

l a °l
three-dimensional position [xlk,ylk, zlk] T. Under this convention, study
the physical meanings of the following two homogeneous transforms:

b O O
°

c O '
0

al�, �l
°
Ts =

00 00 0

b, and c are positive constants, and

1

where

T =
� �
p 0 1 0 .
° - 0I lf ° 1

( Ts is called the stretching transform, and Tp is called the perspective trans­

form - see reference 5.)

I
2.8 Suppose

[� l
-1 o
A TB 0 o 2
�
0 1 3
0 o 1
and

[� �l
0
01
BT
C �
0 01 2 .
1
0 0
Kinematics 79

Give two different interpretations of the product A TB B Te, using a figure like
figure 2. 1 1 .

2.9 Assign link frames to the SCARA-type manipulator in figure 1 . 1 6 and

obtain ° T4.

2. 10 Assign link frames to the manipulator in figure 1.12 and obtain ° T6 .

2.1 1 Solve the inverse kinematics problem for the SCARA-type manipu­
lator in figure 1 . 1 6 on the basis of the result of exercise 2.9. Assume that
the end-effector position vector r is given by the four-dimensional vector
consisting of the three-dimensional position and the angle about the vertical
axi s of the wrist joint.

2.1 2 Solve the inverse kinematics problem for the manipulator in figure
1.12 on the basis of the result of exercise 2. 10.

2.1 3 Sketch figures of the eight solutions obtained in example 2. 1 4 for the
end-effector position shown in figure 2.23b.

2.20 fo r the case when the end-effector frame i � assigned as in figure 2.46.
2.14 Derive the Jacobian matrix Jv of the Stanford manipulator in figure

2.1 5 Prove that the arm configurations given i n figure 2.39 are singular
ones.

Figure 2.46
End-elTector frame rE o
80 Chapter 2

2.16 Assume that the PUMA-type manipulator is in the configuration of

figure 2.24. Calculate the joint driving force equivalent to the following
force and moment in the end-effector frame !:E shown in figure 2.25.

% [O,O, lOOF (N),

[lO,O,OF (Nm).
=

onE =

2. 1 7 For any rotation matrix A.RB = [ "X B , "YB , "ZB ] we have

( "XB) T "xB = 1,
T
( "Y B ) "YB = 1, ( "zB f "ZB =
1, "XB T X "YB = "ZB '
Conversely, for any three vectors "XB ' "YB ' and "ZB satisfying the above
equations, the matrix [ "X B ' AyB , AZB ] is a rotation matrix. Show that equa­
tion 2.2 can be derived from the above equations. Explain why the above
equations cannot be derived from equation 2.2. Hint: For any vectors A a,
A b, and A C, we have the following equalities:
Ac T (Aa x Ab) =
A.a T ( Ab x AC) ,

AC x ( Aa x Ab) =
(AC T "b) "a _ ( AC T Aa) Ab.

References

1. H. Goldstein, Classical Mechanics (Addi s on - Wes le y, \ 9 50).

2. J. Dena vi t and R. S. H arten be rg, "A Ki nema tic Notation for Lower-Pair Mechanisms
Based on Matrices," ASME Journal of Applied Mechanics 77 ( 1 9 5 5): 2 1 5-22 1 .
3 . V . D . Scheinman, De s ign of a Co mp uter Controlled M anipulator, memo A I M 92, Stanford
Artificial Int el l i gen ce Laboratory, 1 969.

4. 1. J. Craig, Introduction to Robotics (Addison-Wesley, 1 986).

5. R. P. Paul, Robot Manipulators ( M IT Press, 1 98 1 ).

6. L. W. Tsai and A. P. Mo rga n , "S olving the Kinematics of the Most General Six- and
Five-Degree-of-Freedom Manipulators by Continuation Methods," ASME Journal of
Mechanisms, Transmission, and A utomation in Design 1 07 ( \ 985): 1 89-200.
7. D. L. Pieper, The Kinematics of Manipulators Under Computer Control, me mo AIM 72,
Stanford Art i fici al Inte lli gence Laboratory, 1 968.

8. K. Ito (ed.), Encyclopedic Dictionary of Mathematics (MIT P ress, 1 986).

9. M . Uch i yama, "A Study of Com pu ter Control of a Mechanical Arm ( 1 st Report : Calcula­
tion of Coordinative Motion Conside ri ng Singular Po i n t s)," Bulletin of the Japan Society of
Mechanical Engineers 22 ( 1 979): 1 640- 1 647.
10. J. M . H o llerbac h , "Optimum Kinematic Desi gn for a Seven Degree of Freedom Manipu­
lator," in Robotics Research : The Second International Symposium, ed. H. Hanafusa and H.
Inoue (MIT Pre ss, 1 985), pp. 2 1 5-222.
3 Dynamics

This chapter presents the fundamentals for analyzing the dynamic charac­
teristics of manipulators. First, we will briefly outline the difference between
the Lagrangian formulation and the Newton-Euler formulation for deriv­
ing the dynamics equations of robot manipulators. We then will describe
in detail the derivation procedures of the dynamics equations using both
formulations, and discuss the use of these equations and the required
amount of computation. Finally, we will develop an identification method
for the inertial parameters of manipulators.

3.1 Lagrangian and Newton-Euler Formulations

It is necessary to analyze the dynamic characteristics of a manipulator in

order to control it, to simulate it, and to evaluate its performance. A

be a good structure from the viewpoint of dynamics (it is usually not very
manipulator is most often an open-loop link mechanism, which may not

rigid, its positioning accuracy is poor, and there is dynamic coupling among
its joint motions). This structure, however, allows us to derive a set of
simple, easily understandable equations of motion.
Two methods for obtaining the equations of motion are well known: the
Lagrangian and the Newton-Euler formulations. At first the Lagrangian
formulation was adopted. 1. 2 This approach has a drawback in that the
derivation procedure is not easy to understand physically; it uses the
concept of the Lagrangian, which is related to kinetic energy. However, the
resulting equation of motion is in a simple, easily understandable form and
is suitable for examining the effects of various parameters on the motion
of the manipulator. For this reason, this approach has been the standard
one since the 1 970s.
Recently, as the need for more rapid and accurate operation of manipula­
tors has increased, the need for real-time computation of the dynamics
equations has been felt more strongly.3 As will be shown later in this
chapter, the Newton-Euler formulation has been found to be superior
to the Lagrangian formulation for the purpose of fast calculation.4-6
Also, the Newton-Euler formulation is valid for computer simulation of
manipulators.7
82 Chapter 3

3.2 Some Basics of Kinetics

3.2.1 Newton's Equation and Euler's Equation

A rigid body with no constraints has six degrees of positional freedom.

Thus, a set of six independent equations can describe its motion. It is well
known that these equations are given by Newton's and Euler's equations
of motion.
Let us consider a rigid body in an inertial coordinate frame :Eu(Ou -
Xu YuZu), as shown in figure 3.1. We assume that the rigid body has linear
momentum D and angular momentum E about its center of mass G. The
total external force acting on it is F, and the external moment about G is
N. We also assume that all these vectors are expressed in :Eu, but we will
omit the leading superscript U when no confusion occurs. From Newton's
second law of motion, we have

F=dD (3.1)
dt '

=dE
N (3.2)
dt ·

If we further assume that the mass of the body is constant, m, and the
position vector of G is given by s, we have D = ms. Hence, equation 3.1
reduces to the following form, known as Newton's equation:
F=ms. (3.3)

Note that Newton's equation is valid only when an inertial reference frame
is used.

XII

Figure 3.1
Rigid body and inertial coordinate frame.
Dynamics 83

On the other hand, to find the angular momentum E, let m denote the
angular velocity vector, dv the volume of an infinitesimal particle of the
rigid body, p its density, r its position vector from G, and r dr/dt its
=

velocity vector. Since the angular momentum of dv is r x (rpdv) and the

relation r = m x r holds, we have

E = Iv r x (m x r)pdv

= Iv [(rTr)m - (rTm)r]pdv

= Iv [rTrl3 - rrT]pdvm, (3.4)

where Iv denotes the integral over the whole rigid body and 13 denotes the
3 x 3 identity matrix. If we define

I= Iv [rTrl3 - rrTJpdv, (3.5)

then E = 1m and equation 3.2 can be rewritten as

d
N = t (1m). (3.6)
d
This I is called the inertia tensor.
For any given orthogonal frame LA(OA - XA YAZA), let the XA, YA, and

[
ZA components of Ar be Arx, Ary, and Arz, respectively. Then the inertia
tensor AI with respect to LA is given by
AI
�
AIX AI,,
Y
AI= AIxy AIy AIyz
y
AIxz AIyz AI, ,
] (3.7)

.
where

AIxx = i V
(Ary 2 + Arz2)pdv,

AI yy =1 V
(Arz2 + Ar:x2)pdv, (3 . 8a)
84 Chapter 3

AIxz = 1v (Arx2+Ar)'2)pdv,

-
AIxy= - AHxy- - rv Ar" Aryp dv,
J
(3.Sb)

-
AIxz - - AHx. - - rv Arx Ar.pd V.
J
Here AIxx, AIyy, and AI•• are called the moments of inertia and AH"y, AHyZ'
and AHxz are called the products of inertia. Hence, I given by equation 3.5
is the inertia tensor with respect to frame :EUG(G - XUG YUGZUG) with its
origin at the center of mass G and with its coordinate axes parallel to those
of :Eu.
When we consider the rotational motion of a rigid body on the basis of
equation 3.6, the analysis and the computation are not very transparent.
This is because the elements of I change with time as the orientation of the
rigid body relative to :EUG changes with time. To avoid this difficulty,
we express equation 3.6 with respect to the object frame :EB(G - XB YBZB )
fixed to the rigid body as shown in figure 3.2. Denoting E, N, I, and ro
B
expressed with respect to:EB as BE, BN, BI, and ro, respectively, we have

E = uR BE, (3.9)
B
u
N= R BN, (3.10)
B

Zue
XB

Zu
XUG

0�
) Yu

Xu

and :EUG•
Figure 3.2
Frames :E 8
Dynamics 85

(3.1 1)

(3.12)

Hence we obtain

BE = BI Bw. (3. 1 3)

In this case BI is a constant determined solely by the rigid body. Now we

Ba and Bb:
note that, from equation 2.90, the following relation generally holds for any

( U R Ba)
B
x (URB Bb) = U R ( Ba
B
x Bb). (3. 14)

Using this and equations 3.9, 3.1 3, and 2.9 1 , we get

(3. 1 5)

Hence, from equations 3.6, 3.10, 3.14, and 3.1 5 , we obtain

(3.16)

This is Euler's equation of motion expressed in �B' This equation can also
be rewritten in �u. From equations 2.9 1 and 3. 1 2, the time derivative of w
IS

(3. 1 7)

Applying equation 3. 1 7 to equation 3. 1 6, we obtain

N = J-(w) + w
d
dt
x (Iw). (3.18)

This is Euler's equation of motion expressed in � u,

86 Chapter 3

If we take � such that the coordinate axes coincide with the principal
B

=
axes of the moment of inertia of the rigid body, then Blbecomes a diagonal
matrix diagU�.Iy,I:]' Introducing the notations BN [BNx• BNy• BN:Y and
Ben [Bwx• BWy• BW:Y, we obtain from equation 3. 1 6 the following well­
=

known expression for the motion of a top:

BNx = Ix Bwx - (ly - I:)Bwy BW.,

BNy = Iy BWy - (I: - Ix)Bw: Bwx, (3. 1 9)

BN: = I: BW: - (Ix - Iy)Bwx B Wy .

3.2.2 Lagrange's Equation

We have used orthogonal coordinate variables so far to describe the

position of objects. One of the merits of Lagrange's equation of motion is

as they can uniquely specify the positions of objects. These variables are
that any variables can be used instead of orthogonal coordinates as long

called generalized coordinates. Although Lagrange's equation of motion is

usually derived from Hamilton's principle, we will derive it from Newton's
equation of motion because that is intuitively easier.
Suppose that a system of particles with n degrees of freedom can be
described by the generalized coordinates Ql,q2, ...• qn. The three-dimen­
sional position vector xI' for an arbitrary particle PI' of the system in an
inertial frame �u is

(3.20)

With the mass of particle PI' denoted as ml' and the force working on PI' as
1;" Newton's equation of motion yields

(3.2 1 )

Taking the inner product o f OXI'/OQi with equation 3.2 1 and summing for
all of the particles in the system, we have

�
L..
I'
FI' T -0
OXI'
Qi
=
�

I'
_

0 '
TOXI'
£.., ml'xl' -
qi
i = 1 , 2, ... , n. (3.22)

From equation 3.20 we have

(3.23)
Dynamics 87

(3.24)

Hence, equation 3.22 can be rewritten as

(3.25)

where

m
K ="
i.J 2i" Ti" (3.26)
" 2
and

(3.27)

K is called the kinetic energy of the system, and Qi is called the generalized
force corresponding to qi'
We further divide the force 1;, into two parts: 1;,a (the force due to gravity)
and F"b (the remainder). The gravity force, 1;,a, can be expressed by a
suitable potential energy P as

oP
pa - ox,,'
F = (3.28)

Introducing the Lagrangian function L = K - P and using equations 3.25,

3.27, and 3.28, we finally obtain

(3.29)

where

(3.30)

Equation 3.29 is called Lagrange's equation of motion.

We usually write a kinetic-energy expression K and a potential-energy
expression P when determining an explicit equation of motion. Thus, the
following alternative form is more convenient than equation 3.29:

(3.29')
88 Chapter 3

Note that we can derive equations 3.29 and 3.29' also for a system of rigid
bodies with n degrees of freedom just by regarding the rigid bodies as a

kinetic energy K of a rigid body can be expressed as

collection of many small particles with infinitesimal mass. In this case, the

K = tiiisTs + twT/W, (3.31)

where s is the translational velocity ofthe center of mass G, w is the angular
velocity, iii is the mass, and / is the inertia tensor of the rigid body.
Readers who wish to know more about the foundations of kinetics
should see references 8-10.

3.3 Derivation of Dynamics Equations Based on Lagrangian

Formulation

3.3.1 Two-Link Manipulator

of a simple two lin k manipul ato r moving in the X Y plane. Let us consider
Before treating general manipulators, we will derive the dynamics equation

the manipulator in figure 3.3. The following notations are used in the figure:
- -

OJ the joint angle of joint i,

mj the mass of link i,
� the moment of inertia of link i about the axis that passes th rough the
center of mass and is parallel to the Z axis
Ii the length of link i,
,

�---x
----<>

Figure 3.3
Two-link manipulator.
Dynamics 89

Igi the distance between joint i and the center of mass of link i (the center
of mass is assumed to be on the straight line connecting the two joints ).

link 1, and the second joint driving torque tl acts between links 1 and 2.
We assume that the first joint driving torque tl acts between the base and

We will also assume that the gravitational force acts in the negative Y
direction.
Choosing ql 01 and ql = =
0l as generalized coordinates, we will find
the Lagrangian function. Let the kinetic energy and the potential energy
for link i be Ki and Pi' respectively. For link 1, we have

Kl = I 2'2
"lml lg1 01 +
1-'2
"lIIOI , (3.32)

PI = mdJlgl SI' (3.33)

=
where g is the magnitude of gravitational acceleration. For link 2, since the
position of its center of mass Sl [Slx,S2yY is given by

Slx = IICI + Ig2C12' (3.34a)

Sly = llSI + 192512, (3.34b)

we have the relation

(3.35)
Hence,

K2 = "lI m2s2 T·S2 + .1

• 2 1-2(0·1 + 0·2)
2 (3.36)

and

(3.37)
Calculating L KI + K2 Pl = P2 and substituting this into equation
- -

3.29 yields the equations of motion for the two-link manipulator:

tl = [m11g1
2
+ 71 + m 2(l12 + Ig/ + 2/1/g2C2) + 72]81

+ [m2(lg/ + 11 Ig2C2) + 72J82 - m2/1/g2S2(20/J2 + 0/)

+ mlglg1 C1 + m2g(/l CI + [92C12), (3.38a)

(3.38b)
90 Chapter 3

This can also be rewritten as

(3.39a)
00 . .
2
+ (3.39b)
•
'2 = M2181 + M2282 + h21181 g2'

where

(3.40a)

(3.40b)

M22 = 2 -
m21g2 + 12, (3.4Oc)

hl22 = hl12 -h211 = -m211Ig2S2, (3.4 1 )

=
=

g) m1blgl C1 + m2{J(l1 C1 + Ig2C12)' (3.42a)

g2 = m2blg2C120 (3.42b)

We call Mii the effective inertia, Mij the coupling inertia, hijj the centrifugal
acceleration coefficient, and hijk (j #- k) the Corio lis acceleration coefficient.
The term gi represents the gravity force.

that the kinetic energy K K 1 + K 2 may be expressed in a quadratic form

We can further rewrite equation 3.39 in a more compact form by noting
=

as

K = tOTM(8)0, (3.43)

where 0 = [el ,e2y and M(8) is the following positive-definite matrix:

(3.44)

Using equations 3.43, 3.44, and 3.29, the equations of motion 3.39 are also
given by

T = M(8)O + h(8,0) + g(8), (3.45)

where, with col[· ] denoting a column vector,

(3.46)

g(8) = col[gJ, (3.47)

and where M(8)O is the inertial force term, h(8,O) is the centrifugal and
Dynamics 91

Coriolis force term, and g(O) is the gravity force term. M(O) is called the
inertia matrix in joint coordinates.

3.3.2 n-Link Manipulator

We will n ow find the e quat i on s of motion of a general n-link manipulator,

using the homogeneous transforms among the link frames fixed to each
link (see reference 3). For simplicity, we assume that the base of the
manipulator is fixed to an inertial frame 1:u, and we regard 1:0 as the

w o rk s between links i-I and i, and that its value is positive when it works
reference frame. We also assume that the joint driving force ' ; at joint i

in the direction that makes the joint variable larger. This kind of driving
mechanism is called a serial drive.

i (see figure 2. 17). One characteristic of 1:; is that its origin is on an axis of
In subsection 2.3.3 we introduced the frame 1:;(0; - Xi liZ;) fixed to link

joint i, which is closer to the base than the other joint (joint i + 1 ) of lin k
i. When joint i is revolute, the axis of rotation coincides with the Z; axis;
when j o in t i is prismatic, the direction of translation coincides with that of
the Zi axis. We also introduced the homogeneous transform i-1 T; relating
1:; to 1:i-1• Hence, the homogeneous transform ° Ti given by

(3.48 )
is the one relating 1:i to the reference frame 1:0' Note that i-1 Ti is a function
of qi ' the joint variable at joint i.
Suppose that we are given an arbitrary point on link i which is expressed
as ir with respect to 1:i (see fi g ure 3.4). The position of the same point with
respect to the reference frame 1:0 is given by

(3.49)

Since ir relative to 1:i is constant, d ;r/dt = O. Thus,

d or
- = °r
.
= (�iJ )
L.
° Ti
--
j=1 iJqj
qj
. i
r. (3.50)

=
dt
O tr[O,O,T], we have
Using equation 3.50 and the relation O,T ,

O,TO, =
i

L L
i ( tr
iJOT
__ 'irirT
iJ
__
OTT
'_
) 4Ak' (3.51)
j=1 k=1 oqj iJqk
where tr( . ) denotes the trace of a matrix.
With these preparations, we can now calculate the kinetic energy Ki and
the potential energy Pi of link i. The kinetic energy of link i is found by
92 Chapter 3

Link i

Link i + 1

�-------<> y"

2"

x"

Figure 3.4
Point ir on link i.

integrating the kinetic energy of the differential element over the whole
body of link i. Denoting the kinetic energy of the differential element as
dKi, its volume as dm, its density as p, and its position as ir, we have

Ki= r d Ki= r tOfTOfp d v

Jlinki J linki
= i i (
L L tr �fi;_:::l_
0 ° T. 00 T.T )
uq ' 4/ik '
t (3.52)
j�l k� l uq j k
where Slinki denotes the integral over the whole body of link i and where Iii
is the pseudo inertia matrix defined by

- f;xx + f;yy + f;zz

flixz misix
2

f;xx - f;yy + f;zz

fliyz misiy
2

f;xx + f;yy - tz
misiz
2
misiz mi
( 3 . 53)
Dynamics 93

i
With y = Crx, iry, irzY, the elements of � are given by

�xx r er/ + ir/)p dv (moment of inertia), (3.54a)

Jlinki
=

Bixy r irx iryp dv (product of inertia), (3.54b)

Jlinki
=

mi r p dv (mass of link i), (3.54c)

]Iinki
=:=

Sx r irxpdv/mi (center of mass), (3.54d)

i Jlinki
=

and by similar equations for �yY' �ZZ BiX%' fJjyZ' Sjy, and Siz' Since lij is
'

defined with respect to I:j fixed to link i, the value of � is a constant

independent of q. Note that we can regard � as consisting of the zeroth
(mJ, first (mjsix, mjsiy, misiz), and second moments (top left 3 x 3 submatrix
of Iii) of link i.
Before proceeding further, let us clarify the relation between Iii and the
inertia tensor of link i. From equations 3.7 and 3.54, the inertia tensor of
link i with respect to I:i is given by

i
f; -fJixy
- Bixy
�yy
-0..
- BiYZ .
,] (3.55)
�
=:=

-fliXZ
[ 1,. -Biyz Jizz
Another expression (which will be used later, in subsection 3.4. 3) is the
inertia tensor with respect to the frame with its origin at the center of mass
(which is generally different from the origin of I:J and with its coordinate
axes parallel to those of I:i . Let us denote this tensor by

-Hixy
Jiyy (3.56)
- Hjyz

The following relations hold between elements of il; and if;:

(3.57a)

(3. 57b)

Similar relations also hold for l;yy, l;ZZ' Biyz, and Bixz. Putting these rela-
94 Chapter 3

tions together, we get

(3.58)

where iii is the vector from q to the center of mass of link i expressed in
1:i; it is expressed as

(Recall that the notation [. x] was defined by equation 2.123). Equation

Next we calc ula te the potential energy Pi oflink i. Let g [Ox,Oy,Oz,oy

3.58 is known as the parallel-axis theorem (reference 8).

be the grav i t ati onal acceleration vector in 1:0 ' and let the reference plane
=

be the one normal to g which includes the origin of 1:0 ' Then we obtain

(3.59)

The Lagrangian in this case is

n
L = L: (Ki - PJ (3.60)
i=l

Substituting this into eq uati on 3.29 and changing the order of the trace
and differentiation operations yields

(3.6 1)

Rewriting this equation, we finally have the general equation of motion:

r = M(q)ij + h(q,q) + g(q), (3.62)

where M(q) is the n x n s y m met ri c inertia matrix whose (i,j) element is

Mij
� (00 Tk 0 0 T/)
Ii.
k=max(i.)) oqj Oqi
_

- L... tr -- k-- , (3.63)

h(q,q) is the n-dimensional vect or respresentin g the centrifugal and Coriolis

forces whose ith element is giv en by
Dynamics 95

hi = ""
L L
"L (020 00 T)
j=l m=l k=max(i,j,m)
tr __
T,

oq/Jqm
k
lik
T,
__

Oqi
k
iIAm, (3.64)

and g(q) is the n-dimensional vector representing the gravity load whose
ith element is given by

gi = - �
L..
j=l
-T
mjg --jSj.
a
OOTj A

qj
(3.65)

Note that the total kinetic energy, K = L:7=1 Kj, may be written as

(3.66)
Since generally K > 0 for any nonzero q, M(q) is a positive-definite matrix.
Also, from equations 3.29',3.62, and 3.66, we have

h(q,q) = . - [1 aMa j ]
Mq col 2qT
q
q . (3.67)

Further, h(q,q) and hijm can be expressed in terms of Mij as

h(q,q) =
[ .L L (aM.
col
. ) ]
�- �
n n
1 aM.k
-2 4/Jk , (3.68)
)=1 k=l uqk uqi

hijk -
_ oMi} 1 aMjk
aqi
(3.69)
aqk -"2
The matrices

appearing in the above equations can be calculated using the following

relations. First we note that, from equation 2.45,

ai-iTi
__ = i-l T-t1. (3.70)
1 "
Oq i

where
96 Chapter 3

-1 0

r� �]
0 0
0 0
if R

0 0

0 0
A;= (3.71)

r� 1]
0 0
0 0
ifP.

0 0

Here "if R" means that joint i is revolute, and "if P" means that joint i is

j) °
prismatic. Hence,

-j)-i
T . 1 . . 1
= ° T11 T2···r TjA/Tj+1""- Ti, (3.72)
qj
i � k �j
max(j,k) > i.
(3.73)

The case when the dynamics of the actuators at joints cannot be neglected
will be treated in subsection 5.2.1.

Example 3.1 Let us check that the equations of motion 3.39 for the
two-link manipulator can also be derived from equation 3.61. We define
the link frames Li as in figure 3.5. Then

x"

Z,,=Z,

Figure 3.5
Link frames for two-link manipulator.
Dynamics 97

,e
�[
-S

�l
l 0
'T, C1 0
�

0 1

e[�, 1 -
0 0

S2 0

�l
'T, C2 0
�

0 1 o '
0 0
and

-
- C1 0

r= H
-S,
SI
iJOTI
iJql
= C1
0 0
0
0
0 0 0

iJoT2 iJOT1 1
--
--
Tz
iJql iJql
- " I
S '

l
-
- Cll

r
0
C12 - S12 11 CS1
o '
_
0
- '
0 0 0
0 0 0 0

- S" - C 12 0

r �l
-Sl2
iJOT2
iJq2
= Cl 2
0 0
0
0 o .
0 0 0 0

Thus, from equation 3.63 we obtain

M22 = e tr
OT2 Ii iJOT 2T
iJq2
2�
)= 12zz = 12
-
+ m2 1g22

4Am on the right-hand side of equation 3.69, we find

and the other equations in 3.40. Also, by calculating the coefficients of

hZll _ iJM2 l _ � iJMl1

iJql 2 iJq2
98 Chapter 3

and the other equations in 3.4 1 . Finally, noting that i = [0, -g,O,O]T and
jSj = [lgj,O,O,lY, we obtain from equation 3.65

_rOoT2 2�
g2 = - m2g -- S2
OQ2

= m2glg2C12
and the other equation in 3.42. Thus, we have derived equation 3.39 from
equation 3.6 1. 0

3.3.3 Parallel-Drive Two-Link Manipulator

The manipulators discussed in subsections 3.3.l and 3.3.2 are of the serial­
drive type, in the sense that the joint driving force 'i works between links
i-I and i for all i. A parallel drive is another type often used. An example
of this type is the two-link manipulator shown in figure 3.6. Actuator 1
provides the driving torque, 1 between the base and link 1; actuator 2
provides the driving torque '2 between the base and link 2 through a belt.
This is called a parallel-drive manipulator because t 1 and t 2 can be regarded

Instead of the belt, a chain, a set of gears, or a parallelogram linkage

as working in parallel.

could also be used to transmit the torque '2' Figure 3.7 shows a mechanism
consisting of the above parallel-drive mechanism and one revolute joint.
This mechanism is often used for the arm portion of a manipulator because
the actuators can be located near the base, to reduce the mass and the

The equations of motion for the manipulator in figure 3.6 will be derived
moments of inertia.

using the Lagrangian formulation. The only difference between this manip­

is that in this case, 2 works between the base and link 2 instead of between
ulator and the serial-drive two-link manipulator treated in subsection 3.3. 1

Figure 3.6
Parallel-drive two-link manipulator.
Dynamics 99

links 1 and 2. We can take care of this difference by taking as (}2 the
angle between the base and link 2 instead of that between links 1 and 2.
Kinetically speaking, we make this change because !2 is the generalized
force corresponding to the generalized coordinate (}2. Figure 3.8 shows the
joint variables thus taken.
With all other parameters of the manipulator defined as in figure 3.3, the
kinetic energy K 1 and the potential energy Pl of link 1 are given by
equations 3.32 and 3.33. As for link 2, the position S2 = [S2x,S2YY of the
center of mass is

S2x = I1C1 + Ig2C2, ( 3.74a)

S2y = 11S1 + Ig2S2, (3.74b)

and S2 TS2 is given by

(3 .75)

Figure 3.7
Three-link manipulator.

�--L-----�--�X

Figure 3.8
Joint variables for parallel-drive manipulator.
100 Chapter 3

Hence, the kinetic energy is

2
K2 = tm 2 s2 Ts2 + t1202 (3.76)

and the potential energy is

P2 =: m29[llSl + Ig2S2]. (3.77)

Substituting L = K1 + K2 - Pi - P2 into equation 3.29, we obtain

'1 = [m1lg12 11
2
+ + m2/1 ]01 + [m2/11g2COs(82 - 81)]02
(3.78a)

(3.78b)

Comparing equations 3.38 and 3.78, we can see that there is a fairly big
difference in dynamics between the serial-drive type and the parallel-drive
type even for otherwise identical two-link manipulators. For instance,
the off-diagonal elements of the inertia matrix for equation 3.78 (i.e., the

of the first term on the right-hand side of equation 3.78b) are always smaller
coefficients of the second term on the right-hand side of equation 3.78a and

Ig2 = O. This occurs when the center of mass of link 2 is right on the axis
than those of equation 3.38. In particular, these elements vanish when

of joint 2. Hence, we can conclude that the parallel-drive type has less
coupling between links 1 and 2 than the serial-drive type. Note, however,
that the mechanism of the serial-drive type is generally simpler than that
of t he parallel-drive type.

3.4 Derivation of Dynamics Equations Based on Newton-Euler

Formulation

3.4.1 Basic Procedure of Newton-Euler Formulation

The basic procedure for deriving the equations of motion based on the
Newton-Euler formulation will be outlined here, with the serial-drive three­
link planar manipulator in figure 3.9a used as an example. This procedure
can be easily understood if we consider it as a way to calculate the joint
driving force for realizing a given trajectory of joint vector q.
Suppose that the present values of joint displacements qi and joint
velocities qi and the desired values of joint accelerations iii are given for all
Dynamics 101

(a)

ii,

ii{l�. ' � f,
(el

f,
.

(e)

Figure 3.9
Newton-Euler formulation.

by the environment or a grasped object are given. First, we calculate the

links. Also suppose that the force/4 and moment R4 exerted on the end link

angular velocity Wi' the angular acceleration Wi' the linear velocity P i ' and
the linear acceleration Pi of link i with respect to the reference frame,
starting from the base and moving outward to the end link (figure 3.9b).
Second, using Newton's and Euler's equations, we calculate the force]; and
the moment iii that must be applied to the center of mass of link i to realize
such motion (figure 3.9c). Third, we calculate the force J; and t he moment
Ri that must be applied at joint i to produce ]; and iii' starting from the end
link and moving inward to the base, with the given values of/4 and R4 as
the boundary condition (figure 3.9d). Finally, we calculate the joint driving
force 'j (figure 3.ge).

the equations of motion based on the Newton-Euler formulation.

The set of equations necessary for the above calculations corresponds to
102 Chapter 3

3.4.2 Link Accelerations of a Manipulator

The relation among the link velocities of a manipulato r was derived in

acce l e r ati o ns As in subsection 2.5.3. we first consider a relative motion

subse ction 2.5.3. The next step is to derive the relation among the link
.

sati sfy e quati ons 2.93 and 2.94; that is.

among three frames 1:A• 1:B• and 1:c• shown in figure 2.35. Their ve locities

(3.79)
d
AR
Pc = PB B dt PCB)
A A (B AW ( ARB BPCB')
+ + ( 3 .80)
. .

B X

Differentiating e q uatio n 3.79 and applying the relation 2.91 yields

(3.8 1)

Likewise. d iffe rentiati n g equation 3.80 and ap plyi ng the relation 2.9 1. we
have
d
Pc = PB AR B d 2 (BPCB ) + 2 AWB
t 8 dt ( PCB
A A -
2 [AR d B )]
+ X
"

+ (ARB BpC8) + (ARBBpCB)].

AW X AWB X [AWB X (3.82)
B
The relation among accelerations of the link frames defined in subsection
2.3.3 can be derived on the basis of equations 3.8 1 and 3.82. As in the case
of link velocities. we define vectors °Pi and HPi as shown in figure 3. 10.
We then consider the correspondences 1:A +-? Lo. LB +-? 1:i-1• and Lc +-? 1:i
to obtain the foll o wi ng e quati on s :
Revolute joint If joint i is revolute, by an argument s im ilar to that used
to find equations 2.95 and 2.96 we obtain

(3.83)

and

(3.84)

where e. = [O,O,IY.
Dynamics 103

Link j - 1

Xo

i-'fl.. and iSi.

Figure 3.10
Vectors °Pi'

Prismatic joint If joint i is prismatic, by an argument similar to that used

O·W; oW;· -l
to find equations 2.98 and 2.99 we obtain

= (3.85)

and

(3.86)

3.4.3 n-Link Manipulator

In this subsection we will derive the equations of motion for a general n-link
manipulator based on the Newton-Euler formulation (refs. 4, 1 1).
First, from equations 2.95 and 2 . 98, the angular velocities satisfy

oWi {OW;W _l 0
+ R;eA; if R,
i-l
_

0 (3.87)
if P.
-

Second, from equations 3 . 8 3-3.86, the accelerations satisfy

(3.88)

and
104 Chapter 3

0p
;
=
0p;_1 + 0 R ;e A ; + 20W;_1 x (0RieAi) (3.89)

+ °Wi_1 x (ORi_1Hpi)
+ °Wi_l x [OWi_1 x (ORi_1Hp;)] ifP.

later. To find this acceleration we define °Si as the vector from 0 to the
The linear acceleration of the center of mass of each link becomes necessary

center of mass Gi of link i, expressed in l:o. The vector ° Si is shown in figure

0

from the origin 0; of l:i to Gi expressed in l:j. We also denote an arbitrary

3. 10. We recall that iii is, as defined in relation to equation 3.58, the vector

coordinate frame with its origin located at Gj as l:Gi' Then, by considering

the correspondences l:A +-+ l:o, l:B +-+ l:j, and l:c +-+ l:Gj, and using equation
3.82, we obtain

(3.90)

Second, we let mj be the mass of link i and let 01; be the inertia tensor
with respect to the frame with its origin located at the center of mass and
with the same orientation as l:o. Then the total external force OJ; and the
total external moment °ii; are given by Newton's equation (3.3) and Euler's
equation (3.18) as

J i = mi 0Si
Or " (3.9 1 )

and

( 3 . 9 2)
Let oJ; and °ni be the force and the moment exerted by link i-Ion link
i. Also let °Si ° Ri iii and °Pi+1
= 0Ri ;Pi+l' Then
=

(3.9 3)
and
On,. _
0n"+1 = 0pA
i+1
X Or
Ji+1
+ OnA + OsA
i i x oJii' (3.94)

Finally, the relation between the joint driving force t; and {oJ;, On;} is
given by
Dynamics 105

(3.95 )

Equations 3.87-3.95 are the equations of motion by the Newton-Euler

formulation.
In the above formulation, all vectors were expressed with respect to the
base frame �o. lf we express the vectors related to link i with respect to link
frame �i ' a constant inertia tensor iIi appears instead of Of; and constant
vectors iZi, i-lpi, ;s; appear instead of °Z;, 0p;, os;, making the numerical
computation easier (references 4, 6). This change of frames in which the

{
vectors are expressed results in the following equations:
i-I R Ti-l . R,
ll);-1 + ezqi 1·f
i-IR;T i-lll)
i _l
ill). = ;
I
(3.87')
if P,

if R,
(3.88 ')
if P,

(3. 89')

if P,

(3.90')

(3.9 1 ')

(3.92')
i A
i
'/; =
i
Ri+l i+l/;+1 + ,/;, (3.93')
i i i+l i'" i" ii i'" i i+l�
R;+1 + + S; Ji + Pi+1 ( Ri+l J;+I)' (3.94')
{
ni = ni+1 n; x X

_ e/ini if R,
!i . ( 3.95')
- ez Ti'/; IfP.

Note that we have made use of the property 3.14 of the vector product in
deriving the above equations.
If it is necessary to consider gravity, we only have to set
106 Chapter 3

If it is necessary to consider friction at joints, we may include a friction

term YFi to equation 3.95:

(3.96)

Various mathematical models of friction can be used for YFi. For a simple
model consisting of viscous friction and Coulomb friction, equation 3.96
becomes

(3.97)

where YFYi is the viscous friction coefficient, YFCi is the Coulomb friction

{
coefficient, and

if q > 0,

sgn(q) = ° if q = 0, (3.98)

-1 ifq < o.

If there is significant static friction whenq = 0, the model of equation 3.97

may not be appropriate.

Example 3.2 Let us again derive the equations of motion for the two-link
manipulator in figure 3.3, this time on the basis of the Newton-Euler
formulation. In the following equations, * denotes the elements which we
will not explicitly calculate because they are not related to ! 1 or ! 2. From
the mechanism of the manipulator we have

-So' 0
i-IR., =
[cS: , �
i
]� , i = 1,2
o

If we consider the case where no external force or moment is applied to the

Dynamics 107

end effector, the terminal conditions are

3/
3 = 0, 3n = 0
3
and the initial conditions are

The "if R" portions of equations 3.87'-3.95' with i = 1, 2, and with the
above boundary conditions, are the equations of motion for the manipu­
lator under consideration based on the Newton-Euler formulation. We
can of course show that these simultaneous equations are equivalent to
equations 3.38 by eliminating several intermediate variables. This is shown
below: From equation 3 . 87',

lWl = [�], [. � ].
81
2W2 =

81 + 82
.

From equation 3.88',

lWl = [�], [ . � ].
2W =
2 . .

.
8i 81 + 82

From equation 3.89',

From equation 3.90',

(3.99)

(3. 1 00)
108 Chapter 3

From equations 3.91/ and 3.92/,

ItA1 = 1 1··1 2 ;2 = 2··
m S ' J m2 S2, (3.10 1 )

1iil = [ : ], [ :
2ii =
2 (3.102)
1181 12(81 + (2) ].
From equations 3.93/ and 3.94/,

(3.103)

(3. 104)

{I2(81 + 82) + ig2[C 12y + 11(S281 + C281)

_ .. .
. . 2 ..
Inl = + i92(81 + (2)]m2 (3.106)
+ 1181 + m1ig1(C1y + ig18d
+ m2[l/81 + ilig2C2(81 + (2)
•
2 '"
-il1g2S2(81 + 82) +gi1C1]}
•

From equation 3.95/,

(3. 107)

We can easily see from equations 3.105, 3.106, and 3.107 that the equations
of motion given by the Newton-Euler formulation agree with equations
3.38 given by the Lagrangian formulation. 0

3.5 Use of Dynamics Equations and Computational Load

3.5.1 Real-Time Control-Inverse Dynamics Problem

The conventional way of solving differential equations (in our case, the
equations of motion) is to obtain the motion trajectory of the object when
Dynamics 109

the applied force is known. In some control algorithms of manipulators,

however, it is necessary to compute in real time the joint driving force T(t)
to realize a desired joint trajectory q(t) given as a time function. This is
often called the inverse dynamics problem, because this problem is the
inverse of the conventional one (which is called the direct dynamics problem
to distinguish it from the former).
One approach to the inverse dynamics problem is first to calculate q(t)
and q(t) from the given q(t) and then to calculate T(t) from these values
using equation 3.62 obtained by the Lagrangian formulation. Another
approach is to use equations 3.87'-3.95' obtained by the Newton-Euler
formulation instead of equation 3.62. Although the two approaches give
the same solution in principle, it is known that the Newton-Euler formula­
tion requires less computation than the Lagrangian formulation (reference
4). The following example in linear algebra will help us understand by
analogy the difference in computational loads of the two approaches.
Suppose we wish to calculate the value of an n-dimensional vector x
satisfying

x=Cy, C = AB, (3.108)

where A and B are known n x n matrices and y is a known n-dimensional

C from A and B and then to find x from C and y. The other is to

vector. There are two typical methods of computation. One is to compute

obtain z = By and then to compute Az. The former requires (n3 + n2)
multiplications and (n3 n) additions, the latter only 2n2 multiplications
-

and (2n2 - 2n) additions. We could consider that the former corresponds
to the Lagrangian formulation and the latter the Newton-Euler formula­
tion. In fact, it is known (ref. 4), as is shown in table 3.1, that the computa-

Table 3.1
Comparison of computational load.

25n4 + 66tn l + 1291-n2 + 4 2 t n - 96

Method Multiplication Addition

Lagrangian 32tn4 + 86f2nl + 171!n2 + 53tn - 128

(66,271) (51,548)

Newton-Euler 150n - 48 Bin - 48

(852) (738)

This table is q uot ed from reference 11. The integer

n is the number of joints of manipulators and the
number in parentheses for the case of n = 6. The assignment of link frames in reference II follows
that in figu re 2.22. When the assignment described in this s ecti on is used, the numbers are a little
different from those in the table.
110 Chapter 3

tionalload of the Lagrangian formulation for a general n-link manipulator

is of order n4 and that of the Newton-Euler formulation is of order n.
We can easily understand this difference in computational loads by the
following rough evaluation. In the Newton-Euler formulation, we can
perform the calculation of equations 3.87'-3.95' fo r any joint i by Nm
multiplications and N. additions for some integers Nm and Na which are
independent o f i. Thus, for an n-link manipulator the calculation is done
by Nmn multiplications and Nan additions. On the other hand, in the
Lagrangian formulation we have to calculate equation 3.62. The second
term on the right-hand side, h(q,q), has a computational requirement of
order n4, since the computational requirement of its ith element, hi> by
equation 3.64 is of order n3 .
More details of various usages of dynamic equations for real-time con­
trol and of the structure of their control system will be discussed in chapters
5 and 6.

3.5.2 Simulation- Direct Dynamics Problem

To do dynamic computer simulations of a manipulator, we need to deter­

mine its motion for a given joint driving force. This means that we have to
solve the differential equations which are the equations of motion. For
this direct dynamics problem, the Newton-Euler formulation is again
advantageous in view of its relative speed of computation (reference 7). A
basic procedure for solving the inverse dynamics problem with a digital
computer is described using the equations of motion 3.62 for conceptual
ease.

t 0, i.e., the joint displacement q(O) and velocity q(O), and the joint driving
Suppose that we are given the initial state of the manipulator at time
=

force T(t) for the period from the initial time to a terminal time. We first
specify a small time step, L1t. Then we calculate the state of the manipulator,
q(t) and q(t), at times t 0,L1t,2L1t,3L1t, , step by step until the terminal
= . . .

time. In this procedure, the equations of motion are used to calculate

q(t + L1t) and q(t + L1t) at time t + L1t when q(t) and q(t) at time tare

One of the simplest ways to do this is as follows: Solving equation 3.62

available from the previous step.

for ij, we obtain

ij = M-1(q)[T - h(q,q) - g(q)]. (3.109)

We can calculate ij(t) by substituting the known values of q(t) and q(t) into
Dynamics 111

equation 3. 1 09. Assuming that the value of ij( t) remains constant during
the interval [ t , t + A t], we have

q(t + A t) = q(t) + q (t) A t, ( 3 . 1 l Oa)

q(t + A t) = q (t) + q(t)A t + ij(t) (A t)2/2. (3. 1 l Ob)

Neglecting the term with (A t) 2 in equation 3. 1 l Ob, we obtain

q(t + A t) � q(t) + q(t)At. ( 3 . 1 l 0c)

Thus, we can calculate q(t + A t) and q(t + At) using equations 3. 1 1 0a and
3. l 10c.
The above method corresponds to the so-called Euler method for obtain­
ing a numerical solution of ordinary differential equations, and can be
considered as a method based on the Taylor-series expansion in At up to

it is faster to compute. However, a large change in q may occur between t

the first-order term. A large step size reduces the total number of steps, so

and t + A t, which violates the assumption of ij (t) being constant in the

of q and q at several time instants in the interval [t, t + A t] for a more

interval. The Runge-Kutta method and many other methods use estimates

accurate solution. 1 2
In any of the above numerical methods, it is necessary to obtain q
satisfying equation 3 . 1 09 for given q, q, and T. To do this, rather than usng
equation 3. 1 09 directly, it is more efficient to adopt the following procedure
(ref. 7): First, rewrite equation 3.62 as

M(q)ij = T - TN' (3. 1 1 1 a)

TN = h(q,q) + g (q). (3. 1 1 1 b)

from the Newton-Euler formulation (equations 3.87' -3.95' ) with ij = O. The

Then TN can be found by using the fact that TN coincides with T calculated

matrix M(q) can also be determined by using the fact that its jth column
vector � coincides with T calculated from the Newton-Euler formulation
with q ej ( the unit vector with the jth element equal to 1, j
= 1 , 2, . . . , n)
=

and with all the other terms that are unaffected by q set equal to zero.
Having calculated TN and M(q), and given T, we know that 3 . 1 1 1 a is an
algebraic equation with unknown vector q. As is well k nown, this equation
can be effectively solved by the Gauss elimination method.
The amount of computation needed for the above procedure is

in3 + (75 + t)n2 + ( 1 1 4 + !)n - 22

112 Chapter 3

multiplications and

in 3 + 55n2 + (82 + �)n - 11

additions (ref. 7). Hence when n = 6, there are 3,418 multiplications and 2,502
,

in which TN and M(q) are calculated directly by the Lagrangian formulation

addi t ions This is, of course, less than the amount needed for the procedure
.

(that is, by mean s of equations 3.63, 3.64, and 3.65).

3.6 Identification of Manipulator Dynamics

3.6.1 Identification Problem of Manipulators

When we wish to use the dy nam ic equ a ti ons of motion for a manipulator,
the values of various parameters in the equations must be known. These
parameters can be divided int o two groups: kinematic parameters and
dyn amic parameters. Kinematic parameters are those that appear in the
kinematic equatio ns as well as in the dynamic equations; and dynamic
parameters are t hose that appear only in the dynamic equations. Kinematic
parameters are typically given by the set of link parameters, which can be
obtained from the d rawings or by static measurements of sizes from a
manipula to r itself. Dynamic parameters usually consist of inertial parame­
ters (i.e., zeroth-, first-, and second-order moments of i nertia of the links),
and friction constants. The inertial parameters could be calculated by
meas u ring the size and weight of each part of a m an ip ula tor, either before
assembling it or by dismantli ng an already assembled m a nipula tor Not .

very accurate results. I t would be more p ractical to estimate the dynamic

only are these practices ti me consum ing; they usually do not produce

parameters by some identification method based on data taken during

certain test motions of the manipulato r 1 3 . 1 4
.

Various identification schemes have bee n deve lo ped for general dynamic
systems. 1 5 Roughly speaking, the identification problem of manipu lators
is also one of these problems. However, it has the following s pecia l features:

(i) Since the structures of m an ip ulators are not very complex a black-box
,

approach ( i den tific atio n of the structure and i ts order) is not ne cessary
.

It is usually enough to consider the problem as tha t of identifying the

pa rameters of a system with a known structure.
(ii) Since the dynamics change when a m anipul at or grasps an object or
Dynamics 113

performs some task, it is desirable to identify this change and to use the
result for control. This process requires an on-line identification method.

An identification scheme taking these points into consideration is the

subject of the next subsection.

3.6.2 Identification Scheme Based on Lagrangian Formulation

As can be seen from equation 3.6 1 , which is based on the Lagrangian

matrix IIi given by equation 3.53. Note that, since

formulation, all the dynamic parameters are contained in the pseudo inertia

m/Si = Ii;[O,O,O, IY, (3. 1 1 2)

gravity force, can also be expressed using IIi . Note also that, for any pair
the third term on the right-hand side of equation 3.6 1 , which represents the

of n x m matrix A = [ a iJ and m x n matrix B [bij], we have

=

tr( AB) = aijbji = tr( BA ) .

n m

L L
i = l j=l
(3 . 1 1 3)

Using equation 3. 1 13, we can rewrite the dynamic equation 3.6 1 as

(3. 1 1 4)

This can further be rewritten as

n

'i = L tr (Dik lIk),

k =i
(3. 1 1 5)

where

Note that the kinematic parameters are all contained in the homogeneous
transforms 0 Ti (i = 1 , 2, . , n), and the dynamic parameters are all con­
. .

tained in Ii; (i 1 , 2, , n).

= . . .
1 14 Chapter 3

We assume that all the kinematic parameters are known, but the dynamic

estimating the unknown parameters in Iii from a set of data acquired

ones are unknown. The identification problem we consider is that of

during a test motion of the manipulator. We have two kinds of data: data
about motion (i.e., the position q;, velocity 4;, and acceleration iii of each
joint) and data about force (i.e., the joint driving force ti, and measurements
from force sensors attached to the manipulator, if any).
We first consider the case where only the joint driving forces are available

the right-hand side of equation 3. 1 1 5 is linear in all elements of Iii ' In order
as data about force. From equation 3 . 1 1 3, it can be easily understood that

to make this fact clearer, we define the parameter vector t/Ji' which has
a one-to-one correspondence with Iii:

(3. 1 1 7)

Then equation 3. 1 1 5 can be expressed as

(3. 1 1 8)

where t/J is a lOn-dimensional unknown parameter vector given by

(3. 1 1 9)

and Ki is a I On-dimensional row vector determined by Dik (k = i, i + 1,

, n). More specifically, Ki is given as follows: Write
. . .

(3. l 20a)

(3. l 20b)
and write the (j,h) element of Dik as (Dik )jh' Then, for j = 1 , 2, . . . , i - I , we
have

(l 1 2Oc)
and for j = i, i + 1, . . ., n, we have

Kij2 = (Dij ) 1 4 + ( Dij)4 1 '

K;j 3 = (Dijh 4 + (Dij )42 '

K ij4 =
( Dij h 4 + ( Dij )4 3 '
Dynamics 115

+ +
(3 . 1 20d )
K ij5 =
[ - (Dij)l l (Di)22 (Dij h 3 ]/2,

Kij6 = [(Djj)l l - (Dij)22 + (Dijh3]/2,

Kjj7 = [(Dij) l l + ( Djj h 2 - (Dijh3 ]/2,

Kij8 = (Dijb + (Dijb ,

Kjj9 = (Dijh3 + (Djj h 2 ·

Kjj1 0 = (Dj)3 1 + (Dij)1 3 ·

Summing equation 3.1 1 8 up for i = 1 , 2, . . . , n, we obtain the linear equa­

1
tion 6

r = Kt/J, (3.1 21)

where

(3 . 1 22)

Each element of K of equation 3.122 is a continuous function of q, q, and

ij. Therefore, it is possible to identify some elements of t/J from data q, q, ij,
and r taken from various motions of the manipulator. However, not all the
elements are identifiable. Let us address the question of which part of t/J is
identifiable. 1 7 For this purpose, we consider an expression of K as a
prod uct of two matrices,

(3. 1 23)

where Kd is an n x nd matrix whose elements are functions of q, q, and ij;

Ld is an nd x l On constant matrix; and nd is a positive integer sa tisfying
nd � IOn. The matrix Ld can be a function of the link parameters because

we regard the link parameters as known constants. Further, we assume

that the row vectors of Ld are linearly i ndependent, which implies that the
rank of Ld is nd . There is an infinite number of pairs of Kd and Ld satisfying
equation 3.123. We select one pair Kd* and L/ s uc h that nd takes the
smallest value, which we call nd * . Then, of the identi fiable linear functions
of the parameter t/J, the one with the largest dimension is given by

(3. 1 24)
116 Chapter 3

and we write

(3. 1 25)

A collection of data T, q, q, and ij gathered at an instant in time will be

called a data point. We consider an arbitrary set of N data points and let
1', Kd, and Kd* for the ith data point be T(i), Kd(i), and Kd* (i). We further

define

(3. 1 26)

Then we have

Tg = K/ r/J/ . (3. 1 27)

If N � nd *, then we can show that

(3. 1 28)

equation 3. 1 28 were not satisfied by t he given set of data points, we would

for almost all sets of data points. If it should accidentally happen that

only have to add more data points until that equation held. The minimum
mean square error estimate of r/Jd* (that is, the estimate minimizing IITg -

K/ r/Jd * lI ) is given by

( 3. 1 29)

(See equations A2.23 and A2. 1 3 in appendix 2.)

Instead of giving an algorithm for finding an Ld* for a given K, we will
consider a different approach to finding an Ld*' We assume that somehow
a candidate Ld for Ld* satisfying equation 3. 1 23 is given, and that we are
asked to confirm whether this Ld is really an Ld*' The answer is that
r/Jd Ldr/J is an identifiable linear function of r/J with the largest dimension
=

if there exist an integer N and a set of data points such that the rank of

Kg = [K/( 1), K/(2), . . . , K/(N)JT

i s nd •

because we need data about motion but not data about force.
We can use a hypothetical set of data points in the above criterion,
Dynamics 1 17

Y"

z"

Figure 3.1 1
Two-link manipulator.

Example 3.3 Consider the serial-drive two-link manipulator shown in

figure 3 . 1 1 . The two axes are parallel to the axis Zo of the reference frame.
The length of link 1 is denoted by I I ' Gravity acts in the negative Yo
direction. We will consider the identification problem for this manipulator
when the joint driving forces , 1 and , 2 are measurable. We first assign the
link frames � l and � 2 so that each origin is in the Xo- Yo plane. We denote
the mass of link i as m i , the center of mass as iSi, and the inertial tensor as
iIi' The dynamic equations found by the Lagrangian formulation are

2 h 1 l 2 () 1 ()2 +
2
.. .. • • •

' I = Ml l ()1 + M1 2 ()2 + h 1 2 2 ()2 + gl (3. 1 30)

and

(3. 1 3 1 )

where the coefficients are

+ +
2 12ZZ '
Ml l = m 1 /g l 2 1 1 1 zz m2 [/1 2 + Ig/ + 211(C2 s2x - S2 S2 y )] +
(3. 1 32a)

(3. 1 32b)

(3. 1 32c)

+
/g l 2 ' 2 ' 2 /g 2 2 ' 2 ' 2
= Six Sly , =
S2x + S2y , (3. 1 33)

(3. 1 34)
118 Chapter 3

gl = m 1 g (SlxCI - slySd + mZg(l1 C1 + SZx CI Z - SZ yS1 2 )' (3. 1 35a)

gz = mzg(SZxCI Z - SZyS1 2 )' ( 3. 1 35b)

Next we find equation 3. 1 2 1 and study the identifiability of t/J. Noting that
¢>i l mi,
=

and

we have from equations 3. 1 30 and 3. 1 3 1

't l = [tP1 7 + tPz l ll

z + �Z 7 + 211 (Cz tPzz - SZ tPZ 3 )] 81
+ [�2 7 +
11 (CZ �2 2 - S2 tPZ 3 )] 8z
- ll (S2 tPzz + CZ tPZ 3 ) (OZ + 201 9z )
Z •
• •

+ g( C l tP1 2 - SI �1 3 + 11 C1 tPZ l + C1 Z tP22 - S I Z tPZ 3 ), (3. 1 36)

and

't z = [tP2 7 + 1 1 (CZ tP2 2 - Sz tP2 3 )] 8 1 + tPZ 7 8Z

(3. 1 37)

or

(3 . 1 38)

where

K= [� 0
O O
0 81
O
0 0
O O O �
0 : 11 281 + gII CI :
0 :

k 1 . 2. 2 k1•2. 3 0 : 0 0 �1 + �2 0 0 O
k2• 2 • 2 kZ• Z• 3 0 I 0 0 01 + 92 0 0 0 ' J (3. l 39)

k 1 . 2• 2 = 'I C2(281 + 82 ) - II S2(0/ + 201 (2 ) + gC1 2 • (3. l 40a)

k2• Z • 2 = II C2 81 + I I S2 0/ + gCw (3. l 40b)

k 1 . 2 . 3 = - 11 S2 (281 + 82 ) - 1 1 C2 (0/ + 201 0Z ) - gS1 2 . (3. l 40c)

2
kZ• 2• 3 =- 11 Sz 81 + I I C2 01 - gS1 2 . (3. l 40d)
Dynamics 1 19

One way of guessing the identifiable parameters is to consider equations

3 . 1 36 and 3 . 1 37 with several special data points. For example, from the case
0; = 0; = 0 (i = 1 , 2) we can easily see that rP22 , rP2 3 ' rP1 3 ' and rP 1 2 + 1 1 rPZ 1
are identifiable. Considering another case, when 0; ° and 0; i' 0, we see
that rP2 7 and rP1 7 + l/rP2 1 are identifiable. A candidate for t/Jd = Ldt/J is,
=

therefore, given by

rP1 2 + 1 1 ¢J2 1
rP1 3
rPl ? + 1 1 2 rP2 1
(3. 1 4 1 )
rP2 2
rP2 3
rP2 7
The corresponding Kd is

(3. 1 42)

and nd = 6. Now we will check whether this nd is actually *

nd . Let us take,
for example, the following data points:

Data point 1 : 8; = 0; = 0; = 0,

Data point 2: 81 = 90°, 82 = 0, 0; = 0; = 0,

Data point 3: 8; = 0, 0; = 0, 01 i' 0, O2 = 0.

Then

[Kil)]
rank Kd(2) = 6.
Ki 3)

Therefore we have n/ = 6, verifying that t/Jd given by equation 3. 1 4 1 is

really a t/J/ and that Kd given by equation 3. 1 42 is the corresponding Kd * .
(Equation 3. 1 38 can also be determined from equations 3. 1 20 and 3 . 1 1 6,
but here we have derived it from a more common form of the dynamic
equations 3. 1 30 and 3. 1 3 1.) rJ

Finally, let us consider the identification problem when some outputs

from a force sensor are available. As a preparation, note that the force
1 20 Chapter 3

lation. A fo rce sensor is a de vice that measures the torque about an

exerted on the sensor is expressed analytically using the Lagrangian formu­

axis, the force along an axis, or a set of several such forces and torques
simultaneously. For simplicity, we will consider only the case of a one-axis
force sensor attached to a link of a manipulator.
The link with the sensor is divided into two links with a virtual joint
between them. When the sensor is a torque sensor, the virtual joint is

is assumed to be prismatic. The axis of the virtual j oint is assigned to be

assumed to be revolute. When the sensor is a force sensor, the virtual joint

the axis of the force sensor. We derive the dynamic equation for this
virtual manipulator with the extra degree of freedom using the Lagrangian

tion of the virtual joint set equal to zero, the force or torque exerted on the
formulation. From this dynamic equation with the velocity and accelera­

sensor can be found as the j oint drivin g force for the virtual joint with its
sign changed. We can also determine the force and torque exerted on a
multi-axis force sensor by regarding it as a combination of one-axis force
sensors and repeating the above procedure.

Example 3.4 Consider a one-link manipulator with a force sen so r as

shown in figure 3. 1 2. The joint is revolute, with its axis Z\ equal to ZO o The
sensor measures the torque about an axis Z2 parallel to Z0 0 We want to
identify the dynamic parameters usin g the sensor output as well as the
torque measurement at the joint.
We first determine the analytical expression for the output of the force

the force sensor is exactly the same as that in example 3.3. Its dynamic
sensor. The two-j oint manipulator produced by adding a virtual joint at

z"

Figure 3.12
One-link manipulator with force sensor.
Dynamics 121

equation, therefore, i s given b y equations 3. l 30- 3. l 35. Setting (}2 = 82 =

82 0, we have
=

'1 = M 1 l 81 + 9 1 '
(3. l 43)
2
'2 = M1 2 81 + h2 1 1 81 + 92 ,
where
2 2
Ml l = m 1 lg / + m2(l1 + Ig22 + 211 s2;,; ) + 12 ZZ '
1 I1 zz +

2
M1 2 = m 2 (lg/ + 1 1 s2:.) + 12 ...

9 1 = m 1 g (sl X Cl - SlySl ) + m2!J(l1 C1 + S2x Cl - S2ySl ),

92 = mA(S2x Cl - S2yS1 )'
From equation 3. l 43 we obtain

'1 = (¢J 1 7 + ¢J2 1 1/ + ¢J27 + 211 ¢J22 ) 81

+ (J(C1 ¢J1 2 - S1 ¢J1 3 + l1 C1 ¢J2 1 + C1 ¢J22 - S1 ¢J2 3 )'

By an argument similar to that for example 3.3, we can conclude that l/Jd
given by equation 3 . l 4 1 is a l/J/. In this case we have

C [ S
Kd * = {J 1 : - {J I : 8 1 : 211 .� 1 g 1 :
+ C
o : 0 : 0 : 11 (} 1 + {J C 1 : 11 {}1
S
--; g 1 :
gSl : (}1
•

-
�lJ. 0

Even when the friction at j oints cannot be neglected, if we can measure

the joint driving torque on the actuator side we can identify the Coulomb­
friction and viscous-friction coefficients, along with other dynamic parame­
ters, using an approach similar to the one described above. For example,
if the model of friction is given by equation 3.97, instead of equation 3. 1 2 1
we have

t = Kl/J + diag [qJtfov + di a g [sg n(q ; )]l/Jc

= [ K : diag [qJ : diag [s g n(q ; )]] [ l/J T, tfovT, l/JcTY, (3. 1 44)

where
1 22 Chapter 3

f/Iv = [YFV 1 ,YFV 2 '' ' · ,YFVn] T, (3 . 145a)

tPC = [YFC1 ,YFC2 ,· · · ,YFCn] T. (3. 1 45b)

Hence, by regarding [tP T,f/lvT, tPcT] T as a new tP, and by regarding the
coefficient matrix [K : diag[q;] : diag [sgn(q;)] ] (which relates this new tP
to T) as a new K, we obtain an equation of the same form as equation 3. 1 2 1 .
Therefore, using equation 3 . 1 29 w e can iden tify f/Iv and tPc a s well a s tP/.

3.6.3 Identification of Load

Since the load grasped by the end effector may be considered part of the
last link, we can gain information about the dynamic parameters of the
load through identification of tPn.

Example 3.5 Suppose that the manipulator of example 3.3 is grasping a

load as in figure 3.13. Also suppose that the true value of parameter tP/
when the manipulator does not grasp the load is known to be tPd * D and the
identified value of the same parameter when it grasps the load turns out to
be tPd*b. We will examine what information can be gained from these data
about the mass m l , the location of the center of mass, 2Sl, and the inertia
2
te n s or 1, of the load with respect to I:2. As we distinguished tP/D and ;/D,
let us distinguish other parameters without load from those with load by
using the superscripts a and b, respectively. Then, by definition, tP2 1 b, lP2 /'
tP2 3 b, and tP2 / are

(3. 146a)

Zoo

Figure 3.1 3
Two-link manipulator with load.
Dynamics 1 23

f/J22b = m 2 s2x + m i SIx, (3. 1 46b)

f/J2 3 b = m 2 S 2 y + m l Sly, (3. 1 46c)

f/J2/ = 2 1 2%% + m 2 1g/ + 2 11%% + ml lg12, (3. 1 46d)

where 2 S1 = [Slx,Sly, sl%Y and

Igl = JSI/ + SI/. (3. 1 47)

Hence, when we let tPd* = [f/Jd l , . . . , f/Jd6 Y we have

m l = f/J2 l b - f/J2 1a = (f/Jd 1 b - f/Jd la )/ll ' (3. 1 48a)

mlslx = f/J2 2 b - f/J2 2a = f/Jd/ - f/Jd4a, (3. 1 48b)

mlsly = f/J2 3 b - f/J2 3a = f/Jd s b - f/Jd Sa, (3. 148c)

2 +
11%% mllg / = f/J2 / - f/J2 7a = f/Jd6b - f/Jd 6a. (3. 1 48d)

From these equations we can obtain the values of ml, Six , Sly , and 2 � %% =
2 11 +
%% ml lg / . Although we cannot know the mass m2 of link 2, we can find
the mass ml of the load alone. D

Exercises

3.1 Prove equation 3. 1 1 using equation 3.5.

3.2 Derive the equations of motion for the two-link manipulator in figure
3. 14 using the Lagrangian formulation. Let 19 1 be the distance between joint

Figure 3.14
Two-link manipulator.
1 24 Chapter 3

Figure 3. 15
Two-link manipulator.

1 and the center of mass of link 1 , let m; be the mass of link i, a nd let l; be
the moment of inertia about the cen te r of mass of link i. Assume that gravity
acts vertically downward.

3.3 Derive the equa tion s of motion for the two-link m ani p ula t or in figure
3. 1 4 u sin g the Ne w t o n E uler formulation. Show that the re sul t is equivalent
-

to that of e xerci se 3.2.

3.4 Derive t he e q uatio ns o f m otion for t he two l ink m ani p ula to r in figure
3. 1 5 using the Lagrangian formulation. In the figure 8; is the joint angle of
-

link i, m; is the mass of lin k i, l; is the m ome n t of inertia of link i about the
a xi s that pa ss es t h rough the center of mass and is p a r allel to the Z axi s Ij
is the le ngt h of link i, and 19; is the distance between joint i a nd the center
,

of mass of li nk i. Assume that the first j oi nt d ri vi ng torque, ' 1 ' acts between
the base and link 1, that the second j oint drivi n g torque, ' 2 , acts between
links 1 and 2, and that gravity act s in the - Z direction.

3.5 Suppose that Ib = ° fo r the th ree l in k m an ipu lat o r of e xample 2. l 3,

which is illustrated in figure 2.26. D e rive the eq ua ti ons of motion for thi s
-

dri ve that the center of mass of link 1 is on the Zl axis, and that the centers
manipulator, usi ng the Lagrangian formulation. Assume that it has a serial
,

of mass of links 2 and 3 are at 2S2 [lg 2 ,O,OY on X2 and 3 S3 [lg 3,O,OY
on X3 , respectively. Also assume that the i nerti a tensor of link i about the
= =
Dynamics 125

Figure 3.16
Parallel-drive two-degrees-of-freedom manipulator.

center of mass expressed in its own link frame is give n by a diagonal matrix,
diag[Iix, liy,liz], and that gravit y acts in the - Zo direction.

3.6 Solve exercise 3.5 using the Newton-Euler formulation.

3.7 Derive the e q uations of motion for a parallel-drive two-link manipu ­

lator using the parallelogram shown in figure 3. 1 6. Ass ume that the lengt hs
of links A, B, C, and D are Ia> lb' Ic{ = la), and Id; that their centers of mass
are at Iga' 19b, Ige ' and Igd as shown in the figure; that their masses are rna,
mb, me. and md ; and that their moments of inertia about the center of ma ss
are la, lb' Ie. and ld' Also a s s ume that torque ! l works between the base
and link A, that torq ue !2 works between the base and link B, and that
gravi ty is directed downward.

3.8 Gi v e an id en tifi able parameter vect or q,/ fo r the m anip ulat o r in

exe r cise 3.7 when the value of the joint driving force is available as the data
about force.

References

1. J. 1. Uicker, "Dynamic Force Analysis of Spatial Linkages," ASME Journal of Applied

2. M. Kahn and B. Roth. "The Near-Minimum-Time Control of Open Loop Kinematic

Mechanics 34 ( 1 967): 4 1 8 -424.

(MIT Press.
Chains," ASME Journal of Dynamic Systems. Measurement. and Control 93 ( 1 9 7 1 ): 1 64- 1 72.

3. R. Paul. Robot Manipulators 1 9 8 1 ).

1 26 Chapter 3

Mechanical Manipulators," ASM E Journal of Dynamic Systems, Measurement, and Control

4. J. Y. S. Luh, M. W. Walker, and R. P. C. Paul, "On-line Computational Scheme for

102 ( 1980): 69-76.

5. Y. Stepanenko and M. Vukobratovic, "Dynamics of Articulated Open-Chain Active
Mechanisms," Mathematical Biosciences 28 ( 1976): 1 37- 1 70.

6. D. E. Orin et aI., "Kinematic and Kinetic Analysis of Open-Chain Linkages Utilizing

Newton-Euler Methods," Mathematical Biosciences 43, no. 1 /2 ( 1 9 79): 1 0 7 1 3 0.
-

Mechanisms," ASME Journal of Dynamic Systems, Measurement, and Control 104 ( 1 982):
7. M. W. Walker and D. E. Orin, "Efficient Dynamic Computer Simulation of Robot

205-2 1 1 .

I.
8 . K . R . Symon, Mechanics, third edition (Addison-Wesley, 1 9 7 1 ).

9. Shames, Engineering MechaniCS, second edition (Prentice-Hall, 1 967).

to. H. Goldstein, Classical Mechanics, second edition (Addison-Wesley, 1 980).

1 1 . 1. M. Hollerbach, "A Recursive Lagrangian Formulation of Manipulator Dynamics and

Man, and Cybernetics to (1980): 730-736.

a Comparative Study of Dynamics Formulation Complexity," IEEE Transactions on Systems,

1 2. K. Ito (ed.), Encyclopedic Dictionary of Mathematics (MIT Press, 1 986).

ManipUlator Arms," in Proceedings of Ninth IF AC World Congress ( 1984), vol. VI, pp. 74-79.
1 3 . H. Mayeda, K. Osuka, and A. Kangawa, "A New Identification Method for Serial

1 4. C. G. Atkeson, C. H. An, and J. M. Hollerbach, "Estimation of Inertial Parameters of

Manipulator Loads and Links," International Journal of Robotics Research 5, no. 3 ( 1 986):
1 0 1 - 1 1 9.

1 6. P. K. Khosla, Real-Time Control and Identification of Direct-Drive Manip ulators, Ph. D.

1 5. P. EykholT, System Identification: Parameter and State Estimation (Wiley, 1 974) .

thesis, Robotics Institute, Carnegie-Mellon University, 1 986.

17. T. Yoshikawa, "Evaluation of Identification Tests for Manipulators," in Proceedings of

USA-Japan Symposium on Flexible Automation ( 1 988), pp. 65-7 1 .
4 Manipulability

Various factors should be taken into account when we choose the mecha­
nism and the size of a robot manipulator at the design stage, or when we
determine the posture of the manipulator in the workspace for performing
a given task during operation. An important factor among these is the ease
of arbitrarily changing the position and orientation of the end effector at
the tip of the manipulator.
In this chapter we will develop an approach for evaluating quantitatively
this ability of manipulators from the viewpoints of both kinematics and
dynamics . First, from the kinematics aspect, the concepts of the manipul­
ability ellipsoid and the manipulability measure will be introduced.1•2
Various robotic mechanisms will be analyzed using the manipulability
measure. Then, the dynamic-manipulability ellipsoid and the dynamic­
manipulability measure, which are extensions to the dynamic case, will be
discussed.
3
For a total evaluation of manipulators, we should of course consider
many other factors, including size of workspace,4.5 positioning accuracy,
load capacity, speed, reliability, safety, cost, ease of operation, and settling
time.

4.1 Manipulability Ellipsoid and Manipulability Measure

Consider a manipulator with n degrees of freedom, as in subsection 2.3.1.

The joint variables are denoted by an n-dimensional vector, q. An m­
dimensional vector r = [r1,r2, . ,rm]T (m � n) describes the position and!
. .

or orientation of the end effector. The kinematic relation between q and r

is assumed to be

r =t(q). (4.1)
The relation between the velocity vector" corresponding to r and the joint
velocity q is

,, = J(q)q, (4.2)

where J(q) is the Jacobian matrix. J(q) may also be written as J hereafter.
For the case where n � 6 and both the position and the orientation of the
end effector are considered (m 6), the matrix J means Jv given by equation
=

2.85. On the other hand, for the case where only the position of the end
effector is of concern or, as pointed out in subsection 2.5.2, the axis of
128 Chapter 4

rotation of the end effector is invariant with respect to the reference frame,
we can use r = J,(q)q in place of equation 4.2. All the arguments in this
chapter are valid for the latter case if we simply replace v with rand J with Jr'
Now we consider the set of all end-effector velocities v which are realiz­
able by joint velocities such that the Euclidean norm of q,
(4/ + 4/ + + 4,,2)1/2,
IIql!
= . . .

satisfies 11411 � 1. This set is an ellipsoid in the m-dimensional Euclidean

space. In the direction of the major axis of the ellipsoid, the end effector
can move at high speed. On the other hand, in the direction of the minor
axis the end effector can move only at low speed. If the ellipsoid is almost
a sphere, the end effector can move in all directions uniformly. Also, the
larger the ellipsoid is, the faster the end effector can move. Since this
ellipsoid represents an ability of manipulation, it is called the manipulability
ellipsoid. (See the schematic representation in figure 4.1.)
We now prove that the manipulability ellipsoid is given by the set of all
v satisfying

(4.3a)

where r is the pseudo-inverse matrix of J and where R(J) denotes the

range of J. From equations 4.2 and A2.22 we obtain

4 = rv + (I - r J)k,
where k is an arbitrary constant vector. From this equation, equations

Figure 4.1
Manipulability ellipsoid.
Manipulability 129

A2.2 and A2.4, and the equality (I - r I)T r = 0, we have

IItjll2 = tjTtj
=
"T(r )T r" + 2kT(1 - r I)T r "
+ e (I - r If (I - r I)k
= "T(r f r " + e(I - r If(1 - r I)k
� "T(rf r ".
Hence, if IItjll � 1, then "T(rf r" � 1 holds. It is clear from equation
4.2 that any velocity " realizable by an appropriate tj satisfies "E R(/).
Conversely, if we let an arbit rarily selected " satis fying equation 4.3a be ,,*,
then since ,,* E R(/) there exists a vector z such that ,,* = Jz. Hence, by
letting

we have

Itj* = 11+,,* = 11+ Jz = Jz = ,,*

and

This completes the proof of equation 4.3a.

If the manipulator is not in a singular configuration (that is, if rank I =

m), then, since" E R(/) for any", the manipulability ellipsoid is given by
(4.3b)

rather than by equation 4.3a.

Now we find the principal axes of the manipulability ellipsoid by makin g
use of the singular-value dec ompos it ion (see appendix 3) of J. We let the
singular-value decomposition of I be

(4.4a)

where U and V are, respecti vely, m x m and n x n orthogonal matrices,

and where 1: is an m x n matri x defined by
130 Chapter 4

(4.4b)

The scalars 0"1,0"2" " ,O"m are called singular values of I, and they are
equal to the m larger values of the n roots {jf;, i 1, 2, , n}, whe re A.j
= . . .

(i = 1, 2, . . , n) are eigenvalues of the matrix ITI. Further we let Uj be the

. ,

ith column vector of U. Then the principal axes of the manipu labili ty
ellipsoid are 0"1U1>0"2"2,'" ,00mUm (see figure 4.1). This is now shown.
From equations 4.4 and A2.l6,

(4.5a)

where E+ is the pseudo-inverse of E, wh ich is given by

(4.5b)
o

where O"j -10 if O"j = 0 for notational convenience. We consider the follow­
=

ing orthogonal t ra ns fo rm ation of 1':

V = UTI' = col[va.

Then, by equation 4.3a, we have

1 -2
zVj <
"
1.- 1.
=
adO O"j

This implies that the direction of the coordinate axis for Vj (that is, the
d irection of uJ is that of a principal axis, and that the radius in that direction
is given by O"j. Therefore, the principal axes are 0"1 U1 ,0"2U2,·· . ,00mUm'
One of the represe ntati ve measures for the ability of manipulation derived
from the manipu labi lity ellipsoid is the volume of the ellipsoid. This is given
by em w, whe re
Manipulability 131

(4.6)

{
(2ltt/2/[2.4.6 . . . . . (m - 2)·m]
Cm= 2(2lt)(m-ll/2/[1' 3· S .. ·(m - 2)'m] if m is odd.
. .
if m is even
(4.7)

Since the coefficient Cm is a constant when m is fixed, the volume is propor­

tional to w. Hence, we can regard w as a representative measure. We call
w the manipulability measure for manipulator configuration q.
The manipulability measure w has the following properties:

(i) w = JdetJ(q)JT(q). (4.8)

(ii) When m = n (that is, when we consider non-redundant manipulators),

the measure w reduces to

w= JdetJ(q)J. (4.9)

(iii) Generally w � 0 holds, and w = 0 if and only if

rankJ(q) < m (4.10)

(in other words, if and only if the manipulator is in a singular configuration).

From this fact we can regard the manipulability measure as a kind of
distance of the manipulator configuration from singular ones.
(iv) When m=n, the set of all v which is realizable by a joint velocity q such
that

14;\ � 1, i = 1,2, ... , m (4.11)

is a parallelpiped in m-dimensional space, with a volume of 2mw. In other

words, the measure w is proportional to the volume of the parallel-piped.
This gives another physical interpretation of w, different from the volume
of the manipulability ellipsoid, although this is valid only for the case of
m=n.
In the next section various robotic mechanisms will be evaluated from
the viewpoint of manipulability. First, however, several general remarks
are in order.
So far, we have implicitly assumed that the maximum velocities of all
joints are the same and that the linear velocities and angular velocities can
be regarded as having the same degree of importance. In order to satisfy
this assumption, the following normalization of the variables in necessary:
After fixing a set of units for distance, angle, and time (typically, meters,
132 Chapter 4

,
radians, and seconds) we denote the maximum (angular) velocity of joint
i as qimax' We also select a desirable maximum (angular) velocity Vjma• for
each element of the end-effector velocity Vj' taking into consideration the
class of tasks that the manipulator is supposed to perform. Then, letting

q=[,It.,L,.·· ,qnY, qi = 4d4ima., (4.12)

Ii = [1\,V2, .. . ,Vm]T, Vj = Vj/Vjma., (4.13)

we have

(4.14)

where

J(q)= TvJ(q) Tq-1, (4.15)

Tv =
( 1 1 , 1) ,
diag -- ,--, .. . -- (4.16)
v 1 max V2 max Vm max

. (1 ), 1
Tq = dlag -. -'-'--" " ,-. -
1
(4.17)
qlmax Q2max qnmax
and diag(' ) denotes a dia gonal matrix. Since the normalized variables Ii
and q satisfy the assumption that we have made imp lici tly for v and q, we

t
can define the manipulability ellipsoid and the manipulability measure
using the normalized Jacobian J(q). Especially when n=m, he relation

r
between the measure w for J(q) and the manipulabili ty measure w for the
no ma lized Jacobian J(q) is given by

w=det Tv det J(q) det Tq-1

(4.18)

Thus, the transformations 4.12 and 4 . 13 have only the effect of multiplying
w by a scalar constant
m

f1 (4imax!Vimax)'
i=1

In other words, the relative shape of w as a function of the arm configura­

tion q is independent of the normalization of v and q.
It is also easy to take into cons iderat io n the effects of actuators and
tr ansmission mechanisms. Let qa be the position vector of the actuators,
Manipulability 133

and let the relation between qa and q be expressed by

(4.19)

where G, is an m x m constant matrix representing the transmission ratio.

Then we have

(4 .20)
Regarding ](q)Gr-1 as a new Jacobian, we can develop a similar argument
as above. Note that when differential gears are used, G, is not diagonal.
Finally, let us consider the relation between the manipulability and
the force (and moment) which can be exerted on an object by an end
effector. We assume that a manipulator is not in a singular configuration,
i.e., rank] m. We let the force exerted by the end effector of the manipula­
=

tor at rest on the object be represented by an m-dimensional vector f

corresponding to v. From equation 2.139, the joint driving torque T which
is equivalent to f sa tisfies

T = JT(q)/ (4.21)
Hence, the set of all f realizable by some T such that liT \I � 1 forms an
ellipsoid in the m-dimensional Euclidean space described by

(4.22)

This ellipsoid is called the manipulating-force ellipsoid. Its volume, cm/w, is

inversely proportional to the manipulability measure. The principal axes
of the manipulating-force ellipsoid are given by u1/a1, u2/a2, ... , urn/am·
This means that the direction in which a large manipUlating force can be
generated is the one in which the manipulability is poor, and vice versa.

4.2 Best Configurations of Robotic Mechanisms from

Manipulability Viewpoint

4.2.1 Two-Link Mechanism

In this section, the manipulability measure is calculated for various robotic

mechanisms, and the best configurations of these mechanisms from the
viewpoint of manipulability are determined. These configurations will be
called the optimal configurations. The end-effector position for the optimal
134 Chapter 4

·���----�x
o

Figure 4.2
Two-link mechanism.

configuration in the workspace gives the most desirable position for manip­
ulation tasks.
Let us consider a two-link mechanism shown in figure 4.2. When the
hand position [X,y]T is used for r, the Jacobian matrix is

(4.23)

and the manipulability measure w is

(4.24)

Thus, the manipulator takes its optimal configuration when (J2 = ±90°,
for any given values of 11, 12, and (Jl' If the lengths 11 and 12 can be specified
under the condition of constant total length (that is, 11 + 12 = constant),
then the manipulability measure attains its maximum when 11 12 for any
=

given (Jl and (Jl.

When the human arm is regarded as a two-link mechanism by neglecting
the degree of freedom of sideward direction at the shoulder and the degree
of freedom at the wrist, it approximately satisfies the relation 11 = 12,
Moreover, when we use our hands to perform some task such as writing
letters, the angle of the elbow is usually in the neighborhood of 90°. Thus,
it could be said that humans unconsciously use the arm postures that are
best from the viewpoint of manipulability.
We now find the two principal axes of the manipulability ellipsoid for
the case of 11 = 12 = 1. They are specified by 0'1> 0'2' "1' and "2' which can
be derived as follows using the method described in appendix 3.
Manipulability 135

L-....JScale of
o 1 ellipsoid

��. o 1 2 /a

Figure 4.3
Manipulability ellipsoid and manipulability measure.

First, JP is given by
JJT
(Sl +S12)2+S1/ ]
[
-(Sl +S12)(CI +C12) -S12C12
=

-(SI +S12)(CI +C12)-S12C12 (CI +C12)2 + C122 '

and its eigenvalues are

Al = [3 2C2 + J5 + 12C2 + 8C/]/2,

+

A2 = [3 + 2C2 J5 + 12C2 + 8C/]/2.

-

Thus, the singular values are given by (Ii = jf;, where i = 1,2. Next, from
equation A3.8',

Ui = [{(Sl + Su)(CI + Cd + SI2CI2}/"i> {(SI + SI2)2 + SI2 Ai}/"iY, -

where

"i = {[(SI + Sd(Cl + Cu) + SUC12]2 + [(Sl + SU)2 + S12 - AJ2PI2.

Figure 4.3 shows the manipulability ellipsoid and the manipulability mea­
sure for II 12 1 obtained by the use of the above expressions. Figure
= =

4.4 shows the manipulating-force ellipsoid. From figure 4.3 we can clearly
see the direction in which it is easy to manipulate the end effector; from
figure 4.4 we can see the direction in which it is easy to exert a force on the
object.

4.2.2 SCARA- Type Robot Manipulator

Let us consider the SCARA-type manipulator with four degrees of freedom

shown in figure 4.5. Let r [x,y,z,a]T, where [x,y,zY is the hand position
=
136 Chapter 4

Scale of
ellipsoid

Figure 4.4
Manipulating-force ellipsoid.

z�

Figure 4.5
SCARA-type manipulator.
Manipulability 137

r---\-y
-t--<>

Figure 4.6
PUMA-type manipulator.

and ex is the rotational angle of the hand about the Z axis. The Jacobian
matrix for this case is

l
° 0
° ° (4.25)
-1 0 .
o 1

Hence,

(4.26)

Therefore, as in the case of two-joint link mechanism in subsection 4.2.1,

the best posture is given by O2 = ± 900 for any given values of 11' 12, °1, °3,
and °4, Also, under the constraint 11 + 12 constant, the manipulability
=

measure attains its maximum value when 11 12,=

4.2.3 PUMA-Type Robot Manipulator

Most commercially available PUMA-type robot manipulators have five or

six degrees of freedom. Many of them have links with some displacements
in the direction of the joint axis. However, we will consider only the main
three joints shown in figure 4.6, neglecting the degrees of freedom placed
at the wrist and neglecting the displacements in the direction of the joint
axes. The joint vector is q = [Ol,02,03]T. Let the manipulation vector r be
[X,y,z]T. Then the Jacobian matrix is
138 Chapter 4

J =
[ -SI( 12S2 + 13S23) C1(12C2 + 13C23)
C1( 12S2 + 13S23) SI( 12C2 + 13C23) ( 4.27)
o -( 12S2 + 13S23)
and the manipulability measure is

(4.28)
The best posture for given 12 and 13 is found as follows: First, (11 is not
related to wand can have any value. Second, by assuming that S3 * 0, we
see from OW/(}(l2 0 that =

II 12 + 13C3
tan {72 =

13S3 ( 4.29)
This means that the tip of the ann should be on the X Y plane, which is
-

at the same height as the second joint. This can further be interpreted as
maximizing the contribution of the angular velocity of the first joint to the
manipulability measure.
Substituting equation 4.29 into equation 4.28 yields

w = 1213 J
l/ + 1/ + 212 13C31S31. (4.30)

The value of (13 that maximizes the above wis given by

J(1/ + 1/)2 + 121221/ - ( 1/ + 132)
cos (13 . (4.31)
61213
=

Figure 4.7 shows the best postures for the cases 13 y12' where y 0.5,1,
= =

2. (Only those satisfying 0° � (12 � 90° are shown in the figure.) If the
manipulator is regarded as a two-joint mechanism consisting of (12 and 03,

Figure 4.7
Optimal configurations for PUMA-type manipulator.
Manipulability 139

the optimal angle for 03 is 90°, as was discussed in subsection 4.2.1. In the
present case, however, the optimal 03 is smaller than 90° because the
contribution of 01 to w can be made larger by placing the tip of the arm
farther from the first joint axis.

4.2.4 Orthogonal-, Cylindrical-, and Polar-Coordinate Manipulators

Only the three main degrees of freedom of manipulators and the hand
position are considered in this subsection, as in the preceding subsection.
The manipulability measure w of an orthogonal-coordinate manipulator
(figure 1.3a) is 1 everywhere in the workspace. The most manipulable
posture for a cylindrical-coordinate manipulator (figure 1.3b) is attained
when the arm is fully stretched out. Similarly, a polar-coordinate manipula­
tor (figure 1.3c) is most manipulable when the arm is fully stretched out in
the horizontal direction. Therefore, for cylindrical-coordinate and polar­
coordinate manipulators, the best posture is achieved at the boundary of
their workspace. Although this is inconvenient, it is true for all robotic
mechanisms that have a prismatic joint whose axis is in the radial direction
of a rotational joint. This is also intuitively understandable. For these
manipulators, we will need considerations other than the manipulability
measure, such as preferring a position near the center of the workspace, in
order to determine the best working position.

4.2.5 Four-Joint Robotic Finger

Various robotic hands with multi-articulated fingers have been developed

to achieve the dexterity and flexibility of human hands for tasks involving
handling and assembling. In this subsection we will consider the four-joint
finger shown in figure 4.8 from the viewpoint of the manipulability measure.
The Jacobian matrix relating 0 = [01,02,03AY to r = [x,y,zY is

where
-Clbl -Clb2 ��:- ],
-Clb3
(4.32)

iiI = i2S2 + i3S23 + i4S234,

140 Chapter 4

Figure 4.8
Four-joint linger.

b1 = l2C2 + l3C23 + l4C234,

b2 = l3C23 + t4C234,

and

From equation 4.8, the manipulability measure is calculated as

where

W(02,03,04) = JdetJJT
and

[b1�l ii2 �3
J =

b2 b3 . J
Note that W(02,03,04) is the manipulability mea s ure of the three-joint
mechanism that consists of joints 2, 3, and 4 and moves in the X -Z'
plane shown in figure 4.8. The maximum value of W(02,03,04) and the
corresponding finger posture for a given di stance ta between joint 2 and the
tip of the finger are shown in figure 4.9 for the case wh ere 12
= 13 0.4 and
=
Manipulability 14 1

0.3

0.2

0.1

0 �� --��--�� �--L-��--�-� --L-�

0 0.5 1. 0 1.
(a)

Z'

1.1
(b)

Figure 4.9
Maximum value of wand corresponding best finger posture. (a) Maximum value of w as a
function of I•. (b) Best finger posture.
142 Chapter 4

Z'

--+-------�-w-O--_r�
1.1 X

1.1

Figure 4.10
Maximum value of manipulability measure as a function of fingertip position.

14 0.3. (These postures are independent of the angle 92 .) Figure 4.10 shows
=

the maximum value of was a function of the fingertip's position in the X -Z'
plane (only the lower half-portion is shown, since the value in the upper
half is symmetric with respect to the X axis). The best finger posture is
shown in figure 4.10 by a broken line.
The finger postures illustrated in figure 4.9b are quite similar to those
taken by human fingers during the manipulation of small, light objects.

4.3 Various Indices of Manipulability

Sections 4.1 and 4.2 discussed the manipulability measure, a typical index
of manipulability induced from the manipulability ellipsoid. This section
presents several other indices that are developed from the same ellipsoid.
A remark is also made concerning global indices, which are required for
the evaluation of a manipulator as a whole.
As was stated in section 4.1, the manipulability measure WI ( W will be
denoted as WI in this section) represents the volume of the manipulability
ellipsoid. Other indices that might be induced from the manipulability
ellipsoid are

(4.3 3a)

(4.33b)

and

(4.33c)
Manipulability 143

Index W2 is the ratio of the minimum and maximum radii of the ellipsoid.
The closer to unity this index is, the more spherical the ellipsoid is. In other
words, W2 is an index of the directional uniformity of the ellipsoid and is
independent of its size. The reciprocal of W2 is the condition number of the
Jacobian matrix 1.6 Index W3 is the minimum radius of the ellipsoid.7 This
gives the upper bound of the magnitude of velocity at which the end effector
can be moved in any direction. Index W4 is the geometric mean of the radii
0"1' 0"2' , O"rn, and it is equal to the radius of the sphere whose volume is
. • •

the same as that of the ellipsoid. It is also equal to the mth root of W1.
The selection of an index among W1, • • • , W4 depends on the purpose of

evaluation, the ease of calculation, etc. Index w1 or W4 would be easy to

calculate and would generally be good for a rather rough evaluation. Index
W2 would be suitable for the case where the uniformity of manipulating
ability is important. Index W3 is suited for the case where the minimum
manipulating ability might be critical. However, W2 and W3 are usually more
difficult to calculate than either W1 or W4.
The above indices are local ones in the sense that they are functions of
the joint vector q, implying that they are for evaluating a given particular
arm configuration. When evaluating a manipulator as a whole, we need
some global index. A simple way to produce a global index based on a local
index is to choose a proper evaluation region S in the space of q and to
define the index Wig by

Wig = min Wi. (4. 34)

liES

In this way, we can obtain a global index with a clear physical meaning.B

4.4 Dynamic Manipulability

4.4.1 Dynamic-Manipulability Ellipsoid and Dynamic-Manipulability

Measure

The manipulability discussed in the preceding sections is based on kinemat­

ics; the manipulator dynamics are completely ignored. Therefore, although
one can apply it to the conceptual design of arm mechanisms and to the
avoidance of singularities without considering complicated dynamics of
arms, it may not be suitable for the detailed design of arms or for high­
speed, high-precision motion control. This section introduces the concepts
of the dynamic-manipulability ellipsoid and the dynamic-manipulability
144 Chapter 4

measure for evaluating the manipulating ability, with the dynamics of the
arm taken into consideration (ref. 3).
Assume that the manipulator dynamics is given by equation 3.62, i.e.,

M(q)ij + h(q,q) + g(q) = T. (4.35)

Also assume that the relation between the end-effector position vector r
and the joint vector q is given by equation 4.1, and that the relation between
the end-effector velocity vector II and the joint velocity vector q is given by
equation 4.2, i.e.,

11 = J(q)q. (4.36)

Differentiating this with respect to time yields

Ii = J(q)ij + ar(q,q), (4.37)

ar(q,q) = j(q)q. (4.38)

The term ar(q,q) can be interpreted as the virtual acceleration caused by

the nonlinear relationship between the two coordinate systems for q and
r. Since

ar = Jrar + (i - Jr)ar

= JM�lMrar + (I - Jr)a., (4.39)

we obtain from equations 4.35 and 4.37

Ii - (I - r J)ar = JM-1 [T - h(q,q) - g(q) + Mr ar). (4.40)

Introducing the new vectors

i = T - h(q,q) - g(q) + Mr ar (4.41a)

and

(4.41b)

we can rewrite equation 4.40 as

j = JM�li. (4.42)

Note that equation 4.39 is a decomposition of ar (part of the end-effector

acceleration) into two acceleration components, one of which can be pro­
duced by the joint driving force and the other of which cannot. Note
Manipulability 145

also that if the manipulator is not in a singular configuration, (that is, if

rank J m), then the end effector can be accelerated in any direction and
=

the term (I Jr )ar in equations 4.40 and 4.41b vanishes, resulting in ; V.

- =

The basic idea here is to quantify the degree of arbitrariness in changing

the end-effector acceleration; under some constraint on the joint driving
force i on the basis of equation 4.42. We then adopt this "arbitrariness"
quantity as a measure of the manipulability of the arm in the dynamic case.
As the constraint, we consider the inequality lIill � 1. The set of all end­
effector accelerations ; that the joint driving force can achieve such that
lIill � 1 is an ellipsoid in m-dimensional Euclidean space described by

(4.43)

This ellipsoid is called the dynamic-manipulability ellipsoid, which will be

abbreviated as DME hereafter. Let the singular-value decomposition of
(JM-1) be

JM-1 = lJ.iEd v,/, (4.44)

where

(4.45)
o .

Let

(4.46)

Then the principal axes of the DME are

Note that except at the singular configurations where the volume of the
ellipsoid becomes zero (that is, except at configurations for which rank
J = m), equation 4.43 can be replaced by

(4.47)

A typical measure of dynamic manipulability corresponding to the ma­

nipulability measure would be the volume of DME. This is given by Cm Wd,
146 Chapter 4

where

(4.48)

and Cm is the constant given by equation 4.7. Hence, we adopt Wd as an

index and call it the dynamic-manipulability measure at configuration q. The
measure Wd has the following properties, which are analogous to those of
the manipulability measure w:

(4.49)

(ii) When m = n (that is, when we consider nonredundant manipula­

tors), Wd reduces to

I det J I
Wd (4.50)
IdetMI'
= ---

where the denominator represents the effect of manipulator dynamics on

Wd and the numerator is equal to the manipulability measure that represents
the kinematic effect.
(iii) The index Wd satisfies Wd � 0 generally, and satisfies Wd = 0 if and
only if rank J(q) -1= m (i.e. the manipulator is in a singular configuration).
(iv) When m = n, Wd has the following physical interpretation as well
as that of the ellipsoid's volume: The set of all accelerations J achievable
by a joint drivin g force i such that

Ifd � 1, i = 1, 2, . , n
. . (4.51)

is a parallelepiped in m-dimensional Euclidean space, with volume 2mWd'

In other words, the measure Wd is proportional to the volume of the
parallelepiped.

So far, we have implicitly assumed that the maximum joint driving forces
at all joints are 1 irrespective of q, and that the weights of all components
of the end-effector acceleration are the same. When these assumptions do
not hold, each variable should be normalized as follows: Regard a state in
which the manipulator is at rest (q 0) as a standard one for considering
=

the dynamic manipulability. The constraint on f is assumed to be given by

l'td � 'timn, i = 1,2,... , n. (4.52)

From 4 = 0 we have h(q,q) = 0 and a.(q,q) = O. Hence, from equation 4.41

we have
Manipulability 147

i = f - g(q), (4.53)

v = V. (4.54)

Now we take

(4.55)

as the constraint on i, where

(4.56)

Here we assume that Ti max � 0 is always satisfied. This assumption is

reasonable because Ti max < 0 means that the manipulator cannot support
its own weight for certain arm configurations. Also, equation 4.56 corres­
ponds to setting Tima. by choosing the case where the gravitational force
works in the worst direction. For the acceleration i, we assume that the
maximum desirable acceleration 6j mal for each element 6j of i can be found
by considering the set of tasks to be performed by the manipulator. If it is
difficult to determine 6j ma.. this value may be roughly selected as the weight
of relative importance among the elements of �.
Once the values Tima. and i5jmax are given, we can normalize the variable
i and � by

(4.57)

and

(4.58)

Then, from equation 4.42, we obtain

� =
JIfr1f, (4.59)

where
J= Tal, (4.60)

Jft = 1'.M. (4.61)

Ta = diag (� � ..... �).

.
V1max V2max V",ma.
(4.62)

1'. = diag ( �
--
1
�
.--
1
)
.. . ..-

timax t2max
1
� -

'"max
. (4.63)
148 Chapter 4

We can now define the DME and the dynamic-manipulability measure

using the normalized Jacobian J and the normalized moment-of-inertia
matrix 1ft instead of J and M.
Even when the DME is defined on the basis of equation 4.59, as long as
the manipulator is not in a singular configuration, any vector �* included
in the DME is a realizable end-effector acceleration in the sense that the
corresponding �* given by

(4.64)

is achievable by some T that satisfies equation 4.52.

As well as Wd, there are other useful indices of dynamic manipulability,
such as the minimum singular value, Udm, or the reciprocal of the condition
number, Udm /Ud1. Similarly to the case of manipulability, Udm is the upper
bound of the magnitude of acceleration with which the end effector can be
moved in any direction. Udm/Udl is a measure of directional uniformity of
realizable end-effector acceleration.

4.4.2 Two-Link Mechanism

We will now analyze the two-link mechanism shown in figure 4.11 from
the viewpoint of dynamic manipulability. The following notations are used
in the figure:

mi the mass of link i,

1; the moment of inertia of link i about the center of mass,
Ii the length of link i,
Igi the distance between the joint i and the center of mass of link i,

-L�---X
---�

Figure 4.11
Two-link mechanism.
Manipulability 149

me the mass of the end effector and load,

Ie the moment of inertia of the end effector and load.

It is assumed that the first joint driving torque, 't1, acts between the base
and link 1, and that the second joint driving torque, 't2' acts between links
1 and 2. We let r = [x,yY, so that from subsection 3.3.1 we have

(4.65)

M =
[M21Mll ]
M12 ,
(4.66)
M22
2 2 2
Mll 11 + 12* +ml/g1 +m2* (l1 + Ig2* + 2/1/g2*C2),
= (4.67a)

M12 M21 12* +m2*(l92*2 + i1/g2*C2),

= = (4.67b)
- 2
M22 - 1 2* +m2*1g2* , (4.67c)

wherem2*' Ig2*' and 12* correspond tom2, Ig2' and 12 , respectively, but
since in this case the end effector and the load are regarded as part of link
2, these parameters become

(4.68a)

Ig2* (m21g2 +meI2)/(m2 +me),

= (4.68b)

12* 12 +m2(ig2* - 192)2 + Ie +m.(l2 - 192* )2.

=
(4.68c)

Sincem 2 , we obtain from eq ation 4.50

= n = u

-
* 2*11 2 +m2*21g2* 21 2S22"
m1 1gl 2 ) (/ 2* +m2*192* 2) + 12m
Wd =
(/
1 + - 1

(4.69)

From now on we will assume that the link mechanism is at rest. The case
where there is no gravity effect will be considered first. In this case, since
Tima• 'tim •• by an argument similar to equation 4.18, Ta and Tt have only
= ,

the effect of multiplying Wd by a scalar constant. Therefore, equation 4.69

is an adequate expression for evaluating dynamic manipulability.
If we let

(4.70)
m1 191 2 +m2*1 2)(/-2* +m2*192*2)
IX = -
(Ii + 1
150 Chapter 4

and

a
m2*2192*211 2
(4. 71)
P = (1-1 + m1 191 )(1-2* + m2*192*2) + 12
2 2
'7 *m2 *1 1 '

then equation 4.69 becomes

ex(1 + I1)1S21
w" (4.72)
1 + I1S22
=

Therefore, the relative shape ofw" as a function of q is uniquely determined

by the parameter 11, and the parameter ex determines its scale.
Since the special case where 11 = 0 is of some interest, we will consider
this case first. The condition 11 0 can be reduced to Ig2 * 0 under the
= =

natural assumptions that m2* #- 0 and 11 #- O. The implication of 192* 0 =

is that the center of mass of the set of link 2, the end effector, and the load
is located precisely at joint 2 (say, by the use of a counterbalance). When
192* = 0, the value 1 detMI is independent of (}2' and the dynamic-manipul­
ability measure w" is equal to the manipulability measure W 11121S21 times
=

the scalar 1/ldetMI (as was stated in the preceding subsection relating
to equation 4.50). Therefore, the best arm posture from the viewpoint of
w-that is, ()2 ±900-is also the best from the viewpoint ofwd•
=

Next we will consider the case where f3 #- O. Figure 4.12 shows the value
of Wd as a function of (}2 using 11 as a parameter. It can be seen from the
figure that as f3 starts from zero and becomes larger, Wd approaches a
trapezoidal shape, but it still attains its maximum ex at (}2 90° for P
=

satisfying 0 � f3 � 1. For f3 > 1, Wd attains its maximum at two values of

(}2, one of which is larger than 90° and one of which is smaller. As we have
seen, the parameter 11 determines the relative shape ofw" and the parameter
ex determines its magnitude.

Figure 4.12
Dynamic-manipulability measure.
Manipulability 151

Now let us consider a numerical example. Let 11 = 12 = 1.0, m1 20, =

m2 = 10, m. =5,191 0.5, Ig2 0.3, 11 = 20/12,12 10/12, T. = 0, t1 max

= = = =

600, !2ma. 200 (units are m, kg, and sec), and 61mB• = 62mB• 1 (the
= =

relative importance of acceleration in the X and Y directions is the same).

Note that in determining the values of 11 and 12 we have assumed a uniform
mass distribution over the links and have used the formula 1 = mF/12 for
the moment of inertia 1 ofa thin beam with uniform mass distribution and
with mass m and length I. The DME and the dynamic-manipulability
measure Wd as functions of the distance la between the origin and the arm
tip are given in figure 4.13. From equation 4.71 we have p = 0.78, so Wd
reaches a maximum when (}2 90°. As can be seen from the figure, however,
=

the difference between Wd and the maximum value is not very large for a
wide range of la' Hence, the mechanism with the gi ven values of the inertial
parameters could be judged to be a rather good design from the viewpoint
of uniformity of the dynamic-manipulability measure over a wide area of
the workspace.
Next we will consider the case where the gravitational force acts on the
above mechanism in the - Y direction. Let 9 be the gravitational accelera­
tion constant. The gravity term g(q) is given by

y
1.0
� Scaleof
o 20 40 ellipsoid

'J 2.0
..
10

Figure 4.13 . . . . . .
(without graVIty).
Dynamic-manipulability ellipsoid and dynalIlJc·marupulabllity measure
152 Chapter 4

y
1.0
Scale of
rnO ellipsoid

Without gravity

,,_- _---L ________

"

" \
\

1.0 /"

Figure 4.14
Dynamic-manipulability ellipsoid and dynamic-manipulability measure (with gravity;
I., = 0.5, 1,2 = 0.3).

y
1 .0
L-.L....J Scale of
o 20 40 ellipsoid

XI/"i

o 1 .0 2.0 /"

Figure 4.15
Dynamic-manipulability ellipsoid and dynamic-manipulability measure (with gravity;
I., = 0.4, 1,2 = 0).
Manipulability 153

(4.73)

Figure 4.14 shows the DME and the dynamic-manipulability measure

calculated from J and 1ft derived from equations 4.73, 4.56, and 4.61.
Because of the effect of gravity, the dynamic-manipulability measure is
rather small for stretched arm postures. Since this is usually not desirable,
we consider a modification. If, for example, we can attain 1111 = 0.4 and
1112*= 0 by a change in the mass distribution of each link, then we see the
change shown in figure 4.15. In comparison with the original design, the
modified one has an improved dynamic-manipulability measure in the
region 0.7 � la � 1.8.

Exercises

4.1 Prove properties i-iv of the manipulability measure given in section

4.1.

4.2 Exp lain the relation between the arm posture and the ma nipulati ng ­
force ellipsoid given in figure 4.4 for the two-link mechanism shown in
figure 4.2 by taking the human arm as an example.

4.3 Find the optimal arm posture with respect to the index W2 given in
equation 4.33a for the two-link mechanism shown in figure 4.2. Sketch the
change of the optimal arm posture as the ratio of link lengths II and 12
changes.

4.4 Derive the principal axes of the manipulability ellipsoid and the
manipulability measure for the parallel-drive two-link manipulator treated
in subsection 3.3.3, and draw a figure corresponding to figure 4.3. Find the
optimal arm posture with respect to the manipulability measure.

4.5 Derive the dynamic-manipulability measure for the two-link mecha­

nism in subsection 4.4.2 with the drive mechanism replaced by a parallel
drive. Compare the result with equation 4.69.

References

1. T. Yoshikawa, "Analysis and Control of Robot Manipulators with Redundancy," in

Robotics Research : The First International Symposium, ed. M. Brady and R. Paul (MIT Press,
1984), pp. 735-747.
154 Chapter 4

2. T. Yoshikawa, Ma nip ul ability of Robotic Mechanisms," International Journal of Robotics

"

Research 4, no. 2 (1985): 3-9.

3. T. Yoshikawa, "Dynamic Manip ul abili ty of Robotic Mechanisms," Journal of Robotic
Syste ms 2, no. 1 (1985): 113-124.

4. A Kumar and K. 1. W aldron "The Work space of a Mechanical Manipulator," ASME

,

Journal of Me chan ical Design 103 (1981): 665-672.

5. D. C. M. Yang and T. W. Lee, "Heuristic Combinational Op timization in the Design of

Manipulator Workspace," IEEE Transactions on Systems, Man, and Cybernetics 14, no. 4
(1984): 571-580.
6. 1. K. Salisbury and 1. 1. Craig, "Articulated Hands, Force C o ntrol and Kinematic Issues,"
International Journal of Robotics Research 1, no. 1 (1982): 4-17.

7. C. A. Kle in, "Use of Red un dancy in the Design of Robotic System," in Robotics Research:
The Second International Symposium, ed. H. Hanafusa and H. Inoue (MIT Press, 1985), pp.
207-214.
8. T. Yoshikawa, Analysi s and Design of Articulated Robot Arms from the Viewpoi nt of
"

Dynamic Manipulability," in Ro botics Research: The Third International Symposium, ed. O.

Faugeras and G. Giralt (MIT Press, 1986), pp. 273-279.
5 Position Control

Control algorithms and the construction of control systems will be dis­

cussed in this chapter for tasks such as transferring the end effector of a
manipulator from one position to another or making the end effector follow
a given trajectory. Although the end effector may hold a load, it is assumed
that the end effector and its load do not come in contact with any other
environmental structure, and that they can move freely in three­
dimensional space. The case where there is some contact with the environ­
ment will be treated in chapter 6.
First we will develop several methods for determining a desired trajectory
that passes through given initial and final points and, intermediate points,
if any. Then we will consider several control algorithms for realizing the
desired trajectory: linear feedback control, two-stage control by lineariza­
tion and servo compensation, decoupling control, and adaptive control.

5.1 Generating a Desired Trajectory

5.1.1 Joint-Variable Scheme

In this section we will consider methods to determine a desired trajectory

of joint variables over time. A basic problem is how to select a trajectory
between a given initial position (Yo ) and final position (Yr) of the end effector
with a time interval 'r allowed for moving between them Various methods
.

of solving this problem have been proposed.1 A simple method based on

polynomial functions of time2• 3 will be presented here. This method can
be divided into two schemes, one in which the trajectory is considered in
terms of joint variables and one in which it is considered in terms of position
variables of the end effector. The present subsection will describe the
joint-variable scheme. The other scheme will be developed in the next
subsection.
Suppose we are given joint vectors fo and fr, corresponding to Yo and 'r
respectively. If only '0 and 'r are known, we determine fo and qr in advance
by solving the inverse kinematics problem. We first choose an arbitrary
joint variable, q;, and represent it by e. We assume that the value of e at
the initial time (0) is �o and that the value at the final time (tr) is �r:

(5.1 )

We further assume that the velocity and the acceleration o f � must sati s fy
156 Chapter 5

the following boundary conditions:

e(O) = eo, e(tc) = er, (5.2)

�(O) = �o, �(tr) = er. (5.3)
Although there are many smooth functions that satisfy these constraints,
we select polynomials of time t because of their ease of computation and
simplicity of expression. Since the polynomials of the lowest order that
satisfy the arbitrarily given boundary conditions 5.1-5.3 are of fifth order,
we express �(t) as a fifth-order polynomial:

(5.4)
Then the unknown coefficients a1-as that satisfy equations 5. 1-5.3 are

(5.5a)
(5.5b)
(5.5c)
1 . .
a3 = - 3 [20�r - 20�o - (8�r + 12�o)tr
2 tr
-(3eo - �r) t/J, (5.5d)
1 . .
a4 4 [30�o - 30�r + (14�r + 16�o)tr
2 tr
=

+(3�o - 2er)t/J, (5.5e)

1 .
as (6�r + 6�o)tr
.

- 5 [12�r - 12�o
2 tr
= -

-(eo - er)tr2], (5.50

In particular, if eo er = 0 and the relation shown in figure 5.1 is satisfied
=

by �o, �r, eo, and er-that is, if

(5.6)

-then ao = 0, which implies that only a fourth-order polynomial is re­

quired for �(t).
By using combinations of fourth-order polynomials and straight lines,
trajectories for various cases can be determined fairly easily. Let us consider
two such cases.
Position Control 157

�,

o t,

Figure 5.1
Boundary conditions �o = �r = 0 and (5.6).

Figure 5.2
Trajectory with acceleration, constant velocity, and deceleration phases.

(1) Only the initial point �o and the final point �r are known.
We develop a trajectory that starts from rest at �o, passes through the
phases of acceleration, constant velocity, and deceleration, and finally
comes to a complete stop at �r. For this purpose, we first choose the value
of parameter A that denotes one-half of the acceleration and deceleration
period. Second, we determine the auxiliary points �02 and �rl shown in
figure5.2 by the following procedure: First two points �Ol and �r2 at time
t = A and t = tf A are taken as �Ol = �o and �r2 = �f· Then �Ol and �f2
-

are connected by a straight line and �02 and �f1 are determined as the points
on the straight line at time t = 2A and t = tr 2A. -

We determine the trajectory segments between �o and �02' and between

�f1 and �r, by fourth-order polynomials such that their velocity coincides
with the composition of straight lines connecting �o, �O ' �r2' and �r at
l
boundary points �o, �02' �f1' and �r, and such that their acceleration is zero
158 Chapter 5

y y

nr-------<>x
a
(a) (b)

Figure S.3
Two-link manipulator. (a) Initial position. (b) Final position.

at these boundary points. The trajectory segment between e02 and eft is
specified by the straight line. Thus, the whole trajectory is given by the bold
solid line in figure 5.2.

Example 5.1 Applying the above method to the two-link planar mani­
pulator shown in figure 5.3, we generate a trajectory that starts from rest
at the initial position in figure 5.3a and comes to a complete stop at the
final position in figure 5.3b in 2 seconds. Suppose we choose L1 = 0.25. Then
the first joint angle, (Jl' is given by

{ 90 - 80t3 + 80t4, 0 � t � 0.5

(J1 (t) = 85 - 20(t - 0.5), 0.5 < t � 1.5
65 - 20(t - 1.5) + 80(t - 1 .5)3 - 80(t - 1.5)4, 1.5 < t � 2.0

and the second joint angle, (J2, is given by

{ -60-160t3 + 160t4, 0� t� 0.5

(J2(t)= -70-40(t-0.5), O.5<t� 1.5
-110-40(t-1.5)+ 1 60(t-1.5)3 -160(t-1 .5)4, 1.5<t �2.0.

Figure 5.4 shows the resulting endpoint trajectory. 0

(2) The initial point �o, several intermediate points, and the final poi nt
�r are given.

We determine a trajectory that starts from rest at initial position eo, passes
through intermediate positions e1 and e2, and stops at fmal position 'f' We
first consider the case where it is not necessary to pass exactly through '1
Position Control 159

Figure 5.4
Endpoint trajectory.

and �2' In this case, we specify auxiliary points �02'�1l'�12""'�f1 as in

figure 5.5. Note that the values at points �Ol and �o are the same, that the
values at �f2 and �f are the same, and that �02 and elI are on the straight
line between �Ol and � l'Points �12" '�f1 are also determined in the same
"
way. We then determine the trajectory segments for gO'�02},{�11'�12}.""
gn,eel by fourth-order polynomials, and those for g02'�1l}, g12>�21}'
and {�22'�fl} by straigh t lines. The resulting traj ectory is illustrated by a
bold line in figure 5.5.
Next, we consider the case when the trajectory is required to pass exactly
through �1 and e2• This case can be dealt with by increasing the number
of auxiliary points. A scheme is shown in figure 5.6a. Auxiliary points are
determined as follows: As figure 5.6b shows, �j2 (i = 1,2) is the midpoint
between A (the intersection of the line of t tj L1 with the s traight line
= -

between �j-l (eOI when i = 1) and 0 and B (the intersection of the line of
t tj L1 with straight line between ej and ei+1 (�f2 when i 1». Point �i3
= - =

is determined s imilarly We then determine the trajectory segments for

.

gO'�02}' g l1>�d, { el'�14}'"'' gfl '�f} by fourth-order polynomials, and

those for g02,e11}' g14'�21}' and g24,efl} by straight lines, as in the
previous case.

In the basic problem of connecting �o and �f by a polynomial, which was

mentioned at the beginning of the sec tion if we consider the boundary
,
160 Chapter 5

Figure 5.5
Trajectory passing near the intermediate points.

(a)

I,

(b)

Figure 5.6
Trajectory passing through the intermediate points. (a) Intermediate points. (b)
Determination of points </2 and </3'
Position Control 161

condition on position (equation 5.1) and that on velocity (equation 5.2),

and do not take into account the continuity of acceleration (equation 5.3),
then we can simplify the polynomials. More specifically, we may use a
third-order polynomial,

e(t) = ao + alt + a2t2 + a3 tl, (5.7)

instead of a fifth-order one. The constants in equation 5.7 are

(5.8a)

(5.8b)

(5.8c)

and

eo) + (er + eo)trJ·

1 . .
a3 = 3"" [ - 2(er - (5.8d)
tr
In particular, if eo, eft eo, and er satisfy equation 5.6, then a3 ;;::: 0, and so
W) becomes a second-order polynomial. Hence, for cases 1 and 2 treated
above, we can also use second-order polynomials.

5.1.2 Scheme for Position Variables of End Effector

When a trajectory between Yo and Yf is generated by a joint variable scheme,

as described in the preceding subsection, it is sometimes difficult to predict
the motion of the end effector in space. There are also cases in which some
specific end-effector trajectory is required. For example, in arc welding the
electrode should follow a seam precisely. In such cases we wish to generate
a trajectory between '0 and rf in terms of the position variables of the end
effector.
There are several ways to describe the position and orientation of the
end effector using six variables. Once we choose one way, we can determine
a trajectory by representing an arbitrarily selected variable of the six by e
and then determining a trajectory of e by a sequence of polynomials of the
form of equation 5.4 or equation 5.7.
A trajectory of an end effector obtained in this way is very simple and
easy to visualize. For example, in case 1 of the preceding subsection the
reSUlting trajectory moves on the straight line connecting '0 and rr in the
162 Chapter 5

six-dimensional Euclidean space of end-effector position and orientation.

The X, Y, and Z coordinates of the origin of the end-effector frame with
respect to the reference frame are often used as the three variables for
describing the translational position of the end effector. In this case, the
position of the end effector literally moves along a straight line in space.

Example 5.2 Let us obtain a trajectory for the problem in example 5.1
not in terms ofjoint variables but in terms of end-effector position variables
r = [X,y]T. Since the initial position ro is [J3/2, 3/2Yand the final position

x(t) =
{
rf is [1, O]T, the trajectory ret) = [x(t),y(t)Y is given by

J3/2 + 4(2 - J3)(t3 - t4)/ 3,

(sJ3 + 2)/ 1 2 + (2 - J3) (t - 0.5)/ 3,
o � t � 0.5
0.5 < t � 1.5
(10 + J3)/ 1 2 + (2 - J3)(t -
1 .5)/ 3
- 4(2 1 -t
- J3)[(t .W ( - 1.5)4]/3, -
1.5 < t � 2.0

{ 3/ 2 - 4t3 + 4t4, o � t � 0.5

yet) = 5/4 - (t - 0.5), 0.5 < t � 1.5
1/4 - (t - 1.5) + 4(t - 1.W - 4(t - 1.5)4, 1.5 < t � 2.0.
Figure 5.7 shows the resulting endpoint trajectory. 0

As for the end-effector orientation, if we take Euler angles or roll, pitch,

and yaw angles, the end effector moves in a straight line with respect to

1=0

_ 1=0.5

II
...j\.
1'_
-
-

'" --
-
I
1=1.0
I
I I
I 1
I I
I I
I
1
/ = I. 5
1
I

/=2.0 X

FigureS.7
Endpoint trajectory.
Position Control 163

these variables. However, the corresponding motion in the reference frame

is difficult to understand intuitively, because it is given as a combination
of three rotations about three different time-varying axes. The following
two methods are effective in avoiding this difficulty. The first method is
based on a representation of orientation by a rotation about a fixed axis,
the second on a representation by rotations about two axes.

(1) Single-axis rotation method

In preparation, we give a representation of the relation between two frames

1: and 1:B having the same origin by the pair [k,�], where k = [kx,ky,kzY
A
is the equivalent axis of rotation and � is the equivalent angle of rotation.
We use this pair to obtain 1:B from 1:" by a single rotation about the fixed
unit vector k by an angle �. Note that the pair [k,�] can also be represented
by a three-dimensional vector �k having direction k and magnitude �. Also
note that the rotation matrix R(k,�) corresponding to [k,�] is given by

[ kx2 v" + C« kxky v" kzS« kxkz v" + kyS« ]

2
-

R(k,�) = kxky v" + kzS/Z ky v" + CII kyk v" - kxS« , (5.9)
�
kxkz v" - kyS« kykz v" + kxS« kz v" + C«

where S« = sin�, C« = cos�, and v" = 1 Ca. -

The proof of equation 5.9 is as follows. First we note that

R(k,rx) = ["XB"YBAZB]'

where "XB' AyB, and AZB are the unit vectors in the X, Y, and Z directions
of 1:B expressed in 1: . Letting'l be an arbitrary vector in 1: and '2 be the
A A
vector obtained by rotating'l about k by rx, we have general equality

(5.1 0)

Since when '1 [l,O,OY we have

= '2 = AXB from the definitions of'l and
'2' we obtain from equ ation 5 . 1 0

'2 = [k/ v" + C«, kxky v" + k2S«, kxkz v" - kyS«Y.

Hence the first column on the right-hand side of equation 5.9 is derived.
The second and third columns are derived similarly.
Now the single-axis rotation method is given. Let the orientation of'r
with respect to the end-effector frame of '0 be [kf>rxr] when expressed by
the above representation, [k,rx]. Then a trajectory from,o to'r can be given
164 Chapter 5

as one for which IX changes from 0 to IXr about the fixed axis kf• We can
specify the change of IX by, for example, the solution of �(t) in equation 5.4
with the following boundary conditions:

�(O) = 0, Wr) = IXr'

�(O) = <XO, �(tf) = <Xf,

�(O) = ao , e(tf) = af·
The trajectory for orientation is given by [kr,W)], where 0 � t � tf• Since
this trajectory is given by a rotation about an axis kr which is fixed in the
reference frame, it is easy to understand intuitively.

(2) Double-axis rotation method

This method, proposed by Paul (ref. 3), will be described here using a
somewhat different formulation. Suppose the orientation of 'f with respect
to,o is given by (tft , O, IjJ) when expressed with Euler angles. Using equations
5.9 and 2.20, we can easily see that the orientation of'r is also given by the
rotation matrix R(k,O)R(t,t/I + tft), where k = [ -sintft,costft,OY and where
t is the Z axis after the transfer by R(k,O). Thus, we can realize a trajectory
from '0 to 'r using R(k'�6(t»)R(2:(t), �",+�(t», where �6(t) and �"'+t,6(t) are
polynomials specified by equation 5.4 with appropriate boundary condi­
tions and where t(t) is the Z axis at time t after the transfer by R(k'�6(t».
Simply put, this trajectory is a combination of two simultaneous rotations;
one about the axis perpendicular to the approach vectors of,o and'r (which
has a constant direction in the end-effector frame of '0 ) through an angle
0, and the other rotation through an angle (t/I + tft) about the approach
vector at each instant. The axes of these two rotations are both easy to
understand. In particular, when the last joint of a manipulator is revolute
with its rotational axis coincident with the end-effector approach vector,
this joint can perform the second rotation about t(t) by itself.
Although the double-axis rotation method is a little more complex than
the single-axis rotation method, it makes it easier to understand the change
of the approach vector.

Example 5.3 Suppose that we want to find a trajectory that transfers the
end-effector orientation from rest at the initial orientation shown in figure
5.8a to the final orientation shown in figure 5.8b, coming to rest at time
tr = 1. First, we solve the problem by the single-axis rotation method. It is
easily seen from figure 5.8 that the rotation matrix R representing the final
Position Control 165

z"
y,

+--c>Y" ':)Hf---<> X,

x" z,
(a) (b)

Figure S.S
Initial and final orientations of the end effector. (a) Initial orientation. (b) Final orientation.

orientation with respect to the initial one is

0 0 [ 1]
R= 1 0 0 .
0 1 0
Comparing this matrix with equation 5.9, we get
kf = [1/)3, 1/J3, 1/J3Y,
�f = 120°.

If we use a fifth-order polynomial (equation 5.4) for interpolation, the

trajectory of orientation is given by

with

An example of the rotation matrix representing the intermediate orienta­

tion at t = 0.5 of this trajectory is
[ 2/3 -1/3 2/3 ]
R = 2/3 2/3 -1/3 .
-1/3 2/3 2/3
This orientation is shown in figure 5.9a. Next, we solve the problem by the
double-axis rotation method. Euler angles for the initial and final orienta­
tions are �o = [O°,oo,oo]T and � = [O°,900,900y. Hence, the trajectory is
given by
166 Chapter 5

z. z.
8( t)
z
, z
I ,
, ,
I ,
\ I
I
\ I
\ /
l,p I I
/

Xo , .....
',(I Xo \ / I I ./
'-
'
X
I _
\�, ____ .-{- :
,.

x , ,
... _1. ...... '
/

(a) (b)

Figure 5.9
Intermediate orientation. (a) Single-axis rotation method. (b) Double-axis rotation method.

where

�8(t) = �",+;(t) = 900t3 - t350t4 + 540t5.

The rotation matrix R for the intermediate orientation at time t = 0.5 for
this case is calculated, from k = [O,t,oy and �6(O.5) = �",+;(0.5) = 45°, to
be

[ 1/../i 0 1/../i ][ l/../i -1/J2 O � ]

R = 0 1 0 1 /J2 1/J2
-1/J2 0 1/J2 0 o
[ 1/2 -
1/2 1/../i ]
= 1/../i 1/../i 0 .
-1/2 1/2 1/../i
This orientation is shown in figure 5.9b. Comparing this with figure 5.9a,
we see that the approach vector is always perpendicular to the Yo axis,
resulting in its shorter path. 0

5.2 Linear Feedback Control

5.2.1 Effectiveness of Linear Feedback Control

Most manipulators in practical use today have a controller consisting of

position and velocity feedback loops which are independent for each joint
Position Control 167

and receive a desired joint trajectory as its reference input. In spite of

inherent nonlinear coupling among the joints of a manipulator, such a
controller is usually fairly effective. The main two reasons for this seem to
be the large reduction ratios between the actuators and the link mechanism
and the large feedback gains in the control loops. An analytical argument
supporting this will be developed in this subsection.
Suppose that the dynamics equation of a manipulator (equation 3.62)
has a friction term added to the right-hand side. Then

T = M(q)ij + h(q, q) + Vq + g(q), (5.11)

where V is the joint friction coefficient matrix. The inertia matrix M(q)
satisfies M(q) MT(q) > 0, where M> 0 means that a square matrix M
=

is positive definite (see appendix 4). Denoting the reduction ratio between
the actuator displacement vector qa and the joint vector q by a nonsingular
matrix Gr> we have

Grq = qa' (5.12)

We also assume that the dynamics equation of the actuators is

(5. 1 3)
where M. is the inertia matrix of the actuators, v.. is the frictional coefficient
matrix, fm is the force generated by the actuators, and Ta is the joint driving
force transmitted to the link mechanism from the actuators. If we assume
that Gr is constant, the relation between f. and f is, by the principle of
virtual work,

(5.14)

Putting equations 5.1 1 - 5. 1 4 together, we find that the dynamics equation

for the whole system of the actuators and the link mechanism is

G/Tm = [G/MaGr + M(q)]ij + h(q, q)

+ [G/v..Gr + VJq + g (q). (5. 1 5)

Now we apply to this system a PD (proportional and differential) feed­

back law:

Tm(t) = Gr -T { - K. q(t) + Kp[qd (t) - q(t)] } , (5. 1 6)

where qd(t) is the desired trajectory for q(t).

168 Chapter 5

When each joint has its own independent actuator with a large reduction
ratio, the matrices Ma, v;., and Gr are diagonal and the values of the
diagonal elements of Gr are large. Further, we consider the case in which
Ky and Kp are diagonal and the values of these diagonal elements are
positive and large. Then the terms M(q), h(q,q), Vq, and g(q) are small
relative to other terms, so the dynamics equation of the closed-loop system
consisting of equations 5.15 and 5.16 may be approximated by

(5.17)
This represents a set of independent second-order systems for each joint.
We can adjust the response characteristics of these second-order systems
by properly selecting gains Ky and Kp.
The above argument is dependent on various assumptions. Particularly,
if the terms M(q), h(q,q), and g(q) are not negligible in comparison with
either of the terms G/MaGr and G/v;.G.. the above argument does not
hold. Fast and accurate motion may not be possible without more sophis­
ticated control algorithms like those given in later subsections. Note,
however, that if we can at least compensate for the gravity term g(q), we
know that a closed-loop system with a PO feedback control law is stable
under certain conditions. A typical condition is that the desired trajectory
qd(t) is constant and the control mode is point-to-point control in which
the end effector is required to stop at several desired points sequentially.
This stability will be shown in the next subsection.

5.2.2 Stability of Proportional and Differential Feedback Control

Suppose that the control algorithm

(5.18)
is applied to the system of equation 5.15. We obtain this algorithm from
equation 5. 16 by adding gravity compensation. For simplicity we assume
that G/v;.Gr + V� O. We also assume that Kp and Ky are symmetric
positive definite. Then, by using the Lyapunov stability theorem (appendix
4), we can prove that the constant equilibrium point qd is asymptotically
stable-that is, q(t) converges to qd after an infinite period of time.4•5
We first give another expression for the centrifugal and Coriolis force
term h(q,q) in equation 5.11. We saw in chapter 3 that this is generally given
by equation 3.68. Noticing that
Position Control 169

we can easily see that h(q,q) can be expressed as

h(q,q) = C(q,q)q, (5.19a)

where C(q,q) is an n x n matrix whose (i,j) element Cij is

Cij = "21 f
L,.,
k=l
(fJMij fJMile OMjk).
--

iJqle
+ --

iJqj
- --

iJqi
q/c. (5. 1 9b)

We can also see that this C(q,q) has the property that the matrix [M(q) -
2C(q,q)] becomes skew-symmetric. Hence, for any n-dimensional vector x,

(5.20)

Now we prove the convergence to qd' Substituting equation 5.15 into

equation 5.18 yields

[G/MaGr + M(q)]q + h(q,q) + [G/v.Gr + V]q

+ Kyq - Kp(qd - q) = O. (5. 21)

We let a scalar function v(x) of x = [qdT - qT,qT]T be

v(x) = tqT[G/MaGr + M(q)]q + t(qd - qfKp(qd - q). ( 5 22)
.

Then v(x) is positive definite. Using equation 5.21, the time derivative of
v(x) is
v (x) = qT{[G/MaGr + M(q)]tj + tM(q)q} - qTKp(qd - q)
= _qT[G/V.Gr + V + Ky]q + qTaM(q)q - h(q,q)}.
On the other hand, using equation 5.20 we can easily see that the expression

holds. Thus we have

(5.23)

and so v(x) is a Lyapunov function. From equation 5.23, any solution q(t)
for v (x) = 0 must satisfy q(t) = O. Such a solution that also satisfies equation
5.21 is uniquely determined by q(t) - qd = O. Therefore, by theorem 2 of
appendix 4, the equilibrium state qd is globally asymptotically stable.
170 Chapter 5

When Gr and g(q) are exactly known and the current values of q and q
are measurable, this asymptotic stability is guaranteed by the control
algorithm 5.18 even if there is no information at all about M(q), h(q,q), V,
Ma, and v.. . However, if g(q) is not known exactly, a steady-state error may
occur. A way to cope with this error is to add an in tegral action term to
equation 5. 1 8:

l'm(t) = {
Gr-T Kp [qd - q(t)] - K.q(t) + I KJqd - q(t)]dt + }
g(q) ,

where Kj is the integral feedback gain matrix.

The result presented in this subsection does not guarantee anything
about the quality of transient response other than asymptotic stability. In
particular, when Gr is comparatively small, there may be a large change in
transient response, depending on the arm configuration. Various control
schemes have been proposed to cope with this problem (see refs. 2 and
6-14), and some of them will be discussed in later subsections. Although
these schemes have not yet reached the point of common practice, they do
have great potential.

5.3 Two-Stage Control by Linearization and Servo Compensation

5.3.1 Basic Concept of Two-Stage Control

Suppose that the dynamics of a manipulator are given by equation 5. 11;

that is,

l' = M(q)ij + hN(q,tj), (5.24)

where

hN (q, q ) = h (q , tj ) + Vtj + g(q). (5.25)

Observing that [q T,tjTy may be taken as the state variables of this me­
chanical system, we consider a no nlinear- sta te feedback com pensati on
described by

(5.26)

where uq is a new input vector. Then the resulting closed-loop system is

(5.27)
Position Control 171

Linearized system
/� inearizing compensator

Ir� ------��-==---�
II
, I
I I
I T,
r
r I
r
T,=h.v(q,q)
I I r--- ______ J

Figure 5.10
Two-stage control using joint variables.

which is linear and decoupled with respect to the joint variables. Thus,
equation 5.26 is a linearizing compensation law.
Let us assume an ideal situation where equation 5.24 is an exact model
of the manipulator without any modeling error and there exists no external
disturbance to the system. Then we obtain q(t) qd(t) by assigning the
=

second derivative iid (t) of a given desired trajectory qd(t) as uq• This implies
that the desired trajectory is perfectly achieved. In practice, however, some
modeling error and some external disturbances are inevitable. The basic
idea here is to reduce their effect on the control performance by installing
a servo compensator to the linearized system (equation 5.27). Figure 5 . 1 0
i s a schematic diagram o f this two-stage approach o f linearization and
servo compensation.
Suppose that we adopt a servo compensator described by
(5.28)

Defining the error e as

e = qd - q, (5.29)

we obtain, from equations 5.27 and 5.28,

e+ Kve + Kpe = O. (5. 30)

Hence, if we set
(5. 3 1 a)
172 Chapter 5

and

Kp = diag(w/ ), (5.3 1b)

where 0 < , � 1 and We > 0, then equation 5.28 amounts to installing a

PD feedback control loop for each qj so that each component of e converges
to zero with damping coefficient ,. Therefore, we can expect that the effect
of modeling error and disturbances, if there is any, will be reduced.

Example 5.4 Suppose that we wish to apply the above two-stage control
concept using the servo compensator specified by equations 5.28 and 5.3 1
to the two-link manipulator in subsection 3 3 1 . Letting 8d
and uq = [Uql,Uqz]T and using equations 5.28 and 5. 3 1 , we have
Od2F
. . = [Odl,
[ ] [� :(Odl - Od .
� �1)
(Od2 ]
Uq 1 dl + 2'We ( dl - + we
=
Uq2 0dZ + 2,We(Od2 - (2) + We - (2)
From equations 5.26 and 3.39 we finally obtain the control algorithm:

= + + Mlluq1 + M1 2uq2,
Z
• •

'1 h1 2202
•
2h1 120102 + gl
.
'2 = hZll(}l
2
+ g2 + M21 Uql + M22Uq2· 0

So far we have considered an approach based on linearization in terms

of joint variables. However, there are cases where it is more desirable to
linearize and servo in terms of variables which are more directly related to
the manipulator tasks, such as the position and orientation of the end
effector. Hence we consider an n-dimensional output vector y given by

y = J;(q), (5.32)

and describe a two-stage control approach based on y. Differentiating

equation 5.32, we have

(5.33)

where Jy(q) = 8y/8qT. Assuming that the Jacobian matrix Jy(q) is nonsin­
gular in a certain region of q, we consider a nonlinear state feedback
compensation

(5 .34)

where uy is a new input vecto r. The closed-loop system is

(5. 35)
Position Control 173

Linearized system
r - - - - - - - -- LZ�i�e�i�.-':�m£e�s:t�
r----------- ---------- �
- --l
I
u" I
I 1 . . I
II u,,=J,(q) q I
I

I I I I
I
1I
. I
qq I I
1 I I 1
--, II ...J 1
I lu,
•

1
.-__
II
_ __

11+
M(q)J' - '(q)I-<H--=--i
L-----,r---' 1 L--
L
-- _____ -
+
-
____________________________
_J
I L-__ ...J

J
I
I

Figure S.ll
Two-stage control using output variables.

which is linear and decoupled with respect to y. Hence, as in the case of

equation 5. 27, by installing a servo compensator to the linear system 5.35,
we obtain the control system shown in figure 5 . 1 1. If we futher adopt

(5.36)

as the servo compensator, by letting e = Yd - Y we find the same equation

as 5.30.

Example 5.5 For the two-link manipulator in subsection 3.3. 1 , let us

derive the state feedback law for linearizing the dynamics with respect to
the end-effector position y = [x,y] T. Since

y
=
[
11 C1 + 12C12 ]
+
,
I1S1 12S12

the Jacobian matrix Jy(q) is

Thus,

iy(q) = -llS[-11
le1
C1 �1
174 Chapter 5

and

-J' )
y(qq-
['1 2
CJJ1 + 12 Cu(Ol + (2)2
2
]
11 SI 01. + 12S12(01 + O2 )
2
. _
• • •

Letting uy = [uYI , Uy2]T, we then have, from the above equation and 5.34,

+
2
+
•

+ 12[Mlli2 - Mu(/2 I1C2)](01 O2)

•

+ [Mlli2C12 - M12(l2C1 2 + 11 Cl)]UYl

+ [Mlli2Sl2 - M12(l2S12 + 11Sl)]Uy2}

/11/2S2
and

. 2
+ {II [M 1 2 /2 C2 - M22(l2C2 + Id]Ol
2
+ 12 [M12i2 - M22(l2 + 11 C2) ] (0 1 + (2)

+ [ M 1 2/2C12- M2 2(l2C1 2 + 11 Ctl]UYl

+ [M12/2S12 - M22(l2S12 + 11S1)]Uy2}

/11/2S2•
When O2 = 0 (that is, when the manipulator is in a singular configuration),
S2 O. This means that 't'1 and 't' 2 may become infinitely large. This is a
=

difficulty we did not have in linearization with respect to the joint variables.
We have to provide some measure to deal with this difficulty when we adopt
a linearization with respect to the output variables. 0

The two-stage control scheme is essentially the same as the computed

torque method (refs. 2, 6) or the resolved acceleration control scheme (ref.
7). The idea of two-stage control by linearization and servo compensation
has been studied by many researchers. 1 5-17
When we want to use the two-stage control scheme, we need to consider
several problems. One is the complexity of the algorithm, which usually
Position Control 175

requires digital computer processing. We therefore need to make the sam­

pling rate high enough to minimize the deterioration of control perfor­
mance due to computational delay. Another problem is the design of a
robust servo compensator. We wish to design a servo compensator that
makes the dosed-loop system rugged against modeling errors of the mani­
pulator dynamics and to external disturbances. These problems will be
discussed in subsections 5.3.2, 5.3.3, and 5.4.2.

5.3.2 Structure of Control System

The problem of computing T from equation 5.26 for given q, 4, and uq and
the problem of computing T from equation 5.34 for given q, 4, and Jy -l(q)
[ -iy(q)4 + uy ] are equivalent to the inverse dynamics problem of com­
puting T from equation 5.24 for given q, 4, and ij. The computation scheme
conceived earlier involved calculating all the terms on the right-hand side
of equation 5.24 from their analytical expressions derived by the Lagran­
gian formulation. Since this requires a large amount of computation, it was
regarded as impractical unless some simplification were to be employed.
To cope with this difficulty, a computational scheme based on the Newton­
Euler formulation was proposed. As was stated in chapter 3, whereas the
Lagrangian approach requires a number of computations on the order of
4
n , Newton-Euler computations are only of order n.

Even if we use the Newton-Euler approach, however, the amount of

computation for the linearizing algorithm 5.34 depicted in figure 5.11 may
still result in too large a sampling period for practical use. A large sampling
period is generally undesirable from the viewpoint of steady-state error,
stability, and response time. Two means of reducing this problem are
calculating certain terms in advance and employing dual sampling rates in
the controller.
The first means, which is applicable when qd or Yd is given prior to the
execution of control, is to calculate the terms hN(q,4), M(q), and Jy(q) in
equation 5.34 in advance by replacing q with qd. This can be regarded as
the linearization of the dynamics using the desired trajectory. The structure
of control system for this case is shown in figure 5.12. Although the amount
of on-line computation with this system is of course less than that of figure
5. 1 1 , the linearization is less accurate because of the difference between q
and qd. This will result in degradation of control performance. If the
improvement in performance due to the shorter sampling period is larger
176 Chapter 5

�--�------�---.

Figure 5.12
Linearization using the desired trajectory.

than the above degradation, then we can justify using the system of figure
5.12.
The second means is to distinguish the sampling period for linearization
from that for servo compensation in the control system shown in figure
5.11 (ref. 8). This concept is based on the idea that the linearization, which
requires a large amount of computation, may have a large sampling period
provided that the servo compensation has a small enough sampling period
to cover the error caused by the large sampling period of the linearization.
For example, let

(5.37a)

and

(5.37b)

Then, from equations 5.34 and 5.36, we have

(5.38)

When the values A and b are given, very little computation is needed to
obtain T from equation 5.38. Thus, if we calculate A and b with a low
sampling rate and calculate T from equation 5.38 with a high sampling rate,
we can expect a better control performance than if we do all the computa­
tion at only one sampling rate.18 Figure 5.13 shows a block diagram for
this dual-sampling-rate approach. Note that the linearizing compensation
loop is outside the servo compensation loop.

5.3.3 Parallel Processing Scheme

So far we have discussed control systems under the implicit assumption

that the necessary inverse-dynamics computation is done with only one
Position Control 177

Servo compensator

gd--i-�"'" �qo 5038 o

O _ t---l'
H<--_R_b li
L---_______�
A, b I I

� -1.L--:-:E_q_o_5_03_7_-Ir - - - - - - - j
Linearizing compensator
-

Figure 5.13
Dual-sampling-rate approach to two-stage control.

processor. However, it is of course possible to shorten the computation

time for inverse dynamics by parallel processing using a multiple-processor
system.
This approach was proposed for the first time by Luh and Lin. 19 A less
theoretical, more intuitive algorithm will be presented below to illustrate
the advantage of parallel processing. For a given n-link manipulator, we
assign one microprocessor unit (MPU) to each joint. This means that we
use n processors in all. Each processor is supposed to do all computations
related to the assigned joint. On the other hand, the computation of
equations 3.87'-3.95' is decomposed into smaller subtasks as follows:

(5/i) C;c = O{ ,
2):
"a X e.qi + e.qi'
. ..
ifR
if P
(6/»): i-I' i-I � i-I �
i "d =
i-I (i-l
Wi-l X Pi + Wi-l X Wi-l X Pi ),
178 Chapter 5

(9/i ) 1; = mi(�e + p;),

(l0/i ) �f = i1iiwi,
( 1 1/i) �g = iWi x el/w;),
( 1 2/i) iiii = �f + �g,
( 1 3/i) �p = iRHI i+1j;i+l,

(14/i) 1; �p + 9;,
Y;
=

= +
i� iA
(1S/i) �q "i Si X i'

(16/i) �r = Pi+1 X C;P'

(17Ii) i"i iRHI i+1 "HI +

{ TiTi
=
C;q + �"

ez "i, if R
(18/i) "i
=
ez 1';, ifP.
Among these subtasks, there exists a hierarchy that determines which
subtasks must be done before a particular subtask is pe rforme d This .

precedence relation is shown in figure 5.14. We want to develop an execu­

tion sequence for the subtasks that gives a short computation time under
the constraint of the precedence relation.
The above decomposition was, in fact, organized in such a way that: (1)
a sub task requiring information from other processors is done as late as
possible (subtask (7/i) needs Hpi_1, ( 1 3/i) needs H1j;+1 ' and ( 1 7/i) needs
Hl"Hd; (2) a subtask with a result needed by other processors is done as
early as possible (MPU i can pass iWi after (2/i), iWi after (4/i), and 1; after
( 1 4/i»; and (3) duplication of computation is avoided (�a is used in (2/i) and
(3/i), and �P is used in ( 1 4/i) and (16/i». We can therefore expect that the
following simple algorithm will give a fairly good performance.

Algorithm I Processor i (i 1 , 2, ... , n) executes the subtasks in the order

=

(1/i),(2/i), . . . , (18/i) as soon as data become available.

Example 5.6 Cons ider a six-link manipulator with all joints revolute.
.

Assume that the initial values of °Wo and °Wo are not necessarily 0, and
that one multiplication in MPU takes 0.005 msec and one addition 0.004
msec. The computation time needed to execute each subtask is given in
table 5.1. Using this table, we can determine the time history of the status
Position Control 179

MPU i-I MPU i MPU i+l

Figure 5.14
Precedence relation among subtasks.

of each processor for algorithm I, which is shown in figure 5.15. (The bold
solid line means that the processor is on duty; blank space means waiting
time.) We see that the total computation time of the above algorithm is
1 .745 msec. Since the computation time is 5.790 msec when only one
processor is used, the above parallel processing speeds up this computation
by a factor of 3.3. 0

We can also improve the above algorithm by changing the order of

subtasks for the terminal processor, processor n, so that it computes the
data necessary for processor n - 1 as quickly as possible:

Algorithm II Processors 1 through n 1 use algorithm I. Processor n

-

executes the subtasks in the order (13/n), (16In), (lin), (2In), (3/n), (Sin), (ll/n).
180 Chapter 5

Table 5.1
Computat ion time for each subtask.

Subtask Multiplications Add itions Computation t ime

(l/i) 8 5 00
. 60 ms
( 2/i ) 0 0 .004
( 3 /i) 2 0 0.0 1 0
(4/i ) 8 8 00
. 72
( 5/i) 0 0 0

(6 / i) 18 1 2 0 .1 3 8
(7 /i) 8 18 00
.72
( 8/ i) 18 1 2 0. 1 3 8
( 9/i) 3 3 00
. 27
( 1 0/i) 9 6 0.06 9
( ll/i) 1 5 9 0. 1 1 1
(12/i) 0 3 0.01 2
(13/i) 8 1 5 00
. 60
(14 /i) 0 3 0.0 1 2
(lS/i) 6 6 0.0 54
( 16 /i) 6 3 0.04 2

(17/i) 8 11 0.084
( 1 8 /i) 0 0 0
Total 1 17 95 0 .96 5

(4/n), (6/n), . . . , ( 1 0/n), ( 1 2/n), ( 1 4/n), (15/n), (17/n), (18/n) as soon as data
become available.

Example 5.7 Consider the six-link manipulator in example 5.6. By draw­

ing a figure similar to figure 5. 1 5 it is easy to see that the total computation
time of algorithm II is 1.604 msec, which is an 8 percent improvement from
algorithm I. A lower bound of the computation time obtained by consider­
ing a critical path shown in figure 5 . 1 6, which is completely connected by
the precedence relations, is 1.5 1 6 msec. Consequently, the difference be­
tween the computation time of algorithm II and that of the minimum-time
algorithm is less than 6 percent. 0

We can also consider algorithms that minimize the computation time

without any restriction on the correspondence between processors and
joints of a manipulator. 20
Position Control 181

MPU
1 2 3 6
o
I
I
I
I
I
o. 5

� 1.0
<>

1.5 T
I
I
..
I
1
I I
I
1 . 745 U
2.0

Figure 5.15
Processor status for Algorithm I.

Figure 5.16
A critical path giving a lower bound of computation time for Algorithm II.
1 82 Chapter 5

5.4 Design and Evaluation of Servo Compensators

5.4.1 Linear Servosystem Theory

In this section we will consider the foundations of design and evaluation

for general linear time-invariant servosystems. First, we will consider the
design of servo compensators from the viewpoint of step response in this
section.
Let us consider the object with one degree of freedom shown in figure
5. 1 7. This system is an armature-controlled d.c. motor connected to a load
through a set of reduction gears. We want to control the angular velocity
of the load. The dynamics equation of the system is derived as follows.
Electrically, the relation between the torque generated by the motor, 'm '
and the armature current, i, is

( 5 . 39)
where kl is the torque constant of the motor. The back electromotive-force
voltage, Vb ' is

(5.40)

where 8m is the rotational angle of the motor and kb is the back electro­
motive-force constant. Regarding the armature circuit, we have
di
L dt + Ri + Vb = V, (5.4 1 )

where v is the input voltage, L the inductance, and R the resistance of the
armature.
Mechanically, we have

(5.42)

L R

-0
Figure 5. 1 7
Direct-current servomotor and load.
Position Control 1 83

where 'I is the load torque on the motor, Jm is the moment of inertia, and
Dm is the viscous-damping coefficient of the annature. Letting 01 be the
rota tional angle, J1 the momen t of inertia, Dc t he viscous damping coefficien t
of the load, and n the gear ratio of the reduction system, we obtain

(5.43)

From equations 5. 39-5.43, we have the following relation between the

input v and the output °1 :

(5.44)

where

J n 2Jm + J"
= (5.45)

D = n 2Dm + D,. (5.46)

Hence, from equation 5.44, the transfer function from v to 0, is

(5.47)

which represents a third-order system co ntaini n g an integrator. The in­

d uctance L of the annature is gene rally small. If we neglect L, then the
transfer function 5.47 reduces to a sec ond o rder system containing an
-

in tegrato r :

nkt
G(s) (5.48)
s { RJs + (RD + n 2 ktkb)}
= •

With these transfer functions expressed in general forms, the controlled

objec t is represented by

(5.49)

or

(5.50)

where a 1 , a2 ,
and a 3 are appropriate constants.
One bas ic structure for a servosystem that forces the output y(t) of the
above control object to fo llow the desired trajectory r(t) is shown in figure
5. 1 8. Transfer functions G1 (s) and Gz (s) are compensators to be designed.
1 84 Chapter 5

Figure 5.18
A basic servosystem structure.

When G (s) is given by equation 5.49, it is very common to make G1 (s) a

proportional gain, that is, G1 (s) = b1 , and to make G2 (s) a velocity feedback,
that is, G 2(s) = b2s. In this case, the closed-loop transfer function becomes

we 2
(5.5 1)

which is a standard second-order form. The natural angular frequency and

the damping coefficient are, respectively,

(5. 52)

and

(5.53)

Now let us transform the Laplace operator s into a new one defined by

(5.54)

Then equation 5.5 1 becomes

(5.55)

Transform 5.54 changes the time scale from t to t = we t. The shape of the
step response in the new time variable, t, is determined only by (. For
example, if ( < 1, the step response is given by

-�
e-{i (� . �)
y (t )
_( t +
� �

1 sm tan-1 . (5.56)
=

1 2 (
The step responses for various values of ( are shown in figure 5. 1 9.
Position Control 185

15

Figure 5.19
Step response of second-order system.

When designing the compensator, one good way is first to select a

desirable shape of the step response from figure 5. 1 9 and then to make We
as large as possible within the limits of hardware in order to have a quick
response.
Now we will consider the case when G(s) is given by equation 5.50. In
figure 5. 1 8, let us make Gl (s) a proportional gain bl , as in the case of the
second-order system, and G2 (s) a velocity and acceleration feedback b2s +
b3s2 • Then the closed-loop transfer function is
a3bl . (5.57)
Gf (s) =
S
3 + (al + a3b3)s + (a 2 + a3b2 )s + a3bl
2
As in the previous case, let us transform s into s in the form

s = s/(a3 bd t/3 , (5.58)

and define scalars rx and P by

IX = (al + a3b3)/(a3bdl/3 , (5.59)

P = (a2 + a3b2 )/(a3bt ) 2/3 . (5.60)

From equation 5.57 we obtain a standard form for third-order systems:

 p -
00
0'\
o
a
Figure S.20
Step response of third-order system.
(j
::r
i
.u,
Position Control 1 87

or-­----�--�I�O--
I

Figure 5.21
Step response of third-order system (IX = 1 .3, fJ = 2.0).

1
Gf(s) = 53 + ex52 + P5 + 1. (5.6 1 )

The parameters ex and P specify the shape of the step response o f the
third-order system (equation 5.6 1 )2 1 in the same way that ' specified the
shape of the step response of the second-order system (equation 5.55). The
step responses for various combinations of ex and P are shown in figure 5.20.
As in the second-order case, a possible design procedure for the third­
order system is first to select a pair of ex and P such that a desirable step
response is obtained, and then to determine b1 so that a3b1 becomes as
large as is practical. Parameters b2 and b3 are determined from ex, p, and
b1 • In figure 5.2 1 we see the step response for the case of ex = 1 . 3 and p = 2.0.
Note that the response has a reverse motion before reaching the desired
value of 1 . This is one of the differences between third-order and second­
order systems. This characteristic can be used to reduce the settling time.

5.4.2 Stability Margin and Sensitivity

In the preceding subsection we considered the design of compensators only

from the viewpoint of step response, that is, the immedia t e response to the
command signal. Another function expected of a servosystem is that it is
not affected very much either by errors in the mathematical model of
the controlled object or by external disturbances. This property is called
robustness. As has always been recognized in the design of control systems,
one advantage of closed loop systems over open l oop ones is robustness.
- -

The design method of subsection 5.4.1 is robust enough for most situations;
however, it is not always guaranteed. In this subsection, robustness will be
1 88 Chapter 5

Figure 5.22
Servosystem with feedforward and feedback compensators.

discussed quantitatively and a method for using it in the analysis and design
of servosystems will be presented.22 - 24 For simplicity, we wi1l consider only
single-input, single-output systems.
Let us consider the servosystem shown as a block diagram in figure 5.22.
G is the transfer function of the controlled object, and Ca and Cb are those
of feedforward and feedback compensators. The scalar variables r, y, and
u are the reference input, the controlled variable, and the controlling input,
respectively. This block diagram is in a general form in the sense that
current values of both the reference input and the controlled variable are
available for determining the control input. For example, the control
system in figure 5. 1 8 is a special case of this general form, as we can see by
using

(5.62)
and

(5.63)
The response characteristic of this system with respect to the reference
input is given by the transfer function from input r to output y:

( 5 .64)
If we want to make Gy, equal to some desired transfer function, Gd, we
only have to specify Ca as

(5.65)
If the controlled object, G, has unstable zeros, Gd must also have these zeros.
Otherwise, G-1 Gd turns out to be an unstable transfer function, causing an
instability problem for the whole system.
Next, suppose that the transfer function G in figure 5.22 is perturbed and
changes into G = (1 + Lf G ) G. The transfer function Gy, from r to y changes
into
Position Control 1 89

(5.66)
Thus, we have

(5.67)
and

(5.68)
Equation 5.67 implies that S is a ratio between the change rate of the
transfer function for the controlled object, (G - G)G-l, and that for the
closed-loop system, (Gyr - Gyr)Gyr - 1 . The ratio S is called the sensitivity
function. While S is a function of the Laplace operator s, the absolute
value of SUw), I SUw) l , is more convenient for evaluating sensitivity. Using
I SUw) 1 corresponds to evaluating sensitivity from the gain of the change
rates of the transfer functions at each frequency.
On the other hand, the Small Gain Theorem gives a sufficient condition
for the closed-loop system to be stable in spite of the change of G to G (refs.
23, 24). Let us define T by

(5.69)
According to the Small Gain Theorem, if the original closed-loop system
is stable, then the perturbed closed-loop system also remains stable for any
perturbation L1 G satisfying

(5.70)
for any w. Hence, we can say that I TI-1 expresses a stability margin. From
equations 5.68 and 5.69 we have

s + T = 1, (5.71)
and T is called the complementary sensitivity function.
In lieu of a rigorous proof of the Small Gain Theorem, an intuitive
interpretation of equation 5.70 based on the Nyquist stability criterion is
developed below. Focusing on the feedback loop consisting of G and Cb in
figure 5.22, we assume that this part of the feedback loop is stable. Then,
by the Nyquist stability criterion, the vector locus of the open-loop transfer
function GCb does not pass through the point ( - 1 + OJ), as shown in figure
5.23. Let d 1 (w) be the distance between ( - 1 + OJ) and the point Pa on the
vector locus specified by GUw) CbUw) for a given w. Then d1 (w) is given by
190 Chapter 5

1m

-----.--�--��- R e

Vector locus
of GC.

Figure S.23
Interpretation of Small Gain Theorem.

(5.72)

and from the above argument it satisfies d 1 (m) > O. Now we assume that
G cha nges to G = (1 + LlG)G, and that the number of unstable zeros re­
mains the same in spite of this change. The distance between Pa and the
point Pb specified by G( jm)Cb( jm) is given by

(5.73)

Hence, if AG satisfies equation 5.70, we have d2(m) < d1 (m) for all m, which
implies that the numbers of rotation about ( - 1 + OJ) of GCb and G Cb are
identical. Because of the Nyquist stability criterion, the stability property
of the closed-loop system remains the same despite the change from G to
G. Since the original system is assumed to be stable, the perturbed system
is also stable.
Since the modeling error of the controlled object can be regarded as a
perturbation LlG of G, the functions S and T may be interpreted as indices
for the robustness of the control system against the modeling error of G (S
representing the sensitivity aspect of the robustness and T representing the
stability-margin aspect). Another interpretation of S and T is also possible:
If we consider the control system shown in figure 5.24, obtained by adding
a system disturbance d and a measurement disturbance dn to the original
system of figure 5.22, it is easy to show that S is the transfer function from
d to y and T is that from dn to y.
-
Position Control 19 1

Figure 5.24
Servosystem with system noise and measurement noise.

Roughly speaking, for both of the above interpretations of S and T, it is

desirable for both l S I and I TI to be small; however, because of the relation
5.71 it is impossible to make both of them arbitrarily small simultaneously.
One reasonable compromise is to make l SI small in a lower-frequency
region and I TI' small in a higher.frequency region. At higher frequencies
the modeling error is usually large, so it is desirable to have a large stability
margin (in other words, a small I TI ). At lower frequencies the modeling
error is usually small, making it possible to give priority to reducing the
sensitivity. Also, when we consider the interpretation of S and T as transfer
functions from disturbances d and dn to y, the system disturbance d usually
consists oHow-frequency components and dn of high-frequency ones. Again
we see that it is desirable to have small l S I at low frequencies and small I TI
at high frequencies. Because S and T are functions of Cb but not of Ca, the
robustness properties are determined by the feedback compensator Cb only,
whereas the overall transfer function from r to y is determined by Cb and
the feedforward compensator Ca.
2
Example 5.8 Consider the case when G = 1/s , Ca = b1 , and Cb b1 +
b2 s in figure 5.24. This is identical to the case of figure 5. 1 8 with G 1 (s) = b1
=

and G2 (s) = b2 s. Let us examine this commonly used servosystem from the
viewpoint of robustness. From equations 5.68 and 5.69 we have

and

b2 s + b1
=
b1
T 2
S + b2 s +
1 92 Chapter 5

2or-----,----,--r---�

O r-----+---���--_+----_4

� - 20 r-----�--�_+----+_�--�

- 40r-----�--_+--+_--�

We 10 We 100 We
radlsec

Figure S.2S
l S I and I T I for example 5.8.

The closed-loop transfer function Gy• is

b1
- S2 + b2 s + bl
GY
_

' •

By supposing that ' = 1 and letting We = A, we have

b2= 2we•
For this b2, l S I and I T I
are shown in figure 5.25 in units of decibels (dB).
Note that the frequency We is an index locating the boundary between the
frequency region in which the sensitivity is the important design criterion
and the region in which the stability margin is important. Thus, if we specify
We properly to take into consideration the extent of modeling error and the

frequency of external disturbance, we can expect that the servosystem has

an acceptable amount of robustness. 0

5.5 Decoupling Control

5.5.1 Theory of D ecoupling Control for Nonlinear Systems

In this section, we will first consider the theory of decoupling control for
nonlinear systems; then we will develop its application to the control of a
Position Control 193

manipulator (ref. 9). This theory, used for achieving linearization of dynam­
ics, decoupling among output variables, and arbitrary assignment of poles,
also provides a theoretical grounding for linearizing compensation in
two-stage control (see section 5.3).
Consider a nonlinear, time-varying system with n inputs and n outputs:

x(t) a(x, t) B(x, t)u(t),

= + (5.74)

y(t) c(x,t) D(x,t)u(t),

= + (5.75)

where x is an n.-dimensional state vector, u is an n-dimensional input

vector, is an n-dimensional output vector, and a (x,t), c(x,t), B(x, t ), and
y
D(x,t) are known vectors or matrices with appropriate dimensions. A
design procedure is developed for which individual elements of the output
y have arbitrarily assigned poles and no longer interact with one another
by applying to the system a nonlinear time-varying state feedback law:

u(t) e(x,t) + F(x, t)ii(t),

= (5.76)

where ii( t) is a new n-dimensional vector.

Let the ith row vector of D (x, t) be di(x, t), let the ith element of c(x,t) be
ci(x,t), and let the ith element of y be Yi' Also, let the jth derivative of Yi
with respect to time be y /J1. Then, by repeating the diffe rentiation of Yi and
the substitution of equation 5.74 until the coefficient of u becomes nonzero,
we obtain, for a positive integer Vi '

YiUl cPl(x,t), j 0, 1, .
= = . . , Vi - 1 (5. 77a)

yti) c/".J(x, t) + d/"i l (x,t) u,

= (S.77b)

where Y/O) Y i, C i[ O l(x,t) Ci(X, t), dlOl(x, t) di(x,t), and

= = =

C/11(X, t) :t YiU -1) [a� T yY-1 ] a(x, t),

= + (5.78)

dPl(x,t) [a�TYiU-l)JB(X, t).

= ( 5 9)
.7

The integer Vi may be written as

Vi min{j:dlJl(x,t) # 0, j 1, 2, . . }.
= = . (5.80)
194 Chapter 5

Assume that Vi is constant for any x and t in a region under consideration.

Further, define

(5.8 1)

(5.82)

(5.83)

and

(5.84)

where col [ · ] denotes a column vector, diag [ · ] denotes a diagonal matrix,

and Clik and Ai are arbitrary constants. If D * (x,t) is nonsingular, then we
can decouple the system with respect to the output by setting e(x,t) and
F(x,t) of equation 5.76 as
e(x,t) = D*-l (X,t) [c*(x, t) + Gt* (x,t)]
- (5.85)

and

F(x,t) = D* -l (X,t)A. (5.86)

In fact, when equations 5.76, 5.85, and 5.86 are substituted into equations
5. 74 and 5.75, we obtain

(5.87)

where i = 1 , 2, , n . Appropriate selection of Clik and Ai provides this

. . .

single-input, single-output system with the desired pole placement and

input gain.

Example 5.9 Let us derive a decoupling feedback law for the following
two-input, two-output linear system:
Position Control 195

X= �
[ -2 -1
-4
-2 ° °
�}+ [� �}
[� �JX.
°
y
=

S i nce

Yl = [1,O,O] x,

Yl( l ) = [ - 2, - 1, O] x + [1 ,O] u,
Y2 = [O,O, l ] x,
{l)
Yz = [0, 1 , - 2] x,
(2 )
Y2 = [2, - 6,4] x + [O, I ] u.
We have Vl = 1, and V2 = 2. From equations 5.8 1 and 5.82,

c *(x, t) = [ - � =! �J x

and

D*(x, t) = [� �J
Since D* is nonsin gular, we obtain from equations 5.76, 5.85, and 5.86 the
feedback law:

u = _[� �il� _t- -6--=l��1- -t- �4-+ 2�2� -=-=-�2�JX + [�l �Ju.
Applying this law yields the decoupled dynamics:

If we further wish to assign a pole of

to Y t , and two poles of { - 6,
- 5 to the transfer function from ill
to t hat from il 2 to Y2 ' then the arbitrary
= 5, (1.2 1
- 6}
constants (1.ij should be (1. 1 0 = 1 2, and (1. 2 0 = 36. 0
1 96 Chapter 5

5.5.2 Application to Manipulators

Now we will apply the above theory to manipulators (ref. 9). Let us consider
a general n-link manipulator with its dynamics described by equation 5. 1 1;
that is,

(5.88)
where hN (q, q) is a function defined by equation 5.25. We assume that the
n-dimensional output vector y is given by equation 5.32:

y = /y(q). (5.89)
We also assume that the dynamics of the actuators producing the joint
driving force T are fast enough in comparison with those of the link
mechanism that we can regard T as a directly operable input vector. In this
case, [q T, q T ] T becomes a state vector and equation 5.74 is given by

(5.90)

For the system given by equations 5.89 and 5.90, we have Vi = 2 (i = 1 , 2, . . . ,

n) and

(5.9 1)
As was explained in subsection 2.5.6, arm configurations for which
rankly(q) < n are called singular configurations. For any configuration
except singular ones, D * is nonsingular, and so decoupling is possible.
Letting I; (q) denote the ith row vector of Iy(q), we obtain from equations
5.76, 5.85, and 5.86 the state feedback law
T =
hN(q, q) + M(q) ly -1 (q) col [ - .i;(q)q - IXi I :Vi - IXiO Yi + A i U;], (5.92)
which yields a decoupled system with the linear second-order property

(5.93)
The constants lXi i ' IXiO ' and A i are to be determined by the designer using,
for example, the approach of section 5.4.
The feedback control law (equation 5.92) for decoupling is equivalent to
two-stage control in that both laws compensate for hN(q, q), the term for
centrifugal, coriolis, frictional, and gravitational forces. The block diagram
Position Control 197

of the control law is given by figure 5. 1 1 when the block of the servo
compensator is specified by

5.5.3 Consideration of Actuator Dynamics

So far we have treated the joint driving force T as the input, assuming that
T can be changed arbitrarily in any short period of time. This assumption
holds, for example, for d.c. servomotors with negligible inductance, or for
hydraulic cylinders or motors with negligible oil compressibility and little
pipeline elasticity. When we consider the control of lightweight, high-speed,
high-accuracy manipulators, however, there are cases where actuator dy­
namics are significant. Application of the decoupling control theory to such
cases is discussed in this subsection. 2 s
Let u s assume again that equation 5 . 8 8 gives the dynamics o f the mani­
pulator. Let us also assume that the dynamics of the actuator are given by

p = r(q,q,p) + G(q,q,p)u ( 5.94)

and

T = I(q,p), (5.95)
where is an n-dimensional input signal vector to the actuators and p is
u
an n-dimensional actuator state vector. The state p represents, for example,
the armature current in a d.c. servomotor, or the operating pressure in a
hydraulic actuator. In equations 5.94 and 5.95, r(q,q,p) and I(q,p) are
n-dimensional vectors, and G(q,q,p) is an n x n matrix. It is assumed that
G(q,q,p) and ol(q,p)/OpT are nonsingular in the required range of (q,q,p).
When individual elements of T are generated by different actuators, G(q,q,p)
and o/(q,p)/OpT are generally diagonal, and so the above assumption is
valid. Under these assumptions, we can employ [q T, q T,pTF as the state
vector of the total system, and equation 5.74 becomes

:t [:] [
p
= ;
M-l (q) [I(q, � - hN(q,q)]
r(q,q,p)
] [ : ]u.
+ .

G(q,q,p)
(5.96)

For the output given by equation 5.89, the value of Vi given by equation
5.80 is Vi = 3 (i = 1 , 2, , n) and we have
. . .
1 98 Chapter 5

(5.97)

and

c
*
;::
J(q)M (q)
-1 [
(q q )
8pT r , ,p
81 .
+
01 .
o qT q -
dhN(q,q)
dt
]
+
[ J(q)
dM-1 (q)
+
. ]
2J(q) M-1 (q) [/(q,p) - hN(q,q) ]
dt
J(q)q. (5.98)
+
Since D * is nonsingular (owing to the assumptions about the matrices on
the right-hand side of equation 5.97), we can decouple the system using the
state feedback law obtained from equations 5.97, 5.98, 5.85, 5.86, and 5.76.
The resulting dosed-loop system is

(5.99)
This is a linear third-order decoupled system for each output element.
When the output y is the joint vector q itself, equations 5.97 and 5.98
simplify to

(5. 100)

and

* _ M -1 (q)[o01 .
+
81 . dhN(q,q) ]
c -
pT r ( q, q,p) oq T q - dt
dM-1 ( q) .
+ [I(q,p) - hN(q, q)]. (5. 1 01)
dt

Example 5. 1 0 Let us consider a d.c.-servomotor-driven n-link mani­

pulator with its mechanical elements modeled by equations 5. 1 1 -5. 1 3. We
will decouple the system dynamics with respect to the joint vector q, taking
into consideration the dynamics of the electrical elements. Let ij and Vj be
the armature current and the input voltage of the jth motor, and let

and
Position Control 1 99

Assume that the electric circuit of the motors is described by

Tm = Kli, KI = diag [ ktj],

di '
L + R, + Vb = V, L = diag [Lj], R = diag [Rj],
dt

Vb = Kb qa , Kb = diag [kbj],

where klj, kbj, Lj, and Rj are the torque constant, the back electromotive­
force constant, the inductance, and the resistance of the jth motor, respec­
tively. If we assume that equation 5.88 represents the system of actuators
and the arm given by equation 5. 1 5, the dynamics of the actuator (equations
5.94 and 5.95) are given by

- C 1 (Ri + KbGr q) + L-1 v

di
=
dt

and

From equations 5. 100, 5. 1 0 1 , 5.85, 5.86, and 5.76, we can see that the
feedback law

+ LKI -1Gr - T {M(q)M- 1 (q) [ G/Kl i - hN(q, q)]

+
dhN(q,q)
dt
_
M(q) (ot * - AU)}
achieves the linearization and decoupling with respect to q. 0

5.6 Adaptive Control

The two-stage and decoupling control methods developed above require

an accurate dynamic model of the manipulator being controlled. These
control schemes are not always effective when the dynamics of the mani­
pulator are not known exactly beforehand, or when they change because
200 Chapter 5

of uncertain factors (including the weight of an object grasped by the end

effector and friction at the joints). One control scheme proposed to cope
with such cases is ada ptive control, 26 which uses some means to identify the
unknown dynamics or to detect the change in the dynamics of the con­
trolled object caused by changes in the environmental and/or the opera­
tional conditions. The parameters of the controller are adjusted on-line on
the basis of the information gathered.
Various adaptive-control algorithms have been proposed to cope with
the situation where the form of the dynamics equations of a manipulator
is known but some of the dynamic parameters are not known exactly (refs.
1 0- 1 2, 27, 28). An adaptive-control algorithm which is a modification of
that in reference 1 1 will be developed in this section. This algorithm is
composed of a two-stage-control law and a parameter-estimation law. The
control and estimation laws are chosen so that the error between the
desired and actual joint trajectories converges to zero. Although we assume
that the unknown parameters are constant, this adaptive-control algorithm
is practically valid also for the case when the parameters are piecewise
constant or slowly varying with time.
We assume that the manipulator dynamics are described by equation
S. l l -that is,

(S. l 02a)

where M(q) is the inertia matrix satisfying M(q) = M T (q) > 0 and where
hN(q,tj) is the vector defined by equation S.2S which represents centrifugal,
Coriolis, frictional, and gravitational forces. To indicate explicitly that
these are functions of the parameter vector ; consisting of the dynamic
parameters and viscous friction coefficients, we write M(q) and hN(q, tj) as
M(q, ;) and hN(q, tj, ;). Then we have

(S. l02b)

From equation 3. 1 44, we know that the right-hand side of equation 5. l 02b
can be expressed as a linear function of t/J:

M(q, t/J)ii + hN(q,tj, t/J) = K(q, tj,ij) t/J. (S. 1 03)

When we wish to remove the redundancy of parameters, we may replace

t/J and K of equation 5. 103 by t/J/ given by equation 3. 1 24 and the corre­
sponding K/" respectively. We will assume that this replacement is done,
and t/Jd* and Kd* are denoted by ; and K hereafter in this section. We will
Position Control 201

I
Initial condition
qm(to) = q(to)
qm(to) = q(to)

(a) (b)

Figure 5.26
Modified desired trajectory q (t) (t > to) at time t o ' (a) Generator of modified desired
..

trajectory. (b) Original and modified desired trajectories.

also assume that q and q are measurable, that the desired trajectory for q,
qd (t), is known, and that its time derivatives qd and lid exist and are finite.
Under these assumptions, we can construct an adaptive-control system in
which the error between q and qd converges to O.
First, assuming that an estimate � of t/J is given, we consider the following
control algorithm (similar to equations 5 .26 and S.28):

T = M(q, �)lim + hN(q,q, �) + Kve + Kpe, (S. 1 04)

lim = lid + Uti + .il2 e, (S. 1 0S)

where e is the tracking error given by

e = qd - q, (S. 1 06)

.it is an appropriate positive constant, and Kv and Kp are square matrices

(which may be functions of time and which will be determined later). The
vector lim can be interpreted as a modified desired acceleration produced
by applying a second-order fIlter to the original desired acceleration with the
initial condition set to the current joint position and velocity as shown in fig­
ure S.26a. In fact, let the current time be to, and let fm(t) (t ;?; to ) be given by

qm (t) = qd(t) - e- A(I-Io)e(to ) - (t - to)e- A(I-Io) [e(to) + .il e(to )]. (S . 1 07)

Then, by differentiating equation S. 1 07, we get

qm(tO ) = q(to ), (S. 1 08a)

qm (t O ) = q(to ), (S. 1 08b)

lim( to ) = lid (tO ) + 2 .il e(to) + .il2 e(to ), (S. 1 08c)

202 Chapter 5

q, q

Figure S.27
Block diagram of adaptive control system.

showing that qm(t) (t � to) is a modified desired joint trajectory that starts
with the current position and velocity and converges to the original desired
trajectory (see figure 5.26b). The last two terms on the right-hand side of
equation 5. 1 04, Kv e + Kpe, are proportional and differential control terms
to compensate for the performance deterioration due to the parameter­
estimation errors.
For updating the estimate �, we use the following parameter-estimation
law based on the tracking error:

(5. 1 09)
where r is a constant positive-definite symmetric matrix, and where
K(q,q,ijm ) is given by equation 5. 1 03 with ij = ijm.
The resulting adaptive-control algorithm consists of the control law
(equation 5. 1 04) and the estimation law (equation 5. 1 09). Figure 5.27 shows
the block diagram of the adaptive control system.
We will now prove the convergence of the tracking error e to 0 as time

Noting that the vector x = [eT,e T,( � - �) T ] T is a state vector of the adap­
tends to infinity by using the Lyapunov stability theory (see appendix 4).

tive control system, we define a scalar function

v(x) = t(e + Ae) TM(q, �) (e + Ae) + t e T Kl e

+ t (� - �fr�l (� - �), (5. 1 1 0)
where Kl is a constant symmetric positive-definite matrix. Obviously, v(x)
is a positive-definite function. Differentiating v(x) with respect to time and
using equation 5. 1 06 and � = 0, we have

v(x) = (e + AefM(q,�) (ijd - ij + M) + t (e + Ae) TM(q,�)(e + Ae)

(5. 1 1 1)
Position Control 203

By equations 5. 1 02b, 5. 1 03, and 5. 1 04,

v (x) = - (e + ?ee)T [?e M(q, ,p) - t M( q, ,p) + Kv] (e + ?ee)

- e T( Kp - ?eKv - Kd e - ?ee T(Kp - ?eK. )e

- (e + ?eefK(q, q, ijm)( � - ,p) + ( � - ,pfr-l �. (5. 1 1 2)

Substituting equation 5. 1 09 into equation 5. 1 1 2 cancels the last two terms

on the right-hand side.
If we can choose a velocity feedback gain Kv satisfying either

Kv - tM(q, t/J) + ?e M(q, ,p) � 0 (5. 1 1 3a)

or

Kv - tM(q, t/J) � 0, (5. 1 1 3b)

then by choosing the position feedback gain Kp such that

Kp = ?eKv + Kl (5. 1 1 4)

we obtain

v (x ) = - (e + ?ee)T [?e M(q, ,p) - tM(q, t/J) + K. ] (e + ?ee) - ?ee TKl e

� O. (5. 1 1 5)

Hence, v(x) is a Lyapunov function.

By theorem 3 in appendix 4, e and e converge to 0 as time tends to infinity.
In other words, the joint trajectory q(t) converges to its desired trajectory,
qd (t) . Thus we have proved the validity of the adaptive-control algorithm
consisting of the control law (equation 5 . 1 04 with conditions 5. 1 1 3 and
5. 1 14) and the estimation law (equation 5. 1 09).
The above algorithm requires some a priori information on the bound
for values of [M(q, ,p)/2 - AM(q, ,p)] or M(q, t/J)/2 that may occur during the
control. Even when this information is not available, however, the above
adaptive-control approach is still valid with the following slight modifica­
tion of the parameter-estimation law (equation 5 . 1 09) and the condition for
choosing Kv (equation 5. 1 1 3): Since M(q, t/J) is linear in ,p, so is M(q, ,p).
Hence, there exists a matrix KM(q, q, e, e) such that
/

( 5. 1 1 6)
Using this relation, we can rewrite equation 5. 1 1 2 as
204 Chapter 5

v(x) = - (e + 2e)T [2 M(q, tP) - !M(q, �) + K.] (e + 2e)

- e T ( Kp - 2K. - Kl ) e - 2e T (Kp - 2 K. ) e

+ (e + 2e)T [ K(q,q,qm) + KM (q,q,e,e) ] ( � - tP)

+ (� - tPfr - l �. (5. 1 1 7)
Suppose we use the parameter-estimation law

� = r[K(q, q,qm ) + KM(q,q,e,e)] T(e + 2e) (5. 1 1 8)

instead of equation 5. 1 09, and we choose a velocity feedback gain K. such
that
K. - ! M(q, �) � 0, (5. 1 19)
instead of equation 5.1 1 3b. Then we have

v x
( ) = - (e + 2e)T [2 M(q, tP) - !M(q, �) + K.] (e + 2e) - 2e TKl e � o.
(5. 1 20)
Hence the tracking error e converges to 0, proving the validity of the
modified adaptive-control algorithm consisting of the control la w (equa­
tion 5.104 with conditions 5. 1 19 and 5. 1 14) and the estimation law (equa­
tion 5.1 1 8). A simple choice of K. satisfying equation 5. 1 19 is, of course,

K. = ! M(q, �).

Since equation 5.20 holds, we can replace M(q, tP)/2 in the development
of this section by C(q, q, tP) given by equation 5. 1 9 , with an additional
argument tP to express its dependency on tP. This could be ·used to reduce
the computational load in the above algorithms.

Example 5. 1 1 Let us apply the above adaptive-control scheme to the

two-link manipulator of figure 3. 1 1 . By regarding tPd of equation 3.141 and
Kd of equation 3 . 1 42 as the new tP and K( fJ, iJ,ih we obtain

T = M(fJ, tP) + h(fJ,iJ, tP) + g(fJ, tP) = K(fJ,iJ, O) tP, (5. 1 2 1 )

where
Position Control 205

�1 2 + 1 1 �Z I m 1 §1x + l1 mz
�1 3 m l §lY
2 z
�1 7 + 11 �2 1 fIZZ + Il m 2
,= =
(5. 1 22)
�Z 2 m2 §zx
�2 3 m2 §2y
�2 7 f2 ••
and

(5. 1 23)

From equations 3. 1 32 and 5. 1 22, we have

[ �/ � J
O 2
t M(O, t/J) = -h
O 2 . ( 5. 1 24)

where

(5. 1 25)

With these preparations, from equations 5. 1 04 and 5. 1 05, the control law is

(5. 1 26)

(5. 1 27)

where fJd is the desired trajectory of fJ, and e = fJd - fJ. First, consider the
case where we have the a priori information

3 � �zz = m2 §2 x � 6, (5. 1 28a)

- 1 � �2 3 = m2 §2Y � 1 , (5. 1 28b)

1 0;! � 2n, i = 1 , 2. (5. 1 28c)

For this case the velocity feedback gain Kv in equation 5. 1 26 can be

determined as follows. For simplicity, let Kv = kv l. Then the condition
5. 1 1 3b becomes

(5. 1 29)

By Sylvester's criterion (theorem 4 in appendix 4), condition 5. 1 29 is satis­

fied if and only if all its principal minors are non-negative:
206 Chapter 5

kv + h 82 � 0, kv � 0, (5. 1 30a)
kv(kv + h(2 ) - h2 8//4 � o. (5. 1 30b)
Hence, we can choose any kv such that

kv � 2 .)3711: 1 / 1 1 � - h 82 · (5. 1 3 1)
If we let K1 k 1 I (k 1 > 0), from equation 5. 1 14 the position feedback gain
=

Kp in equation 5. 1 26 is
(5. 1 32)
With r = yI (y > 0) in equation 5.109, the parameter-estimation law is
given by

(5. 1 33)
The resulting adaptive controller consists of equations 5. 1 26, 5. 1 3 1 , 5. 1 32,
and 5. 1 33. Next, consider the case where no a priori information on the
parameters (such as equation 5.1 28) is available. From equations 5. 1 1 9 and
5. 1 14, Kv and Kp in equation 5. 1 26 are, for example, given by

(5. 134)

and

(5. 135)

where

h =
11 (S2�22 + C2�23).
Substituting equations 5. 1 24 and 5. 125 into equation 5. 1 1 6, we obtain

KM(O,iJ, e,e)t/J

=
- 8
[
82
2 /2
Thus, we have
Position Control 207

From equation 5 . 1 1 8, the parameter-estimation law is

�
tP = y [K(O,O,Om)
• "

+
•

KM(O,O,e,e)] (eT + le). (5. 1 36)

The resulting adaptive controller consists of equations 5. 1 26, 5. 1 34, 5. 1 35,

and 5.1 36. 0

Exercises

5.1 For the three-link manipulator shown in figure 5.28, use method 1 of
subsection 5. 1 . 1 to determine a trajectory to bring the manipulator state
from rest at the initial position in figure 5.28a to a complete stop at the
final position in figure 5.28b in 1 second. Assume that L1 = 0.2 second.

5.2 Solve exercise 5 . 1 using the interpolation of end-effector position

variables instead of joint variables.

5.3 Give an algorithm to determine k and IX such that R(k,lX) = R for any
given value of the rotation matrix R.

5.4 Using the single-axis rotation method, specify an orientation trajec­

tory to bring the end effector from rest at the initial orientation of figure
5.29a to a complete stop at the final orientation of figure 5.29b in 2 seconds.
Adopt the interpolation by a quintic polynomial function of time.

5.5 Solve exercise 5.4 using the double-axis rotation method. Using dia­
grams, show the difference between the mid orientations at time t = 1 for
both the single-axis and the double-axis rotation method.

z z

x
x
(b)
(a)

Figure 5.28
Three-link manipulator. (a) Initial position. (b) Final position.
208 Chapter 5

Zoo
Zoo

y,
\
\

y-:;r�-.--'---<>I x.
I
I
I
,
/

x" x" � - - - _ _ - - - - - z,

x,

(a) (b)

Figure 5.29
End-effector orientation. (a) Initial. (b) Final.

5.6 For the parallel-drive two-link manipulator treated in sub section

3.3.3, deri v e a state feedback law that linearizes the dynamics with respect
to the end-effector position y [x,y] T . =

5.7 Rewrite the step response (e qu ati on 5.6 1) of a third-order system using
parameters a, ( and We defined by
,

+ (J.52 + /ls + 1 2 +
53 =
(5 + a) (5 2(wcs + w/ ).
Explain why the response for (J. = 1 . 3 and /l = 2.0 involves a backward
motion before reaching the desired value 1 .

5.8 Consider a nonlinear system with two-dimensional state vector x =

[X 1 ,X2Y:

y
= [� �] x.
Give a state feedback law that decouples this system and allocates all poles
at - 3.

5.9 Design an adaptive controller for the parallel-drive two-link mani­

pulator in figure 3. 1 6.
Position Control 209

References

1. M. Brady, "Trajectory Planning," in Robot Motion, ed. M. Brady et a\. (MIT Press, 1 982).
2. R. P. Paul, "Modeling, Trajectory Calculation and Servoing of a Computer Controlled
Arm," Stanford Artificial I ntelligence Laboratory memo AIM 1 77, 1 972.
3. R. P. Paul, Robot Manipulators: Mathematics. Programming. and Control (M IT Press,
198 1 ).
4. M. Takegaki and S. Arimoto, "A New Feedback Method for Dynamic Control of Mani­
pulators," AS M E Journal of Dynamic Systems. Measurement. and Control 102 ( 1 98 1 ): 1 1 9- 1 25.
5. S. Arimoto and F. Miyazaki, "Stability and Robustness of PID Feedback Control for
Robot Manipulators of Sensory Capability," in Robotics Research: The First International
Symposium, ed. M. Brady and R. Paul (MIT Press, 1984), pp. 783-799.
6. A. K. Bejczy, "Robot Arm Dynamics and Control," Jet Propulsion Laboratory, California
Institute of Technology, report TM3 3-669, 1 974.
7. J. Y. S. Luh, M. H. Walker, and R. P. C. Paul, "Resolved Acceleration Control of
Mechanical Manipulators," IEEE Transactions on Automatic Control 25, no. 3 (1980): 468-
474 .
8. O. Khatib, "A Unified Approach for M otion and Force Control of Robot Manipulators,"
IEEE Journal of Robotics and Automation 3 (l987): 43-53.
9. E. Freund, "Fast Nonlinear Control with Arbitrary Pole-Placement for Industrial Robots
and M anipulators," International Journal of Robotics Research I, no. 1 ( 1982): 65 78 .
-

10. J. 1. Craig, P. Hsu, and S. Sastry, "Adaptive Control of Mechanical Manipulators,"

International Journal of Robotics Research 6, no. 2 (1 987): 1 6-28.
I I . 1. 1. Slotine and W . Li, "On the Adaptive Control of Robot Manipulators," International
Journal of Robotics Research 6, no. 3 ( 1 987): 49-59.
1 2. R. H. Middleton and G. C. Goodwin, "Adaptive Computed Torque Control for Rigid
Link M anipulators," in Proceedings of the 25th IEEE Conference on Decision and Control
( 1 986), pp. 68-73.
1 3. J. S. Albus, "A New Approach to Manipulator Control: The Cerebellar Model Articula­
tion Controller (CMAC}," ASM E Journal of Dynamic Systems. Measurement. and Control 97
( 1 975): 220-227.
14. S. Arimoto, S. Kawamura, and F. Miyazaki, "Bettering Operation of Robots by Learn­
ing," Journal of Robotic Systems I, no. 2 ( 1 984): 1 2 3 - 1 40.
1 5. E. G. Gilbert and I. J. Ha, "An Approach to Nonlinear Feedback Control with Applica­
tions to Robotics," in Proceedings of the 22nd IEEE Conference on Decision and Control
( 1 983), pp. 1 34-1 38.
1 6. 1. J. Craig, Introduction to Robotics (Addison-Wesley, 1 986).
1 7. M. W. Spong and M. Vidyasagar, "Robust Linear Compensator Design for Nonlinear
Robotic Control," IEEE Journal of Robotics and Automation 3, no. 1 ( 1 987): 345-35 1 .
1 8 . T. Yoshikawa and M . Nose, "Study o n Dynamic Control o f Manipulators using Hier­
archical Structure Method," Transactions of the Institute of Systems. Control. and Information
Engineers I, no. 7 ( 1 988): 25 5-262 (in Japanese).
19. J. Y. S. Luh and C. S. Lin, "Scheduling of Parallel Computation for a Computer­
Controlled Mechanical Manipulator," IEEE Transactions on Systems, Man, and Cybernetics
1 2, no. 2 ( 1 982): 2 14-234.
210 Chapter 5

20. H. Kasa h a ra and S. Narita, " Pa ra ll el P rocess i n g of Robot-Arm Con tro l Co m pu ta tio n on
a M ultiprocessor System," IEEE Journal of Robotics and Automation 1 , no. 2 ( 1 985): 104- 1 1 l
2 1 . D . S ilj ak , Nonlinear Systems (Wiley, 1 968).

22. L M. Horowiz, Synthesis of Feedback Systems (Ac ademi c , 1963).

23. M. 1. Chen and C. A. Desoer, "Algebraic Theory for Robust S t abi li t y of ln terconnected
Systems; Necessary and Sufficient Conditions," IEEE Transac tions on Automatic Con trol 29,
no. 6 ( 1 984): 5 1 1 -5 1 9.

24. J. C. Doyle and G. S tein , "Multivariable Feedback Design: Concepts for Classical!
Modem S yn thesis," IEEE Transactions on Automatic Control 26, no. 1 ( 1 9 8 1 ): 75-93.

25. T. Yoshikawa, "Multi-Variable Control of Robot M a nipu lators," Advanced Robotics 2,

no. 2 ( 1 987): 1 8 1 - 1 9 1 .

26. Y . D . Landau, Adaptive Control- The Model Reference Approach (Marcel Dekker, 1 979).

27. 1. 1. Siotine and W. Li, "Adaptive M an i pul a tor Control: A Case Stu dy, " IEEE Transac­
tions on Automatic Control 33, no. 1 1 ( 1 988): 995- 1 003.

28. D. S. Bayard and J. T. Wen, "New Class of Control Laws for Robotic Manipulators, Part
2:Adaptive Case," International Jo ur na l of Control 47, no. 5 ( 1 988): 1 387- 1 406.
6 Force Control

Many of the tasks we wish to perform using manipulators require control

not only of the position of a manipulator but also of the force exerted by
its end effector on an object. Assembly, polishing, deburring, opening and
closing a door, and turning a crank are typical examples of such tasks. Two
methods have been developed for force control: impedance control and
hybrid control. l
Impedance contro12•3 aims at controlling position and force by adjusting
the mechanical impedance of the end effector to external forces generated
by contact with the manipulator's environment. Mechanical impedance is
roughly an extended concept of the stiffness of a mechanism against a force
applied to it. Impedance control can further be divided into passive­
impedance and active-impedance methods. In the passive-impedance meth­
od, the desired mechanical impedance of the end effector is achieved by
using only mechanical elements, such as springs and dampers. The active­
impedance method, on the other hand, realizes the desired mechanical
impedance by driving joint actuators using feedback control based on
measurements of end-effector position, velocity, contact force, and so on.
Hybrid contro14 uses two feedback loops for direct separate control of
position and force. In this method, the directions in which the position is
controlled and those in which the force is controlled are determined for a
given task during operation. Current position in position-control directions
and current force in force-control directions are measured. Corrections
based on these measurements are applied by joint actuators in order to
make them follow the desired position and force trajectories.
Procedures for implementing these two methods will be developed in
this chapter.

6.1 Impedance Control

6.1.1 Passive-Impedance Method

Let us consider the task of inserting a peg into a hole with a tight clearance.
If we wish to do this with a manipulator capable of position control only,
then we need extremely accurate position control. Insertion of a peg re­
quires the peg axis to line up almost exactly with the hole axis. If a
significant discrepancy exists between the center axes of the peg and the
hole in either distance or direction, then the insertion may not be accom­
plished, because of jamming or wedging. To overcome this difficulty using
212 Chapter 6

(a) (b) (c)

Figure 6.1
Conceptual sketch of RCC hand. (a) Structure. (b) Compliance to horizontal force. (c)
Compliance to moment.

a mechanical device, the Remote Center Compliance (RCC) hand was

developed 5 This is a hand with great elasticity in certain directions, making
.

it suitable for peg-in-hole tasks. A conceptual sketch of the mechanism is

shown in figure 6.1a. Each of the four rigid parts Sl-S4 has two springs,
one at either end, illustrated by short solid lines. These springs can bend
but are neither expandable nor compressible. The axes of Sl and S4 are
parallel; however, those of S2 and S3 intersect at point Oc, near the end of
peg P grasped by fingers Fl and F2•
Let us assume that the axes of the peg and the hole are parallel but do
not coincide. When the hand moves down vertically toward the hole, it
usually gets into the state shown in figure 6.1 b, with the wall of the hole
exerting a force on the peg. Owing to the horizontal force applied at the
tip of the peg, the springs on Sl and S4 bend, but those on S2 and S3 do
not. Consequently, the peg moves horizontally without changing orienta­
tion, and it slides into the hole. When the axes of the peg and the hole are
not parallel, the hand usually pushes the peg into the hole until it jams (the
state shown in figure 6.1c). Owing to the moment applied at the tip of the
peg, S 2 and S3 bend this time, turning the direction of the peg axis closer
to tha t of the hole. Thus, the insertion can be accomplished just by moving
the hand vertically downward.
Force Control 213

Figure 6.2
Three-dimensional RCC hand.

A three-dimensional version of the above mechanism is shown in figure

6.2. The axes of the three outer springs are parallel; those of the three inner
springs intersect at point Oc t near the tip of the peg. When we assign a
frame �c with its origin at Oc and with its three axes parallel to those of
the hand frame. as can be seen from the argument for the two-dimensional
case, the relation between the external force exerted on the peg,

and the differential displacement of the peg,

is statically given by

,,= Kf, (6. 1)

where

K=

where ksoft are small stiffness coefficients and khard is a large one. When a
214 Chapter 6

translational force is applied at Dc, only a translational displacement

occurs; when a moment is applied at Dc, only a rotational displacement
results. Therefore, the point Dc is called the compliance center. A unique
feature of this hand is that the compliance center is located near the tip of
the peg and not inside the hand mechanism.
As is seen from the above example, the passive-impedance method does
not require any force-control loop, and so the control system becomes
simple. It generally lacks versatility, however, because different hardware
must be designed for each different task. This limitation leads to the idea
of changing the virtual mechanical impedance to make a manipulator
versatile without continual hardware modifications. We can do this by
driving the joint actuators using an appropriate feedback control law based
on measurements of end-effector position and velocity, external force, and
so on. This approach could be applied to various tasks just by changing
the software of the controller feedback law. We will consider this active
impedance approach in the following subsections.

6.1.2 Active-Impedance Metbod-One-Degree-of-Freedom Case

A simple one-degree-of-freedoom system will serve to illustrate the active

impedance method. We assume that the dynamics equation of the mechani­
cal system shown in figure 6.3 is given by

max + dax + kax f.. + F,

= (6.2)
where ma is the mass of body M, F is the external force, fu is the driving
force which we can apply, x is the displacement from the equilibrium of
F f.. 0, ka is the spring constant, and da is the damping coefficient. We
= =

r--------,
r-�I Controller I
:- ' x. x. i:. F
I L _______ -' I
I I
I I
I I
I
III.
F
'--I
I
I I
L __ J

o .r

Figure 6.3
Active impedance control of one-degree-of-freedom system.
Force Control 215

also assume that the desired impedance of the body to the external force
is e xpressed by

(6.3)

where md, dd' and kd are the desired mass, damping coefficient, and spring
constant, respectively, and Xd is the desired position trajectory.
When x, x, and x are measurable, we can use the control law

fu = {ma - md)x + {d. - dd)X + (k. - kd)x + dd Xd + kdXd' (6.4a)

Substi tuting equation 6.4a into equation 6.2 y ields equation 6.3, showing
that the closed-loop system has the desired impedance. When the external
force F is measurable. the control law represented by equation 6.4a can be
replaced by

f. = (d. - mamd -ldd)x + (ka - m.md -lkd)x - (1 - mamd -l)F

(6.4b)

If it is allowable to have the origi nal mass m. as the desired mass md, then
equations 6.4a and 6.4b reduce to a simple position and velocity feedbac k
law:

(6.5)

We have developed control laws to achieve the desired impedance (equa­

tion 6.3). A rem ainin g problem is how to determine the coefficients md• dd'
and kd of that equation. We first consider the case when the system makes
no contact with other objects, or when we can regard the external force F
to be zero because there is only a small perturbing force acting, if any. One
proced ure for this case is to let md mao to make the natural frequency
=

(6.6)

as large as possible for better transient response, and to let the damping
coefficient

,= dd (6.7)
2}mdkd

be around 0.7-1.0. As long as md, dd' and kd are positive, the steady-state
216 Chapter 6

I-F

� o Xc .r
/77

Figure 6.4
Contact with a fixed body.

position error and velocity error converge to zero for any desired trajectory
Xd•
Next we consider the case when the body M is in contact with a fixed
body E. We assume that the interaction through their place of contact can
be modeled as in figure 6.4, or by

dex + ke(x - xc) = -F, (6.8)

where Xc is the equilibrium position for which F = When either the

contact surface on the side of body M or that of E is fairly elastic (or when
O.
both are), the value of ke is small. When both surfaces are very stiff, ke is
large. The value of de also depends on the materials of the two bodies.
Substituting equation 6.8 into equation 6.3 yields

mdX + (dd + de)x + ( kd + ke)x = ddxd + kdXd + kexe' (6.9)

The natural frequency is

k d + kc
We-
J---, (6.10)
_

m d
and the damping coefficient is

, dd + de
(6.11)
2Jmd(kd + kJ
=

If we know the values of ke and dc, we can determine md , dd' and kd so that
the above We and, become acceptable. Unfortunately, the exact values of
ke and de are usually unknown. In particular, if the real value of ke is larger
than estimated and that of de is smaller, then the damping characteristic of
the system described by equation 6.11 may be inadequate. For this reason
we need to choose a fairly large dd' If we further choose smaller kd and md'
there is less possibility of an excessively large contact force (which may
Force Control 217

cause damage to bodies M and E), and the body M will comply better with
the constraint given by body E. An advantage of active impedance over
passive impedance is that a system can always adjust to a desirable im­
pedance by changing the values of md, kd' and dd during the performance
of a given task and by considering contacts and noncontacts with other
objects.

Example 6.1 The dynamics equation for the system shown in figure 6.3
is assumed to be given by

x + O.1x = fu + F.
The interaction with another body is modeled as in figure 6.4 and is
described by equation 6.8 with coefficients de and ke such that

(6.12)

(6.13)

For this system we wish to determine a desirable impedance (equation 6.3)

to satisfy the specification that the natural frequency during contact should

kd
be between 30 and 50, the damping coefficient should be more than 0.5, md
should be as close to 1 as possible, and and dd should be as small as
possible. From equation 6.10 we have

30 �
kd +

J
ke
� 50,
md

or

900md � kd + 500, (6.14a)

kd + 2000 � 2500md. (6. 14b)

From equation 6.11 we obtain

dd � Jmd(kd + ke) - de· (6.15)

Equations 6.14 are satisfied by

218 Chapter 6

and the smallest dd satisfying equation 6.15 is given by

dd = J2400 � 49.

Hence, a desirable impedance is described by

By equation 6.5, the control algorithm achieving this impedance is

fu = -48.9x - 400x + 49xd + 400xd·

We see that, from equation 6.11, the damping coefficient during contact is
within the following limits:

0.5 � ( � 1.07. 0

While maintaining the desired impedance, it is still possible to adjust the

force f = F applied by body M against body E to the desired value
-

fd = Fd in the steady state. For example, if we know Xc and kc in equation

-

6.8, then we can achieve the desired force by just choosing the desired
trajectory Xd in equation 6.3 to be

d+k
Xd Xc - k k kc c Fd•
= - (6.16)
d
This means that we have replaced the desired force with the corresponding
desired position. When kc» kd, we can approximate equation 6.16 by
Xd Xc
= Fd/kd' Another way to achieve the desired force Fd is to replace
-

the desired impedance (equation 6.3) with

(6.17)

Both methods eventually drive the system to the steady state of F = Fd;
that is, the applied force converges to the desired one.

6.1.3 Active-Impedance Method-General Case

In this subsection we will generalize the approach introduced above to

manipulators with multiple degrees of freedom (ref. 2). As is shown in figure
6.5a, measurements of the manipulation vector y and the external force F
acting on the end effector are used to drive the actuators at the joints
through a feedback control law. By selecting this feedback law properly,
we wish to develop a sytem that behaves like an end effector with a desired
Force Control 219

Ca) Cbl

Figure 6.5
Active impedance method. (a) Feedback control system. (b) Mechanical impedance realized
by (a).

mechanical impedence supported by an ideal arm that perfectly follows the

desired trajectory without being affected by any external force. This system
is shown pictorially in figure 6.5b.
Consider a six-link manipulator, and assume that the desired mechanical
impedance for its end effector is described by

(6.18)

where Y. is the difference between the current value of an appropriate

six-dimensional manipulation vector y and its desired value Yd :

(6.19)

A typical example of Y is the six-dimensional vector r expressing the

position and orientation of the end effector. In equation 6.18, F represents
the six-dimensional generalized force corresponding to y. Physically, F is
the external force exerted on the end effector by its environment. The 6 x 6
matrices Md , Dd , and Kd are, respectively, the inertia matrix, the damping­
coefficient matrix, and the stiffness matrix, all of which are symmetric and
nonnegative definite. A simple way of choosing Md, Dd, and Kd is to make
them diagonal and to determine the diagonal elements by considerations
similar to those in the one-degree-of-freedom case. Also taken into account
are the directions in which large impedance is desirable and the directions
in which small impedance is desirable.
The next step is to develop a control law that achieves the desired
impedance (equation 6.18). Suppose that the dynamics of the manipulator
are described by equation 5.24, or by
220 Chapter 6

(6.20)

The relation between y and q is

y =/y(q). (6.21)

Differentiating equation 6.21 yields

(6.22)

where Jy = oy/oqT. The joint torque equivalent to the external force F is

given by

(6.23)

Thus, the dynamic equation of the manipulator with external force F

applied to it is found from equation 6.20 to be

M (q)ij + hN(q,q) = T + J/(q)F. (6.24)

Assuming that Jy(q) is nonsingular for any q in a region under considera­

tion, we obtain from equations 6.22 and 6.24

My (q)ji + hNy(q,q) = J,-T(q)T + F, (6.25)

where Jy -T = (JyTtl and

My (q) = Jy-T(q)M(q)Jy-1(q), (6.26)

h Ny (q, tj) = Jy-T(q)hN(q,tj) - My (q)iy(q)tj. (6.27)

Now we apply a nonlinear feedback law of the form

T = J/(q){hNy(q,tj) - My(q)Md -1 (DdYe + KdYe)

+ [My(q) Md -1 - I]F}
= hN(q,q) - M(q)Jy -1 (q)iy(q)q

- M(q)J, -1 (q)Md -1 (Ddie + KdYe)

+ [M(q)Jy -1 (q)Md -1 - Jy T(q)JF. (6.28)

Then the closed-loop system becomes equal to equation 6.18.

If we do not require the inertia matrix Md of the desi red impedance
(equation 6.18) to be a constant independent of q and allow it to be the
current inertia matrix My(q), that is, if the desired impedance is described by
Force Control 221

My(q)j + DdY. + Kdyc = KFdF, (6.29)

then the control law (equation 6.28) reduces to

T = hN(q,tj) - M(q)Jy-1(q)iy(q)tj

- J/(q){Dje + Kdy. - (KFd - I)F} . (6.30)

The coefficient matrix KFd was introduced on the right-hand side of equa­
tion 6.29 to compensate for not being able to change My(q). For example,
we can produce a large (or small) impedance in a given direction by letting
KFd be a diagonal matrix and assigning a small (or large) positive value to
the diagonal element corresponding to that direction.

Example 6.2 Consider the two-link manipulator treated in subsection

3.3.1. Suppose that the desired mechanical impedance is described by

..
My(q)y +
[ Ddl 0 ] [Kdl
.

D d2 Y e +
0 ] [Ye =
KFdl 0 ] (6.3 1 )
0 0 KdZ 0 KFdZ F,

and that w e wish t o find the control law that produces this impedance.
Using equation 6.30 and letting Ye = [Yex,Ycy]T and F = [Fx,Fy]T, we
obtain the following control law by an argument similar to that of example
5.5:

+ {l1[MlllzCz - Md12C2 + ltlJOI.

z

2
+ l2[Mlll2 - Mdl2 + II C2)](81 + 82) }/11/2Sz
+ (lISI + 12S12)Fx - (11C1 + l2Cl2)Fy,

2
+ + II C2)](01 +
•

12[M1212 - MZ2(lz (2) }111/2S2

•

+ 12S12Fx - l2Cl2Fy,
where

Fx = Dd1Ye", +
KdlYex - (KFdl - l)F"" (6.32a)

Fy = DdzYey + Kd2Yey - (KFd2 - l) Fy" 0 (6.32b)

222 Chapter 6

The first term on the right-hand side of equation 6.30 compensates for
the centrifugal and Coriolis forces of the manipulator dynamics; the second
term compensates for the nonlinearity of transformation between y and (j.
All these terms (except for the gravity force) are functions of q and small
when the manipulator moves at low speed. When these terms are neglected,
the control law (equation 6.30) becomes

(6.33)

This indicates that the impedance control is achievable to a certain degree

by simple linear feedback of position error, velocity error, and external
force. If the gravity force is not negligible, we only have to add the term
g(q) to the right-hand side of equation 6.33 for compensation.
If we further consider the case when Dd 0 and KFd I, then by letting
= =

Ye Jy(q)qe (qe is the joint error vector corresponding to Ye) under the
=

assumption that Ye is small, we obtain

(6.34)

The impedance control based on equation 6.34 is called stiffness control

(ref. 3), because it makes the stiffness matrix expressed in the manipulation
variables equal to the desired Kd. It is also called compliance control,
because the inverse of a stiffness matrix is a compliance matrix. Finally, if
we wish to achieve the desired force Fd in the steady state, we adopt an
approach similar to equation 6.16 or 6.17 in the one-degree-of-freedom
case.

6.2 Hybrid Control

6.2.1 Hybrid Control via Feedback Compensation

The hybrid control method has been developed on the basic recognition
that, in many tasks requiring the control of force, there are some directions
in which position should be controlled and other directions in which force
should be controlled. These directions may change during the task, but a
single direction never requires both position and force control. This method
fulfills both position and force control of a task simultaneously (ref. 4). The
basic idea of the method is now illustrated by a simple example.

Example 6.3 Consider the two-link planar manipulator of figure 6.6.

The task is to push the endpoint of the manipulator with a specified
Force Control 223

Xc

I>
x
o

Figure 6.6
Example of hybrid control by feedback compensation.

force against the flat smooth surface of the shaded object, while moving
the endpoint along the surface in a specified way. We assign a frame
1:dOc - Xc Yd such that the Xc axis is along the flat surface and the Yc
axis is normal to the surface. Thus, Xc is the position-control direction and
Yc is the force-control direction. We assume that a force sensor is attached
to the endpoint to measure the pushing force, �(t), expressed in frame 1:c.
We also assume that the position of the endpoint, Cy(t), expressed in 1:c, is
measured by a position sensor. Denoting the desired values for �(t) and
Cy(t) as �(t) and Cyd(t), we obtain expressions for the errors that include
consideration of their direction in 1:c:

Cy.(t) = [� OJ ° [CYd(t) - Cy(t)J, (6.35)

C
fe(t) =
[00 �J [ �d(t ) - �(t)J. (6.36)

Letting Jy denote the Jacobian matrix of cy with respect to q, we can

transform the above errors back to the joint coordinate space using the
following equations:

q. ( t) � Jy -1 Cy.(t), (6.37a)

ti.(t) = .fy -1 Cye(t), (6.37b)

t.(t) = JyT Cj".(t). (6.37c)

Note that equation 6.37a is an approximation in the case of small cYe. On

the basis of these error expressions, we now calculate two joint driving
forces, tp and tF. Force tp is to compensate for the position error and is
224 Chapter 6

given by an appropriate position control law; force TF is to compensate for

the force error and is given by an appropriate force control law. Applying
the sum of Tp and TF to the joints, we can expect the result that the Xc
directional component of the desired position Cyd(t) and the Yc directional
component of the desired force <Jd(t) are closely tracked. Although a variety
of position and force control laws are applicable, a typical pair is the PO
(proportional and differential) position control law

Tp (t) = Kppqe(t) + Kpd4e(t), (6.38)

and the I (integral) force control law

TF(t) = KFi L T.(t')dt', (6.39)

where Kp p Kpd, and KFi are feedback gain matrices. The joint driving
'

torque is given by

(6.40)

Equations 6.35-6.40 constitute a hybrid control algorithm for the manipu­

lator. Note that Raibert and Craig (ref. 4) adopt a PIO (proportional,
integral, and differential) position control law and a PI (proportional and
integral) control law combined with a feedforward term from the desired
force signal. 0

Consider the Xc and Yc directions in example 6.3 from the viewpoint of

the const raint imposed on the endpoint by a given task. The Yc axis is the
direction in which the end-effector position is constrained. In other words,
the environment restricts the possible positions along Yc with the surface
as a barrier. The Xc axis is the direction in which the force exerted on the
object by the end effector is constrained. In example 6.3, this force is
constrained to be zero because we assume that the surface is smooth and
there is no friction force. Since these directions of position constraint and
force constraint can be easily described by the coordinate frame I:c, I:c is
called the constraint frame (refs. 6, 4); this explains the subscript C. In example
6.3, the frame I:c is fixed with respect to the reference frame. However, when
the object surface is not flat, as in figure 6.7, the directions of the position
and force constraints change according to where the endpoint is currently
located. In such cases, it is convenient to define the constraint frame I:c by
a moving frame with its origin at the current endpoint position, its Xc axis
Force Control 225

y Yc
Xc

Figure 6.7
Constraint by a curved surface.

Figure 6.8
Turning a crank.

in the direction tangent to the surface, and its Yc axis in the direction normal
to the surface.
The concept of a constraint frame can, of course, be generalized to the
case of three-dimensional space, as illustrated in the next example.

Example 6.4 Consider the task of turning a crank. As shown in figure 6.8,
the hand of the manipulator firmly holds the handle, which rotates about
the handle axis on the crank. A convenient choice for the constraint frame
is one with its origin at an appropriate point on the handle axis, its Zc axis
parallel to the handle axis, its Xc in the direction radiating outward from
the crank axis, and its Yc axis such that the three axes form a right-hand
coordinate frame. Having chosen this frame, we can describe the motion
which the hand can make as a translation along the Yc axis and a rotation
226 Chapter 6

about the Zc axis. These are the two position-controlled directions in the
six-dimensional space of the translational and rotational velocity vector.
On the other hand, the hand can make no translation along the Xc or the
Zc axis. Also, no rotation is possible about the Xc or the Yc axis. This means
that the hand can apply force in these directions, so these four are the
force-controlled directions. We can also look at the above argument from
the viewpoint of the constraints posed by the task. There are four position
constraints mentioned above and two force constraints, in the sense that
no force can be produced along Yc or about Zc if we consider the ideal case
of a frictionless, massless crank and handle. These are called natural con­
straints, because they are determined naturally by the given task. In con­
trast, there are also force commands in the four directions of position
constraints and velocity commands in the two directions of force con­
straints which are artificially given by the designer. For this reason, they
are called artificial constraints. With the hand velocity expressed in 1:c
denoted as Cv = [Cvx,CVy,CVZ,CWX,CWy,Cwz]T and the force exerted on the
handle by the hand as � = [Cfx, c/y, cfz, c"x, c"y, c"z] T, the natural constraints
for the task of turning a crank are described by

and the artificial constraints are described by

c
Cfx = 0, fz = 0, c"x = 0, c"y = 0,

where Vo is determined from the desired rotational speed of the crank and
Wo is given by the designer from the viewpoint of easy operation. Generally,
c
fx, cfz, c"x, and c"y are given nonzero force commands, but in this case
zero commands prevent the application of unnecessary force. 0
Figure 6.9 is a block diagram of a general hybrid control system based
on the above consideration. The function of the "position-error extraction"
block is to retain position errors in the directions of position control and
to neglect those in the directions of force control. The "force-error extrac­
tion" block has the complementary function of retaining force errors in the
directions of force control. The "position (force) control law" block com­
putes the joint driving force using the extracted errors. In the case of
Force Control 227

Desired position
trajectory

Control direction ___-.1

command
Force
Desired force
+
--0...--1
trajectory

Figure 6.9
Block diagram of hybrid control system.

example 6.3, it calculates equations 6.37a, 6.37b, and 6.38 for position and
equations 6.37c and 6.39 for force.

6.2.2 Dynamic Hybrid Control

In this subsection we will develop a hybrid control approach taking into

consideration the dynamics of the manipulator. 7.8 First, the constraint on
the end effector is described by a set of hypersurfaces in the end-effector
coordinates. Second, on the basis of this constraint description and the
manipulator dynamics, we derive the basic equations for what we will call
dynamic hybrid control. These equations give the joint driving force needed
to achieve the desired position and force trajectories. Finally, a basic
structure of the dynamic hybrid control system is given. This control
approach can also be regarded as an extension of two-stage control
(section 5.3) to the case of force control.

( 1) Description of end-effector constraint

We will express the constraint on the end effector caused by a contact with
its environment by a set of hypersurfaces in the space of the position vector
of the end effector. We assume that the environment is rigid, and therefore
the hypersurfaces are independent of the force applied by the end effector.
Consider a manipulator with six degrees of freedom whose end-effector
position with respect to a fixed reference frame is denoted by a six­
dimensional vector r. Assume that a given end-effector constraint can be
228 Chapter 6

expressed by a set of m hypersurfaces (m � 6),

Pi(r) = 0, i = 1,2, ... ,m (6.41)

which are differentiable twice and mutually independent in a subset S of

6
the six-dimensional Euclidean space R . When equation 6.41 is unilateral,
in the sense of approaching from only one side as in the case of pushing a

0 0
tool against the surface of an object, assume that Pi(r) > inside the object
and p;(r) < outside. Differentiating equation 6.41 with respect to time
yields

(6.42)

where

(6.43)

°Pi (r) .
e6-m+i = � ' 1= 1,2,... ,m. (6.44)

The rank of EF is m, owing to the assumed independence of equation 6.41 .

Differentiating equation 6.42 again, we obtain

(6.45)

To express the end-effector trajectory on the constraint surfaces given

by equation 6.41 , we assume that there exists a (6 - m)-dimensional function

YP= s(r) = [Sl (r),s2(r), . . . ,S6-m(r)Y

such that sj(r) (j = 1, 2, .. . ,6 - m) are scalar functions which are differ-
entiable twice and {Pi(r),i = 1, 2, ... ,m;sj(r),j 1, 2, ... ,6 - m} are mutually
=

independent in the region S. If we define

(6.46)

where

(6.47)

then we have

yp = Epr, (6.48)

yp = Epr + Epr. (6.49)

Force Control 229

For each r on the constraint surface, vectors el ,e2, ,e6 are linearly
• • •

independent of one other. The set {el ,e2" " ,e6-m } represents the directions
in which the end effector can move; the set {e7-m,eS-m" " ,e6 } represents
the directions in which the end effector is constrained. Hence, the coordi­
nate frame with its origin at the current position r and with {el ,e2,oo. ,e6 }
as its basis could be called the constraint frame. This is a generalization of
the constraint frame introduced in subsection 6.2. 1 . If we let YF = p(r) and
denote the six-dimensional generalized force vector corresponding to a gen­
eralized coordinate vector Y = [ypT'YFTY aS h [f/,/FT]T, the natural
=

constraints in terms of the above constraint frame are given by YF 0 and

=

fp 0, and the artificial constraints are given by yp = YPd andf fFd' The
= =

values of YPd andfFd are to be determined by the designer.

Let a 6 x 6 matrix E,be

E= [!:l (6.50)

Then we have

Ef= [�J (6.5 1 )

and

EF = [�J- Ef. (6.52)

Obvious physical interpretations of the above equations are that the nor­
mal component of the velocity of the end effector to the constraint surface
is zero and that the normal component of its acceleration to the constraint
surface is determined only by the velocity r.

Example 6.5 Consider the case where the position of the end effector is
constrained to a sphere of radius ex with its approach vector directed to the
center of sphere as shown in figure 6.10. Two examples of tasks of this kind
are polishing a spherical surface with a whetstone and operating a ball-joint
joystick. Take [x,y,z,¢J,e,t/I Y as the end-effector position vector r, where
[x,y,zY is the translational position in the reference frame and [¢J,e,t/lY is
the orientation expressed by Euler angles with the configuration shown in
figure 6.1 1 as the zero state. For simplicity, the region of interest, S, is taken
as one such that ex - Izi > 6, where 6 is a positive constant. The constraint
230 Chapter 6

Figure 6.10
Constraint by a spherical surface.

x
�-y
Figure 6.11
Reference orientation of hand.

hypersurfaces are

a2 _ (x2 + y2 + Z2) = 0,
r/J - atan2( - y, -x) = 0,
e - atan2( Ja2 - Z2, -z) = O.

Differentiating these equations and using equation Al.S, we have

EF =
[ -x
y/(a2 - Z2 )
-y
-x/(a2 - Z2)
-z
0
0
1
0 OJ
0 o .
0 0 -1/J(a2 - Z2) 0 0
(6.53a)

If we let yp = s r ( ) = [r/J,e,,,, y, then

Force Control 231

Ep =
[ 0
0
0
0
0
0
1
0
0
1
0
0 .
] (6 53b)
.

0 0 0 0 0 1

This yp is an easily understandable variable for position control: ¢> and (J

denote the translational position on the sphere, and", denotes the rotational
position about the approach vector of the end effector. 0

As was mentioned in subsection 2.5.2, the end-effector velocity f can also

be expressed by a six-dimensional vector v, consisting of the translational
velocity along each axis and the rotational velocity about each axis of the
reference frame. The relation between f and I' is

1' = Ti, (6.54)

where T, is a 6 x 6 transformation matrix which is a function of r and which

is assumed to be nonsingular in the subset S. For example, when Euler
angles are used in r for representing the orientation, 1',. is given by equation
2.86b.

(2) Motion of manipulator under constraint and constraining force

Consider a manipulator with n degrees of freedom, and denote its joint

vector by q. Assume that the relation between the end-effector position r
and the joint vector q is

r = /,.(q). (6.55)

Differentiating equation 6.55 yields

f = J,q, 1,. = iJrjiJqT, (6.56)

(6.57)

Also assume that the dynamics of the arm are given by equation 5.24, or

M(q)ij + hN(q,q) = 'to (6.58)

The motion of the joints (that is, ii) and the constraining force will be
found for the case when an arbitrary driving force command 'te is applied
to the manipulator at state (q,q) and under the constraint 6.41. From
equations 6.42 and 6.54 we have

(6.59)
232 Chapter 6

The six-dimensional generalized force corresponding to the generalized

velocity" is given by the forces along and the torques about each axis of
the reference frame. For simplicity, assume that the contact between the
end effector and the object is frictionless. From the principle of virtual work,
the force I exerted on the surface by the end effector, expressed in terms of
the generalized force, satisfies" TI = 0 for any realizable" under equation
6.59. Hence we have

(6.60a)

and

(6.60b)

where IF is an m-dimensional unknown vector. In equation 6.60a each

column vector of EFT can be interpreted as a reference force vector normal
to each constraint hypersurface. The vector IF' therefore, represents the
force I in terms of these reference force vectors. The force -I can be
interpreted as the force that constrains the end effector to the surface. The
joint force Tf equivalent to the constraining force -lis given by

(6.61 )

Since Tc and Tf are applied to the joints, by substituting T = Tc + Tf into

equation 6.58 we obtain

Mij + J/EFT/F = bi, (6.62a)

where

bl =
Tc - hN(q,tj)· (6.62b)

Also, from equations 6.45 and 6.57 we obtain

EF'l,.ij = b2, (6.63a)

where

b2 = - EFirtj - EFf. (6.63b)

Note that the vectors bl and b2 can be calculated from the given values of
q and tj, i.e., the state of the manipulator. From equations 6.62a and 6.63a,
we finally have

(6.64)
Force Control 233

(6.65)

where

(6.66)

(3) Calculation of joint driving force

Having just derived the manipulator motion and the constraining force for
an arbitrary joint driving force Teo let us consider the inverse problem.
Assume that the desired value for jip is jiPd' and that the desired value for
IF is/Fd•
Consider the following nonlinear state feedback law:

(6.67)

Tp = Mijd + hN(q,tj), (6.68)

lid =
J, + {£-1 ([� ] J,tj) }
-E - i,tj + (I - J, + J,)k, (6.69)

(6.70)

where U1 and U2 are (6 - m)-dimensional and m-dimensional new input

vectors. We can easily show using equations 6.64-6.66 that the closed-loop
system is a linear system described by

(6.71)

Therefore, if the models given by equations 6.41 and 6.58 are exactly correct
and if yp(O) = YPd(O) and jp(O) jPd(O) at the initial time, t 0, then the
= =

desired position and force trajectories will be perfectly produced by

(6.72)

The vector k in equation 6.69 is an arbitrary time function. This k represents

the arbitrariness of the joint acceleration that appears when n > 6, i.e.,
when we consider redundant manipulators. If n 6 and the rank of J, is
=

6, then the second term on the right-hand side of equation 6.69 is O.

It is a natural result of these arguments that the position and the force
can be controlled simultaneously by applying the sum of the force Tp to
achieve the desired acceleration specified by jPd and applying the force TF
to achieve the desired force specified by fFd as the joint driving force. In
234 Chapter 6

order for this result to be achieved, the condition rankJ 6 must be

=

satisfied; that is, the manipulator must not be in a singular configuration.

The control law given by equations 6.67-6.70 corresponds to the lin­
earizing compensation given by equation 5.34 in two-stage control, and
equation 6.71 corresponds to the linearized system of equation 5.35.
When some friction force exists between the constraint surface and the
end effector, and its exact value is known, then we can compensate for the
friction force by including it in hN(q,tj) of equation 6.68.

Example 6.6 Consider a task of following the surface of a sphere using a

six-degree-of-freedom manipulator under the constraint described in ex­
ample 6.5. Assume that the end effector should move from the initial
position y,(O) [0, n12, oy (which corresponds to the point of ex on the
= -

X axis) in the direction of increasing ¢> with velocity d, that the force should
be applied in the direction of the center of the sphere with magnitude J
without applying a moment, and that the condition rankJ, 6 is satisfied
=

during the task. Substituting equations 6.60b, 6.53a, and 2.86b into 6.60a
yields

r �
/= y/(a� z2) ::_Z2)
-x/( �z -C,,:e/Se -S,,�e/S6 �]T IF'
l 0 0 -1/Ja2-z2 -S" C" 0

where So sinO, Co cosO, S,p sin¢>, and C,p cos¢>. As was mentioned
= = = =

in (2), the columns of the coefficient matrix of/F' EFT, in the above equation
express the reference-force vectors shown in figure 6.12. Each of them is a
constraining force and causes no motion of the end effector; we can see this
by considering the case of operating a ball-joint joystick, where the moment
at the center is zero for each of the three reference forces shown in the figure.
When we regard yp and /F as the controlled variables, the linearizing
control law is given by equations 6.67-6.70, with E obtained from equation
6.53. The desired position and force trajectories are described in terms of
yp and /F by

and
Force Control 235

z z z
CUrrent end­
effector position

x x
Constraint
surface x
- : Force � : Torque

(a) (b) (c)

Figure 6.12
Reference force vectors represented by column vectors of EFT. End-efTector position:
{x = 11./2, Y = 11./2, Z = 11./.j2, tP = -1350, (J = 135°}. (a) First column vector. (b) Second
column vector. (c) Third column vector.

Therefore, when we wish to achieve these desired trajectories in an open­

loop manner, the joint driving fo rce 1', is given by equations 6.67 and 6.68,
and

q"
d
=
J,-1 {E':'l ([d/a.0 ] _
,
.) .}
£J q _ j
,q ,

1'F =
A
-J, T EF T U/a.,O,O] T . 0

(4) Basic structure of dynamic hybrid control system

If equations 6.41 and 6.58 are exactly correct, then the open-loop system
in figure 6.13 can produce both the desired position and the desired force.
However, because of modeling errors and unpredictable disturbances, the

,
real response of this system may deviate from the desired one. To cope with
this deviati on we have to add a servo compensator to the system. One
basic configuration among various possible ones is given in figure 6.14,
where two independent servo compensators (one for the positio n servo loop
.
and one for the force servo loop) are installed The specific design of the
servo compensators could be carried out using any conventional design
approach to servo systems, one instance being the approach based on the
sensitivity and the stability margin described in subsection 5.4.2.

Example 6.7 Consider the two-link manipulator treated in s u bsection

3.3.1. S u ppose we wish to design a hybrid control system for the case where
236 Chapter 6

YPd
Eqs. 6.68, 6.69,
6.72 r

T,
Manipulator
+
f
fFd Eqs. 6.70, 6.72
1"1'

Figure 6.13
Open-loop dynamic hybrid control system.

<D To blocks
Eqs.6.68, <D.®
6.69
r
+
Manipulator
+
f

Eq.6.70

Figure 6.14
Dynamic hybrid control system.

the end-effector position vector r [x,yY is constrained on a smooth

=

curve as shown in figure 6.15. Obviously, m 1. We adopt as yp the distance

=

along the curve from a fixed point on it. Using the tangent vector e1 and
normal vector e2 shown in the figure, we find

EF = e2T
and

Ep = e1T.

Thus, E is given by the 2 x 2 matrix [e1,e2Y. The Jacobian matrix J, is

and the arm dynamics are described by equation 3.39. Hence, from equa­
tions 6.67-6.70 the linearizing control law is given by
Force Control 237

PI (r)=o

� -------�
x
o

Figure 6.15
Constraint by a curve.

and the closed-loop system is described by

(6.73a)

(6.73b)

One of the simplest servo compensators for this linearized system is

U1 = Wc/(YPd - Yp) - 2wcpYp, (6.74a)

u2 = WcF I (fFd - fF)dt', (6.74b)

where WcP and WcF are constant parameters. This means that we use a P
control associated with velocity feedback for the position servo loop and
an I control for the force servo loop. This also corresponds to using the
servo system in figure 5.22, with the feedforward compensator CaP and the
feedback compensator CbP for the position servo loop given by

and with the feedforward compensator CaF and the feedback compensator
CbF for the force servo loop given by
238 Chapter 6

_
WcF
CaF -
S
­

and
_
Wc F
CbF -
S
•

After we implement equation 6.74, the transfer functions for position and
force of the overall dosed-loop system, GJP and GJ F , become

and

Since these characteristics are quite reasonable, we have completed the

design of a hybrid controller. The matrix E, which is necessary to compute
Tc , is determined when the constraint curve is given. For example, if the

constraint curve is given as in figure 6. 1 6, then, since it is described by

Figure 6.16
Example of constraint curve.
Force Control 239

we have

Ep = e2 T = [1/J2, 1/J2].
If we further let yp be
yp = s l (r) = (y - x + 1 + 12)/J2,
then

Ep = e1 T = [ - 1 /J2 , 1/J2].
Hence, we finally obtain

E=
[ - 1/J2 1/J2 ]
1/J2 1/J2 .
We can evaluate the sensitivity and the stability margin of the above servo
systems using the curves of l S I and I TI introduced in subsection 5.4.2. The
curves for the position control loop are identical to those in figure 5.25;
those for the force control loop are shown in figure 6. 1 7. Both loops have
small sensitivity at low frequencies and large stability margin at high
frequencies. Thus, we can expect to get good control performance by

20 .------,---,--,

radlsec

Figure 6.17
lSI and I TI for force-control loop.
240 Chapter 6

adjusting the parameters WcP and WcF . Finally, by modifying only the
feedforward compensators CaP and CbP' we can produce some other desired
overall transfer characteristics without changing the sensitivity and stability­
margin properties. 0

In many tasks requiring force control, it is sufficient to operate manipu­

lators at low velocity. In such cases, we can neglect the terms hN (q,q), t J,q,
and .i,.q in equation 6.68 and 6.69 and develop the approximate linearizing
control law

Tc = M (q)J/ E- 1 [�J - J,TEF TUZ · (6.75)

This control law and a servo compensator like that given by equations 6.74
constitute a simplified hybrid controller.

Exercises

6.1 Determine a set of md. kd• and dd when the specification is

5 � de � 10,

500 � ke � 2500
instead of equations 6. 1 2 and 6. 1 3.

6.2 Find a control law for the parallel-drive manipulator in subsection

3.3.3 when the desired mechanical impedance is given by equation 6.31.

6.3 Find a constraint frame for the task of polishing a sphere with a
whetstone (figure 6. 1 8). Determine the natural and artificial constraints.

6.4 Consider the two-link manipulator shown in figure 6. 1 9. Both link

lengths are 1. Give a detailed computational expression for each block of
figure 6.9 for the task of moving the manipulator to follow the flat surface
at an inclination of 45°. Assume that position sensors measure the joint
angles ()1 and ()z , and that a force sensor measures the contact-force
components in the XH and YH directions of the hand coordinate frame
LH(OH - XH YH). Assume also that Kpp kppIz, Kpd = kpdI 2, and KFi
=
=

kFiI2 •
Force Control 24 1

Figure 6.18
Polishing with a whetstone.

Xc

--'-'--'-----(> X
o

Figure 6. 19
Hybrid control of two-link manipulator.

6.5 Find a set of constraint hypersurfaces for the task of turning a crank
in example 6.4. A simple expression results from the use of the reference
frame l:o(Oo Xo YoZo) with its origin 00 on the crank axis and its Zo axis
-

parallel to the crank axis.

6.6 Find a set of constraint hypersurfaces for the task of turning a nut on
a bolt (figure 6.20).

6.7 Consider the case, shown in figure 6.2 1 , where the position of the end
effector is constrained to an arbitrary smooth surface with its approach
vector normal to the surface. Assume that the surface is described by
y = u (x,z), where u (x,z) is a single-valued function that is differentiable
three times. Find EF and Ep .

6.8 Consider a case of dynamic hybrid control where the constraint

hypersurfaces change with time and are described by

Pi(r, t) = 0, i = 1, 2, .
. . ,m
242 Chapter 6

Figure 6.20
Turning a nut on a bolt.

Figure 6.21
Constraint by an arbitrary surface.

instead of equation 6.41 . What modification is necessary to the argument

in ( 1 )-(3) of subsection 6.2.2 to obtain a linearized system like that of
equation 6. 7 1 ?

References

I. D. E. Whitney, "Historical Perspective and State of the Art in Robot Force Control," in
Proceedings of the 1 98 5 IEEE International Conference on Robotics and Automation, pp.
262-268.
2. N. Hogan, "Impedance Control: An Approach to M anipulation, Parts I-III," ASME
Journal of Dynamic Systems, Measurement, and Control 107,
no. 1 (1985): 1 -24.
3. J. K. Salisbury, "Active Stiffness Control of a Manipulator in Cartesian Coordinates," in
Proceedings o f the 1 9 t h IEEE Conference on Decision and Control ( 1 980), pp. 95- 1 00.
4. M. H. Raibert and 1. J. Craig, "Hybrid Position/Force Control of Manipulators," A SME
Journal of Dynamic Systems, Measurement, and Control 1 03, no. 2 ( 1 9 8 1 ): 1 26- 1 3 3.
5. D. E. Whitney, "Quasi-Static Assembly of Compliantly Supported Rigid Parts," ASME
Journal of Dynamic Systems, Measurement, and Control104, no. 1 ( 1 982): 65-77.
Force Control 243

6. M. T. Mason, "Compliance and Force Control for Computer Controlled Manipulators,"

IEEE Transactions on Systems, Man, and Cybernetics 1 1 , no. 6 ( 1 98 1 ): 4 1 8-432.

7. T. Yoshikawa, "Dynamic Hybrid Position/Force Control of Robot Manipulators­

Description of Hand Constraints and Calculation of Joint Driving Force," IEEE Journal of
Robotics and Automation 3, no. 5 ( 1 987): 386-392.

8. T. Yoshikawa, T. Sugie, and M . Tanaka, "Dynamic Hybrid Control of Robot Manipulators

-Controller Design and Experiment," IEEE Journal of Robotics and Automation 4, no. 6
( 1 988): 699-705.
7 Control of Redundant Manipulators

A manipulator is said to have redundancy if it has more degrees of freedom

than are necessary to perform a given task. In this chapter, after a discussion
about the advantages and disadvantages of redundant manipulators, a
method of actively utilizing such redundancy will be given. This method
will be applied to avoiding obstacles and avoiding singular configurations,
and a numerical calculation method that is effective in the above approach
will be described.

7.1 Redundant Manipulators

A human arm can be considered to have seven degrees of freedom. A hand

requires a total of six degrees of freedom (three to hold an object at a certain
position in space, and three more to orient it); therefore, the human arm
has one redundant degree of freedom for this fundamental task of posi­
tioning and orienting the grasped object. This explains why we can move
the elbow freely while grasping an object steadily in a certain position and
orientation. This excess degree of freedom is the redundancy, and it gives
the human arm great versatility and broad applicability.
The same argument applies to manipulators. Since six degrees of freedom
are enough for full position and orientation control of an end effector, a
manipulator with seven degrees of freedom can be said to be redundant.
Even manipulators with six or fewer degrees of freedom are often regarded
to have redundancy because the particular task to be performed requires
fewer degrees of freedom than the manipulator has available. This is one
of the main reasons why we need to consider the issue of redundancy.
What are the advantages of redundant manipulators? Generally sepak­
ing, as in the case of the human arm, they excel in versatility and applic­
ability. More specifically, they have the potential to avoid singularities,
avoid obstacles, avoid structual limitations (e.g., angle limits of a rotational
joint), carry out reasonable actions (e.g., low-energy-consuming motion,
balanced motion among individual joint velocities, balanced configuration
among joint driving forces), reach behind an object, crawl into concaves,
and so on. Redundancy can also be used to make a manipulator more
reliable in the sense that it can perform certain tasks even after a failure of
some joints. This is important in certain application fields, such as space
and the deep sea.
Of course, redundant manipulators have disadvantages too. They have
more joints and actuators. Their structure is more complex, bulkier, and
Control of Redundant Manipulators 245

heavier. More complicated control algorithms are required, so the amount

of necessary computation increases. Redundant manipulators will not be
truly beneficial unless these disadvantages are offset by some advantages.

7.2 Task-Decomposition Approach

7.2.1 Decomposing a Task into Subtasks with Priority Order 1. 2

Most complicated tasks given to a manipulator can be formulated in such

a way that the task is broken down into several subtasks with a priority
order. Each subtask is performed using the degrees of freedom that remain
after all the subtasks with higher priority have been implemented. A weld­
ing task, for instance, might be broken down into hand-position control
and hand-orientation control, with hand positioning given first priority. A
task of tracking a trajectory through a workspace with obstacles might be
decomposed into controlling the position and orientation of the hand and
avoiding obstacles, with the former given first priority. It is obvious that
control problems of redundant manipulators can be approached via the
subtask method by regarding a task to be done by a redundant manipulator
as the subtask with first priority and regarding the reason for using the
redundancy (singularity avoidance, joint limit avoidance, etc.) as the sub­
task with second priority.
We assume that each subtask is given in the form of either a desired
trajectory of variables suitable for describing the subtask, or a certain
criterion. The first form is suitable when the operator can give a trajectory
that achieves the given subtask. The second form is used when the operator
does not know the actual desired trajectory but knows how to evaluate the
trajectory; for example, when the subtask is to keep the manipulator
configuration away from singularities or to keep the joint angles within
their limits, the configuration is evaluated using a criterion.
For ease of explanation, the discussion will be limited to simple cases in
which the task is broken down into two subtasks and the first subtask is
given in the form of a desired trajectory.

7.2.2 Basic Equations

Consider a manipulator with n degrees of freedom. The joint variable of

the ith joint is qj (i
= 1,2, ... ,n). The manipulator configuration is denoted
by the vector q [ql ,q2'"'' qn] T.
=
246 Chapter 7

Assume that the first subtask can be described properly by an m1-

dimensional vector, Yl' which is a function of q:

Yl = 11 (q). (7.1)

Assume also that the desired trajectory forYl is given: Yld(t) (0 � t � tf; tf
is a terminal time). A vector (such asYl) which is suitable for describing a
manipulation task is called a manipulation vector.
For the second subtask, consider the following two cases:
Case 1 An m2-dimensional manipulation vectorY2 is given by

(7.2)

and the second subtask is specified by the desired trajectory Y2d(t)

(0 � t � tf) forY2'
Case 2 A criterion function

p = V(q) (7.3)

is given, and the second sub task is to keep this criterion as large as possible.

The problem we consider is first to obtain the joint velocity that achieves
the first subtask of realizing Yl Yld and then, if there is any ability left,
=

to perform as much of the second subtask as possible at each time instant.

The dynamics of the manipulator are not considered; that is, the problem
is treated as purely kinematic. It is possible to formulate the problem in
both case 1 and case 2 as an instantaneous optimal control problem in a
mathematically more rigorous way, as we will do in subsection 7.2.5;
however, we will first solve the problem in a rather heuristic way.
The general solution of the joint velocity that performs the first subtask
will be given. Differentiating equation 7.1 with respect to time yields

(7.4)

where � djj(q)jdqT (j
= 1,2) is the Jacobian matrix ofYj with respect to
=

q. When the desired trajectoryYld is given, from equation A2.22 in appendix

2, the general solution for 4 of equation 7.4 is
(7.5)

where kl is an n-dimensional arbitrary constant vector.3 The first term on

the right-hand side of equation 7.5 is the joint velocity to achieve the desired
Control of Redundant Manipulators 247

trajectory, Y1d(t). When there are multiple solutions for 4 satisfying equa­
tion 7.4, this term gives a solution that minimizes 11411, the Euclidean norm
of 4. Also, even when there is no solution that satisfies equation 7.4, this
term provides an approximate solution that minimizes the norm IIY1d -
J1 411, which is a kind of measure for approximation error. The second term
on the right-hand side of equation 7.5 represents the redundancy left after
performing the first subtask. Equation 7.5 is the basic equation for using a
redundancy.

7.2.3 Second Subtask Given by Desired Trajectory

In this subsection we will consider case 1, where the second subtask is

specified by the desired trajectory Y2d(t) of the manipulation variable Y2'
What we have to do is select k 1 in equation 7.5 so as to realize Y2d as closely
as possible. Differentiating equation 7.2, we have

Y2 =
Jz 4 ·
(7.6)

Substituting Yz = Y2d and equation 7.5 into 7.6, we obtain

Y2d - J2 J1 + Y1d = J2(1 - J1 + Jdk1· (7.7)

Letting J2 J2(1 = - J1 + Jd and using equation A2.22, we obtain from

equation 7.7

(7.8)

where kz is an n-dimensional arbitrary constant vector. Note that the

relation

(7.9)

holds (see ref. 4 and exercise 7. 1 ). Therefore, from equations 7.5, 7.8, and 7.9,

(7.lOa)

is the desired joint velocity that first realizes trajectory Y1d and then realizes
YZd as closely as possible using the remaining redundancy.
If (I - J1 + J1 - Jz + Jz) is not zero, then some further redundancy is still
left to perform a third subtask. When Y2 q (that is, when the second
==

subtask provides the desired trajectory of the whole arm configuration),

since J2 = 1 we have

J2 + = (I - J1 + Jd + = (I - J1 + Jd·
248 Chapter 7

Hence, equation 7.10a simplifies to

tid =
JI + Yld + (I - JI + JI)Y2d, (7.l0b)

in which the term with k2 disappears.

When the servo loop of each joint axis functions perfectly to produce the
desired joint velocity, Yld(t) is followed exactly. Some deviation often
occurs, however, between the desired subtask trajectory Y2d(t) and the
actually realized trajectory Y2(t), because of the insufficient number of
remaining degrees of freedom. One way to cope with such a situation is to
modify the desired velocity Y2d to take the deviation into consideration.
For example, we can define the modified velocity Y2d* by

(7.11)
where H is an appropriate diagonal gain matrix, and then replace Y2d in
2
equation 7.l0a or 7.l0b with Y2d*'

7.2.4 Second Sub task Given by Criterion Function

In this subsection we will consider case 2, where the second subtask is

specified by a criterion function. As in case 1, we select the value k I in
equation 7.5 so as to make the criterion p given by equation 7.3 as large as
possible. One natural approach is to determine kl by the following equa­
tions (ref. 3):

kl = �kp, (7.12)

� = [�1'�2"" '�nY, (7.13)

�, = iJ V(q)!iJq" (7.14)

where kp is an appropriate positive constant. The desired joint velocity, tid'

is given by

(7.15)

Hence, we have

(7.16)

Since (I J1 + J1) is nonnegative definite, the second term on the right-hand

-

side of equation 7.16 is always nonnegative, causing the value of criterion

p to increase. 5
Control of Redundant Manipulators 249

Mathematically, the vector k 1 of equation 7.12 is the steepest gradient

vector of the function V(q); the vector (I J1 + J1 )l;kp corresponds to the
-

orthogonal projection of k 1 on the null space of J1• The constant kp is

chosen so as to make p increase as quickly as possible under the condition
that tid does not become excessively large.

7.2.5 Formulation as Instantaneous Optimization Problem

The problem formulation in subsection 7.2.2 and the solutions in subsec­

tions 7.2.3 and 7.2.4 are rather intuitive. In this subsection it is shown that
these results can be obtained as the rigorous solutions to some instantane­
ous optimization problems by adopting an appropriate criterion function
for the second subtask.
For case 1, let the criterion function for the second subtask be

(7.17)
The optimal solution, which minimizes equation 7.17 at each time instant
under the constraint 7.5, is given by equations 7.10a and 7.11.
For case 2, using the original criterion V(q) of equation 7.3, let the
criterion function for the second subtask be

dV(q)
P2
_
- kp � - 111'
2 q
- J1 + Yld
. 112 . (7.18)

The optimal solution maximizing equation 7.18 at each time instant under
the constraint 7.5 is given by equation 7.15. The first term of equation 7.18
attempts to increase the original criterion; the second term tries to minimize
the difference between ti and the minimal norm solution J1 + Yld of the first
subtask. The coefficient kp represents the weight of importance of the first
effort compared to the second effort.

7.3 Application to Avoiding Obstacles and Singularities

7.3.1 Avoiding Obstacles

Consider the three-link manipulator shown in figure 7.1, which moves in

the X Y plane. The required task is to make the end effector follow a
-

desired trajectory while avoiding collisions with the obstacle shown as the
hatched area in the figure. We will solve this problem of avoiding the
obstacle as an example of case 1 (see ref. 2).
250 Chapter 7

y
r"

I
I
I
I
I
I Desired
: trajectory
I
I
I
I
I
I
I

2 X

Figure 7.1
Three-link manipulator.

The link lengths are II 12 = 1 and 13

= 0.3, and there is no hardware
=

limit on the rotation of any joint. At the beginning of the task, the initial
configuration at time t 0 is given by
=

which corresponds to

The end effector is required to move from initial position ro to final position
rf [xo, 0] T by time t 1, along a straight line parallel to the Y axis. At
= =

the same time, the manipulator should avoid colliding with the hatched
obstacle zone. This task is divided into a first subtask (moving along the
desired trajectory) and a second subtask (avoiding the obstacle).
For the manipulation variable YI of the first subtask, we use the end­
effector position r:

YI - _
[IIC1 12CI2 + 13C123
+ ] (7.19)
IISI + 12S12 + 13S123
.

Hence, the Jacobian matrix JI is given by

-12S12 -/3S123 (7.20)

12C12 + 13CI23
Control of Redundant Manipulators 251

f r"
I
I
I
I
I
I

![>-
r, 2 X

Figure 7.2
Obstacle and reference configuration taught for obstacle avoidance.

If we choose the desired trajectory using equation 5.7, then we have

Yld( t) = [
Yo - (3 x 2t)t2yo] '
0

-
O<t<1
= = . (7.21)

For the second subtask, we assume that a constant reference arm con­
figuration, suitable for obstacle avoidance, has already been taught (say,
by a human operator). Hence, the second subtask can be specified as trying
to come as close to this configuration as possible. This means that the
manipulation variable Y2 for the second subtask is given by the joint vector
q. We assume that the reference arm configuration that plays the role of
the desired trajectory for the second subtask has been given as

(7.22)

This reference configuration for collision avoidance is shown in figure 7.2.

Now that we have defined our terms, from equations 7.10b and 7.1 1 the
desired joint velocity tid is given by

tid =: J1 + [ -6(1
0
- t)tyo
] + (13 - JI + JI)H2(Y2d - Y2)' (7.23)
252 Chapter 7

y
1=0
0.2

0.4

0.6

0.8

1.0 X

(a)

0.2

0.4

0.6

0.8
1.0 X

(b)

Figure 7.3
Simulation of obstacle avoidance. (a) Without using redundancy (H2 = 0). (b) Using
redundancy (H2 = 0.212)'
Control of Redundant Manipulators 253

(The second term on the right hand side of this equation is needed because
of the inconsistency between Y2d and Yld(t).)
The simulation results shown in figure 7.3 were obtained by assuming
that the desired joint velocity of equation 7.23 occurs perfectly. Figure 7.3a
depicts the case when H2 = 0, so ti has been chosen to minimize "till,
ignoring the obstacle avoidance. Figure 7.3b depicts the case when H2 =

0.212 (that is, when the redundancy is utilized). It is clear from the figure
that collision with the obstacle has been avoided by the use of redundancy
in the simulated case.
The above example treated a very simple obstacle. This approach to
avoiding obstacles could also be adopted to cope with more complicated
obstacles or time-varying obstacles by using several reference configura­
tions taught beforehand and switching them in turn.
Another approach is to define a criterion function to express the pro­
ximity between the manipulator and obstacles6 and to make it the criterion
for the second subtask (ref. 2). This approach does not need any reference
configuration, although it requires detailed information about the obstacles
and a large amount of calculation.

7.3.2 Avoiding Singularities

As an example of case 2, where the second subtask is specified by a criterion

function, let us consider the problem of making the end effector track
a desired trajectory while avoiding singular configurations as much as
possible (ref. 5).
A manipulator generally has certain configurations in which it can no
longer move its end effector to change position or orientation in certain
directions. These configurations are called singular configurations, as was
discussed in subsection 2.5.6. It is undesirable not only for a manipulator
to fall into such a singular configuration but also for it even to come close
to one. Some joint velocities would become excessively large trying to
maintain the desired trajectory. The manipulability measure was discussed
in section 4.2 as a quantitative measure of the capability of a manipulator
to move its end effector freely in any direction. This measure can also be
regarded as an index of the distance from singular configurations. Hence,
the manipulability measure can be used as the criterion when the second
subtask is the avoidance of singularities.
Consider the same three-link manipulator as in the preceding subsection.
The end-effector position r is again taken as the first manipulation variable,
254 Chapter 7

t 0

o 1 X

(al

I'

-1 I X

(bl

Figure 7.4
Simulation of singularity avoidance. (al Without using redundancy (kp = 0). (b) Using
redundancy (k. = 20).
Control of Redundant Manipulators 255

YI' The manipulability measure for, is taken as the criterion for the second
subtask. Hence, using equation 4.8 and the Jacobian matrix J1 of equation
7.20, we have

V(q) = JdetJ1J/ .
Therefore, e, of equation 7. 1 4 is given by
2

e, = !JdetJ1 J1 T L qij(JWJI/ + J1}IJ1/), 1= 1,2,3 (7.24)

i,j=1
where qij is the (i,j) element of the inverse matrix of (J1 J1 T), Ju is the ith
row vector of J1, and JIil is the partial differential coefficient of Ju with
respect to 8" The vector c; in equation 7.15 is obtained from equation 7.24.
Some results of computer simulation are shown in figure 7.4. The initial
configuration is qo = [180°, 170°, -lOoy, which corresponds to the end­
effector position '0 [xo,YoF = [O.28,O.17Y. The desired trajectory is
=

given by

Yld( t=
)
[ x0 ] <t<l
O= (7.25)
Yo (3 - 2t)t2(yo 0.1)'
=
- +

which is a straight line parallel to the Y axis going from '0 to the final
position'f [xo, 0.1] T. Equation 7.25 is, again, obtained from equation
= -

5.7. Figure 7.4a shows the change of arm configuration for the case of kp = 0
in equation 7.15; that is, the case where the simple pseudo-inverse control
law is applied and no attempt is made to avoid singularity. Figure 7.4b is
for the case where kp = 20; the singularity is avoided by trying to keep the
manipulability measure large. Figure 7,5 shows the change of manipula-

U'

0.4
( bl

Figure 7.5
Trajectories of manipulability measure. (a) Without using redundancy (k. = 0). (b) Using
redundancy (kp = 20).
256 Chapter 7

bility measure in cases a and b of figure 7.4. As can be seen from the figure,
the utilization of redundancy is quite effective in avoiding singularity.

7.4 Computational Method for Desired Joint Velocity

One of the difficulties in applying the method described above is the large
amount of computation needed for obtaining the pseudo-inverse matrix.
In this section, we will develop an efficient computational method for
obtaining tj satisfying

tj = r Yd + (I - r J)k, (7.26)

which corresponds to equation 7. 5, for given values of J, Yd' and k.

If the matrix (JJT) is nonsingular, tj can be obtained by the following
procedure 7: First, obtain the solution IX of

(7.27)

by an appropriate method for solving linear algebraic equations, such as

Gauss elimination. Then, calculate tj by

(7.28)

It is easy to check that tj calculated this way is equal to tj given by equation

7.26, since r JT(JJT)-l. =

At singular configurations, however, (JJT) becomes singular and the

solution tj to equation 7.26 is not necessarily continuous with respect to
the elements of J. Because of the relationship of tj to q through J, tj is also
not necessarily continuous with respect to q. One way to cope with this
problem is to apply regularization, so that (JJT) is replaced by (JJT + k.I)
near singularities. B Thus, in place of equation 7.27 we use

( JJT + ksl)1X = Yd - Jk, (7.29)

where ks is a scalar continuous function of q that is positive near singulari­

ties and zero for other q. For example, using the manipulability measure
W .jdetJiT, ks can be assigned to be
{ko(1
=

k - W/WO)2, W < Wo
= (7.30)
s 0, W � Wo

where ko and Wo are appropriate positive constants. The parameter ko

Control of Redundant Manipulators 257

specifies the magnitude of regularization, and Wo represents the threshold

separating the vi cinit y of singularities from the other region.

Exercises

7.1 Show that the equality B(CB)+ =(CBt holds for any n x n sym­
metric and idempotent matrix B (i.e., BT Band BB B) and m x n
= =

matrix C (ref. 4). Using this relation, prove equation 7.9.

7.2 Show that the solution that minimizes equation 7.17 at each instant
of time under the constraint 7.5 is given by equations 7.10a and 7.11.

7.3 Show that the solution that maximizes equation 7.18 at each instant
of time under the constraint 7.5 is given by equation 7.15.

7.4 Calculate tj from equation 7.26 when

J =
2 1
[0� � � 2]� [�]
, Yd =
1
, k=
[ �l
-

. �
.

Also calculate the same value using equations 7.27 and 7.28.

7.5 Show that tj obtained from equation 7.28 using ac calculated from
equation 7.29 is equal to tj minimizing

7.6 Decompose the task of writing letters on a sheet of paper with a

ballpoint pen into several subtasks with priority order, and state the
reasons for that decomposition. Do the same with the task of carrying a
cup full of liquid.

7.7 Assume that the three-link manipulator shown in figure 7.1 has the
following joint angle limits:

Describe a formula for the desired joint velocity that makes the endpoint
of the manipulator follow a given desired trajectory while keeping the joint
angles within their limits as much as possible.
258 Chapter 7

References

1. H. Hanafusa, T. Yoshikawa, and Y. Nakamura, "Analysis and Control of Articulated

Robot Arms with Redundancy," in Proceedings of the 8th IFAC World Congress (1981),
vol. XIV, pp. 78-83.

2. Y. Nakamura, H. Hanafusa, and T. Yoshikawa, "Task-Priority Based Redundancy Control

of Robot Manipulators," International Journal of Robotics Research 6 (1987): 3-15.

3. A. Liegeois, "Automatic Supervisory Control of the Configuration and Behavior of Multi­

body Mechanisms," IEEE Transactions on Systems. Man. and Cybernetics 7, no. 12 (1977):
868-871.

4. A. A. Maciejewski and C. A. K lein , "Obstacle A voidance for Kinematically Redundant

Manipulators," International Journal of Robotics Research 4, no. 3 (1985): 109-117.

5. T. Yoshikawa, "Analysis and Control of Robot Manipulators with Redundancy," in

Robotics Research: The First International Symposium, ed. M. Brady and R. Paul (MIT Press,
1984), pp. 735-747.

6. O. Khatib and J. F. LeMaitre, "Dynamic Control of Manipulators Operating in a Complex

Environment," in Proceedings of the Third International CISM-IFToMM Symposium
(1978), pp. 267-282.

7. C. A. K l e in and C. H. Huang, "Review of Pseudoinverse Control for Use with Kinemati­

cally Redundant Manipulators," IEEE Transactions on Systems. Man, and Cybernetics 13, no.
3 (1983): 245-250.

8. Y. Nakamura and H. Hanafusa, "Inverse Kinematic Solutions with Singularity Robustness

for Robot Manipulator Control," ASME Journal of Dynamic Systems. Measurement, and
Control 108 (1986): 163-171.
Appendix 1 Function atan2

When we consider the problem of finding an angle 0 for given consistent

values of sinO and cosO, it is obvious that there exists a unique solution in
the region -180° < 0 � 180°, or under a module of 360°. There has not,
however, been a commonly used means for expressing this solution by a
single function. Function atan2 provides such a means in the form of an
extension of the function tan -1 0. The name "atan2" comes from a sub­
routine name in computer programming languages that gives a unique
solution of the arc tangent function.
Function atan2 is a scalar function of two scalar arguments defined by

atan2(a,b) = arg(b + ja), (ALl)

where a and b are real numbers, j is the imaginary unit, and arg is the
argument of a complex number (see figure Al.1). For notational conve­
nience, we define atan2(0,0) to be indeterminate. Several properties and
applications of this function are summarized in this appendix.
It is obvious that

8 = atan2(sinO, cosO) (Al.2)

and

{} = atan2(k sin{}, k cosO) (AU)

for any positive scalar k.

It is also easy to show the following equalities:

atan2( -a,b) = -atan2(a,b), (Al.4)

tn ± atan2(a,b) = atan2(b, =+= a), (Al.5)

n ± atan2(a,b) = ± atan2( - a, -b). (A 1.6)

For the differentiation of atan2, we have

oatan2(a,b) b
(A1.7a)
a2
=

oa + b2'

oatan2(a,b) -a
(A1.7b)
a2
=

ab + b2'

Hence, when a and b are differentiable functions of time t, we have

260 Appendix 1

1m

a 1------,.,

arg(b+ja)
o b

Figure AU
Argument of complex number.

Figure Al.2
Cosine theorem.

datan2(a,b) lib - ab
(A 1.8)
dt a2 + b2'
where a = da/dt and b = db/dt.
As an application, the cosine theorem will be expressed using atan2. For
the triangle shown in figure Al.2, assume that 11 � 0, 12 > 0, and 13 > O.
The cosine theorem tells us that

112 = 1/ + 1/ - 21213cos(J1' (A 1.9)

We let

K = J(l12 + V + 1/ )2 - 2(114 + 1 / + 134), (AUO)

and then we have

21213sin(J1 = ±K.

Hence, we obtain from equation AU

(J1 = ±atan2(K, 1/ + 132 -/12). (AU 1)
Function atan2 261

Figure AI.3
Geometric expression of k1sin6 + k2cos6 = k3.

Although this expression has been derived under the assumption that
12 > 0 and 13 > 0, it is valid also for the case of 12 = 0 or 13 = 0 in the sense
that 01 is indeterminate by the notational convention we have made. Note
that the cosine theorem does not hold if the condition

is not satisfied.
As a second application, we derive the solution 0 of

kl sinO + k2cosO = k3, (AU2)

where k l' k 2, and k3 are known scalar constants satisfying k 12 + k22 � k/.
Geometrically, as shown in figure Al.3, the problem is to find angle 0
between the X axis and a straight line such that k3 is the distance between
the origin and the foot of the normal drawn from point (k1, k2) to the
straight line. Now we let

(Al.13)
Since

k3 = Jk/ + k/cos(O - ¢J), (A 1 . l 4)

we have

Jk1 2 + k/sin(O - ¢J) = ±Jk12 + kz2 - k/ . (ALl5)

Thus, from equations AU, A1.l4, and A Ll5 we obtain

o - ¢J = ± atan2(Jk1z + k/ - k/ , k3)' (A1.16)
262 Appendix 1

Therefore, the solution to equation A 1.12 is gi v en by

(J = atan2(k1,k2) ± atan2(Jk12 + k/ - k/ ,k3)' (AU7)

Function atan2 was first introduced into robotics by R. P. Paul in Robot

Manipulators: Mathematics. Programming. and Control (MIT Press, 1981).
Appendix 2 Pseudo-Inverses

Inverses of matrices play an important role in linear algebra. They are

particularly familiar as a means to express compactly the solution to a
system of simultaneous linear equations. However, a matrix has an inverse
only when it is square and nonsingular. The pseudo-inverse is a generaliza­
tion of the inverse to the case of singular or even nonsquare matrices. In
the field of robotics, pseudo-inverses are often used in the identification of
manipulator dynamics and in the control of redundant manipulators.
Although this appendix will deal only with matrices of real elements, the
following arguments are also valid for matrices of complex elements with
a little modification.
We start with the definition of the pseudo-inverse. For every finite m x n
real matrix A, there is a unique n x m real matrix A+ satisfying the follow­
ing four conditions:

(A2.1)

(A2 2)
.

(AA+)T = AA+, (A2.3)

(A+ Af = A+ A. (A2.4)

This A+ is called the pseudo-inverse of A.

We first show the existence of A+ satisfying equations A2.1-A2.4. If
A =0, then the obvious solution A+ = 0 satisfies these conditions. If we
assume that A # 0 and let r = rankA, then A can be expressed as the
product of an m x r matrix B and an r x n matrix C:

A = BC (A2.S)

Using the above Band C, we define an n x m matrix D by

D = CT(CCT(I(BTB)-lBT. (A2.6)

Then it is easy to show that A+ = D satisfies equations A2.1-A2.4. This

completes the proof of the existence of A + .
Next we will show the uniqueness of A+. Let any two matrices satisfying
equations A2.1-A2.4 be A I + and A2 +. Then

Al+ -A2+ =AI+AA1+ -A2+AA2+

=ATAI+TAt+ -A2+A2+TAT
== (AA2 + A)TAl +TAl + -A2 + A2 +T(AAt + Af
264 Appendix 2

=
ATA2 +TATAl +TAl + - A2 + A2+TATAl +TAT

=ATA2+TA1+ -A2+A1+TAT
=A2+AA1+ -A2+AAl+ =
0 , (A2.7)

proving the uniqueness of A+.

The pseudo-inverse has the following properties:

(i) (A+)+ =
A. (A2.8)

(ii) (AT)+ =
(A+ )T,(AATt =(A+fA+. (A2.9)

(iii) Let A be an m x n matrix and Bbe an n x p matrix. Then generally

(ABt is not equal to B+ A+. If, however, rankA =
rankB n, then
=

(ABY =
B+ A+. (A2.l0)

(iv) A+ =
(ATAt AT = AT(AATt. (A2.11)

(v) If an m x n matrix A satisfies rankA =

m, then
A+ = AT(AATfl. (A2.l2)

Also, if rankA = n, then

A+ =
(ATA)-lAT. (A2.l3)

(vi) Let r rankA, and let the singular-value decomposition (see appen­
=

dix 3) of A be

(A2.l4)

where U and Vare appropriate orthogonal matrices and I is the following

m x n matrix:

o I
I
I
I

I
I
0
r
I
I =

I
(A2.l5)
o ar :
---------- I --
T
I
o I
0 }m - r
"-y-----) '-.,-'
r n-r
Pseudo-Inverses 265

w ith

Then A+ is given by

A+ = VI+UT, (A2.16)
where I+ is the n x m matrix defined by

o I
I
I
I
I
I
0
r.
I
I
(A2.17)
-1 I
o (1, I
-------------- � --
I
o i 0 }n- r

�----�yr----�I �

r m-r

Note that I+ is the pseudo-inverse of I.

(vii) Let us consider a system of simultaneous linear equations gi v en by

Ax=h, (A2.l8)

where A is a known m x n matrix, h is a known m-dimensional vector, and

x is an unknown n-dimensional vector. When A is square (m n) and =

nonsingular the solution to equation A2.18 is, as is well known, given by

,

(A2.l9)

When A is not nonsingular, if A and h satisfy

bE R(A), (A2.20)

then equation A2.18 is solvable but the solution is not unique. Taking this
into account, we now consider a more general problem of minimizing the
Euclidean norm of error (Ax - b):

!lAx - hll = J{Ax - b)T{Ax - h). (A2.21)

As is proved below, the general solution to this problem is

(A2.22)
266 Appendix 2

where k is an arbitrary n-dimensional vector. If equation A2.l8 has a

solution, equation A2.22 is its general solution; if equation A2.l8 has no
solution, then equation A2.22 is the general form of its best approximate
solution in the sense that it minimizes the norm (equation A2.21). Further­
more, if the best approximate solution is not unique, the one that minimizes
its own norm Il xll is given by

(A2.23)

We will prove that equation A2.22 is the general solution to the problem
of minimizing IIAx - hll. From equations A2.8-A2.l1, we have

(A2.24)

Using this relation, we can show that

hT[I - (AA+f AA+Jh

+ [x - A+ h - (I - A+ A)kFATA[x - A+ h - (I - A+ A)k]

= xTATAx - 2XTATh + hTh

(A2.25)

Since the first term on the left-hand side of equation A2.25 is independent
of x, IIAx - hll becomes minimum if and only if the second term is equal
to zero, that is, if and only if

(A2.26)

Hence, equation A2.22 is a solution to the problem for any k. On the other
hand, since equation A2.26 is equivalent to

(A2.27)

by equation A2.1, any solution x* to the problem must satisfy

Ax* = AA+h. (A2.28)

Thus,

x* = A+ b + x* - A+ b

= A+ h + (I - A+ A)x*. (A2.29)
Pseudo-Inverses 267

Therefore, x* can be expressed in the form of equation A2.22. This com­

pletes the proof.

The reader who wishes to learn more about pseudo-inverses is referred

to the following:

A. Ben-Israel and T. N. E. Greville, Generalized Inverses: Theory and

Applications (Wiley, 1974).
T. L. Boullion and P. L. Odell, Generalized Inverse Matrices (Wiley, 1971).
C. R. Rao and S. K. Mitra, Generalized Inverse of Matrices and Its Applica­
tions (Wiley, 1971).
Appendix 3 Singular-Value Decomposition

Suppose that we are given an m x n real matrix A. Then ATA is a non­

negative matrix whose eigenvalues (i.e., the solutions of det ( .Un - AT A) =

0) are nonnegative real numbers. We let the eigenvalues be A1,A2, ,An • • .

(,.1.1 � ,.1.2 � '" � An � 0). We also let

(1i = fi, i = 1,2, ... , min (m, n). (A3.l)

Obviously we have (11 � (12 � • . • � (1min(m.n) � O. We can now express the

matrix A as the product of three matrices:

(A3.2)

where U is an m x m orthogonal matrix, V is an n x n orthogonal matrix,

and E is an m x n matrix defined by

(11 0

o ifm � n

E= o (A3.3)

if m < n.

The right-hand side of equation A3.2 is called the singular-value decom­

position, and (Ji (i = 1,2, .. . , min (m, n) are called singular values. The num­
ber of nonzero singular values is

r = rankA. (A3.4)

Since U and V are orthogonal, they satisfy

UUT = UTU = 1m, VVT = VTV = In. (A3 . S)

Let us consider the meaning of the singular-value decomposition of A

in relation to the linear transformation y = Ax. Letting Yu = UTy
- and
Xv = VT x, from equation A3.2 we have

Yu = Exv· (A3.6)

This implies that the transformation from x to y can be decomposed into

three consecutive transformations: the orthogonal transformation from x
Single-Value Decomposition 269

to Xv by VT, which does not change length; the one from Xv to Yu, in which
the ith element of Xv is multiplied by (jj and becomes the ith element of Yu
without changing its direction; and the orthogonal transform from Yu to
y by U, which does not change length. Therefore, the singular-value de­
composition highlights a basic property of linear transformation.
A scheme to obtain the singular-value decomposition follows. First, we
calculate the singular values by equation A3.I. We note that, since the
numbers of nonzero eigenvalues for ATA and AAT are the same, it is
computationally more efficient to find the singular values using the eigen­
values of AAT when m < n and those of ATA when m > n.
Next we obtain U and V. We define a diagonal matrix 1:, using r nonzero
singular values by

(A3.7)

This is the r x r principal minor of 1:. We let the ith row vectors of U and
V be Ui and Vi' respectively, and let

and

Then, from equation A3.2,

Also, from equation A3.5,

U,rU, = I,

and

Hence, we have

ATA V. = V.1:,2 , (A3.8)

U, = A V.1:, -1. (A3.9)

Since equation A3.8 can be decomposed into

270 Appendix 3

ATAvi=ViO/, i=1,2, . . . ,r (A 3 1 O)
.

we see that Vi is the eigenvector of unit length for eigenvalue Ai of ATA.

Thus we can determine V. from the eigenvectors of ATA for eigenvalues
Al ,A2,· .. , Ar· The part of V other than v., which consists of vectors Vr+l,
Vr+2, , Vn, is arbitrary as long as it satisfies equation A3.5. From the
• . .

obtained V we can determine Ur by equation A3.9 and the other part of U

by equation A3.5. Instead of equations A3.8 and A3.9, we can also use

AAT Ur = UrIr2 (A3.8')

and

�
(A3.9')

r�! �� l].
A numerical example will illustrate the above sheme. Suppose that

A =

�l
o 1 -1

[ �� ]
Then

-1
ATA � 3 -2 .

-1 -2 3

Since

det(AI3 - ATA) = A(A - 3)(A - 5) = 0,

the eigenvalues of ATA are Al = 5, A2 = 3, and A3 = 0, and the singular

values of A are crt = J"S, cr2 = J3, and (13 = O. The eigenvectors Vi of ATA
for Ai (i = 1,2) are

VI = [0, l/.ji, -l/.jiF

and

Substituting V. = [VI' V2] into equation A3.9, we obtain

Single-Value Decomposition 271

ur
= [2/fo -lifo -lifo 2/fo T ]
.

[vrs
0 -1/J2 1/J2 0
Hence, a singular-value decomposition of A is given by

�l
0
O J3

fl JW
I= 0 0
o 0

l
0 1/J2

[�JW
_ -lifo -1/J2 0 2/fo
u- 0
-lifo 1/J2 2/fo
2/fo 0 -lfJ2 lffo
'

and

v= [ 1�v'2 21./6 1/J3 ]

- 1 /./6 1/J3 .
- 1/J2 -1/.[6 1/J3
The reader who wishes to learn more about singular-value decomposi­
tion is referred to the following:

V. C. Klema and A. J. Laub, ''The Singular Value Decomposition: Its

Computation and Some Applications," IEEE Transactions on Automatic
Control 25, no. 2 (1980): 164-176
.
G. E. Forsythe, M. A. Malcolm, and C. B. Moler, f
Computer Methods or
Mathematical Computations (Prentice-Hall, 1977).
Appendix 4 Lyapunov Stability Theory

This appendix is a brief review of the stability theory by the Lyapunov

method for a nonlinear autonomous system

i =f(x), (A4.l)

where x is an n-dimensional state vector and f(x) is a nonlinear function

ofx. In many cases the dynamics of a system are given not by equation
A4.1 but by an nth-order differential equation

x(n) = f(x,x, . . . ,x(n-1», (A4.2)

where x is a scalar variable and

xli) is its ith time derivative. Even in that
case, however, we can reduce the expression A4.2 into A4.1 by letting

x = [x,x,i, ... ,x(n-1) ] T (A4.3)

and

X
i
f(x) =
(A4.4)
x(n -1)

f(x,x, . . ,x(n-1»
.

We can reduce the expression A4.2 to A4.1. Thus, A4.l is a fairly general
expression for dynamics systems. In the following, we discuss the stability
of the equilibrium point of system A4.l, that is, the equilibrium state i such
that

f(i) = O. (A4.5)

Note, however, that there is no loss of generality even if we assume that

the equilibrium point is at the origin 0, i.e., i
O. We can say this because
=

when i =P 0 we can always transfer the equilibrium point to

0 by letting
x - i be a new state vector x and expressing equation A4.1 in terms of the
new state vector.
The stability of the origin 0 is defined as follows:

(i) The origin 0 is said to be stable if there exists a <> > 0 for any given e > 0
such that the solution x(t) to equation A4.l with an arbitrary initial state
x(O) such that Ilx(O)1I < <> satisfies IIx(t)1I < E for all t � O.
(ii) The origin 0 is said to be asymptotically stable if it is stable and there
Lyapunov Stability Theory 273

c,

���----�! �! ---- �
Surface with Surface with
no friction viscous friction

Figure A4.1
Stability of equilibrium point.

exists a tJ' > 0 such that the solution x(t) starting from any initial state x(O)
satisfying IIx(O)II < (j' converges to 0 as time goes to infinity .
(iii) The origin 0 is globally asymptoticall y stable if it is stable and the
solution x(t) starting from any initial point x(O) converges to 0 as time goes
to infinity.

For example, consider the ball in figure A4.l sliding on a curved surface
with gravi ty acting downward. The equilibrium point A is stable (the ball
continues to oscillate near the bottom because of the frictionless surface),
B is asymptotically stable (the ball reaches the bottom and stops there after
infinite length of time because of viscous friction), and C1 and C2 are not
stable.
Before defining the Lyapunov function, we introduce the concept of
positive-definite and positive-semidefinite functions of x. Let us consider a
scalar function v(x) defined over a region D includi ng the origin. The
function v(x) is said to be positive definite (or positive semidefinite) in D if
v(O) = 0 and v(x) > 0 (or v(x) � 0) for any XED such that x i= O. Similarl y,
v(x) is said to be negative definite (or negative semidefinite) in D if v(O) 0
=

and v(x) < 0 (or v(x) � 0) for any xED such that x i= O.
The function v(x) is called the Lyapunov function if v(x) is positive
definite in D, its partial derivative ov(x)/ox is continuous, and the time
derivative

v(x) dv(x(t» (OV(X»)T f(x) (A4.6)

= =

dt ox

is negative semidefinite.
274 Appendix 4

vLr)

x,

X,

Figure A4.2
Lyapunov function.

Theorem 1 The equilibrium point 0 of system A4.1 is stable if and only if

the following condition is satisfied in a n eighborhood Q of the origin:

(i) There exi sts a Lyapunov function.

The equilibrium point 0 is asymptotically stable if and only if condition i

and the followi ng condition are satisfied in Q:

(ii) The time derivative of v(x) v(x), is negative definite.

,

Condition ii can be replaced by the following:

(ii') No solution x(t) to equation A4.1 for any initial condition x(O) such
that x(O) # 0 satifies v(x(t» = 0 identically. 0

This theorem is called Lyapunov's stability theorem.

Schematically, the Lyapunov function for the case of n = 2 is like the
cup shown in figure A4.2. Condition i in the above theorem implies that
the point [xT(t), v(x(t»Y on the cup, which corresponds to a point x(t) of
the trajectory of the system A4.1 on the Xl -X2 plane, does not ascend as
time passes. Condition ii implies that the point always descends until the
point reaches the o rigin after an infinite length oftime. Condition ii' implies
that, as shown in figure A4.3, although the point on the cup may not
deecend but may move in a horizontal di rection either at an instant of time
or for a fi nite duration of ti me it eventually starts to descend again
, .

The following theorem gi ves a condi tion for global asymptotic stability .

Theorem 2 The equilib ri um point 0 of system A4.l is globally asymptoti­

cally stable if and only if conditions i and ii and the following condition
Lyapunov Stability Theory 275

v( x)

X,

Figure A4.3
Condition ii'.

are satisfied in the whole space of x:

(iii) The function v(x) tends to infinity when IIxll tends to infinity.

Condition ii can be replaced by condition ii'. 0

The next result is a generalization of condition ii'. A subset S of the

n-dimensional Euclidean space R" of x is called an invariant set for A4.1 if
the solution trajectory of A4.l starting from arbitrary initial state x(O) in S
remains in S all the time.

Theorem 3 Let us assume that there exists a Lyapunov function v(x) for
system A4.1 in the whole space of x. Let E be the set of all states x satisfying
6(x) = 0, and let M be the largest invariant set for A4.l included in E. Then
an arbitrary finite solution of A4.1 tends to M as time goes to infinity. In
addition, if condition iii is satisfied for all x, all solutions of A4.l tend to
M as time goes to infinity. 0

A special case of theorem 3 where M consists only of the origin 0

corresponds to the case where condition ii' holds in theorem 2.
As a candidate for a Lyapunov function, we often use a quadratic form
xTAx, where A is a real symmetric matrix. When A is not symmetric, the
matrix B (A + AT}/2 is symmetric and satisfies xTAx xTBx, so without
= =

any loss of generality we can assume that A is symmetric. The following

classes of symmetric matrices are useful. An n x n real symmetric matrix
A is said to be positive definite (positive semidefinite) if xTAx> 0 (� 0) for
any nonzero n-dimensional real vector x. Similarly, A is said to be negative
276 Appendix 4

definite (negative semidefinite) if -

A is positive definite (positive semi­
definite).
The following theorem is useful for jUdging whether a given matrix A is
positive definite (positive semidefinite) or not. Let the (i,j) element of A be
a(i,j), and let the principal minors be denoted by A(il,i2,··· ,ik):

a(il,il} a(il,i2} a(il,ik}

a(i2,id a(i2,i2) a(i2,ik)
A(il,i2,··· ,ik) =
(A4.7)

a(ik,id a(ikoi2) a(ik,ik)

where 1 � il < i2 < ... < ik � n, k = 1 ,2, ... ,n, and 1'1 denotes the deter­
minant.

Theorem 4 (Sylvester's criterion)

(i) A symmetric matrix A is positive definite if and only if all the principal
minors are positive, or, equivalently,

A (1 2 ... ,k)
, , > 0, k = 1,2, . . . , no (A4.8)

(ii) A symmetric matrix A is positive semidefinite if and only if all the

principal minors are non-negative:

(A4.9)

where 1 � il < i2 < . . .

< ik � nand k = 1 ,2, . . . ,no 0

The reader who wish to learn more about Lyapunov stability theory is
referred to 1. La Salle and S. Lefschetz, Stability by Lyapunov's Direct
Method (Academic, 1961 ).
Solutions to Selected Exercises

Chapter 1

1.1 Seven degrees of freedom. An example of the structural model is

shown in figure E.1.

1.2 The singular configuration is shown in figure E.2.

1.3 Five degrees of freedom.

Chapter 2

2.1 If cosO then r/J

-# atan2( ± Rw ± R 11 )' 0
0,
atan2( -R 3 1 , ± JR3/ + R3/), and t/! atan2( ±R32, ± R33)' If cosO = =

0, then rP is arbitrary, 0 -R31 X 90°, and t/!

= atan2(R12,R22) + R31r/J· =

2.2 From equations 2.23a and 2.23d, sint/! -sinr/JRll + COSrPR21; from =

equations 2.23b and 2.23e, cost/! -sinrPR12 + cosr/JR22· Thus, t/!

= =

atan2(-sinrPR11 + COSr/JR21,-sinr/JR12 + COSr/JR22)·

2.5 (r/J, 180°, 120° + rP), where rP is arbitrary.

2.7 Transform Tp gives the image of an original point when a convex lens
with focal distance f is placed on the X -Z plane.

2.11 rx = 11 C1 + 12 C1 2 ' ry = l1 S1 + 1 2S1 2, rz = d3, (X = 01 + O2 + 04,

2.16 T = [0, - 100(lc + Ie), -1OOle,O,O,O]T.

2.17 AXBT AZB AXBT(AXB X AyB) AYBTeXB

= = X A XB) = 0,
1 = AZBTAZB AXBT[AYB X (AXB X AyB»
=

= AXBT[(AYBT AYB)AXB - (AYBT AXB)AYBJ 1 = - (AYBT AXBf.

Chapter 3

B B B
3.1 J = fv [rTrI3 - rrT]pdv = fv [BrT rI3 - vRB r rT(VRB)T]pdv
VR BJ(VRB)T.
B
=

2 2
3.2 r1 = (m 1l9 1 + II 12)01 + 2m2d 2d281
+ m2d2 +
+ (m11g1
..
m2d2)C1g,
+
r2 = m2d2 - m2d201 2 + m2S1g· •
278 Solutions

Figure E.t
Seven-joint manipulator.

Figure E.2
Singular configuration of four-joint wrist.

Chapter 4

4.3 The singular values are

(Ji = [(at ± Ja/ + 4ao }/2]1/2,

where
Solutions 279

y
---- : 1,= I,
- : 1,=0.7071,
--- : 1,=0.51,

FigureE.3
Solution to exercise 4.3.

a1 = 11 2 + 2/1/2C2 + 2//
and

ao = 1/122S/.

The arm posture maximizing W2 is given by e2 satisfying

The optimal arm posture is shown in figure E.3 for various ratios of 11 and
12·

4.4 W = I de tll= il/2lsin(e2 - e1 )1. The optimal arm posture is given by

e2 - e1 = ± 90°.

Ui =.j""i;,
where

Ai = {(I/ + 1/) ± [(112 + 122)2 - 4/12/22sin2(e2 - (1 )]1/2}/2

(i = 1 corresponds to +, and i = 2 corresponds to -).

Chapter 5

5.4 kr = [ -0.358,0.358,0.862Y, IXr = 81.6°.

5.7 Let t = (uw/)1/3t and let C = Ju2 - 2u(w, + w/. Then

y(t) = 1 - Aexp(-ut) + ABexp(-(wJ)sin( J1=12wJ + e),
2
where A w//C , B uC/(w/J1=12), and sine
(2(w, - u)J1=12/c.
280 Solutions

5.8 U = - [xt2 - 2xtx2 - 4x/ + 3x1 + X2 + A1Ut - A2X2U2,

2xt + 4X2 + A2U2Y.

Chapter 6

6.1 md = 1.25, kd = 625, dd = 57.5.

6.5 Let r = [x,y,z,t/J,e,,,,Y, where x, y, and z are the X, Y, and Z coordinates

and t/J, e, and '" are Euler angles of the end effector with respect to the
reference frame. Then the constraint su rfac es are given by /2 x2 - y2 = 0, -

Z - Zo 0, e - (}o 0, and", "'0 0, where zo, eo, and "'0 are constants.
= = - =

6.7 Letting Ux = au/ox, uxy = 02U/(oxoy), et c , we have

.

1 o o OJ
EF = -u x : 0 1 + u / 0 0 ,
[ -. <>1 0 o 153 0

where

and

Matrix Ep is, for example, g i ven by

Ep = [� � � � � �].
0 0 0 0 0 1

Chapter 7

7.1 Show t hat A = CB an d A + = B(CBt satisfy equations A2.1-A2.4 in

appen dix 2. Use th e fact that (I - J1 + J1) is s ymme tr ic and idempotent.

7.2 Substitute equations 7.5 and 7.6 into equation 7.17, and let the deriva­
tive of PI with respect to kl be equal to O.
Solutions 281

7.3 Substitute equations 7.5 and 7.6 into equation 7.18, and let derivative
of P2 with respect to k1 be equal to O. Use

(1- J1 + J1f(1 - J1 + Jd = (I - J1 + Jd·

7.7 A solution method is to specify the second subtask by the performance
criterion
3

V(q) L {1/(Oimax - oy + 1/(Oi - Oimin)2},

i=1
=

which is to be kept small, and to obtain qd by the approach in subsection

7.2.4.
Index

Accelerations, link, 102 Dynamic hybrid control, 227

Active impedance,211,214,218 Dynamic manipulability, 143
Actuator dynamics, 197 Dynamic-manipulability ellipsoid, 145
Actuators, 167 Dynamic-manipulability measure, 146
Adaptive control, 199 Dynamic parameters, 1 12
Algebraic approach, 45 Dynamics, 81
Angular momentum, 82
velocity vector,54 Effecti ve inertia, 90
Arm mechanisms,2 Elbow singularities, 70
Artificial constraints, 226 Elephant Trunk wrist,4
Assignment of poles, 193 End effector, 39, 161,227
Asymptotically stable,168,272 position of, 28
atan2, 20, 259 velocity of, 57
Avoiding obstacles, 249 Equations of motion, 8 1
Avoiding singularities,253 Equivalent angle o f rotation, 163
Equivalent axis of rotation, 163
Centrifugal force, 90 ET wrists, 4
Complementary sensitivity function, 189 Euclidean norm, 128,247
Compliance center, 214 Euler angles, 18
Compliance control, 222 Euler method, 1 1 1
Computation, amount of, 175 Euler's equation, 82
Computational load, 108 Evaluation of manipulators, 127
Computed torque method, 174
Condition number, 143 Feedback compensator, 188
Constraining force, 231 Feedforward compensator, 188
Constraint frame, 224, 229 Fingers, 139
Constraint surfaces, 228 Force, 71, 23 1
Control,9 Force control,9,211
dynamic hybrid, 227 Force sensor, 1 1,73, 120
force, 21 1 Force servo loop, 237
hybrid, 222 Four-degrees-of-freedom wrist, 4
of redundant manipulators, 244 Frame, 13
position, 155 Friction, 106, 121
Controller, 9
Control subsystem, 1 Gear ratio, 183
Coordinate transformation,23 Generalized coordinates, 86
Coriolis force, 91 Generalized force, 87
Cosine theorem, 260 Geometric approach, 45
Coupling inertia, 90 Globally asymptotically stable, 273
Criterion function,248 Gravity force,91
Cylindrical-coordinate manipulators, 2, 139
Hand, 14,73, 139
Damping coefficient, 184 RCC, 212
Decoupling control, 192 Homogeneous transform, 23
Degrees of freedom,2 Horizontal multi-joint type, 2
Denavit-Hartenberg notation,33 Hybrid control, 222
Desired trajectory, 155,247 dynamic,227
Direct dynamics problem,109
Direct kinematics problem, 29,39 IBM 7565 robotic system,8
Direction cosine matrix, 17 Identifiable parameter, 115
Disturbances, 172 Identificartion, 1 12
DME,145 ofload,122
Double-axis rotation method, 164 Impedance control, 211
Dual-sampling-rate approach, 176 Indices of manipulability, 142
284
Inertia matrix, 91
Inertia tensor, 83 93
, Momentum, 82
Inertial coordinate frame, 82
Inertial force, 90
Instantaneous axis of rotation, 54
Instantaneous optimization problem, 249
Integral action, 170
Intennediate point, 158
Inverse dynamics, 177
Inverse dynamics problem, 108
Inverse kinematics problem, 29, 45
Inverse, of homogeneous transfonn, 26
pseudo-, 67, 256, 263
Jacobian matrix, 53, 57, 63,246
Joint actuators, 9
Joint angle, 33
Joint axis, 33
Joint length, 33
Joint variable, 28
Joint-variable scheme, 155
Joint vector, 28
Joint velocity 57
, Parallel proce ssing 176
Kawasaki Unimate, 8
Kinematic equation, 29, 47, 51
Kinematic parameters, 112
Kinematics, 13
Kinetic energy, 87
Lagrange's equation, 86
Lagrangian fonnulation, 81, 113
Lagrangian function, 87
Linear feedback control, 166
Linearization, 170
Linearizing compensation, 193
Link accelerations, 102
Link frames, 33
Link length, 33
Link parameters, 31
Lyapunov function, 169,203,273
Lyapunov stability theorem, 168
Lyapunov stability theory, 202, 272
Manipulability, 127
dynamic, 143
Manipulability ellipso id 128
, Product of homogeneous transfonn, 26
Manipulability measure, 131, 255
Manipulating-force ellipsoid, 133
Manipulation vector, 246
Manipulators, 1
Mechanisms, 1
Modeling error, 172
Moment, 72
Index
Moments of in er tia 84
,
Motion subsystem, 1
Natural angular frequency, 184
Natural constraints, 226
Negative definite, 275
Newton's equation, 82
Newton-Euler fonnulation, 81, 100
Nonnalization, 131, 146
Nyquist stability criter i on, 189
Object frame, 13
Obstacle, 249
Open-loop link mechanisms, I
Optimal configura tions, 1 33
Orientation of objects, 13
Orthogonal-coordinate manipulators, 2,
139
Parallel-axis theorem, 94
Parallel drive, 98,125
,
Parallelogram, 125
Parameter estimation, 202
Passive impedance 211
,
Perspective transform, 78
PI c ont rol law, 224
PID control law, 224
Pieper's approach, 51
Pivot join, 1
Point-to-point control, 168
Polar-coordinate manipulators, 2, 139
Polynomial, 156
Position control, 9, 155
of objects, 13
servo loop, 237
Positioning accuracy, 2
Positive definite, 275
Potential energy, 87
Principal axes,
of manipulability ellip so id 130
,
of moment of inertia, 86
Principal minors, 276
P rismatic j oint, 1,63,65
Processor, 177
Products of inertia, 84
Pseudo inertia matrix, 92
Pseudo-inverse, 67, 256, 263
PUMA, 6, 40, 52, 65, 70, 137
RCC hand, 212
Reachab le region, 70
Index 285

Real time, 109 Third-order sytems, 185

Recognition subsystem, 1 Three Roll Wrist, 4
Reduction ratios, 167 Three-link manipulator, 46, 124, 249
Redundant manipulators, 244 Trace, 91
Redundant representation, 17 Tracking error, 201
Reference frame, 13 Trajectory control, 9
Regularization, 256 Transform, 78
Remote center compliance hand, 212 Transmission ratio, 133
Representational singularities, 56 Twist angle, 33
Resolved acceleration control, 174 Two-link manipulator, 30, 69, 88, 96, 98,
Revolute joint, I, 63, 64 117, 172, 235
Robot system, 1 Two-link mechanism, 133, 148
Robustness, 187 Two-stage control, J 70,200
Roll, pitch, and yaw angles, 22
Rotation, Universal joints, 4
equivalent angle of, 163
equivalent axis of, 163 Vector product, 60
Rotation matrix, 14 Velocity, 53, 57, 60
Runge-Kutta method, 111 Vertical multi-joint type, 2
Virtual work, 75
Sampling period, 175
SCARA,9,135 Wrist, 4
Second-order system, 185 Wrist mechanisms, 2
Sensitivity, 187,239 Wrist singularities, 70
function, 189
Sensors, 9, 11
Serial drive, 91
Servo compensation, 170
Servo compenstors, 182
Servomechanism, 9
Servomotor, 182
Shoulder singularities, 70
Simulation, 110
Single-axis rotation method, 163
Singular configuration, 4, 67, 131, 196,253
Singular-value decomposition, 129, 264, 268
Singular values, 268
Singularities, 70
representational, 56
Skew coordinate system, 56
Small gain theorem, 189
Stability, 272
Stability margin, 187,239
Stanford manipulator, 36
Statics, 71
Step response, 184
Stiffness control, 222
Stretching transform, 78
Subtasks, 177,245
Sylvester's criterion, 276

T3-786 robot, 7
Task-decomposition approach, 245
Third-order decoupled system, 198