Shaoping Bai

Dynamics of robotic manipulators

– Lecture notes for AAU Robotics –

August 27, 2019

Aalborg University, Denmark

Contents

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1 Introduction to manipulator dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Basic concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2.1 Position, velocity, and accelerations . . . . . . . . . . . . . . . . . . . . . 2
1.2.2 Mass and Moment of Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 Free-body diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4 Basic equation of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4.1 Pure translation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.4.2 Fixed-Axis Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2 Kinematics and Statics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.1 Jacobian matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.2 Kinematic analysis of 2D manipulator . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.2.1 Position analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2.2 Velocity analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.3 Singularity analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.4 Acceleration analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.3 2D parallel robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.4 Statics analysis of 2D serial manipulator . . . . . . . . . . . . . . . . . . . . . . . 19

3 Basic dynamics equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.2 Example 1: 1-dof robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.3 Example: 2-dof robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

v
vi Contents

4 Recursive Newtonian Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4.2 Velocity calculation by geometric approach . . . . . . . . . . . . . . . . . . . . 29
4.2.1 Angular velocity and acceleration . . . . . . . . . . . . . . . . . . . . . . . 30
4.2.2 Linear velocity and acceleration . . . . . . . . . . . . . . . . . . . . . . . . 30
4.3 Recursive method for planar manipulators . . . . . . . . . . . . . . . . . . . . . 31
4.3.1 Recursive method in 2D case . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5 Lagrange equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
5.1 Mechanical energies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
5.1.1 Kinetic energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
5.1.2 Potential Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.2 Lagrangian equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.3 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.4 The Lagrange equations of first kind (optional) . . . . . . . . . . . . . . . . . 41

6 3D Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
6.1 Newton-Euler equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
6.2 Lagrange equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
6.3 Recursive Formulation for 3D manipulators (optional) . . . . . . . . . . . 48
6.3.1 Outward Recursion to Calculate Velocities and Accelerations 49
6.3.2 Inward Recursion to Calculate Forces and Torques . . . . . . . . . 51

References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
Preface

This notes is prepared for the second year students of Robotics Programme at
Aalborg University, Denmark. The objective is to provide fundamentals of rigid-
body dynamics and the dynamics of manipulators. In the notes, the basic concepts
of dynamics are reviewed. Two major approaches to establish dynamic equations,
namely, Newton-Euler equations and Lagrange equations, are introduced.
To get an easy start, the notes studies first the planar robotic manipulators, for
which the two approaches of dynamic equation formulation are applied. An ad-
vanced method, namely, the recursive method to calculate dynamics efficiently is
also presented. The notes finally describes the dynamics equations in 3D manipu-
lators. In total, there are six chapters in the notes, covering these contents.
Besides the notes, Slides are also available for the study. The students are en-
couraged to cross-reference two materials in the study.
As this is the first time the notes is prepared, errors and mistakes are inevitable.
The author appreciates any comments on the notes for improvement. Please send
your comments to shb@mp.aau.dk.

Shaoping Bai
Aalborg, Denmark
Aug. 2018 (1st edition); Aug. 2019 (revision)

vii
Chapter 1
Introduction

Summary This chapter is about fundamentals of manipulator dynamics. Basic

concepts of kinematics and dynamics are reviewed. The technique of free-body
diagram for mechanics analysis is described. Basic equations of motion for differ-
ent type of motions, including pure motion, fixed-axis rotation and general plane
motion are developed.

1.1 Introduction to manipulator dynamics

Manipulator dynamics concerns with the actuation forces required to generate

motion of manipulators. The manipulator dynamics is expressed in terms of equa-
tions of motion, which describe the relationships between the actuation, reaction
forces, and the accelerations and motion trajectories of manipulators. In robotics,
dynamics is a fundamental for the manipulator design, analysis and control.
Two types of problems are involved in the dynamics study, namely, the inverse
and forward dynamics. The inverse dynamics calculates the required actuating
forces at joints for a prescribed trajectories or known kinematic information (po-
sition, velocity and acceleration). The forward dynamics, on the other hand, de-
termines the joints’ and end-effector accelerations with applied actuating forces at
joints.
The equations of motion can be established with several approaches. The most
used approaches include recursive Newton-Euler approach, and the Lagrange
equations, which can be found in literatures [1, 2]. In this note the dynamics
equation formulation with Newton-Euler approach will be introduced in Chap-
ters 3 and 4, while the Lagrange equations will be described in Chapter 5. The

1
2 1 Introduction

most chapters cover the study of robot dynamics for planar cases. The last chapter
introduces briefly 3D dynamics.

1.2 Basic concepts

• Dynamics is the branch of mechanics which deals with the motion of bodies
under the action of forces. Dynamics covers two topics: kinematics and kinetics.
• Kinematics is the study of motion without considering forces
• Kinetics relates the action of forces on bodies to their resulting motions.
A rigid body movement is either a rotation, or a translation, or a combination
of the two, i.e., a general motion. A general motion of a rigid body is described
by a linear velocity/acceleration and a angular velocity/acceleartion.
Translation is defined as any motion in which every line in the body remains
parallel to its original position at all times.
Rotation is the movement of a body in a circular motion. Planar motion is a
motion in which all parts of a body move in parallel planes. In analysis of planar
motion, all information is projected on one plane.

1.2.1 Position, velocity, and accelerations

Let the position of one point P in a plane motion is described by vector r. The
trajectory of the point P is a curve Γ . The position of point P is described by its
coordinates, as shown in Fig. 1.1

r(t) = [x, y] (1.1)

Note: both x and y are time dependant, i.e. x = x(t), y = y(t). We drop out the
time parameter for clarity.
The velocity and acceleration of point P can be found by the time derivatives of
its coordinates:
dr
v= = ṙ = [ẋ, ẏ] (1.2)
dt
dv
a= = v̇ = [ẍ, ÿ] (1.3)
dt
1.2 Basic concepts 3

v (t)

r (t)

x
Fig. 1.1 Planar trajectory and velocity

Note:
dx dy
[ẋ, ẏ]=[ , ] (1.4)
dt dt
(1.5)

Example 1
The trajectory of a point P on a body is given by
(a) x = 5 cos 3.14t, y = 5 sin 3.14t
(b) x = 3t 3 + 2t 2 + 1, y = 2t 3 + 4t
Solutions: The velocity and acceleration for the two cases are:
(a)

ṙ = [ẋ, ẏ] = [−15.7 sin 3.14t, 15.7 cos 3.14t] (1.6)

r̈ = [ẍ, ÿ] = [−49.3 cos 3.14t, −49.3 sin 3.14t] (1.7)

(b)

ṙ = [ẋ, ẏ] = [9t 2 + 4t, 6t 2 ] (1.8)

r̈ = [ẍ, ÿ] = [18t + 4, 12t] (1.9)
4 1 Introduction

Example 2: to find the position and velocity of the end point P in the manipu-
lator in Fig.1.2.

C2 l2

2
2
A

C1
l1
1
1
O
x
(a)

y P

s2
r
2

s1

1
O x
(b)
Fig. 1.2 A 2D manipulator

Solution:
1. The position of point P can be found from the geometry of the robot
1.2 Basic concepts 5

