OF ROBOTICS
Analysis
and Control
FUNDAMENTAIS
OF ROBOTICS
Analysis
and Control
7
Robotic Manipulation
The term robot can convey many different meanings in the mind of the reader, depending on the context. In the treatment presented here, a robot will be taken to
mean an industrial robot, also called a robotic manipulator or a robotic arm. An example of an industrial robot is shown in Fig. 11. This is an articulated robotic arm
and is roughly similar to a human arm. It can be modeled as a chain of rigid links
interconnected by flexible joints. The links correspond to such features of the human
anatomy as the chest, upper arm, and forearm, while the joints correspond to the
shoulder, elbow, and wrist. At the end of a robotic arm is an endeffector, also called
a tool, gripper, or hand. The tool often has two or more fingers that open and cise.
To further characterize industrial robots, we begin by examining the role they
play in automation in general. This is followed by a discussion of robot classifications using a number of criteria including: drive technologies, work envelope geometries, and motion control methods. A brief summary of the most common applications of robots is then presented; this is followed by an examination of robot
design specifications. Chap. 1 concludes with a discussion of the use of notation and
a summary of the notational conventions adopted in the remainder of the text.
7
Robotic Manipulation
The term robot can convey many different meanings in the mind of the reader, depending on the context. In the treatment presented here, a robot will be taken to
mean an industrial robot, also called a robotic manipulator or a robotic arm. An example of an industrial robot is shown in Fig. 11. This is an articulated robotic arm
and is roughly similar to a human arm. It can be modeled as a chain of rigid links
interconnected by flexible joints. The links correspond to such features of the human
anatomy as the chest, upper arm, and forearm, while the joints correspond to the
shoulder, elbow, and wrist. At the end of a robotic arm is an endeffector, also called
a tool, gripper, or hand. The tool often has two or more fingers that open and cise.
To further characterize industrial robots, we begin by examining the role they
play in automation in general. This is followed by a discussion of robot classifications using a number of criteria including: drive technologies, work envelope geometries, and motion control methods. A brief summary of the most common applications of robots is then presented; this is followed by an examination of robot
design specifications. Chap. 1 concludes with a discussion of the use of notation and
a summary of the notational conventions adopted in the remainder of the text.
referred to as hard automation. Here the machines and processes are often very
efficient, but they have limited flexibility.
More recently, the auto industry and other industries have introduced more
flexible forms of automation in the manufacturing cycle. Programmable mechanical
manipulators are now being used to perform such tasks as spot welding, spray painting, material handiing, and component assembly. Since computercontrolled me
chanical manipulators can be easily converted through software to do a variety of
Figure 11
Robotic Manipulation
Chap. 1
referred to as hard automation. Here the machines and processes are often very
efficient, but they have limited flexibility.
More recently, the auto industry and other industries have introduced more
flexible forms of automation in the manufacturing cycle. Programmable mechanical
manipulators are now being used to perform such tasks as spot welding, spray painting, material handiing, and component assembly. Since com putercontrolled me
chanical manipulators can be easily converted through software to do a variety of
Figure 11
Robotic M anipularon
Chap. 1
Unit cost
Production volume
duction volumes [ui, U2] over which they are costeffective contines to expand at
both ends of the production spectrum.
In order to more clearly distinguish soft automation from hard automation, it is
useful to introduce a specific definition of a robot. A number of definitions have been
proposed over the years. However, as robotic technology contines to evolve, any
definition proposed may need to be refined and updated before long. For the purpose
of the material presented in this text, the following definition is used:
Definition 111: Robot. A robot is a softwarecontrollable mechanical device that uses sensors to guide one or more endeffectors through programmed motions in a workspace in order to maniplate physical objects.
Contrary to popular notions about robots in the Science fiction literature (see,
for instance, Asimov, 1950), todays industrial robots are not androids built to impersonate humans. Indeed, most are not even capable of selflocomotion. However,
many of todays robots are anthropomorphic in the sense that they are patterned after the human arm. Consequently, industrial robots are often referred to as robotic
arms or, more generally, as robotic manipulators.
12 ROBOT CLASSIFICATION
In order to refine the general notion of a robotic manipulator, it is helpful to classify
manipulators according to various criteria such as drive technologies, work envelope
geometries, and motion control methods.
121 Drive Technologies
One of the most fundamental classification schemes is based upon the source of
power used to drive the joints of the robot. The two most popular drive technologies
are electric and hydrauic. Most robotic manipulators today use electric drives in the
form of either DC servomotors or DC stepper motors. However, when highspeed
See. 12
Robot Classification
TABLE 11
Type
Symbol
Description
Revolute
Prismatic
Revolute joints (R) exhibit rotary motion about an axis. They are the most
common type of joint. The next most common type is a prismatic joint (P), which
exhibits sliding or linear motion along an axis. The particular combination of revo
lute and prismatic joints for the three major axes determines the geometry of the
work envelope, as summarized in Table 12. The list in Table 12 is not exhaustive,
since there are eight possibilities, but it is representative of the vast majority of commercially available robots. As far as analysis of the motion of the arm is concerned,
prismatic joints tend to be simpler than revolute joints. Therefore the last column in
Table 12, which specifies the total number of revolute joints for the three major
axes, is a rough indication of the complexity of the arm.
For the simplest robot listed in Table 12, the three major axes are all pris
matic; the resulting notation for this configuration is PPP. This is characteristic of a
Cartesiancoordinate robot, also called a rectangularcoordnate robot. An example
4
Robotic Manipulation
Chap. 1
TABLE 12
Robot
Cartesian
Cylindrical
Spherical
SCARA
Articulated
Axis 2
Axis 3
P
R
R
R
R
P
P
R
R
R
P
P
P
P
R
Total revolute
0
1
2
J2bt
3
P = prismatic, R = revolute.
of a Cartesiancoordinate robot is shown in Fig. 13. Note that the three sliding
joints correspond to moving the wrist up and down, in and out, and back and forth.
It is evident that the work envelope or work volume that this configuraron generales
is a rectangular box. When a Cartesiancoordinate robot is mounted from above in a
rectangular frame, it is referred to as a gantry robot.
Figure 13
Cartesian robot.
Figure 14
Sec. 12
Robot Classification
Cylindrical robot.
Figure 15
Spherical robot.
Like a sphericalcoordinate robot, a SCARA robot (Selective Compliance Assembly Robot Arm) also has two revolute joints and one prismatic joint (in the
configuration RRP) to position the wrist. However, for a SCARA robot the axes of
all three joints are vertical, as shown in Fig. 16. The first revolute joint swings the
arm back and forth about a base axis that can also be thought of as a vertical shoul
der axis. The second revolute joint swings the forearm back and forth about a verti
cal elbow axis. Thus the two revolute joints control motion in a horizontal plae.
The vertical component of the motion is provided by the third joint, a prismatic
joint which slides the wrist up and down. The shape of a horizontal cross section of
the work envelope of a SCARA robot can be quite complex, depending upon the
limits on the ranges of travel for the first two axes.
LnJ
1 <r
Figure 16
SCARA robot
Robotic Manipulation
Chap
h a revolute
[Coordnate 1
5. Here the f
axis, while
tal shoulder
rk envelope
rhe spheres
te ranges of
When the last rem aining prismatic joint is replaced by a revolute joint (to
yield the configuration RRR), this produces an articulatedcoordnate robot An
articulatedcoordnate robot is the dual of a Cartesian robot in the sense that all
three of the m ajor axes are revolute rather than prismatic. The articulatedcoordinate
robot is the most anthropomorphic configuration; that is, it most closely resembles
the anatomy of the human arm. Articulated robots are also called revolute ro
bots. An example of an articulatedcoordinate robot is shown in Fig. 17. Here the
first revolute joint swings the robot back and forth about a vertical base axis. The
second joint pitches the arm up and down about a horizontal shoulder axis, and the
third joint pitches the forearm up and down about a horizontal elbow axis. These
motions create a complex work envelope, with a sideview cross section typically
being crescentshaped.
t.
pliance A slint (in the
the axes of
swings the
tical shoul3ut a vertintal plae.
I prismatic
section of
; upon the
Figure 17
Articulated robot.
Control method
p
Applications
Point to point
Spot welding
Pickandplace
Loading and unloading
Continuous path
Spray painting
Are welding
Gluing
Chap. 1
Sec. 12
Robot Classification
The other type of motion is continuouspath motion, sometimes called controlledpath motion. Here the endeffector must follow a prescribed path in threedimensional space, and the speed of motion along the path may vary. This clearly
presents a more challenging control problem. Examples of applications for robots
with continuouspath motion control include paint spraying, are welding, and the
application of glue or sealant.
13 APPLICATIONS
Robotic applications often involve simple, tedious, repetitive tasks such as the loading and unloading of machines. They also include tasks that must be performed in
harsh or unhealthy environments such as spray painting and the handiing of toxic
materials. A summary of robot applications (Brody, 1987) is displayed in Table 14.
Here the percentages listed in the second column represent shares of the overall
robot market in the United States for the year 1986. The size of the market in 1986,
excluding seprate sales of visin systems, was $516 million (Brody, 1987).
TABLE 14
Application
Material handiing
Spot welding
Are welding
Spray painting and finishing
Mechanical assembly
Electronic assembly
Material removal
Inspection and testing
Water jet cutting
Other
Percent
24.4
16.5
14.5
12.4
6.2
4.8
4.5
2.9
2.7
11.1
Robotic Manipulation
Chap. 1
Country
Percent
Japan
USA.
West Germany
Sweden
France
Great Britain
Italy
Others
44.1
22.1
8.8
7.4
2.9
2.9
2.2
9.6
ROBOT CHARACTERISTICS
Characteristic
Number o f axes
Load carrying capacity
Mximum speed, cycle time
Reach and stroke
Tool orientation
Repeatability
Precisin and accuracy
Operating environment
Units
kg
mm/sec
mm
deg
mm
mm
Function
Major
Minor
Redundant
Axes
1 3
4 6
1n
Sec. 14
Robot Specifications
(141)
For example, the horizontal reach of a cylindricalcoordinate robot is the radius of the outer cylinder of the work envelope, while the horizontal stroke is the
difference between the radii of the concentric outer and the inner cylinders, as
shown in Fig. 18.
The vertical reach of a robot is the mximum elevation above the work surface
that the wrist mounting flange can reach. Similarly, the vertical stroke is the total
vertical distance that the wrist can travel. Again, the vertical stroke is less than or
10
Robotic Manipulation
Chap. I
Figure 18
equal to the vertical reach. For example, the vertical reach of a cylindrical robot will
be larger than the vertical stroke if the range of travel of the second axis does not
allow the wrist to reach the work surface, as shown in Fig. 18. One of the useful
characteristics of articulated robots lies in the fact that they often havefu ll work envelopes, in the sense that the stroke equals the reach. However, this feature gives
rise to a need for programming safeguards, because an articulated robot can be programmed to collide with itself or the work surface.
144 Tool Orientation
W hile the three major axes of a robot determine the shape of the work envelope, the
remaining axes determine the kinds of orientation that the tool or hand can assume.
If three independent minor axes are available, then arbitrary orientations in a three dimensional workspace can be obtained. A number of conventions are used in the
robotics literature to specify tool orientation (Fu et al., 1987). The tool orientation
convention that will be used here is the yawpitchroll (YPR) system. Yaw, pitch,
and roll angles have long been used in the aeronautics industry to specify the ori
entation of aircraft. They can also be used to specify tool orientation, as shown in
Fig. 19.
Figure 19
tool.
Sec. 14
Robot Specifications
11
Operation
Description
Axis
1
2
3
Yaw
Pitch
Roll
f l
P
P
An alternative way to specify the YPR motions is to instead perform the rota
tions in reverse order about the axes of the mobile tool frame M rather than the fixed
wrist ffame F. That is, first a roll motion is performed about m 3, then a pitch motion
is performed about m2, and finally a yaw motion is performed about m l. This is
equivalent to performing the rotations about the axes of the fixed wrist frame F in
the original YPR order, in the sense that the tool ends up at the same orientation.
For this reason, the YPR system is often referred to as the RPY system. A sequence
of rotations about the mobile frame M axes is often easier to visualize, particularly
when the angles are not mltiples of tt/2 .
Definition 141: Spherical W rist, A robot has a spherical wrist if and only
if the axes used to orient the tool intersect at a point.
The robot wrist shown in Fig. 19 is an example of a spherical wrist because
the three axes used to orient the tool, {m1, m2, m 3}, intersect at a point. More generally, if a robot possesses fewer than three axes to orient the tool, then it can still
have a spherical wrist. However, in these cases the spherical wrist will not be a gen
eral spherical wrist, because, with fewer than three axes, some orientations of the
tool cannot be achieved. Spherical wrists are useful because they simplify the mathematical descriptions of robotic manipulators. In particular, the larger naxis problem
can be partitioned into a two simpler problems, a 3axis problem and an (n 3)12
Robotic Manipulation
Chap. 1
axis problem (Pieper, 1968). Many industrial robots are designed with spherical
wrists.
145 Repeatability, Precisin, and Accuracy
Repeatability is another important design characteristic. Repeatability is a measure
of the ability of the robot to position the tool tip in the same place repeatedly. On
account of such things as backlash in the gears and flexibility in the links, there will
always be some repeatability error, perhaps on the order of a small fraction of a millimeter for a welldesigned manipulator (Groover et al., 1986).
Closely related to the notion of repeatability is the concept of precisin. The
precisin of a robotic manipulator is a measure of the spatial resolution with which
the tool can be positioned within the work envelope. For example, if the tool tip is
positioned at point A in Fig. 110 and the next closest position that it can be moved
to is B, then the precisin along that dimensin is the distance between A and B.
Precisin
A
V
A
Adjacent
tool
position
Figure 110
Accuracy
More generally, in threedimensional space the robot tool tip might be posi
tioned anywhere on a threedimensional grid of points within the workspace. Typically, this grid or lattice of points where the tool tip can be placed is not uniform.
The overall precisin of the robot is the mximum distance between neighboring
points in the grid. For example, an interior grid point of a Cartesiancoordinate
robot will have eight neighbors in the horizontal plae plus nine neighboring grid
points in the planes above and below. One of the virtues of a Cartesiancoordinate
robot is that the precisin is uniform throughout the work envelope; the state of one
axis does not influence the precisin along another axis. However, this is not the
case for other types of robots, as can be seen in Table 19, where the overall preci
sin is decomposed into horizontal and vertical componen ts.
For the case of a cylindricalcoordinate robot, the horizontal precisin is
highest along the inside surface of the work envelope and lowest along the outside
surface. This is because the are length swept out by an incremental movement A< in
TABLE 19
Vertical
Precisin
Uniform
Decreases radially
Decreases radially
Varies
Varies
Uniform
Uniform
Decreases radially
Uniform
Varies
Robot
Type
Cartesian
Cylindrical
Spherical
SCARA
Articulated
Sec. 14
Robot Specifications
13
the base joint is proportional to the radial position r, as can be seen in Fig. 111.
Note how the shape of a horizontal grid element is not square and grid elements near
the outside surface of the work envelope are larger than grid elements near the inside
surface of the work envelope.
r A0
Here the radial precisin is Ar, while the angular precisin about the base expressed in terms of the are length is r A</>. In the worst case, the angular precisin is
R A<>, where R is the horizontal reach of the robot. Thus the overall horizontal pre
cisin in this case is approximately
A/i [(Ar)2 + (R A<>)2]1/2
(142)
Here we have assumed that A</>and Ar are sufficiently small and that the angle bac
in Fig. 111 is approximately tt/2 . If Az represents the vertical precisin of a cylin
dricalcoordinate robot, then the total robot precisin is
Ap ~ [(Ar)2 + (R A<f))2 + (Az)2]1/2
(143)
For a sphericalcoordinate robot, both the horizontal precisin and the vertical
precisin are highest along the inside surface and lowest along the outside surface of
the work envelope. For SCARA robots, the horizontal precisin varies, but the ver
tical precisin is uniform. It is this feature, together with its open work envelope,
that makes the SCARA robot useful for light assembly work such as the vertical insertion of electrical components into circuit boards. With articulated robots, both
the horizontal precisin and the vertical precisin vary over the work envelope.
An alternative way to specify the precisin of a robot is to specify the precisin
of the individual joints. Clearly this is easier to analyze, although it is perhaps less
useful. The precisin or spatial resolution of an individual joint is affected by the
drive system, the position sensors, and the power transmission mechanism, including
gears, sprockets, and cables. For example, suppose the reference position signal is
generated internally in a control Computer and then sent to an external analog feedback position control system through an nbit digitaltoanalog converter (DAC). If
the range of load shaft positions attainable goes from </>min through (w , then the
load shaft precisin Adac in this case is given by:
a
W
'ta
k
x
Ymin
Robotic Manipulation
,,
& .\
(144)
Chap. 1
Figure 112
gray code.
More generally, if d emitterdetector pairs are used, and if there are k slots
around the circumference of the disk, then the precisin with which the shaft posi
tion can be measured is 360/{kl*) degrees per count. The encoded disk is usually
mounted on the highspeed shaft of the motor, rather than the load shaft. If the gear
reduction ratio from the highspeed shaft to the load shaft is m : 1, then the overall
incremental load shaft precisin A<frnc is
A<mc = , d degrees per count
(145)
Sec. 14
Robot Specifications
_
precisin
Error > E 
(146)
491482
15
Clearly, if the precisin of the robot dictates that only a discrete grid of points
can be visited within the work envelope, then the accuracy with which an arbitrary
point can be visited is, at best, half of the grid size, as shown in Fig. 110. In gen
eral, the accuracy will not be that good. For example, the robot may gradually drift
out of calibration with prolonged operation. To maintain reasonable accuracy, robots
are periodically reset or zeroed to a standard hard home position using limit switch
sensors.
Although robotic manipulators are not nearly as accurate as their more expensive and more specialized hard automation counterparts, they do provide increased
flexibility. It is one of the great challenges of robotics to make use of this increased
flexibility to compnsate for less than perfect accuracy. In particular, clever algorithms and control strategies are needed that make use of external sensors to com
pnsate for uncertainties in the environment and the uncertainties in the exact posi
tion of the robot itself.
146 Operating Environment
The operating environments within which robots ftmction depend on the nature of
the tasks performed. Often robots are used to perform jobs in harsh, dangerous, or
unhealthy environments. Examples include the transport of radioactive materials,
spray painting, welding, and the loading and unloading of furnaces. For these appli
cations the robot must be specifically designed to oprate in extreme temperatures
and contaminated air. In the case of paint spraying, a robotic arm might be clothed
in a shroud in order to minimize the contamination of its joints by the airborne paint
particles.
At the other extreme of operating environments are clean room robots. Clean
room robots are used in the semiconductor manufacturing industry for such tasks as
the handiing of Silicon wafers and photomasks. Specialized equipment has to be
loaded and unloaded at the various clean room processing stations. Here the robot is
operating in an environment in which parameters such as temperature, humidity, and
airflow are carefully controlled. In this case it is the robot itself that must be de
signed to be very clean so as to minimize the contamination of work pieces by submicron particles. Some clean room robots are evacuated internally with suction in
order to scavange particles generated by ffiction surfaces. Others use special nonshedding materials and employ magnetic washers to hold ferromagnetic lubricant in
place.
147 An Example: Rhino XR3
In recent years a number of educational tabletop robots have appeared in the marketplace. Many educational robots employ openloop position control using DC
stepper motors. Some tabletop educational robots are also available which use
closedloop position control with DC servomotors and incremental encoders. An ex
ample of the latter type of educational robot is the Rhino XR3 robot, pictured in
Fig. 113.
16
Robotic Manipulation
Chap. 1
In term s o f broad classification, the Rhino XR3 robotic manipulator is a fiveaxis electricdrive articulatedcoordinate robot with pointtopoint motion control
(H endrickson and Sandhu, 1986). The specifications of the Rhino XR3 are summarized in Table 110. The loadcarrying capacity, mximum tooltip speed, and re
peatability are rough estimates based on use of this robot in an instructional laboratory. The reach and stroke specifications are measured with respect to the tool tip
assum ing that standard fingers are used for the tool.
A nother usefiil specification for the Rhino robot is the load shaft precisin for
each o f the five axes. Each axis is driven by a 12V DC servomotor that has an inTABLE 110
Sec. 14
Characteristic
Valu
Number of axes
Loadcarrying capacity
Mximum tooltip speed
Horizontal reach and stroke
Vertical reach and stroke
Tool pitch range
Tool roll range
Repeatability
5
0.5
650.0
628.6
889.0
270.0
360.0
2.5
Robot Specifications
Units
kg
mm/sec
mm
mm
deg
deg
mm
17
In terms of broad classification, the Rhino XR3 robotic manipulator is a fiveaxis electricdrive articulatedcoordinate robot with pointtopoint motion control
(Hendrickson and Sandhu, 1986). The specifications of the Rhino XR3 are summarized in Table 110. The loadcarrying capacity, mximum tooltip speed, and re
peatability are rough estimates based on use of this robot in an instructional laboratory. The reach and stroke specifications are measured with respect to the tool tip
assuming that standard fingers are used for the tool.
A nother useful specification for the Rhino robot is the load shaft precisin for
each o f the five axes. Each axis is driven by a 12V DC servomotor that has an inTABLE 110
Characteristic
Number of axes
Loadcarrying capacity
Mximum tooltip speed
Horizontal reach and stroke
Vertical reach and stroke
Tool pitch range
Tool roll range
Repeatability
Sec. 14
Robot Specifications
Valu
5
0.5
650.0
628.6
889.0
270.0
360.0
2 .5
Units
kg
mm/sec
mm
mm
deg
deg
mm
17
cremental encoder attached to the highspeed shaft. The encoders resolve the highspeed position to 60 degrees. Since each motor has a builtin gear head with a tums
ratio of 66.6 : 1, this results in a precisin for each load shaft of 0.901 degrees per
count. Finally, each joint has its own set of sprockets and chains which then deter
mine the joint angle precisin. The resulting precisin in degrees per count for each
joint and for the tool is summarized in Table 111.
TABLE 111 LOAD SHAFT PRECISION
OF THE RHINO XR3 ROBOT
Motor
F
E
D
C
B
A
Joint
Precisin
Base
Shoulder
Elbow
Tool pitch
Tool roll
Tool closure
0.2269
0.1135
0.1135
0.1135
0.3125
1.2500
The Rhino XR3 robot is one of several robots that are used as vehicles for illustrating theoretical concepts covered in the remainder of this text. Direct and inverse kinematics, workenvelope calculations, trajectory planning, and control are
examined. A series of laboratory projects designed around a robotic work cell which
features a Rhino XR3 robot and a Micron Eye camera are included in a seprate
Laboratory Manual that accompanies this text (Schilling and White, 1990, in press).
These laboratory programming projects are designed to play an integral part in any
course using this text and are strongly recommended. If a Rhino XR3 or other edu
cational robot is not available, a threedimensional graphics robot simulator program
called SIMULATR that is supplied with the Laboratory Manual can be used as a
substitute. The use of the graphics simulator for development of robot control programs is highly recommended even when a physical robot is available. This way
robot control programs can be conveniently developed and debugged offline with
the final versin of the program tested on an actual robot.
15 NOTATION
Robotics is a broad interdisciplinary field that encompasses a variety of traditional
topics from electrical engineering, mechanical engineering, and Computer science.
This breadth makes the field quite exciting, but at the same time it poses a challenge
when it comes to developing a clear and consistent notation. Since notation and terminology play an important role in any detailed treatment of robotics, we pause at
this point to discuss and summarize the notational conventions that will be used in
the remainder of this text.
Scalars, vectors, matrices, and sets are used to simplify the presentation and
make it more concise. To distinguish vectors and matrices fforn scalars, boldface
print is traditionally employed. Although this approach is effective for printed mate
rial, the use of boldface characters is, at best, inconvenient when instructors are
18
Robotic Manipulation
Chap1
w riting on a blackboard or when students are taking notes in a class notebook. One
com m on alternative for these media is the underlining convention whereby vectors
are underlined once and matrices twice. However, repeated underlining becomes
very cumbersome when vectors and matrices are used extensively.
The general convention employed here is to instead regard all mathematical
quantities as either column vectors, matrices, or sets. Column vectors are denoted
w ith lowercase letters, both English and Greek, while matrices and sets are denoted
w ith uppercase letters. Single subscripts denote scalar components of column vec
tors, whereas double subscripts denote scalar components of matrices. In the case of
m atrices, the first subscript is the index of the row, while the second is the index of
the column. These general notational conventions are summarized in Table 112.
TABLE 112
NOTATIONAL CONVENTIONS
Examples
Entity
Notation
Scalars
Subscripted
a ,, o2, a i. 2, A n , A 12
Column vectors
Lowercase
a , b , c, a , /3, y , x \ x 2
Matrices, sets
Uppercase
a,
b,
c , d , r, l, , Y
Note that scalars can be regarded as important special cases of column vectors,
that is, column vectors with only one component. Similarly, vectors are themselves
special cases of matrices, that is, matrices with only one row or only one column.
Vectors. A column vector is represented by arranging its components in an
n x 1 array and enclosing them in square brackets as follows:
X\
x =
X2
(151)
*3
The superscript T is reserved to denote the transpose of a vector or, more generally, of a matrix. The transpose is obtained by interchanging the rows with the
columns. Thus the transpose of an n x 1 column vector is a 1 x n row vector, and
conversely. The following is therefore a spacesaving way of writing a column vec
tor as a transposed row vector:
x = [x\, x2, x 3]r
(152)
Notation
19
{ jc
E U: P ( jc) } .
(154)
Here E is the set membership symbol, and P(x) is a property of x. For example,
P (x) might represent one or more equality or inequality constraints involving x or its
components.
Matrices. Just as a vector can be represented as a onedimensional array of
scalar components, as in Eq. (151), a matrix can be represented by a twodimensional array of scalar components as follows:
(156)
(157)
(158)
The nuil space of A is the subspace of R consisting of the set of all vectors
x E R which are mapped into the zero vector in Rmby the transformation A. Thus
the nuil space is the inverse image of the zero vector in Rmunder the transformation
A. Similarly, the range space of A is the subspace of Rmconsisting of all vectors y
such that y = Ax for at least one x E R". Thus the range space R(A)is the image of
R" under the transformation A. A pictorial representation of the nuil and range
spaces of a matrix A is shown in Fig. 114.
The dimensin of N(A) is called the nullity of A, and the dimensin of R(A) is
called the rank of A. One of the fundamental results of linear algebra is that the di
mensin of the nuil space plus the dimensin of the range space equals the dimensin
of the space over which the transformation A is defined. Therefore:
nullity (A) + rank (A) = n
20
Robotic Manipulation
(159)
Chap. 1
Rn
R m
Figure 114
0 < k < n
(1510)
It is often necessary to transform or map the coordinates of a point with respect to one frame into its coordinates with respect to another frame. The matrix T
is reserved to denote a general coordinate transformation matrix. Typically, T will
have a superscript index or identifier which specifies the source coordinate frame and
a subscript index or identifier which specifies the destination or reference coordinate
frame. For example, T K denotes the coordinate transformation matrix which transforms robot tool coordinates into robot base coordinates. More generally, Tj transforms frame L k coordinates into frame Lj coordinates.
Some authors of robotics texts use either presuperscripts or presubscripts, or
both, such as *7} and KT, for coordinate transformation matrices (Paul, 1981;
Craig, 1986). We will refrain from using any presuperscripts or presubscripts, in an
effort to simplify the notation somewhat. The price we pay is that in some cases
there may be ambiguity as to whether or not a superscript should be interpreted as
taking a matrix to a power. In these cases we again use parentheses to remove the
ambiguity. Thus (Ti)2 is the square of the matrix Ti, while T\ is the matrix which
transforms frame L2 coordinates into frame L\ coordinates.
Sec. 15
Notation
21
c*
s*
Q,
Siy
C kj
Sk j
Meaning
COS (f>
sin <>
1 eos 4>
eos dk
sin dk
eos {6k + dj)
sin {dk + dj)
eos {dk  dj)
sin {dk  dj)
Here 0* denotes the joint angle for the kth joint of a robotic arm.Clearly the
notation can be extended in the obvious way to include sums or differences of three
or more joint angles. The joint angle 0* is often replaced by the variable qk, which
denotes the joint variable for the kth joint. That is, qk and 0* can be used interchangeably in Table 113.
16 PROBLEMS
11. What is the essential feature that distinguishes soft automation from hard automation?
12. Which types of drive technologies would be most suitable for the following applica
tions:
(a) Unload 1000kg items from a die casting machine
(b) Install integrated circuit chips on a printed circuit board
13. What is the minimum number of axes required for a robot to insert and tighten four
nuts (from above) on four vertical bolts as shown in Fig. 115? The nuts are available
from a vertical springactivated parts feeder. Explain why each axis is needed.
14. Suppose a cylindricalcoordinate robot has a vertical reach of 480 mm and a vertical
stroke of 300 mm. How far off the floor do parts have to be raised in order to be reachable by the robot?
15. The base joint of a cylindricalcoordinate robot is driven by a 12bit digitaltoanalog
22
Robotic Manipulation
Chap. 1
T
R obot
Bolts
N u t feeder
16.
17.
18.
19.
Figure 115
converter (DAC) and has a swing range of 360 degrees. The radial axis is driven by an
8bit DAC and has a horizontal reach of 300 mm and a horizontal stroke of 200 mm.
Finally, the vertical axis is driven by a 10bit DAC and has a vertical reach of 480 mm
and a vertical stroke of 360 mm.
(a) What is the volume of the work envelope?
(b ) What is the vertical precisin?
(c) What is the radial precisin?
(d ) What is the angular precisin about the base?
(e) What is the horizontal precisin?
( f ) What is the total precisin?
For a sphericalcoordinate robot, where within the work envelope is the tooltip preci
sin the highest?
For what type of robot is the precisin uniform throughout the work envelope? For
which robots is the vertical precisin uniform?
An incremental shaft encoder with 2 emitterdetector pairs and 12 slots around the
circumference is used to monitor the angular position of a highspeed
motor shaft.The
precisin of the load shaft is measured and found to be 0.05 degrees per count. What is
the gear ratio between the highspeed shaft and the loadshaft?
Consider the robotic tool shown in Fig. 116. Sketch the tool position after each intermediate position of the following YPR operation: yaw 7t / 2 , pitch 7t / 2 , roll 7 r / 2 .
rotations are taken about the axes of the mobile frame M rather than the fixed frame F,
then the final tool position is the same. Are the intermedate tool positions the same?
Sec. 16
Problem s
23
REFERENCES
I. (1950). /, Robot, Fawcett Publications: Greenwich, Conn.
B oyes , W. E. (1 9 8 5 ). LowCost Jigs, Fixtures, and Gages for Limited Production, SME Pub
lications: Dearborn, Mich.
B r o d y , H . (1987). U .S . ro b o t m a k e rs try to b o u n c e b a c k , High Technology Business, Octo b e r , pp. 1824.
C r a ig , J. J. (1986). Introduction to Robotics: Mechanics & Control, AddisonWesley: Reading, Mass.
D o r f , R. C. (1983). Robotics and Automated Manufacturing, Reston: Reston, Va.
Fu, K. S., R. C. G o n z l e z , a n d C . S. G. L ee (1987). Robotics: Control, Sensing, Vision, and
Intelligence, McGrawHill: New York.
G r o o v e r , M. P., M . W eiss, R . N. N a g e l, a n d N. G. O drey (1986). Industrial Robotics, Mc
GrawHill: New York.
H e n d r ic k s o n , T., a n d H. S a n d h u (1986). XR3 Robot Arm, Mark III 8 Axis Controller:
Owners Manual, Versin 3.00, Rhino Robots, Inc.: Champaign, 111.
P a u l , R. (1981). Robot Manipulators: Mathematics, Programming, and Control, MIT Press:
Cambridge, Mass.
P ie p e r , D. L. (1968). The kinematics of manipulators under Computer control, AIM 72,
Stanford AI Laboratory, Stanford Univ.
R obot I nstitu te o f A m er ic a (1982). Worldwide Robotics Survey and Directory, Dearborn,
Mich.
Roth, B. (19831984). A table of contemporary manipulator devices, Robotics Age, July
1983, pp. 3839; September 1983, pp. 3031, January 1984, pp. 3435.
S c m l l in g , R. J., a n d R. W h it e (1990, in press). Robotic Manipulation: A Laboratory Man
ual, Prentice Hall, Englewood Cliffs, N.J.
W o l o v ic h , W. A. (1987). Robotics: Basic Analysis and Design, Holt, Rinehart & Winston:
New York.
A s im o v ,
24
Robotic Manipulation
Chap1
2
Direct Kinematics:
The Arm Equation
A robotic manipulator can be modeled as a chain of rigid bodies called links. The
links are interconnected to one another by joints, as shown in Fig. 21. One end of
the chain of links is fixed to a base, while the other end is free to move. The mobile
end has a flange, or face pate, with a tool, or endeffector, attached to it. There are
typically two types of joints which interconnect the links: revolute joints, which ex
hibit rotational motion about an axis, and prismatic joints, which exhibit sliding mo
tion along an axis.
The objective is to control both the position and the orientation of the tool in
threedimensional space. The tool, or endeffector, can then be programmed to follow a planned trajectory so as to maniplate objects in the workspace. In order to
program the tool motion, we must first formlate the relationship between the joint
variables and the position and the orientation of the tool. This is called the direct
kinematics problem, which we define formally as follows:
Base
25
The vectors pictured in Fig. 22 are special in the sense that any vector in the
space R 3 can be easily represented in terms of these vectors. To see this, it is useful
to first introduce the notion of an inner product, or dot product, of two vectors.
Definition 211: Dot Product.
is denoted x y and is defined:
x*y = 2 xkyk
k= 1
The symbol 4 should be read equals by definition. The dot product is also
called the Euclidean inner product in R". The matrix transpose operation can be used
to express the dot product more compactly as
26
Chap 2
x y x Ty.
( 2  1 1)
Here x T denotes the row vector obtained by taking the transpose of the column vec
tor x. The dot product in R" has all the properties that are characteristic of inner
products in general. In particular, the following properties are fundamental:
Proposition 211: Dot P roduct. Let { jc, y, z} be vectors R" and let {a,
/?} be real scalars. Then the dot product has the following properties:
1.
jc
> 0
2.
jc
jc
<=>
jc
3. jc y ~ y jc
4.(ax + Py) z = a
( jc
z) + /3 (y z)
These properties follow directly from Def. 21 1. Note that the first two properties
indcate that the dot product of a vector with itself is always nonnegative and is zero
only for the zero vector. The third property specifies that the dot product is a commutative function of its two arguments, that is, interchanging the order of jc and y
does not alter the result. Finally, the last and perhaps most important property is that
the dot product is a linear function of its first argument jc.Thus the dot product of
the sum is the sum of the dot products. In view of the commutative property, the dot
product is also a linear function of its second argument y.
The dot product of two vectors is a measure of the orientation between the two
vectors. To see this, it is useful to first examine the concept of orthogonality.
Definition 212: O rthogonality.
if and only if jc y = 0 .
Two vectors
jc
For the Euclidean inner product or dot product in Def. 21 1, orthogonal vectors can
be interpreted geometrically in threedimensional space as perpendicular vectors.
Thus the three vectors shown in Fig. 22, which correspond to the three columns of
the identity matrix / , are mutually orthogonal. We therefore refer to them as an or
thogonal set. Not only is the set of vectors in Fig. 22 an orthogonal set, it is also a
complete set. A complete orthogonal set is a set of vectors with the property that the
only vector orthogonal to every member of the set is the zero vector.
. . .
for
1 < < /i
{ jc 1,
jc2 ,
y = 0
The number of vectors it takes to form a complete orthogonal set for a vector
space is called the dimensin of the space. Thus R 3 is a threedimensional space and,
more generally, R is an dimensional space. The vectors pictured in Fig. 22 are
also special in one final sense. The length of each vector is unity, and so we refer to
these vectors as unit vectors. The length or norm of an arbitrary vector in R" can be
defined in terms of the dot product as follows:
Sec. 21
27
Prof
and let 6 I
\ 1/2
jc = ( x  x ) m =
Thus the norm is an n dimensional generalization of the absolute valu function. A
set of orthogonal unit vectors is referred to as an orthonormal set. The vectors
shown in Fig. 22 are an example of a complete orthonormal set, or an orthonormal
coordinate fram e. It is a righthanded coordinate frame, because when the fingers of
the right hand are curled from the first vector into the second vector, the thumb
points in the direction of the third vector. One important result that will be useful
when we examine rotating coordinate frames is the following geometric relationship
between the dot product and the norm:
Thus, whe
vectors, ti
between t
R 3 and thi
Dot
of the kin
kinematic
l?2
U3_
_U 1V2 
U2V]_
Here the determinant notation is used in a formal sense to indicate how to evalate
the cross product. In this case the unit vectors {z\ z2, i 3} are treated as if they were
scalars for the purpose of computing the determinant. For the righthanded or
thonormal coordinate frame shown in Fig. 22, i 1 = i 2 x z3, i 2 = i 3 x i 1, and
i 3 = z1 x i 2. Recall that the valu of the dot product u v depends upon the angle
between the two vectors u and u. Similarly, the length of the crossproduct vector
u x t depends on the angle between the two vectors u and o, as can be seen from
the following result:
28
Chap. 2
COORDINA
The inne
arbitrary
bers of a
trary vec
thonorm
(weighte
coordina
De
spect to
Th
na te fra
vectors
frame a
mental
Sec. 25
Figure 23
= 2 \p\uk
ife*i
Coordinate Frames
29
lp]i * P * k
Proof. From Def. 221, it is sufficient to show that c is the zero vector where
c is the difference or error:
C = p
 i
** 1
p X k) x k
Using Def. 212, Def. 214, and the properties of the dot product summarized in
Prop. 211, we have for each 1 ^ j ss n:
c x J = p x J
Figure 24
spect to the
( p  x k)x
k= l
I
= p Xj  2
k= 1
t
F
t
r
i
l
( p mXk)(xk X1)
p  x J  2 (p Xk)hj
k=l
p XJ p ' XJ
= 0
Thus c is orthogonal to every member of i g Because X is complete, it follows from
Def. 213 that c 0 and thus that Prop. 221 is valid.
We see that the &th coordinate of p with respect to an orthonormal coordinate
frame X is simply the dot product of p with the kth member of the set X. The geometric interpretation of the relationship between a vector and its coordinates is
shown in Fig. 24, where, for simplicity, the twodimensional space R 2 is used.
Note how p is decomposed into the sum of its orthogonal projections onto the onedimensional subspaces (lines) generated by jc1 and jc2. In view of the simple relation
ship between a vector and its orthonormal coordinates, all coordinate frames used in
the remainder of the text are assumed to be righthanded orthonormal frames unless
otherwise noted.
The solution to the direct kinematics problem in robotics requires that we rep
resent the position and orientation of the mobile tool with respect to a coordinate
frame attached to the fixed base. This involves a sequence of coordinate transforma^ tions from tool to wrist, wrist to elbow, elbow to shoulder, and so on. Each of these
coordinate transformations can be represented by a matrix. To see this, it is useful to
first examine a simple special case, the singleaxis robot displayed in Fig. 25.
Here a single revolute joint connects a fixed link (the base) to a mobile link (the
body). The problem is to represent the position of a point p on the body with respect
to a coordinate frame attached to the base. The coordinates of p in this case will
vary as the joint angle 6 changes. Let F = { / \ / 2, / 3} and M = {m \ m 2, m 3} be or
thonormal coordinate frames attached to the fixed and the mobile links, respec30
Chap. 2
1
r
c
t
f
]
(
Mobile link
Revolute joint
Fixed link
Figure 25
Singleaxis robot.
tively. Note that m 2 is not shown in Fig. 25, since it is obscured by the body in the
position shown, which corresponds to 6 tt/2. Since p is a point attached to the
body, the coordinates of p with respect to the mobile coordinate frame M will remain fixed. However, the coordinates of p with respect to the fixed coordinate frame
F will vary as the body is rotated. From Prop. 221, the two sets of coordinates of
p are given by:
[p]M  P *w 1, p m 2, p m 3]T
(221)
[ pY = [ p  f \ P ' f \ p  T
(222)
The problem then is to find the coordinates of p with respect to F, given the coordi
nates of p with respect to M. This is a coordinate transformation problem. The coor
dinate transformation problem has a particularly simple solution whenthe destination or reference coordinate frame F is orthonormal, as can beseen from the
following result:
P roposition 222: C oordinate Transform ations. Let F = { / \ / 2, . . . ,
/" } and M = {m \ m 2, . . . , m n} be coordinate frames for R with F orthonormal.
Next, let A be the n x n matrix defined by Akj = /* mj for 1 ^ k, j ^ n. Then for
each point p in R :
lp]F = A[p]M
Proof. Since F is an orthonormal coordinate frame, it follows from Prop.
211, Prop. 221, and Def. 221 that for 1 < k < n:
lP)F
> ~ P k f
=
/*
n
V
. fk
= Z l( n
p i] f (" * '*
/ )
Sec. 22
Coordnate Frames
31
m 2
f 2 m '
P  m 2
f 2
f  m '
P  m 2
0
1
0
m
m
m
0 . 6'
0.5
1.4
1 0* 0 . 6 '
0 0 0.5
0 1 1.4
Chap. 2
Proof. By multiplying both sides of the equation in Prop. 222 on the Ieft by
A ~ \ we see that A~x maps F coordinates into M coordinates. It remains to verify that
the inverse is just the transpose. Let 1 ^ k, j < n. Since both F and M are or
thonormal, we can use Prop. 222 and the commutative property of the dot product
as follows:
( A~% = m k f
lili
== Ajk
= (AT)kj
Since the above equation holds identically for all 1 ^ k %j j /i, Prop. 223 then
follows.
23 ROTATIONS
In order to specify the position and orientation of the mobile tool in terms of a coor
dinate frame attached to the fixed base, coordinate transformations involving both
rotations and translations are required. We begin by investigating the representation
of rotations.
231 Fundamental Rotations
If the mobile coordinate frame M is obtained from the fixed coordinate frame F by
rotating M about one of the unit vectors of F, then the resulting coordinate transfor
mation matrix is called a fundamental rotation matrix. In the space R3 there are
three possibilities, as shown in Fig. 26.
Rotations
33
Figure 27
angle <J>.
f\ m
m
m
m
/?! (4>)
Rotation of M a b o u t/1 by
Pm 2 p m 3
Pm 2 Pm 3
Pm 2 P m 3
(231)
Since we are rotating about the/ ' axis, it is clear from Fig. 27 that/ 1 = m'. Substituting this into Eq. (231) and recalling that { p , p , f 3} and { m [, m 2, m } are or
thonormal sets, it follows that the fundamental rotation matrix
(<) simplifies to:
R<t>) =
1
0
0
2
0
p  m 2f m 3
0 P  m 2 f 3 m 3
Example 2
Refe
the f
p is i
the c
Solu
(232)
Next note from Fig. 27 that the vectors { / 2, / 3, m 2, m 3} all lie in a plae or
thogonal to / 1 = m l. Recalling Prop. 212, the dot product of two vectors is proportional to the cosine of the angle between the vectors. When the vectors are nor
mal (unit length), the dot product is in fact equal to the cosine of the angle between
the vectors. From Fig. 27 it is evident that the angle from / 2 to m 2 is <f>, as is the
angle from / 3 to m3. Consequently the diagonal elements in the 2 x 2 submatrix in
Eq. (232) are equal to eos </>.
In order to represent the offdiagonal elements in terms of </>, note from Fig.
27 that the angle from f 2 to m3 is t t / 2 + </>. Similarly, the angle from / 3 to m 2 is
tt/2 + </>. Using the trigonometric identity for the cosine of the sum (Appendix 1)
and Prop. 212, the general form for the first fundamental rotation matrix then re
duces to:
1
0
0
0 eos (f> sin 4>
0 sin (> eos 4>
The
rotation n
row and t
the diago
the angle
for the ti
givei
coor<
Solui
the i
Prop
(233)
A similar analysis can be used to derive expressions for the second and the
thirdfundamental rotation matrices. In particular, if R2(<) and R3(</>) represent rota
tions by <f) about the second and third unit vectors of the fixed coordinate frame f f
then:
R2<f>) i
34
eos 4>
0
0 sin (f>
1
0
 s in
0 eos
(234)
Note
Chap. 2
Sec. 23
R4>) I
(235)
= i (f) [p]M
"1 0
= 0 0.5
0 0.866
= [2
2.598
0
 0.866
0.5
i.5 r
Next, suppose q is a point whose coordinates in the fixed coordinate ffame F are
given by [qY = [3, 4, 0]r . What are the coordinates of q with respect to the mobile
coordinate ffame M l
0
0
0.5
0.866
= [3 2
Solution Since the two coordinate frames are orthonormal and have the same origin,
the inverse of the coordinate transformation matrix is just the transpose. Applying
Prop. 222 and Prop. 223:
0.866
0.5
"3
4
_ 0_
3 .4 6 4 f
Note that both results are consistent with the picture shown in Fig. 27.
Sec. 23
Rotations
35
Figure 28
tool.
36
Chap. 2
Thus composite rotation matrices are built up in steps starting with the identity
matrix which corresponds to no rotation at all. Rotations of frame M about the unit
vectors of frame F are represented by premultiplication (multiplication on the left)
by the appropriate fundamental rotation matrix. Similarly, rotations of frame M
about one of its own unit vectors are represented by postmultiplication (multiplica
tion on the right) by the appropriate fundamental rotation matrix.
Since a convention has been adopted for the order of the yaw, pitch, and roll
operations, a general expression for the composite yawpitchroll transformation ma
trix can be obtained. Suppose that the yawpitchroll angles are represented by a
vector 0 in R 3. For notational convenience, let S* = sin 0* and C* = eos 0*. We then
have the following result, which summarizes the composite yawpitchroll tool trans
formation:
Proposition 231: YawPitchRoll Transformation. Let YRP (0) repre
sent the composite rotation matrix obtained by rotating a mobile coordinate frame
M = {m \ m 2, m 3} first about unit vector / ' with a yaw of 0i, then about the unit
vector f 2 with a pitch of 02, and finally about unit vector/ 3 with a roll of 03. The
resulting composite yawpitchroll matrix YPR (0) maps M coordinates into F coor
dinates and is given by:
C2C3 S1S2C3 C ,S 3 C1S2C3 + Si S3
C2S3 S1S2S3 4* C1C3 C1S2S3 ~ S1C3
S2
Si C2
Ci C2
Y P R (0) =
Proof. This is verified by simply applying Algorithm 231 and using the ex
pressions for the three fundamental rotation matrices. Using the notational short
hand C* = eos dk and S* = sin 0* and Algorithm 231, we have:
Y P R (0) = R 3(03)/?2(02)Rl(0l)
C3  s 3 o
c3 0
s3
o
c3  s 3 0
c3 0
s3
o
c2
o
s 2
s2 "1 0
0 Ci
o
c2 0 Si
0"
S i
CiJ
c2 s ,s 2 C1S2
S i
o
c,
 s 2 S1C2 c,c2
S e a 23
Rotations
37
S o lu tion
r 7r
~ } r 2I
!^ 2
\ 2/
~o
0 r
1 0
0
1
[p]M [0,
0 , 0 .6 ]r.
Ip Y
=RIpY4
i
0  1 0
l
0_ _0.6_
= [0.6, 0, Of
Exercise 231: YawPitchRoll. Verify that Example 232 is correct by
sketching the tool position and the coordinate frames after each fundamental rotation
is performed.
Exercise 232: RollPitchYaw. An alternative way to define the yawpitchroll transformation is to perform the rotations about unit vectors of the mobile
frame M and in the reverse order: roll, pitch, yaw. This tends to be easier to visualize, particularly when the angles are arbitrary. Show that the resulting composite ro
tation matrix RPY (0), is the same as in Prop. 231. Is the route taken by the tool
the same?
Exercise 233: Fundam ental Rotations. Verify that the general yawpitchroll transformation YPR (0) includes the three fundamental rotations {R\, R2, Ri} as
special cases.
A general rotation of a mobile frame M with respect to a fixed frame F can always be decomposed into a sequence of three fundamental rotations as in Prop.
231. It is also possible to represent a general rotation as a single rotation by an an
gle <f>about an arbitrary unit vector u as shown in Fig. 29. This is called the equivalent angleaxis representation. For notational convenience, let C</> = eos
S(f> = sin <fi, and V< = 1 eos <f>. The equivalent angleaxis rotation can then be
represented as follows (Fu et al., 1987):
Proposition 232: Equivalent AngleAxis. Let F and M be two orthonor
mal coordinate frames in R3 with M initially coincident with F. Let u be a unit vec
tor, and suppose Af is rotated about u by an angle </>in the righthanded sense. Then
the equivalent angleaxis rotation matrix R(<), u) which maps M coordinates into F
coordinates is:
38
Chap. 2
Figure 29
tion.
/?(<, u) =
u\ V<j) + Q<)
U\U2 Wcf) u$S(f) U\U2 V0 + U2$<t>
U\U2V(j> + w3S<>
u \ V<> + C<j>
u2ui\<)  WiS$
W1W3V0 w2S</> M2W3V0 + wiS</>
W3V<> 4 C<f>
Proof. Let w = [1/1, w2, 0]r denote the orthogonal projection of u onto the/ / 2
plae as shown in Fig. 29. Then:
A = nwti
I (m + u\)m
Next, let a be the angle from f to m, and let p be the angle from to u as shown in
Fig. 29. Then, since w = 1, it follows that:
r
Ux
"a
c
Ul
~K
C/5 = A
sp
= w3
Rotations
39
This aligns unit vector u with the/ 1 axis. At this point we perform the desired rota
tion by an angle <f>about the unit vector u f x. Thus the composite rotation matrix
is:
R  Ri(4>)R2(P )R a )
Next we reverse the process to restore u to its original position. First we rotate
24 HO
<3
The equivalent angleaxis representation for a general rotation can also be inverted (Paul, 1982). For example, given a rotation matrix /?, suppose we want to
find the axis u and angle <f>. The sum of the diagonal terms of a square matrix is
called the trace of the matrix. If we compute the trace of the matrix R{<), u) in
Prop. 232, then after simplification using trigonometric identities and w = 1,
the result is trace [/?(<, u)] 1 + 2 eos $ . Thus the angle part of the equivalent
angleaxis representation is:
 rceos
trace (R) 1
(236)
Once the angle of rotation is identified over the range (0, 7r), the axis or unit
vector about which the rotation is performed can then be determined. In particular,
if we form differences between offdiagonal terms of /?(<, u) in Prop. 232, we
can isolate the components of u as follows:
(237)
40
Chap. 2
Note that u is well defined for 0 < <f> < 7r, but at <f> = 0 and at <j> = n the ex
pressions for the components of u in Eq. (237) reduce to 0/0. Thus Eq. (237) is
not well defined numerically for small vales of <f>or as (f>approaches tr. For a further discussion of these cases, see Paul (1982).
HOMOGENEOUS COORDINATES
Pur rotations are sufficient to characterize the orientation of a robotic tool. How
ever, to characterize the position of the tool relative to a coordinate frame attached
to the robot base, we must use another type of transformation, a translation. Translations are qualitatively different fforn rotations in one important respect. With rota
tions, the origin of the transformed coordinate frame is the same as the origin of the
original coordinate frame. This invariance of the origin is characteristic of linear
operations in general, and it is this feature that allows us to represent rotations in
threedimensional space with 3 x 3 matrices. However, the origin of a translated
coordinate frame is not the same as the origin of the original coordinate frame. Consequently, it is not possible to represent a translation in R3 with a 3 x 3 matrix.
2*41 Homogeneous Coordinate Frames
Instead we must move to a higherdimensional space, the fourdimensional space of
homogeneous coordinates. We define the homogeneous coordinates of a point as fol
lows:
Definition 241: Homogeneous Coordinates. Let q be a point in R3, and
let F be an orthonormal coordinate frame for R3. If cr is any nonzero scale factor,
then the homogeneous coordinates of q with respect to F are denoted [q]F and de
fined:
[q]F = [aqu
crq3, cr]r
Thus the homogeneous coordinates of a point q in R3 are represented by a vector [q]F in fourdimensional space R4. The fourth component cr is a nonzero scale
factor. To recover the original physical threedimensional vector from its fourdimensional homogeneous coordinates, we can use q = Ha[qY, where Ha is a 3 x 4
homogeneous coordinate conversin matrix, defined as follows:
=[/,()]
cr
(241)
Note that the homogeneous coordinates [qY are not unique, because any scale
factor cr ~t~ 0 will yield the same threedimensional physical vector q. In robotics,
we normally use a scale factor of cr = 1 for convenience. In this case, fourdimensional homogeneous coordinates are obtained from threedimensional physical
coordinates by simply augmenting the vector with a unit fourth component. Similarly, threedimensional physical coordinates are recovered from fourdimensional
homogeneous coordinates by merely dropping the unit fourth component.
Sec. 24
Homogeneous Coordinates
41
R
rf
p o s itio n
i
i
p e rs p e c tiv e
scale
R 4>)
Rot (<, k) =
o
i
1
t
(243)
Here Rk(<t>) is simply the fcth fundamental rotation matrix introduced in Sec. 23.
We refer to Rot ($, k) as the kth fundamental homogeneous rotation matrix. Com
posite homogeneous rotation matrices are then built up from the fundamental homo
geneous rotation matrices in a manner analogous to the method summarized in Al
gorithm 231. Note that the translation vector p in each case has been set to zero.
42
Chap. 2
41) i0
: o
m
>
Rot W *
i
. . j ____ _
0
l
0 i 1
0
0
7T
eos
sin
4
7r
0
0 0.707
0 0.707
0
J
0
7T
0
4
7r
eo s 0
4
0
1
sin
0
0.707
0
0
0.707 0
0
Henee the homogeneous coordinates of the point q in the fixed coordinate frame F fol
lowing the rotation are:
I q f = Rot ( j , l)[<?]M
1
0
0 0.707
0 0.707
0
0
0
0 Y
0.707 0
0
0.707 0 10
1
1
0
m
= [0
7.07
7.07
l ]5
Finally, the physical coordinates of the point q in the fixed coordinate frame F are
q = [0, 7 .0 7 , 7.07]r.
The pow er o f homogeneous coordinates lies in the fact that matrices can also
be used to represent translations. For example, let F and M be two initially coinci
dent orthonorm al coordinate frames, and suppose we want to transate the origin of
the mobile coordinate frame M by an amount p k along the fcth unit vector of F for
1 ^ k < 3. Then in term s o f homogeneous coordinates, this operation can be repre1
Sec. 24
H om ogeneous Coordinates
43
Tran (p) =
1
0
0
0
0
1
0
1
0
P\
P2
P>
(24*4)
Suppose the homogeneous coordinates of a point are [q]M = [0, 0, 10, l]r, as in Example 241. However, this time suppose we transate the mobile M coordinate ffame rel
ative to the fixed F coordinate frame by 5 units along the / ' axis and 3 units along
thep axis. Then the homogeneous transformation matrix which represents this operation is:
' 1 0
Tran (p)
1 0  3
It follows that the homogeneous coordinates of point q relative to the reference frame F
following the translation are given by:
q\ = Tran (p) [q]M
1
0
0
0
= [5
0
1
0
0
3
0
5
0  3
1
0
1
0
10
0
0
10
1
l]r
Finally, the physical coordinates of the point q in the fixed F coordinate ffame follow
ing the translation are q = [5, 3, 10]r.
Exercise 241: Inverse T ranslation. Verify that the inverse of the funda
mental homogeneous translation matrix Tran (p) always exists and is given by:
Tran1 (p) = Tran ( p)
Exercise 242: Inverse R otation. Verify that the inverse of the fundamen
tal homogeneous rotation matrix Rot (</>, k) always exists and is given by:
Rot1 ($, k) = Rot (< p, k) = Rotr (<f>, k)
44
1< k ^ 3
Chap. 2
In the general case, a homogeneous transformation matrix will represent both a rota
tion and a translation of the mobile frame with respect to the fixed ffame. A sequence of individual rotations and translations can be represented as a product of
fundamental homogeneous transformation matrices. However, because matrix mul
tiplicaron is not a commutative operation, the order in which the rotations and trans
lations are to be performed is important. Furthermore, the mobile coordinate ffame
can be rotated or translated about the unit vectors of the fixed frame or about its own
unit vectors. The effects of these different operations on the composite transforma
tion matrix are summarized in the following algorithm:
Algorithm 241: Composite Homogeneous Transformations
1. Initialize the transformation matrix to T = /, which corresponds to the or
thonormal coordinate frames F and M being coincident.
2. Represent rotations and translations using seprate homogeneous transforma
tion matrices.
3. Represent composite rotations as seprate fundamental homogeneous rotation
matrices.
4. If the mobile coordinate ffame M is to be rotated about or translated along a
unit vector of the fixed coordinate ffame F, premultiply the homogeneous
transformation matrix T by the appropriate fundamental homogeneous rotation
or translation matrix.
5. If the mobile coordinate ffame M is to be rotated about or translated along one
of its own unit vectors, postmultiply the homogeneous transformation matrix T
by the appropriate fundamental homogeneous rotation or translation matrix.
6. If there are more fundamental rotations or translations to be performed, go to
step 4; otherwise, stop. The resulting composite homogeneous transformation
matrix T maps mobile M coordinates into fixed F coordinates.
Thus composite homogeneous transformation matrices are built up in steps
starting with the identity matrix, which corresponds to coincident mobile and fixed
frames. Rotations and translations of ffame M along the unit vectors of ffame F are
represented by premultiplication (multiplication on the left) by the appropriate fun
damental homogeneous transformation matrix. Similarly, rotations and translations
of frame M along one of its own unit vectors are represented by postmultiplication
(multiplication on the right) by the appropriate fundamental homogeneous transfor
mation matrix.
Example 243: Composite Homogeneous Transformation
Let F = i / 1 /2, / 3} and M {m \ m2, m3} be two initially coincident fixed and mo
bile orthonormal coordinate ffames, respectively. Suppose we transate M along/ 2 by
3 units and then rotate M about/ 3 by 7r radians. Find [m']F after the composite trans
formation.
Sec. 24
Homogeneous Coordinates
45
Solution
1
0
0
0
0
0 0
1 0  3
0
0 1
1
0 0
3
[1
0 0 0
1 0 3
0 1 0
0 0 1
1
0
0
0
1
0
0
0
1
0
0
1
1
0
0
1
If
Thus, in terms of physical coordinates, [ml]F = [1, 3, 0]r. This is consistent with
the diagram of the two coordinate frames displayed in Fig. 210.
f3i k 1
1
/
/
/
/
Figure 210 Coordinate frames for Ex
ampie 243.
Exampie 244: Composite Homogeneous Transformation
Suppose we repeat the transformation of Example 243 but this time with the order of
the operations reversed. First we rotate M by v radians about/3, and then we transate
the rotated M by 3 units along/2. Find [ml]F following the composite transformation.
Solution
1 0
0
0
0
0
1 0
0 1
1
0
0
0
0 0
1 0
0 1
0 0
[1
3 0
1
0
0
0
0
3
0
1
0 0 0
1 0 0
0 1 0
0 0 1
0
0
1
1 0
1
0
0
1
l]r
Chap. 2
f2
m2
final position of the mobile ffame M in Fig. 211 differs from that in Fig. 210. Henee
the order in which the fundamental transformations are performed is important.
The geometric interpretation of the homogeneous transformation matrix T is
that it provides the position and the orientation of a mobile coordinate frame M with
respect to a fixed reference frame F. In the analysis of robotic manipulators, we will
sometimes also want to express the position and orientation of the fixed frame F in
terms of the mobile frame M. Consequently, we need to find the inverse of the ho
mogeneous transformation matrix T. Because T is no longer a pur rotation between
orthonormal frames, the inverse of T is not simply the transpose. However, when
the perspective vector is 17 = 0 and the scale factor is cr = 1, the inverse of T does
have a particularly simple form, as can be seen from the following useful result:
Proposition 241: Inverse Homogeneous Transformation. Let T be a ho
mogeneous coordinate transformation matrix with rotation R and translation p be
tween two orthonormal coordinate frames. If 17 = 0 and cr = 1, then the inverse of
lis :
RT
!  R Tp
i _
0
1
0
0
0
R TR
 R Tp
O
T'T =
1O
RT
Homogeneous Coordinates
47
In view of Prop. 2 4 1, we can just as easily transform coordinates from the
base to the tool tip as from the tool tip back to the base. There is no need to perform
computationally expensive and potentially inaccurate numerical inverses.
Example 245: Inverse Homogeneous Transformation
1 0 0
0 0 2
0 1 2
0 0 1
Find the homogeneous coordinate transformation which maps fixed F coordinates into
mobile M coordinates, and use it to find [ f 2]M.
f2
f1
Solution
t '
21F
[ f 2]
 1 0
o'
i
o'
0
2
= [1
2
lf
0
1
Thus, in terms of physical coordinates, [ f 2]M = [1, 0, 2]r . This is consistent with
Fig. 212.
48
Chap. 2
(245)
Figure 213
and screws.
Sec. 24
Homogeneous Coordinates
Translations, rotations,
49
could instead have been formulated the other way around as a rotation followed by a
translation, as can be seen ffom the following exercise. This is a consequence of the >
fact that the rotation and translation operations are being performed along the same
axis.
Exercise 243: Screw Transform ation. Show that the fundamental rota
tion and translation matrices associated with the unit vectors commute. That is,
screw
bitrar)
Solution
Screw ( :3, , 2 I
1 0
1 0
0 0
0 0
0
1 0
1 0
1 0
0 0
= [ 1 3 0
0 1 0
0
0
0
0
1
o
0
1
1
o '
0 0
0
[ m 3]M
0 10
 1 0
0 0
0
1
1
l]r
T hus, in terms o f physical coordinates, [m 3]F = [1, 3, 0 ]r. This is consistent with the
diagram o f the two coordinate frames displayed in Fig. 2 1 4 . The pitch o f the screw is
p = j 2 threads per unit length.
50
Every
The r<
joint /
251
[m 3]M
Recal 1
either
assign
equati
then t
physic
Chap. 2
joint
first
to m
joint
axis
the
para
on ti
is Vi
Sec
1< A< 3
Joint k
Link Coordinates
51
TABLE 21
KINEMATIC PARAMETERS
Arm
Parameter
Joint angle
Joint distance
Link length
Link twist angle
Symbol
Revolute
Joint (R)
Prismatic
Joint (P)
e
d
a
a
Variable
Fixed
Fixed
Fixed
Fixed
Variable
Fixed
Fixed
another about the axis of joint k. For a prismatic joint, the joint distance dk is vari
able and the joint angle 6k is fixed. In this case the two links transate relative to one
another along the axis of joint k.
Just as there is a joint connecting adjacent links, there is a link between successive joints. The relative position and orientation of the axes of two successive
joints can be specified by two link parameters, as shown in Fig. 216. Here link k
connects joint k to joint k + 1. The parameters associated with link k are defined
with respect to jc*, which is a common normal between the axes of joint k and joint
k + 1. The first link parameter, aky is called the link length. It is the translation
along ;c* needed to make axis z*1 intersect with axis z*. The second link parameter,
a*, is called the link twist angle. It is the rotation about jc* needed to make axis z*_I
parallel with axis z*.
Joint k + 1
Link k
I
Joint k
Unlike the joint parameters, the two link parameters are always constant and
are specified as part of the mechanical design. For industrial robots, the link twist
angle is usually a mltiple of tt/ 2 radians. Sometimes the axes of joint k and joint
k + 1 intersect, in which case the length of link k is zero. Links of length zero occur, for example, in robots with spherical wrists, where the last n 3 axes inter
sect. More generally, robots can be designed to have many of the constant kinematic
parameters equal to zero, in which case they are referred to as kinematically simple
manipulators.
For an axis robotic manipulator, the 4n kinematic parameters constitute
the minimal set needed to specify the kinematic configuration of the robot. For
each axis, three of the parameters are fixed and depend on the mechanical design,
while the fourth parameter is the joint variable, as indicated in Table 21. For a
rectangularcoordinate robot the first three joint variables are all joint distances,
whereas for an articulatedcoordinate robot the first three joint variables are all joint
52
Chap. 2
angles. Between these two extremes are cylindrical, spherical, and SCARA robots,
whose first three joint variables are a combination of joint angles and joint distances.
252 Normal, Sliding, and Approach Vectors
By convention, the joints and limes of a robotic arm are numbered outward starting
with the fixed base, which is link 0, and ending with the tool, which is link n. For
an naxis robot there are n + 1 links interconnected by n joints, with joint k connecting link : 1 to link k. In order to systematically assign coordinate frames to
the links of an axis robotic arm, special attention must be paid to the last link
the tool, or endeffector. The orientation of the tool can be expressed in rectangular
coordinates by a rotation matrix R = [ r 1, r 2, r 3], where the three columns of R cor
respond to the normal, sliding, and approach vectors, respectively, as shown in Fig.
217. The approach vector r 3 is aligned with the tool roll axis and points away from
the wrist. Consequently, the approach vector specifies the direction in which the tool
is pointing. The sliding vector r 2 is orthogonal to the approach vector and aligned
with the openclose axis of the tool. Finally, the normal vector r l is orthogonal to
the plae defined by the approach and sliding vectors and completes a righthanded
orthonormal coordinate frame.
Recall that the yaw, pitch, and roll motions are rotations about the normal,
sliding, and approach vectors, respectively. For the purpose of representing tool ori
entation, the origin of the {r1, r 2, r 3} frame can be taken either at the wrist, as in
Chap. 1, or at the tool tip, as in Fig. 217. For the remainder of this text we will
assume that the origin of the { r \ r 2, r 3} frame is placed at the tool tip, as in Fig.
217, because this simplifies the subsequent representation of tooltip position.
253 The DenavtHartenberg (DH) Representation
Denavit and Hartenberg (1955) proposed a systematic notation for assigning righthanded orthonormal coordinate frames, one to each link in an open kinematic chain
of links. Once these linkattached coordinate frames are assigned, transformations
between adjacent coordinate frames can then be represented by a single standard
4 x 4 homogeneous coordinate transformation matrix. To assign coordinate frames
to the links of the robotic manipulator, let Lk be the frame associated with link k.
That is:
Sec. 25
Link Coordinates
53
Q< k < n
(251)
Coordinate frame Lk will be attached to the distal end of link k for 0 < k < n. This F
puts the last coordinate frame, L, at the tool tip. The coordinate frames are assigned
to the links using the the following procedure:
H
Ib
Chap. 2 ;
m
the
Ti*
sigi
pie
reI
*ur<
1
i
Figure 219
Sec. 25
Link Coordinates
Elbow
Tool pitch
55
1
2
3
4
5
01
02
03
04
05
215.0 mm
177.8 mm
177.8 mm
0
0
129.5 mm
Home
tt/2
0
7r/2
0
0
0
re2
The vales listed in the last column of Table 22 for the joint variable 6 corre
spond to the soft home position pictured in the linkcoordinate diagram of Fig. 219.
When assigning linkcoordinates using the DH algorithm, it is always easier to
choose a soft home position that corresponds to the joint angles being mltiples of
radians even if the original picture of the robot is not so configured.
It is evident from inspection of Fig. 219 that the origins of the coordinate
frames associated with the tool orientation {L3, L4} coincide at the wrist. This is in
dicad ve of the fact that the Alpha II robot has a spherical wrist. However, it is not a
general spherical wrist, because there is no tool yaw motion. The mechanical design
that enables the Alpha II robot to achieve a pitchroll spherical wrist is shown in Fig.
7r/2
2  20 .
The wrist design employs a set of three bevel gears {A, B, C} in a universal
joint configuration as shown in Fig. 220. Gears A and B are driven by seprate
stepper motors and cables, while gear C is attached to the tool roll axis. If gears A
and B are driven in opposite directions at the same speed, the tool will exhibit a pur
roll motion. However, if gears A and B are driven in the same direction at the same
speed, the tool will exhibit a pur pitch motion with the tool tip moving into and
Pitch axis
Roll axis
P
Tool
Figure 220 Pitchroll spherical wrist
using bevel gears.
56
Chap. 2
2(
out o f the page. More complex motions involving simultaneous pitch and roll
movement can also be achieved. It is clear that the pitch and roll axes intersect at the
wrist.
Description
3*
4
From Fig. 215 we see that the effect of operation 1 is to make axis j c * ' 1 parallel to axis jc * . Following operation 2, axis jc * 1 is then aligned (collinear) with axis
jc * . Next, from Fig. 216 we observe that operation 3 ensures that the origins of
frames Lk~i and Lk coincide. Finally, the effect of operation 4 is to align axis z * '1
with axis z*. Thus the two coordinate frames Lk 1 and Lk are coincident at this point.
The four fundamental operations summarized in Table 23 each can be interpreted as
a rotation or a translation of L*i along one of its own unit vectors. It follows from
A lgorithm 251 that we can postmultiply I by the four fundamental homogeneous
transformation matrices to get the composite homogeneous transformation matrix
associated with Table 23. We see from Table 23 that operations 1 and 2 together
form a screw transformation along axis z * '1. In view of Exercise 243, operations 3
and 4 also form a screw transformation, in this case along axis j c * ' 1. Consequently,
the composite homogeneous transformation summarized in Table 23 can be exSec. 26
57
(261)
cek
sek
0
0
Ca S6k
QockC6 k
Sak
0
Particular cases of the transformation matrix of Prop. 261 include 7o, which
maps shoulder coordinates into base coordinates; 71, which maps elbow coordinates
into shoulder coordinates; Ti, which maps wrist or tool yaw coordinates into elbow
coordinates, and so forth. If instead we want to move from the base toward the tool,
we need to use the transformation 7*1, which is the inverse of 711.
Utilize Prop.
241 and Prop. 261 to show that the following transformation maps link k  1
coordinates into link k coordinates:
Exercise 261: Inverse LinkCoordinate Transform ation.
7T1=
S 6k
0
C$k
C a kS 6 k C ak C6 k Sak
SakS 6 k  S a kCOk Cak
0
0
0
~ak
 d kSak
dk Ccc/t
1
Recall that three of the kinematic parameters that appear in the coordinate
transformation matrix 711 are constant, while the remaining parameter is the joint
variable. The joint variable will sometimes be 6 k (for revolute joints) and sometimes
be dk (for prismatic joints). Rather than treat the two cases separately, we introduce
a joint type parameter, defined as follows:
A
joint k revolute
joint k prismatic
( 2  6  2)
Thus & is a binaryvalued function of the joint type. With the aid of & we can then
58
Chap. 2
qk =
+ (1  j<f*
(263)
It is clear that & has the effect of selecting either 0* or dk, as appropriate. In
general, the kth homogeneous linkcoordinate transformation matrix T1 is a func
tion of qk for 1 ^ k ^ n. For articulatedcoordinate robots, we have q = 0, but for
all other robots q is a mixture of joint angles and joint distances. To solve the direct
kinematics problem, we must determine the position and orientation of the tool rela
tive to a coordinate ffame attached to the base. The transformation from tool coordi
nates to base coordinates is obtained by a succession of coordinate transformations
starting at the tool tip and working backward, one frame at a time, to the base. In
particular, if TSSl represents a transformation from tooltip coordinates (link n) to
base coordinates (link 0), then:
TBUq) = T xo{qx)T]{q2) \ T U (q n) Tn0(q)
(264)
Note from the notation in Eq. (264) that when a sequence of coordinate
transformations is performed, the coordinate transformation algebra is such that di
agonal ndices cancel. Consequently, for a composite transformation, the super
script on the rightmost factor is the identifier of the source coordinate frame, while
the subscript on the leftmost factor is the identifier of the destination, or reference,
coordinate ffame.
In order to compute the arm matrix, it is often helpful to partition the problem
at the wrist. This effectively decomposes the problem into two smaller subproblems,
one subproblem associated with the major axes used to position the tool, and the
other subproblem associated with the minor axes used to orient the tool. Here we
break the expression for the arm matrix into two factors at the wrist, or third axis, as
follows:
TS(q) = Th(qx)T 2M 2)T\{q2) = Tl{q)
(265)
(266)
For a general sixaxis robot, Ttiqe) maps tool tip coordinates into roll coordi
nates, T(q5) maps roll coordinates into pitch coordinates, and
maps pitch co
ordinates into yaw coordinates or wrist coordinates. Thus the composite transforma
tion Tz{q) maps tool tip coordinates into wrist coordinates. Similarly, T\(q3) maps
wrist coordinates into elbow coordinates, T 2\(q2) maps elbow coordinates into shoul
der coordinates, and T l0(q\) maps shoulder coordinates into base coordinates. Thus
the composite transformation To(q) maps wrist coordinates into base coordinates.
Once the two wristpartitioned factors are obtained, the general arm matrix is then
computed by forming their product, as follows:
(267)
Again, notice that the diagonal identifiers in the composite transformation can
cel. If one interchanges the subscript and superscript of a given transformation, this
changes the direction of the transformation. Consequently, interchanging the sub
script and the superscript is equivalent to inverting the transformation matrix, as in
the following example:
Sec. 26
59
TSSf(q) [ r ( q ) ]  '
1268)
R(q)
tool
base
piq)
(269)
(<?) =
0
0 !
For each valu of the joint vector q , the arm matrix T'SSU^) can be evaluated.
The 3 x 3 upper left submatrix R(q) specifies the orientation of the tool, while the
3 x 1 upper right submatrix p(q) specifies the position of the tool tip. The three
columns of R indicate the directions of the three unit vectors of the tool frame with
respect to the base frame. Similarly, p specifies the coordinates of the tool tip with
respect to the base frame. The solution of the direct kinematics problem in Eq.
(269) is shown schematically in Fig. 221.
Chap. 2
c, 0
= Si 0 c,
0 1 0
0 0 0
c,c2 ~CiS2
s,s2
s2 ~c2
0 0
a S
C,C23
S .C
23
c1
0
0
23
___ I
5,
~"CiS
S iS
23
23
S 2 3
a2C,C2
S i
S1C2
<3
= T 0l T } T l
wrist
base
r*
' Q S2
0
s2 c2 0 2 2
0 0 1 0
di
1 0
0
0 1
0
S i
0
0
C3
di
 S
S
0
3
2S1C2
2 2
a S
c3
s3
0
0
3 0
c3 0
0 1
0 0
s3 0 fl3C3*
c3 0 O S
0 i 0
0 0 1
3
C
1
3
C
3
ySx
0
Ci(fl2C2 4 a3C23)
2 2
S.(tf C
di
a3c23)
a 2S 2
(2610)
3
S2
3
Here, to simplify the notation, we have used C*, 4 eos (qt + q,) and S*, 4
sin ( qk+ q).
Next, to compute the tool coordinates relative to the wrist, we have:
tool
rwrist
n r4 tt5
3 * 4
r0
c4 0 s4 0 C5 S 0 0
s4 0 c4 0 s5 c5 0 0
0 1 0 0 0 0 1 di
0 0 0 1
0 1
c4c5 c4s5 s4 d s S 4
s4c5 s4s5 c:4 d $ Q 4
0
Ss Cs 0
0
1
0 0
( 2  6  11)
4
S1S5
 c,s5
S234C5
~ C jC
234S5 4 
S1C234S5
S
234
CiC5 SiS234
s ,c 5
234Ss
CiS
C i*
117 .8 C2
+ 177 8
177 . 8 C2
C ,(
S ,(
2 1 5 .9
23
9 6 . 5 S234)
117 . 8 C23
9 6 .5 S234)
177 . 8S2 
177 . 8 S23
9 6 .5 C234
1
(2  6  12)
Here the notation Cijk is short for eos (qt + qj 4 qk) and the notation Sijk is short for
sin (q + qj + <?*). Note that the approach vector r 3 and position vector p are inde
pendent of the tool roll angle qs, as expected.
Sec. 26
61
wrist
base
ThT\T\
c,
Si
0
0
s
,s
2
c
1 j2SiC2
=
s3
c 3 0 J3S3
S2 c 2
0 di ~ a2$ 2 0
0
1 0
0
0
0
1
0
0
1
0
A
C1C23 "~CiS23
StC23
s,
s,s23
S
L 0
c
0
0
~C23
0
Ci(a2c 2 + E3C23)
Si(a2C2 + a3C23)
di
a2S2
U3S23
J3C3
3S3
0
1
( 2  6  10)
c4
s4
0
0
0
0
1
0
Xn
A
1
C
/3
wrist
rtool
Ul
s4 0'
00
c4 0 s5 c5 0 0
0 0 0 0 1 d5
0 1 0 0 1
C4C5
c4s5
s4 /5S4
s4c5 s4s5 c4 /5C4
s5 ~c5 0
0
0
0 0
1
( 2  6  11)
Note that the final expression for r ^ 'sl depends only on the last two joint vari
ables. Similarly, the final expression for rgS? depends only on the first three joint
variables. To find the arm matrix, we multiply the two wristpartitioned factors to
gether. The product, after simplification using the trigonometric identities in Appendix 1, and after substitution using the numerical vales for d and a in Table 22,
is the following arm matrix:
C1C234S3 + s ,c 5  C 1S234
C1C234C5 + S1S5
S1C234C5  c ,s 5 S1C234S5 c ,c 5 S1S234
S234S5
C234
S234C3
0
Here the notation Cljk is short for eos {q + qj + q) and the notation Sijk is short for
sin (<?, + q} + qk). Note that the approach vector r 3 and position vector p are inde
pendent of the tool roll angle <75, as expected.
Sec. 26
61
The physical structure of the Rhino XR3 is such that the base motor is
mounted vertically and remains fixed, while the shoulder, elbow, and tool pitch motors are mounted horizontally on the body, which rotates about the base axis. The
tool roll and tool closure motors are smaller motors mounted in the hand.
62
Chap. 2
We begin by constructing a linkcoordinate diagram that is based on the DenavitHartenberg algorithm. Applying steps 0 to 7 of the DH algorithm to the Rhino XR3 robot, we get the diagram of link coordinates shown in Fig. 223.
Elbow
Tool pitch
(271)
(272)
mm
Sec. 27
63
r wrist
base
T 1T2
i O* m
t 3
2
0 Si 0* c 2  s 2 0 02 C2 c 3  s 3 0
c,
c3 0
0 c, 0 s2 c 2 0 02 S2 S3
Si
0 1
0
0 1 0
0 di 0
0 1
0 0
0 0 1 .0
0 0 1 0
0
c 3  s3 0 03 C3
Ci C2
c ,c 2 c,s2 s Si
s ,c 2 S iS 2 c, 02Si C2 s3 c*3 0 03 S3
0 1 0
0 d\ 2 S2 0
S2 C a
1
0 0 1
0
0
0
0
CIC23 Ct S23 s 1 C\{a2 C2 + 03C23)
c\ S,(02C2 + 03C23)
S,C23 Si S23
0 d\ a2 S2 03S23
C 23
S23
1
0
0
0
03
C3
03 S 3
0
1
(27
Here, to simplify the notation, we have used C*, = eos (<7* + qj) and S*,
= sin {qk + qj). The transformation TbS? provides the position and orientation of
the wrist frame relative to the base frame. Since there is no tool yaw motion for this
fiveaxis robot, the wrist frame corresponds to the tool pitch frame
As a partial
check of the expression for Tbas?, we can evalate the transformation matrix at the
soft home position. From Table 24, this is q [0, ir /2, 7r / 2, 0, 7r / 2]r, which
yields:
rSe(home) =
1
0
0
0
0
1
0
1
0
03
0
d\ + a2
(274)
KINEMATIC PARAMETERS OF A
FIVEAXIS ARTICULATED ROBOT
TABLE 24
Axis
1
2
3
4
5
Home
<?3
dx
0
0
0
d5
0
a2
as
a4
0
tt/ 2
0
0
t 2
0
0
tt/ 2
7t/2
0
7t/2
?5
Inspection of Fig. 223 reveis that this is consistent with the linkcoordinate dia
gram. The first column of the rotation matrix indicates that, in the soft home posi
tion, axis x 3 of the tool pitch frame has coordinates (1, 0, 0) in the base frame. Con
sequently, it is pointing in the same direction as base axis x. The second column of
R indicates that axis y 3 of the tool pitch ffame has coordinates (0, 0,  1 ) in the base
ffame, in which case it is pointing in the opposite direction of base axis z. The last
64
Chap. 2
c4
s4
0
0
0 S 4
0 C4
1
0
0 0
C4C5
S4C5
s5
0
c5 s, 0
4 S4 s5 C5 0
0 0 0 1
1 .0 0 0
s4 Cl4 C4  /5S4
c4 Q4 S4 + d$ C4
/ 0
0
1
0
C4 C4
C4 S 5
S4S5
n1
wnst
column o f R specifies that axis z 3 of the tool pitch ffame has coordinates (0, 1, 0)
relative to the base. Thus 2 3 points in the same direction as base axis y. Finally, the
position vector p indicates that the coordinates of the origin of the tool pitch frame
L3 relative to the base frame Lo are (a3, 0, di + a2). Thus L3 is located a distance <23
in ffont o f the base and d\ + a 2 above the base. This is consistent with Fig. 223.
Next we determine the position and orientation of the tool relative to the wrist:
5
1
(275)
Note that the final expression for the position and orientation of the tool relative to
the wrist depends only on the last two joint variables, as expected. At this point we
could again make a partial check by evaluating this transformation at the home posi
tion. This should provide the position and orientation of frame L5 relative to ffame
L3. It remains to find the position and orientation of the tool relative to the base.
This is obtained by multiplying the two wristpartitioned factors together. After sim
plification using the trigonometric identities in Appendix 1, we get the following
final result, which is the solution of the direct kinematics problem for the fiveaxis
articulated robot in Fig. 223:
"C 1 C 234S 5 + S 1 C 5
C , S 234
'
C 1 (*2 C 2
Si C 234C 5 c , s 5
S 1 C 234S 5 c , c 5
 'S 1 S 234
S 234 Cs
0
S 234S 5
C 234
Z3 C 23 + I4 C 234 ~ ds S 234)
S 234)
Note how the final expression for the position and orientation of the tool rela
tive to the base now depends on all of the kinematic parameters. We can make a
partial check by evaluating the arm matrix at the soft home position, which ffom
Table 24 is q = [0, tt/2 , 7t/2, 0, tt/2]7. This yields:
rgSiChome) =
0 1
1 0
0 0
0
0
1
#3 + 4
0
d[ + a 2  d5
.0
Sec. 27
1
65
Inspection of Fig. 223 reveis that this is consistent with the linkcoordinate dia
gram. The first column o f the rotation matrix indicates that, in the soft home posi
tion, tool axis x 5 (the normal vector) points in the same direction as axis y of the
base. The second column o f R indicates that tool axis y 5 (the sliding vector) points
in the same direction as axis x o f the base. The last column o f R specifies that tool
axis z 5 (the approach vector) points in the opposite direction from axis z of the
base. Finally, the position vector p indicates that the origin o f the tool coordinate
ffame (the tool tip) is located a distance ai + z4 in front of the base and
di 4* a 2  di above the base when the robot is in the soft home position.
The solution to the direct kinematics problem in Prop. 271 is generic, in the
sense that all o f the nonzero joint distances and link lengths appear as explicit
parameters. Consequently, other robotic arms having a similar kinematic
configuration but different vales for these kinematic parameters can also be analyzed using this general expression for the arm matrix. O f course, not all fiveaxis
articulated robots will fit this pattern. The Alpha II industrial robot is one that does,
as can be seen from the following example.
mm
c5
C, s5 Si C234 S 5 C, c3
Ci C;>34 C5 f" S, s 5
Si Ci34 C5
S234 C 5
Cj C234 S 5 + Si
S234 S 3
Cj S 234
S{ S234
C234
I!
!i
This is precisely the same result as obtained in Sec. 26, where an independent analysis
of the kinematics of the Alpha II robot was performed. Thus, in terms of its kinematic
structure, the Rhino XR3 robot includes the Alpha II robot as a special case. As a par
tial check on this expression for the arm matrix for the Alpha II robot, we can evalate
it at the soft home position, which, from Table 22, corresponds to q  [0, 0, 0,
7t/2, 0]r . For these vales of the joint variables, we have the following transforma
tion matrix. Inspection of Fig. 219 reveis that this is indeed consistent with the linkcoordinate diagram.
Chap. 2
TOUhome) =
1
485.1
215.9
Ah
Sec. 27
0
0
P2
0
0
0
p4
0
0
0
Ps
0
1
0
0
1
0
1
0
0
0
A0 = P'C A0
(277)
67
Here A8 is a vector with five components which specifies the desired change in the
five joint angles expressed in degrees. The coefficient matrix C is a coupling matrix.
Note the ls beiow the diagonal for the elbow command A/j3 and the tool pitch com
mand A/14. These offdiagonal terms effectively remove the coupling introduced by
the parallel bar mechanism. The matrix P~x is the inverse of a diagonal precisin
matrix. Here the p s specify the precisin of each joint expressed in degrees per en
coder count. From Table 111, we have:
p = [0.2269, 0.1135, 0.1135, 0.1135, 0.3125]7 degrees per count
(278)
Note that the terms p3 and p4 appearing in Eq. (277) have negative signs preceding
them. This is because positive directions for the 0s in the linkcoordinate diagram
in Fig. 223 do not necessarily correspond to positive encoder counts in the Rhino
movement command (Hendrickson and Sandhu, 1986). Given the way the Rhino
XR3 robots are wired at the factory, the base, shoulder, and tool roll motors have
positive encoder counts corresponding to positive joint angles, but the elbow and
tool pitch motors have negative encoder counts corresponding to positive joint an
gles; henee the minus signs to compnsate. Of course, the Rhino XR3 hardware
could be modified by reversing the power leads to the motors and remounting
(flipping) the optical encoder disks. However, this is really not necessary, since the
introduction of minus signs in the software achieves the same result.
The vector Ah specifies the proper number of encoder hole counts to send to
each of the five motors, starting with the base and ending with the tool roll, in order
to achieve a move of A6 degrees in joint space. A matrix formulation of A/i is used
in Eq. (277) to highlight the interaxis coupling, which manifests itself through offdiagonal terms in C. For a software implementation of Eq. (277), clearly it would
be more efficient to premultiply the matrix expression for Ah by hand to produce five
seprate scalar equations for the individual components of Ah.
28 A FOURAXIS SCARA ROBOT (ADEPT ONE)
Another important type of robotic manipulator is the fouraxis horizontaljointed
robot, or SCARA robot. Examples of robots which belong to this general class in
clude the Adept One robot, the IBM 7545 robot, the Intelledex 440 robot, and the
Rhino SCARA robot. For illustration purposes we examine the Adept One robot,
shown in Fig. 224. The treatment, however, is generic, and consequently the
method used is applicable to SCARA robots in general.
The Adept One robot is unique in that it was the first commercial robot to implement a direct drive system for actuation. No gears or other mechanical power
conversin devices are used. Instead, hightorque lowspeed brushless DC motors
are used to drive the joints directly. This eliminates gear friction and backlash and
allows for clean, precise, highspeed operation.
281 The UnkCoordinate Diagram
To construct the linkcoordinate diagram, we apply steps 0 to 7 of the DH al
gorithm. The resulting set of link coordinates for the SCARA class of robots is
shown in Fig. 225.
68
Chap. 2
Figure 224 Fouraxis SCARA robot (Adept One). (Courtesy of Adept Technol
ogy, Inc., San Jos, CA.)
Elbow
Vertical
extensin
Sec. 28
69
mm
In this case the vector of joint variables is q = [0*, 02, fe, fe]7. The first two
joint variables {ft, 62 } are revolute variables which establish the horizontal compo
nent of the tool position p. The third joint variable, fe, is a prismatic variable which
determines the vertical component of p. Finally, the last joint variable, 04, is a revo
lute variable which Controls the tool orientation R. Applying steps 8 to 13 of the
DH algorithm yields the kinematic parameters shown in Table 25.
Note that 7 of the 12 constant parameters in Table 25 are zero, which makes
the SCARA robot kinematically simple. Indeed, of all the practical industrial robots,
the SCARA robotis perhaps the simplest to analyze. Thevales for the joint dis
tances d and linklengths a of the Adept One robot are asfollows:
(281)
mm
(282)
Here a tool length of fe = 200 mm has been assumed. Note that a specific valu for
fe has not been specified, because fe is a joint variable in this case. The vales for fe
range from 0 to 195 mm, which implies a vertical stroke of 195 mm.
282 The Arm Matrix
Next we compute the arm matrix Tb?e(q) for the SCARA robot. Since there are only
four axes, we will not perform the intermedate step of partitioning the problem at
the wrist. Instead we compute the arm matrix directly in one step, as follows:
7 tool
base
C,
S,
0
0
T i t 2 t 3 t 4
0 i l 2 3
0
0
1
0
Si
C y
0
0
"C 1 2
S i 2
0
.0
" Ci 2
S ,2
0
0
y C y
ay
Si
dy
1
S i 2
 C i  2
0
0
Si 2
 C i  2
0
0
s2 0
c2 0
1
0
o
0
c2
s2
0
0
0
0
1
0
1
0
2S2
0
1
C
*iS
di
1
0
0
0,2 C2
C
ay S
c4 s4 o o
s4 c4 o o
1 0
0 0
0 1 0
0
0 0
1 q3
0 0
1
0
0
0
c4
s4
0
0
o
o
0 0 0'
1 0 0
0 1 qi
0 0 1
s4
c4
0
0
0
o
1 fe
o 1
c4  s 4 o o
s4 c4 o o
o
o
0 0
0 0
1 d
0 1
0
o
1 fe
o 1
(283)
After multiplication and simplification using trigonometric identities from Appendix 1, we get the following arm matrix for a SCARA robot. Since the nonzero
joint distances d and link lengths a have been left as explicit parameters, the follow
70
Chap *
ibt
<72
0
3
4
<?4
Home
d,
0
a.
a2
ir
>
4
ISO 6
7r/2
ing expression for the arm matrix is generic and applies to a variety of SCARA class
robots:
Proposition 281: Adept One. The arm matrix Tbc(q) for a fouraxis
SCARA robot whose linkcoordinate diagram is given in Fig. 225 is:
't
1
(S
1
00
__
1
N
1
U
L _
base
rtool
S i.2 4
Q\ C ,
Ci  2 4
fli S i
1
di
+
+
0 2 C 1 2
0 2 S 1 2
<73
d4
Here the notation C 1 24 denotes eos (<71 q 2 q*)> and similarly, S 1 24 de
notes sin (<?i qi <74). Note that the approach vector is fixed at r3 = i 3, inde
pendent of the joint variables. This is characteristic of SCARA robots, which are de
signed to maniplate objeets from directly above. They are often used in light
assembly tasks such as the insertion of components into circuit boards.
Example 281: Adept One
Find the position of the tool tip of the Adept One robot when the joint variables are
q = [7t/4, 7r/3, 120, 7r/2]r.
Solution
Sec. 29
71
Figure 226 Sixaxis articulated robot (Intelledex 660T). (Courtesy o f Intelledex. Inc.,
Corvallis, OR )
The physical structure of the Intelledex 660 robot is quite unique. It is a full
sixaxis robot yet there are only two joints out near the end of the arm , a tool pitch
joint (outboard") and a tool roll joint ( tool spin). Even though the wrist has only
two joints, arbitrary orientations of the tool are possible. Tool yaw motion is
achieved indrectly through a unique twoaxis shoulder design. The first axis of the
shoulder rolls the entire arm , while the second axis achieves a pitchtype motion in
the plae selected by the shoulder roll. Thus, to implement a pur horizontal yaw
m otion o f the tool, one can first roll the shoulder by 7r/2 radians and then activate
the tool pitch joint. By eectively relocating the tool yaw joint in the shoulder, a reduction in the weight out at the end of the arm is realized. In some robots, such as
the M icrobot Alpha II and the GE P50 robot, weight reduction at the end of the arm
is achieved by placing the actuators for the distal joints back near the base of the
robot and then running cables or chains up through the arm . The virtue of the Intelledex design is that there is no need for mechanical power transmission through
the arm . Instead, the yaw motion is achieved indirectly through software.
72
Chap. 2
Figure 227
Tool pitch
1
2
3
4
5
6
<71
<72
<73
<74
<7s
<?6
dx
0
0
0
0
de
Home
7t/2
7t/2
tt/2
tt / 2
a3
it
Oa
tt/2
7t/2
/2
Here 11 of the 18 fixed parameters are zero. In this case de represents the tool
length, which may vary from robot to robot, depending upon which type of tool is
installed. The vales for the joint distances d and link lengths a of the Intelledex 660
robot are as follows:
Sec. 29
73
mm
(291)
mm
(292)
Here d* 228.6 mm corresponds to the standard pneumatic gripper. If, instead, the
small servo tool or some other tool is installed, then the calibration technique discussed in the operators manual should be used to measure d(>.
292 The Arm Matrix
Next consider the problem of computing the arm matrix of the robot in Fig. 227.
First we partition the problem after the third axis, which, in this case, is the elbow
joint. Using Prop. 261 and Table 26, we have:
elbow
'T'l nr2 nr>3
7base
* 0 1 * 2
0 s2 0
 s 3 0 #3C3
0 c2 0 s 3 c 3 0 03S3
1 0 0 0
0 1 0
0 0 1
0 0 1 0
c3 s3 0 03 C3
s3 c3 0 0 3 S3
0
0 1 0
1 .0
0 0 1
C1 C2 S3 + s,c3 C1 S2 (CiC2c3 + s,s3W
S1 C2 S3  c,c3 s,s2 (S1 C2 C3 ~ Ci 83)0 3
d\ + S2C303
c 2
S 2 S3
Ci C2 C3 + Si S3
S1 C2 C3  C1 S3
S2 C3
fO

'
c, 0 s, 0 c 2
s, 0 Ct 0 s2
0 1 0 di 0
0 0 0 1 0
c,c2 s, c,s2 o
s,c2 c, s,s2 0
s2
o
c 2 d.
1
(293)
The transformation TbST provides the position and orientation of the elbow
frame L3 relative to the base frame L0. As a partial check of the expression for JS",
we can evalate it at the soft home position. From the last column of Table 26, this
is q = [tt/2,
tt/I, 7t / 2 , 0, 7t / 2 , 0]r, which yields:
1 0 0
0 0 1
Lbibsew(home) = 0 1 0
0 0
03
0
di
(294)
Inspection of Fig. 227 reveis that this specification of the position and orientation
of ffame L3 relative to ffame Lo is consistent with the linkcoordinate diagram.
Next we determine the position and orientation of the tool relative to the el
bow:
74
Chap. 2
rssu 
n n n
C45 c 6
S4 5 C6
s6
0
0
0
1
0
s5 0
s4 s5 0 Cs 0
0
0 1
0 0
1 .0
0
0 1
Oa Q a
S45
CI4 c4
C45
0
4 S4
C 45 s 6
S45
"S45 s6  c45
c6
0
c6
s6 o o
s6
c6 0 o
o
o
o
o
1 do
O 1
C.  s6 0 o
c6 0 0
s6
0
0 1 do
0
0 0 1
40
C45
S45
0
0
s4 0
c4 0
0
1
0 0
L
C4
S4
0
0
(295)
C4 f s,15 6
24 s4  G \sd o
c4
Note that the final expression for the position and orientation of the tool rela
tive to the elbow depends only on the last three joint variables, as expected. At this
point we could again make a partial check by evaluating this transformation at the
soft home position. This should provide the position and orientation of frame Lo rel
ative to frame L3. We still need to find the position and orientation of the tool rela
tive to the base. This is obtained by multiplying the two elbowpartitioned factors
together. After considerable simplification using the trigonometric identities in Appendix 1, we get the following final result, which is the solution of the direct kine
matics problem for the sixaxis Intelledex 660 robot:
Proposition 291: Intelledex 660. The position p(q) and orientation R(q)
of the tool ffame relative to the base frame for the sixaxis articulatedcoordinate
robot whose linkcoordinate diagram is given in Fig. 227 are:
Cl
P=
C3 + 4C34
+6 S345) + S i(fl3S 3 + #4S 34 ~
C 34s)
6 C 34s)
S 1 C2O33 C3 +
4C34 +
d\
+ 82(03 C 3 +
04 C34
+ ?6 S345)
(C 1C 2 C 3 4 5 + S i S 345) C 6 + C j S2 Sg
C 1S 2 C 6
(S i C2 C 345 ~ C i S 345)C6 + S( S2 S
S 1S 2 C 6 (SiC2C345 ~ C , S 345)S 6
S2 C 345 C ~ C2 Sg
(C1C2C345 + S 1 S345)Sg
~ C2 C
S2C345S6
S1C345 + C i C2 S 345
C1C345 + S1C2S345
S2 S345
It is evident that the expressions for the approach vector r 3(q) and the tooltip
position p (q ) are independent of the tool roll angle q6, as expected. We can make a
partial check of the final expression for the arm matrix by evaluating it at the soft
home position, which corresponds to q = [tt/2 ,  n / 2 , 7t/2, 0, 7t/2, 0]r. This
yields:
Sec. 29
75
"
7bUhome) =
0
0
1
0
1
0
1
0
0
03 + 04 + d
0
dx
Note that this is consistent with the linkcoordinate diagram in Fig. 227. The first
column of the rotation matrix indicates that, in the soft home position, tool axis jc6
(the normal vector) points in the same direction as axis z of the base. The second *
column of R indicates that tool axis y 6 (the sliding vector) points in the opposite di
rection from axis y of the base. The last column of R specifies that tool axis z6 (the
approach vector) points in the same direction as axis x of the base. Finally, the po
sition vector p indicates that the origin of the tool coordinate ffame (the tool tip) is
located a distance <a3 + a4 + d 6 in front of the base and d\ abo ve the base when the
robot is in the soft home position.
210 PROBLEMS
21. Which of the kinematic parameters are variable for a revolute joint? Which are vari
able for a prismatic joint?
22. Consider the singleaxis robot in Fig. 228 shown in the home position, which corre
sponds to 0 = 7r/2. Suppose the point p on the mobile link has coordinates
[p]M = [0.5, 0.5, 2.0f.
(a) Find an expression for R(0), the coordinate transformation matrix which maps mo
bile M coordinates into fixed F coordinates as a function of the joint variable 6 .
(b) Use R( 6 ) to find [p]F when 0 = 7r and when 0 = 0.
Figure 228
Singleaxis robot.
23. Consider the following coordinate transformation matrix, which represents a funda
mental rotation. What is the axis of rotation (1, 2, or 3), and what is the angle of rota i
tion?
R =
0.500
0
0.866
0 0.866'
1 0
0 0.500
76
Chap. *
24. Consider the robotic tool shown in Fig 229. Suppose we yaw the tool by tt about / ,
then pitch the tool by  t t /2 about / 2, and finally roll the tool by t t /2 a b o u t /\
(a) Sketch the sequence of tool positions after each of the yaw, pitch, and roll movements.
(b) Find the transformation matrix T which maps tool coordinates M into wrist coordi
nates F following the sequence of rotations.
(c) Find [p]Fy the location of the tool tip p in wrist coordinates F following the se
quence of rotations, assuming that the tooltip coordinates in terms of the tool
frame are [p]M = [0, 0, 0.8]r.
25. Do Prob. 24 but with the yaw, pitch, and roll of the tool performed in reverse order
about the unit vectors of the M coordinate frame rather than about the unit vectors of
the F coordinate frame.
Tool
Roll
Yaw
nr
Pitch
! p
0 1
Assuming that R and p represent a rotation and a translation of the mobile frame about
its own unit vectors, which of the following sequence of operations does the composite
transformation T represent? That is, what is the implicit order of the two fundamental
operations?
(a) T represents a rotation of R followed by a translation of p?
(b) T represents a translation of p followed by a rotation of /??
(c) T represents neither in general?
27. Find a numerical example which shows that the order in which a rotation and a transla
tion are performed does affect the final relationship between two initially coincident
coordinate frames. Specify the coordinate transformation matrices, and sketch the se
quence of positions.
28. Homogeneous coordinate transformation matrices can be used to represent scaling op
erations as well as rotations and translations. Consider the following fundamental ho
mogeneous scaling matrix:
0
c1 0 0
0
0 C2 0
0
0 C3
0
Scale (c, cr) 4
_0
Sec. 21 0
Problem s
77
(a) Show that when a * 0 and *   l . i, l) r, Scale (c, cr) scales all three physical
coordinates by 1ja He re r is referred to as a global scale factor
(b) Show that Scale (c l) wales the rth physical coordnate by ck for 1 s k s 3.
Here c is referred to a s a vector of local scale factors.
(c) Show that when cr * 0 and c = [cr, cr, tr]7, the local and global scale factors can
cel one another, in which case multiplication by Scale (c. cr) has no effect on the
physical coordinates.
29. Consider the Ummation PUMA 200 manipulator, shown in Fig. 230. This is a sixaxis
articulated robot with a rollpitchroll type of spherical wrist. Assign link coordinates
using the first half of the DH algorithm Label the diagram with a 's and d's as appro
priate.
210. Use the second half of the DH algorithm to fill in the kinematic parameters for the
PUMA 200 robot in Table 27, consistent with your linkcoordinate diagram from
Prob. 29. Indicate which parameters are the joint variables.
TABLE 27
Axis
Home
1
2
3
4
5
6
78
Chap. 2
211. Compute the first factor of the arm matrix T(q) for the PUMA 200 robot using your
table of kinematic parameters from Prob. 210.
212. Compute the second factor of the arm matrix Tl tv(q) for the PUMA 200 robot using
your table of kinematic parameters from Prob. 210.
213. Compute the arm matrix Tl^ ( q ) for the PUMA 200 robot using your wristpartitioned
factors from Prob. 211 and Prob. 212.
214. An alternative way to specify the tool orientation is the ZTZ Euler angle representa
tion. The mobile frame M and fixed frame F start out coincident. Frame M is first ro
tated about m 3 by an angle of a; it is then rotated about m2 by an angle of j8; and
finally, frame M is again rotated about m3 by an angle of y. Find the composite ZKZ
Euler angle rotation matrix RZYZ(a y /3, y) which maps mobile M coordinates into fixed
F coordinates.
215. Consider the United States Robots Maker 110 manipulator shown in Fig. 231. This is
a fiveaxis spherical coordinate robot with a pitchroll spherical wrist. Assign link co
ordinates using the first half of the DH algorithm. Label the diagram with a s and d 's
as appropriate.
Figure 231
216. Use the last half of the DH algorithm to fill in the table of kinematic parameters for
the Maker 110 in Table 28, consistent with your linkcoordinate diagram for Prob.
215. Indicate which parameters are the joint variables.
217. Compute the first tactor of the arm matrix T\f(q) for the Maker 110 robot using your
table of kinematic parameters from Prob. 216.
218. Compute the second factor of the arm matrix TOUg) for the Maker 110 robot using
your table of kinematic parameters from Prob. 216.
Sec. 210
Problem s
79
TABLE 28
Axis
Home
1
2
3
4
5
219. Compute the arm matrix 7b.(#) for the Maker 110 robot using your wristpartitioned
factors from Prob. 217 and Prob. 218.
REFERENCES
Craig, J. (1986). Introduction to Robotics: Mechantes and Control, AddisonWesley: Reading, Mass.
D e n a v it, J., and R. S. H a rte n b e rg (1955). A kinematic notation for lowerpair mechanisms
based on matrices, A
J. Appl. Mech., June, pp. 215221.
Fu, K. S., R. C. G o n z le z , and C. S. G. L e e (1987). Robotics: Control, Sensing, Vision, and
Intelligence, McGrawHill: New York.
H e n d ric k so n , T., and H. S a n d h u (1986). XR3 Robot Arm, Mark I I I 8 Axis Controller:
Owners Manual, Versin 3.00, Rhino Robots, Inc.: Champaign, 111.
N o b le , B. (1969). Applied Linear Algebra, PrenticeHall, Inc.: Englewood Cliffs, N.J.
Paul, R. P. (1982). Robot Manipulators: Mathematics, Programming, and Control, MIT
Press: Cambridge, Mass.
S c h illin g , R. J. and H. L e e (1988). Engineering Analysis: A Vector Space Approach, Wiley:
New York.
80
Chap 2
3
In v e rse Kinematics:
Solving
The Arm Equation
In Chap. 2 we developed a procedure for determining the position and orientation of
the tool of a robotic manipulator given the vector of joint variables. We now exam
ine the inverse problem of determining the joint variables given a desired position
and orientation for the tool. The inverse kinematics problem is important because
manipulation tasks are naturally formulated in terms of the desired tool position and
orientation. This is the case, for example, when external sensors such as overhead
cameras are used to plan robot motion. The information provided by the camera is
not in terms of joint variables; it specifies the positions and orientations of the objects that are to be manipulated.
The inverse kinematics problem is more difficult than the direct kinematics
problem because a systematic closedform solution applicable to robots in general is
not available. Moreover, when closedform Solutions to the arm equation can be
found, they are seldom unique. The ways in which mltiple Solutions arise are investigated. A compact representation of tool position and orientation called the toolconfiguration vector is then introduced. Solutions to the arm equation for four
generic classes of robots are presented. Representative members of these classes in
clude: the fiveaxis Rhino XR3, the fouraxis Adept One, the sixaxis Intelledex
660, and a threeaxis planar articulated arm. Chapter 3 concludes with an example
o f an application of inverse kinematics to plan motion in a robotic work cell using
information available from an overhead camera.
3'1 THE INVERSE KINEMATICS PROBLEM
The key to the solution of the direct kinematics problem was the DenavitHartenberg
(DH) algorithm, a systematic procedure for assigning link coordinates to a robotic
manipulator. Successive transformations between adjacent coordinate frames, start81
ing at the tool tip and working back to the base of the robot, then led to the arm ma
trix. The arm matrix represents the position p and orientation R of the tool in the
base frame as a function of the joint variables q, as shown in Fig. 31.
(94.......9}
<*3
For convenience, we will refer to the position and orientation of the tool collectively as the configuration of the tool. The solution to the direct kinematics prob
lem can be expressed in the following form, where R represents the rotation and p
represents the translation of the tool ffame relative to the base frame:
R
I
0 I 1
T'S&iq)
0
(311)
The solution to the direct kinematics problem is useful because it provides us with a
relationship which explicitly shows the dependence of the tool configuration on the
joint variables. This can be utilized, for example, in determining the size and shape
of the work envelope. In particular, suppose Q represents the range of vales in R"
that the joint variables can assume. We refer to Q as the jointspace work envelope of
the robot. Typically, the jointspace work envelope is a convex polyhedron in Rn of
the following general form:
Q=
Here q mn and # raax are constant vectors in Rm which represent joint limits and C is a
joint coupling matrix. For the simplest special case, the joint coupling matrix is
C = /; in this case q f in and qf represent lower and upper limits, respectively, for
joint k. More generally, if the joint coupling matrix C has some offdiagonal terms,
then Eq. (312) represents constraints on linear combinations of joint variables. In
this case the number of inequality constraints m may exceed the number of variables
n. The set of positions in Cartesian space R3 that are reachable by the tool tip can be
investigated by examining the vales of p(q) as q ranges over Q. Similarly, the set of
orientations achievable by the tool can be determined by examining the vales of
R(q) as q ranges over the jointspace work envelope Q.
Perhaps the most important benefit provided by the solution of the direct kine
matics problem is that it lays a foundation for solving a related problem, the inverse
82
Chap. 3
{p. R}
Toolconfiguration
Joint space
space R 6
I nverse
kinematics
equations
Figure 32
{p. R}
The inverse kinematics problem is more difficult than the direct kinematics
problem, because no single explicit systematic procedure analogous to the DH al
gorithm is available. As a result, each robot or generically similar class of robots has
to be treated separately. However, the solution to the inverse kinematics problem is
much more useful. Indeed, a key to making robots more versatile lies in using feedback from external sensors such as tactile arrays and visin. External sensors supply
information about part location and part orientation directly in terms of
configurationspace variables. It is then necessary to use this information to deter
mine appropriate vales for the joint variables to properly configure the tool. Thus
we must find a mapping from a toolconfiguration space input specification into a
jointspace output specification. This is the inverse kinematics problem. We charac
terize this problem more formally as follows:
Problem : Inverse Kinematics. Given a desired position p and orientation R
for the tool, find vales for the joint variables q which satisfy the arm equation in
Eq. (311).
The solution to the inverse kinematics problem is useful even when external
sensors are not employed. A case in point is the problem of getting the endeffector
or tool to follow a straightline path. Certain types of assembly, welding, and sealing
operations require that straightline paths be followed or at least closely approximated. A straightline trajectory is naturally formulated in toolconfiguration space.
It is then necessary to find a corresponding trajectory in joint space which will proSec. 3t
83
duce the straightline motion of the tool tip. This is an important special case of the
inverse kinematics problem.
T g (q) =
j P\
P2
Su
Ri\
Rn
R 22
Rn
Rn
R
i\
R 32
R 33 1 P3
(321)
Since the last row of the arm matrix is always constant, the arm equation constitutes
a system of 12 simultaneous nonlinear algebraic equations in the n unknown compo
nents of q.However, the 12 equations are by no meansindependent of one another.
Since R is acoordinate transformation matrix representing a purrotation from one
orthonormal frame to another, it follows from Prop. 223 that R~x = R T. But if
R TR / , then the three columns of R form an orthonormal set. The mutual orthog
onality puts three constraints on the columns of R , namely:
r 1 r 2 = 0
(322)
r 1 r 3 = 0
(323)
r3 = 0
(324)
These constraints come from the offdiagonal terms of R TR gj I. The diagonal terms
dictate that each column of R must also be a unit vector. This adds three more con
straints to the columns of R:
r
= 1
1< k < 3
(325)
Thus the 12 constraints implicit in the arm equation actually represent only six inde
pendent constraints on the n unknown components of the vector of joint variables q.
If we are to have a general solution to the inverse kinematics problem, one for which
84
Chap. 3
a q can be found which generates an arbitrary tool configuration, then the number of
unknowns must at least match the number of independent constraints. That is:
General manipulation
n > 6
(326)
This lower bound on the number of axes n is a necessary but not sufficient condition
for the existence of a solution to the inverse kinematics problem when arbitrary tool
configurations are specified. Clearly, the tool position must be within the work en
velope of the robot, and the tool orientation must be such that none of the limits on
the joint variables are violated. Even when these additional constraints on the vales
of p and R are satisfied, there is no guarantee that a closedform expression for a so
lution to the inverse kinematics problem can be obtained (Pieper, 1968).
The general strategy for solving the inverse kinematics problem simplifies
somewhat when the robot has a spherical wrist. To see this, consider the case of an
naxis robot where 4 < n < 6. Suppose the last axis is a tool roll axis, and suppose
the robot has a spherical wrist, which means that the n 3 axes at the end of the
arm all intersect at a point. For this class of robots, the inverse kinematics problem
can be decomposed into two smaller subproblems by partitioning the original prob
lem at the wrist. Given the tool tip position p and tool orientation /?, the wrist posi
tion p wnst can be inferred from p by working backward along the approach vector:
pwnst p __ dy
(327)
Here the joint distance dn represents the tool length for an axis robot as long as the
last axis is a tool roll axis. The approach vector r 3 is simply the third column of the
rotation matrix R. Once the wrist position p wnst is obtained from {p, i?, d}y the first
three joint variables {q\, q2, #3} that are used to position the wrist can then be ob
tained from the following reduced arm equation:
(328)
The fourth column of 7*2? represents the homogeneous coordinates of the origin of
the wrist ffame L3 relative to the base ffame Lo. Since the wrist coordinates depend
only on the joint variables {#1, q2, ^3}, these joint variables of the major axes can be
solved for separately using Eq. (328). Once the major axis variables {qx, q2> qy)
are found, their vales can then be substituted into the general arm equation in Eq.
(321) and it can be solved for the remaining tool orientation variables {q4,
85
freedom add flexibility to the manipulator. For example, a redundant robot might be
commanded to reach around an obstacle and maniplate an otherwise inaccessible
object. Here some of the degrees of freedom can be used to avoid the obstacle while
the remaining degrees of freedom are used to configure the tool. Consider, for ex
ample, the top view of a redundant SCARA robot shown in Fig. 33. Since only two
joints are needed to establish the horizontal position of the tool, the second elbow
joint of the SCARA robot is redundant.
Redundant
elbow
Even when a robot is not kinematically redundant, there are often circumstances in which the solution to the inverse kinematics problem is not unique. Several distinct Solutions can arise when the size of the jointspace work envelope Q is
sufficiently large. As a case in point, consider the articulatedcoordinate robot
shown in Fig. 34. If the limits on the range of travel for the shoulder, elbow, and
tool pitch joints are sufficiently large, then two distinct Solutions exist for the simple
task of placing the tool out in ffont of the robot.
Elbow up
We refer to the two Solutions in Fig. 34 as the elbowup and the elbowdown
solution. In toolconfiguration space the two Solutions are identical, because they
produce the same p and R, but in joint space they are clearly distinct. Typically the
elbowup solution is preferred, because it reduces the chance of a collision between
the links of the arm and obstacles resting on the work surface.
86
Chap. 3
(331)
Other scaling functions might also be used, but the function/is sufficient because/(g,,) > 0 for all qn and f ~ l(qn) is well defined. If we scale the approach vector
r 3 by f { q n), this yields a compact representation of tool orientation. We then com
bine this representation of orientation with the tool tip position and refer to the re
sulting vector in R6 as a toolconfiguration vector.
Sec. 33
Tool Configuration
87
Definition 331: ToolC onfiguration Vector. Let p and R denote the posi
tion and orientation of the tool frame relative to the base frame where qn represents
the tool roll angle. Then the toolconfiguration vector is a vector w in R6 defined:
w1
P
A
_[exp(<?/7r)r}]_
qn = ir ln (w4 + W5 + h>6)1/2
The toolconfiguration vector w provides us with a more convenient way to
specify the desired tool position and orientation as input data to the inverse kinemat
ics problem. Furthermore, the structure of the toolconfiguration vector can reveal
useftil qualitative information about the types of positions and orientations achievable by the tool. To see this, we examine a fiveaxis articulated robot.
332 Tool Configuration of a FiveAxis Articulated Robot
Many industrial robots have five axes; some have only four. Even though these ma
nipulators are not general in the sense of being able to generate arbitrary tool
configurations, they are still quite versatile. However, when robotic work cells em
ploy ing these robots are planned, the layout of the part feeders and other fixtures
must be constrained so they do not require the flexibility of a full sixaxis robot. As
an illustration, consider a fiveaxis articulatedcoordinate robot which has no tool
yaw motion. In this case, the part feeders might be arranged concentrically around
the robot base as shown in Fig. 35.
The part feeders can be tilted at any angle y that is consistent with the range of
global tool pitch angles realizable by the robot. However, the part feeders must be
oriented in such a way as to be aligned with radial lines or spokes emanating from
the base axis. This is because with the tool yaw angle fixed as in Fig. 35, the ap
proach vector of the tool is constrained to lie in a vertical plae through the base
axis. This constraint can be summarized algebraically in terms of the components of
the arm matrix as follows:
R u p i = Rupx
(332)
Here the ratio of the first two components of the approach vector r 3 must equal the
ratio of the first two components of the position vector p. In geometric terms, the
88
Chap. 3
OA
tool can maniplate objects from above, from the front, from the back, and even
from below consistent with workenvelope constraints and limits on the range of tool
pitch angles. However, the fiveaxis robot shown in Fig. 35 cannot maniplate ob
jects from the side. That is, the orthogonal projection of the approach vector onto
the worksurface or jty plae cannot have any component that is tangent to a circle
of radius  p  centered at the base axis. Another way to view the constraint on tool
orientation is to substitute Eq. (332) in the general expression for the toolconfiguration vector in Def. 311. This yields the following locus of tool
configurations, where /3 is a constant:
w = [wi, w2, vv3, /3wi, /3w2,
w6]t
(333)
Thus, for example, if the tool tip is directly in front of the robot (w2 = 0), the ap
proach vector must point forward (vv5 = 0). More generally, the ratio of the hori
zontal components of the tooltip position, wi/w2, must equal the ratio of the hori
zontal components of the approach vector, w4/w 5. It is evident by inspection of the
formulation in Eq. (333) that this robot has only five degrees of freedom, because
components vv4 and w5 cannot be specified independently of wi and w2.
333 Tool Configurations of a FourAxis SCARA Robot
The most important special case for manipulating parts corresponds to approaching
parts from directly above. This includes picking up a part from a horizontal work
surface and placing it down on a horizontal work surface. As an example of a robot
designed for these types of operations, consider the fouraxis horizontaljointed or
SCARA robot shown in Fig. 36. Here the tool yaw and tool pitch angles are fixed
in such a way that the tool always points straight down.
Recall that the fiveaxis robot of Fig. 35 had an approach vector r 3 that was
restricted to lie in a vertical plae through the base axis. The geometry of a SCARA
robot removes one more degree of freedom and restricts the approach vector to lie
along a vertical line within that plae, namely:
r3
= i3
(334)
The fouraxis SCARA robot in Fig. 36 is a minimal configuration for a generalpurpose robot. The first three axes are the major axes used to position the tool
tip, while the fourth axis is a minor axis used to orient the tool by varying the slidSec. 33
Tool Configuration
89
rn
Figure 36
ing vector. If Eq. (334) is substituted in the general expression for the toolconfiguration vector in Def. 331, this yields:
w
p i, p 2, p 3, 0, 0, ex p
(335)
The four degrees of freedom that are available are evident from inspection of Eq. (335), where we see that two of the components of the toolconfiguration vector are
always zero. The SCARA robot is a particularly simple robot in terms of analysis,
yet it is a practical robot for many applications.
Chap. 3
The second basic approach to solving the inverse kinematics problem is to ex*
ploit the specific nature of the direct kinematic equations to determine an analytical
or closed*form expression for the solution. These exact methods are considerably
faster than the numerical approximation methods, and they can be used to identify
mltiple Solutions. The main drawback of the analytical methods is that they are
robotdependent. Consequently a seprate analysis has to be performed for each
robot or generic class of robots (Pieper, 1968).
We Ilstrate the analytical solution technique by applying it to several different
types of robots. The first class of robots we consider are the fiveaxis verticallyjointed or articulated robots with tool pitch and tool roll motion. This class of robots
includes the Rhino XR3 robot, the Microbot Alpha II robot, and several other commercial robotic manipulators as special cases. The linkcoordinate diagram for a
fiveaxis articulated robot was previously developed as Fig. 223 in Chap. 2. For
convenient reference, we redisplay it here as Fig. 37.
Elbow
Tool pitch
The solution to the inverse kinematics problem starts with the expression for
the toolconfiguration vector w(q), which can be obtained from the arm matrix de
veloped in Chap. 2. From Prop. 271 and Def. 331, the toolconfiguration vector
for the fiveaxis articulated arm is:
C i( #
2C 2
0 3 C 23
S i(#
2C 2
04C234 ~
d
\ C
I2
S2 ~
~Q
3
S2
3~
~ 04S234
^5 S234)
/5C234
(341)
w (q)

[ e x p ^ s / tt)]C 1S
[e x p (g
234
5/ 7r ) ] S iS 234
[ e x p ( ^ / t t ) ] C
234
Since this is an articulated robot, q = 0. Recall that the notation Cy* is short
for eos (q, + qj + qk) and similarly Sy* denotes sin (q + q} F <?*). In order to extract the jo in t variable vector q from the nonlinear toolconfiguration function w(q ),
Sec. 34
91
(342)
The function atan2 in Eq. (342) denotes a fourquadrant versin of the arctan function. The atan2 function allows us to recover angles over the entire range
[ 7r, 7r]. This function is used repeatedly in subsequent derivations. It can be implemented with the more common twoquadrant arctan function as indicated in
Table 31, where the notation sgn represents the signum, or sign, function. That is,
sgn (a) retums 1, 0 , or 1 depending upon whether a is negative, zero, or positive,
respectively.
TABLE 31
Case
x > 0
x = 0
x < 0
Quadrants
1 ,4
1 ,4
2 ,3
atan2
( y , jc)
arctan ( y / x )
[sgn (y)] t t / 2
arctan { y / x ) + [sgn (y)]
tt
(343)
Once the shoulder angle # 2 and elbow angle #3 are known, the tool pitch angle
#4 can be computed from the global tool pitch angle #234. In order to isolate the
shoulder and elbow angles, we define the following two intermediate variables:
b\ = C1W1 + Siw2  Z4C234 + /5S234
(344)
b2 ~ d\ {Z4S234 ~ ds C234 ~ W3
(345)
Note that b\ and b2 are constants whose vales are known at this point because
#1 and #234 have already been determined. If we take the expressions for the components of w in Eq. (341) and substitute them in the expressions for b\ and b2y this
92
Chap. 3
yields:
b\ = a2C2 + 3C23
(346)
b2 =
(347)
122S2
4* 1Z3S23
We are now left with two independent expressions involving the shoulder and
elbow angles; the coupling with the tool pitch angle has been removed. The elbow
angle can be isolated by computing 2. Using trigonometric identities from Appendix 1 , we find, after some simplification, that:
 7 12 = a\ + IdidiC* + al
(348)
Thus the cosine of the elbow angle has been determined. If we now solve Eq.
(348) to recover #3 , this results in two possible Solutions, as illustrated previously
in Fig. 34. The first one is the elbowup solution, and the second one is the elbowdown solution:
43
= rceos M
 ai
a)
(3 4 9 )
In view of Eq. (349), the solution to the inverse kinematics problem for a
fiveaxis articulatedcoordinate robot is not unique. In most instances, we use the
elbowup solution, because this keeps the elbow joint further away from the work
surface. Indeed, for some commercial robots, this is the only solution allowed,
because of joint limit restrictions contained in Q.
(3410)
(3411)
Since the elbow angle q3 is already known, Eqs. (3410) and (3411) constitute a system of two simultaneous linear equations in the unknowns C2 and S2. If we
use row operations to solve this linear system, the result is:
_
v2
(a2+ a,C,)bi
li b\\2
_
(a2 + aiCi)b2  a 3S3>!
S2 =  r^ii
,,
.
(3413)
Since we have expressions for both the cosine and the sine of the shoulder an
gle, we can now recover the shoulder angle over the complete range [tt, 7r] using
the atan2 function. In particular:
q2 = atan2 [(a2 + a3 C3)>2 a 3S3?i, (a2 + a 3C3)?i f a 3 S3?2]
Sec. 34
(3414)
93
8
I
The work for extracting the tool pitch angle #4 is already in place. We know the
shoulder angle #2, the elbow angle #3, and the global tool pitch angle #234. Thus:
#4
#234 
#2 
(3415)
#3
+ w l ) /2
(3 4 1 6 )
R{q)
CiC234C5 + S 1S5
S1C234C5 ~ C 1S5
S234C5
C 1C234S5 + S1C5
S,C 234S5  C ,C 5
S234S5
C 1S234
 S , S 234
C234
(3 4 1 7 )
On the surface, it appears that the easiest way to recover #5 from R is to use the
last row of R , where R32A ^31) = S5/C 5. However, the ratio R31/R 32 is undefined
whenever the global tool pitch angle # 2 3 4 is a mltiple of t t . Rather than pursue this
approach further by decomposing it into special cases, we instead consider the first
two components of the normal vector r l. Taking Si times the first component and
subtracting Ci times the second component, the result is:
11
1*V21
(3 4 1 8 )
(3 4 1 9 )
Ci1R
A.22
Given both the cosine and the sine of the tool roll angle, we can now evalate
#5 over the complete range [ t t , t t ] using the atan2 function. In particular, we
have:
#5 atan2 (Si/?n
94
C 1R21 S 1R12
Ci/?22)
(3 4 2 0 )
Chap3
See. 34
(3421)
95
Here the tooltip position p is supplied and two parameters describing tool orienta
tion are added. The first is the global tool pitch angle q234 = qi + <73 + q4, which is
the tool pitch measured relative to the work surface or jcy plae. The second is the
tool roll angle q$. If the reduced toolconfiguration vector vv is used as input data,
then Algorithm 341 simplifies with #234 = vv4 and q$ = vv5.
For the fiveaxis articulated robot in Fig.
37, use Algorithm 341 to find q when the robot is in the following position,
called the home position. Does your answer agree with the last column of Table 24?
Exercise 341: Home Position.
use Algorithm 341 to find q when the robot is in the following position, called
the zero position:
w = [a2 + 03 + 04, 0, d\ ?5, 0, 0, l]r
For the fiveaxis articulated robot in Fig.
37, use Algorithm 341 to find q when the robot is in the following positions:
Exercise 343: Mximum Reach.
01C1
"
tfiSi
02S12
d\ ~ q^ ~ d4
(351)
w{q)
0
0
e x p { q j tt)
96
Chap. 3
Elbow
Vertical
extensin
Recall that C 1 2 is short for eos {q\ q2) and S 1 2 denotes sin (^1  q2). Note
from the last three components of w that the approach vector r 3 is fixed at r 3 =  t 3.
This is characteristic of SCARAtype robots in general, which always approach parts
directly fromabove. In order to extract the joint angle vector q from the nonlinear
toolconfiguration function w(q), we again apply row operations to the components
of w and exploit trigonometric identities from Appendix 1 in order to isolate the in
dividual joint variables.
<22
(352)
Thus the cosine of the elbow angle has been determined in terms of known constants. If we now solve Eq. (352) to recover q2, this results in two possible Solu
tions. The first one is called the lefthanded solution, and the second one is the
righthanded solution:
q2 = rceos
Sec. 35
w\
wi  a\  al
2a\a2
(353)
97
It follows from Eq. (353) that the solution to the inverse kinematics problem
for a fouraxis SCARA robot is not unique. The motivation for the terms lefthanded
and righthanded can be seen in Fig. 38. If the robot is viewed from above, then,
when q 2 > 0 , the robotic arm curls in from the left, whereas when q2 < 0, it curls
in from the right.
352 Base Joint
The base angle q i9 which might also be thought of as a horizontal shoulder angle,
can now be obtained from q2. First we take the expressions for
and w2 in Eq.
(351), and expand the C 12 and S i2 terms using the cosine of the difference and
sine of the difference trigonometric identities in Appendix 1, After rearranging the
coefficients, this yields:
(ai + a 2C2)Ci + (a2S2)Si = wi
(354)
(355)
Since the elbow angle qi is already known, this is a systemof two simultaneous lin
ear equations in the unknowns Ci and Si. If we use row operations to solve this lin
ear system, the result is:
c
n
,
{' ' )
=S + 2^ Wl ~ fl2S2W2
(a2S2)2 + (ax + a 2 C 2)2
,
{
.
)
Since we have expressions for both the cosine and the sine of the base angle, we can
again recover the base angle over the complete range [ 77*, 77] using the atan2 func
tion:
q\ = atan2 [a2S2wi + (a\ + a 2 C 2)w2, (ai + a2C2)wi a2S2w2]
(358)
(359)
(3510)
1
98
Chap. 3
C124
R(q)
S124
24
C l
24
0
0
(3511)
1
The angle <71 24 is called the global tool roll angle. It is the tool roll angle
measured relative to the base frame of the robot. Note from the normal vector in Eq.
(3511) that R21/R11 = S124/C124. Thus the global tool roll angle can be com
puted from R as follows:
<7 1 
2 4
= atan2
(R2
1,
/fu )
(3512)
Once the global tool roll angle is known, the tool roll angle <74 can be com
puted directly from it because q\ and <72 are also known:
<74
<71 * #2
<7124
(3513)
99
w = Cpi* Pi P3, # 13, # 23, # 33]r can be used to compute {#i, qi, #3} from Algorithm
351 and then # 4 can be computed from R using Eqs. (3512) and (3513). A
third possibility is to again use a reduced toolconfiguration vector vv as input to the
inverse kinematics algorithm. In the case of a fouraxis SCARA robot, the follow
ing vector in R4 is an example of a reduced toolconfiguration vector:
vv = [ p u p 2 i P3, q i 2  4]T
(3514)
Here the tooltip position p is supplied and one additional parameter describing tool
orientation is added. It is the global tool roll angle # 1  2 4 which is the tool roll mea
sured relative to the base frame. When vv4 = 0 , the jc axes of the tool and the base
point in the same direction and positive vales for vv4 correspond to counterclockwise tool rotations as seen from above. If the reduced toolconfiguration vector is
used as input data, Algorithm 351 can be used to compute {q 1, #2, #3}, and #4 is
w = [0, 0, 0, 0, 0,  l ] r
For the fouraxis SCARA robot, find the
joint variables # when the radial extensin of the arm is mximum, as follows:
Exercise 352: M ximum Reach.
vv = [ai + a2 , 0, 0, 0, 0, l]r
C344 +
C34Z4 +
S 345*4 ) +
S 345*4) ~
S i( S 3 f l3 +
C j (S3Z3 +
S34CZ4 C345 d)
S344 C 3454 )
di + S2(C3fi3 + C344 + S ^ d t )
(361)
w(q)
[e x p ( # 6 / ,w)]("S i C 3 4 5 + C 1C 2 S 3 4 5 )
[e x p (# 6 / f r ) ] ( C i C 345 + S 1C 2S 34 5 )
[e x p (# 6 / ' 7r ) ] S 2 S 345
Recall that the notation Cljk is short for eos (#, f q + #*) and, similarly, S,>*
100
Chap. 3
Figure 39
denotes sin (#, + q + qk). We also make use of the expression for the rotation ma
trix R (q) developed in Prop. 291:
R =
( C 1 C 2 C 345 + S iS 345)C 6 + C 1 S2 S6
(S 1 C 2 C 345 ~ C j 8345)06 + S 1 S2 S6
S 2 C 345C 6 C 2 S6
C 2C 6 S2 C 345S6
1
1
S 1C 345 + C 1 C 2 S345
C 1 C 345 + S 1 C 2 S 345
828345
(362)
In order to extract the joint variable vector q from the toolconfiguration vector
w(q) and the rotation matrix R{q), we apply row operations to vv and R and exploit a
host of trigonometric identities in order to isolate the individual joint variables.
361 Tool Roll Joint
The Intelledex 660 robot is quite complex, and in order to develop closedform ex
pressions for the joint variables, we assume that the tool configuration is available
both in the form of the toolconfiguration vector vv and the translation and rotation
pair {p, /?}. From Exercise 331, the tool roll angle q6 can be recovered from the
last three components of the toolconfiguration vector vv as follows:
#6 = tt ln (vv4 + vv2 + w l ) 1/2
(363)
Sec. 36
101
Cl
+ S  = 1, is simply C2 . Consequently, we
(364)
Clearly, the solution is not unique. Recall from Table 26 that in the home po
sition, q2 = 7 t / 2 . We therefore choose the negative solution and restrict our attention to angles in the range
7r < q2 < 0
(365)
The solution corresponding to q2 < 0 is called the lefthanded solution, because, from Fig. 226, it corresponds to the upper arm being on the left side of the
small shoulder and forearm. If the shoulder is rolled by t t to produce angles in the
range 0 < q2 < t t , this places the arm in the righthanded configuration. The end
points {7T, 0} are not included in Eq. (365), for reasons which will become apparent as we proceed.
(3 66)
Recall from Eqs. (364) and (365) that S2 is a known nonzero quantity. But
q6 is also known, from Eq. (363), and henee we have isolated Ci. To recover the
base angle over the entire range [ 7r, 7r], we must also isolate Si. This can be
achieved with the second components of the normal and sliding vector. Again using
the trigonometric identity S i + C l = 1, we find:
R21 S "1" R22C6 == S1S2
(3 67)
If we now divide Eq. (367) by Eq. (366), the nonzero factor S2 caneis
and we are left with an expression for S 1 /C 1 . Thus the base angle qx over the range
[ 7r, 7r] is:
(368)
b = p dr3 d \i3
102
(369)
Chap. 3
I
1
The first two components of the intermediate variable b specify the x and y co
ordinates of the wrist ffame L4 relative to the base ffame Lo. The last component of b
specifies the vertical distance from the wrist frame L4 to the shoulder ffame L2. If we
substitute for p and r 3 in Eq. (369), we get the following expressions for the com
ponents of b:
b\ = CiC2(C3a 3 + C344) + Si(S3tf3 + 834^4)
(3610)
(3611)
bi = S2(C303 + C34fl4)
(3612)
Note how the effects of the tool pitch angle qs have been removed from the
original expression for p. The quantity  b  represents the straightline distance from
the shoulder joint to the wrist joint. Because the elbow joint is the only joint be
tween the shoulder and the wrist, > should depend exclusively on q4. If we com
pute \\b\\2,the result after simplification using various trigonometric identities in
Appendix 1 is:
\\b 2
+ 2a3tf4C4 + al
(3613)
Thus ? depends exclusively on q4y as expected. Solving Eq. (3613) for q4 then
yields:
g4 = arccos M
. 7. ? L ~ *
2Z34
(3614)
Again we see that the solution is not unique. The positive solution returns
angles in the range [0 , ir] and is referred to as the elbowdown solution, while the
negative solution, which produces angles in the range [  7r, 0], is the called the el
bowup solution.
365 Shoulder Pitch Joint
The shoulder pitch angle q3 is also a challenge to extract. The basic approach is to
find two independent linear equations in S3 and C3. These can be obtained from the
expressions for b now that the angles q i9 q2, and q4 are known. First, note from Eqs.
(3610) and (3611) that S1&1 C ib2 S3a3 + S34a4 If we use the sine of the
sum trigonometric identity for S34 and rearrange the coefficients, this yields:
Sibi C\b 2 (3 + C4a4)S3 + (S4a4)C3
(3615)
To produce a second equation involving the unknowns S3 and C3, we note ffom
Eq. (3612) that &3/S 2 = C33 +
If we use the cosine of the sum trigono
metric identity for C m and rearrange the coefficients, the result is:
S'2
(3616)
Note that the expression in Eq. (3616) involves divisin by S2. This is possible because S2 # 0 in view of Eq. (365). We can now solve Eqs. (3615) and (3616) simultaneously using row operations, and the result is:
Sec. 36
103
c, =
S 4 u (S j6 i C i bz)
(ay + C U a J b s / S i
S3
(3617)
(3618]
Given the expressions for C3 and S3, the atan2 function can now be used to
solve for the shoulder pitch angle over the entire range [ tt. tt]. This yields:
#3 = atan2
(ay
+ C4a4)(Si/?i  C i62)
S44b}
S2
S4fl4(S,> C lW +
(a3 + C4a4)^3
S
(3619)
(3 6 2 0 )
The work for extracting the final joint variable is now in place because qy, #4,
and #345 are all known. Recalling that #345 = #3 + #4 + #5, it follows that:
#5
q
345
q
y~
#4
(3 6 2 1 )
/? =
104
0
0
1
0 f
1 0
0 0
Chap. 3
Figure 310
lated robot.
Sec. 37
105
of the DH algorithm to the threeaxis planar articulated robot yields the linkcoordinate diagram shown in Fig. 311.
y3i
\
Tool
Next, the application of steps 8 to 13 of the DH algorithm results in the kine
matic parameters shown in Table 32.
TABLE 32 KINEMATIC PARAMETERS
OF A THREEAXIS PLANAR ARTICULATED
ROBOT
A xis
1
2
3
<l\
0
0
a\
a2
0
7r/3
7f/3
0
<72
<73
d*
Home
c,
s,
0
0
Cn
Sl2
0
0
Cl23
S23
0
0
106
0 ,C,: c 2  s
C, 0 tfiSi s 2 c 2
0 0
0 1 0
0 0 1 .0
0
 s 12 0 0,C1+ Cl2C\2
Cj2 0 (3iSi + a 2 S12
0
0 1
1
0 0
Si23 0 ajCi + CliCn
C,23 0 OiSi + C12 S 12
0 1
1
0 0
s,
0 02 C2
0 02S2
1 0
1
0
c3 s 3
s3 c 3
0
0
0
0
c3
s3
0
.0
S i
Cl
0
0
0 0
0 0
1 di
0 1
0 0'
0 0
1 di
0 1
(371)
Chap. 3
Exercise 371: Planar Axis Manipulator. Use the pattern in 7o, 7o, and
7o to write down, by inspection, the arm matrix of an axis planar articulated
robot, where n is arbitrary.
At this point we combine the tooltip position and tool orientation into the
more compact toolconfiguration vector w. Although the toolconfiguration space
could be regarded as threedimensional in this case, to faciltate comparisons with
other robots we use the general case w (vv1, vv2), where w 1 = p and
w2 = [exp ( q i/ir ^ r 3. From inspection of the arm matrix in Eq. (371), we see that
the toolconfiguration function of the threeaxis planar articulated robot is:
i C\
4 Z2C12
tfiSi 4 22S12
di
(372)
w(<?) =
0
0
exp f a / n )
The planar nature of the threeaxis robot is evident ffom Eq. (372), since
W<?), W4(<?), and w5(q) are all constant. If the tool length d$ is zero, then the tool tip
moves in the jcy plae. More generally, the tool tip p moves in a plae parallel to
the xy plae at a distance dj above it.
In order to extract the joint angle vector q from the nonlinear toolconfiguration function w(q), we again apply row operations to the components of w
and exploit trigonometric identities from Appendix 1 in order to isolate the individ
ual joint variables.
= a \ C \ + l a xa 2C i C n + 02 C2 + u?S2 + 2 a i a 2 S \ S n 4 a l S h
= a j + 2zi^(CnCi
S12S1) 4 * 2
ai + 2z]Q2 C2 + a\
(373)
We can now solve Eq. (373) for the shoulder angle. Isolating C2 and applying the
rceos function to both sides yields the follow ing two Solutions:
q2 = rceo s
w2, + wl  a 2,  a 2
2 i 2
^
(374)
As was the case with the SCARA robot, the first solution is called the lefthanded
solution and the second is the righthanded solution.
Sec. 37
107
(375a)
(375b)
Thus we have two linear equations in the unknowns Ci and Si. These equations can
be solved simultaneously using elementary row operations, and the result is:
Ci =
(a\ +
r tf2C2)wi
u2K^2jyv\ ~r
+ au2a2w2
2 S 2 w2
;7 ^ , ,7  7
(ai + a 2 C2) 2 + (a2 S2):
_
(fifi + a2 C2)w2  a2 S 2 w\
Si
7
n \2 * / c~vT
(i I a 2 C2) + (a 2s 2y
(376a)
t
.
(376b)
Note that if a 2 a\, then the numerators and the denominators of the expressions
for Ciand Sigo to zero when q2 = tt. This corresponds to a workspace singularity
with thetool roll axiscollinear with the base axis. If we avoid this singularity, then
both Ci and Si are known and the base angle q\ can be recovered over the complete
range [ t t , t t ] using:
q\ = atan2 [(ai + a2 C2)w2 a2 S 2 w\, (a\ + a 2 C2)w 1 + z2S2w2]
(377)
tt
ln vv6
(378)
The complete inverse kinematics solution for the threeaxis planar articulated robot
in Fig. 311 is summarized in Algorithm 371. Recall that the two expressions for
q 2 specified by the plus and minus signs represent the lefthanded and righthanded
Solutions, respectively.
The solution to the inverse kinematics problem outlined in Algorithm 371
assumes that the toolconfiguration vector vv is supplied as input data. If instead the
pair {/?, i?} is specified, then w = [ p x, p u p 3, R i3, R23, R33]T can be used to com
pute qx and q 2 using Algorithm 371. From Eq. (372), the tool roll angle can then
be computed from the global tool roll angle <7123, as follows:
108
q 3 = atan2 (Ru , R 2 1)  q\ ~ qi
(379)
Chap. 3
2d^3^
I
q3 = ir In w6
Exercise 372: Right Angle. For the threeaxis planar articulated robot in
Fig. 311, find the joint variables q when the first two links form a right angle, as
follows:
w = [a2, ai, <3, 0, 0, l]r
38 A ROBOTIC WORK CELL
The technique for transforming between tool coordinates and base coordinates using
homogeneous coordinate transformation matrices can also be applied more generally
to coordinate transformations between various stations in a robotic work cell. As an
illustration, consider the singlerobot work cell shown in Fig. 312. Here a part in
the shape of a block has been extracted, say, from a feeder, and placed in the viewing area of an overhead camera. The visin system inspeets the part and then informs the robot controller to pick up the part and place it in either a pass bin or a
reject bin, depending upon the outeome of the inspection process.
Base
xb
Figure 312
Sec. 38
In order to perform a kinematic analysis of this robotic work cell, let TISU
represent the position and orientation of the part as seen by the camera. For ex
ampie:
y p art
*
cam era
1
5
1
19
(381)
Next let TcSera represent the position and orientation of the base of the robot as
seen by the camera. In this case suppose that:
base
7camera
__
1
15
25
1
20
(382)
Consider the problem of finding the position and orientation of the part rela
tive to the base of the robot. Using homogeneous coordinate transformation ma
trices, recalling that diagonal identifiers cancel, and using Prop. 241, we have:
'T c a m e r a 'T p a r t
n art
ase
* base
* camera
( TTbase \ 1 'Tpart
V* camera/
camera
i
0
1
15'
1
1
25
1
5
1
20
1
19
1
25'
1
1
15
5
1
20
1
19
1 o o
0 1 o
o o 1
0
F
i
o'
30
15
1
(383)
Thus the position of the part relative to the base ffame of the robot is
p = [30, 15, l]7. Furthermore, the three axes of the part frame point in the same
directions as the corresponding axes of the base ffame, as is indicated by the fact
that the rotation submatrix is R /. This is consistent with the picture in Fig. 312.
Next suppose we want to have the robot reach down and pick up the part di
rectly ffom above by grasping it on the front and back faces. To determine the re
110
Chap. 3
quired rotation matrix R  [x\ y \ z'], first note from Fig. 312 that to pick up the
part from above we need an approach vector which points down. In terms of base
coordinates, this corresponds to r 3 = i3. Next, to grip the object on the front and
back faces, we need a sliding vector of r 2 = i2. Finally, to complete the righthanded orthonormal coordinate frame, it is necessary that r l = T / 1. Thus there are
two possible Solutions for the rotation matrix R, one being:
1
R = 0
0
0
1
0
0
0
1
(384)
We can now put the position and orientation information together to develop
the tool configuration needed to grasp the part. In particular, the arm matrix which
places the tool in the position p and orientation R to pick up the part is:
ynool
base
(<?) =
1
0
0
0
1
0
0
0
1
30
15
1
(385)

(386)
This is consistent with Fig. 37, where the robot is pictured in the soft home posi
tion, which corresponds to q [0, t t / 2 , t t / 2 , 0, t t / 2 ] t . From Def. 331, the
toolconfiguration vector w(q) consists of the position vector p augmented by the
approach vector r 3 scaled by exp (^s/tt). Thus, in this case, the tool configuration
needed to grasp the block from above on the front and back faces is:
w = [30, 15, 1, 0, 0, 1.129]r
(387)
To actually command the robot to reach down and pick up the part, we now
need to know the valu of the joint variables q that will produce the configuration of
the tool specified in Eq. (387). At this point, Algorithm 341 would be used to
determine the vector of joint variables q for the fiveaxis articulated robot.
Exercise 381: Focus Camera. Find the arm matrix needed to have the
robot in Fig. 312 reach up and focus the camera by twisting the lens using tool roll
motion.
Sec. 38
111
39 PROBLEMS
31. Suppose a sixaxis articulated robot has the following limits on the range of travel of its
joint variables:
37r/4
IJ qi <
tt/4 <
ir /2 ^
3 ir /4
< 37r/4
q2 + qi ^
tt ^
q 6 < tt
Find the joint limit vectors {^min, ^ m*x} and the joint coupling matrix C that character
ize the jointspace work envelope Q of this robot.
32. Suppose the tool of a fiveaxis articulated robot is put into a singular configuration in
which the tool roll axis is collinear with the base axis as shown in Fig. 3 1 3 . Show
that there are an infinite number of Solutions to the inverse kinematics problem for this
particular tool configuration. Is this a kinematically redundant robot?
Figure 313
A singular tool
configuration.
33. Consider the linkcoordinate diagram for the fouraxis cylindricalcoordinate robot
with tool roll motion shown in Fig. 314. Here the vector of joint variables is
q = [0i, 2, 3, 04]r , and the kinematic parameters are as specified in Table 33. Find
the arm matrix 7bl(</) for this cylindricalcoordinate robot.
34. Find the toolconfiguration vector w{q) for the cylindricalcoordinate robot in Fig.
314.
35. Solve the inverse kinematics equations for the cylindricalcoordinate robot in Fig.
314. Do parts (a) through (d) in whichever order is easiest.
(a) Find the base angle #i(w).
(b) Find the vertical extensin qi(w).
(c) Find the radial extensin q^w).
(d) Find the tool roll angle #4(w).
36. Find a reduced toolconfiguration vector vv for the cylindricalcoordinate robot in Fig.
314.
37. For the cylindricalcoordinate robot in Fig. 314, use the rotation matrix R{q) to find
the tool roll angle <?4.
112
Chap. 3
3 3
KINEMATIC PARAMETERS
Axis
1
2
3
4
Home
tt / 2
0
0
<?2
43
4
0
0
0
0
0
7r/2
tt/2
0
0
d
r
TT
38. Consider the linkcoordinate diagram for the fiveaxis sphericalcoordinate robot with
tool pitch and tool roll motion shown in Fig. 315. Here the vector of joint variables is
q [0i, 02, d3, 04, 05]r , and the kinematic parameters are as specified in Table 34.
Find the arm matrix 7bae(#) for this sphericalcoordinate robot.
39. Find the toolconfiguration vector w(q) for the sphericalcoordinate robot in Fig. 315.
310. Solve the inverse kinematics equations for the sphericalcoordinate robot in Fig. 315.
Do parts (a) through (e) in whichever order is easiest.
(a) Find the base angle qi(w).
(b) Find the shoulder angle qiiyv).
(c) Find the radial extensin q3{w).
(d) Find the tool pitch angle qi(w).
(e) Find the tool roll angle qM311. Find a reduced toolconfiguration vector w for the sphericalcoordinate robot in Fig.
315.
312. For the sphericalcoordinate robot in Fig. 315, use the rotation matrix R(q) to find the
tool roll angle q5.
313. Recall from Chap. 2 that the composite yawpitchroll transformation can be repre
sented by the following rotation matrix YPR. Solve for the yaw, pitch, and roll angles
0 G R3 in terms of the components of YPR.
Sec. 39
Problems
113
YPR (0) =
C2C3
S .S jC j 
C ,S 3
C ,S 2C3 +
C2S3
S,S2S3 +
C ,C j
C ,S 2S3
S2
Figure 315
SIC2
S ,S 3'
 s ,c 3
C1C2
TABLE 34
KINEMATIC PARAMETERS
Axis
<7i
dx
2
3
4
5
<?2
77
<74
?5
0
?3
0
ds
0
0
0
0
0
77/2
7t/2
77/2
77/2
Home
0
r
77/2
77/2
tt/
314. Suppose the jointspace work envelope Q for the fouraxis cylindrical robot is of the
following form. Show that there are two Solutions to the inverse kinematics problem in
this case by completing Table 35. The second solution should be expressed in terms of
the first solution. For example, a joint variable might be negated, or 7r might be added
to a joint variable.
T T < q x <TT
H < <73 < H
0 < q2 < V
7r < <y4 < 7r
TABLE 35
MULTIPLE SOLUTIONS
Solution
1
2
114
Base
Vertical
Extensin
Radial
Extensin
Tool Roll
<2
<?3
44
Chap. 3
315. Suppose the jointspace work envelope Q for the fiveaxis spherical robot is of the fol
lowing form. Show that there are two Solutions to the inverse kinematics problem in
this case by completing Table 36. To simplify the problem somewhat, you can assume
that q2 = 0 corresponds to the arm pointing straight up.
TT <
q x <
TT ^
7T
TT
0 < q3 < H
TT <
TT <
qs
<
TT
<
7T
TABLE 36
MULTIPLE SO LUTIO NS
OF THE FIVEAXIS SPHERICAL ROBOT
Solution
1
2
Base
Shoulder
Radial
Extensin
Tool
Pitch
Tool
Roll
<72
<73
<?4
<75
REFERENCES
D. L. (1968). The kinematics of manipulators under Computer control, AIM 72,
Stanford AI Laboratory, Stanford Univ.
U i c k e r , J. J., J. D e n a v it , and R. S. H a k t en b er g (1964). An iterative method for the dis
placement analysis of spatial mechanisms, Trans. ASME J. Appl. Mech., Vol. 31, Ser. E,
pp. 309314.
W h it n e y , D. E. (1972). The mathematics of coordinated control of prosthetic arms and ma
nipulators, J. Dynamic Systems, Measurement and Control, Vol. 122, pp. 303309.
P ie p e r ,
115
Sec. 39
Problems