CHAPTER 5: Trajectory Generation

Introduction
Trajectory: Describes the desired motion of a manipulator in
multidimensional space, and refers to a time history of position, velocity,
and acceleration for each DOF.
CHAPTER 5: Trajectory Generation
Introduction
Point-to-point planning: The manipulator has to move from an initial to a
final joint configuration in a given time, the actual end-effector path is of
no concern. The algorithm should generate a trajectory which is also
capable to optimize some performance index when the joint moved from
one position to another.
Continuous path generation: For the complex applications, it may be
convenient to assign a sequence of points so as to guarantee better
monitoring on the executed trajectories. Along with spatial constraints on
the motion, the user also wish to specify temporal attributes of the motion.

Trajectory planning can be done either in the joint, or in Cartesian

space. The latter is preferred since it allows a description of the task the
robot has to perform. And allows accounting for the presence of
constraints along the path. In the neighborhood of singular configuration,
trajectory planning in the Cartesian space is not possible, it is advisable to
specify the end-effector movement in terms of joint variables.
CHAPTER 5: Trajectory Generation
Introduction
CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
The path shapes are described in terms of functions of joint angles.
– Path points are usually specified in terms of a desired position and
orientation of the tool frame relative to the station frame.
– Path points are “converted” into a set of desired joint angles by
application of the inverse kinematics.
– A smooth function is found for joints that pass though the via points
and end at the goal point. The time required for each segment is the
same for each joint so that all joints will reach the via point at the
same time, thus resulting in the desired Cartesian position of tool at
each via point.
– Other than specifying the same duration for each joint, the
determination of the desired joint angle function for a particular joint
does not depends on the functions for the other joints.
Joint-space schemes achieve the desired position and orientation at
the via point. In between via points, the shape of the path is complex if
described in Cartesian space.
CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
Joint space schemes are the easiest to compute because we make no
continuous correspondence between joint space and Cartesian space, there
is essentially no problem with singularities of the mechanism.
1. Cubic polynomials
Consider the problem of moving the tool from its initial position to a goal
position in a certain amount of time.
– Inverse kinematics allow the set of joint angles that correspond to the
goal position and orientation to be calculated.
– There are many smooth functions that might be used to interpolate the
joint value.

 (t )  a0  a1t  a2t 2  a3t 3

The velocity profile for any cubic function
is a parabola and the acceleration profile is
linear.
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
In making a single smooth motion, at least four constraints are evident.
Two come from the selection of initial and final values, Two are that the
function be continuous in velocity:  (0)  0 ,  (t f )   f ,  (0)  0 ,  (t f )  0
These constraints uniquely specify a particular cubic:
 0  a0 a0   0
 f  a0  a1t f  a2t 2f  a3t 3f a1  0
0  a1 a2  3( f   0 ) / t 2f
0  a1  2a2t f  3a3t 2f a3  2( f   0 ) / t 3f

Example: A single-link robot is motionless at   15 degrees. It is desired to

move the joint in a smooth manner to   75 degrees in 3 seconds.

a0  15  (t )  15  20t 2  4.44t 3
a1  0  (t )  40t  13.33t 2
a2  20  (t )  40  26.66t
a3  4.44
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
2. Cubic polynomials for a path with via points
When the path includes intermediate point, and we wish to be able to
pass through a intermediate via point without stopping. We can construct
cubic polynomials as before, but velocity constraints become:

 (t0 )  0 ,  (t f )   f ,  (t0 )  0 ,  (t f )   f

0  a0 a0  0
 f  a0  a1t f  a2t 2f  a3t f3 a1  0
0  a1 a2  3( f  0 ) / t 2f  20 / t f   f / t f
 f  a1  2a2t f  3a3t 2f a3  2( f  0 ) / t 3f  ( f  0 ) / t 2f

There are several ways in which the desired velocity at the via point
might be specified.
1) The user specifies the desired velocity at each via point
Cartesian desired velocities at the via points are “mapped” to desired
joint rates. It would be a burden to the user.
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
2) The system automatically chooses the velocities at the via points by
applying a suitable heuristic in either Cartesian space or joint space.
The via point connected with straight line segments. If the slope of these
lines changes sign at the via point, choose zero velocity; if the slope of
these lines does not changes sign, choose the average of the two slopes as
the via velocity.

