Bahir Dar Institute of Technology (BIT)

Introduction to Robotics and Automation

Chapter-3: Robot Dynamics

1 Dynamics
Euler-Lagrange
Kinetic Energy
Inertia Tensor Matrix
Potential Energy
Newton-Euler

2 Trajectory Planning
Trajectories for Point to Point Motion
Cubic Polynomial Trajectories
Quintic Polynomial Trajectories
Linear Segments with Parabolic Blends (LSPB)
Minimum Time (BangBang) Trajectories

3 Control Algorithm
 Dynamics

Dynamics
Robot Dynamics is the study of the relation between the applied
forces/torques and the resulting motion.

The dynamic response of the time rate of change of robot config-

uration to input joint torques can be expressed by a second order
nonlinear ordinary differential equations called equation of motion.

τ = f (q, q̇, q̈)

where τ is force/torque or generalized forces and q is joint variable

The reasons to study the dynamics of a manipulator is:

simulation and animation: test desired motions without
resorting to real experimentation
Analysis and synthesis of suitable control algorithms
Dynamics

There are two approaches to formulate the dynamics of a robot

1 Euler-Lagrange
In the Lagrangian formulation we treat the robot as a whole
and perform the analysis using Lagrangian function (the
difference between the kinetic energy and the potential energy)
2 Newton-Euler
In Newton-Euler formulation we treat each link of the robot
separetly, and write down the equations describing its linear
and angular motion of each link.
The two techniques are equivalent (provide the same results).
 Dynamics
Euler-Lagrange

Euler-Lagrange

Lagrangian of the mechanical system can be defined as:

L=T −U

where T and U denote the total kinetic energy and potential energy
of the system, respectively.

Euler-Lagrange equations leads to a second order nonlinear ordinary

differential equations of the form

d ∂L ∂L
− = τi i = 1, ..., n
dt ∂ q̇i ∂qi
where n is the number of joints and τi is the generalized force asso-
ciated with the generalized coordinate qi
Dynamics
Euler-Lagrange

The generalized forces are the sum of the nonconservative forces,

which include the joint actuator torques, joint friction torques, and
joint torques induced by end-effector forces at the contact with the
environment

Therefore, The above equations establish the relations existing be-

tween the generalized forces applied to the robot and the joint po-
sitions, velocities and accelerations.
 Dynamics
Euler-Lagrange

Kinetic Energy
The kinetic energy of a rigid object is the sum of two terms:
1 Translational kinetic energy: obtained by concentrating the
entire mass of the object at the center of mass
2 Rotational kinetic energy of the body about the center of
mass.
For example, for a point mass the kinetic energy is given as
1 1
T = mv 2 + Iω 2
2 2
where v = r ω and r is the distance between the center of rotation
and point mass

Therefore the total kinetic energy is dependent on the angular mo-

tion of the system
T = Iω 2
 Dynamics
Euler-Lagrange

Inertia Tensor
Moment of inertia, usually denoted by I, measures the extent to
which an object resists rotational acceleration about a particular
axis

Let T
i

r= x y z 1
be the coordinates in frame i of the infinitesimal mass dm.

The Inertial tensor with respect to the i th reference frame is given

by R 2 R R R 
R x dm R yxdm 2
R zxdm R xdm
 xydm
R y dm R zydm R ydm

Ji =  R
 xzdm 2
R yzdm R z dm R zdm

R
xdm ydm zdm dm
Dynamics
Euler-Lagrange

where I is the moment of inertia of a 3x3 matrix and given as

R 2 R R 
x dm yxdm zxdm
I =  R xydm R y 2 dm R zydm 
R R R

xzdm yzdm z 2 dm
R R R T
The vector xdm ydm zdm is first moments which
R is the
product of mass and the position of center of mass, and dm is the
mass of the link

Ji is a constant matrix that is evaluated once for each link. It

depends on the geometry and mass distribution of link i.
Dynamics
Euler-Lagrange

If the axes of Link i frame coincide with the central axes of inertia,
then the inertia products are null and the inertia tensor relative to
the centre of mass is a diagonal matrix.

