trajectory planning in cartesian space -...

Robotics 1

Trajectory planning in Cartesian space

Prof. Alessandro De Luca

Robotics 1 1

Trajectories in Cartesian spacen in general, the trajectory planning methods proposed in the

joint space can be applied also in the Cartesian spacen consider independently each component of the task vector (i.e., a

position or an angle of a minimal representation of orientation)n however, when planning a trajectory for the three

orientation angles, the resulting global motion cannot be intuitively visualized in advance

n if possible, we still prefer to plan Cartesian trajectories separately for position and orientation

n 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

pf

GIVENpi, pf, vmax, amax

vi, vf (typically = 0)

path parameterizationp(s) = pi + s (pf - pi)

s Î [0,1]

L

setting s = s/L, s Î [0,L] is the arc length(gives the current length of the path)

p(s) = s = (pf - pi) sdpds

. . .p(s) = s2d2p

ds2

...= (pf - pi) ssdp

ds

..+

..

pf - pi

Ls.

= s=pf - pi

L..

unit vector of directionalcosines of the line

pf - pi

║pf - pi║=

L = ║pf - pi║

Robotics 1 3

Timing law with trapezoidal speed - 1

s(t)

s(t)

s(t)

.

.

..

Ts T-Ts Tt

t

t

bang- coast- bangamax

vmaxvmax

amaxTs =

L amax + vmax2

amaxvmaxT =

L

given*: L, vmax, amaxfind: Ts, T

Vmax (T - Ts) = L = area of the speed profile

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

s(t)

s(t)

s(t)

.

.

..

Ts T-Ts Tt

t

t

amax

vmax

L

amax t2/2

vmax t -

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

amax

vmax2

2amaxs(t) =

t Î [0,Ts]

t Î [Ts,T-Ts]

t Î [T-Ts,T]

can be used alsoin the joint space!

Robotics 1 5

Concatenation of linear paths

A

C

B =“via point”

A’C’

B - A║B - A║

= KAB

C - B║C - B║

= KBC

unit vectors ofdirection cosines

given: constant speeds v1 on linear path ABv2 on linear path BC

desired transition: with constant acceleration for a time DTx

z

y

x(t)y(t)z(t)

p(t) = t Î [0, DT] (transition starts at t = 0)

over-fly

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)

no need to pass(and stop!) there

Robotics 1 6

Time profiles on components

t

t

t

t

t

t

x(t)..

x(t).

y(t).

z(t).

y(t)..

z(t)..

v1 KAB,x

v2 KBC,x

v1 KAB,y

v2 KBC,y

v1KAB,z v2 KBC,z

DT

DT

Robotics 1 7

Timing law during transition

A

B

CA’C’

B - A║B - A║

= KAB

C - B║C - B║

= KBC

unit vectors ofdirection cosines

x

z

yx(t)y(t)z(t)

p(t) = t Î [0, DT] (transition starts at t = 0)

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

p(t) = v1 KAB + (v2 KBC - v1 KAB) t/DT.

p(t) = A’ + v1 KAB t + (v2 KBC - v1 KAB) t2/(2DT)

óõ

óõ thus, we obtain a

parabolic blending(see textbook

for this same approachin the joint space)

Robotics 1 8

Solution(various options)

A

B

CA’C’

B - A’= d1 KAB

C’- B = d2 KBC

p(t) = A’ + v1 KAB t + (v2 KBC - v1 KAB)t2/(2DT)

d1d2

DT = 2d1/v1 d2 = d1 v2/v1by choosing, e.g., d1

(namely A’)

1

- B + A’ + (DT/2) (v1 KAB + v2 KBC) = C’ - B

p(DT) = A’ + (DT/2) (v1 KAB + v2 KBC) = C’

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

d1 = v1 DT/2 d2 = v2 DT/2

1

Robotics 1 9

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

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

DT=4

A

B C

d1=3

A

B C

A’

Robotics 1 10

A numerical example (cont’d)

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

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

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

Robotics 1 11

Alternative solution(imposing acceleration)

A

B

CA’C’

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

v1 = v2 = vmax (for simplicity)

║p(t)║ = amax

..

DT = (vmax /amax) ║KBC - KAB║

= (vmax /amax) Ö 2 (1 - KBC,xKAB,x - KBC,yKAB,y - KBC,zKAB,z)

then, d1 = d2 = vmax DT/2

Robotics 1 12

Application exampleplan a Cartesian trajectory from A to C (rest-to-rest) that avoids the obstacle O, with a £ amax and v £ vmax

