C-code generation (4-link, point mass)

This notebook assumes that the definitions in "manipulator_defs.nb" have already been evaluated, and allows the user to export robot manipulator dynamic models to C code. It allows the user to define two different dynamic models, one with numeric parameters that correspond to the actual robot, and another with numeric parameters that correspond to an "assumed model." For each dynamic model, two C functions are generated, one that computes the dynamic model as a set of first-order differential equations for Runge Kutta integration, and another that computes the dynamic model in terms of  \(TraditionalForm\`M(Θ), \ \(V(Θ, \ Θ\& . )\)\ and\ \(G(Θ)\)\). All required user inputs can be specified in the Global user definitions.

Global user definitions

Kinematic parameters (variable = DH)

DH  =  {{0, 0, 0, θ1, 0}, {π/2, 0,  0,  θ2, 
            0},  {0,  L1,  0, θ3, 0}, {0, L2, 0, θ4, 0}, {0, 
            L3, 0, 0, 0}};

Dynamic parameters

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

mass  =  {m1, m2, m3, m4};

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

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

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

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

Assumed gravity vector (variable = gravityVectorModel)

gravityVectorModel  =  {0, 0, g};

Actual gravity vector (variable = gravityVectorRobot)

gravityVectorRobot  =  
        g {1/Sqrt[9803], 1/Sqrt[9803], 99/Sqrt[9803]};

Numeric values for parameters

Assumed model parameters (variable = modelParameterTable)

modelParameterTable  =  {{L1, 1}, {L2, 3/4}, {L3, 1/2},  {m1, 
            1},  {m2, 2},  {m3, 3/2}, {m4, 1}};

Actual robot parameters (variable = robotParameterTable)

robotParameterTable  =  {{L1, 1}, {L2, 3/4}, {L3, 1/2},  {m1, 
            1},  {m2, 18/10},  {m3, 1575/1000}, {m4, 11/10}};

Output file name (variable = outputFile)

outputFile  =  "4link_pointmass.c";

Set appropriate directory for file output (OS dependent)

SetDirectory["/Users/nechyba/Desktop"];

C function names (variables = f1, f2, f3 and f4)

These are the function names that are assigned in the C output file. The function name f1 corresponds to the differential equations for the assumed model parameters; the function name f2 corresponds to the matrix representation for the assumed model parameters [\(TraditionalForm\`M(Θ), \ \(V(Θ, \ \(Θ\& . \))\)\ and\ \(G(Θ)\)\)].The function name f3 corresponds to the differential equations for the actual robot parameters; and the function name f4 corresponds to the matrix representation for the actual robot parameters [\(TraditionalForm\`M(Θ), \ \(V(Θ, \ \(Θ\& . \))\)\ and\ \(G(Θ)\)\)].

f1 = "robot_assumed_4link_pointmass";
f2 = "model_assumed_4link_pointmass";
f3 = "robot_actual_4link_pointmass";
f4 = "model_actual_4link_pointmass";

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

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]
DisplayManipulatorInertiaTensor[inertia, 3]
DisplayManipulatorInertiaTensor[inertia, 4]

Display of center-of-mass vectors

Pc  //  TraditionalForm

Display of model parameters

modelParameterTable  //  TraditionalForm

Display of actual robot parameters

robotParameterTable  //  TraditionalForm

Robot C code

Exact robot dynamic model (potentially computationally intensive)

Assignment of gravity vector

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

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

exactModel =  
        Table[τ[dyn, i]  //  TrigReduce, {i, 1, 
            Length[vars]}];

Robot dynamic model

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

robot =  exactModel  /.  robotParameters  //  Chop;

Remove semicolons below to view dynamic equations

DisplayManipulatorDynamicModel[robot, 
        1];
DisplayManipulatorDynamicModel[robot, 
        2];
DisplayManipulatorDynamicModel[robot, 
        3];
DisplayManipulatorDynamicModel[robot, 4];

C code generation

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

robotMatrices  =  
        DynamicModelToMatrixCCode[robot, vars,  ng];

Remove semicolons below to view generated C-code

robotMatrices  // TableForm

Dynamic model as set of first-order differential equations (suitable for Runge-Kutta integration)

robotDiffEqs  =  
        DynamicModelToDiffEqCCode[robot, vars,  ng];

Remove semicolons below to view generated C-code

robotDiffEqs  //  TableForm

Assumed model C code

Exact assumed dynamic model (potentially computationally intensive)

Assignment of gravity vector

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

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

exactModel =  
        Table[τ[dyn, i]  //  TrigReduce, {i, 1, 
            Length[vars]}];

Compute assumed robot dynamics

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

model  =  
        exactModel  /.  modelParameters  //  Chop;

Remove semicolons below to view dynamic equations

DisplayManipulatorDynamicModel[model, 
        1];
DisplayManipulatorDynamicModel[model, 
        2];
DisplayManipulatorDynamicModel[model, 
        3];
DisplayManipulatorDynamicModel[model, 4];

C code generation

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

modelMatrices  =  
        DynamicModelToMatrixCCode[model, vars,  ng];

Remove semicolons below to view generated C-code

modelMatrices  // TableForm

Dynamic model as set of first-order differential equations (suitable for Runge-Kutta integration)

modelDiffEqs  =  
        DynamicModelToDiffEqCCode[model, vars,  ng];

Remove semicolons below to view generated C-code

modelDiffEqs  //  TableForm

Save to disk

Write output file

CompleCCodeToFile[outputFile,  dyn,  modelParameters,  
        robotParameters,  modelDiffEqs, modelMatrices,  robotDiffEqs,  
        robotMatrices,  f1, f2, f3, f4];