Department of Mechanical Engineering
University of Bath
ME40331
Robotics Engineering
Lecture Notes
Dr Min Pan
Academic Year 2024/25
This page is intentionally left (almost) blank
Contents
Preface
vii
0.1 Module Aim and Objectives . . . . . . . . . . . . . . . . . . . . vii
0.2 Reading List . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
1
Introduction
1.1 Robot structure . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2 Robot forward kinematics . . . . . . . . . . . . . . . . . . . . . .
1.3 Robot inverse kinematics . . . . . . . . . . . . . . . . . . . . . .
1.4 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.5 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . .
1.6 Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . .
1.7 Transform Derivatives . . . . . . . . . . . . . . . . . . . . . . .
1.8 Pseudo-inertia Matrices . . . . . . . . . . . . . . . . . . . . . . .
1.9 Control Architecture . . . . . . . . . . . . . . . . . . . . . . . .
1.10 Principle of Virtual Work . . . . . . . . . . . . . . . . . . . . . .
1.11 Inverse Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . .
1.12 Lagrangian Mechanics . . . . . . . . . . . . . . . . . . . . . . .
1
1
2
3
4
4
4
4
5
5
5
5
6
2
Kinematics
2.1 Summary and learning outcomes . . . . . . . . . . . . . . . . . .
2.2 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3 Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . . .
2.4 Rigid motions and homogeneous transformations . . . . . . . . .
2.4.1 Rotations in three dimensions . . . . . . . . . . . . . . .
2.4.2 Rotation matrix as a mapping . . . . . . . . . . . . . . .
2.4.3 Rotation matrix as a vector operator . . . . . . . . . . . .
2.4.4 Translations . . . . . . . . . . . . . . . . . . . . . . . . .
2.4.5 General transformations . . . . . . . . . . . . . . . . . .
2.4.6 Parametrisation of rotations by Euler and Fixed angles . .
2.5 Homogeneous transformations . . . . . . . . . . . . . . . . . . .
2.5.1 Composition of Homogeneous transformations . . . . . .
7
7
7
8
10
11
12
14
15
15
17
21
22
iii
iv
CONTENTS
2.6
Denavit-Hartenberg (D-H) convention . . . . . . . . . . . . . . .
2.6.1 DH Parameters . . . . . . . . . . . . . . . . . . . . . . .
2.6.2 Assigning coordinate frames . . . . . . . . . . . . . . . .
Forward Kinemtics Matlab Example . . . . . . . . . . . . . . . .
Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . .
2.8.1 Kinematic decoupling . . . . . . . . . . . . . . . . . . .
2.8.2 Geometric solution . . . . . . . . . . . . . . . . . . . . .
Tutorial Questions . . . . . . . . . . . . . . . . . . . . . . . . . .
24
24
26
28
30
30
33
35
3
Jacobian
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2 Example 2 DOF planar robot . . . . . . . . . . . . . . . . . . . .
3.3 Direct differentiation . . . . . . . . . . . . . . . . . . . . . . . .
3.4 Explicit Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . .
3.4.1 Link velocity . . . . . . . . . . . . . . . . . . . . . . . .
3.4.2 Jacobian computation using explicit method . . . . . . . .
3.5 Tutorial Questions . . . . . . . . . . . . . . . . . . . . . . . . . .
43
43
43
44
46
46
47
50
4
Trajectory Planning
4.1 Joint-Space Schemes . . . . . . . . . . . . . . . . . . . . . . . .
4.1.1 Cubic Polynomials . . . . . . . . . . . . . . . . . . . . .
4.1.2 Cubic Polynomials and Via Points . . . . . . . . . . . . .
4.1.3 Higher Order Polynomials . . . . . . . . . . . . . . . . .
4.1.4 Linear Segments with Parabolic Blends . . . . . . . . . .
4.2 Cartesian-Space Schemes . . . . . . . . . . . . . . . . . . . . . .
4.3 Tutorial Questions . . . . . . . . . . . . . . . . . . . . . . . . . .
51
53
53
54
58
62
65
68
5
Equations of Motion
5.1 Introduction & learning outcomes . . . . . . . . . . . . . . . . .
5.2 Generalised Coordinates and Generalised Inputs . . . . . . . . . .
5.3 Matrix-Vector Equations of Motion . . . . . . . . . . . . . . . . .
5.4 Types of dynamical problem . . . . . . . . . . . . . . . . . . . .
5.5 Constructing the equations of motion . . . . . . . . . . . . . . . .
69
69
70
71
74
74
6
Transform Derivatives
6.1 Introduction & learning outcomes . . . . . . . . . . . . . . . . .
6.2 Velocities and Accelerations . . . . . . . . . . . . . . . . . . . .
6.3 Homogeneous Transform Derivatives . . . . . . . . . . . . . . .
6.4 Relationship to Jacobian . . . . . . . . . . . . . . . . . . . . . .
6.4.1 A note on symbols used in dynamics . . . . . . . . . . . .
75
75
76
77
79
80
2.7
2.8
2.9
ROBOTICS ENGINEERING
2024/2025
CONTENTS
v
7
Pseudo-inertia Matrices
7.1 Introduction & learning outcomes . . . . . . . . . . . . . . . . .
7.2 Mass moments of inertia . . . . . . . . . . . . . . . . . . . . . .
7.2.1 Moments of inertia . . . . . . . . . . . . . . . . . . . . .
7.2.2 Products of inertia . . . . . . . . . . . . . . . . . . . . .
7.2.3 Parallel axis theorem . . . . . . . . . . . . . . . . . . . .
7.2.4 Inertia tensor . . . . . . . . . . . . . . . . . . . . . . . .
7.3 Kinetic Energy . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4 Pseudo-inertia Matrix . . . . . . . . . . . . . . . . . . . . . . . .
81
81
82
82
82
83
84
84
87
8
Control Architecture
8.1 Introduction & learning outcomes . . . . . . . . . . . . . . . . .
8.2 Operator interface . . . . . . . . . . . . . . . . . . . . . . . . . .
8.3 Controller Architecture . . . . . . . . . . . . . . . . . . . . . . .
8.4 Feedback Control . . . . . . . . . . . . . . . . . . . . . . . . . .
8.5 Feedforward Control . . . . . . . . . . . . . . . . . . . . . . . .
8.6 Inverse Dynamics for a Serial Robot . . . . . . . . . . . . . . . .
89
89
90
91
93
94
95
9
Principle of Virtual Work
99
9.1 Introduction & Learning Outcomes . . . . . . . . . . . . . . . . . 99
9.2 Principle of Virtual Work . . . . . . . . . . . . . . . . . . . . . . 100
9.3 Gravity Compensation . . . . . . . . . . . . . . . . . . . . . . . 102
10 Inverse Dynamics
105
10.1 Introduction & learning outcomes . . . . . . . . . . . . . . . . . 105
10.2 Gravitational forces . . . . . . . . . . . . . . . . . . . . . . . . . 105
10.3 Inertial Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
10.4 Equations of Motion for Serial Manipulators . . . . . . . . . . . . 109
11 Lagrangian Mechanics
111
11.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
11.2 Intended Learning Outcomes . . . . . . . . . . . . . . . . . . . . 112
11.3 Lagrangian Mechanics . . . . . . . . . . . . . . . . . . . . . . . 112
11.4 Generalised Coordinates . . . . . . . . . . . . . . . . . . . . . . 116
11.5 Planar Two-link Manipulator . . . . . . . . . . . . . . . . . . . . 117
11.6 Inverse Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 119
A Matrix Inversion
2024/2025
123
ROBOTICS ENGINEERING
vi
CONTENTS
B Lagrangian Inverse Dynamics Derivation
125
B.1 Potential Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
B.2 Kinetic Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
B.3 Lagrangian and Equations of Motion . . . . . . . . . . . . . . . . 128
B.4 Application of equations of motion . . . . . . . . . . . . . . . . . 131
ROBOTICS ENGINEERING
2024/2025
Preface
0.1
Module Aim and Objectives
The module aim is to introduce the theory and practice or robotic manipulators.
Two main subject areas are covered in the course (i) Robot kinematics, Jacobian
and trajectory planning, (ii) Robot dynamics and control. Each of these subject
areas are briefly introduced in the following Chapter.
After taking this course, students should be able to:
• Compute forward and inverse kinematics for common manipulator designs
• Plan robot trajectories
• Compute dynamic analysis for robotic systems
• Develop high-fidelity controllers for robot manipulators
0.2
Reading List
• M.W. Spong, S. Hutchinson and M.Vidyasagar. Robot modeling and control. Willey 2006
• J. J. Craig. Introduction to robotics mechanics and control. Pearson 2005
• S.V. Niku. Introduction to robotics analysis, control, applications. Willey
2010
• L.Sciavicco, B.Siciliano. Modelling and Control of Robot Manipulators.
Springer 2005
• S. Niku. Introduction to Robotics. 2nd Edition. John Wiley & Sons 2010
vii
This page is intentionally left (almost) blank
Chapter 1
Introduction
1.1
Robot structure
Currently, there are many types of robots being developed and used, including,
autonomous cars, space exploration rovers, UAVs, humanoids, exoskeletons and
so on. In this course we will concentrate on industrial robots or robot arms as these
share the fundamental principles that apply to other systems. In general, robot
arms are actuated and instrumented at their joints. In other words, the have motors
to drive their links and sensors to measure their position. Commonly, servomotors
are used to actuate robots, these systems provide with position feedback usually
using some sort of rotational encoder attached to the motor.
Figure 1.1: Common structure for a 6DOF robot
1
2
CHAPTER 1 Introduction
Figure 1.1 illustrates a common 6DOF robot structure. As it can be observed,
each of the DOF is a revolute joint actuated by a servomechanism on each joint.
Figure 1.2 illustrates a diagram of a servo motor and control unit. As it can be
observed, the servo motor feeds back encoder-based position information to the
controller and this drives the current to the motor (AC Servo in this particular
example).
Figure 1.2: Servo motor diagram
Using this general structure has two important implications:
• Robots can only measure the relative position of their links
• Robots can only actuate links to change their relative position
This means that robots can only measure and actuate a link with respect to its
predecessor, in other words, the position of Second arm in Figure 1.1 can only be
measured and actuated with respect to First arm.
1.2
Robot forward kinematics
One of the most important aspects of robot control is to be able to position its
end-effector accurately. For example, if the robot is being used for a welding
application, it is important that its end-effector or tool are positioned exactly where
the weld is required.
ROBOTICS ENGINEERING
2024/2025
Section 1.3 Robot inverse kinematics
3
As discussed previously, given the robot’s structure the position of each link
is only known in relation with its predecessor. Therefore, in order to calculate the
position of the end-effector it will be required to perform a calculation that maps
joint (or angle) positions into end-effector ones. This analysis, of mapping joint
positions into end-effector ones is known as forward kinematics.
Figure 1.3: Forward kinematics
For example, in Figure 1.3, the forward kinematics analysis will solve the
position and orientation for the end-effector (reference frame {3}) with respect to
the robot’s base (frame {0}) given the joint angle values θ1 , θ2 , θ3 .
1.3
Robot inverse kinematics
In most cases, the required position for the robot will be defined in terms of its endeffector position. For example, in the welding task, the robot’s required position
will be relating to the welding locations. In other words, the robot’s task will be
given in terms of the position of the tool.
Given that a robot can only actuate its joints angle, a relation between endeffector and robot joints is required. As we have seen in the previous Section,
the forward kinematics maps joint angles into end-effector position. The inverse
problem, mapping end-effector positions into joint angles is known as inverse
kinematic analysis. In other words, inverse kinematics deals with the question,
what are the joint angles required to position the end-effector at a desired location?
2024/2025
ROBOTICS ENGINEERING
4
1.4
CHAPTER 1 Introduction
Jacobian
The forward kinematic analysis maps joint positions into end-effector ones. By
taking the derivative of this mapping, it is possible to calculate the relation between joint velocity and end-effector velocity. In most cases, this derivative can
be represented by a matrix called the Jacobian Matrix.
The Jacobian is extensively used in robotics as it allows to control the robot
in a simple manner. Given that the Jacobian relates joint velocities to end-effector
ones, it is possible to use the inverse of the Jacobian to relate end-effector velocities to joint velocities. In other words, if the trajectory required for the end-effector
can be differentiated (i.e. expressing the trajectory in terms of velocity) then the
inverse of the Jacobian can be used to control the joints of the robot. This simplifies the problem of having to compute the inverse kinematics of each desired
point.
It will be shown that the Jacobian is also useful to convert forces at the endeffector into joint torques. In other words, by using the Jacobian, it will be possible
to control the force of the end-effector by controlling the joint/motor torques.
1.5
Trajectory Planning
Chapter 4 is concerned with describing the motion of the manipulators in either
joint space (motor angles) or workspace (Cartesian) coordinates. Trajectory planning involves in determining the time history of position, velocity, and acceleration of each degree of freedom. In most industrial robots, the trajectory generation is performed automatically from the user specified motion. This chapter will
cover typical trajectory generation algorithms to move the end effector between
two positions via a set of desired points.
1.6
Equations of Motion
Chapter 5 presents key concepts for dynamic analysis including how to choose
the generalised coordinates to be used to represent the system dynamics. The full
nonlinear equations of motion needed for analysing robot systems are presented,
and this chapter lays the foundation for the dynamics analyses that follow.
1.7
Transform Derivatives
Chapter 6 considers the computation of velocities and accelerations. Methods
based on Denavit-Hartenberg transform matrices are developed, and these rely on
ROBOTICS ENGINEERING
2024/2025
Section 1.8 Pseudo-inertia Matrices
5
derivatives of the transform matrices. Analytical and computational methods for
evaluating these transform derivatives are explored.
1.8
Pseudo-inertia Matrices
Chapter 7 revises inertial properties for rigid bodies, including moments and products of inertia. Computational methods for assembling the equations of motion
rely on something called the pseudo-inertia matrix, which contains all of the relevant inertial information. An example using a kinetic energy calculation is presented to show why the pseudo-inertia matrix is the preferred arrangement for this
information, and it is shown how to compute pseudo-inertia matrices for simple
link shapes.
1.9
Control Architecture
Chapter 8 details the arrangement of the components in a robot controller, integrating the kinematic and dynamic computations along with feedback control to
produce a high fidelity motion trajectory in the real world. The full computational
framework for the inverse dynamics is presented here for the first time, and it is
shown how this is used and the effects of the different components.
1.10
Principle of Virtual Work
Chapter 9 explains the principle of virtual work and it is shown how this allows
forces to be transformed between coordinate systems. The chapter concludes by
demonstrating how this technique can be used analytically to calculate the control
forces needed to oppose gravity and maintain static equilibrium.
1.11
Inverse Dynamics
Chapter 10 uses the principle of virtual work to derive computational methods
which can be used to determine the equations of motion for a serial manipulator
based on knowledge of the kinematic transform matrices (covered in Chapter 2).
The equations of motion can be used in inverse dynamic calculations to determine the control forces needed to follow a given motion trajectory. This chapter
provides the background theory for the methods first presented in Chapter 8.
2024/2025
ROBOTICS ENGINEERING
6
1.12
CHAPTER 1 Introduction
Lagrangian Mechanics
Chapter 11 deals with more general relationships between robot motion and forces
/ torques arising from the actuators or external effects. There are two problems related to dynamics. The first one is the forward dynamics, where the motion is calculated from the input forces/torques (useful for simulating robot behaviour), and
the other is the inverse dynamics, where the required driving inputs are calculated
from the desired or specified motion (useful for controlling robots). Lagrangian
dynamics is commonly used in robot dynamics due to its versatility. It is used in
this chapter for both forward and inverse dynamic formulations.
ROBOTICS ENGINEERING
2024/2025
Chapter 2
Kinematics
2.1
Summary and learning outcomes
This chapter introduces the concepts of forward and inverse kinematics. First, it
develops the basic geometric representations and manipulations required to understand the mechanisms that drive the computation of forward kinematics. Secondly, a the Denavit-Hartenberg (DH) convention is introduced to simplify and
mechanise the definition of coordinate frames. Finally, the chapter describes how
to compute the inverse kinematics of a robot.
The learning outcomes for this chapter are:
1. Concept and computation of robot forward kinematics
2. Understanding and performing rigid body transformations
3. Usage of homogeneous coordinates in rigid body transformations
4. Application of the Denavit-Hartenberg convention
5. Concept and computation of geometric robot inverse kinematics
2.2
Introduction
Kinematics refers to the study of the relations between a robot’s geometry and its
motion. In general, the study relates robot geometry to joint and end-effector positions and its derivatives, i.e. velocities, accelerations, and higher order ones. Kinematics does not take into account the forces or control methods that are required
to produce any of such motions. The derivation of such forces are addressed by
the robot dynamics Chapter 11.
7
8
CHAPTER 2 Kinematics
The robots that will be studied in this course consist of rigid links connected by
joints. Joints are usually instrumented by sensors (e.g. encoders) that measure the
relative position between neighbouring links. More complex sensors that measure,
e.g. torques, are also available. Most robot joints are actuated by some sort of
mechanism, usually a servomotor system. This configuration will allows us to
control the position of each joint with respect to its neighbouring ones. Figure 2.1
illustrates six different types of joints that are usually found on robots. Mechanical
design favours the use of joints with a single degree of freedom, such as revolute
or prismatic, thus most robots are built by chains of these joints.
Figure 2.1: Robot joints
As it will be seen in the following section, the study of kinematics can be into
divided into forward and inverse. Forward kinematics can be seen as the problem
of computing end-effector position and orientation as a function of joint variables.
Inverse kinematics tries to solve the inverse problem of finding joint variables as
a function of end-effector position and orientation.
2.3
Forward Kinematics
Forward kinematics is the problem of finding the robot’s end-effector position
and orientation as a function of the robot joints angles. Given that robots usually
have only sensors at their joints (encoders) it is necessary to compute the forward
kinematics to locate the robot’s end-effector.
ROBOTICS ENGINEERING
2024/2025
Section 2.3 Forward Kinematics
9
As an illustrative example, let us consider the two link planar robot in Figure
2.2. The Cartesian coordinates for the end-effector can described by the vector
X = [x, y]T and the joint angles by Θ = [θ1 , θ2 ]T .
Y
X1
Y1
(x,y)
d2
θ2
d1
θ1
X
Figure 2.2: Two link planar robot
The coordinates (x, y) of the end-effector can expressed as a function of the
joint angles as:
x = d1 cos(θ1 ) + d2 cos(θ1 + θ2 )
y = d1 sin(θ1 ) + d2 sin(θ1 + θ2 )
Y0
Y1
(2.1)
(2.2)
X1
sin(β)
β=θ1+θ2
cos(β)
X0
Figure 2.3: Rotation of frame {1} with respect to frame {0}
Figure 2.3 illustrates the orientation of the end-effector frame {1} over the
robot base frame {0}. A simple way to specify the orientation of frame {1} (endeffector) with respect to frame {0} (robot base) is to specify their angle of rotation
2024/2025
ROBOTICS ENGINEERING
10
CHAPTER 2 Kinematics
i.e. β = θ1 + θ2 . This representation has two immediate disadvantages, (i) discontinuity around the value of β = 0 and (ii) the representation does not scale well to
3D space.
A more suitable representation, as it will be seen later, is to specify the coordinate vectors of the axis with respect to each other. In our case, the orientation of
the frame {1} attached to the end-effector relative to the base frame {0} is given
by:
R10 = x01 |y10 ,
where:
x01 =
R10 =
cos(β)
sin(β)
cos(β) − sin(β)
sin(β) cos(β)
, y10 =
− sin(β)
cos(β)
(2.3)
Note that the rotation matrix R10 , which represents the rotation of frame {1}
with respect to {0}, can also be obtained from the dot product of the unit vectors
associated with each frame as follows:
R10 =
x1 · x0 y 1 · x0
x1 · y 0 y 1 · y 0
Equations 2.1, 2.2 and 2.3 describe the forward kinematics of the two-link
planar robot in Figure 2.2.
2.4
Rigid motions and homogeneous transformations
Figure 2.4 illustrates how the forward kinematics problem can be addressed by
defining coordinate frames attached to the robot’s links, an that the relation between joint angles and end-effector position can be computed using these frames
and their transformations.
In other words, compute the forward kinematics of a robot, it is important to
understand how rigid motions (translations and rotations) can be described and
transformed between different coordinate frames.
For example, to compute the forward kinematics of a robot it is common to
attach a coordinate frame on each link (including the end-effector) and compute
the transformations that transform a frame to any other.
ROBOTICS ENGINEERING
2024/2025
Section 2.4 Rigid motions and homogeneous transformations
11
{3}
{2}
Z0
{1}
{0}
Y0
X0
Figure 2.4: Forward kinematics as transformations between multiple frames
2.4.1
Rotations in three dimensions
In general, the rotation between of frame {1} with respect to frame {0} in 3D
space can be represented by the following rotation matrix :

x1 · x0 y1 · x0 z1 · x0
R10 =  x1 · y0 y1 · y0 z1 · y0 
x1 · z0 y1 · z0 z1 · z0

(2.4)
Equation 2.4 is the projection of each unit vector in frame {1} over frame {0}.
For example the first column,


x1 · x0
x01 =  x1 · y0 
x1 · z0
is the unit vector corresponding to the x axis of frame {1} projected into the x, y, z
axis of frame {0}.
Since the dot product is commutative i.e. xi · yj = yj · xi , it can be seen that
the following equivalence applies to the rotation matrix in Equation 2.4:
(R10 )T = (R10 )−1
2024/2025
ROBOTICS ENGINEERING
12
CHAPTER 2 Kinematics
which in other words, it means that the transpose of rotation matrix that maps from
frame {1} to {0} maps the other way around, i.e. from {0} to {1}. The following
properties also hold for these type of matrices:
• RT = R−1
• The columns (also rows) of R are mutually orthogonal
• Each column (also row) of R is a unit vector
• det(R) = 1
Example: Figure 2.5 shows two coordinate frames related by a simple rotation.
Calculate the rotation matrix that maps frame {A} with respect to {B} and viceversa.
ZA
YB
{B}
{A}
YA
ZB
XB
XA
Figure 2.5: Simple rotation
2.4.2
Rotation matrix as a mapping
Consider the situation illustrated in Figure 2.6. The coordinates of a point p as
described by frame {1} are p1 = [u, v, w]T . Then, we know that p satisfies the
equation:
p = ux1 + vy1 + wz1
The coordinates of p can be projected into coordinate axis of frame {0}:
ROBOTICS ENGINEERING
2024/2025
Section 2.4 Rigid motions and homogeneous transformations
13
Z0
{1}
Z1
{0}
w
p
v
Y1
Y0
u
X0
X1
Figure 2.6: Point p and two coordinate frames {0} and {1}


p · x0
p0 =  p · y0 
p · z0
Combining the two equations, one obtains:


 
x1 · x0 y1 · x0 z1 · x0
u
(ux1 + vy1 + wz1 ) · x0
p0 =  (ux1 + vy1 + wz1 ) · y0  =  x1 · y0 y1 · y0 z1 · y0   v  (2.5)
x1 · z0 y1 · z0 z1 · z0
w
(ux1 + vy1 + wz1 ) · z0

