Skip to content

Contents

Introduction

To make use of CRS we must first ensure that we have an appropriate model. Currently, CRS includes pacejka and kinematic models for the miniature racing system, and models for parafoil and rocket systems were recently added.

If none of these systems suit your use case then it may be necessary to define a new model. Luckily CRS provides a template so users can easily add a new model and take advantage of CRS' built in functionality! The custom model template can be found in src/crs/dynamic_models/custom_model_template.

By duplicating this entire directory all we will have to do is redefine our paramters, state, and inputs, update the dynamics and rename files accordingly. In this example we use the parafoil_4dof model as a placeholder.

Tip: It makes sense to use a descriptive prefix (e.g: parafoil_4dof_) together with systematic use of the find-and-replace function in your IDE to rename the custom_ model's variables.

prefix model state
custom_(model) custom_model_continuous custom_state
parafoil_4dof_ parafoil_4dof_continuous parafoil_4dof_state

Step-by-Step

Duplicate Template Folder

Duplicate custom_model_template & rename e.g to parafoil_4dof_model

custom_model_template
├── CMakeLists.txt
├── config
│   └── model_params.yaml
├── include
│   └── custom_model_template
│       ├── custom_input.h
│       ├── custom_model_continuous.h
│       ├── custom_model_discrete.h
│       ├── custom_params.h
│       └── custom_state.h
├── package.xml
└── src
   ├── custom_model_continuous.cpp
   ├── custom_model_discrete.cpp
   └── utils
      └── data_conversion.cpp

Configuration Files

Update config/model_params/

  1. rename type
  2. resize Q
  3. update model parameters

Header Files

Update include/custom_model_template/

  1. rename folder
  2. Update custom_params.h
    1. rename file
    2. update ifdefs, e.g. to PARAFOIL_4DOF_MODEL_PARAFOIL_4dof_PARAMS
    3. replace all custom_*, this will automatically
      • rename namespace
      • rename struct
    4. update params to match config
    5. rename/update struct components
    6. update operators
  3. Update custom_state.h
    1. rename file
    2. update ifdefs
    3. find & replace all custom_*
    4. rename/update struct components
    5. update operators
  4. Update custom_input.h
    1. rename file
    2. update ifdefs
    3. find & replace all custom_*
    4. rename/update struct components
    5. update operators
  5. Update custom_model_continuous.h
    1. rename file
    2. update ifdefs
    3. update includes
    4. rename ContinuousCustomModel as desired
    5. find & replace all custom_*
    6. update state/input matrix dimensions
  6. Update custom_model_discrete.h
    1. rename file
    2. update ifdefs
    3. update includes
    4. rename DiscreteCustomModel
    5. find & replace all custom_*
    6. update state/input dimensions

Source Files

Update src/

  1. Update custom_model_continuous.cpp
    1. rename file
    2. update includes
    3. find & replace all custom_*
    4. rename ContinuousCustomModel as desired
    5. rename states/inputs in constructor

  2. Update dynamics in getContinuousDynamics to match your desired model. Shown below are the dynamics for the 4 degrees-of-freedom parafoil model

    /**
* @brief Get the analytical state equations f(state, input) = state_dot
*
* @param state
* @param control_input
* @return std::vector<casadi::MX> returns the analytical state equations f(state, input) = state_dot
*/
std::vector<casadi::MX> ContinuousParafoil4dofModel::getContinuousDynamics(const std::vector<casadi::MX> state,
                                                                     const std::vector<casadi::MX> control_input)
{
using namespace casadi;

// Just for better readability, function inputs (states and control inputs)
auto x = state[0];  // Make sure to match these with states MX defined in constructor
// --snip--
auto w = state[8];
auto deflection_symmetric = control_input[0];
auto deflection_asymmetric = control_input[1];

// Intermediate function values
auto V_a = sqrt(pow(u,2) + pow(w,2));
auto alpha = atan2(w,u);
auto q = params.rho*pow(V_a, 2)/2;
auto L = q*params.S*(params.C_L0 + params.C_Ldelta_s * deflection_symmetric);
auto D = q*params.S*(params.C_D0 + params.C_Ddelta_s * deflection_symmetric);

Eigen::Matrix<casadi::MX, 3, 3> R_bn;
R_bn(0, 0) = cos(psi)*cos(theta);
// --snip--
R_bn(2, 2) = cos(theta)*cos(phi);

Eigen::Matrix<casadi::MX, 3, 1> body_rate(u, v, w);
Eigen::Matrix<casadi::MX, 3, 1> ned_rate = R_bn.transpose() * body_rate;

auto phi_rate = (-phi + params.K_phi * deflection_asymmetric)/params.T_phi;
auto psi_rate = 9.81*tan(phi)/u + w*phi_rate/(u*cos(phi));

// Output of f(state, control)
auto x_dot = ned_rate(0);
auto y_dot = ned_rate(1);
auto z_dot = ned_rate(2);
auto phi_dot = phi_rate;
auto theta_dot = 0.0;
auto psi_dot = psi_rate;
auto u_dot = (L*sin(alpha) - D*cos(alpha))/params.m - w*psi_rate*sin(phi);
auto v_dot = 0.0;
auto w_dot = (-L*cos(alpha) - D*sin(alpha))/params.m + 9.81*cos(phi) + u*psi_rate*sin(phi);

std::vector<casadi::MX> state_dot = { x_dot, y_dot, z_dot, phi_dot, theta_dot, psi_dot, u_dot, v_dot, w_dot };
return state_dot;
}

  3. Update custom_model_discrete.cpp

    1. rename file
    2. update includes
    3. find & replace all custom_*
    4. rename ContinuousCustomModel & DiscreteCustomModel matching header files
    5. rename states/inputs in constructor (matching continuous case)
    6. resize state/input dimensions
  4. Update utils/data_conversion
    1. update includes
    2. find & replace all custom_*
    3. also replace custom_model_template
    4. update state/input vectors
    5. resize state dimensions

Admin Files

Update CMakeLists.txt

  1. rename project
  2. rename libraries

Update package.xml

  1. rename package name
  2. write package description
  3. update author email & name

Review

Folder structure should then be equivalent to the following:

parafoil_4dof_model
├── CMakeLists.txt
├── config
│   └── model_params.yaml
├── include
│   └── parafoil_4dof_model
│       ├── parafoil_4dof_input.h
│       ├── parafoil_4dof_continuous.h
│       ├── parafoil_4dof_discrete.h
│       ├── parafoil_4dof_params.h
│       └── parafoil_4dof_state.h
├── package.xml
└── src
   ├── parafoil_4dof_continuous.cpp
   ├── parafoil_4dof_discrete.cpp
   └── utils
      └── data_conversion.cpp

Up Next

Now that we have defined our system dynamics model we can define our sensor models in the next tutorial. With both of these models defined in CRS we will be able to call these classes/their methods from ROS to easily simulate our system, but let's not get ahead of ourselves