Ho Chi Minh city University of Technology and Education

Faculty of Electrical & Electronics Engineering

Robotics and Intelligent Control Laboratory

Robotics

Presenter: Dr. Duc Thien, Tran

1
 Tài liệu tham khảo

1. John J. Craig, Introduction to Robotics: Mechanics and Control, 2018.

2. F. Fahimi, Autonomous Robots: modeling, path planning, and control, 2009.
3. PGS. Nguyễn Trường Thịnh, Giáo trình Kỹ thuật Robot, NXB Đại học Quốc gia TP.HCM, 2014.
4. Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani and Giuseppe Oriolo, Robotics: Modelling,
Planning and Control, 2009.
5. https://www.youtube.com/watch?v=8MzC8Nmr2rg
6. https://www.youtube.com/watch?v=OYEvD8UHT8Y

2
 Contents

Topic 1: Introduction
Topic 2: Basic robotic concepts
Topic 3: Spatial Representations of Rigid Bodies
Topic 4: Forward Kinematics of Robot Manipulators
Topic 5: Inverse Kinematics of Robot Manipulators
Topic 6: Kinematic Trajectory Generation.

3
 - Topic 6 -

• Kinematic Trajectory
Generation
Trajectory Generation

5
 Outline

1. Trajectory Generation
2. Trajectories in the Joint Space
2.1. Point to point trajectories
2.2 Trajectories using waypoints
3. Trajectories in the Operational Space
 Functional Scheme of a Robot

Interoceptive
Sensors

External IN Trajectory OUT

References Tasks Control Robot
Generation

Exteroceptive External
Sensors environment

Trajectory generator:
• Inputs (IN): sequence of poses (for end effector) or joint configurations
• Outputs (OUT): references (continuous/discrete) for the robot controller
 Motion planning

Trajectory planning is a subset of the overall problem that Task

is navigation or motion planning. The typical hierarchy of motion planning is as planning
follows:
1. Task planning – Designing a set of high-level goals, such as “go pick up Path
the object in front of you”. planning

2. Path planning – Generating a feasible path from a start point to a goal Trajectory
point. A path usually consists of a set of connected waypoints. planning
3. Trajectory planning – Generating a time schedule for how to follow a
path given constraints such as position, velocity, and acceleration. Trajectory Following

4. Trajectory following – Once the entire trajectory is planned, there needs Controller
to be a control system that can execute the trajectory in a sufficiently
accurate manner. Robot

Sensor

8
 Motion planning

Path planning Task

• Geometric path. planning

• Problem: Avoid obstacles, shortest paths, constraints. Path

• Solution: Optimization and evolutionary methods. planning

Trajectory planning Trajectory

• Build time-based polynomial functions related to position, velocity, and planning
acceleration for the robot from the starting point to the destination.
Trajectory Following
• Solution: Approximate polynomial functions.
Controller

Robot

Sensor

9
 Trajectory Generation

Introduction
1. Objective: from points to (Cartesian/joint) trajectories

A A B
Cartesian Cartesian
trajectory trajectory
C C

Move from pose A to pose C via

Move from pose A to pose C
pose B

Does time matter?

Trajectory Generation

11
 Trajectory Generation

Trajectory vs Path
1. Path (geometric):
• Set of points (in the joint or in the Cartesian space) that the robot must follow B

p( s ) =  x( s ) y(s) z (s)
T

• It is a purely geometric description

A
2. Trajectory:
• Geometric path p(s) + associated time s(t)
p ( s (t )) =  x( s (t )) y ( s (t )) z ( s (t )) 
T

• It considers velocity/acceleration constraints

Trajectory: Time matters (constraints of v, a)

Path: Purely geometric

 Trajectory Generation
Trajectory vs Path
1. Usually we first select a path, and then the timing law associated with it (the full trajectory)
Path Timing law

p = p(s) s = s (t ) Cartesian space

q = q ( )  =  (t ) Joint space

Trajectory
• Natural parameterization: s = t
• Derivatives: dp ( s ) dp ( s ) d 2 p(s) 2
p (t ) = s (t) p (t ) = s (t ) + 2
s (t )
ds ds ds
2. Timing law:
• It is based on specifications (velocities, where to stop, etc.)
• Constraints imposed by actuators or tasks (max torque, max velocity)
• Sometimes optimization criteria (minimum time, minimum energy, etc.)
 Trajectory Generation

Example of Path
1. Objective:
• Pass through the Cartesian points A, B, C Using inverse kinematics