3) The system automatically chooses the velocities at the via points in such
a way as to cause the acceleration at the via points to be continuous.
Replace the two velocity constraints at the connection of two cubics with
the two constraints that velocity be continuous and acceleration be
continuous.
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
Example: Solve for the coefficients of two cubics that are connected in a
two-segment spline with continuous acceleration at the intermediate via
point. The initial angle is  0 , the via point is  v , and goal point is  g .
Cobics of two-segment, each cubic will be evaluate over an interval starting
at t  0 and ending at t  t fi .

 (t )  a10  a11t  a12t 2  a13t 3 ,  (t )  a20  a21t  a22t 2  a23t 3

0  a10 a10   0
 v  a10  a11t f 1  a12t 2f 1  a13t 3f 1 a11  0
 v  a20 a12  (12 v  3 g  9 0 ) / (4t 2f )
 g  a20  a21t f 2  a22t 2f 2  a23t 3f 2 a13  ( 8 v  3 g  5 0 ) / (4t 3f )
0  a11 a20   v
0  a21  2a22t f 2  3a23t 2f 2 a21  (3 g  3 0 ) / (4t f )
a11  2a12t f 1  3a13t 2f 1  a21 a22  ( 12 v  6 g  6 0 ) / (4t 2f )
2a12  6a13t f 1  2a22 a23  (8 v  5 g  3 0 ) / (4t 3f )
CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
3. Higher-order polynomials
If we wish to be able to specify the position, velocity, and acceleration at
the beginning and end of a path segment, a quintic polynomial is required:

 (t )  a0  a1t  a2t 2  a3t 3  a4t 4  a5t 5

a0   0
0  a0 a1   0
 f  a0  a1t f  a2t 2f  a3t 3f  a4t 4f  a5t 5f 0
a2 
2
0  a1
20 f  20 0  (8 f  12 0 )t f  (3 0   f )t 2f
 f  a1  2a2t f  3a3t 2f  4a4t 3f  5a5t 4f a3 
2t 3f
 0  2 a2
300  30 f  (14 f  160 )t f  (30  2 f )t 2f
 f  2a2  6a3t f  12a4t 2f  20a5t 3f a4 
2t 4f
12 f  12 0  (6 f  6 0 )t f  ( 0   f )t 2f
a5 
2t 5f
CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
4. Linear function with parabolic blends
– Simply interpolate linearly to move from the present joint position to the
final position. The velocity is discontinuous at the beginning and the end
of the motion.
– To create a smooth path with continuous position and velocity, we start
with the linear function but add a parabolic blend region at each path
point. During the blend portion, constant acceleration is used to change
velocity smoothly.
The line function and the two parabolic functions are splined together so
that the entire path is continuous in position and velocity.
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
Assume that the parabolic blends both have the same duration, the same
constant acceleration (modulo a sign) is used during both blends.
The velocity at the end of the blend region must equal the velocity of the
linear section:
  h  b
   h  b 1
 th  tb   tb  , b  0   tb2   tb2   ttb  ( f   0 )  0
     t   t 2 th  tb 2
 0 0

Given  f ,0 , t , any of the paths can be

followed by the choices of  , and that tb
satisfy:

t  2t 2  4 ( f  0 )
tb  
2 2
4( f  20 )

t2
CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes

• When equality occurs, the

linear portion has shrunk
to zero length and the
path is composed of two
blends that connect with
equivalent slope.
• As the acceleration used
becomes larger and
larger, the length of the
blend region becomes
shorter and shorter. In the
limit, with infinite
acceleration, we are back
to the simple linear-
interpolation case.
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
5. Linear function with parabolic blends for a path with via points
Consider that there are an arbitrary number of via points specified and
three neighboring path points j, k , t , use the notation:
tk : duration of the blend region at path point k.
t jk : duration of the linear portion between points j and k.
tdjk : overall duration of the segment connecting point j and k.
 jk : velocity during the linear portion.
 j : acceleration during the blend at point j.

Given all the path points  k , the tdjk

and magnitude of acceleration  k , we
can compute the blend times t k .
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
For interior path points, this follows simply from the equations:
k   j
 jk 
