Handbook of Robotics

Chapter 1: Kinematics

Ken Waldron Jim Schmiedeler

Department of Mechanical Engineering Department of Mechanical Engineering
Stanford University The Ohio State University
Stanford, CA 94305, USA Columbus, OH 43210, USA

September 15, 2005

Contents

1 Kinematics 1
1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Position and Orientation Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.1 Representation of Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.2 Representation of Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
Rotation Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
Fixed Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
Angle-Axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2.3 Homogeneous Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2.4 Screw Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2.5 Plücker Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.3 Joint Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.3.1 Revolute . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3.2 Prismatic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3.3 Helical . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.3.4 Cylindrical . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.3.5 Spherical . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.3.6 Planar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.7 Universal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.8 Rolling Contact . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.9 Holonomic and Non-Holonomic Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.10 Generalized Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.4 Workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.5 Geometric Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.6 Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.7 Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.7.1 Closed-Form Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
Algebraic Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
Geometric Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.7.2 Numerical Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
Symbolic Elimination Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
Continuation Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
Iterative Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.8 Forward Instantaneous Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.8.1 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1.9 Inverse Instantaneous Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

i
CONTENTS ii

1.9.1 Inverse Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

1.10 Static Wrench Transmission . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
1.11 Conclusions and Further Reading . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
List of Figures

1.1 place holder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

1.2 Example Six-Degree-of-Freedom Serial Chain Manipulator. . . . . . . . . . . . . . . . . . . . . . . . 11

iii
List of Tables

1.1 Equivalent rotation matrices for various representations of orientation. . . . . . . . . . . . . . . . . . 3

1.2 Conversions from a rotation matrix to various representations of orientation. . . . . . . . . . . . . . 4
1.3 Conversions from angle-axis to unit quaternion representations of orientation and vice versa. . . . . 4
1.4 Conversions from a screw transformation to a homogeneous transformation and vice versa. . . . . . 6
1.5 Tabulated Geometric Parameters for Example Serial Chain Manipulator. . . . . . . . . . . . . . . . . 11
1.6 Forward Kinematics of the Example Serial Chain Manipulator in Figure 1.2. . . . . . . . . . . . . . 12
1.7 Inverse Position Kinematics of the Articulated Arm Within the Example Serial Chain Manipulator
in Figure 1.2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.8 Inverse Orientation Kinematics of the Spherical Wrist Within the Example Serial Chain Manipulator
in Figure 1.2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.9 Forward Kinematics of the Example Serial Chain Manipulator in Figure 1.2. . . . . . . . . . . . . . 15

iv
Chapter 1

Kinematics

Kinematics is the geometry of motion, which is to say from the specified values of the manipulator’s joint vari-
geometry with the addition of the dimension of time. ables. Inverse kinematics is the determination of the
values of the manipulator’s joint variables required to
It pertains to the position, velocity, acceleration, and all
higher order derivatives of the position of a body in spacelocate the end-effector in space with a specified position
without regard to the forces that cause the motion of the and orientation. The problems of forward and inverse
body. Since robotic mechanisms are by their very essence instantaneous kinematics are analogous, but address
designed for motion, kinematics is the most fundamental velocities rather than positions. While kinematics does
aspect of robot design, analysis, control, and simulation. not consider the forces that generate motion, kinematic
Unless explicitly stated otherwise, the kinematic de- velocity analysis is closely related to static wrench analy-
scription of robotic mechanisms typically employs a num- sis. The Jacobian that maps the joint velocities to the
ber of idealizations. The links that compose the robotic end-effector velocity also maps the end-effector’s trans-
mechanism are assumed to be perfectly rigid bodies hav- mitted static wrench to the joint forces/torques that
ing surfaces that are geometrically perfect in both posi- produce the wrench.
tion and shape. Accordingly, these rigid bodies are con- The goal of this chapter is to provide the reader with
nected together at joints where their idealized surfaces an overview of different approaches to representing the
are in complete contact without any clearance between position and orientation of a body in space and algo-
them. The respective geometries of these surfaces in con- rithms to use these approaches in solving the problems
tact determine the freedom of motion between the two of forward kinematics, inverse kinematics, and static
links, or the joint kinematics. In an actual robotic wrench transmission for robotic manipulators.
mechanism, these joints will have some physical limits
beyond which motion is prohibited. The workspace of
a robotic manipulator is determined by considering the
1.1 Introduction
combined limits and freedom of motion of all of the joints It is necessary to treat problems of the description and
within the mechanism. displacement of systems of rigid bodies since robotic
A common task for a robotic manipulator is to locate mechanisms are systems of rigid bodies connected by
its end-effector in a specific position and with a specific kinematic joints. Among the many possible topologies
orientation within its workspace. Therefore, it is critical in which systems of bodies can be connected, two are of
to have a representation of the position and orientation of particular importance in robotics: serial chains and fully
a body, such as an end-effector, in space. While there are parallel mechanisms. A serial chain is a system of rigid
in principle an infinite number of ways to describe the po- bodies in which each member is connected to two others,
sition and orientation of a body in space, this chapter will except for the first and last members that are each con-
provide an overview of the representations that are par- nected to only one other member. A fully parallel mech-
ticularly convenient for the kinematic analysis of robotic anism is one in which there are two members that are
mechanisms. Regardless of the selected representation, connected together by multiple joints. In practice, each
two fundamental kinematic problems arise for robotic “joint” is often itself a serial chain. This chapter focuses
manipulators. Forward kinematics is the determina- almost exclusively on serial chains. Parallel mechanisms
tion of the position and orientation of the end-effector are dealt with in more detail in Chapter 13 Kinematic

1
CHAPTER 1. KINEMATICS 2

Structure and Analysis. the projections of the vector j pi onto the corresponding
Obviously, the draft of this chapter in its current form axes. The vector components could also be expressed
is incomplete. It is missing figures and tables. It is short as the spherical or cylindrical coordinates of Oi in the j
on references in many places. The later sections are par- frame. Such representations have advantages for analysis
ticularly inadequate in their present form. This Intro- of robotic mechanisms including spherical and cylindrical
duction section itself is essentially yet to be written. joints.
A translation is a displacement in which no point in the
rigid body remains in its initial position and all straight
1.2 Position and Orientation lines in the rigid body remain parallel to their initial ori-
Representation entations. The translation of a body in space can be
represented by the combination of its position prior to
Spatial, rigid body kinematics can be viewed as a com- the translation and its position following the translation.
parative study of different ways of representing the po- Conversely, the position of a body can be represented as a
sition and orientation of a body in space. Translations translation that takes the body from a position in which
and rotations, referred to in combination as rigid body the coordinate frame fixed to the body coincides with the
displacements, are also expressed with these representa- fixed coordinate frame to the current position in which
tions. No one approach is optimal for all purposes, but the two fames are not coincident. Thus, any representa-
the advantages of each can be leveraged appropriately to tion of position can be used to create a representation of
facilitate the solution of different problems. displacement, and vice-versa.
The minimum number of coordinates required to lo-
cate a body in Euclidean space is six: three for position 1.2.2 Representation of Orientation
and three for orientation. Many representations of spa-
tial position and orientation employ sets with superabun- There is significantly greater breadth in the representa-
dant coordinates in which auxiliary relationships exist tion of orientation than in that of position. This section
among the coordinates. The number of independent aux- does not include an exhaustive summary, but focuses on
iliary relationships is the difference between the number the representations most commonly applied to robotic
of coordinates in the set and six. manipulators.
This chapter and those that follow it make frequent A rotation is a displacement in which at least one point
use of “coordinate reference frames” or simply “frames”. of the rigid body remains in its initial position and not
A coordinate reference frame i consists of an origin, de- all lines in the body remain parallel to their initial ori-
noted Oi , and a triad of mutually orthogonal basis vec- entations. The points and lines addressed here and in
tors, denoted [x̂i ŷ i ẑ i ], that are all fixed within a par- the definition of translation above are not necessarily
ticular body. The position and orientation of a body contained within the boundaries of the finite rigid body.
in space will always be expressed relative to some other Rather, any point or line in space can be taken to be
body, so they can be expressed as the position and orien- rigidly fixed in a body. For example, a body in a cir-
tation of one coordinate frame relative to another. Sim- cular orbit rotates about an axis through the center of
ilarly, rigid body displacements can be expressed as dis- its circular path, and every point on the axis of rotation
placements between two coordinate frames, one of which is a point in the body that remains in its initial posi-
may be referred to as “moving”, while the other may be tion despite not being physically within the boundaries
referred to as “fixed”. This simply indicates that the ob- of the body. As in the case of position and translation,
server is located in a stationary position within the fixed any representation of orientation can be used to create a
reference frame, not that there exists any absolutely fixed representation of rotation, and vice-versa.
frame.
Rotation Matrices
1.2.1 Representation of Position
The orientation of coordinate frame i relative to coordi-
The position of the origin of coordinate frame i relative nate frame j can be denoted by expressing the! basis vec- "
to coordinate frame j can be denoted by the 3 × 1 vector tors [x̂i ŷ i ẑ i ] in terms of the basis vectors x̂j ŷ j ẑ j .
! "T ! "
j
pi = jpxi jpyi jpzi . The components of this vector are This yields j x̂i j ŷ i j ẑ i , which when written together
the Cartesian coordinates of Oi in the j frame, which are as a 3 × 3 matrix is known as the rotation matrix. The
CHAPTER 1. KINEMATICS 3