• Avoid discontinuities

3 dof robot

Cartesian space Joint space

 Trajectory Generation

Example of Path
1. Steps:
Desired geometric path p(s), where s is a parameter

3 dof robot

Cartesian space Joint space

 Trajectory Generation

Example of Path
1. Steps:
Equivalent in the joint
Sampling the Cartesian path to find the desired points space

3 dof robot

Cartesian space Joint space

 Trajectory Generation

Example of Path
1. Steps:
Geometric path in the joint space: q1(λ), q2(λ), q3(λ)

3 dof robot

Cartesian space Joint space

 Trajectory Generation

1. Typical Procedure
Tasks

Points in Cartesian space

Interpolation in Cartesian Space

Cartesian geometric path:

p(s)

Sampling and inverse kinematics

Sequence of points in joint

space

Interpolation in joint space

Geometric path in joint

space: q(λ)
• Task space means the waypoints and interpolation are on the Cartesian pose (position and
orientation) of a specific location on the manipulator – usually the end effector.
• Joint space means the waypoints and interpolation are directly on the joint positions (angles or
displacements, depending on the type of joint)
The main difference is that task-space trajectories tend to look more “natural” than joint-space
trajectories because the end effector is moving smoothly concerning the environment even if the
joints are not.
The big drawback is that following a task-space trajectory involves solving inverse kinematics (IK)
more often than a joint-space trajectory, which means a lot more computation especially if your IK
solver is based on optimization.

Trajectory generation Trajectory following Trajectory generation Trajectory following

Task space Inverse Inverse Joint space

Path planning Manipulators Path planning Manipulators
trajectory kinematics kinematics trajectory

19
 Trajectory Generation

Joint Space vs Cartesian Space

1. Trajectories in the Cartesian (operational) space:
• More direct visualization of the generated path
• It allows for obstacle avoidance
• It can describe a given Cartesian path (do not
“wander”)
• It requires inverse kinematics (computationally
more expensive)
2. Trajectories in the joint space
• More complex to visualize
• It is harder to avoid obstacles
• It cannot follow Cartesian paths (example: a
“line”)
• It does not require inverse kinematics at every
point (less computationally expensive)
Examples

21
 Trajectory Generation

Classification of Trajectories
1. According to the space:
• Cartesian (operational) trajectories
• Joint trajectories
2. According to the type of task:
• Point-to-point trajectories
• Multiple-point trajectories (“knots”)
• Continuous trajectories (continuity of speed, acceleration)
• Concatenated trajectories (example: “overfly”)
3. According to the path geometry:
• Linear trajectories
• Polynomial trajectories
• Exponential trajectories
• Cicloid trajectories, etc.
 Trajectory Generation
4. According to the time law:
• Bang-bang (on/off) trajectories in acceleration
• Trapezoidal trajectories in velocity
• Polynomial trajectories, etc.

5. According to the coordination:

• Coordinated trajectories (all joints start and end the motion at the same time and simultaneously)
• Independent trajectories (independent motion of every joint)
 Trajectory Generation
Joint Space vs Cartesian Space
1. Examples

Not coordinated joint interpolation Coordinated joint interpolation

Not coordinated joint interpolation Interpolation in Cartesian space

 Outline

1. Trajectory Generation
2. Trajectories in the Joint Space
2.1. Point to point trajectories
2.2 Trajectories using waypoints
3. Trajectories in the Operational Space
 (Review: polynomials)

1. Linear: q = a1t + a0 2 parameters (a1, a0)

2 equations are needed to find a1, a0

2. Quadratic: q = a2t 2 + a1t + a0 3 parameters (a2, a1, a0)

3 equations are needed

3. Cubic: q = a3t 3 + a2t 2 + a1t + a0 4 parameters (a3, a2, a1, a0)

4 equations are needed

4. Degree 4: q = a4t 4 + a3t 3 + a2t 2 + a1t + a0 5 parameters (a4, a3, a2, a1, a0)
5 equations are needed

5. Degree 5: q = a5t 5 + a4t 4 + a3t 3 + a2t 2 + a1t + a0 6 parameters (a5, a4, a3,
a2, a1, a0), 6 equations
are needed

6. Degree n: q = ant n + an −1t n −1 + + a1t + a0 n+1 parameters (an, an-1, …, a1, a0)
n+1 equations are needed
 (Review: polynomials)

Example:
Interpolate the points shown in the figure using a line, where t0 = 2 [s], q0 = 16 [°], tf = 10 [s], qf = 40 [°]

