├── .gitignore ├── CMakeLists.txt ├── README.md ├── bounds ├── bounds_manager.cpp ├── bounds_manager.h └── bounds_types.h ├── constraints ├── constraint_manager.cpp ├── constraint_manager.h ├── constraint_types.h ├── tire_ellipsis_constraint.cpp ├── tire_ellipsis_constraint.h ├── tire_slip_constraint.cpp ├── tire_slip_constraint.h ├── track_constraint.cpp └── track_constraint.h ├── costs ├── contouring_error.cpp ├── contouring_error.h ├── cost_manager.cpp ├── cost_manager.h ├── cost_params.cpp ├── cost_params.h ├── cost_types.h ├── slip_angle.cpp └── slip_angle.h ├── install.sh ├── main.cpp ├── models ├── dynamic_bicycle_model.cpp ├── dynamic_bicycle_model.h ├── model_interface.h ├── model_params.cpp ├── model_params.h ├── state.cpp └── state.h ├── mpc ├── mpc.cpp ├── mpc.h ├── mpc_params.cpp ├── mpc_params.h └── mpc_types.h ├── params ├── bounds_params.json ├── cost_params.json ├── model_params.json ├── mpc_params.json └── track.json ├── plotter ├── plotter.cpp └── plotter.h ├── solvers ├── hpipm_interface.cpp └── hpipm_interface.h └── splines ├── binary_search.cpp ├── binary_search.h ├── cubic_spline.cpp ├── cubic_spline.h ├── cubic_spline2d.cpp ├── cubic_spline2d.h ├── track.cpp └── track.h /.gitignore: -------------------------------------------------------------------------------- 1 | # IDE 2 | .idea/ 3 | 4 | # Catkin and CMake generated files 5 | atomic_configure/ 6 | catkin/ 7 | catkin_generated 8 | cmake-build-debug/ 9 | CMakeFiles/ 10 | devel/ 11 | gtest/ 12 | cmake_install.cmake 13 | CMakeCache.txt 14 | Makefile 15 | 16 | # Executables 17 | *.cbp 18 | mpcc 19 | 20 | # Statically linked libraries. These can be installed 21 | # by running ./install.sh 22 | External/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7) 2 | project(mpcc) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | # Release build, full optimisation 6 | set(CMAKE_CXX_FLAGS "-O3") 7 | #set(CMAKE_BUILD_TYPE Debug) 8 | 9 | # Static link the libraries under External 10 | # The libraries can be obtained by running install.sh 11 | include_directories(.) 12 | include_directories(External/blasfeo/lib/include) 13 | include_directories(External/hpipm/lib/include) 14 | include_directories(External/matplotlib) 15 | include_directories(External/Eigen) 16 | include_directories(External/Json/include) 17 | 18 | add_executable(mpcc 19 | main.cpp 20 | models/state.h 21 | models/model_interface.h 22 | models/dynamic_bicycle_model.h 23 | models/dynamic_bicycle_model.cpp 24 | models/model_params.h 25 | models/model_params.cpp 26 | splines/binary_search.h 27 | splines/binary_search.cpp 28 | splines/cubic_spline.cpp 29 | splines/cubic_spline.h 30 | splines/cubic_spline2d.cpp 31 | splines/cubic_spline2d.h 32 | costs/cost_types.h 33 | costs/cost_params.cpp 34 | costs/cost_params.h 35 | constraints/constraint_types.h 36 | costs/contouring_error.cpp 37 | costs/contouring_error.h 38 | costs/cost_manager.cpp 39 | costs/cost_manager.h 40 | costs/slip_angle.cpp 41 | costs/slip_angle.h 42 | constraints/constraint_manager.cpp 43 | constraints/constraint_manager.h 44 | constraints/track_constraint.cpp 45 | constraints/track_constraint.h 46 | splines/track.h 47 | constraints/tire_ellipsis_constraint.cpp 48 | constraints/tire_ellipsis_constraint.h 49 | constraints/tire_slip_constraint.cpp 50 | constraints/tire_slip_constraint.h 51 | bounds/bounds_manager.cpp 52 | bounds/bounds_manager.h 53 | mpc/mpc.cpp 54 | mpc/mpc.h 55 | mpc/mpc_types.h 56 | bounds/bounds_types.h 57 | mpc/mpc_params.cpp 58 | mpc/mpc_params.h 59 | models/state.cpp 60 | solvers/hpipm_interface.cpp 61 | solvers/hpipm_interface.h 62 | splines/track.cpp 63 | plotter/plotter.cpp 64 | plotter/plotter.h) 65 | 66 | # For matplotlib 67 | find_package(PythonLibs REQUIRED) 68 | include_directories(${PYTHON_INCLUDE_DIRS} ${Python2_NumPy_INCLUDE_DIRS}) 69 | target_link_libraries(mpcc ${PYTHON_LIBRARIES}) 70 | 71 | # For HPIPM and Blasfeo 72 | target_link_libraries(mpcc 73 | ${CMAKE_SOURCE_DIR}/External/hpipm/lib/lib/libhpipm.a 74 | ${CMAKE_SOURCE_DIR}/External/blasfeo/lib/lib/libblasfeo.a m) 75 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Model Predictive Contouring Control 2 | 3 | ### Disclaimer 4 | This repository is a rewrite from the original [MPCC](https://github.com/alexliniger/MPCC) by Alex Liniger (AL-MPCC). 5 | Why the rewrite and not forking ? Our team had several differing software ideas with the original repository, 6 | so we opted to rewrite by translating the research paper concept into code, relying on AL-MPCC for reference at times. 7 | Additionally, we only require the C++ version, and we also have a few different constraints from the ones in AL-MPCC. 8 | 9 | ### Installation 10 | 1. Clone this repository: `git clone https://github.com/MURDriverless/mpcc` 11 | 2. Install external dependencies: `sudo sh ./install.sh` on Linux, not sure about other systems 12 | 3. Compile project: `cmake CMakeLists.txt && make` 13 | 4. Run executable: `./mpcc` 14 | -------------------------------------------------------------------------------- /bounds/bounds_manager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "bounds_manager.h" 6 | 7 | using json = nlohmann::json; 8 | 9 | BoundsManager::BoundsManager(const std::string &file_path) { 10 | std::ifstream file_stream(file_path); 11 | json jsonBounds = json::parse(file_stream); 12 | 13 | // Lower state bounds 14 | x_lower(IndexMap.X) = jsonBounds["X_l"]; 15 | x_lower(IndexMap.Y) = jsonBounds["Y_l"]; 16 | x_lower(IndexMap.yaw) = jsonBounds["yaw_l"]; 17 | x_lower(IndexMap.vx) = jsonBounds["vx_l"]; 18 | x_lower(IndexMap.vy) = jsonBounds["vy_l"]; 19 | x_lower(IndexMap.wz) = jsonBounds["wz_l"]; 20 | x_lower(IndexMap.s) = jsonBounds["s_l"]; 21 | x_lower(IndexMap.accel_D) = jsonBounds["accel_D_l"]; 22 | x_lower(IndexMap.steering_angle) = jsonBounds["steering_angle_l"]; 23 | x_lower(IndexMap.vs) = jsonBounds["vs_l"]; 24 | 25 | // Upper state bounds 26 | x_upper(IndexMap.X) = jsonBounds["X_u"]; 27 | x_upper(IndexMap.Y) = jsonBounds["Y_u"]; 28 | x_upper(IndexMap.yaw) = jsonBounds["yaw_u"]; 29 | x_upper(IndexMap.vx) = jsonBounds["vx_u"]; 30 | x_upper(IndexMap.vy) = jsonBounds["vy_u"]; 31 | x_upper(IndexMap.wz) = jsonBounds["wz_u"]; 32 | x_upper(IndexMap.s) = jsonBounds["s_u"]; 33 | x_upper(IndexMap.accel_D) = jsonBounds["accel_D_u"]; 34 | x_upper(IndexMap.steering_angle) = jsonBounds["steering_angle_u"]; 35 | x_upper(IndexMap.vs) = jsonBounds["vs_u"]; 36 | 37 | // Lower input bounds 38 | u_lower(IndexMap.d_accel_D) = jsonBounds["d_accel_D_l"]; 39 | u_lower(IndexMap.d_steering_angle) = jsonBounds["d_steering_angle_l"]; 40 | u_lower(IndexMap.d_vs) = jsonBounds["d_vs_l"]; 41 | 42 | // Upper input bounds 43 | u_upper(IndexMap.d_accel_D) = jsonBounds["d_accel_D_u"]; 44 | u_upper(IndexMap.d_steering_angle) = jsonBounds["d_steering_angle_u"]; 45 | u_upper(IndexMap.d_vs) = jsonBounds["d_vs_u"]; 46 | 47 | // Lower soft bounds 48 | s_lower.setZero(); 49 | 50 | // Upper soft bounds 51 | s_upper.setZero(); 52 | } 53 | -------------------------------------------------------------------------------- /bounds/bounds_manager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_BOUNDS_MANAGER_H 6 | #define MPCC_BOUNDS_MANAGER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "bounds_types.h" 12 | 13 | class BoundsManager { 14 | public: 15 | BoundsManager() = default; 16 | explicit BoundsManager(const std::string &file_path); 17 | 18 | Bounds_x x_lower; 19 | Bounds_x x_upper; 20 | Bounds_u u_lower; 21 | Bounds_u u_upper; 22 | Bounds_s s_lower; 23 | Bounds_s s_upper; 24 | }; 25 | 26 | #endif //MPCC_BOUNDS_MANAGER_H 27 | -------------------------------------------------------------------------------- /bounds/bounds_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_BOUNDS_TYPES_H 6 | #define MPCC_BOUNDS_TYPES_H 7 | 8 | #include 9 | #include "../constraints/constraint_types.h" 10 | #include "../models/state.h" 11 | 12 | // Bounds matrices 13 | typedef Eigen::Matrix Bounds_x; 14 | typedef Eigen::Matrix Bounds_u; 15 | typedef Eigen::Matrix Bounds_s; 16 | 17 | #endif //MPCC_BOUNDS_TYPES_H 18 | -------------------------------------------------------------------------------- /constraints/constraint_manager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "constraint_manager.h" 6 | 7 | ConstraintManager::ConstraintManager(const DynamicBicycleModel &model_args) { 8 | model = model_args; 9 | } 10 | 11 | ConstraintsMatrix ConstraintManager::getConstraints(const Track &track, const State &xk) const { 12 | const Constraint1D track_constraint = TrackConstraint::getTrackConstraint(track, xk); 13 | const Constraint1D tire_constraint_rear = TireEllipsisConstraint::getRearTireConstraint(model, xk); 14 | const Constraint1D tire_constraint_front = TireEllipsisConstraint::getFrontTireConstraint(model, xk); 15 | const Constraint1D alpha_constraint_rear = TireSlipConstraint::getRearAlphaConstraint(model, xk); 16 | const Constraint1D alpha_constraint_front = TireSlipConstraint::getFrontAlphaConstraint(model, xk); 17 | 18 | C_MPC C; 19 | d_MPC dl; 20 | d_MPC du; 21 | 22 | // 1. Track constraint 23 | C.row(ConstraintIndex.track) = track_constraint.C_i; 24 | dl(ConstraintIndex.track) = track_constraint.lower_bound; 25 | du(ConstraintIndex.track) = track_constraint.upper_bound; 26 | 27 | // 2. Rear tire force ellipsis 28 | C.row(ConstraintIndex.tire_rear) = tire_constraint_rear.C_i; 29 | dl(ConstraintIndex.tire_rear) = tire_constraint_rear.lower_bound; 30 | du(ConstraintIndex.tire_rear) = tire_constraint_rear.upper_bound; 31 | 32 | // 3. Front tire force ellipsis 33 | C.row(ConstraintIndex.tire_front) = tire_constraint_front.C_i; 34 | dl(ConstraintIndex.tire_front) = tire_constraint_front.lower_bound; 35 | du(ConstraintIndex.tire_front) = tire_constraint_front.upper_bound; 36 | 37 | // 4. Rear tire slip angle 38 | C.row(ConstraintIndex.alpha_rear) = alpha_constraint_rear.C_i; 39 | dl(ConstraintIndex.alpha_rear) = alpha_constraint_rear.lower_bound; 40 | du(ConstraintIndex.alpha_rear) = alpha_constraint_rear.upper_bound; 41 | 42 | // 5. Front tire slip angle 43 | C.row(ConstraintIndex.alpha_front) = alpha_constraint_front.C_i; 44 | dl(ConstraintIndex.alpha_front) = alpha_constraint_front.lower_bound; 45 | du(ConstraintIndex.alpha_front) = alpha_constraint_front.upper_bound; 46 | 47 | return { C, D_MPC::Zero(), dl, du }; 48 | } 49 | -------------------------------------------------------------------------------- /constraints/constraint_manager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_CONSTRAINT_MANAGER_H 6 | #define MPCC_CONSTRAINT_MANAGER_H 7 | 8 | #include "../models/dynamic_bicycle_model.h" 9 | #include "constraint_types.h" 10 | #include "tire_ellipsis_constraint.h" 11 | #include "tire_slip_constraint.h" 12 | #include "track_constraint.h" 13 | 14 | class ConstraintManager { 15 | public: 16 | ConstraintManager() = default; 17 | explicit ConstraintManager(const DynamicBicycleModel &model_args); 18 | ConstraintsMatrix getConstraints(const Track &track, const State &xk) const; 19 | private: 20 | DynamicBicycleModel model; 21 | }; 22 | 23 | #endif //MPCC_CONSTRAINT_MANAGER_H 24 | -------------------------------------------------------------------------------- /constraints/constraint_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_CONSTRAINT_TYPES_H 6 | #define MPCC_CONSTRAINT_TYPES_H 7 | 8 | #include 9 | #include "../models/state.h" 10 | 11 | using Eigen::Vector2d; 12 | using Eigen::Matrix; 13 | 14 | 15 | /* Polytopic constraint constants */ 16 | // 17 | // Number of polytopic constraints: 18 | // 1. Track constraint (lateral deviation) 19 | // 2. Rear tire forces ellipse constraint 20 | // 3. Front tire forces ellipse constraint 21 | // 4. Rear alpha constraint 22 | // 5. Front alpha constraint 23 | // while state and input constraints are termed as lower and upper "bounds" 24 | #define NPC 5 25 | // Number of soft constraints (we have 5 for all our polytopic constraints): 26 | #define NS 5 27 | // Infinity 28 | static constexpr double INF = 1E5; 29 | 30 | 31 | /* Polytopic constraint types */ 32 | typedef Eigen::Matrix C_MPC; // Jacobian of all constraints versus state 33 | typedef Eigen::Matrix C_i_MPC; // Jacobian of one constraint versus state 34 | typedef Eigen::Matrix D_MPC; // Jacobian of all constraints versus input 35 | typedef Eigen::Matrix d_MPC; // limit of all constraints (min or max) 36 | 37 | struct Constraint1D { 38 | C_i_MPC C_i; 39 | const double lower_bound; 40 | const double upper_bound; 41 | }; 42 | 43 | struct ConstraintsMatrix { 44 | // dl <= C*xk + D*uk <= du 45 | C_MPC C; 46 | D_MPC D; 47 | d_MPC dl; 48 | d_MPC du; 49 | }; 50 | 51 | 52 | /* Polytopic constraint indices */ 53 | struct ConstraintIndexStruct { 54 | int track = 0; // constrain the car within the track 55 | int tire_rear = 1; // rear tire force ellipsis 56 | int tire_front = 2; // front tire force ellipsis 57 | int alpha_rear = 3; // rear tire slip angle 58 | int alpha_front = 4; // front tire slip angle 59 | }; 60 | 61 | static const ConstraintIndexStruct ConstraintIndex; 62 | 63 | #endif //MPCC_CONSTRAINT_TYPES_H 64 | -------------------------------------------------------------------------------- /constraints/tire_ellipsis_constraint.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "tire_ellipsis_constraint.h" 6 | 7 | Constraint1D TireEllipsisConstraint::getRearTireConstraint(const DynamicBicycleModel &model, const State &xk) { 8 | // 0a. Unpack model params 9 | const double e_long = model.params.e_long; 10 | const double e_eps = model.params.e_eps; 11 | const double Dr = model.params.Dr; 12 | // 0b. Set up helper variables 13 | TireForces F_rear = model.getForceRear(xk); 14 | TireForcesDerivatives dF_rear = model.getForceRearDerivatives(xk); 15 | double Fn_rear = model.params.lf / (model.params.lf + model.params.lr) * model.params.m * model.params.g; 16 | double e_long_Fn_2 = pow(e_long / Fn_rear, 2); 17 | double Fn_inv_2 = pow(1.0 / Fn_rear, 2); 18 | 19 | // 1. Calculate Jacobian terms 20 | // Check dynamic_bicycle.cpp to identify non-zero derivatives 21 | // TC (Tire Constraints) = (e_long*(F_{rear,x}/Fn_rear))^2 + (F_{rear,y}/Fn_rear)^2 22 | const double dTC_dvx = e_long_Fn_2 * 2.0 * F_rear.Fx * dF_rear.dFx_vx + 23 | Fn_inv_2 * 2.0 * F_rear.Fy * dF_rear.dFy_vx; 24 | const double dTC_dvy = Fn_inv_2 * 2.0 * F_rear.Fy * dF_rear.dFy_vy; 25 | const double dTC_dwz = Fn_inv_2 * 2.0 * F_rear.Fy * dF_rear.dFy_wz; 26 | const double dTC_daccel_D = e_long_Fn_2 * 2.0 * F_rear.Fx * dF_rear.dFx_accel_D; 27 | 28 | // 2. Construct Jacobian 29 | C_i_MPC J_TC = C_i_MPC::Zero(); 30 | J_TC(IndexMap.vx) = dTC_dvx; 31 | J_TC(IndexMap.vy) = dTC_dvy; 32 | J_TC(IndexMap.wz) = dTC_dwz; 33 | J_TC(IndexMap.accel_D) = dTC_daccel_D; 34 | 35 | // 3. Compute constraint bounds 36 | // 0 <= J_TC*(x - x0) + TC <= F_max 37 | // 0 <= J_TC*x + (-J_TC*x0 + TC) <= F_max 38 | // J_TC*x0 - TC <= J_TC*x <= F_max + J_TC*x0 - TC 39 | // where x is the decision variable and x0 is our linearisation point (xk in our definition). 40 | const double TC = pow(e_long*F_rear.Fx/Fn_rear, 2) + pow(F_rear.Fy/Fn_rear, 2); 41 | const double F_max = pow(e_eps * Dr, 2); 42 | const double lower_bound = J_TC*xk - TC; 43 | const double upper_bound = J_TC*xk + F_max - TC; 44 | 45 | return { J_TC, lower_bound, upper_bound }; 46 | } 47 | 48 | Constraint1D TireEllipsisConstraint::getFrontTireConstraint(const DynamicBicycleModel &model, const State &xk) { 49 | // 0a. Unpack model params 50 | const double e_long = model.params.e_long; 51 | const double e_eps = model.params.e_eps; 52 | const double Df = model.params.Df; 53 | // 0b. Set up helper variables 54 | TireForces F_front = model.getForceFront(xk); 55 | TireForcesDerivatives dF_front = model.getForceFrontDerivatives(xk); 56 | double Fn_front = model.params.lr / (model.params.lf + model.params.lr) * model.params.m * model.params.g; 57 | double Fn_inv_2 = pow(1.0 / Fn_front, 2); 58 | 59 | // 1. Calculate Jacobian terms 60 | // Check dynamic_bicycle.cpp to identify non-zero derivatives 61 | // TC (Tire Constraints) = (param_.e_long*(F_{front,x}/Fn_rear))^2 + (F_{front,y}/Fn_rear)^2 62 | const double dTC_dvx = Fn_inv_2 * 2.0 * F_front.Fy * dF_front.dFy_vx; 63 | const double dTC_dvy = Fn_inv_2 * 2.0 * F_front.Fy * dF_front.dFy_vy; 64 | const double dTC_dwz = Fn_inv_2 * 2.0 * F_front.Fy * dF_front.dFy_wz; 65 | const double dTC_dsteering_angle = 2.0 * F_front.Fy * dF_front.dFy_steering_angle; 66 | 67 | // 2. Construct Jacobian 68 | C_i_MPC J_TC = C_i_MPC::Zero(); 69 | J_TC(IndexMap.vx) = dTC_dvx; 70 | J_TC(IndexMap.vy) = dTC_dvy; 71 | J_TC(IndexMap.wz) = dTC_dwz; 72 | J_TC(IndexMap.steering_angle) = dTC_dsteering_angle; 73 | 74 | // 3. Compute constraint bounds 75 | // 0 <= J_TC*(x - x0) + TC <= F_max 76 | // 0 <= J_TC*x + (-J_TC*x0 + TC) <= F_max 77 | // J_TC*x0 - TC <= J_TC*x <= F_max + J_TC*x0 - TC 78 | // where x is the decision variable and x0 is our linearisation point (xk in our definition). 79 | const double TC = pow(e_long * F_front.Fx / Fn_front, 2) + pow(F_front.Fy / Fn_front, 2); 80 | const double F_max = pow(e_eps * Df, 2); 81 | const double lower_bound = J_TC*xk - TC; 82 | const double upper_bound = J_TC*xk + F_max - TC; 83 | 84 | return { J_TC, lower_bound, upper_bound }; 85 | } 86 | -------------------------------------------------------------------------------- /constraints/tire_ellipsis_constraint.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_TIRE_ELLIPSIS_CONSTRAINT_H 6 | #define MPCC_TIRE_ELLIPSIS_CONSTRAINT_H 7 | 8 | #include "../models/dynamic_bicycle_model.h" 9 | #include "../models/state.h" 10 | #include "constraint_types.h" 11 | 12 | class TireEllipsisConstraint { 13 | public: 14 | // Tire force ellipsis constraint 15 | static Constraint1D getRearTireConstraint(const DynamicBicycleModel &model, const State &xk); 16 | static Constraint1D getFrontTireConstraint(const DynamicBicycleModel &model, const State &xk); 17 | }; 18 | 19 | #endif //MPCC_TIRE_ELLIPSIS_CONSTRAINT_H 20 | -------------------------------------------------------------------------------- /constraints/tire_slip_constraint.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "tire_slip_constraint.h" 6 | 7 | Constraint1D TireSlipConstraint::getRearAlphaConstraint(const DynamicBicycleModel &model, const State &xk) { 8 | // 0. Unpack parameters 9 | const double vx = xk(IndexMap.vx); 10 | const double vy = xk(IndexMap.vy); 11 | const double wz = xk(IndexMap.wz); 12 | const double lr = model.params.lr; 13 | 14 | // 1. Calculate Jacobian terms (use Matlab) 15 | // alpha_r = -atan((vy-wz*lr)/vx) 16 | const double dalpha_dvx = (vy-lr*wz) / (pow(vy-lr*wz, 2) + pow(vx, 2)); 17 | const double dalpha_dvy = -1 / (pow(vy-lr*wz, 2)/vx + vx); 18 | const double dalpha_dwz = lr / (pow(vy-lr*wz, 2)/vx + vx); 19 | 20 | // 2. Construct Jacobian 21 | C_i_MPC J_alpha = C_i_MPC::Zero(); 22 | J_alpha(IndexMap.vx) = dalpha_dvx; 23 | J_alpha(IndexMap.vy) = dalpha_dvy; 24 | J_alpha(IndexMap.wz) = dalpha_dwz; 25 | 26 | // 3. Compute bounds 27 | // -alpha_max <= J_alpha*(x - x0) + alpha <= alpha_max 28 | // -alpha_max <= J_alpha*x + (-J_alpha*x0 + alpha) <= alpha_max 29 | // -alpha_max + J_alpha*x0 - alpha <= J_alpha*x <= alpha_max + J_alpha*x0 - alpha 30 | const double alpha = -atan((vy-wz*lr)/vx); 31 | const double alpha_max = model.params.max_alpha; 32 | const double lower_bound = -alpha_max + J_alpha*xk - alpha; 33 | const double upper_bound = alpha_max + J_alpha*xk - alpha; 34 | 35 | return { J_alpha, lower_bound, upper_bound }; 36 | } 37 | 38 | Constraint1D TireSlipConstraint::getFrontAlphaConstraint(const DynamicBicycleModel &model, const State &xk) { 39 | // 0. Unpack parameters 40 | const double vx = xk(IndexMap.vx); 41 | const double vy = xk(IndexMap.vy); 42 | const double wz = xk(IndexMap.wz); 43 | const double steering_angle = xk(IndexMap.steering_angle); 44 | const double lf = model.params.lf; 45 | 46 | // 1. Calculate Jacobian terms (use Matlab) 47 | // alpha_f = -atan((vy+wz*lf)/vx) + steering_angle. 48 | const double dalpha_dvx = (vy+lf*wz) / (pow(vy+lf*wz, 2) + pow(vx, 2)); 49 | const double dalpha_dvy = -vx / (pow(vy+lf*wz, 2) + pow(vx, 2)); 50 | const double dalpha_dwz = -(vx*lf) / (pow(vy+lf*wz, 2) + pow(vx, 2)); 51 | const double dalpha_dsteering_angle = 1.0; 52 | 53 | // 2. Construct Jacobian 54 | C_i_MPC J_alpha = C_i_MPC::Zero(); 55 | J_alpha(IndexMap.vx) = dalpha_dvx; 56 | J_alpha(IndexMap.vy) = dalpha_dvy; 57 | J_alpha(IndexMap.wz) = dalpha_dwz; 58 | J_alpha(IndexMap.steering_angle) = dalpha_dsteering_angle; 59 | 60 | // 3. Compute bounds 61 | // -alpha_max <= J_alpha*(x - x0) + alpha <= alpha_max 62 | // -alpha_max <= J_alpha*x + (-J_alpha*x0 + alpha) <= alpha_max 63 | // -alpha_max + J_alpha*x0 - alpha <= J_alpha*x <= alpha_max + J_alpha*x0 - alpha 64 | const double alpha = -atan((vy+wz*lf)/vx) + steering_angle; 65 | const double alpha_max = model.params.max_alpha; 66 | const double lower_bound = -alpha_max + J_alpha*xk - alpha; 67 | const double upper_bound = alpha_max + J_alpha*xk - alpha; 68 | 69 | return { J_alpha, lower_bound, upper_bound }; 70 | } 71 | -------------------------------------------------------------------------------- /constraints/tire_slip_constraint.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_TIRE_SLIP_CONSTRAINT_H 6 | #define MPCC_TIRE_SLIP_CONSTRAINT_H 7 | 8 | #include "../models/dynamic_bicycle_model.h" 9 | #include "../models/state.h" 10 | #include "constraint_types.h" 11 | 12 | class TireSlipConstraint { 13 | public: 14 | // Tire slip angle constraint 15 | static Constraint1D getRearAlphaConstraint(const DynamicBicycleModel &model,const State &xk); 16 | static Constraint1D getFrontAlphaConstraint(const DynamicBicycleModel &model, const State &xk); 17 | }; 18 | 19 | #endif //MPCC_TIRE_SLIP_CONSTRAINT_H 20 | -------------------------------------------------------------------------------- /constraints/track_constraint.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "track_constraint.h" 6 | 7 | using Eigen::Matrix2d; 8 | 9 | Constraint1D TrackConstraint::getTrackConstraint(const Track &track, const State &xk, double safety_margin) { 10 | const double s = xk(IndexMap.s); 11 | 12 | const Vector2d outer_pos = track.outer.getPosition(s); 13 | const Vector2d centre_pos = track.centre.getPosition(s); 14 | 15 | // diff_vec = [x_diff; y_diff] 16 | const Vector2d diff_vec = outer_pos - centre_pos; 17 | const double diff_vec_norm_gain = 1 / (pow(diff_vec(0), 2) + pow(diff_vec(1), 2)); 18 | 19 | // Container to fix the inability to use Matrix2d constructor like: Matrix2d { el(0), el(1), el(2), el(3) } 20 | Matrix2d T_matrix; 21 | T_matrix << diff_vec(0), diff_vec(1), 22 | -diff_vec(1), diff_vec(0); 23 | 24 | // G 25 | const Matrix G = diff_vec_norm_gain * T_matrix; 26 | 27 | // Construct Jacobian first 28 | Matrix J_p = Matrix::Zero(); 29 | // dp_dx 30 | J_p.col(IndexMap.X) = G * Vector2d {1.0, 0.0}; 31 | // dp_dy 32 | J_p.col(IndexMap.Y) = G * Vector2d { 0.0, 1.0 }; 33 | 34 | // dG_dtheta 35 | const Vector2d J_centre_pos = track.centre.getDerivative(s); 36 | const Vector2d J_diff_vec = track.outer.getDerivative(s) - J_centre_pos; 37 | // Container to fix the inability to use Matrix2d constructor like: Matrix2d { el(0), el(1), el(2), el(3) } 38 | Matrix2d J_T_matrix; 39 | J_T_matrix << J_diff_vec(0), J_diff_vec(1), 40 | -J_diff_vec(1), J_diff_vec(0); 41 | const Matrix2d dG_dtheta_1 = diff_vec_norm_gain * J_T_matrix; 42 | const Matrix2d dG_dtheta_2 = pow(diff_vec_norm_gain, 2) * 43 | (2*diff_vec(0)*J_diff_vec(0) + 2*diff_vec(1)*J_diff_vec(1)) * 44 | T_matrix; 45 | const Matrix2d dG_dtheta = dG_dtheta_1 - dG_dtheta_2; 46 | 47 | // dp_dtheta 48 | const Vector2d state_from_centre = Vector2d { 49 | xk(IndexMap.X) - centre_pos(0), 50 | xk(IndexMap.Y) - centre_pos(1) 51 | }; 52 | J_p.col(IndexMap.s) = -G * J_centre_pos + dG_dtheta * state_from_centre; 53 | 54 | // Compute limits 55 | const Vector2d p_bar = G * state_from_centre; 56 | const double cross_track_limit_abs = 1.0 - safety_margin; 57 | const Vector2d lower_bound = Vector2d {-cross_track_limit_abs, -INF} - p_bar; 58 | const Vector2d upper_bound = Vector2d {cross_track_limit_abs, INF} - p_bar; 59 | 60 | // Note that we only want to constrain x-prime as it is the lateral part of the track, 61 | // and it does not make sense to constraint the longitudinal part of the track: y-prime 62 | C_i_MPC J_p_x = J_p.row(0); 63 | const double lower_bound_x = lower_bound(0); 64 | const double upper_bound_x = upper_bound(1); 65 | 66 | return { J_p_x, lower_bound_x, upper_bound_x }; 67 | } 68 | -------------------------------------------------------------------------------- /constraints/track_constraint.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_TRACK_CONSTRAINT_H 6 | #define MPCC_TRACK_CONSTRAINT_H 7 | 8 | #include "../splines/track.h" 9 | #include "../constraints/constraint_types.h" 10 | 11 | class TrackConstraint { 12 | public: 13 | // Default is 0.1 (10%): car can only use up to 90% of relative track width 14 | static Constraint1D getTrackConstraint(const Track &track, const State &xk, double safety_margin = 0.1); 15 | }; 16 | 17 | #endif //MPCC_TRACK_CONSTRAINT_H 18 | -------------------------------------------------------------------------------- /costs/contouring_error.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "contouring_error.h" 6 | 7 | RefPoint ContouringErrorDelegate::getRefPoint(const CubicSpline2D &path, const State &xk) { 8 | const double theta_path = xk(IndexMap.s); 9 | const Vector2d x_y = path.getPosition(theta_path); 10 | const Vector2d dx_dy = path.getDerivative(theta_path); 11 | const Vector2d ddx_ddy = path.getSecondDerivative(theta_path); 12 | 13 | // Unpack variables 14 | const double x = x_y(0); 15 | const double y = x_y(1); 16 | const double dx = dx_dy(0); 17 | const double dy = dx_dy(1); 18 | const double yaw = atan2(dx_dy(1), dx_dy(0)); 19 | 20 | // Note that dyaw is not angular rate of change of yaw, because dyaw is partial derivative (we need time 21 | // for angular rate of change). Instead, we now have the curvature of the path. 22 | // Formula for curvature = |dx*ddy + dy*ddx| / (dx^2 + dy^2)^(3/2) 23 | const double ddx = ddx_ddy(0); 24 | const double ddy = ddx_ddy(1); 25 | const double dyaw_numerator = dx*ddy + dy*ddx; 26 | const double dyaw_denominator = pow(dx*dx + dy*dy, 1.5); 27 | const double dyaw = dyaw_numerator / dyaw_denominator; 28 | 29 | return { x, y, dx, dy, yaw, dyaw }; 30 | } 31 | 32 | ContouringError ContouringErrorDelegate::getContouringError(const CubicSpline2D &path, const State &xk) { 33 | const RefPoint ref = getRefPoint(path, xk); 34 | Matrix error = Matrix::Zero(); 35 | Matrix d_error = Matrix::Zero(); 36 | 37 | // Exact error 38 | error(0) = sin(ref.yaw)*(xk(IndexMap.X) - ref.x) - cos(ref.yaw)*(xk(IndexMap.Y) - ref.y); 39 | error(1) = -cos(ref.yaw)*(xk(IndexMap.X) - ref.x) - sin(ref.yaw)*(xk(IndexMap.Y) - ref.y); 40 | 41 | // d_contour_error and d_lag_error are partial derivatives of the errors with respect to the path parameter 42 | const double d_contour_error = ref.dyaw*cos(ref.yaw)*(xk(IndexMap.X) - ref.x) - ref.dx*sin(ref.yaw) + 43 | ref.dyaw*sin(ref.yaw)*(xk(IndexMap.Y) - ref.y) + ref.dy*cos(ref.yaw); 44 | 45 | const double d_lag_error = ref.dyaw*sin(ref.yaw)*(xk(IndexMap.X) - ref.x) + ref.dx*cos(ref.yaw) - 46 | ref.dyaw*cos(ref.yaw)*(xk(IndexMap.Y) - ref.y) + ref.dy*sin(ref.yaw); 47 | 48 | // Jacobian of contouring error 49 | d_error(0, IndexMap.X) = sin(ref.yaw); 50 | d_error(0, IndexMap.Y) = -cos(ref.yaw); 51 | d_error(0, IndexMap.s) = d_contour_error; 52 | 53 | // Jacobian of lag error 54 | d_error(1, IndexMap.X) = -cos(ref.yaw); 55 | d_error(1, IndexMap.Y) = -sin(ref.yaw); 56 | d_error(1, IndexMap.s) = d_lag_error; 57 | 58 | return { error, d_error }; 59 | } 60 | -------------------------------------------------------------------------------- /costs/contouring_error.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_CONTOURING_ERROR_H 6 | #define MPCC_CONTOURING_ERROR_H 7 | 8 | #include 9 | #include "../models/state.h" 10 | #include "../splines/cubic_spline2d.h" 11 | #include "cost_types.h" 12 | 13 | using Eigen::Matrix; 14 | 15 | // Position and partial derivatives of x, y and yaw with respect to theta (spline parameter) 16 | struct RefPoint { 17 | const double x; 18 | const double y; 19 | const double dx; 20 | const double dy; 21 | const double yaw; 22 | const double dyaw; 23 | }; 24 | 25 | // Contains the exact non-linear error and Jacobian of the error at the predicted "xk" 26 | struct ContouringError { 27 | const Matrix error; 28 | const Matrix d_error; 29 | }; 30 | 31 | class ContouringErrorDelegate { 32 | public: 33 | static RefPoint getRefPoint(const CubicSpline2D &path, const State &xk); 34 | static ContouringError getContouringError(const CubicSpline2D &path, const State &xk); 35 | }; 36 | 37 | #endif //MPCC_CONTOURING_ERROR_H 38 | -------------------------------------------------------------------------------- /costs/cost_manager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "cost_manager.h" 6 | 7 | CostManager::CostManager(const CostParams &cost_params, const ModelParams &model_params) { 8 | costParams = cost_params; 9 | modelParams = model_params; 10 | } 11 | 12 | CostMatrix CostManager::getCosts(const CubicSpline2D &path, const State &xk) const { 13 | CostTerm contouring = getContouringCost(path, xk); 14 | CostTerm raw_input = getRawInputCost(); 15 | CostTerm input_change = getInputChangeCost(); 16 | CostTerm soft_constraints = getSoftConstraintsCost(); 17 | CostTerm slip_angle = getBetaCost(xk); 18 | 19 | // Q 20 | Q_MPC Q_not_sym = contouring.quad_cost + raw_input.quad_cost + slip_angle.quad_cost; 21 | Q_not_sym = 2.0 * Q_not_sym; 22 | Q_MPC Q = 0.5 * (Q_not_sym.transpose() + Q_not_sym); 23 | 24 | // R 25 | R_MPC R = input_change.quad_cost; 26 | // Solver expects 0.5*uT*R*u 27 | R = 2.0 * R; 28 | 29 | // S (polytopic cost) will be 0 as it is not used 30 | S_MPC S = S_MPC::Zero(); 31 | 32 | // q 33 | q_MPC q = contouring.lin_cost + raw_input.lin_cost + slip_angle.lin_cost; 34 | // Not sure about the line below, this was implemented in AL-MPCC 35 | q = q + (xk.adjoint()*Q).adjoint(); 36 | 37 | // r, which should be zero 38 | r_MPC r = input_change.lin_cost; 39 | 40 | // Z 41 | Z_MPC Z = soft_constraints.quad_cost; 42 | // Similar to Q and R, quad cost has to be multiplied by 2.0 43 | Z = 2.0 * Z; 44 | 45 | // z 46 | z_MPC z = soft_constraints.lin_cost; 47 | 48 | return { Q, R, S, q, r, Z, z }; 49 | } 50 | 51 | CostTerm CostManager::getContouringCost(const CubicSpline2D &path, const State &xk) const { 52 | const ContouringError contour_error = ContouringErrorDelegate::getContouringError(path, xk); 53 | const Matrix error_bar = contour_error.error - contour_error.d_error * xk; 54 | 55 | Matrix contouring_weights = Matrix::Zero(); 56 | contouring_weights(0, 0) = costParams.q_c; 57 | contouring_weights(1, 1) = costParams.q_l; 58 | 59 | // TODO: Link Github issue or post in README on the error linearisation maths 60 | Q_MPC Q = contour_error.d_error.transpose() * contouring_weights * contour_error.d_error; 61 | q_MPC q = 2 * error_bar.transpose() * contouring_weights * contour_error.d_error; 62 | 63 | // Maximise progression via virtual input 64 | Q(IndexMap.vs, IndexMap.vs) = -costParams.q_vs; 65 | 66 | return { Q, q }; 67 | } 68 | 69 | CostTerm CostManager::getRawInputCost() const { 70 | // Raw input cost is linear, so there are only quadratic terms 71 | Q_MPC Q = Q_MPC::Zero(); 72 | Q(IndexMap.accel_D, IndexMap.accel_D) = costParams.r_accel_D; 73 | Q(IndexMap.steering_angle, IndexMap.steering_angle) = costParams.r_steering_angle; 74 | Q(IndexMap.vs, IndexMap.vs) = costParams.r_vs; 75 | return { Q, q_MPC::Zero() }; 76 | } 77 | 78 | CostTerm CostManager::getInputChangeCost() const { 79 | // Input change cost is also linear, so there are only quadratic terms 80 | R_MPC R = R_MPC::Zero(); 81 | R(IndexMap.d_accel_D, IndexMap.d_accel_D) = costParams.r_d_accel_D; 82 | R(IndexMap.d_steering_angle, IndexMap.d_steering_angle) = costParams.r_d_steering_angle; 83 | R(IndexMap.d_vs, IndexMap.d_vs) = costParams.r_d_vs; 84 | return { R, r_MPC::Zero() }; 85 | } 86 | 87 | CostTerm CostManager::getSoftConstraintsCost() const { 88 | Z_MPC Z_cost = Z_MPC::Zero(); 89 | z_MPC z_cost = z_MPC::Zero(); 90 | // Quadratic cost 91 | Z_cost(ConstraintIndex.track, ConstraintIndex.track) = costParams.sc_quad_track; 92 | Z_cost(ConstraintIndex.tire_rear, ConstraintIndex.tire_rear) = costParams.sc_quad_tire; 93 | Z_cost(ConstraintIndex.tire_front, ConstraintIndex.tire_front) = costParams.sc_quad_tire; 94 | Z_cost(ConstraintIndex.alpha_rear, ConstraintIndex.alpha_rear) = costParams.sc_quad_alpha; 95 | Z_cost(ConstraintIndex.alpha_front, ConstraintIndex.alpha_front) = costParams.sc_quad_alpha; 96 | // Linear cost 97 | z_cost(ConstraintIndex.track) = costParams.sc_lin_track; 98 | z_cost(ConstraintIndex.tire_rear) = costParams.sc_lin_tire; 99 | z_cost(ConstraintIndex.tire_front) = costParams.sc_lin_tire; 100 | z_cost(ConstraintIndex.alpha_rear) = costParams.sc_lin_alpha; 101 | z_cost(ConstraintIndex.alpha_front) = costParams.sc_lin_alpha; 102 | return { Z_cost, z_cost }; 103 | } 104 | 105 | CostTerm CostManager::getBetaCost(const State &xk) const { 106 | LinearisedVar beta_kin = SlipAngleDelegate::getBetaKin(xk, modelParams.lf, modelParams.lr); 107 | LinearisedVar beta_dyn = SlipAngleDelegate::getBetaDyn(xk); 108 | 109 | const double beta_bar = beta_kin.bar - beta_dyn.bar; 110 | const Matrix d_beta_kin = beta_kin.jac; 111 | const Matrix d_beta_dyn = beta_dyn.jac; 112 | const Matrix d_beta = d_beta_kin - d_beta_dyn; 113 | 114 | Q_MPC Q = d_beta.transpose() * costParams.q_beta * d_beta; 115 | q_MPC q = 2 * beta_bar * costParams.q_beta * d_beta; 116 | 117 | return { Q, q }; 118 | } 119 | -------------------------------------------------------------------------------- /costs/cost_manager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_COST_MANAGER_H 6 | #define MPCC_COST_MANAGER_H 7 | 8 | #include "../constraints/constraint_types.h" 9 | #include "../models/model_params.h" 10 | #include "../splines/cubic_spline2d.h" 11 | #include "cost_params.h" 12 | #include "contouring_error.h" 13 | #include "cost_types.h" 14 | #include "slip_angle.h" 15 | 16 | class CostManager { 17 | public: 18 | CostManager() = default; 19 | CostManager(const CostParams &cost_params, const ModelParams &model_params); 20 | CostMatrix getCosts(const CubicSpline2D &path, const State &xk) const; 21 | private: 22 | CostTerm getContouringCost(const CubicSpline2D &path, const State &xk) const; 23 | CostTerm getRawInputCost() const; 24 | CostTerm getInputChangeCost() const; 25 | CostTerm getSoftConstraintsCost() const; 26 | CostTerm getBetaCost(const State &xk) const; 27 | 28 | CostParams costParams; 29 | ModelParams modelParams; 30 | }; 31 | 32 | #endif //MPCC_COST_MANAGER_H 33 | -------------------------------------------------------------------------------- /costs/cost_params.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "cost_params.h" 6 | 7 | using json = nlohmann::json; 8 | 9 | CostParams::CostParams(const std::string &file_path) { 10 | std::ifstream file_stream(file_path); 11 | json jsonCost = json::parse(file_stream); 12 | 13 | // Assign to variables 14 | q_c = jsonCost["q_c"]; 15 | q_l = jsonCost["q_l"]; 16 | q_vs = jsonCost["q_vs"]; 17 | q_beta = jsonCost["q_beta"]; 18 | r_accel_D = jsonCost["r_accel_D"]; 19 | r_steering_angle = jsonCost["r_steering_angle"]; 20 | r_vs = jsonCost["r_vs"]; 21 | r_d_accel_D = jsonCost["r_d_accel_D"]; 22 | r_d_steering_angle = jsonCost["r_d_steering_angle"]; 23 | r_d_vs = jsonCost["r_d_vs"]; 24 | sc_quad_track = jsonCost["sc_quad_track"]; 25 | sc_quad_tire = jsonCost["sc_quad_tire"]; 26 | sc_quad_alpha = jsonCost["sc_quad_alpha"]; 27 | sc_lin_track = jsonCost["sc_lin_track"]; 28 | sc_lin_tire = jsonCost["sc_lin_tire"]; 29 | sc_lin_alpha = jsonCost["sc_lin_alpha"]; 30 | } 31 | -------------------------------------------------------------------------------- /costs/cost_params.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_COST_PARAMS_H 6 | #define MPCC_COST_PARAMS_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | class CostParams { 13 | public: 14 | CostParams() = default; 15 | explicit CostParams(const std::string &file_path); 16 | 17 | double q_c; 18 | double q_l; 19 | double q_vs; 20 | double q_beta; // vehicular slip angle regularisation cost 21 | 22 | double r_accel_D; 23 | double r_steering_angle; 24 | double r_vs; 25 | 26 | double r_d_accel_D; 27 | double r_d_steering_angle; 28 | double r_d_vs; 29 | 30 | double sc_quad_track; 31 | double sc_quad_tire; 32 | double sc_quad_alpha; 33 | 34 | double sc_lin_track; 35 | double sc_lin_tire; 36 | double sc_lin_alpha; 37 | }; 38 | 39 | #endif //MPCC_COST_PARAMS_H 40 | -------------------------------------------------------------------------------- /costs/cost_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_COST_TYPES_H 6 | #define MPCC_COST_TYPES_H 7 | 8 | #include "../models/state.h" 9 | #include "../constraints/constraint_types.h" 10 | 11 | // Quadratic cost 12 | typedef Matrix Q_MPC; // Error cost 13 | typedef Matrix R_MPC; // Input cost 14 | typedef Matrix S_MPC; // Soft constraints cost 15 | 16 | // Linear cost 17 | typedef Matrix q_MPC; 18 | typedef Matrix r_MPC; 19 | 20 | // Soft constraints 21 | typedef Eigen::Matrix Z_MPC; 22 | typedef Eigen::Matrix z_MPC; 23 | 24 | struct CostMatrix { 25 | Q_MPC Q; 26 | R_MPC R; 27 | S_MPC S; 28 | q_MPC q; 29 | r_MPC r; 30 | Z_MPC Z; 31 | z_MPC z; 32 | }; 33 | 34 | // Return type for contouring, input, soft constraints and slip angle cost functions 35 | template 36 | struct CostTerm { 37 | QuadCost quad_cost; 38 | LinCost lin_cost; 39 | }; 40 | 41 | #endif //MPCC_COST_TYPES_H 42 | -------------------------------------------------------------------------------- /costs/slip_angle.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "slip_angle.h" 6 | 7 | LinearisedVar SlipAngleDelegate::getBetaDyn(const State &xk) { 8 | const double vx = xk(IndexMap.vx); 9 | const double vy = xk(IndexMap.vy); 10 | 11 | // Exact value 12 | const double beta_dyn = atan(vy / vx); 13 | 14 | // Partial derivatives with respect to vx and vy, computed symbolically using Matlab 15 | Matrix d_beta_dyn = Matrix::Zero(); 16 | // vx: -vy/(vx^2 + vy^2) 17 | d_beta_dyn(0, IndexMap.vx) = (-vy) / (vx*vx + vy*vy); 18 | // vy: vx/(vx^2 + vy^2) 19 | d_beta_dyn(0, IndexMap.vy) = (vx) / (vx*vx + vy*vy); 20 | 21 | // Setpoint value from linearisation 22 | const double beta_dyn_bar = beta_dyn - d_beta_dyn*xk; 23 | 24 | return { beta_dyn_bar, d_beta_dyn }; 25 | } 26 | 27 | LinearisedVar SlipAngleDelegate::getBetaKin(const State &xk, double lf, double lr) { 28 | const double delta = xk(IndexMap.steering_angle); 29 | 30 | // Exact value 31 | const double beta_kin = atan((lr*tan(delta)) / (lf*lf + lr*lr)); 32 | 33 | // Partial derivatives with respect to steering angle (delta), computed symbollicaly using Matlab 34 | Matrix d_beta_kin = Matrix::Zero(); 35 | // Define shorthand for a simpler formula 36 | const double l_ratio = lr / (lf + lr); 37 | // delta: l_ratio * (tan(delta)^2 + 1) / (l_ratio^2 * tan(delta)^2 + 1) 38 | d_beta_kin(0, IndexMap.steering_angle) = l_ratio*(pow(tan(delta), 2)+1.0) / 39 | (pow(l_ratio, 2)*pow(tan(delta), 2) + 1.0); 40 | 41 | const double beta_kin_bar = beta_kin - d_beta_kin*xk; 42 | 43 | return { beta_kin_bar, d_beta_kin }; 44 | } 45 | -------------------------------------------------------------------------------- /costs/slip_angle.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_SLIP_ANGLE_H 6 | #define MPCC_SLIP_ANGLE_H 7 | 8 | #include 9 | #include "../models/state.h" 10 | 11 | using Eigen::Matrix; 12 | 13 | // General container to hold linearisation variables: setpoint value (bar) and Jacobian (jac) 14 | struct LinearisedVar { 15 | const double bar; 16 | const Matrix jac; 17 | }; 18 | 19 | class SlipAngleDelegate { 20 | public: 21 | // Functions to determine vehicular slip angle beta, keep in mind that alpha denotes tire slip angle 22 | static LinearisedVar getBetaKin(const State &xk, double lf, double lr); 23 | static LinearisedVar getBetaDyn(const State &xk); 24 | }; 25 | 26 | #endif //MPCC_SLIP_ANGLE_H 27 | -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | ## Copyright 2019 Alexander Liniger 3 | 4 | ## Licensed under the Apache License, Version 2.0 (the "License"); 5 | ## you may not use this file except in compliance with the License. 6 | ## You may obtain a copy of the License at 7 | 8 | ## http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | ## Unless required by applicable law or agreed to in writing, software 11 | ## distributed under the License is distributed on an "AS IS" BASIS, 12 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | ## See the License for the specific language governing permissions and 14 | ## limitations under the License. 15 | ########################################################################### 16 | ########################################################################### 17 | ## Install dependencies 18 | set -e 19 | 20 | ## clone blasfeo 21 | repository_blasfeo="https://github.com/giaf/blasfeo.git" 22 | localFolder_blasfeo="External/blasfeo" 23 | git clone "$repository_blasfeo" "$localFolder_blasfeo" 24 | ## clone hpipm 25 | repository_hpipm="https://github.com/giaf/hpipm.git" 26 | localFolder_hpipm="External/hpipm" 27 | git clone "$repository_hpipm" "$localFolder_hpipm" 28 | ## clone matplotlib-cpp 29 | repository_matplotlib="https://github.com/lava/matplotlib-cpp.git" 30 | localFolder_matplotlib="External/matplotlib" 31 | git clone "$repository_matplotlib" "$localFolder_matplotlib" 32 | ## clone eigen 33 | repository_eigen="https://gitlab.com/libeigen/eigen.git" 34 | localFolder_eigen="External/Eigen" 35 | git clone "$repository_eigen" "$localFolder_eigen" 36 | ## clone json 37 | repository_json="https://github.com/nlohmann/json.git" 38 | localFolder_json="External/Json" 39 | git clone "$repository_json" "$localFolder_json" 40 | 41 | 42 | cd External/blasfeo 43 | mkdir -p build 44 | mkdir -p lib 45 | cd build 46 | cmake .. -DCMAKE_INSTALL_PREFIX=$(realpath ../lib) 47 | make 48 | make install 49 | 50 | cd ../../hpipm 51 | mkdir -p build 52 | mkdir -p lib 53 | cd build 54 | cmake .. -DCMAKE_INSTALL_PREFIX=$(realpath ../lib) -DBLASFEO_PATH=$(realpath ../../blasfeo/lib) 55 | make 56 | make install -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include "costs/cost_params.h" 7 | #include "models/model_params.h" 8 | #include "mpc/mpc_params.h" 9 | 10 | #include "models/dynamic_bicycle_model.h" 11 | #include "models/state.h" 12 | 13 | #include "bounds/bounds_manager.h" 14 | #include "constraints/constraint_manager.h" 15 | #include "costs/cost_manager.h" 16 | #include "mpc/mpc.h" 17 | 18 | #include "splines/cubic_spline2d.h" 19 | #include "splines/track.h" 20 | 21 | #include "solvers/hpipm_interface.h" 22 | 23 | #include "plotter/plotter.h" 24 | 25 | using std::string; 26 | using json = nlohmann::json; 27 | using std::cout; 28 | using std::endl; 29 | 30 | int main() { 31 | // Define params file paths 32 | const string bounds_params_file = "params/bounds_params.json"; 33 | const string cost_params_file = "params/cost_params.json"; 34 | const string model_params_file = "params/model_params.json"; 35 | const string mpc_params_file = "params/mpc_params.json"; 36 | const string track_file = "params/track.json"; 37 | 38 | // Create params objects 39 | CostParams cost_params = CostParams(cost_params_file); 40 | ModelParams model_params = ModelParams(model_params_file); 41 | MPCParams mpc_params = MPCParams(mpc_params_file); 42 | 43 | // Create model 44 | DynamicBicycleModel model = DynamicBicycleModel(model_params); 45 | 46 | // Create managers 47 | BoundsManager bounds = BoundsManager(bounds_params_file); 48 | ConstraintManager constraints = ConstraintManager(model); 49 | CostManager costs = CostManager(cost_params, model_params); 50 | 51 | // Create track 52 | Track track = mpcc::getTrack(track_file); 53 | 54 | // Create solver 55 | HpipmInterface solver = HpipmInterface(); 56 | 57 | // Create MPC object 58 | MPC mpc = MPC(bounds, constraints, costs, model, mpc_params, track, solver); 59 | 60 | // Construct initial state 61 | const Vector2d init_pos = track.path.getPosition(0); 62 | const double init_yaw = atan2(init_pos(1), init_pos(0)); 63 | const double v0 = 5.0; 64 | State x0 = { 65 | init_pos(0), 66 | init_pos(1), 67 | init_yaw, 68 | v0, 69 | 0.0, // vy 70 | 0.0, // wz 71 | 0.0, // s 72 | 0.0, // accel_D 73 | 0.0, // steering_angle 74 | v0 75 | }; 76 | 77 | // Logger object 78 | std::list log; 79 | 80 | // Run simulation 81 | int n_sim = 3000; 82 | int i; 83 | for (i = 0; i < n_sim; i++) { 84 | OptSolution mpc_sol = mpc.runMPC(x0); 85 | x0 = model.predictRK4(x0, mpc_sol.u0, mpc_params.Ts); 86 | log.push_back(mpc_sol); 87 | } 88 | 89 | // Run visualisation 90 | Plotter plotter = Plotter(0.05, model_params); 91 | plotter.plot_simulation(log, track); 92 | 93 | // Analyse execution time 94 | double mean_time = 0.0; 95 | double max_time = 0.0; 96 | for (const OptSolution& log_i : log) { 97 | mean_time += log_i.exec_time; 98 | if (log_i.exec_time > max_time) { 99 | max_time = log_i.exec_time; 100 | } 101 | } 102 | mean_time = mean_time / (double)(n_sim); 103 | cout << "Mean MPCC time: " << mean_time << endl; 104 | cout << "Max MPCC time: " << max_time << endl; 105 | cout << "i: " << i << endl; 106 | 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /models/dynamic_bicycle_model.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "dynamic_bicycle_model.h" 6 | 7 | DynamicBicycleModel::DynamicBicycleModel() : params() {} 8 | 9 | DynamicBicycleModel::DynamicBicycleModel(const ModelParams &modelParams) { 10 | this->params = modelParams; 11 | } 12 | 13 | State DynamicBicycleModel::predictRK4(const State &xk, const Input &uk, double Ts) const { 14 | const State k1 = predictContinuous(xk, uk); 15 | const State k2 = predictContinuous((xk + k1*Ts/2.0), uk); 16 | const State k3 = predictContinuous((xk + k2*Ts/2.0), uk); 17 | const State k4 = predictContinuous((xk + k3*Ts), uk); 18 | State xk1 = xk + Ts * (k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0); 19 | return xk1; 20 | } 21 | 22 | LinModelMatrix DynamicBicycleModel::lineariseExpm(const State &xk, const Input &uk, double Ts) const { 23 | // We firstly calculate the Jacobians for linearisation, then we discretise it using expm 24 | LinModelMatrix linModel = calcContinuousJacobian(xk, uk); 25 | 26 | // As per the MPCC issue (https://github.com/alexliniger/MPCC/issues/9), we can define: 27 | // 28 | // \hat{A} = [A, B, g; zeros(NU+1, NX+NU+1)] 29 | // \hat{x} = [x; u; 1] 30 | // 31 | // such that we "lift" the state and input up into a single matrix, so 32 | // the problem just becomes an exponential of this single matrix 33 | // 34 | // \dot{x} = A*x + B*u + g, to 35 | // \dot{\hat{x}} = \hat{A}*\hat{x} 36 | // 37 | // We can then compute the matrix exponential of \dot{\hat{x}} to perform ZOH discretisation. 38 | Eigen::Matrix A_hat = Eigen::Matrix::Zero(); 39 | A_hat.block(0, 0) = linModel.A; 40 | A_hat.block(0, NX) = linModel.B; 41 | A_hat.block(0, NX+NU) = linModel.g; 42 | 43 | // Just reassign to save computation 44 | A_hat = A_hat * Ts; 45 | 46 | // Take the matrix exponential of A_hat 47 | const Eigen::Matrix A_hat_expm = A_hat.exp(); 48 | 49 | // Extract the result 50 | const A_MPC Ad = A_hat_expm.block(0, 0); 51 | const B_MPC Bd = A_hat_expm.block(0, NX); 52 | const g_MPC gd = A_hat_expm.block(0, NX+NU); 53 | return { Ad, Bd, gd }; 54 | } 55 | 56 | double DynamicBicycleModel::getSlipAngleFront(const State &xk) const { 57 | // alpha_f = -atan2(vy + lf*wz, vx) + steering_angle 58 | return -atan2(xk(IndexMap.vy) + xk(IndexMap.wz) * params.lf, xk(IndexMap.vx)) + xk(IndexMap.steering_angle); 59 | } 60 | 61 | double DynamicBicycleModel::getSlipAngleRear(const State &xk) const { 62 | // alpha_r = -atan2(vy - lr*wz, vx) 63 | return -atan2(xk(IndexMap.vy) - xk(IndexMap.wz) * params.lr, xk(IndexMap.vx)); 64 | } 65 | 66 | TireForces DynamicBicycleModel::getForceFront(const State &xk) const { 67 | const double alpha_f = getSlipAngleFront(xk); 68 | // F_fy = Df * sin(Cf * atan(Bf * alpha_f)) 69 | const double Fy = params.Df * sin(params.Cf * atan(params.Bf * alpha_f)); 70 | // F_fx = 0.0 as we assume front wheels do not provide actuation 71 | const double Fx = 0.0; 72 | return { Fy, Fx }; 73 | } 74 | 75 | TireForces DynamicBicycleModel::getForceRear(const State &xk) const { 76 | const double alpha_r = getSlipAngleRear(xk); 77 | // F_ry = Dr * sin(Cr * atan(Br * alpha_r)) 78 | const double Fy = params.Dr * sin(params.Cr * atan(params.Br*alpha_r)); 79 | // F_rx = (Cm1 - Cm2*vx)*accel_D - Cr0 - Cr2*vx^2 80 | const double Fx = (params.Cm1 - params.Cm2*xk(IndexMap.vx)) * xk(IndexMap.accel_D) 81 | - params.Cr0 - params.Cr2*pow(xk(IndexMap.vx), 2); 82 | return { Fy, Fx }; 83 | } 84 | 85 | TireForcesDerivatives DynamicBicycleModel::getForceFrontDerivatives(const State &xk) const { 86 | const double alpha_f = getSlipAngleFront(xk); 87 | const double vx = xk(IndexMap.vx); 88 | const double vy = xk(IndexMap.vy); 89 | const double wz = xk(IndexMap.wz); 90 | 91 | // F_fx = 0.0 as we assume front wheels do not provide actuation 92 | const double dFx_vx = 0.0; 93 | const double dFx_vy = 0.0; 94 | const double dFx_wz = 0.0; 95 | const double dFx_accel_D = 0.0; 96 | const double dFx_steering_angle = 0.0; 97 | // F_fy = Df * sin(Cf * atan(Bf * alpha_f)), where 98 | // alpha_f = -atan2(vy + lf*wz, vx) + steering_angle 99 | const double dFy_vx = (params.Bf*params.Cf*params.Df*cos(params.Cf*atan(params.Bf*alpha_f))) 100 | / (1.0 + pow(params.Bf, 2)*pow(alpha_f, 2)) 101 | * ((params.lf*wz + vy) / (pow(params.lf*wz + vy, 2) + pow(vx, 2))); 102 | const double dFy_vy = (params.Bf*params.Cf*params.Df*cos(params.Cf*atan(params.Bf*alpha_f))) 103 | / (1.0 + pow(params.Bf, 2)*pow(alpha_f, 2)) 104 | * (-vx / (pow(params.lf*wz + vy, 2) + pow(alpha_f, 2))); 105 | const double dFy_wz = (params.Bf*params.Cf*params.Df*cos(params.Cf*atan(params.Bf*alpha_f))) 106 | / (1.0 + pow(params.Bf, 2)*pow(alpha_f, 2)) 107 | * ((-params.lf*vx) / (pow(params.lf*wz + vy, 2) + pow(vx, 2))); 108 | const double dFy_accel_D = 0.0; 109 | const double dFy_steering_angle = (params.Bf*params.Cf*params.Df*cos(params.Cf*atan(params.Bf*alpha_f))) 110 | / (1.0 + pow(params.Bf, 2)*pow(alpha_f, 2)); 111 | return { 112 | dFy_vx, dFy_vy, dFy_wz, dFy_accel_D, dFy_steering_angle, 113 | dFx_vx, dFx_vy, dFx_wz, dFx_accel_D, dFx_steering_angle 114 | }; 115 | } 116 | 117 | TireForcesDerivatives DynamicBicycleModel::getForceRearDerivatives(const State &xk) const { 118 | const double alpha_r = getSlipAngleRear(xk); 119 | const double vx = xk(IndexMap.vx); 120 | const double vy = xk(IndexMap.vy); 121 | const double wz = xk(IndexMap.wz); 122 | const double accel_D = xk(IndexMap.accel_D); 123 | 124 | // F_rx = (Cm1 - Cm2*vx)*accel_D - Cr0 - Cr2*vx^2 125 | const double dFx_vx = -params.Cm2*accel_D - 2*params.Cr2*vx; 126 | const double dFx_vy = 0.0; 127 | const double dFx_wz = 0.0; 128 | const double dFx_accel_D = params.Cm1 - params.Cm2*vx; 129 | const double dFx_steering_angle = 0.0; 130 | // F_ry = Dr * sin(Cr * atan(Br * alpha_r)), where 131 | // alpha_r = -atan2(vy - lr*wz, vx) 132 | const double dFy_vx = ((params.Br*params.Cr*params.Dr*cos(params.Cr*atan(params.Br*alpha_r))) 133 | / (1.0 + pow(params.Br, 2)*pow(alpha_r, 2))) * 134 | (-(params.lr*wz - vy) / (pow((-params.lr*wz + vy), 2) + pow(vx, 2))); 135 | const double dFy_vy = ((params.Br*params.Cr*params.Dr*cos(params.Cr*atan(params.Br*alpha_r))) 136 | / (1.0 + pow(params.Br, 2)*pow(alpha_r, 2))) * 137 | ((-vx) / (pow(-params.lr*wz + vy, 2) + pow(vx, 2))); 138 | const double dFy_wz = ((params.Br*params.Cr*params.Dr*cos(params.Cr*atan(params.Br*alpha_r))) 139 | / (1.0 + pow(params.Br, 2)*pow(alpha_r, 2))) * 140 | ((params.lr*vx) / (pow(-params.lr*wz + vy, 2) + pow(vx, 2))); 141 | const double dFy_accel_D = 0.0; 142 | const double dFy_steering_angle = 0.0; 143 | return { 144 | dFy_vx, dFy_vy, dFy_wz, dFy_accel_D, dFy_steering_angle, 145 | dFx_vx, dFx_vy, dFx_wz, dFx_accel_D, dFx_steering_angle 146 | }; 147 | } 148 | 149 | State DynamicBicycleModel::predictContinuous(const State &xk, const Input &uk) const { 150 | // Destructure state 151 | const double yaw = xk(IndexMap.yaw); 152 | const double vx = xk(IndexMap.vx); 153 | const double vy = xk(IndexMap.vy); 154 | const double wz = xk(IndexMap.wz); 155 | const double steering_angle = xk(IndexMap.steering_angle); 156 | const double vs = xk(IndexMap.vs); 157 | // Destructure input 158 | const double d_accel_D = uk(IndexMap.d_accel_D); 159 | const double d_steering_angle = uk(IndexMap.d_steering_angle); 160 | const double d_vs = uk(IndexMap.d_vs); 161 | 162 | const TireForces front_force = getForceFront(xk); 163 | const TireForces rear_force = getForceRear(xk); 164 | 165 | // TODO: Add torque vectoring term in wz once it is implemented in mursim 166 | // const double wz_target = (steering_angle * vx) / (params.lf + params.lr); 167 | // // Torque vectoring yaw moment, where P_TV is the proportional gain of low level TV controller 168 | // const double tau_TV = (wz_target - wz) * P_TV; 169 | 170 | State xdot; 171 | xdot(IndexMap.X) = vx * cos(yaw) - vy * sin(yaw); 172 | xdot(IndexMap.Y) = vx * sin(yaw) + vy * cos(yaw); 173 | xdot(IndexMap.yaw) = wz; 174 | xdot(IndexMap.vx) = 1.0 / params.m * (rear_force.Fx - front_force.Fy * sin(steering_angle) + params.m * vy * wz); 175 | xdot(IndexMap.vy) = 1.0 / params.m * (rear_force.Fy + front_force.Fy * cos(steering_angle) - params.m * vx * wz); 176 | xdot(IndexMap.wz) = 1.0 / params.Iz * (front_force.Fy * params.lf * cos(steering_angle) - rear_force.Fy * params.lr); 177 | xdot(IndexMap.s) = vs; 178 | xdot(IndexMap.accel_D) = d_accel_D; 179 | xdot(IndexMap.steering_angle) = d_steering_angle; 180 | xdot(IndexMap.vs) = d_vs; 181 | return xdot; 182 | } 183 | 184 | LinModelMatrix DynamicBicycleModel::calcContinuousJacobian(const State &xk, const Input &uk) const { 185 | // Destructure state 186 | const double yaw = xk(IndexMap.yaw); 187 | const double vx = xk(IndexMap.vx); 188 | const double vy = xk(IndexMap.vy); 189 | const double wz = xk(IndexMap.wz); 190 | const double steering_angle = xk(IndexMap.steering_angle); 191 | 192 | A_MPC Ac = A_MPC::Zero(); 193 | B_MPC Bc = B_MPC::Zero(); 194 | g_MPC gc = g_MPC::Zero(); 195 | 196 | const State xdot = predictContinuous(xk, uk); 197 | 198 | const TireForces F_front = getForceFront(xk); 199 | const TireForcesDerivatives dF_front = getForceFrontDerivatives(xk); 200 | const TireForcesDerivatives dF_rear = getForceRearDerivatives(xk); 201 | 202 | // Common term, declare here to minimise repetitive division 203 | const double mass_inv = 1.0 / params.m; 204 | const double Iz_inv = 1.0 / params.Iz; 205 | 206 | // Derivatives of function 207 | // f1 = \dot{X} = vx*cos(yaw) - vy*sin(yaw) 208 | const double df1_dyaw = -vx*sin(yaw) - vy*cos(yaw); 209 | const double df1_dvx = cos(yaw); 210 | const double df1_dvy = -sin(yaw); 211 | 212 | // f2 = \dot{Y} = vy*cos(yaw) + vx*sin(yaw) 213 | const double df2_dyaw = -vy*sin(yaw) + vx*cos(yaw); 214 | const double df2_dvx = sin(yaw); 215 | const double df2_dvy = cos(yaw); 216 | 217 | // f3 = \dot{yaw} = wz 218 | const double df3_dwz = 1.0; 219 | 220 | // f4 = \dot{vx} = 1/m*(F_rx - F_fy*sin(steering_angle) + m*vy*wz); 221 | const double df4_dvx = mass_inv * (dF_rear.dFx_vx - dF_front.dFy_vx*sin(steering_angle)); 222 | const double df4_dvy = mass_inv * (-dF_front.dFy_vy*sin(steering_angle) + params.m*wz); 223 | const double df4_dwz = mass_inv * (-dF_front.dFy_wz*sin(steering_angle) + params.m*vy); 224 | const double df4_daccel_D = mass_inv * dF_rear.dFx_accel_D; 225 | const double df4_dsteering_angle = mass_inv * (-dF_front.dFy_steering_angle*sin(steering_angle) - F_front.Fy*cos(steering_angle)); 226 | 227 | // f5 = \dot{vy} = 1/m*(F_ry + F_fy*cos(steering_angle) - m*vx*wz); 228 | const double df5_dvx = mass_inv * (dF_rear.dFy_vx + dF_front.dFy_vx*cos(steering_angle) - params.m*wz); 229 | const double df5_dvy = mass_inv * (dF_rear.dFy_vy + dF_front.dFy_vy*cos(steering_angle)); 230 | const double df5_dwz = mass_inv * (dF_rear.dFy_wz + dF_front.dFy_wz*cos(steering_angle) - params.m*vx); 231 | const double df5_dsteering_angle = mass_inv * (dF_front.dFy_steering_angle*cos(steering_angle) - F_front.Fy*sin(steering_angle)); 232 | 233 | // f6 = \dot{wz} = 1/Iz*(F_fy*lf*cos(steering_angle)- F_ry*lr) 234 | const double df6_dvx = Iz_inv * (dF_front.dFy_vx*params.lf*cos(steering_angle) - dF_rear.dFy_vx*params.lr); 235 | const double df6_dvy = Iz_inv * (dF_front.dFy_vy*params.lf*cos(steering_angle) - dF_rear.dFy_vy*params.lr); 236 | const double df6_dwz = Iz_inv * (dF_front.dFy_wz*params.lf*cos(steering_angle) - dF_rear.dFy_wz*params.lr); 237 | const double df6_dsteering_angle = Iz_inv * (dF_front.dFy_steering_angle*params.lf*cos(steering_angle) - F_front.Fy*params.lf*sin(steering_angle)); 238 | 239 | // Jacobian of A 240 | // Column 1 (x): all zero 241 | // Column 2 (y): all zero 242 | // Column 3 (yaw): 243 | Ac(IndexMap.X, IndexMap.yaw) = df1_dyaw; 244 | Ac(IndexMap.Y, IndexMap.yaw) = df2_dyaw; 245 | // Column 4 (vx): 246 | Ac(IndexMap.X, IndexMap.vx) = df1_dvx; 247 | Ac(IndexMap.Y, IndexMap.vx) = df2_dvx; 248 | Ac(IndexMap.vx, IndexMap.vx) = df4_dvx; 249 | Ac(IndexMap.vy, IndexMap.vx) = df5_dvx; 250 | Ac(IndexMap.wz, IndexMap.vx) = df6_dvx; 251 | // Column 5 (vy): 252 | Ac(IndexMap.X, IndexMap.vy) = df1_dvy; 253 | Ac(IndexMap.Y, IndexMap.vy) = df2_dvy; 254 | Ac(IndexMap.vx, IndexMap.vy) = df4_dvy; 255 | Ac(IndexMap.vy, IndexMap.vy) = df5_dvy; 256 | Ac(IndexMap.wz, IndexMap.vy) = df6_dvy; 257 | // Column 6 (wz): 258 | Ac(IndexMap.yaw, IndexMap.wz) = df3_dwz; 259 | Ac(IndexMap.vx, IndexMap.wz) = df4_dwz; 260 | Ac(IndexMap.vy, IndexMap.wz) = df5_dwz; 261 | Ac(IndexMap.wz, IndexMap.wz) = df6_dwz; 262 | // Column 7 (s): all zero 263 | // Column 8 (accel_D): 264 | Ac(IndexMap.vx, IndexMap.accel_D) = df4_daccel_D; 265 | // Column 9 (steering angle): 266 | Ac(IndexMap.vx, IndexMap.steering_angle) = df4_dsteering_angle; 267 | Ac(IndexMap.vy, IndexMap.steering_angle) = df5_dsteering_angle; 268 | Ac(IndexMap.wz, IndexMap.steering_angle) = df6_dsteering_angle; 269 | // Column 10 (vs): 270 | Ac(IndexMap.s, IndexMap.vs) = 1.0; 271 | 272 | // Jacobian of B 273 | // Column 1 (d_accel_D): 274 | Bc(IndexMap.accel_D, IndexMap.d_accel_D) = 1.0; 275 | // Column 2 (d_steering_angle): 276 | Bc(IndexMap.steering_angle, IndexMap.d_steering_angle) = 1.0; 277 | // Column 3 (d_vs): 278 | Bc(IndexMap.vs, IndexMap.d_vs) = 1.0; 279 | 280 | // Zero order term 281 | gc = xdot - Ac*xk - Bc*uk; 282 | 283 | return {Ac, Bc, gc }; 284 | } 285 | -------------------------------------------------------------------------------- /models/dynamic_bicycle_model.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_DYNAMIC_BICYCLE_MODEL_H 6 | #define MPCC_DYNAMIC_BICYCLE_MODEL_H 7 | 8 | #include 9 | #include "state.h" 10 | #include "model_interface.h" 11 | #include "model_params.h" 12 | 13 | // Refers to front and rear tire forces 14 | struct TireForces { 15 | // Fy (lateral force) 16 | const double Fy; 17 | // Fx (longitudinal force) 18 | const double Fx; 19 | }; 20 | 21 | // Jacobian terms of tire forces. From the report, the common 22 | // dependencies are vx, vy, wz, accel_D and steering_angle 23 | struct TireForcesDerivatives { 24 | // Fy (lateral force) 25 | const double dFy_vx; 26 | const double dFy_vy; 27 | const double dFy_wz; 28 | const double dFy_accel_D; 29 | const double dFy_steering_angle; 30 | // Fx (longitudinal force) 31 | const double dFx_vx; 32 | const double dFx_vy; 33 | const double dFx_wz; 34 | const double dFx_accel_D; 35 | const double dFx_steering_angle; 36 | }; 37 | 38 | class DynamicBicycleModel : public ModelInterface { 39 | public: 40 | DynamicBicycleModel(); 41 | explicit DynamicBicycleModel(const ModelParams &modelParams); 42 | State predictRK4(const State &xk, const Input &uk, double Ts) const override; 43 | LinModelMatrix lineariseExpm(const State &xk, const Input &uk, double Ts) const override; 44 | 45 | double getSlipAngleFront(const State &xk) const; 46 | double getSlipAngleRear(const State &xk) const; 47 | 48 | TireForces getForceFront(const State &xk) const; 49 | TireForcesDerivatives getForceFrontDerivatives(const State &xk) const; 50 | 51 | TireForces getForceRear(const State &xk) const; 52 | TireForcesDerivatives getForceRearDerivatives(const State &xk) const; 53 | 54 | ModelParams params; 55 | private: 56 | State predictContinuous(const State &xk, const Input &uk) const; 57 | LinModelMatrix calcContinuousJacobian(const State &xk, const Input &uk) const; 58 | }; 59 | 60 | #endif //MPCC_DYNAMIC_BICYCLE_MODEL_H 61 | -------------------------------------------------------------------------------- /models/model_interface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_MODEL_INTERFACE_H 6 | #define MPCC_MODEL_INTERFACE_H 7 | 8 | #include "./state.h" 9 | 10 | class ModelInterface { 11 | public: 12 | // Predict the next state using 4th order Runge-Katta approximate discretisation 13 | virtual State predictRK4(const State &xk, const Input &uk, double Ts) const = 0; 14 | 15 | // For linearising the Jacobians, Eigen has exp() which is the exponential matrix function 16 | virtual LinModelMatrix lineariseExpm(const State &xk, const Input &uk, double Ts) const = 0; 17 | }; 18 | 19 | #endif //MPCC_MODEL_INTERFACE_H 20 | -------------------------------------------------------------------------------- /models/model_params.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "model_params.h" 6 | 7 | using json = nlohmann::json; 8 | 9 | ModelParams::ModelParams(const std::string &file_path) { 10 | std::ifstream file_stream(file_path); 11 | json jsonModel = json::parse(file_stream); 12 | 13 | // Assign to variables 14 | Cm1 = jsonModel["Cm1"]; 15 | Cm2 = jsonModel["Cm2"]; 16 | Cr0 = jsonModel["Cr0"]; 17 | Cr2 = jsonModel["Cr2"]; 18 | Br = jsonModel["Br"]; 19 | Cr = jsonModel["Cr"]; 20 | Dr = jsonModel["Dr"]; 21 | Bf = jsonModel["Bf"]; 22 | Cf = jsonModel["Cf"]; 23 | Df = jsonModel["Df"]; 24 | m = jsonModel["m"]; 25 | Iz = jsonModel["Iz"]; 26 | lf = jsonModel["lf"]; 27 | lr = jsonModel["lr"]; 28 | car_l = jsonModel["car_l"]; 29 | car_w = jsonModel["car_w"]; 30 | g = jsonModel["g"]; 31 | e_long = jsonModel["e_long"]; 32 | e_eps = jsonModel["e_eps"]; 33 | max_alpha = jsonModel["max_alpha"]; 34 | vx_zero = jsonModel["vx_zero"]; 35 | initial_vx = jsonModel["initial_vx"]; 36 | } 37 | -------------------------------------------------------------------------------- /models/model_params.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_MODEL_PARAMS_H 6 | #define MPCC_MODEL_PARAMS_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | class ModelParams { 13 | public: 14 | ModelParams() = default; 15 | explicit ModelParams(const std::string &file_path); 16 | 17 | // Coefficients for determining longitudinal forces 18 | double Cm1; 19 | double Cm2; 20 | double Cr0; 21 | double Cr2; 22 | // Rear tyre coefficients 23 | double Br; 24 | double Cr; 25 | double Dr; 26 | // Front tyre coefficients 27 | double Bf; 28 | double Cf; 29 | double Df; 30 | // Mass of car 31 | double m; 32 | // Moment of inertia about z-axis 33 | double Iz; 34 | // Length of front wheel axle to centre of mass 35 | double lf; 36 | // Length of rear wheel axle to centre of mass 37 | double lr; 38 | // Length of vehicle: sometimes can be approximated to lf + lr 39 | double car_l; 40 | // Width of vehicle 41 | double car_w; 42 | // Acceleration due to gravity 43 | double g; 44 | // Coefficients for tire friction ellipse constraint (see report) 45 | double e_long; 46 | double e_eps; 47 | // Max slip angle 48 | double max_alpha; 49 | // Placeholder to prevent division of vx=0 in our model 50 | double vx_zero; 51 | double initial_vx; 52 | }; 53 | 54 | #endif //MPCC_MODEL_PARAMS_H 55 | -------------------------------------------------------------------------------- /models/state.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "state.h" 6 | 7 | State mpcc::vxNonZero(const State &xk, double min_vx) { 8 | if (xk(IndexMap.vx) < min_vx) { 9 | State xk_nz = xk; 10 | xk_nz(IndexMap.vx) = min_vx; 11 | xk_nz(IndexMap.vy) = 0.0; 12 | xk_nz(IndexMap.wz) = 0.0; 13 | xk_nz(IndexMap.steering_angle) = 0.0; 14 | return xk_nz; 15 | } 16 | return xk; 17 | } 18 | 19 | State mpcc::constrainState(const State &xk, double path_length) { 20 | State xk_cnstr = xk; 21 | if (xk_cnstr(IndexMap.yaw) > M_PI) { 22 | xk_cnstr(IndexMap.yaw) -= 2.0*M_PI; 23 | } 24 | if (xk_cnstr(IndexMap.yaw) < -M_PI) { 25 | xk_cnstr(IndexMap.yaw) += 2.0*M_PI; 26 | } 27 | if (xk_cnstr(IndexMap.s) > path_length) { 28 | xk_cnstr(IndexMap.s) -= path_length; 29 | } 30 | if (xk_cnstr(IndexMap.s) < 0) { 31 | xk_cnstr(IndexMap.s) += path_length; 32 | } 33 | return xk_cnstr; 34 | } 35 | -------------------------------------------------------------------------------- /models/state.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_STATE_H 6 | #define MPCC_STATE_H 7 | 8 | #include 9 | #include 10 | 11 | using Eigen::Matrix; 12 | 13 | #define NX 10 14 | #define NU 3 15 | 16 | struct StateInputIndex { 17 | /* State indices */ 18 | int X = 0; // global x position 19 | int Y = 1; // global y position 20 | int yaw = 2; // global heading of car 21 | int vx = 3; // linear velocity in local frame 22 | int vy = 4; // linear velocity in local frame 23 | int wz = 5; // angular velocity around z-axis (yaw) 24 | int s = 6; // path parameter, track progress (virtual state) 25 | int accel_D = 7; // duty cycle of acceleration: [-1, 1] 26 | int steering_angle = 8; // steering angle in radians 27 | int vs = 9; // "velocity" of path parameter, i.e. rate of change of track progress 28 | 29 | /* Input indices */ 30 | int d_accel_D = 0; 31 | int d_steering_angle = 1; 32 | int d_vs = 2; 33 | }; 34 | 35 | // State vector 36 | typedef Matrix State; 37 | 38 | // Input vector 39 | typedef Matrix Input; 40 | 41 | // Linearised state matrix 42 | typedef Matrix A_MPC; 43 | 44 | // Linearised input matrix 45 | typedef Matrix B_MPC; 46 | 47 | // Linearised offset such that the dynamics forms -> xk1 = A*xk + B*uk + g 48 | typedef Matrix g_MPC; 49 | 50 | // Contrainer to store linear model matrices 51 | struct LinModelMatrix { 52 | A_MPC A; 53 | B_MPC B; 54 | g_MPC g; 55 | }; 56 | 57 | static const StateInputIndex IndexMap; 58 | 59 | namespace mpcc { 60 | State vxNonZero(const State &xk, double min_vx); 61 | State constrainState(const State &xk, double path_length); 62 | } 63 | 64 | #endif //MPCC_STATE_H 65 | -------------------------------------------------------------------------------- /mpc/mpc.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "mpc.h" 6 | 7 | using Eigen::Vector2d; 8 | 9 | MPC::MPC() : 10 | bounds(), 11 | constraints(), 12 | costs(), 13 | model(), 14 | params(), 15 | solver() {} 16 | 17 | MPC::MPC(const BoundsManager &bounds_args, const ConstraintManager &constraint_args, const CostManager &cost_args, 18 | const DynamicBicycleModel &model_args, const MPCParams ¶ms_args, const Track &track_args, 19 | const HpipmInterface &solver_args) { 20 | bounds = bounds_args; 21 | constraints = constraint_args; 22 | costs = cost_args; 23 | model = model_args; 24 | params = params_args; 25 | track = track_args; 26 | solver = solver_args; 27 | } 28 | 29 | OptSolution MPC::runMPC(const State &x0) { 30 | auto t1 = std::chrono::high_resolution_clock::now(); 31 | int solver_status = -1; 32 | State xk = x0; 33 | xk(IndexMap.s) = track.path.projectOnSpline(x0); 34 | xk = mpcc::constrainState(xk, track.path.getLength()); 35 | if (valid_initial_guess) { 36 | updateInitialGuess(xk); 37 | } else { 38 | generateInitialGuess(xk); 39 | } 40 | 41 | params.n_no_solves_sqp = 0; 42 | for (int i = 0; i < params.n_sqp; i++) { 43 | setProblem(); 44 | optimal_solution = solver.solveMPC(stages, x0, &solver_status); 45 | if (solver_status != 0) 46 | params.n_no_solves_sqp++; 47 | if (solver_status <= 1) 48 | initial_guess = sqpMix(initial_guess, optimal_solution); 49 | } 50 | 51 | const int max_error = std::max(params.n_sqp-1, 1); 52 | if (params.n_no_solves_sqp >= max_error) 53 | params.n_non_solves++; 54 | else 55 | params.n_non_solves = 0; 56 | if (params.n_non_solves >= params.n_reset) 57 | valid_initial_guess = false; 58 | 59 | auto t2 = std::chrono::high_resolution_clock::now(); 60 | std::chrono::duration time_span = std::chrono::duration_cast>(t2-t1); 61 | double time_nmpc = time_span.count(); 62 | 63 | return { initial_guess[0].uk, initial_guess, time_nmpc }; 64 | } 65 | 66 | void MPC::setProblem() { 67 | for (int i = 0; i <= N; i++) { 68 | setStage(initial_guess[i].xk, initial_guess[i].uk, i); 69 | } 70 | } 71 | 72 | void MPC::setStage(const State &xk, const Input &uk, int k) { 73 | stages[k].nx = NX; 74 | stages[k].nu = NU; 75 | 76 | // If we're at time-step k=0, which means the current "real" state we are in, 77 | // we have not done any predictions, and no constraints is needed as the state 78 | // has already happened (lol Tenet) 79 | if (k == 0) { 80 | stages[k].ng = 0; 81 | stages[k].ns = 0; 82 | } else { 83 | stages[k].ng = NPC; 84 | stages[k].ns = NS; 85 | } 86 | 87 | State xk_nz = mpcc::vxNonZero(xk, model.params.vx_zero); 88 | // Set 3 core elements of an optimisation problem (model is an exception actually) 89 | stages[k].constraints = constraints.getConstraints(track, xk); 90 | stages[k].costs = costs.getCosts(track.path, xk); 91 | stages[k].model = model.lineariseExpm(xk, uk, params.Ts); 92 | // Set bounds 93 | stages[k].x_lower = bounds.x_lower; 94 | stages[k].x_upper = bounds.x_upper; 95 | stages[k].u_lower = bounds.u_lower; 96 | stages[k].u_upper = bounds.u_upper; 97 | stages[k].s_lower = bounds.s_lower; 98 | stages[k].s_upper = bounds.s_upper; 99 | } 100 | 101 | void MPC::updateInitialGuess(const State &x0) { 102 | for (int i = 1; i < N; i++) { 103 | initial_guess[i-1] = initial_guess[i]; 104 | } 105 | initial_guess[0].xk = x0; 106 | initial_guess[0].uk.setZero(); 107 | initial_guess[N-1].xk = initial_guess[N-2].xk; 108 | initial_guess[N-1].uk.setZero(); 109 | initial_guess[N].xk = model.predictRK4(initial_guess[N-1].xk, initial_guess[N-1].uk, params.Ts); 110 | initial_guess[N].uk.setZero(); 111 | constrainInitialGuess(); 112 | } 113 | 114 | void MPC::generateInitialGuess(const State &x0) { 115 | initial_guess[0].xk = x0; 116 | initial_guess[0].uk.setZero(); 117 | Vector2d path_pos_i; 118 | Vector2d path_dpos_i; 119 | for (int i = 0; i <= N; i++) { 120 | initial_guess[i].xk.setZero(); 121 | initial_guess[i].uk.setZero(); 122 | initial_guess[i].xk(IndexMap.s) = initial_guess[i-1].xk(IndexMap.s) + params.Ts*model.params.initial_vx; 123 | path_pos_i = track.path.getPosition(initial_guess[i].xk(IndexMap.s)); 124 | path_dpos_i = track.path.getDerivative(initial_guess[i].xk(IndexMap.s)); 125 | initial_guess[i].xk(IndexMap.X) = path_pos_i(0); 126 | initial_guess[i].xk(IndexMap.Y) = path_pos_i(1); 127 | initial_guess[i].xk(IndexMap.yaw) = atan2(path_dpos_i(1), path_dpos_i(0)); 128 | initial_guess[i].xk(IndexMap.vx) = model.params.initial_vx; 129 | initial_guess[i].xk(IndexMap.vs) = model.params.initial_vx; 130 | } 131 | constrainInitialGuess(); 132 | valid_initial_guess = true; 133 | } 134 | 135 | void MPC::constrainInitialGuess() { 136 | double L = track.path.getLength(); 137 | for (int i = 0; i <= N; i++) { 138 | if ((initial_guess[i].xk(IndexMap.yaw)-initial_guess[i-1].xk(IndexMap.yaw)) < -M_PI) { 139 | initial_guess[i].xk(IndexMap.yaw) += 2.0*M_PI; 140 | } 141 | if ((initial_guess[i].xk(IndexMap.yaw)-initial_guess[i-1].xk(IndexMap.yaw)) > M_PI) { 142 | initial_guess[i].xk(IndexMap.yaw) -= 2.0*M_PI; 143 | } 144 | if ((initial_guess[i].xk(IndexMap.s)-initial_guess[i-1].xk(IndexMap.s)) > L/2.0) { 145 | initial_guess[i].xk(IndexMap.s) -= L; 146 | } 147 | } 148 | } 149 | 150 | array MPC::sqpMix(const array &previous_sol, 151 | const array ¤t_sol) const { 152 | array updated_sol; 153 | State updated_xk; 154 | Input updated_uk; 155 | for (int i = 0; i <= N; i++) { 156 | updated_xk = params.sqp_mixing * current_sol[i].xk; 157 | updated_uk = params.sqp_mixing * current_sol[i].uk; 158 | updated_sol[i].xk = updated_xk; 159 | updated_sol[i].uk = updated_uk; 160 | } 161 | return updated_sol; 162 | } 163 | -------------------------------------------------------------------------------- /mpc/mpc.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_MPC_H 6 | #define MPCC_MPC_H 7 | 8 | #include 9 | #include 10 | #include "../bounds/bounds_manager.h" 11 | #include "../constraints/constraint_manager.h" 12 | #include "../costs/cost_manager.h" 13 | #include "../models/dynamic_bicycle_model.h" 14 | #include "../models/state.h" 15 | #include "../splines/track.h" 16 | #include "mpc_params.h" 17 | #include "mpc_types.h" 18 | #include "../solvers/hpipm_interface.h" 19 | 20 | using std::array; 21 | 22 | class MPC { 23 | public: 24 | MPC(); 25 | MPC(const BoundsManager &bounds_args, 26 | const ConstraintManager &constraint_args, 27 | const CostManager &cost_args, 28 | const DynamicBicycleModel &model_args, 29 | const MPCParams ¶ms_args, 30 | const Track &track_args, 31 | const HpipmInterface &solver_args); 32 | OptSolution runMPC(const State &x0); 33 | private: 34 | void setProblem(); 35 | void setStage(const State &xk, const Input &uk, int k); 36 | 37 | void updateInitialGuess(const State &x0); 38 | void generateInitialGuess(const State &x0); 39 | void constrainInitialGuess(); 40 | array sqpMix(const array &previous_sol, 41 | const array ¤t_sol) const; 42 | 43 | bool valid_initial_guess = false; 44 | array stages; 45 | array initial_guess; 46 | array optimal_solution; 47 | 48 | BoundsManager bounds; 49 | ConstraintManager constraints; 50 | CostManager costs; 51 | DynamicBicycleModel model; 52 | MPCParams params; 53 | Track track; 54 | 55 | HpipmInterface solver; 56 | }; 57 | 58 | #endif //MPCC_MPC_H 59 | -------------------------------------------------------------------------------- /mpc/mpc_params.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "mpc_params.h" 6 | 7 | using json = nlohmann::json; 8 | 9 | MPCParams::MPCParams(const std::string &file_path) { 10 | std::ifstream file_stream(file_path); 11 | json jsonConfig = json::parse(file_stream); 12 | 13 | n_sqp = jsonConfig["n_sqp"]; 14 | sqp_mixing = jsonConfig["sqp_mixing"]; 15 | n_non_solves = jsonConfig["n_non_solves"]; 16 | n_no_solves_sqp = jsonConfig["n_no_solves_sqp"]; 17 | n_reset = jsonConfig["n_reset"]; 18 | Ts = jsonConfig["Ts"]; 19 | } 20 | -------------------------------------------------------------------------------- /mpc/mpc_params.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_MPC_PARAMS_H 6 | #define MPCC_MPC_PARAMS_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | class MPCParams { 13 | public: 14 | MPCParams() = default; 15 | explicit MPCParams(const std::string &file_path); 16 | 17 | int n_sqp; 18 | double sqp_mixing; 19 | int n_non_solves; 20 | int n_no_solves_sqp; 21 | int n_reset; 22 | double Ts; 23 | }; 24 | 25 | #endif //MPCC_MPC_PARAMS_H 26 | -------------------------------------------------------------------------------- /mpc/mpc_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_MPC_TYPES_H 6 | #define MPCC_MPC_TYPES_H 7 | 8 | #include 9 | #include "../bounds/bounds_types.h" 10 | #include "../constraints/constraint_types.h" 11 | #include "../costs/cost_types.h" 12 | #include "../models/state.h" 13 | 14 | // Horizon length constant 15 | static constexpr int N = 30; 16 | 17 | struct OptVariable { 18 | State xk; 19 | Input uk; 20 | }; 21 | 22 | struct OptStage { 23 | LinModelMatrix model; 24 | CostMatrix costs; 25 | ConstraintsMatrix constraints; 26 | 27 | Bounds_x x_lower; 28 | Bounds_x x_upper; 29 | 30 | Bounds_u u_lower; 31 | Bounds_u u_upper; 32 | 33 | Bounds_s s_lower; 34 | Bounds_s s_upper; 35 | 36 | // nx -> number of states 37 | // nu -> number of inputs 38 | // nbx -> number of bounds on x 39 | // nbu -> number of bounds on u 40 | // ng -> number of polytopic constratins 41 | // ns -> number of soft constraints 42 | int nx, nu, nbx, nbu, ng, ns; 43 | }; 44 | 45 | struct OptSolution { 46 | const Input u0; 47 | const std::array opt_vars; 48 | const double exec_time; 49 | }; 50 | 51 | #endif //MPCC_MPC_TYPES_H 52 | -------------------------------------------------------------------------------- /params/bounds_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "X_l":-3, 3 | "Y_l":-3, 4 | "yaw_l":-10, 5 | "vx_l":0.05, 6 | "vy_l":-3.0, 7 | "wz_l":-8.0, 8 | "s_l":-1, 9 | "accel_D_l":-0.1, 10 | "steering_angle_l":-0.35, 11 | "vs_l":0.0, 12 | 13 | "X_u":3, 14 | "Y_u":3, 15 | "yaw_u":10, 16 | "vx_u":3.5, 17 | "vy_u":3.0, 18 | "wz_u":8.0, 19 | "s_u":25, 20 | "accel_D_u":1, 21 | "steering_angle_u":0.35, 22 | "vs_u":6.0, 23 | 24 | "d_accel_D_l": -15.0, 25 | "d_steering_angle_l": -15.0, 26 | "d_vs_l": -100.0, 27 | 28 | "d_accel_D_u": 15.0, 29 | "d_steering_angle_u": 15.0, 30 | "d_vs_u": 100.0 31 | } -------------------------------------------------------------------------------- /params/cost_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "q_c" : 0.01, 3 | "q_l" : 1000.0, 4 | "q_vs" : 0.2, 5 | "q_beta": 5, 6 | 7 | "r_accel_D" : 1E-3, 8 | "r_steering_angle" : 1E-3, 9 | "r_vs" : 1E-5, 10 | 11 | "r_d_accel_D" : 1E-1, 12 | "r_d_steering_angle" : 5E2, 13 | "r_d_vs" : 1E-4, 14 | 15 | "sc_quad_track": 100.0, 16 | "sc_quad_tire": 100.0, 17 | "sc_quad_alpha": 100.0, 18 | 19 | "sc_lin_track": 10.0, 20 | "sc_lin_tire": 10.0, 21 | "sc_lin_alpha": 10.0 22 | } -------------------------------------------------------------------------------- /params/model_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "Cm1" : 8000.0, 3 | "Cm2" : 172.0, 4 | "Cr0" : 180.0, 5 | "Cr2" : 0.7, 6 | 7 | "Br" : 10.0, 8 | "Cr" : 1.38, 9 | "Dr" : 1500.0, 10 | 11 | "Bf" : 10.0, 12 | "Cf" : 1.38, 13 | "Df" : 1500.0, 14 | 15 | "m" : 190.0, 16 | "Iz" : 110.0, 17 | "lf" : 1.22, 18 | "lr" : 1.22, 19 | "car_l": 2.0, 20 | "car_w": 1.0, 21 | "g" : 9.81, 22 | 23 | "e_long" : 1.25, 24 | "e_eps" : 0.95, 25 | 26 | "max_alpha" : 0.15, 27 | 28 | "vx_zero": 0.3, 29 | "initial_vx": 15.0 30 | } -------------------------------------------------------------------------------- /params/mpc_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "n_sqp": 2, 3 | "sqp_mixing": 0.9, 4 | "n_non_solves": 0, 5 | "n_no_solves_sqp": 0, 6 | "n_reset": 2, 7 | "Ts": 1.0 8 | } -------------------------------------------------------------------------------- /params/track.json: -------------------------------------------------------------------------------- 1 | { 2 | "X_o":[-1.7667433023452759,2.7608964443206787,7.2786498069763184,12.03497314453125,16.539243698120117,21.033550262451172,25.824544906616211,29.008146286010742,32.853988647460938,36.054130554199219,39.640327453613281,41.841136932373047,43.794937133789062,45.312129974365234,46.155391693115234,46.422065734863281,46.532150268554688,46.376113891601562,45.840171813964844,45.092380523681641,43.663112640380859,41.371543884277344,37.8901481628418,34.6198616027832,31.2377872467041,27.435110092163086,24.486721038818359,21.939889907836914,17.776226043701172,14.972742080688477,13.464027404785156,11.351754188537598,9.2848196029663086,7.4833602905273438,4.7759575843811035,1.2875914573669434,-1.6338890790939331,-3.1794874668121338,-3.273709774017334,-3.0779802799224854,-2.7758777141571045,-3.4180958271026611,-5.1704211235046387,-6.5503630638122559,-5.8644695281982422,-4.3775758743286133,-2.1729903221130371,0.14787301421165466,4.0886449813842773,8.0548028945922852,11.959933280944824,14.95924186706543,17.607585906982422,19.570516586303711,22.606513977050781,25.340339660644531,27.526002883911133,29.16377067565918,30.100679397583008,30.184701919555664,29.283241271972656,26.989700317382812,23.76495361328125,19.983386993408203,15.700919151306152,11.52437686920166,7.5542244911193848,3.993138313293457,-0.066810213029384613,-3.4422705173492432,-5.4904098510742188,-7.2459101676940918,-9.3612260818481445,-11.545180320739746,-14.415183067321777,-16.5186710357666,-18.579502105712891,-21.812421798706055,-24.726701736450195,-25.750303268432617,-26.047666549682617,-25.931560516357422,-25.726341247558594,-25.001352310180664,-24.292245864868164,-23.305007934570312,-22.131742477416992,-20.788522720336914,-18.463239669799805,-15.602277755737305,-13.574502944946289,-11.514601707458496,-7.0433979034423828,-4.10932731628418,-1.7667433023452759], 3 | "X_i":[-1.2053545713424683,3.1781075000762939,7.4855895042419434,12.131684303283691,16.790725708007812,21.015966415405273,25.949274063110352,29.048070907592773,32.611961364746094,35.958824157714844,39.908271789550781,42.197879791259766,42.643951416015625,42.622211456298828,42.524066925048828,42.15972900390625,41.456150054931641,40.532562255859375,38.5180549621582,36.203975677490234,32.758670806884766,28.473556518554688,25.443721771240234,22.964296340942383,18.79351806640625,16.173812866210938,13.563885688781738,11.889633178710938,10.303607940673828,6.79945707321167,4.3283038139343262,1.123018741607666,-2.7538237571716309,-4.8145432472229,-7.1526875495910645,-8.1841411590576172,-7.9077849388122559,-7.0089120864868164,-8.4211645126342773,-10.343802452087402,-9.2157754898071289,-6.9203634262084961,-4.6059727668762207,-2.3527348041534424,-0.16583850979804993,2.041154146194458,4.1573920249938965,8.7517633438110352,12.996903419494629,16.446369171142578,19.58946418762207,21.404584884643555,25.262435913085938,26.204153060913086,26.147523880004883,24.710147857666016,21.122005462646484,16.892978668212891,12.550552368164062,8.6141061782836914,5.1298866271972656,1.2918307781219482,-0.911457359790802,-3.1927506923675537,-4.8897614479064941,-7.3426680564880371,-9.3245267868042,-11.927055358886719,-13.990721702575684,-15.855617523193359,-17.421369552612305,-18.963479995727539,-20.869033813476562,-21.783098220825195,-22.118432998657227,-22.378229141235352,-22.289163589477539,-21.683494567871094,-20.839620590209961,-19.733856201171875,-18.799781799316406,-17.489906311035156,-15.561161041259766,-13.941559791564941,-11.831618309020996,-9.602931022644043,-6.3578038215637207,-3.796358585357666,-1.2053545713424683], 4 | "Y_o":[1.4703056812286377,1.715367317199707,1.7270327806472778,1.7770155668258667,1.8575179576873779,1.9057978391647339,1.8236978054046631,1.6881827116012573,1.5323630571365356,1.4059867858886719,1.3329223394393921,0.46170023083686829,-0.82433044910430908,-2.6312391757965088,-4.62501859664917,-7.1025485992431641,-9.6071414947509766,-11.95250415802002,-14.408729553222656,-16.519775390625,-19.095273971557617,-21.7454833984375,-24.082986831665039,-25.537734985351562,-26.407459259033203,-27.594654083251953,-28.271196365356445,-28.539289474487305,-27.49943733215332,-25.154607772827148,-23.703035354614258,-21.9376277923584,-20.390314102172852,-18.919614791870117,-17.498115539550781,-17.333209991455078,-18.27589225769043,-21.729055404663086,-25.020490646362305,-27.428274154663086,-30.082868576049805,-34.082412719726562,-37.148227691650391,-40.391315460205078,-42.861595153808594,-44.948749542236328,-47.4768180847168,-48.763973236083984,-48.879444122314453,-47.916030883789062,-47.655147552490234,-47.935005187988281,-48.6755485534668,-49.775543212890625,-52.183685302734375,-55.391071319580078,-58.727699279785156,-62.033397674560547,-65.721969604492188,-69.022377014160156,-72.07379150390625,-73.962455749511719,-74.3958969116211,-74.25006103515625,-73.0300064086914,-71.985565185546875,-71.014366149902344,-70.147529602050781,-69.102874755859375,-67.230987548828125,-65.111968994140625,-62.596157073974609,-60.314739227294922,-58.312305450439453,-54.93426513671875,-52.561187744140625,-50.139446258544922,-46.621086120605469,-43.475151062011719,-40.398113250732422,-36.970062255859375,-32.619327545166016,-28.64410400390625,-24.676288604736328,-20.863292694091797,-17.203592300415039,-13.642368316650391,-9.2805862426757812,-6.4125361442565918,-3.6869914531707764,-2.1660451889038086,-0.89286273717880249,0.44019073247909546,1.1095694303512573,1.4703056812286377], 5 | "Y_i":[-2.4342195987701416,-1.9323287010192871,-1.813536524772644,-1.6839693784713745,-1.6339786052703857,-1.5776764154434204,-1.6844339370727539,-1.7288788557052612,-1.8369622230529785,-2.02518367767334,-3.0469145774841309,-5.7311501502990723,-7.45758581161499,-9.8456096649169922,-11.47083854675293,-13.486030578613281,-15.181887626647949,-16.879753112792969,-18.998863220214844,-20.470911026000977,-21.973819732666016,-23.481792449951172,-24.171453475952148,-24.4952335357666,-23.467641830444336,-21.452484130859375,-18.866912841796875,-17.364273071289062,-16.09937858581543,-13.866070747375488,-13.511563301086426,-13.713901519775391,-14.165826797485352,-15.676466941833496,-19.083581924438477,-23.216394424438477,-27.239025115966797,-31.045099258422852,-34.921249389648438,-39.310264587402344,-43.814994812011719,-47.6307487487793,-50.547756195068359,-51.889274597167969,-52.89080810546875,-53.272285461425781,-53.364963531494141,-53.05743408203125,-52.164520263671875,-52.551719665527344,-54.879924774169922,-57.288639068603516,-63.392913818359375,-65.976722717285156,-68.3555679321289,-69.915901184082031,-70.259590148925781,-69.464889526367188,-68.420356750488281,-67.524505615234375,-66.636421203613281,-65.518829345703125,-64.00048828125,-61.759052276611328,-59.921844482421875,-57.379077911376953,-55.3648567199707,-52.585880279541016,-50.405086517333984,-48.291885375976562,-46.340396881103516,-44.472003936767578,-41.121730804443359,-39.553520202636719,-36.980892181396484,-33.375526428222656,-29.805479049682617,-25.672634124755859,-21.799604415893555,-18.067205429077148,-14.369050979614258,-10.844764709472656,-8.869563102722168,-7.2787680625915527,-5.82119607925415,-4.7861251831054688,-3.6278462409973145,-2.9046835899353027,-2.4342195987701416], 6 | "X":[-1.5610852094660372,-0.95416017787396612,-0.3437240576363359,0.27065536268327728,0.88878343596276643,1.5104760062576412,2.1355635333511094,2.7638937295817856,3.3953328489883674,4.02976577789959,4.6670950654207015,5.3072389816218415,5.9501288242654446,6.595705367199022,7.2439139625711455,7.8932827484080459,8.542027486784054,9.1901551965826762,9.8376824305317356,10.484627976262091,11.131011827814385,11.776854104064039,12.422196466903316,13.0671525977149,13.711732101256668,14.355927086059637,14.999727937602287,15.643124345359952,16.286106506264652,16.928597278893882,17.570265007309665,18.211082409214441,18.851066970730784,19.490233425027192,20.128591582314975,20.766142691673444,21.40295261058948,22.03932044519323,22.675243171201643,23.310689157724084,23.945639775745157,24.580090870563019,25.214054882174935,25.847600727097518,26.483325932599485,27.122811853007555,27.766059493096311,28.413044661145385,29.063608578316249,29.712654966796684,30.357536882442425,30.998107539496985,31.634200236888717,32.265653978499955,32.892809948973934,33.529759129030062,34.183513777492969,34.854736464425244,35.544628468792205,36.253581213679666,36.937987086560796,37.5731310405665,38.149731881736528,38.658993583325781,39.097598135417947,39.493778558357,39.926668342271192,40.39796899362775,40.896923309021616,41.382601895172066,41.818294904326166,42.204646819018762,42.5450043184326,42.861922691069431,43.159054717299867,43.433462194192515,43.680043719368136,43.889304156892479,44.057868795168488,44.185857710200779,44.297179529597344,44.399003137216518,44.485707558351507,44.548474764716275,44.580887147453396,44.591610619781349,44.588231128705452,44.574371730081495,44.5498375701697,44.512123785974069,44.457594908401774,44.383469189818882,44.286489430169766,44.16460485232119,44.015646696670572,43.839777092202915,43.64125482535222,43.425795333115062,43.19632424810051,42.95029156957095,42.685694472846905,42.39997759667844,42.089491757073574,41.75605472637276,41.403885878631961,41.034592560135493,40.651848095589727,40.256369950026887,39.848548827374742,39.417837801420049,38.963924870890565,38.48730975224845,37.989315809410115,37.471815936167687,36.936667217864425,36.395650305112355,35.859619038239764,35.329431178943949,34.805449739737092,34.287417769915535,33.770219803175216,33.22461233578133,32.646376506909334,32.038196826094051,31.403215230164307,30.744170070410583,30.083717851614878,29.439335038440724,28.809266387506739,28.193161041748652,27.591479618687078,27.00470653663821,26.425756645563695,25.841953080697436,25.253142092917635,24.658933359315675,24.059343165144959,23.472106705471425,22.908642133821431,22.369800867847516,21.854164005209874,21.346455592279902,20.844029120126422,20.352874368686631,19.8784845468898,19.425694633998219,18.998477351141357,18.593800145389523,18.175564923582513,17.735867862442582,17.276710141975322,16.803265279099048,16.318829307286883,15.830180207215877,15.344193465867509,14.856202480227095,14.362820651372445,13.865887490845342,13.36372767599784,12.854192230761278,12.336323880904093,11.824515299117984,11.328550535768047,10.853263363839151,10.400383301585878,9.927798981287669,9.4154324410596928,8.859976378073064,8.2601774756426529,7.6216397880335318,6.9351052238030029,6.1900198510035631,5.3843689898579754,4.5546524592957383,3.7456746204966569,2.9613922355838436,2.1990325440984479,1.453013988783374,0.70912879149294594,-0.0860928594919427,-0.9377816334011897,-1.8314710011515754,-2.7068458544682903,-3.4846362995953069,-4.1102929384133082,-4.6061463085354166,-4.9890424331803676,-5.2748287518224064,-5.48022691367481,-5.6217906015581267,-5.7121801727675514,-5.7598688416881254,-5.7717724450481249,-5.7528185132489433,-5.7077197333690073,-5.6332585888616657,-5.5265139716330216,-5.3860157183704906,-5.2356202966018905,-5.1025169336968563,-4.9989021199534047,-4.9308678681446692,-4.8986167228256132,-4.9007109295232887,-4.9432210947568924,-5.021740987293267,-5.1334246438943332,-5.2753307242654177,-5.446151765590427,-5.6538277941985315,-5.8993921568579353,-6.1827809396241946,-6.5000670257275619,-6.8461606196182725,-7.2193941109987074,-7.6059030384820119,-7.97723958995024,-8.28710235941138,-8.44786946615476,-8.3872764357424909,-8.1698371514343915,-7.8647777809373061,-7.505027559784085,-7.1128252554171141,-6.7100563232935952,-6.31186335011956,-5.9304077545138725,-5.5653732282753019,-5.1824535664749032,-4.7693004741598752,-4.3099748785266954,-3.7816502941329482,-3.1610216822866923,-2.4685122665843391,-1.7359629608684379,-0.97876633775068833,-0.18598463631857495,0.63156325326378349,1.4546535088204868,2.2699319009448189,3.0734269424935015,3.8639574217198573,4.6397358106216151,5.3849586849563646,6.0895673264069581,6.7504984031386819,7.3662085941091737,7.9371174551676393,8.4656005778017214,8.9749841580933136,9.5000511168263628,10.041606483726419,10.599123530317957,11.171450538513568,11.756809127451373,12.348227514409524,12.934036300068133,13.51395011206143,14.088136438753562,14.656612115344853,15.211053227736144,15.74262408324369,16.247499209842495,16.722998073249688,17.184685079944174,17.663781350138855,18.156722684587031,18.659274017878438,19.149203414794869,19.620272105876232,20.071973341921264,20.504996642678712,20.919604171864751,21.316105841438031,21.699873954504298,22.074295102950735,22.4409864900703,22.801871990715863,23.159253710928226,23.514996704768237,23.868328465433827,24.216608119652385,24.559350792285151,24.896135791135904,25.226504146412324,25.549953446531291,25.865085729185857,26.166959790633388,26.45345482028349,26.722820236832373,26.97397795869627,27.205890864006236,27.418458432141854,27.61309189456065,27.788205413370555,27.940475722792186,28.069075507440552,28.17249490647648,28.245206904190496,28.284169064319514,28.281536585452564,28.230757288263892,28.12660458427407,27.967910638284842,27.790798069553659,27.615068239366984,27.455270850362268,27.303124466356323,27.124491583885728,26.8916170055276,26.622556894785028,26.335379836456916,26.046670409289359,25.715186187591261,25.306169053782369,24.811415366906331,24.237446678245611,23.599510888138731,22.95887508849215,22.343022030505871,21.75378692623098,21.192625244875195,20.659708432557828,20.150997351330354,19.628990608783326,19.08085187656047,18.506499558912473,17.907084543982286,17.284128075504576,16.638939836628232,15.974678194559415,15.313490312724927,14.660107326204761,14.01410993696609,13.375355748959841,12.743853175777229,12.1196651558379,11.5006024880303,10.880555787530874,10.259396131060633,9.6373813791586382,9.0147903206797135,8.3919167989873973,7.76889380329356,7.1437121075637364,6.5152226855292321,5.8827018921911058,5.2454075192373582,4.6025905372781049,3.958821367870287,3.3302126214931134,2.720009546078455,2.1304108121554624,1.5635697551114254,1.021336555078219,0.50494667169353213,0.0066867593674065362,-0.47675226050873742,-0.94318959710406247,-1.391846207047696,-1.8216136344368925,-2.2320237292448053,-2.6284040332631875,-3.0133998268030466,-3.3875067569400841,-3.7506519119118265,-4.10775999335492,-4.4809911537588549,-4.8762960829083566,-5.2971187129199189,-5.7477016904194542,-6.22245135075635,-6.7036980357115272,-7.1903849660350048,-7.6810435194791857,-8.1738579638324929,-8.6606893510884149,-9.1316152231571657,-9.5819634184363167,-10.007507768604846,-10.407408818066154,-10.803690824438657,-11.204121004759562,-11.610861171737804,-12.025935612772161,-12.451381218094074,-12.889428418185037,-13.340059094876088,-13.789801052497117,-14.23451460781855,-14.672041562697785,-15.100358467358284,-15.519599079198244,-15.93726020174676,-16.354008099848627,-16.769504064850771,-17.183348297422434,-17.596169821349221,-18.011702786636945,-18.434516691737613,-18.869292400103511,-19.315488483193882,-19.766299437756057,-20.213566715831639,-20.651509452123822,-21.0687276969313,-21.460931003426204,-21.8254138014862,-22.156271006796672,-22.446807066969967,-22.690893322187335,-22.892063730564431,-23.084901014267729,-23.278774543595834,-23.472798647376976,-23.658995836337731,-23.815519756167813,-23.926458493956307,-23.999420426534662,-24.044079210675982,-24.0699452309955,-24.087922435115111,-24.104917888770263,-24.121806142923909,-24.137978059495239,-24.152158512707722,-24.162510094680435,-24.16637271110163,-24.163041096215032,-24.152430152180937,-24.13352195610068,-24.105188933969593,-24.066232394420169,-24.015400653655707,-23.951399328917745,-23.871894027733624,-23.778018880386306,-23.672270738087896,-23.55738916188356,-23.436326268780341,-23.312149417461903,-23.188618414728488,-23.066089424793656,-22.942391269280868,-22.815485189287223,-22.683426887632592,-22.544426851963259,-22.396437429187444,-22.239157217638045,-22.074054068711561,-21.902570357631618,-21.726122803037292,-21.546096142698559,-21.361798995544483,-21.174370019874829,-20.986418988836977,-20.800341056769746,-20.618362628199087,-20.442449345881684,-20.277340288664643,-20.115112565967731,-19.946871532262289,-19.767497420734298,-19.575051452177341,-19.369758867748956,-19.150749312934479,-18.892160879771307,-18.5802512538544,-18.218506601356808,-17.812057672091896,-17.371118587697367,-16.91059039740443,-16.460658540032526,-16.020527815119792,-15.5828488744868,-15.141502141522697,-14.693215890742987,-14.23758120582481,-13.762833771963162,-13.26560244533526,-12.746979385652645,-12.210977217144841,-11.690790140300944,-11.198493402057998,-10.736274508269005,-10.292353858621528,-9.8210707239574546,-9.316694912787078,-8.7797872610892611,-8.2123886267217543,-7.6172928608823129,-6.99772089645733,-6.3589193772017651,-5.7265646504235761,-5.1065255593838446,-4.4977895432516615,-3.8994101650654036,-3.3104302005535526,-2.7297128509848489,-2.1558699018613252,-1.5871260676063934], 7 | "Y":[-0.49131685450487939,-0.41139880947096541,-0.33755413571933857,-0.27499442209308522,-0.22284580262528575,-0.18017758678484397,-0.14600655770015436,-0.11930121739899724,-0.098985671722730317,-0.083942950547930772,-0.073017650031344861,-0.065039571627756865,-0.058746466754855842,-0.052835706926741888,-0.046002472152257279,-0.037094240731837758,-0.026057944382493092,-0.01345489672153044,9.93380471978611E-5,0.014033049375972606,0.027778644471782021,0.040766297574161392,0.052396501314459609,0.062644489080499355,0.071874156784737631,0.080467249747577241,0.08880254553598943,0.097266465227961785,0.10625487209841034,0.11611739689454414,0.12678322711528722,0.13749825388473869,0.14742445229753753,0.15572960871945085,0.16153607362379785,0.16406037730231682,0.16257584703008277,0.15706776117311005,0.14806491664706456,0.13611494158006587,0.12176062023190437,0.10553962214675872,0.088005069127945257,0.069692737560203777,0.051160589883881613,0.032741883745231348,0.014418821617645516,-0.0038801014382062737,-0.022287339360152592,-0.041165868169496411,-0.060988061328775434,-0.082239519283273443,-0.10542275769978249,-0.13105708909598379,-0.15969801234642367,-0.19171163618142861,-0.22566198468443111,-0.25979099769976166,-0.29276629963219647,-0.32391250345786848,-0.35360398767725565,-0.3902618133924598,-0.43881986681984719,-0.50111510660682157,-0.57695388910797807,-0.67975313401068838,-0.835392150887057,-1.044132711389939,-1.3076641171363828,-1.603442158856083,-1.9066657940511265,-2.2108884990917144,-2.5180181103634633,-2.8511899992998875,-3.21434554338374,-3.6041416407755489,-4.0133775966565874,-4.427281879773699,-4.8394821699346355,-5.2499502856817672,-5.714606565720902,-6.2517474605665715,-6.8556277239835888,-7.5099541408455579,-8.1524768368118963,-8.771404602350291,-9.3660751517481842,-9.94025975853517,-10.509600243099175,-11.075288426165841,-11.636278312734319,-12.195971184820966,-12.759536027584584,-13.325732398778413,-13.892639095937319,-14.450537461411201,-14.991097753220515,-15.514646564177324,-16.023589046654546,-16.527049196834113,-17.025596744390114,-17.517323761180016,-17.998740361190045,-18.464519178903242,-18.909324032944216,-19.331136580706868,-19.730227014067673,-20.104969951901161,-20.457568599942661,-20.79604762100282,-21.123668483844259,-21.4406042608557,-21.74733484842173,-22.04471142892033,-22.333315492895707,-22.608880506247544,-22.866612366044272,-23.106041900187332,-23.32673557301354,-23.528443481095614,-23.712188446694391,-23.891729698151948,-24.073829293513647,-24.261619569569788,-24.457260966934452,-24.662332620695935,-24.869842331699395,-25.07081827686347,-25.262135640389033,-25.442173928572352,-25.610044926656375,-25.764686132355124,-25.906563993409563,-26.038899885937482,-26.161948026515624,-26.272808377775394,-26.367240087228645,-26.438789414608912,-26.48332614675963,-26.500745076396136,-26.490180578697746,-26.449945230317109,-26.379858863750943,-26.280594791985109,-26.153103448758461,-25.998495006283608,-25.817874803373762,-25.60996458830175,-25.361981522527618,-25.070772468746448,-24.737169974593161,-24.3630270832114,-23.953683407556838,-23.518695886060485,-23.069766842866549,-22.610255665297952,-22.143625133126456,-21.676713277192793,-21.212307533888954,-20.75283100266477,-20.301607797676127,-19.873728453810287,-19.47109313690159,-19.090257447978594,-18.724816840916354,-18.338378405777597,-17.922382101744354,-17.480198690004794,-17.022385516912856,-16.577186197166736,-16.166874751617868,-15.82284532293864,-15.578921886511054,-15.446022711867514,-15.398847844953934,-15.409076096300808,-15.45049180051171,-15.502615103141641,-15.553750270208097,-15.619444935594462,-15.741525133704238,-16.004873072995387,-16.51715355290052,-17.234223550159957,-18.01205746458389,-18.822164728706476,-19.649200771699967,-20.476709218344645,-21.2872071750046,-22.071373403485456,-22.833641043013682,-23.571747818865862,-24.284700339827019,-24.972896287123923,-25.647295036182559,-26.329657187589785,-27.017872887576871,-27.708604831935819,-28.373739699276541,-28.990850455174407,-29.565160524397864,-30.100316948347484,-30.615192072134086,-31.133717740020977,-31.649009760969005,-32.154346578839807,-32.643758236364469,-33.112087103286882,-33.561474454157867,-34.023112956485789,-34.507042340247708,-35.015979300831553,-35.55241516528357,-36.118652429372268,-36.730926388481564,-37.401804406499117,-38.143161211839768,-38.976425838465246,-39.917264950471008,-40.908433302578381,-41.817780597700562,-42.646624691505323,-43.41001403029685,-44.113543889637029,-44.7533832669321,-45.337878709610209,-45.875029875983081,-46.386848939615732,-46.927620846337163,-47.494787374304025,-48.076783259863525,-48.654215564837628,-49.192436456361648,-49.653130014018537,-50.054685281561873,-50.426585779324043,-50.754760825351809,-50.986922538608589,-51.114902738535761,-51.173533788906894,-51.186289625006467,-51.163799414877737,-51.112888669736918,-51.036028699925517,-50.937720742163918,-50.824259356505515,-50.703539607326491,-50.584217362914877,-50.474174330230255,-50.377883552408363,-50.289160200056642,-50.207666571005291,-50.133484711562694,-50.067768878586023,-50.012549547042013,-49.972180818564738,-49.952171200041832,-49.955109776253437,-49.98223987844311,-50.0353747116458,-50.114672384245445,-50.219697223519205,-50.351063452152033,-50.508476079364428,-50.696385993336094,-50.924781843035873,-51.196754136702516,-51.507879966304714,-51.844549353698909,-52.20028384366006,-52.573349146615513,-52.960870079475725,-53.361056592903083,-53.773223446245687,-54.201555756824689,-54.645901044133645,-55.10456415351981,-55.575614701455279,-56.056873333684052,-56.5465391707402,-57.042556886697945,-57.541791348142254,-58.0446766343194,-58.551751030765665,-59.063520196463472,-59.580459545704386,-60.101718158227641,-60.621709892592264,-61.139994158951509,-61.657023079975595,-62.173114092403317,-62.688885502846205,-63.208788163674242,-63.738775398465215,-64.278158380459487,-64.826233524022314,-65.38135186898856,-65.941871296709479,-66.504437301262925,-67.06536143120681,-67.6238550981911,-68.177927979401659,-68.72456815409339,-69.248477359319509,-69.719226594391074,-70.127450356456166,-70.473769861835564,-70.762983555263986,-71.019820916457547,-71.264080595184879,-71.481715554891736,-71.658672936299979,-71.782095892440509,-71.888086895218208,-72.011382822999792,-72.1438953371266,-72.263257146898923,-72.346882536465628,-72.385323781149154,-72.388735056290528,-72.3677108597006,-72.327730326276878,-72.271056732669379,-72.196609831817312,-72.098926264456821,-71.97844733481044,-71.838753282624282,-71.683216736738757,-71.5158089161747,-71.340973025628855,-71.16380406630941,-70.992399490242661,-70.827616040861983,-70.668904693274584,-70.5155817472482,-70.366910175965629,-70.222101075146639,-70.079474774891366,-69.936539131690154,-69.792488006886828,-69.646584124437936,-69.498070380090226,-69.346164146793882,-69.190031072301366,-69.03031310913039,-68.86912124563294,-68.708811795603964,-68.55188240657921,-68.4010542074659,-68.257322199218464,-68.117331599301835,-67.973789911844335,-67.820583404659317,-67.653075252636683,-67.468185040491989,-67.263312782652946,-67.033386025762454,-66.779562653319573,-66.503619612076619,-66.2090096451528,-65.8973784164744,-65.57016065635662,-65.224356707549418,-64.858132914155149,-64.472984984555609,-64.070130266117,-63.646371338150431,-63.184459559550859,-62.687226736214242,-62.161112674795909,-61.613106671497157,-61.061288619161758,-60.528446847084226,-60.013068383706113,-59.513246861758077,-59.027447032621922,-58.560070664300646,-58.113965608090069,-57.685827572605938,-57.272428838735294,-56.867740683769327,-56.446393871129757,-56.003922675691008,-55.54251056400355,-55.0648985450198,-54.574162768812073,-54.0735663567604,-53.568948321547751,-53.07563917550678,-52.593549425051471,-52.120237474381369,-51.653488002688235,-51.189152830307435,-50.717476957751543,-50.237683816157571,-49.749997444310218,-49.254519596912985,-48.75222600622191,-48.246583614045583,-47.740813053099238,-47.238368039867815,-46.738189385105272,-46.232949731291654,-45.712569965541796,-45.16885702939534,-44.629098765083796,-44.11736954350468,-43.644352502183708,-43.214373796561595,-42.8257860788444,-42.471456714197487,-42.129074987585916,-41.741664686075872,-41.305127710696141,-40.822913849925143,-40.300206372835078,-39.7454096085997,-39.176939990864,-38.599773060353741,-38.015704281888866,-37.425434086979017,-36.82878377972277,-36.2232025601035,-35.60737956389724,-34.981425748000873,-34.345835263039881,-33.701323061699618,-33.048633723325196,-32.390173128130385,-31.741536027751035,-31.106241298272479,-30.484795738593778,-29.87779805654263,-29.285687887742881,-28.707628133464624,-28.124525245422909,-27.528284550051488,-26.918763713439354,-26.296057717143736,-25.660293866986997,-25.011465517358587,-24.360439311556846,-23.718860441623981,-23.087067595188888,-22.465526286708041,-21.854764289519327,-21.255278139713514,-20.662897784264963,-20.072614887898368,-19.484431333393196,-18.898265213562624,-18.313945592103785,-17.731160066968421,-17.139950435910023,-16.531923541681849,-15.906130618006264,-15.262040167744143,-14.599169713287004,-13.918331847517294,-13.251267936379174,-12.614370994549674,-12.012985831367928,-11.453272601657272,-10.941164142296863,-10.480583482371109,-10.070916694557475,-9.67262944979488,-9.2649864428081621,-8.848338828243687,-8.4238607854449246,-7.9881775010334408,-7.5371848680745739,-7.0853083500785736,-6.6418833854272528,-6.2135414736125725,-5.8060453429149463,-5.4226434480772854,-5.0620283429285058,-4.7143843092855828,-4.375882854892744,-4.0456659409075435,-3.723821476382037,-3.4253315521213814,-3.1575120311833831,-2.9232955357948511,-2.7217477649985464,-2.5340510111394963,-2.3546150906628975,-2.1808452100977873,-2.0101514859796015,-1.840513170247033,-1.6698091580151129,-1.4964638438545861,-1.3277772050450141,-1.1680556037210033,-1.0190560394146344,-0.88259151014070891,-0.76032585273131237,-0.6539742019602609,-0.56501536105525352,-0.49456514641095861] 8 | } -------------------------------------------------------------------------------- /plotter/plotter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "plotter.h" 6 | 7 | Plotter::Plotter(double Ts, const ModelParams modelParams) { 8 | model_params = modelParams; 9 | } 10 | 11 | void Plotter::plot_simulation(const std::list &log, const Track &track) const { 12 | std::vector plot_xc(track.centre.sx.ft_spline.data(), track.centre.sx.ft_spline.data() + track.centre.sx.ft_spline.size()); 13 | std::vector plot_yc(track.centre.sy.ft_spline.data(), track.centre.sy.ft_spline.data() + track.centre.sy.ft_spline.size()); 14 | 15 | std::vector plot_xi(track.inner.sx.ft_spline.data(), track.inner.sx.ft_spline.data() + track.inner.sx.ft_spline.size()); 16 | std::vector plot_yi(track.inner.sy.ft_spline.data(), track.inner.sy.ft_spline.data() + track.inner.sy.ft_spline.size()); 17 | std::vector plot_xo(track.outer.sx.ft_spline.data(), track.outer.sx.ft_spline.data() + track.outer.sx.ft_spline.size()); 18 | std::vector plot_yo(track.outer.sy.ft_spline.data(), track.outer.sy.ft_spline.data() + track.outer.sy.ft_spline.size()); 19 | 20 | 21 | std::vector plot_x; 22 | std::vector plot_y; 23 | 24 | for(const OptSolution& log_i : log) 25 | { 26 | plot_x.resize(0); 27 | plot_y.resize(0); 28 | for(const auto & opt_var : log_i.opt_vars) 29 | { 30 | plot_x.push_back(opt_var.xk(IndexMap.X)); 31 | plot_y.push_back(opt_var.xk(IndexMap.Y)); 32 | } 33 | plt::clf(); 34 | plt::plot(plot_xc,plot_yc,"r--"); 35 | plt::plot(plot_xi,plot_yi,"k-"); 36 | plt::plot(plot_xo,plot_yo,"k-"); 37 | plot_box(log_i.opt_vars[0].xk); 38 | plt::plot(plot_x,plot_y,"b-"); 39 | plt::axis("equal"); 40 | // plt::xlim(-2,2); 41 | // plt::ylim(-2,2); 42 | plt::pause(0.01); 43 | } 44 | } 45 | 46 | void Plotter::plot_box(const State &x0) const { 47 | std::vector corner_x; 48 | std::vector corner_y; 49 | double body_xl = std::cos(x0(IndexMap.yaw))*model_params.car_l; 50 | double body_xw = std::sin(x0(IndexMap.yaw))*model_params.car_w; 51 | double body_yl = std::sin(x0(IndexMap.yaw))*model_params.car_l; 52 | double body_yw = -std::cos(x0(IndexMap.yaw))*model_params.car_w; 53 | 54 | corner_x.push_back(x0(IndexMap.X) + body_xl + body_xw); 55 | corner_x.push_back(x0(IndexMap.X) + body_xl - body_xw); 56 | corner_x.push_back(x0(IndexMap.X) - body_xl - body_xw); 57 | corner_x.push_back(x0(IndexMap.X) - body_xl + body_xw); 58 | corner_x.push_back(x0(IndexMap.X) + body_xl + body_xw); 59 | 60 | corner_y.push_back(x0(IndexMap.Y) + body_yl + body_yw); 61 | corner_y.push_back(x0(IndexMap.Y) + body_yl - body_yw); 62 | corner_y.push_back(x0(IndexMap.Y) - body_yl - body_yw); 63 | corner_y.push_back(x0(IndexMap.Y) - body_yl + body_yw); 64 | corner_y.push_back(x0(IndexMap.Y) + body_yl + body_yw); 65 | 66 | plt::plot(corner_x,corner_y,"k-"); 67 | } 68 | -------------------------------------------------------------------------------- /plotter/plotter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_PLOTTER_H 6 | #define MPCC_PLOTTER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "../models/model_params.h" 13 | #include "../models/state.h" 14 | #include "../mpc/mpc_types.h" 15 | #include "../splines/track.h" 16 | 17 | namespace plt = matplotlibcpp; 18 | 19 | class Plotter { 20 | public: 21 | void plot_simulation(const std::list &log, const Track &track) const; 22 | void plot_box(const State &x0) const; 23 | Plotter(double Ts, ModelParams modelParams); 24 | ModelParams model_params; 25 | }; 26 | 27 | 28 | #endif //MPCC_PLOTTER_H 29 | -------------------------------------------------------------------------------- /solvers/hpipm_interface.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "hpipm_interface.h" 6 | 7 | void HpipmInterface::setDynamics(array &stages, const State &x0) { 8 | b0 = (stages[0].model.A*x0 + stages[0].model.g); 9 | for (int i = 0; i < N; i++) { 10 | if (i == 0) { 11 | hA[i] = nullptr; 12 | hB[i] = stages[i].model.B.data(); 13 | hb[i] = b0.data(); 14 | nx[i] = 0; 15 | nu[i] = NU; 16 | } else { 17 | hA[i] = stages[i].model.A.data(); 18 | hB[i] = stages[i].model.B.data(); 19 | hb[i] = stages[i].model.g.data(); 20 | nx[i] = NX; 21 | nu[i] = NU; 22 | } 23 | } 24 | nx[N] = NX; 25 | nu[N] = 0; 26 | } 27 | 28 | void HpipmInterface::setCost(array &stages) { 29 | for (int i = 0; i <= N; i++) { 30 | hQ[i] = stages[i].costs.Q.data(); 31 | hR[i] = stages[i].costs.R.data(); 32 | hS[i] = stages[i].costs.S.data(); 33 | 34 | hq[i] = stages[i].costs.q.data(); 35 | hr[i] = stages[i].costs.r.data(); 36 | 37 | if (stages[i].ns != 0) { 38 | hZl[i] = stages[i].costs.Z.data(); 39 | hZu[i] = stages[i].costs.Z.data(); 40 | hzl[i] = stages[i].costs.z.data(); 41 | hzu[i] = stages[i].costs.z.data(); 42 | } else { 43 | hZl[i] = nullptr; 44 | hZu[i] = nullptr; 45 | hzl[i] = nullptr; 46 | hzu[i] = nullptr; 47 | } 48 | } 49 | } 50 | 51 | void HpipmInterface::setBounds(array &stages, const State &x0) { 52 | nbu[0] = 0; 53 | hpipm_bounds[0].idx_u.resize(0); 54 | hpipm_bounds[0].u_lower.resize(0); 55 | hpipm_bounds[0].u_upper.resize(0); 56 | for (int j = 0; j < NU; j++) { 57 | if (stages[0].u_lower(j) > -INF && stages[0].u_upper(j) < INF) { 58 | nbu[0]++; 59 | hpipm_bounds[0].idx_u.push_back(j); 60 | hpipm_bounds[0].u_lower.push_back(stages[0].u_lower(j)); 61 | hpipm_bounds[0].u_upper.push_back(stages[0].u_upper(j)); 62 | } 63 | } 64 | nbx[0] = 0; 65 | hidxbx[0] = nullptr; 66 | hidxbu[0] = hpipm_bounds[0].idx_u.data(); 67 | 68 | hlbx[0] = nullptr; 69 | hubx[0] = nullptr; 70 | hlbu[0] = hpipm_bounds[0].u_lower.data(); 71 | hubu[0] = hpipm_bounds[0].u_upper.data(); 72 | 73 | for (int i = 1; i <= N; i++) { 74 | hpipm_bounds[i].idx_u.resize(0); 75 | hpipm_bounds[i].u_lower.resize(0); 76 | hpipm_bounds[i].u_upper.resize(0); 77 | nbu[i] = 0; 78 | for (int j = 0; j < NU; j++) { 79 | if (stages[i].u_lower(j) > -INF && stages[i].u_upper(j) < INF) { 80 | nbu[i]++; 81 | hpipm_bounds[i].idx_u.push_back(j); 82 | hpipm_bounds[i].u_lower.push_back(stages[i].u_lower(j)); 83 | hpipm_bounds[i].u_upper.push_back(stages[i].u_upper(j)); 84 | } 85 | } 86 | 87 | hpipm_bounds[i].idx_x.resize(0); 88 | hpipm_bounds[i].x_lower.resize(0); 89 | hpipm_bounds[i].x_upper.resize(0); 90 | nbx[i] = 0; 91 | for (int j = 0; j < NX; j++) { 92 | if (stages[i].x_lower(j) > -INF && stages[i].x_upper(j) < INF) { 93 | nbx[i]++; 94 | hpipm_bounds[i].idx_x.push_back(j); 95 | hpipm_bounds[i].x_lower.push_back(stages[i].x_lower(j)); 96 | hpipm_bounds[i].x_upper.push_back(stages[i].x_upper(j)); 97 | } 98 | } 99 | 100 | hidxbx[i] = hpipm_bounds[i].idx_x.data(); 101 | hidxbu[i] = hpipm_bounds[i].idx_x.data(); 102 | hlbx[i] = hpipm_bounds[i].x_lower.data(); 103 | hubx[i] = hpipm_bounds[i].x_upper.data(); 104 | hlbu[i] = hpipm_bounds[i].u_lower.data(); 105 | hubu[i] = hpipm_bounds[i].u_upper.data(); 106 | } 107 | 108 | nbu[N] = 0; 109 | hidxbu[N] = nullptr; 110 | hlbu[N] = nullptr; 111 | hubu[N] = nullptr; 112 | } 113 | 114 | void HpipmInterface::setPolytopicConstraints(array &stages) { 115 | for (int i = 0; i <= N; i++) { 116 | ng[i] = stages[i].ng; 117 | if (stages[i].ng > 0) { 118 | hC[i] = stages[i].constraints.C.data(); 119 | hD[i] = stages[i].constraints.D.data(); 120 | hlg[i] = stages[i].constraints.dl.data(); 121 | hug[i] = stages[i].constraints.du.data(); 122 | } else { 123 | hC[i] = nullptr; 124 | hD[i] = nullptr; 125 | hlg[i] = nullptr; 126 | hug[i] = nullptr; 127 | } 128 | } 129 | } 130 | 131 | void HpipmInterface::setSoftConstraints(array &stages) { 132 | for (int i = 0; i <= N; i++) { 133 | hpipm_bounds[i].idx_s.resize(0); 134 | if (stages[i].ns != 0) { 135 | nsbx[i] = 0; 136 | nsbu[i] = 0; 137 | nsg[i] = stages[i].ns; 138 | 139 | for (int j = 0; j < stages[i].ns; j++) { 140 | hpipm_bounds[i].idx_s.push_back(j + nbx[i] + nbu[i]); 141 | } 142 | 143 | hidxs[i] = hpipm_bounds[i].idx_s.data(); 144 | hlls[i] = stages[i].s_lower.data(); 145 | hlus[i] = stages[i].s_upper.data(); 146 | } else { 147 | nsbx[i] = 0; 148 | nsbu[i] = 0; 149 | nsg[i] = 0; 150 | hidxs[i] = nullptr; 151 | hlls[i] = nullptr; 152 | hlus[i] = nullptr; 153 | } 154 | } 155 | } 156 | 157 | array HpipmInterface::solveMPC(array &stages, const State &x0, int *status) { 158 | setDynamics(stages, x0); 159 | setCost(stages); 160 | setBounds(stages, x0); 161 | setPolytopicConstraints(stages); 162 | setSoftConstraints(stages); 163 | 164 | array solution = solve(status); 165 | solution[0].xk = x0; 166 | 167 | return solution; 168 | } 169 | 170 | array HpipmInterface::solve(int *status) { 171 | // ocp qp dim 172 | hpipm_size_t dim_size = d_ocp_qp_dim_memsize(N); 173 | void *dim_mem = malloc(dim_size); 174 | 175 | struct d_ocp_qp_dim dim; 176 | d_ocp_qp_dim_create(N, &dim, dim_mem); 177 | 178 | d_ocp_qp_dim_set_all(nx, nu, nbx, nbu, ng, nsbx, nsbu, nsg, &dim); 179 | // ocp qp 180 | hpipm_size_t qp_size = d_ocp_qp_memsize(&dim); 181 | void *qp_mem = malloc(qp_size); 182 | 183 | struct d_ocp_qp qp; 184 | d_ocp_qp_create(&dim, &qp, qp_mem); 185 | d_ocp_qp_set_all(hA, hB, hb, hQ, hS, hR, hq, hr, 186 | hidxbx, hlbx, hubx, hidxbu, hlbu, hubu, 187 | hC, hD, hlg, hug, hZl, hZu, hzl, hzu, 188 | hidxs, hlls, hlus, &qp); 189 | 190 | // ocp qp sol 191 | hpipm_size_t qp_sol_size = d_ocp_qp_sol_memsize(&dim); 192 | void *qp_sol_mem = malloc(qp_sol_size); 193 | 194 | struct d_ocp_qp_sol qp_sol; 195 | d_ocp_qp_sol_create(&dim, &qp_sol, qp_sol_mem); 196 | 197 | hpipm_size_t ipm_arg_size = d_ocp_qp_ipm_arg_memsize(&dim); 198 | printf("\nipm arg size = %d\n", ipm_arg_size); 199 | void *ipm_arg_mem = malloc(ipm_arg_size); 200 | 201 | struct d_ocp_qp_ipm_arg arg; 202 | d_ocp_qp_ipm_arg_create(&dim, &arg, ipm_arg_mem); 203 | 204 | // enum hpipm_mode mode = SPEED_ABS; 205 | enum hpipm_mode mode = SPEED; 206 | // enum hpipm_mode mode = BALANCE; 207 | // enum hpipm_mode mode = ROBUST; 208 | 209 | // int mode = 1; 210 | double mu0 = 1e2; 211 | int iter_max = 20; 212 | double tol_stat = 1e-6; 213 | double tol_eq = 1e-6; 214 | double tol_ineq = 1e-6; 215 | double tol_comp = 1e-6; 216 | double reg_prim = 1e-12; 217 | int warm_start = 0; 218 | int pred_corr = 1; 219 | int ric_alg = 0; 220 | 221 | d_ocp_qp_ipm_arg_set_default(mode, &arg); 222 | 223 | // d_ocp_qp_ipm_arg_set_mu0(&mu0, &arg); 224 | d_ocp_qp_ipm_arg_set_iter_max(&iter_max, &arg); 225 | // d_ocp_qp_ipm_arg_set_tol_stat(&tol_stat, &arg); 226 | // d_ocp_qp_ipm_arg_set_tol_eq(&tol_eq, &arg); 227 | // d_ocp_qp_ipm_arg_set_tol_ineq(&tol_ineq, &arg); 228 | // d_ocp_qp_ipm_arg_set_tol_comp(&tol_comp, &arg); 229 | // d_ocp_qp_ipm_arg_set_reg_prim(®_prim, &arg); 230 | // d_ocp_qp_ipm_arg_set_warm_start(&warm_start, &arg); 231 | // d_ocp_qp_ipm_arg_set_pred_corr(&pred_corr, &arg); 232 | // d_ocp_qp_ipm_arg_set_ric_alg(&ric_alg, &arg); 233 | 234 | hpipm_size_t ipm_size = d_ocp_qp_ipm_ws_memsize(&dim, &arg); 235 | // printf("\nipm size = %d\n", ipm_size); 236 | void *ipm_mem = malloc(ipm_size); 237 | 238 | struct d_ocp_qp_ipm_ws workspace; 239 | d_ocp_qp_ipm_ws_create(&dim, &arg, &workspace, ipm_mem); 240 | 241 | int hpipm_return; // 0 normal; 1 max iter; 2 linesearch issues? 242 | 243 | struct timeval tv0, tv1; 244 | 245 | gettimeofday(&tv0, nullptr); // start 246 | d_ocp_qp_ipm_solve(&qp, &qp_sol, &arg, &workspace); 247 | d_ocp_qp_ipm_get_status(&workspace, &hpipm_return); 248 | gettimeofday(&tv1, nullptr); // stop 249 | double time_ocp_ipm = (tv1.tv_usec - tv0.tv_usec)/(1e6); 250 | 251 | printf("comp time = %f\n", time_ocp_ipm); 252 | printf("exitflag %d\n", hpipm_return); 253 | printf("ipm iter = %d\n", workspace.iter); 254 | 255 | // extract and print solution 256 | int ii; 257 | int nu_max = nu[0]; 258 | for(ii=1; ii<=N; ii++) 259 | if(nu[ii]>nu_max) 260 | nu_max = nu[ii]; 261 | double *u = (double*)malloc(nu_max*sizeof(double)); 262 | for(ii=0; ii<=N; ii++) { 263 | d_ocp_qp_sol_get_u(ii, &qp_sol, u); 264 | // d_print_mat(1, nu_[ii], u, 1); 265 | } 266 | 267 | int nx_max = nx[0]; 268 | for(ii=1; ii<=N; ii++) 269 | if(nx[ii]>nx_max) 270 | nx_max = nx[ii]; 271 | double *x = (double*)malloc(nx_max*sizeof(double)); 272 | // printf("\nx = \n"); 273 | for(ii=0; ii<=N; ii++) 274 | { 275 | d_ocp_qp_sol_get_x(ii, &qp_sol, x); 276 | // d_print_mat(1, nx_[ii], x, 1); 277 | } 278 | 279 | array optimal_solution; 280 | optimal_solution[0].xk.setZero(); 281 | for(int i=1;i<=N;i++){ 282 | d_ocp_qp_sol_get_x(i, &qp_sol, x); 283 | optimal_solution[i].xk(IndexMap.X) = x[IndexMap.X]; 284 | optimal_solution[i].xk(IndexMap.Y) = x[IndexMap.Y]; 285 | optimal_solution[i].xk(IndexMap.yaw) = x[IndexMap.yaw]; 286 | optimal_solution[i].xk(IndexMap.vx) = x[IndexMap.vx]; 287 | optimal_solution[i].xk(IndexMap.vy) = x[IndexMap.vy]; 288 | optimal_solution[i].xk(IndexMap.wz) = x[IndexMap.wz]; 289 | optimal_solution[i].xk(IndexMap.s) = x[IndexMap.s]; 290 | optimal_solution[i].xk(IndexMap.accel_D) = x[IndexMap.accel_D]; 291 | optimal_solution[i].xk(IndexMap.steering_angle) = x[IndexMap.steering_angle]; 292 | optimal_solution[i].xk(IndexMap.vs) = x[IndexMap.vs]; 293 | } 294 | 295 | for(int i=0;i 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include "hpipm_d_ocp_qp_ipm.h" 18 | #include "hpipm_d_ocp_qp_dim.h" 19 | #include "hpipm_d_ocp_qp.h" 20 | #include "hpipm_d_ocp_qp_sol.h" 21 | #include "hpipm_timing.h" 22 | 23 | #include "../models/state.h" 24 | #include "../mpc/mpc_types.h" 25 | 26 | using std::array; 27 | using std::vector; 28 | using Eigen::Matrix; 29 | 30 | struct HpipmBounds { 31 | vector idx_u; 32 | vector idx_x; 33 | vector idx_s; 34 | vector u_lower; 35 | vector u_upper; 36 | vector x_lower; 37 | vector x_upper; 38 | }; 39 | 40 | class HpipmInterface { 41 | public: 42 | array solveMPC(array &stages, const State &x0, int *status); 43 | private: 44 | int nx[N+1]; // number of states 45 | int nu[N+1]; // number of inputs 46 | int nbx[N+1]; // number of state bounds 47 | int nbu[N+1]; // number of input bounds 48 | int ng[N+1]; // number of polytopic constraints 49 | int nsbx[N+1]; // number of slack variables on state 50 | int nsbu[N+1]; // number of slack variables on input 51 | int nsg[N+1]; // number of slack variables on polytopic constraints 52 | 53 | // LTV dynamics 54 | // xk1 = Ak*xk + Bk*uk + bk 55 | double *hA[N]; // hA[k] = Ak 56 | double *hB[N]; // hB[k] = Bk 57 | double *hb[N]; // hb[k] = bk 58 | 59 | // Cost (without soft constraints) 60 | // min(x,u) sum(0<=k<=N) 0.5 * [xk;uk]^T * [Qk, Sk; Sk^T, Rk] * [xk;uk] + [qk; rk]^T * [xk;uk] 61 | double *hQ[N+1]; 62 | double *hS[N+1]; 63 | double *hR[N+1]; 64 | double *hq[N+1]; 65 | double *hr[N+1]; 66 | 67 | // Polytopic constraints 68 | // g_(lower, k) <= Dk*xk + Ck*uk <= g_(upper, k) 69 | double *hlg[N+1]; 70 | double *hug[N+1]; 71 | double *hD[N+1]; 72 | double *hC[N+1]; 73 | 74 | // General bounds 75 | // x_(lower, k) <= xk <= x_(upper, k) 76 | // hbxid can be used to select bounds on a subset of states 77 | int *hidxbx[N+1]; 78 | double *hlbx[N+1]; 79 | double *hubx[N+1]; 80 | // u_(lower, k) <= uk <= u_(upper, k) 81 | int *hidxbu[N+1]; 82 | double *hlbu[N+1]; 83 | double *hubu[N+1]; 84 | 85 | // Cost (only soft constraints) 86 | // s_(lower, k) -> slack variable of lower polytopic constraint (3) + lower bounds 87 | // s_(upper, k) -> slack variable of upper polytopic constraint (4) + upper bounds 88 | // min(x,u) sum(0<=k<=N) 0.5 * [s_lower, k; s_upper, k]^T * [Z_lower, k, 0; 0, Z_upper, k] * [s_lower, k; s_upper, k] 89 | // + [z_lower, k; z_upper, k]^T * [s_lower, k; s_upper, k] 90 | double *hZl[N+1]; 91 | double *hZu[N+1]; 92 | double *hzl[N+1]; 93 | double *hzu[N+1]; 94 | 95 | // Bounds of soft constraint multipliers 96 | double *hlls[N+1]; 97 | double *hlus[N+1]; 98 | // index of the bounds and constraints that are softened 99 | // order is not really clear 100 | int *hidxs[N+1]; 101 | 102 | // Bounds that are different to stage bounds and need to be stored somewhere 103 | array hpipm_bounds; 104 | Matrix b0; 105 | 106 | void setDynamics(array &stages, const State &x0); 107 | void setCost(array &stages); 108 | void setBounds(array &stages, const State &x0); 109 | void setPolytopicConstraints(array &stages); 110 | void setSoftConstraints(array &stages); 111 | 112 | array solve(int *status); 113 | }; 114 | 115 | #endif //MPCC_HPIPM_INTERFACE_H 116 | -------------------------------------------------------------------------------- /splines/binary_search.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "binary_search.h" 6 | 7 | int mpcc::binary_search_left(const VectorXd &arr, double x) { 8 | int lo = 0; 9 | int hi = arr.size(); 10 | int mid; 11 | 12 | if (x < arr(lo)) { 13 | std::cout << "Binary search for x = " << x << " is outside minimum range" << std::endl; 14 | x = arr(lo); 15 | } 16 | else if (x > arr(hi-1)) { 17 | std::cout << "Binary search for x = " << x << " is outside maximum range" << std::endl; 18 | x = arr(hi-1); 19 | } 20 | 21 | // Taken from Python's bisect.bisect_left() 22 | while (lo < hi) { 23 | mid = (int) (lo + hi) / 2; 24 | if (arr(mid) < x) { lo = mid + 1; } 25 | else { hi = mid; } 26 | } 27 | 28 | // For some reason, bisect.bisect_left() returns the right element 29 | // So we decide to return this index - 1 30 | if (lo > 0) { 31 | return lo - 1; 32 | } 33 | // But, if the index is 0, we return it as it is 34 | else { 35 | return lo; 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /splines/binary_search.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_BINARY_SEARCH_H 6 | #define MPCC_BINARY_SEARCH_H 7 | 8 | #include 9 | #include 10 | 11 | using Eigen::VectorXd; 12 | 13 | namespace mpcc { 14 | int binary_search_left(const VectorXd &arr, double x); 15 | } 16 | 17 | #endif //MPCC_BINARY_SEARCH_H 18 | -------------------------------------------------------------------------------- /splines/cubic_spline.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "cubic_spline.h" 6 | 7 | CubicSpline::CubicSpline(const VectorXd &t_data, const VectorXd &ft_data) { 8 | t_spline = t_data; 9 | ft_spline = ft_data; 10 | setSplineCoefficients(t_data, ft_data); 11 | } 12 | 13 | int CubicSpline::searchIndex(const double t) const { 14 | return mpcc::binary_search_left(t_spline, t); 15 | } 16 | 17 | double CubicSpline::getPosition(double t) const { 18 | t = constrainInput(t); 19 | int index = searchIndex(t); 20 | double h = t - t_spline(index); 21 | double h2 = h * h; 22 | double h3 = h2 * h; 23 | return a(index) + b(index)*h + c(index)*h2 + d(index)*h3; 24 | } 25 | 26 | double CubicSpline::getDerivative(double t) const { 27 | t = constrainInput(t); 28 | int index = searchIndex(t); 29 | double h = t - t_spline(index); 30 | double h2 = h * h; 31 | return b(index) + 2*c(index)*h + 3*d(index)*h2; 32 | } 33 | 34 | double CubicSpline::getSecondDerivative(double t) const { 35 | t = constrainInput(t); 36 | int index = searchIndex(t); 37 | double h = t - t_spline(index); 38 | return c(index)*h + 6*d(index)*h; 39 | } 40 | 41 | void CubicSpline::setSplineCoefficients(const VectorXd &t_data, const VectorXd &ft_data) { 42 | const int n_points = t_data.size(); 43 | int i; 44 | 45 | // Algorithm can be found on 46 | // https://fac.ksu.edu.sa/sites/default/files/numerical_analysis_9th.pdf#page=168 47 | a.setZero(n_points); 48 | b.setZero(n_points - 1); 49 | c.setZero(n_points); 50 | d.setZero(n_points - 1); 51 | 52 | // Helper variables to compute a, b, c and d 53 | VectorXd mu = VectorXd::Zero(n_points - 1); 54 | VectorXd h = VectorXd::Zero(n_points - 1); 55 | VectorXd alpha = VectorXd::Zero(n_points - 1); 56 | VectorXd l = VectorXd::Zero(n_points); 57 | VectorXd z = VectorXd::Zero(n_points); 58 | 59 | // "a" is equal to y 60 | a = ft_data; 61 | 62 | // Compute intervals of t_data, called h 63 | for (i = 0; i < n_points-1; i++) { 64 | h(i) = t_data(i+1) - t_data(i); 65 | } 66 | 67 | // Compute alpha 68 | for(i = 1; i < n_points-1; i++) { 69 | alpha(i) = 3.0/h(i)*(a(i+1)-a(i)) - 3.0/h(i-1)*(a(i)-a(i-1)); 70 | } 71 | 72 | for (i = 1; i < n_points - 1; i++) { 73 | l(i) = 2.0 * (t_data(i+1) - t_data(i-1)) - h(i-1)*mu(i-1); 74 | mu(i) = h(i) / l(i); 75 | z(i) = (alpha(i) - h(i-1)*z(i-1)) / l(i); 76 | } 77 | 78 | l(n_points-1) = 1.0; 79 | z(n_points-1) = 0.0; 80 | // "c" corresponds to 2nd derivative of the spline. At the end-point, we want the 2nd derivative to be 0 81 | c(n_points - 1) = 0.0; 82 | 83 | // Now we work through backwards to compute b, c, d 84 | for (i = n_points - 2; i >= 0; i--) { 85 | c(i) = z(i) - mu(i) * c(i + 1); 86 | b(i) = (a(i + 1) - a(i)) / h(i) - 87 | (h(i)*(c(i + 1) + 2.0 * c(i))) / 3.0; 88 | d(i) = (c(i + 1) - c(i)) / (3.0 * h(i)); 89 | } 90 | } 91 | 92 | double CubicSpline::constrainInput(double t) const { 93 | int n_points = t_spline.size(); 94 | // If t is outside the minimum of the spline parameter, set it to the minimum 95 | if (t < t_spline(0)) { 96 | return t_spline(0); 97 | } 98 | // If t is outside the maximum of the spline parameter, set it to the maximum 99 | if (t > t_spline(n_points-1)) { 100 | return t_spline(n_points-1); 101 | } 102 | return t; 103 | } 104 | -------------------------------------------------------------------------------- /splines/cubic_spline.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_CUBIC_SPLINE_H 6 | #define MPCC_CUBIC_SPLINE_H 7 | 8 | #include 9 | #include 10 | #include "binary_search.h" 11 | 12 | using Eigen::VectorXd; 13 | 14 | class CubicSpline { 15 | public: 16 | CubicSpline() = default; 17 | CubicSpline(const VectorXd &t_data, const VectorXd &ft_data); 18 | int searchIndex(double t) const; 19 | double getPosition(double t) const; 20 | double getDerivative(double t) const; 21 | double getSecondDerivative(double t) const; 22 | // Spline parameter breakpoints 23 | VectorXd t_spline; 24 | VectorXd ft_spline; // f(t) 25 | // Spline coefficients 26 | VectorXd a; 27 | VectorXd b; 28 | VectorXd c; 29 | VectorXd d; 30 | private: 31 | void setSplineCoefficients(const VectorXd &t_data, const VectorXd &ft_data); 32 | double constrainInput(double t) const; 33 | }; 34 | 35 | #endif //MPCC_CUBIC_SPLINE_H 36 | -------------------------------------------------------------------------------- /splines/cubic_spline2d.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "cubic_spline2d.h" 6 | 7 | using Eigen::ArrayXd; 8 | using std::vector; 9 | 10 | CubicSpline2D::CubicSpline2D(const VectorXd &x_data, const VectorXd &y_data, double max_dist_proj_params) { 11 | s_vector = cumulativeSum(calcLineDistances(x_data, y_data)); 12 | sx = CubicSpline(s_vector, x_data); 13 | sy = CubicSpline(s_vector, y_data); 14 | max_dist_proj = max_dist_proj_params; 15 | } 16 | 17 | VectorXd CubicSpline2D::calcLineDistances(const VectorXd &x_data, const VectorXd &y_data) { 18 | double dx, dy; int i; 19 | VectorXd arr = VectorXd::Zero(x_data.size()); 20 | for (i = 1; i < x_data.size(); i++) { 21 | dx = x_data(i) - x_data(i-1); 22 | dy = y_data(i) - y_data(i-1); 23 | arr(i) = hypot(dx, dy); 24 | } 25 | return arr; 26 | } 27 | 28 | VectorXd CubicSpline2D::cumulativeSum(const VectorXd &xy_data) { 29 | double sum = 0.0; int i; 30 | VectorXd arr = VectorXd::Zero(xy_data.size()); 31 | for (i = 0; i < xy_data.size(); i++) { 32 | arr(i) = sum + xy_data(i); 33 | sum += xy_data(i); 34 | } 35 | return arr; 36 | } 37 | 38 | double CubicSpline2D::constrainInput(double s) const { 39 | if (s < s_vector(0)) { 40 | return s_vector(0); 41 | } 42 | if (s > s_vector(s_vector.size()-1)) { 43 | return s_vector(s_vector.size()-1); 44 | } 45 | return s; 46 | } 47 | 48 | Vector2d CubicSpline2D::getPosition(double s) const { 49 | s = constrainInput(s); 50 | Vector2d position = Vector2d::Zero(); 51 | position(0) = sx.getPosition(s); 52 | position(1) = sy.getPosition(s); 53 | return position; 54 | } 55 | 56 | Vector2d CubicSpline2D::getDerivative(double s) const { 57 | s = constrainInput(s); 58 | Vector2d velocity = Vector2d::Zero(); 59 | velocity(0) = sx.getDerivative(s); 60 | velocity(1) = sy.getDerivative(s); 61 | return velocity; 62 | } 63 | 64 | Vector2d CubicSpline2D::getSecondDerivative(double s) const { 65 | s = constrainInput(s); 66 | Vector2d acceleration = Vector2d::Zero(); 67 | acceleration(0) = sx.getSecondDerivative(s); 68 | acceleration(1) = sy.getSecondDerivative(s); 69 | return acceleration; 70 | } 71 | 72 | double CubicSpline2D::getLength() const { return s_vector(s_vector.size()-1); } 73 | 74 | double CubicSpline2D::projectOnSpline(const State &xk) const { 75 | Vector2d pos; 76 | pos(IndexMap.X) = xk(IndexMap.X); 77 | pos(IndexMap.Y) = xk(IndexMap.Y); 78 | double s_guess = xk(IndexMap.s); 79 | Vector2d pos_path = getPosition(s_guess); 80 | 81 | double s_opt = s_guess; 82 | double dist = (pos - pos_path).norm(); 83 | 84 | if (dist >= max_dist_proj) { 85 | ArrayXd diff_x_all = sx.ft_spline.array() - pos(0); 86 | ArrayXd diff_y_all = sy.ft_spline.array() - pos(1); 87 | ArrayXd dist_square = diff_x_all.square() + diff_y_all.square(); 88 | vector dist_square_vec(dist_square.data(), dist_square.data() + dist_square.size()); 89 | auto min_iter = std::min_element(dist_square_vec.begin(), dist_square_vec.end()); 90 | s_opt = s_vector(std::distance(dist_square_vec.begin(), min_iter)); 91 | } 92 | 93 | double s_old = s_opt; 94 | Vector2d ds_path, dds_path, diff; 95 | double jac, hessian; 96 | for (int i = 0; i < 20; i++) { 97 | pos_path = getPosition(s_opt); 98 | ds_path = getDerivative(s_opt); 99 | dds_path = getSecondDerivative(s_opt); 100 | diff = pos_path - pos; 101 | jac = (2.0 * diff(0) * ds_path(0)) + (2.0 * diff(1) * ds_path(1)); 102 | hessian = 2.0 * ds_path(0) * ds_path(0) + 2.0 * diff(0) * dds_path(0) + 103 | 2.0 * ds_path(1) * ds_path(1) + 2.0 * diff(1) * dds_path(1); 104 | // Newton method 105 | s_opt -= jac / hessian; 106 | s_opt = constrainInput(s_opt); 107 | 108 | if (abs(s_old - s_opt) <= 1e-5) { 109 | return s_opt; 110 | } 111 | s_old = s_opt; 112 | } 113 | // something is strange if it did not converge within 20 iterations, give back the initial guess 114 | return s_guess; 115 | } 116 | -------------------------------------------------------------------------------- /splines/cubic_spline2d.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_CUBIC_SPLINE2D_H 6 | #define MPCC_CUBIC_SPLINE2D_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "../models/state.h" 13 | #include "cubic_spline.h" 14 | 15 | using Eigen::VectorXd; 16 | using Eigen::Vector2d; 17 | 18 | class CubicSpline2D { 19 | public: 20 | CubicSpline2D() = default; 21 | CubicSpline2D(const VectorXd &x_data, const VectorXd &y_data, double max_dist_proj_params); 22 | double getLength() const; 23 | Vector2d getPosition(double s) const; 24 | Vector2d getDerivative(double s) const; 25 | Vector2d getSecondDerivative(double s) const; 26 | double projectOnSpline(const State &xk) const; 27 | CubicSpline sx; 28 | CubicSpline sy; 29 | private: 30 | // Function to calculate Pythagorean distance between successive data-points 31 | static VectorXd calcLineDistances(const VectorXd &x_data, const VectorXd &y_data); 32 | static VectorXd cumulativeSum(const VectorXd &xy_data); 33 | double constrainInput(double s) const; 34 | VectorXd s_vector; 35 | double max_dist_proj = 3.0; // TODO: find out what 3.0 means 36 | }; 37 | 38 | #endif //MPCC_CUBIC_SPLINE2D_H 39 | -------------------------------------------------------------------------------- /splines/track.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #include "track.h" 6 | 7 | using json = nlohmann::json; 8 | using std::vector; 9 | 10 | Track mpcc::getTrack(const std::string &file_path) { 11 | std::ifstream file_stream(file_path); 12 | json jsonTrack = json::parse(file_stream); 13 | 14 | vector x_outer = jsonTrack["X_o"]; 15 | vector y_outer = jsonTrack["Y_o"]; 16 | vector x_inner = jsonTrack["X_i"]; 17 | vector y_inner = jsonTrack["Y_i"]; 18 | vector x_centre = jsonTrack["X"]; 19 | vector y_centre = jsonTrack["Y"]; 20 | 21 | VectorXd X_outer = Eigen::Map(x_outer.data(), x_outer.size()); 22 | VectorXd Y_outer = Eigen::Map(y_outer.data(), y_outer.size()); 23 | VectorXd X_inner = Eigen::Map(x_inner.data(), x_inner.size()); 24 | VectorXd Y_inner = Eigen::Map(y_inner.data(), y_inner.size()); 25 | VectorXd X_centre = Eigen::Map(x_centre.data(), x_centre.size()); 26 | VectorXd Y_centre = Eigen::Map(y_centre.data(), y_centre.size()); 27 | 28 | CubicSpline2D outer = CubicSpline2D(X_outer, Y_outer, 3.0); 29 | CubicSpline2D inner = CubicSpline2D(X_inner, Y_inner, 3.0); 30 | CubicSpline2D centre = CubicSpline2D(X_centre, Y_centre, 3.0); 31 | CubicSpline2D path = CubicSpline2D(X_centre, Y_centre, 3.0); 32 | 33 | return { outer, inner, centre, path }; 34 | } 35 | -------------------------------------------------------------------------------- /splines/track.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Dennis Wirya (dwirya@student.unimelb.edu.au). 3 | // Copyright (c) 2021 MUR Driverless. All rights reserved. 4 | // 5 | #ifndef MPCC_TRACK_H 6 | #define MPCC_TRACK_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "cubic_spline2d.h" 14 | 15 | struct Track { 16 | CubicSpline2D outer; 17 | CubicSpline2D inner; 18 | CubicSpline2D centre; 19 | CubicSpline2D path; 20 | }; 21 | 22 | namespace mpcc { 23 | Track getTrack(const std::string &file_path); 24 | } 25 | 26 | #endif //MPCC_TRACK_H 27 | --------------------------------------------------------------------------------