Observe that this equation is equivalent to:
p0 = R10 p1
Example: Figure 2.7 shows two coordinate frames related by a simple rotation.
Calculate the coordinates of point p with respect to frame {0} (p0 ) given the coordinates p1 = [2, 2, −1]T with respect to frame {1}.
2024/2025
ROBOTICS ENGINEERING
14
CHAPTER 2 Kinematics
Z0
Y1
p1=(2,2,-1)
Y0
Z1
X1
X0
Figure 2.7: Mapping of a point under frame rotation
2.4.3
Rotation matrix as a vector operator
A rotation matrix can also be used as a vector operator if used on a fixed frame.
Figure 2.8 illustrates a vector u being rotated into vector v. This operation can be
summarised as:
v = Ru
where R is the matrix representing the required rotation.
Z0
{0}
u
v
Y0
X0
Figure 2.8: Rotating a vector over a fixed frame
Thus, in general a rotation matrix R can be used to represent (i) the orientation
between frames (ii) to transform the coordinates of a point p represented with
ROBOTICS ENGINEERING
2024/2025
Section 2.4 Rigid motions and homogeneous transformations
15
respect to different frames and (iii) to transform a vector on a fixed frame.
Example: Figure 2.8 shows a vector u being rotated by π/2 around the Y0 axis, resulting on vector v. Assuming that the coordinates of are u = [0, 1, 1]T , calculate
the coordinates of v.
2.4.4
Translations
Z1
Z0
p
v
u
Y1
o10
Y0
X1
X0
Figure 2.9: Simple translation
Consider the translation illustrated in Figure 2.9. The coordinates of a point p
are represented on frame {1} by vector u. Vector o01 represents the origin of frame
{1} on {0}. Then, the coordinates of vector v representing point p with respect to
frame {0} can be calculated by:
v = u + o01
Where o01 is the coordinates of the origin of frame {1} with respect to frame {0}.
2.4.5
General transformations
Consider the example illustrated in Figure 2.10 in which a frame {1} has been
translated as well as rotated with respect to a fixed one, {0}. The position of
2024/2025
ROBOTICS ENGINEERING
16
CHAPTER 2 Kinematics
point, p, with respect to frame {1} is represented by vector p1 . The coordinates of
p0 in frame {0} can be computed as follows:
p0 = R10 p1 + o01
(2.6)
where R10 is the rotation matrix between frames and o01 is the vector that goes from
the origin of coordinate {0} to {1} .
Z1
p
p0
Z0
o10
X1
p1
Y1
Y0
X0
Figure 2.10: General transformation following a translation and rotation
Consider the case that a third frame of reference is added to the transformation.
In this case, the position of point p is given with regards to frame {2}, p2 , and the
task is to find its representation with respect to frame {0}, p0 . Using Equation 2.6,
we can find the representation of the point with respect to frame {1}, p1 . Given
this, we can re-apply the equation to convert into p01 . The resulting composition
is:
p1 = R21 p2 + o12
p0 = R10 p1 + o01 = R10 R21 p2 + R10 o12 + o01
(2.7)
(2.8)
As it can be seen by Equation 2.8 the calculation of such transformation for robots
with many links is not simple, and becomes soon intractable. Section 2.5 introduces the Homogeneous transformation notation which helps solving this problem.
ROBOTICS ENGINEERING
2024/2025
Section 2.4 Rigid motions and homogeneous transformations
2.4.6
17
Parametrisation of rotations by Euler and Fixed angles
It is not possible to define rotation trajectories using rotation matrices directly as
linear interpolations over the matrices parameters violates some of the constraints.
To define rotational trajectories, it is necessary to parametrise rotational matrices
using only three angles. Although a rotation matrix is composed of nine parameters, only three angles are required to define any rotation. There are two ways in
which rotation matrices can be parametrised using three rotational angles, using
either Euler or Fixed angles.
Euler angles describe rotation over what are known as relative frames, in other
words, angles that are defined over frames which themselves move. In contrast,
Fixed angles rotations are defined over frames which are always fixed to the world.
ZYX Euler angles
The ZYX Euler angle rotation indicates that a complex rotation is formed by three
simple rotations, first over the Z, then Y and finally the X axis. The order in which
the rotations are undertaken is very important as a different order will result on a
different final rotation. Note that other Euler angle conventions exist which define
the rotations over a different sequence. Figure 2.11 illustrates an example of such
a rotation, where α, β, and γ are the angles rotated with respect to each axis.
Z0 ZA
α
ZB ZA
XA (Cα,Sα,0)
Z1
YA YB
YA (-Sα,Cα,0)
β
Y0
X0
ZB
γ Y1
X1 XB
XB
XA
Figure 2.11: ZYX Euler angles rotation
The resulting rotation matrix R10 for this example can be computed by:
0 A B
R10 (α, β, γ) = RA
RB R1 = Rz,α Ry,β Rx,γ
where:


Cα −Sα 0
Rz,α =  Sα Cα 0 
0
0
1
2024/2025
(2.9)
ROBOTICS ENGINEERING
18
CHAPTER 2 Kinematics


Cβ 0 Sβ
1 0 
Ry,β =  0
−Sβ 0 Cβ
(2.10)


1 0
0
Rx,γ =  0 Cγ −Sγ 
0 Sγ Cγ
(2.11)
by multiplying these matrices, we obtain that a ZYX Euler angle rotation can be
generally represented by:


CαCβ CαSβSγ − SαCγ CαSβCγ + SαSγ
RZY X (α, β, γ) =  SαCβ SαSβSγ + CαCγ SαSβCγ − CαSγ  (2.12)
−Sβ
CβSγ
CβCγ
Given the values of a rotation matrix such as the one in Equation 2.12 it is in
many times important to calculate the individual angles (α, β, γ) that result on the
matrix in question. In other words, given a rotation matrix, we can find the angles
that a robot needs to produce to achieve the desired orientation. To compute the
angles, algebraic and trigonometric rules need to be applied and the system of
equations solved. In summary, after performing the operations, the following are
found:
q
2
2
+ r21
) = atan2(Sβ, Cβ)
β = atan2(−r31 , r11
α = atan2(r21 /Cβ, r11 /Cβ) = atan2(Sα, Cα)
γ = atan2(r32 /Cβ, r33 /Cβ) = atan2(Sγ, Cγ)
where atan2 is the arctangent function with two arguments (which enables the
identification of the angle’s correct quadrant). The notation atan2(y,x) is equivalent to the more conventional arctan(y/x), where x and y are the respective projections on the cartersian axis.
Note that by two solutions exist by using the positive or negative values of
the square root in the for the formula to compute β. It is usually good practice to
use the positive solution so that −π/2 ≤ β ≤ π/2. In the case that Cβ = 0 the
solution degenerates. In this particular case, a convention is to set α = 0 which
results in:
ROBOTICS ENGINEERING
2024/2025
Section 2.4 Rigid motions and homogeneous transformations
β=
19
π
, α = 0, atan2(r12 , r22 )
2
or
π
β = − , α = 0, − atan2(r12 , r22 )
2
ZYZ Euler angles
Another common Euler angle configuration is ZYZ. In this case, α over the relative Z axis, then β over the relative Y axis and finally γ over the relative Z axis.
As before, the final rotation transformation can be generated by multiplying:




Cα −Sα 0
Cβ 0 Sβ
Cγ −Sγ 0
1 0   Sγ Cγ 0 
RZY Z =  Sα Cα 0   0
0
0
1
−Sβ 0 Cβ
0
0
1


CαCβCγ − SαSγ −CαCβSγ − SαCγ CαSβ
RZY Z (α, β, γ) =  SαCβCγ + CαSγ −SαCβSγ + CαCγ SαSβ 
−SβCγ
SβSγ
Cβ
(2.13)
Given a rotation matrix R, the ZYZ Euler angles that compose it can be found
as follows:
q
2
, r33 )
β = atan2( 1 − r33
or
q
2
β = atan2(− 1 − r33
, r33 )
If the first value for β is chosen, then:
α = atan2(r23 /Sβ, r13 /Sβ)
γ = atan2(r32 /Sβ, −r31 /Sβ)
If the secont value for β is chosen, then:
α = atan2(−r23 /Sβ, −r13 /Sβ)
γ = atan2(−r32 /Sβ, r31 /Sβ)
2024/2025
ROBOTICS ENGINEERING
20
CHAPTER 2 Kinematics
XYZ Fixed angles
A similar convention can be defined for rotations over a fixed coordinate frame,
i.e. one that remains attached rigidly to the world while the rest of them rotate.
In this case, the rotation angles are commonly named Roll, Pitch and Yaw. These
angles are illustrated in Figure 2.12.
Z
Roll
Pitch
Yaw
Y
X
Figure 2.12: Roll, Pitch and Yaw angles
Figure 2.13 illustrates a rotation following the XYZ convention over fixed axis.
To compute the final rotation matrix R10 :
Z0
ZA Z0
Z0
ZB
Z1
YA
X0
γ
Y0
XA
X0
α
YB
β Y0
XB
Y1
Y0
X0
X1
Figure 2.13: XYZ rotation over fixed axis
R10 (γ, β, α) = Rz,α Ry,β Rx,γ
Note that this equivalent to the ZYX Euler angle computation in Equation 2.12.
In other words, rotating over fixed angles using the opposite sequence results in
the equivalent relative angle matrix. For this reason, the matrices are not shown
here again.
ROBOTICS ENGINEERING
2024/2025
Section 2.5 Homogeneous transformations
2.5
21
Homogeneous transformations
A Homogeneous transformation is a matrix representation of a rigid motion between frames {0} and {1} which uses a 4 × 4 format given by:
T10 =
R10 o01
0 1
(2.14)
where R10 is a 3 × 3 rotation matrix, and o01 represents the position of the origin of
frame {1} with respect to frame {0}.
The coordinates of a point expressed on frame {1}, p1 , can be converted into
frame {0} as follows:
p0
1
=
R10 o01
0 1
p1
1
(2.15)
Example: Figure 2.14 shows two coordinate frames related by a π/2 rotation
around their X axis and a translation o01 = [0, 3, 1]T . Calculate the coordinates
of point p with respect to frame {0} (p0 ) given the coordinates p1 = [0, 1, 1]T with
respect to frame {1} using a Homogeneous transformation representation.
Y1
p
p1
Z0
p0
Z1
o10
X1
Y0
X0
Figure 2.14: General transformation following a translation and rotation
2024/2025
ROBOTICS ENGINEERING
22
CHAPTER 2 Kinematics
The inverse of a transformation expressed using a Homogeneous representation can be calculated by:
(T10 )−1 = (T01 ) =
2.5.1
(R10 )T −(R10 )T o01
0
1
(2.16)
Composition of Homogeneous transformations
In many cases, it is important to express the position and orientation of a set
of frames with respect to each other. For example, in Figure 2.15 a robot on a
mobile platform has the task of picking up the object on the table. Let us assume
that the overhead camera can visually recognise and locate the object and the
base of the robot. In order to drive the end-effector to the object’s location it
is important to compute the relation between frames.That is to find the relative
position and orientation of the object and robot with respect to the camera and the
end-effector’s with respect to the robot’s base. Once these relations are known, it
will be possible to compute how much the robot base needs to move and how the
arm needs to approach the object.
{cam}
{end-effector}
{base}
{object}
Figure 2.15: Multiple frames and relations
In order to map between different frames, homogeneous representations have
nice properties about how to compute these compositions. Figure 2.16 three
frames of references and a point p. Tji is the homogeneous transformation that
relates frame {j} with respect to {i} and so on. The following relations hold between the transformations:
ROBOTICS ENGINEERING
2024/2025
Section 2.5 Homogeneous transformations
23
TAA = TBA TCB TAC = I
rearranging the equation above, one can easily find any unknown transformations.
For example,
TAB = (TBA )−1 = TCB TAC
A
TB
{A}
{B}
B
A
T
pA
C
TA
{C}
pB
p
B
TC
C
p
Figure 2.16: Point relation to multiple frames
Let us assume that the position of point p can be measured with respect to
frame {C} which may be the camera. Then, TCB is the homogeneous transformation matrix which references {C} to {B}. As it has already been discussed earlier,
the point p as measured over the frame {B} can be computed by:
pB = TCB pC
and so,
pA = TBA pB = TBA TCB pC
which means that a composition of rotations and translations using homogeneous
matrices can be simply solved by multiplying the matrices. Compare this operation with that in Equation 2.8 and you’ll very clearly see why using homogeneous
matrices simplify the calculation of a long chain of rigid transformations.
2024/2025
ROBOTICS ENGINEERING
24
CHAPTER 2 Kinematics
2.6 Denavit-Hartenberg (D-H) convention
As described in the previous section, a robot manipulator can be represented by
a chain of rigid links connected by various different joints (Figure 2.1). In the
models described here, it will be considered that all joints have a single Degree
Of Freedom (DOF). Some robot joints may have more than a single DOF, e.g.
spherical joint. In these cases, joints with several DOF will be considered as
multiple single DOF joints located at the same point. This simplification, allows
any joint to be described by a single parameter, an angle for revolute joints, and a
distance for prismatic ones.
A robot with n joints has n + 1 links. By convention, the base of the robot is
designated by link 0 and the end-effector by n. Joints are designated from 1 to n.
The i-th joint connects link i − 1 with i. In this manner, when joint i is actuated,
link i moves with respect to i − 1.
To perform the kinematic analysis of a complex robot, a coordinate frame will
be rigidly attached to each link. In particular, frame {i} will be attached to the the
i-th link. For example, frame {0} will be attached to the robot’s base. This frame
is sometimes known as the base or inertial frame.
As we have seen in the previous sections, the transformation between coordinate frames can be expressed using Homogeneous representations. Also, transformations between composition of frames is simply achieved by the multiplication
of the intermediate transformations. Following this insight, it is obvious that the
transformation between the frame attached to the robot’s end-effector and the base
frame can be computed by:
Tn0 = T10 T21 . . . Tnn−1
In principle, this is all what is needed to compute the forward kinematics of
a robot, as the results maps the position and orientation of the end-effector to the
base frame. There are infinite ways in which coordinate frames can be attached to
each of the links. The DH convention allows for a standard and simple approach
to define robot frames.
2.6.1
DH Parameters
The DH convention allows for a systematic procedure when performing the kinematic analysis of a robot manipulator. In this convention a homogeneous transformation between two consecutive frames, T , is represented as the product of four
basic transformations:
T = Rotz,θ T ransz,d T ransx,a Rotx,α
ROBOTICS ENGINEERING
(2.17)
2024/2025
Section 2.6 Denavit-Hartenberg (D-H) convention
25
where the θ, d, a and α are the parameters of each basic transformation. Figure
2.17 illustrates two coordinate frames satisfying the DH convention and represented by the previous parameters.
Figure 2.17: Coordinate frames satisfying the DH convention
Solving the above matrix multiplication in Equation 2.17 results in:


Cθ −SθCα SθSα aCθ
 Sθ CθCα −CθSα aSθ 

T =
 0
Sα
Cα
d 
0
0
0
1
(2.18)
where S and C refer to sin and cos respectively.
As we have seen previously, general Homogeneous transformation need 6 parameters (3 positions and 3 orientations). In order to represent frames using only
4 parameters as the DH convention does, the following constraints need to be
placed:
• DH1 The axis x1 is perpendicular to axis z0
• DH2 The axis x1 intersects the axis z0
this constraints are used in the frame assignment convention.
2024/2025
ROBOTICS ENGINEERING
26
CHAPTER 2 Kinematics
2.6.2
Assigning coordinate frames
The choice of the location of the coordinate frames must follow the previous two
constraints. Despite this, there are still various ways in which to define frames
which are all equivalent. Different frame assignments will result in different parameters, but the whole robot configuration will be equivalent. In other words, the
final transformation matrix from the end-effector to the robot base Tn0 will be the
same. Figure 2.18 illustrates two links, two joints and a possible frame assignment
using the DH convention.
Figure 2.18: DH Convention frame assignment
In order to perform frame assignment in a systematic manner, the following
steps can be performed:
• Step 1: Assign zi – For frame i of the robot, the choice of the zi axis is
arbitrary, thus we can select any that is intuitive, it is common to choose zi
to be the axis of rotation (revolute) or elongation (prismatic) of joint i + 1.
If the joint is revolute, then zi is the revolution of the joint, if the joint is
prismatic, zi is the translation axis.
• Step 2: Base frame establishment – Assign the coordinates x0 and y0 of the
base frame arbitrarily, but following the right-hand rule.
• For joint 1 to n repeat the following steps:
– Step 3: Origin location – locate the origin of frame i, oi , where the
common normal of zi and zi−1 intersect zi . If zi and zi−1 are parallel,
place oi at any arbitrary point along zi .
ROBOTICS ENGINEERING
2024/2025
Section 2.6 Denavit-Hartenberg (D-H) convention
27
– Step 4: Establish xi – along the common normal between zi and zi−1
through oi . If zi and zi−1 are co-planar, xi should be in the direction
normal to the zi−1 − zi plane.
– Step 5: Establish yi – following the right-hand convention
• Step 6: Establish the end-effector frame – assuming the end-effector joint
is a revolute, set zn parallel to zn−1 . Set yn in the direction of the gripper
closure. The origin along zn and preferably at the tip of any tool the robot
may be carrying
• Step 7:Create the DH table of parameters – where:
– ai is the distance along xi from the intersection of the xi and zi−1 to oi
– di is the distance along zi−1 from oi−1 to the intersection of the xi and
zi−1 axis
– αi is the angle from zi−1 to zi measured about xi
– θi is the angle from xi−1 to xi measured about zi−1
• Step 8: Form the homogeneous transformations – using the DH parameters
in the table and performing the multiplication shown in Equation 2.17.
• Step 9: Form the total transformation – multiplying the homogeneous transformations
Example: Figure 2.19 shows a two link planar robot with their axis definition and
DH parameters. Find the DH table and the T20 transformation for this particular
frame assignment.
Figure 2.19: DH Convention on a two-link planar robot
2024/2025
ROBOTICS ENGINEERING
28
CHAPTER 2 Kinematics
2.7
Forward Kinemtics Matlab Example
Figure 2.20 illustrates a 6 DOF robot with DH axis definitions and variable names.
Table 2.1 contains the DH parameters for this robot.
a2
θ2
z1
θ3
x3
d4
θ5
x2
x4
x5
z2 z
3
z4
z5
d6
x6
θ4
x1
z6
θ6
d1
Axis pointing out from the page
θ1
z0
x0
Figure 2.20: A six DOF robot
Table 2.1: DH parameters for the 6 DOF robot in Figure 2.20
Link
1
2
3
4
5
6
ai
0
a2
0
0
0
0
αi
90
0
90
-90
90
0
di
d1
0
0
d4
0
d6
θi
θ1
θ2
θ3
θ4
θ5
θ6
The following code computes the forward kinematics of the robot. The variable o6 stores the (x, y, z) cartesian coordinates of the robot’s end-effector. The
input variables (th1, .., th6) are the joint angles.
ROBOTICS ENGINEERING
2024/2025
Section 2.7 Forward Kinemtics Matlab Example
29
function sixDOFrobot( th1, th2, th3, th4, th5, th6)
%Example for 6DOF robot kinematics
%input to the function are the robot joints
%definition of the link lenghts
d1 = 0.3;
a2 = 0.2;
d4 = 0.2;
d6 = 0.1;
%computation of joint angle projections required for
%matrix transformations
c1 = cos(th1); s1 = sin(th1);
c2 = cos(th2); s2 = sin(th2);
c3 = cos(th3); s3 = sin(th3);
c4 = cos(th4); s4 = sin(th4);
c5 = cos(th5); s5 = sin(th5);
c6 = cos(th6); s6 = sin(th6);
%Matrix transformations following DH convention
t01 = [c1 0 s1 0; s1 0 -c1 0; 0 1 0 d1; 0 0 0 1];
t12 = [c2 -s2 0 a2*c2; s2 c2 0 a2*s2; 0 0 1 0; 0 0 0 1];
t23 = [c3 0 s3 0; s3 0 -c3 0; 0 1 0 0; 0 0 0 1];
t34 = [c4 0 -s4 0; s4 0 c4 0; 0 -1 0 d4; 0 0 0 1];
t45 = [c5 0 s5 0; s5 0 -c5 0; 0 1 0 0; 0 0 0 1];
t56 = [c6 -s6 0 0; s6 c6 0 0; 0 0 1 d6; 0 0 0 1];
%Computing the transformation matrix for the end-effector
%w.r.t base frame
T06 = t01*t12*t23*t34*t45*t56;
%get the origin position for the end-effector axis w.r.t
%to the base frame
o6 = T06(1:3,4);
2024/2025
ROBOTICS ENGINEERING
30
2.8
CHAPTER 2 Kinematics
Inverse Kinematics
Inverse Kinematics is concerned with the problem for finding robot joint angles
given the end-effectors position and orientation. This course will only discuss
the simpler case in which the robot can be decomposed kinematically, in other
words, when the position and orientation of the end-effector can be considered as
independent problems.
Given the homogeneous transformation Hd , representing the desired end-effector
position and orientation for the robot, the inverse kinematics problem is to find the
joint variables q1 , q2 , . . . qn so that the total robot forward kinematics transformation Tn0 (q1 , . . . qn ) is equal to Hd . That is:
Tn0 (q1 , . . . qn ) = Hd
The above, results in twelve non-linear equations with n unknowns. The solutions
of these equations are non-trivial and in most cases too complex to be solved in a
closed form.
2.8.1
Kinematic decoupling
In the particular situation in which manipulators with six joints have the last three
axis intersecting at a point, it is possible to decouple the inverse kinematics into
two simpler problems, inverse analysis of (i) position and (ii) orientation. In other
words, the inverse kinematics problem can be decomposed into finding (usually,
the first three) joint angles to position the wrist on a desired position, and (usually the last three angles) to satisfy the orientation requirements. The important
characteristic for kinematic decoupling is that the motion of the last three joints
will not change the intersection position of the robot’s wrist. In other words the
postion of the wrist oc is independent of the values of the last three joint angles,
see Figure 2.21 for a visual example.
Figure 2.21 illustrates a robot configuration in which the three last axis intersect on point oc . As it can be observed, by moving any of the three last axis,
this point remains constant. The position of oc is only determined by the three
first joints. Note that, a simple rotation and translation of d6 will provide with the
end-effector frame o6 . This in an example of a kinematic decoupling.
Let us express the desired position for the end effector as, o = o06 (q1 , ..., q6 ),
and its orientation as, R = R60 (q1 , ..., q6 ). Where q1 to q6 are the joint variables, R
a rotation matrix and o a Cartesian position vector. Thus, the inverse kinematics
problem is to find the qi given o and R.
Figure 2.22 illustrates a common spherical wrist used by various robots. As
it can be observed, changing any of the wrist angles θ4 , θ5 , θ6 do not change the
ROBOTICS ENGINEERING
2024/2025
Section 2.8 Inverse Kinematics
31
Figure 2.21: Kinematic decoupling
d4
θ5
x2
x4
x5
z3
z4
z5
d6
x6
θ4
θ6
z6
Figure 2.22: Spherical wrist following from Figure 2.20
position of the intersecting joint position. This type of joint is commonly used
by industrial robots to simplify their inverse kinematics. Given the DH frame
assignment in 2.22 the DH parameters for this wrist configuration are given in
Table 2.2.
The homogeneous transformation for this wrist given the previous axis definition is:


c4 c5 c6 − s4 s6 −c6 s4 − c4 c5 s6 c4 s5
c4 d 6 s 5
 c4 s6 + c5 c6 s4 c4 c6 − c5 s4 s6 s4 s5 d6 s4 s5 

Hw = 

−c6 s5
s5 s6
c5 d4 + c5 d6 
0
0
0
1
(2.19)
The rotation part of Equation 2.19 is equivalent to the result of rotating frames
2024/2025
ROBOTICS ENGINEERING
32
CHAPTER 2 Kinematics
Table 2.2: DH parameters for spherical wrist in Figure 2.22
Link
4
5
6
ai
0
0
0
αi
-90
90
0
di
d4
0
d6
θi
θ4
θ5
θ6
as defined by the ZYZ Euler angles over frame {3} (as defined in Figure 2.20).
Observe that in Equation 2.13, (α, β, γ) correspond to (θ4 , θ5 , θ6 ). In other words,
rotating over the relative axis Z by α, followed by a β over the relative Y axis and
finalising with a γ angle over the relative Z axis.
The origin of the tool frame (end-effector) can be obtained by translating a
distance d6 along z6 (or z5 in this case as they are the same). Therefore, the origin
of the tool frame with respect to the base or frame {0} can be found as:
o = o0c + d6 R[0, 0, 1]T
in other words, by taking the third column (Z direction) of the rotation matrix R
and translating a distance of d6 . Recall that the rotation matrix R encodes the
projection of the last frame (end-effector frame) w.r.t the base frame {0}).
Thus, in order to compute the wrist centre position given the desired tool position and orientation:
o0c = o06 − d6 R[0, 0, 1]T
in other words, the position of the wrist centre o0c = [xc , yc , zc ]T can be computed
from those of the tool frame o06 = [ox , oy , oz ]T by:

 