Solution q
qf
Generic equation: q (t ) = a1t + a0
- Point 1: q0 = a1t0 + a0 16 = 2a1 + a0 q0
- Point 2: q f = a1t f + a0 40 = 10a1 + a0 t0 tf
t

System of equations: Position (q)

 2 1  a1  16 
Ax = b 10 1  a  =  40 
  0   Velocity (𝑞)ሶ

 a1   1  1 −1  16   3 
 a  =  −8  −10 2    40  = 10 
−1
x=A b
 0      
Equation of the line:
q (t ) = 3t + 10 How would acceleration look like?
%% Programmed by Tran Duc Thien
function q = poly1st(t,q0,t0,qf,tf)
b=[q0,qf]';
A=[t0,1;...
tf,1];
x=inv(A)*b;
q=x(1)*t+x(2);
end

28
 Examples

%% Programmed by Tran Duc Thien function [q] = TPp2p(t0,q0,tf,qf,t)

%% Date: 24/11/2023 A=[t0 1;... tf 1];
%% b=[q0;... qf];
clc; x=inv(A)*b;
clear all; q=x(1)*t+x(2);
close all; end
t0=2;q0=16;tf=10;qf=40;
%%
A=[t0 1;... tf 1];
b=[q0;... qf];
x=inv(A)*b;

29
 Examples

function q = TPp2p(t0,q0,tf,qf,t) function [q] = TP3p(t0,q0,t1,q1,tf,qf,t) x=inv(A)*b;

A=[t0 1;... tf 1]; if (t<=t0) q=x(1)*t+x(2);
b=[q0;... qf]; q=q0; else
if (t<=t0) elseif (t<=t1) q=qf;
q=q0; A=[t0 1; t1 1]; end
elseif (t<=tf) b=[q0; q1]; end
x=inv(A)*b; x=inv(A)*b;
q=x(1)*t+x(2); q=x(1)*t+x(2);
else elseif (t<tf)
q=qf; A=[t1 1; tf 1];
end b=[q1; qf];
end

30
 Trajectories in the Joint Space

1. They use each of the joints qi(t) directly

q6final
Example for joint 6

Final q6

q6final

q6init
q6init

Initial
tinit tfinal t

How to go from q6init to q6final?

KUKA’s LBR iiwa

 Trajectories in the Joint Space

1. It uses each joint qi(t) directly

• It can depend on time: qi(t)
• It can be parameterized: qi(λ) where λ = λ(t)
2. The desired values are usually found using specifications in the operational space
3. Specifications (some or all the following):
• Initial and final position
• Initial and final velocity
• Initial and final acceleration
• Intermediate poses (“way points”): interpolation
• Continuity of the curve (up to the k-th order: class 𝒞 𝑘 )
4. Problem:
• Motion in the operational space is not predictable
• There can be “overshooting” in the operational space
 Outline

1. Trajectory Generation
2. Trajectories in the Joint Space
2.1. Point to point trajectories
2.2 Trajectories using waypoints
3. Trajectories in the Operational Space
 Point to Point Trajectories

1. Move from an initial position qi to a final qf in a time tf.

qf
Interpolations
between qi and qf
qi

2. It does not matter:

• Any specific point in the intermediate path between qi and qf
• The path followed by the end effector
3. Examples of interpolation:
Polynomials:
• Cubic
• Fifth grade, etc.
• Interpolation with trapezoidal velocity, minimum time trajectory, etc.
 Point to Point Trajectories

Cubic Polynomial
1. We specify: Analytic expression
• Initial and final time: t0, tf
Position: q (t ) = a3t 3 + a2t 2 + a1t + a0
• Initial and final point: q0, qf
• Initial and final velocity: 𝑞ሶ 0 , 𝑞ሶ 𝑓 Velocity: q (t ) = 3a3t 2 + 2a2t + a1

2. System of equations:
q0 = a3t03 + a2t02 + a1t0 + a0  t03 t02 t0 1   a3   q0 
 3   
q f = a3t 3f + a2t 2f + a1t f + a0  tf t 2f tf 1   a2   q f 
=
q0 = 3a3t02 + 2a2t0 + a1  3t02 2t0 1 0   a1   q0 
 2    
q f = 3a3t 2f + 2a2t f + a1 3t f 2t f 1 0   a0   q f 

3. Note:
• Only (initial/final) positions and velocities can be specified
• Acceleration cannot be specified
 Point to Point Trajectories

