Skip to content

Forces MPCC Solver

The FORCESPRO implementation of the Pacejka MPCC solver uses the Pacejka Model and the FORCESPRO solver to solve the nonlinear optimization problem.

Solver Scripts

The utils.py files formulates the model dynamics, cost functions as well as the constraint functions. The Forces solver is then written in python (generate_forces_solver.py) loads all dynamics, constraint and cost functions from utils.py.

Generating the solver

In order to generate the Forces solver you will need a license. After starting the proxy server and navigating to the script folder you can execute 

./create_solver.sh

The C code is generated and built when the corresponding froces_pacejka_mpcc_solver package is built using catkin build froces_pacejka_mpcc_solver or crs build. Alternatively, one can manually generate code:

crs generate
For more detailed information on how to start the proxy server and generate solvers using Forces see FORCESPRO Solver

CRS Interface

The file froces_pacejka_mpcc_solver.cpp formulates the interfaced between the solver and the CRS framework. This file is used to e.g. set model parameters used in the solver or formulate useful helper functions such as getHorizonLength(), which allow us to access solver specific information from CRS.

Solver Parameters

For each stage of the MPC formulation, the following parameters need to be provided.

tracking_costs

  • Q1 Contouring Cost : Punishes the lateral error to the reference track point
  • Q2 Lag Cost : Punishes the longitudinal error to the reference track point
  • R1 dTorque Cost : Punishes changes in torque
  • R2 dSteer Cost : Punishes changes in steering
  • R3 dArclength Cost : (Reference Sampling) punishes changes in distance between reference points
  • q Arclength Cost : (Reference Sampling) rewards longer distance of reference points

trajectory_track_point

  • x Reference point x coordinate
  • y Reference point y coordinate
  • grad_x Track gradient in x direction
  • grad_y Track gradient in y direction
  • theta Distance on centerline (from start of track)
  • phi Reference yaw angle

image

Example

The following code illustrates the usage of the solver interface

Example Code 
// Last solution is a struct containing two arrays states_, inputs_
// x0, measured state
solver_->setInitialStateConstraint(x0);
// Run solver
for (int current_stage = 0; current_stage < solver_->getHorizonLength(); current_stage++)
{
   // next stage points to current_stage + 1. If current_stage is at end of horizon, next stage directy points to
   // current stage i.e. current_stage = 2 -> next_stage = 3, current_stage = 29 -> next_stage = 29, assuming
   // horizon of 30
   int next_stage = current_stage + (current_stage != solver_->getHorizonLength() - 1);
   mpc_solvers::pacejka_solvers::trajectory_track_point track_point;  // Calculate reference point for this stage
   // ...
   mpc_solvers::pacejka_solvers::tracking_costs mpc_costs  mpc_costs;  // Set mpc costs
   // ...

   solver_->updateParams(current_stage,
                        pacejka_params,      // Model Dynamics
                        mpc_costs,            // Costs
                        track_point           // Tracking point
   );

   // Use previous solution as initial guess this time
   solver_->setStateInitialGuess(current_stage, &last_solution.states_[next_stage * solver_->getStateDimension()]);
   solver_->setInputInitialGuess(current_stage, &last_solution.inputs_[next_stage * solver_->getInputDimension()]);
}
solver_->solve(&last_solution.states_[0], &last_solution.inputs_[0]);