tdjk
 k  sgn( kl   jk )  k
 kl   jk
tk 
k
1 1
t jk  tdjk  t j  tk
2 2
For the first and last segments:
1  sgn( 2  1 ) 1  n  sgn( n 1   n )  n
 2  1  n   n 1
12  12 
 2  1 1  n 1   n 1
 1t1 td 12  t1   n tn td ( n 1) n  tn
1 2 1
td 12  t1 td ( n 1) n  tn 2
2 2( 2  1 ) 2
2( n   n 1 )
t1  td 12  td212  tn  td ( n 1) n  td2 ( n 1) n 
1 n
1 1
t12  td 12  t1  t2 t( n 1) n  td ( n 1) n  tn  tn 1
2 2
 CHAPTER 5: Trajectory Generation
§5.1 Joint-Space Schemes
Note that the via points are not actually reached unless the manipulator
comes to a stop. When the acceleration capability is sufficiently high, the
paths will come close to the desired via point.
If the user wishes the manipulator pass exactly through a via point
without stopping. The system automatically replaces two pseudo via points,
one on each side of the original. The original via point will lie in the linear
region of the path connecting the two pseudo via points.

Problem: examples of trajectory generation in joint space.

CHAPTER 5: Trajectory Generation
§5.2 Cartesian-Space Schemes
Paths computed in joint-space can ensure that via and goal points are
attained, even when these path points were specified by means of Cartesian
frames. However, the spatial shape of the path taken by the end-effector is
some complicated shape that depends on the particular kinematics of
manipulator being used.

Consider methods of path generation in which the path shapes are

described in terms of functions that compute Cartesian position and
orientation as function of time (straight line, circular, sinusoidal…). In this
way, we can also specify the spatial shape of the path between path points.

The paths can be planned directly from user’s definition of path point,
which are {T} specifications relative to {S}, without first performing inverse
kinematics. For example, in arc welding the electrode should follow a seam
precisely.
 CHAPTER 5: Trajectory Generation
§5.2 Cartesian-Space Schemes
Cartesian schemes are more computationally expensive to execute,
because, at run time, inverse kinematics must be solved at the path-update
rate—that is, after the path is generated in Cartesian space, as a last step
the inverse kinematic calculation is performed to calculate desired joint
angles.

1. Cartesian straight-line motion

Often, we specify a spatial path that causes the tip of the tool to move in
a straight-line. It is a subset of the more general capability of Cartesian
motion.
It is much more convenient if the tool follows straight-line paths between
even widely separated via points.
CHAPTER 5: Trajectory Generation
§5.2 Cartesian-Space Schemes
In planning and generating Cartesian straight-line paths, a spline of
linear functions with parabolic blends is appropriate.
• During the linear portion of each segment, all three components of
position change in a linear fashion, and the end-effector will move along
a linear path in space.
• If we specify the orientation as a rotation matrix at each via point, we can
not linearly interpolate its elements, because doing so would not
necessarily result in a valid rotation matrix at all times.

Angle-axis representation can be used to specify an orientation with

three numbers. Rotation matrix can be converted to the angle-axis
representation.
 S
PAORG 
S
A   S 
 K A 
We then need to describe spline functions that smoothly vary these six
quantities from path point to path point as functions of time.
CHAPTER 5: Trajectory Generation
§5.2 Cartesian-Space Schemes
If linear splines with parabolic blends are used, the path shape between
via point will be linear. When via points are passed, the linear and angular
velocity of the end-effector are changed smoothly.
This method does not guarantee that rotations occur about a single
equivalent axis in moving from point to point. One slight complication arises
from the fact that the angle-axis representation of orientation is not unique:
( S Kˆ A , SA )  ( S Kˆ A , SA  n3600 )

In going from a via point {A} to a via

point {B}, the total amount of rotation
should be minimized, such that S K B  S K A
is minimized. The difference vectors are
compared to learn which will result in
minimum rotation,
CHAPTER 5: Trajectory Generation
§5.2 Cartesian-Space Schemes
We can use the same mathematics (splines that are composed of linear
and parabolic sections) for the six values of  . We must add one more
constraint: The blend times for each DOF must be the same. This will ensure
that the resultant motion of all the DOF will be a straight line in space.
Because all blend times must be the same, the acceleration used during
the blend for each DOF will differ. The blend time can be chosen so that a
certain upper bound on acceleration is not exceeded.

