Robot Dynamics

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.

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 θ
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
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
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
T2
I2
l2 ,
D θ2
y1
m2
T1 B x1
y0 I1
l 1, C
A m1 θ1
x0
Figure 6.6 A 2-DOF robot arm.
Therefore, the total velocity of the center of mass of link 2 is:
v2D = x2D + y2D = θ21 l12 + 0 25l22 + l1 l2 C2 + θ22 0 25l22 + θ1 θ2 0 5l22 + l1 l2 C2 (6.7)
The total kinetic energy of the system is the sum of the kinetic energies of links 1 and 2. Remembering
that the kinetic energy for a link rotating about a fixed point (for link 1) and about the center of mass (for
link 2) is given in Eq. (6.8), we get:
1 2 1 2 1
K = K1 + K2 = IA θ1 + ID θ1 + θ2 + m2 v2D
2 2 2
(6.8)
1 1 1 1 2 1
= m1 l12 θ21 + m2 l22 θ1 + θ2 + m2 v2D
2 3 2 12 2
Substituting Eq. (6.7) into Eq. (6.8) and rearranging, we get:
1 1 1 1
K = θ 21 m1 l12 + m2 l22 + m2 l12 + m2 l1 l2 C2
6 6 2 2
1 1 1
+ θ22 m2 l22 + θ1 θ2 m2 l22 + m2 l1 l2 C2 (6.9)
6 3 2
The potential energy of the system is the sum of the potential energies of the two links:
l1 l2
P = m1 g S1 + m2 g l1 S1 + S12 (6.10)
2 2
The Lagrangian for the two-link robot arm is:
1 1 1 1 1
L = K − P = θ 21 m1 l12 + m2 l22 + m2 l12 + m2 l1 l2 C2 + θ22 m2 l22
6 6 2 2 6
1 1 l1 l2
+ θ1 θ2 m2 l22 + m2 l1 l2 C2 −m1 g S1 −m2 g l1 S1 + S12
3 2 2 2
Taking the derivatives of the Lagrangian and substituting the terms into Eq. (6.4) yields the following
two equations of motion:
1 1 1 1
T1 = m1 l12 + m2 l12 + m2 l22 + m2 l1 l2 C2 θ 1 + m2 l22 + m2 l1 l2 C2 θ 2
3 3 3 2
(6.11)
1 2 1 1
− m2 l1 l2 S2 θ1 θ2 − m2 l1 l2 S2 θ2 + m1 + m2 gl1 C1 + m2 gl2 C12
2 2 2
1 1 1 1 1
T2 = m2 l22 + m2 l1 l2 C2 θ 1 + m2 l22 θ 2 + m2 l1 l2 S2 θ21 + m2 gl2 C12 (6.12)
3 2 3 2 2
Equations (6.11) and (6.12) can be written in matrix form as:
1 1 1 1
m1 l12 + m2 l12 + m2 l22 + m2 l1 l2 C2 m2 l22 + m2 l1 l2 C2
T1 3 3 3 2 θ1
=
T2 1 1 1 θ2
m2 l22 + m2 l1 l2 C2 m2 l22
3 2 3
1
0 − m2 l1 l2 S2 2
− m2 l1 l2 S2
2 θ1 0 θ1 θ2
+ + (6.13)
1 2
θ2 0 0 θ2 θ1
m2 l1 l2 S2 0
2
1 1
m1 + m2 gl1 C1 + m2 gl2 C12
2 2
+
1
m2 gl2 C12 ■
2
Example 6.5 Using the Lagrangian method, derive the equations of motion for the 2-DOF polar
robot arm shown in Figure 6.7. The center of mass for each link is at the center of the link. The
moments of inertia are I1 and I2.
r
I2
l 2, D
T1
y0 C m2
I1
l 1, B
A m1 θ1
x0
Figure 6.7 A 2-DOF polar robot arm.
Solution:
Note that in this case, we have a prismatic joint. We denote the distance from the base of the robot to
the center of the outer arm as r, which serves as one of the two variables. The total length of the arm is
r + l2 /2 . As before, we derive the Lagrangian and take the proper derivatives as follows:
K = K1 + K2
1 2 1 1 2 1 2
K1 = I1,A θ = m1 l1 2 θ = m1 l1 2 θ
2 2 3 6
xD = rCθ xD = rCθ − rθSθ
yD = rSθ yD = rSθ + rθCθ
and
2
v2D = r 2 + r 2 θ
1 2 1 1 1 2 1 2
K2 = I2,D θ + m2 v2D = m2 l22 θ + m2 r 2 + r 2 θ
2 2 2 12 2
1 1 1 2 1
K= m1 l1 2 + m2 l22 + m2 r 2 θ + m2 r 2
6 24 2 2
l1
P = m1 g Sθ + m2 grSθ
2
1 1 1 2 1 l1
L= m1 l1 2 + m2 l22 + m2 r 2 θ + m2 r 2 − m1 g + m2 gr Sθ
6 24 2 2 2
d ∂L 1 1
= m1 l1 2 + m2 l22 + m2 r 2 θ + 2m2 rθ
dt ∂θ 3 12
∂L l1
= − m1 g + m2 gr Cθ
∂θ 2
1 1 l1
T= m1 l1 2 + m2 l22 + m2 r 2 θ + 2m2 rrθ + m1 g + m2 gr Cθ (6.14)
3 12 2
d ∂L
= m2 r
dt ∂r
∂L 2
= m2 rθ −m2 gSθ
∂r
2
F = m2 r −m2 rθ + m2 gSθ (6.15)
Writing these two equations in matrix form, we get:
1 1 2
T m1 l1 2 + m2 l22 + m2 r 2 0 θ 0 0 θ
= 3 12 +
F r − m2 r 0 r2
0 m2 (6.16)
m2 r m2 r rθ m1 g l1 /2 + m2 gr Cθ
+ +
0 0 θr m2 gSθ
■
6.3 Effective Moments of Inertia
To simplify the writing of the equations of motion, Eqs. (6.5), (6.6), (6.13), or (6.16) can be rewritten in sym-
bolic form as follows:
2
Ti Dii Dij θi Diii Dijj θi Diij Diji θi θj Di
= + + + (6.17)
Tj Dji Djj θj Djii Djjj θ2j Djij Djji θj θi Dj
In this equation, which is written for a 2-DOF system, a coefficient in the form of Dii is known as the effective
inertia at joint i, such that an acceleration at joint i causes a torque at joint i equal to Dii θ i . A coefficient in the
form Dij is known as the coupling inertia between joints i and j, as an acceleration at joint i or j causes a torque
at joint j or i equal to Dij θ j or Dji θ i . Dijj θ2j terms represent centripetal forces acting at joint i due to a velocity at
joint j. All terms with θi θj represent Coriolis-type accelerations (the influence of one rotating frame on
another), and when multiplied by corresponding inertias, represent Coriolis forces. The remaining terms
in the form Di represent gravity forces at joint i.
6.4 Dynamic Equations for Multiple-DOF Robots
As you see, the dynamic equations for a 2-DOF system are much more complicated than a 1-DOF system.
Similarly, these equations for a multiple-DOF spatial robot are very long and complicated but can be found by
calculating the kinetic and potential energies of the links, defining the Lagrangian, and differentiating the
Lagrangian equation with respect to the joint variables. The following is a summary of this procedure. For more
information, see [1, 2, 3, 4, 5, 6, 7].
6.4.1 Kinetic Energy

