Trajectory generation examples

This notebook illustrates examples of trajectory generation using cubic polynomials; it assumes that the definitions in "manipulator_def.nb" have already been evaluated. All user input sections are highlighted as cell boxes.

Simple cubic trajectory derivation from end-point constraints

General solution

End point definitions

\(\(p0 = {0, θ\_0, θ\& . \_0};\)\)
\(\(pf = {t\_f, θ\_f, θ\& . \_f};\)\)

System of equations

SimpleCubicEquationsDisplay[p0, pf]  //  ViewSolution

Solution for coefficients

SimpleCubicCoefficientsDisplay[p0, pf]  // Simplify // 
        Sort //  ViewSolution

Zero velocity at end points

End point definitions

p0 = {0, (Subscript[θ, 0]), 0};
pf = {(Subscript[t, f]), (Subscript[θ, f]), 0};

System of equations

SimpleCubicEquationsDisplay[p0, pf]  //  ViewSolution

Solution for coefficients

SimpleCubicCoefficientsDisplay[p0, pf]  // Simplify // 
        Sort //  ViewSolution

Simple cubic trajectory example

Path endpoints (format = {time, value})

p0 = {0, 15};
pf = {3, 75};

Corresponding cubic polynomial

poly = CubicPolyTrajectory[{p0, pf}, 0, 0]

Plot result

PlotPolyTrajectory[{p0, pf},  {p0[[1]],  pf[[1]] + 2},  
        Blue,  CubicPolyTrajectory, 0, 0, 
        PlotLabel -> "Position"];
PlotDPolyTrajectory[{p0, pf},  {p0[[1]],  pf[[1]] + 2},  
      Blue, CubicPolyTrajectory, 0, 0, PlotLabel -> "Velocity"]; 
    PlotDDPolyTrajectory[{p0, pf},  {p0[[1]],  pf[[1]] + 2},  
      Blue, CubicPolyTrajectory, 0, 0, 
      PlotLabel -> "Acceleration"];

Cubic trajectory with via points (continuous accelerations)

One via point

Path definition

path  =  {{0, (Subscript[θ, 0])}, {(Subscript[t, 1]),  (Subscript[θ, 1])},  {(Subscript[t, 2]),  (Subscript[θ, 2])}};

path  //  TraditionalForm

System of equations

CubicEquationsDisplay[path]  //  ViewSolution

Matrix representation

CubicCoefficientMatrices[path]

Solution for coefficients

CubicCoefficientsDisplay[path]  // Sort //  
      ViewSolution

Example 7.2 (Craig)

CubicCoefficientsDisplay[
          path  /.  {(Subscript[t, 1]) -> t, (Subscript[t, 2]) -> 2  t}]  // Sort //  
      ViewSolution

Two via point

Path definition

path  =  {{0, (Subscript[θ, 0])}, {(Subscript[t, 1]),  (Subscript[θ, 1])},  {(Subscript[t, 2]),  (Subscript[θ, 2])}, {(Subscript[t, 3]),  (Subscript[θ, 3])}};

path  //  TraditionalForm

System of equations

CubicEquationsDisplay[path]  //  ViewSolution

Matrix representation

CubicCoefficientMatrices[path]

Solution for coefficients

CubicCoefficientsDisplay[path]  // Simplify  // Sort //  
      ViewSolution

CubicCoefficientsDisplay[path]  // Sort //  
      ViewSolution

Numeric example (one-dimensional)

Path definition

path  =  {{0,  0}, {5, 60}, {7, 30}, {12, 45}, {20, 
            90}};

tmin  =  First[First[path]];
tmax  =  First[Last[path]] + 5;
path  //  TraditionalForm

Continuous acceleration path

System of equations

CubicEquationsDisplay[path]  //  ViewSolution

Matrix representation

CubicCoefficientMatrices[path]

Solution for coefficients

CubicCoefficientsDisplay[path  //  N]  // Simplify  // 
        Sort //  ViewSolution

Plot result

fnc  =  CubicPolyTrajectory;
g1p = 
        PlotPolyTrajectory[path,  {tmin, tmax},  Blue, fnc, 0, 0, 
          PlotLabel -> "Position"];
g1v = 
      PlotDPolyTrajectory[path,  {tmin, tmax},  Blue, fnc, 0, 0, 
        PlotLabel -> "Velocity"]; 
    g1a = PlotDDPolyTrajectory[path,  {tmin, tmax},  Blue, fnc, 0, 0, 
        PlotLabel -> "Acceleration"];

Path with velocity heuristic

fnc  =  CubicPolyTrajectoryWithVelocityHeuristic;
g2p = 
        PlotPolyTrajectory[path,  {tmin, tmax},  Green,  fnc, 0, 0, 
          PlotLabel -> "Position"];
g2v = 
      PlotDPolyTrajectory[path,  {tmin, tmax},  Green, fnc, 0, 0, 
        PlotLabel -> "Velocity"]; 
    g2a = PlotDDPolyTrajectory[path,  {tmin, tmax},  Green, fnc, 0, 0, 
        PlotLabel -> "Acceleration"];

Compare results

Show[g1p, g2p];
Show[g1v, g2v];
Show[g1a, g2a];

Numeric examples (multi-dimensional)

Path definition

path = {{0,  0, 10}, {5, 60, 30}, {10, 30, 60}, {12, 45, 10}, {20, 
            90, 50}};

tmin  =  First[First[path]];
tmax  =  First[Last[path]] + 5;
path //  TraditionalForm

Continuous acceleration paths

g1 = 
        PlotMultiplePointsTrajectories[path, {tmin, tmax}, {Red, Red}, 
          MultipleCubicPolyTrajectories];

Paths with velocity heuristic

g2 = 
        PlotMultiplePointsTrajectories[path, {tmin, tmax}, {Blue, Blue},  
          MultipleCubicPolyTrajectoriesWithVelocityHeuristic];

Compare results

Show[g1, g2];

Animation of previous result for a two-link manipulator

Compute trajectory as interpolation functions

Interpolation spacing

dt  =  1/10;

Convert path specification to radians

pathRadians  =  ({First[#]}~
                  Join~(Drop[#, 1] Degree)) &  /@  path  // 
          N;

Path conversion

traj1  =  
        CreateDesiredTrajectory[pathRadians,  {tmin, tmax, dt}]  //  
          First;
traj2  =  
        CreateDesiredTrajectory[
            pathRadians, {tmin, tmax, 
              dt}, 
MultipleCubicPolyTrajectoriesWithVelocityHeuristic]  //  
          First;

Kinematic parameters (variable = DH)

DH  =  {{0, 0, 0, θ1, 0}, {0, 1,  0,  θ2, 
            0},  {0,  1/2,  0,  0, 0}};

Animation specifications

2d or 3d?

dim  =  2;

Number of frames

nFrames  =  50;

Animation

The blue manipulator shows the continous acceleration trajectory, while the yellow manipulator shows the trajectory with the velocity heuristic.

AnimateTwoTrajectories[DH,  traj2, traj1,  dim, nFrames, 
        tmax];