components of jRi are the dot products of basis vectors Z-Y-X Euler Angles [α, β, γ]:
of the two coordinate frames.  
    cα cβ cα sβ sγ − sα cγ cα sβ cγ + sα sγ
r11 r12 r13 x̂i · x̂j ŷ i · x̂j ẑ i · x̂j sα cβ sα sβ sγ + cα cγ sα sβ cγ − cα sγ 
j
Ri = r21 r22 r23  = x̂i · ŷ j ŷ i · ŷ j ẑ i · ŷ j  −sβ cβ sγ cβ cγ
r31 r32 r33 x̂i · ẑ j ŷ i · ẑ j ẑ i · ẑ j
(1.1)
X-Y-Z Fixed Angles [ψ, θ, φ]:
Because the basis vectors are unit vectors and the dot
product of any two unit vectors is the cosine of the angle  
cφ cθ cφ sθ sψ − sφ cψ cφ sθ cψ + sφ sψ
between them, the components are commonly referred sφ cθ sφ sθ sψ + cφ cψ sφ sθ cψ − cφ sψ 
to as direction cosines. −sθ cθ sψ cθ cψ
The rotation matrix jRi contains nine elements, while
only three parameters are required to define the orienta-
tion of a body in space. Therefore, six auxiliary relation- Angle-Axis θK̂ (where vθ = 1 − cθ ):
ships exist between the elements of the matrix. Because  
the basis vectors of coordinate frame i are mutually or- kx2 vθ + cθ kx ky vθ − kz sθ kx kz vθ + ky sθ
kx ky vθ + kz sθ ky2 vθ + cθ ky kz vθ − kx sθ 
thonormal, as are the basis vectors of coordinate frame j,
the columns of jRi formed from the dot products of these kx kz vθ − ky sθ ky kz vθ + kx sθ kz2 vθ + cθ
vectors are also mutually orthonormal. A matrix com-
posed of mutually orthonormal vectors is known as an Unit Quaternions [q0 , q1 , q2 , q3 ]:
orthogonal matrix and has the property that its inverse  
is simply its transpose. This property provides the six 1 − 2(q22 + q32 ) 2(q1 q2 − q0 q3 ) 2(q1 q3 + q0 q2 )
auxiliary relationships. Three require the column vectors 2(q1 q2 + q0 q3 ) 1 − 2(q12 + q32 ) 2(q2 q3 − q0 q1 )
to have unit length, and three require the column vectors 2(q1 q3 − q0 q2 ) 2(q2 q3 + q0 q1 ) 1 − 2(q12 + q22 )
to be mutually orthogonal. Alternatively, the orthogo-
nality of the rotation matrix can be seen by considering Table 1.1: Equivalent rotation matrices for various rep-
the frames in reverse order. The orientation of coordi- resentations of orientation.
nate frame j relative to coordinate frame i is the rotation
matrix iRj whose rows are clearly the columns of the ma-
the rotations must accompany the three angles to de-
trix jRi . Rotation matrices are combined through simple
fine the orientation. For example, the symbols [α, β, γ]
matrix multiplication such that the orientation of frame
are used throughout this handbook to indicate Z-Y-X
i relative to frame k can be expressed as kRi = kRj jRi .
Euler angles. α is the rotation about the ẑ axis, β is the
In summary, jRi is the rotation matrix that transforms
rotation about the rotated ŷ axis, and finally, γ is the
a vector expressed in coordinate frame i to a vector ex-
rotation about the twice rotated x̂ axis. Z-Y-Z Euler an-
pressed in coordinate frame j. It provides a representa-
gles are another commonly used convention from among
tion of the orientation of frame i relative to j and thus,
the 12 different possible orders of rotations.
can be a representation of rotation from frame i to frame
j. Table 1.1 lists the equivalent rotation matrices for the
other representations of orientation listed in this section. Fixed Angles
Table 1.2 contains the conversions from a known rotation
matrix to these other representations. A vector of three angles can also denote the orientation
of coordinate frame i relative to coordinate frame j when
each angle represents a rotation about an axis of a fixed
Euler Angles
reference frame. Appropriately, such angles are referred
For a minimal representation, the orientation of coordi- to as Fixed Angles, and the order of the rotations must
nate frame i relative to coordinate frame j can be de- again accompany the angles to define the orientation. X-
noted as a vector of three angles [α, β, γ]. These angles Y-Z Fixed Angles, denoted here as [ψ, θ, φ], are a com-
are known as Euler angles when each represents a rota- mon convention from among the, again, 12 different pos-
tion about an axis of a moving coordinate frame. In this sible orders of rotations. ψ is the yaw rotation about the
way, the location of the axis of each successive rotation fixed x̂ axis, θ is the pitch rotation about the fixed ŷ axis,
depends upon the preceding rotation(s), so the order of and φ is the roll rotation about the fixed ẑ axis. As can
CHAPTER 1. KINEMATICS 4

Z-Y-X Euler Angles [α, β, γ]: Angle-Axis to Unit Quaternions:

' ( )
β = Atan2 −r31 , r11 2 + r2 q0 = cos θ2
21
' ) q1 = kx sin θ2
α = Atan2 cos β , cos β
r21 r11
q2 = ky sin θ2
' )
γ = Atan2 cos r32 r33 q3 = kz sin θ2
β , cos β

X-Y-Z Fixed Angles [ψ, θ, φ]: Quaternions to Angle-Axis:

' ( ) θ = 2 cos−1 q0
θ = Atan2 −r31 , r11 2 + r2
21 q1
kx = sin
' ) θ
2
ψ = Atan2 cθ , cθ
r21 r11
ky = q2
sin θ2
' ) q3
φ = Atan2 rc32 , r33 kz = sin θ2
θ cθ

Angle-Axis θK̂: Table 1.3: Conversions from angle-axis to unit quater-

* r +r +r −1 + nion representations of orientation and vice versa.
θ = cos −1 11

22
2
33

r32 − r23
K̂ = 2 sin
1 r13 − r31 
θ because it contains four parameters. The auxiliary rela-
r21 − r21
tionship that resolves this is the unit magnitude of vector
Unit Quaternions [q0 , q1 , q2 , q3 ]: K̂. Even with this auxiliary relationship, the angle-axis
√ representation is not unique because rotation through
q0 = 12 1 + r11 + r22 + r33 an angle of −θ about −K̂ is equivalent to a rotation
q1 = r324q −r23
0
through θ about K̂. Table 1.3 contains the conversions
q2 = 4q0r13 −r31 from angle-axis representation to unit quaternions and
q3 = r214q −r12 vice versa. The conversions from these two representa-
0
tions to Euler angles or fixed angles can be easily found
by using the conversions in Table 1.2 in conjunction with
Table 1.2: Conversions from a rotation matrix to various the equivalent rotation matrices in Table 1.1.
representations of orientation.

Quaternions
be seen by comparing the respective equivalent rotation
matrices in Table 1.1 and the respective conversions in Quaternions provide a representation of the orientation
Table 1.2, a set of X-Y-Z Fixed Angles is exactly equiva- of one coordinate frame relative to another that in some
lent to the same set of Z-Y-X Euler Angles (α = φ, β = θ, ways corresponds to complex numbers. A quaternion q
and γ = ψ). This result holds in general such that three is defined to have the form, q = q0 + q1 i + q2 j + q3 k,
rotations about the three axes of a fixed frame define the where q0 , q1 , q2 , and q3 are scalars, sometimes referred
same orientation as the same three rotations taken in the to as Euler parameters, and i, j, and k are √ operators
opposite order about the three axes of a moving frame. somewhat analogous to the operator i = −1 used in
complex numbers. The operators are defined to satisfy
the following combinatory rules: ii = jj = kk = −1,
Angle-Axis
ij = k, jk = i, ki = j, ji = −k, kj = −i, and
A single angle θ in combination with a unit vector K̂ can ik = −j. Two quaternions are added by adding the
also denote the orientation of coordinate frame i relative respective scalar elements separately, so the operators
to coordinate frame j. In this case, frame i is rotated act as separators. The null element for addition is the
through the angle θ about an axis defined by the vec- quaternion 0 = 0 + 0i + 0j + 0k, and quaternion sums
T
tor K̂ = [kx ky kz ] relative to frame j. The vector K̂ is are associative, commutative, and distributive. The null
sometimes referred to as the equivalent axis of a finite ro- element for multiplication is I = 1 + 0i + 0j + 0k, as can
tation. The angle-axis representation, typically written be seen using Iq = q for any quaternion q. Quaternion
T
as either θK̂ or [θkx θky θkz ] , is superabundant by one products are associative and distributive, but not com-
CHAPTER 1. KINEMATICS 5

