Robotics 1

Trajectory planning
in Cartesian space
Prof. Alessandro De Luca

Robotics 1 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 2
 Planning a linear Cartesian path
(position only)

pi GIVEN
L pi, pf, vmax, amax
vi, vf (typically = 0)
pf
L = !pf - pi!
path parameterization pf - pi
= unit vector of directional
p(s) = pi + s (pf - pi) !pf - pi! cosines of the line

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

(gives the current length of the path)

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

Robotics 1 3
 Timing law with trapezoidal speed -1

bang- coast- bang

.. amax given*: L, vmax, amax
"(t)
find: Ts, T
t
= area of the
. Vmax (T - Ts) = L
speed profile
"(t) vmax
vmax
t Ts =
amax
L L amax + vmax2
"(t) T=
amaxvmax
t
Ts T-Ts T
a “coast” phase exists iff: L > vmax2/amax

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

Robotics 1 4
 Timing law with trapezoidal speed -2

.. amax
"(t) amax t2/2 t ! [0,Ts]

t
vmax2
"(t) = vmax t- t ! [Ts,T-Ts]
. 2 amax
"(t) vmax
vmax2
t - amax (t-T)2/2 + vmax T-
amax

L t ! [T-Ts,T]
"(t)
t can be used also
Ts T-Ts T
in the joint space!

Robotics 1 5
 Concatenation of linear paths
no need to pass
z B =“via point” (and stop!) there B- A
= KAB
C’ !B - A!
unit vectors of
A’ over-fly C direction cosines
C- B
= KBC
y !C - B!
A
given: constant speeds v1 on linear path AB
x v2 on linear path BC
desired transition: with constant acceleration for a time #T