The Inertia Tensor matrix with respect to frame-0 is given by

Ji0 = Hi0 Ji (Hi0 )T
The kinetic energy of link i is written as
1
Ti = trace(Vi0 Ji0 (Vi0 )T )
2
The trace of an n-by-n square matrix is defined to be the sum of the
elements on the main diagonal.

Therefore the total Kinetic energy of the an n-link robot is given as

n
X
T = Ti
i=1
 Dynamics
Euler-Lagrange

Moment of Inertia of Rod

The moment of inertia of a point mass is given by I = mr 2 , but
the rod would have to be considered as an infinite number of point
masses, and each must be multiplied by the square of its distance
from the axis.The general form for the moment of inertia is:

ZM
I = x 2 dm
0
Dynamics
Euler-Lagrange

As shown from the figure above, a thin uniform rod of mass M and
length L is mounted on an axis passing through the end of the rod,
and perpendicular to the plane of the rod.

Choose the origin at the end of the rod and the x-axis oriented along
the rod, positive to the right in the figure. Denote the length of a
small element of the rod by

dl = dx

Since the rod is uniform, the mass per unit length is a constant
dm M
=
dl L
Therefore the mass in the infinitesimal length is given by
M
dm = dx
L
Dynamics
Euler-Lagrange

In order to calculate the moment of inertia through an axis passing

through the end of the rod
Z
Iend = x 2 dm
Z L
M 2
Iend = x dx
0 L

Therefore the moment of inertia of the rod at the end point is

1
Iend = ML3
3
 Dynamics
Euler-Lagrange

Potential Energy
In the case of rigid dynamics, the only source of potential energy is
gravity.

The potential energy of the i-th link can be computed by assuming

that the mass of the entire object is concentrated at its center of
mass and is given by
Ui = Trace(Hgi Ji0 )
where the matrix Ji0 gives the coordinates of the center of mass of
link i with respect to inertial frame. Hgi is a 4x4 matrix, which gives
the direction of gravity in the inertial frame.
 
0 0 0 gx
0 0 0 gy 
Hgi =  0 0 0 gz 


0 0 0 0
Dynamics
Euler-Lagrange

The total potential energy applied by the actuator to resist the grav-
itational potential energy of the n-link robot is given by
n
X
U =− Ui
i=1

In the case that the robot contains elasticity, for example, flexible
joints, then the potential energy will include terms containing the
energy stored in the elastic elements.

Note that the potential energy is a function only of the general-

ized coordinates and not their derivatives, i.e. the potential energy
depends on the configuration of the robot but not on its velocity.
 Dynamics
Euler-Lagrange

Example of Two Link Robot

For Link-1 the Inertia tensor matrix with respect to frame-1 is

given by
 1 2 0 0 − 21 m1 L1

3 m1 L1
 0 0 0 0 
J1 =  
 0 0 0 0 
1
− 2 m1 L1 0 0 m1
Dynamics
Euler-Lagrange

For lumped mass located at center of gravity, the Inertia is given by

1
I = m1 L21
4
The Inertia tensor with respect to frame-0 is given by

J10 = H10 J1 (H10 )T

The kinetic energy of link-1 is given by

1
T1 = trace(V10 J10 (V10 )T )
2
1 d
T1 = ( θ1 (t))2 m1 L21
6 dt
For Lumped mass the kinetic energy is
1 d
T1 = ( θ1 (t))2 m1 L21
8 dt
Dynamics
Euler-Lagrange

The potential energy of link-1 is

U1 = Hg 1 J10

where the Hg 1 is given by

 
0 0 0 0
0 0 0 −g 
Hg 1 =
0

0 0 0 
0 0 0 0

Therefore, the potential energy applied by the actuator to resist the

gravitational potential energy of link-1 is given as
1
U1 = gsin(θ1 (t))L1 m1
2
Dynamics
Euler-Lagrange

Similarly, for link-2 the Inertia tensor with respect to frame-2 is given
by  1 2 0 0 − 21 m2 L2