Cubic Polynomial
Example
Determine the cubic trajectory of a joint from q(2)=10° to q(4)=60° with null initial and final velocities. Plot the
position, velocity and acceleration.

System of equations:
10 = 23 a3 + 22 a2 + 2a1 + a0 0 = 3(22 )a3 + 2(2)a2 + a1
60 = 43 a3 + 42 a2 + 4a1 + a0 0 = 3(42 )a3 + 2(4)a2 + a1

Solution:

 23 22 2 1   a3  10   a3   −12.5
 3      a  112.5 
 4 42 4 1   a2  60   2 =  
=  a1   −300 
3(22 ) 2(2) 1 
0 a1  0
 2        
3(4 ) 2(4) 1 0   a0   0   0 
a 260 

Trajectory:
q (t ) = −12.5t 3 + 112.5t 2 − 300t + 260
%% Programmed by Tran Duc Thien
function q = QHQD(t,t0,q0,v0,tf,qf,vf)
if (t<=t0)
q=q0;
elseif (t<=tf)
b=[q0,qf,v0,vf]';
A=[t0^3,t0^2,t0,1;...
tf^3,tf^2,tf,1;...
3*t0^2,2*t0,1,0;...
3*tf^2,2*tf,1,0];
x=inv(A)*b;
q=x(1)*t^3+x(2)*t^2+x(3)*t+x(4);
else
q=qf;
end
end

37
 Point to Point Trajectories

Quintic Polynomial (5th degree)

Analytic expression
1. We specify:
Position: q (t ) = a5t + a4t + a3t + a2t + a1t + a0
5 4 3 2
• Initial and final time: t0, tf
• Initial and final point: q0, qf Velocity: q (t ) = 5a5t 4 + 4a4t 3 + 3a3t 2 + 2a2t + a1
• Initial and final velocity: 𝑞ሶ 0 , 𝑞ሶ 𝑓 Acceleration: q (t ) = 20a5t + 12a4t + 6a3t + 2a2
3 2

• Initial and final accel.: 𝑞ሷ 0 , 𝑞ሷ 𝑓

2. System of equations:
q0 = a5t05 + a4t04 + a3t03 + a2t02 + a1t0 + a0
 t05 t04 t03 t02 t0 1   a5   q0 
q f = a t + a t + a t + a t + a1t f + a0
5 4 3 2
 5   
5 f 4 f 3 f 2 f
 tf t 4f t 3f t 2f tf 1   a4   q f 
q0 = 5a t + 4a t + 3a t + 2a2t0 + a1
4
5 0
3
4 0
2
3 0  5t04 4t03 3t02 2t0 1 0   a3   q0 
q f = 5a t + 4a t + 3a t + 2a2t f + a1
4 3 2  4 3   =  
5 f 4 f 3 f  f5t 4 t f 3t 2f 2t f 1 0   a2   q f 
q0 = 20a5t03 + 12a4t02 + 6a3t0 + 2a2  20t 12t 2
3
6t0 2 0 0   a1   q0 
 03 0
   
 20t f 12t f
2
6t f 2 0 0   a0   q f 
q f = 20a5t 3f + 12a4t 2f + 6a3t f + 2a2
 Point to Point Trajectories
Quintic Polynomial (5th degree)
Example
Determine the quintic trajectory of a joint from q(2)=10° to q(4)=60° with null initial and final velocity and
acceleration.

System of equations:

 25 24 23 22 2 1   a5  10 
 5 4 3 2    
 4 4 4 4 4 1   a4  60 
 5(24 ) 4(23 ) 3(22 ) 2(2) 1 0   a3   0 
 4 3 2   =  
 5(4 ) 4(4 ) 3(4 ) 2(4) 1 0   a2   0 
 20(23 ) 12(22 ) 6(2) 2 0 0   a1   0 
    
   a0   0 
3 2
20(4 ) 12(2 ) 6(2) 2 0 0
Trajectory:
q (t ) = 9.375t 5 − 140.625t 4 + 812.5t 3 − 2250t 2 + 3000t − 1540
 Examples

function q = TPp2p(t0,q0,tf,qf,t)
A=[t0^3 t0^2 t0 1;... tf^3 tf^2 tf 1;... 3*t0^2 2*t0 1 0;... 3*tf^2 2*tf 1 0];
b=[q0;... qf; 0 0];
if (t<=t0)
q=q0;
elseif (t<=tf)
x=inv(A)*b;
q=x(1)*t^3+x(2)*t^2+x(3)*t+x(4);
else
q=qf;
end
end