As you may remember from your dynamics course [8], the kinetic energy of a rigid body in 3D motion
(Figure 6.8a) is:
1 1
K = mvG 2 + ω h G (6.18)
2 2
where h G is the angular momentum of the body about G.
The kinetic energy of a rigid body in plane motion (Figure 6.8b) simplifies to:
1 1
K = mvG 2 + I ω2 (6.19)
2 2
ω ω
hG vG
G
G
(a) (b)
Figure 6.8 A rigid body in 3D motion and in plane motion.

Therefore, we need to derive expressions for the velocity of a point on a rigid body (e.g. the center of mass
G) as well as the moments of inertia.
The velocity of a point on a robot’s link can be defined by differentiating the position equation of the point,
which in our notation is expressed by a frame relative to the robot’s base, RTP. Here, we use the D-H trans-
formation matrices Ai to find the velocity terms for points along the robot’s links. In Chapter 2, we defined the
transformation between the hand frame and the base frame of the robot in terms of the A matrices as:
R
T H = R T 1 1 T 2 2 T 3 … n− 1 Tn = A1 A2 A3 …An 2 55
For a 6-axis robot, this equation can be written as:

0
T 6 = 0 T 1 1 T 2 2 T 3 …5 T 6 = A1 A2 A3 …A6 (6.20)
Referring to Eq. (2.53), the derivative of an Ai matrix for a revolute joint with respect to its joint variable θi is:
Cθi − Sθi Cαi Sθi Sαi ai Cθi − Sθi −Cθi Cαi Cθi Sαi −ai Sθi
∂Ai d Sθi Cθi Cαi − Cθi Sαi ai Sθi Cθi − Sθi Cαi Sθi Sαi ai Cθi
= = (6.21)
∂θi ∂θi 0 Sαi Cαi di 0 0 0 0
0 0 0 1 0 0 0 0
This matrix can be broken into a constant matrix Qi and the Ai matrix such that:
−Sθi − Cθi Cαi Cθi Sαi − ai Sθi 0 −1 0 0 Cθi −Sθi Cαi Sθi Sαi ai Cθi
Cθi − Sθi Cαi Sθi Sαi ai Cθi 1 0 0 0 Sθi Cθi Cαi − Cθi Sαi ai Sθi
= × (6.22)
0 0 0 0 0 0 0 0 0 Sαi Cαi di
0 0 0 0 0 0 0 0 0 0 0 1
or
∂Ai
= Qi Ai (6.23)
∂θi
Similarly, the derivative of an Ai matrix for a prismatic joint with respect to its joint variable di is:
Cθi −Sθi Cαi Sθi Sαi ai Cθi 0 0 0 0
∂Ai ∂ Sθi Cθi Cαi −Cθi Sαi ai Sθi 0 0 0 0
= = (6.24)
∂di ∂di 0 Sαi Cαi di 0 0 0 1
0 0 0 1 0 0 0 0
which, as before, can be broken into a constant matrix Qi and the Ai matrix such that:
0 0 0 0 0 0 0 0 Cθi − Sθi Cαi Sθi Sαi ai Cθi
0 0 0 0 0 0 0 0 Sθi Cθi Cαi − Cθi Sαi ai Sθi
= × (6.25)
0 0 0 1 0 0 0 1 0 Sαi Cαi di
0 0 0 0 0 0 0 0 0 0 0 1
or:
∂Ai
= Qi Ai (6.26)
∂θi
In both Eqs. (6.23) and (6.26), the Qi matrices are always constant, as shown, and can be summarized as:
0 −1 0 0 0 0 0 0
1 0 0 0 0 0 0 0
Qi revolute = Qi prismatic = (6.27)
0 0 0 0 0 0 0 1
0 0 0 0 0 0 0 0
Using qi to represent the joint variables (θ1, θ2… for revolute joints and d1, d2 … for prismatic joints), and
extending the same differentiation principle to the 0Ti matrix of Eq. (6.20) with multiple joint variables (θi and
di), differentiated with respect to only one variable qj results in:
∂ 0Ti ∂ A1 A2 Aj Ai
Uij = = = A1 A2 Qj Aj Ai j≤i (6.28)
∂qj ∂qj
Note that since 0Ti is differentiated only with respect to one variable qj, there is only one Qj. Higher-order
derivatives can similarly be formulated from:
Uijk = ∂Uij /∂qk (6.29)
Let’s see how this method works before we continue with this subject.
Example 6.6 Find the expression for the derivative of the transformation of the fifth link of the Stan-
ford arm relative to the base frame, with respect to the second and third joint variables.
Solution:
The Stanford arm is a spherical robot, where the second joint is revolute and the third joint is prismatic.
Therefore:
0
T5 = A1 A2 A3 A4 A5
∂ 0 T5
U52 = = A1 Q2 A2 A3 A4 A5
∂θ2
∂ 0 T5
U53 = = A1 A2 Q3 A3 A4 A5
∂d3
where Q2 and Q3 are as defined in Eq. (6.27). ■
Example 6.7 Find an expression for U635 of the Stanford arm.