on AA’ ® amax on A’B and BC’ ® vmax on C’C ® - amax+ over-fly between A” e C”

A

B

C

A”C”

OA’

C’

add a via point B“sufficiently far” from O

Robotics 1 13

Other Cartesian pathsn circular path through 3 points in 3D (often built-in feature)n linear path for the end-effector with constant orientationn 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 lawn 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 = Rq for circular paths), so thatn velocity: dp/dt = dp/ds · ds/dt

n dp/ds = unit vector (║·║=1) tangent to the path: tangent direction t(s)n ds/dt = absolute value of tangential velocity (= speed)

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

n ║d2p/ds2║ = curvature k(s) (= 1/radius of curvature)n 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) n d2s/dt2 = scalar value (with any sign) of tangential acceleration

Robotics 1 14

Definition of Frenet framen 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

s

t(s)n(s)

b(s)

p’ = dp/ds p’’ = d2p/ds2

derivatives w.r.t. the parameter

t(s) = p’(s)/║p’(s)║

n(s) = p’’(s)/║p’’(s)║

b(s) = t(s) � n(s)

unit tangent vector

unit normal vector(∈ osculating plane)

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

k(s) = ║p’(s) � p’’(s)║/║p’(s)║3

Robotics 1 15

Optimal trajectoriesn 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)

n for articulated robots (with at least a R joint)n 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/accelerationn straight line paths in the joint space do not correspond to straight line paths

in the Cartesian space, and vice-versan bounds on joint acceleration are conservative (though kinematically tractable)

w.r.t. actual ones on actuator torques, which involve the robot dynamics n when changing robot configuration/state, different torque values are needed

to impose the same joint accelerations

Robotics 1 16

Planning orientation trajectories

n using minimal representations of orientation (e.g., ZXZ Euler angles f,q,y), we can plan independently a trajectory for each component n e.g., a linear path in space f q y, with a cubic timing law

Þ but poor prediction/understanding of the resulting intermediate orientations

n alternative method: based on the axis/angle representation

n determine the (neutral) axis r and the angle qAB: R(r,qAB) = RAT RB (rotation

matrix changing the orientation from A to B Þ inverse axis-angle problem)

n plan a timing law q(t) for the (scalar) angle q interpolating 0 with qAB (with possible constraints/boundary conditions on its time derivatives)

n "t, RAR(r,q(t)) specifies then the actual end-effector orientation at time t

A BxA

yA

zA zB

yB

xB

Robotics 1 17

A complete position/orientationCartesian trajectory

Robotics 1 18

§ initial given configuration

§ initial end-effector position

§ initial orientation

x(0)

y(0)

linear pathfor position

axis-angle methodfor orientation

zw

yw

! 0 = 0 $/2 0 0 0 0 '

( 0 = 0.540 0 1.515 '

- 0 =0 0 10 −1 01 0 0

zw

yw

§ final end-effector position

§ final orientation

§ the final configuration is NOT specified a priori

y(T)

z(T)( ' = 0 0.540 1.515 '

- ' =1 0 00 0 10 −1 0

Axis-angle orientation trajectory

Robotics 1 19

coordinatedCartesian motion

with boundsvideo

triangularspeed profile "̇($)with minimumtime T = 5.52 s

(imposed by the boundson linear motion)

& = ()̇ → & = )̇

&̇ = ()̈ → &̇ = )̈

, = -./012 − -/0/4= 0.763 [m]

=>?@ = 0.4 [ ⁄m s]D>?@ = 0.1 [ ⁄m s2]&>?@ = G/4 [ ⁄rad s]&̇>?@ = G/8 [ ⁄rad s2]

- " = -/0/4 + " -./012 − -/0/4= 0.540 0 1.515 O + " −0.540 0.540 0 O, " ∈ 0,1

R/0/4 =0 0 10 −1 00 0 0

= R/0/4O

R./012 =1 0 00 0 10 −1 0

R/0/4O R./012 =0 −1 00 0 −11 0 0

= RS$ (, )TU

( =13

1−11

, )/U =2G3

rad (= 120°)

R " = R/0/4RS$((, ) " )

) " = ")/U , " ∈ 0,1

W = W X , X ∈ [Y, Z]

Axis-angle orientation trajectory

Robotics 1 20

actual joint

motion

z = y =x =

plannedmotion ofCartesianposition

and velocity

§ the robot joint velocity was commanded by inversion of the geometric Jacobian

