Path and trajectory generation

Path and trajectory

Path: Geometric representation in space

00.1

0.20.3

0.4

0.20.4

0.60.8

10.9

0.95

1

1.05

1.1

1.15

xy

z

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

lcra

d

joint 123

Path and trajectory

Path: Geometric representation in space

Trajectory: Path with a velocity (time) profile

0 0.5 1 1.5 2 2.5 3 3.5 4-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

Time [sec]

[rad]

Joint 123

0 0.5 1 1.5 2 2.5 3 3.5 40

0.05

0.1

0.15

0.2

0.25

Time [sec]

Path

spe

ed [m

/s]

The Trajectory generation problem

… finding the time history of position, velocity, and acceleration for each degree of freedom ...

Relevant questions:1. How to specify a trajectory?2. How is the trajectory represented in the computer?3. How to compute the trajectory from internal

representation?

The Trajectory generation problem

User inputGoal position and orientation.Via points, intermediate points between start and goal position.Sometime both spatial and temporal information (velocities and so on).

RequirementsContinuous positionContinuous derivativeContinuous second derivative, accelerationDynamic limitations

Different levels of user specifications

Move freely from position1 to position 2.

point-to-point motion

Generalization: Move from position 1 to position N via positions k = {2, 3, …, N-1}. Velocities might be specified in the intermediate positions.

path motion

Different levels of user specifications

Move along a line from position1 to position 2.

linear motion

Move along an arc from position1 to position 2.

circular motion

Extension (?): Move along a general function in space, for example a spline path

Point-to-point motion

Input:

4 equations ⇒ solve for 4 parameters:

(cubic spline, complete spline)

Point-to-point motion

Result when

Matlab code: csp = completesp([0 1],[0 4],[0 0]);or [q dq ddq] = jtraj2(0,4,1,0:0.01:1,'ptp');

0 0.5 10

1

2

3

4

Time [sec]

[rad]

0 0.5 10

2

4

6

Time [sec]

[rad/

s]

0 0.5 1-40

-20

0

20

40

Time [sec]

[rad/

s2 ]

Point-to-point motion

Is a good solution?

Well … if we solve the following minimization problem

the solution becomes

Intermediate points

Use

How to find the derivatives?From Cartesian path specification. The the inverse Jacobianis used to find joint velocities.Chosen by the system such that the acceleration is

continuous. This is called complete splines.

No guarantee on that the robot can actually follow the trajectory.

Point-to-point

More advanced schemes: Linear with parabolic blendsMatlab code: [q dq ddq] = jtraj2(0,4,0.25,t,'lpb');

(jtraj2 is written by Måns Östring)

0 0.5 10

1

2

3

4

Time [sec]

[rad]

0 0.5 10

2

4

6

Time [sec]

[rad/s]

0 0.5 1-40

-20

0

20

40

Time [sec]

[rad/s 2 ]

Point-to-point

Robotics ToolboxJTRAJ Compute a joint space trajectory between two points

[Q QD QDD] = JTRAJ(Q0, Q1, N)

[Q QD QDD] = JTRAJ(Q0, Q1, N, QD0, QD1)[Q QD QDD] = JTRAJ(Q0, Q1, T)

[Q QD QDD] = JTRAJ(Q0, Q1, T, QD0, QD1)Returns a joint space trajectory Q from state Q0 to Q1. The number

of points is N or the length of the given time vector T. A 7thorder polynomial is used with default zero boundary conditions forvelocity and acceleration. Non-zero boundary velocities can beoptionally specified as QD0 and QD1.The function can optionally return a velocity and acceleration

trajectories as QD and QDD.Each trajectory is an mxn matrix, with one row per time step, and

one column per joint parameter.

The trajectory generation problem

Cart. path

Geo.correct path

Dyn. plan.+optimization

P(lc)

φ(lc)

lc(t)

p1

p0

Example in Matlab (PGT)

p1 = [0.4,0.3,0.9]; p2 = [0.1,0.45,1.1];

p3 = [0.3,0.60,1.1]; p4 = [0.2,0.8,1.1];zone1 = 0.1; zonemethod = 1; v1 = 0.25; v2 = 0.25;

esec = emptysec(p1);lsec = moveline(esec,p2,zone1,[],v1);csec = movecirc(lsec,p3,p4,0,1,v2);

rpath = makepath(lsec,csec)

00.1

0.20.3

0.4

0.20.4

0.60.8

10.9

0.95

1

1.05

1.1

1.15

xy

zThe freedom is in the interpolation method inside the zone.

Path generation in cartesian space

Linear motionSimple 1st order polZone representation

0.10.2

0.30.4

0.5

0.2

0.4

0.6

0.81