! "T
mutative, and following the conventions of the operators is the 4 × 4 homogeneous transform matrix and j ρ 1
and addition, have the form, ! "T
and i ρ 1 are the homogeneous representations of the
a0 b0 − a1 b1 − a2 b2 − a3 b3 position vectors j ρ and i ρ. The matrix j T i transforms
+(a0 b1 + a1 b0 + a2 b3 − a3 b2 )i vectors from coordinate frame i to coordinate frame j.
ab = . (1.2) Its inverse, j T −1 transforms vectors from coordinate
+(a0 b2 + a2 b0 + a3 b1 − a1 b3 )j i
+(a0 b3 + a3 b0 + a1 b2 − a2 b1 )k frame j to coordinate frame i.
, j T
-
j −1 Ri −jRTi j pi
It is convenient to define the conjugate of a quaternion, T i = iT j = . (1.6)
0 0 0 1
q̃ = qo − q1 i − q2 j − q3 k, so that qq̃ = q̃q = q02 + q12 +
q32 . A unit quaternion can then be defined such that, Composition of 4 × 4 homogeneous transform matrices is
qq̃ = 1. Often, q0 is referred to as the scalar part of the accomplished through simple matrix multiplication, just
T
quaternion, and [q1 q2 q3 ] is referred to as the vector as in the case of 3×3 rotation matrices. Therefore, kT i =
part. k
T j jT i . Since matrix multiplications do not commute,
Since a quaternion contains four elements, it is su- the order or sequence is important.
perabundant by one for describing orientation, and the Homogeneous transformations are particularly attrac-
unit magnitude of a unit quaternion provides the auxil- tive when compact notation is desired and/or when ease
iary relationship to resolve this. A vector is defined in of programming is the most important consideration.
quaternion notation as a quaternion with zero real part. This is not, however, a computationally efficient repre-
T
Thus, a vector p = [px py pz ] can be expressed as a sentation since it introduces a large number of additional
quaternion p = px i + py j + pz k. For any unit quaternion multiplications by ones and zeros. Although homoge-
q, the operation qpq̃ performs a rotation of the vector neous transform matrices technically contain sixteen el-
T
p about the direction [q1 q2 q3 ] . This is clearly seen by ements, four are defined to be zero or one, and the re-
expanding the operation qpq̃ and comparing the results maining elements are composed of a rotation matrix and
with the equivalent rotation matrix listed in Table 1.1. a position vector. Therefore, the only truly superabun-
dant coordinates come from the rotation matrix com-
1.2.3 Homogeneous Transformations ponent, so the relevant auxiliary relationships are those
associated with the rotation matrix.
The preceding sections have addressed representations of
position and orientation separately. With homogeneous
transformations, position vectors and rotation matrices
1.2.4 Screw Transformations
are combined together in a compact notation. Any vec- The transformation in Equation 1.3 can be viewed as
tor i ρ expressed relative to the i coordinate frame can composed of a rotation between coordinate frames i and
be expressed relative to the j coordinate frame if the j and a separate displacement between those frames. To
position and orientation of the i frame are known rela- get from frame i to frame j, one could perform the rota-
tive to the j frame. Using the notation of Section 1.2.1, tion first, followed by the displacement, or vice versa. Al-
the position of the origin of coordinate frame i rela- ternatively, the spatial displacement between the frames
tive to coordinate frame j can be denoted by the vec- can be expressed, except in the case of a pure transla-
! "T
tor j pi = jpxi jpyi jpzi . Using the notation of Section tion, as a rotation about a unique line combined with a
1.2.2, the orientation of frame i relative to frame j can translation parrallel to that line. Since this is precisely
be denoted by the rotation matrix jRi . Thus, the type of displacement produced by a screw, the line
about which rotation takes place is referred to as the
j
ρ = jRi i ρ + j pi . (1.3) screw axis. The displacement itself is often called the
twist about that screw axis, and the ratio of the linear
This equation can be written, displacement d to the rotation θ is referred to as the
, j - , j j
-, i - pitch h of the screw axis. Thus, d = hθ. In a screw dis-
ρ Ri pi ρ
= , (1.4) placement, every point in the body has the same linear
1 0 0 0 1 1
displacement d parallel to the screw axis. Each point is
where, also rotated about that axis through the same angle θ.
, -
j
Ri j
pi Points that lie on the screw axis remain on the axis and
j
Ti = , (1.5) are displaced a distance d along it. The screw axis of a
0 0 0 1
CHAPTER 1. KINEMATICS 6

Screw Transformation to Homogeneous Transformation 1.2.5 Plücker Coordinates

(where vθ = 1 − cθ ):
A minimum of four coordinates are needed to define a

j
Ri =  line in space. The Plücker coordinates of a line form
wx2 vθ + cθ wx wy vθ − wz sθ wx wz vθ + wy sθ a six-dimensional vector, so they are superabundant by
wx wy vθ + wz sθ wy2 vθ + cθ wy wz vθ − wx sθ  two. They can be viewed as a pair of three-dimensional
wx wz vθ − wy sθ w*y wz vθ ++wx sθ wz2 vθ + cθ vectors; one is parallel to the line, and the other is the
j
pi = I − jRi ρ + hθŵ “moment” of that vector about the origin. Thus, if u is
any vector parallel to the line and ρ is the position of
Homogeneous Transformation to Screw Transformation:
any point on the line relative to the origin, the Plücker
 T coordinates (L, M, N, P, Q, R) are given by:
r32 − r23
l = r13 − r31 
(L, M, N ) = uT ; (P, Q, R) = (ρ × u)T . (1.7)
r. 21 − r12
* Tj + * +.
θ = sign l pi .cos−1 r11 +r222+r33 −1 . For simply defining a line, the magnitude of u is not
lT j pi unique, nor is the component of ρ parallel to u. Two
h = 2θ sin θ
( I− i )
j T j
R p auxiliary relationships are imposed to reduce the set to
ρ = 2(1−cos
i
θ) just four independent coordinates. One is that the scalar
ŵ = 2 sin l
θ product of the two three-dimensional vectors is identi-
cally zero.
Table 1.4: Conversions from a screw transformation to a LP + M Q + N R ≡ 0. (1.8)
homogeneous transformation and vice versa. The other is the invariance of the line designated when
the coordinates are all multiplied by the same scaling
factor.
pure translation is not unique. Any line parallel to the
translation can be regarded as the screw axis, and since (L, M, N, P, Q, R) ≡ (kL, kM, kN, kP, kQ, kR). (1.9)
the rotation θ is zero, the axis of a translation is said to
This relationship may take the form of constraining u
have infinite pitch.
to have unit magnitude so that L, M , and N are the
A screw axis is most conveniently represented in any direction cosines.
reference frame by means of a unit vector ŵ parallel to In this handbook, it is often useful to express velocities
it and the position vector ρ of any point lying on it. in Plücker coordinates, wherein unlike the definition of
Additional specification of the pitch h and the angle of lines, the magnitudes of the the two three-dimensional
rotation θ completely defines the location of a second co- vectors are not arbitrary. Using the notation above, if
ordinate frame relative to the reference frame. Thus, a u is the vector of a body’s angular velocity ω and ρ
total of eight coordinates define a screw transformation, is the position vector from a point O fixed in the body
which is superabundant by two. The unit magnitude of to any point on the axis of rotation defined by ω, the
vector ŵ provides one auxiliary relationship, but in gen- Plücker coordinates are the angular velocity of the body
eral, there is no second auxiliary relationship because the and the translational velocity of the point O in the body.
same screw axis is defined by all points lying on it, which When the point O is the origin of the coordinate frame
is to say that the vector ρ contains one free coordinate. associated with that body, these coordinates are known
Table 1.4 contains the conversions between screw trans- as the spatial velocity of the body.
formations and homogeneous transformations. Note that , - , -
the equivalent rotation matrix for a screw transformation ω ω
v= = . (1.10)
has the same form as the equivalent rotation matrix for ρ×ω v
an angle-axis representation of orientation in Table 1.1.
Also, the auxiliary relationship that the vector ρ be or-
thogonal to the screw axis is used in Table 1.4 to provide
1.3 Joint Kinematics
a unique conversion to the screw transformation. A kinematic joint is a connection between two bodies
Additional information on screw theory can be found that constrains their relative motion. Two bodies that
in [2][11][7][3]. are in contact with one another create a simple kinematic
CHAPTER 1. KINEMATICS 7

