Control of industrial robots

Motion planning

Prof. Paolo Rocco ([email protected])

Politecnico di Milano, Dipartimento di Elettronica, Informazione e Bioingegneria
 Motion planning

 With the motion planning we want to assign the way the robot evolves from an initial posture to a final
one.

 Motion planning is one of the essential problems in robotics. Most of the success on the market of a robot
depends on the quality of motion planning.

 We will address the following issues related to motion planning:

• introductory definitions (path vs. trajectory)
• motion programming
• path planning with obstacle avoidance
• trajectories in joint space (point-to-point and with intermediate points)
• kinematic and dynamic scaling of trajectories
• trajectories in Cartesian space (position and orientation trajectories)

Control of industrial robots – Motion planning – Paolo Rocco

 Definitions

We need to introduce some terminology concerning concepts which are often confused:

 Path: it is a geometric concept and stands for a line in a certain space (the space of Cartesian
positions, the space of the orientations, the joint space,..) to be followed by the object whose
motion has to be planned

 Timing law: it is the time dependence with which we want the robot to travel along the
assigned path

 Trajectory: it is a path over which a timing law has been assigned

The final result of a motion planning problem is thus a trajectory that will then serve as an input
to the real-time position/velocity controllers.

Control of industrial robots – Motion planning – Paolo Rocco

 Trajectories in the operational space

Trajectories in the operational space: the path (position and

orientation) of the robot end effector is specified in the
common Cartesian space

 task description is natural

 constraints on the path can be accounted for
 singular points or redundant degrees of
freedom generate problems
 online kinematic inversion is needed

Control of industrial robots – Motion planning – Paolo Rocco

 Trajectories in joint space

Trajectories in joint space: the desired joint positions are directly

specified

 problems related to kinematic singularities and redundant

degrees of freedom are solved directly
 it is a mode of interest when we just want that the axes move
from an initial to a final pose (and we are not interested in the
resulting motion of the end effector)
 online kinematic inversion is not needed

Control of industrial robots – Motion planning – Paolo Rocco

 Elements of a motion planning and control system

TrajGen

Inverse Axis
target Kinematics controller
state
INSTRUCTION
STACK
state update

 Instruction stack: list of instructions to be executed, specified using the proprietary programming
language
 Trajectory generation: converts an instruction into a trajectory to be executed
 Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed)
 Axis controllers & drives: closes the control loop ensuring tracking performance

Control of industrial robots – Motion planning – Paolo Rocco

 Elements of a motion planning and control system

TrajGen

Inverse Axis
target Kinematics controller
state
INSTRUCTION
STACK
state update

 Instruction stack: list of instructions to be executed, specified using the proprietary programming
language
 Trajectory generation: converts an instruction into a trajectory to be executed
 Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed)
 Axis controllers & drives: closes the control loop ensuring tracking performance

Control of industrial robots – Motion planning – Paolo Rocco

 Teaching by showing

A first mode for motion programming is the so called teaching-by-showing (also

known as lead-through programming).

Using the teach-pendant, the operator moves the manipulator along the desired
path. Position transducers memorize the positions the robot has to reach, which
will be then jointed by a software for trajectory generation, possibly using some of
the intermediate points as via points. COMAU SpA
The robot will be then able to autonomously repeat the motion.

No particular programming skills are requested to the operator.

On the other hand the method has limitations, since making a program requires that the
programmer has the robot at his/her disposal (and then the robot is not productive), there is
no possibility to execute logical functions or wait cycles and in general it is not possible to
program complex activities.

Control of industrial robots – Motion planning – Paolo Rocco

 Programming environments

 The generation of the robot program can also be

done in a robot programming environment. The
robot programmer can move the robot in a
virtual environment with high fidelity rendering
of the robot motion in the robotic cell.
 There are tools to record positions and to make
the robot move along a path formed by such
positions.
 At the end the robot programming environment
produces the code ready to be downloaded into
the robot controller.
ABB RobotStudio

Control of industrial robots – Motion planning – Paolo Rocco

 Programming languages

The robot programmer can also write the robot program directly using a robotic programming
language.

With a robotic programming language the operator can program the motion of the robot as
well as complex operations where the robot, inside a work-cell, interacts with other machines
and devices. With respect to a general purpose programming language, the language provides
specific robot oriented functionalities.

In the following, we will discuss some elements of the PDL2 programming language by COMAU
Robotics.

Control of industrial robots – Motion planning – Paolo Rocco

 Program example

PROGRAM pack The program moves pieces from a feeder to

VAR a table or to a discard bin, depending on
digital input signals:
home, feeder, table, discard : POSITION
BEGIN CYCLE
MOVE TO home
OPEN HAND 1
WAIT FOR $DIN[1] = ON
-- signals feeder ready
MOVE TO feeder
CLOSE HAND 1
IF $DIN[2] = OFF THEN
-- determines if good part
MOVE TO table
ELSE
MOVE TO discard 1. Feeder
ENDIF 2. Robot
OPEN HAND 1 3. Discard Bin
-- drop part on table or in bin 4. Table
END pack

Control of industrial robots – Motion planning – Paolo Rocco

 Data types

Besides data types typical of any programming language (integer, real, boolean, string, array), some
types specific to robotic applications are defined in PDL2. Among them:

VECTOR: representation of a vector through three components

POSITION: three components of Cartesian position, three orientation components (Euler angles)
and a configuration string (which indicates whether it is a shoulder/elbow/wrist
upper/lower configuration)

JOINTPOS: positions of the joints, measured in degrees

Control of industrial robots – Motion planning – Paolo Rocco

 Reference frames

In PDL2 for each manipulator a world reference frame is defined. The operator might redefine
the base frame ($BASE) relative to the world frame. This is useful when the robot has to be
repositioned in the work area, since it avoids re-computation of all the positions of objects in
the cell.

Similarly the programmer can define a

frame ($TOOL) relative to the tool frame of
the manipulator, which is useful whenever
the tool mounted on the end effector is
changed.

Control of industrial robots – Motion planning – Paolo Rocco

 The instruction MOVE

With the instruction MOVE, motion commands of the arms are given. The format
of the instruction is as follows:
MOVE <ARM[n]> <trajectory> dest_clause <opt_clauses> <sync_clause>

(note that a single controller can manage several arms).

The trajectory clause can take one of the following values:

LINEAR (linear motion in Cartesian space)
CIRCULAR (circular motion in Cartesian space)
JOINT (motion in joint space)

The default is a motion in joint space.

Control of industrial robots – Motion planning – Paolo Rocco

 MOVE: destination clauses

There are several destination clauses for the instruction MOVE. Main ones are:
MOVE TO
It moves the arm towards the specified destination, which might be a variable of
type POSITION or JOINTPOS. For example:
MOVE LINEAR TO POS(x,y,z,e1,e2,e3,config)
MOVE TO home
The optional VIA clause can be used with the MOVE TO destination clause to specify
a position through which the arm passes between the initial position and the
destination. The VIA clause is used most commonly to define an arc for circular
moves. For example:

MOVE TO initial
MOVE CIRCULAR TO destination VIA arc

Control of industrial robots – Motion planning – Paolo Rocco

 MOVE: destination clauses

MOVE NEAR
With this clause it is possible to specify a destination which is located along the approach vector
of the tool, within a certain distance (expressed in mm) from a certain position.
Example:
MOVE NEAR destination BY 250.0

MOVE AWAY
A destination can be specified along the approach vector of the
tool, at a specified distance from the current position.
Example:
MOVE AWAY 250.0

Control of industrial robots – Motion planning – Paolo Rocco

 MOVE: destination clauses

MOVE RELATIVE
A destination relative to the current position of the
frame can be specified. Example:
MOVE RELATIVE VEC(100,0,100) IN frame

frame can be either TOOL or BASE

MOVE ABOUT
It defines the destination that the tool has to reach
after a rotation around the specified vector, with
respect to the current position. Example:
MOVE ABOUT VEC(0,100,0) BY 90 IN frame

Control of industrial robots – Motion planning – Paolo Rocco

 MOVE: destination clauses

MOVE BY
Allows the programmer to specify a destination as a list of REAL expressions, with
each item corresponding to an incremental move for the joint of an arm. Example:
MOVE BY {alpha, beta, gamma, delta, omega}

MOVE FOR
Allows the programmer to specify a
partial move along the trajectory
towards a theoretical destination.
The orientation of the tool changes
in proportion to the distance.
Example:

Control of industrial robots – Motion planning – Paolo Rocco

 Setting the speed and the acceleration

Using the optional clause WITH it is possible to assign values to some predefined
temporary variables. In particular we can operate over the following variables:
$PROG_SPD_OVR
It is a percentage by which we can modify the default speed value used by the
controller for joint space motions.
$PROG_ACC_OVR, $PROG_DEC_OVR
They are percentages by which we can modify the default values of acceleration
and deceleration used by the controller for joint space motions.
$LIN_SPD
It is the value of linear speed for a Cartesian motion, expressed in m/s:

Examples: MOVE TO p1 WITH $PROG_SPD_OVR=50

MOVE LINEAR TO p2 WITH $LIN_SPD=0.6

Control of industrial robots – Motion planning – Paolo Rocco

 Continuous motion (MOVEFLY)

If we use the instruction MOVEFLY and the motion is followed by another motion, the arm will not
stop at the first destination, but will move from the starting point of the first motion to the end point
of the second motion, without stopping in the common point of the two motions. Example:

MOVE TO a
MOVEFLY TO b ADVANCE
MOVE TO c

(the ADVANCE clause allows the

interpretation of the next MOVE instruction
as soon as the first motion begins)

Control of industrial robots – Motion planning – Paolo Rocco

 Path planning with obstacle avoidance

 If the robot moves in a cluttered environment, the definition of the path might be troublesome
 In fact we need a path which is collision free: such path, at the current stage of robotics practice, is
generated manually by the programmer as a sequence of MOVE commands

 An active research line consists in

automatic path planning, i.e. in
finding an algorithm that
autonomously generate the
geometric path, given the
kinematics of the robot and the
known positions and shapes of the
obstacles

Control of industrial robots – Motion planning – Paolo Rocco

 Configuration space

 Usually the path planning problem is addressed in the configuration space (C-space), where
the robot is at each time instant represented as a mobile point
 For an articulated robot, the common choice of the configuration space is the space of the
joint variables (joint space)
 Obstacles are mapped from the workspace to the configuration space: in case of an
articulated manipulator they take complicated shapes

We call Cfree the

subset of C-space
that does not cause
collisions with the
obstacles.
A path in C-space is
free if it is entirely
Workspace C-space contained in Cfree

Control of industrial robots – Motion planning – Paolo Rocco

 Setting the problem

The path planning

problem can be set as
follows in general:

C-space Workspace
Assume that the initial and final posture of the robot in the Workspace are mapped into corresponding
configurations in C-space, respectively a start configuration 𝐪𝐪S and a goal configuration 𝐪𝐪G .

Planning a collision-free motion for the robot means to generate a path from 𝐪𝐪S to 𝐪𝐪G if they belong to
the same connected component of Cfree, and to report a failure otherwise.

