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.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
bool MyCarModel::init(std::map<std::string, std::string> &info,
                     std::map<std::string, std::string> &params)
{
    // Get the initial internal state values
    x_[X] = std::stod(info["x"]); //x
    x_[Y] = std::stod(info["y"]); //y
    x_[Z] = std::stod(info["z"]); //y
    x_[THETA] = sc::Angles::deg2rad(std::stod(info["heading"]));

    length_ = sc::get<double>("length", params, 100.0);

    // Set the state_ variable that is used by the entity and the main
    // simulation controller
    state_->vel() << 0, 0, 0;
    state_->pos() << x_[X], x_[Y], x_[Z];
    state_->quat().set(0, 0, x_[THETA]);

    return true;
}

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).

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
void MyCarModel::model(const vector_t &x , vector_t &dxdt , double t)
{
    // Get the controller's actuator inputs
    Eigen::Vector2d &u = std::static_pointer_cast<Controller>(parent_->controllers().back())->u();
    double u_vel = u(FORWARD_VELOCITY);
    double u_theta = u(TURN_RATE);

    // Saturate wheel angle:
    if (u_theta >= M_PI/4) {
        u_theta = M_PI/4 - 0.0001;
    } else if (u_theta <= -M_PI/4) {
        u_theta = -M_PI/4 + 0.0001;
    }

    // Define the state differential
    dxdt[X] = u_vel*cos(x[THETA]);
    dxdt[Y] = u_vel*sin(x[THETA]);
    dxdt[THETA] = u_vel/length_*tan(u_theta);

    dxdt[Z] = 0; // Remain at initial z-position
}

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

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
bool MyCarModel::step(double time, double dt)
{
    // Save the previous x, y, z to calculate velocities
    double prev_x = x_[X];
    double prev_y = x_[Y];
    double prev_z = x_[Z];

    ode_step(dt); // step the motion model ODE solver

    // Save state (position, velocity, orientation) used by simulation
    // controller
    state_->vel() << (x_[X] - prev_x) / dt,
        (x_[Y] - prev_y) / dt,
        (x_[Z] - prev_z) / dt;

    state_->pos() << x_[X], x_[Y], x_[Z];
    state_->quat().set(0, 0, x_[THETA]);
    return true;
}

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>