Robotics 1

Trajectory planning
in Cartesian space
Prof. Alessandro De Luca

Robotics 1

Trajectories in Cartesian space

!

in general, the trajectory planning methods proposed in the

joint space can be applied also in the Cartesian space
!

consider independently each component of the task vector (i.e., a

position or an angle of a minimal representation of orientation)

however, when planning a trajectory for the three

orientation angles, the resulting global motion cannot be
intuitively visualized in advance
if possible, we still prefer to plan Cartesian trajectories
separately for position and orientation
the number of knots to be interpolated in the Cartesian
space is typically low (e.g., 2 knots for a PTP motion, 3 if a
via point is added) use simple interpolating paths, such
as straight lines, arc of circles,

Robotics 1

Planning a linear Cartesian path

(position only)

pi

GIVEN
pi, pf, vmax, amax
vi, vf (typically = 0)

L
pf

L = !pf - pi!
path parameterization
p(s) = pi + s (pf - pi)
s ! [0,1]
.
.
dp .
p(s) =
s = (pf - pi) s
ds
pf - pi .
=
"
L
Robotics 1

pf - pi

= unit vector of directional

cosines of the line
!pf - pi!

setting s = "/L, " ! [0,L] is the arc length

(gives the current length of the path)

..
..
d2p . 2
dp ..
p(s) =
s +
s = (pf - pi) s
ds2
ds
pf - pi ..
=
"
L
3

Timing law with trapezoidal speed

..
"(t)

amax

bang- coast- bang

t
.
"(t)

L
"(t)
T-Ts T

given*: L, vmax, amax

find: Ts, T
Vmax (T - Ts) = L

vmax

Ts

-1

= area of the
speed profile

vmax
Ts =
amax
L amax + vmax2
T=
amaxvmax
a coast phase exists iff: L > vmax2/amax

* = other input data combinations are possible (see textbook)

Robotics 1

Timing law with trapezoidal speed

..
"(t)

amax

amax t2/2
t
"(t) =

.
"(t)

vmax
t

Robotics 1

T-Ts T

t ! [0,Ts]
t ! [Ts,T-Ts]

- amax (t-T)2/2 + vmax

vmax2
Tamax

t ! [T-Ts,T]

"(t)
Ts

vmax

vmax2
t2 amax

-2

can be used also

in the joint space!

Concatenation of linear paths

z B =via point

no need to pass
(and stop!) there

!B - A!

over-fly

B- A

C- B

!C - B!

= KAB
= KBC

unit vectors of
direction cosines

given: constant speeds v1 on linear path AB

v2 on linear path BC
desired transition: with constant acceleration for a time #T
x(t)
p(t) = y(t)
z(t)