Control of industrial robots – Motion planning – Paolo Rocco

 Probabilistic planning

 The exact planning algorithms that are based on the a priori knowledge of the complete C-space are
exponential in the dimensionality of the C-space and thus hardly applicable in practice
 Probabilistic planners are instead a class of methods of remarkable efficiency, especially for high-
dimension configuration spaces
 They belong to the general family of sampling-based methods, where the basic idea is to randomly
select a finite-set of collision-free configurations that adequately represent how Cfree is connected, and
using these configurations to build a roadmap
 At each iteration a sample configuration is chosen and it is checked whether it entails a collision
between the robot and the obstacles: if yes, the configuration is discarded, otherwise it is added to the
current roadmap
 Two versions of such randomized sampling-based approach are:
 PRM (Probabilistic Roadmap)
 RRT (Rapidly-exploring Random Tree)

Control of industrial robots – Motion planning – Paolo Rocco

 PRM (Probabilistic roadmap)

 a random sample 𝐪𝐪rand of the C-space is selected using a uniform probability distribution
and tested for collision
 if 𝐪𝐪rand does not cause collisions it is added to a roadmap which is progressively being
formed and connected (if possible) through free local paths to sufficiently “near”
configurations already in the roadmap
 the generation of a free local path between 𝐪𝐪rand and a near configuration 𝐪𝐪near is made by
a procedure called local planner
 the iterations terminate when either a maximum number of iterations has been reached or
the number of connected components in the roadmap becomes smaller than a given
threshold
 then we try to verify whether the path panning problem can be satisfied by connecting 𝐪𝐪S
and 𝐪𝐪G to the same connected component of the PRM by free local paths

Control of industrial robots – Motion planning – Paolo Rocco

 PRM (Probabilistic roadmap)

This picture is taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo:

Robotics: Modelling, Planning and Control, 3rd
Ed.
Springer, 2009

Control of industrial robots – Motion planning – Paolo Rocco

 PRM (Probabilistic roadmap)

Advantages of PRM:
 it is remarkably fast in finding solutions to motion planning problems
 unlike other methods, there is no need to generate the obstacles in C-space

Disadvantages of PRM:
 it is only probabilistically complete: the probability to find a solution to the planning
problem, if it exists, tends to 1 as the execution time tends to infinity

Control of industrial robots – Motion planning – Paolo Rocco

 RRT (Rapidly-exploring random trees)

 the method makes use of a data structure called tree

 a random sample 𝐪𝐪rand of the C-space is selected using a uniform probability distribution
 the configuration 𝐪𝐪near in the tree T (which is progressively formed) that is the closest one to
𝐪𝐪rand is found and a new candidate configuration 𝐪𝐪new is produced on the segment joining
𝐪𝐪near to 𝐪𝐪rand at a predefined distance δ from 𝐪𝐪near
 it is checked that both 𝐪𝐪new and the segment joining 𝐪𝐪near to 𝐪𝐪new belong to Cfree
 if this is true, the tree T is expanded by incorporating 𝐪𝐪new and the said segment
 to speed up the search, two trees are expanded, rooted at 𝐪𝐪S and 𝐪𝐪G , respectively
 at each iteration, both trees are expanded with the randomized procedure
 after a certain number of expansion steps, the algorithm tries to connect the two trees and
thus to complete the path

Control of industrial robots – Motion planning – Paolo Rocco

 RRT (Rapidly-exploring random trees)

This picture is taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo:

Robotics: Modelling, Planning and Control, 3rd
Ed.
Springer, 2009

Control of industrial robots – Motion planning – Paolo Rocco

 RRT (Rapidly-exploring random trees)

 the RRT expansion technique, though simple, results in a very efficient exploration of the C-
space, since the procedure for generating new candidate configurations is intrinsically biased
towards regions in Cfree that have not been visited yet
 as in the PRM method, there is no need to generate the obstacles in C-space
 like the PRM method, it is only probabilistically complete

Example of path
generated with a
RRT algorithm

Control of industrial robots – Motion planning – Paolo Rocco

 RRT (Rapidly-exploring random trees)

Ample free space Cluttered space Narrow passages

Control of industrial robots – Motion planning – Paolo Rocco

 RRT (Rapidly-exploring random trees)

Control of industrial robots – Motion planning – Paolo Rocco

 Artificial potentials

 in the artificial potential method, the motion of the point that represents the robot in C-
space is influenced by a potential field 𝑈𝑈
 this field is obtained as the superposition of an attractive potential to the goal and a
repulsive potential from the obstacles.
 at each configuration 𝐪𝐪 the artificial force generated by the potential is defined as the
negative gradient −∇𝑈𝑈 𝐪𝐪 of the potential local minima!

This picture is taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo:

Robotics: Modelling, Planning and Control, 3rd
Ed.
Springer, 2009

paraboloid potential conic potential

The method is particularly used in online path planning

Control of industrial robots – Motion planning – Paolo Rocco

 Elements of a motion planning and control system

TrajGen

Inverse Axis
target Kinematics controller
state
INSTRUCTION
STACK
state update

 Instruction stack: list of instructions to be executed, specified using the proprietary programming
language
 Trajectory generation: converts an instruction into a trajectory to be executed
 Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed)
 Axis controllers & drives: closes the control loop ensuring tracking performance

Control of industrial robots – Motion planning – Paolo Rocco

 Inputs and outputs of a trajectory planner

As we have seen, in general the user, when assigning a motion command, specifies a restricted number
of parameters as inputs:
 Space of definition: joint space or Cartesian space

 For the path: endpoints, possible intermediate points, path geometry (segment, circular arc, etc..)

 For the timing law: overall travelling time, maximum velocity and/or acceleration (or percentages
thereof)
Based on this information the trajectory planner generates a dense sequence of intermediate points in the
relevant space (joint space or Cartesian space) at a fixed rate (e.g. 10 ms).
In case of Cartesian space trajectories these points are additionally converted into points in joint space
through kinematic inversion.
These values can the be interpolated in order to match the rate of the motion controller (e.g 1ms or 500µs):
this is also called micro-interpolation

Control of industrial robots – Motion planning – Paolo Rocco

 Criteria for trajectory selection

Some criteria for the selection of the trajectories might be:

 computational efficiency and memory space

 continuity of positions, velocities (and possibly of accelerations and jerks)

 minimization of undesired effects (oscillations, irregular curvature)

 accuracy (no overshoot on final position)

Control of industrial robots – Motion planning – Paolo Rocco

 Trajectories in joint space

When we plan the trajectory in joint space we want to generate a function 𝐪𝐪 𝑡𝑡 which interpolates the
values assigned for the joint variables at the initial and final points.
It is sufficient to work component-wise (i.e. we consider a single joint variable 𝑞𝑞𝑖𝑖 𝑡𝑡 ): in the following we
will then consider planning of a scalar variable.
Two situations are of interest:

 Point-to-point motion:
we just specify the endpoints

 Interpolation of points:
we also specify some intermediate points

When planning in joint space, the definition of the path as a geometric entity is not an
issue, since we are not interested in a coordinated motion of the joints (apart from
having all the joints complete their motion at the same instant).

Control of industrial robots – Motion planning – Paolo Rocco

 Polynomial trajectories

The simplest case of trajectory planning for point-to-point q

motion is when some initial and final conditions are assigned on
qf
positions, velocities and possibly on acceleration and jerk and

?
the travel time.

Polynomial functions of the following kind can be considered: qi

ti tf
t
𝑞𝑞 𝑡𝑡 = 𝑎𝑎0 + 𝑎𝑎1 𝑡𝑡 + 𝑎𝑎2 𝑡𝑡 2 + ⋯ + 𝑎𝑎𝑛𝑛 𝑡𝑡 𝑛𝑛

The higher the degree 𝑛𝑛 of the polynomial, the larger the number of boundary
conditions that can be satisfied and the smoother the trajectory will be.

Control of industrial robots – Motion planning – Paolo Rocco

 Cubic polynomials

Suppose that the following boundary conditions are assigned:

 an initial and a final instants 𝑡𝑡𝑖𝑖 and 𝑡𝑡𝑓𝑓
 initial position and velocity 𝑞𝑞𝑖𝑖 and 𝑞𝑞̇ 𝑖𝑖
 final position and velocity 𝑞𝑞𝑓𝑓 and 𝑞𝑞̇ 𝑓𝑓
We then have four boundary conditions. In order to satisfy them we need a
polynomial of order at least equal to three (cubic polynomial):
𝑞𝑞 𝑡𝑡 = 𝑎𝑎0 + 𝑎𝑎1 𝑡𝑡 − 𝑡𝑡𝑖𝑖 + 𝑎𝑎2 𝑡𝑡 − 𝑡𝑡𝑖𝑖 2 + 𝑎𝑎3 𝑡𝑡 − 𝑡𝑡𝑖𝑖 3
If we impose the boundary conditions:
𝑎𝑎0 = 𝑞𝑞𝑖𝑖
𝑞𝑞 𝑡𝑡𝑖𝑖 = 𝑞𝑞𝑖𝑖 𝑎𝑎1 = 𝑞𝑞̇ 𝑖𝑖
𝑞𝑞̇ 𝑡𝑡𝑖𝑖 = 𝑞𝑞̇ 𝑖𝑖 −3 𝑞𝑞𝑖𝑖 − 𝑞𝑞𝑓𝑓 − 2𝑞𝑞̇ 𝑖𝑖 + 𝑞𝑞̇ 𝑓𝑓 𝑇𝑇
we obtain: 𝑎𝑎2 =
𝑞𝑞 𝑡𝑡𝑓𝑓 = 𝑞𝑞𝑓𝑓 𝑇𝑇 2
𝑞𝑞̇ 𝑡𝑡𝑓𝑓 = 𝑞𝑞̇ 𝑓𝑓 2 𝑞𝑞𝑖𝑖 − 𝑞𝑞𝑓𝑓 + 𝑞𝑞̇ 𝑖𝑖 + 𝑞𝑞̇ 𝑓𝑓 𝑇𝑇
𝑎𝑎3 = 𝑇𝑇 = 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖
𝑇𝑇 3

Control of industrial robots – Motion planning – Paolo Rocco

 Cubic polynomials: example

𝑡𝑡𝑖𝑖 = 0, 𝑡𝑡𝑓𝑓 = 1 𝑠𝑠, Position Velocity

40 40

30
𝑞𝑞𝑖𝑖 = 10°, 𝑞𝑞𝑓𝑓 = 30°, 30
20

deg/s
deg
20
10
𝑞𝑞̇ 𝑖𝑖 = 𝑞𝑞̇ 𝑓𝑓 = 0°/𝑠𝑠 10
0

0 -10
-0.2 0 0.2 0.4 0.6 0.8 1 -0.2 0 0.2 0.4 0.6 0.8 1
t(s)
Acceleration
150
100
50
deg/s2

0
-50
-100
-150
-0.2 0 0.2 0.4 0.6 0.8 1
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Polynomials of degree five