3 m2 L2
 0 0 0 0 
J2 =  
 0 0 0 0 
1
− 2 m2 L2 0 0 m2
For lumped mass located at center of gravity, the Inertia is given by
1
I = m2 L22
4
The Inertia tensor with respect to frame-0 is given by

J20 = H20 J2 (H20 )T

The kinetic energy of link-2 is given by

1
T2 = trace(V20 J20 (V20 )T )
2
Dynamics
Euler-Lagrange

The potential energy of link-2 is given by

U2 = Hg 2 J20

where the Hg 2 is given by

 
0 0 0 0
0 0 0 −g 
Hg 2 =
0

0 0 0 
0 0 0 0

Therefore, the potential energy applied by the actuator to resist the

gravitational potential energy of link-2 is given as
1
U2 = g (m2 L1 sin(θ1 (t)) + sin(θ1 (t) + θ2 (t))m2 L2 )
2
Dynamics
Euler-Lagrange

The Lagrangian equation of the two link robot is given by

n
X n
X
L= Ti − Ui
i=1 i=1
where the number of joints are n = 2

The Euler-Lagrange equations are

d ∂L ∂L
− = τi i = 1, 2
dt ∂ q̇i ∂qi
For link-1 the torque equation is given by
d ∂L ∂L
τ1 = − for i =1
dt ∂ θ̇1 (t) ∂θ1 (t)
similarly for link-2 the torque equation is given by
d ∂L ∂L
τ2 = − for i =2
dt ∂ θ̇2 (t) ∂θ2 (t)
 Dynamics
Newton-Euler

Newton-Euler

Reading Assignment
 Trajectory Planning

Trajectory Planning

Path and trajectory planning means the way that a robot is moved
from one location to another in a controlled manner.

A trajectory is a function of time q(t) such that q(t0 ) = qinit and

q(tf ) = qfinal . In this case, tf − t0 represents the amount of time
taken to execute the trajectory.

The task of point to point motion is to plan a trajectory from q(t0 ) to

q(tf ), i.e., the path is specified by its initial and final configurations.

In some cases, there may be velocity and acceleration constraints on

the trajectory.
 Trajectory Planning
Trajectories for Point to Point Motion

Trajectories for Point to Point Motion

The objective is to find a joint trajectory q(t) that connects an initial
to a final configuration while satisfying other specified constraints
at the endpoints (e.g., velocity and/or acceleration constraints).

Suppose at time t0 the joint variable satisfies

q(t0 ) = q0
q̇(t0 ) = v0
and we want to attain the values at tf
q(tf ) = qf
q̇(tf ) = vf
In addition, The initial and final acceleration constraints are.
q̈(t0 ) = α0
q̈(tf ) = αf
 Trajectory Planning
Trajectories for Point to Point Motion

Cubic Polynomial Trajectories

The objective is to generate a cubic polynomial function between

two configurations.

If we have four constraints to satisfy, we require a polynomial with

four independent coefficients that can be chosen to satisfy these
constraints.

Thus we consider a cubic trajectory of the form

q(t) = a0 + a1 t + a2 t 2 + a3 t 3

Then the desired velocity is given as

q̇(t) = a1 + 2a2 t + 3a3 t 2

Trajectory Planning
Trajectories for Point to Point Motion

Combining the above two equations with the four constraints yields
four equations and four unknowns at the initial and final configura-
tions
q0 = a0 + a1 t0 + a2 t02 + a3 t03
v0 = a1 + 2a2 t0 + 3a3 t02
qf = a0 + a1 tf + a2 tf2 + a3 tf3
vf = a1 + 2a2 tf + 3a3 tf2
These four equations can be combined into a single matrix equation
1 t0 t02 t03
    
a0 q0
0 1 2t0 3t 2  a1   v0 
0  
1 tf t 2 t 3  a2  = qf 
  
