2nd International Summer School on Geometric Methods in Robotics, Mechanism Design and Manufacturing Research-Lecture 05 Trajectory Generation

1. Chapter 5 Trajectory Generation
1
Chapter Lecture Notes for
Trajectory
Generation
A Geometrical Introduction to
Introduction
to Trajectory
Generation
Robotics and Manipulation
Joint Space
Trajectory Richard Murray and Zexiang Li and Shankar S. Sastry
Generation
( -D) CRC Press
Zexiang Li and Yuanqing Wu
ECE, Hong Kong University of Science & Technology
July ,

2. Chapter 5 Trajectory Generation
2
Chapter
Trajectory
Generation
Chapter 5 Trajectory Generation
Introduction
to Trajectory
Generation
Joint Space Introduction to Trajectory Generation
Trajectory
Generation
( -D)
Joint Space Trajectory Generation ( -D)

3. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
3
◻ Motion planning and Trajectory generation:
Chapter
Trajectory
Generation Definition: Motion planning
Introduction Given the initial and goal config.s
to Trajectory ginit , ggoal , find a collision-free path,
g ∶ [ , ] ↦ SE( ), such that g( ) = ginit
Generation
and g( ) = ggoal .
Joint Space
Trajectory
Generation
( -D) Play/Pause Stop

4. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
3
◻ Motion planning and Trajectory generation:
Chapter
Trajectory
Generation Definition: Motion planning
Introduction Given the initial and goal config.s
to Trajectory ginit , ggoal , find a collision-free path,
g ∶ [ , ] ↦ SE( ), such that g( ) = ginit
Generation
and g( ) = ggoal .
Joint Space
Trajectory
Generation
( -D) Play/Pause Stop
Let gk = g(kT), k = , . . . , n, where T = n , and θ k ∈ Q the corresponding
joint configuration, through the inverse kinematics.
θ k , k = , . . . , n with θ = θ init and θ n = θ goal , can also be obtained
through a teaching pendant
(Continues next slide)

5. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
4
Definition: Trajectory generation
Given θ init and θ goal , and a sequence of via points θ k , k = , . . . , n − ,
compute a joint trajectory θ ∶ [t , tn ] ↦ Q such that θ(t ) = θ init ,
Chapter
Trajectory
θ(tn ) = θ goal , and θ(tk ) = θ k , k = , . . . , n − .
Generation
Introduction
to Trajectory
Generation
Joint Space
Trajectory
Generation
( -D)

6. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
4
Definition: Trajectory generation
Given θ init and θ goal , and a sequence of via points θ k , k = , . . . , n − ,
compute a joint trajectory θ ∶ [t , tn ] ↦ Q such that θ(t ) = θ init ,
Chapter
Trajectory
θ(tn ) = θ goal , and θ(tk ) = θ k , k = , . . . , n − .
Generation
Introduction
to Trajectory
Generation valid region
Joint Space
Trajectory forbidden region
valid configurations
Generation
( -D) forbidden region
forbidden configurations
Motion planning
of via points in config. space C
C = Rn , SO(n), SE(n), . . .
obstacle avoidance
potential field/voronoi dia-
gram/visibility graph methods,
etc

7. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
4
Definition: Trajectory generation
Given θ init and θ goal , and a sequence of via points θ k , k = , . . . , n − ,
compute a joint trajectory θ ∶ [t , tn ] ↦ Q such that θ(t ) = θ init ,
Chapter
Trajectory
θ(tn ) = θ goal , and θ(tk ) = θ k , k = , . . . , n − .
Generation
Introduction
to Trajectory
Generation valid region
Joint Space
Trajectory forbidden region
valid configurations
Generation
( -D) forbidden region
Joint Space Trajectory Planning
forbidden configurations Workspace Trajectory Planning
Motion planning Trajectory Generation
of via points in config. space C Interpolation (exact fitting)
C = Rn , SO(n), SE(n), . . . Approximation (inexact fitting)
obstacle avoidance of via points
potential field/voronoi dia- with appropriate basis functions
gram/visibility graph methods, satisfying certain dynamic con-
etc straints

8. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
5
Config. space C, obstacles, constraints;
initial, goal config. g , g n ∈ C
Chapter Path planning
Trajectory
Generation
Computed via points:
Introduction g , g , . . . , gn ∈ C
to Trajectory Config. space traj. planning
Generation Joint traj. planning
Inverse kinematics
Joint Space
Via points in joint space: Interp./approx. in N-D space:
Trajectory
Generation θ , θ , . . . , θn ∈ Θ C = Rn , SO(n), SE(n)
( -D)
Online generation of g(t), g (t), . . .
˙
Interp./approx. of each joint ×N
˙
and computation of θ(t), θ(t), . . . .
Trajectory tracking control
(Continues next slide)

9. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
6
◻ Notes on assigning the via points:
1. Via points assigned by path planner
Chapter Type of path planner:
Trajectory
Generation
Visibility graph
Introduction
to Trajectory
Generation
Voronoi diagrams
Joint Space Approximate Cell decomposition
Trajectory
Generation
( -D) Numerical Potential field
ggoal V(S)
w
DT(S) s
p r
v
ginit
q
⇒ Usually generate a sequence of via-points rather than a smooth
Visibility graph Voronoi diagrams Cell decomposition Potential field
trajectory. (Continues next slide)

10. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
7
2. Via Points generated by a teaching pendant
via points recorded as joint angles, no inverse kinematics required
Chapter
Trajectory
Generation 3. Via points assigned by a sequence of end-effector
Introduction configurations (such as G-code for machine tools)
to Trajectory
Generation
inverse kinematics required for planning joint velocity profile
Joint Space
constraints from both joint and workspace need be considered
Trajectory
Generation
( -D)

11. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
7
2. Via Points generated by a teaching pendant
via points recorded as joint angles, no inverse kinematics required
Chapter
Trajectory
Generation 3. Via points assigned by a sequence of end-effector
Introduction configurations (such as G-code for machine tools)
to Trajectory
Generation
inverse kinematics required for planning joint velocity profile
Joint Space
constraints from both joint and workspace need be considered
Trajectory
Generation
( -D) ◻ Issues on trajectory design:
Dynamic constraints:
Continuity in velocity, acceleration, jerk, etc (trapezoidal,
s-curve, squared sine,. . . )
Velocity limits θ i (t) ≤ θ max (saturated velocity)
˙ ˙i
Acceleration limits θ i (t) ≤ θ max (rated torque)
¨ ¨i
(Continues next slide)

12. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
8
Performance requirements:
minimal time trajectory (achieved with maximal
Chapter
Trajectory
velocity/acceleration)
...i ...i
Bounded Jerk trajectory (avoiding excitation): θ (t) ≤ θ max
Generation
Introduction
to Trajectory
Generation Least square error
Joint Space
Trajectory
Generation
( -D)

13. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
8
Performance requirements:
minimal time trajectory (achieved with maximal
Chapter
Trajectory
velocity/acceleration)
...i ...i
Bounded Jerk trajectory (avoiding excitation): θ (t) ≤ θ max
Generation
Introduction
to Trajectory
Generation Least square error
Joint Space
Trajectory
Choice of parameterization:
Generation The trajectory θ(u) can be parameterized by a parameter u other
( -D)
than time t (e.g. u(t) length of the traj.)
˙ dθ = dθ du
θ=
dt du dt
where v(t) ≜ du
dt
is the feedrate

14. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
8
Performance requirements:
minimal time trajectory (achieved with maximal
Chapter
Trajectory
velocity/acceleration)
...i ...i
Bounded Jerk trajectory (avoiding excitation): θ (t) ≤ θ max
Generation
Introduction
to Trajectory
Generation Least square error
Joint Space
Trajectory
Choice of parameterization:
Generation The trajectory θ(u) can be parameterized by a parameter u other
( -D)
than time t (e.g. u(t) length of the traj.)
˙ dθ = dθ du
θ=
dt du dt
where v(t) ≜ du is the feedrate
dt
Online traj. generation (solving differential/difference
equations):
u((k + )T) = u(kT) + T ⋅ v(kT), T sampling time

15. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
9
◻ Classification of Trajectories:
Trajectory
Chapter
Trajectory
Generation One-dimensional Multi-dimensional
Introduction
to Trajectory Multipoint
Point-to-point Multipoint Point-to-point
Generation
Joint Space
Trajectory Interpolation Approximation Interpolation Approximation
Generation
( -D)

16. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
9
◻ Classification of Trajectories:
Trajectory
Chapter
Trajectory
Generation One-dimensional Multi-dimensional
Introduction
to Trajectory Multipoint
Point-to-point Multipoint Point-to-point
Generation
Joint Space
Trajectory Interpolation Approximation Interpolation Approximation
∎ -D v.s. N-D
Generation
( -D)
-D: scalar function (e.g., single joint variable)
N-D: vector function (e.g., position, orientation, rigid config.)
g0
g1
inverse
g2
......
kinematics →
g3
→
gn
Joint Space Trajectory Planning
sequence of end-effector config.s sequence of joint variables -D traj. planning ×n
(Continues next slide)

17. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
10
◻ Interpolation v.s. approximation:
Chapter
Trajectory
Generation
Introduction
to Trajectory
Generation
Joint Space
Trajectory
Generation
( -D)
interpolation approximation
Definition: Interpolation
Constructing new data points within the range of a discrete set of
known data points (exact fitting).

18. Chapter 5 Trajectory Generation
5.1 Introduction to Trajectory Generation
10
◻ Interpolation v.s. approximation:
Chapter
Trajectory
Generation
Introduction
to Trajectory
Generation
Joint Space
Trajectory
Generation
( -D)
interpolation approximation
Definition: Interpolation
Constructing new data points within the range of a discrete set of
known data points (exact fitting).
Definition: Approximation
Inexact fitting of a discrete set of known data points.
† End of section †

19. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
11
Problem 1: -D Point-to-point Trajectory Generation
boundary condition: θ(t ), θ(t ), θ(t ), θ(t ), . . .
˙ ˙
Chapter Solution: a parametric curve θ(t), t ∈ [t , t ]
Trajectory
Generation
Introduction
to Trajectory
Generation
Joint Space
Trajectory
Generation
( -D)

20. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
11
Problem 1: -D Point-to-point Trajectory Generation
boundary condition: θ(t ), θ(t ), θ(t ), θ(t ), . . .
˙ ˙
Chapter Solution: a parametric curve θ(t), t ∈ [t , t ]
Trajectory
Generation
Introduction
Solution 1: Polynomial curves:
to Trajectory
Generation θ(t) = a + a t + a t + a t + ⋯ + an t n , t ∈ [t , t ]
Joint Space Basis function: {t i }
n + Boundary conditions: θ (k) (tj ), k = , , . . . , j = ,
Trajectory
Generation
( -D)
⇒ k!ak + (k + )!ak+ tj + ⋯ + an t n−k = θ (k) (tj )
n!
(n − k)! j
Ma = b
where
⎡ ⎤ m , ⋯ m ,n
a ⎢ ⎥
a= ∈ Rn+ , b = ⎢ θ (k) (tj ) ⎥ ∈ Rn+ , M = ⋱ ∈ R(n+ )×(n+ )
⎢ ⎥ mn, ⋯ mn,n
an ⎣ ⎦
⇒ a = M− b

21. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
12
n = : Linear trajectory (constant velocity)
θ(t) = a + a (t − t )
Chapter
Trajectory θ
Generation
Boundary condition: θ , θ
Introduction
θ(t ) = θ = a
to Trajectory
⇒
Generation t
Joint Space
Trajectory .
˙
θ θ(t ) = θ = a + a (t − t )
= θ ⇒
Generation .
a
t −t
.
( -D) .
. a θ
θ −θ
a = θ ,a =
t
.
¨
θ t −t
.
.
− .
Disadvantage:
−
t infinite acceleration at t , t
Example: Let t = , t = , θ = , θ =

22. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
13
n = : Cubic trajectory
θ
˙ ˙
Boundary condition: θ , θ , θ , θ
θ
Chapter
Trajectory
Generation
θ(t) = a + a (t − t ) + a (t − t ) + a (t − t )
θ
⎧ θ(
⎪ )=a =θ
Introduction t t ⎪
⎪
⎪
⎪
t
to Trajectory
⎪
⎪˙
⎪ θ(
Generation ˙
θ ⎪
⎪
⎪ )=a =θ
˙
⎨ ⇒
Joint Space
⎪ θ(
⎪
⎪ )=a +a +a +a = θ
Trajectory ⎪
⎪
⎪
˙ ˙ ⎪
⎪
⎪ ˙
⎪ θ(
Generation θ θ
( -D)
t t t
⎪
⎩ )=a + a + a =θ
˙
⎡ ⎤⎡ ⎡ ⎤
⎢ ⎥⎢ ⎤ ⎢ θ ⎥
⎥ ⎢ ⎥
a
⎢ ⎥⎢
¨
θ
⎢ ⎥⎢ ⎥=⎢ ⎥⇒
a ˙
θ
⎢ ⎥⎢ ⎥ ⎢ ⎥
⎢ ⎥⎣
a ⎥ ⎢ θ ⎥
⎣ ⎦ ⎦ ⎢ ⎥
⎣ ⎦
a ˙
θ
⎧a
⎪ =θ
t t t
⎪
⎪
⎪
⎪
⎪
⎪
⎪a
⎪
⎪
⎪ =θ
˙
⎨
⎪a
⎪
⎪ = − θ − θ + (θ − θ )
⎪
⎪
˙ ˙
⎪
⎪
⎪
⎪
⎪
⎪a
⎩ = θ + θ − (θ − θ )
˙ ˙

23. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
14
Chapter
Extension to multi-points: (figure here)
ai + ai (t − ti ) + ai (t − ti ) + ai (t − ti ) , t ∈ [ti , ti+ ]
Trajectory
Generation
Introduction
to Trajectory
Generation
˙
θ : assigned
⎧
⎪
˙ ⎪ sign(dk ) ≠ (dk+ )
Joint Space
θk = ⎨
Trajectory
⎪ (dk + dk+ ) otherwise
Generation
( -D)
⎪
⎩
˙
θ n : assigned
Fig 2.8
Feature:
Continuous position and velocity profiles
Discontinuous acceleration

24. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
15
n = : Quintic polynomial
θ(t) = ai (t − t )i , t ∈ [t , t ]
Chapter
10
Trajectory
8
Generation
i=
Position
6
4
Introduction
to Trajectory
2
0
Advantage:
Generation
can achieve continuity of acceleration
2.5
2
when composing two quintic curves
Velocity
1.5
Joint Space
1
Trajectory 0.5
Generation 0 Boundary
( -D) 1
˙ ˙ ¨ ¨
condition: θ , θ , θ , θ , θ , θ
0.5
Acceleration
0
−0.5
−1
0 1 2 3 4 5 6 7 8
(Continues next slide)

25. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
16
Define T ≜ t − t , h ≜ θ − θ , then:
Chapter
a =θ
Trajectory
Generation
a =θ
˙
Introduction
to Trajectory
Generation
Joint Space a = θ
¨
Trajectory
a = [ h−( θ + θ )T − ( θ − θ )T ]
Generation
( -D) ˙ ˙ ¨ ¨
T
a = [− h − ( θ +
˙ θ )T − ( θ − θ )T ]
˙ ¨ ¨
T
a = [ h − (θ + θ )T − (θ − θ )T ]
˙ ˙ ¨ ¨
T
(Continues next slide)

26. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
17
◻ Extension to multi-point trajectories:
Comparison of cubic and quintic segments
Chapter
Trajectory
Generation 40 40
30 30
Introduction
Position
Position
20 20
to Trajectory
Generation 10 10
0 0
Joint Space
Trajectory 10
10
Generation 5
Velocity
Velocity
( -D) 0 0
−5
−10
−10
−15 −20
20 20
Acceleration
Acceleration
0 0
−20 −20
0 2 4 6 8 10 0 2 4 6 8 10
composition of cubic polynomials: without accel- composition of quintic polynomials: with acceler-
eration continuity ation continuity

27. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
18
n = : Linear Trajectory with Parabolic Blend (Trapezoidal)
Chapter
θ Description:
Trajectory θf
Generation θ
Composition of an acc. (quadratic)
Introduction θ phase, a linear phase, and a dec.
θ
to Trajectory (quadratic) phase, with C continuity.
Generation t t t tf t
˙
θ ˙ ˙
Joint Space
Boundary condition: θ , θ f , θ , θ f
Trajectory ˙
θ max Define acceleration phase Ta , constant
Generation
˙
θ ˙
θf
˙
velocity θ v
( -D)
tf t
⎧θ + θ v (t − t )
t t t
⎪ t ≤ t < t + Ta
¨
⎪
θ maxθ
¨
⎪
˙
⎪
⎪
Ta
θ(t) = ⎨θ + θ v t − t −
˙ t + Ta ≤ t < tf − Ta
⎪
Ta
t
⎪
⎪
⎪θ f − θ v (t − t)
⎪
⎩ tf − Ta ≤ t ≤ tf
t t t tf ˙
... Ta
θ
(Continues next slide)
t t t tf t

28. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
19
◻ Trapezoidal trajectory with preassigned acceleration and
velocity:
⎧T = θ max
Chapter
⎪ a θ
⎪
˙
⎪
Trajectory
⎪
acceleration phase
⎪˙
Generation ¨ max
⎨θ max (T − Ta ) = θ f − θ ≜ h displacement
⎪
Introduction
⎪
⎪T = h θ max +θ max
⎪
to Trajectory
⎪
¨ ˙
⎩
Generation
¨ ˙
θ max θ max
total duration
Joint Space
If h ≥
Trajectory ˙
θ max
Generation
( -D) ¨
θ max
, linear segment exists:
⎧θ + θ (t − t )
⎪ t ≤ t ≤ t + Ta
⎪
¨max
⎪
⎪
θ(t) = ⎨θ + θ max Ta t − t −
¨ Ta
t + Ta < t ≤ t − Ta
⎪
⎪
⎪θ − θ max (t − t)
⎪ f
⎩
¨ t − Ta < t ≤ t
Otherwise:
(Continues next slide)

29. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
20
⎧
⎪Ta = ¨ h
Chapter
⎪
⎪
⎪
Trajectory
acceleration phase
⎪ θ max
Generation
⎨T = Ta
⎪
⎪˙
total time
⎪
Introduction
⎪θ max = θ max Ta =
⎪ θ max h =
to Trajectory
⎩
Generation ¨ ¨ h
maximum velocity
Ta
Joint Space
⎧
and
⎪θ + θ max (t − t )
Trajectory
⎪ ¨ t ≤ t ≤ t + Ta
Generation
θ(t) = ⎨
( -D)
⎪θ f − θ max (tf − t)
⎪ tf − T a < t ≤ tf
⎩
¨

30. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
21
n = : Double “S”trajectory (7-segment)
Chapter
θ
Trajectory θ Advantage:
Generation
Bounded jerk avoids excitation
Introduction
to Trajectory θ from jerk impulse.
Generation
˙
t t t t t t t t t Description of the “S”curve:
θ
Joint Space ˙
θ max Composition of acc. (cubic),
Trajectory
Generation constant vel. (quadratic) and dec.
( -D) (cubic), with C continuity at via points.
t t t t t t t t t
Acc.: t ∈ [t , t ]
¨
θ
¨
θ max
max vel.: t ∈ [t , t ]
Dec.: t ∈ [t , t ]
... t t t t t t t t t
θ
...
θ max
(Continues next slide)
t t t t t t t t t

31. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
22
Assumption:
... ...
θ max = −θ min , θ max = −θ min , θ max = − θ min , t =
˙ ˙ ¨ ¨
then T = T , T = T . Also assume θ = θ f = to avoid end point
Chapter
Trajectory ¨ ¨
Generation
jerk impulse.
Introduction
to Trajectory
˙
Boundary condition: θ , θ , θ f , θ f ˙
...
Generation
Given θ max , θ max , θ max , and define Tk ≜ tk+ − tk . Consider the limit
˙ ¨
Joint Space
Trajectory case of a single acc/dec. phase (t ∈ [t , t ])
1 θ f is reached before θ could reach θ max (T = , T = T )
Generation
( -D)
˙ ¨ ¨
... ... ...
θf − θ =
˙ ˙ θ max T + θ max T = θ max T
acceleration deceleration
¨
2 θ max is reached before deceleration
¨
T = ...
θ max
θ max
(Continues next slide)

32. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
23
⎧ ⎫
Thus
⎪
⎪
⎪ θf − θ
˙ ˙ θ ¨max ⎪
⎪
⎪
T = min ⎨ , ... ⎬
⎪ θ max θ max ⎪
...
Chapter
⎪
⎪ ⎪
⎪
Trajectory
⎩ ⎭
The trajectory is feasible in the first case if (recall that T = T ):
Generation
Introduction
to Trajectory τ ...
θf − θ > ( θ max t) dt + θ dτ+
T
Generation
Joint Space
∫ ∫ ˙
Trajectory
...
( θ max (T − t)) dt + θ + T
T τ
Generation
( -D) ∫ ∫ ˙ dτ
= T (θ f + θ )
˙ ˙
For the second case,
T ⋅ θ max + ⋅ θ max T = θ f − θ ⇒
¨ ¨ ˙ ˙
θf − θ
˙ ˙
T = −T
¨
θ max
(Continues next slide)

33. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
24
then the second case is feasible if:
¨
θf − θ > dt + θ
T τ θ max t
Chapter
Trajectory
∫ ∫ T
˙ dτ+
Generation
θ max dt + θ + θ max T
T τ
Introduction
to Trajectory ∫ ∫ ¨ ˙ ¨ dτ+
θ max (T − t)
Generation
¨
dt + θ + θ max T + θ max T
T τ
Joint Space
Trajectory
Generation
∫ ∫ T
˙ ¨ ¨ dτ
˙ ⎛ θf − θ ⎞
( -D)
˙ ˙
= (θ f + θ ) T +
˙
⎝ θ max ⎠
¨
maximal velocity limit: Also has two cases:
Case 1: If θ max is reached, T ≠
˙
...
if (θ max − θ )... max < θ max ⇒ θ max is not reached
˙ ˙ θ ¨ ¨ (∗)
if (θ˙max − θ f ) θ max < θ
˙ ¨
max ⇒ θ max is not reached (∗∗)
¨
(Continues next slide)

34. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
25
If (∗) holds, then:
θ max − θ
˙ ˙
T =T = ... ,T =
Chapter θ max
Trajectory
Generation otherwise
¨ θf − θ
˙ ˙
T = T = ... ,T = −T
Introduction θ max
to Trajectory θ max θ¨max
Generation
If (∗∗) holds, then:
Joint Space
Trajectory θ max − θ f
˙ ˙
Generation T =T = ... ,T =
( -D) θ max
otherwise
¨ θf − θ
˙ ˙
T = T = ... ,T = −T
θ max
θ max θ¨max
Finally
θf − θ T +T +T ˙ T +T +T ⎛ θf ⎞
˙
T = − + − +
θ
˙
θ max ˙
θ max ⎝ θ max ⎠
˙
If T ≥ , this verifies that θ max is actually reached. Otherwise, θ max is not
˙ ˙
reached must be checked.

35. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
26
Example: Examples of Case 1 and Case 2
Chapter 10 10
Trajectory Position 8 8
Position
Generation 6 6
4 4
Introduction 2 2
0 0
to Trajectory
Generation ˙ 5
θ max ˙ 10
θ max
5
Joint Space
Velocity
Velocity
0 0
Trajectory
−5
Generation
−5 −10
( -D)
¨ 10
θ max ¨ 10
θ max
Acceleration
Acceleration
5 5
0 0
−5 −5
−10 −10
... ...
θ max θ max
20 20
Jerk
Jerk
0 0
−20 −20
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2
˙
Case 1:θ max is reached ˙
Case 2:θ max is not reached

36. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
27
◻ Computation of the double S trajectory (θ f > θ ):
Chapter ˙
θ lim maximal velocity
Trajectory ¨
θ lima maximal acceleration in the acceleration phase
Generation
¨
Notations θ limd maximal acceleration in the deceleration phase
Ta ≜ T + T + T acceleration phase
Introduction
Ta
Ta ≜ T + T + T deceleration phase
to Trajectory
Td
T ≜ Ta + T + Td total duration
Generation
Joint Space T
Trajectory Acceleration phase:
⎧θ + θ t + ... t
Generation
⎪
⎪ t ∈ [ ,t ]
( -D)
⎪
˙
⎪
θ max
⎪
θ(t) = ⎨θ + θ ˙ t + θ lima ( t − T t + T ) t ∈ [t , t ]
¨
⎪
⎪
⎪
⎪θ + (θ lim + θ ) Ta − θ lim (Ta − t) + θ max (Ta −t) t ∈ [t , t ]
⎪
...
⎩
˙ ˙ ˙
Constant velocity phase:
θ(t) = θ + (θ lim + θ ) + θ lim (t − Ta ), t ∈ [t , t ]
˙ ˙ Ta ˙
(Continues next slide)

37. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
28
Deceleration phase:
Chapter
⎧
⎪θ f − (θ lim + θ f ) Td + θ lim (t − T + Td ) − θ max (t−T+Td )
Trajectory
...
Generation
⎪
⎪
˙ ˙ ˙ t ∈ [t , t ]
⎪
⎪
⎪θ f − (θ lim + θ f ) Td + θ lim (t − T + Td )+
⎪
⎪
Introduction
˙ ˙ ˙
θ(t) = ⎨ θ lim
to Trajectory
⎪ d (t − T + Td ) − T (t − T + Td ) + T
⎪ t ∈ [t , t ]
Generation ¨
⎪
⎪
⎪
⎪
⎪θ f − θ f (T − t) − ...max (T−t)
Joint Space
Trajectory
Generation ⎪
⎩
˙ θ t ∈ [t , t ]
( -D)

38. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
29
Solution 2: B´zier curves: θ(t) = ∑n Bi,n (t)pi ,
e
basis function: {Bi,n (t)}
i=
Chapter
Trajectory
Generation
Introduction
to Trajectory
Generation
Joint Space
Trajectory
Generation
( -D)

39. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
29
Solution 2: B´zier curves: θ(t) = ∑n Bi,n (t)pi ,
e
basis function: {Bi,n (t)}
i=
Chapter
Trajectory
Generation
Definition: Bernstein Polynomials
Introduction
Bi,n (t) ≜ t ( − t)n−i , t ∈ [ , ], i = , . . . , n
to Trajectory n i
Generation i
Joint Space
Trajectory
Generation
( -D)

40. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
29
Solution 2: B´zier curves: θ(t) = ∑n Bi,n (t)pi ,
e
basis function: {Bi,n (t)}
i=
Chapter
Trajectory
Generation
Definition: Bernstein Polynomials
Introduction
Bi,n (t) ≜ t ( − t)n−i , t ∈ [ , ], i = , . . . , n
to Trajectory n i
Generation i
Joint Space
Trajectory
Generation
( -D)
Property 1:
nonnegativity : Bi,n (t) ≥ , ∀t ∈ [ , ], i
symmetry : Bi,n (t) = Bn−i,n ( − t), t ∈ [ , ]
recursive definition: Bi,n (t) = tBi− ,n− (t) + ( − t)Bi,n− (t), t ∈ [ , ]
derivative : Bi,n (t) = n(Bi− ,n− (t) − Bi,n− (t)),
˙
with B− ,n− (t) = Bn,n− (t) ≡ , t ∈ [ , ]
⇒ θ(t) = n ∑n− Bi,n− (t)(θ i+ − θ i )
˙
i=
(Continues next slide)

41. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
30
Example: Cubic B´zier curves
e
Chapter
boundary conditions θ , θ , θ , θ ⇒
˙ ˙ unknowns p , p , p , p
(n = , cubic)
Trajectory
Generation
Introduction θ
to Trajectory
⎧θ ⎧p
Generation
⎪
⎪ =p ⎪
⎪ =θ
⎪ ⎪
p
⎪
⎪ ⎪
⎪
Joint Space
⎪θ
⎪
⎪ =p ⎪p
⎪
⎪ =θ +θ
Trajectory p
˙
⎨˙ ⇒⎨
Generation
⎪θ = (p − p ) ⎪p
( -D)
⎪
⎪
⎪ ⎪
⎪
⎪ = −θ
˙ +θ
⎪
⎪˙
⎪θ ⎪
⎪
⎪p
⎪ = (p − p ) ⎪ =θ
p
p
⎩ ⎩
t
t= t= t= t= (more geometrical than algebraic!)

42. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
30
Example: Cubic B´zier curves
e
Chapter
boundary conditions θ , θ , θ , θ ⇒
˙ ˙ unknowns p , p , p , p
(n = , cubic)
Trajectory
Generation
Introduction θ
to Trajectory
⎧θ ⎧p
Generation
⎪
⎪ =p ⎪
⎪ =θ
⎪ ⎪
p
⎪
⎪ ⎪
⎪
Joint Space
⎪θ
⎪
⎪ =p ⎪p
⎪
⎪ =θ +θ
Trajectory p
˙
⎨˙ ⇒⎨
Generation
⎪θ = (p − p ) ⎪p
( -D)
⎪
⎪
⎪ ⎪
⎪
⎪ = −θ
˙ +θ
⎪
⎪˙
⎪θ ⎪
⎪
⎪p
⎪ = (p − p ) ⎪ =θ
p
p
⎩ ⎩
t
t= t= t= t= (more geometrical than algebraic!)
Generalizations of B´zier curve: B-spline, NURBS (Non-Uniform
e
Rational B-Spline)

43. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
31
Problem 2: -D Multi-point Trajectory Generation
˙ ˙ ˙
Init: θ , θ , . . . , θ n , θ , θ , . . . , θ n
Chapter Solution: a parametric curve θ(t), t ∈ [t , tn ] satisfying boundary
Trajectory
Generation
and smoothness condition
Introduction
to Trajectory
Generation
Joint Space
Trajectory
Generation
( -D)

44. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
31
Problem 2: -D Multi-point Trajectory Generation
˙ ˙ ˙
Init: θ , θ , . . . , θ n , θ , θ , . . . , θ n
Chapter Solution: a parametric curve θ(t), t ∈ [t , tn ] satisfying boundary
Trajectory
Generation
and smoothness condition
Introduction
to Trajectory Solutions:
Generation
interpolation by polynomial functions
Joint Space
Trajectory
Generation
interpolation by splines (piecewise polynomial)
( -D)
approximation by smoothing splines

45. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
31
Problem 2: -D Multi-point Trajectory Generation
˙ ˙ ˙
Init: θ , θ , . . . , θ n , θ , θ , . . . , θ n
Chapter Solution: a parametric curve θ(t), t ∈ [t , tn ] satisfying boundary
Trajectory
Generation
and smoothness condition
Introduction
to Trajectory Solutions:
Generation
interpolation by polynomial functions
Joint Space
Trajectory
Generation
interpolation by splines (piecewise polynomial)
( -D)
approximation by smoothing splines
How to specify velocities at via points
˙
User specifies desired velocities θ i at via points θ i .
System automatically chooses the velocities at the via points by
applying heuristics at either Cartesian or joint speeds.
The system automatically chooses velocities at the via points so
that acceleration at the via points are continuous.

46. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
32
Solution 1: Interpolation by polynomial
Chapter
θ(t) = a + a t + a t + ⋯ + an t n , t ∈ [t , tn ]
Trajectory
Generation
Boundary conditions:
Introduction
θ(ti ) = θ i , i = , . . . , n ⇒ Ta = b
to Trajectory
Generation
⎡ ⋯ tn ⎤
Joint Space
⎢ t t ⎥
Trajectory
⎢ ⋯ t n ⎥ , Vandermonde matrix,
T=⎢ ⎥
Generation
t t
⎢ ⎥
( -D)
⎢ n ⎥
tn ⋯ tn ⎦
⎣ tn
⎡ θ ⎤
⎢ ⎥
⎢ ⎥
a
a= ,b = ⎢ θ ⎥
a
⎢ ⎥
⎢ θn ⎥
⎣ ⎦
an
det(T) = (tj − ti ) ≠ ⇒ T always invertible!
≤i<j≤n
Disadvantage: bad numerical conditioning; oscillating behavior.

47. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
33
Solution 2: Interpolation by splines (piecewise polynomial)
Solution 2A: Linear trajectory with parabolic blend
Chapter
Trajectory composition of point-to-point linear trajectory with specified via
Generation ˙
points velocity θ i (local interpolation)
Introduction
to Trajectory using via points as control points (approximation)
Generation
θ ˙ θ
θ ,θ θ
Joint Space ˙
θ ,θ θ
˙ ˙
θ ,θ θ
Trajectory θ ,θ θ
˙
θ ,θ θ
Generation
( -D)
t t t t t t
˙
θ ˙
θ
t t t t t t
¨
θ ¨
θ
t t t t t t
composition with preassigned via point velocities composition with via points as control points

48. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
34
Solution 2B: Piecewise cubic polynomials
Composition of point-to-point cubic curves by specifying via
Chapter
Trajectory point velocities.
Generation
Introduction
Cubic splines by specifying end point velocity only and requiring
to Trajectory continuity of acceleration at via points
Generation
θ ˙ θ
θ ,θ ˙ θ
Joint Space θ ,θ ˙ θ ˙
Trajectory ˙
θ ,θ θ ,θ θ θ ,θ
˙
θ ,θ ˙
θ ,θ
Generation
( -D)
t t t t t t
˙
θ ˙
θ
t t t t t t
¨
θ ¨
θ
t t t t t t
composition with preassigned via point velocities composition with continuity of acceleration

49. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
35
◻ Computation of interpolating cubic splines based on
continuity of acceleration:
Init.: Given θ , . . . , θ n , t , . . . , tn and boundary condition θ , θ n . Tk ≜
˙ ˙
Chapter
tk+ − tk , α(t) ≜ θ(t).
Trajectory
Generation ¨
Introduction
to Trajectory
Step 1: Definition of the cubic spline
(tk+ − t) (t − tk )
Generation
θ(t) = αk + α k+ + − (t − tk )
Joint Space θ k+ Tk α k+
(∗)
Trajectory
Generation Tk Tk Tk
+ − (tk+ − t), t ∈ [tk , tk+ ], k = , , . . . , n −
( -D) θ k Tk α k
Tk
⎧
⎪ θ(t) = (tk+ − t) α k + (t − tk ) α k+ + θ k+ − θ k − Tk (α k+ − α k )
⎪˙
⎪
⎪
⎪
⎪
⇒⎨
Tk Tk Tk
⎪
⎪
⎪ α(t) = α k+ (t − tk ) + α k (tk+ − t)
⎪
⎪
⎪
⎩ Tk
(Continues next slide)

50. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
36
Step 2: Imposing C continuity
(Tk + Tk− ) θ k+ − θ k θ k − θ k−
α k− + α k + α k+ = −
Chapter
Tk−
Trajectory Tk Tk Tk Tk Tk−
Generation
Introduction for k = , . . . , n − .
to Trajectory
Generation Step 3: Imposing boundary condition:
Joint Space
θ(t ) = θ , θ(tn ) = θ n ⇒
Trajectory
Generation
˙ ˙ ˙ ˙
( -D)
⎧
⎪
⎪
⎪
T
α +
T
α = θ −θ −T θ
⎪
⎪
⎪
˙
⎨ ⇒ Aα = B
⎪T
⎪ n−
⎪
⎪
Tn−
⎪
⎪ αn + α n− = θ n− − θ n + Tn− θ
˙n
⎩
⎡ T ⋯ ⎤
⎢ T ⎥ ⎡θ ⎤
⎢T (T + T ) T ⎥ ⎢¨ ⎥
⎢ ⎥
A=⎢ ⎥,α = ⎢ ⎥,
⎢ ⎥
⎢ ⎥ ⎢ θn ⎥
⋱
⎢ (Tn− + Tn− ) Tn− ⎥ ⎣¨ ⎦
⎢ ⎥
Tn−
⎣ ⋯ Tn− Tn− ⎦
(Continues next slide)

51. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
37
Chapter
⎡ ⎤
Trajectory and
⎢ −θ ⎥
Generation
⎢ ⎥
θ −θ ˙
⎢ ⎥
T
⎢ ⎥
Introduction
to Trajectory
B=⎢
θ −θ
− θT
−θ
⎥
⎢ ⎥
Generation T
⎢ ⎥
⎢ θ n −θ n− − θn − ⎥
⎢ ⎥
Joint Space
θ n− −θ n− ˙ θ n −θ n−
⎣ ⎦
Trajectory
Generation T n− T n− T n−
( -D)
Step 4: Solving for α
Aα = B ⇒ α = A− B
Step 5: Computation of the spline by substituting α back into (∗)

52. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
38
Solution 3: Approximation by smoothing splines
ˆ
Given θ , . . . , θ n and t , . . . , tn , design θ(t) s.t.:
Chapter n
θ(t) = arg min µ ω k (θ(tk ) − θ k ) + ( − µ)
tn
Trajectory
Generation
ˆ
θ(t) k=
∫t
¨
θ(t) dt
Introduction
to Trajectory
Generation
Joint Space
Trajectory
Generation
( -D)

53. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
38
Solution 3: Approximation by smoothing splines
ˆ
Given θ , . . . , θ n and t , . . . , tn , design θ(t) s.t.:
Chapter n
θ(t) = arg min µ ω k (θ(tk ) − θ k ) + ( − µ)
tn
Trajectory
Generation
ˆ
θ(t) k=
∫t
¨
θ(t) dt
Special case : µ =
Introduction
to Trajectory
Generation
n
ω k (θ(tk ) − θ k )
Joint Space
Trajectory
Generation
arg min
θ(t) k=
( -D)
Exact interpolation

54. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
38
Solution 3: Approximation by smoothing splines
ˆ
Given θ , . . . , θ n and t , . . . , tn , design θ(t) s.t.:
Chapter n
θ(t) = arg min µ ω k (θ(tk ) − θ k ) + ( − µ)
tn
Trajectory
Generation
ˆ
θ(t) k=
∫t
¨
θ(t) dt
Special case : µ =
Introduction
to Trajectory
Generation
n
ω k (θ(tk ) − θ k )
Joint Space
Trajectory
Generation
arg min
θ(t) k=
( -D)
Exact interpolation
Special case : µ =
tn
arg min
θ(t) ∫t
¨
θ(t) dt
natural Cubic spline curve: θ(t) piecewise cubic polynomial, ∈ C and
ˆ
θ = θn =
¨
ˆ ¨
ˆ
(Continues next slide)

55. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
39
10
Position
5
Chapter 0
Trajectory
Generation −5
6
Introduction
to Trajectory 4
Generation
Velocity 2
Joint Space 0
Trajectory −2
Generation
−4
( -D)
8
6
Acceleration
4
2
0
−2
−4
0 2 4 6 8 10 12 14 16 18
µ = (solid line): exact interpolation
µ = . (dashdot line): approximation with smaller error
µ = . (dashed line): more smooth
(Continues next slide)

56. Chapter 5 Trajectory Generation
5.2 Joint Space Trajectory Generation ( -D)
40
Derivation of the Smoothing spline
n− n−
t − tk ¨
θ(t)dt = θ k (t) dt = (θ k + (θ k+ − θ k )) dt
tn t k+ t k+
Chapter
Trajectory
Generation
∫
t
¨
k=
∫
tk
¨
k=
∫
tk
¨
tk+ − tk
¨
n−
= (θ k + (θ k+ − θ k )) dt
t k+ −t k t
∫
Introduction
¨ ¨ ¨
tk+ − tk
to Trajectory
Generation k=
= (tk+ − tk )(θ k + θ k θ k+ + θ k+ )
Joint Space
Trajectory ¨ ¨ ¨ ¨
Generation
( -D)