joint. The surfaces of the two bodies that are in con- bodies joined relative to the other. The position of one
tact are able to move over one another, thereby permit- body relative to the other may be expressed as the angle
ting relative motion of the two bodies. Simple kinematic between two lines normal to the joint axis, one fixed in
joints are classified as lower pair joints if contact occurs each body. Thus, the joint has one degree of freedom.
over surfaces and as higher pair joints if contact occurs As noted above, a revolute joint is often realized as a
only at points or along lines. Lower pair joints are me- compound joint. Revolute joints are easily actuated by
chanically attractive since wear is spread over the whole rotating motors and are, therefore, extremely common
surface and lubricant is trapped in the small clearance in robotic systems. They may also be present as passive,
space (in non-idealized systems) between the surfaces, re- unactuated joints.
sulting in relatively good lubrication. As can be proved When the ẑ axis of coordinate frame i is aligned with a
from the requirement for surface contact [Ref], there are revolute joint axis, the free modes Φi and the constrained
only six possible forms of lower pair: revolute, prismatic, modes Φci of the joint are expressed as,
helical, cylindrical, spherical, and planar joints.
Some higher pair joints also have attractive properties,    
particularly rolling pairs in which one body rolls without 0 1 0 0 0 0
0 0 1 0 0 0
slipping over the surface of the other. This is mechan-    
1  0
ically attractive since the absence of sliding means the Φi =   , Φci = 0 0 0 0 . (1.11)
0 0 0 1 0 0
absence of abrasive wear. However, since contact occurs    
0 0 0 0 1 0
at a point, or along a line, application of a load across
the joint may lead to very high local stresses resulting in 0 0 0 0 0 1
other forms of material failure and, hence, wear. Higher
pair joints can be used to create kinematic joints with
special geometric properties, as in the case of a gear pair,
1.3.2 Prismatic
or a cam and follower pair.
Compound kinematic joints are connections between
The most general form of a prismatic joint, sometimes
two bodies formed by chains of other members and sim-
referred to colloquially as a sliding joint, is a lower pair
ple kinematic joints. A compound joint may constrain
formed from two congruent general cylindrical surfaces.
the relative motion of the two bodies joined in the same
These may not be right circular cylindrical surfaces. A
way as a simple joint. In such a case, the two joints are
general cylindrical surface is obtained by extruding any
said to be kinematically equivalent. An example is a ball
curve in a constant direction. Again, one surface is in-
bearing that provides the same constraint as a revolute
ternal and the other is an external surface.
joint but is composed of a set of bearing balls trapped
between two journals. The balls ideally roll without slip- A prismatic joint permits only sliding of one of the
ping on the journals, thereby taking advantage of the members joined relative to the other along the direction
special properties of rolling contact joints. of extrusion. The position of one body relative to the
Additional information on joints can be found in Chap- other is determined by the distance between two points
ter 4 Mechanisms and Actuation. on a line parallel to the direction of sliding, with one
point fixed in each body. Thus, this joint also has one
degree of freedom.
1.3.1 Revolute A prismatic joint may be realized as a compound joint
The most general form of a revolute joint, sometimes re- by means of a roller-rail assembly or other kinemati-
ferred to colloquially as a hinge or pin joint, is a lower cally equivalent mechanism. Prismatic joints are also
pair composed of two congruent surfaces of revolution. relatively easily actuated by means of linear actuators
The surfaces are the same except one of them is an ex- such as hydraulic or pneumatic cylinders, ball screws, or
ternal surface, convex in any plane normal to the axis of screw jacks. However, they always have motion limits
revolution, and one is an internal surface, concave in any since unidirectional sliding can, in principle, produce in-
plane normal to the axis. The surfaces may not be solely finite displacements. They are commonly used in robotic
in the form of right circular cylinders, since surfaces of mechanisms, but not nearly as commonly as revolutes.
that form do not provide any constraint on axial slid- When the ẑ axis of coordinate frame i is aligned with
ing. A revolute joint permits only rotation of one of the a prismatic joint axis, the free and constrained modes of
CHAPTER 1. KINEMATICS 8

the joint are expressed as, revolute in series with a prismatic joint whose direction
    of sliding is parallel to the revolute axis. While simpler to
0 1 0 0 0 0 implement using the geometric representation discussed
0 0 1 0 0 0 in Section 1.5, this approach has disadvantages for dy-
   
0 0 0 1 0 0

Φi =   , Φi = 
 c
0
. (1.12) namic simulation. Modeling a single cylindrical joint as
0
   0 0 1 0
 a combination of a prismatic and revolute joint requires
0 0 0 0 0 1 the addition of a virtual link between the two with zero
1 0 0 0 0 0 mass and zero length. The massless link can create com-
putational problems.
1.3.3 Helical When the ẑ axis of coordinate frame i is aligned with
The most general form of a helical joint, sometimes re- a cylindrical joint axis, the free and constrained modes
ferred to colloquially as a screw joint, is a lower pair of the joint are expressed as,
formed from two helicoidal surfaces formed by extruding
any curve along a helical path. The simple example is    
a screw and nut wherein the basic generating curve is 0 0 1 0 0 0
0 0 0 1 0 0
a pair of straight lines. The angle of rotation about the    
1 0  
axis of the screw joint θ is directly related to the distance Φi =   , Φci = 0 0 0 0 . (1.14)
0 0 0 0 1 0
of displacement of one body relative to the other along    
0 0 0 0 0 1
that axis d by the expression d = hθ, where the constant
h is called the pitch of the screw. Screw joints are most 0 1 0 0 0 0
often found in robotic mechanisms as constituents of lin-
ear actuators such as screw jacks and ball screws. They
are seldom used as primary kinematic joints.
When the ẑ axis of coordinate frame i is aligned with
a helical joint axis and assuming that the joint cannot be 1.3.5 Spherical
backdriven, the free and constrained modes of the joint
are expressed as, A spherical joint is a lower pair formed by contact of
    two congruent spherical surfaces. Once again, one is an
0 1 0 0 0 0 internal surface, and the other is an external surface. A
0 0 1 0 0 0
    spherical joint permits rotation about any line through
1 0 0 0 0 0
 
Φi =   , Φi = 
c  
. (1.13) the center of the sphere. Thus, it permits independent
0 0 0 1 0 0 rotation about axes in up to three different directions and
0 0 0 0 1 0
has three degrees of freedom. Although passive spherical
h 0 0 0 0 1 joints are quite often found in robotic mechanisms, ac-
tuated spherical joints are not found. However, a spher-
1.3.4 Cylindrical ical joint is easily replaced by a kinematically equivalent
compound joint consisting of three revolutes that have
A cylindrical joint is a lower pair formed by contact of
concurrent axes. They do not need to be successively
two congruent right circular cylinders, one an internal
orthogonal, but often they are implemented that way.
surface and the other an external surface. It permits
Each of the three revolute joints can be actuated. The
both rotation about the cylinder axis and sliding par-
arrangement is, in general, kinematically equivalent to
allel to it. Therefore, it is a joint with two degrees of
a spherical joint, but it does exhibit a singularity when
freedom. Joints with more than one degree of freedom
the revolute joint axes become coplanar. This is as com-
are only used passively in robotic mechanisms because
pared to the native spherical joint that never has such
each degree of freedom of an active joint must be sep-
a singularity. Likewise, if a spherical joint is modeled in
arately actuated. However, the lower pair joints with
simulation as three revolutes, computational difficulties
more than one degree of freedom are easily replaced by
again can arise from the necessary inclusion of massless
kinematically equivalent compound joints that are ser-
virtual links having zero length.
ial chains of one degree of freedom lower pairs. In the
present case, the cylindrical joint can be replaced by a The free and constrained modes of a spherical joint are
CHAPTER 1. KINEMATICS 9

expressed as, axis through the point of contact that is, in principle,
    unique. Hence, a three-dimensional rolling contact pair
1 0 0 0 0 0
0 permits relative motion with three degrees of freedom.
 1 0

0
 0 0