Solution:
0
T6 = A1 A2 A3 A4 A5 A6
∂0 T 6
U63 = = A1 A2 Q3 A3 A4 A5 A6
∂d3
∂U63 ∂ A1 A2 Q3 A3 A4 A5 A6
U635 = = = A1 A2 Q3 A3 A4 Q5 A5 A6
∂q5 ∂q5
■
We now continue with the derivation of the velocity term for a point on a link of a robot. We denote ri to
represent a point on any link i of the robot relative to frame i. Notice that this length is measured relative to
frame i as it becomes an important issue soon.
The position of the point can be expressed by pre-multiplying the vector with the transformation matrix
representing its frame, or:
pi = R T i ri = 0 T i ri (6.30)
The velocity of the point is a function of the velocities of all the joints q1 , q2 , …, q6 . Therefore, differentiating
Eq. (6.30) with respect to all the joint variables qj yields the velocity of the point as:
d 0 i
∂ 0 T i dqj i
dqi
vi = T i ri = ri = Uij ri (6.31)
dt j=1
∂qj dt j=1
dt
The kinetic energy of an element of mass dmi on a link is:

1 2 1
dKi = vi dmi = x2i + y2i + z2i dmi (6.32)
2 2
The velocity term can be formalized from the following:
xi xi 2 xi y i xi z i
vi viT = yi xi y i z i = y i xi y i 2 y i z i
zi z i xi z i y i z i 2
and
xi 2 xi y i x i z i
Trace vi viT = Trace yi xi yi 2 yi zi = xi 2 + yi 2 + zi 2 (6.33)
z i xi z i y i z i 2
Combining Eqs. (6.31)–(6.33) yields the following equation for the kinetic energy of the element:
i i T
1 dqp dqr
dKi = Trace Uip ri Uir ri dmi (6.34)
2 p=1
dt r=1
dt
where p and r represent the different joint numbers. This allows us to add the contributions made to the final
velocity of a point on any link i from other joints’ movements. Integrating this equation and rearranging it
yields the total kinetic energy as:
i i
1
Ki = dKi = Trace Uip ri ri T dmi Uir T qp qr (6.35)
2 p=1 r=1
Writing ri in terms of its coordinates relative to its frame, we can derive the following terms:
xi xi 2 xi y i xi z i xi
yi xi yi yi 2 yi zi yi
ri = , riT = xi yi zi 1 ri riT =
zi xi z i y i z i z i 2 z i
1 xi yi zi 1
Therefore,
xi 2 dmi xi yi dmi xi zi dmi xi dmi

xi
xi yi dmi yi 2 dmi yi zi dmi yi dmi
T
yi
ri ri dmi = xi yi zi 1 dmi = (6.36)
zi 2
xi zi dmi yi zi dmi zi dmi zi dmi
1
xi dmi yi dmi zi dmi dmi
Notice that the inertia tensor is usually a 3 × 3 tensor. However, as we have done throughout this book, we
use homogeneous matrices (4 × 4 or 4 × 1) to facilitate pre- and post-multiplication. Using a 4 × 1 position
matrix in Eq. (6.36) results in a 4 × 4 inertia matrix including first moments and the mass. Therefore, this
is called a pseudo inertia matrix. Through the following manipulations of Eq. (6.36), it is possible to derive
the pseudo inertia matrix as shown:
1
2x2 = x2 + x2 + y2 −y2 + z2 −z2 x2 = − y 2 + z 2 + x2 + z 2 + x2 + y 2
2
Ixx = y2 + z2 dm, Iyy = x2 + z2 dm, Izz = x2 + y2 dm
Ixy = xydm, Ixz = xzdm, Iyz = yzdm,
mx = xdm, my = ydm, mz = zdm,
then
1 1 1 1
x2 dm = − y2 + z2 dm + x2 + z2 dm + x2 + y2 dm = −Ixx + Iyy + Izz
2 2 2 2
1 1 1 1
y2 dm = y2 + z2 dm − x2 + z2 dm + x2 + y2 dm = Ixx −Iyy + Izz
2 2 2 2
1 1 1 1
z2 dm = y2 + z2 dm + x2 + z2 dm − x2 + y2 dm = Ixx + Iyy −Izz
2 2 2 2
Therefore, Eq. (6.36) can be written as:
1
− Ixx + Iyy + Izz i
Iixy Iixz mi x i
2
1
Iixy Ixx − Iyy + Izz i
Iiyz mi y i
Ji = 2 (6.37)
1
Iixz Iiyz Ixx + Iyy − Izz i
mi z i
2
mi x i mi y i mi z i mi
Since this matrix is independent of joint angles and velocities, it must be evaluated only once. Substituting
Eq. (6.36) into Eq. (6.35) results in the final form for kinetic energy of the robot manipulator as:
n i i
1
K= Trace Uip Ji Uir T qp qr (6.38)
2 i=1 p=1 r=1
The kinetic energy of the actuators can also be added to this equation. Assuming that each actuator has an
inertia of Ii(act), the kinetic energy of the actuator will be 12 Ii act qi 2 , and the total kinetic energy of the robot is:
n i i
1 1 n
K= Trace Uip Ji Uir T qp qr + Ii act qi 2 (6.39)
2 i=1 p=1 r=1
2 i=1
6.4.2 Potential Energy

