Adding a new Model

In order to simulate a system or use advanced state estimators/controllers, a mathematical description of the underlying model is needed. In the CRS framework, the dynamic_model packages provide abstract classes which encapsulate a model description.

For a custom model to be used with the CRS framework it must inherit either from the ContinuousDynamicModel or the DiscreteDynamicModel class.

Creating a new Discrete Model

Creating a new discrete model usually requires three main steps:

1. Creation of a custom input, state and parameter struct
2. Creation of the continuous model by inheriting from the Continuous Dynamic Model
3. Creation of a discrete model by discretizing the continuous model using e.g. a rk4 integration.

The following step-by-step guide shows in depth how to create a new model:

Creating a new Dynamic Model Package

1. Copy the package custom_model_template in the crs/dynamic_model package.
2. Change the state and input structs:
   1. custom_model_template/include/custom_model_template/custom_input.h
      Update the struct to match your input. Also, overwrite all provided operators, if possible.
   2. custom_model_template/include/custom_model_template/custom_state.h
      Update the struct to match your state. Also, overwrite all provided operators, if possible.
   3. custom_model_template/include/custom_model_template/custom_params.h
      Define all parameters needed for your model here (e.g. mass, friction coefficient, ...). Also, overwrite the yaml parsing.

3. Define your Dynamics in the custom_model_template/include/custom_model_template/custom_model_continuous.h and custom_model_template/src/custom_model_continuous.cpp

   Sections which need changes are highlighted with // ==================== TODO
   

   1. Update all references to state dimensions <6> and input dimensions <2> with your dimension.
   2. Define your input variables with a human readable name in the constructor custom_model_continuous.h)

   ContinuousCustomModel(custom_params params) : params(params)
   {
    // <================= TODO define your model inputs and state in with variable names here =================>
   +   std::vector<casadi::MX> state_mx = {casadi::MX::sym("x"), casadi::MX::sym("y"),
   +         casadi::MX::sym("yaw"), casadi::MX::sym("v_x"), casadi::MX::sym("v_y"), casadi::MX::sym("yaw_rate")};
   +   std::vector<casadi::MX> input_mx = {casadi::MX::sym("torque"), casadi::MX::sym("steer")};
   
    // <================= END REQUIRED CHANGES =================>
   
     std::vector<casadi::MX> state_dot_mx = getContinuousDynamics(state_mx, input_mx);
     // append control inputs to state vector (casadi functions only take one input)
     for (const auto input : input_mx)
    state_mx.push_back(input);
   
     f_ = casadi::Function("f_dot", state_mx, state_dot_mx);
   } // Constructor
   

   1. Provide the mathematical description in getContinuousDyanmics() of custom_model_template/src/custom_model_continuous.cpp

   std::vector<casadi::MX> ContinuousCustomModel::getContinuousDynamics(const std::vector<casadi::MX> state, const std::vector<casadi::MX> control_input)
      {
    // ========================= TODO IMPLEMENT YOUR DYNAMICS HERE =========================
     using namespace casadi;
   
     // Just for better readability, function inputs (states and control inputs)
   + auto x = state[0];
   + auto y = state[1];
   + auto yaw = state[2];
   + auto v = state[3];
   + auto torque = control_input[0];
   + auto steer = control_input[1];
     // Output of f(state, control)
   + auto x_dot = v * cos(yaw);
   + auto y_dot = v * sin(yaw);
   + auto yaw_dot =  v * sin(beta) / params.lr;
   + auto v_dot = = -1 / params.tau * v + 1 / params.tau * torque;
   + auto state_dot = {x_dot, y_dot, yaw_dot, v_dot};
   
     return state_dot;
     // ========================= END TODO =========================
      }
   

4. Wrap the continuous dynamics into a discrete dynamics wrapper. To do this, update custom_model_template/include/custom_model_template/custom_model_discrete.h

   1. Update all references to state dimensions <6> and input dimensions <2> with your dimension.
   2. Define your input variables with a human readable name in the constructor. Make sure they match the definition provided in the continuous implementation, but also include a 'Ts' input which is used to store the integration time.

   // ==================== TODO define your symbols here ====================
   + std::vector<casadi::MX> state_mx = {casadi::MX::sym("x"), casadi::MX::sym("y"),
   + casadi::MX::sym("yaw"), casadi::MX::sym("v_x"), casadi::MX::sym("v_y"), casadi::MX::sym("yaw_rate")};
   + std::vector<casadi::MX> input_mx = {casadi::MX::sym("torque"), casadi::MX::sym("steer"), casadi::MX::sym("Ts")};
   // ==================== END TODO ====================
   

   1. Update the argument mapping in applyModel(). Make sure that the order of the variables matches the order of the states as defined in (2.)

   // ==================== TODO update argument mapping according to your struct definition ====================
   casadi::Function::MapArg arg = {
   + {"x0", {state.pos_x, state.pos_y, state.yaw, state.vel_x, state.vel_y, state.yaw_rate}},
   + {"p", {control_input.torque, control_input.steer, timestep}}};
   custom_state output_state;
   // ==================== END TODO ====================
   

Done, you can build your your package be executing catkin build custom_model_template

Files to change/ update if a new Model was implemented

CRS files that need to be changed for a new model

Estimator files

Files	Todo	Example file
Estimator config	Create new estimator config	ekf_params.yaml

Sensor Models

Files	Todo	Example file
Sensor model include	Create new sensor model	pacejka_sensor_model/imu_sensor_model.h
Sensor model src	Create new sensor model	pacejka_sensor_model/imu_sensor_model.cpp
Sensor model config	Create new sensor model config	sensor_params.yaml

Controller files

Files	Todo	Example file
Controller include	Create new controller	controls/pid_controller/include
Controller src	Create new controller	controls/pid_controller/src

If a preexisting controller wants to be used with a new model: the .cpp and .h files need to be updated.

ROS files that need to be changed for a new model

Estimator files

Files	Todo	Example file
Estimator config	Create new estimator config	ekf_params.yaml
Estimator include	Update state and input types	ros_estimators/kalman_estimator/pacejka_discrete_ekf.h
Estimator src	Update state and input type and dimensions	ros_estimators/kalman_estimator/pacejka_discrete_ekf.cpp

Controller files

Files	Todo	Example file
Controller config	Create new controller config	ros_controllers/pid_controller.yaml

Simulator files

Files	Todo	Example file
Simulator config	Create new simulator config	pacejka_car_simulator.yaml
Simulator src	Create new simulator src	ros_simulator/ros_pacejka_simulator.cpp
Simulator include	Create new simulator include	ros_simulator/ros_pacejka_simulator.h