Create a Motion Model Plugin

A Motion Model plugin applies the controller’s inputs to the current vehicle’s state to generate a new state. To create a motion model plugin, enter the following at the terminal:

$ cd /path/to/scrimmage/scripts
$ ./generate-plugin.sh motion MyCarModel ~/scrimmage/my-scrimmage-plugins

Build the motion model plugin:

$ cd ~/scrimmage/my-scrimmage-plugins/build
$ cmake ..
$ make

Examine Motion Model Plugin

The Motion Model plugin implements the init method and is used to initialize the motion model’s state variables as well as the state_ instance used by the main SCRIMMAGE simulation controller. It is the job of the motion model to update the state_ variable when the step method is called and during initialization.

 1bool MyCarModel::init(std::map<std::string, std::string> &info,
 2                     std::map<std::string, std::string> &params)
 3{
 4    // Get the initial internal state values
 5    x_[X] = std::stod(info["x"]); //x
 6    x_[Y] = std::stod(info["y"]); //y
 7    x_[Z] = std::stod(info["z"]); //y
 8    x_[THETA] = sc::Angles::deg2rad(std::stod(info["heading"]));
 9
10    length_ = sc::get<double>("length", params, 100.0);
11
12    // Set the state_ variable that is used by the entity and the main
13    // simulation controller
14    state_->vel() << 0, 0, 0;
15    state_->pos() << x_[X], x_[Y], x_[Z];
16    state_->quat().set(0, 0, x_[THETA]);
17
18    return true;
19}

This motion model plugin uses a simple car model that has a minimum turning radius. We will use boost’s ODE solver to numerically integrate the car model’s differential state equations. To do this, the plugin has to implement the model method, which is called when ode_step is called in step. The model method retrieves the controller’s inputs, applies possible state saturations, and implements the differential equations (dx/dt form).

 1void MyCarModel::model(const vector_t &x , vector_t &dxdt , double t)
 2{
 3    // Get the controller's actuator inputs
 4    Eigen::Vector2d &u = std::static_pointer_cast<Controller>(parent_->controllers().back())->u();
 5    double u_vel = u(FORWARD_VELOCITY);
 6    double u_theta = u(TURN_RATE);
 7
 8    // Saturate wheel angle:
 9    if (u_theta >= M_PI/4) {
10        u_theta = M_PI/4 - 0.0001;
11    } else if (u_theta <= -M_PI/4) {
12        u_theta = -M_PI/4 + 0.0001;
13    }
14
15    // Define the state differential
16    dxdt[X] = u_vel*cos(x[THETA]);
17    dxdt[Y] = u_vel*sin(x[THETA]);
18    dxdt[THETA] = u_vel/length_*tan(u_theta);
19
20    dxdt[Z] = 0; // Remain at initial z-position
21}

The model method is called when ode_step is called in step.

 1bool MyCarModel::step(double time, double dt)
 2{
 3    // Save the previous x, y, z to calculate velocities
 4    double prev_x = x_[X];
 5    double prev_y = x_[Y];
 6    double prev_z = x_[Z];
 7
 8    ode_step(dt); // step the motion model ODE solver
 9
10    // Save state (position, velocity, orientation) used by simulation
11    // controller
12    state_->vel() << (x_[X] - prev_x) / dt,
13        (x_[Y] - prev_y) / dt,
14        (x_[Z] - prev_z) / dt;
15
16    state_->pos() << x_[X], x_[Y], x_[Z];
17    state_->quat().set(0, 0, x_[THETA]);
18    return true;
19}

It is the responsibility of the step method to run the numerical integration and update the state_ variable, which is later used by the main SCRIMMAGE simulation controller.

The motion model is assigned to an entity by setting the motion_model XML tag in the entity block:

<entity>
...
<motion_model>MyCarModel</motion_model>
...
</entity>