In order to assign conditions also on the accelerations, we need to consider polynomials of degree 5:
2 3 4 5
𝑞𝑞 𝑡𝑡 = 𝑎𝑎0 + 𝑎𝑎1 𝑡𝑡 − 𝑡𝑡𝑖𝑖 + 𝑎𝑎2 𝑡𝑡 − 𝑡𝑡𝑖𝑖 + 𝑎𝑎3 𝑡𝑡 − 𝑡𝑡𝑖𝑖 + 𝑎𝑎4 𝑡𝑡 − 𝑡𝑡𝑖𝑖 + 𝑎𝑎5 𝑡𝑡 − 𝑡𝑡𝑖𝑖

Imposing boundary conditions:

𝑞𝑞 𝑡𝑡𝑖𝑖 = 𝑞𝑞𝑖𝑖 𝑞𝑞 𝑡𝑡𝑓𝑓 = 𝑞𝑞𝑓𝑓 𝑎𝑎0 = 𝑞𝑞𝑖𝑖
𝑞𝑞̇ 𝑡𝑡𝑖𝑖 = 𝑞𝑞̇ 𝑖𝑖 𝑞𝑞̇ 𝑡𝑡𝑓𝑓 = 𝑞𝑞̇ 𝑓𝑓 𝑎𝑎1 = 𝑞𝑞̇ 𝑖𝑖
𝑞𝑞̈ 𝑡𝑡𝑖𝑖 = 𝑞𝑞̈ 𝑖𝑖 𝑞𝑞̈ 𝑡𝑡𝑓𝑓 = 𝑞𝑞̈ 𝑓𝑓 1
𝑎𝑎2 = 𝑞𝑞̈ 𝑖𝑖
2
20 𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 − 8𝑞𝑞̇ 𝑓𝑓 + 12𝑞𝑞̇ 𝑖𝑖 𝑇𝑇 − 3𝑞𝑞̈ 𝑓𝑓 − 𝑞𝑞̈ 𝑖𝑖 𝑇𝑇 2
we obtain: 𝑎𝑎3 =
2𝑇𝑇 3
30 𝑞𝑞𝑖𝑖 − 𝑞𝑞𝑓𝑓 + 14𝑞𝑞̇ 𝑓𝑓 + 16𝑞𝑞̇ 𝑖𝑖 𝑇𝑇 + 3𝑞𝑞̈ 𝑓𝑓 − 2𝑞𝑞̈ 𝑖𝑖 𝑇𝑇 2
𝑎𝑎4 =
2𝑇𝑇 4
12 𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 − 6 𝑞𝑞̇ 𝑓𝑓 + 𝑞𝑞̇ 𝑖𝑖 𝑇𝑇 − 𝑞𝑞̈ 𝑓𝑓 − 𝑞𝑞̈ 𝑖𝑖 𝑇𝑇 2
𝑎𝑎5 = 𝑇𝑇 = 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖
2𝑇𝑇 5

Control of industrial robots – Motion planning – Paolo Rocco

 Polynomials of degree five: example

Position Velocity
40 40

𝑡𝑡𝑖𝑖 = 0, 𝑡𝑡𝑓𝑓 = 1 𝑠𝑠, 30

30

20

deg/s
deg
20
𝑞𝑞𝑖𝑖 = 10°, 𝑞𝑞𝑓𝑓 = 30°, 10
10
0
𝑞𝑞̇ 𝑖𝑖 = 𝑞𝑞̇ 𝑓𝑓 = 0, 0 -10
-0.2 0 0.2 0.4 0.6 0.8 1 -0.2 0 0.2 0.4 0.6 0.8 1
t(s)

𝑞𝑞̈ 𝑖𝑖 = 𝑞𝑞̈ 𝑓𝑓 = 0 150

Acceleration

100
50
deg/s2

0
-50
-100
-150
-0.2 0 0.2 0.4 0.6 0.8 1
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Harmonic trajectory

The harmonic trajectory generalizes the equation of a harmonic motion, where the
acceleration is proportional to the position, with opposite sign. A harmonic trajectory has
continuous derivatives of all orders in all the internal points of the trajectory.

The equations are:

𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 𝜋𝜋 𝑡𝑡 − 𝑡𝑡𝑖𝑖

𝑞𝑞 𝑡𝑡 = 1 − cos + 𝑞𝑞𝑖𝑖
2 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 𝑞𝑞 𝑡𝑡𝑖𝑖 = 𝑞𝑞𝑖𝑖 , 𝑞𝑞 𝑡𝑡𝑓𝑓 = 𝑞𝑞𝑓𝑓

𝜋𝜋 𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 𝜋𝜋 𝑡𝑡 − 𝑡𝑡𝑖𝑖

𝑞𝑞̇ 𝑡𝑡 = sin
2 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 𝑞𝑞̇ 𝑡𝑡𝑖𝑖 = 0, 𝑞𝑞̇ 𝑡𝑡𝑓𝑓 = 0

𝜋𝜋 2 𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 𝜋𝜋 𝑡𝑡 − 𝑡𝑡𝑖𝑖

𝑞𝑞̈ 𝑡𝑡 = 2 cos
2 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖

Control of industrial robots – Motion planning – Paolo Rocco

 Harmonic trajectory (example)

𝑡𝑡𝑖𝑖 = 0, 𝑡𝑡𝑓𝑓 = 1 𝑠𝑠, 40

Position
40
Velocity

30
30
𝑞𝑞𝑖𝑖 = 10°, 𝑞𝑞𝑓𝑓 = 30° 20

deg/s
deg
20
10
10
0

0 -10
-0.2 0 0.2 0.4 0.6 0.8 1 -0.2 0 0.2 0.4 0.6 0.8 1
t(s)
Acceleration
150
100
50
deg/s2

0
-50
-100

-150
-0.2 0 0.2 0.4 0.6 0.8 1
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Cycloidal trajectory

The harmonic trajectory has discontinuities in the acceleration in the initial and final instants, and then
undefined (or infinite) values of jerk. An alternative is the cycloidal trajectory, which is continuous in the
acceleration, too.

Here are the equations:

𝑡𝑡 − 𝑡𝑡𝑖𝑖 1 2𝜋𝜋 𝑡𝑡 − 𝑡𝑡𝑖𝑖

𝑞𝑞 𝑡𝑡 = 𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 − sin + 𝑞𝑞𝑖𝑖 𝑞𝑞 𝑡𝑡𝑖𝑖 = 𝑞𝑞𝑖𝑖 , 𝑞𝑞 𝑡𝑡𝑓𝑓 = 𝑞𝑞𝑓𝑓
𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 2𝜋𝜋 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖

𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 2𝜋𝜋 𝑡𝑡 − 𝑡𝑡𝑖𝑖

𝑞𝑞̇ 𝑡𝑡 = 1 − cos 𝑞𝑞̇ 𝑡𝑡𝑖𝑖 = 0, 𝑞𝑞̇ 𝑡𝑡𝑓𝑓 = 0
𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖

2𝜋𝜋 𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 2𝜋𝜋 𝑡𝑡 − 𝑡𝑡𝑖𝑖

𝑞𝑞̈ 𝑡𝑡 = 2 sin 𝑞𝑞̈ 𝑡𝑡𝑖𝑖 = 0, 𝑞𝑞̈ 𝑡𝑡𝑓𝑓 = 0
𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖

Control of industrial robots – Motion planning – Paolo Rocco

 Cycloidal trajectory (example)

Position Velocity
𝑡𝑡𝑖𝑖 = 0, 𝑡𝑡𝑓𝑓 = 1 𝑠𝑠, 40 40

30
30
𝑞𝑞𝑖𝑖 = 10°, 𝑞𝑞𝑓𝑓 = 30° 20

deg/s
deg
20
10
10
0

0 -10
-0.2 0 0.2 0.4 0.6 0.8 1 -0.2 0 0.2 0.4 0.6 0.8 1
t(s)
Acceleration
150
100
50
deg/s2

0
-50
-100

-150
-0.2 0 0.2 0.4 0.6 0.8 1
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Trapezoidal velocity profile (TVP)

A quite common industrial practice to generate the trajectory consists in planning a linear position
profile adjusted at the beginning and at the end of the trajectory with parabolic bends. The resulting
velocity profile has the typical trapezoidal shape.
.
q
.
qv
The trajectory is then composed of three parts:
ti ti+Ta tf−Ta tf t
1. Constant accel., linear velocity, parabolic position; 1 2 3
2. Zero acceleration, constant velocity, linear position;
3. Constant deceleration, linear velocity, parabolic position.

Often the duration 𝑇𝑇𝑎𝑎 of the acceleration phase (phase 1) is set equal to the duration of
the deceleration phase (phase 3): this way a trajectory is obtained, which is symmetric
with respect to the central time instant. Of course it has to be 𝑇𝑇𝑎𝑎 ≤ 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 ⁄2 .

Control of industrial robots – Motion planning – Paolo Rocco

 TVP: equations

Acceleration: 𝑡𝑡 ∈ 𝑡𝑡𝑖𝑖 , 𝑡𝑡𝑖𝑖 + 𝑇𝑇𝑎𝑎 Constant velocity: 𝑡𝑡 ∈ 𝑡𝑡𝑖𝑖 + 𝑇𝑇𝑎𝑎 , 𝑡𝑡𝑓𝑓 − 𝑇𝑇𝑎𝑎

𝑞𝑞̇ 𝑣𝑣 𝑞𝑞̈ 𝑡𝑡 = 0
𝑞𝑞̈ 𝑡𝑡 =
𝑇𝑇𝑎𝑎 𝑞𝑞̇ 𝑡𝑡 = 𝑞𝑞̇ 𝑣𝑣
𝑞𝑞̇ 𝑣𝑣 𝑇𝑇𝑎𝑎
𝑞𝑞̇ 𝑡𝑡 = 𝑡𝑡 − 𝑡𝑡𝑖𝑖 𝑞𝑞 𝑡𝑡 = 𝑞𝑞𝑖𝑖 + 𝑞𝑞̇ 𝑣𝑣 𝑡𝑡 − 𝑡𝑡𝑖𝑖 −
𝑇𝑇𝑎𝑎 2
𝑞𝑞̇ 𝑣𝑣 2
𝑞𝑞 𝑡𝑡 = 𝑞𝑞𝑖𝑖 + 𝑡𝑡 − 𝑡𝑡𝑖𝑖
2𝑇𝑇𝑎𝑎

Deceleration: 𝑡𝑡 ∈ 𝑡𝑡𝑓𝑓 − 𝑇𝑇𝑎𝑎 , 𝑡𝑡𝑓𝑓

𝑞𝑞̇ 𝑣𝑣
𝑞𝑞̈ 𝑡𝑡 = −
𝑇𝑇𝑎𝑎
𝑞𝑞̇ 𝑣𝑣
𝑞𝑞̇ 𝑡𝑡 = 𝑡𝑡 − 𝑡𝑡
𝑇𝑇𝑎𝑎 𝑓𝑓
𝑞𝑞̇ 𝑣𝑣 2
𝑞𝑞 𝑡𝑡 = 𝑞𝑞𝑓𝑓 − 𝑡𝑡 − 𝑡𝑡
2𝑇𝑇𝑎𝑎 𝑓𝑓

