i i “runall”

2021/5/22
page 117

i i

C H A P T E R 4

Inverse Manipulator Kinematics

4.1 INTRODUCTION
4.2 SOLVABILITY
4.3 THE NOTION OF MANIPULATOR SUBSPACE WHEN n < 6
4.4 ALGEBRAIC VS. GEOMETRIC
4.5 ALGEBRAIC SOLUTION BY REDUCTION TO POLYNOMIAL
4.6 PIEPER’S SOLUTION WHEN THREE AXES INTERSECT
4.7 EXAMPLES OF INVERSE MANIPULATOR KINEMATICS
4.8 THE STANDARD FRAMES
4.9 SOLVE-ING A MANIPULATOR
4.10 REPEATABILITY AND ACCURACY
4.11 COMPUTATIONAL CONSIDERATIONS

4.1 INTRODUCTION
In the previous chapter, we considered the problem of computing the position and
orientation of the tool relative to the user’s workstation when given the joint angles
of the manipulator. In this chapter, we will investigate the more difficult converse
problem: Given the desired position and orientation of the tool relative to the station,
how do we compute the set of joint angles which will achieve this desired result?
Whereas Chapter 3 focused on the direct kinematics of manipulators, here the focus
is on the inverse kinematics of manipulators.
Solving the problem of finding the required joint angles to place the tool frame,
{T }, relative to the station frame, {S}, is split into two parts. First, frame transforma-
tions are performed to find the wrist frame, {W }, relative to the base frame, {B}, then
the inverse kinematics are used to solve for the joint angles.

4.2 SOLVABILITY
The problem of solving the kinematic equations of a manipulator is a nonlinear one.
Given the numerical value of N0T , we attempt to find values of θ1 , θ2 , . . ., θn . Consider
the equations given in (3.14). In the case of the PUMA 560 manipulator, the precise
statement of our current problem is as follows: Given 06T as sixteen numeric values
(four of which are trivial), solve (3.14) for the six joint angles θ1 through θ6 .
For the case of an arm with six degrees of freedom [like the one corresponding
to the equations in (3.14)], we have 12 equations and six unknowns. However, among
the 9 equations arising from the rotation-matrix portion of 06T , only 3 are indepen-
dent. These, added to the 3 equations from the position-vector portion of 06T , give

117

i i

i i
 i i “runall”
2021/5/22
page 118

i i

118 Chapter 4 Inverse Manipulator Kinematics

6 equations with six unknowns. These equations are nonlinear, transcendental equa-
tions, which can be quite difficult to solve. The equations of (3.14) are those of a robot
that had very simple link parameters—many of the αi were 0 or ±90 degrees. Many
link offsets and lengths were zero. It is easy to imagine that, for the case of a gen-
eral mechanism with six degrees of freedom (with all link parameters nonzero) the
kinematic equations would be much more complex than those of (3.14). As with any
nonlinear set of equations, we must concern ourselves with the existence of solutions,
with multiple solutions, and with the method of solution.

Existence of Solutions
The question of whether any solution exists at all raises the question of the manip-
ulator’s workspace. Roughly speaking, workspace is that volume of space that the
end-effector of the manipulator can reach. For a solution to exist, the specified goal
point must lie within the workspace. Sometimes, it is useful to consider two defi-
nitions of workspace. Dextrous workspace is that volume of space that the robot
end-effector can reach with all orientations. That is, at each point in the dextrous
workspace, the end-effector can be arbitrarily oriented. The reachable workspace is
that volume of space that the robot can reach in at least one orientation. Clearly, the
dextrous workspace is a subset of the reachable workspace.
Consider the workspace of the two-link manipulator in Fig. 4.1. If l1 = l2 , then
the reachable workspace consists of a disc of radius 2l1 . The dextrous workspace con-
sists of only a single point, the origin. If l1 = l2 , then there is no dextrous workspace,
and the reachable workspace becomes a ring of outer radius l1 + l2 and inner radius
|l1 − l2 |. Inside the reachable workspace, there are two possible orientations of the
end-effector. On the boundaries of the workspace, there is only one possible orien-
tation.
These considerations of workspace for the two-link manipulator have assumed
that all the joints can rotate 360 degrees. This is rarely true for actual mechanisms.
When joint limits are a subset of the full 360 degrees, then the workspace is obviously
correspondingly reduced, either in extent or in the number of possible orientations