40
 Example

1. Generate trajectory planning between two points A and B. Pass through the Cartesian points A(10,10) with
𝑡𝐴 = 0𝑠, and B(-20,-20) with 𝑡𝐵 = 2𝑠. Velocities at these points are zeros.
2. Generate trajectory planning between two points A and B. Pass through the Cartesian points A(10,10) with
𝑡𝐴 = 2𝑠, and B(-20,-20) with 𝑡𝐵 = 4𝑠. Velocities at these points are zeros.
3. Generate trajectory planning between A, B and C. Pass through the Cartesian points A(10,10) with 𝑡𝐴 = 0𝑠,
B(-20,-20) with 𝑡𝐵 = 2𝑠, C(10,-20) with 𝑡𝐶 = 4s, and A(10,10) with t=6s. Velocities at these points are zeros.
4. Generate trajectory planning between A, B and C. Pass through the Cartesian points A(10,10) with 𝑡𝐴 = 2𝑠,
B(-20,-20) with 𝑡𝐵 = 4𝑠, C(10,-20) with 𝑡𝐶 = 6s, and A(10,10) with t=8s. Velocities at these points are zeros.

9/11/2024 41
 Example

9/11/2024 42
 Example
function [x,y] = TrajectoryPlanning(t,P0,v0,Pf,vf,tf)
a10=P0(1);
a20=P0(2);
a11=v0(1);
a21=v0(2);
a12=3/tf^2*(Pf(1)-P0(1))-2/tf*v0(1)-1/tf*vf(1);
a22=3/tf^2*(Pf(2)-P0(2))-2/tf*v0(2)-1/tf*vf(2);
a13=-2/tf^3*(Pf(1)-P0(1))+1/tf^2*(vf(1)+v0(1));
a23=-2/tf^3*(Pf(2)-P0(2))+1/tf^2*(vf(2)+v0(2));
if (t<=2)
x=a10+a11*t+a12*t^2+a13*t^3;
y=a20+a21*t+a22*t^2+a23*t^3;
Else
x=Pf(1);
Y=Pf(2);
end

9/11/2024 43
 Example

9/11/2024 44
 Example

9/11/2024 45
 Example
function [x,y] = if t<=t0
TrajectoryPlanning(t,P0,v0,t0,Pf,vf,tf) x=P0(1);
a10=P0(1); y=P0(2);
a20=P0(2); elseif t<=tf
a11=v0(1); tt=t-t0;
a21=v0(2); x=a10+a11*tt+a12*tt^2+a13*tt^3;
a12=3/(tf-t0)^2*(Pf(1)-P0(1))-2/(tf-t0)*v0(1)- y=a20+a21*tt+a22*tt^2+a23*tt^3;
1/(tf-t0)*vf(1); else
a22=3/(tf-t0)^2*(Pf(2)-P0(2))-2/(tf-t0)*v0(2)- x=Pf(1);
1/(tf-t0)*vf(2); y=Pf(2);
a13=-2/(tf-t0)^3*(Pf(1)-P0(1))+1/(tf- end
t0)^2*(vf(1)+v0(1));
a23=-2/(tf-t0)^3*(Pf(2)-P0(2))+1/(tf-
t0)^2*(vf(2)+v0(2));

9/11/2024 46
 Example
function [x,y] = x=a10+a11*tt+a12*tt^2+a13*tt^3;
TrajectoryPlanning(t,P0,v0,t0,Pf,vf,tf) y=a20+a21*tt+a22*tt^2+a23*tt^3;
a10=P0(1); end
a20=P0(2);
a11=v0(1);
a21=v0(2);
a12=3/(tf-t0)^2*(Pf(1)-P0(1))-2/(tf-t0)*v0(1)-
1/(tf-t0)*vf(1);
a22=3/(tf-t0)^2*(Pf(2)-P0(2))-2/(tf-t0)*v0(2)-
1/(tf-t0)*vf(2);
a13=-2/(tf-t0)^3*(Pf(1)-P0(1))+1/(tf-
t0)^2*(vf(1)+v0(1));
a23=-2/(tf-t0)^3*(Pf(2)-P0(2))+1/(tf-
t0)^2*(vf(2)+v0(2));

9/11/2024 47
Example

48
 Example

5 times
A B A 5 times B
2 Seconds 2 Seconds

9/11/2024 49
 Example
