Add CasADi C++ API example

This commit adds a CasADi C++ API example of an Ackermann-steered robot
navigating to a target waypoint within the prediction horizon. The
example plots the planned trajectory and the control acutation.

The commit includes a generalised C++ interface for the MPC controller
together with an example executable and an updated CMake file to build
said example. The commit also includes instructions on building the
Matplot++ library, required for generating the trajectory and control
actuation plots.

Author: taskbjorn

CMakeLists.txt

@@ -3,6 +3,26 @@ cmake_minimum_required(VERSION 3.8)
 project(mpc_playground)
 
 find_package(casadi REQUIRED)
+find_package(Matplot++ REQUIRED)
+
+###############
+# MPC example #
+###############
+
+add_executable(mpc_example
+  "src/mpc_interface.cpp"
+  "src/mpc_ms_ackermann.cpp"
+)
+
+target_include_directories(mpc_example
+  PUBLIC
+    "include"
+)
+
+target_link_libraries(mpc_example
+  casadi
+  Matplot++::matplot
+)
 
 ###############
 # CasADi test #

README.md

@@ -18,6 +18,20 @@ Codes are available here:
 
 ![Differential drive robot example controls plot](assets/img/dd-controls.png)
 
+## Ackermann-steered robot
+
+This example shows an Ackermann-steered robot using a 3 DOF vehicle model and a linear tire. The OCP is solved using multiple shooting.
+
+Codes are available here:
+
+* [Multiple shooting, Ackermann-steered robot (C++)](src/mpc_ms_ackermann.cpp)
+
+![Ackermann robot example trajectory animation](assets/img/ack-trajectory.jpg)
+
+![Ackermann robot example states plot](assets/img/ack-controls-1.jpg)
+
+![Ackermann robot example controls plot](assets/img/ack-controls-2.jpg)
+
 # Getting started
 
 Clone the repository to a folder of your choice:
@@ -97,4 +111,39 @@ CasADi looks for the HSL solvers under a legacy name. You may be required to cre
 ```shell
 cd /usr/lib/local
 ln -S libcoinhsl.so libhsl.so
+```
+
+### (Optional) Building the Matplot++ library from sources
+
+To generate plots at the end of the execution of the C++ examples, the [Matplot++](https://alandefreitas.github.io/matplotplusplus/) library is required. Below are instructions to build the Matplot++ library from sources on Ubuntu 22.04.
+
+Install the required dependencies:
+
+```shell
+sudo apt install -y cmake-curses-gui libfftw3-dev
+```
+
+Clone the [alandefreitas/matplotplusplus](https://github.com/alandefreitas/matplotplusplus) in a folder of your choice (the home directory for the current user `~/` in these instructions):
+
+```shell
+git clone https://github.com/alandefreitas/matplotplusplus ~/matplotplusplus
+```
+
+Create a build folder and configure the build using CMake:
+
+```shell
+mkdir -p ~/matplotplusplus/build/
+cd ~/matplotplusplus/build/
+ccmake ../
+```
+
+Press `C` to configure your project. After configuring the project:
+
+* Set `CMAKE_BUILD_TYPE` to `Release`
+* Set `BUILD_EXAMPLES` to `OFF`
+
+Run the build and install processes:
+
+```shell
+sudo make install -j$((`nproc`+1))
 ```

assets/img/ack-controls-1.jpg

@@ -0,0 +1,171 @@
+#include <string>
+
+#include <casadi/casadi.hpp>
+
+using namespace casadi;
+
+namespace MPC {
+
+class ModelPredictiveController {
+    public:
+        ModelPredictiveController();
+        ~ModelPredictiveController();
+        double GetGoalXPosition();
+        double GetGoalYPosition();
+        double GetLateralSpeedInitialValue();
+        double SetLateralSpeedLowerBound();
+        double GetLateralSpeedUpperBound();
+        double GetLongitudinalAccelerationInitialValue();
+        double GetLongitudinalAccelerationLowerBound();
+        double GetLongitudinalAccelerationUpperBound();
+        double GetLongitudinalJerkInitialValue();
+        double GetLongitudinalJerkLowerBound();
+        double GetLongitudinalJerkUpperBound();
+        double GetLongitudinalSpeedInitialValue();
+        double GetLongitudinalSpeedLowerBound();
+        double GetLongitudinalSpeedUpperBound();
+        std::vector<double> GetOptimalLongitudinalJerk();
+        std::vector<double> GetOptimalSteeringRate();
+        std::vector<double> GetOptimalTimeSpan();
+        std::vector<double> GetOptimalXPosition();
+        std::vector<double> GetOptimalYPosition();
+        double GetSteeringAngleInitialValue();
+        double GetSteeringAngleLowerBound();
+        double GetSteeringAngleUpperBound();
+        double GetSteeringRateInitialValue();
+        double GetSteeringRateLowerBound();
+        double GetSteeringRateUpperBound();
+        double GetXPositionInitialValue();
+        double GetXPositionLowerBound();
+        double GetXPositionUpperBound();
+        double GetYPositionInitialValue();
+        double GetYPositionLowerBound();
+        double GetYPositionUpperBound();
+        double GetYawInitialValue();
+        double GetYawLowerBound();
+        double GetYawUpperBound();
+        double GetYawRateInitialValue();
+        double GetYawRateLowerBound();
+        double GetYawRateUpperBound();
+        void Initialise();
+        void Plan();
+        void SetFrontAxleCentreOfGravityDistance(double d_a);
+        void SetFrontAxleCorneringStiffness(double c_alpha_f);
+        void SetGoalXPosition(double goal_x);
+        void SetGoalYaw(double goal_psi);
+        void SetGoalYPosition(double goal_y);
+        void SetLateralSpeedInitialValue(double v_0);
+        void SetLateralSpeedLowerBound(double v_low);
+        void SetLateralSpeedUpperBound(double v_up);
+        void SetLinearSolver(std::string linear_solver);
+        void SetLinearSolverTolerance(double tol);
+        void SetLinearSolverMaximumIterations(int max_iters);
+        void SetLongitudinalAccelerationInitialValue(double a_0);
+        void SetLongitudinalAccelerationLowerBound(double a_low);
+        void SetLongitudinalAccelerationUpperBound(double a_up);
+        void SetLongitudinalJerkInitialValue(double j_0);
+        void SetLongitudinalJerkLowerBound(double j_low);
+        void SetLongitudinalJerkUpperBound(double j_up);
+        void SetLongitudinalSpeedInitialValue(double u_0);
+        void SetLongitudinalSpeedLowerBound(double u_low);
+        void SetLongitudinalSpeedUpperBound(double u_up);
+        void SetMass(double m);
+        void SetRearAxleCorneringStiffness(double c_alpha_r);
+        void SetRearAxleCentreOfGravityDistance(double d_b);
+        void SetSteeringAngleInitialValue(double delta_0);
+        void SetSteeringAngleLowerBound(double delta_low);
+        void SetSteeringAngleUpperBound(double delta_up);
+        void SetSteeringRateInitialValue(double delta_dot_0);
+        void SetSteeringRateLowerBound(double delta_dot_low);
+        void SetSteeringRateUpperBound(double delta_dot_up);
+        void SetXPositionInitialValue(double x_0);
+        void SetXPositionLowerBound(double x_low);
+        void SetXPositionUpperBound(double x_up);
+        void SetYPositionInitialValue(double y_0);
+        void SetYPositionLowerBound(double y_low);
+        void SetYPositionUpperBound(double y_up);
+        void SetYawInitialValue(double psi_0);
+        void SetYawLowerBound(double psi_low);
+        void SetYawMomentOfInertia(double i_zz);
+        void SetYawRateInitialValue(double r_0);
+        void SetYawRateLowerBound(double r_low);
+        void SetYawRateUpperBound(double r_up);
+        void SetYawUpperBound(double psi_up);
+        void UpdateObjectiveFunction();
+    private:
+        SX a_ = SX::sym("a");
+        double a_0_ = 0.0;
+        double a_low_ = -inf;
+        double a_up_ = inf;
+        double c_alpha_f_ = -5.0e4;
+        double c_alpha_r_ = -5.0e4;
+        SX controls_;
+        int controls_count_;
+        double d_a_ = 1.0;
+        double d_b_ = 1.0;
+        SXDict dae_;
+        SX delta_ = SX::sym("delta");
+        double delta_0_ = 0.0;
+        double delta_low_ = -inf;
+        double delta_up_ = inf;
+        SX delta_dot_ = SX::sym("delta_dot");
+        double delta_dot_0_ = 0.0;
+        double delta_dot_low_ = -inf;
+        std::vector<double> delta_dot_opt_;
+        double delta_dot_up_ = inf;
+        double epsilon_ = 1.0e-4;
+        double goal_psi_ = 0.0;
+        double goal_x_ = 0.0;
+        double goal_y_ = 0.0;
+        double i_zz_ = 1.0e4;
+        Function integrator_;
+        Function integrator_parallel_;
+        Dict integrator_opts_;
+        SX j_ = SX::sym("j");
+        double j_0_ = 0.0;
+        double j_low_ = -inf;
+        std::vector<double> j_opt_;
+        double j_up_ = inf;
+        std::string linear_solver_ = "ma27";
+        Dict nlp_opts_;
+        int nodes_count_ = 10;
+        double m_ = 1800;
+        double max_iters_ = 25;
+        SX ode_;
+        MX phi_ = 0.0;
+        SX psi_ = SX::sym("psi");
+        double psi_0_ = 0.0;
+        double psi_low_ = -inf;
+        std::vector<double> psi_opt_;
+        double psi_up_ = inf;
+        SX quadrature_;
+        SX r_ = SX::sym("r");
+        double r_0_ = 0.0;
+        double r_low_ = -inf;
+        double r_up_ = inf;
+        SX states_;
+        int states_count_;
+        std::vector<double> t_opt_;
+        double time_span_ = 5.0;
+        double tol_ = 1.0e-4;
+        SX u_ = SX::sym("u");
+        double u_0_ = 0.0;
+        double u_low_ = -inf;
+        double u_up_ = inf;
+        SX v_ = SX::sym("v");
+        double v_0_ = 0.0;
+        double v_low_ = -inf;
+        double v_up_ = inf;
+        SX x_ = SX::sym("x");
+        double x_0_ = 0.0;
+        double x_low_ = -inf;
+        std::vector<double> x_opt_;
+        double x_up_ = inf;
+        SX y_ = SX::sym("y");
+        double y_0_ = 0.0;
+        double y_low_ = -inf;
+        std::vector<double> y_opt_;
+        double y_up_ = inf;
+};
+
+} // end namespace MPC