Control of industrial robots – Motion planning – Paolo Rocco

 TVP: example

Position Velocity

𝑡𝑡𝑖𝑖 = 0, 𝑡𝑡𝑓𝑓 = 4𝑠𝑠, 𝑇𝑇𝑎𝑎 = 1𝑠𝑠, 30

10

20
𝑞𝑞𝑖𝑖 = 0°, 𝑞𝑞𝑓𝑓 = 30°, 𝑞𝑞̇ 𝑣𝑣 = 10°/𝑠𝑠

deg/s
deg
5
10

0
0
-1 0 1 2 3 4 5 -1 0 1 2 3 4 5
t(s)
Acceleration

10

5
deg/s2

-5

-10
-1 0 1 2 3 4 5
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Trajectory scaling

Once a trajectory has been planned, it is often necessary to scale it in order to satisfy the
constraints on the actuation system, which emerge in terms of saturations.
In particular we will consider:

1. Kinematic scaling: the trajectory needs satisfy the constraints on the

maximum velocity and acceleration;
2. Dynamic scaling: the trajectory needs satisfy the constraints on the maximum
achievable torques/forces by the actuators.

 Kinematic scaling of the trajectory is relevant for those trajectory profiles (cubic, harmonic, …) for
which such values are not assigned in the planning itself.
 For kinematic scaling we can proceed joint by joint (no coupling effects)
 For dynamic scaling we will need to consider the whole coupled model of the robot

Control of industrial robots – Motion planning – Paolo Rocco

 Trajectory normalization

In order to kinematically scale the trajectory it is convenient to express it in a parametric form, as

a function of a suitably normalized parameter 𝜎𝜎 = 𝜎𝜎 𝑡𝑡 .
Given the trajectory 𝑞𝑞 𝑡𝑡 , defined between points 𝑞𝑞𝑖𝑖 and 𝑞𝑞𝑓𝑓 with a travel time of 𝑇𝑇 = 𝑡𝑡𝑓𝑓 − 𝑡𝑡𝑖𝑖 , its
expression in normalized form is as follows:
𝑡𝑡−𝑡𝑡𝑖𝑖
𝑞𝑞 𝑡𝑡 = 𝑞𝑞𝑖𝑖 + ℎ𝜎𝜎 𝜏𝜏 with ℎ = 𝑞𝑞𝑓𝑓 − 𝑞𝑞𝑖𝑖 , 0 ≤ 𝜎𝜎 𝜏𝜏 ≤ 1, 𝜏𝜏 = , 0 ≤ 𝜏𝜏 ≤ 1 (normalized time)
𝑇𝑇

It follows:
𝑑𝑑𝑑𝑑 𝑡𝑡 ℎ Maximum values of velocity, acceleration, etc., are obtained in
= 𝜎𝜎 ′ 𝜏𝜏
𝑑𝑑𝑑𝑑 𝑇𝑇 correspondence to the maximum values of functions 𝜎𝜎 𝑖𝑖 𝜏𝜏 :
𝑑𝑑 2 𝑞𝑞 𝑡𝑡 ℎ ′′
= 2 𝜎𝜎 𝜏𝜏 by modifying the travel time 𝑻𝑻 of the trajectory it is possible to satisfy
𝑑𝑑𝑡𝑡 2 𝑇𝑇 the constraints on the kinematic saturations.
⋮
𝑛𝑛
𝑑𝑑 𝑞𝑞 𝑡𝑡 ℎ 𝑛𝑛
= 𝜎𝜎 𝜏𝜏
𝑑𝑑𝑡𝑡 𝑛𝑛 𝑇𝑇 𝑛𝑛

Control of industrial robots – Motion planning – Paolo Rocco

 Polynomial trajectory of degree 3

This trajectory can be parameterized with the polynomial:

𝜎𝜎 𝜏𝜏 = 𝑎𝑎0 + 𝑎𝑎1 𝜏𝜏 + 𝑎𝑎2 𝜏𝜏 2 + 𝑎𝑎3 𝜏𝜏 3
Assigning the boundary conditions 𝜎𝜎 ′ 0 = 0, 𝜎𝜎 ′ 1 = 0 (besides 𝜎𝜎 0 = 0, 𝜎𝜎 1 = 1):
𝑎𝑎0 = 0, 𝑎𝑎1 = 0, 𝑎𝑎2 = 3, 𝑎𝑎3 = −2
from which:
𝜎𝜎 𝜏𝜏 = 3𝜏𝜏 2 − 2𝜏𝜏 3 𝜎𝜎 ″ 𝜏𝜏 = 6 − 12𝜏𝜏
𝜎𝜎 ′ 𝜏𝜏 = 6𝜏𝜏 − 6𝜏𝜏 2 𝜎𝜎 ‴ 𝜏𝜏 = −12
Maximum values of velocity and acceleration are then:
′ ′
3 3ℎ
𝜎𝜎 max = 𝜎𝜎 0.5 = ⇒ 𝑞𝑞̇ max =
2 2𝑇𝑇
″ ″
6ℎ
𝜎𝜎 max = 𝜎𝜎 0 = 6 ⇒ 𝑞𝑞̈ max = 2
𝑇𝑇
Control of industrial robots – Motion planning – Paolo Rocco
 Polynomial trajectory of degree 5

This trajectory can be parameterized with the polynomial:

𝜎𝜎 𝜏𝜏 = 𝑎𝑎0 + 𝑎𝑎1 𝜏𝜏 + 𝑎𝑎2 𝜏𝜏 2 + 𝑎𝑎3 𝜏𝜏 3 + 𝑎𝑎4 𝜏𝜏 4 + 𝑎𝑎5 𝜏𝜏 5

Assigning boundary conditions 𝜎𝜎 0 = 0, 𝜎𝜎 1 = 1, 𝜎𝜎 ′ 0 = 0, 𝜎𝜎 ′ 1 = 0, 𝜎𝜎 ″ 0 = 0, 𝜎𝜎 ″ 1 = 0

𝑎𝑎0 = 0, 𝑎𝑎1 = 0, 𝑎𝑎2 = 0, 𝑎𝑎3 = 10, 𝑎𝑎4 = −15, 𝑎𝑎5 = 6
from which:
𝜎𝜎 𝜏𝜏 = 10𝜏𝜏 3 − 15𝜏𝜏 4 + 6𝜏𝜏 5 𝜎𝜎 ″ 𝜏𝜏 = 60𝜏𝜏 − 180𝜏𝜏 2 + 120𝜏𝜏 3
𝜎𝜎 ′ 𝜏𝜏 = 30𝜏𝜏 2 − 60𝜏𝜏 3 + 30𝜏𝜏 4 𝜎𝜎 ‴ 𝜏𝜏 = 60 − 360𝜏𝜏 + 360𝜏𝜏 2
15 15ℎ
𝜎𝜎 ′ max = 𝜎𝜎 ′ 0.5 = ⇒ 𝑞𝑞̇ max =
Maximum values of velocity and 8 8𝑇𝑇
acceleration are then: 10 3 10 3ℎ
𝜎𝜎 ″ max ″
= 𝜎𝜎 0.2123 = ⇒ 𝑞𝑞̈ max =
3 3𝑇𝑇 2

Control of industrial robots – Motion planning – Paolo Rocco

 Harmonic trajectory

This trajectory can be parameterized with the function:

1
𝜎𝜎 𝜏𝜏 = 1 − cos 𝜋𝜋𝜋𝜋
2
𝜋𝜋
from which: 𝜎𝜎 ′ 𝜏𝜏 = sin 𝜋𝜋𝜋𝜋
2
″
𝜋𝜋 2
𝜎𝜎 𝜏𝜏 = cos 𝜋𝜋𝜋𝜋
2
‴
𝜋𝜋 3
𝜎𝜎 𝜏𝜏 = sin 𝜋𝜋𝜋𝜋
2
𝜋𝜋 𝜋𝜋𝜋
Maximum values of velocity and 𝜎𝜎 ′
max
′
= 𝜎𝜎 0.5 = ⇒ 𝑞𝑞̇ max =
acceleration are then: 2 2𝑇𝑇
𝜋𝜋 2 𝜋𝜋 2 ℎ
𝜎𝜎 ″ max = 𝜎𝜎 ″ 0 = ⇒ 𝑞𝑞̈ max = 2
2 2𝑇𝑇

Control of industrial robots – Motion planning – Paolo Rocco

 Cycloidal trajectory

This trajectory can be parameterized with the function:

1
𝜎𝜎 𝜏𝜏 = 𝜏𝜏 − sin 2𝜋𝜋𝜋𝜋
2𝜋𝜋
from which:

𝜎𝜎 ′ 𝜏𝜏 = 1 − cos 2𝜋𝜋𝜋𝜋
𝜎𝜎 ″ 𝜏𝜏 = 2𝜋𝜋 sin 2𝜋𝜋𝜋𝜋
𝜎𝜎 ‴ 𝜏𝜏 = 4𝜋𝜋 2 cos 2𝜋𝜋𝜋𝜋

Maximum values of velocity and ′ ′

ℎ
acceleration are then: 𝜎𝜎 max = 𝜎𝜎 0.5 = 2 ⇒ 𝑞𝑞̇ max =2
𝑇𝑇
ℎ
𝜎𝜎 ″ max = 𝜎𝜎 ″ 0.25 = 2𝜋𝜋 ⇒ 𝑞𝑞̈ max = 2𝜋𝜋 2
𝑇𝑇

Control of industrial robots – Motion planning – Paolo Rocco

 Kinematic scaling: an example

We want to design a trajectory with

𝑞𝑞𝑖𝑖 = 10°, 𝑞𝑞𝑓𝑓 = 50°,
Trajectory Formulas Constraints Tmin
 3h  3h
for an actuator characterized by fast  q max = T = 60 = 2
Polin. 3rd degree  2T 2
6h 
𝑞𝑞̇ 𝑚𝑚𝑚𝑚𝑚𝑚 = 30 °⁄𝑠𝑠 , 𝑞𝑞̈ 𝑚𝑚𝑚𝑚𝑚𝑚 = 80 °⁄𝑠𝑠 2 . qmax = T = 6h = 1.732
 T2  80
  15h  15h
 q max = T = 240 = 2.5
The following results can 8T
Polin. 5th degree   2 .5
be obtained (ℎ = 40°): qmax = 10 3h T = 10 3h = 1.699
 3T 2  240
  πh  πh
fast q
 max 2T = T = 60 = 2.094
Harmonic  2  2 2.094
qmax = π h T = π h = 1.571
 2T 2  160
  h  2h
 max q = 2 T = 30 = 2.667
Cycloidal T 2.667
 h 
qmax = 2π T = 2πh = 1.772
 T2  80

Control of industrial robots – Motion planning – Paolo Rocco

 Dynamic scaling

We will discuss the dynamic scaling technique making reference directly to the model of the
robotic manipulator (neglecting friction effects):
𝐁𝐁 𝐪𝐪 𝐪𝐪̈ + 𝐂𝐂 𝐪𝐪, 𝐪𝐪̇ 𝐪𝐪̇ + 𝐠𝐠 𝐪𝐪 = τ