function [x,y] = TrajectoryPlanning(t,P0,v0,Pf,vf,tf)
a10=P0(1);
a20=P0(2);
a11=v0(1);
a21=v0(2);
a12=3/tf^2*(Pf(1)-P0(1))-2/tf*v0(1)-1/tf*vf(1);
a22=3/tf^2*(Pf(2)-P0(2))-2/tf*v0(2)-1/tf*vf(2);
a13=-2/tf^3*(Pf(1)-P0(1))+1/tf^2*(vf(1)+v0(1));
a23=-2/tf^3*(Pf(2)-P0(2))+1/tf^2*(vf(2)+v0(2));
x=a10+a11*t+a12*t^2+a13*t^3;
y=a20+a21*t+a22*t^2+a23*t^3;
end

9/11/2024 50
 Point to Point Trajectories

Trapezoidal Velocity Interpolator

1. Also known as:
“Linear segments with Parabolic Blends” (LSPB)
qf
q (t ) q (t ) qmax

tb: blend time

q0
• Assumptions: tb t f − tb t f tb t f − tb tf
• We assume: 𝑞ሶ 0 = 𝑞ሶ 𝑓 = 0 (null initial and final velocities)
• (Symmetric trajectory: q(tf /2) = qf /2)
2. We specify:
• Final time: tf
• Initial and final point: q0, qf
• Maximum velocity: 𝑞ሶ 𝑚𝑎𝑥 , or tb, or maximum acceleration 𝑞ሷ 𝑚𝑎𝑥
 Point to Point Trajectories

Trapezoidal Velocity Interpolator

1. Trajectory when specifying the maximum velocity:
q0 − q f + qmax t f
 1 qmax 2 tb =
q +
 0 2 t t , 0  t  tb qmax
 b

 1 qmax
q (t ) =  q0 − tb qmax + qmax t , tb  t  t f − tb
 2
 1 qmax tb t f − tb
 q f − (t − t f ) 2
, t f − tb  t  t f tf
 2 tb

2. Limits of the maximum velocity (0 < tb ≤ tf /2):

q f − q0 2(q f − q0 ) qmax
 qmax 
tf tf
t f − tb
3. Maximum acceleration and maximum velocity: tb
q
qmax = max −qmax
tb
 Point to Point Trajectories
Trapezoidal Velocity Interpolator
Example
For a joint, determine its trajectory with trapezoidal velocity such that the initial joint value be 10, the
final value be 60, the maximum velocity 20 deg/s and the total time be 4 s.
Blend time:
tb = (10 − 60 + 20(4)) / 20 = 1.5 s

Trajectory:

 10 + 6.67t 2 , 0  t  1.5

q (t ) =  −5 + 20t ,1.5  t  2.5
60 − 6.67(t − 4) 2 , 2.5  t  4

 Point to Point Trajectories

Minimum Time Trajectory

1. Objective: move from q0 to qf in the minimum possible time, respecting the maximum
acceleration 𝑞ሷ 𝑚𝑎𝑥
2. How?
• Maximum acceleration and then maximum
deceleration (on/off or bang/bang)
ts tf
3. Result:
• “Collapsed” trapezoidal velocity → triangular
tf
• Switch time: b s
t = t =
2 q f − q0
• With this constraint (using the relations of the trapezoidal interpolator): t f = 2
qmax
 1
 q0 + qmax t 2
, 0  t  ts
2
q (t ) = 
 q − 1 q (t − t ) 2 , ts  t  t f
 f 2 max f
 Point to Point Trajectories

Minimum Time Trajectory

Example
For a given joint, determine the minimum time to move it from 10° to 60° if the maximum acceleration
is 13.33 deg/s2. Also, find the resulting trajectory.

Final time:
60 − 10
tf = 2 = 3.8735 s
13.33

Trajectory:

 10 + 6.67t 2 , 0  t  1.94
q (t ) = 
60 − 6.67(t − 3.87) ,1.94  t  3.87
2
 Outline

1. Trajectory Generation
2. Trajectories in the Joint Space
2.1. Point to point trajectories
2.2 Trajectories using waypoints
3. Trajectories in the Operational Space
 Trajectories using waypoints

1. Move from an initial position qi to a final qf “passing by” intermediate points (waypoints)

qf

qi

2. We are interested in:

• Waypoints between qi and qf
• Continuity (of velocity/acceleration) in the waypoints
3. Uses:
• To avoid obstacles
• To follow “fine” trajectories
 Trajectories using waypoints