L2

L1

FIGURE 4.1: Two-link manipulator with link lengths l1 and l2 .

i i

i i
 i i “runall”
2021/5/22
page 119

i i

Section 4.2 Solvability 119

attainable. For example, if the arm in Fig. 4.1 has full 360-degree motion for θ1 , but
only 0 ≤ θ2 ≤ 180◦ , then the reachable workspace has the same extent, but only one
orientation is attainable at each point.
When a manipulator has fewer than six degrees of freedom, it cannot attain
general goal positions and orientations in 3-space. Clearly, the planar manipulator in
Fig. 4.1 cannot reach out of the plane, so any goal point with a nonzero Z-coordinate
value can be quickly rejected as unreachable. In many realistic situations, manipula-
tors with four or five degrees of freedom are employed that operate out of a plane,
but that clearly cannot reach general goals. Each such manipulator must be studied
to understand its workspace. In general, the workspace of such a robot is a subset of a
subspace that can be associated with any particular robot. Given a general goal-frame
specification, an interesting problem arises in connection with manipulators having
fewer than six degrees of freedom: What is the nearest attainable goal frame?
Workspace also depends on the tool-frame transformation, because it is usually
the tool-tip that is discussed when we speak of reachable points in space. Generally,
the tool transformation is performed independently of the manipulator kinematics
and inverse kinematics, so we are often led to consider the workspace of the wrist
frame, {W }. For a given end-effector, a tool frame, {T }, is defined; given a goal frame,
{G}, the corresponding {W } frame is calculated, and then we ask: Does this desired
position and orientation of {W } lie in the workspace? In this way, the workspace with
which we must concern ourselves (in a computational sense) is different from the
one imagined by the user, who is concerned with the workspace of the end-effector
(the {T } frame).
If the desired position and orientation of the wrist frame is in the workspace,
then at least one solution exists.

Multiple Solutions
Another possible problem encountered in solving kinematic equations is that of
multiple solutions. A planar arm with three revolute joints has a large dextrous
workspace in the plane (given “good” link lengths and large joint ranges), because
any position in the interior of its workspace can be reached with any orientation.
Figure 4.2 shows a three-link planar arm with its end-effector at a certain position

FIGURE 4.2: Three-link manipulator. Dashed lines indicate a second solution.

i i

i i
 i i “runall”
2021/5/22
page 120

i i

120 Chapter 4 Inverse Manipulator Kinematics

B
Obstacle

FIGURE 4.3: One of the two possible solutions to reach point B causes a collision.

and orientation. The dashed lines indicate a second possible configuration in which
the same end-effector position and orientation are achieved.
The fact that a manipulator has multiple solutions can cause problems, because
the system has to be able to choose one. The criteria upon which to base a decision
vary, but a very reasonable choice would be the closest solution. For example, if the
manipulator is at point A, as in Fig. 4.3, and we wish to move it to point B, a good
choice would be the solution that minimizes the amount that each joint is required
to move. Hence, in the absence of the obstacle, the upper dashed configuration in
Fig. 4.3 would be chosen. This suggests that one input argument to our kinematic
inverse procedure might be the present position of the manipulator. In this way, if
there is a choice, our algorithm can choose the solution closest in joint-space. How-
ever, the notion of “close” might be defined in several ways. For example, typical
robots could have three large links, followed by three smaller, orienting links near
the end-effector. In this case, weights might be applied in the calculation of which
solution is “closer” so the selection favors moving smaller joints rather than mov-
ing the large joints, when a choice exists. The presence of obstacles might force a
“farther” solution to be chosen in cases where the “closer” solution would cause
a collision—in general, then, we need to be able to calculate all the possible solu-
tions. Thus, in Fig. 4.3, the presence of the obstacle implies that the lower dashed
configuration is to be used to reach point B.
The number of solutions depends upon the number of joints in the manipulator
but is also a function of the link parameters (αi , ai , and di for a rotary joint manip-
ulator) and the allowable ranges of motion of the joints. For example, the PUMA
560 can reach certain goals with eight different solutions. Figure 4.4 shows four solu-
tions; all place the hand with the same position and orientation. For each solution
pictured, there is another solution in which the last three joints “flip” to an alternate
configuration according to the following formulas:

θ4 = θ4 + 180◦ ,
θ5 = −θ5 , (4.1)
θ6 = θ6 + 180◦ .

i i

i i
 i i “runall”
2021/5/22
page 121

i i

Section 4.2 Solvability 121

FIGURE 4.4: Four solutions of the PUMA 560.

So, in total, there can be eight solutions for a single goal. Because of limits on joint
ranges, some of these eight could be inaccessible.
In general, the more nonzero link parameters there are, the more ways there
will be to reach a certain goal. For example, consider a manipulator with six rotational
joints. Figure 4.5 shows how the maximum number of solutions is related to how
many of the link length parameters (the ai ) are zero. The more that are nonzero,
the bigger is the maximum number of solutions. For a completely general rotary-
jointed manipulator with six degrees of freedom, there are up to sixteen solutions
possible [1, 6].

Method of Solution
Unlike linear equations, there are no general algorithms that may be employed to
solve a set of nonlinear equations. In considering methods of solution, it will be wise
to define what constitutes the “solution” of a given manipulator.
A manipulator will be considered solvable if the joint variables can be
determined by an algorithm that allows one to determine all the sets of joint
variables associated with a given position and orientation [2].

i i

i i
 i i “runall”
2021/5/22
page 122

i i

122 Chapter 4 Inverse Manipulator Kinematics

ai Number of solutions

a1 5 a3 5 a5 5 0 <4
a3 5 a5 5 0 <8
a3 5 0 < 16
All ai Þ 0 < 16

FIGURE 4.5: Number of solutions vs. nonzero ai .

The main point of this definition is that we require, in the case of multiple solu-
tions, that it be possible to calculate all solutions. Hence, we do not consider some
numerical iterative procedures as solving the manipulator—namely, those methods
not guaranteed to find all the solutions.
We will split all proposed manipulator solution strategies into two broad
classes: closed-form solutions and numerical solutions. Because of their iterative
nature, numerical solutions generally are much slower than the corresponding
closed-form solution; so much so, for most uses, we are not interested in the numer-
ical approach to solution of kinematics. Iterative numerical solution to kinematic
equations is a whole field of study in itself (see [6,11,12]), and is beyond the scope
of this text.
We will restrict our attention to closed-form solution methods. In this context,
“closed form” means a solution method based on analytic expressions or on the solu-
tion of a polynomial of degree 4 or less, such that noniterative calculations suffice to
arrive at a solution. Within the class of closed-form solutions, we distinguish two
methods of obtaining the solution: algebraic and geometric. These distinctions are
somewhat hazy; any geometric methods brought to bear are applied by means of
algebraic expressions, so the two methods are similar. The methods differ perhaps in
approach only.
A major recent result in kinematics is that, according to our definition of solv-
ability, all systems with revolute and prismatic joints having a total of six degrees
of freedom in a single series chain are solvable. However, this general solution is a
numerical one. Only in special cases can robots with six degrees of freedom be solved
analytically. These robots for which an analytic (or closed-form) solution exists are
characterized either by having several intersecting joint axes or by having many αi
equal to 0 or ±90 degrees. Calculating numerical solutions is generally time consum-
ing relative to evaluating analytic expressions; hence, it is considered very important
to design a manipulator so that a closed-form solution exists. Manipulator design-
ers discovered this very quickly, and now virtually all industrial manipulators are
designed sufficiently simply so a closed-form solution can be developed.
A sufficient condition that a manipulator with six revolute joints have a closed-
form solution is that three neighboring joint axes intersect at a point. Section 4.6
discusses this condition. Almost every manipulator with six degrees of freedom built
today has three intersecting axes. For example, axes 4, 5, and 6 of the PUMA 560
intersect.