For each joint an equation of the following kind holds:

𝑇𝑇 1 𝑇𝑇
𝐁𝐁𝑖𝑖 𝐪𝐪 𝐪𝐪̈ + 𝐪𝐪̇ 𝐂𝐂𝑖𝑖 𝐪𝐪 𝐪𝐪̇ + 𝑔𝑔𝑖𝑖 𝐪𝐪 = 𝜏𝜏𝑖𝑖 𝑖𝑖 = 1, … , 𝑛𝑛
2
where 𝐂𝐂𝑖𝑖 𝐪𝐪 is a suitable matrix.
Let us consider a parameterization of the trajectory in terms of a scalar function:

𝐪𝐪 = 𝐪𝐪 𝜎𝜎 , 𝜎𝜎 = 𝜎𝜎 𝑡𝑡 (this means that all the joint positions

depend on the time in the same way)
𝑑𝑑𝑑𝑑 𝑑𝑑 2 𝐪𝐪 2 𝑑𝑑𝑑𝑑
Then: 𝐪𝐪̇ = 𝜎𝜎,̇ 𝐪𝐪̈ = 2
𝜎𝜎̇ + 𝜎𝜎̈
𝑑𝑑𝑑𝑑 𝑑𝑑𝜎𝜎 𝑑𝑑𝑑𝑑

Control of industrial robots – Motion planning – Paolo Rocco

 Dynamic scaling

By substituting in the dynamic equation of the 𝑖𝑖𝑡𝑡𝑡 joint we have:

𝑑𝑑𝑑𝑑 𝑑𝑑 2 𝐪𝐪 1 𝑑𝑑𝐪𝐪𝑇𝑇 𝑑𝑑𝑑𝑑 2

𝐁𝐁𝑖𝑖𝑇𝑇 𝐪𝐪 𝜎𝜎 𝜎𝜎̈ + 𝐁𝐁𝑖𝑖𝑇𝑇 𝐪𝐪 𝜎𝜎 + 𝐂𝐂 𝐪𝐪 𝜎𝜎 𝜎𝜎̇ + 𝑔𝑔𝑖𝑖 𝐪𝐪 𝜎𝜎 = 𝜏𝜏𝑖𝑖
𝑑𝑑𝑑𝑑 𝑑𝑑𝜎𝜎 2 2 𝑑𝑑𝑑𝑑 𝑖𝑖 𝑑𝑑𝑑𝑑

i.e. an equation in the form:

𝛼𝛼𝑖𝑖 𝜎𝜎 𝜎𝜎̈ + 𝛽𝛽𝑖𝑖 𝜎𝜎 𝜎𝜎̇ 2 + 𝛾𝛾𝑖𝑖 𝜎𝜎 = 𝜏𝜏𝑖𝑖

Observe that γi depends on the position only (and not on the velocity).
Torques needed to execute the motion are thus:

𝜏𝜏𝑖𝑖 𝑡𝑡 = 𝛼𝛼𝑖𝑖 𝜎𝜎 𝑡𝑡 𝜎𝜎̈ 𝑡𝑡 + 𝛽𝛽𝑖𝑖 𝜎𝜎 𝑡𝑡 𝜎𝜎̇ 2 𝑡𝑡 + 𝛾𝛾𝑖𝑖 𝜎𝜎 𝑡𝑡 , 𝑖𝑖 = 1, … , 𝑛𝑛, 𝑡𝑡 ∈ 0, 𝑇𝑇

Control of industrial robots – Motion planning – Paolo Rocco

 Dynamic scaling

In order to obtain a different parameterization of the trajectory, consider now a time scaling, for
instance a linear one:
we change the time scale and
𝜃𝜃 = 𝑘𝑘𝑘𝑘 𝜃𝜃 ∈ 0, 𝑘𝑘𝑘𝑘
consider a new time 𝜃𝜃
We obtain:

𝜎𝜎 𝑡𝑡 = 𝜎𝜎� 𝜃𝜃 , 𝜎𝜎̇ 𝑡𝑡 = 𝑘𝑘𝜎𝜎� ′ 𝜃𝜃 , 𝜎𝜎̈ 𝑡𝑡 = 𝑘𝑘 2 𝜎𝜎� ″ 𝜃𝜃

where ()′ stands for derivative with respect to 𝜃𝜃.

 If 𝑘𝑘 > 1 the scaled trajectory is slower. 0 t T

 If 𝑘𝑘 < 1 the scaled trajectory is faster.

0 θ kT

Control of industrial robots – Motion planning – Paolo Rocco

 Dynamic scaling

𝜏𝜏𝑖𝑖 𝑡𝑡 = 𝛼𝛼𝑖𝑖 𝜎𝜎 𝑡𝑡 𝜎𝜎̈ 𝑡𝑡 + 𝛽𝛽𝑖𝑖 𝜎𝜎 𝑡𝑡 𝜎𝜎̇ 2 𝑡𝑡 + 𝛾𝛾𝑖𝑖 𝜎𝜎 𝑡𝑡 , 𝑖𝑖 = 1, … , 𝑛𝑛, 𝑡𝑡 ∈ 0, 𝑇𝑇

With the new parameterization, the torques become:

2
𝜏𝜏𝑖𝑖 𝜃𝜃 = 𝛼𝛼𝑖𝑖 𝜎𝜎� 𝜃𝜃 𝜎𝜎� ″ 𝜃𝜃 + 𝛽𝛽𝑖𝑖 𝜎𝜎� 𝜃𝜃 𝜎𝜎� ′ 𝜃𝜃 + 𝛾𝛾𝑖𝑖 𝜎𝜎� 𝜃𝜃 =
𝜎𝜎̈ 𝑡𝑡 𝜎𝜎̇ 2 𝑡𝑡
= 𝛼𝛼𝑖𝑖 𝜎𝜎 𝑡𝑡 2
+ 𝛽𝛽𝑖𝑖 𝜎𝜎 𝑡𝑡 2
+ 𝛾𝛾𝑖𝑖 𝜎𝜎 𝑡𝑡 =
𝑘𝑘 𝑘𝑘
1
= 2 𝛼𝛼𝑖𝑖 𝜎𝜎 𝑡𝑡 𝜎𝜎̈ 𝑡𝑡 + 𝛽𝛽𝑖𝑖 𝜎𝜎 𝑡𝑡 𝜎𝜎̇ 2 𝑡𝑡 + 𝛾𝛾𝑖𝑖 𝜎𝜎 𝑡𝑡 =
𝑘𝑘
1
= 2 𝜏𝜏𝑖𝑖 𝑡𝑡 − 𝑔𝑔𝑖𝑖 𝜎𝜎 𝑡𝑡 + 𝑔𝑔𝑖𝑖 𝜎𝜎 𝑡𝑡
𝑘𝑘

1
Then: 𝜏𝜏𝑖𝑖 𝜃𝜃 − 𝑔𝑔𝑖𝑖 𝜃𝜃 = 2 𝜏𝜏𝑖𝑖 𝑡𝑡 − 𝑔𝑔𝑖𝑖 𝑡𝑡
𝑘𝑘

Control of industrial robots – Motion planning – Paolo Rocco

 Dynamic scaling

1
𝜏𝜏𝑖𝑖 𝜃𝜃 − 𝑔𝑔𝑖𝑖 𝜃𝜃 = 2 𝜏𝜏𝑖𝑖 𝑡𝑡 − 𝑔𝑔𝑖𝑖 𝑡𝑡
𝑘𝑘
 With a new parameterization of the trajectory it is not necessary to compute again the dynamics of
the system;
 New torques are obtained, apart from the gravitational term (which does not depend on the
parameterization), multiplying the torques obtained with the original trajectory by the factor 1⁄𝑘𝑘 2 ;
 The travel time of the new trajectory is 𝑘𝑘𝑇𝑇.
q=q(σ)

0 t T
0 q
σ σmax
0 θ kT

Control of industrial robots – Motion planning – Paolo Rocco

 Dynamic scaling: example

Consider a two d.o.f. manipulator subjected to a trajectory which generates the following torques:

τ1 τ2 To scale the trajectory we compute the

U1 U2 value:

t t 𝜏𝜏1 𝜏𝜏2
𝑘𝑘 2 = max 1, , ≥1
𝑈𝑈1 𝑈𝑈2
−U1 −U2

New torques will be realizable (𝜏𝜏 𝜃𝜃 = 𝜏𝜏 𝑡𝑡 ⁄𝑘𝑘 2 ) and at least one of them will saturate in one
point.
Scaling the trajectory in order to avoid that the torque exceeds the maximum value in a given
interval may excessively slow down the execution: we can resort in this case to a variable scaling
(i.e. applied only in those intervals where there is torque saturation).

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation of points

So far we have considered conditions only on the initial and final points of the trajectory.
We will now consider the more general situation where intermediate points have to be interpolated at given
instants:

𝑡𝑡1 𝑞𝑞1
𝑡𝑡2 𝑞𝑞2
⋮ ⇒ ⋮
𝑡𝑡𝑛𝑛−1 𝑞𝑞𝑛𝑛−1
𝑡𝑡𝑛𝑛 𝑞𝑞𝑛𝑛

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with a polynomial

The problem of finding a trajectory that passes through n points can be solved adopting a polynomial function
of degree 𝑛𝑛 − 1:
𝑞𝑞 𝑡𝑡 = 𝑎𝑎0 + 𝑎𝑎1 𝑡𝑡 + 𝑎𝑎2 𝑡𝑡 2 + ⋯ + 𝑎𝑎𝑛𝑛−1 𝑡𝑡 𝑛𝑛−1

Given the values 𝑡𝑡𝑖𝑖 , 𝑞𝑞𝑖𝑖 , 𝑖𝑖 = 1, ⋯ , 𝑛𝑛 vectors 𝐐𝐐, 𝐚𝐚 and matrix 𝐓𝐓 (Vandermonde matrix) are built as:

𝑞𝑞1 1 𝑡𝑡1 ⋯ 𝑡𝑡1𝑛𝑛−1 𝑎𝑎0

𝑞𝑞2 1 𝑡𝑡2 ⋯ 𝑡𝑡2𝑛𝑛−1 𝑎𝑎1
𝐐𝐐 = ⋮ = ⋮ ⋮ = 𝐓𝐓𝐓𝐓
𝑞𝑞𝑛𝑛−1 1 𝑡𝑡𝑛𝑛−1 ⋯ 𝑛𝑛−1
𝑡𝑡𝑛𝑛−1 𝑎𝑎𝑛𝑛−2
𝑞𝑞𝑛𝑛 1 𝑡𝑡𝑛𝑛 ⋯ 𝑡𝑡𝑛𝑛𝑛𝑛−1 𝑎𝑎𝑛𝑛−1

It follows: 𝐚𝐚 = 𝐓𝐓 −1 𝐐𝐐 (matrix 𝐓𝐓 is always invertible if 𝑡𝑡𝑖𝑖 > 𝑡𝑡𝑖𝑖−1 , 𝑖𝑖 = 1, ⋯ , 𝑛𝑛)

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with a polynomial

Position Velocity
50 30
40 20
30
10