0 0 1  0
Φi =   , Φci = 0 0 . (1.15) 1.3.9
0 0 
0 1 0 0 Holonomic and Non-Holonomic
  
0 0 0 0 1 0 Constraints
0 0 0 0 0 1
With the exception of rolling contact, all of the con-
straints associated with the joints discussed in the pre-
1.3.6 Planar ceding sections can be expressed mathematically by
A planar joint is formed by planar contacting surfaces. equations containing only the joint position variables.
Like the spherical joint, it is a lower pair joint with threeThese are called holonomic constraints. The number of
degrees of freedom. Passive planar joints are occasionally equations, and hence the number of constraints, is 6 − n,
found in robotic mechanisms. A kinematically equiva- where n is the number of degrees of freedom of the joint.
lent compound joint consisting of a serial chain of three The constraints are intrinsically part of the axial joint
revolutes with parallel axes can replace a planar joint. model.
As was the case with the spherical joint, the compound A non-holonomic constraint is one that cannot be ex-
joint exhibits a singularity when the revolute axes be- pressed in terms of the position variables alone, but in-
come coplanar. cludes the time derivative of one or more of those vari-
When the ẑ axis of coordinate frame i is aligned with ables. These constraint equations cannot be integrated
the normal to the plane of contact, the free and con- to obtain relationships solely between the joint variables.
strained modes of the joint are expressed as, The most common example in robotic systems arises
    from the use of a wheel or roller that rolls without slip-
0 0 0 1 0 0 ping on another member.
0 0 0 0 1 0
   
1 0 0 0 0 0
Φi =    
 , Φi = 0 0 0 . (1.16) 1.3.10 Generalized Coordinates
c
0 1 0  
0 0 1 0 0 0 In a robot manipulator consisting of N bodies, 6N co-
0 0 0 0 0 1 ordinates are required to specify the position and orien-
tation of all the bodies relative to a coordinate frame.
1.3.7 Universal Since some of those bodies are jointed together, a num-
ber of constraint equations will establish relationships
A universal joint, often referred to as a Cardan or Hooke between some of these coordinates. In this case, the 6N
joint, is a compound joint with two degrees of freedom. coordinates can be expressed as functions of a smaller
It consists of a serial chain of two revolutes whose axes set of coordinates q that are all independent. The coor-
intersect orthogonally and is used in robotic mechanisms dinates in this set are known as generalized coordinates,
in both active and passive forms. and motions associated with these coordinates are con-
sistent with all of the constraints. The joint variables q
1.3.8 Rolling Contact of a robot manipulator form a set of generalized coordi-
nates.
Rolling contact actually encompasses several different
geometries. Planar rolling contact requires that the
mechanism in which the joint is used constrain the rela- 1.4 Workspace
tive motion to a plane. This is the case of a roller bearing,
for example. Rolling contact in planar motion permits Most generally, the workspace of a robotic manipula-
one degree of freedom of relative motion. As was noted tor is the total volume swept out by the end-effector
above, rolling contact has desirable wear properties since as the manipulator executes all possible motions. The
the absence of sliding means the absence of abrasive workspace is determined by the geometry of the manip-
wear. Planar rolling contact can take place along a line, ulator and the limits of the joint motions. It is more
thereby spreading the load and wear somewhat. Three- specific to define the reachable workspace as the total
dimensional rolling contact allows rotation about any locus of points at which the end-effector can be placed
CHAPTER 1. KINEMATICS 10

and the dextrous workspace as the subset of those points 1.5 Geometric Representation
at which the end-effector can be placed while having an
arbitrary orientation. Dexterous workspaces exist only The geometry of a robotic manipulator is conveniently
for certain idealized geometries, so real industrial ma- defined by attaching coordinate frames to each link.
nipulators with joint motion limits almost never possess While these frames could be located arbitrarily, it is ad-
dexterous workspaces. vantageous both for consistency and computational effi-
Many serial-chain robotic manipulators are designed ciency to adhere to a convention for locating the frames
such that their joints can be partitioned into a regional on the links. Denavit and Hartenberg [8] introduced the
structure and an orientation structure. The joints in foundational convention that has been adapted in a num-
the regional structure accomplish the positioning of the ber of different ways, one of which is the convention intro-
end-effector in space, and the joints in the orientation duced by Khalil and Dombre [12] used throughout this
structure accomplish the orientation of the end-effector. handbook. In all of its forms, the convention requires
Typically, the inboard joints of a serial chain manipula- only four rather than six parameters to locate one coor-
tor comprise the regional structure, while the outboard dinate frame relative to another. The four parameters
joints comprise the orientation structure. Also, since are the link length ai , the link twist αi , the joint offset
prismatic joints provide no capability for rotation, they di , and the joint angle θi . This parsimony is achieved
are generally not employed within the orientation struc- through judicious placement of the coordinate frame ori-
ture. gins and axes such that the x̂ axis of one frame both
intersects and is perpendicular to the ẑ axis of the pre-
The regional workspace volume can be calculated from ceding coordinate frame. The convention is applicable to
the known geometry of the serial-chain manipulator and manipulators consisting of revolute and prismatic joints,
motion limits of the joints. With three inboard joints so when multi-degree-of-freedom joints are present, they
comprising the regional structure, the area of workspace are modeled as combinations of revolute and prismatic
for the outer two (joints 2 and 3) is computed first, and joints, as discussed in Section 1.3.
then the volume is calculated by integrating over the
There are essentially four different forms of the con-
joint variable of the remaining inboard joint (joint 1). In
vention for locating coordinate frames in a robotic mech-
the case of a prismatic joint, this simply involves multi-
anism. Each exhibits its own advantages by managing
plying the area by the total length of travel of the pris-
trade-offs of intuitive presentation. In the original De-
matic joint. In the more common case of a revolute joint,
navit and Hartenberg [8] convention, joint i is located
it involves rotating the area about the joint axis through
between links i and i + 1, so it is on the outboard side
the full range of motion of the revolute. By the Theorem
of link i. Also, the joint offset di and joint angle θi are
of Pappus, the associated volume is equal to the product
measured along and about the i − 1 joint axis, so the
of the area, the distance from the area’s centroid to the
subscripts of the joint parameters do not match that of
axis, and the angle through which the area is rotated.
the joint axis. Waldron [35] and Paul [22] modified the
The boundaries of the area are determined by tracing
labeling of axes in the original convention such that joint
the motion of a reference point in the end-effector, typ-
i is located between links i − 1 and i in order to make it
ically the center of rotation of the wrist that serves as
consistent with the base member of a serial chain being
the orientation structure. Starting with each of the two
member 0. This places joint i at the inboard side of link
joints at motion limits and with joint 2 locked, joint 3 is
i and is the convention used in all of the other modi-
moved until its second motion limit is reached. Joint 3
fied versions. Furthermore, Waldron and Paul addressed
is then locked, and joint 2 is freed to move to its second
the mismatch between subscripts of the joint parameters
motion limit. Joint 2 is again locked, while joint 3 is
and joint axes by placing the ẑ i axis along the i + 1 joint
freed to move back to its original motion limit. Finally,
axis. This, of course, relocates the subscript mismatch
joint 3 is locked, and joint 2 freed to move likewise to
to the correspondence between the joint axis and the ẑ
its original motion limit. In this way, the trace of the
axis of the coordinate frame. Craig [6] eliminated all of
reference point is a closed curve whose area and centroid
the subscript mismatches by placing the ẑ i axis along
can be calculated mathematically.
joint i, but at the expense of the homogeneous trans-
More details on manipulator workspace can be found form i−1 T i being formed with a mixture of joint para-
in Chapter 4 Mechanisms and Actuation and Chapter 11 meters with subscript i and link parameters with sub-
Design Criteria. script i − 1. Khalil and Dombre [12] introduced another
CHAPTER 1. KINEMATICS 11

Geometric Parameters
Diagram

Figure 1.1: place holder

variation similar to Craig’s except that it defines the link

parameters ai and αi along and about the x̂i−1 axis. In
this case, the homogeneous transform i−1 T i is formed
only with parameters with subscript i, and the subscript
mismatch is such that ai and αi indicate the length and
twist of link i − 1 rather than link i. Thus, in summary,
the advantages of the convention used throughout this
handbook compared to the alternative conventions are
that the ẑ axes of the coordinate frames share the com-
mon subscript of the joint axes, and the four parameters
that define the spatial transform from coordinate frame i Figure 1.2: Example Six-Degree-of-Freedom Serial Chain
to coordinate frame i − 1 all share the common subscript Manipulator.
i.
In this handbook, the convention for serial chain ma-
nipulators is shown in Figure 1.1 and summarized as fol- i αi ai di θi
lows: 1 0 0 0 θ1
2 − π2 0 0 θ2
1. The N moving bodies of the robotic mechanism are 3 0 a3 0 θ3
numbered from 1 to N . The number of the base is 4 − π2 0 d4 θ4
0. 5 π
0 0 θ5
2
2. The N joints of the robotic mechanism are num- 6 − π
2 0 0 θ6
bered from 1 to N , with joint i located between
members i − 1 and i. Table 1.5: Tabulated Geometric Parameters for Example
3. The ẑ i axis is located along the axis of joint i. Serial Chain Manipulator.