i i

i i
 i i “runall”
2021/5/22
page 123

i i

Section 4.3 The Notion of Manipulator Subspace When n < 6 123

4.3 THE NOTION OF MANIPULATOR SUBSPACE WHEN n < 6

The set of reachable goal frames for a given manipulator constitutes its reachable
workspace. For a manipulator with n degrees of freedom (where n < 6), this reach-
able workspace can be thought of as a portion of an n-degree-of-freedom subspace.
In the same manner in which the workspace of a six-degree-of-freedom manipulator
is a subset of space, the workspace of a simpler manipulator is a subset of its sub-
space. For example, the subspace of the two-link robot of Fig. 4.1 is a plane, but the
workspace is a subset of this plane, namely a circle of radius l1 + l2 for the case that
l1 = l2 .
One way to specify the subspace of an n-degree-of-freedom manipulator is to
give an expression for its wrist or tool frame as a function of n variables that locate
it. If we consider these n variables to be free, then, as they take on all possible values,
the subspace is generated.

EXAMPLE 4.1
B
Give a description of the subspace of WT for the three-link manipulator from
Chapter 3, Fig. 3.6.
The subspace of WB T is given by
⎡ ⎤
cφ −sφ 0.0 x
⎢ sφ cφ 0.0 y ⎥
B ⎢ ⎥
W T = ⎣ 0.0 0.0 1.0 0.0 ⎦ , (4.2)
0 0 0 1

where x and y give the position of the wrist and φ describes the orientation of the
terminal link. As x, y, and φ are allowed to take on arbitrary values, the subspace
is generated. Any wrist frame that does not have the structure of (4.2) lies outside
the subspace (and therefore lies outside the workspace) of this manipulator. Link
lengths and joint limits restrict the workspace of the manipulator to be a subset of
this subspace.

Z1
Z2

Y1
X1
X2
Y2

FIGURE 4.6: A polar two-link manipulator.

i i

i i
 219

6
Dynamic and Force Analysis
6.1 Introduction

In previous chapters, we studied the kinematics of position and differential motions of robots. In this chapter,
we look at the dynamics of serial robots as it relates to accelerations, loads, masses, and inertias. We also study
the static force relationships of serial robots.
As you may remember from your dynamics course, in order to be able to accelerate a mass, we need to exert
a force on it. Similarly, to cause an angular acceleration in a rotating body, a torque must be exerted on it
(Figure 6.1), as:

F = m a and T =I α (6.1)

ΣT ΣF I∙α
= m∙a

Figure 6.1 Force-mass-acceleration and torque-inertia-angular-acceleration relationships for a rigid body.

To accelerate a robot’s links, it is necessary to have actuators capable of exerting large enough forces and
torques on the links and joints to move them at a desired acceleration and velocity. Otherwise, the links may
not be moving as fast as necessary, and consequently, the robot may not maintain its desired positional accu-
racy. To calculate how strong each actuator must be, it is necessary to determine the dynamic relationships
that govern the motions of the robot. These relationships are the force-mass-acceleration and the torque-
inertia-angular-acceleration equations. Based on these equations, and considering the external loads on
the robot, the designer can calculate the largest loads to which the actuators may be subjected, thereby
designing the actuators that can deliver the necessary forces and torques.
In general, the dynamic equations may be used to find the equations of motion of mechanisms. This means
that, by knowing the forces and torques, we can predict how a mechanism will move. However, in our case, we
have already found the equations of motion; besides, in all but the simplest cases, solving the dynamic equa-
tions of multi-axis 3D robots is very complicated and involved. Instead, we use these equations to find what
forces and torques may be needed to induce desired accelerations in the robot’s joints and links. These equa-
tions are also used to analyze the effects of different inertial loads on the robot, and depending on the desired
accelerations, whether certain loads are important. As an example, consider a robot in space. Although
objects are weightless in space, they do have inertia. As a result, the weight of an object that a robot handles