r = s1 + s2 (1.10)
[ ] [ ]
l1 cos θ1 l2 cos(θ1 + θ2 )
= + (1.11)
l1 sin θ1 l2 sin(θ1 + θ2 )
[ ]
l1 cos θ1 + l2 cos(θ1 + θ2 )
= (1.12)
l1 sin θ1 + l2 sin(θ1 + θ2 )

2. The velocity
[ ]
ẋ
ṙ = (1.13)
ẏ
[ ]
−l1 θ˙1 sin θ1 − l2 (θ˙1 + θ˙2 ) sin(θ1 + θ2 )
= (1.14)
l1 θ˙1 cos θ1 + l2 (θ˙1 + θ˙2 ) cos(θ1 + θ2 )

3. The acceleration can be found by time derivative of the velocity. We leave this
task to your exercise.

1.2.2 Mass and Moment of Inertia

The mass properties of a rigid body are described by its mass and moment of
inertia.

Fig. 1.3 Illustration of determination of CoM

6 1 Introduction

1. Center of Mass
All rigid bodies have mass. The mass can be assumed to concentrated at one
point, the center of mass.
The center of mass of a rigid body, or a system, is a point at which the body’s
mass can be assumed, for many purposes, to be concentrated. For the system
shown in Fig. 1.3a, the center of mass is defined as

1 n
rG = ( ∑ mi ri ) (1.15)
m i=1
where m is the total mass of the system given by m = ∑ni=1 mi . In the case of
continuous mass distribution, the center of mass is given by
∫
1
rG = ( rdm) (1.16)
m
If the origin of the coordinate system is taken at the center of mass, then r = 0,
which leads to:
n n
∑ miri = ∑ miρ i = 0
i=1 i=1
as shown in Fig. 1.3b.
2. Moment of Inertia about z axis
For a body in translation, the body’s mass determines the required force for a
desired acceleration. Similarly, in rotational motion, there is another physical
property of a rigid body that determines the torque required for a desired rota-
tional acceleration. It is called moment of inertia, also known as angular mass.
The moment of inertia is dependant on both the mass and its distribution.
n
IG = ∑ mi ρi2 (1.17)
i=1

In the case of continuous mass distribution,

∫
IG = ρ 2 dm [kg · m2 ] (1.18)

The moment of inertia I measures the distribution of mass about z axis. A large
moment of inertia implies that the mass is spread far away from the axis and
thus more torque to change the body’s rotation.
Examples:
1.2 Basic concepts 7

(1) A solid round plate of radius r:

1
IG = mr2
2
(2) A solid ring of radius r:
IG = mr2
(3) A bar of length l:
1 2
IG = ml
12
Remark: in these expressions, IG stands for the moment of inertia about CoM
G. It is always a good practice if we use subscript to indicate the reference point
for moment of inertia. If not a subscript used, the moment of inertia is by default
the one about the center of mass.
3. Parallel Axis Theorem
The moment of inertia depends on the body’s mass distribution and also the
axis chosen.
For a body with the moment of inertia IG at the center of mass, the moment of
inertia about an axis passing through another point O and parallel to the z is

IO = IG + md 2 (1.19)

This is the Parallel Axis Theorem. d is the distance between two axes.
Attention has to be paid for formula (1.19). On the right hand, it is the moment
of inertia about the center of mass, not any other arbitrary point.
Example: A uniform bar of length l has a pin joint O at its one end. What is its
moment of inertia about the pin joint?
Solution: By using parallel axis theorem, the moment of inertia about the pin
joint O is
1 1
IO = IG + md 2 = ml 2 + m(l/2)2 = ml 2 (1.20)
12 3
4. External force and internal forces
External forces are forces arising outside a rigid body or a system.
Internal forces are forces due to pairwise particle interactions in a rigid body or
system.
8 1 Introduction

1.3 Free-body diagram

In static and dynamic analyses, free-body diagram is a very useful tool to identify
and show all forces on each individual body and hence to establish correctly the
equation of motion.
A free-body diagram (FBD) is a sketch of an object of interest with all the sur-
rounding objects stripped away and all of the forces acting on the body displayed.
FBD can be established for any body in a system by following steps of:
1. Show the body of interest separately, imagining the body to be separated from
the system (ground, attached bodies, etc)
2. Identify all external forces:
• Driving (active) and reaction (passive) forces
• Frictional force, unless specific assumption made that this force can be ne-
glected
• Gravitational forces
• Other forces applicable
3. Draw forces on the body, paying attention to the points where forces are applied.
Figure 1.4 shows an example of free-body diagram for a planar 2D robotic
manipulator.
Two more remarks on the diagram: (1) In the diagram, each unknown force is
illustrated by a vector with an arbitrary direction; (2) the pairwise reaction forces
in any joint are noted with opposite signs. This is in accordance with Newton’s
Third Law, namely, For every action, there is an equal and opposite reaction.

1.4 Basic equation of motion

For any body in planar motion, its equation of motion is expressed by two sube-
quations, one for translation, one for rotation,

∑ F=maG (1.21)
∑ MG =IGα (1.22)

where:
1.4 Basic equation of motion 9

C2 l2

2
2
A

C1
l1
1
1
O
x

F
r2

-F2
A
2
m2g
r1 2
A

O m1g F2
1

F1
Fig. 1.4 Free-body diagram of a planar 2-D robot

∑ F, ∑ MG —the total force and torque applied on the body

aG — the acceleration of the center of mass, but not other point!
IG — the moment of inertia about the center of mass.
α — the angular acceleration of the rigid body, all parts of the body having the
same angular velocity and angular acceleration.
10 1 Introduction

The above equations can be expressed in slightly different forms, depending on

motion types, as described below.

1.4.1 Pure translation

In the case of pure translation, there is no angular motion, so that both α and ω
are equal to zero. The equation of motion of translation is thus written as:

∑ F=ma (1.23)
∑ MG =IGα = 0 (1.24)

1.4.2 Fixed-Axis Rotation

F F

G
G

aG
O O

f
(a) (b)

Fig. 1.5 Fixed axis rotation

In the fixed-axis rotation, fig.1.5, the equation of motion is simplified as one

rotational equation only.

∑ MO = IOα (1.25)
where O is a point on the axis of rotation.
1.4 Basic equation of motion 11

When the reaction force at the rotational axis is involved, general equation of
motion can be applied

∑ F=maG (1.26)
∑ MG =IGα (1.27)

where the resultant force has to account for the reactions at the axis, i.e.,

∑F = F+f
.
Chapter 2
Kinematics and Statics

Summary This chapter studies the kinematics of robotic manipulators, particu-

larly the planar manipulators. Both forward and inverse kinematics are studied. A
very important concept, the Jacobian, is introduced and discussed. We will look
into how the Jacobian is derived and its implications. Finally, the statics of robotic
manipulators is studied.

2.1 Introduction

Robot kinematics is concerned on the position, velocity and accelerations. A fun-

damental problem is position analysis. There are two types of problems, one is the
forward position problem and the other is inverse position problem.
• Forward position problem is to find the end-effector position/orientation for a
given set of inputs of driving joints.
• Inverse position problem is to find the required inputs to generate a desired
motion at the end-effector.
Position analysis can be done in many approaches. In your previous study of
2nd semester, the algebra approach with homogenous transformation matrix has
been studied. In this course, we will use geometric algebra approach to solve the
position analysis problem.