x(t)
p(t) = y(t) t ! [0, #T] (transition starts at t = 0)
z(t)
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 6
 Time profiles on components
.. .
x(t) x(t)
v2 KBC,x
v1 KAB,x
.. #T t . t
y(t) y(t)
v2 KBC,y

v1 KAB,y t
.. t .
z(t) z(t)
v1 KAB,z

t v2 KBC,z
#T t

Robotics 1 7
 Timing law during transition
z B B- A
= KAB
C’ !B - A!
A’ unit vectors of
C direction cosines
C- B
= KBC
A y !C - B!

x(t)
x t ! [0, #T] (transition starts at t = 0)
p(t) = y(t)
z(t)
.. .
p(t) = (v2 KBC - v1 KAB)/#T p(t) = v1 KAB + (v2 KBC - v1 KAB) t/#T
$
%

$
%
thus, we obtain a
parabolic blending
p(t) = A’ + v1 KAB t + (v2 KBC - v1 KAB) t2/(2#T) (see textbook
for this same approach
in the joint space)
Robotics 1 8
 Solution
(various options)
B d2 B - A’= d1 KAB
d1 1
C’ C’ - B = d2 KBC
A’ C p(t) = A’ + v1 KAB t + (v2 KBC - v1 KAB)t2/(2#T)

A 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 d2 = v2 #T/2

by choosing, e.g., d1
#T = 2d1/v1 d2 = d1 v2/v1
(namely A’)

Robotics 1 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)

B C B C

A’

A A

#T=4 d1=3
Robotics 1 10
 A numerical example (cont’d)
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)
A ..
!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
B “sufficiently far” from O
C”
A” C’

A’ C
O

on AA’ ( amax on A’B and BC’ ( vmax on C’C ( - amax

+ over-fly between A” e C”

Robotics 1 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
!  !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

Robotics 1 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)
t(s) = p’(s)/!p’(s)!
n(s) unit tangent vector
t(s)
n(s) = p’’(s)/!p’’(s)!
unit normal vector
(∈ osculating plane)
s
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
zA
zB
xA
A B
xB
yA yB

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

we can plan independently a trajectory for each component
!  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
T x(0)!
"  initial given configuration q(0) = (0 " /2 0 0 0 0)
T
"  initial end-effector position p(0) = (0.540 0 1.515) y(0)!
"  initial orientation
! # 0 0 1&
R(0) = % 0 "1 0(
! %
1 0 0
( zw!
$ '

linear path axis-angle method yw!

for position
!
for orientation
T
"  final end-effector position p(T) = (0 0.540 1.515)
z(T)!
"  final orientation
# 1 0 0&
R(T) = % 0 0 1( y(T)!
! % (
$ 0 "1 0' zw!
"  the final configuration is NOT specified a priori
Robotics 1 yw! 18
!
 Axis-angle orientation trajectory
video coordinated
Cartesian motion
with bounds on
L = p final " pinit
v max = 0.4 [m/s]
= 0.763 [m]
amax = 0.1 [m/s2 ]
" max = # /4 [rad/s]
" = r #˙ $ " = #˙
"˙ max = # /8 [rad/s2 ]
!
"˙ = r ˙#˙ $ "˙ = ˙#˙
! triangular
! speed profile s˙(t)
with minimum
!
time T = 5.52 s!
(
p(s) = pinit + s p final " pinit ) (imposed by the bounds
T
= (0.540 0 1.515) + s ( "0.540 0.540 0) , s # [0, 1]
T on linear motion)
!
#0 0 1& #0 "1 0 & s = s(t),
Rinit = %0 "1 0( = Rinit
T
Rinit R final = %0 0 "1( = Rot(r, )if )
T
R(s) = Rinit Rot(r, "(s))
% ( % ( t " [0, T]
! $1 0 0' $1 0 0 '
#1 0 0& #1& "(s) = s"if , s # [0, 1]
1 % ( 2*
R final = %0 0 1( r= "1 , )if = [rad] (= 120°)
% ( 3% 1 ( 3 !
$0 "1 0' $ '
! ! !
Robotics 1 19
!
 Axis-angle orientation trajectory
x=
y=
z=
triangular
planned profile for
motion of linear speed
Cartesian T = 5.52 s!
position
and velocity

T = 5.52 T = 5.52
actual joint angles
2.5

"  the robot joint velocity was commanded

joint 2
2

1.5
by inversion of the geometric Jacobian
actual 1
"  a user program, via KUKA RSI interface
joint at Tc = 12 ms sampling time (one-way
[rad]

motion communication)
0.5

"  robot motion execution is # what was

planned, but only thanks to an external
−0.5

−1
0 1 2 3 4
time [s]
5 6 7 8
kinematic control loop (at task level)
T = 5.52
Robotics 1 20
 Comparison of orientation trajectories
Euler angles vs. axis-angle method
T
"  initial configuration q(0) = (0 " /2 " /2 0 #" /2 0)
T
"  initial end-effector position p(0) = (0.115 0 1.720)
"  initial orientation
.
z(T)! p(T)! x(0)!
.
! # 0 0 1&
R(0) = % 0 "1 0( p(0)!
! % (
$ 1 0 0' z(0)!
T x(T)!
"  initial Euler ZYZ angles " ZYZ (0) = (0 # /2 #)

via a linear path (for position)

!
T
! position p(T) = ( "0.172 0 1.720)
"  final end-effector
"  final orientation zw!
# 0 0 "1&
R(T) = % 0 "1 0 (
! % ( xw!
$ "1 0 0 '
T
"  final Euler ZYZ angles " ZYZ (T) = ( #$ $ /2 0)
Robotics 1 21
!
 Comparison of orientation trajectories
Euler angles vs. axis-angle method

#0 0 1& # "1 0 0 &

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

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

yw! xw!
!

video video
using ZYZ Euler angles using axis-angle method
Robotics 1 22
 Comparison of orientation trajectories
Euler angles vs. axis-angle method
end−effector position end−effector position
1.8 1.8

x=
1.6 1.6

y=
1.4 1.4

1.2 1.2

z= 1
1

linear motion

[m]
[m]

0.8 0.8

0.6 only along the 0.6

0.4 x-direction 0.4

0.2 0.2

planned 0 0

Cartesian −0.2
0 1 2 3 4 5 6 7 8
−0.2
0 1 2 3 4 5 6 7 8

components
time [s] time [s]

T ! 7.2 s! T ! 6 s!
of position
end−effector velocity end−effector velocity
0 0

and velocity −0.01

−0.02
−0.02

−0.03 −0.04
faster motion
time with the
axis-angle method
−0.04

[m/s]
[m/s]

−0.06
−0.05 (imposed by the
−0.06 −0.08 previous bounds
on angular motion)
−0.07
−0.1
−0.08

−0.09 −0.12
0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8
time [s] time [s]

using ZYZ Euler angles using axis-angle method

Robotics 1 23
 Comparison of orientation trajectories
Euler angles vs. axis-angle method
end−effector orientation with ZYZ Euler angles end−effector orientation with ZYZ Euler angles
4 4

2= 3 3 1 = 0 (singularity of
1= 2 2 the representation)
0= 1 1

[rad]
[rad]

0 0

orientation −1 −1

in terms of ZYZ −2 −2

Euler angles pre-planned −3 −3

by post-
offline processing
−4 −4
0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8
time [s] time [s]

T ! 7.2 s! T ! 6 s!
actual joint angles actual joint angles
2 3

1.5 2.5

1 2

actual
0.5 1.5

joint
0 1

[rad]
[rad]

motion
−0.5 0.5

−1 0 only three
−1.5 −0.5
joints move
−2 −1

−2.5 −1.5

−3 −2
0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8
time [s] time [s]

using ZYZ Euler angles using axis-angle method

Robotics 1 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

!  d p/dt = (d p/ds !(ds/d3) + dp/ds!d s/d3 )!1/T
2 2 2 2 2 2 2 2

!  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)
!  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!

Robotics 1 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]
q0 zero initial and
final speed

p0
s=s(t) non-zero
(symmetric)
acceleration
T=1

Robotics 1 26
 Numerical example -2

!  violation of both joint velocity and acceleration bounds with T=1 [s]
!  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 = joint 2

Robotics 1 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