Many other schemes for representing and interpolating the orientation

portion of a Cartesian path can be used. For example, the interpolation of
orientation is done by means of Z-Y-Z Euler angles.

Problem: examples of trajectory generation in Cartesian space.

CHAPTER 5: Trajectory Generation
§5.2 Cartesian-Space Schemes
2. Geometric problems with Cartesian Paths
Because a continuous correspondence is made between a path shape
described in Cartesian space and joint positions, Cartesian paths are prone
to various problems relating to workspace and singularities.
Problems of type 1: Intermediate points unreachable
Problems of type 2: High joint rates near singularity
Problems of type 3: Start and goal reachable in different solutions

Most industrial manipulator support both joint space and Cartesian

space path generation. Because of the difficulties with Cartesian paths, joint
space paths should be used as the default.
 CHAPTER 5: Trajectory Generation
§5.3 Path generation at run time
At run time, the path-generator routine constructs the trajectory, usually
in terms of  ,  , , and feeds this information to the control system at the
path-update rate.
1. Generation of joint-space paths
– Cubic splines: simply computes cubic function as t is advanced. When
the end of one segment is reached, a new set of cubic coefficients is
recalled, t is back to zero.
– Linear splines with parabolic blends: the value of time is checked on
each update to determine whether we are currently in the linear or the
blend portion of the segment.

tinb  t  (1 / 2t j  t jk )
   j   jk t
   j   jk (t  tinb )  1 / 2 k tinb
2
   jk
 0    jk   k tinb
  k
CHAPTER 5: Trajectory Generation
§5.3 Path generation at run time
2. Generation of Cartesian-space paths
We use the path generator for the linear spline with parabolic blend path.
tinb  t  (1 / 2t j  t jk )
x  x j  x jk t
x  x j  x jk (t  tinb )  1 / 2 xk tinb
2
x  x jk
x  x jk  xk tinb
x0
x  xk
Finally, this Cartesian trajectory must be converted into equivalent joint-
space quantities. A complete analytical solution to this problem would use
the inverse kinematics to calculate joint positions, the inverse Jacobian for
velocities, and the inverse Jacobian plus its derivative for accelerations.
  GS T
d  INVKIN ( xd ) d  INVKIN ( xd )
d  J 1 () xd (t )  (t   t )
d 
d  J 1 () xd  J 1 () xd t
(t )  (t   t )
d 
t
CHAPTER 5: Trajectory Generation
§5.4 Other Problems
1. Planning paths when using the dynamic model
Usually, when paths are planned, we use a default or a maximum
acceleration at each blend point. Actually, the amount of acceleration that
the manipulator is capable of at any instant is a function of the dynamics of
the arm and the actuator limits. Most actuators are not characterized by a
fixed maximum torque or acceleration, but rather by a torque-speed curve.
When we plan a path assuming there is a maximum acceleration at each
joint or along each DOF, we are making a tremendous simplification. In
order to be careful not to exceed the actual capabilities of the device, this
maximum acceleration must be chosen conservatively. Therefore, we are
not making full use of speed capabilities of the manipulator.
Question: Given a desired spatial path of the end-effector, find the timing
information (which turns a description of a spatial path into a trajectory)
such that the manipulator reaches the goal point in minimum time. The
solution takes both the rigid-body dynamics and actuator speed—torque
constraint curves into account.
CHAPTER 5: Trajectory Generation
§5.4 Other Problems
2. Collision-free path planning
It would be extremely convenient if we could simply tell the robot system
what the desired goal point of the manipulator motion is and let the system
determine where and how many via points are required so that the goal is
reached without the manipulator’s hitting any obstacles.
– The system must have models of the manipulator, the work area, and all
potential obstacles in the area.
– A second manipulator could even be working in the same area, each
arm would have to be considered a moving obstacle for the other.

Systems that plan collision-free paths are not available commercially.

Research in this area has led to two competing principal techniques and to
several variations and combinations thereof.
 forming a connected-graph representation of the free space
 creating artificial potential fields around obstacles
Problem: examples of collision free path planning.