4. The x̂i axis is located along the common normal

between the ẑ i−1 and ẑ i axes. With this convention, coordinate frame i can be lo-
5. ai is the distance from ẑ i−1 to ẑ i along x̂i−1 . cated relative to coordinate frame i−1 by executing a ro-
tation through an angle αi about the x̂i−1 axis, a trans-
6. αi is the angle from ẑ i−1 to ẑ i about x̂i−1 . lation of distance ai along x̂i−1 , a rotation through an
angle θi about the ẑ i axis, and a translation of distance
7. di is the distance from x̂i−1 to x̂i along ẑ i . di along ẑ i . Through concatentation of these individual
transformations, the equivalent homogeneous transfor-
8. θi is the angle from x̂i−1 to x̂i about ẑ i .
mation is,
The geometric parameters for the example manipula-
 
tor shown in Figure 1.2 are listed in Table 1.5. All of the cθi −sθi 0 ai
joints of this manipulator are revolutes, and joint 1 has sθi cαi cθi cαi −sαi −sαi di 
a vertical orientation. Joint 2 is perpendicular to joint 1
j
Ti = 
sθi sαi cθi sαi
. (1.17)
cαi cαi di 
and intersects it. Joint 3 is parallel to joint 2, and the 0 0 0 1
length of link 2 is a3 . Joint 4 is perpendicular to joint 3
and intersects it. Joint 5 likewise intersects joint 4 per-
pendicularly at an offset of d4 from joint 3. Finally, joint The identification of geometric parameters is ad-
6 intersects joint 5 perpendicularly. dressed in Chapter 3 Model Identification.
CHAPTER 1. KINEMATICS 12
 
1.6 Forward Kinematics r11 r12 r13 0 px6
r21 r22 r23 0 p  y
0
T6 =  6
r31 r32 r33 0 pz6 
The forward kinematics problem for a serial chain ma-
nipulator is to find the position and orientation of the 0 0 0 1
end-effector relative to the base given the positions of r11 = c1 (s2 s3 − c2 c3 ) (s4 s6 − c4 c5 c6 )
all of the joints and the values of all of the geometric −c1 s5 c6 (c2 s3 + s2 c3 ) + s1 (s4 c5 c6 + c4 s6 ) .
link parameters. A more general expression of the prob- r21 = s1 (s2 s3 − c2 c3 ) (s4 s6 − c4 c5 c6 )
lem is to find the relative position and orientation of any −s1 s5 c6 (c2 s3 + s2 c3 ) − c1 (s4 c5 c6 + c4 s6 ) .
two designated members given the geometric structure r31 = (c2 s3 + s2 c3 ) (s4 s6 − c4 c5 c6 )
of the manipulator and the values of a number of joint +s5 c6 (s2 s3 − c2 c3 ) .
positions equal to the number of degrees of freedom of r12 = c1 (s2 s3 − c2 c3 ) (c4 c5 s6 + s4 c6 )
the mechanism. The forward kinematics problem is crit- +c1 s5 s6 (c2 s3 + s2 c3 ) + s1 (c4 c6 − s4 c5 s6 ) .
ical for developing manipulator coordination algorithms r22 = s1 (s2 s3 − c2 c3 ) (c4 c5 s6 + s4 c6 )
because joint positions are typically measured by sensors +s1 s5 s6 (c2 s3 + s2 c3 ) − c1 (c4 c6 − s4 c5 s6 ) .
mounted on the joints and it is necessary to calculate the r32 = (c2 s3 + s2 c3 ) (c4 c5 s6 + s4 c6 )
positions of the joint axes relative to the fixed reference −s5 s6 (s2 s3 − c2 c3 ) .
frame. r13 = c1 c4 s5 (s2 s3 − c2 c3 ) − c1 c5 (c2 s3 + s2 c3 )
−s1 s4 s5 .
In practice, the forward kinematics problem is solved
r23 = s1 c4 s5 (s2 s3 − c2 c3 ) − s1 c5 (c2 s3 + s2 c3 )
by calculating the transformation between a coordinate
+c1 s4 s5 .
frame fixed in the end-effector and another coordinate
r33 = c4 s5 (c2 s3 + s2 c3 ) + c5 (s2 s3 − c2 c3 ) .
frame fixed in the base. This is straightforward for a
p6 =
0 x
a2 c1 c2 − d4 c1 (c2 s3 + s2 c3 )
serial chain since the transformation describing the po- 0 y
p6 = a2 s1 c2 − d4 s1 (c2 s3 + s2 c3 )
sition of the end-effector relative to the base is obtained
p6 =
0 z
−a2 s2 + d4 (s2 s3 − c2 c3 )
by simply concatenating transformations between frames
fixed in adjacent links of the chain. The convention for Table 1.6: Forward Kinematics of the Example Serial
the geometric representation of a manipulator presented Chain Manipulator in Figure 1.2.
in Section 1.5 reduces this to finding an equivalent 4 × 4
homogeneous transformation matrix that relates the spa-
tial displacement of the end-effector coordinate frame to 1.7 Inverse Kinematics
the base coordinate frame.
For the example serial chain manipulator shown in Fig- The inverse kinematics problem for a serial chain ma-
ure 1.2, the transformation is, nipulator is to find the values of the joint positions given
the position and orientation of the end-effector relative
0
T 6 = 0T 11T 22T 33T 44T 55T 6. (1.18) to the base and the values of all of the geometric link pa-
rameters. Once again, this is a simplified statement ap-
plying only to serial chains. A more general statement is:
Table 1.6 contains the elements of 0 T 6 that are calcu- Given the relative positions and orientations of two mem-
lated using Table 1.5 and Equation 1.17. bers of a mechanism, find the values of all of the joint
Once again, homogeneous transformations provide a positions. This amounts to finding all of the joint posi-
compact notation, but are computationally inefficient for tions given the homogeneous transformation between the
solving the forward kinematics problem. A reduction in two members of interest. In the common case of a six-
computation can be achieved by separating the position degree-of-freedom serial chain manipulator, the known
and orientation portions of the transformation to elimi- transformation is 0 T 6 . Reviewing the formulation of
nate all multiplications by the 0 and 1 elements of the this transformation in Section 1.6, it is clear that the
matrices. inverse kinematics problem for serial chain manipulators
The forward kinematics problem for closed chains is requires the solution of non-linear sets of equations. In
much more complicated because of the additional con- the case of a six-degree-of-freedom manipulator, three of
straints present. Solution methods for closed chains are these equations relate to the position vector within the
included in Chapter 13 Kinematic Structure and Analy- homogeneous transform, and the other three relate to the
sis. rotation matrix. In the latter case, these three equations
CHAPTER 1. KINEMATICS 13

cannot come from the same row or column because of θ1 =

