Open-loop "control" example

This notebook assumes that the definitions in "manipulator_defs.nb" have already been evaluated, and allows the user to conduct sophisticated control experiments. All required user inputs can be specified in the Global user definitions section and in the Experiment-specific user definitions sections. The Control experiment sections that follow the Experiment-specific user definitions sections conduct the actual simulations. Note that the Global sections need to be only evaluated once for any given manipulator. To save more than two control experiments, you can duplicate the Experiment-specific user definitions and Control experiment pair of sections as many times as you want. The  Animation section at the end of this notebook allows you to animate any given control experiment, showing both the desired as well as the actual manipulator trajectories.

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 point mass assumption ((Subscript[(^C), i])(Subscript[I, i]),  
      i ∈ {1, 2}) (variable = inertia)

inNone  = Table[0, {3}, {3}];
in1  =  inNone;
in2  =  inNone;
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

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

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]

Experiment-specific user definitions

Controller specification

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

controlLaw  =  OpenLoopControl;

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  =  robot;

Controller frequency (Hz) (variable name = freq)

freq  =  50;

Simulation parameters

Desired trajectory (cubic interpolation) (variable = path)

path  =  {{0, 0, 0}, {5, 90 Degree,  45 Degree},  {7, 
            45 Degree,  90 Degree}};

Total simulation time

tmax  =  10. ;

Open-loop "control" experiment

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}];

Control simulation

Timing[{desiredOut, actual,  torques} = 
          ControlManipulatorFromModel2[robot,  DH,  freq, 
            tmax,  {controlLaw,  desired, 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  =  100;

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];