The potential energy of the system is the sum of the potential energies of each link, and can be written as:
n n
P= Pi = − mi g T 0
Ti ri (6.40)
i=1 i=1
where g T = gx gy gz 0 is the gravity matrix and r i is the location of the center of mass of a link relative to
the frame representing the link. Obviously, the potential energy must be a scalar quantity, and therefore gT,
which is a 1 × 4 matrix, when multiplied by the position vector 0 T i r i which is a 4 × 1 matrix, will yield a
single scalar quantity. Also notice that the values in the gravity matrix are dependent on the orientation of the
reference frame.
6.4.3 The Lagrangian

The Lagrangian can be written as:
n i i n n
1
L = K −P = Trace Uip Ji Uir T qp qr + 1 2 Ii act qi 2 − − mi g T 0
Ti ri (6.41)
2 i=1 p=1 r=1 i=1 i=1
6.4.4 Robot’s Equations of Motion

The Lagrangian can now be differentiated in order to form the dynamic equations of motion. Although this
process is not shown, the final equations of motion for a general multi-axis robot can be summarized as
follows:
n n n
Ti = Dij q j + Ii act q i + Dijk qj qk + Di (6.42)
j=1 j=1 k =1
where
n
Dij = Trace Upj Jp UpiT (6.43)
p = max i, j
n
Dijk = Trace Upjk Jp UpiT (6.44)
p = max i, j, k
and
n
Di = − mp g T Upi r p (6.45)
p=i
In Eq. (6.42), the first part is the angular acceleration-inertia terms, the second part is the actuator inertia
term, the third part is the Coriolis and centripetal terms, and the last part is the gravity term. This equation
can be expanded for a 6-axis revolute robot as follows:
Ti = Di1 θ 1 + Di2 θ 2 + Di3 θ 3 + Di4 θ 4 + Di5 θ 5 + Di6 θ 6 + Ii act θ i
2 2 2 2 2 2
+ Di11 θ1 + Di22 θ2 + Di33 θ3 + Di44 θ4 + Di55 θ5 + Di66 θ6
+ Di12 θ1 θ2 + Di13 θ1 θ3 + Di14 θ1 θ4 + Di15 θ1 θ5 + Di16 θ1 θ6
(6.46)
+ Di61 θ6 θ1 + Di62 θ6 θ2 + Di63 θ6 θ3 + Di64 θ6 θ4 + Di65 θ6 θ5 + Di
Notice that in Eq. (6.46) there are two terms with θ1 θ2 and θ2 θ1 . The two coefficients are Di12 and Di21. To
see what these terms look like, let’s calculate them for i = 5. From Eq. (6.44) for D512 we have i = 5, j = 1, k = 2,
n = 6, p = 5, and for D521 we have i = 5, j = 2, k = 1, n = 6, p = 5, resulting in:
T T
D512 = Trace U512 J5 U55 + Trace U612 J6 U65
T T
(6.47)
D521 = Trace U521 J5 U55 + Trace U621 J6 U65
From Eq. (6.28), we have:
∂A1 A2 A3 A4 A5 ∂ Q1 A1 A2 A3 A4 A5
U51 = = Q1 A 1 A 2 A 3 A 4 A 5 U512 = U 51 2 = = Q1 A1 Q2 A2 A3 A4 A5
∂θ1 ∂θ2
(6.48)
∂A1 A2 A3 A4 A5 ∂ A1 Q2 A2 A3 A4 A5
U52 = = A 1 Q2 A 2 A 3 A 4 A 5 U521 = U 52 1 = = Q1 A1 Q2 A2 A3 A4 A5
∂θ2 ∂θ1
∂A1 A2 A3 A4 A5 A6 ∂ Q1 A1 A2 A3 A4 A5 A6
U61 = = Q1 A1 A2 A3 A4 A5 A6 U612 = U 61 2 = = Q1 A1 Q2 A2 A3 A4 A5 A6
∂θ1 ∂θ2
∂A1 A2 A3 A4 A5 A6 ∂ A1 Q2 A2 A3 A4 A5 A6
U62 = = A1 Q2 A2 A3 A4 A5 A6 U621 = U 62 1 = = Q1 A1 Q2 A2 A3 A4 A5 A6
∂θ2 ∂θ1
Note that in these equations, Q1 and Q2 are the same. The indices are only used to clarify the relationship
with the derivatives. Substituting the result from Eq. (6.48) into Eq. (6.47) shows that D512 = D521 . Clearly, the
summation of the two similar terms yields the corresponding θ1 θ2 acceleration terms. This is true for all
similar coefficients in Eq. (6.46). Therefore, we can simplify this equation for all joints as follows:
T1 = D11 θ 1 + D12 θ 2 + D13 θ 3 + D14 θ 4 + D15 θ 5 + D16 θ 6 + I1 act θ 1

2 2 2 2 2 2
+ D111 θ1 + D122 θ2 + D133 θ3 + D144 θ4 + D155 θ5 + D166 θ6
+ 2D112 θ1 θ2 + 2D113 θ1 θ3 + 2D114 θ1 θ4 + 2D115 θ1 θ5 + 2D116 θ1 θ6 (6.49)
+ 2D123 θ2 θ3 + 2D124 θ2 θ4 + 2D125 θ2 θ5 + 2D126 θ2 θ6 + 2D134 θ3 θ4
+ 2D135 θ3 θ5 + 2D136 θ3 θ6 + 2D145 θ4 θ5 + 2D146 θ4 θ6 + 2D156 θ5 θ6 + D1

2 2 2 2 2 2
+ D211 θ1 + D222 θ2 + D233 θ3 + D244 θ4 + D255 θ5 + D266 θ6
2 2 2 2 2 2
+ D311 θ1 + D322 θ2 + D333 θ3 + D344 θ4 + D355 θ5 + D366 θ6
2 2 2 2 2 2
+ D411 θ1 + D422 θ2 + D433 θ3 + D444 θ4 + D455 θ5 + D466 θ6
2 2 2 2 2 2
+ D511 θ1 + D522 θ2 + D533 θ3 + D544 θ4 + D555 θ5 + D566 θ6