13
14 2 Kinematics and Statics

2.1.1 Jacobian matrix

An important concept in robot kinematics is the Jacobian matrix, or the Jacobian.

Definition: Given a set y = f (x) of n equations in n variables x1 , ..., xn , written
explicitly as
y = [ f1 (x); f2 (x); . . . ; fn (x)], (2.1)
or more explicitly as

y1 = f1 (x1 , ..., xn ); (2.2)

y2 = f2 (x1 , ..., xn ); (2.3)
... (2.4)
yn = fn (x1 , ..., xn ) (2.5)
the Jacobian matrix, or simply called ”the Jacobian” is defined by
 
∂ y1 ∂ y1
· · ·
 ∂ .x1 . ∂ .xn 
J(x1 , ..., xn ) = 
 .
. . . .. 
 (2.6)
∂ yn ∂ yn
∂x · · · ∂ xn
1

Example: To find the Jacobian for

x = 3 cos θ1 + 2 cos θ2 (2.7)
y = 3 sin θ1 + 2 sin θ2 (2.8)
Using eq. 2.6, we have
[ ]
−3 sin θ1 −2 sin θ2
J= (2.9)
3 cos θ1 2 cos θ2
We will further explore the kinematics and dynamics meanings of the Jacobian.

2.2 Kinematic analysis of 2D manipulator

We take a 2-D planar robot as an example to illustrate the kinematics analysis

of robot. The kinematic analysis covers position analysis, velocity analysis and
accelerations.
2.2 Kinematic analysis of 2D manipulator 15
P

C2 l2

2
2
A

C1
l1
1
1
O
x
(a)

y P

s2
r
2

s1

1
O x
(b)
Fig. 2.1 Kinematic diagram of a 2D manipulator

2.2.1 Position analysis

Refer to Fig. 2.2, the position vector of the tip point P is

r = s1 + s2 (2.10)
that is
[ ] [ ] [ ]
x l1 cos θ1 l2 cos(θ1 + θ2 )
= + (2.11)
y l1 sin θ1 l2 sin(θ1 + θ2 )
16 2 Kinematics and Statics

we have

x = l1 cos θ1 + l2 cos(θ1 + θ2 ) (2.12)

y = l1 sin θ1 + l2 sin(θ1 + θ2 ) (2.13)

Inverse problem: first we found θ2

(x2 + y2 ) − (l12 + l22 )

θ2 = arccos (2.14)
2l1 l2
−l2 sin θ2 x + (l1 + l2 cos θ2 )y
θ1 = arctan (2.15)
l2 sin θ2 y + (l1 + l2 cos θ2 )x

2.2.2 Velocity analysis

By differentiating eqs. (2.12) and (2.13) with respect to time, we obtain the veloc-
ity equation of the manipulator as:

ẋ = −l1 θ˙1 sin θ1 − l2 (θ˙1 + θ˙2 ) sin(θ1 + θ2 ) (2.16)

ẏ = l1 θ˙1 cos θ1 + l2 (θ˙1 + θ˙2 ) cos(θ1 + θ2 ) (2.17)

In matrix form
ẋ = Jθ̇ (2.18)
where [ ]
−l1 sin θ1 − l2 sin(θ1 + θ2 ) −l2 sin(θ1 + θ2 )
J= (2.19)
l1 cos θ1 + l2 cos(θ1 + θ2 ) l2 cos(θ1 + θ2 )
here J is the manipulator’s Jacobian. Equation (2.18) shows a very important
relationship between the velocity of the end-effector of a robot and the joint
velocities—–the velocity of the end-effector of a robot is the linear mapping of
the joint velocities.

2.2.3 Singularity analysis

When the Jacobian’s determinant becomes zero, the manipulator is in singularity.

det(J) = 0 (2.20)
2.3 2D parallel robot 17

After expanding, we have

− sin (θ1 ) cos (θ1 + θ2 ) l1 l2 + sin (θ1 + θ2 ) cos (θ1 ) l1 l2 = 0 (2.21)

Simplifying the equation yields

l1 l2 sin (θ2 ) = 0 (2.22)

That means, Simplifying the equation yields

θ2 = 0 or π (2.23)

Equation (2.23) shows that the robot manipulator becomes singular when θ2 =
0 or 180 degrees. In any condition, the two links of the robot manipulator are
collinear, i.e., in a straight line.

2.2.4 Acceleration analysis

ẍ = Jθ̈ + J̇θ̇ (2.24)

In above equations, the joint velocities are mapped into the end-effector velocity
by means of Jacobian. If we want to find joint velocity for a desired end-effector
velocity, this can be done by inverse transformations:

θ̇ = J−1 ẋ (2.25)

So do for the accelerations:

θ̈ = J−1 (ẍ − J̇θ̇ ) (2.26)

2.3 2D parallel robot

We take one more example, a parallel robot, to demonstrate the kinematic analysis.
A parallel robot is different from a serial manipulator in that the parallel robot
has a several ’limbs’ in parallel connecting to the end-effector. Taking the robot
18 2 Kinematics and Statics
P(x, y) P(x, y)

l3 l4 l3 l4

B D B D

1 1
l1 l2 l1 l2
y y
1 1
A x 2
A x 2

d/2 d/2 C d/2 d/2 C

(a) (b)

Fig. 2.2 Kinematic diagram of a 2D parallel manipulator

in Fig. 2.2 as an example, there are two parallel ’limbs’, or kinematic chains—
-CDP and ABP. The two limbs apply geometric constraints together to the end-
effector for its motion. The kinematic analysis is thus starting from its geometric
constraints.
Refer to Fig. 2.2, the geometric constraints for the two limbs are
−→ −→
CP − CD = l4 (2.27)
−→ − →
AP − AB = l3 (2.28)
substitute
[ ]
−
→ x − d/2
CP = (2.29)
y
[ ]
−→ l3 cos θ2
CD = (2.30)
l3 sin θ2
[ ]
−
→ x + d/2
AP = (2.31)
y
[ ]
−
→ l1 cos θ1
AB = (2.32)
l1 sin θ1
into the above equations, and after squaring both sides, we have
(d/2 + l1 cos (θ1 ) − x)2 + (l1 sin (θ1 ) − y)2 − l3 2 = 0 (2.33)
2 2
(−d/2 + l2 cos (θ2 ) − x) + (l2 sin (θ2 ) − y) − l4 2 = 0 (2.34)
2.4 Statics analysis of 2D serial manipulator 19

The velocity equation is found by differentiating both sides of equations (2.33)

and (2.34) with respect to time
Ja ẋ + Jb θ̇ = 0 (2.35)
J = −J−1 a Jb (2.36)
where
[ ]
−d − 2 l1 cos (θ1 ) + 2 x −2 l1 sin (θ1 ) + 2 y
Ja = (2.37)
d − 2 l2 cos (θ2 ) + 2 x −2 l2 sin (θ2 ) + 2 y
[ ]
jb1 0
Jb = (2.38)
0 jb2
with
jb1 = −l1 sin (θ1 ) d + 2 l1 sin (θ1 ) x − 2 l1 cos (θ1 ) y (2.39)
jb2 = l2 sin (θ2 ) d + 2 l2 sin (θ2 ) x − 2 l2 cos (θ2 ) y (2.40)