t ! [0, #T] (transition starts at t = 0)

note: during over-fly, the path remains always in the plane specified
by the two lines intersecting at B (in essence, it is a planar problem)
Robotics 1

Time profiles on components

..
x(t)

.
x(t)

v2 KBC,x

v1 KAB,x
..
y(t)

..
z(t)

#T

.
y(t)

v2 KBC,y

v1 KAB,y

.
z(t)
v1 KAB,z

v2 KBC,z

t
#T
Robotics 1

t
7

Timing law during transition

z

B- A
C

!B - A!

A
x(t)
p(t) = y(t)
z(t)

C- B

..
p(t) = (v2 KBC - v1 KAB)/#T

= KAB
= KBC

!C - B!

t ! [0, #T] (transition starts at t = 0)

$
%

.
p(t) = v1 KAB + (v2 KBC - v1 KAB) t/#T
$
%

p(t) = A + v1 KAB t + (v2 KBC - v1 KAB) t2/(2#T)

Robotics 1

unit vectors of
direction cosines

thus, we obtain a
parabolic blending
(see textbook
for this same approach
in the joint space)

Solution

(various options)
d1

B - A= d1 KAB

d2

C - B = d2 KBC

p(t) = A + v1 KAB t + (v2 KBC - v1 KAB)t2/(2#T)

p(#T) = A + (#T/2) (v1 KAB + v2 KBC) = C

- B + A + (#T/2) (v1 KAB + v2 KBC) = C - B
1

d1 KAB + d2 KBC = (#T/2) (v1 KAB + v2 KBC)

d1 = v1 #T/2

by choosing, e.g., d1
(namely A)
Robotics 1

d2 = v2 #T/2

#T = 2d1/v1

d2 = d1 v2/v1
9

A numerical example
!
!

transition from A=(3,3) to C=(8,9) via B=(1,9), with speed from v1=1 to v2=2
exploiting two options for solution (resulting in different paths!)
!
!

assign transition time: #T=4 (we re-center it here for t ! [-#T/2, #T/2])
assign distance from B for departing: d1=3 (assign d2 for landing is handled similarly)

#T=4
Robotics 1

d1=3
10

A numerical example

(contd)

first option: #T=4 (resulting in d1=2, d2=4)

second option: d1=3 (resulting in #T=6, d2=6)

actually, the same vel/acc profiles only with a different time scale!!
Robotics 1

11

Alternative solution
(imposing acceleration)
B

..
p(t) = 1/#T (v2 KBC - v1 KAB)

C
A

C
v1 = v2 = vmax (for simplicity)
..
!p(t)! = amax

#T = (vmax /amax) !KBC - KAB!

= (vmax /amax) & 2 (1 - KBC,xKAB,x - KBC,yKAB,y - KBC,zKAB,z)
then, d1 = d2 = vmax #T/2
Robotics 1

12

Application example
plan a Cartesian trajectory from A to C (rest-to-rest)
that avoids the obstacle O, with a ' amax and v ' vmax
add a via point B
sufficiently far from O

B
C
A

C
C

A
on AA ( amax
Robotics 1

on AB and BC ( vmax
on CC ( - amax
+ over-fly between A e C
13

Other Cartesian paths

!
!
!

circular path through 3 points in 3D (often built-in feature)

linear path for the end-effector with constant orientation
in robots with spherical wrist: planning may be decomposed into a path
for wrist center and one for E-E orientation, with a common timing law
though more complex in general, it is often convenient to parameterize
the Cartesian geometric path p(s) in terms of its arc length (e.g., with
s = R) for circular paths), so that
velocity: dp/dt = dp/ds ! ds/dt
! dp/ds = unit vector (!!!=1) tangent to the path: tangent direction t(s)
!

ds/dt = absolute value of tangential velocity (= speed)

acceleration: d2p/dt2 = d2p/ds2 ! (ds/dt)2 + dp/ds ! d2s/dt2

!
!

Robotics 1

!d2p/ds2! = curvature *(s) (= 1/radius of curvature)

d2p/ds2!(ds/dt)2 = centripetal acceleration: normal direction n(s) + to the
path, on the osculating plane; binormal direction b(s) = t(s) " n(s)
d2s/dt2 = scalar value (with any sign) of tangential acceleration
14

Definition of Frenet frame

!

For a generic (smooth) path p(s) in R3, parameterized by s (not

necessarily its arc length), one can define a reference frame as in figure
p = dp/ds
p = d2p/ds2
derivatives w.r.t. the parameter

b(s)
n(s)

t(s)

t(s) = p(s)/!p(s)!
unit tangent vector
n(s) = p(s)/!p(s)!
unit normal vector
( osculating plane)
b(s) = t(s) " n(s)
unit binormal vector

general expression of path curvature (at a path point p(s))

*(s) = !p(s) " p(s)!/!p(s)!3

Robotics 1

15

Optimal trajectories
!

for Cartesian robots (e.g., PPP joints)

1. the straight line joining two position points in the Cartesian space is one path

that can be executed in minimum time under velocity/acceleration constraints

(but other such paths may exist, if (joint) motion can also be not coordinated)
2. the optimal timing law is of the bang-coast-bang type in acceleration (in this
special case, also in terms of actuator torques)
!

for articulated robots (with at least a R joint)

!

1. e 2. are no longer true in general in the Cartesian space, but time-optimality

still holds in the joint space when assuming bounds on joint velocity/acceleration
! straight line paths in the joint space do not correspond to straight line paths
in the Cartesian space, and vice-versa
bounds on joint acceleration are conservative (though kinematically tractable)
w.r.t. actual ones on actuator torques, which involve the robot dynamics
! when changing robot configuration/state, different torque values are needed
to impose the same joint accelerations

Robotics 1

16

Planning orientation trajectories

A

xA
yA

zB
xB

B
yB

using minimal representations of orientation (e.g., ZXZ Euler angles ,,),-),

we can plan independently a trajectory for each component
!

zA

e.g., a linear path in space , ) -, with a cubic timing law

. but poor prediction/understanding of the resulting intermediate orientations

alternative method: based on the axis/angle representation

!

determine the (neutral) axis r and the angle )AB: R(r,)AB) = RAT RB (rotation
matrix changing the orientation from A to B . inverse axis-angle problem)
plan a timing law )(t) for the (scalar) angle ) interpolating 0 with )AB (with
possible constraints/boundary conditions on its time derivatives)
/t, RAR(r,)(t)) specifies then the actual end-effector orientation at time t

Robotics 1

17

A complete position/orientation
Cartesian trajectory
" initial given configuration q(0) = (0 " /2 0 0 0 0)
" initial end-effector position p(0) = (0.540 0 1.515)

x(0)!

y(0)!

" initial orientation

!

# 0 0 1&
R(0) = % 0 "1 0(
!
%
(
1
0
0
$
'

linear path
for position
!
" final end-effector position

zw!

axis-angle method
for orientation
p(T) = (0 0.540 1.515)

yw!

z(T)!

" final orientation

# 1 0 0&
R(T) = % 0 0 1(
%
(
!
$ 0 "1 0'
" the final configuration is NOT specified a priori

y(T)!

zw!
yw!

Robotics 1

18

Axis-angle orientation trajectory

coordinated
Cartesian motion
with bounds on
v max = 0.4 [m/s]
amax = 0.1 [m/s2 ]
" max = # /4 [rad/s]
" max = # /8 [rad/s2 ]

video
L = p final " pinit
= 0.763 [m]
" = r # $ " = #

" = r # $ " = #

triangular
speed profile s(t)
with minimum
time T = 5.52 s!

!
!

p(s) = pinit + s p final " pinit

(imposed by the bounds

on linear motion)

= (0.540 0 1.515) + s ( "0.540 0.540 0) , s # [0, 1]

#0 0
Rinit = %0 "1
%
$1 0
#1 0
R final = %0 0
%
$0 "1
Robotics 1

1&
T
0( = Rinit
(
0'
0&
1(
(
0'
!

#0 "1 0 &
Rinit R final = %0 0 "1( = Rot(r, )if )
%
(
$1 0 0 '
#1&
1 % (
2*
r=
"1 , )if =
[rad] (= 120)
3
3% 1 (
!
$ '
T

R(s) = Rinit Rot(r, "(s))

s = s(t),
t " [0, T]

"(s) = s"if , s # [0, 1]

19

Axis-angle orientation trajectory

x=
y=
z=

triangular
profile for
linear speed

planned
motion of
Cartesian
position
and velocity

T = 5.52 s!

actual joint angles

2.5

joint 2

T = 5.52

T = 5.52

" the robot joint velocity was commanded

by inversion of the geometric Jacobian
" a user program, via KUKA RSI interface
at Tc = 12 ms sampling time (one-way
communication)

1.5

1
[rad]

actual
joint
motion

0.5

0.5

1
0

4
time [s]

" robot motion execution is # what was

planned, but only thanks to an external
kinematic control loop (at task level)

T = 5.52

Robotics 1

20

Comparison of orientation trajectories

Euler angles vs. axis-angle method
" initial configuration q(0) = (0 " /2 " /2 0 #" /2 0)
" initial end-effector position p(0) = (0.115 0 1.720)
" initial orientation
!

z(0)!

via a linear path (for position)

" final orientation

" ZYZ (T) = ( #$ $ /2 0)

zw!

# 0 0 "1&
% 0 "1 0 (
R(T)
=
!
%
(
$ "1 0 0 '

" final Euler ZYZ angles

x(T)!

" final end-effector

! position p(T) = ( "0.172 0 1.720)

Robotics 1

x(0)!
p(0)!

z(T)! p(T)!

# 0 0 1&
R(0) = % 0 "1 0(
%
(
!
$ 1 0 0'

" initial Euler ZYZ angles " ZYZ (0) = (0 # /2 #)

xw!
T

21

Comparison of orientation trajectories

Euler angles vs. axis-angle method

#0 0 1&
Rinit = %0 "1 0(
%
(
$1 0 0'
# 0 &
) * ZYZ , init = % + /2(
%
(
$ + '

# "1 0 0 &
Rinit R final = % 0 1 0 (
%
(
$ 0 0 "1'
#0&
) r = % "1(, * = +
% (
$0'
T

# 0 0 "1&
R final = % 0 "1 0 (
%
(
$ "1 0 0 '
# "+ &
) * ZYZ , final = % + /2(
%
(
$ 0 '

zw!
yw!

xw!

video

video
using ZYZ Euler angles

Robotics 1

using axis-angle method

22

Comparison of orientation trajectories

Euler angles vs. axis-angle method
endeffector position

1.8
1.6

1.6

1.4

1.4

1.2

1.2

linear motion
only along the
x-direction

0.8
0.6
0.4

planned
Cartesian
components
of position
and velocity

[m]

[m]

x=
y=
z=

0.8
0.6
0.4

0.2

0.2

0.2

4
time [s]

0.2

T ! 7.2 s!

endeffector velocity

endeffector position

1.8

4
time [s]

T ! 6 s!

endeffector velocity

0.01

0.02
0.02

faster motion
time with the
axis-angle method
(imposed by the
previous bounds
on angular motion)

0.04

0.03

[m/s]

[m/s]

0.04

0.06

0.05

0.08

0.06

0.07

0.1
0.08

0.09

Robotics 1

4
time [s]

using ZYZ Euler angles

0.12

4
time [s]

using axis-angle method

23

Comparison of orientation trajectories

Euler angles vs. axis-angle method
endeffector orientation with ZYZ Euler angles

[rad]

[rad]

2=
1=
0=

orientation
in terms of ZYZ
pre-planned
Euler angles

offline

0.5

1.5

0.5

1.5

0.5

2.5

1.5

4
time [s]

using ZYZ Euler angles

4
time [s]

T ! 6 s!

actual joint angles

0.5

3
2.5

by postprocessing

T ! 7.2 s!

[rad]

[rad]

1.5

Robotics 1

actual joint angles

actual
joint
motion

4
time [s]

1 = 0 (singularity of
the representation)

endeffector orientation with ZYZ Euler angles

only three
joints move

4
time [s]

using axis-angle method

24

Uniform time scaling

!

for a given path p(s) (in joint or Cartesian space) and a given timing law
s(3) (3=t/T, T=motion time), we need to check if existing bounds vmax
on (joint) velocity and/or amax on (joint) acceleration are violated or not
!

unless such constraints have already been taken into account during the
trajectory planning, e.g., by using a bang-coast-bang acceleration timing law

velocity scales linearly with motion time

! dp/dt = dp/ds!ds/d3!1/T
acceleration scales quadratically with motion time
2
2
2
2
2
2
2
2
! d p/dt = (d p/ds !(ds/d3) + dp/ds!d s/d3 )!1/T
if motion is unfeasible, scale (increase) time T kT (k>1), based on the
most violated constraint (max of the ratios |v|/vmax and |a|/amax)
if motion is too slow w.r.t. the robot capabilities, decrease T (k<1)
!

Robotics 1

in both cases, after scaling, there will be (at least) one instant of saturation
(for at least one variable)
no need to re-compute motion profiles from scratch!
25

Numerical example
!
!

-1

2R planar robot with links of unitary length (1 [m])

linear Cartesian path p(s) from q0=(110, 140) p0=f(q0)=(-.684, 0) [m]
to p1=(0.816, 1.4), with rest-to-rest cubic timing law s(t), T=1 [s]
bounds in joint space: max (absolute) velocity vmax,1= 2, vmax,2= 2.5 [rad/s],
max (absolute) acceleration amax,1= 5, amax,2= 7 [rad/s2]
.s #3 [m/s]
max

p1

path length L=2.0518 [m]

zero initial and
final speed

q0

p0

s=s(t)

non-zero
(symmetric)
acceleration
T=1

Robotics 1

26

Numerical example
!

violation of both joint velocity and acceleration bounds with T=1 [s]
!
!

-2

max relative violation of joint velocities: kvel = 2.898

max relative violation of joint accelerations: kacc = 6.2567

minimum uniform time scaling of Cartesian trajectory to recover feasibility

k = max {1, kvel , $kacc} = 2.898 Tscaled = kT = 2.898 > T

kacc
kvel

= joint 1

Robotics 1

= joint 2

27

Numerical example
!

-3

scaled trajectory with Tscaled = 2.898 [s]

!

speed [acceleration] on path and joint velocities [accelerations] scale linearly [quadratically]

traced Cartesian path

and associated joint paths
remain the same!

at least 1 instant of saturation!

Robotics 1

= joint 1

= joint 2

28