1.05

1.1

1.15

1.2

1.25

x [m]y [m]

z [m

]

Path generation in cartesian space

General caseMove p2 to the origin.

0.10.2

0.30.4

0.5

0.2

0.4

0.6

0.81

1.05

1.1

1.15

1.2

1.25

x [m]y [m]

z [m

]

00.1

0.20.3

0.4

-0.2-0.1

0

0.10.2

-0.2

-0.1

0

0.1

0.2

Path generation in cartesian space

General caseMove p2 to the origin.

00.1

0.20.3

0.4

-0.2-0.1

0

0.10.2

-0.2

-0.1

0

0.1

0.2

Path generation in cartesian space

General caseMove p2 to the origin.Rotate the coordinate systemsuch that p1, p2 and p3 are inthe x-y-plane.

Path generation in cartesian space

General caseMove p2 to the origin.Rotate the coordinate systemsuch that p1, p2 and p3 are inthe x-y-plane.

-0.2 -0.1 0 0.1 0.2 0.3

-0.1

-0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

x [m]y

[m]

Path generation in cartesian space

General caseMove p2 to the origin.Rotate the coordinate systemsuch that p1, p2 and p3 are inthe x-y-plane.

-0.1 -0.05 0 0.05-0.02

0

0.02

0.04

0.06

0.08

0.1

0.12

x [m]y

[m]

Path generation in cartesian space

General caseMove p2 to the origin.Rotate the coordinate systemsuch that p1, p2 and p3 are inthe x-y-plane.Find spline description in this coordinate system.

-0.1 -0.05 0 0.05-0.02

0

0.02

0.04

0.06

0.08

0.1

0.12

x [m]y

[m]

Path generation in cartesian space

General caseMove p2 to the origin.Rotate the coordinate systemsuch that p1, p2 and p3 are inthe x-y-plane.Find spline description in this coordinate system.Rotate the spline back into the orignal coordinate system andtranslate back to p2.

0.10.2

0.30.4

0.5

0.2

0.4

0.6

0.81

1.05

1.1

1.15

1.2

1.25

x [m]y [m]

z [m

]

Path generation in cartesian space

Three methods for in-zone path generation• Second order polynomial – resulting path in C1

-0.05 0 0.050

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0.09

Path generation in cartesian space

Three methods for in-zone path generation• Second order polynomial – resulting path in C1

• Two cubic splines – in C2

-0.05 0 0.050

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0.09

Path generation in cartesian space

Three methods for in-zone path generation• Second order polynomial – resulting path in C1

• Two cubic splines – in C2

• Two fourth order and two third order polynomial – in C2

-0.05 0 0.050

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

0.09

Path generation in cartesian space

Robotics ToolboxCTRAJ Compute a Cartesian trajectory between two points

TC = CTRAJ(T0, T1, N)

TC = CTRAJ(T0, T1, R)Returns a Cartesian trajectory TC from point T0 to T1. The number

of points is N or the length of the given path distance vector R.In the first case the points are equally spaced between T0 and T1.

In the second case R gives the distance along the path, and theelements of R must be in the range [0 1].Each trajectory is a 4x4xn matrix, with the last subscript being the

point index.

Path generation in cartesian space

Robotics ToolboxTRINTERP Interpolate homogeneous transformations

TR = TRINTERP(T0, T1, R)

Returns a homogeneous transform interpolation between T0 and T1 asR varies from 0 to 1. Rotation is interpolated using quaternion

spherical linear interpolation.

Path generation in cartesian space

Robotics ToolboxQINTERP Interpolate rotations expressed by quaternion objects

QI = qinterp(Q1, Q2, R)

Return a unit-quaternion that interpolates between Q1 and Q2 as R moves from 0 to 1. This is a spherical linear interpolation (slerp)

that can be interpretted as interpolation along a great circle arc ona sphere.

If r is a vector, QI, is a cell array of quaternions, each element corresponding to sequential elements of R.

See also: CTRAJ, QUATERNION.

The trajectory generation problem

Cart. path

Geo.correct path

Dyn. plan.+optimization

P(lc)

φ(lc)

lc(t)

p1

p0

Example in Matlab (PGT)

% The path is transformed to cubic spline functions in joint% space:

arpath = cart2jsp(rpath,@infofcn,'maxerror',1e-5)

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

lc

rad

joint 123

00.1

0.20.3

0.4

0.20.4

0.60.8

10.9

0.95

1

1.05

1.1

1.15

xy

z

From the cartesian path P(lc) to φ(lc)Inverse kinematics.Describe function using spline such as

Needs error approximation!

Use direct kinematics for ”middle” point, compare with the ”true” path.

Path generation in joint space