2.4 Statics analysis of 2D serial manipulator

We now develop the static equilibrium equation for the 2D planar robot.
We first draw free-body diagram to show all forces on each body. There are two
moving bodies, there FBDs being shown in Fig. 2.3.
From the FBD, the equilibrium equations are obtained as
body 1:
f1 − f2 = 0 (2.41)
τ1 − τ2 + s1 ×(−f2 ) = 0 (2.42)
body 2:

f + f2 = 0 (2.43)
τ2 + s2 ×f = 0 (2.44)
In above equations, we use an operator ×, which is a self-defined symbol. It
stands for the cross product of two planar vectors, from which we get a scalar,
namely, the non-zero entry in the normal three-dimensional cross product.
20 2 Kinematics and Statics
f
P
y

C2 l2

2
2
A

C1
l1
1
1
O
x

(a)

f
P

-f2 s2
A
2

2
s1 A

f2
1
O f1

(b)

Fig. 2.3 Kinematic diagram of a 2D parallel manipulator

We can first find τ2 from equation (2.44), and then substitute into eq. (2.42) to
find τ1 .
τ2 = −s2 ×f (2.45)
τ1 = −(s1 + s2 )×f (2.46)
In expanding form

τ1 = −[l1 cos θ1 + l2 cos(θ1 + θ2 )]v+[l1 sin θ1 + l2 sin(θ1 + θ2 )]u (2.47)

τ2 = −[l2 cos(θ1 + θ2 )]v+[l2 sin(θ1 + θ2 )]u (2.48)
In matrix form

[ ]
−l1 sin θ1 − l2 sin(θ1 + θ2 ) l1 cos θ1 + l2 cos(θ1 + θ2 )
τ = Af; A= (2.49)
−l2 sin(θ1 + θ2 ) l2 cos(θ1 + θ2 )
2.4 Statics analysis of 2D serial manipulator 21

In equation (2.49), matrix A is exactly the transpose of the Jacobian, thus we can
write

τ = JT f; (2.50)
If we know the driving torques and want to find the force generated at end point
P, we can use equation:

f = J−T τ (2.51)

Example:
Refer to Fig. 2.3, the manipulator link lengths are l1 = 3.5m, l2 = 3m; For a
given configuration θ1 = π /6, θ2 = π /6, an external force f = [−4.5, −3]T N is
applied at the end-effector. Try to find the torques at each joint to maintain equi-
librium condition.
Solution: Using eq. (2.50), we can find the torques needed to keep equilibrium
condition is [ ] [ ]
τ1 5.97
τ= T
=J f= N.m (2.52)
τ2 7.19
Chapter 3
Basic dynamics equations

Summary This chapter studies the basic dynamics equations developed particu-
larly for robotic manipulators. The equations are developed based on Newton’s
2nd Law.

3.1 Introduction

We recall that the equation of motion for any moving body can be expressed as

∑ F=maG (3.1)
∑ MG =IGα (3.2)
In the equation,
aG — the acceleration of the center of mass, but not other point!
IG — the moment of inertia about the center of mass.
α — the angular acceleration of the rigid body. All parts of the body have the
same angular velocity and angular acceleration.
For planar movement, in total, there are three equations, two equations for the
x- and the y-direction translation, and the third equation for rotation. However,
the number of equations of motion can reduce to two or even one, depending on
the problem. For example, in the fixed axis rotation, we need only one rotation
equation, if the problem is to find the driving torque for a desired angular acceler-
ation or visa versa. In a pure translation, we need only two translation equations
in order to find the pushing force to generate required acceleration, or visa versa.
If the reactions forces are need to be found, we have to establish all equations.

23
24 3 Basic dynamics equations

3.2 Example 1: 1-dof robot

B B
vc

y l
rc C
mg
A A
x
f0,1

(a) (b)

Fig. 3.1 A pendulum, (a) system, (b) free-body diagram

A 1-dof robot (pendulum) is shown in Fig. 3.1. Taking moment equation about
point A:
l
τ + mg cos θ = IA ω̇ (3.3)
2
that is
1
τ = IA ω̇ − mgl cos θ (3.4)
2
Alternatively, we can build equation including all forces.
f0,1 + mg = mv̇c (3.5)
τ + (−rc )×f0,1 = Ic ω̇ (3.6)
Substitute f0,1 = mv̇c − mg into the second equation
τ − rc ×(mv̇c − mg) = Ic ω̇ (3.7)
note that in the above equations,
[ ]
−rω sin θ
vc = (3.8)
rω cos θ
[ ] [ ]
−rω̇ sin θ −rω 2 cos θ
v̇c = + (3.9)
rω̇ cos θ −rω 2 sin θ
3.3 Example: 2-dof robot 25

where r = l/2, ω = θ̇ .

3.3 Example: 2-dof robot

P
y

C2 l2

. .
. 2 2 vc1
vc1 2
A

. C1
1
l1
1

O 1
x

(a)

s2
-f1,2
2
A
sc2 m2g
sc1 s1 A 2

m1g
O 1

f0,1 f1,2

(b)

Fig. 3.2 2D planar robot dynamics diagram

We look at a 2-dof robotic manipulator. The robot and its free-body diagram
are shown in Fig. 3.2.
26 3 Basic dynamics equations

In the free-body diagram, we use two numbers, separated by comma, in the

subscripts of a force to indicate the two bodies that the reaction force applied.
We will develop explicitly the dynamic equation for each body of the 2D robot.
The dynamic equations for Link 1 are obtained as

f0,1 − f1,2 + m1 g = m1 v̇c1 (3.10)

τ1 − τ2 − sc1 ×f0,1 + (s1 − sc1 )×(−f1,2 ) = I1 ω̇1 (3.11)

Similarly the equations for Link 2 are

f1,2 + m2 g = m2 v̇c2 (3.12)

τ2 − sc2 ×f1,2 = I2 ω̇2 (3.13)

The above equations are the system of dynamic equations for the robot manip-
ulator. The system of equations can be used for both forward and inverse dynamic
problems.
In robotics, we need to find the driving torques at each joints. We thus want to
find closed-form equation for each torque. To this end, we first eliminate f1,2 :

τ2 − sc2 ×(−m2 g + m2 v̇c2 ) = I2 ω̇2 (3.14)

next, eliminating f0,1 :

τ1 − τ2 − sc1 ×m1 v̇c1 − s1 ×m2 v̇c2 + sc1 ×m1 g + s1 ×m2 g = I1 ω̇1 (3.15)

We look further at linear and angular velocities of each body, which are

ω1 = θ̇1 (3.16)
ω2 = θ̇1 + θ̇2 (3.17)

[ ]
−lc1 θ̇1 sin θ1
vc1 = (3.18)
lc1 θ̇1 cos θ1
[ ]
−l1 θ̇1 sin θ1 − (θ̇1 + θ̇2 )lc2 sin(θ1 + θ2 )
vc2 = (3.19)
l1 θ̇1 cos θ1 + (θ̇1 + θ̇2 )lc2 cos(θ1 + θ2 )