xc
ox − d6 r13
 yc  =  oy − d6 r23 
zc
oz − d6 r33
(2.20)
Equation 2.20 shows how to calculate the position of the wrist centre given
the position and orientation of the tool frame. The wrist position will determine
the values for the three first joints, therefore determining the rotation matrix R30 .
Given that the final orientation of the tool frame can be computed as, R = R30 R63 ,
then we have:
R63 = (R30 )−1 R = (R30 )T R
(2.21)
Given that R63 can be computed as shown in Equation 2.21, the angles of the
wrist can be found using the Euler ZYZ angles solution.
ROBOTICS ENGINEERING
2024/2025
Section 2.8 Inverse Kinematics
2.8.2
33
Geometric solution
In most cases, a geometric approach can be adopted to solve the inverse kinematic
problem. Provided that the manipulator can be decoupled, the inverse kinematics
can be solved as two simpler problems inverse position and inverse orientation.
Inverse position is to find the joint angles, usually the lower three, q1 , q2 , q3 ,
related to joints 1, 2, 3 to satisfy the required position of the wrist centre oc . Inverse
orientation is to find the values of the three final joint variables q4 , q5 , q6 such that
the orientation of the end-effector is the required one.
The following presents some examples of using the geometric approach to
solve the inverse kinematics for common manipulators.
Inverse kinematics of Elbow manipulator
Figure 2.23: Elbow manipulator
The robot in Figure 2.21 is composed of a spherical wrist (Figure 2.22) with
centre oc . The base joints 1,2 and 3 are illustrated in Figure 2.23 for which the
wrist centre on the base frame {0} is represented by, o0c = [xc , yc , zc ]T . The
projection of oc on to the (x0 , y0 ) plane is also shown in the Figure.
Given the required end effector position o = [ox , oy , oz ]T and orientation given
by a 3 × 3 rotation matrix, R60 , then we can compute the inverse kinematics and
thus the joint angles as follows:
1. Use o (desired end-efector position) and R60 (desired end-effector orientation) to calculate the position of the wrist centre o0c as shown in Equation
2.20
2. Given o0c = (xc , yc , zc ), calculate θ1 by following the simple trigonometric
relation
yc
−1
θ1 = tan
xc
2024/2025
ROBOTICS ENGINEERING
34
CHAPTER 2 Kinematics
or
atan2(yc , xc )
The other possible solution for this angle is
yc
−1
θ1 = π + tan
xc
3. Given θ1 , the values for θ2 and θ3 can be solved as a simple two-link robot
Figure 2.24. Given that:
p2 = a21 + a22 − 2a1 a2 cos(β)
β = 180 − θ3
then
cos(θ3 ) =
x2c + yc2 + s2 − a21 − a22
=D
2a1 a2
s = zc − d1
−1 ±
√
θ3 = tan
1 − D2
D
and
−1
θ2 = tan
s
r
−1
− tan
a2 sin θ3
a1 + a2 cos θ3
Note that care must be taken when calculating all the possible solutions to
the IK problem as the combination of angles must be ensure to be correct.
4. Compute the rotation matrix R30 using the previously identified angles, θ1 , θ2 , θ3 .
Given that these are the only variables, it is straight forward to compute the
rotation matrix using the DH convention and the parameters in Table 2.3 .
5. Compute R63 = (R30 )T R
6. Identify the Euler angles (α, β, γ) that relate to this configuration as explained in Section 2.4.6 and in Equation 2.19. This would be the angles
corresponding to θ4 , θ5 , θ6 of the spherical wrist in Figure 2.22
ROBOTICS ENGINEERING
2024/2025
Section 2.9 Tutorial Questions
s
p
a1
35
a2
θ3
β
θ2
r
Figure 2.24: Two Link robot as a projection of Elbow robot
Table 2.3: DH parameters for Elbow robot in Figure 2.23
Link
1
2
3
2.9
ai
0
a1
a2
αi
90
90
0
di
d1
0
0
θi
θ1
θ2
θ3
Tutorial Questions
P
2.1. Find the rotation matrix RQ
where {Q} is a frame coincident with {P} but
rotated by 40 deg about the Z axis.
Answer:


0.766 −0.643 0
P
RQ
=  0.643 0.766 0 
0
0
1
P
2.2. Find the rotation matrix RQ
where {Q} is a frame coincident with {P} but
rotated by −60 deg about the X axis.
Answer:


1
0
0
P
=  0 0.500 0.866 
RQ
0 −0.866 0.500
2.3. Frame {Q} and {P} are initially coincident. Frame {Q} is rotated by θ radians
over the YQ axis. The resulting frame is rotated by α radians over the ZQ axis.
P
Determine the rotation matrix RQ
.
2024/2025
ROBOTICS ENGINEERING
36
CHAPTER 2 Kinematics
Answer:


CθCα −CθSα Sθ
P
Cα
0 
RQ
=  Sα
−SθCα SθSα Cθ
2.4. Frames {U} and {V} are shown in Figure 2.25. Find the homogeneous transformation matrix TVU (position and orientation of {V} w.r.t. {U}). If P V =
[2, 3, 4]T , determine P U .
Figure 2.25: Diagram
Answer:


−0.5 −0.86 0 −3.72
 0.866 −0.5 0 2.10 

TVU = 
 0
0
1
0 
0
0
0
1
P U = [−7.32, 2.33, 4]T
2.5. Given a single fixed frame {A} and a vector P A , find the rotational operation
matrix that will rotate the vector P A , α radians w.r.t. to the ZA axis, followed by
θ about the YA axis.
Answer:
Rα,θ = RY,θ RZ,α
2.6. Given the following homogeneous transformation, TAB , calculate TBA . Compute P A , given θ = π/4 and P B = [4, 5, 6]T .


1 0
0
1
 0 Cθ −Sθ 2 

TAB = 
 0 Sθ Cθ 3 
0 0
0
1
ROBOTICS ENGINEERING
2024/2025
Section 2.9 Tutorial Questions
37
Answer:


1
0
0
−1
 0 Cθ Sθ −2Cθ − 3Sθ 

TBA = 
 0 −Sθ Cθ 2Sθ − 3Cθ 
0
0
0
1
P A = [3, 4.24, 0]T
2.7. Assume three coordinate frames {1}, {2} and {3}. Calculate R32 given:




1
0
0
0 0 −1
√
R21 =  0 √1/2 − 3/2  R31 =  0 1 0 
1 0 0
0
3/2
1/2
Answer:

0
0
−1
√
1/2
0 
R32 =  3/2
√
1/2 − 3/2 0

2.8. Given the diagram in Figure 2.26 calculate the homogeneous transformation,
T10 , T20 and T30 , given that:
• table is located at 1m from the robot’s base
• table is 1m2
• cube measures 20cm and is located at the centre of the table
• the origin of frame{2} (cube) is at the centre of the cube
• the camera is located directly above the cube
Answer:

1

0
T10 = 
 0
0
0
1
0
0
0
0
1
0


0
1

1 
 T20 =  0
 0
1 
1
0
0
1
0
0


0
0 −0.5

0 1.5 
 T30 =  1
 0
1 1.1 
0
1
0

1 0 −0.5
0 0
1.5 

0 −1
3 
0 0
1
2.9. Consider the robot in the Figure 2.27 and derive the forward kinematic equations using the DH convention.
2.10. Consider the robot in the Figure 2.28 and derive the forward kinematic equations using the DH convention.
2024/2025
ROBOTICS ENGINEERING
38
CHAPTER 2 Kinematics
Figure 2.26: Diagram
2.11. Consider the robot in the Figure 2.29 and derive the forward kinematic equations using the DH convention.
2.12. Consider the robot in the Figure 2.30 and derive the forward kinematic equations using the DH convention.
2.13. Consider the robot in the Figure 2.31 and derive the forward kinematic equations using the DH convention.
2.14. Consider the robot in the Figure 2.32 and derive the forward kinematic equations using the DH convention.
2.15. Consider the planar 3 DOF robot in the Figure 2.33. Given a desired position, how many solutions are there to the inverse kinematics? How many remain
if the orientation for the end-effector is also given?
2.16. Consider the Cartesian robot in Figure 2.34 and derive the inverse position
kinematics
2.17. Add a spherical wrist to the robot in Figure 2.28 and write the complete
inverse kinematics solution
ROBOTICS ENGINEERING
2024/2025
Section 2.9 Tutorial Questions
39
Figure 2.27: Two-link Cartesian robot
Figure 2.28: Two-link planar robot
2.18. Consider the robot in the Figure 2.35, and given the desired position o06 and
orientation R60 for the end-effector. (i) Compute the desired coordinates of the
wrist centre oc . (ii) Solve the inverse position kinematics, is the solution unique?
(iii) compute the rotation matrix R30 and solve the inverse orientation problem,
find the corresponding Euler angles for the last three joints.
2024/2025
ROBOTICS ENGINEERING
40
CHAPTER 2 Kinematics
Figure 2.29: Three link arm with prismatic joint
Figure 2.30: 3 DOF Cartesian robot
Figure 2.31: 3 DOF Cartesian robot with spherical wrist
ROBOTICS ENGINEERING
2024/2025
Section 2.9 Tutorial Questions
41
Figure 2.32: Puma robot
Figure 2.33: Three link planar robot
2024/2025
ROBOTICS ENGINEERING
42
CHAPTER 2 Kinematics
Figure 2.34: Cartesian robot
Figure 2.35: Puma robot
ROBOTICS ENGINEERING
2024/2025
Chapter 3
Jacobian
3.1
Introduction
The Jacobian relates to the study of robot differential motion (small movements
of joints and end-effector) that can be used to find velocity relationships between
joints and end-effector. In other words, given the forward kinematic function of a
robot, the Jacobian of this function will relate joint and end-effector velocities.
The Jacobian has many applications in robotics, such as in, trajectory generation, determination of singularities, derivation of dynamic equations and transfer
of forces.
3.2
Example 2 DOF planar robot
Let us here consider the simple 2 DOF robot as an example. The robot is illustrated in Figure 2.2. It possible to differentiate the forward kinematic Equations
2.1 and 2.2 with respect to time, resulting in:
ẋ = −d1 sin θ1 θ̇1 − d2 sin(θ1 + θ2 )(θ̇1 + θ̇2 )
ẏ = d1 cos θ1 θ̇1 + d2 cos(θ1 + θ2 )(θ̇1 + θ̇2 )
(3.1)
(3.2)
These equations can be re-arranged as:
ẋ
ẏ
=
−d1 sin θ1 − d2 sin(θ1 + θ2 ) −d2 sin(θ1 + θ2 )
d1 cos θ1 + d2 cos(θ1 + θ2 ) d2 cos(θ1 + θ2 )
θ̇1
θ̇2
(3.3)
Observe that Equation 3.3 relates end-effector linear velocity [ẋẏ]T to joint velocities [θ̇1 θ̇2 ]T . The matrix that relates the two, is known as the Jacobian J of the
43
44
CHAPTER 3 Jacobian
robot. In other words:
3.3
ẋ
ẏ
= [J]
θ̇1
θ̇2
Direct differentiation
As shown in the previous Section, the Jacobian can be calculated by talking the
derivatives of each position equation with respect to all joint variables.
Let us assume a set of equations γi as a function of a set of variables vj :
γi = f (v1 , v2 , ..., vj )
The differential change in γi as a result of differential changes in the xj can be
expressed as:
∂f1
∂f1
∂f1
δv1 +
δv2 + . . . +
δvj
∂v1
∂v2
∂vj
∂f2
∂f2
∂f2
δγ2 =
δv1 +
δv2 + . . . +
δvj
∂v1
∂v2
∂vj
..
.
∂fi
∂fi
∂fi
δγi =
δv1 +
δv2 + . . . +
δvj
∂v1
∂v2
∂vj
δγ1 =
This can be written in matrix format representing the relationship between
individual variables and the functions. The matrix is the Jacobian, therefore this
can be calculated by talking the derivative of each equation with respect to all
variables.


 ∂f1
δγ1
∂v1
 δγ2 

  ..
 ..  =  .
 . 
δγi
∂fi
∂v1
∂f1
∂v2
∂fi
∂v2
...
∂f1
∂vj
...
∂fi
∂vj


δv1
  .. 
 . 
(3.4)
δvj
Using the relation in Equation 3.4, it is possible to describe the differential
motion of a robot as:


 ∂f1
δx1
∂q1
 δx2 

  ..
 ..  =  .
 . 
δxm
∂fi
∂q1
∂f1
∂q2
∂fi
∂q2
ROBOTICS ENGINEERING
...
∂f1
∂qj
...
∂fi
∂qn

δq1
  .. 
 . 

(3.5)
δqj
2024/2025
Section 3.3 Direct differentiation
45
Where xi are the end-effector position and orientation, qi are the joint variables,
that is angles (revolute joints) and distances (prismatic joints). In general, the
expression can be summarised as:
δx(m×1) = J(m×n) δq(n×1)
Example 3.1. Figure 3.1 illustrates a simple two link robot with a revolute and
a prismatic joint. Assuming that the end-effector position is represented by the
position and orientation vector [x, y, α]T . Note that in this simple example, the
orientation is given by a single angle value α = θ. The joint variables are q =
[q1 , q2 ]T , where q1 = θ (revolute variable) and q2 = d (prismatic variable).
y0
d=q2
a
(x,y,a)
q=q1
x0
Figure 3.1: Two-link planar robot
The forward kinematics of the robot can be calculated as follows:
  
 

x
f1 (q)
q2 Cq1
 y  =  f2 (q)  =  q2 Sq1 
α
f3 (q)
q1
Thus following from Equation 3.5 we get the following:
∂f1
= −q2 Sq1
∂q1
∂f1
= Cq1
∂q2
∂f2
= q2 Cq1
∂q1
∂f2
= Sq1
∂q2
∂f3
=1
∂q1
∂f3
=0
∂q2
and written in a matrix format:


−q2 Sq1 Cq1
J(q) =  q2 Cq1 Sq1 
1
0
2024/2025
ROBOTICS ENGINEERING
46
CHAPTER 3 Jacobian
The end-effector linear and angular velocity can now be obtained by:
  