2 2 2 2 2 2
+ D611 θ1 + D622 θ2 + D633 θ3 + D644 θ4 + D655 θ5 + D666 θ6
Substituting the numerical values related to the robot in these equations yields the equations of motion for
the robot. These equations can also show how each term can affect the dynamics of the robot or whether a
particular term is important or not. For example, in the absence of gravity, such as in space, the gravity terms
may be neglected. However, inertia terms will be important. On the other hand, if a robot moves slowly, many
terms in these equations that relate to centripetal and Coriolis θj θk accelerations may be negligible. In gen-
eral, using these equations, the robot can be properly designed and controlled.
Example 6.8 Using the previous equations, derive the equations of motion for the 2-DOF robot arm
from Example 6.4, shown in Figure 6.9. The two links are assumed to be of equal length.
Solution:
To use the earlier equations of motion for a 2-DOF robot, we first write the A matrices for the two links,
and then we develop the Dij, Dijk, and Di terms for the robot. Finally, we substitute the results into
y2
x2
2
l, I
D θ2
y1
m2
B x1
y0
l, I 1 C
A
m1 θ1
x0
Figure 6.9 The 2-DOF robot arm from Example 6.4.
Eqs. (6.49) and (6.50) to get the final equations of motion. The joint and link parameters of the robot are
d1 = 0, d2 = 0, a1 = a2 = l, α1 = 0, α2 = 0.
C1 − S1 0 lC1 C2 − S2 0 lC2
S1 C1 0 lS1 S2 C2 0 lS2
A1 = A2 =
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
C12 − S12 0 l C12 + C1
0
S12 C12 0 l S12 + S1
T 2 = A1 A2 =
0 0 1 0
0 0 0 1
From Eq. (6.27),
0 −1 0 0
1 0 0 0
Q revolute =
0 0 0 0
0 0 0 0
From Eq. (6.28), we have
T i ∂ A1 A2 Aj Ai
Uij = ∂0 = = A1 A2 Qj Aj Ai
∂qj ∂qj
Therefore,
−S1 − C1 0 −lS1
C1 − S1 0 lC1 ∂ QA1 ∂ QA1
U11 = QA1 = U111 = = QQA1 and U112 = =0
0 0 0 0 ∂θ1 ∂θ2
0 0 0 0
− S12 − C12 0 − l S12 + S1

C12 − S12 0 l C12 + C1 ∂ QA1 A2 ∂ QA1 A2
U21 = QA1 A2 = U211 = = QQA1 A2 and U212 = = QA1 QA2
0 0 0 0 ∂θ1 ∂θ2
0 0 0 0
−S12 −C12 0 −lS12
C12 − S12 0 lC12 ∂ A1 QA2 ∂ A1 QA2
U22 = A1 QA2 = U221 = = QA1 QA2 and U222 = = A1 QQA2
0 0 0 0 ∂θ1 ∂θ2
0 0 0 0
∂A1
U12 = =0
∂θ2
From Eq. (6.36), assuming that all products of inertia are zero, we get:
1 1 1 1
m1 l2 0 0 − m1 l m2 l2 0 0 − m2 l
3 2 3 2
0 0 0 0 0 0 0 0
J1 = J2 =
0 0 0 0 0 0 0 0
1 1
− m1 l 0 0 m1 − m2 l 0 0 m2
2 2
Notice that since ri in Eq. (6.36) is relative to its own frame, the location of the center of mass relative
to the frame is −l/2. As was mentioned earlier, this is an important issue. Measuring this length relative
to frame 0 would involve components relative to x0, y0, and z0, whereas measuring it relative to frame 1
only requires a length in the –x1 direction. Consequently, for terms requiring l, there is a negative sign,
but for terms requiring l2, the term is positive.
From Eqs. (6.49) and (6.50), for a 2-DOF robot we get:
T1 = D11 θ 1 + D12 θ 2 + D111 θ21 + D122 θ22 + 2D112 θ1 θ2 + D1 + I1 act θ 1 (6.55)
T2 = D21 θ 1 + D22 θ 2 + D211 θ21 + D222 θ22 + 2D212 θ1 θ2 + D2 + I2 act θ 2 (6.56)
From Eqs. (6.43)–(6.45), we have:

T T
D11 = Trace U11 J1 U11 + Trace U21 J2 U21 for i = 1,j = 1, p = 1,2
T
D12 = Trace U22 J2 U21 for i = 1, j = 2,p = 2
T
T
T T
D111 = Trace U111 J1 U11 + Trace U211 J2 U21 , for i = 1, j = 1,k = 1, p = 1,2
T
D122 = Trace U222 J2 U21 for i = 1, j = 2, k = 2, p = 2
T
T
T
T
D1 = − m1 g T U11 r 1 −m2 g T U21 r 2 for i = 1,p = 1,2
D2 = − m1 g U12 r 1 −m2 g U22 r 2
T T
for i = 2,p = 1,2
Although forbiddingly long, even for a 2-DOF robot, substituting all given matrices into these
equations yields:
1 4
D11 = m1 l2 + m2 l2 + m2 l2 C2
3 3
1 1
D12 = D21 = m2 l2 + m2 l2 C2
3 2
1
D22 = m2 l2
3
1
D111 = 0 D112 = D121 = − m2 l2 S2
2
1 1
D122 = − m2 l2 S2 D211 = m2 l2 S2
2 2
D212 = 0 D221 = 0 D222 = 0
and from Eq. (6.45) for g T = 0 −g 0 0 (because acceleration of gravity is in the minus direction of the
y-axis) and r T1 = r T2 = − l/2 0 0 1 (because the center of mass of the bar is at − 2l ), we get:
D1 = −m1 g T U11 r 1 −m2 g T U21 r 2