By substituting the velocities into Eqs. (3.15) and (3.14), we finally have the final
expression of the closed-form solution:
3.3 Example: 2-dof robot 27

τ1 = H11 θ̈1 + H12 θ̈2 − hθ̇22 − 2hθ˙1 θ˙2 + G1 (3.20)

2
τ2 = H21 θ̈1 + H22 θ̈2 + hθ˙1 + G2 (3.21)

with
2
H11 = m1 lc1 + I1 + I2 + m2 (l12 + lc2
2
+ 2l1 lc2 cos θ2 ) + I2 (3.22)
2
H22 = m1 lc2 + I2 (3.23)
2
H12 = m2 (lc2 + 2l1 lc2 cos θ2 ) + I2 (3.24)
h = m2 l1 lc2 sin θ2 ) (3.25)
G1 = m1 g(lc1 cos θ1 ) + m2 g(l1 cos θ1 + lc2 cos(θ1 + θ2 )) (3.26)
G2 = m2 glc2 cos(θ1 + θ2 ) (3.27)

Equations (3.20) and (3.21) can be put into a matrix in the form of

τ = M(θ )θ̈ + v(θ , θ̇ ) + G(θ ) (3.28)

In the equation:
[ ] [ ] [ ] []
τ1 H11 H12 −hθ̇22 − 2hθ˙1 θ˙2 G1
τ= ; M= ; v= ; G= ;
τ2 H21 H22 hθ˙1
2
G2

The equation (3.28) can be used to solve inverse dynamic problem.

If you want to solve a forward problem, the following equation can be used:

θ̈ = M−1 (θ )(τ − (v(θ , θ̇ ) + G(θ ))) (3.29)

Chapter 4
Recursive Newtonian Method

Summary This chapter formulates the equations of motion for robotic manipula-
tors by means of Recursive Newtonian method.

4.1 Introduction

The motion of a rigid body can be described by the Newton and Euler’s equations,
the former for the translation and the latter for the rotation. Basically, the equations
of motion of a manipulator can be established for each body of the manipulator
on the basis of free-body diagrams. Alternatively, the equations of motion can be
formulated through the recursive Newton-Euler method for serial manipulators,
with which the free-body diagrams can be omitted. The formulation presented
is based on the method introduced by Luh et al. [3]. This formulation is well-
suited for inverse dynamic problems, i.e. to find the driving torques with known
kinematics information.
The recursive Newton-Euler method consists of two steps. The first step is an
outward recursion, from the base to the end-link, to calculate velocity and accel-
eration. The second step is an inward recursion, from the end-link to the base, to
calculate the forces and torques at joints.

4.2 Velocity calculation by geometric approach

We have learned how to calculate velocity and accelerations from the derivatives
of position vectors. Such an approach is algebraic one. As an alternative, veloc-

29
30 4 Recursive Newtonian Method

ity and acceleration can also be found from geometric approach, using relative
velocity and accelerations.

4.2.1 Angular velocity and acceleration

VA
w w

y y

q A VB A
s

f B f
B

w0
r
q0
O

x x
(a) (b)

Fig. 4.1 velocity diagram

Referring to diagram in Fig.4.1a, two links are connected by a pin joint at point
B. Let link OB rotating speed is ω0 , then the angular velocity of link BA can be
found by
ω = ω0 + θ̇ (4.1)
where θ̇ is the rotating speed of link BA relative to link OB.
The acceleration is
ω̇ = ω˙0 + θ̈ (4.2)
where θ̈ is the rotating acceleration of link BA relative to link OB.

4.2.2 Linear velocity and acceleration

Referring to diagram in Fig.4.1b, let the velocity of point B is vB . Then the velocity
of point A can be found by
4.3 Recursive method for planar manipulators 31

vA = vB + ω × s (4.3)
Note that equations (4.2) and (4.3) both are equations of 3-dimensional vectors.
The angular velocity in both equations and the angular speed in equation (4.1) are
related by
ω = ωz (4.4)
with z = [0, , 0, 1]T .
The linear acceleration of point A is

v̇A = v̇B + ω̇ × s + ω × (ω × s) (4.5)

You can try to use algebraic method to verify the above formula.

4.3 Recursive method for planar manipulators

The recursive method is applicable for inverse dynamics problems, that is, we
know the motion of driving joints of a manipulator, then we are able to calcu-
late recursively the accelerations and find forces directly, without establishing and
solving the equation of motion.
The recursive method consists of two recursions, the outward and the inward
recursion:
• Outward recursion — calculate accelerations starting from base link and ending
at the end link
• Inward recursion — calculate forces, starting from the end link, and finishing
at the base link
We start the formulation with a planar case.

4.3.1 Recursive method in 2D case