ẋ
−q2 Sq1 Cq1  ẏ  =  q2 Cq1 Sq1  q̇1
q̇2
α̇
1
0
3.4
Explicit Jacobian
In many cases, when the robot has various DOF and operates in 3D space, the
direct differentiation of forward kinematics to compute the Jacobian is tedious.
More over, differentiating the Homogeneous matrix (as calculated using the DH
convention) would result in the adequate computation of the linear velocity of the
end-effector but not the angular one. In other words, differentiating the rotation
matrix (expressing the orientation of the end-effector w.r.t to the base frame) does
not directly represent the end-effector angular velocity [wx , wy , wz ] w.r.t to the {0}
frame.
The explicit Jacobian method allows the derivation of the linear and angular
end-effector velocity by dividing the Jacobian matrix into a angular velocity jacobian Jw and linear velocity jacobian Jv . The full jacobian is then composed
as:
Jv (q)
J(q) =
Jw (q)
computing the Jacobian matrix in this way will result in a matrix of dimension
6 × m where m is the number of robot joints, and 6 is the result of 3 dimensional
linear and angular velocities.
3.4.1
Link velocity
Any moving robot link and associated frame have two velocity components, a
linear and an angular. The linear velocity can be seen as the velocity of a point
moving in space, for example the velocity of the frame’s origin. The angular
velocity of a frame relates to how it changes its orientation in time.
Prismatic joints
Prismatic joints do not change the orientation of a frame with respect to its predecessor’s. In other words the angular velocity, w of Frame i w.r.t. Frame i − 1
is:
wi−1,i = 0
ROBOTICS ENGINEERING
2024/2025
Section 3.4 Explicit Jacobian
47
the linear velocity, v, is:
vi−1,i = d˙i zi−1
where zi−1 is the unit vector of Joint i axis (recall that when following the DH
convention, the axis of translation of a prismatic joint i is zi−1 . The d˙i represents
the actual joint velocity (recall that variable d represents prismatic ones).
Revolute joints
The angular velocity of frame due to a revolute joint i is:
wi−1,i = θ̇zi−1
where θ̇ is the joint variable velocity. The linear velocity can be computed as:
vi−1,i = wi−1,i × ri−1,i
where ri−1,i is the vector between the rotation axis of joint i and the position where
the velocity is measured.
3.4.2
Jacobian computation using explicit method
The Jacobian matrix is decomposed into Jv and Jw accounting for linear and angular velocity respectively.
The linear velocity Jacobian, Jv , for a robot with m joints can be computed as
follows:
Jv = [Jv1 , Jv2 , . . . , Jvm ]
if i is a prismatic joint, then:
0
Jvi = zi−1
if i is a revolute joint, then:
0
Jvi = zi−1
× (o0n − o0i−1 )
where o0k represents the origin of a frame {k} w.r.t. {0}.
The angular velocity Jacobian, Jw , for a robot with m joints can be computed
as follows:
Jw = [Jw1 , Jw2 , . . . , Jwm ]
if i is a prismatic joint, then:
2024/2025
ROBOTICS ENGINEERING
48
CHAPTER 3 Jacobian
Jwi = 0
if i is a revolute joint, then:
0
Jwi = zi−1
Example 3.2. Figure 3.2 illustrates a 3 DOF robot with two revolute and prismatic
joints. Given the DH frame assignment on the figure, the DH parameters are in
Table 3.1.
q2
x2
x1z2
z1
d1
x3
z3
d3
q1
z0
y0
x0
Figure 3.2: 3 DOF robot
Table 3.1: DH parameters for robot in Figure 3.2
Link
1
2
3
ai
0
0
0
αi
90
90
0
di
d1
0
d3
θi
θ1
θ2
0
the resulting T matrices are:

C1

S1
T10 = 
 0
0

0 S1
0
0 −C1 0 

1
0
d1 
0
0
1
ROBOTICS ENGINEERING
(3.6)
2024/2025
Section 3.4 Explicit Jacobian

C2

S2
T21 = 
 0
0

1

0
T32 = 
 0
0
0 S2
0 −C2
1
0
0
0
0
1
0
0
49

0
0 

0 
1
(3.7)

0 0
0 0 

1 d3 
0 1
(3.8)


C1 C2 S1 C1 S2 0
 C2 S1 −C1 S1 S2 0 

T20 = 
 S2
0
−C2 d1 
0
0
0
1
(3.9)


C1 C2 S1 C1 S2 C1 d3 S1
 C2 S1 −C1 S1 S2
d3 S1 S2 

T30 = 
 S2
0
−C2 d1 − C2 d3 
0
0
0
1
(3.10)
Given these matrices it is easy to find the required zi0 and o0i to complete the
Jacobian. In this example, the Jacobian is:

−d3 S1 S2
C 1 C 2 d3
C1 S2

C2 S1 d3
S1 S2 

 C 1 d3 S 1
2
2

Jv (q)
0
d3 C1 S1 + d3 S2 S1 −C2 


J=
=

0
S
0
Jw (q)
1



0
−C1
0 
1
0
0

2024/2025
(3.11)
ROBOTICS ENGINEERING
50
3.5
CHAPTER 3 Jacobian
Tutorial Questions
3.1. Calculate the Jacobian matrix for the robot in Figure 3.3. Calculate the linear
velocity v which is located at the end of the second link.
L3
θ3
v
L2
θ2
Y0
θ1
X0
L1
Figure 3.3: 3 DOF Planar robot
3.2. Calculate the Jacobian matrix for the robot in Figure 3.4
L2
L3
θ2
θ3
θ1
L1
Z0
Y0
X0
Figure 3.4: 3DOF Elbow robot
ROBOTICS ENGINEERING
2024/2025
Chapter 4
Trajectory Planning
Trajectory is defined as the time history of position, velocity, and acceleration for
each degree of freedom. This should not be confused with path planning, which
is the geometric planning of the whole way from point A to point B, including
stopping in defined path points. For example selecting the way that the end effector moves from point A to B in Fig. 4.1(a) is path planning, but determining the
time history of the motion in Fig. 4.1(b) is trajectory planning. There are infinite
possible paths from A to B. The path planning may involve kinematic limits of
the manipulator and obstacles as well as preventing collisions between the robot
arms. For the same path, there are many trajectories exist depending on how fast
the manipulator moves.
(b) Trajectory
(a) Path
1.5
1.5
x−axis
2
B
0.5
0
0.5
0
1
2
0
1
Time (s)
2
2
0
A
−0.5
y−axis
y−axis
1
1
1
0
−1
−0.5
0 0.5
x−axis
−1
1
Figure 4.1: Path and trajectory planning
51
52
CHAPTER 4 Trajectory Planning
The trajectory can be defined either in the joint-space or the Cartesian-space
(also called workspace). Both methods are used in industry depending on the
application. When the initial and final positions are specified in Cartesian coordinates, these can be converted to the joint coordinates. Then path and trajectory
planning can be carried out in the joint-space between the initial and final joint
positions as shown in Fig. 4.2(a). This usually gives a smooth motion, but it is
difficult to visualise the motion of the end effector. It is necessary to carry out
forward kinematics repeatedly to visualise the motion.
To visualise
Cartesian
Trajectory
Initial and final position
In Cartesian-space
Forward
Kinematics
Inverse
Kinematics
(a)
Initial
and final
position
In Jointspace
Trajectory
Planning
Cartesian
Trajectory
(b)
Trajectory
Planning
Joint
Trajectory
Joint
Trajectory
Motor
Controller
Inverse
Kinematics
Figure 4.2: Comparison of (a) joint-space and (b) work-space path and trajectory
planning
The path and trajectory planning can also be carried out to specify the time history of Cartesian coordinates of the end effector as shown in Fig. 4.2(b). However,
it is necessary to convert the calculated trajectory to joint coordinates by applying inverse kinematics repeatedly for the purpose of control. Although it is easy
to visualise the motion, Cartesian-space trajectories are computationally expensive due to the need for repeated inverse kinematics calculations and singularities
may occur. Also the motion in Cartesian coordinates may result challenging or
demanding motor motions.
The MATLAB codes to generate the solutions for all examples in this chapter
are included in the online course material in Moodle.
ROBOTICS ENGINEERING
2024/2025
Section 4.1 Joint-Space Schemes
4.1
53
Joint-Space Schemes
This section considers the path and trajectory generation in which the trajectories
in space and time are described in terms of the joint angles. The start and end
points as well as the desired via points are first converted to the joint angles. Then
a smooth function is found for each joint that passes through the via points and
the end point. The time required for each segment is the same for each joint so
that all joints will reach the via points and the end point at the same time, thus
resulting the desired Cartesian position. Trajectory planning of each angle will be
performed independently of other angles except having the same duration.
4.1.1
Cubic Polynomials
In order to make a single smooth motion between the initial (θ0 ) and final (θf )
positions, there are at least four constraints specifying the initial and final angular
position and velocities as follows:
θ(0) = θ0 ,
θ̇(0) = 0,
θ(tf ) = θf
θ̇(tf ) = 0
(4.1)
These four constraints can be satisfied by a cubic polynomial, which has four
coefficients as follows:
θ(t) = a0 + a1 t + a2 t2 + a3 t3
(4.2)
giving the following joint velocity and acceleration functions:
θ̇(t) = a1 + 2a2 t + 3a3 t2
θ̈(t) = 2a2 + 6a3 t
(4.3)
Note that the velocity profile is a parabola, and that the acceleration profile is
linear. Inserting (4.2) and (4.3) into the constraints in (4.1) gives:
θ(0) = θ0 = a0
θ(tf ) = θf = a0 + a1 tf + a2 t2f + a3 t3f
θ̇(0) = 0 = a1
θ̇(tf ) = 0 = a1 + 2a2 tf + 3a3 t2f
(4.4)
Solving these four equations for four coefficients of the cubic polynomials gives:
a0 = θ0
a1 = 0
a2 = 3(θf − θ0 )/t2f
a3 =
2024/2025
(4.5)
−2(θf − θ0 )/t3f
ROBOTICS ENGINEERING
54
CHAPTER 4 Trajectory Planning
There is always a limit to the velocity and acceleration of each joint. The
maximum velocity amplitude can be calculated by making the acceleration zero:
a2
a22
2a2 + 6a3 t = 0 ⇒ t = −
⇒ θ̇max = a1 −
3a3
3a3
(4.6)
Example 4.1. Consider a joint angle in the steady state initial position at θ = 15
degrees. It is desired to move the joint in a smooth manner to θ = 75 degrees in
3 seconds. Define a cubic trajectory to achieve this motion and bring the manipulator to rest at the destination. Plot the position, velocity and acceleration of the
joint as a function of time.
Inserting θ0 = 15, θf = 75, and tf = 3 into (4.5) gives the following:
[ a0 , a1 , a2 , a3 ] = [ 15, 0, 20, −4.44 ]
This results in the following trajectory for the joint:
θ(t) = 15 + 20t2 − 4.44t3
θ̇(t) = 40t − 13.33t2
θ̈(t) = 40 − 26.66t
The maximum velocity can be calculated from Eq. (4.6) as θ̇max = 30.03. Figure 4.3 shows the position, velocity and acceleration functions as a function of
time. It is important to note that the polynomial functions are only valid for
0 ≥ t ≥ tf .
4.1.2
Cubic Polynomials and Via Points
The above cubic trajectory generation algorithm in (4.5) assumes zero initial and
final velocities. This can be applied to paths with specified via points, if the manipulator is allowed to come to rest at each via points. However, in most applications, it is desirable to pass through via points without stopping. In this case,
a more general trajectory planning is required to take into account the non-zero
initial and final velocities for a cubic function.
If velocities at via points are specified
To construct a cubic function for each section between via points, the following
more general initial and final conditions can be used:
θ0 (0) = θ0 ,
θ̇(0) = θ̇0 ,
θ(tf ) = θf
θ̇(tf ) = θ̇f
ROBOTICS ENGINEERING
(4.7)
2024/2025
Section 4.1 Joint-Space Schemes
55
80
Position
Velocity
Acceleration
Signal values
60
40
20
0
−20
−40
0
0.5
1
1.5
Time (s)
2
2.5
3
Figure 4.3: Cubic trajectory in Example 4.1
Combining these four constraints with the cubic function would give the following linear equations for the unknown cubic coefficients:

 
θ0
1
 θf   1

 
 θ̇0  =  0
0
θ̇f
 

0 0
0
a0


tf t2f t3f 
  a1 


1 0
0
a2 
2
1 2tf 3tf
a3
(4.8)
This can be solved numerically by using matrix inversion, or analytically by
elimination to get:
a0 = θ0
a1 = θ̇0
2
1
3
a2 = 2 (θf − θ0 ) − θ̇0 − θ̇f
tf
tf
tf
2
1
a3 = − 3 (θf − θ0 ) + 2 (θ̇f + θ̇0 )
tf
tf
(4.9)
The following MATLAB function generates a cubic trajectory for a given time
duration, initial and final positions and velocities:
2024/2025
ROBOTICS ENGINEERING
56
CHAPTER 4 Trajectory Planning
function p3=cubic(x_0,x_f,dx_0,dx_f,t_f)
% generic function to calculate cubic polynomial
% coefficients from the intial and final
% positions and velocities and time duration.
B=[x_0; x_f; dx_0; dx_f];% left hand side vector
% matrix of parameters
A=[ 1 0 0 0;...
1 t_f t_f^2 t_f^3;...
0 1 0 0;...
0 1 2*t_f 3*t_f^2];
% coefficient values
C=inv(A)*B;
% store this in reverse order
% (Matlab convention)
p3=C(4:-1:1);
If the velocities at via points are not specified
The user can either apply a suitable heuristic, or introduce additional constraints
such as the velocity and acceleration at the via point to be continuous. If there n
sections, there will be n cubic functions with a total of 4n unknown coefficients
to solve. For example, if there is a single via point (i.e. two-section trajectory),
the following MATLAB function generates two cubic splines such that the cubics
are continuous (same velocities) at the via point.
function [p3_1,p3_2]=cubicvia(x_0,x_v,x_f,t_f1,t_f2)
% calculates two cubic functions to define a trajectory
% starting at x_0 ending at x_f through a via point x_v.
% Initial and final velocities are zero. The algorithm
% ensures smooth switch from cubic1 to cubic2 with the
% same velocity and acceleration at the via point.
% left hand side vector
B=[x_0; x_v; x_v; x_f; 0; 0; 0; 0];
% matrix of parameters
A=[1 0 0 0 0 0 0 0;...
1 t_f1 t_f1^2 t_f1^3 0 0 0 0;...
0 0 0 0 1 0 0 0;...
0 0 0 0 1 t_f2 t_f2^2 t_f2^3;...
0 1 0 0 0 0 0 0;...
0 0 0 0 0 1 2*t_f2 3*t_f2^2;...
0 1 2*t_f1 3*t_f1^2 0 -1 0 0;...
ROBOTICS ENGINEERING
2024/2025
Section 4.1 Joint-Space Schemes
57
0 0 2 6*t_f1 0 0 -2 0];
% coefficient values
C=inv(A)*B;
% store this in reverse order (Matlab convention)
p3_1=C(4:-1:1); p3_2=C(8:-1:5);
Example 4.2. Solve for the coefficients of two cubics that are connected in a twosegment spline with continuous acceleration at the via point. The initial angle is
15 degrees, the via point is 35 degrees, and the final destination is 75 degrees. It
is desired to reach the via point at 2 seconds and the final destination in 4 seconds
from the start of the motion.
First, define two cubics, one for each segment.
Section 1: θ1 (t) = a10 + a11 t + a12 t2 + a13 t3 , 0 ≤ t ≤ 2
Section 2: θ2 (t) = a20 + a21 t + a22 t2 + a23 t3 , 0 ≤ t ≤ 2
There are 8 coefficients to determine. The eight constraints can be constructed
as follows:
θ1 (0)
θ2 (0)
θ̇1 (0)
θ̇1 (2)
=
=
=
=
15, θ1 (2) = 35
35, θ2 (2) = 75
0, θ˙2 (2) = 0;
θ̇2 (0), θ̈1 (2) = θ̈2 (0)
This gives the following 8 linear equations:

 
15
1 0
0
0 0 0
 35   1 2
4
8 0 0

 
 35   0 0
0
0 1 0

 
 75   0 0
0
0 1 2

 
 0 = 0 1
0
0 0 0

 
 0   0 0
0
0 0 1

 
 0   0 −1 −4 −12 0 1
0
0 0 −2 −12 0 0

 
0 0
a10


0 0 
  a11 


0 0 
  a12 


4 8   a13 

 a20 
0 0 
 



4 12 
  a21 


a22 
0 0
a23
2 0
Solving this would give the following coefficient values:
[ a10 , a11 , a12 , a13 ] = [ 15, 0, 3.75, 0.625 ]
and
[ a20 , a21 , a22 , a23 ] = [ 35, 22.5, 7.5, −4.375 ]
The overall trajectory is shown in Fig. 4.4.
2024/2025
ROBOTICS ENGINEERING
58
CHAPTER 4 Trajectory Planning
80
Position
Velocity
Acceleration
Signal values
60
40
20
0
−20
−40
0
0.5
1
1.5
2
2.5
Time (s)
3
3.5
4
Figure 4.4: Trajectory for Example 4.2
4.1.3
Higher Order Polynomials
Higher order polynomials are also used in trajectory planning. The most common
one is the fifth order polynomial. If the position, velocity and acceleration at
the start and the end of the motion is specified, i.e. six constraints, a fifth order
polynomial (with six coefficients) is needed:
θ(t) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4 + a5 t5
(4.10)
Inserting this into six constraints gives the following linear equations for unknown
polynomial coefficients:
 
θ0
1
 θf   1

 
 θ̇0   0

 
 θ̇f  =  0

 
 θ̈0   0
0
θ̈f

 

0 0
0
0
0
a0


tf t2f t3f
t4f
t5f 
  a1 


1 0
0
0
0 
  a2 
2
3
4  
1 2tf 3tf 4tf 5tf   a3 

0 2
0
0
0   a4 
a5
0 2 6tf 12t2f 20t3f
ROBOTICS ENGINEERING
(4.11)
2024/2025
Section 4.1 Joint-Space Schemes
59
The analytical solution is:
a0 = θ 0
a1 = θ̇0
a2 = θ̈20
a3 =
a4 =
a5 =
20θf −20θ0 −(8θ̇f +12θ̇0 )tf −(3θ̈0 −θ̈f )t2f
(4.12)
2t3f
30θ0 −30θf +(14θ̇f +16θ̇0 )tf +(3θ̈0 −2θ̈f )t2f
2t4f
12θf −12θ0 −(6θ̇f +6θ̇0 )tf −(θ̈0 −θ̈f )t2f
2t5f
Alternatively, equation (4.11) can be written in the following matrix form:
B = AC
(4.13)
This can be solved numerically for the coefficient vector C
C = A−1 B
(4.14)
The following MATLAB function calculates a fifth order polynomial trajectory function to achieve given position, velocity and acceleration at the start and
end points:
function p5=fifthorder(x_0,x_f,dx_0,dx_f,ddx_0,ddx_f,t_f)
% generic function to calculate a fifth order polynomial
% that satisfies specified intial and final positions,
% velocities and accelerations within a given time duration.
% left hand side vector
B=[x_0; x_f; dx_0; dx_f; ddx_0; ddx_f];
% matrix of parameters
A=[1 0 0 0 0 0; 1 t_f t_f^2 t_f^3 t_f^4 t_f^5; 0 1 0 0 0 0;...
0 1 2*t_f 3*t_f^2 4*t_f^3 5*t_f^4; 0 0 2 0 0 0;...
0 0 2 6*t_f 12*t_f^2 20*t_f^3];
% coefficient values
C=inv(A)*B;
% store this in reverse order (Matlab convention)
p5=C(6:-1:1);
Example 4.3. Solve the problem in Example 4.1 on Page 54 to achieve much
smoother motion with added requirements of zero acceleration at the start and the
end of the motion.
Equations (4.11) and 4.12 can be used with θ0 = 15, θf = 75, tf = 3, and
θ̇0 = θ̇f = θ̈0 = θ̈f = 0, giving the following fifth order trajectory coefficients:
[ a0 , a1 , a2 , a3 , a4 , a5 ] = [ 15, 0, 0, 22.2222, −11.1111, 1.4815 ]
2024/2025
ROBOTICS ENGINEERING
60
CHAPTER 4 Trajectory Planning
80
Position
Velocity
Acceleration
Signal values
60
40
20
0
−20
−40
0
0.5
1
1.5
Time (s)
2
2.5
3
Figure 4.5: Fifth order polynomial trajectory for Example 4.3
The resultant trajectory is shown in Fig. 4.5.
Example 4.4. Solve the problem in Example 4.2 on Page 57 by using a single
polynomial.
There are 5 constraints:
θ(0) = 15, θ(2) = 35, θ(4) = 75, θ̇(0) = 0, θ̇(4) = 0
Therefore a forth order polynomial is needed to satisfy all of the constraints:
θ(t) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4
with derivatives:
θ̇ = a1 + 2a2 t + 3a3 t2 + 4a4 t3
θ̈ = 2a2 + 6a3 t + 12a4 t2
Writing all constraints in a matrix form:
 

 

1 0 0 0
0
a0
15
 35   1 2 4 8 16   a1 

 
 

 75  =  1 4 16 64 256   a2 

 
 

 0   0 1 0 0
0   a3 
0
0 1 8 48 256
a4
ROBOTICS ENGINEERING
2024/2025
Section 4.1 Joint-Space Schemes
61
and solving this should give:
[a0 , a1 , a2 , a3 , a4 ] = [15, 0, 1.25, 3.125, −0.625]
The resulting trajectory is given in Fig. 4.6, which has a smoother acceleration
profile than the trajectory in Fig. 4.4.
80
Position
Velocity
Acceleration
60
Signal values
40
20
0
−20
−40
−60
0
0.5
1
1.5
2
2.5
Time (s)
3
3.5
4
Figure 4.6: A forth order polynomial trajectory through a via point in example 4.4
The following MATLAB function generates a forth order polynomial trajectory with a via point.
function p4=forthvia(x_0,x_v,x_f,dx_0,dx_f,t_fv, t_f)
% generic function to calculate forth order polynomial
% coefficients from the intial and final positions
% and velocities with a via point and time duration.
% left hand side vector
B=[x_0; x_v; x_f; dx_0; dx_f];
% matrix of parameters
A=[ 1 0 0 0 0;...
1 t_fv t_fv^2 t_fv^3 t_fv^4;...
1 t_f t_f^2 t_f^3 t_f^4;...
0 1 0 0 0;...
0 1 2*t_f 3*t_f^2 4*t_f^3];
2024/2025
ROBOTICS ENGINEERING
62
CHAPTER 4 Trajectory Planning
% coefficient values
C=inv(A)*B;
% store this in reverse order (Matlab convention)
p4=C(5:-1:1);
When the trajectory planning involves more constraints and via points, it is
possible to use higher order polynomials so that the number of coefficients is
equal to the number of constraints. However, using higher order polynomials has
its own computational problems. It is preferable to use combination of lower order
polynomials for different segments of the trajectory and blend them to satisfy all
conditions as done in Example 4.2.
4.1.4
Linear Segments with Parabolic Blends
Another popular path shape is linear, that is a constant velocity between the initial
and final locations. However, this ideal trajectory is not practical as it requires
infinite accelerations at the beginning and infinite deceleration at the end of the
motion. Therefore, a parabolic blend region at each end of the path is used. A
constant finite acceleration in blend regions would results a second order polynomial as shown in Fig. 4.7. For a given acceleration a, the trajectory equations for
three regions are as follows:
Region I: 0 ≤ t ≤ tb
1
θ = θ1 + at2 ,
2
Region II:
θ̇ = at,
tb < t ≤ tf − tb
θ = θb1 + ω(t − tb ),
Region III:
θ̈ = a
θ̇ = atb = ω,
θ̈ = 0,
1
where θb1 = θ1 + at2b
2
tf − tb < t ≤ tf
1
θ = θf − a(tf − t)2 ,
2
θ̇ = a(tf − t),
θ̈ = −a
The value of tb can be calculated by using the fact that the area under the
velocity curve gives the change in position, i.e.
θf − θ0 = atb (tf − tb )
(4.15)
at2b − atf tb + (θf − θ0 ) = 0
(4.16)
or
ROBOTICS ENGINEERING
2024/2025
Section 4.1 Joint-Space Schemes
63
θ
Position
2
Linear
θ
Velocity
1
ω
Acceleration
0
a
0
−a
0
t_b
t_f
Time (s)
Figure 4.7: Trapezoid velocity profile for trajectory planning
If a is specified (usually the maximum acceleration safely allowed for the joint),
then
q
a2 t2f − 4a(θf − θ0 )
tf
tb =
−
(4.17)
2
2a
Obviously, in order the solution to exist, the term inside the square root in (4.17)
must be positive, i.e.
a≥
4(θf − θ0 )
t2f
(4.18)
Therefore the acceleration a must be sufficiently high in order the robot to complete the move within the specified time duration tf . When equality occurs in
(4.18) the linear portion (constant speed) of the trajectory disappears. As the acceleration becomes larger, the length of constant speed region becomes longer.
The following MATLAB function can be used to calculate a trapezoid velocity
trajectory:
2024/2025
ROBOTICS ENGINEERING
64
CHAPTER 4 Trajectory Planning
function [x,dx,ddx]=Trapezoid(x_0,x_f,t_f,acc,time)
% calculates a trapezoid velocity trajectory from
% x_0 to x_f in time duration t_f with a given
% acceleration. "time" is a time vector.
% initialise output in case of an error
x=zeros(size(time)); dx=zeros(size(time));
ddx=zeros(size(time));
% check errors in the argument list
if acc<4*(x_f-x_0)/t_f^2
disp([’Error: acceleration is not sufficient to ’...
’achieve the motion’]); return;
elseif time(1)<0
disp([’Error: time vector starts with a negative ’...
’time value’]); return;
elseif time(end)>t_f
disp([’Error: time vector goes beyond the ’...
’duration of motion’]); return
end
t_b=0.5*t_f-0.5*sqrt(acc^2*t_f^2-4*acc*(x_f-x_0))/acc;
t_b2=t_f-t_b; w=acc*t_b;
x_b=x_0+0.5*acc*t_b^2; x_b2=x_b+w*(t_f-2*t_b);
x=(time<t_b).*(x_0+0.5*acc*time.^2)+...
(time>=t_b).*(time <=t_b2).*(x_b+w*(time-t_b))+...
(time>t_b2).*(x_b2+w*(time-t_b2)-0.5*acc*(time-t_b2).^2);
dx=(time<t_b).*(acc*time)+...
(time>=t_b).*(time<=t_b2).*(w)+...
(time>t_b2).*(w-acc*(time-t_b2));
ddx=(time<t_b).*acc+(time>=t_b2).*(-acc);
Example 4.5. Apply trapezoid velocity profile to move a joint from the initial 15
degrees to the final destination at 75 degrees in 3 seconds for an acceleration of
(a) the minimum possible, (b) 30 degree/s2 , (c) 48 degree/s2 .
(a) The minimum acceleration is:
amin =
4(75 − 15)
= 26.667 degree/s2
2
3
This gives tb = tf /2 = 1.5 s, and ω = 40 degree/s for zero seconds in
Region II.
(b) Inserting a = 30 in (4.17) gives tb = 1 s, which corresponds to a constant
speed value of ω = 30 degree/s for 1 s in Region II.
ROBOTICS ENGINEERING
2024/2025
Section 4.2 Cartesian-Space Schemes
65
(c) Inserting a = 48 in (4.17) gives tb = 0.5 s, which corresponds to a constant
speed value of ω = 24 degree/s for 2 s in Region II.
The resulting trajectories are shown in Fig. 4.8 for all three acceleration values.
Position (deg)
80
60
40
Acc=26.667
Acc=30
Acc=48
20
0
0
0.5
1
1.5
Time (s)
2
2.5
3
0
0.5
1
1.5
Time (s)
2
2.5
3
0
0.5
1
1.5
Time (s)
2
2.5
3
Velocity (deg/s)
40
30
20
10
Acceleration (deg/s2)
0
50
0
−50
Figure 4.8: Trapezoid velocity profile in Example 4.5
4.2
Cartesian-Space Schemes
When trajectories are constructed in joint space, the via and destination points
are attained from their specification in Cartesian coordinates. However, spatial
shape of the path is not a straight line between via points, but some complicated
shape depending on the kinematics of the manipulator as shown in the following
example.
2024/2025
ROBOTICS ENGINEERING
66
CHAPTER 4 Trajectory Planning
Example 4.6. Consider a two-link manipulator with equal link lengths of 1 m
each to move from the initial position of (1.3, -0.75) m to a final position of (1.3,
1.2) m. Plan the trajectory in joint coordinates and show the motion of the endeffector in Cartesian space.
In joint coordinates, the start and end positions corresponds (11.392, -82.747)
degrees and (70.508, -55.998) degrees, respectively. A fifth order polynomial
trajectory planning scheme would result in the following trajectories:
θ1 = 11.392 + 73.895t3 − 55.4212t4 + 11.0842t5
θ2 = −82.747 + 33.4362t3 − 25.0772t4 + 5.0154t5
When forward kinematics calculations are carried out along the path, the trajectory
in the Cartesian coordinates is not a straight line as shown in Fig. 4.9.
Joint−space
Path
100
Angles (deg)
2
1.5
B
y−axis
1
θ1
0
θ2
−100
0.5
↑
0
0
1
Cartesian−space
2
2
Disp (m)
−0.5
A
−1
−1.5
0
1
x−axis
2
x
1
0
−1
y
0
1
Time (s)
2
Figure 4.9: Trajectory planning in the joint-space, and corresponding motion in
the Cartesian-space in Example 4.6
Trajectory planning can also be performed in Cartesian coordinates directly
from the user definition of via and final points without first performing the inverse kinematics. The generation of Cartesian-space trajectories follows the same
strategies as the generation of joint-space trajectories as discussed in Section 4.1.
Example 4.7. Consider the two-link manipulator in Example 4.6. Design a straight
line trajectory in Cartesian coordinates and show the corresponding trajectory of
the joint angles.
ROBOTICS ENGINEERING
2024/2025
Section 4.2 Cartesian-Space Schemes
67
Applying a fifth order polynomial trajectory planning scheme to the motion of
x and y gives the following trajectories:
x = 1.3
y = −0.75 + 2.4375t3 − 1.8281t4 + 0.3656t5
When inverse kinematic calculations are carried out along the path, the trajectories
in joint coordinates can be obtained as shown in Fig. 4.10.
(a) Cartesian−space
Path
2
Disp (m)
2
1.5
B
0
−1
0.5
↑
0
y
0
1
(b) Joint−space
2
100
Angles (deg)
y−axis
1
x
1
−0.5
A
−1
−1.5
0
1
x−axis
2
θ1
0
−100
θ2
0
1
Time (s)
2
Figure 4.10: Trajectory planning in (a) Cartesian-space, and (b) the corresponding
motion in joint-space in Example 4.7
However, to run the manipulator, inverse kinematics must be solved repeatedly
along the trajectory at the path-update rate. This is computationally expensive.
Other problems with Cartesian-Space schemes include
1. Intermediate points may be unreachable, i.e. outside the workspace of the
manipulator.
2. Singularity may exist along the path. This results very high (theoretically
infinite) desired joint velocities, and due to velocity limits of the joints,
results deviation from the desired path.
3. Start and final positions may be reachable in different physical solutions.
Most manipulators demonstrate multiple physical solutions when performing inverse kinematics. For example, a two-link manipulator has two configurations to achieve the same end-effector position.
2024/2025
ROBOTICS ENGINEERING
68
4.3
CHAPTER 4 Trajectory Planning
Tutorial Questions
4.1. How many individual cubics are computed when a six-jointed robot moves
along a cubic spline path through two via points and stops at a goal points? How
many coefficients are stored to describe these cubics?
Answer: 18, 72
4.2. It is desired to have a joint of a robot move from the initial angle of 50o to
a final angle of 80o in 3 seconds. Calculate a third-order polynomial joint-space
trajectory and determine the joint angles, velocities and accelerations at 1, 2, and
3 seconds. It is assumed that the robot starts from rest and stops at its destination.
Answer: θ = 50 + 10t2 − 2.222t3 ; 57.78o , 13.332 deg/s, 6.668 deg/s2 ; 72.22o ,
13.34 deg/s, -6.664 deg/s2 ; 80o , 0 degree/s, -20 deg/s2
4.3. Construct a two-segment spline where each segment is a cubic. The requirement is to move the joint angle from 5o to 40o in 2 s by passing through a via point
at 15o in 1 s with a velocity of 17.5o /s.
Answer: 5 + 12.5t2 − 2.5t3 and 15 + 17.5t + 40t2 − 32.5t3
4.4. A fifth order polynomial is to be used to control the motions of the joints of
a robot in the joint-space. Find the coefficients of a fifth-order polynomial that
will allow a joint to go from an initial angle of 0o to a final joint angle of 75o
in 3 seconds, while the initial and final velocities are zero and initial and final
accelerations are 10 deg/s2 .
Answer: θ(t) = 5t2 + 24.44t3 − 13.33t4 + 1.85t5
4.5. Joint 1 of a 6-axis robot is to go from an initial angle of θ0 = 30o to the final
angle of θf = 120o in 4 seconds with a cruising velocity of ω1 = 30 deg/s. Find
the blending time for a trajectory with linear segments and parabolic blends and
trajectory functions in all regions.
Answer: 1 s; θ = 30 + 15t2 , θ̇ = 30t, θ̈ = 30; θ = 45 + 30(t − 1), θ̇ = 30, θ̈ = 0;
θ = 120 − 15(4 − t)2 , θ̇ = 30(4 − t), θ̈ = −30
4.6. A 2-DOF planar robot is to follow a straight line in Cartesian-space between
the start (0.2,0.6) m and the end (1.2,0.3) m points in 4 seconds. (a) Use a cubic
spline to plan the trajectory in the Cartesian-space, and (b) calculate the joint
angles, and joint accelerations at t = 0 and 4 seconds. Each link is 1 m long, and
the first joint angle has a restricted operation between 10o and 180o .
Answer: (a) x(t) = 0.2 + 0.1875t2 − 0.0313t3 , y(t) = 0.6 − 0.0562t2 + 0.0094t3
(b) (143.13,-143.13)o , (65.86,-103.29)o , (-35.81,0.72)o /s2 , (21.53,-24.53)o /s2
ROBOTICS ENGINEERING
2024/2025
Chapter 5
Equations of Motion
5.1
Introduction & learning outcomes
Up to now we have considered only positions and trajectories, without taking into
account the dynamic behaviour of robotic systems. In the chapters that follow,
the dynamics of the system will be explored and used to determine appropriate
controllers. This chapter introduces the core concepts needed for the dynamic
analysis of robot linkage systems. First, generalised coordinates are introduced
as a general means of describing a robot’s position. These are accompanied by
generalised inputs to represent the forces acting on the robot. Equations of
motion are written in terms of the system states and state derivatives. These
equations balance all the forces acting on a system including the inertial forces,
gravitational forces and externally applied forces, and can be concisely represented using matrix-vector form. The chapter finishes by looking at the types of
dynamics problems that can be solved using these equations, and an overview of
the methods that will be used in this course to assemble the equations of motion.
The intended learning outcomes are:
1. To know what generalised coordinates are, how they are selected, and how
they relate to the joint space, the Cartesian space, and the system’s degrees
of freedom.
2. To understand how generalised inputs relate to generalised coordinates and
what they represent.
3. To understand the general form of the nonlinear equations of motion and
when to use forward and inverse dynamic methods.
4. To be able to assemble a matrix-vector equation of motion from individual
equations of motion.
69
70
CHAPTER 5 Equations of Motion
5. To know what types of forces are represented by each of the terms in the
equations of motion.
5.2
Generalised Coordinates and Generalised Inputs
In Chapters 2 and 3 you became familiar with two coordinate spaces:
Joint space uniquely defines the position of every part of the robot in terms of
the joint angles or displacements.
End effector space uses Cartesian coordinates and rotations of the end effector
but does not uniquely define the robot configuration (as you know from
solving the inverse kinematics problem).
If the end effector space description is supplemented with a small amount of further information (e.g. elbow-up or elbow-down) then both of these coordinate
spaces can be used to provide a unique definition of the robot position. The minimum number of coordinates needed to fully describe the robot’s position in any
coordinate system is equal to the number of degrees of freedom, M .
In the chapters to come, generalised coordinates will be used to determine
the equations of motion. You can choose whatever position measurements you
like to be the generalised coordinates, but as you will see, some choices are more
convenient than others. The number of generalised coordinates, N , must be at
least as big as the number of degrees of freedom of the system:
N ≥M
It is also important that at least M linearly independent coordinates are used.
Where N > M there will be (N − M ) dependent variables. For now, we will
consider the most straightforward case of M = N , and we will normally use the
joint space as the generalised coordinates.
The generalised coordinates are denoted q1 , q2 , . . . , qN , or in vector form




q
1



 q2 

q=
..

. 



 q 

N
The generalised coordinates are accompanied by a set of generalised inputs, one
ROBOTICS ENGINEERING
2024/2025
Section 5.3 Matrix-Vector Equations of Motion
71
for each generalised coordinate. These are denoted Q1 , Q2 , . . . , QN or




Q
1



 Q2 

Q=
..

. 



 Q 

N
Each generalised input corresponds with a force or torque acting in the direction
of the respective generalised coordinate. If the generalised coordinate is a relative
displacement between two links (such as a revolute joint angle) then the generalised input acts physically on both links with equal and opposite measure.
Example 5.1. Figure 5.1 shows a planar polar robot. The system is comprised of
two rigid bodies, one revolute joint, and a prismatic joint. It has two degrees of
freedom, M = 2. A minimum of two coordinates are needed to define its position.
The obvious choices are to use the Cartesian coordinates of the end effector (x, y)
or the joint positions (θ, d). In this case, both choices uniquely define the position
of the robot and would make suitable choices for the generalised coordinates:
x
θ
q=
or
q=
y
d
It’s tempting to think that the coordinates could be mixed and matched, but in
this case that would not work well. For example the choice of q = [d x]T could
simultaneously describe two possible positions (above and below y = 0). A more
promising choice is q = [θ x]T , but using this choice one encounters a singularity
(more on which later) where θ = ± π2 because then x = 0 and it is not possible to
determine the position of the prismatic joint.
Example 5.2. Figure 5.2 shows a planar SCARA robot. The system is comprised
of two rigid bodies and two revolute joints. Again it has two degrees of freedom
and again the position can be described either in terms of the Cartesian coordinates
of the end effector (x, y) or the joint positions (θ1 , θ2 ). In this case the Cartesian
coordinates do not produce a unique description, as the elbow can face in either
direction, so the most suitable choice of generalised coordinates is
θ1
q=
θ2
5.3
Matrix-Vector Equations of Motion
The equations of motion for a nonlinear, conservative system such as the robotic
manipulators studied here can be written in the following general form:
M(q)q̈ + C(q, q̇) + G(q) = Q
2024/2025
(5.1)
ROBOTICS ENGINEERING
72
CHAPTER 5 Equations of Motion
Figure 5.1: Planar polar robot
where
M(q) is the N × N mass matrix,
C(q, q̇) is an N × 1 vector of centrifugal (q̇i2 terms) and Coriolis and precession (q̇i q̇j , j 6= i, terms) forces/torques,
G(q) is an N ×1 vector of gravity forces/torques (or more generally, restorative forces/torques), and
Q is the vector of generalised inputs.
Together, this matrix-vector equation comprises N equations of motion, corresponding to the N degrees of freedom in the system. The vectors C and G and the
matrix M in this equation are nonlinear functions of the generalised coordinates,
q, and their first derivatives with respect to time, q̇. These positions and velocities
together form the states, x, of the system:
q
z=
q̇
The states contain all the information needed to describe the dynamics of a system.
With knowledge of the states at a given moment in time it is possible to determine
ROBOTICS ENGINEERING
2024/2025
Section 5.3 Matrix-Vector Equations of Motion
73
Figure 5.2: Planar SCARA robot
how the states will change in the next instant. There are 2N states: twice as
many states as degrees of freedom (a position and a velocity for each linearly
independent generalised coordinate).
The equation of motion is a force balance equation. The forces collected on
the left hand side here arise from the intrinsic dynamics of the system, and are
all conservative forces. This means that no energy is lost or gained in the system
as a result of these forces: it is simply transferred between kinetic and potential
energy. The mass matrix multiplied by the accelerations of the generalised coordinates, M(q)q̈, describes the inertial forces due to linear accelerations (where
linear in this case can refer to translational or rotational motion, but differentiates these accelerations from those due to angular velocity). The gyroscopic
force vector, C(q, q̇) represents the remainder of the inertial forces, principally
the centrifugal/centripetal, Coriolis, and precession forces/torques arising from
angular velocities in the system. The gravity force vector, G(q), is comprised
of the restorative forces, which generally include forces arising from any kind
of potential, such as electric fields, magnetic fields, elastic potential (springs), or
gravitational fields. In the examples covered here only gravitational fields will be
considered.
The right hand side of the equation of motion contains only the generalised
inputs, Q. These inputs can be comprised of torques/forces from a number of
different sources, including the actuator and motor forces used to control the robot,
any external forces acting on the robot (except gravity) and any non-conservative
forces that may be present such as damping in the joints.
2024/2025
ROBOTICS ENGINEERING
74
CHAPTER 5 Equations of Motion
5.4
Types of dynamical problem
As with kinematics, there are two types of dynamical problem. Almost every
problem in classical dynamics is a special case of one of the following general
types:
• Forward dynamics: Allows the motion of the system (i.e. the position,
velocity and acceleration of each generalised coordinate as a function of
time) to be found given the forces and torques acting on the system, any
constraints, and the initial conditions. This is useful for simulating the behaviour of robotic systems.
• Inverse dynamics: Allows the calculation of a possible set of forces and
torques, as a function of time, that will produce a specified motion trajectory. This can be used to create high fidelity control systems. Most of the
work in this course will consider inverse dynamics.
5.5
Constructing the equations of motion
There are many ways of constructing the equations of motion for a system. In this
course we will cover two of the most popular methods:
• Computational methods for serial manipulators: These methods are used
for the special case of serial robotic manipulators, described by DenavitHartenberg parameters. They follow naturally from the kinematic transforms covered in the preceding chapters. They are useful because once the
kinematics of a serial robot are known, the only further information needed
is the mass properties of the robot; the computer can then determine the
equations of motion for a given state programatically.
• Lagrangian Mechanics: These methods are some of the most generally applicable, and can be used across a wide range of physical domains, not just
mechanical. For robotics, they apply equally well to parallel robots and serial robots, planar or 3-dimensional systems, and are especially useful for
complex mechanisms where Newtonian methods can be more difficult to
formulate. They are also used to derive procedures for special cases such as
the DH-based serial manipulators above.
Both these methods will be covered in the chapters to come. Principally we
will be interested in the inverse dynamics, used to determine control forces/torques.
Later we will cover the forward dynamics used to simulate physical systems.
ROBOTICS ENGINEERING
2024/2025
Chapter 6
Transform Derivatives
6.1
Introduction & learning outcomes
Velocity and acceleration are important quantities in dynamic analyses. The internal forces acting on the robots considered here are comprised of inertia forces
and gravitational body forces. To compute these forces it is necessary first to
derive expressions for the velocity and acceleration of every point in the robot
structure. These quantities can be determined for a serial robot by considering
the kinematic transform matrices assembled using the Denavit-Hartenberg conventions descirbed in the preceding chapters. The expressions for velocity and
acceleration rely on derivatives of the homogeneous transform matrices with respect to the joint motions (or more formaly, with respect to the motion of the
generalised coordinates). This chapter will show how the transform derivatives
are computed, and how the velocity and acceleration can be determined from the
transform derivatives. The intended learning outcomes are:
1. Understand what a transform derivative is and how to compute it for the general case, as well as shortcuts for computing prismatic and revolute joints
using the DH convention.
2. Know when to use a second order derivative, and know how to use the
associated computational shortcuts.
3. Know how to calculate velocity and acceleration in the base frame for arbitrary points on a link given joint velocities and transform matrices.
4. Understand how the calculations here relate to the Jacobian matrix.
75
76
6.2
CHAPTER 6 Transform Derivatives
Velocities and Accelerations
The position in the base frame of a point on a link is given by
pi = Ti0 ri
(6.1)
where i is the link number, Ti0 is the homogeneous transform relating the ith coordinate frame to the base frame, and ri is the position of the point on the link in
the ith coordinate frame. Remember that ri is a constant because the point moves
with the link’s coordinate frame. The velocity in the base frame is then given by
ṗi =
dTi0
ri
dt
(6.2)
Using partial derivatives, this becomes
ṗi =
N
X
∂T 0
i
∂qj
j=1
q̇j ri
(6.3)
The acceleration is derived using the product rule:
p̈i =
N X
d ∂T 0
i
dt
j=1
∂qj
∂Ti0
q̈j ri
q̇j +
∂qj
Using partial derivatives again,
" N
#
N
0
X
X ∂ 2T 0
∂T
i
q̇j q̇k + i q̈j ri
p̈i =
∂q
∂q
∂qj
j
k
j=1 k=1
(6.4)
(6.5)
Two new matrices are defined to represent the transform derivatives, and their
computation is discussed in the next section. The transform derivatives are
Uij =
∂Ti0
∂qj
Uijk =
∂ 2 Ti0
∂qj ∂qk
(6.6)
The velocity and acceleration of the point on link i are then
ṗi =
N
X
Uij q̇j ri
(6.7)
j=1
and
p̈i =
" N N
XX
Uijk q̇j q̇k +
j=1 k=1
ROBOTICS ENGINEERING
N
X
#
Uij q̈j ri
(6.8)
j=1
2024/2025
Section 6.3 Homogeneous Transform Derivatives
6.3
77
Homogeneous Transform Derivatives
The transform derivatives describe the motion of the coordinate frames with respect to joint motions:
Uij describes how the ith coordinate frame moves when the j th joint is
moved.
Uijk describes how the movement of the ith coordinate frame with the j th
joint is affected by changing the position of the k th joint.
To find the derivative of a homogeneous transform you first note that the transform
is the product of all the individual transforms to go from the base frame to the link
frame:
Ti0 =
i
Y
Tpp−1 = T10 × T21 × · · · × Tii−1
(6.9)
p=1
For example,
T40 = T10 T21 T32 T43
(6.10)
The derivative can be obtained using product rule:
0
∂T1
∂Ti0
i−1
1
=
× T2 × · · · × Ti
Uij =
∂qj
∂qj
∂T21
i−1
0
+ T1 ×
× · · · × Ti
∂qj
∂Tii−1
0
1
+ · · · + T1 × T2 × · · · ×
∂qj
(6.11)
(6.12)
(6.13)
Then observe that the only transform affected by qj is Tjj−1 . So all you have to do
to get the derivative of the full transform with respect to qj is to replace Tjj−1 in
equation 6.9 with its derivative, ∂Tjj−1 /∂qj :
j−1
∂Tj
∂Ti0
= T10 × · · · ×
× · · · × Tii−1
Uij =
∂qj
∂qj
(6.14)
For example,
U42 =
2024/2025
∂T40
∂T 1
= T10 2 T32 T43
∂q2
∂q2
(6.15)
ROBOTICS ENGINEERING
78
CHAPTER 6 Transform Derivatives
To find the derivatives of the transform matrices, we return to the DenavitHartenberg convention for the homogeneous transform:


cosθj −sinθj cosαj sinθj sinαj aj cosθj
 sinθj cosθj cosαj −cosθj sinαj aj sinθj 

Tjj−1 = 
(6.16)
 0

sinαj
cosαj
dj
0
0
0
1
The derivative of this transform with respect to a prismatic joint parameter, dj , is


0 0 0 0
∂Tjj−1  0 0 0 0 

=
(6.17)
 0 0 0 1 
∂dj
0 0 0 0
The derivative of the transform with respect to a revolute joint parameter, θj , is


−sinθj −cosθj cosαj cosθj sinαj −aj sinθj
∂Tjj−1  cosθj −sinθj cosαj sinθj sinαj aj cosθj 

(6.18)
=


0
0
0
0
∂θj
0
0
0
0

 

0 −1 0 0
cosθj −sinθj cosαj sinθj sinαj aj cosθj
 1 0 0 0   sinθj cosθj cosαj −cosθj sinαj aj sinθj 
 

=
 0 0 0 0 × 0

sinαj
cosαj
dj
0 0 0 0
0
0
0
1
(6.19)


0 −1 0 0
 1 0 0 0 
j−1

=
(6.20)
 0 0 0 0  × Tj
0 0 0 0
In practice this means you negate the second row of Tjj−1 , then swap the first and
second rows and set the third and fourth rows to all zeros.
To compute the higher order derivative Uijk , simply replace both Tjj−1 and
Tkk−1 in equation 6.10 with their derivatives. For example:
∂T 1 ∂T 3
∂ 2 T50
= T10 2 T32 4 T54
(6.21)
∂q2 ∂q4
∂q2
∂q4
If j = k then you need to take the second derivative; these are computed the
same way as above to give


0 0 0 0
∂ 2 Tjj−1  0 0 0 0 

=
(6.22)
 0 0 0 0 
∂dj 2
0 0 0 0
U524 =
ROBOTICS ENGINEERING
2024/2025
Section 6.4 Relationship to Jacobian
79
and


−1 0 0 0
∂ 2 Tjj−1  0 −1 0 0 

 × T j−1
j
2 =  0

0
0
0
∂θj
0
0 0 0
(6.23)
The second derivative of a transform with respect to the revolute joint is equivalent
to performing the first derivative operation twice in succession.
Finally, note the following:
if
i<j
then Uij = 0
(6.24)
if
i<j
or i < k
(6.25)
then Uijk = 0
where 0 is a 4 × 4 matrix of zeros.
6.4
Relationship to Jacobian
It is worth noting that these expressions for velocity and acceleration can be related to similar expressions for the Jacobian. From Chapter 3 you should recognise
this expression for the end effector velocity:
ṗi = J q̇
(6.26)
where J is the Jacobian matrix, and q̇ is the vector of joint velocities, or generalised coordinate velocities. Differentiating eqn. (6.26) gives the end effector
acceleration as
p̈i = J˙q̇ + J q̈
(6.27)
Rearranging eqn. (6.7) it can be seen that
ṗi =
Ui1 ri Ui2 ri




q
1





q2 
. . . UiN ri
..

. 



 q 

N
(6.28)
where {q1 q2 . . . qN }T = q̇. If ri is the end effector position on the last link
then another expression for the Jacobian is given by
J = Ui1 ri Ui2 ri . . . UiN ri
(6.29)
2024/2025
ROBOTICS ENGINEERING
80
CHAPTER 6 Transform Derivatives
If the end effector is assumed to be at the origin of the frame for the last link, then
ri = {0 0 0 1}T and each column of the Jacobian corresponds to the final
column of one of the transform derivatives.
Now, comparing eqn. (6.8) to (6.27), the second term can be seen to agree with
the expression for the Jacobian derived above, while the first term can be used to
demonstrate the relationship
h P
i
PN
PN
N
J˙ =
(6.30)
U
q̇
r
U
q̇
r
.
.
.
U
q̇
r
j=1 ij1 j i
j=1 ij2 j i
j=1 ijN j i
It is less common to need to compute J˙ in isolation, so this last equation is given
just for interest.
6.4.1
A note on symbols used in dynamics
In the dynamics notes that follow, we will not be using the Jacobian but will
instead work in terms of the transform derivatives. This is important because we
will be introducing another term, the pseudo-inertia matrix, which also typically
uses the symbol J. Do not get them confused - from here on all J symbols will
refer to the pseudo-inertia matrix and not the Jacobian.
ROBOTICS ENGINEERING
2024/2025
Chapter 7
Pseudo-inertia Matrices
7.1
Introduction & learning outcomes
The pseudo-inertia matrices are commonly used in robotics to represent the inertial properties of the links: the location and distribution of the mass around the
link. They are related to the mass itself, the position of the centre of mass, the moments and the products of inertia. They are used to compute the inertial forces in
the system and are ideally suited to computations involving homogeneous transforms and transform derivatives. The reason for arranging the mass properties in
a pseudo-inertia matrix is best illustrated with an example, and the most obvious
example is in calculating the kinetic energy, so this is also covered in this chapter.
The first section in this chapter should be revision, reminding you how to calculate moments and products of inertia. The latter sections discuss first the kinetic
energy calculation, then show how this leads to the pseudo-inertia matrix itself.
The intended learning outcomes are:
1. Revision of the definitions for moments of inertia and products of inertia.
2. Know how to calculate moments and products of inertia, including where
appropriate the use of the parallel axis theorem.
3. Know the difference between an inertia tensor, or inertia matrix, and a
pseudo-inertia matrix. Be able to explain when each should be used.
4. Explain why the pseudo-inertia matrix is used, both qualitatively and based
on a derivation for kinetic energy.
5. Be able to determine the pseudo-inertia matrix for simple link geometries
based on cylinders and cuboids.
81
82
7.2
CHAPTER 7 Pseudo-inertia Matrices
Mass moments of inertia
This section describes material you should be familiar with: moments of inertia,
products of inertia, and their combination in the inertia tensor. It explains how to
calculate them and what they are used for.
7.2.1
Moments of inertia
The mass moment of inertia of the ith rigid link about its x-axis is defined as
Z
(7.1)
Ixx = (y 2 + z 2 ) dm
i
where y and z are position coordinates in the link’s frame and m is the mass. This
can be written
Z
Ixx = ρ (y 2 + z 2 ) dV
(7.2)
Vi
where V is the volume and ρ is the density, or alternatively
Z Z Z
Ixx = ρ
(y 2 + z 2 ) dx dy dz
i
i
(7.3)
i
Similarly, exressions for the moment of inertia about the y- and z-axes can be
written
Z Z Z
Iyy = ρ
(x2 + z 2 ) dx dy dz
(7.4)
i
i
i
Z Z Z
Izz = ρ
i
i
(x2 + y 2 ) dx dy dz
(7.5)
i
Examples of the mass moment of inertia for two common shapes, the cylinder
and the cuboid, are shown in Fig. 7.1
7.2.2
Products of inertia
The products of inertia are defined as
Z
Ixy = Iyx = (xy) dm
(7.6)
i
ROBOTICS ENGINEERING
2024/2025
Section 7.2 Mass moments of inertia
83
Figure 7.1: Mass moment of inertia for a cylinder and a cuboid. Pay attention to
the axis alignment for the cylinder.
Z
Iyz = Izy =
(yz) dm
(7.7)
(xz) dm
(7.8)
i
Z
Ixz = Izx =
i
These quantities describe the degree of inertial coupling between rotations about
the different axes. Note that the products of inertia are zero if the mass distribution
is symmetric about either of the respective axes. (These are then referred to as the
principle axes of rotation.)
7.2.3
Parallel axis theorem
The parallel axis theorem can be used to relate the moment of inertia about an axis
through the centre of mass to the moment of inertia about a second axis, parallel
with the first, but offset. This is particularly useful here, because often it is easieast
to calculate or measure the moment of inertia about the centre of mass, but for our
calculations we are going to need to know the moment of inertia about each link’s
reference frame. For an x-axis offset of (∆y, ∆z) from the centre of mass in the
yz-plane, the moment of inertia is given by
(CoM )
Ixx = Ixx
+ m(∆y 2 + ∆z 2 )
2024/2025
(7.9)
ROBOTICS ENGINEERING
84
CHAPTER 7 Pseudo-inertia Matrices
p
(CoM )
where Ixx
is the moment of inertia about the centre of mass and d = ∆y 2 + ∆z 2
is the magnitude of the offset.
Similarly, the product of inertia about an axis offset from the centre of mass
can be given by
(CoM )
Ixy = Ixy
+ m∆x∆y
(7.10)
(CoM )
where Ixy
is the product of inertia about the centre of mass and ∆x and ∆y
are the offsets in the x and y directions respectively.
7.2.4
Inertia tensor
The inertia tensor is one of the most common methods of representing the moments and products of inertia. As far as we are concenred here, a second-order
tensor, such as the inertia tensor, is just a fancy way of saying matrix, and the
inertia tensor is sometimes referred to as the inertia matrix. The inertia tensor is


Ixx Ixy Ixz
I =  Ixy Iyy Iyz 
(7.11)
Ixz Iyz Izz
This is commonly used to compute the kinetic energy of a rotating rigid bidy from
the angular velocity vector, ω:
K = ω T Iω
(7.12)
While this is a neat result, we will not be using it for two reasons:
1. We do not have the angular velocity vectors explicitly available to us.
2. We need the kinetic energy due not only to the angular velocity, but also to
the linear translation. There is a neater solution to this using computations
based on the DH transforms and their derivatives.
To reiterate, we are not going to be using the inertia tensor in this work - it is
provided here for contrast and comparison, and to highlight that you should not
confuse it with the pseudo-inertia matrix that will be used instead.
7.3
Kinetic Energy
As will be seen in depth in later chapters, derivations of the equations of motion
fundamentally boil down to the consideration of energy. The inertia comes into
these equations via the kinetic energy. Expressions for the kinetic energy of the
ROBOTICS ENGINEERING
2024/2025
Section 7.3 Kinetic Energy
85
links are derived below to show the form that the inertia terms take. It will be
seen later that the inertia terms appear in the same form regardless of the method
by which the equations of motion are computed (i.e. whether they are assembled
using energy methods, Newtonion methods, etc.).
To determine an expression for the kinetic energy of a single link, consider first
the energy of an infinitesimally small point on the link. Its position and velocity
relative to the base frame are given by


x


0




y0
p=
z0 





1


ẋ


0




ẏ0
ṗ =
ż0 





0
(7.13)
where x0 , y0 , and z0 are the Cartesian components of that position, dots indicate
a time derivative, and the 0 subscripts indicate that they are relative to the 0th
reference frame: the base frame. Note that p is a homogeneous coordinate vector
so it has four elements and the last element is always 1. The final element of the
velocity ṗ is zero because the derivative of a constant is zero. The kinetic energy
of the point is now written in terms of an infinitesimal mass, δm, associated with
that point:
1
1 2
ẋ0 + ẏ02 + ż02 δm
δK = |ṗ|2 δm =
2
2
(7.14)
The square of the absolute speed, (ẋ20 + ẏ02 + ż02 ), can be found as the sum of the
diagonal elements of


 2
ẋ
ẋ0 ẋ0 ẏ0 ẋ0 ż0


0





ẏ
ẏ
ẏ02 ẏ0 ż0
0 ẋ0
0
ẋ0 ẏ0 ż0 0 = 
ṗṗT =
 ż0 ẋ0 ż0 ẏ0 ż02
ż0 





0
0
0
0

0
0 

0 
0
(7.15)
The sum of the diagonal elements of a matrix is called its trace, so the kinetic
energy is written
1
δK = trace ṗṗT δm
2
(7.16)
This expression is integrated to get the kinetic energy for the whole ith link:
Z
1
Ki = dK = trace
2
i
2024/2025
Z
ṗṗ dm
T
(7.17)
i
ROBOTICS ENGINEERING
86
CHAPTER 7 Pseudo-inertia Matrices
All that remains is to find an expression for ṗṗT . This is done by considering that


xi 





yi
0
0
p = Ti ri = Ti

 zi 



1
(7.18)
where ri is the position of the point on link i relative to its own coordinate system,
and xi , yi , and zi are the Cartesian components of that position in the ith coordinate
frame. The velocity can be written
N
N
X
X
∂Ti0 dqj
∂Ti0
d
0
Ti r i =
ri =
q̇j ri
ṗ =
dt
∂qj dt
∂qj
j=1
j=1
The integral in equation 7.17 becomes

!T 
Z
Z X
N
N
0
0
X
∂Ti
∂Ti
ṗṗT dm = 
q̇j ri
q̇k ri  dm
∂q
k
i
i j=1 ∂qj
k=1
0 T #
Z "X
N
N
X
∂Ti
∂Ti0
q̇j ri
ri T
q̇k dm
=
∂q
k
i j=1 ∂qj
k=1
0 T
Z
N
N
X X ∂T 0 ∂Ti
i
T
ri ri dm
q̇j q̇k
=
∂qj i
∂qk
j=1 k=1
(7.19)
(7.20)
(7.21)
(7.22)
The integral in the centre is constant because ri is defined relative to the link
itself. This is called the pseudo-inertia matrix, Ji . Using this definition along with
the definition of the transform derivative in Chapter 6, the kinetic energy can be
simply expressed as
N
Ki =
N
1 XX
trace Uij Ji Uik T q̇j q̇k
2 j=1 k=1
(7.23)
Or if you prefer matrix-vector notation,
1
Ki = q̇T Mi q̇
2
(7.24)
where
Mi(j,k) = trace Uij Ji Uik T
ROBOTICS ENGINEERING
(7.25)
2024/2025
Section 7.4 Pseudo-inertia Matrix
7.4
87
Pseudo-inertia Matrix
The pseudo-inertia matrix is a constant matrix for each link, defined in the previous section as
Z
Ji = ri ri T dm
(7.26)
i
Expanding this out gives


x


i

Z 


yi
xi yi zi 1 dm
Ji =
zi 
i




1
R
R
R

 R 2
R i xi dm
Ri xi y2 i dm Ri xi zi dm Ri xi dm

 xi yi dm
Ri
R i yi dm
Ri yi z2 i dm Ri yi dm 
=
 xi zi dm

Ri
Ri yi zi dm Ri zi dm
Ri zi dm
x dm
y dm
z dm
dm
i i
i i
i i
i
(7.27)
The following identity is used to transform the first diagonal term:
2x2 = x2 + x2 + y 2 − y 2 + z 2 − z 2
= −(y 2 + z 2 ) + (x2 + z 2 ) + (x2 + y 2 )
⇒ x2 =
(7.28)
(7.29)
1
−(y 2 + z 2 ) + (x2 + z 2 ) + (x2 + y 2 )
2
(7.30)
and
Z
1
x2i dm =
i
Z
−(y 2 + z 2 ) + (x2 + z 2 ) + (x2 + y 2 ) dm
2 i
1 (i)
(i)
(i)
+ Izz
−Ixx + Iyy
=
2
(7.31)
(7.32)
(i)
where Ixx is the mass moment of inertia of link i about the x-axis of the ith refer(i)
(i)
ence frame and so on for Iyy and Izz . Similarly,
Z
1 (i)
(i)
(i)
yi2 dm =
(7.33)
Ixx − Iyy
+ Izz
2
i
Z
1 (i)
(i)
(i)
zi2 dm =
Ixx + Iyy
− Izz
(7.34)
2
i
2024/2025
ROBOTICS ENGINEERING
88
CHAPTER 7 Pseudo-inertia Matrices
The remaining integrals in the pseudo-inertia matrix can be resolved directly
to show that


(i)
(i)
(i)
(i)
(i)
1
(−I
+
I
+
I
)
I
I
m
x̄
xx
yy
zz
xy
xz
i
i
 2

(i)
(i)
(i)
(i)
1 (i)

(I − Iyy + Izz )
Iyz
mi ȳi 
Ixy
2 xx
Ji = 

(i)
(i)
(i)
(i)
1 (i)


Ixz
Iyz
(I
+
I
−
I
)
m
z̄
xx
yy
zz
i
i
2
mi x̄i
mi ȳi
mi z̄i
mi
(7.35)
(i)
(i)
(i)
where Ixx , Iyy and Izz are the moments of inertia for the ith link about the ith ref(i)
(i)
(i)
erence frame; Ixy , Ixz and Iyz are the products of inertia for the ith link about the
ith reference frame; mi is the mass of the ith link; and x̄i , ȳi and z̄i are the coordinates of the centre of mass of the ith link with respect to the ith reference frame.
Note that the pseudo-inertia matrices only need to be computed once for any robot:
one pseudo-inertia matrix per link. Also note that the moments and products of
inertia in the pseudo-inertia matrix are about the link’s reference frame, not the
link’s centre of mass. It is necessary to use the parallel axis theorem in most cases.
ROBOTICS ENGINEERING
2024/2025
Chapter 8
Control Architecture
8.1
Introduction & learning outcomes
Robot controllers are set up to allow operators to program them using simple combinations of segments, each with its own start and end points, speed, and trajectory
type. The kinematics and dynamics of the robot are transparent to the operator,
who only needs to understand where and when s/he wants the machine to move.
In this section the architecture of a typical controller is explained, with some detail
on the two principle control schemes: feedforward and feedback control. This will
lay the foundations for understanding how the dynamics calculations in the chapters that follow allow for high fidelity control of the robot. The chapter finishes
by presenting a framework for computing the equations of motion for the feedforward control, the derivation of which will be elaborated upon in later chapters.
The learning outcomes are:
1. To be able to sketch a typical robot control architecture and explain the
function of each of the components.
2. To be able to explain how feedback and feedforward control complement
each other to provide high fidelity control.
3. To know in outline how to use a computational framework to calculate
the equations of motion for a serial manipulator based on the DenavitHartenberg kinematic relationships.
4. To understand where the different types of forces appear in the computations, what is needed to compute them, and how this should affect the behaviour of the controller.
89
90
CHAPTER 8 Control Architecture
8.2
Operator interface
Most modern commercial robot controllers come with a proprietary programming
language which can be used to construct complex motions from a series of simple trajectories pieced together. Figure 8.1 shows a typical “teach pendant”, the
device used by many operators to both control and program the robot. (Although
advanced users with the most complex tasks to accomplish will usually connect
to the robot controller with a computer and program it using a full keyboard and
screen.) An example section of robot instructions can be seen in figure 8.2. In
Figure 8.1: Example of a teach pendant for a robot controller
this case it is comprised mostly of “MoveL” commands, which tell the robot to
follow a linear trajectory in the workspace (as opposed to a linear trajectory in
joint space). There are two “WaitDI” commands, which tell the controller to wait
for an external digital input signal, for example on a production line this might be
a signal to tell the robot that a welding process has finished and a component can
be moved from the welder to the conveyor belt. The specific syntax of the commands used here are not part of this course, but you should be aware that most of
the motion commands are comprised of a series of trajectories specified by:
• The type of trajectory: often arcs or straight lines in the Cartesian workspace,
or linear trajectories in the joint space (usually the quickest and easiest way
to get from pose A to pose B if there are no other constraints).
• The start and end points (either the pose in Cartesian space, or the joint
positions).
ROBOTICS ENGINEERING
2024/2025
Section 8.3 Controller Architecture
91
• The target linear and angular speed in the Cartesian space.
• The target linear and angular acceleration in the Cartesian space.
• The maximum speeds/accelerations of each of the axes (joints); these may
limit the speeds/accelerations in Cartesian space.
• The corner behaviour - this tells the robot how to transition from one trajectory segment to the next.
Figure 8.2: Example of programming a motion trajectory on a robot teach pendant
8.3
Controller Architecture
A robot controller is normally comprised of:
• An axis computer, which runs a real-time control loop at a high rate (∼2 kHz)
for the joint motors.
• A main computer, which runs the program supplied by the operator and
outputs the control inputs to the axis computer.
• A teach pendant or external computer for the user interface.
2024/2025
ROBOTICS ENGINEERING
92
CHAPTER 8 Control Architecture
• A variety of supporting hardware, such as analogue, digital, and fieldbus
measurement and communications cards, safety relays (connected to interlocks, end stops, light gates, etc.), backup power systems, and bespoke
equipment tailored to specific operations.
The teach pendant is generally a computer in its own right, running the user interface software that speaks to the main computer on the robot controller, as well
as emergency stop buttons which link in with the safety relay chain. The teach
pendant or an external computer are used to modify the motion programs which
reside on the main computer. The main computer executes these programs on the
fly and interprets the instructions to produce a motion trajectory comprised of the
joint positions, velocities, and expected motor torques at each time step. These
will typically be output at a rate of ∼250 Hz. The main computer will usually run
a real-time operating system but the program execution times themselves are nondeterministic, so the trajectory planning happens “off-line”, i.e. a section of the
trajectory is planned in advance of being sent to the axis computer, and buffered
ready to send. If the buffer is depleted, the robot will stop moving and wait for the
buffer to replenish.
In computing the motion trajectory for a given instruction, the main computer
goes through a number of steps:
1. Interpret the motion instruction and perform trajectory planning.
2. If the instruction was in the Cartesian space, perform inverse kinematics at
each time step.
3. Apply safety limits in the joint space and the workspace: speed, acceleration, position.
4. Compute the expected motor torques.
In practise there are some algorithms used to speed things up and get around the
computational burden imposed by following this procedure verbatim, but they are
beyond the scope of this course.
An diagram of a typical control system is shown in figure 8.3. The motion
instruction is the input on the left. The axis computer and the robot itself are contained in the “position controller” box on the right. Everything else is performed
in the main computer. The control techniques are split into two important parts:
feedback control and feedforward control. These are outlined below.
ROBOTICS ENGINEERING
2024/2025
Section 8.4 Feedback Control
93
Figure 8.3: Typical robot controller architecture
8.4
Feedback Control
Feedback control is the most common type of control in industrial control. The
feedback control in a robot controller takes place in the axis computer. Figure 8.4
shows an expanded diagram for the “position controller” box in figure 8.3. The
feedback control is everything in line with the pos_ref input and below. There are
in fact two feedback controllers cascaded here: one for position, which produces
a velocity demand signal, then one for velocity.
The joint positions are measured, usually with encoders on revolute joints,
and these are fed back and compared to the demand (reference) signals. The
resulting error signal is passed through a controller. In this example, the postion
feedback loop uses a simple proportional controller. The velocity is computed
by differentiating the postion measurement and conditioning it (this is important
- differentiating signals for feedback control can produce erratic results). The
velocity feedback loop then uses a proportional-integral (PI) controller to produce
a torque demand signal. What is not shown here is the inverter (motor drive)
which supplies the power to the motors - this itself has a control loop which can
regulate the voltage and current to the motor to meet the torque demand.
Figure 8.4: Axis controller with velocity and torque feedforward signals
2024/2025
ROBOTICS ENGINEERING
94
CHAPTER 8 Control Architecture
Feedback control with PID controllers is a mature technology. It is well understood and therefore robust and reliable. But it has some drawbacks:
• It relies on an error signal to drive it, so can never perfectly reproduce the
demand.
• It introduces dynamics of its own, which may include lag or oscillations.
• It is tuned for a particular operating condition and is less able to cope with
a wide range of operating speeds, accelerations, etc.
• In a highly nonlinear system like a robot it is hard to find optimal tuning for
all joint configurations.
• At the higher end of the performance range it has small stability margins.
There are workarounds to some of these problems, such as gain scheduling where
the feedback control gains are tuned on the fly to respond to changing conditions,
but the most common practice is to use feedforward control.
8.5
Feedforward Control
Feedforward control is used to take into account of information known in advance
about what the robot needs to do. Two feedforward signals can be used: velocity
feedforward and torque feedforward.
Velocity feedforward is the easiest to use: the velocity demands are already
known from the trajectory planning. The velocity feedforward demands are therefore added to the velocity demand coming out of the position feedback loop. If
the robot responds perfectly to the velocity feedforward signal, then there will
never be an error signal for the position feedback control to act on, so the disadvantages listed above are circumvented. of course, the robot will never respond
perfectly to the velocity demand, so the position feedback is still required to correct the errors. But now the position feedback controller is acting on much smaller
errors. If a large velocity is demanded, the robot doesn’t need to wait for a big
error to develop before it starts to move, but can instead try to match that velocity
immediately.
The torque feedforward works on a similar premise, and circumvents the need
for an error in the velocity to develop before the motors start producing a force to
correct it. The torque feedforward demand is added to the torque demand coming
from the velocity feedback loop. Again, if the robot responds as expected to
the torque demand then there is no velocity error and the velocity feedback loop
has nothing to do. But now the torque demand is based off modelling of the
ROBOTICS ENGINEERING
2024/2025
Section 8.6 Inverse Dynamics for a Serial Robot
95
robot dynamics to determine what torques are needed to produce a given motion
trajectory. The modelling will suffer from inaccuracies in the mass properties
and geometry of the robot, as well as unmodellable elements such as damping or
externally applied forces. But now the feedback controllers have even less work
to do, and the fidelity of the robot trajectory following is significantly improved.
The torque feedforward signals are computed using inverse dynamics.
8.6
Inverse Dynamics for a Serial Robot
To compute the inverse dynamics for a serial robot, described in terms of N generalised coordinates, all that is needed is to compute the N generalised inputs, Qn ,
n = 1..N . The generalised input Qn is given by the sum of the gravity forces and
the inertial forces (further subdivided into the gyroscopic forces and the acceleration forces). A full derivation will be undertaken in due course, but for now the
nth equation can be stated as:
Qn =
N
X
Mnj q̈j +
j=1
N X
N
X
Cnjk q̇j q̇k + Gn
(8.1)
j=1 k=1
where
Mnj =
N
X
trace Uij Ji Uin T
(8.2)
i=1
Cnjk =
N
X
trace Uijk Ji Uin T
(8.3)
i=1
Gn =
N
X
−mi gT Uin ri
(8.4)
i=1
where ri is the position of the centre of mass for the ith link in the ith coordinate
frame, and is a constant for all joint positions.
The form of eqn. (8.1) should be familiar from Chapter 5, and the full set of
N equations of motion that result from it can be assembled into matrix form:
M(q)q̈ + C(q, q̇) + G(q) = Q
2024/2025
(8.5)
ROBOTICS ENGINEERING
96
CHAPTER 8 Control Architecture
where

M11
 ..
M(q) =  .

· · · M1N

...

MN 1




C
1


.
..
C(q, q̇) =

 C 

N



 G1 

.
.
G(q) =
.

 G 

N
(8.6)
MN N

Cn11
T 
Cn = q̇  ...
CnN 1

· · · Cn1N

..
 q̇
.
CnN N
(8.7)
(8.8)
As before, the Mnj terms correspond with linear inertia terms, or forces due
to translational or rotational accelerations. Without these terms in a torque controller the robot controller will be unable to account for rapid acceleration in the
trajectory, so only slow, gentle motions can be effectively controlled. These terms
depend nonlinearly upon the joint configuration (generalised coordinates) of the
robot.
The Cnjk terms correspond with inertial effects due to angular velocity. These
are sometimes called gyroscopic forces, and include Coriolis forces, precession
forces, and centrifugal/centripetal forces. The terms depend nonlinearly on both
the joint positions and the joint velocities. The terms where j = k are centrifugal/centripetal terms, while those with j 6= k are Coriolis and/or precession terms.
If the axes corresponding to joints j and k are parallel then the forces are Coriolis
forces; if they are perpendicular the forces are precession forces. It is very difficult to contrive to produce a trajectory in a robot with revolute joints that does not
result in gyroscopic forces. Special cases exist where the C terms can be ignored,
but for most paths, unless the motion is very slow then there will be significant
contributions from the C vector.
The Gn terms correspond with gravitational forces. Without accounting for
these forces the robot will respond badly to changes in configuration that change
the weight carried by joint motors, for example moving a link from a horizontal position where the motor carries the resulting torque, to a vertical position
where the mass is directly above the joint and no torque results. Robots are generally heavy to ensure stiffness, so the gravity forces can often dominate the other
forces. A feedback controller may respond badly to changes in gravity forces on
ROBOTICS ENGINEERING
2024/2025
Section 8.6 Inverse Dynamics for a Serial Robot
97
the joints so a gravity compensation controller is advisable even for slow trajectories. One situation where this becomes even more important is where the robot
is first started, and the feedback controller has not had a chance to detect any errors. Sometimes robots have physical systems designed to counter graviy to some
extent, to reduce the workload of the motors - these can include counterbalances,
springs, and gas struts.
Finally, the Qn terms are the generalised inputs. These contain the forces
arising from external forces, motor inputs, non-conservative damping and timedependent forcing.
2024/2025
ROBOTICS ENGINEERING
This page is intentionally left (almost) blank
Chapter 9
Principle of Virtual Work
9.1
Introduction & Learning Outcomes
The equations of motion upon which the analysis of robotic systems is based are
fundamentally force balance equations. The forces described act along the generalised coordinates, and the choice of generalised coordinates affects the complexity of the analysis for a given problem. Generally it is desireable to choose
the joint space as the generalised coordinate system, or to align the generalised
coordinates with the forces acting on the robot. Often these can be one and the
same, and that makes the analysis straightforward.
If a generalised coordinate measures a position or angle of a link relative to the
base frame then the corresponding generalised input represents a force or torque
acting only on that link, for example a hydraulic piston controlling the position
of the base of a robot along a linear track. Where the generalised coordinate describes a relative position or distance between two links, the corresponding generalised input represents a force or torque acting in equal and opposite measure on
both links, for example an electric motor controlling the angle of a revolute joint
between two successive links. If the generalised coordinate chosen in the last example were the absolute angle of the second link instead of the angle between the
two, then the corresponding force would be more akin to a belt drive controlling
the angle of the second link from a motor mounted on the floor.
Often it is not possible to choose a coordiante system which is convenient
for both the kinematic considerations and the control inputs. It is common, for
example, for 6DOF serial robots to use a linkage system to control the motion
of the third link, but the simplest choice of generalised coordinate is usually the
angle of the joint. In addition, there may be external forces acting on the system
as a result of it doing useful work, and these rarely act in a manner that is easy to
express in the genralised coordinate system. For these cases a method is needed
to convert from forces acting along arbitrary coordinates to an equivalent set of
99
100
CHAPTER 9 Principle of Virtual Work
forces acting along the generalised coordinates. Where these forces arise from
motor controls or other external inputs, they are referred to as generalised inputs.
This chapter outlines a method that can be used to convert forces and torques
between coordinate systems. It is based on the principle of virtual work. The
intended learning outcomes are:
1. To be able to include external forces (inlcuding actuator/motor forces) in
the equations of motion.
2. To understand how generalised inputs correspond to real-world forces.
3. To know how to use the principle of virtual work to convert forces between
arbitrary coordinate systems.
4. To be able to apply the principle of virtual work to the problem of gravity
compensation for a robotic system.
9.2
Principle of Virtual Work
There may be forces acting on the system which are not along the direction of
generalised coordinates. A useful method of expressing these forces along the
generalised coordinates is to utilise the principle of virtual work.
The work done by forces Fvi , i = 1 · · · n, acting in infinitesimal displacements vi , i = 1 · · · n, is:
δW =
n
X
Fvi δvi
(9.1)
i=1
If the system has M degrees of freedom and is represented by N = M generalised
coordinates qi , i = 1 · · · N , the coordinates (vi ’s) can be eliminated by using the
constraint or transformation equations as follows:
vi = vi (q1 , q2 , · · · , qN ),
i = 1, · · · , n
(9.2)
This gives
δvi =
N
X
∂vi
j=1
∂qj
δqj , i = 1 · · · n
(9.3)
Substituting (9.3) into (9.1) gives the total virtual work in the following general
form:
δW = [· · · ] δq1 + [· · · ] δq2 + · · · + [· · · ] δqN
|{z}
|{z}
|{z}
Q1
Q2
ROBOTICS ENGINEERING
(9.4)
QN
2024/2025
Section 9.2 Principle of Virtual Work
101
where the brackets represents the generalised inputs for the Lagrange’s equation
of motion.
Example 9.1. A force Fy acts vertically on the centre of mass of the single link
manipulator in Figure 9.1. Determine the generalised input resulting from this
force and the torque input τ1 , if the generalised coordinate is θ1 .
y
a1
m1, I1
τ1
θ1
x
Figure 9.1: Single-link manipulator
Virtual work from both forces:
δW = τ1 δθ1 + Fy δy
We need to write this in terms of the virtual displacement δθ1 .
y = a1 sinθ1
so the partial derivative is
∂y
= a1 cosθ1
∂θ1
Hence the virtual work is
δW = τ1 δθ1 + Fy
∂y
δθ1 = (τ1 + Fy a1 cosθ1 ) δθ1
∂θ1
Therefore,
Q1 = τ1 + Fy a1 cosθ1
Example 9.2. Consider the two-link manipulator in Fig. 9.2, where the absolute
angles θ1 and α2 are used as generalised coordinates when developing the equations of motion. However, the system input are the torques generated by the two
2024/2025
ROBOTICS ENGINEERING
102
CHAPTER 9 Principle of Virtual Work
y
θm 2
τ m2
τm1
θ1 � θ m1
x
Figure 9.2: Two-link manipulator
motors driving each joint. Calculate the generalised inputs in terms of the motor
torques Tm1 and Tm2 .
Motor torque inputs Tm1 and Tm2 act on the relative angles θm1 = θ1 and
θm2 = α2 − θ1 , respectively. The total virtual work
δW = Tm1 δθm1 + Tm2 δθm2
Replacing motor coordinates with generalised coordinates (i.e. absolute angles)
gives
δW = Tm1 δθ1 + Tm2 (δα2 − δθ1 )
or
δW = [Tm1 − Tm2 ]δθ1 + [Tm2 ]δα2
Hence the generalised inputs:
Q1 = Tm1 − Tm2 ,
and
Q2 = Tm2
or
Q=
9.3
1 −1
0 1
Tm
(9.5)
Gravity Compensation
An example of virtual work in action is for gravity compensation, to help a robot
support its own weight. Gravitational forces always act down (to a good approximation for our purposes), but the generalised coordinates will not generally align
ROBOTICS ENGINEERING
2024/2025
Section 9.3 Gravity Compensation
103
with this. Virtual work can be used to determine the gravity forces acting on the
system in the generalised coordinate system, which can then be used to compensate for it with motor inputs. For static equilibrium, the gravity forces need to be
balanced by the motor torques, which form part of the generalised input Q. This
static equilibrium is written
G(q) = Q
(9.6)
where −G(q) is the vector of gravity forces resolved into generalised coordinates,
and is a function of the robot configuration q. To compute the elements of G we
can use the principle of virtual work again. The virtual work done by gravity is
given by
δW =
P
X
−mi gδzi
(9.7)
i=1
where g ≈ 9.81 is the gravitational constant, P is the number of links, and mi and
zi are the mass and vertical position of link i. The quantity is negative because the
gravitational force is in the negative z direction. As before, the positions zi can be
expressed as a function of the generalised coordinates, zi = zi (q1 , q2 , . . . , qN ), so
using partial derivatives the virtual displacements become:
δzi =
N
X
∂zi
∂qj
j=1
δqj
(9.8)
Substitution of (9.8) into (9.7) gives the virtual work due to gravity forces:
δW = [· · · ] δq1 + [· · · ] δq2 + · · · + [· · · ] δqN
|{z}
|{z}
|{z}
−G1
−G2
(9.9)
−GN
where the bracketed terms represent the gravity-compensation forces in the direction of the respective generalised coordinates. Note that the Gn terms are negative
here; this is because the G term in the equations of motion describes a negative
force (it is a restorative force, which acts in a negative direction). Insertion of
the resulting Gn values into eqn. (9.6) allows the determination of the necessary
generalised inputs, Q to keep the robot static in a gravitational field, which in turn
specifies the motor torques/forces needed.
2024/2025
ROBOTICS ENGINEERING
This page is intentionally left (almost) blank
Chapter 10
Inverse Dynamics
10.1
Introduction & learning outcomes
This chapter covers inverse dynamics for the special case of a serial robotic manipulator. It uses homogeneous transforms which can be derived from the DenavitHartenberg methods. The techniques will be useful for the computational exercises in the lab. In later chapters both forward and inverse dynamics are covered
for more general linkage systems. This chapter derives first an expression for the
gravitational forces acting on a robot system, and then the inertial forces. The
intended learning outcomes are:
1. To be able to derive a computational method for determining the gravitational forces acting on a serial robot.
2. To understand the derivation of computational methods for determining the
inertial forces acting on a serial robotic manipulator.
3. To be able to construct equations of motion for serial robotic manipulators
based on homogeneous transform matrices.
4. To be able to use inverse dynamics to compute the motor forces needed to
execute a given motion trajectory on a serial robotic manipulator.
10.2
Gravitational forces
This section picks up where the last chapter left off. The methods used there
are straightforward for simple systems with a small number of links, but rapidly
become unmanageable for hand calculations with more complicated robots. A
systematic approach can be taken using the homogeneous transforms explored in
105
106
CHAPTER 10 Inverse Dynamics
Chapter 2 and the derivatives from Chapter 6. The position of the centre of mass
of link i is given by


xi 





yi
0
(10.1)
pi = Ti ri =
zi 





1
where ri is the position of the centre of mass of link i with respect to its own
coordinate frame (i.e. the coordinate frame attached to that link). Because the
coordinate frame moves with the link, ri is constant for a given robot, regardless
of the joint motions. Equation (9.7) can then be written
δW =
N
X
mi gT δpi
(10.2)
i=1
where g is the gravity vector


0






0
g=
−g 





0
This formulation is particularly convenient because it is valid for any orientation
of the base frame; all that is needed to compensate for a rotation of the base frame
is to change the orientation of the gravity vector accordingly. Now following the
procedure in section 9.3, applying partial derivatives to eqn. (10.1) gives
δpi =
N
X
∂T 0
i
∂qn
n=1
ri δqn
(10.3)
(where again the local centre of mass position ri is constant with respect to the
joint motion so is invariant with qj ) Substitution of (10.3) in (10.2) produces
δW =
N X
N
X
i=1 n=1
−mi gT
∂Ti0
ri δqn
∂qn
(10.4)
The gravity force vector is found by extracting the coefficients of the virtual displacements for each generalised coordinate:



G1 



N
 G2 

0
X
T ∂Ti
G=
Gn =
−mi g
ri
(10.5)
..

∂qn
. 


i=1


 G 
N
ROBOTICS ENGINEERING
2024/2025
Section 10.3 Inertial Forces
107
Again, note that G is the negative force: a restorative force. The formulation
arrived at here is particularly suitable to implementation in a computational algorithm. You should recognise the transform derivative from Chapter 6, and using
eqn. (6.6) the gravity forces can be written
Gn =
N
X
−mi gT Uin ri
(10.6)
i=1
10.3
Inertial Forces
The remaining forces on the left hand side of our equation of motion are all due
to inertial forces. Consider an infinitesimal mass, δm within link i. The force and
acceleration of the mass are related by Newton’s second law:
δF = δmp̈i
(10.7)
where δF and p̈i are both vectors:




δF
ẍ




x
i








δFy
ÿi
δF =
p̈i =
δFz 
z¨i 










0
0
(10.8)
The virtual work on this infinitesimal mass is
δ 2 Wi = δF · δpi = δFx δxi + δFy δyi + δFz δzi
(10.9)
where δ 2 W indicates that it is a virtual work on an infinitesimal mass. (The virtual
work for the whole of link i is δWi .) Another way of writing this equation, for
reasons that may be clear from Chapter 7 and will become clear here, is to use the
trace of a matrix. The trace of a matrix is simply the sum of its diagonal elements.
The virtual work can therefore be written


δFx δxi δFx δyi δFx δzi 0
 δFy δxi δFy δyi δFy δzi 0 


δ 2 Wi = trace(δFδpi T ) = trace 
 δFz δxi δFz δyi δFz δzi 0  (10.10)
0
0
0
0
Substituting (10.7) into (10.10) gives
δ 2 Wi = trace(p̈i δpi T )δm
2024/2025
(10.11)
ROBOTICS ENGINEERING
108
CHAPTER 10 Inverse Dynamics
This can be integrated to determine the virtual work for the whole link:
Z
δWi = trace(p̈i δpi T ) dm
(10.12)
The expressions for position and acceleration are substituted in from (6.1) and
(6.8) to give
" N N
!
#
Z
N
XX
X
T
δWi = trace
Uijk q̇j q̇k +
dm (10.13)
Uij q̈j ri ri T δTi0
j=1 k=1
j=1
As the mass is integrated around the link, all of the terms in this equation except
ri remain constant. The integral can therefore be moved inside the parentheses
and summations, so
#
!
" N N
N
X
XX
T
Uijk q̇j q̇k +
Uij q̈j Ji δTi0
(10.14)
δWi = trace
j=1 k=1
j=1
where
Z
ri ri T dm
Ji =
(10.15)
The 4 × 4 matrix Ji is the pseudo-interia matrix for link i and it is a constant
property of the link. The pseudo-inertia matrix can only be separated from the
other terms because of the formulation of these equations in terms of the trace;
this is the reason for constructing the equation this way. Expanding δTi0 in terms
of partial derivatives gives
T
δTi0 =
N
X
Uin T δqn
(10.16)
n=1
Inserting this into (10.14) produces,
" N N
#
!
N
N
X
X
XX
δWi =
trace
Uijk q̇j q̇k +
Uij q̈j Ji Uin T δqn
n=1
j=1 k=1
(10.17)
j=1
The virtual work expressed in terms of the generalised coordinates and inputs is
δWi =
N
X
Q(i)
n δqn
(10.18)
n=1
ROBOTICS ENGINEERING
2024/2025
Section 10.4 Equations of Motion for Serial Manipulators
109
P
(i)
Comparing the above two equations, and summing for all the links, Qn = Pi=1 Qn ,
the generalised inputs due to the inertia of the complete robot can be found using
" N N
#
!
N
N
X
XX
X
Qn = trace
Uijk q̇j q̇k +
Uij q̈j Ji Uin T
(10.19)
i=1
j=1 k=1
j=1
or
N X
N X
N
X
Qn =
N X
N
X
trace Uijk Ji Uin T q̇j q̇k +
trace Uij Ji Uin T q̈j (10.20)
i=1 j=1 k=1
i=1 j=1
This is a daunting equation for manual calculations, but it is very easy to apply
using loops in a computer program.
10.4
Equations of Motion for Serial Manipulators
It is now possible to construct the full equations of motion from the results of
sections 10.2 and 10.3. The result is the equations first stated in Chapter 8 and
repeated here:
Qn =
N
X
Mnj q̈j +
j=1
N X
N
X
Cnjk q̇j q̇k + Gn
(10.21)
j=1 k=1
where
Mnj =
N
X
trace Uij Ji Uin T
(10.22)
i=1
Cnjk =
N
X
trace Uijk Ji Uin T
(10.23)
i=1
Gn =
N
X
−mi gT Uin ri
(10.24)
i=1
These equations can be implemented easily in a computer using functions and
loops, and allow the determination of the external forces required (the generalised
inputs) to produce a given motion trajectory. This is the inverse dynamics solution, and is the basis of the feedforward control used to create high fidelity robot
controllers.
If you are interested, when you have learned the Lagrangian mechanics methods from the next chapter, you can revisit the derivation of these equations using
a Lagrangian approach instead of the Newtonian approach here. The result is the
same. The full Lagrangian derivation is given in Appendix B.
2024/2025
ROBOTICS ENGINEERING
This page is intentionally left (almost) blank
Chapter 11
Lagrangian Mechanics
11.1
Introduction
So far the equations of motion have been derived by relying on Newton’s laws and
the principle of virtual work. In this chapter Lagrangian mechanics will be used
to derive the equations of motion for general linkage systems, and to use these
equations of motion in both forward and inverse dynamics problems.
Newton’s three laws and the concept of virtual work may be regarded as the
foundation of classical mechanics. However, the basic laws of dynamics can be
formulated in several ways other than that given by Newton such as D’Alembert’s
principle, Lagrange’s equations, Hamilton’s equations and Hamilton’s principle.
All are basically equivalent, but vary in terms of ease of developing the equations of motion. Although Newton’s equations are convenient for simple cases,
Lagrange’s equations offer significant advantages when dealing with multi-body
and/or multi-physics mechatronics systems and are widely used in robotics. As
already discussed, there are two general types of dynamical problems. Almost every problem in classical dynamics is a special case of one of the following general
types:
• Forward dynamics: Allows the "motion" of the system (i.e. the position,
velocity and acceleration of each mass as a function of time) to be found
from the given forces and torques acting on the system, constraints, and
known position and velocity of each mass at a given instant of time (e.g.
initial conditions).
• Inverse dynamics: Allows the calculation of a possible set of forces and
torques as a function of time to produce a specified motion.
Both forward and inverse dynamic analysis are covered in this Chapter.
111
112
CHAPTER 11 Lagrangian Mechanics
11.2
Intended Learning Outcomes
1. To be able to formulate the Lagrangian function for a robotic system.
2. To be able to use Lagrange’s equations of motion to describe the system
dynamics.
3. To understand the principles of state space and forward dynamics.
4. To be able to recognise singularities in equations of motion.
5. To be able to compute inverse dynamic solutions for simple robot linkages.
11.3
Lagrangian Mechanics
Lagrangian dynamics offer many advantages over Newton’s method of writing
equations of motion. These include:
• For a large class of mechanical systems, the Lagrange equations provide a
unique and sufficiently simple method of constructing equations of motion
that is independent of the form (complexity) of the actual system.
• Only work and energy are used, which are scalar quantities and have the
same unit for any branches of physics whether mechanical, electrical or
chemical.
• The chief advantage of the Lagrange equations is that their number is equal
to the number of degrees of freedom of the system and is independent of the
number of points and bodies in the system.
• Internal forces or any force not contributing any work are not needed in the
derivation. This is a great advantage over Newton’s reaction force balancing
method for any slightly complicated system.
• Lagrange’s equations take the same form for any coordinate system, so that
the method of solution proceeds in the same way for any problem.
• It is invariant under coordinate transformations.
• Only positions and velocities and not accelerations (unlike Newton’s method)
are needed in the derivation
ROBOTICS ENGINEERING
2024/2025
Section 11.3 Lagrangian Mechanics
113
Lagrange’s equation of motion can be stated as:
d ∂L
∂L
−
= Qi , i = 1 · · · N
dt ∂ q̇i
∂qi
(11.1)
where:
qi , i = 1 · · · N are the generalised coordinates
N is the number of generalised coordinates (normally equal to the number
of degrees of freedom of the system M )
Defining q and q̇ as the vectors of generalised coordinates and derivatives,
respectively
L(q, q̇, t) is the Lagrangian function (L = K − P)
K(q, q̇, t) is the kinetic energy function
P(q, t) is the potential energy function
Qi , i = 1 · · · N are the generalised inputs (forces/torques)
When Lagrange’s equations of motion are developed for any manipulator, the
resulting dynamic equations will be in the following general form:
M(q)q̈ + C(q, q̇) + G(q) = Q
(11.2)
where M(q) is the N ×N mass matrix, C(q, q̇) is an N ×1 vector of centrifugal
(q̇i2 terms) and Coriolis (q̇i q̇j , j 6= i, terms) forces/torques, G(q) is an N ×1 vector
of gravity forces/torques, and Q is the vector of generalised inputs.
These equations can be written in the state-space form and be integrated numerically by using a suitable integration algorithm or software packages such as
Matlab/Simulink. The state space form converts N second order differential equations into 2N first order differential equations.
d
ẋ =
dt
x1
x2
d
=
dt
q
q̇
=
x2
M−1 (Q(x1 , x2 ) − C(x1 , x2 ) − G(x1 ))
(11.3)
where x is the 2N × 1 state variable vector. If M is not constant, there may
exist a position, where the corresponding q values cause the determinant of M
to become zero. This is known as a singularity, and there is no solution for the
equations of motion at the singular position.
In summary, developing Lagrange’s equations of motion of a system involves
the following basic steps:
2024/2025
ROBOTICS ENGINEERING
114
CHAPTER 11 Lagrangian Mechanics
1. Establish the number of degrees of freedom of the system, M , and select any
independent N = M coordinates as generalised coordinates. The number
of degrees of freedom is defined as the number of independent coordinates
required to specify completely the position of each and every component
part of the system.
2. Write the Lagrangian function (i.e. total kinetic and potential energy functions) in terms of the generalised coordinates and their derivatives.
3. Express external forces and torques as generalised inputs. If external forces
and/or torques are not along the generalised coordinates, virtual work principle can be used.
4. Use (11.1) to generate a set of N second order differential equations of
motion from the Lagrangian function. This step consists of routine manipulations.
Example 11.1. Consider the single link manipulator in Fig. 11.1. The symbols
y
a1
m1, I1
τ1
θ1
x
Figure 11.1: Single-link manipulator
are defined in the figure, where I1 is the moment of inertia of the link about its
mass centre of gravity. The viscous friction coefficient at the joint is c1 = 0.5
Nms/rad. Develop the equation of motion relating the motor torque to motion.
This is a M = 1 DOF system because defining a single variable, say θ1 ,
completely defines the position of the manipulator. Selecting q1 = θ1 (N = 1)
as the generalised coordinate, the kinetic and potential energy functions can be
written as follows:
1
1
K = m1 (ẋ21 + ẏ12 ) + I1 θ̇12
2
2
ROBOTICS ENGINEERING
2024/2025
Section 11.3 Lagrangian Mechanics
115
and
P = m1 gy1
In order to express the Lagrangian function in terms of the generalised coordinate
θ1 , the dependant coordinates x1 and y1 must be substituted.
x1 = a1 cos θ1
and y1 = a1 sin θ1
The derivatives:
ẋ1 = −a1 sin θ1 θ˙1
and ẏ1 = a1 cos θ1 θ˙1
Lagrangian function:
1
L = (m1 a21 + I1 )θ̇12 − m1 ga1 sin θ1
2
Substituting this into (11.1) gives the following equation of motion
(m1 a21 + I1 )θ̈1 + m1 ga1 cos θ1 = τ1 − c1 θ̇1
where τ1 is the motor torque input. The MATLAB code for this example is available in Moodle. The results are shown in Fig. 11.2 for a free fall of the manipulator (τ1 = 0) from an initial position at θ1 = 0. The data used are: m1 = 1 kg,
a1 = 0.5 m, I1 = 0.01 kgm2 , and c1 = 0.5 Nms/rad.
Angle (deg)
0
−50
−100
−150
0
1
2
3
Time (s)
4
5
6
Figure 11.2: Result of the simulation output for Example 11.1
2024/2025
ROBOTICS ENGINEERING
116
CHAPTER 11 Lagrangian Mechanics
11.4
Generalised Coordinates
A great variety of coordinates can be employed as generalised coordinates. For
example in Example 11.1, the angle θ1 was selected as the generalised coordinate,
and hence the torque τ1 acting on θ1 became the generalised input. However, if,
say y1 , was used as a generalised coordinate, then the force acting in the direction of y1 , Fy , would become the generalised input. This is demonstrated in the
following example.
Example 11.2. For the single link manipulator shown in Fig. 11.1, develop the
equation of motion by using y1 as the generalised coordinate.
Using the same kinetic and potential expressions as in Example 11.1, and
substituting for x1 and θ1
q
−y1
ẏ1
x1 = a21 − y12 , and ẋ1 = p 2
a1 − y12
1
θ˙1 = p 2
ẏ1
a1 − y12
This gives the following Lagrangian function
1 m1 a21 + I1
L=
ẏ12 − m1 gy1
2
a21 − y12
By applying the Lagrange’s equations of motion in (11.1),
∂L
m1 a21 + I1
=
ẏ1
∂ ẏ1
a21 − y12
2(m1 a21 + I1 )y1 2
d ∂L
m1 a21 + I1
=
ÿ1 +
ẏ1
dt ∂ ẏ1
a21 − y12
(a21 − y12 )2
∂L
(m1 a21 + I1 )y1 2
=
ẏ − m1 g
∂y1
(a21 − y12 )2 1
gives the following equation of motion
m1 a21 + I1
(m1 a21 )y1 2
ÿ
+
ẏ + m1 g = Fy
1
a21 − y12
(a21 − y12 )2 1
Obviously, this formulation leads to a singularity at y12 = a21 , i.e. when θ1 =
±90o . Due to using +ve sign in the expression for x1 above, the simulation is valid
only for motions where x1 > 0.
ROBOTICS ENGINEERING
2024/2025
Section 11.5 Planar Two-link Manipulator
11.5
117
Planar Two-link Manipulator
Consider the two-link manipulator in Fig. 11.3. The inertia of the load mass, m3 ,
is ignored. The manipulator has 2-DOF, i.e. M = 2. Using the notations in the
figure, the equations of motion can be first developed by using two (N = M = 2),
and then four (N = 4 > M ) generalised coordinates.
m3
y
m2
θ2
a2
τ2
α2
a1
m1
τ1
x
θ1
Figure 11.3: Two-link manipulator
The Lagrangian function of the manipulator:
3
3
2
X
1X
1X
2
2
mi g yi
mi vi +
Ii θ̇i −
L=
2 i=1
2 i=1
i=1
(11.4)
where vi is the velocity of mi , and vi2 = ẋ2i + ẏi2 . Or equivalently, treating each
link independently,
L=
3
X
Li =
3 X
1
i=1
i=1
2
1
mi vi2 + Ii θ̇i2 − mi g yi
2
(11.5)
Equation of Motion
By using two generalised coordinates:
q = [θ1 , θ2 ]T
(11.6)
The Lagrangian function can be written for each body:
2024/2025
ROBOTICS ENGINEERING
118
CHAPTER 11 Lagrangian Mechanics
Link1:
As in Example 11.1 on Page 114.
1
L1 = (m1 a21 + I1 )θ̇12 − m1 ga1 sin θ1
2
(11.7)
Link2:
x2 = L1 cos θ1 + a2 cos(θ1 + θ2 )
y2 = L1 sin θ1 + a2 sin(θ1 + θ2 )
(11.8)
Derivatives:
ẋ2 = −L1 sin θ1 θ̇1 − a2 sin(θ1 + θ2 )(θ̇1 + θ̇2 )
ẏ2 = L1 cos θ1 θ̇1 + a2 cos(θ1 + θ2 )(θ̇1 + θ̇2 )
(11.9)
Velocity:
v22 = L21 + a22 + 2L1 a2 cos θ2 θ̇12 + a22 θ̇22 +2 a22 + L1 a2 cos θ2 θ̇1 θ̇2 (11.10)
Lagrangian function:
1
1
L2 = m2 v22 + I2 (θ̇1 + θ̇2 )2 − m2 g(L1 sin θ1 + a2 sin(θ1 + θ2 )
2
2
(11.11)
Link 3: This is not a full link but simply a point mass at the end of Link 2, so
the velocity expression is the same as Link 2 except replacing a2 with L2 :
v32 = L21 + L22 + 2L1 L2 cos θ2 θ̇12 + L22 θ̇22 +2 L22 + L1 L2 cos θ2 θ̇1 θ̇2 (11.12)
and it has no inertia so
1
L3 = m3 v32 − m3 g(L1 sin θ1 + L2 sin(θ1 + θ2 )
2
The Lagrangian function for all three rigid bodies can be written as:
1
1
L = L1 + L2 + L3 = A1,1 q̇12 + A2,2 q̇22 + A1,2 q̇1 q̇2 + A0
2
2
(11.13)
where
A1,1 = m1 a21 + m2 (L21 + a22 ) + m3 (L21 + L22 ) + I1 + I2
+2L1 (m2 a2 + m3 L2 ) cos θ2
A2,2 = m2 a22 + m3 L22 + I2
A1,2 = m2 (a22 + L1 a2 cos θ2 ) + m3 (L22 + L1 L2 cos θ2 ) + I2
A0 = −m1 ga1 sin θ1 − m2 g(L1 sin θ1 + a2 sin(θ1 + θ2 ))
−m3 g(L1 sin θ1 + L2 sin(θ1 + θ2 ))
ROBOTICS ENGINEERING
(11.14)
2024/2025
Section 11.6 Inverse Dynamics
119
This leads to the following equations of motion:
A1,1 A1,2
q̈1
b1 + τ 1
=
A1,2 A2,2
q̈2
b2 + τ 2
(11.15)
where τ1 and τ2 are the motor torques applied to θ1 and θ2 , and
b1 = 2L1 (m2 a2 + m3 L2 ) sin θ2 θ̇1 θ̇2 + L1 (m2 a2 + m3 L2 )sinθ2 θ̇22
−(m1 a1 + m2 L1 + m3 L1 )g cos θ1 − (m2 a2 + m3 L2 )g cos(θ1 + θ2 )
b2 = −L1 (m2 a2 + m3 L2 ) sin θ2 θ̇12
(11.16)
−(m2 a2 + m3 L2 )g cos(θ1 + θ2 )
Example 11.3. Simulate the free fall response of the two-link manipulator in
Fig. 11.3 by using the data in Table 11.1, where ci is the viscous friction coefficient of the i-th joint.
Table 11.1: Data for the two-link manipulator in Fig. 11.3
i
1
2
3
mi
kg
1.0
1.0 3.0
Li
m
1.0
1.0
2
Ii kgm 0.01 0.01 ai
m
0.5
0.5
ci Nms 0.5
0.5
The MATLAB code that implements the equations of motion given by (11.15)
to simulate the two-link manipulator is available in Moodle. The time response
results are shown in Fig. 11.4.
11.6
Inverse Dynamics
Inverse dynamics allow the calculation of required control inputs in order to achieve
a desired motion. The solution of inverse dynamics relies on the forward dynamic
equations written in the form of (11.2) on page 113. The motion definition means
that q(t), q̇(t), and q̈(t) are given. In this case there are no external forces and the
generalised input consists solely of the control inputs. Therefore the control input
can easily be calculated as:
Q = M(q)q̈ + C(q, q̇) + G(q)
(11.17)
It is important to note that no integration or solution of differential equations
are needed for this case.
2024/2025
ROBOTICS ENGINEERING
120
CHAPTER 11 Lagrangian Mechanics
50
Angle (deg)
0
−50
−100
θ1
−150
−200
θ2
0
2
4
6
8
10
Time (s)
Figure 11.4: Free fall trajectory of the two-link manipulator in Example 11.3
Example 11.4. Consider the two-link manipulator in Example 11.3 on Page 119,
where the desired periodic steady-state motion of period T = 2 second (ω =
2π/T ) is defined as:
π
sin(ωt)
θ1
= π π4
q=
+ 4 sin(ωt + π/6)
θ2
8
Calculate the required motor torque inputs to achieve this motion.
For inverse dynamic analysis, the first and second derivatives of the desired
motion are required.
π
θ˙1
ω cos(ωt)
4
q̇ =
= π
ω cos(ωt + π/6)
θ˙2
4
and
q̈ =
θ¨1
θ¨2
=
− π4 ω 2 sin(ωt)
π 2
− 4 ω sin(ωt + π/6)
The required motion is shown in Fig. 11.5(a)-(c).
After establishing the motion, the required motor torque inputs can be calculated by implementing (11.17) on the equations of motion already developed in
(11.15) with Q = [τ1 , τ2 ]T , giving the results as shown in Fig. 11.5(d). This example can be run for different speeds (i.e. different ω values) to observe the effect
of speed on the shape and amplitude of the required torque.
ROBOTICS ENGINEERING
2024/2025
Section 11.6 Inverse Dynamics
121
(a) Angle (deg)
(b) Velocity (deg/s)
100
200
θ1
100
θ2
50
0
0
−50
−100
0
1
2
−200
0
(c) Acceleration (deg/s2)
1
2
(d) Torque (Nm)
500
300
τ1
200
τ2
100
0
0
−100
−500
0
1
Time (s)
2
−200
0
1
Time (s)
2
Figure 11.5: Desired motion and the required torque in Example 11.4
2024/2025
ROBOTICS ENGINEERING
This page is intentionally left (almost) blank
Appendix A
Matrix Inversion
The inversion of a N xN square matrix A can be calculated as follows:
A−1 =
Adj(A)
Det(A)
where Det() is the determinant of the matrix.
The ith row and jth column of the Adjoint of A, Adj(A), is defined as:
Adj(A)ij = [(−1)i+j Det(Mij )]T
Mij is the (N − 1)x(N − 1) matrix obtained by eliminating the ith row and
jth column of A. The validity of the result can be assesed by checking if the
following property is satisfied.
A × A−1 = IN ×N
where IN ×N is the N × N unity matrix.
Example A.1. Calculate the inverse of the following 3x3 matrix.


1 2 1
A= 0 3 1 
0 1 2
First calculate the determinant. Using the first column
Det(A) = 1 × (3 × 2 − 1 × 1) = 5
Then calculate the Adj(A):

T
+(6 − 1)
0
0
Adj(A) =  −(4 − 1) +(2 − 0) −(1 − 0) 
+(2 − 3) −(1 − 0) +(3 − 0)

T 

5
0
0
5 −3 −1
=  −3 2 −1  =  0 2 −1 
−1 −1 3
0 −1 3
123
124
CHAPTER A Matrix Inversion
This gives:


5 −3 −1
Adj(A)
1
A−1 =
=  0 2 −1 
Det(A)
5
0 −1 3
Finally, validate the result.

 
 

1 2 1
5 −3 −1
1 0 0
1
A × A−1 =  0 3 1  ×  0 2 −1  =  0 1 0  = I3×3
5
0 1 2
0 −1 3
0 0 1
ROBOTICS ENGINEERING
2024/2025
Appendix B
Lagrangian Inverse Dynamics
Derivation
In this appendix the equations of motion for a serial robotic manipulator are derived from homogenous transforms using Lagrangian Mechanics. It arrives at the
same result as that obtained in Chapter 10 and is provided here for information
only. In this derivation it is assumed that the number of links, P , is equal to the
number of generalised coordinates, N , and that these are both equal to the degrees
of freedom in the system, M . This is a sensible assumption for a serial robotic
manipulator.
B.1
Potential Energy
The first step in the derivation is to find an expression for the potential energy.
The potential energy for each link is equal to the vertical position of its centre of
mass times its mass. The position of the centre of mass for link i is
p̄i = Ti0 r̄i
(B.1)
where Ti0 is the transform from the base frame to the ith link and r̄i is the position
of the centre of mass for the ith link relative to its own coordinate frame, located
at the end of the link. Note that r̄i is a constant for a given robot. To get the
potential energy for link i, its position vector is multiplied by its mass mi and the
gravity vector
g T = [0
0
− 9.81
0]
(B.2)
to give
Pi = −mi g T p̄i = −mi g T Ti0 r̄i
(B.3)
125
126
CHAPTER B Lagrangian Inverse Dynamics Derivation
The potential energy for the whole robot is then the sum of all the link potential
energies:
P =
N
X
i=1
B.2
Pi =
N
X
−mi g T Ti0 r̄i
(B.4)
i=1
Kinetic Energy
The second step in the derivation is to determine an expression for the kinetic
energy of each link. This is done by considering the energy in a single point on
the link. Its position anbd velocity relative to the base frame are given by




x
ẋ




0
0








y0
ẏ0
ṗ =
(B.5)
p=
z0 
ż0 










1
0
where x0 , y0 , and z0 are the Cartesian components of that position, dots indicate
a time derivative, and the 0 subscripts indicate that they are relative to the 0th
reference frame: the base frame. Note that p is a homogeneous coordinate vector
so it has four elements and the last element is always 1. The final element of the
velocity ṗ is zero because the derivative of a constant is zero. The kinetic energy
of the point is now written in terms of an infinitesimal mass, δm, associated with
that point:
1 2
1
δK = |ṗ|2 δm =
ẋ0 + ẏ02 + ż02 δm
(B.6)
2
2
The square of the absolute speed, (ẋ20 + ẏ02 + ż02 ), can be found as the sum of the
diagonal elements of



 2
ẋ
ẋ
ẋ
ẏ
ẋ
ż
0


0
0
0
0
0
0




 ẏ0 ẋ0 ẏ02 ẏ0 ż0 0 
ẏ0
T

ẋ0 ẏ0 ż0 0 = 
(B.7)
ṗṗ =
 ż0 ẋ0 ż0 ẏ0 ż02 0 
ż


0




0
0
0
0 0
The sum of the diagonal elements of a matrix is called its trace, so the kinetic
energy is written
1
δK = trace ṗṗT δm
(B.8)
2
This expression is integrated to get the kinetic energy for the whole ith link:
Z
Z
1
T
Ki = dK = trace
ṗṗ dm
(B.9)
2
i
i
ROBOTICS ENGINEERING
2024/2025
Section B.2 Kinetic Energy
127
All that remains is to find an expression for ṗṗT . This is done by considering that


x


i




y
i
0
0
p = Ti r i = Ti
zi 





1
(B.10)
where ri is the position of the point on link i relative to its own coordinate system,
and xi , yi , and zi are the Cartesian components of that position in the ith coordinate
frame. The velocity can be written
N N X
X
∂Ti0 dqj
∂Ti0
d
0
T ri =
ri =
q̇j ri
ṗ =
dt i
∂qj dt
∂qj
j=1
j=1
(B.11)
The integral in equation B.9 becomes
Z
ṗṗT dm =
Z
N X
∂T 0 dqj
!
N X
∂T 0 dqj
!T
i
i
dm (B.12)
ri
ri
∂q
dt
∂q
dt
j
j
i
j=1
j=1
! N
0 T !
Z X
N X
∂Ti0
∂Ti
T
=
q̇j ri
q̇k
ri
dm (B.13)
∂qj
∂qk
i
j=1
k=1
0 T !
Z
N X
N
X
∂Ti
∂Ti0
q̇j
ri ri T dm
q̇k
=
(B.14)
∂q
∂q
j
k
i
j=1 k=1
!
0 T
Z
N X
N
X
∂Ti
∂Ti0
T
ri ri dm
q̇j q̇k
(B.15)
=
∂qj
∂qk
i
j=1 k=1
i
The integral in the centre is constant because ri is defined relative to the link
itself. The final expression for the kinetic energy of a link is found by substituting
equation B.15 into equation B.9 to get
N
N
1 XX
Ki =
trace Uij Ji Uik T q̇j q̇k
2 j=1 k=1
(B.16)
∂Ti0
Uij =
∂qj
(B.17)
where
Z
and
Ji =
ri ri T dm
i
These are the transform derivatives and the pseudo-inertia matrix, respectively
defined in sections 6.3 and 7.4. The kinetic energy for the full robot is then given
2024/2025
ROBOTICS ENGINEERING
128
CHAPTER B Lagrangian Inverse Dynamics Derivation
by
N
X
N
N
N
1 XXX
K=
Ki =
trace Uij Ji Uik T q̇j q̇k
2 i=1 j=1 k=1
i=1
B.3
(B.18)
Lagrangian and Equations of Motion
The kinetic and potential energies from sections B.1 and B.2 can be combined to
give the Lagrangian for the system, and from this the equations of motion can be
derived. The principles are no different from those in Chapter 11, but the algebra
is more protracted.
The Lagrangian can be assembled from equations B.4 and B.18 to give
L = K −P =
N
N
N
N
X
1 XXX
trace Uij Ji Uik T q̇j q̇k −
−mi g T Ti0 r̄i (B.19)
2 i=1 j=1 k=1
i=1
The two terms are treated separately here. Firstly, from the potential energy term
in equation B.4 (the second term in the Lagrangian above), the homogeneous
transform is a function of the generalised coordinates so
N
N
X
X
∂P
∂T 0
=
−mi g T i r̄i =
mi g T Uin r̄i
∂qn
∂q
n
i=1
i=1
(B.20)
The partial derivative with respect to the generalised velocities is seen to be zero:
∂P
=0 ,
∂ q̇n
∀n
(B.21)
Next, looking at the kinetic energy in equation B.18 (the first term in the Lagrangian above), the derivative with respect to the generalised coordinates is given
by
N
N
N
∂Uij
∂K
1 XXX
∂Uik T
T
=
trace
Ji Uik + Uij Ji
q̇j q̇k
(B.22)
∂qn
2 i=1 j=1 k=1
∂qn
∂qn
N
N
N
1 XXX
=
trace Uijn Ji Uik T + Uij Ji Uikn T q̇j q̇k
2 i=1 j=1 k=1
(B.23)
Now, because trace(A + B) ≡ trace(A) + trace(B), and because trace(AT ) ≡
trace(A), this equation can be rearranged as
N
N
N
∂K
1 XXX
=
trace Uijn Ji Uik T + Uikn Ji UijT q̇j q̇k
∂qn
2 i=1 j=1 k=1
ROBOTICS ENGINEERING
(B.24)
2024/2025
Section B.3 Lagrangian and Equations of Motion
129
And the two terms inside the brackets can be seen to be the same but with the j
and k subscripts swapped, so each term is repeated twice and the equation can be
written
N
N
N
∂K X X X
=
trace Uijn Ji Uik T q̇j q̇k
∂qn
i=1 j=1 k=1
(B.25)
The kinetic energy’s derivative with respect to the generalised velocities is given
by
N
i
i
X
1X X
∂K
=
trace Uin Ji Uik T q̇k +
trace Uij Ji Uin T q̇j
∂ q̇n
2 i=1 k=1
j=1
=
N X
i
X
trace Uin Ji Uij T q̇j
!
(B.26)
i=1 j=1
Finally, taking the derivative of this with respect to time gives
d
dt
∂K
∂ q̇n
N X
i X
=
trace Uin Ji Uij T q̈j
i=1 j=1
+ trace
dUin
dUij T
Ji Uij T + Uin Ji
dt
dt
q̇j
(B.27)
(B.28)
which becomes
d
dt
∂K
∂ q̇n
=
N X
i
X
(
trace Uin Ji Uij T q̈j
i=1 j=1
+ trace
N X
∂Uin
k=1
∂qk
+ Uin Ji
q̇k Ji Uij T
N X
∂Uij T
k=1
∂qk
! )
q̇k
q̇j
(B.29)
(B.30)
(B.31)
2024/2025
ROBOTICS ENGINEERING
130
CHAPTER B Lagrangian Inverse Dynamics Derivation
or
d
dt
∂K
∂ q̇n
=
N X
i
X
(
trace Uin Ji Uij T q̈j
i=1 j=1
+
N
X
∂Uin
Ji Uij T
∂qk
!
)
∂Uij T
+ Uin Ji
q̇j q̇k
∂qk
trace
k=1
(B.32)
(B.33)
(B.34)
or
d
dt
∂K
∂ q̇n
=
N X
i
X
(
trace Uin Ji Uij T q̈j
i=1 j=1
+
N
X
)
trace Uink Ji Uij T + Uin Ji Uijk
T
q̇j q̇k
k=1
(B.35)
The equations of motion are derived from
∂P
d ∂K
d ∂P
∂K
+
= Qn
−
−
dt ∂ q̇n
dt ∂ q̇n
∂qn ∂qn
(B.36)
Putting equations B.35,B.21, B.25 and B.20 into equation B.36 gives
(
)
i
N X
N
X
X
trace Uin Ji Uij T q̈j +
trace Uink Ji Uij T + Uin Ji Uijk T q̇j q̇k
i=1 j=1
k=1
−
N X
N X
N
X
trace Uijn Ji Uik
T
i=1 j=1 k=1
q̇j q̇k +
N
X
−mi g T Uin r̄i = Qn
i=1
(B.37)
Interchanging the j and k symbols from the penultimate term on the LHS (and
because Uikn = Uink ), this term cancels with one of the bracketed terms above to
give
N X
i
X
trace Uin Ji Uij
T
i=1 j=1
q̈j +
N X
i X
N
X
trace Uin Ji Uijk T q̇j q̇k
i=1 j=1 k=1
+
N
X
−mi g T Uin r̄i = Qn
(B.38)
i=1
ROBOTICS ENGINEERING
2024/2025
Section B.4 Application of equations of motion
131
These are the equations of motion and you should recognise the first term as the
inertia term, the second term as the centripetal/centrifugal/gyroscopic term, and
the third term as the gravity term.
B.4
Application of equations of motion
This section summarises the results derived in the foregoing sections, and explains
how to use those results to compute the forward dynamics of a general threedimensional serial link arrangement.
Firstly, by rearranging the order of the summation in equation (B.38) you can
write the equations of motion as
N
X
Mnj q̈j +
j=1
N X
N
X
Cnjk q̇j q̇k + Gn = Qn
(B.39)
j=1 k=1
where
Mnj =
N
X
trace Uin Ji Uij T
(B.40)
i=1
Cnjk =
N
X
trace Uin Ji Uijk T
(B.41)
i=1
Gn =
N
X
−mi g T Uin r̄i
(B.42)
i=1
2024/2025
ROBOTICS ENGINEERING
132
CHAPTER B Lagrangian Inverse Dynamics Derivation
This can be written in the familiar matrix form:
Mq̈ + C + G = Q
(B.43)
where

M11
 ..
M= .
MN 1

· · · M1N

..

.
MN N




C
1


.
..
C=

 C 

N
(B.44)

Cn11
T 
Cn = q̇  ...
CnN 1




G
1


.
..
G=

 G 

N

· · · Cn1N

...
 q̇
(B.45)
CnN N
(B.46)
The pseudo-inertia matrices only need to be computed once for any robot (one
pseudo-inertia matrix per link). They are given by


(i)
(i)
(i)
(i)
(i)
1
(−I
+
I
+
I
)
I
I
m
x̄
xx
yy
zz
xy
xz
i
i

 2
(i)
(i)
(i)
(i)
1 (i)

(Ixx − Iyy + Izz )
Iyz
mi ȳi 
Ixy
2
Ji = 

(i)
(i)
(i)
(i)
1 (i)

Ixz
Iyz
(I + Iyy − Izz ) mi z̄i 
2 xx
mi x̄i
mi ȳi
mi z̄i
mi
(i)
(i)
(i)
where Ixx , Iyy and Izz are the moments of inertia for the ith link about the ith
(i)
(i)
(i)
reference frame; Ixy , Ixz and Iyz are the products of inertia for the ith link about
the ith reference frame; mi is the mass of the ith link; and x̄i , ȳi and z̄i are the
coordinates of the centre of mass of the ith link with respect to the ith reference
frame.
To find the derivative of a homogeneous transform Uij , follow the same procedure you would for computing the transform itself:
Ti0 =
i
Y
Tpp−1
(B.47)
p=1
but with the transform Tjj−1 replaced with its derivative, ∂Tjj−1 /∂qj . For example,
U42 = T10
∂T21 2 3
T T
∂q2 3 4
ROBOTICS ENGINEERING
(B.48)
2024/2025
Section B.4 Application of equations of motion
133
The derivative of a transform with respect to a prismatic joint is always


0 0 0 0
∂Tjj−1  0 0 0 0 

=
 0 0 0 1 
∂qj
0 0 0 0
The derivative of a transform with respect to a revolute joint is


0 −1 0 0
∂Tjj−1  1 0 0 0  j−1

=
 0 0 0 0  Tj
∂qj
0 0 0 0
(B.49)
(B.50)
In practice this means you negate the second row of Tjj−1 , then swap the first and
second rows and set the third and fourth rows to all zeros.
To compute a higher order derivative, Uijk , simply replace both Tjj−1 and Tkk−1
with their derivatives. If j = k then you need to take the second derivative; for a
prismatic joint this is zero. For a revolute joint equation B.50 is applied twice, to
give the top two rows of the original transform negated, and the bottom two rows
all zero.
2024/2025
ROBOTICS ENGINEERING
Index
Acceleration, 53, 63, 64
kinetic energy, 113
Blend region, 62
Lagrange’s equation, 111
Lagrangian Dynamics, 112
Lagrangian function, 113
Linear segments, 62
Cartesian-space, 52, 65
Constraints, 53
Cubic polynomial, 53
Degrees of freedom, 112
Denavit-Hartemberg, 24
DH parameters, 24
Dynamics
Forward, 74, 111
Inverse, 74, 111
Lagrangian, 112
Euler angles, 17
Fifth order polynomial, 58
Forth order polynomial, 60
Forward kinematics, 8, 52
Generalised coordinates, 113
Generalised inputs, 70, 72, 113
Higher order polynomials, 58
MATLAB, 55, 56, 59, 61, 63, 115, 119
maximum velocity, 54
Parabolic blend, 62
Path planning, 51
Potential energy, 113
Second order polynomial, 62
Single-link manipulator, 114
Singularity, 52, 67, 113
Trajectory planning, 51
Transformation
General, 16
Homogeneous, 21
Rotation, 11
Translation, 15
Trapezoid, 62
Two-link manipulator, 66, 101
Inverse kinematics, 8, 30, 52
Jacobian, 43
Direct differentiation, 44
Explicit, 46
Joint space, 53
Joint-space, 52
Velocity, 53
Via point, 53, 54
Virtual work, 100
Workspace, 52
Kinematic decoupling, 30
Kinematics, 7
134