− S1 − C1 0 − lS1 l − S12 − C12 0 − l S12 + S1 l
− −
2 2
C1 −S1 0 lC1 0 C12 − S12 0 l C12 + C1 0
= − m1 0 − g 0 0 − m2 0 −g 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 1
and similarly, for D2 we get:
1 1
D1 = m1 glC1 + m2 glC12 + m2 glC1
2 2
1
D2 = m2 glC12
2
Substituting the results into Eqs. (6.55) and (6.56) will result in the final equations of motion:
1 4 1 1
T1 = m1 l2 + m2 l2 + m2 l2 C2 θ 1 + m2 l2 + m2 l2 C2 θ 2
3 3 3 2
1 2 1 1
− m2 l2 S2 θ2 − m2 l2 S2 θ1 θ2 + m1 glC1 + m2 glC12 + m2 glC1 + I1 act θ 1
2 2 2
1 1 1 1 1
T2 = m2 l2 + m2 l2 C2 θ 1 + m2 l2 θ 2 + m2 l2 S2 θ 21 + m2 glC12 + I2 act θ 1
3 2 3 2 2
which, except for the actuator inertia terms, are the same as Eqs. (6.11) and (6.12). ■
6.5 Static Force Analysis of Robots
Robots may be under either position control or force control. Imagine a robot that is following a line, say, on
the flat surface of a panel, and is cutting a groove in the surface. If the robot follows a prescribed path, it is
under position control. As long as the surface is flat and the robot is following the line on the flat surface, the
groove will be uniform. However, if the surface is not flat, since the robot is following a given path, it will
either cut deeper into the surface or not cut deep enough. Alternately, suppose the robot were to measure
the force it is exerting on the surface while cutting the groove. If the force becomes too large or too small,
indicating that the tool is cutting too deep or not deep enough, the robot could adjust the depth until it cuts
uniformly. In this case, the robot is under force control.
Similarly, suppose it is required that a robot tap a hole in a machine part. The robot would need to exert a
known axial force along the axis of the hole as well as rotate the tap by exerting a moment on it. To be able to
do this, the controller would need to move the joints and rotate them at particular rates to create the desired
forces and moments at the hand frame.
Collaborative robots need to assess the forces and torques they exert on their counterpart, too, whether
another robot, a part, or a human. If two robots are to work together to accomplish a task, e.g. moving a
large part together, each one must sense and control the forces and torques that it exerts on the part. When
interacting with humans, for example in shaking someone’s hand, the robot must sense and control the
exerted forces and torques by controlling its actuating forces or torques.
To relate the joint forces and torques to forces and moments generated at the hand frame of the robot [1, 9,
10], we define the following:
H T
F = fx fy fz mx my mz (6.57)
where fx, fy, fz are the forces along the x-, y-, and z-axes of the hand frame, and mx, my, mz are the moments
about the x-, y-, and z-axes of the hand frame. Similarly, we define the following:
T
H
D = dx dy dz δx δy δz (6.58)
which are differential displacements and rotations about the x-, y-, and z-axes of the hand frame. We also
define similar entities for the joints as:
T
T = T1 T2 T3 T4 T5 T6 (6.59)
which are the torques (for revolute joints) and forces (for prismatic joints) at each joint, and:
T
Dθ = dθ1 dθ2 dθ3 dθ4 dθ5 dθ6 (6.60)
which describe the differential movements at the joints, either an angle for a revolute joint, or a linear dis-
placement for a prismatic joint.
Using the method of virtual work [11], which indicates that the total virtual work at the joints must be the
same as the total virtual work at the hand frame, we get:
T H T
δW = H
F D = T Dθ (6.61)
or that the forces and moments times the displacements at the hand frame are equal to the torques or forces
times the displacements at the joints. Notice that since work is a scalar, we multiply [HF]T and [T]T (a 1 × 6
matrix) by the displacement matrices (6 × 1 matrices), yielding a single scalar value. Substituting the values,
we get the following for the left-hand side of Eq.(6.61):
fx fy fz mx my mz dx
dy
dz
= fx dx + fy dy + + mz δz (6.62)
δx
δy
δz
However, from Eq. (5.25), we have