In the outward recursion, the objective is to calculate all accelerations. This re-
quires the calculation of velocities too, in order to calculate accelerations.
In the outward recursion, the positions, velocities and accelerations (θi , θ̇i , θ̈i
for a revolute joint are known. The outward recursion propagates velocities, and
accelerations of each link from the base to the end link.
32 4 Recursive Newtonian Method

-fi,i+1
C
i+1
C

si .
i .
vci
Ci

sci
. i
i
vi B
B
si-1 i-1 fi-1, i
vi-1 A A

Fig. 4.2 Forces on a single body

The outward recursion to find accelerations takes the following steps, refer to
fig. 4.2:
1. angular velocity
ωi = ωi−1 + θ̇i (4.6)
2. velocity of body attached frame

vi = vi−1 + ωi−1 z × si−1 (4.7)

3. velocity of the center of mass

vci = vi + ωi z × sci (4.8)

4. angular acceleration

ω̇i = ω̇i−1 + θ̈i (4.9)

5. acceleration of body attached frame (origin)

v̇i = v̇i−1 + ω̇i−1 z × si−1 + ωi−1 z × (ωi−1 z × si−1 ) (4.10)

4.3 Recursive method for planar manipulators 33

6. acceleration of CoM

v̇ci = v̇i + ω̇i z × sci + ωi z × (ωi z × sci ) (4.11)

The inward recursion to find driving forces takes the following steps:
1. Inertia forces at CoM of the i-th body

fci = mi v̇ci (4.12)

Mci = Ii ω̇i (4.13)

2. the force and torques applied at joint

fi−1,i = fci + fi,i+1 (4.14)

Mi = Mci + Mi+1 + sci × fci + si × fi,i+1 (in 2D only) (4.15)

Refer to the 2-DOF robot in Fig. 4.3, the recursion for the dynamic analysis can
be carried out straightforward:
1. Outward recursion:
calculate the velocity and accelerations of CoM of body 1, using the known
angular velocity and acceleration of first joint-¿ calculate the velocity and acceler-
ation at the second joint-¿calculate the velocity and acceleration at CoM of body
2.
By this way, we are able to obtain all accelerations at CoM and bodies’ angular
accelerations
2. Inward recursion
The procedure of inward recursion:
• calculate the inertia force for the second body
• ⇒calculate the force and torques applied at the second joint
• ⇒calculate the inertia force for the first body
• ⇒calculate the force and torque at the first joint
With the results of inward recursion, we are able to obtain the the driving
torques at each joint.
34 4 Recursive Newtonian Method
.
vc2
y
I2, m2
l2
sc2
2
.
2, 2, 2
. A
vc1
I1, m1
. sc1
1, 1, l1
1 1
O
x

-f1,2
sc2
2 m2g
sc1 s1 f1,2 2

1
m1g

f0,1
Fig. 4.3 Accelerations and forces notations
Chapter 5
Lagrange equations

In our previous study, we have learned how to developed equation of motion by

means of Newton-Euler equation. An alternative approach of establishing equa-
tions is the Lagrange equation, which is also an approach commonly adopted in
robotics.
While Newton-Euler equations deals directly the relationship among force,
mass and accelerations, the Lagrange equations describe the motion in terms of
the mechanical energy of a motion system. This is the major different between
two approaches.
Two types of Lagrange’s equations can be formulated, one with dependent co-
ordinates (1st kind) and the other with independent coordinates (2nd kind). In
our study, we deal only with the 2nd kind of Lagrange equations, which are of
independent coordinates.
As the Lagrange equation is a mechanical energy-based approach, we study
first the mechanical energies of robots.

5.1 Mechanical energies

The mechanical energy of a robot consists of two types of energies —- kinetic

energy and potential energy. Both energies carry units of Joule (J).

35
36 5 Lagrange equations

5.1.1 Kinetic energy

The kinetic energy of a body is the energy that it possesses due to its motion. It is
defined as the work needed to accelerate a body of a given mass from rest to its
stated velocity.

V VG

y y

x x x

(a) (b) (c)

Fig. 5.1 Three types of rigid-body movements

The kinetic energy expressions for a variety of motion:

• a body in translation (Fig. 5.1a)
1
T = mv2 (5.1)
2
where m—mass, v—velocity of the body
• a body in a fixed axis rotation (Fig. 5.1b)
1
T = Io ω 2 (5.2)
2
where Io — moment of inertia about the fixed axis, ω —angular velocity of the
body
• a body in general motion (motion involving both rotation and translation,
Fig. 5.1c)
1 1
T = IG ω 2 + mv2G (5.3)
2 2
where IG – moment of inertia about the CoM, vG –velocity of the CoM
5.2 Lagrangian equations 37

5.1.2 Potential Energy

Potential energy is the energy possessed by body because of its position relative to
other bodies, deformations, or stresses. Common types of potential energy include
the gravitational potential energy and the elastic potential energy
• Gravitational Potential Energy
V = mgh (5.4)
where m–mass, g–gravity acceleration, h–the height of the body’s CoM relative
to a reference position
• elastic potential energy of a spring
1
V = kx2 (5.5)
2
where k–spring stiffness, x–extension or compression of a spring

5.2 Lagrangian equations

We first define the Lagrangian, which is

L = T −V (5.6)
Lagrange equations are then established as
d ∂L ∂L
( )− = Qi (5.7)
dt ∂ q̇i ∂ qi
where:
L — the Lagrangian of the robot
qi , q̇i — the i-th indepedent general coordinate and velocity
Qi — general force/torque that is applied to the robot with respect to the i-th
coordinate
The Lagrange equation has a very simple form. It deals with only the energies,
which are functions of positions and velocities. So the challenges in developing
dynamic equations by means of Lagrange equation are mainly to find expressions
of velocity and thereafter the energies. Once the Lagrangian is obtained with en-
ergy expressions, the derivation of the Lagrangian equation is very straightfor-
ward.
38 5 Lagrange equations

5.3 Examples

We look at two examples.

Example 1: One-dof robot

l
y C

A
x

Fig. 5.2 1-dof robot

Figure 5.2 shows an one-dof manipulator. We have formulated its equation with
Newton’s 2nd Law. Now we are going to formulate its dynamic equation by means
of the Lagrange equation.
For this 1-dof robot the independent general coordinate is the rotation angle θ .
So all energies and the Lagrangian have to be expressed in terms of θ .
We formulate the equation stepwise as following:
1. The kinetic energy (this is a body rotating about fixed body
1 1 1
T = Io ω 2 = Io ω 2 = Io θ̇ 2
2 2 2
2. The potential energy
l
V = mgy = mg sin θ
2
3. We can now define the Lagrangian
1 l
L = T −V = Io θ̇ 2 − mg sin θ
2 2
5.3 Examples 39

As we can see, the Lagrangian is expressed in the independent general coordi-

nate θ .
4. The Lagrangian equation

d ∂L ∂L
( )− =τ
dt ∂ θ̇ ∂θ
Please pay attention to the general force, which is the torque τ that cooresponds
to rotation θ .
5. The final equation is
l
Io θ̈ + mg cos θ = τ
2
Example 2: 2-dof robotic manipulator

.
vc2
y
I2, m2
l2
rc2
2
.
2, 2, 2
. A
vc1
I1, m1
. rc1
1, 1, l1
1 1
O
x

Fig. 5.3 2-dof robot

We look at a 2-dof robotic manipulator shown in Fig. 5.3.

This is a two-dof robot. The general independent coordinates are θ1 and θ2 ,
their corresponding general forces are torques τ1 and τ2 .
The Lagrange equations for this robot can be formulated by:
1. We need first to find the angular and linear velocities of two moving bodies:
40 5 Lagrange equations

ω1 = θ˙1 (5.8)
ω2 = ω1 + θ˙2 = θ˙1 + θ˙2 (5.9)
[ ]
d d l1 cos θ1 + 12 l2 cos(θ1 + θ2 )
vC2 = (r1 + rC2 ) = ( (5.10)
dt dt l1 sin θ1 + 12 l2 sin(θ1 + θ2 )
2. The mechanical energies of two moving bodies
1 1 ( 2 )
T1 = I1O θ̇12 = θ̇12 m1 lc1 + I1 (5.11)
2 2
V1 = m1 glc1 sin θ1 (5.12)
1 1
T2 = I2 ω22 + m2 vC2 2
(5.13)
2 2
V2 = m2 g(l1 sin θ1 + lc1 sin θ1 ) (5.14)
we substitute expression of ω2 and vC2 into T2 to get its explicit expression, as
given below.
3. Upon the energies obtained, we can now define the Lagrangian
L = T −V = T1 −V1 + T2 −V2
4. The Lagrange equations of the robot is
d ∂L ∂L
( )− = τ1 (5.15)
dt ∂ θ̇1 ∂θ 1
d ∂L ∂L
( )− = τ2 (5.16)
dt ∂ θ̇2 ∂θ 2
The final explicit equations are the same as the results in Chapter 3.

1 ( 2 )
T1 = θ̇12 m1 lc1 + I1 (5.17)
2
V1 = m1 glc1 sin θ1 (5.18)
1 1 1
T2 = lc2 2 m2 θ̇12 + lc2
2
m2 θ̇1 θ̇2 + lc2 2 m2 θ̇22 + m2 θ̇12 l1 2
2 2 2
+ sin (θ1 + θ2 ) sin θ1 l1 lc2 m2 θ̇1 θ̇2 + sin (θ1 + θ2 ) sin θ1 l1 lc2 m2 θ̇12
+ cos (θ1 + θ2 ) cos θ1 l1 lc2 m2 θ̇12 + cos (θ1 + θ2 ) cos θ1 l1 lc2 m2 θ̇1 θ̇2
1 1
+ I2 θ̇12 + I2 θ̇1 θ̇2 + I2 θ̇22 (5.19)
2 2
V2 = m2 g (l1 sin θ1 + lc2 sin (θ1 + θ2 )) (5.20)
5.4 The Lagrange equations of first kind (optional) 41

5.4 The Lagrange equations of first kind (optional)

This section starts with the Lagrange’s equation of 1st kind. Hereafter, the vector
q represents a set of n unknown dependent coordinates, m is the total number of
independent kinematically constraint equations, and f = n − m is the number of
dynamic degrees of freedom [2]. The constraint conditions for the entire system
are written in the following general form:

Φ (q) = 0 (5.21)

Differentiating Eq. (5.21) with respect to time leads to

dΦ (q) ∂ Φ dq
= = Φ q q̇ = 0 (5.22)
dt ∂ q dt
where matrix Φ q is the Jacobian matrix of the constraint equations (5.21).
Let L = T − V be the Lagrangian of the system, where T = T (q, q̇) and
V = V (q) are the kinetic and potential energies, respectively, and Qex is the vector
of generalized external forces acting along the dependent coordinates q. Consid-
ering a manipulator as a constrained multibody system, the Lagrange equation
can be derived by generalizing the Hamilton’s principle by means of Lagrange
multipliers technique[2]. Skipping details of derivation, the equations of motion
become ( )
d ∂L ∂L
− + Φ Tq λ = Qex (5.23)
dt ∂ q̇ ∂q
which is the Lagrange’s equation of the 1st kind. In the equation, λ is a m-
dimensional vector of Lagrange multipliers. Item Φ Tq λ represents the generalized
forces due to reaction forces at joints.
With independent coordinates, the constraint Jacobian vanishes, and the term
with Lagrange multipliers is dropped out. The Lagrange’s equation of 2nd kind is
thus obtained: ( )
d ∂L ∂L
− = Qex (5.24)
dt ∂ q̇ ∂q
The Lagrange equation of 1st kind stands for a system of n equations for n de-
pendent coordinates and m unknown Lagrange multipliers. To solve the equations,
additional acceleration equations from the kinematic constraints are required, as
presented below.
The kinetic energy of a robotic system can be written as follows:
42 5 Lagrange equations
1
T = q̇T M(q)q̇ (5.25)
2
where M(q) = M is the mass matrix of the system. The first and second terms in
Eq. (5.23) can be expressed as below:
( )
d ∂L ∂L
= Mq̈ + Ṁq̇; = Lq = Tq −Vq (5.26)
dt ∂ q̇ ∂q

For the general case in which the kinetic energy depends on q, Eq. (5.23) becomes

Mq̈ + Φ Tq λ = Qex + Lq − Ṁq̇ (5.27)

On the other hand, differentiating the constraint equation (5.21) twice with re-
spect to time yields
Φ q q̇ = 0
Φ q q̈ + Φ̇ (5.28)
Φ q q̇ and n = Qex + Lq − Ṁq̇. Combining Eqs. (5.27) and (5.28) leads
Let c = −Φ̇
to: [ ][ ] [ ]
M Φ Tq q̈ n
= (5.29)
Φq 0 λ c
Equation (5.29) can solve simultaneously the accelerations and the Lagrange mul-
tipliers.
Chapter 6
3D Dynamics

The robot dynamics in 3D is very challenging. The challenges are seen in the
formulations of velocities, accelerations and forces.
In this chapter, we introduce briefly how the two approaches, namely, the
Newton-Euler equations and the Lagrange equations can be applied in the 3D
manipulator dynamics

6.1 Newton-Euler equations

Refer to Fig. 6.1, the dynamics equation for a single body in 3D is expressed as

∑ f = mv̇c (6.1)
∑ n = Iω̇ + ω × (Iω ) (6.2)

where:
∑ f — total force on the body
∑ n — total moment on the body and calculated at the CoM
v̇c — acceleration of the CoM
ω̇ — angular acceleration of the body
I — inertia tensor of the body
If we compare the 3D equations with the 2D ones, we can find there is an
extra term on the second equation, namely, ω × (Iω ). This term accounts for the
contribution of angular velocities. The associated forces include Coriolis forces,
centripetal forces, etc.
In the equation, the inertia tensor can be local or global.

43
44 6 3D Dynamics
ni
Z
.

zi yi

xi f
.
vc
r
Y
X

Fig. 6.1 1-dof robot

A local inertia tensor is the inertia tensor calculated with respect to the body-
attached frame. A local inertia tensor is fixed.
The global inertia tensor, on the other hand, is calculated with respect to the
global frame. As the body is rotating, while the global frame is fixed, the global
inertia tensor is thus varying, rather than fixed.
The local and global inertia tensors are associated by

I = RI′ RT (6.3)

where
I — global inertia tensor of the body
I′ — local inertia tensor of the body
R — rotation matrix of the body
Corresponding to the two tensors, the rotation equation of a 3D body can have
two formats, one in local components, on in global components:

∑ n = Iω̇ + ω × (Iω ) (6.4)

6.1 Newton-Euler equations 45

and

∑ n′ = I′ω̇ ′ + ω ′ × (I′ω ′) (6.5)

We prefer to use the local format, as the local inertia tensor remains unchanged.
Example 1
A gyro is rotating with constant angular velocity in yawing and rolling, ω =
[5, 0, 3], viewed in a local (moving) frame.
The moment of inertia of the gyro is: Ix = 10, Iy = 5, Iz = 5kg.m2 . Try to find
the driving torques of the gyro.
Solutions:
We use the Euler equation with local components

∑ n′ = I′ω̇ ′ + ω ′ × (I′ω ′) (6.6)

= 0 + [0, 75, 0]N.m = [0, 75, 0]N.m (6.7)

There is a torque about y-axis, which implies that the bearing or support to the
shaft has to overcome this torque due to rotation.

Z’

X’

Fig. 6.2 A gyro model

46 6 3D Dynamics

Example 2
Quadrotors are commonly used in today life, such as photography, transporta-
tion, military, etc, due to their ease of construction and control.
A quadrotor is shown in Fig.6.3a. It is flying with angular velocity ω =
[0.5, 0, 0.1] rad/s, viewed in a global (moving) frame. For the given moment,
thrusting forces at each rotor are: F′1 = [0, 0, 3.2]; F′2 = [0, 0, 1.4]; F′3 = [0, 0, 2.5]; F′4 =
[0, 0, 3.6], all in Newton.
The mass is 5 kg and moment of inertia of the dradrotor is: Ix =, Iy = 5, Iz =
10kg.m2 . The distance from each rotor to the center of the quadrotor is 0.5m.
Its orientation at the moment, described by Euler angles (Z-X-Z convention), is
expressed as ϕ = [π /4, 0.1, 0.2]. Try to find the linear and angular acceleration.
We can find solutions though the following steps
• find the rotation matrix

 
0.5532 −0.83002 0.07059
R = Rz (ϕ1 ) · Rx (ϕ2 ) · Rz (ϕ3 ) =  0.8327 0.54906 −0.07059  (6.8)
0.01983 0.09784 0.9950
• To find all forces in theinertial frame, and all torques in the local frame

 
4 0.755333
F = R ∑ Fi + mg =  −0.75533  N (6.9)
i=1 −38.3535
s′i = 0.5[cos((i − 1) · π /2), sin((i − 1) · π /2), 0]T m (6.10)
 
4 −1.1
∑n = R( ∑ s′i × F′i) =  −0.35  N.m (6.11)
i=1 0
 
5.02483 −0.02489 0.35119
 
I = RI′ RT =   −0.02489 5.02483 −0.35119  kg.m2
 (6.12)
0.35119 −0.35119 9.95008

Note: all given forces are expressed in local frame, we need to convert to the
global frame (the inertial frame)
• to find linear and angular accelerations
6.1 Newton-Euler equations 47

(a)
z
B
C
A x
s1 O
Z
D
y Y
X
(b)
F2
v
F3
F1

F4

mg
(c)
Fig. 6.3 A quadrotor, (a) physical system, (b) kinematic diagram, (c) dynamic diagram
48 6 3D Dynamics
 
0.151
v̇ =F/m =  −0.151  m/s2 (6.13)
2.129
 
−0.063869
ω̇ =I−1 (∑ n − ω × (Iω )) =  −0.15480  rad/s2 (6.14)
−0.001131

6.2 Lagrange equations

The Lagrange equations remains the same for both 2D and 3D manipulators.

d ∂L ∂L
( )− = Qi (6.15)
dt ∂ q̇i ∂ qi
But there are slight difference in the expression of energies:
• Ti = 12 mi vci · vci + 21 ω i · (Ii ω i )
• Vi = mi ghi = −mi g · rci
The velocities in 3D space can be determined using equations in following sec-
tions.

6.3 Recursive Formulation for 3D manipulators (optional)

In Chapter 4, we have learned how to formulate recursively Newton-Euler equa-

tions for 2D manipulators. The recursion method for 3D manipulators is the same
as that for the 2D manipulators, with only difference in the formulation. The dif-
ference in formulation is seen in the angular velocity calculation and accelerations.
In the following formulation, the recursive method is applied to a manipula-
tor with n rigid links. Coordinate systems are established by following Denavit-
Hartenberg’s convention (D-H convention) [4].
6.3 Recursive Formulation for 3D manipulators (optional) 49

. .
i . . i +1
vci i

-fi,i+1
sci si

mig -ni,i+1
ni-1,i fi-1,i

Fig. 6.4 The illustration of the symbols for dynamics formulation

6.3.1 Outward Recursion to Calculate Velocities and

Accelerations

6.3.1.1 Revolute joints

Referring to Fig. 6.4, the angular velocity of a revolute joint, propagated from link
i − 1 to link i, is given by
ω i = ω i−1 + θ̇i zi (6.16)
or in local components

Ri ω ′i = Ri−1 ω ′i−1 + θ̇i Ri z′i (6.17)

where:
Ri — the orientation matrix of link i,
ω ′i — the angular velocity of link i with respect to the ith frame (local coordi-
nate system).
Premultiplying both sides of Eq. (6.17) with RTi and simplifying yield

ω ′i = Rii−1 ω ′i−1 + θ̇i z′i (6.18)

here:
Rii−1 — the rotation matrix relating link i − 1 and link i.
z′i — the unit vector [0, 0, 1]T parallel to the zi axis.
50 6 3D Dynamics

Similarly, the propagation of linear velocity from link i − 1 to link i can be

derived as
v′i = Rii−1 (v′i−1 + ω ′i−1 × si−1 ) (6.19)
The angular acceleration is calculated through

ω ′i = Rii−1 ω̇
ω̇ ω ′i−1 + θ̈i z′i + Rii−1 ω ′i−1 × θ̇i z′i (6.20)

The linear acceleration with respect to the link-attached coordinate system is

given by

ω ′i−1 × pii−1 + ω ′i−1 × (ω ′i−1 × si−1 ))

v̇′i = Rii−1 (v̇′i−1 + ω̇ (6.21)

here v̇′i is the linear acceleration of link i in the ith frame.

The linear acceleration of the center of mass of link i is further calculated from
the velocity and acceleration of the link

ω ′i × sC′ i + ω ′i × (ω ′i × sC′ i )
v̇C′ i = v̇′i + ω̇ (6.22)

where sC′ i denotes the position vector of the center of mass of link i in its own
frame.

6.3.1.2 Prismatic joints

Zi-1
i-1

.
vi-1 i-1

di
Yi-1

ri-1, ci
Xi-1

(i-1) vci
(i)

Fig. 6.5 Illustration of prismatic joint

6.3 Recursive Formulation for 3D manipulators (optional) 51

If the joints in question are prismatic, the angular velocities are identical for
link i and link i − 1, which means ω i = ω i−1 .
Referring to Fig. 6.5, expressing with local components yields

Ri ω ′i = Ri−1 ω ′i−1 (6.23)

that is
ω ′i = Rii−1 ω ′i−1 (6.24)
where matrix Rii−1 is independent to rotations.
Likewise, the local angular acceleration of link i is

ω ′i = Rii−1 ω̇
ω̇ ω ′i−1 (6.25)

Furthermore, the local linear velocity and acceleration become

v′i = Rii−1 (v′i−1 + ω ′i−1 × pi−1 ˙ ′

i ) + di zi (6.26)
v̇′i = ω ′i−1 × pi−1
Rii−1 (v̇′i−1 + ω̇ i + ω ′i−1 × (ω ′i−1 × pi−1
i ))
′ ′ ′
+2ω i × di zi + di zi
˙ ¨ (6.27)

where di is the displacement from axis xi−1 to xi along the zi axis.

6.3.2 Inward Recursion to Calculate Forces and Torques

With the velocities and accelerations found, the inertial force and torque applied
to link i at its center of mass can be calculated by applying the Newton-Euler
equations.

fC′ i = mi v̇C′ i (6.28)

ω ′i + ω ′i × Ii ω ′i
nC′ i = Ii ω̇ (6.29)

where mi is the mass of the link, and Ii denotes the inertia tensor of link i about
its center of mass, calculated in a frame identical to the link’s coordinate system
except the origin. In the equations above, all items are expressed in the local co-
ordinate systems.
The forces and torques acting on the links are calculated through the outward
recursion. The readers are referred to [5] for formulation of the forces and torques
recursion of prismatic joint
References

1. J. Angeles. Fundamentals of Robotic Mechanical Systems: Theory, Methods, and Al-

gorithms. Mechanical Engineering Series. Springer International Publishing, 2013.

2. Javier Garcia de Jalon and Eduardo Bayo. Kinematic and Dynamic Simulation of
Multibody Systems: The Real Time Challenge. Springer-Verlag, Berlin, Heidelberg,
1994.

3. J. Y. S. Luh. On-line computational scheme for mechanical manipulators. Trans.

ASME, J. of Dyn. Sys. Mes. and Cont., 102(6):69, 1980.

4. J. Denavit and R.S. Hartenberg. A kinematic notation for lower pair mechanisms based
on matrices. ASME J Appl Mech, 77:215–221, 1955.

5. Shaoping Bai, Lelai Zhou, and Guanglei Wu. Manipulator Dynamics, pages 1855–
1872. Springer London, London, 2015.

53