f f
0 1 2tf 3tf2 a3 vf
The determinant of the coefficient matrix is (tf −t0 )4 and the above
equation always has a unique solution if a nonzero time interval is
allowed for the execution of the trajectory.
 Trajectory Planning
Trajectories for Point to Point Motion

Example

It is desired to have the first joint of a two link robot moves from
initial angle of 30o to a final angle of 75o in 5 seconds. Using a
third-order polynomial, calculate the joint angle at 1, 2 3, and 4
seconds.
 Trajectory Planning
Trajectories for Point to Point Motion

Quintic Polynomial Trajectories

A cubic trajectory gives continuous positions and velocities at the
start and final points but discontinuities in the acceleration.

A discontinuity in acceleration leads to an impulsive jerk, which

may excite vibrational modes in the manipulator and reduce tracking
accuracy.

For this reason, we have to specify constraints on acceleration in

addition to position and velocity.

Therefore to include the six constraints, we need a fifth order poly-

nomial of the form

q(t) = a0 + a1 t + a2 t 2 + a3 t 3 + a4 t 4 + a5 t 5
Trajectory Planning
Trajectories for Point to Point Motion

From the above equation we obtain the following equations

q0 = a0 + a1 t0 + a2 t02 + a3 t03 + a4 t04 + a5 t05
v0 = a1 + 2a2 t0 + 3a3 t02 + 4a4 t03 + 5a5 t04
α0 = 2a2 + 6a3 t0 + 12a4 t02 + 20a5 t03
qf = a0 + a1 tf + a2 tf2 + a3 tf3 + a4 tf4 + a5 tf5
vf = a1 + 2a2 tf + 3a3 tf2 + 4a4 tf3 + 5a5 tf4
αf = 2a2 + 6a3 tf + 12a4 tf2 + 20a5 tf3
which can be written as
t0 t02 t03 t04 t05
    
1 a0 q0
0 1 2t0 3t02 4t03 5t04  a1   v0 
0 2 6t0 12t02 20t03 
    
0
 a2  = α0 
   

1 tf tf2 tf3 tf4 5
tf   a3   qf 
   

0 2
1 2tf 3tf 4tf 3 4
5tf  a4   vf 
0 0 2 6tf 12tf 20tf3
2 a5 αf
 Trajectory Planning
Trajectories for Point to Point Motion

Linear Segments with Parabolic Blends (LSPB)

This type of trajectory is appropriate when a constant velocity is

desired along a portion of the path.

The LSPB trajectory is such that the velocity is initially ramped up

to its desired value and then ramped down when it approaches the
goal position.

To achieve this we specify the desired trajectory in three parts.

The first part from time t0 to time tb is a quadratic polynomial.

This results in a linear ramp velocity.

At time tb , called the blend time, the trajectory switches to a linear

function. This corresponds to a constant velocity.
Trajectory Planning
Trajectories for Point to Point Motion

Finally, at time tf − tb the trajectory switches to a quadratic poly-

nomial so that the velocity is linear.

We choose the blend time tb so that the position curve is symmetric

as shown in the figure below. For convenience suppose that t0 = 0
and q̇(tf ) = 0 = q̇(0). Then between times 0 and tb we have
q(t) = a0 + a1 t + a2 t 2
Trajectory Planning
Trajectories for Point to Point Motion

So that the velocity is

q̇(t) = a1 + 2a2 t
The constraints q(0) = q0 and q̇(0) = 0 imply
a0 = q0
a1 = 0
At time tb the velocity starts to be constant, say v . Thus, we have
v
q̇(t) = 2a2 tb = v which gives a2 =
2tb
Therefore the required trajectory between 0 and tb is given as
v 2 α
q(t) = q0 + t = q0 + t 2
2tb 2
v
q̇(t) = t = αt
tb
v
q̈(t) = =α
tb
where α denotes the acceleration.
Trajectory Planning
Trajectories for Point to Point Motion

Now, between time tb and tf − tb , the trajectory is a linear segment

(corresponding to a constant velocity v )

q(t) = a0 + a1 t = a0 + vt