T6 T6
D = J Dθ
or
H H
D = J Dθ
Substituting this into Eq. (6.61) results in:
H T H T H T H T
F J Dθ = T Dθ F J = T (6.63)
Referring to Appendix A, this equation can be written as:
H T H
T = J F (6.64)
which indicates that the joint forces and moments can be determined from the desired set of forces and
moments at the hand frame. Since the Jacobian is already known from previous analysis for differential
motions, the controller can calculate the forces and moments at the joints and control the robot based
on the desired values.
Force control of robots may also be accomplished through the use of sensors such as force and torque sen-
sors. This includes robots that can “feel” the object they are handling and that can relay this information back
to the controller or the “master” operator [12]. We will discuss sensors in Chapter 10.
We discussed collaborative robots in Chapter 1 and cooperative robots in Chapter 2. The previous discus-
sion can also be used in the control of these robots. The robot can “sense” the resistive forces exerted on it by a
human or another robot and either respond to it (e.g. stop) or adjust the forces and torques at the joints to not
exceed preset values to avoid injuries to humans or damages to other robots, parts, or itself.
Example 6.9 The numerical value of the Jacobian of a spherical-RPY robot (like the Stanford arm) is
given. It is desired to apply a force of 1 lb along the z-axis of the hand frame, as well as a moment of 20
lb.in about the z-axis of the hand frame, to drill a hole in a block. Find the necessary joint forces and
torques.
20 0 0 0 0 0
−5 0 1 0 0 0
H
0 20 0 0 0 0
J=
0 1 0 0 1 0
0 0 0 1 0 0
−1 0 0 0 0 1
Solution:
Substituting the values given into Eq.(6.64), we get:
H T H
T = J F
T1 20 − 5 0 0 0 −1 0 −20
T2 0 0 20 1 0 0 0 20
F3 0 1 0 0 0 0 1 0
T = = =
T4 0 0 0 0 1 0 0 0
T5 0 0 0 1 0 0 0 0
T6 0 0 0 0 0 1 20 20
As you see, for this instantaneous configuration of the robot and with its specific dimensions, it is
necessary to exert the indicated torques at the first, second, and sixth joints in order to create the
desired force and torque at the hand frame. There is no need for the third joint – the prismatic joint –
to exert any force, even though we want a force at the hand frame. Can you visualize why?
Obviously, as the configuration of the robot changes, so does the Jacobian. Therefore, for continued
exertion of the same force and moment at the hand frame as the robot moves, the joint torques will
have to change, requiring continuous calculation of joint torques by the controller. ■
6.6 Transformation of Forces and Moments between Coordinate Frames
Suppose that a force and a moment are acting on an object, both described relative to a reference frame.
The principle of virtual work can also be used to find an equivalent force and moment relative to another
coordinate frame such that they will have the same effect on the object. To do this, we define F as forces
and moments acting on the object, and D as the displacements caused by these forces and moments, also
relative to the same reference frame, as:
T
F = fx , fy ,fz , mx , my , mz (6.65)
D T = dx , dy , dz , δ x , δ y , δ z (6.66)
We also define BF to be the forces and moments acting on the object relative to a frame B, and BD to be the
displacements caused by these forces and moments, also relative to frame B, as:
T B
B
F = f x , B f y , B f z , B mx , B my , B mz (6.67)
T
B
D = B
d x , B d y , B d z , B δx , B δy , B δz (6.68)
Since the total virtual work performed on the object in either frame must be the same, then:
T T B
δW = F D = B
F D (6.69)
Paul [1] has shown that displacements relative to the two frames are related to each other by the following
relationship:
B B
D = J D (6.70)
Substituting Eq. (6.70) into Eq. (6.69) results in:
T B T B T B T B
F D = F J D or F = F J (6.71)
which can be rearranged to:
B T B
F = J F (6.72)
Paul [1] has also shown that instead of calculating the Jacobian with respect to frame B, the forces and
moments with respect to frame B can be directly found from the following equations:
B
fx=n f
B
fy=o f
B
fz =a f
(6.73)
B
mx = n f × P + m
B
my = o f × P + m
B
mz = a f × P + m
Using Eq. (6.73), we can find equivalent forces and moments in different frames that have the same effect on
an object.
Example 6.10 An object attached to a frame B is subjected to the forces and moments given relative
to the reference frame. Find the equivalent forces and moments in frame B.
F T = 0, 10 lb , 0, 0, 0, 20 lb in
0 1 0 3
0 0 1 5
B=
1 0 0 8
0 0 0 1
Solution:
From the information given, we have:
f T = 0, 10, 0 m T = 0, 0, 20 P T = 3,5,8
n T = 0, 0, 1 o T = 1,0,0 a T = 0,1,0
i j k
f × P = 0 10 0 = i 80 − j 0 + k − 30
3 5 8
f × P + m = 80i − 10k
From Eq. (6.73), we get:
B
fx=n f =0
B
fy=o f =0
B B
f z = a f = 10 f = 0,0,10
B
mx = n f × P + m = − 10
B
my = o f × P + m = 80
B
mz = a f × P + m = 0 B
m = − 10, 80,0
This means that a 10 lb force along the a-axis of frame B, together with two moments of –10 lb in along
the n-axis and 80 lb in along the o-axis, will have the same effect on the object as the original force
and moment relative to the reference frame. Does this match what you learned in your Statics
course? Figure 6.10 shows the two equivalent force-moment systems. ■
z n
Bm
n
Bm
o Bfa
o
a
mz
fx
y
x
Figure 6.10 Equivalent force-moment systems in two different frames.