γ<− )(ˆ)(max cc lPlP

Path generation in joint space

Iterative solution based on 2 knots1. Compute inverse kinematic solution in P(l), P(l+dl) ⇒ φ(l),

φ(l+dl)2. Find estimate of dφ/dl in l and l+dl (Jacobian)3. Compute the coeff in the cubic spline using completesp4. Evaluate the resulting spline in l+dl/2

If the error is less than γ, keep the spline and letl = l+dl, dl = 2*dl

else let dl = dl/25. Goto 1

Similar algorithms can be formulated for 3 knots and 4 knots.

The trajectory generation problem

Cart. path

Geo.correct path

Dyn. plan.+optimization

P(lc)

φ(lc)

lc(t)

p1

p0

Compute the path as a function of lc

Path: P(lc), φ(lc)Path speed and acceleration:

,dtdl

dldPP c

c

=&2

2

dtld

dldPP c

c

=&&

,dtdl

dld c

c

φφ =&2

22

2

2

dtld

dld

dtdl

dld c

c

c

c

φφφ +⎟⎠⎞

⎜⎝⎛=&&

Dynamic optimization

Let

A (sub) optimal minimum time trajectory is found by solving the following LP problem

Dynamic optimization

Let

An alternative is to use the following algorithm

Example

using the path from the PGT examples.

0 0.5 1 1.5 2 2.5 3 3.5 40

0.05

0.1

0.15

0.2

0.25

Time [sec]

Path

spe

ed [m

/s]

0 0.5 1 1.5 2 2.5 3 3.5 4-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

Time [sec]

Acceleration

joint 123

lin. acc

Programming a robot (ABB robot :-)

Programming language: RAPID

“Home made” language, developed by SoftLab and ABB.

IF, WHILE, FOR, TEST (in C, if, while, for, switch)Assigning variables: :=Data types: bool, num, stringProcedures vs. FunctionsTPWrite "No of produced parts="\Num:=reg1;

anglex := GetEuler(\X, object.rot);

Programming a robot (ABB robot :-)

4 motion primitivesMoveC TCP moves along a circular path MoveJ Joint movement MoveL TCP moves along a linear path MoveAbsJ Absolute joint movement

ExampleMoveL p1, v500, fine, tool1;MoveC p2, p3, v500, z20, tool1;MoveC p4, p1, v500, fine, tool1;

MoveL p1, v500, fine, tool1;

Motion data structures

Motion data structures

MoveL p1, v500, fine, tool1;

robtarget - [trans,rot,robconf,extax][[x,y,z],[q1,q2,q3,q4],[cf1,cf4,cf6,cfx],[eax_a, eax_b ... eax_f]

Example:CONST robtarget p15 := [ [600, 500, 225.3], [1, 0, 0, 0], [1, 1, 0, 0],

[ 11, 12.3, 9E9, 9E9, 9E9, 9E9] ];

MoveL p1, v500, fine, tool1;

speeddata - [v_tcp, v_ori, v_leax, v_reax]

Example:VAR speeddata v500 := [ 500, 500, 5000, 1000];

mm/s deg/s mm/s deg/s

Motion data structures

MoveL p1, v500, fine, tool1;

zonedata - [finep, pzone_tcp, pzone_ori, pzone_eax, zone_ori, zone_leax, zone_reax]

Example:VAR zonedata z10:= [FALSE,10,15,15,1.5,15,1.5]

mm mm deg mm mm deg

Motion data structures

MoveL p1, v500, fine, tool1;

tooldata - [robhold,tframe,tload][robhold,[[x,y,z],[q1,q2,q3,q4],[m,[pcx,pcy,pcz],[q1,q2,q3,q4],Ix,Iy,Iz]]

Example:PERS tooldata gripper := [TRUE,[[97.4, 0, 223.1], [0.924, 0, 0.383,0]],

[5, [23, 0, 75], [1, 0, 0, 0], 0, 0, 0]];

Motion data structures

Useful additional comment

MoveL p1, v500, fine, tool1\Wobj:=fixture;

The position and orientation in p1 is related to the work object frame (fixture)Very useful if the program should be independent with respect to where the work object is positioned

References

L. Sciavicco and B. Siciliano: Modelling and Control of Robot Manipulators (Chapter 5)Exjobb, Maria Nyström: Bangenerering förindustrirobotM. Nyström and M. Norrlöf: Path generation for industrial robots, Mekatronikmöte 2003, Göteborg, August 27-28, 2003.M. Norrlöf: On path planning and optimization using splines, LiTH-ISY-R-2490M. Nyström, M. Norrlöf: PGT - A path generation toolbox for Matlab (v0.1), LiTH-ISY-R-2542