Methods
1. High order polynomials
If we have N points, we can use a polynomial of degree N - 1
Problems?
• Oscillating trajectory (more oscillations as we increase the degree)
• We cannot set initial and final velocities
2. Low-order polynomials in every segment
Example:
• Cubic polynomials (positions+velocities)
• Quintic polynomials (positions+velocities+accelerations)
Possibilities:
• Each segment on its own
• Interdependent segments (splines)
 Trajectories using waypoints

4-3-4 Polynomial Interpolation

1. Trajectory has:
qf
• Initial part: 4th order
q2
• Intermediate part: 3rd order
• Final part: 4th order q1
q0
2. How many coefficients?
t0 t1 t2 tf
• 14 coefficients need to be determined
We specify: They are used for “pick and
place” operations
• Joint positions: q0, q1, q2, qf
• Times: t0, t1, t2, tf
• Null initial and final velocities and accelerations, and continuity
− + − +
Constraints: q(t0 ) = q0 , q(t f ) = q f , q(t1 ) = q(t1 ) = q1 , q(t2 ) = q(t2 ) = q 2
q (t0 ) = 0, q (t f ) = 0
+ 4 continuity constraints of velocity
q (t0 ) = 0, q (t f ) = 0 and acceleration in t1 and t2
 Trajectories using waypoints

Polynomials Specifying Velocity

1. Given N points, we use N-1 cubic polynomials (one per segment)
2. For every point, we specify
• Position: qk , with k = 1, , N
• Velocity: qk , with k = 1, , N
3. For every cubic polynomial:
• Notation: k-th polynomial represented as  k (t ) = a3t 3 + a2t 2 + a1t + a0
• Imposed conditions:

 k (tk ) = qk
For velocity
 k (tk +1 ) = qk +1 4 conditions to continuity
 k (tk ) = qk
compute the k-th  k (tk +1 ) =  k +1 (tk +1 )
cubic polynomial
 k (tk +1 ) = qk +1
 Trajectories using waypoints

Polynomials Specifying Velocity

Example
Trajectory passing through the points q1 = 0, q2 = 2π, q3 = π/2, q4 = π, at times t1 = 0, t2 = 2, t3 = 3, t4 =
5, with velocities 𝑞ሶ 1 = 0, 𝑞ሶ 2 = π, 𝑞ሶ 3 = -π, 𝑞ሶ 4 = 0.

We pass through the desired points in position and velocity

Acceleration is not continuous (Why?)
 Trajectories using waypoints

Cubic “Splines”
1. Spline: smooth curve (in this case, 𝒞 2 [continuity up to the 2nd derivative]) that joins N points
2. How?
• N-1 cubic polynomials concatenated to pass through N points
• Continuity in velocity and acceleration every N-2 internal points

qN −1
qN
qk +1  N −1

qk
k
q2
q1 1

t1 t2 tk tk +1 t N −1 tN

Notation: k-th polynomial represented as Θk

 Trajectories using waypoints

Cubic “Splines”
1. How many coefficients need to be determined?
• 4 (N-1) coefficients (4 per polynomial)
2. How many imposed constraints?
• 2(N-2): continuity in velocity and acceleration of internal points
 k (tk ) =  k +1 (tk )  k (tk ) =  k +1 (tk )
• 2(N-1): waypoints for every polynomial
 k (tk ) = qk  k (tk +1 ) = qk +1
3. How many free parameters?
• 2: we can specify initial and final velocity (𝑞ሶ 1 , 𝑞ሶ 𝑁 )
1 (t1 ) = q1  N −1 (t N ) = qN
4. With these constraints we can determine every cubic polynomial
 Trajectories using waypoints

Cubic “Splines”
1. Properties of splines
• It is the curve with minimum possible curvature (for 𝒞 2 functions)
• A spline is uniquely determined when we specify: q1, …, qN, t1, … tN, 𝑞ሶ 1 , 𝑞ሶ 𝑁
• They do not allow us to specify the initial or final acceleration

2. How to specify the initial and final acceleration?

Add 2 “fictitious” (“virtual”) points at the beginning and at the end
• 2 extra cubic polynomials
• 8 extra coefficients to determine
In these “fictitious” points we must impose:
• Continuity in position, velocity and acceleration (2x3=6 constraints)
Use the free parameters to:
• Initial and final acceleration (2 constraints)
 Trajectories using waypoints

Cubic “Splines”
Example
Find a spline that passes through the points q1 = 0, q2 = 2π, q3 = π/2, q4 = π, at times t1 = 0, t2 = 2, t3 =
3, t4 = 5, considering null initial and final velocities.
To obtain a null acceleration, use fictitious points in a) ta = 0.5, tb = 4.5, y b) ta = 1.5, tb = 3.5
 Trajectories using waypoints