6.7 Design Project
You may want to continue with the analysis of your robot. You will have to develop the dynamic equations of
motion, which enables you to calculate the power needed at each joint to move the robot at desired accel-
erations. This information may be used for choosing the appropriate actuators as well as in controlling the
robot’s motions.
Alternately, since your robot will not be moving too fast, you can calculate the toque needed at each joint by
trying to find the worst-case situations for each joint. For example, try to model the robot with both links
extended outward. In this case, the first actuator acting on the first joint experiences the largest load. This
estimate is not very accurate, since we are eliminating all coupling inertia terms, Coriolis accelerations, and so
on. But as we discussed earlier, under low-load conditions, and with low velocities, you can get a relatively
acceptable estimate of the torques needed.
6.8 Summary
In this chapter, we discussed the derivation of the dynamic equations of motion of robots. These equations
can be used to estimate the power needed at each joint to drive the robot with desired velocity and accel-
erations. They can also be used to choose appropriate actuators for a robot.
Dynamic equations of multi-DOF, 3D mechanisms such as robots are complicated and, at times, very dif-
ficult to use. As a result, they are mostly used in simplified forms with simplifying assumptions. As an exam-
ple, we may determine the importance of a particular term and its contribution to the total torque or power
needed by considering how large it is relative to other terms. For instance, we may determine the importance
of Coriolis terms in these equations by knowing how large the velocity terms are. Conversely, the importance
of gravity terms in space robots may be determined and, if appropriate, dropped from the equations of
motion.
In the next chapter, we discuss how a robot’s motions are controlled and planned in order to yield a desired
trajectory.
References
1 Paul, Richard P., Robot Manipulators, Mathematics, Programming, and Control, The MIT Press, 1981.
2 Shahinpoor, Mohsen, A Robot Engineering Textbook, Harper and Row Publishers, N.Y., 1987.
3 Asada, Haruhiko, J.-J. E. Slotine, Robot Analysis and Control, John Wiley and Sons, N.Y., 1986.
4 Sciavicco, Lorenzo, B. Siciliano, Modeling and Control of Robot Manipulators, McGraw-Hill, N.Y., 1996.
5 Fu, K.S., R.C. Gonzalez, C.S.G. Lee, Robotics: Control, Sensing, Vision, and Intelligence, McGraw-Hill, 1987.
6 Featherstone, R., “The Calculation of Robot Dynamics Using Articulated-Body Inertias,” The International
Journal of Robotics Research, vol. 2, no. 1, Spring 1983, pp. 13–30.
7 Shahinpoor, M., “Dynamics,” International Encyclopedia of Robotics: Applications and Automation, Richard
C. Dorf, editor, John Wiley and Sons, N.Y., 1988, pp 329–347.
8 Pytel, Andrew, J. Kiusalaas, Engineering Mechanics, Dynamics, 3rd edition, Cengage Learning, Inc., 2010.
9 Paul, Richard, C.N. Stevenson, “Kinematics of Robot Wrists,” The International Journal of Robotics Research,
vol. 2, no. 1, Spring 1983, pp. 31–38.
10 Whitney, D.E., “The Mathematics of Coordinated Control of Prosthetic Arms and Manipulators,” Transactions
of ASME, Journal of Dynamic Systems, Measurement, and Control, 94G(4), 1972, pp. 303–309.
11 Pytel, Andrew, J. Kiusalaas, Engineering Mechanics, Statics, 3rd edition, Cengage Learning, Inc., 2010.
12 Chicurel, Marina, “Once More, With Feeling,” Stanford Magazine, March/April 2000, pp.70–73
Problems
6.1 Using Lagrangian mechanics, derive the equations of motion for a cart with two tires under the cart,
as shown
k
m F
r, I r, I
x
Figure P.6.1
6.2 Calculate the total kinetic energy of the link AB, attached to a roller with negligible mass, as shown.
B
l, I
y
C
θ̇
x
m
A Vo
Figure P.6.2
6.3 Derive the equations of motion for the 2-link mechanism with distributed mass, as shown.
O y0
l 1,
x0 θ1
I1
y1
m1
l2 ,
θ2 I2
x1
m2
Figure P.6.3
6.4 Write the equations that express U62, U35, U53, U623, and U534 for a 6-axis cylindrical-RPY robot in
terms of the A and Q matrices.
6.5 Using Eqs. (6.49)–(6.54), write the equations of motion for a 3-DOF revolute robot and describe
each term.
6.6 Expand the D134 and D15 terms of Eq. 6.49 in terms of their constituent matrices.
6.7 An object is subjected to the following forces and moments relative to the reference frame. Attached to
the object is a frame, which describes the orientation and the location of the object. Find the equivalent
forces and torques acting on the object relative to the current frame.
0 707 0 707 0 2
0 0 1 5
B= F T = 10,0,5,12,20,0 N, N m
0 707 − 0 707 0 3
0 0 0 1
6.8 In order to assemble two parts together, one part must be pushed into the other with a force of 10 lb in
the x-axis direction and 5 lb in the y-direction, and be turned with a torque of 5 lb in along the x-axis
direction. The object’s location relative to the base frame of a robot is described by RT0. Assuming that
the two parts must be aligned together for this purpose, find the necessary forces and moments that the
robot must apply to the part relative to its hand frame.
0 − 0 707 0 707 4
R
1 0 0 6
T0 =
0 0 707 0 707 3
0 0 0 1

Robot Dynamics

Uploaded by

Copyright:

Available Formats

Robot Dynamics

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robot Dynamics

Uploaded by

Copyright:

Available Formats

219

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

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

6.2 Lagrangian Mechanics: A Short Overview

Figure 6.2 Schematic of a simple cart-spring system.

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

Figure 6.4 Schematic of a cart-pendulum system.

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

Figure 6.5 A two-link mechanism with concentrated masses.

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

The kinetic energy for the mass at B is:

The total kinetic energy is:

The Lagrangian for the system is:

The derivatives of the Lagrangian are:

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

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

Figure 6.6 A 2-DOF robot arm.

Therefore, the total velocity of the center of mass of link 2 is:

Figure 6.7 A 2-DOF polar robot arm.

Writing these two equations in matrix form, we get:

6.3 Effective Moments of Inertia

6.4 Dynamic Equations for Multiple-DOF Robots

6.4.1 Kinetic Energy

Figure 6.8 A rigid body in 3D motion and in plane motion.

For a 6-axis robot, this equation can be written as:

where Q2 and Q3 are as defined in Eq. (6.27). ■

Example 6.7 Find an expression for U635 of the Stanford arm.

The kinetic energy of an element of mass dmi on a link is:

xi 2 dmi xi yi dmi xi zi dmi xi dmi

Ixx = y2 + z2 dm, Iyy = x2 + z2 dm, Izz = x2 + y2 dm

Ixy = xydm, Ixz = xzdm, Iyz = yzdm,

mx = xdm, my = ydm, mz = zdm,

6.4.2 Potential Energy

6.4.3 The Lagrangian

6.4.4 Robot’s Equations of Motion

From Eq. (6.28), we have:

T1 = D11 θ 1 + D12 θ 2 + D13 θ 3 + D14 θ 4 + D15 θ 5 + D16 θ 6 + I1 act θ 1

T2 = D21 θ 1 + D22 θ 2 + D23 θ 3 + D24 θ 4 + D25 θ 5 + D26 θ 6 + I2 act θ 2

T6 = D61 θ 1 + D62 θ 2 + D63 θ 3 + D64 θ 4 + D65 θ 5 + D66 θ 6 + I6 act θ 6

Figure 6.9 The 2-DOF robot arm from Example 6.4.

From Eq. (6.27),

From Eq. (6.28), we have

− S12 − C12 0 − l S12 + S1

T1 = D11 θ 1 + D12 θ 2 + D111 θ21 + D122 θ22 + 2D112 θ1 θ2 + D1 + I1 act θ 1 (6.55)

T2 = D21 θ 1 + D22 θ 2 + D211 θ21 + D222 θ22 + 2D212 θ1 θ2 + D2 + I2 act θ 2 (6.56)

From Eqs. (6.43)–(6.45), we have:

D1 = −m1 g T U11 r 1 −m2 g T U21 r 2

and similarly, for D2 we get:

6.5 Static Force Analysis of Robots

However, from Eq. (5.25), we have

6.6 Transformation of Forces and Moments between Coordinate Frames

Figure 6.10 Equivalent force-moment systems in two different frames.

6.7 Design Project

You might also like