§ a user program, via KUKA RSI interface at !" = $% ms sampling time (one-way communication)

§ robot motion execution is ≈ what was planned, but only thanks to an external kinematic control loop (at task level)

triangularprofile for

linear speed T = 5.52 s

joint 2

T = 5.52 T = 5.52

T = 5.52

Comparison of orientation trajectoriesEuler angles vs. axis-angle method

Robotics 1 21

§ final end-effector position

§ final orientation

§ final Euler ZYZ angles

§ initial configuration

§ initial end-effector position

§ initial orientation

§ initial Euler ZYZ anglesx(T)

z(T) x(0)

z(0)

zw

xw

p(T). p(0).

via a linear path (for position)

! 0 = 0 $/2 $/2 0 −$/2 0 (

) 0 = 0.115 0 1.720 (

. 0 =0 0 10 −1 01 0 0

/010 0 = 0 $/2 $ 2

) ( = −0.172 0 1.720 (

. ( =0 0 −10 −1 0−1 0 0

/010 ( = −$ $/2 0 2

Comparison of orientation trajectoriesEuler angles vs. axis-angle method

Robotics 1 22using ZYZ Euler angles using axis-angle method

zw

xwyw

video video

!"#"$ =0 0 10 −1 01 0 0

⟹ *+,+,"#"$ =0./2.

!1"#23 = −0 0 10 1 01 0 0

⟹ *+,+,1"#23 =−../20

!"#"$4 !1"#23=

−1 0 00 1 00 0 −1

⟹ 5 =0−10

,

6 = .

Robotics 1 23

z = y =x =

plannedCartesian

componentsof position

and velocity

Comparison of orientation trajectoriesEuler angles vs. axis-angle method

using ZYZ Euler angles using axis-angle method

T ≈ 6 sT ≈ 7.2 s

faster motiontime with the

axis-angle method(imposed by the previous bounds

on angular motion)

linear motiononly along the

x-direction

Robotics 1 24

g = b =a =

orientationin terms of ZYZ

Euler angles

Comparison of orientation trajectoriesEuler angles vs. axis-angle method

T ≈ 7.2 s

using ZYZ Euler angles using axis-angle method

T ≈ 6 s

actual joint

motion

pre-plannedoffline

by post-processing

b = 0 (singularity ofthe representation)

only three joints move

Uniform time scalingn for a given path p(s) (in joint or Cartesian space) and a given timing law

s(t) (t=t/T, T=“motion time”), we need to check if existing bounds vmaxon (joint) velocity and/or amax on (joint) acceleration are violated or notn … unless such constraints have already been taken into account during the

trajectory planning, e.g., by using a bang-coast-bang acceleration timing lawn velocity scales linearly with motion time

n dp/dt = dp/ds·ds/dt·1/T n acceleration scales quadratically with motion time

n d2p/dt2 = (d2p/ds2·(ds/dt)2 + dp/ds·d2s/dt2)·1/T2

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

n if motion is “too slow” w.r.t. the robot capabilities, decrease T (k<1)n in both cases, after scaling, there will be (at least) one instant of saturation

(for at least one variable) n no need to re-compute motion profiles from scratch!

Robotics 1 25

Numerical example - 1

n 2R planar robot with links of unitary length (1 [m])n linear Cartesian path p(s) from q0=(110o, 140o) ⇒ p0=f(q0)=(-.684, 0) [m] to

p1=(0.816, 1.4), with rest-to-rest cubic timing law s(t), T=1 [s]n 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]

Robotics 1 26

p0

p1

q0

path length L=2.0518 [m]zero initial and

final speed

T=1

non-zero(symmetric)acceleration

s=s(t)

smax≈3 [m/s].

Numerical example - 2

n violation of both joint velocity and acceleration bounds with T=1 [s]n max relative violation of joint velocities: kvel = 2.898 = max{1, |q1|/vmax,1, |q2|/vmax,2}n max relative violation of joint accelerations: kacc = 6.2567 = max{1, |q1|/amax,1, |q2|/amax,2}

n minimum uniform time scaling of Cartesian trajectory to recover feasibilityk = max {1, kvel , √kacc } = 2.898 ⇒ Tscaled = kT = 2.898 > T

Robotics 1 27

= joint 2= joint 1

kvel

kacc

.. ... .

Numerical example - 3n scaled trajectory with Tscaled = 2.898 [s]

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

Robotics 1 28= joint 2= joint 1

at least 1 instant of saturation!

traced Cartesian path and associated joint paths

remain the same!