Introduction to Robotics: Analysis, Control, Applications, Third Edition. Saeed B. Niku.

© 2020 John Wiley & Sons Ltd. Published 2020 by John Wiley & Sons Ltd.
Companion website: www.wiley.com/go/niku3ed
220 Introduction to Robotics

in space may be trivial, but its inertia is not. As long as the movements are very slow, a light robot may be able
to move very large loads in space with little effort. This is why the very slender robots used in the Space Shut-
tle program were able to handle very large satellites. The dynamic equations allow the designer to investigate
the relationship between different elements of the robot and design its components appropriately.
In general, techniques such as Newtonian mechanics can be used to find the dynamic equations for robots.
However, due to the fact that robots are 3D and multi-DOF mechanisms with distributed masses, it is very
difficult to use Newtonian mechanics. Instead, we opt to use other techniques such as Lagrangian mechanics,
which is based on energy terms only and, therefore, in many cases, is easier to use. Although Newtonian
mechanics and other techniques can be used for this derivation, most references are based on Lagrangian
mechanics. In this chapter, we briefly study Lagrangian mechanics with some examples, and then we see
how it can be used to solve for robot equations. Since this is an introductory book, these equations will
not be completely derived; only the results will be demonstrated and discussed. Interested readers are
encouraged to refer to other references for more detail [1, 2, 3, 4, 5, 6, 7].

6.2 Lagrangian Mechanics: A Short Overview

Lagrangian mechanics is based on the differentiation of the system’s energy terms with respect to the system’s
variables and time, as follows. For simple cases, it may take longer to use this technique than Newtonian
mechanics. However, as the complexity of the system increases, the Lagrangian method becomes relatively
simpler to use. Lagrangian mechanics is based on the following two generalized equations: one for linear
motions and one for rotational motions. First we define a Lagrangian as:
L = K −P (6.2)
where L is the Lagrangian, K is the kinetic energy of the system, and P is the potential energy of the sys-
tem. Then:
∂ ∂L ∂L
Fi = − (6.3)
∂t ∂xi ∂xi
∂ ∂L ∂L
Ti = − (6.4)
∂t ∂θi ∂θi

where Fi is the summation of all external forces, Ti is the summation of all external torques, and θi and xi are
system variables. As a result, in order to get the equations of motion, we need to derive the system’s energy
equations and differentiate the Lagrangian according to Eqs. (6.3) and (6.4). The following five examples
demonstrate the application of Lagrangian mechanics in deriving equations of motion. Notice how the com-
plexity of the terms increases as the number of DOF (and variables) increases.

Example 6.1 Derive the force-acceleration relationship for the 1-DOF system shown in Figure 6.2,
using both Lagrangian mechanics as well as Newtonian mechanics. Assume the wheels have
negligible inertia.

k
m F

Figure 6.2 Schematic of a simple cart-spring system.

 Dynamic and Force Analysis 221

Solution:
The x-axis denotes the motion of the cart and is the only variable in this system. Since this is a 1-DOF
system, there is only one equation describing the motion. Because the motion is linear, we use only
Eq. (6.3), as follows:

1 1 1 1 1
K = mv2 = mx2 and P = kx2 L = K − P = mx2 − kx2
2 2 2 2 2
The derivatives of the Lagrangian are:

∂L d ∂L
= mx and mx = mx and = − kx
∂x dt ∂x
Therefore, the equation of motion for the cart will be:
F = mx + kx
To solve the problem with Newtonian mechanics, we draw the free-body diagram of the cart
(Figure 6.3) and solve for forces as follows:

F = ma
F − kx = max F = max + kx

mg

kx F = ma

R x

Figure 6.3 Free-body diagram for the cart-spring system.

which is exactly the same result. For this simple system, it appears that Newtonian mechanics is
simpler. ■