the dependency within the rotation matrix. With these θ3 =
non-linear equations, it is possible that no solutions ex- θ2 =
ist or multiple solutions exist. For a solution to exist,
the desired position and orientation of the end-effector
Table 1.7: Inverse Position Kinematics of the Articulated
must lie in the workspace of the manipulator. In cases
Arm Within the Example Serial Chain Manipulator in
where solutions do exist, they often cannot be presented
Figure 1.2.
in closed form, so numerical methods are required.
θ4 =
1.7.1 Closed-Form Solutions θ5 =
θ6 =
Closed-form solutions are desirable because they are
faster than numerical solutions and readily identify all
possible solutions. The disadvantage of closed-form solu- Table 1.8: Inverse Orientation Kinematics of the Spheri-
tions are that they are not general, but robot-dependent. cal Wrist Within the Example Serial Chain Manipulator
The most effective methods for finding closed-form solu- in Figure 1.2.
tions are ad hoc techniques that take advantage of partic-
ular geometric features of specific mechanisms. In gen-
eral, closed-form solutions can only be obtained for six- C1 sin θi − C2 cos θi + C4 = 0, (1.21)
degree-of-freedom systems with special kinematic struc-
ture characterized by a large number of the geometric pa- is particularly useful because only one solution results,
rameters defined in Section 1.5 being zero-valued. Most
industrial manipulators have such structure because it θi = Atan2 (−C1 C4 − C2 C3 , C2 C4 − C1 C3 ) . (1.22)
permits more efficient coordination software. Sufficient
conditions for a six-degree-of-freedom manipulator to Geometric Methods
have closed-form inverse kinematics solutions are [26]: 1)
three consecutive revolute joint axes intersect at a com- Geometric methods involve identifying points on the ma-
mon point, as in a spherical wrist; 2) three consecutive nipulator relative to which position and/or orientation
revolute joint axes are parallel. can be expressed as a function of a reduced set of the
Closed-form solution approaches are generally divided joint variables. This often amounts to decomposing the
into algebraic and geometric methods. spatial problem into separate planar problems. The re-
sulting equations are solved using algebraic manipula-
Algebraic Methods tion. The two sufficient conditions for existence of a
closed-form solution for a six-degree-of-freedom manip-
Algebraic methods involve identifying the significant ulator that are listed above enable the decomposition of
equations containing the joint variables and manipulat- the problem into inverse position kinematics and inverse
ing them into a soluble form. A common strategy is re- orientation kinematics. This is the decomposition into
duction to a transcendental equation in a single variable regional and orientation structures discussed in Section
such as, 1.4, and the solution is found by rewriting Equation 1.18,
C1 cos θi + C2 sin θi + C3 = 0, (1.19)
(1.23)
where C1 , C2 , and C3 are constants. The solution to
such an equation is,
The example manipulator in Figure 1.2 has this struc-
1 ( 2
C2 ± C2 − C3 + C1
2 2 2 ture, and regional structure is commonly known as an
θi = 2 tan−1 . (1.20) articulated or anthropomorphic arm or an elbow manip-
C1 − C3
ulator. The solution to the inverse position kinemat-
Special cases in which one or more of the constants are ics problem for such a structure is summarized in Ta-
zero are also common. Reduction to a pair of equations ble 1.7.1. The orientation structure is simply a spheri-
having the form, cal wrist, and the corresponding solution to the inverse
orientation kinematics problem is summarized in Table
C1 cos θi + C2 sin θi + C3 = 0 1.7.1.
CHAPTER 1. KINEMATICS 14

1.7.2 Numerical Methods Iterative Methods

Unlike the algebraic and geometric methods used to A number of different iterative methods can be employed
find closed-form solutions, numerical methods are not to solve the inverse kinematics problem. Most of them
robot-dependent, so they can be applied to any kine- converge to a single solution based on an initial guess,
matic structure. The disadvantages of numerical meth- so the quality of that guess greatly impacts the solution
ods are that they can be slower and in some cases, they time. Newton-Raphson methods provide a fundamen-
do not allow computation of all possible solutions. For a tal approach that uses a first-order approximation to the
six-degree-of-freedom serial chain manipulator with only original equations. Pieper [26] was among the first to
revolute and prismatic joints, the translation and rota- apply the method to inverse kinematics, and others have
tion equations can always be reduced to a polynomial in followed [32][19]. Optimization approaches formulate the
a single variable of degree not greater than 16 [15]. Thus, problem as a nonlinear optimization problem and employ
such a manipulator can have as many as sixteen real solu- search techniques to move from an initial guess to a so-
tions to the inverse kinematics problem [18]. Since closed lution [34][40]. Resolved motion rate control converts
form solution of a polynomial equation is only possible the problem to a differential equation [37], and a mod-
if the polynomial is of degree four or less, it follows that ified predictor-corrector algorithm can be used to per-
many manipulator geometries are not soluble in closed form the joint velocity integration [5]. Control-theory-
form. In general, a greater number of non-zero geometric based methods cast the differential equation into a con-
parameters corresponds to a polynomial of higher degree trol problem [29]. Interval analysis [28] is perhaps one
in the reduction. For such manipulator structures, the of the most promising iterative methods because it offers
most common numerical methods can be divided into rapid convergence to a solution and can be used to find
categories of symbolic elimination methods, continuation all possible solutions.
methods, and interative methods.

1.8 Forward Instantaneous Kine-

Symbolic Elimination Methods
matics
Symbolic elimination methods involve analytical manip-
ulations to eliminate variables from the system of non- The forward instantaneous kinematics problem for a ser-
linear equations to reduce it to a smaller set of equa- ial chain manipulator is: Given the positions of all mem-
tions. Raghavan and Roth [27] used dialytic elimination bers of the chain and the rates of motion about all the
to reduce the inverse kinematics problem of a general joints, find the total velocity of the end-effector. Here
six-revolute serial chain manipulator to a polynomial ofthe rate of motion about the joint is the angular ve-
degree 16 and to find all possible solutions. The roots locity of rotation about a revolute joint or the transla-
tional velocity of sliding along a prismatic joint. The
provide solutions for one of the joint variables, while the
other variables are computed by solving linear systems. total velocity of a member is the velocity of the origin
Manocha and Canny [17] improved the numerical prop- of the reference frame fixed to it combined with its an-
gular velocity. That is, the total velocity has six in-
erties of this technique by reformulating the problem as a
generalized eigenvalue problem. An alternative approach dependent components and therefore, completely repre-
to elimination makes use of Gröbner bases [4][13]. sents the velocity field of the member. It is important to
notice that this definition includes an assumption that
the position of the mechanism is completely known. In
Continuation Methods most situations, this means that either the forward or
inverse position kinematics problem must be solved be-
Continuation methods involve tracking a solution path fore the forward instantaneous kinematics problem can
from a start system with known solutions to a target be addressed. The same is true of the inverse instanta-
system whose solutions are sought as the start system neous kinematics problem discussed in the following sec-
is transformed into the target system. These techniques tion. The forward instantaneous kinematics problem is
have been applied to inverse kinematics problems [33], important when doing acceleration analysis for the pur-
and special properties of polynomial systems can be ex- pose of studying dynamics. The total velocities of the
ploited to find all possible solutions [36]. members are needed for the computation of Coriolis and
CHAPTER 1. KINEMATICS 15
 
centripetal acceleration components. J11 J12 J13 J14
Differentiation with respect to time of the forward po- J21 J22 J23 J24 
J=  
sition kinematics equations yields a set of equations of J31 J32 J33 J34 
the form, J41 J42 J43 J44
vN = J (q)q̇ (1.24) J11 =
J21 =
where vN is the spatial velocity of the end-effector, q̇ J31 =
is an N -dimensional vector composed of the joint rates, J41 =
and J (q) is a 6 × N matrix whose elements are, in gen- J12 =
eral, non-linear functions of q1 , ..., qN . J (q) is called J22 =
the Jacobian matrix of this algebraic system. If the joint J32 =
positions are known, Equation 1.24 yields six linear al- J42 =
gebraic equations in the joint rates. If the joint rates J13 =
are given, solution of Equation 1.24 is a solution of the J23 =
forward instantaneous kinematics problem. Notice that J33 =
J (q) can be regarded as a known matrix for this purpose J43 =
provided all the joint positions are known. J14 =
J24 =
J34 =
1.8.1 Jacobian
J44 =
Formulation of the Jacobian. Differentiation of posi-
tion equations. From screw parameters. Development of Table 1.9: Forward Kinematics of the Example Serial
equation for transforming the Jacobian from one frame Chain Manipulator in Figure 1.2.
to another.
Table 1.8.1 contains the Jacobian of the example ma-
nipulator show in Figure 1.2. 1.9.1 Inverse Jacobian
Additional information about Jacobians can be found
in Chapter 12 Kinematically Redundant Manipulators. In order to solve the linear system of equations in the
joint rates obtained by decomposing Equation 1.24 into
its component equations when v is known, it is necessary
1.9 Inverse Instantaneous Kine- to invert the Jacobian matrix. The equation becomes,

matics q̇ = J −1 (q)vN (1.25)

The important problem from the point of view of ro-
botic coordination is the inverse instantaneous kinemat- Since J is a 6 × 6 matrix, numerical inversion is not
ics problem. The inverse instantaneous kinematics prob- very attractive in real-time software which must run at
lem for a serial chain manipulator is: Given the positions computation cycle rates of the order of 100 Hz or more.
of all members of the chain and the total velocity of the Worse, it is quite possible for J to become singular
end-effector, find the rates of motion of all joints. When (|J | = 0). The inverse does not then exist. Even when
controlling a movement of an industrial robot which op- the Jacobian matrix does not become singular, it may
erates in the point-to-point mode, it is not only necessary become ill conditioned leading to degraded performance
to compute the final joint positions needed to assume the in significant portions of the manipulator’s workspace.
desired final hand position. It is also necessary to gen- Most industrial robot geometries are simple enough that
erate a smooth trajectory for motion between the ini- the Jacobian matrix can be inverted analytically leading
tial and final positions. There are, of course, an infinite to a set of explicit equations for the joint rates. This
number of possible trajectories for this purpose. How- greatly reduces the number of computation operations
ever, the most straightforward and successful approach needed as compared to numerical inversion.
employs algorithms based on the solution of the inverse More information on singularities can be found in
instantaneous kinematics problem. This technique orig- Chapter 4 Mechanisms and Actuation and Chapter 13
inated in the work of Whitney [38] and of Pieper [26]. Kinematic Structure and Analysis.
CHAPTER 1. KINEMATICS 16

1.10 Static Wrench Transmission

Static wrench analysis of a manipulator establishes the
relationship between wrenches applied to the end-effector
and forces/torques applied to the joints. Through the
priniciple of virtual work, this relationship can be shown
to be,
τ = JT f. (1.26)

1.11 Conclusions and Further

Reading
A number of excellent texts provide a broad introduc-
tion to robotics with significant focus on kinematics
[1][6][12][16][22][29][30][31][39][20].
Bibliography

[1] H. Asada and J.-J. E. Slotine, Robot Analysis and Con- [14] C. S. G Lee, “Robot Arm Kinematics, Dynamics, and
trol, New York: John Wiley & Sons, 1986. Control,” Computer, vol. 15, no. 12, pp. 62-80, 1982.

[2] R. S. Ball, A Treatise on the Theory of Screws, Cam- [15] H. Y. Lee and C. G. Liang, “A New Vector Theory for
bridge: Cambridge University Press, 1998. the Analysis of Spatial Mechanisms,” Mechanisms and
Machine Theory, vol. 23, no. 3, pp. 209–217, 1988.
[3] O. Bottema and B. Roth, Theoretical Kinematics, New
York: Dover Publications, 1990. [16] F. L. Lewis, C. T. Abdallah, and D. M. Dawson, Control
of Robot Manipulators, New York: Macmillan, 1993.
[4] B. Buchberger, “Applications of Grobner Basis in Non-
Linear Computational Geometry,” Trends in Computer [17] D. Manocha and J. Canny, Real Time Inverse Kine-
Algebra. Lecture Notes in Computer Science, vol. 296, matics for General 6R Manipulators, Technical report,
Springer Verlag, 1989. University of California, Berkeley, 1992.

[5] H. Cheng and K. Gupta, “A Study of Robot Inverse [18] R. Manseur and K. L. Doty, “A Robot Manipulator
Kinematics Based Upon the Solution of Differential with 16 Real Inverse Kinematic Solutions,” Interna-
Equations,” Journal of Robotic Systems, vol. 8, no. 2, tional Journal of Robotics Research, vol. 8, no. 5, pp.
pp. 115–175, 1991. 75–79, 1989.

[6] J. J. Craig, Introduction to Robotics: Mechanics and [19] R. Manseur and K. L. Doty, “Fast Inverse Kinematics
Control, Reading, MA: Addison-Wesley, 1986. of 5-Revolute-Axis Robot Maniputlators,” Mechanisms
and Machine Theory, vol. 27, no. 5, pp. 587–597, 1992.
[7] J. K. Davidson and K. H. Hunt, Robots and Screw The-
ory: Applications of Kinematics and Statics to Robotics, [20] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical
Oxford: Oxford University Press, 2004. Introduction to Robotic Manipulation, Boca Raton, FL:
CRC Press, 1994.
[8] J. Denavit and R. S. Hartenberg, “A Kinematic No-
tation for Lower-Pair Mechanisms Based on Matrices,” [21] D.E. Orin and W.W. Schrader, “Efficient Computation
Journal of Applied Mechanics, vol. 22, pp. 215–221, of the Jacobian for Robot Manipulators,” International
1955. Journal of Robotics Research, vol. 3, no. 4, pp. 66–75,
1984.
[9] J. Duffy, Analysis of Mechanisms and Robot Manipula-
tors, New York: Wiley, 1980. [22] R. Paul, Robot Manipulators: Mathematics, Program-
ming and Control, Cambridge, MA: MIT Press, 1982.
[10] K. S. Fu, R. C. Gonzalez, and C. S. G. Lee, Robotics:
Control, Sensing, Vision, and Intelligence, New York: [23] R. P. Paul, B. E. Shimano, and G. Mayer, “Kinematic
McGraw-Hill, 1987. Control Equations for Simple Manipulators,” IEEE
Transactions on Systems, Man, and Cybernetics, vol.
[11] K. H. Hunt, Kinematic Geometry of Mechanisms, Ox- SMC-11, no. 6, pp. 339-455, 1981.
ford: Clarendon Press, 1978.
[24] R. P. Paul and C. N. Stephenson, “Kinematics of Robot
[12] W. Khalil and E. Dombre, Modeling, Identification and Wrists,” International Journal of Robotics Research, vol.
Control of Robots, New York: Taylor & Francis, 2002. 20, no. 1, pp. 31–38, 1983.
[13] P. Kovacs, “Minimum Degree Solutions for the Inverse [25] R. P. Paul and H. Zhang, “Computationally Efficient
Kinematics Problem by Application of the Buchberger Kinematics for Manipulators with Spherical wrists based
Algorithm,” Advances in Robot Kinematics, S. Stifter on the Homogeneous Transformation Representation,”
and J. Lenarcic Eds., Springer Verlag, pp. 326–334, International Journal of Robotics Research, vol. 5, no.
1991. 2, pp. 32–44, 1986.

17
BIBLIOGRAPHY 18

[26] D. Pieper, The Kinematics of Manipulators Under Com- [39] T. Yoshikawa, Foundations of Robotics, Cambridge,
puter Control, Doctoral Dissertation, Stanford Univer- MA: MIT Press, 1990.
sity, 1968.
[40] J. Zhao and N. Badler, “Inverse Kinematics Positioning
[27] M. Raghavan and B. Roth, “Kinematic Analysis of the Using Nonlinear Programming for Highly Articulated
6R Manipulator of General Geometry,” In Proceedings of Figures,” Transactions on Computer Graphics, vol. 13,
the 5th International Symposium on Robotics Research, no. 4, pp. 313–336, 1994.
1990.

[28] R. S. Rao, A. Asaithambi, and S. K. Agrawal, “Inverse

Kinematic Solution of Robot Manipulators Using Inter-
val Analysis,” ASME Journal of Mechanical Design, vol.
120, no. 1, pp. 147–150, 1998.

[29] L. Sciavicco and B. Siciliano, Modeling and Control of

Robot Manipulators, London: Springer, 2000.

[30] R. J. Schilling, Fundamentals of Robotics: Analysis and

Control, Englewood Cliffs, NJ: Prentice-Hall, 1990.

[31] M. W. Spong and M. Vidyasagar, Robot Dynamics and

Control, New York: John Wiley & Sons, 1989.

[32] S. C. A. Thomopoulos, and R. Y. J. Tam, “An Iterative

Solution to the Inverse Kinematics of Robotic Manipu-
lators,” Mechanisms and Machine Theory, vol. 26, no.
4, pp. 359–373, 1991.

[33] L. W. Tsai and A. P. Morgan “Solving the Kinematics

of the Most General Six- and Five-Degree-of-Freedom
Manipulators by Continuation Methods,” ASME Jour-
nal of Mechanisms, Transmissions, and Automation in
Design, vol. 107, pp. 189-195, 1985.

[34] J. J. Uicker, Jr., J. Denavit, and R. S. Hartenberg,

“An Interactive Method for the Displacement Analysis
of Spatial Mechanisms,” Journal of Applied Mechanics,
vol. 31, pp. 309–314, 1964.

[35] K. J. Waldron, “A Study of Overconstrained Linkage

Geometry by Solution of Closure Equations, Part I: A
Method of Study,” Mechanism and Machine Theory,
vol. 8, no. 1, pp. 95–104, 1973.

[36] C. W. Wampler, A. P. Morgan, and A. J. Sommese,

“Numerical Continuation Methods for Solving Polyno-
mial Systems Arising in Kinematics,” ASME Journal of
Mechanical Design, vol. 112, pp. 59-68, 1990.

[37] D. E. Whitney, “Resolved Motion Rate Control of Ma-

nipulators and Human Prostheses,” IEEE Transactions
on Man-Machine Systems, vol. 10, pp. 47–63, 1969.

[38] D. E. Whitney, ”The Mathematics of Coordinated Con-

trol of Prosthetic Arms and Manipulators,” Journal of
Dynamic Systems, Measurement, and Control, vol. 122,
pp. 303–309, 1972.