Trajectory Generation• Goal:

– Turn a specified Cartesian-space trajectory of Pe into appropriate joint position reference values

• Steps:– Use inverse kinematics of a robot

manipulator arm to find joint values for any particular location of Pe

– Use sampling and curve fitting to reduce computation

• Output:– a series of joint position/velocity reference

values to send to the controller

5 Step Process for Trajectory Generation

1. Obtain function for workspace path

2. Sample function to get discrete joint pts

3. Apply IK & Jacobian calculations

4. Fit function to joint points5. Sample to get discrete

reference points

1. C

2. D

3. D

4. C

5. D

C=continuous

D=discrete

Step One: Continuous Fcn

• Obtain an analytic function to describe motion with respect to the base frame

• Obtain rate of change of location

Step 2: Sample

• Sample the trajectory to obtain a finite number, m, of sample points on the continuous trajectory:

• Sample rate of change

Step 3: IK & J

• (a) Use inverse kinematics to convert each Cartesian trajectory sample point vector, into a corresponding joint space vector,– Handle multiple solutions,

admissibility, etc.

Step 3: IK & J

• (b) Use the inverse Jacobian relation to convert each velocity vector, into a corresponding joint speed vector, – Handle singular configurations

Step 4: Fit Continuous Curve to Joint Points

• Use the sequence of vectors and i=1,…,m to generate continuous expressions for each joint and j=1,…,dof which pass through or sufficiently near to each of joint space sample points, and rate of change sample points, to produce continuous joint space trajectories for each joint.

Step 4: Fit Continuous Curve to Joint Points

Spline or Polynomial Fit

& derivatives:

Step 4: Fit Continuous Curve to Joint Points

Let’s look at fitting a curve to one interval

Step 4. Fit Continuous Curve to Joint Points

• Fit a continuous function, q(t) to the points:

• Time info – from original sampling• For now use notation (get rid of

subscripts i and i+1):

t=t0

t=tf

Step 4. Fit Continuous Curve to Joint Points

• Splines, polynomials,…• To match position, velocity and

acceleration at end points use a quintic polynomial (6 parameters to match the 6 unknowns):

Step 4. Fit Continuous Curve to Joint Points

• Note: To match only position and velocity at end points use a cubic polynomial (4 parameters to match the 4 unknowns):

Step 4. Fit Continuous Curve to Joint Points

• Use endpoints and time values in quintic polynomial (6 linear equations, 6 unknowns)

Step 4. Fit Continuous Curve to Joint Points

• In matrix form:

Step 4. Fit Continuous Curve to Joint Points

• In matrix form:

Step 4: Computational Thoughts

• Need to perform fit for each joint but…

on each TIME interval, this matrix is the same for each joint – compute inverse only once

Step 4: Fit Continuous Curve to Joint Points

Piecewise polynomials: one polynomial for each joint for each timte interval (and we can easily take derivatives)

…..

one for each time interval(i, i+1)

Step 5: Sample Joint Curve

• Sample each continuous joint trajectory to generate a sequence of discrete reference values for each joint, where ttotal/N is the sampling period used.

Sample

Step 5: Sample Joint Curve

• Sample joint speeds.

Sample

5 Step Process for Trajectory Generation

1. Workspace path function2. Sample -- discrete joint pts3. Apply IK & Jacobian

calculations4. Joint function5. Sample -- discrete reference

points

1. C2. m pts3. m pts

4. C5. N pts

Note: N >> m

m ~ 10 N ~ 1000+

Example: Linear Motion• Step One: Express line as continuous

function (i.e. parameterize by time):– x(t), y(t), (t)

• Suppose we specify the line y= mx +b• Want to move along line with constant

speed, v

Parameterize the Line

• Equation of line

• Differentiate

• Velocity vector

• Substitute

Parameterize the Line

• Velocity

• Speed (magnitude)

• Solve (pick appropriate sign)

Parameterize the Line

• Velocity,

Sample Continuous Path Fcn

• Use M samples for total time, ttotal

time

Example Line• Let’s take a 2-link planar arm with

– Link lengths: l1 = 4, l2 = 3 m– Line y= - x + l1+l2/4 or y=-x +4.75

with speed, v = 3m/s– Start point = (0, l1 + l2 /4)=(0,4.75)– endpoint = (l1 + l2 /4,0)=(4.75,0)– constant speed

• Note, in practice, speed usually follows a trapezoidal profile with acceleration/deceleration at start/end of the motion

Example Line

• Using our 2-link planar data the path has

• And for constant speed will take

Example Line

• We will use M=9 sample points

• And for constant speed we will have

Step 3: IK & Jacobian

• inverse kinematics:

– where

Step 3: IK & Jacobian

• Jacobian relationship:

Step 3: Discrete joint values

Step 3: Discrete Joint Velocity Values

Step 4: Fit Cubic To Positions/Speeds

rmse = 0.014 rad ~ 1 deg

Curve Fitting Comments

• Typically, a single cubic is not sufficient for the entire motion

• A quintic polynomial can fit position, velocity and accelerations of end-points

• What are the implications of fitting only position/velocity?

Reference Positions with 0.02s timesteps (sample cubic function)

Cartesian vs. Joint spaceCONS • Actual robot

position is sometimes unclear, particularly in presence of known obstacles

PROS • Actual control of

the robot occurs in joint space

• Simpler to plan trajectories in real-time; less computation

• No problem with singularities

Cartesian vs. Joint spaceCONS • Need Inverse kinematics

(can be multi-valued)• Smooth trajectory in

Cartesian space may not map to continuous joint space trajectory

• Joint limits may prevent position from being realized

• End-effector paths may not be safe for rest of manipulator

• computational demand and analytic complexity

PROS • We usually desire a

Cartesian path.• Easy to visualize the

trajectory• Occurs in many robotic

activities• Shortest Euclidean path • Straight line path

minimizes inertial forces • Cartesian trajectory can

be robot independent

Path not in workspace

May need to flip between configurations

Start & goal in different solution branches