Cartesian Trajectory Planning

• Trajectories can include via points– Pass close to but not necessarily pass through

– (knot points in b-splines: graphics)

• E.g. straight line paths connected by via point– Both position and orientation have to be interpolated

p0

p(t1

p2

p1

p(t1

Positional Translation

• Shape of the interpolated region ?– RH Taylor 1979– Start at p0, arrive at p1 in time t1 under

constant velocity– End at p2, from at p1 in time t2 under

constant velocity– At time before arrival at p1 begin the

curved transition thus p(t1- ) and p(t1+ ) are the two transition points

– The curved segment is a parabola with constant acceleration

p0

p(t1

p2

p1

p(t1

• The initial conditions p0, p1, p2, t1and t2 are all specified.

• We now consider the position and velocity constraints of the transition points

p

2

21

1

11

12221

11

01111

11

a (t)pon acceleraticonstant

p

)(p and p

)(p

:sConstraintVelocity

ppp where)(

ppp where)(

:sConstraintPosition

t

tt

t

pt

ptp

pt

ptp

• Integrating the acceleration equation we get

• Rearrange and express in terms of the position function p(t)

• Solve for ap at the second transition point t=t1+

• Substitute into the equation for p(t)

21111

2

0

)(2

))(()()(

2Vs :onacceleraticonstant under motion using

tta

tttptptp

att

p

211

1

11 )(

2)(

tt

att

t

pptp p

1

1

2

2

2

1

t

p

t

pap

21

2

221

1

11 )(

44)(

tt

t

ptt

t

pptp

• The solution to the trajectory reduces to:

• Note the solution does not pass through point p1

211

112

12

2

21

1

21

1

11

11

1 0

44

)(

tttt

ttt

tt

ttt

p

pt

ttp

ttt

pp

pt

ttp

tp

Rotational Transition• The rotational transition is found by finding the equivalent

axis of rotation k

• R0 is the start orientation, R1 the orientation at the via point and R2 the orientation at the goal.

101110 11 RRRRRR T

kk

122

011

212220

to s transform and

to s transform axis-angle where

2

1

22

RRR

RRR

RRRRRR

k

k

Tkk

• The rotation about k1 and k2 can be made a linear function of time

• Rotation along the straight lines

• The rotations can be derived in a similar way to the positions

21122

11

111

11

,

0 ,

2

1

ttttt

ttRRtR

ttt

ttRRtR

k

k

112

2

21

11

21

1 ,4

)(

4

)(11

tttt

ttR

t

ttRRtR kk

Velocity and Acceleration• We need to describe the velocities and

accelerations of tools or of grasped objects

• Position of a link rotating about and origin

• Joint angle velocity

• Same as swinging ball velocity perpendicular to position vector

Oo

x1y1

a1

x0

1

r

v

1

111

1101

0

s

caxad

1111

11101

0 yac

sad

• More general form based on rotation matrices

• Y is found by rotating x by /2

10

1010

1111

11111

0

11

1111

0

11

110

010

11

110

010

2

yd

Rcs

scRR

cs

scRR

xaRdxaRd

• Consider the more general case where the link length is not fixed

010

11

1100

11

11

1010

00

11

1010

00

2

)0(

dpRp

p

pRdp

pRdp

O1

O0

p0

p1

1

d01

y0

x0

x1y1

PR(+/2)p1

3D-Motion

• Derivatives of Rotational Matrices

• Euler angle rates– Representational singularities: Some valid

velocities cannot be represented by Euler angles

• Quaternion Rates– Convert from to q and integrate to get q

IqS

q

dt

d T

0

0

)(2

1

q

q

q

Manipulator Jacobian• Matrix of differentials

• Describe the motion of the tool in terms of changes in the joints

• Jacobian calculated by differentiating the Forward Kinematic transform

CartesianVelocities

JointVelocities

ddx J

dxd 1J

Inverse Kinematic Velocities and Accelerations

• Given a tool speed…. Find angle rates • Inverse Jacobian Method

– Assumes that the Jacobian is non-singular(has an inverse at all points)

– Not true at Singularities

– Very Computationally expensive

• Block Matrix Method– Split the Jacobian up into components exploiting the

geometries of the robot arm

Joint Force and Torque• Gravity acts at the centre of mass• Force/torque equations for link 2

• Force /torque equations for link 12

1

a1

a2

O2

O1

O0

x1

x0

x2

y0

m1g

m2g

z0

z1

2link on 1link by exerted troqueand force2

0

0

1212

12212

122

n

ngd

g

f

m

fm

1link on 2link by

exerted troqueand force2

0

0

2121

21012101101

21011

n

nndgd

g

f

fm

ffm

• Rearranging the equations

• Joint 1 gravity compensation torque compensates for its own weight plus the torque due to link 2

• Forces and torques are generated at the end effector– Again the Jacobian is used to compute the transmitted

forces and torques

gd

dgd

n

g

212

01101

01

2101

22

)(

mm

mmf

Dynamics

• Newton-Euler equations

IIrp

Irp

I

lnmf

nf

lm

m

Equation sEuler'Equation Newtons

TorqueNet ForceNet

MomentumAngular MomentumLinear

InertiaMass

• Calculating the dynamic Joint torques – For the planar manipulator the Newton Euler

equations can be derived – Equations used to determine the driving forces

and torques

• Forward Dynamics– Joint Force and Torque Joint Motion

• Inverse Dynamics– Joint Motion Joint Forces and Torques

• General Manipulator dynamics– Recursive application of Newton-Euler

dynamics

Position Control• Proportional Derivative(PD) control

• Damp the energy out of the motion to stop at end point of path

• K is the proportional part, B is the damping part

• Motion can be • under damped: oscillations• over-damped: sluggish response• or critically damped: best response without

ossilations

dBKdf

Trajectory following

• Proportional Velocity(PV) control– Remove damping except where there are

deviations from the path

• For manipulator each joint can have an independent PV controller

)()( rr ddBddKf

Other Schemes

• Computed Torque Control– Feedback at one joint affects the others– Take these effects into accout

• Resolved Acceleration Control– Use quaternions to allow the use of cartesian

controller

• Resolved Acceleration Force Control– Combine the above with the transmitted forces

as detailed by the J matrix

Manipulator Robotics

• Haptics and Interaction– Force Control and Compliance

• Non Rigid manipulators– Space and surgery

• Task Based and Shared Control– Tele-manipulation