deg/s
deg
20
0
10
0 -10

-10 -20
-2 0 2 4 6 8 10 12 -2 0 2 4 6 8 10 12
t(s)
Acceleration
10

0
𝑡𝑡1 = 0 𝑡𝑡2 = 2 𝑡𝑡3 = 4 𝑡𝑡4 = 8 𝑡𝑡5 = 10
-10 𝑞𝑞1 = 10° 𝑞𝑞2 = 20° 𝑞𝑞3 = 0° 𝑞𝑞4 = 30° 𝑞𝑞5 = 40°
deg/s2

-20

-30

-40
-2 0 2 4 6 8 10 12
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with a polynomial

A clear advantage of the polynomial interpolation is that function 𝑞𝑞 𝑡𝑡 has continuous derivatives of all
the orders inside the interval 𝑡𝑡1 𝑡𝑡𝑛𝑛 .

However the method is not efficient from a numerical point of view: as the number n of points increases,
the condition number 𝑞𝑞 (ratio between the maximum and minimum eigenvalue) of the Vandermonde
matrix 𝐓𝐓 increases too, making the inversion problem numerically ill-conditioned.

𝑖𝑖
If, for example, 𝑡𝑡𝑖𝑖 = , 𝑖𝑖 = 1, ⋯ , 𝑛𝑛) :
𝑛𝑛

n 3 4 5 6 10 15 20
k 15.1 98.87 686.43 4924.37 1.519 ⋅ 10 7 4.032 ⋅ 1011 1.139 ⋅ 1016

Other, more efficient, methods exist to compute the coefficient of the polynomial, however the
numerical difficulties stand for high values of 𝑛𝑛.

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with a polynomial

Regardless the numerical difficulties, the interpolation of n points with only one polynomial of degree
𝑛𝑛 − 1 has other disadvantages:
1. the degree of the polynomial depends on 𝑛𝑛 and, for large values of 𝑛𝑛, the amount of computations
might be remarkable;
2. changing a single point 𝑡𝑡𝑖𝑖 , 𝑞𝑞𝑖𝑖 implies to compute again the entire polynomial;
3. adding a final point 𝑡𝑡𝑛𝑛+1 , 𝑞𝑞𝑛𝑛+1 implies the use of a higher degree polynomial and thus the
computation of all the coefficients;
4. the obtained solution in general presents undesired oscillations
An alternative, instead of using a single polynomial of degree 𝑛𝑛 − 1, is to use 𝑛𝑛 − 1 polynomials of
degree 𝑝𝑝 (typically lower), each of which defined in an interval of the trajectory.
The degree 𝑝𝑝 of the polynomials is usually equal to 3 (pieces of cubic trajectories).
A first, and obvious, way to proceed is to assign positions and velocities in all the points and then to
compute the coefficients of the cubic polynomials between two consecutive points.

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with cubics

Position Velocity
50 20
40
10
30

deg/s
deg

20 0
10
-10
0
-10 -20
-2 0 2 4 6 8 10 12 -2 0 2 4 6 8 10 12
t(s)
Acceleration
40
𝑡𝑡1 = 0 𝑡𝑡2 = 2 𝑡𝑡3 = 4 𝑡𝑡4 = 8 𝑡𝑡5 = 10
20
𝑞𝑞1 = 10° 𝑞𝑞2 = 20° 𝑞𝑞3 = 0° 𝑞𝑞4 = 30° 𝑞𝑞5 = 40°
deg/s2

0 𝑞𝑞̇ 1 = 0°/𝑠𝑠 𝑞𝑞̇ 2 = −10°/𝑠𝑠 𝑞𝑞̇ 3 = 10°/𝑠𝑠 𝑞𝑞̇ 4 = 3°/𝑠𝑠 𝑞𝑞̇ 5 = 0°/𝑠𝑠
-20

-40
-2 0 2 4 6 8 10 12
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with cubics

If only the intermediate positions are specified, and not the intermediate velocities, these can be
assigned with rules like:

𝑞𝑞̇ 1 = 0
0 sign 𝑅𝑅𝑘𝑘 ≠ sign 𝑅𝑅𝑘𝑘+1
𝑞𝑞̇ 𝑘𝑘 = �𝑅𝑅𝑘𝑘 + 𝑅𝑅𝑘𝑘+1
sign 𝑅𝑅𝑘𝑘 = sign 𝑅𝑅𝑘𝑘+1
2
𝑞𝑞̇ 𝑛𝑛 = 0

where:
𝑞𝑞𝑘𝑘 − 𝑞𝑞𝑘𝑘−1
𝑅𝑅𝑘𝑘 =
𝑡𝑡𝑘𝑘 − 𝑡𝑡𝑘𝑘−1

is the slope in the interval 𝑡𝑡𝑘𝑘−1 , 𝑡𝑡𝑘𝑘

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with cubics

Position Velocity
50 20
40
10
30

deg/s
deg
20 0
10
-10
0
-10 -20
-2 0 2 4 6 8 10 12 -2 0 2 4 6 8 10 12
t(s)
Acceleration
30
20 𝑡𝑡1 = 0 𝑡𝑡2 = 2 𝑡𝑡3 = 4 𝑡𝑡4 = 8 𝑡𝑡5 = 10
10 𝑞𝑞1 = 10° 𝑞𝑞2 = 20° 𝑞𝑞3 = 0° 𝑞𝑞4 = 30° 𝑞𝑞5 = 40°
deg/s2

0
-10
-20
-30
-2 0 2 4 6 8 10 12
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Spline

The interpolation with cubic polynomials generates a trajectory which presents a discontinuity in the
acceleration in the intermediate points.

In order to avoid this problem, while keeping cubic interpolation, we must avoid to assign specific values of
velocity in the intermediate points, assigning just the continuity of velocities and accelerations (and of
course positions) in these points.

The trajectory which is obtained this way is called spline (smooth path line).

The spline is the minimum curvature interpolating function, given some conditions on continuity of
derivatives.

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: conditions

Since with 𝑛𝑛 points we need 𝑛𝑛 − 1 polynomials like:

𝑞𝑞 𝑡𝑡 = 𝑎𝑎0 + 𝑎𝑎1 𝑡𝑡 + 𝑎𝑎2 𝑡𝑡 2 + 𝑎𝑎3 𝑡𝑡 3

each of which has 4 coefficients, the total number of coefficients to be computed is 4 𝑛𝑛 − 1 . The
conditions to be imposed are:
 2 𝑛𝑛 − 1 conditions of passage through the points (each cubic has to interpolate the points at its
boundaries)
 𝑛𝑛 − 2 conditions on continuity of velocities in the intermediate points
 𝑛𝑛 − 2 conditions on continuity of accelerations in the intermediate points
We thus have: 4 𝑛𝑛 − 1 − 2 𝑛𝑛 − 1 − 2 𝑛𝑛 − 2 = 2
residual degrees of freedom.
A (not unique) way to use these 2 degrees of freedom consists in assigning suitable initial and final
conditions on the velocity.

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: analytical position of the problem

We want to determine a function:

𝑞𝑞 𝑡𝑡 = 𝑞𝑞𝑘𝑘 𝑡𝑡 , 𝑡𝑡 ∈ 𝑡𝑡𝑘𝑘 , 𝑡𝑡𝑘𝑘+1 , 𝑘𝑘 = 1, … , 𝑛𝑛 − 1
𝑞𝑞𝑘𝑘 𝜏𝜏 = 𝑎𝑎𝑘𝑘𝑘 + 𝑎𝑎𝑘𝑘𝑘 𝜏𝜏 + 𝑎𝑎𝑘𝑘𝑘 𝜏𝜏 2 + 𝑎𝑎𝑘𝑘𝑘 𝜏𝜏 3 , 𝜏𝜏 ∈ 0, 𝑇𝑇𝑘𝑘 𝜏𝜏 = 𝑡𝑡 − 𝑡𝑡𝑘𝑘 , 𝑇𝑇𝑘𝑘 = 𝑡𝑡𝑘𝑘+1 − 𝑡𝑡𝑘𝑘

with the conditions:

𝑞𝑞𝑘𝑘 0 = 𝑞𝑞𝑘𝑘 , 𝑞𝑞𝑘𝑘 𝑇𝑇𝑘𝑘 = 𝑞𝑞𝑘𝑘+1 𝑘𝑘 = 1, … , 𝑛𝑛 − 1

𝑞𝑞̇ 𝑘𝑘 𝑇𝑇𝑘𝑘 = 𝑞𝑞̇ 𝑘𝑘+1 0 = 𝑣𝑣𝑘𝑘+1 𝑘𝑘 = 1, … , 𝑛𝑛 − 2
𝑞𝑞̈ 𝑘𝑘 𝑇𝑇𝑘𝑘 = 𝑞𝑞̈ 𝑘𝑘+1 0 𝑘𝑘 = 1, … , 𝑛𝑛 − 2
𝑞𝑞̇ 1 0 = 𝑣𝑣1 , 𝑞𝑞̇ 𝑛𝑛−1 𝑇𝑇𝑛𝑛−1 = 𝑣𝑣𝑛𝑛

where the quantities 𝑣𝑣𝑘𝑘 , 𝑘𝑘 = 2, … , 𝑛𝑛 − 1 are not specified.

The problem consists in finding the coefficients 𝑎𝑎𝑘𝑘𝑖𝑖 .

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: algorithm

Assume initially that the velocities 𝑣𝑣𝑘𝑘 , 𝑘𝑘 = 2, … , 𝑛𝑛 − 1 in the intermediate points are known. In
this way, for each cubic polynomial we have four boundary conditions on position and velocity,
which give rise to the system:

𝑞𝑞𝑘𝑘 0 = 𝑎𝑎𝑘𝑘𝑘 = 𝑞𝑞𝑘𝑘

𝑞𝑞̇ 𝑘𝑘 0 = 𝑎𝑎𝑘𝑘𝑘 = 𝑣𝑣𝑘𝑘
𝑞𝑞𝑘𝑘 𝑇𝑇𝑘𝑘 = 𝑎𝑎𝑘𝑘𝑘 + 𝑎𝑎𝑘𝑘𝑘 𝑇𝑇𝑘𝑘 +𝑎𝑎𝑘𝑘𝑘 𝑇𝑇𝑘𝑘2 +𝑎𝑎𝑘𝑘𝑘 𝑇𝑇𝑘𝑘3 = 𝑞𝑞𝑘𝑘+1
𝑞𝑞̇ 𝑘𝑘 𝑇𝑇𝑘𝑘 = 𝑎𝑎𝑘𝑘𝑘 +2𝑎𝑎𝑘𝑘𝑘 𝑇𝑇𝑘𝑘 +3𝑎𝑎𝑘𝑘𝑘 𝑇𝑇𝑘𝑘2 = 𝑣𝑣𝑘𝑘+1