Example 6.2 Derive the equations of motion for the 2-DOF system shown in Figure 6.4.

x
k
m1 F

l
θ T

m2

Figure 6.4 Schematic of a cart-pendulum system.

222 Introduction to Robotics

Solution:
In this problem, there are 2 DOF, two coordinates x and θ, and two equations of motion: one for the
linear motion of the system and one for the rotation of the pendulum.
The kinetic energy of the system comprises the kinetic energies of the cart and the pendulum. Notice
that the velocity of the pendulum is the summation of the velocity of the cart and of the pendulum
relative to the cart, or:
v p = v c + v p/c = x i + lθ cos θ i + lθ sin θ j = x + lθ cos θ i + lθ sin θ j
and
2 2
v2p = x + lθcosθ + lθsinθ
We find:
K = Kcart + Kpendulum
1
Kcart = m1 x2
2
1 2 2
Kpendulum = m2 x + lθcosθ + lθsinθ
2
1 1 2
K = m1 + m2 x2 + m2 l2 θ + 2lθx cos θ
2 2
Likewise, the potential energy is the summation of the potential energy in the spring and in the pen-
dulum, or:
1
P = kx2 + m2 gl 1 −cos θ
2
Notice that the zero-potential-energy line (datum) is chosen at θ = 0∘ . The Lagrangian is:
1 1 2 1
L = K −P = m1 + m2 x2 + m2 l2 θ + 2lθx cos θ − kx2 −m2 gl 1− cos θ
2 2 2
The derivatives and the equations of motion related to the linear motion are:
∂L
= m1 + m2 x + m2 lθ cos θ
∂x
d ∂L 2
= m1 + m2 x + m2 lθ cos θ −m2 lθ sin θ
dt ∂x
∂L
= −kx
∂x
2
F = m1 + m2 x + m2 lθ cos θ − m2 lθ sin θ + kx
and for the rotational motion they are:
∂L
= m2 l2 θ + m2 lx cos θ
∂θ
d ∂L
= m2 l2 θ + m2 lx cos θ − m2 lxθ sin θ
dt ∂θ
∂L
= −m2 glsin θ − m2 lθx sin θ
∂θ
T = m2 l2 θ + m2 lx cos θ + m2 gl sin θ
 Dynamic and Force Analysis 223

If we write the two equations of motion in matrix form, we get:

2
F = m1 + m2 x + m2 lθ cos θ − m2 lθ sin θ + kx
T = m2 l2 θ + m2 lx cos θ + m2 glsin θ

F m1 + m2 m2 l cos θ x 0 − m2 l sin θ x2 kx
= + 2 + (6.5)
T m2 l cos θ m2 l 2 θ 0 0 θ m2 gl sin θ
■

Example 6.3 Derive the equations of motion for the 2-DOF system shown in Figure 6.5.

o x
l1
θ1 T1

A m1
l2 T2
θ2
B m2

Figure 6.5 A two-link mechanism with concentrated masses.

Solution:
Notice that this example is somewhat more similar to a robot, except that the mass of each link is
assumed to be concentrated at the end of each link, and there are only 2 DOF. However, in this example
we will see many more acceleration terms, as we would expect to see with robots, including centripetal
and Coriolis accelerations.
We follow the same format as before. First we calculate the kinetic and potential energies of the sys-
tem, as follows:

K = K1 + K2

1 2
where K1 = m1 l1 2 θ1
2
To calculate the kinetic energy of the mass at B, first we write the position equation for m2 at B, and
subsequently we differentiate it for the velocity:

xB = l1 sinθ1 + l2 sin θ1 + θ2 = l1 S1 + l2 S12

yB = −l1 C1 − l2 C12
xB = l1 C1 θ1 + l2 C12 θ1 + θ2

yB = l1 S1 θ1 + l2 S12 θ1 + θ2
224 Introduction to Robotics

Since v2 = x2 + y2 , we get:

2 2 2 2
v2B = l1 2 θ1 S12 + C12 + l2 2 θ1 + θ2 + 2θ1 θ2 2
S12 2
+ C12 + 2l1 l2 C1 C12 + S1 S12 θ1 + θ1 θ2
2 2 2 2
= l1 2 θ1 + l2 2 θ1 + θ2 + 2θ1 θ2 + 2l1 l2 C2 θ1 + θ1 θ2

The kinetic energy for the mass at B is:

1 2 1 2 2 2
K2 = m2 l1 2 θ1 + m2 l2 2 θ1 + θ2 + 2θ1 θ2 + m2 l1 l2 C2 θ1 + θ1 θ2
2 2

The total kinetic energy is:

1 2 1 2 2 2
K= m1 + m2 l1 2 θ1 + m2 l2 2 θ1 + θ2 + 2θ1 θ2 + m2 l1 l2 C2 θ1 + θ1 θ2
2 2

With the datum (zero potential energy) at point o, the potential energy of the system can be
written as:

P1 = −m1 gl1 C1
P2 = −m2 gl1 C1 − m2 gl2 C12
P = P1 + P2 = − m1 + m2 gl1 C1 −m2 gl2 C12

The Lagrangian for the system is:

1 2 1 2 2
L = K −P = m1 + m2 l1 2 θ1 + m2 l2 2 θ1 + θ2 + 2θ1 θ2
2 2
2
+ m2 l1 l2 C2 θ1 + θ1 θ2 + m1 + m2 gl1 C1 + m2 gl2 C12

The derivatives of the Lagrangian are:

∂L
= m1 + m2 l1 2 θ1 + m2 l2 2 θ1 + θ2 + 2m2 l1 l2 C2 θ1 + m2 l1 l2 C2 θ2
∂θ1
d ∂L
= m1 + m2 l1 2 + m2 l2 2 + 2m2 l1 l2 C2 θ 1 + m2 l2 2 + m2 l1 l2 C2 θ 2
dt ∂θ1

−2m2 l1 l2 S2 θ1 θ2 −m2 l1 l2 S2 θ22

∂L
= − m1 + m2 gl1 S1 −m2 gl2 S12
∂θ1

From Eq. (6.4), the first equation of motion is:

T1 = m1 + m2 l1 2 + m2 l2 2 + 2m2 l1 l2 C2 θ 1 + m2 l2 2 + m2 l1 l2 C2 θ 2
−2m2 l1 l2 S2 θ1 θ2 −m2 l1 l2 S2 θ22 + m1 + m2 gl1 S1 + m2 gl2 S12
 Dynamic and Force Analysis 225

Similarly:
∂L
= m2 l2 2 θ1 + θ2 + m2 l1 l2 C2 θ1
∂θ2
d ∂L
= m2 l2 2 θ 1 + θ 2 + m2 l1 l2 C2 θ 1 −m2 l1 l2 S2 θ1 θ2
dt ∂θ2
∂L 2
= − m2 l1 l2 S2 θ1 + θ1 θ2 −m2 gl2 S12
∂θ2
2
T2 = m2 l2 2 + m2 l1 l2 C2 θ 1 + m2 l2 2 θ 2 + m2 l1 l2 S2 θ1 + m2 gl2 S12
Writing these two equations in matrix form, we get:
T1 m1 + m2 l1 2 + m2 l2 2 + 2m2 l1 l2 C2 m2 l2 2 + m2 l1 l2 C2 θ1
=
T2 m2 l2 2 + m2 l1 l2 C2 m2 l2 2 θ2
2
0 −m2 l1 l2 S2 θ1 −m2 l1 l2 S2 −m2 l1 l2 S2 θ1 θ2
+ + (6.6)
2
m2 l1 l2 S2 0 θ2 0 0 θ2 θ1
m1 + m2 gl1 S1 + m2 gl2 S12
+
m2 gl2 S12
First, note how much more complicated and involved these equations of motion are compared to
Example 6.2. Also note that in Eq. (6.6), the θ terms are related to the angular accelerations of the links,
2
the θ terms are centripetal accelerations, and the θ1 θ2 terms are Coriolis accelerations.
It should be mentioned here that the Coriolis acceleration was originally derived for cases where a
linear motion occurs within a rotating frame; consequently, the direction of the linear velocity changes
and creates the Coriolis term. In this case, although both velocities are angular, nevertheless there is a
motion within a rotating frame that causes the θi θj terms. In this book, as is also customary in avionics,
we refer to these terms as Coriolis acceleration. In this example, the first link acts as a rotating frame for
link 2, and, therefore, Coriolis acceleration is present; whereas in Example 6.2, the cart is not rotating,
and, therefore, there is no Coriolis acceleration. Based on this, we should expect to have multiple Cor-
iolis acceleration terms for a multi-axis, 3D manipulator arm, because each link acts as a rotating frame
for the links succeeding it. ■