Parabolic Blending
1. Parts:
Linear interpolation in the “intermediate” part
Smooth “intersections” using parabolas

parab

parab

parab
 Trajectories using waypoints

Parabolic Blending
Example
Generate a trajectory with parabolic blending that passes through the points q1 = 0, q2 = 2π, q3 = π/2,
q4 = π, at times t1 = 0, t2 = 2, t3 = 3, t4 = 5, with null initial and final velocities. Consider two cases for
the durations of the parabolas (blending time): 0.2 s y 0.6 s

Note: blending time = 0.2 (solid line), 0.6 (dashed line)

 Outline

1. Trajectory Generation

2. Trajectories in the Joint Space

2.1. Point to point trajectories

2.2 Trajectories using waypoints

3. Trajectories in the Operational Space

 Trajectories in the Operational Space

1. They are used to follow a path that is geometrically specified (line, circle, etc.)
2. In general:
• We can apply the same interpolation methods used in the joint space
• Each position (x, y, z) and minimal representation of orientation (φr, φp, φy) is independently
considered
3. Problems with orientation:
• When interpolating, the result cannot be intuitively visualized
• We prefer to deal with position and orientation separately
4. Number of waypoints is typically low
• Usually simple paths: lines, circular arcs, etc.
5. They always need inverse kinematics
 Trajectories in the Operational Space

1. Some Problems

Initial and final point are Initial and final points reachable in Singularities cause high
reachable. Intermediate point different configurations velocities
is not reachable
 Trajectories in the Operational Space

Linear Cartesian Path

1. Line segment from pi to pf
pi
2. Parameterization of the path: p( s ) = pi + s ( p f − pi ), s  [0,1] L = p f − pi
pf
• Path length:  = Ls
3. Velocity and acceleration:
dp p − pi
p( s) = s = ( p f − pi ) s = f 
ds L
dp p f − pi
p( s) = s = ( p f − pi ) s = 
ds L
We can use σ to determine a temporal profile
 Trajectories in the Operational Space

Linear Cartesian Path

1. Example of temporal profile of σ: using trapezoidal velocity
Given: L, vmax, amax
amax Trajectory:
 1
 amax t 2 , 0  t  tb
vmax  2
 2
1 vmax
 (t ) =  vmax t − , tb  t  t f − tb
 2 amax
 1 2
vmax
− amax (t − t f ) + vmax t f −
2
, t f − tb  t  t f
 2 amax

where
tb t f − tb t f v Lamax + vmax
2
tb = max tf =
amax amax vmax
 Trajectories in the Operational Space

Concatenation of linear paths

1. Move from an initial point to a final point passing by an intermediate point
 Trajectories in the Operational Space

Orientation Trajectory
1. How to interpolate orientation?

{A} {B}

2. Alternative 1:
• Using a minimal representation for orientation (Euler angles)
Example: linear path in space

 ( s ) = i + s ( f − i ), s  [0,1] For every joint

Problem:
• Difficult interpretation/prediction of intermediate orientations
• The system can move in an “unpredictable” way
 Trajectories in the Operational Space

Orientation Trajectory
1. Alternative 2:
• Using the axis/angle representation
Procedure:
• Determine the rotation that starts at {A} and reaches {B}: R = ( 0 RA ) ( 0 RB )
T

• Determine axis r and angle θAB for R

• Assign a temporal law to θ(t) that interpolates from θ = 0 to θ = θAB (with possible
conditions on the derivatives)
• For every t, the orientation of the end effector will be:
0
RA R(r ,  (t ))
2. Alternative 3:
• Using quaternions (“slerp” interpolation)
 Exercises

John J. Craig, Introduction to Robotics: Mechanics and Control (3rd Edition),

Chapter 7: Trajectory generation
• 7.2, 7.3, 7.6, 7.8, 7.9, 7.10, 7.18, 7.19, 7.20
• MATLAB Exercise 7 (a, c)

76
 References

• B. Siciliano, L. Sciavicco, L. Villani, y G. Oriolo. Robotics: modelling, planning and control. Springer
Science & Business Media, 2010 (Chapter 4)
• M.W. Spong, S. Hutchinson, y M. Vidyasagar. Robot Modeling and Control. John Wiley & Sons,
2006 (Chapter 5)
Thank you for your listening

78