𝑎𝑎𝑘𝑘𝑘 = 𝑞𝑞𝑘𝑘
Solving the system yields: 𝑎𝑎𝑘𝑘𝑘 = 𝑣𝑣𝑘𝑘
1 3 𝑞𝑞𝑘𝑘+1 − 𝑞𝑞𝑘𝑘
𝑎𝑎𝑘𝑘𝑘 = − 2𝑣𝑣𝑘𝑘 − 𝑣𝑣𝑘𝑘+1
𝑇𝑇𝑘𝑘 𝑇𝑇𝑘𝑘
1 2 𝑞𝑞𝑘𝑘 − 𝑞𝑞𝑘𝑘+1
𝑎𝑎𝑘𝑘3 = 2 + 𝑣𝑣𝑘𝑘 + 𝑣𝑣𝑘𝑘+1
𝑇𝑇𝑘𝑘 𝑇𝑇𝑘𝑘

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: algorithm

Obviously the velocities 𝑣𝑣𝑘𝑘 , 𝑘𝑘 = 2, … , 𝑛𝑛 − 1 must be computed. Let us impose

the continuity of the accelerations in the intermediate points:

𝑞𝑞̈ 𝑘𝑘 𝑇𝑇𝑘𝑘 = 2𝑎𝑎𝑘𝑘𝑘 + 6𝑎𝑎𝑘𝑘𝑘 𝑇𝑇𝑘𝑘 = 2𝑎𝑎𝑘𝑘+1,2 = 𝑞𝑞̈ 𝑘𝑘+1 0 , 𝑘𝑘 = 1, … , 𝑛𝑛 − 2

By substituting the expressions for the coefficients 𝑎𝑎𝑘𝑘2 , 𝑎𝑎𝑘𝑘3 , 𝑎𝑎𝑘𝑘+1,2 and multiplying by 𝑇𝑇𝑘𝑘 𝑇𝑇𝑘𝑘+1 ⁄2 we
obtain:
3
𝑇𝑇𝑘𝑘+1 𝑣𝑣𝑘𝑘 + 2 𝑇𝑇𝑘𝑘+1 + 𝑇𝑇𝑘𝑘 𝑣𝑣𝑘𝑘+1 + 𝑇𝑇𝑘𝑘 𝑣𝑣𝑘𝑘+2 = 𝑇𝑇𝑘𝑘2 𝑞𝑞𝑘𝑘+2 − 𝑞𝑞𝑘𝑘+1 + 𝑇𝑇𝑘𝑘+1
2
𝑞𝑞𝑘𝑘+1 − 𝑞𝑞𝑘𝑘
𝑇𝑇𝑘𝑘 𝑇𝑇𝑘𝑘+1

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: algorithm

3
𝑇𝑇𝑘𝑘+1 𝑣𝑣𝑘𝑘 + 2 𝑇𝑇𝑘𝑘+1 + 𝑇𝑇𝑘𝑘 𝑣𝑣𝑘𝑘+1 + 𝑇𝑇𝑘𝑘 𝑣𝑣𝑘𝑘+2 = 𝑇𝑇𝑘𝑘2 𝑞𝑞𝑘𝑘+2 − 𝑞𝑞𝑘𝑘+1 + 𝑇𝑇𝑘𝑘+1
2
𝑞𝑞𝑘𝑘+1 − 𝑞𝑞𝑘𝑘
𝑇𝑇𝑘𝑘 𝑇𝑇𝑘𝑘+1
In matrix form:
𝑇𝑇2 2 𝑇𝑇1 + 𝑇𝑇2 𝑇𝑇1 𝑣𝑣1 𝑐𝑐1
0 𝑇𝑇3 2 𝑇𝑇2 + 𝑇𝑇3 𝑇𝑇2 𝑣𝑣2 𝑐𝑐2
⋮ ⋮ = ⋮
𝑇𝑇𝑛𝑛−2 2 𝑇𝑇𝑛𝑛−3 + 𝑇𝑇𝑛𝑛−2 𝑇𝑇𝑛𝑛−3 0 𝑣𝑣𝑛𝑛−1 𝑐𝑐𝑛𝑛−3
𝑇𝑇𝑛𝑛−1 2 𝑇𝑇𝑛𝑛−2 + 𝑇𝑇𝑛𝑛−1 𝑇𝑇𝑛𝑛−2 𝑣𝑣𝑛𝑛 𝑐𝑐𝑛𝑛−2

where constants 𝑐𝑐𝑘𝑘 depend only on the intermediate positions and on the lengths of the intervals, all
known quantities.
Since the velocities 𝑣𝑣1 and 𝑣𝑣𝑛𝑛 are known (they are specified as initial data), by eliminating the related
columns we have:

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: algorithm

2 𝑇𝑇1 + 𝑇𝑇2 𝑇𝑇1

𝑇𝑇3 2 𝑇𝑇2 + 𝑇𝑇3 𝑇𝑇2 𝑣𝑣2
⋮ ⋮
𝑇𝑇𝑛𝑛−2 2 𝑇𝑇𝑛𝑛−3 + 𝑇𝑇𝑛𝑛−2 𝑇𝑇𝑛𝑛−3 𝑣𝑣𝑛𝑛−1
𝑇𝑇𝑛𝑛−1 2 𝑇𝑇𝑛𝑛−2 + 𝑇𝑇𝑛𝑛−1
=
3
𝑇𝑇12 𝑞𝑞3 − 𝑞𝑞2 + 𝑇𝑇22 𝑞𝑞2 − 𝑞𝑞1 − 𝑇𝑇2 𝑣𝑣1
𝑇𝑇1 𝑇𝑇2
3
𝑇𝑇22 𝑞𝑞4 − 𝑞𝑞3 + 𝑇𝑇32 𝑞𝑞3 − 𝑞𝑞2
𝑇𝑇2 𝑇𝑇3
⋮
3 2 2
𝑇𝑇𝑛𝑛−3 𝑞𝑞𝑛𝑛−1 − 𝑞𝑞𝑛𝑛−2 + 𝑇𝑇𝑛𝑛−2 𝑞𝑞𝑛𝑛−2 − 𝑞𝑞𝑛𝑛−3
𝑇𝑇𝑛𝑛−3 𝑇𝑇𝑛𝑛−2
3 2
𝑇𝑇𝑛𝑛−2 2
𝑞𝑞𝑛𝑛 − 𝑞𝑞𝑛𝑛−1 + 𝑇𝑇𝑛𝑛−1 𝑞𝑞𝑛𝑛−1 − 𝑞𝑞𝑛𝑛−2 − 𝑇𝑇𝑛𝑛−2 𝑣𝑣𝑛𝑛 i.e an equation in the form 𝐀𝐀𝐀𝐀 = 𝐜𝐜
𝑇𝑇𝑛𝑛−2 𝑇𝑇𝑛𝑛−1

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: algorithm

 Matrix 𝐀𝐀 is a dominant diagonal matrix and is always invertible provided that 𝑇𝑇𝑘𝑘 > 0;

 Matrix 𝐀𝐀 has a tridiagonal structure: for these matrices there exist efficient numerical
techniques (Gauss-Jordan method) for its inversion;

 Once the inverse of 𝐀𝐀 is known we might compute velocities 𝑣𝑣2 , ⋯ , 𝑣𝑣𝑛𝑛−1 as:

𝐯𝐯 = 𝐀𝐀−1 𝐜𝐜

which completely solves the problem.

It is also possible to determine the spline with an alternative (yet completely equivalent) algorithm
which computes the accelerations instead of the velocities in the intermediate points.

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: example

Position Velocity
50 20
40
10
30

deg/s
deg
20 0
10
-10
0
-10 -20
-2 0 2 4 6 8 10 12 -2 0 2 4 6 8 10 12
t(s)
Acceleration
20

10 𝑡𝑡1 = 0 𝑡𝑡2 = 2 𝑡𝑡3 = 4 𝑡𝑡4 = 8 𝑡𝑡5 = 10

𝑞𝑞1 = 10° 𝑞𝑞2 = 20° 𝑞𝑞3 = 0° 𝑞𝑞4 = 30° 𝑞𝑞5 = 40°
deg/s2

-10

-20
-2 0 2 4 6 8 10 12
t(s)

Control of industrial robots – Motion planning – Paolo Rocco

 Spline: travel time

The overall travel time of a spline is given by:

𝑛𝑛−1

𝑇𝑇 = � 𝑇𝑇𝑘𝑘 = 𝑡𝑡𝑛𝑛 − 𝑡𝑡1

𝑘𝑘=1
It is possible to conceive an optimization problem which minimizes the overall travel time. The problem is
to determine the values 𝑇𝑇𝑘𝑘 so as to minimize 𝑇𝑇, with the constraints on the maximum velocities and
accelerations.
Formally: 𝑛𝑛−1

min𝑇𝑇 = � 𝑇𝑇𝑘𝑘
𝑇𝑇𝑘𝑘
𝑘𝑘=1
𝑞𝑞̇ 𝜏𝜏, 𝑇𝑇𝑘𝑘 < 𝑣𝑣max 𝜏𝜏 ∈ 0, 𝑇𝑇
such that
𝑞𝑞̈ 𝜏𝜏, 𝑇𝑇𝑘𝑘 < 𝑎𝑎max 𝜏𝜏 ∈ 0, 𝑇𝑇

It is then a non linear optimum problem with a linear cost function, which can be solved with operations
research techniques.

Control of industrial robots – Motion planning – Paolo Rocco

 Interpolation with linear segments

An alternative, quite simple, way to handle the interpolation problem is to link the points with
linear functions (segments). In order to avoid discontinuities in the velocity, the linear segments
can be connected through parabolic bends

The resulting trajectory q(t) does not pass

through any of the intermediate points,
though it is close to them. In this case the
intermediate points are called via points.

This picture is taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo:

Robotics: Modelling, Planning and Control, 3rd Ed.
Springer, 2009

Control of industrial robots – Motion planning – Paolo Rocco

 TVP and interpolation of points

If we use a sequence of trajectories with trapezoidal velocity profile to interpolate points, we

would obtain a motion which passes through these points with zero velocity (i.e. stopping).
To avoid this we can start the planning of a trajectory before the end of the preceding one:

This picture is taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo:
Robotics: Modelling, Planning and Control, 3rd Ed., Springer, 2009

Control of industrial robots – Motion planning – Paolo Rocco

 Trajectories in the operational space

Trajectory planning in the joint space yields unpredictable motions of the end-
effector. When we want the motion to evolve along a predefined path in the
operational space, it is necessary to plan the trajectory directly in this space.

Trajectory planning in the operational space entails both a path

planning problem and a timing law planning problem: both the
path and the timing law can be expressed analytically, as it will be
shown in the following.

We will first address the trajectory planning for the position, and
then we will concentrate on the orientation.

Control of industrial robots – Motion planning – Paolo Rocco

 Path parameterization

Let us consider a parametric representation of a curve in space. The parameterization can be

performed with respect to the natural coordinate (length of the arc of trajectory): 𝐩𝐩 = 𝐩𝐩 𝑠𝑠

We can define the tangent, normal and