Example 6.4 Using the Lagrangian method, derive the equations of motion for the 2-DOF robot arm
shown in Figure 6.6. The center of mass for each link is at the center of the link. The moments of inertia
are I1 and I2.
Solution:
The solution of this example robot arm is in fact similar to the solution of Example 6.3. However, in
addition to a change in the coordinate frames, the two links have distributed masses, requiring the use
of moments of inertia in the calculation of the kinetic energy. We follow the same steps as before. First
we calculate the velocity of the center of mass of link 2 by differentiating its position:
xD = l1 C1 + 0 5l2 C12 xD = − l1 S1 θ1 − 0 5l2 S12 θ1 + θ2
yD = l1 S1 + 0 5l2 S12 yD = l1 C1 θ1 + 0 5l2 C12 θ1 + θ2
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Scanned by CamScanner
Jacobian
 Jacobian relates the linear and angular velocity of
the end-effector to the vector of joint velocities

 For an n-link manipulator Jacobian is of the form

Linear part of Jacobian

 The upper half of the

Jacobian Jv is given as

 where the i-th column

Jvi is
Angular part of Jacobian

 The lower half of the

Jacobian Jw is given as

 where the i-th column

Jwi is
 Two-Link Planar Robot Arm
 The coordinates (x, y) of the tool are

a2

a1
 Differentiate equations above to
obtain the relationship between the
velocity of the tool and the joint
velocities.
 Two-Link Planar Robot Arm
 Using the vector notation

a2

a1

 The angular speed of tool is

 Derivation of Jacobian for
Two-Link Planar Robot Arm
 Since there are two joints size of Jacobian
matrix must be 6 × 2
 Since the joints are revolute, form of the a2
Jacobian must be

a1
 Derivation of Jacobian for
Two-Link Planar Robot Arm
 Performing the required calculations then
yields
a2

a1
 Example 4.6
Jacobian for an Arbitrary Point
 Consider the three-link planar manipulator.
 To compute the linear velocity v and the angular velocity w of the center
of link 2 as shown.
Stanford Manipulator
With A Spherical Wrist
DH parameters for Stanford Manipulator
 Derivation of Jacobian
 Note that o1=o2, joint 3 is
prismatic and that o3 = o4 =o5
as a consequence of the
spherical wrist and the frame
assignment.
 First, oj is given by the first
three entries of the last column
of 𝑇𝑗0 = A1 · · ·Aj , with o0 = (0,
0, 0)T = o1.
 The vector zj is given as
 SCARA Manipulator

one degree-of-freedom
wrist is connected

The SCARA (Selective Compliant

The Epson E2L653S SCARA Robot Articulated Robot for Assembly).
DH parameters for SCARA
forward kinematic equations
 Derivation of Jacobian For
SCARA Manipulator
 This Jacobian is a 6 × 4 matrix since the SCARA
has only four degrees-of freedom.
 o0=[0 0 0]T, o1=?, o2=?, o4=?
 o1 is the first three elements of last column of A1
matrix

 o2 is the first three elements of last column of

𝑇20 =A1 A2

 o4 is the first three elements of last column of 𝑇40