Since, by symmetry,
tf q0 + qf
q( )=
2 2
we have
q0 + qf tf
= a0 + v
2 2
which implies that
q0 + qf − vtf
a0 =
2
Since the two segments must blend at time tb
v q0 + qf − vtf
q0 + tb = + vtb
2 2
Trajectory Planning
Trajectories for Point to Point Motion

Solving for the blend time tb

q0 − qf + vtf
tb =
v
Note that we have the constraint 0 < tb ≤ t2f . This leads to the
inequality
qf − q0 qf − q0
< tf ≤ 2
v v
To put it another way we have the inequality
qf − q0 qf − q0
<v ≤2
tf tf
Thus the specified velocity must be between these limits or the
motion is not possible.
Trajectory Planning
Trajectories for Point to Point Motion

The portion of the trajectory between tf − tb and tf is now found

by symmetry considerations. The complete LSPB trajectory is given
by
q0 + α2 t 2

 0 ≤ t ≤ tb
q0 +qf −vtf
q(t) = 2 + vt tb < t ≤ tf − tb
αtf2

qf − 2 + αtf t − α2 t 2 tf − tb < t ≤ tf

 Trajectory Planning
Trajectories for Point to Point Motion

Minimum Time (BangBang) Trajectories

This trajectory is obtained by leaving the final time tf unspecified

and seeking the fastest trajectory between q0 and qf with a given
constant acceleration α, that is, the trajectory with minimum final
time tf .

The optimal solution is achieved with the acceleration at its maxi-

mum value +α until an appropriate switching time ts at which time
it abruptly switches to its minimum value +α (maximum decelera-
tion) from ts to tf

We assume that the trajectory begins and ends at rest, that is,
with zero initial and final velocities, symmetry considerations would
suggest that the switching time ts is just t2f .
Trajectory Planning
Trajectories for Point to Point Motion

If we let Vs denote the velocity at time ts then we have

Vs = αts

and also
q0 − qf + vs tf
ts =
vs
tf
the symmetry condition ts = 2 implies that
qf − q0
vs =
ts
Combining these two we have the conditions
qf − q0
= αts
ts
which implies that r
qf − q0
ts =
α
 Control Algorithm

Control Algorithm
The dynamic equations of a serial rigid manipulator is given as:
M(q)q̈ + C (q, q̇)q̇ + G (q) = τ
where, M(q) is an nxn inertia matrix of a manipulator, C (q, q̇) is a
nxn matrix related to the centrifugal and Coriolis terms, G (q) is an
nx1 vector related to gravitational force and τ is the joint generalized
force.

The purpose of this section is to figure out a strategy of calculating

the torque τi that each motor should produce in order to follow a
desired trajectory (qd ).

In order to ensure trajectory qd tracking by the joint variables, we

define the tracking error as follows:
e = qd − q
Control Algorithm

The velocity and acceleration error is given as

ė = q̇d − q̇ and ë = q̈d − q̈

From the dynamic equation the actual acceleration q̈ is computed

as
q̈ = M(q)−1 (−C (q, q̇)q̇ − G (q) + τ )
substituting acceleration error into the above equation gives as

ë = q̈d + M(q)−1 (C (q, q̇)q̇ + G (q) − τ )

The control input function u = ë

u = q̈d + M(q)−1 (C (q, q̇)q̇ + G (q) − τ )

The following tracking error dynamics equation is derived as

      
ė 0 I e 0
= + u
ë 0 0 ė I
Control Algorithm

The above equation shows how the tracking error evolves over time
with respect to the control input u. The equation is linear and
time-invariant system.

The state space representation is

ẋ = Ax + Bu,
 T
where x = e ė are the states
To stabilize the system we have to choose a control input
 
u = −Kx = − Kp Kd x = −Kp e − Kd ė

This means that choosing a control input u = ë is much easier than

choosing a stabilizing τ as a control input. For each value of u the
corresponding torque/force can be calculated by

τ = M(q)(q̈d − u) + C (q, q̇)q̇ + G (q)

Control Algorithm

This is known as the computed-torque control law.