y
binormal unit vectors:
p t
pi pf 𝑑𝑑𝑑𝑑 𝑠𝑠
𝐭𝐭 = unit tangent vector
n 𝑑𝑑𝑑𝑑
b
x
𝑑𝑑 2 𝐩𝐩 𝑠𝑠 ⁄𝑑𝑑𝑠𝑠 2 unit normal vector
𝐧𝐧 = 2 (belongs to the
𝑑𝑑 𝐩𝐩 𝑠𝑠 ⁄𝑑𝑑𝑠𝑠 2 osculating plane)
z
𝐛𝐛 = 𝐭𝐭 × 𝐧𝐧 unit binormal vector

Control of industrial robots – Motion planning – Paolo Rocco

 Linear path

As an example of path parameterization we can consider a segment in space

(linear Cartesian path):
pf
𝑠𝑠
𝐩𝐩 𝑠𝑠 = 𝐩𝐩𝑖𝑖 + 𝐩𝐩𝑓𝑓 − 𝐩𝐩𝑖𝑖 pi
𝐩𝐩𝑓𝑓 − 𝐩𝐩𝑖𝑖
𝑑𝑑𝑑𝑑 1
= 𝐩𝐩𝑓𝑓 − 𝐩𝐩𝑖𝑖
𝑑𝑑𝑑𝑑 𝐩𝐩𝑓𝑓 − 𝐩𝐩𝑖𝑖
𝑑𝑑 2 𝐩𝐩
=0
𝑑𝑑𝑠𝑠 2

In this case it is not possible to define the frame (𝐭𝐭, 𝐧𝐧, 𝐛𝐛) uniquely.

Control of industrial robots – Motion planning – Paolo Rocco

 Linear path

A linear path is completely characterized once two points in Cartesian space are given.

Control of industrial robots – Motion planning – Paolo Rocco

 Concatenation of linear paths

Linear paths can be concatenated in order to obtain more B

elaborated paths.
A
The intermediate point between two consecutive segments can C
be considered as a via point, meaning that there is no need to
pass and stop there. over-fly

During the over-fly, i.e. the passage near a via point, the path remains always in the plane specified by the
two lines intersecting in the via point. This means that the problem of planning the over-fly is planar.

Formulas can be derived to define the blending (typically a parabolic one)

Control of industrial robots – Motion planning – Paolo Rocco

 Circular path

A parametric representation of a circumference of radius 𝜌𝜌 laying in a plane 𝑥𝑥 ′ 𝑦𝑦 ′ and

having the centre in the origin of such plane is:

𝜌𝜌 cos 𝑠𝑠⁄𝜌𝜌
𝐩𝐩′ 𝑠𝑠 = 𝜌𝜌 sin 𝑠𝑠⁄𝜌𝜌
0

Defining:
 𝐜𝐜 the vector that identifies the centre of the circumference in the base frame
 𝐑𝐑 the rotation matrix from base frame to frame 𝑥𝑥 ′ 𝑦𝑦 ′ 𝑧𝑧 ′
the general parametric representation of a circumference in space is:

𝐩𝐩 𝑠𝑠 = 𝐜𝐜 + 𝐑𝐑𝐩𝐩′ 𝑠𝑠

Control of industrial robots – Motion planning – Paolo Rocco

 Circular path

The circular path can also be defined assigning three points in space belonging to the same plane:

Control of industrial robots – Motion planning – Paolo Rocco

 Position trajectories

For the position trajectories, taking into account the 0 t t

parameterization of the path with respect to the natural
coordinate 𝐩𝐩 = 𝐩𝐩 𝑠𝑠 , we will assign the timing law through the
function 𝑠𝑠 𝑡𝑡 . 0 s s

In order to determine function 𝑠𝑠 𝑡𝑡 we can use any of the px=px(s)

timing laws (polynomials, harmonic, trapezoidal velocity py=py(s)
profile, spline, etc.) already studied. pz=pz(s)
Also we notice that:

𝑑𝑑𝑑𝑑 the time law 𝑠𝑠 𝑡𝑡

𝐩𝐩̇ = 𝑠𝑠̇ = 𝑠𝑠𝐭𝐭̇ 𝑠𝑠̇ is then the norm of the velocity takes then an
𝑑𝑑𝑑𝑑 immediate meaning!
𝑠𝑠̇
For the segment: 𝐩𝐩̇ = 𝐩𝐩𝑓𝑓 − 𝐩𝐩𝑖𝑖 = 𝑠𝑠𝐭𝐭̇
𝐩𝐩𝑓𝑓 − 𝐩𝐩𝑖𝑖

Control of industrial robots – Motion planning – Paolo Rocco

 Orientation trajectories

For the orientation planning we might interpolate (e.g. linearly) the components of the unit
vectors 𝐧𝐧 𝑡𝑡 , 𝐬𝐬 𝑡𝑡 and 𝐚𝐚 𝑡𝑡 .
This procedure is however not advisable as the orthonormality of unit vectors cannot be
guaranteed at all times.

An alternative way is to interpolate three Euler angles, using the following relations:

𝑠𝑠
φ 𝑠𝑠 = φ𝑖𝑖 + φ𝑓𝑓 − φ𝑖𝑖 We can use any timing law s(t) on the natural parameter.
φ𝑓𝑓 − φ𝑖𝑖
𝑠𝑠̇ The angular velocity ω, which is linearly related to 𝜑𝜑,̇ has a
φ̇ = φ𝑓𝑓 − φ𝑖𝑖 continuous variation of the amplitude.
φ𝑓𝑓 − φ𝑖𝑖
𝑠𝑠̈ Poor predictability and understanding of the
φ̈ = φ𝑓𝑓 − φ𝑖𝑖
φ𝑓𝑓 − φ𝑖𝑖 intermediate orientation!

Control of industrial robots – Motion planning – Paolo Rocco

 Orientation trajectories

Orientation can also be planned by resorting to the axis/angle

representation: if we assign two frames with the same origin but different
orientation, it is always possible to determine a unit vector 𝐫𝐫 such that the
second frame is obtained from the first one through a rotation of an angle
𝜗𝜗𝑓𝑓 around the axis of such unit vector.
Let 𝐑𝐑 𝑖𝑖 and 𝐑𝐑𝑓𝑓 the rotation matrices, with respect to the base frame, of the
initial frame and of the final frame. The rotation matrix between the two
frames, with related axis/angle representation is thus:
trace of matrix 𝐑𝐑

𝑟𝑟11 𝑟𝑟12 𝑟𝑟13 𝑟𝑟11 + 𝑟𝑟22 + 𝑟𝑟33 − 1

𝜗𝜗𝑓𝑓 =cos −1
𝐑𝐑𝑓𝑓𝑖𝑖 = 𝐑𝐑𝑇𝑇𝑖𝑖 𝐑𝐑𝑓𝑓 = 𝑟𝑟21 𝑟𝑟22 𝑟𝑟23 2
𝑟𝑟31 𝑟𝑟32 𝑟𝑟33 𝑟𝑟32 − 𝑟𝑟23
1
𝐫𝐫 = 𝑟𝑟13 − 𝑟𝑟31
2 sin 𝜗𝜗𝑓𝑓 𝑟𝑟21 − 𝑟𝑟12

Control of industrial robots – Motion planning – Paolo Rocco

 Orientation trajectories

Define with 𝐑𝐑𝑖𝑖 𝑡𝑡 the matrix that describes the transition from 𝐑𝐑 𝑖𝑖 to 𝐑𝐑𝑓𝑓 .
Then:

𝐑𝐑𝑖𝑖 0 = 𝐈𝐈, 𝐑𝐑𝑖𝑖 𝑡𝑡𝑓𝑓 = 𝐑𝐑𝑓𝑓𝑖𝑖 𝐑𝐑𝑖𝑖

𝐑𝐑 𝑖𝑖

𝐑𝐑𝑓𝑓

Matrix 𝐑𝐑𝑖𝑖 𝑡𝑡 can be interpreted as 𝐑𝐑𝑖𝑖 𝜗𝜗 𝑡𝑡 , 𝐫𝐫 , where:

 𝐫𝐫 is constant and can be computed from the elements of 𝐑𝐑𝑓𝑓𝑖𝑖
 𝜗𝜗 𝑡𝑡 can be made variable with time, through a timing law, with 𝜗𝜗 0 = 0, 𝜗𝜗 𝑡𝑡𝑓𝑓 = 𝜗𝜗𝑓𝑓
In order to characterize the orientation in the base frame it is then enough to compute: 𝐑𝐑 𝑡𝑡 = 𝐑𝐑 𝑖𝑖 𝐑𝐑𝑖𝑖 𝑡𝑡

Control of industrial robots – Motion planning – Paolo Rocco

 Orientation trajectories

Alternatively we can use unit quaternions to specify the orientation.

They are defined as:
ℚ = 𝜂𝜂, 𝜺𝜺

with:
𝜗𝜗
𝜂𝜂 = cos
2 𝜂𝜂 2 + 𝜀𝜀𝑥𝑥2 + 𝜀𝜀𝑦𝑦2 + 𝜀𝜀𝑧𝑧2 = 1
𝜗𝜗 1
𝜺𝜺 = sin 𝐫𝐫 𝜂𝜂 = 𝑟𝑟 + 𝑟𝑟22 + 𝑟𝑟33 + 1
2 2 11
where 𝜗𝜗 and 𝐫𝐫 have the same sgn 𝑟𝑟32 − 𝑟𝑟23 𝑟𝑟11 − 𝑟𝑟22 − 𝑟𝑟33 + 1
meaning as with the axis/angle 1
𝜺𝜺 = sgn 𝑟𝑟13 − 𝑟𝑟31 𝑟𝑟22 − 𝑟𝑟33 − 𝑟𝑟11 + 1
representation, but the 2
representation singularity has sgn 𝑟𝑟21 − 𝑟𝑟12 𝑟𝑟33 − 𝑟𝑟11 − 𝑟𝑟22 + 1
been removed:

Control of industrial robots – Motion planning – Paolo Rocco

 Elements of a motion planning and control system

TrajGen

Inverse Axis
target Kinematics controller
state
INSTRUCTION
STACK
state update

 Instruction stack: list of instructions to be executed, specified using the proprietary programming
language
 Trajectory generation: converts an instruction into a trajectory to be executed
 Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed)
 Axis controllers & drives: closes the control loop ensuring tracking performance

Control of industrial robots – Motion planning – Paolo Rocco

 Inverse kinematics

Inverse kinematics has already been discussed with reference to the study of the
kinematics of the (possibly redundant) manipulator.

Options are:
.
q0
 Closed form analytic solution
(whenever possible) .
rd In−J#(q)J(q)
 Numerical solution through
(pseudo) inverse of the .
Jacobian rd + + + J#(q) + + q q
K
−

r
f(q)

Control of industrial robots – Motion planning – Paolo Rocco

 Elements of a motion planning and control system

TrajGen

Inverse Axis
target Kinematics controller
state
INSTRUCTION
STACK
state update

 Instruction stack: list of instructions to be executed, specified using the proprietary programming
language
 Trajectory generation: converts an instruction into a trajectory to be executed
 Inverse kinematics: maps the trajectory from the Cartesian space to the joint space (if needed)
 Axis controllers & drives: closes the control loop ensuring tracking performance (see next set of
slides)

Control of industrial robots – Motion planning – Paolo Rocco