Two-link manipulator with point-mass links

This notebook assumes that the definitions in "manipulator_defs.nb" have already been evaluated, and illustrates an example of partitioned control with mass parameter modeling errors.

Global user definitions

Kinematic parameters (variable = DH)

DH  =  {{0, 0, 0, θ1, 0}, {0, L1,  0,  θ2, 
            0},  {0,  L2,  0,  0, 0}};

Dynamic parameters

Link masses ((Subscript[m, i]),  i ∈ {1, 2}) (variable = mass)

mass  =  {m1, m2};

Inertia tensors for cylindrical links ((Subscript[(^C), i])(Subscript[I, i]),  
      i ∈ {1, 2}) (variable = inertia)

inZero  =  Table[0, {3}, {3}];
in1  =  inZero;
in2  =  inZero;
inertia  =  {in1, in2};

Center-of-mass vectors (\(TraditionalForm\`\(\^i\)\(P\_\(C\_i\)\), \ i ∈ {1, 2}\)) (variable = Pc)

Pc  =  {{L1, 0, 0}, {L2, 0, 0}} ;

Gravity vector (variable = gravityVector)

gravityVector  =  {0, g, 0};

Numeric values for parameters

Assumed model parameters (variable = modelParameterTable)

modelParameterTable  =  {{L1, 1}, {L2, 1/2},  {m1, 1},  {m2, 
            1/2}};

Actual robot parameters (variable = robotParameterTable)

robotParameterTable  =  {{L1, 1}, {L2, 1/2},  {m1, 
            1.1},  {m2,  .45}};

Global evaluations

Kinematic parameters

Extraction of variable list

vars  =  ExtractVariables[DH];

Dynamic parameters

If not present, add zero mass for link 0

If[Length[mass]  <  Length[DH], mass = {0}~Join~mass];

If not present, add zero inertia tensor for link 0

If[Length[inertia] < Length[DH],  
        inertia  =  {Table[0, {3}, {3}]}~Join~inertia];

If not present, add zero Pc vector for link 0

If[Length[Pc] < Length[DH],  
        Pc  =  {Table[0, {3}]}~Join~Pc];

Group parameters

dyn  =  {DH,  inertia,  mass,  Pc};

Assignment of gravity vector

a[dh_, 0, 0] := gravityVector;

Numerical parameters

Numerical value for gravity

ng = {g -> 9.81} ;

Conversion of model parameters to rules list

modelParameters = 
        ConvertTableToRules[modelParameterTable]  //  N;

Conversion of actual robot parameters to rules list

robotParameters = 
        ConvertTableToRules[robotParameterTable]  //  N;

Numerical values for DH parameters

nDH  =  DH  /.  robotParameters;

Nice display of global user definitions

Display of DH parameters

DH  // TraditionalForm

Display of mass parameters

mass  //  DisplayMatrixOrVector

Display of inertia tensors

DisplayManipulatorInertiaTensor[inertia, 1]
DisplayManipulatorInertiaTensor[inertia, 2]

Display of center-of-mass vectors

Pc  //  TraditionalForm

Display of model parameters

modelParameterTable  //  TraditionalForm

Display of actual robot parameters

robotParameterTable  //  TraditionalForm

Robot dynamics (potentially computationally intensive)

Compute robot dynamics in form: τ = \(TraditionalForm\`M(Θ) + 
        V(Θ, Θ\& . ) + G(Θ)\)

robot  =  τAll[dyn]  /.  robotParameters  /.  
          ng;

Remove semicolons below to view dynamic equations

DisplayManipulatorDynamicModel[robot, 1]
DisplayManipulatorDynamicModel[robot, 2]

Compute robot dynamics in form: \(TraditionalForm\`Θ\&¨ = \(\(M(Θ)\)\^\(-1\)\)[τ\  - \ 
            V(Θ, \(Θ\& . \)) - 
            G(Θ)]\)

diffeqs  =  ManipulatorDiffEqsFromModel[robot, vars];

Remove semicolons below to view dynamic equations

DisplayManipulatorDifferentialEquation[diffeqs, DH, 1]
DisplayManipulatorDifferentialEquation[diffeqs, DH, 2]

Robot models (potentially computationally intensive)

Compute ideal robot model in terms of M(Θ), V(Θ, \(TraditionalForm\`Θ\& . \)) and G(Θ)

model = τMatrix[dyn]  /.  robotParameters  /.  ng  //  
          N;
idealModel  =  model;

Remove semicolons below to view dynamic equations

DisplayManipulatorMassMatrix[model]
DisplayManipulatorVelocityVector[model]
DisplayManipulatorGravityVector[model]

Compute robot model with parameter errors in terms of M(Θ), V(Θ, \(TraditionalForm\`Θ\& . \)) and G(Θ)

model = τMatrix[dyn]  /.  modelParameters  /.  
          ng; 
nonidealModel = model;

Remove semicolons below to view dynamic equations

DisplayManipulatorMassMatrix[model]
DisplayManipulatorVelocityVector[model]
DisplayManipulatorGravityVector[model]

Partitioned control with mass parameter modeling errors

Controller specification

Controller type (choose from list below) (variable name = controlLaw)

controlLaw  =  PartitionedControl;

Controller options (select one above)

Controller model (used only by control laws GravityCompensation[ ] and PartitionedControl[ ]) (variable name = controlModel)

The two choices are "idealModel" or "nonidealModel".

controlModel  =  nonidealModel;

Control gain vectors (variable names = kp, kv and ki)

kp  =  {100, 100}  //  N;
kv  =  {20, 20}  //  N;
ki  =  {50, 50}  //  N;

Controller frequency (Hz) (variable name = freq)

freq  =  50;

Simulation parameters

Desired trajectory (cubic interpolation) (variable = path)

path  =  {{0, 0, 0}, {10, 90 Degree,  90 Degree}};

Total simulation time

tmax  =  15. ;

Desired trajectory

View desired trajectory

path  //  TraditionalForm

Convert desired trajectory to numeric representation

path  =  path  //  N;

Create desired trajectory as interpolation function

desired  =  
        CreateDesiredTrajectory[path, {0, tmax, 1/freq}];

Controller conversions

Convert gain vectors to gain matrices

If[Length[Dimensions[kp]] == 1, 
        kp = DiagonalMatrix[kp]];
If[Length[Dimensions[kv]] == 1, 
        kv = DiagonalMatrix[kv]];
If[Length[Dimensions[ki]] == 1, 
        ki = DiagonalMatrix[ki]];

Control simulation

Timing[{desiredOut, actual,  torques} = 
          ControlManipulatorFromModel2[robot,  DH,  freq, 
            tmax,  {controlLaw,  desired, kp,  kv,  ki,  controlModel,  
              vars}];]

Visualization of results

Join trajectories

gdesired = 
        PlotMultipleTrajectory[
          desiredOut, {0, tmax}, {{dsh, Red}, {dsh, Blue}}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {{Red}, {Blue}}, 
          nS];
Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desiredOut, {0, tmax}, 
          Subtract];
gerror = 
        PlotMultipleTrajectory[error, {0, tmax}, {{Red}, {Blue}}, 
          imgSize];

Applied torques

gtorques = 
        PlotMultipleTrajectory[torques, {0, tmax}, {{Red}, {Blue}}, 
          imgSize];

Animation of results

Animation specifications

2d or 3d?

dim  =  2;

Number of frames

nFrames  =  150;

Simulation time

tend  =  tmax;

Animation

The blue manipulator shows the actual manipulator trajectory, while the yellow manipulator shows the desired manipulator trajectory.

AnimateTwoTrajectories[nDH,  desiredOut, actual,  dim, nFrames, 
        tend];