├── C++ ├── .gitignore ├── CMakeLists.txt ├── Constraints │ ├── bounds.cpp │ ├── bounds.h │ ├── constraints.cpp │ ├── constraints.h │ ├── soft_constraints.cpp │ └── soft_constraints.h ├── Cost │ ├── cost.cpp │ └── cost.h ├── Interfaces │ ├── hpipm_interface.cpp │ ├── hpipm_interface.h │ ├── solver_interface.cpp │ └── solver_interface.h ├── MPC │ ├── mpc.cpp │ └── mpc.h ├── Model │ ├── integrator.cpp │ ├── integrator.h │ ├── model.cpp │ └── model.h ├── Params │ ├── bounds.json │ ├── config.json │ ├── cost.json │ ├── model.json │ ├── normalization.json │ ├── params.cpp │ ├── params.h │ ├── track.cpp │ ├── track.h │ └── track.json ├── Plotting │ ├── plotting.cpp │ └── plotting.h ├── README.md ├── Spline │ ├── arc_length_spline.cpp │ ├── arc_length_spline.h │ ├── cubic_spline.cpp │ └── cubic_spline.h ├── Tests │ ├── constratins_test.cpp │ ├── constratins_test.h │ ├── cost_test.cpp │ ├── cost_test.h │ ├── model_integrator_test.cpp │ ├── model_integrator_test.h │ ├── spline_test.cpp │ └── spline_test.h ├── config.h ├── install.sh ├── main.cpp ├── types.cpp └── types.h ├── Images ├── MPC_sim.gif ├── Model.jpg ├── NMPCC_problem.jpg ├── TireModel.jpg ├── constraints_cpp.jpg ├── cost_cpp.jpg ├── forces_cpp.jpg ├── model_cpp.jpg ├── state-input.jpg └── state_input_cpp.jpg ├── LICENSE ├── Matlab ├── CVXInterface.m ├── DiscretizedLinearizedModel.m ├── ObstacelsState.m ├── PlotLog.m ├── PlotPrediction.m ├── QuadProgInterface.m ├── README.md ├── SimTimeStep.m ├── Tracks │ ├── track2.mat │ └── trackMobil.mat ├── YalmipInterface.m ├── augState.m ├── borderAdjustment.m ├── carBox.m ├── env.sh ├── findTheta.m ├── getMPC_vars.m ├── getMPCmatrices.m ├── getModelParams.m ├── getNewBorders.m ├── hpipmInterface.m ├── optimizer_mpcc.m ├── simulation.m ├── splines │ ├── computeCenter.m │ ├── getSplineDerivatives.m │ ├── normalizedSplineInterp.m │ ├── regularindex.m │ ├── spline5.m │ ├── splineInterp.m │ ├── splinelength.m │ └── splinify.m └── unWrapX0.m └── README.md /C++/.gitignore: -------------------------------------------------------------------------------- 1 | # Project specific excludes 2 | /External/ 3 | /MPCC 4 | 5 | # CMake 6 | CMakeCache.txt 7 | CMakeFiles/ 8 | Makefile 9 | cmake_install.cmake 10 | -------------------------------------------------------------------------------- /C++/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.7) 2 | project(MPCC) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_FLAGS "-O2") 6 | 7 | include_directories(.) 8 | include_directories(External/blasfeo/lib/include) 9 | include_directories(External/hpipm/lib/include) 10 | include_directories(External/matplotlib) 11 | include_directories(External/Eigen) 12 | include_directories(External/Json/include) 13 | 14 | add_executable(MPCC 15 | main.cpp 16 | types.cpp 17 | types.h 18 | config.h 19 | Params/params.cpp 20 | Spline/cubic_spline.cpp 21 | Spline/arc_length_spline.cpp 22 | Interfaces/hpipm_interface.cpp 23 | Interfaces/solver_interface.cpp 24 | Constraints/constraints.cpp 25 | Constraints/bounds.cpp 26 | Cost/cost.cpp 27 | Model/model.cpp 28 | Model/integrator.cpp 29 | Tests/spline_test.cpp 30 | Tests/cost_test.cpp 31 | Tests/model_integrator_test.cpp 32 | Tests/constratins_test.cpp 33 | MPC/mpc.cpp 34 | Params/track.cpp 35 | Params/track.h 36 | Plotting/plotting.cpp 37 | Plotting/plotting.h) 38 | 39 | find_package(Python COMPONENTS Development) 40 | target_include_directories(MPCC PRIVATE ${Python_INCLUDE_DIRS}) 41 | target_link_libraries(MPCC ${Python_LIBRARIES}) 42 | 43 | target_link_libraries(MPCC ${CMAKE_SOURCE_DIR}/External/hpipm/lib/lib/libhpipm.a ${CMAKE_SOURCE_DIR}/External/blasfeo/lib/lib/libblasfeo.a m) 44 | -------------------------------------------------------------------------------- /C++/Constraints/bounds.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "bounds.h" 18 | namespace mpcc{ 19 | Bounds::Bounds() 20 | { 21 | std::cout << "default constructor, not everything is initialized properly" << std::endl; 22 | } 23 | 24 | Bounds::Bounds(BoundsParam bounds_param) 25 | { 26 | l_bounds_x_(0) = bounds_param.lower_state_bounds.X_l; 27 | l_bounds_x_(1) = bounds_param.lower_state_bounds.Y_l; 28 | l_bounds_x_(2) = bounds_param.lower_state_bounds.phi_l; 29 | l_bounds_x_(3) = bounds_param.lower_state_bounds.vx_l; 30 | l_bounds_x_(4) = bounds_param.lower_state_bounds.vy_l; 31 | l_bounds_x_(5) = bounds_param.lower_state_bounds.r_l; 32 | l_bounds_x_(6) = bounds_param.lower_state_bounds.s_l; 33 | l_bounds_x_(7) = bounds_param.lower_state_bounds.D_l; 34 | l_bounds_x_(8) = bounds_param.lower_state_bounds.delta_l; 35 | l_bounds_x_(9) = bounds_param.lower_state_bounds.vs_l; 36 | 37 | u_bounds_x_(0) = bounds_param.upper_state_bounds.X_u; 38 | u_bounds_x_(1) = bounds_param.upper_state_bounds.Y_u; 39 | u_bounds_x_(2) = bounds_param.upper_state_bounds.phi_u; 40 | u_bounds_x_(3) = bounds_param.upper_state_bounds.vx_u; 41 | u_bounds_x_(4) = bounds_param.upper_state_bounds.vy_u; 42 | u_bounds_x_(5) = bounds_param.upper_state_bounds.r_u; 43 | u_bounds_x_(6) = bounds_param.upper_state_bounds.s_u; 44 | u_bounds_x_(7) = bounds_param.upper_state_bounds.D_u; 45 | u_bounds_x_(8) = bounds_param.upper_state_bounds.delta_u; 46 | u_bounds_x_(9) = bounds_param.upper_state_bounds.vs_u; 47 | 48 | l_bounds_u_(0) = bounds_param.lower_input_bounds.dD_l; 49 | l_bounds_u_(1) = bounds_param.lower_input_bounds.dDelta_l; 50 | l_bounds_u_(2) = bounds_param.lower_input_bounds.dVs_l; 51 | 52 | u_bounds_u_(0) = bounds_param.upper_input_bounds.dD_u; 53 | u_bounds_u_(1) = bounds_param.upper_input_bounds.dDelta_u; 54 | u_bounds_u_(2) = bounds_param.upper_input_bounds.dVs_u; 55 | 56 | l_bounds_s_ = Bounds_s::Zero(); 57 | u_bounds_s_ = Bounds_s::Zero(); 58 | 59 | std::cout << "bounds initialized" << std::endl; 60 | } 61 | 62 | Bounds_x Bounds::getBoundsLX() const 63 | { 64 | return l_bounds_x_; 65 | } 66 | 67 | Bounds_x Bounds::getBoundsUX() const 68 | { 69 | return u_bounds_x_; 70 | } 71 | 72 | Bounds_u Bounds::getBoundsLU() const 73 | { 74 | return l_bounds_u_; 75 | } 76 | 77 | Bounds_u Bounds::getBoundsUU() const 78 | { 79 | return u_bounds_u_; 80 | } 81 | 82 | Bounds_s Bounds::getBoundsLS() const 83 | { 84 | return l_bounds_s_; 85 | } 86 | 87 | Bounds_s Bounds::getBoundsUS() const{ 88 | return u_bounds_s_; 89 | } 90 | } -------------------------------------------------------------------------------- /C++/Constraints/bounds.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_BOUNDS_H 18 | #define MPCC_BOUNDS_H 19 | 20 | #include "config.h" 21 | #include "types.h" 22 | #include "Params/params.h" 23 | 24 | namespace mpcc{ 25 | class Bounds { 26 | public: 27 | Bounds(); 28 | Bounds(BoundsParam bounds_param); 29 | 30 | Bounds_x getBoundsLX() const; 31 | Bounds_x getBoundsUX() const; 32 | 33 | Bounds_u getBoundsLU() const; 34 | Bounds_u getBoundsUU() const; 35 | 36 | Bounds_s getBoundsLS() const; 37 | Bounds_s getBoundsUS() const; 38 | 39 | private: 40 | 41 | Bounds_x u_bounds_x_; 42 | Bounds_x l_bounds_x_; 43 | 44 | Bounds_u u_bounds_u_; 45 | Bounds_u l_bounds_u_; 46 | 47 | Bounds_s u_bounds_s_; 48 | Bounds_s l_bounds_s_; 49 | }; 50 | } 51 | #endif //MPCC_BOUNDS_H 52 | -------------------------------------------------------------------------------- /C++/Constraints/constraints.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "constraints.h" 18 | namespace mpcc{ 19 | Constraints::Constraints() 20 | { 21 | std::cout << "default constructor, not everything is initialized properly" << std::endl; 22 | } 23 | 24 | Constraints::Constraints(double Ts,const PathToJson &path) 25 | :model_(Ts,path), 26 | param_(Param(path.param_path)) 27 | { 28 | } 29 | 30 | OneDConstraint Constraints::getTrackConstraints(const ArcLengthSpline &track,const State &x) const 31 | { 32 | // given arc length s and the track -> compute linearized track constraints 33 | const double s = x.s; 34 | 35 | // X-Y point of the center line 36 | const Eigen::Vector2d pos_center = track.getPostion(s); 37 | const Eigen::Vector2d d_center = track.getDerivative(s); 38 | // Tangent of center line at s 39 | const Eigen::Vector2d tan_center = {-d_center(1),d_center(0)}; 40 | 41 | // inner and outer track boundary given left and right width of track 42 | // TODO make R_out and R_in dependent on s 43 | const Eigen::Vector2d pos_outer = pos_center + param_.r_out*tan_center; 44 | const Eigen::Vector2d pos_inner = pos_center - param_.r_in*tan_center; 45 | 46 | // Define track Jacobian as Perpendicular vector 47 | C_i_MPC C_track_constraint = C_i_MPC::Zero(); 48 | C_track_constraint(0,0) = tan_center(0); 49 | C_track_constraint(0,1) = tan_center(1); 50 | // Compute bounds 51 | const double track_constraint_lower = tan_center(0)*pos_inner(0) + tan_center(1)*pos_inner(1); 52 | const double track_constraint_upper = tan_center(0)*pos_outer(0) + tan_center(1)*pos_outer(1); 53 | 54 | return {C_track_constraint,track_constraint_lower,track_constraint_upper}; 55 | } 56 | 57 | OneDConstraint Constraints::getTireConstraintRear(const State &x) const 58 | { 59 | // compute tire friction elipse constraints 60 | // (param_.E_long*Frx)^2 + Fry^2 <= (param_.E_eps*F_max)^2 61 | const StateVector x_vec = stateToVector(x); 62 | const TireForces f_rear = model_.getForceRear(x); 63 | const NormalForces f_normal = model_.getForceNormal(x); 64 | 65 | // compute tire friction constraint jacobean 66 | const C_i_MPC C_tire_constraint = getTireConstraintRearJac(x); 67 | 68 | // compute zero order term and max force 69 | // const double tireCon0 = std::sqrt(std::pow(param_.e_long*f_rear.F_x,2) + std::pow(f_rear.F_y,2)); //zero order term 70 | const double tireCon0 = std::pow(param_.e_long*f_rear.F_x/f_normal.F_N_rear,2) + std::pow(f_rear.F_y/f_normal.F_N_rear,2); //zero order term 71 | const double maxForce = std::pow(param_.e_eps*param_.Dr/f_normal.F_N_rear,2);//param_.e_eps*param_.Dr;// //max allowed force 72 | 73 | // set bounds given linearized constraint 74 | // 0 <= 'Jac TC' (x - x0) + TC(x0) <= F_max 75 | const double tire_constraint_lower = C_tire_constraint*x_vec-tireCon0; 76 | const double tire_constraint_upper = maxForce+C_tire_constraint*x_vec-tireCon0; 77 | 78 | return {C_tire_constraint,tire_constraint_lower,tire_constraint_upper}; 79 | } 80 | 81 | C_i_MPC Constraints::getTireConstraintRearJac(const State &x) const 82 | { 83 | // compute Jacobean of the tire constraints 84 | const TireForces f_rear = model_.getForceRear(x); 85 | const TireForcesDerivatives df_rear = model_.getForceRearDerivatives(x); 86 | const NormalForces f_normal = model_.getForceNormal(x); 87 | 88 | // const double TC = 2.0*std::sqrt(std::pow(param_.e_long*f_rear.F_x,2) + std::pow(f_rear.F_y,2)); 89 | // 90 | // // Tire constraint derivatives 91 | // // TC = (param_.E_long*Frx)^2 + Fry^2 92 | // const double dTC_dvx = (2.0*param_.e_long*f_rear.F_x*df_rear.dF_x_vx + 2.0*f_rear.F_y*df_rear.dF_y_vx)/TC; 93 | // const double dTC_dvy = (2.0*f_rear.F_y*df_rear.dF_y_vy)/TC; 94 | // const double dTC_dr = (2.0*f_rear.F_y*df_rear.dF_y_r)/TC; 95 | // const double dTC_dD = (2.0*param_.e_long*f_rear.F_x*df_rear.dF_x_D)/TC; 96 | 97 | const double TC = std::pow(param_.e_long*f_rear.F_x/f_normal.F_N_rear,2) + std::pow(f_rear.F_y/f_normal.F_N_rear,2); 98 | 99 | // Tire constraint derivatives 100 | // TC = (param_.e_long*Frx)^2 + Fry^2 101 | const double dTC_dvx = (2.0*param_.e_long*f_rear.F_x/f_normal.F_N_rear*param_.e_long*df_rear.dF_x_vx/f_normal.F_N_rear + 102 | 2.0*f_rear.F_y/f_normal.F_N_rear*df_rear.dF_y_vx/f_normal.F_N_rear); 103 | const double dTC_dvy = (2.0*f_rear.F_y/f_normal.F_N_rear*df_rear.dF_y_vy/f_normal.F_N_rear); 104 | const double dTC_dr = (2.0*f_rear.F_y/f_normal.F_N_rear*df_rear.dF_y_r/f_normal.F_N_rear); 105 | const double dTC_dD = (2.0*param_.e_long*f_rear.F_x/f_normal.F_N_rear*param_.e_long*df_rear.dF_x_D/f_normal.F_N_rear); 106 | 107 | // Copy partial derivatives in jacobean matrix 108 | C_i_MPC Jac_tireCon = C_i_MPC::Zero(); 109 | Jac_tireCon(si_index.vx) = dTC_dvx; 110 | Jac_tireCon(si_index.vy) = dTC_dvy; 111 | Jac_tireCon(si_index.r) = dTC_dr; 112 | Jac_tireCon(si_index.D) = dTC_dD; 113 | 114 | return Jac_tireCon; 115 | } 116 | 117 | OneDConstraint Constraints::getAlphaConstraintFront(const State &x) const 118 | { 119 | // compute linearized slip angle constraints 120 | // -alpha_max <= alpha_f <= alpha_max 121 | const StateVector x_vec = stateToVector(x); 122 | const double alpha_f = model_.getSlipAngleFront(x); 123 | // compute the jacobean of alpha_f 124 | const C_i_MPC C_alpha_constraint = getAlphaConstraintFrontJac(x); 125 | // compute the bounds given the Tylor series expansion 126 | const double alpha_constraint_lower = -param_.max_alpha-alpha_f+C_alpha_constraint*x_vec; 127 | const double alpha_constraint_upper = param_.max_alpha-alpha_f+C_alpha_constraint*x_vec; 128 | 129 | return {C_alpha_constraint,alpha_constraint_lower,alpha_constraint_upper}; 130 | } 131 | 132 | C_i_MPC Constraints::getAlphaConstraintFrontJac(const State &x) const 133 | { 134 | // compute the alpha_f jacobian 135 | const double vx = x.vx; 136 | const double vy = x.vy; 137 | const double r = x.r; 138 | const double delta = x.delta; 139 | 140 | C_i_MPC Jac_alphaCon; 141 | // alpha_f = -atan(vy+r*param_.lf/vx) + delta; 142 | // compute partial derivatives 143 | const double dalpha_f_dvx = (vy+r*param_.lf)/(std::pow(vy+r*param_.lf,2)+std::pow(vx,2)); 144 | const double dalpha_f_dvy = -vx/(std::pow(vy+r*param_.lf,2)+std::pow(vx,2)); 145 | const double dalpha_f_dr = -(vx*param_.lf)/(std::pow(vy+r*param_.lf,2)+std::pow(vx,2)); 146 | const double dalpha_f_ddelta = 1.0; 147 | 148 | // Copy partial derivatives in jacobean matrix 149 | Jac_alphaCon.setZero(); 150 | Jac_alphaCon(si_index.vx) = dalpha_f_dvx; 151 | Jac_alphaCon(si_index.vy) = dalpha_f_dvy; 152 | Jac_alphaCon(si_index.r) = dalpha_f_dr; 153 | Jac_alphaCon(si_index.delta)= dalpha_f_ddelta; 154 | 155 | return Jac_alphaCon; 156 | 157 | } 158 | 159 | ConstrainsMatrix Constraints::getConstraints(const ArcLengthSpline &track,const State &x,const Input &u) const 160 | { 161 | // compute all the polytopic state constraints 162 | // compute the three constraints 163 | 164 | ConstrainsMatrix constrains_matrix; 165 | const OneDConstraint track_constraints = getTrackConstraints(track,x); 166 | const OneDConstraint tire_constraints_rear = getTireConstraintRear(x); 167 | const OneDConstraint alpha_constraints_front = getAlphaConstraintFront(x); 168 | 169 | C_MPC C_constrains_matrix; 170 | d_MPC dl_constrains_matrix; 171 | d_MPC du_constrains_matrix; 172 | 173 | C_constrains_matrix.row(si_index.con_track) = track_constraints.C_i; 174 | dl_constrains_matrix(si_index.con_track) = track_constraints.dl_i; 175 | du_constrains_matrix(si_index.con_track) = track_constraints.du_i; 176 | 177 | C_constrains_matrix.row(si_index.con_tire) = tire_constraints_rear.C_i; 178 | dl_constrains_matrix(si_index.con_tire) = tire_constraints_rear.dl_i; 179 | du_constrains_matrix(si_index.con_tire) = tire_constraints_rear.du_i; 180 | 181 | C_constrains_matrix.row(si_index.con_alpha) = alpha_constraints_front.C_i; 182 | dl_constrains_matrix(si_index.con_alpha) = alpha_constraints_front.dl_i; 183 | du_constrains_matrix(si_index.con_alpha) = alpha_constraints_front.du_i; 184 | 185 | return {C_constrains_matrix,D_MPC::Zero(),dl_constrains_matrix,du_constrains_matrix}; 186 | } 187 | } -------------------------------------------------------------------------------- /C++/Constraints/constraints.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_CONSTRAINTS_H 18 | #define MPCC_CONSTRAINTS_H 19 | 20 | #include "config.h" 21 | #include "Spline/arc_length_spline.h" 22 | #include "Model/model.h" 23 | namespace mpcc{ 24 | struct ConstrainsMatrix { 25 | // dl <= C xk + D uk <= du 26 | C_MPC C; //polytopic state constraints 27 | D_MPC D; //polytopic input constraints 28 | d_MPC dl; //lower bounds 29 | d_MPC du; //upper bounds 30 | }; 31 | 32 | struct OneDConstraint { 33 | const C_i_MPC C_i; 34 | const double dl_i; 35 | const double du_i; 36 | }; 37 | 38 | class Constraints { 39 | public: 40 | ConstrainsMatrix getConstraints(const ArcLengthSpline &track,const State &x,const Input &u) const; 41 | 42 | Constraints(); 43 | Constraints(double Ts,const PathToJson &path); 44 | private: 45 | OneDConstraint getTrackConstraints(const ArcLengthSpline &track,const State &x) const; 46 | 47 | OneDConstraint getTireConstraintRear(const State &x) const; 48 | C_i_MPC getTireConstraintRearJac(const State &x) const; 49 | 50 | OneDConstraint getAlphaConstraintFront(const State &x) const; 51 | C_i_MPC getAlphaConstraintFrontJac(const State &x) const; 52 | 53 | Model model_; 54 | Param param_; 55 | }; 56 | } 57 | 58 | #endif //MPCC_CONSTRAINTS_H 59 | -------------------------------------------------------------------------------- /C++/Constraints/soft_constraints.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "soft_constraints.h" 18 | -------------------------------------------------------------------------------- /C++/Constraints/soft_constraints.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_SOFTCONSTRAINTS_H 18 | #define MPCC_SOFTCONSTRAINTS_H 19 | 20 | 21 | #include "config.h" 22 | #include "types.h" 23 | namespace mpcc { 24 | class SoftConstraints { 25 | 26 | private: 27 | Q_MPC Zx; 28 | R_MPC Zu; 29 | 30 | q_MPC zx; 31 | r_MPC zu; 32 | }; 33 | } 34 | 35 | #endif //MPCC_SOFTCONSTRAINTS_H 36 | -------------------------------------------------------------------------------- /C++/Cost/cost.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_COST_H 18 | #define MPCC_COST_H 19 | 20 | #include "config.h" 21 | #include "types.h" 22 | #include "Spline/arc_length_spline.h" 23 | 24 | namespace mpcc{ 25 | struct CostMatrix{ 26 | Q_MPC Q; 27 | R_MPC R; 28 | S_MPC S; 29 | q_MPC q; 30 | r_MPC r; 31 | Z_MPC Z; 32 | z_MPC z; 33 | }; 34 | 35 | struct TrackPoint{ 36 | const double x_ref; 37 | const double y_ref; 38 | const double dx_ref; 39 | const double dy_ref; 40 | const double theta_ref; 41 | const double dtheta_ref; 42 | }; 43 | 44 | struct ErrorInfo{ 45 | const Eigen::Matrix error; 46 | const Eigen::Matrix d_error; 47 | }; 48 | 49 | class Cost { 50 | public: 51 | CostMatrix getCost(const ArcLengthSpline &track, const State &x,int k) const; 52 | 53 | Cost(const PathToJson &path); 54 | Cost(); 55 | 56 | private: 57 | TrackPoint getRefPoint(const ArcLengthSpline &track,const State &x) const; 58 | ErrorInfo getErrorInfo(const ArcLengthSpline &track,const State &x) const; 59 | 60 | CostMatrix getContouringCost(const ArcLengthSpline &track, const State &x,int k) const; 61 | CostMatrix getHeadingCost(const ArcLengthSpline &track, const State &x,int k) const; 62 | CostMatrix getInputCost() const; 63 | CostMatrix getBetaCost(const State &x) const; 64 | CostMatrix getBetaKinCost(const State &x) const; 65 | CostMatrix getSoftConstraintCost() const; 66 | 67 | CostParam cost_param_; 68 | Param param_; 69 | }; 70 | } 71 | #endif //MPCC_COST_H 72 | -------------------------------------------------------------------------------- /C++/Interfaces/hpipm_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_HPIPM_INTERFACE_H 18 | #define MPCC_HPIPM_INTERFACE_H 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include "hpipm_d_ocp_qp_ipm.h" 28 | #include "hpipm_d_ocp_qp_dim.h" 29 | #include "hpipm_d_ocp_qp.h" 30 | #include "hpipm_d_ocp_qp_sol.h" 31 | #include "hpipm_timing.h" 32 | 33 | #include "config.h" 34 | #include "types.h" 35 | #include "Model/model.h" 36 | #include "Cost/cost.h" 37 | #include "Constraints/constraints.h" 38 | #include "Constraints/bounds.h" 39 | #include "solver_interface.h" 40 | 41 | #include 42 | #include 43 | 44 | namespace mpcc{ 45 | struct OptVariables; 46 | struct Stage; 47 | 48 | struct HpipmBound { 49 | std::vector idx_u; 50 | std::vector idx_x; 51 | std::vector idx_s; 52 | std::vector lower_bounds_u; 53 | std::vector upper_bounds_u; 54 | std::vector lower_bounds_x; 55 | std::vector upper_bounds_x; 56 | }; 57 | 58 | class HpipmInterface : public SolverInterface { 59 | public: 60 | std::array solveMPC(std::array &stages,const State &x0, int *status); 61 | 62 | ~HpipmInterface(){ 63 | std::cout << "Deleting Hpipm Interface" << std::endl; 64 | } 65 | private: 66 | int nx_[N+1];// -> number of states 67 | int nu_[N+1];// -> number of inputs 68 | int nbx_[N+1];// -> number of bounds on x 69 | int nbu_[N+1];// -> number of bounds on u 70 | int ng_[N+1];// -> number of polytopic constratins 71 | int nsbx_[N+1];// -> number of slacks variables on x 72 | int nsbu_[N+1];// -> number of slacks variables on u 73 | int nsg_[N+1];// -> number of slacks variables on polytopic constraints 74 | 75 | // LTV dynamics 76 | // x_k+1 = A_k x_k + B_k u_k + b_k 77 | double *hA_[N]; //hA[k] = A_k 78 | double *hB_[N]; //hB[k] = B_k 79 | double *hb_[N]; //hb[k] = b_k 80 | 81 | // Cost (without soft constraints) 82 | // min_x,u sum_k=0^N 1/2*[x_k;u_k]^T*[Q_k , S_k; S_k^T , R_k]*[x_k;u_k] + [q_k; r_k]^T*[x_k;u_k] 83 | double *hQ_[N+1]; //hQ[k] = Q_k 84 | double *hS_[N+1]; //hS[k] = S_k 85 | double *hR_[N+1]; //hR[k] = R_k 86 | double *hq_[N+1]; //hq[k] = q_k 87 | double *hr_[N+1]; //hr[k] = r_k 88 | 89 | // Polytopic constraints 90 | // g_lower,k <= D_k*x_k + C_k*u_k 91 | // D_k*x_k + C_k*u_k <= g_upper,k 92 | double *hlg_[N+1]; //hlg[k] = g_lower,k 93 | double *hug_[N+1]; //hug[k] = g_upper,k 94 | double *hC_[N+1]; //hC[k] = C_k 95 | double *hD_[N+1]; //hD[k] = D_k 96 | 97 | // General bounds 98 | // x_lower,k <= x_k <= x_upper,k 99 | // hidxbx can be used to select bounds on a subset of states 100 | int *hidxbx_[N+1]; // hidxbx[k] = {0,1,2,...,nx} for bounds on all inputs and states 101 | double *hlbx_[N+1]; // x_lower,k 102 | double *hubx_[N+1]; //x_upper,k 103 | // u_lower,k <= u_k <= u_upper,k 104 | // hidxbu can be used to select bounds on a subset of inputs 105 | int *hidxbu_[N+1]; // hidxbuk] = {0,1,2,...,nu} for bounds on all inputs and states 106 | double *hlbu_[N+1]; // u_lower,k 107 | double *hubu_[N+1]; // u_upper,k 108 | 109 | // Cost (only soft constriants) 110 | // s_lower,k -> slack variable of lower polytopic constraint (3) + lower bounds 111 | // s_upper,k -> slack variable of upper polytopic constraint (4) + upper bounds 112 | // min_x,u sum_k=0^N 1/2*[s_lower,k;s_upper,k]^T*[Z_lower,k , 0; 0 , Z_upper,k]*[s_lower,k;s_upper,k] + [z_lower,k; z_upper,k]^T*[s_lower,k;s_upper,k] 113 | double *hZl_[N+1]; // hZl[k] = Z_lower,k 114 | double *hZu_[N+1]; // hZu[k] = Z_upper,k 115 | double *hzl_[N+1]; // hzl[k] = z_lower,k 116 | double *hzu_[N+1]; // hzu[k] = z_upper,k 117 | 118 | // Bounds on the soft constraint multipliers 119 | // s_lower,k >= s_lower,bound,k 120 | // s_upper,k >= s_upper,bound,k 121 | double *hlls_[N+1]; 122 | double *hlus_[N+1]; 123 | // index of the bounds and constraints that are softened 124 | // order is not really clear 125 | int *hidxs_[N+1]; 126 | 127 | //bounds that are different to stages bounds and need to be stored somewhere such the a pointer can point 128 | std::array hpipm_bounds_; 129 | Eigen::Matrix b0_; 130 | 131 | void setDynamics(std::array &stages,const State &x0); 132 | void setCost(std::array &stages); 133 | void setBounds(std::array &stages,const State &x0); 134 | void setPolytopicConstraints(std::array &stages); 135 | void setSoftConstraints(std::array &stages); 136 | 137 | std::array Solve(int *status); 138 | 139 | void print_data(); 140 | }; 141 | } 142 | #endif //MPCC_HPIPM_INTERFACE_H -------------------------------------------------------------------------------- /C++/Interfaces/solver_interface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "solver_interface.h" 18 | -------------------------------------------------------------------------------- /C++/Interfaces/solver_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_SOLVER_INTERFACE_H 18 | #define MPCC_SOLVER_INTERFACE_H 19 | 20 | #include "config.h" 21 | #include "types.h" 22 | 23 | 24 | 25 | #include 26 | namespace mpcc{ 27 | struct OptVariables; 28 | struct Stage; 29 | class SolverInterface { 30 | public: 31 | virtual std::array solveMPC(std::array &stages,const State &x0,int *status) = 0; 32 | virtual ~SolverInterface(){ 33 | std::cout << "Deleting Solver Interface" << std::endl; 34 | } 35 | }; 36 | } 37 | 38 | #endif //MPCC_SOLVER_INTERFACE_H 39 | -------------------------------------------------------------------------------- /C++/MPC/mpc.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_MPC_H 18 | #define MPCC_MPC_H 19 | 20 | #include "config.h" 21 | #include "types.h" 22 | #include "Params/params.h" 23 | #include "Spline/arc_length_spline.h" 24 | #include "Model/model.h" 25 | #include "Model/integrator.h" 26 | #include "Cost/cost.h" 27 | #include "Constraints/constraints.h" 28 | #include "Constraints/bounds.h" 29 | 30 | #include "Interfaces/solver_interface.h" 31 | #include "Interfaces/hpipm_interface.h" 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace mpcc{ 40 | 41 | struct OptVariables { 42 | State xk; 43 | Input uk; 44 | }; 45 | 46 | struct Stage { 47 | LinModelMatrix lin_model; 48 | CostMatrix cost_mat; 49 | ConstrainsMatrix constrains_mat; 50 | 51 | Bounds_x u_bounds_x; 52 | Bounds_x l_bounds_x; 53 | 54 | Bounds_u u_bounds_u; 55 | Bounds_u l_bounds_u; 56 | 57 | Bounds_s u_bounds_s; 58 | Bounds_s l_bounds_s; 59 | 60 | //nx -> number of states 61 | //nu -> number of inputs 62 | //nbx -> number of bounds on x 63 | //nbu -> number of bounds on u 64 | //ng -> number of polytopic constratins 65 | //ns -> number of soft constraints 66 | int nx, nu, nbx, nbu, ng, ns; 67 | }; 68 | 69 | struct MPCReturn { 70 | const Input u0; 71 | const std::array mpc_horizon; 72 | const double time_total; 73 | }; 74 | 75 | class MPC { 76 | public: 77 | MPCReturn runMPC(State &x0); 78 | 79 | void setTrack(const Eigen::VectorXd &X, const Eigen::VectorXd &Y); 80 | 81 | MPC(); 82 | MPC(int n_sqp, int n_reset, double sqp_mixing, double Ts,const PathToJson &path); 83 | 84 | private: 85 | bool valid_initial_guess_; 86 | 87 | std::array stages_; 88 | 89 | std::array initial_guess_; 90 | std::array optimal_solution_; 91 | 92 | void setMPCProblem(); 93 | 94 | void setStage(const State &xk, const Input &uk, int time_step); 95 | 96 | CostMatrix normalizeCost(const CostMatrix &cost_mat); 97 | LinModelMatrix normalizeDynamics(const LinModelMatrix &lin_model); 98 | ConstrainsMatrix normalizeCon(const ConstrainsMatrix &con_mat); 99 | std::array deNormalizeSolution(const std::array &solution); 100 | 101 | void updateInitialGuess(const State &x0); 102 | 103 | void generateNewInitialGuess(const State &x0); 104 | 105 | void unwrapInitialGuess(); 106 | 107 | std::array sqpSolutionUpdate(const std::array &last_solution, 108 | const std::array ¤t_solution); 109 | 110 | int n_sqp_; 111 | double sqp_mixing_; 112 | int n_non_solves_; 113 | int n_no_solves_sqp_; 114 | int n_reset_; 115 | 116 | const double Ts_; 117 | 118 | Model model_; 119 | Integrator integrator_; 120 | Cost cost_; 121 | Constraints constraints_; 122 | ArcLengthSpline track_; 123 | 124 | Bounds bounds_; 125 | NormalizationParam normalization_param_; 126 | Param param_; 127 | 128 | std::unique_ptr solver_interface_; 129 | }; 130 | 131 | } 132 | 133 | #endif //MPCC_MPC_H 134 | -------------------------------------------------------------------------------- /C++/Model/integrator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "integrator.h" 18 | namespace mpcc{ 19 | Integrator::Integrator() 20 | { 21 | std::cout << "default constructor, not everything is initialized properly" << std::endl; 22 | } 23 | 24 | Integrator::Integrator(double Ts,const PathToJson &path) 25 | :model_(Ts,path) 26 | { 27 | } 28 | 29 | State Integrator::RK4(const State &x, const Input &u,const double ts) const 30 | { 31 | // 4th order Runge Kutta (RK4) implementation 32 | // 4 evaluation points of continuous dynamics 33 | const StateVector x_vec = stateToVector(x); 34 | const InputVector u_vec = inputToVector(u); 35 | // evaluating the 4 points 36 | const StateVector k1 = model_.getF(vectorToState(x_vec),u); 37 | const StateVector k2 = model_.getF(vectorToState(x_vec+ts/2.*k1),u); 38 | const StateVector k3 = model_.getF(vectorToState(x_vec+ts/2.*k2),u); 39 | const StateVector k4 = model_.getF(vectorToState(x_vec+ts*k3),u); 40 | // combining to give output 41 | const StateVector x_next = x_vec + ts*(k1/6.+k2/3.+k3/3.+k4/6.); 42 | return vectorToState(x_next); 43 | } 44 | 45 | State Integrator::EF(const State &x, const Input &u,const double ts) const 46 | { 47 | // Euler Forward integration 48 | const StateVector x_vec = stateToVector(x); 49 | const StateVector f = model_.getF(x,u); // evaluation continuous dynmaics 50 | // compute next time step 51 | const StateVector x_next = x_vec + ts*f; 52 | return vectorToState(x_next); 53 | } 54 | 55 | State Integrator::simTimeStep(const State &x, const Input &u,const double ts) const 56 | { 57 | // integrate time step 58 | State x_next = x; 59 | const int integration_steps = (int)(ts/fine_time_step_); 60 | if(ts/fine_time_step_ != integration_steps) 61 | { 62 | std::cout << "Warning" << std::endl; 63 | } 64 | for(int i = 0;i> jsonModel; 33 | // Model Parameters 34 | Cm1 = jsonModel["Cm1"]; 35 | Cm2 = jsonModel["Cm2"]; 36 | 37 | Cr0 = jsonModel["Cr0"]; 38 | Cr2 = jsonModel["Cr2"]; 39 | 40 | Br = jsonModel["Br"]; 41 | Cr = jsonModel["Cr"]; 42 | Dr = jsonModel["Dr"]; 43 | 44 | Bf = jsonModel["Bf"]; 45 | Cf = jsonModel["Cf"]; 46 | Df = jsonModel["Df"]; 47 | 48 | m = jsonModel["m"]; 49 | Iz = jsonModel["Iz"]; 50 | lf = jsonModel["lf"]; 51 | lr = jsonModel["lr"]; 52 | 53 | car_l = jsonModel["car_l"]; 54 | car_w = jsonModel["car_w"]; 55 | 56 | g = jsonModel["g"]; 57 | //Constraint Parameters 58 | r_in = jsonModel["R_in"]; 59 | r_out = jsonModel["R_out"]; 60 | 61 | max_dist_proj = jsonModel["max_dist_proj"]; 62 | 63 | e_long = jsonModel["E_long"]; 64 | e_eps = jsonModel["E_eps"]; 65 | 66 | max_alpha = jsonModel["maxAlpha"]; 67 | // initial warm start and trust region (model dependent) 68 | initial_velocity = jsonModel["initial_velocity"]; 69 | s_trust_region = jsonModel["s_trust_region"]; 70 | 71 | vx_zero = jsonModel["vx_zero"]; 72 | } 73 | 74 | CostParam::CostParam(){ 75 | std::cout << "Default initialization of cost" << std::endl; 76 | } 77 | 78 | CostParam::CostParam(std::string file){ 79 | ///////////////////////////////////////////////////// 80 | // Loading Cost Parameters ////////////////////////// 81 | ///////////////////////////////////////////////////// 82 | // std::cout << "cost" << std::endl; 83 | 84 | std::ifstream iCost(file); 85 | json jsonCost; 86 | iCost >> jsonCost; 87 | 88 | q_c = jsonCost["qC"]; 89 | q_l = jsonCost["qL"]; 90 | q_vs = jsonCost["qVs"]; 91 | 92 | q_mu = jsonCost["qMu"]; 93 | 94 | q_r = jsonCost["qR"]; 95 | 96 | q_beta = jsonCost["qBeta"]; 97 | beta_kin_cost = 1;//jsonCost["betaKin"]; 98 | 99 | r_D = jsonCost["rD"]; 100 | r_delta = jsonCost["rDelta"]; 101 | r_vs = jsonCost["rVs"]; 102 | 103 | r_dD = jsonCost["rdD"]; 104 | r_dDelta = jsonCost["rdDelta"]; 105 | r_dVs = jsonCost["rdVs"]; 106 | 107 | q_c_N_mult = jsonCost["qCNmult"]; 108 | q_r_N_mult = jsonCost["qRNmult"]; 109 | 110 | sc_quad_track = jsonCost["sc_quad_track"]; 111 | sc_quad_tire= jsonCost["sc_quad_tire"]; 112 | sc_quad_alpha = jsonCost["sc_quad_alpha"]; 113 | 114 | sc_lin_track = jsonCost["sc_lin_track"]; 115 | sc_lin_tire = jsonCost["sc_lin_tire"]; 116 | sc_lin_alpha = jsonCost["sc_lin_alpha"]; 117 | } 118 | 119 | BoundsParam::BoundsParam() { 120 | std::cout << "Default initialization of bounds" << std::endl; 121 | } 122 | 123 | BoundsParam::BoundsParam(std::string file) { 124 | 125 | ///////////////////////////////////////////////////// 126 | // Loading Cost Parameters ////////////////////////// 127 | ///////////////////////////////////////////////////// 128 | // std::cout << "bounds" << std::endl; 129 | 130 | std::ifstream iBounds(file); 131 | json jsonBounds; 132 | iBounds >> jsonBounds; 133 | 134 | lower_state_bounds.X_l = jsonBounds["Xl"]; 135 | lower_state_bounds.Y_l = jsonBounds["Yl"]; 136 | lower_state_bounds.phi_l = jsonBounds["phil"]; 137 | lower_state_bounds.vx_l = jsonBounds["vxl"]; 138 | lower_state_bounds.vy_l = jsonBounds["vyl"]; 139 | lower_state_bounds.r_l = jsonBounds["rl"]; 140 | lower_state_bounds.s_l = jsonBounds["sl"]; 141 | lower_state_bounds.D_l = jsonBounds["Dl"]; 142 | lower_state_bounds.delta_l = jsonBounds["deltal"]; 143 | lower_state_bounds.vs_l = jsonBounds["vsl"]; 144 | 145 | upper_state_bounds.X_u = jsonBounds["Xu"]; 146 | upper_state_bounds.Y_u = jsonBounds["Yu"]; 147 | upper_state_bounds.phi_u = jsonBounds["phiu"]; 148 | upper_state_bounds.vx_u = jsonBounds["vxu"]; 149 | upper_state_bounds.vy_u = jsonBounds["vyu"]; 150 | upper_state_bounds.r_u = jsonBounds["ru"]; 151 | upper_state_bounds.s_u = jsonBounds["su"]; 152 | upper_state_bounds.D_u = jsonBounds["Du"]; 153 | upper_state_bounds.delta_u = jsonBounds["deltau"]; 154 | upper_state_bounds.vs_u = jsonBounds["vsu"]; 155 | 156 | lower_input_bounds.dD_l = jsonBounds["dDl"]; 157 | lower_input_bounds.dDelta_l = jsonBounds["dDeltal"]; 158 | lower_input_bounds.dVs_l = jsonBounds["dVsl"]; 159 | 160 | upper_input_bounds.dD_u = jsonBounds["dDu"]; 161 | upper_input_bounds.dDelta_u = jsonBounds["dDeltau"]; 162 | upper_input_bounds.dVs_u = jsonBounds["dVsu"]; 163 | } 164 | 165 | NormalizationParam::NormalizationParam(){ 166 | std::cout << "Default initialization of normalization" << std::endl; 167 | } 168 | 169 | NormalizationParam::NormalizationParam(std::string file) 170 | { 171 | ///////////////////////////////////////////////////// 172 | // Loading Normalization Parameters ///////////////// 173 | ///////////////////////////////////////////////////// 174 | // std::cout << "norm" << std::endl; 175 | 176 | std::ifstream iNorm(file); 177 | json jsonNorm; 178 | iNorm >> jsonNorm; 179 | 180 | T_x.setIdentity(); 181 | T_x(si_index.X,si_index.X) = jsonNorm["X"]; 182 | T_x(si_index.Y,si_index.Y) = jsonNorm["Y"]; 183 | T_x(si_index.phi,si_index.phi) = jsonNorm["phi"]; 184 | T_x(si_index.vx,si_index.vx) = jsonNorm["vx"]; 185 | T_x(si_index.vy,si_index.vy) = jsonNorm["vy"]; 186 | T_x(si_index.r,si_index.r) = jsonNorm["r"]; 187 | T_x(si_index.s,si_index.s) = jsonNorm["s"]; 188 | T_x(si_index.D,si_index.D) = jsonNorm["D"]; 189 | T_x(si_index.delta,si_index.delta) = jsonNorm["delta"]; 190 | T_x(si_index.vs,si_index.vs) = jsonNorm["vs"]; 191 | 192 | 193 | T_x_inv.setIdentity(); 194 | for(int i = 0;i 21 | // #include 22 | #include 23 | #include 24 | #include "config.h" 25 | #include "types.h" 26 | 27 | namespace mpcc{ 28 | //used namespace 29 | using json = nlohmann::json; 30 | 31 | class Param{ 32 | public: 33 | double Cm1; 34 | double Cm2; 35 | double Cr0; 36 | double Cr2; 37 | 38 | double Br; 39 | double Cr; 40 | double Dr; 41 | 42 | double Bf; 43 | double Cf; 44 | double Df; 45 | 46 | double m; 47 | double Iz; 48 | double lf; 49 | double lr; 50 | 51 | double car_l; 52 | double car_w; 53 | 54 | double g; 55 | 56 | double r_in; 57 | double r_out; 58 | 59 | double max_dist_proj; 60 | 61 | double e_long; 62 | double e_eps; 63 | 64 | double max_alpha; 65 | 66 | double initial_velocity; 67 | double s_trust_region; 68 | 69 | double vx_zero; 70 | 71 | Param(); 72 | Param(std::string file); 73 | 74 | }; 75 | 76 | class CostParam{ 77 | public: 78 | double q_c; 79 | double q_l; 80 | double q_vs; 81 | 82 | double q_mu; 83 | 84 | double q_r; 85 | 86 | double q_beta; 87 | int beta_kin_cost; 88 | 89 | double r_D; 90 | double r_delta; 91 | double r_vs; 92 | 93 | double r_dD; 94 | double r_dDelta; 95 | double r_dVs; 96 | 97 | double q_c_N_mult; 98 | double q_r_N_mult; 99 | 100 | double sc_quad_track; 101 | double sc_quad_tire; 102 | double sc_quad_alpha; 103 | 104 | double sc_lin_track; 105 | double sc_lin_tire; 106 | double sc_lin_alpha; 107 | 108 | CostParam(); 109 | CostParam(std::string file); 110 | 111 | }; 112 | 113 | class BoundsParam{ 114 | public: 115 | struct LowerStateBounds{ 116 | double X_l; 117 | double Y_l; 118 | double phi_l; 119 | double vx_l; 120 | double vy_l; 121 | double r_l; 122 | double s_l; 123 | double D_l; 124 | double delta_l; 125 | double vs_l; 126 | }; 127 | struct UpperStateBounds{ 128 | double X_u; 129 | double Y_u; 130 | double phi_u; 131 | double vx_u; 132 | double vy_u; 133 | double r_u; 134 | double s_u; 135 | double D_u; 136 | double delta_u; 137 | double vs_u; 138 | }; 139 | struct LowerInputBounds{ 140 | double dD_l; 141 | double dDelta_l; 142 | double dVs_l; 143 | }; 144 | struct UpperInputBounds{ 145 | double dD_u; 146 | double dDelta_u; 147 | double dVs_u; 148 | }; 149 | 150 | LowerStateBounds lower_state_bounds; 151 | UpperStateBounds upper_state_bounds; 152 | 153 | LowerInputBounds lower_input_bounds; 154 | UpperInputBounds upper_input_bounds; 155 | 156 | BoundsParam(); 157 | BoundsParam(std::string file); 158 | 159 | }; 160 | 161 | class NormalizationParam{ 162 | public: 163 | TX_MPC T_x; 164 | TX_MPC T_x_inv; 165 | 166 | TU_MPC T_u; 167 | TU_MPC T_u_inv; 168 | 169 | TS_MPC T_s; 170 | TS_MPC T_s_inv; 171 | 172 | NormalizationParam(); 173 | NormalizationParam(std::string file); 174 | }; 175 | } 176 | #endif //MPCC_PARAMS_H 177 | -------------------------------------------------------------------------------- /C++/Params/track.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "track.h" 18 | namespace mpcc{ 19 | Track::Track(std::string file) 20 | { 21 | ///////////////////////////////////////////////////// 22 | // Loading Model and Constraint Parameters ////////// 23 | ///////////////////////////////////////////////////// 24 | std::ifstream iTrack(file); 25 | json jsonTrack; 26 | iTrack >> jsonTrack; 27 | // Model Parameters 28 | std::vector x = jsonTrack["X"]; 29 | X = Eigen::Map(x.data(), x.size()); 30 | std::vector y = jsonTrack["Y"]; 31 | Y = Eigen::Map(y.data(), y.size()); 32 | 33 | std::vector x_inner = jsonTrack["X_i"]; 34 | X_inner = Eigen::Map(x_inner.data(), x_inner.size()); 35 | std::vector y_inner = jsonTrack["Y_i"]; 36 | Y_inner = Eigen::Map(y_inner.data(), y_inner.size()); 37 | 38 | std::vector x_outer = jsonTrack["X_o"]; 39 | X_outer = Eigen::Map(x_outer.data(), x_outer.size()); 40 | std::vector y_outer = jsonTrack["Y_o"]; 41 | Y_outer = Eigen::Map(y_outer.data(), y_outer.size()); 42 | } 43 | 44 | TrackPos Track::getTrack() 45 | { 46 | return {Eigen::Map(X.data(), X.size()), Eigen::Map(Y.data(), Y.size()), 47 | Eigen::Map(X_inner.data(), X_inner.size()), Eigen::Map(Y_inner.data(), Y_inner.size()), 48 | Eigen::Map(X_outer.data(), X_outer.size()), Eigen::Map(Y_outer.data(), Y_outer.size())}; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /C++/Params/track.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_TRACK_H 18 | #define MPCC_TRACK_H 19 | 20 | #include "config.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace mpcc { 28 | //used namespace 29 | using json = nlohmann::json; 30 | 31 | struct TrackPos { 32 | const Eigen::VectorXd X; 33 | const Eigen::VectorXd Y; 34 | 35 | const Eigen::VectorXd X_inner; 36 | const Eigen::VectorXd Y_inner; 37 | 38 | const Eigen::VectorXd X_outer; 39 | const Eigen::VectorXd Y_outer; 40 | }; 41 | 42 | class Track { 43 | public: 44 | Track(std::string file); 45 | TrackPos getTrack(); 46 | 47 | private: 48 | Eigen::VectorXd X; 49 | Eigen::VectorXd Y; 50 | 51 | Eigen::VectorXd X_inner; 52 | Eigen::VectorXd Y_inner; 53 | 54 | Eigen::VectorXd X_outer; 55 | Eigen::VectorXd Y_outer; 56 | }; 57 | }; 58 | 59 | #endif //MPCC_TRACK_H 60 | -------------------------------------------------------------------------------- /C++/Plotting/plotting.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "plotting.h" 18 | namespace mpcc{ 19 | 20 | Plotting::Plotting(double Ts,PathToJson path) 21 | :model_(Model(Ts,path)), 22 | param_(Param(path.param_path)) 23 | { 24 | } 25 | void Plotting::plotRun(const std::list &log, const TrackPos &track_xy) const 26 | { 27 | 28 | std::vector plot_xc(track_xy.X.data(),track_xy.X.data() + track_xy.X.size()); 29 | std::vector plot_yc(track_xy.Y.data(),track_xy.Y.data() + track_xy.Y.size()); 30 | 31 | std::vector plot_xi(track_xy.X_inner.data(),track_xy.X_inner.data() + track_xy.X_inner.size()); 32 | std::vector plot_yi(track_xy.Y_inner.data(),track_xy.Y_inner.data() + track_xy.Y_inner.size()); 33 | std::vector plot_xo(track_xy.X_outer.data(),track_xy.X_outer.data() + track_xy.X_outer.size()); 34 | std::vector plot_yo(track_xy.Y_outer.data(),track_xy.Y_outer.data() + track_xy.Y_outer.size()); 35 | 36 | std::vector plot_x; 37 | std::vector plot_y; 38 | std::vector plot_phi; 39 | std::vector plot_vx; 40 | std::vector plot_vy; 41 | std::vector plot_r; 42 | std::vector plot_s; 43 | std::vector plot_d; 44 | std::vector plot_delta; 45 | std::vector plot_vs; 46 | 47 | std::vector plot_dd; 48 | std::vector plot_ddelta; 49 | std::vector plot_dvs; 50 | 51 | std::vector plot_alpha_f; 52 | std::vector plot_F_rx0; 53 | std::vector plot_F_ry0; 54 | std::vector plot_F_rx1; 55 | std::vector plot_F_ry1; 56 | 57 | for(MPCReturn log_i : log) 58 | { 59 | plot_x.push_back(log_i.mpc_horizon[0].xk.X); 60 | plot_y.push_back(log_i.mpc_horizon[0].xk.Y); 61 | plot_phi.push_back(log_i.mpc_horizon[0].xk.phi); 62 | plot_vx.push_back(log_i.mpc_horizon[0].xk.vx); 63 | plot_vy.push_back(log_i.mpc_horizon[0].xk.vy); 64 | plot_r.push_back(log_i.mpc_horizon[0].xk.r); 65 | plot_s.push_back(log_i.mpc_horizon[0].xk.s); 66 | plot_d.push_back(log_i.mpc_horizon[0].xk.D); 67 | plot_delta.push_back(log_i.mpc_horizon[0].xk.delta); 68 | plot_vs.push_back(log_i.mpc_horizon[0].xk.vs); 69 | 70 | plot_dd.push_back(log_i.mpc_horizon[0].uk.dD); 71 | plot_ddelta.push_back(log_i.mpc_horizon[0].uk.dDelta); 72 | plot_dvs.push_back(log_i.mpc_horizon[0].uk.dVs); 73 | 74 | double alpha_f = model_.getSlipAngleFront(log_i.mpc_horizon[0].xk); 75 | TireForces F_r0 = model_.getForceRear(log_i.mpc_horizon[0].xk); 76 | TireForces F_r1 = model_.getForceRear(log_i.mpc_horizon[1].xk); 77 | plot_alpha_f.push_back(alpha_f); 78 | plot_F_rx0.push_back(F_r0.F_x); 79 | plot_F_ry0.push_back(F_r0.F_y); 80 | plot_F_rx1.push_back(F_r1.F_x); 81 | plot_F_ry1.push_back(F_r1.F_y); 82 | } 83 | 84 | std::vector plot_eps_x; 85 | std::vector plot_eps_y; 86 | for(double t = 0; t<2*M_PI;t+=0.1) 87 | { 88 | plot_eps_x.push_back(cos(t)*param_.Dr*param_.e_eps); 89 | plot_eps_y.push_back(sin(t)*param_.Dr*1./param_.e_long*param_.e_eps); 90 | } 91 | 92 | plt::figure(1); 93 | plt::plot(plot_xc,plot_yc,"r--"); 94 | plt::plot(plot_xi,plot_yi,"k-"); 95 | plt::plot(plot_xo,plot_yo,"k-"); 96 | plt::plot(plot_x,plot_y,"b-"); 97 | plt::axis("equal"); 98 | plt::xlabel("X [m]"); 99 | plt::ylabel("Y [m]"); 100 | plt::figure(2); 101 | plt::subplot(3,2,1); 102 | plt::plot(plot_x); 103 | plt::ylabel("X [m]"); 104 | plt::subplot(3,2,2); 105 | plt::plot(plot_y); 106 | plt::ylabel("Y [m]"); 107 | plt::subplot(3,2,3); 108 | plt::plot(plot_phi); 109 | plt::ylabel("phi [rad]"); 110 | plt::subplot(3,2,4); 111 | plt::plot(plot_vx); 112 | plt::ylabel("v_x [m/s]"); 113 | plt::subplot(3,2,5); 114 | plt::plot(plot_vy); 115 | plt::ylabel("v_y [m/s]"); 116 | plt::subplot(3,2,6); 117 | plt::plot(plot_r); 118 | plt::ylabel("r [rad/s]"); 119 | 120 | 121 | plt::figure(3); 122 | plt::subplot(3,1,1); 123 | plt::plot(plot_d); 124 | plt::ylabel("D [-]"); 125 | plt::subplot(3,1,2); 126 | plt::plot(plot_delta); 127 | plt::ylabel("delta [rad]"); 128 | plt::subplot(3,1,3); 129 | plt::plot(plot_vs); 130 | plt::ylabel("v_s [m/s]"); 131 | 132 | plt::figure(4); 133 | plt::subplot(3,1,1); 134 | plt::plot(plot_dd); 135 | plt::ylabel("dot{D} [-]"); 136 | plt::subplot(3,1,2); 137 | plt::plot(plot_ddelta); 138 | plt::ylabel("dot{delta} [rad/s]"); 139 | plt::subplot(3,1,3); 140 | plt::plot(plot_dvs); 141 | plt::ylabel("dot{v_s} [m/s^2]"); 142 | 143 | plt::figure(5); 144 | plt::plot(plot_s); 145 | plt::ylabel("s [m]"); 146 | 147 | plt::figure(6); 148 | plt::subplot(1,2,1); 149 | plt::plot(plot_alpha_f); 150 | plt::ylabel("alpha_f [rad]"); 151 | plt::subplot(1,2,2); 152 | plt::plot(plot_F_ry0,plot_F_rx0); 153 | plt::plot(plot_F_ry1,plot_F_rx1); 154 | plt::plot(plot_eps_x,plot_eps_y); 155 | plt::axis("equal"); 156 | plt::xlabel("F_y [N]"); 157 | plt::ylabel("F_x [N]"); 158 | plt::show(); 159 | 160 | } 161 | void Plotting::plotSim(const std::list &log, const TrackPos &track_xy) const 162 | { 163 | std::vector plot_xc(track_xy.X.data(),track_xy.X.data() + track_xy.X.size()); 164 | std::vector plot_yc(track_xy.Y.data(),track_xy.Y.data() + track_xy.Y.size()); 165 | 166 | std::vector plot_xi(track_xy.X_inner.data(),track_xy.X_inner.data() + track_xy.X_inner.size()); 167 | std::vector plot_yi(track_xy.Y_inner.data(),track_xy.Y_inner.data() + track_xy.Y_inner.size()); 168 | std::vector plot_xo(track_xy.X_outer.data(),track_xy.X_outer.data() + track_xy.X_outer.size()); 169 | std::vector plot_yo(track_xy.Y_outer.data(),track_xy.Y_outer.data() + track_xy.Y_outer.size()); 170 | 171 | 172 | std::vector plot_x; 173 | std::vector plot_y; 174 | 175 | for(MPCReturn log_i : log) 176 | { 177 | plot_x.resize(0); 178 | plot_y.resize(0); 179 | for(int j=0;j corner_x; 200 | std::vector corner_y; 201 | double body_xl = std::cos(x0.phi)*param_.car_l; 202 | double body_xw = std::sin(x0.phi)*param_.car_w; 203 | double body_yl = std::sin(x0.phi)*param_.car_l; 204 | double body_yw = -std::cos(x0.phi)*param_.car_w; 205 | 206 | corner_x.push_back(x0.X + body_xl + body_xw); 207 | corner_x.push_back(x0.X + body_xl - body_xw); 208 | corner_x.push_back(x0.X - body_xl - body_xw); 209 | corner_x.push_back(x0.X - body_xl + body_xw); 210 | corner_x.push_back(x0.X + body_xl + body_xw); 211 | 212 | corner_y.push_back(x0.Y + body_yl + body_yw); 213 | corner_y.push_back(x0.Y + body_yl - body_yw); 214 | corner_y.push_back(x0.Y - body_yl - body_yw); 215 | corner_y.push_back(x0.Y - body_yl + body_yw); 216 | corner_y.push_back(x0.Y + body_yl + body_yw); 217 | 218 | plt::plot(corner_x,corner_y,"k-"); 219 | } 220 | } -------------------------------------------------------------------------------- /C++/Plotting/plotting.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_PLOTTING_H 18 | #define MPCC_PLOTTING_H 19 | 20 | #include "config.h" 21 | #include "types.h" 22 | #include "Params/params.h" 23 | #include "Params/track.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace plt = matplotlibcpp; 30 | 31 | namespace mpcc { 32 | class Plotting { 33 | public: 34 | void plotRun(const std::list &log, const TrackPos &track_xy) const; 35 | void plotSim(const std::list &log, const TrackPos &track_xy) const; 36 | 37 | Plotting(double Ts, PathToJson path); 38 | 39 | private: 40 | void plotBox(const State &x0) const; 41 | 42 | Model model_; 43 | Param param_; 44 | }; 45 | } 46 | 47 | #endif //MPCC_PLOTTING_H 48 | -------------------------------------------------------------------------------- /C++/README.md: -------------------------------------------------------------------------------- 1 | # MPCC 2 | This is a C++ implementation of the MPCC controller. The implementation is NOT the version which was used in the paper [Optimization‐based autonomous racing of 1:43 scale RC cars](https://onlinelibrary.wiley.com/doi/abs/10.1002/oca.2123) but a new version with some additional features inspired the work we did in this paper [AMZ Driverless: The Full Autonomous Racing System](https://arxiv.org/abs/1905.05150) 3 | 4 | ## Use With Full-Sized Car Model 5 | The code in the master branch does not work well with the parameters of a full-sized car. However, I added a new branch (full-size) that uses a different SQP formulation which works for model parameters of a full-sized car. The simulation in this branch is performed for a formula student style car on a full-sized race track, following a pre-computed ideal line. 6 | 7 | ## Difference to Matlab implementation 8 | 9 | ### Solver 10 | This version only supports hpipm as a solver. However, the goal is to add an [acados](https://github.com/acados/acados) qp interface, which would allow us to use the large list of solvers supported by acoados. 11 | 12 | ### Input rate cost and constraints - Dynamics 13 | Instead of lifting the state and using the difference in inputs as new inputs, this version uses the continuous time approach where the new inputs are the rate of change of the inputs, similar to [AMZ Driverless: The Full Autonomous Racing System](https://arxiv.org/abs/1905.05150). 14 | 15 | In detail we use the following dynamics, 16 | 17 | 18 | this also includes some changes in the notation, to match better the literature. Mainly the yaw rate is not `r` and the progress `s`. Thus, we have the following states and inputs, 19 | 20 | 21 | 22 | We also split up the force in x-direction into two components, the force at the wheel `F_r,x` and the friction force `F_fric`, which are defined as follows, 23 | 24 | 25 | ### Tire constraints 26 | The C++ implementation adds the tire constraints used in [AMZ Driverless: The Full Autonomous Racing System](https://arxiv.org/abs/1905.05150). More precisely, I added a slip angle constraint for the front wheel (since the 1:43 scale cars are rear wheel drive and have no brakes), and a tire friction ellipse constraint for the rear wheel. Thus, the MPC problem the following three constraints, on top of state and input bounds, 27 | 28 | 29 | Note that if the car is all wheel drive or has brakes at the front wheel, also a tire ellipse constraint should be used for the front tire. 30 | 31 | ### Beta Cost 32 | We added an additional regularization cost, which penalizes high sideslip angles. This second regularization cost augments the small cost on the yaw rate. These regularization costs force the car to behave more reasonably and help the NLP to converge better. 33 | 34 | 35 | ### Obstacle Avoidance 36 | There is no obstacle avoidance available yet in the C++ version 37 | 38 | ### Options 39 | Currently, only one track and car model is implemented. However, adapting the parameters only requires changing the json files in the Params folder. 40 | 41 | ## Installation 42 | 43 | To install all the dependencies run 44 | ``` 45 | ./install.sh 46 | ``` 47 | this clones `blasfeo`, `hpipm`, `matplotlip-cpp`, `nlohmann/json`, and `eigen`, from their git repo, and safes them in a folder External. Additionally, it installs `blasfeo` and `hpipm` in the same External folder, thus no admin rights are necessary. 48 | 49 | Note that `matplotlib-cpp` does also require `Python-2.7` and `matplotlib`, for more details see (https://github.com/lava/matplotlib-cpp). 50 | 51 | Once all dependencies are installed `cmake` can be used to build the project 52 | ``` 53 | cmake CMakeLists.txt 54 | make 55 | ``` 56 | To run the code simply execute the `MPCC` 57 | ``` 58 | ./MPCC 59 | ``` 60 | 61 | ### TODO 62 | 63 | There are still several things that should be added to the project. Most of them are marked with TODO in the code. 64 | -------------------------------------------------------------------------------- /C++/Spline/arc_length_spline.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_ARC_LENGTH_SPLINE_H 18 | #define MPCC_ARC_LENGTH_SPLINE_H 19 | 20 | #include "cubic_spline.h" 21 | #include "types.h" 22 | #include "Params/params.h" 23 | #include 24 | 25 | namespace mpcc{ 26 | //return value 27 | struct RawPath{ 28 | Eigen::VectorXd X; 29 | Eigen::VectorXd Y; 30 | }; 31 | // data struct 32 | struct PathData{ 33 | Eigen::VectorXd X; 34 | Eigen::VectorXd Y; 35 | Eigen::VectorXd s; 36 | int n_points; 37 | }; 38 | 39 | class ArcLengthSpline { 40 | public: 41 | // X and Y spline used for final spline fit 42 | void gen2DSpline(const Eigen::VectorXd &X,const Eigen::VectorXd &Y); 43 | Eigen::Vector2d getPostion(double) const; 44 | Eigen::Vector2d getDerivative(double) const; 45 | Eigen::Vector2d getSecondDerivative(double) const; 46 | double getLength() const; 47 | double porjectOnSpline(const State &x) const; 48 | 49 | ArcLengthSpline(); 50 | ArcLengthSpline(const PathToJson &path); 51 | // void setParam(const Param ¶m) { param_ = param; }; 52 | 53 | private: 54 | void setData(const Eigen::VectorXd &X_in,const Eigen::VectorXd &Y_in); 55 | void setRegularData(const Eigen::VectorXd &X_in,const Eigen::VectorXd &Y_in,const Eigen::VectorXd &s_in); 56 | Eigen::VectorXd compArcLength(const Eigen::VectorXd &X_in,const Eigen::VectorXd &Y_in) const; 57 | PathData resamplePath(const CubicSpline &initial_spline_x,const CubicSpline &initial_spline_y,double total_arc_length) const; 58 | RawPath outlierRemoval(const Eigen::VectorXd &X_original,const Eigen::VectorXd &Y_original) const; 59 | void fitSpline(const Eigen::VectorXd &X,const Eigen::VectorXd &Y); 60 | double unwrapInput(double x) const; 61 | 62 | PathData path_data_; // initial data and data used for successive fitting 63 | // PathData pathDataFinal; // final data 64 | CubicSpline spline_x_; 65 | CubicSpline spline_y_; 66 | Param param_; 67 | }; 68 | } 69 | #endif //MPCC_ARC_LENGTH_SPLINE_H -------------------------------------------------------------------------------- /C++/Spline/cubic_spline.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "cubic_spline.h" 18 | 19 | namespace mpcc{ 20 | CubicSpline::CubicSpline() 21 | : data_set_(false) 22 | { 23 | } 24 | 25 | void CubicSpline::setRegularData(const Eigen::VectorXd &x_in,const Eigen::VectorXd &y_in,const double delta_x) { 26 | //if x and y have same length, stare given data in spline data struct 27 | if(x_in.size() == y_in.size()) 28 | { 29 | spline_data_.x_data = x_in; 30 | spline_data_.y_data = y_in; 31 | spline_data_.n_points = x_in.size(); 32 | spline_data_.is_regular = true; 33 | spline_data_.delta_x = delta_x; 34 | 35 | data_set_ = true; 36 | } 37 | else 38 | { 39 | std::cout << "input data does not have the same length" << std::endl; 40 | } 41 | } 42 | 43 | void CubicSpline::setData(const Eigen::VectorXd &x_in,const Eigen::VectorXd &y_in) 44 | { 45 | //if x and y have same length, stare given data in spline data struct 46 | if(x_in.size() == y_in.size()) 47 | { 48 | spline_data_.x_data = x_in; 49 | spline_data_.y_data = y_in; 50 | spline_data_.n_points = x_in.size(); 51 | spline_data_.is_regular = false; 52 | spline_data_.delta_x = 0; 53 | for(int i = 0;i=0;i--) 117 | { 118 | spline_params_.c(i) = z(i) - mu(i)*spline_params_.c(i+1); 119 | spline_params_.b(i) = (spline_params_.a(i+1) - spline_params_.a(i))/h(i) - (h(i)*(spline_params_.c(i+1) + 2.0*spline_params_.c(i)))/3.0; 120 | spline_params_.d(i) = (spline_params_.c(i+1) - spline_params_.c(i))/(3.0*h(i)); 121 | } 122 | 123 | return true; 124 | } 125 | 126 | int CubicSpline::getIndex(const double x) const 127 | { 128 | // given a x value find the closest point in the spline to evalute it 129 | // special case if x is regularly space 130 | // assumes wrapped data! 131 | 132 | // if special case of end points 133 | if(x == spline_data_.x_data(spline_data_.n_points-1)) 134 | { 135 | return spline_data_.n_points-1; 136 | } 137 | // if regular index can be found by rounding 138 | if(spline_data_.is_regular == 1) 139 | { 140 | return int(floor(x/spline_data_.delta_x)); 141 | } 142 | // if irregular index need to be searched 143 | else 144 | { 145 | auto min_it = spline_data_.x_map.upper_bound(x); 146 | if(min_it==spline_data_.x_map.end()) 147 | return -1; 148 | else{ 149 | return min_it->second-1; 150 | } 151 | 152 | } 153 | } 154 | double CubicSpline::unwrapInput(double x) const 155 | { 156 | double x_max = spline_data_.x_data(spline_data_.n_points-1); 157 | return x - x_max*std::floor(x/x_max); 158 | } 159 | 160 | void CubicSpline::genSpline(const Eigen::VectorXd &x_in,const Eigen::VectorXd &y_in,const bool is_regular) 161 | { 162 | // given x and y data generate spline 163 | // special case for regular or irregular spaced data points in x 164 | // if regular the spacing in x is given by deltaX 165 | 166 | // store data in data struct 167 | if(is_regular) 168 | { 169 | double delta_x = x_in(1) - x_in(0); 170 | setRegularData(x_in,y_in,delta_x); 171 | } 172 | else 173 | { 174 | setData(x_in,y_in); 175 | } 176 | // given data compute spline parameters 177 | 178 | bool succes = compSplineParams(); 179 | 180 | // TODO if succes is flase call exeption 181 | } 182 | 183 | double CubicSpline::getPoint(double x) const 184 | { 185 | // evaluation of spline a x 186 | int index; 187 | double x_i; 188 | double dx,dx2,dx3; 189 | // wrape input to data -> x data needs start at 0 and contain end point!!! 190 | x = unwrapInput(x); 191 | // compute index 192 | index = getIndex(x); 193 | // access previous points 194 | x_i = spline_data_.x_data(index); 195 | // compute diff to point and it's powers 196 | dx = x-x_i; 197 | dx2 = dx*dx; 198 | dx3 = dx*dx2; 199 | // return spline value y = a + b dx + c dx^2 + d dx^3 200 | return spline_params_.a(index) + spline_params_.b(index)*dx + spline_params_.c(index)*dx2 + spline_params_.d(index)*dx3; 201 | } 202 | 203 | double CubicSpline::getDerivative(double x) const 204 | { 205 | // evaluate first derivative of spline 206 | // identical to noram spline with y' = b + 2 c dx + 3 d dx^2 207 | int index; 208 | double x_i; 209 | double dx,dx2; 210 | 211 | x = unwrapInput(x); 212 | index = getIndex(x); 213 | x_i = spline_data_.x_data(index); 214 | 215 | dx = x-x_i; 216 | dx2 = dx*dx; 217 | // y' = b + 2 c dx + 3 d dx^2 218 | return spline_params_.b(index) + 2.0*spline_params_.c(index)*dx + 3.0*spline_params_.d(index)*dx2; 219 | } 220 | 221 | double CubicSpline::getSecondDerivative(double x) const 222 | { 223 | // evaluate second derivative of spline 224 | // identical to noram spline with y' = 2 c + 6 d dx 225 | int index; 226 | double x_i; 227 | double dx; 228 | 229 | x = unwrapInput(x); 230 | index = getIndex(x); 231 | x_i = spline_data_.x_data(index); 232 | 233 | dx = x-x_i; 234 | // y' = 2 c + 6 d dx 235 | return 2.0*spline_params_.c(index) + 6.0*spline_params_.d(index)*dx; 236 | } 237 | } -------------------------------------------------------------------------------- /C++/Spline/cubic_spline.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_CUBIC_SPLINE_H 18 | #define MPCC_CUBIC_SPLINE_H 19 | 20 | #include "config.h" 21 | #include 22 | 23 | namespace mpcc{ 24 | // spline parameter struct y = a + b dx + c dx^2 + d dx^3 25 | struct SplineParams{ 26 | Eigen::VectorXd a; 27 | Eigen::VectorXd b; 28 | Eigen::VectorXd c; 29 | Eigen::VectorXd d; 30 | }; 31 | // input data for spline 32 | struct SplineData{ 33 | Eigen::VectorXd x_data; //x data 34 | Eigen::VectorXd y_data; //y data 35 | int n_points; //number of points 36 | bool is_regular; //regular (1) or irregular (0) spaced points in x direction 37 | double delta_x; //spacing of regular space points 38 | std::map x_map; 39 | }; 40 | 41 | class CubicSpline { 42 | public: 43 | void genSpline(const Eigen::VectorXd &x_in,const Eigen::VectorXd &y_in,bool is_regular); 44 | double getPoint(double x) const; 45 | double getDerivative(double x) const; 46 | double getSecondDerivative(double x) const; 47 | 48 | CubicSpline(); 49 | private: 50 | bool data_set_; 51 | 52 | void setRegularData(const Eigen::VectorXd &x_in,const Eigen::VectorXd &y_in,double delta_x); 53 | void setData(const Eigen::VectorXd &x_in,const Eigen::VectorXd &y_in); 54 | bool compSplineParams(); 55 | int getIndex(double x) const; 56 | double unwrapInput(double x) const; 57 | 58 | SplineParams spline_params_; 59 | SplineData spline_data_; 60 | }; 61 | } 62 | #endif //MPCC_CUBIC_SPLINE_H -------------------------------------------------------------------------------- /C++/Tests/constratins_test.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "constratins_test.h" 18 | namespace mpcc{ 19 | void genRoundTrack(ArcLengthSpline &track){ 20 | int NT = 100; //number of track points 21 | double TrackRadius = 1.0; // track radius 22 | 23 | Eigen::VectorXd X; 24 | Eigen::VectorXd Y; 25 | Eigen::VectorXd phiR; 26 | 27 | X.setZero(NT); 28 | Y.setZero(NT); 29 | // // randomly distribute training points around circle 30 | // // generate random agles between [0,2pi] 31 | // phiR.setRandom(NT); 32 | // phiR = (phiR/2.0+0.5*Eigen::VectorXd::Ones(NT))*2*M_PI; 33 | // // sort training points 34 | // std::sort(phiR.data(), phiR.data()+phiR.size()); 35 | // // fix start and end point 36 | // phiR(0) = 0; 37 | // phiR(NT-1) = 2*M_PI; 38 | // generate equally spaced points around circle 39 | phiR.setLinSpaced(NT,0,2*M_PI); 40 | // compute circle points 41 | for(int i=0;i Xv(NT),Yv(NT),Xsv(NT*10),Ysv(NT*10); 52 | // for(int i=0;i right of circle 44 | CostMatrix cost_mat0 = cost.getCost(track,vectorToState(xk0_vec),1); 45 | 46 | if(std::fabs((0.5*xk0_vec.transpose()*cost_mat0.Q*xk0_vec + 0.5*uk0_vec.transpose()*cost_mat0.R*uk0_vec + cost_mat0.q.transpose()*xk0_vec + cost_mat0.r.transpose()*uk0_vec + 0.5*xk0_vec.transpose()*cost_mat0.Q*xk0_vec).value()) >= 0.1){ 47 | return 2; 48 | } 49 | // s = R*pi/2 -> top of circle 50 | CostMatrix cost_mat1 = cost.getCost(track,vectorToState(xk1_vec),1); 51 | 52 | 53 | if(std::fabs((0.5*xk1_vec.transpose()*cost_mat1.Q*xk1_vec + 0.5*uk1_vec.transpose()*cost_mat1.R*uk1_vec + cost_mat1.q.transpose()*xk1_vec + cost_mat1.r.transpose()*uk1_vec + 0.5*xk1_vec.transpose()*cost_mat1.Q*xk1_vec).value()) >= 0.1) { 54 | return 3; 55 | } 56 | // s = R*pi -> left of circle 57 | CostMatrix cost_mat2 = cost.getCost(track,vectorToState(xk2_vec),1); 58 | 59 | // std::cout << std::fabs((0.5*xk2_vec.transpose()*Q*xk2_vec + 0.5*uk2_vec.transpose()*R*uk2_vec + q.transpose()*xk2_vec + r.transpose()*uk2_vec + 0.5*xk2_vec.transpose()*Q*xk2_vec).value()) << std::endl; 60 | if(std::fabs((0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec + 0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec).value()) >= 0.1){ 61 | return 4; 62 | } 63 | 64 | 65 | // Test offset from trajectory 66 | // given a point with error 0 does moving in X-Y change the error? 67 | if( (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec).value() >= 68 | (0.5*xk3_vec.transpose()*cost_mat2.Q*xk3_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk3_vec + cost_mat2.r.transpose()*uk2_vec).value()){ 69 | return 5; 70 | } 71 | xk3_vec(0) = 0.8; 72 | if( (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec).value() >= 73 | (0.5*xk3_vec.transpose()*cost_mat2.Q*xk3_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk3_vec + cost_mat2.r.transpose()*uk2_vec).value()){ 74 | return 6; 75 | } 76 | xk3_vec(0) = 1; 77 | xk3_vec(1) = 0.1; 78 | if( (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec).value() >= 79 | (0.5*xk3_vec.transpose()*cost_mat2.Q*xk3_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk3_vec + cost_mat2.r.transpose()*uk2_vec).value()){ 80 | return 7; 81 | } 82 | xk3_vec(1) = -0.1; 83 | if( (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec).value() >= 84 | (0.5*xk3_vec.transpose()*cost_mat2.Q*xk3_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk3_vec + cost_mat2.r.transpose()*uk2_vec).value()){ 85 | return 8; 86 | } 87 | 88 | // Test input increase 89 | // do increased inputs increase the cost? 90 | if( (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec).value() >= 91 | (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk3_vec.transpose()*cost_mat2.R*uk3_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk3_vec).value()){ 92 | return 9; 93 | } 94 | 95 | // Test Beta Cost 96 | // does an increased slip angle increase the cost? 97 | if( (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec).value() >= 98 | (0.5*xk4_vec.transpose()*cost_mat2.Q*xk4_vec + 0.5*uk4_vec.transpose()*cost_mat2.R*uk4_vec + cost_mat2.q.transpose()*xk4_vec + cost_mat2.r.transpose()*uk4_vec).value()){ 99 | return 10; 100 | } 101 | xk4_vec(4) = -0.5; 102 | if( (0.5*xk2_vec.transpose()*cost_mat2.Q*xk2_vec + 0.5*uk2_vec.transpose()*cost_mat2.R*uk2_vec + cost_mat2.q.transpose()*xk2_vec + cost_mat2.r.transpose()*uk2_vec).value() >= 103 | (0.5*xk4_vec.transpose()*cost_mat2.Q*xk4_vec + 0.5*uk4_vec.transpose()*cost_mat2.R*uk4_vec + cost_mat2.q.transpose()*xk4_vec + cost_mat2.r.transpose()*uk4_vec).value()){ 104 | return 11; 105 | } 106 | 107 | return 1; 108 | } 109 | } -------------------------------------------------------------------------------- /C++/Tests/cost_test.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_COST_TEST_H 18 | #define MPCC_COST_TEST_H 19 | 20 | #include "Cost/cost.h" 21 | #include "constratins_test.h" 22 | namespace mpcc{ 23 | int testCost(const PathToJson &path); 24 | } 25 | 26 | #endif //MPCC_COST_TEST_H 27 | -------------------------------------------------------------------------------- /C++/Tests/model_integrator_test.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "model_integrator_test.h" 18 | namespace mpcc{ 19 | int testIntegrator(const PathToJson &path){ 20 | double Ts = 0.02; 21 | const Integrator integrator = Integrator(Ts,path); 22 | 23 | // test integrator by comparing Euler forward to RK4 24 | // 3 differnet test points, hand picked, going straight and random 25 | 26 | //Integrator integrator; 27 | ////////////////////////////////////////////////////////////////////////////////////////////////////////// 28 | // Hand picked x and u 29 | StateVector error1; 30 | State xk1 = {0,0,0,2,0.1,-0.3,0.1,0.2,-0.1,2}; 31 | Input uk1 = {0.2,-0.1,0}; 32 | 33 | error1 = stateToVector(integrator.EF(xk1,uk1,Ts)) - stateToVector(integrator.RK4(xk1,uk1,Ts)); 34 | std::cout << "hand picked point RK4 - EF error = " << error1.norm() << std::endl; 35 | ////////////////////////////////////////////////////////////////////////////////////////////////////////// 36 | // x and u corresponding to going straight with 1m/s 37 | StateVector error2; 38 | State xk2 = {0,0,0,1,0,0,0,0,0,0}; 39 | Input uk2 ={0,0,0}; 40 | 41 | error2 = stateToVector(integrator.EF(xk2,uk2,Ts)) - stateToVector(integrator.RK4(xk2,uk2,Ts)); 42 | std::cout << "straight RK4 - EF error = " << error2.norm() << std::endl; 43 | ////////////////////////////////////////////////////////////////////////////////////////////////////////// 44 | // random x and u 45 | StateVector error3; 46 | StateVector xkr = StateVector::Random(); 47 | InputVector ukr = InputVector::Random(); 48 | State xk3 = vectorToState(xkr); 49 | Input uk3 = vectorToInput(ukr); 50 | 51 | error3 = stateToVector(integrator.EF(xk3,uk3,Ts)) - stateToVector(integrator.RK4(xk3,uk3,Ts)); 52 | std::cout << "random RK4 - EF error = " << error3.norm() << std::endl; 53 | ////////////////////////////////////////////////////////////////////////////////////////////////////////// 54 | // test how good fit is 55 | if(error1.norm()/10.0 <= 0.3 && error2.norm()/10.0 <= 0.3 && error3.norm()/10.0 <= 0.3 ){ 56 | return 1; 57 | } 58 | else{ 59 | return 0; 60 | } 61 | 62 | } 63 | 64 | int testLinModel(const PathToJson &path){ 65 | // test Liniear model by comparing it to RK4 66 | // 3 differnet test cases, hand picked, going straight and test how good linear model generalizes 67 | double Ts = 0.02; 68 | const Integrator integrator = Integrator(0.02,path); 69 | const Model model = Model(0.02,path); 70 | 71 | ////////////////////////////////////////////////////////////////////////////////////////////////////////// 72 | // Hand picked x and u 73 | StateVector error1; 74 | State xk1 = {0,0,0,2,0.1,-0.3,0.1,0.2,-0.1,2}; 75 | Input uk1 = {0.2,-0.1,0}; 76 | StateVector xk1_vec = stateToVector(xk1); 77 | InputVector uk1_vec = inputToVector(uk1); 78 | 79 | const LinModelMatrix lin_model_d1 = model.getLinModel(xk1,uk1); 80 | 81 | error1 = (lin_model_d1.A*xk1_vec + lin_model_d1.B*uk1_vec + lin_model_d1.g) - stateToVector(integrator.RK4(xk1,uk1,Ts)); 82 | std::cout << "hand picked point RK4 - lin error = " << error1.norm() << std::endl; 83 | ////////////////////////////////////////////////////////////////////////////////////////////////////////// 84 | // x and u corresponding to going straight with 1m/s 85 | StateVector error2; 86 | State xk2 = {0,0,0,1,0,0,0,0,0,0}; 87 | Input uk2 = {0,0,0}; 88 | StateVector xk2_vec = stateToVector(xk2); 89 | InputVector uk2_vec = inputToVector(uk2); 90 | 91 | const LinModelMatrix lin_model_d2 = model.getLinModel(xk2,uk2); 92 | 93 | error2 = (lin_model_d2.A*xk2_vec + lin_model_d2.B*uk2_vec + lin_model_d2.g) - stateToVector(integrator.RK4(xk2,uk2,Ts)); 94 | std::cout << "straight RK4 - lin error = " << error2.norm() << std::endl; 95 | ////////////////////////////////////////////////////////////////////////////////////////////////////////// 96 | // generalization test 97 | // perturbe xk1 slightly, however us the model linearized around xk1 and uk1 98 | StateVector error3; 99 | State xk3; 100 | // xk3 is slightly perturbed version of xk1 101 | xk3 = xk1; 102 | xk3.vx += 0.2; //vx 103 | xk3.vy += 0.05; //vy 104 | xk3.r += 0.8; //r 105 | 106 | xk3.D += 0.2; //D 107 | xk3.delta -= 0.05; //delta 108 | 109 | Input uk3; 110 | uk3 = uk1; 111 | //still linearize around xk1 and uk1 112 | StateVector xk3_vec = stateToVector(xk3); 113 | InputVector uk3_vec = inputToVector(uk3); 114 | error3 = (lin_model_d1.A*xk3_vec + lin_model_d1.B*uk3_vec + lin_model_d1.g) - stateToVector(integrator.RK4(xk3,uk3,Ts)); 115 | // std::cout << error3 << std::endl; 116 | std::cout << "generalization test RK4 - lin error = " << error3.norm() << std::endl; 117 | if(error1.norm()/10.0 <= 0.03 && error2.norm()/10.0 <= 0.03){ 118 | return 1; 119 | } 120 | else{ 121 | return 0; 122 | } 123 | } 124 | } -------------------------------------------------------------------------------- /C++/Tests/model_integrator_test.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_MODEL_INTEGRATOR_TEST_H 18 | #define MPCC_MODEL_INTEGRATOR_TEST_H 19 | 20 | #include "Model/model.h" 21 | #include "Model/integrator.h" 22 | namespace mpcc{ 23 | int testIntegrator(const PathToJson &path); 24 | int testLinModel(const PathToJson &path); 25 | } 26 | #endif //MPCC_MODEL_INTEGRATOR_TEST_H 27 | -------------------------------------------------------------------------------- /C++/Tests/spline_test.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "spline_test.h" 18 | namespace mpcc{ 19 | int testSpline() { 20 | 21 | //Fit spline to a cos and test if the spine interpolation is accurate 22 | //Also test if first and second derivatives are accurate 23 | 24 | CubicSpline spline; 25 | 26 | Eigen::VectorXd x; 27 | Eigen::VectorXd y; 28 | 29 | int NT = 50; //number of "training" points 30 | int NV = 100; //number of validation points 31 | x.setLinSpaced(NT,0,2*M_PI); 32 | y.setZero(NT,1); 33 | // std::cout << x.size() << std::endl; 34 | //set y ot cos(x) 35 | for(int i=0;i initialize to zero 42 | Eigen::VectorXd xt; 43 | Eigen::VectorXd yt,ytt; 44 | Eigen::VectorXd dyt,dytt; 45 | Eigen::VectorXd ddyt,ddytt; 46 | 47 | xt.setLinSpaced(NV,0,2*M_PI); //test x points 48 | yt.setZero(NV,1); 49 | ytt.setZero(NV,1); 50 | 51 | dyt.setZero(NV,1); 52 | dytt.setZero(NV,1); 53 | 54 | ddyt.setZero(NV,1); 55 | ddytt.setZero(NV,1); 56 | 57 | //compute spline outputs and true outputs given the y = cos(x) 58 | for(int i=0;i 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace mpcc{ 28 | 29 | // #define MAX(a,b) (a < b) ? b : a 30 | 31 | #define NX 10 32 | #define NU 3 33 | 34 | #define NB 13 //max number of bounds 35 | #define NPC 3 //number of polytopic constraints 36 | #define NS 3 37 | 38 | static constexpr int N = 60; 39 | static constexpr double INF = 1E5; 40 | static constexpr int N_SPLINE = 5000; 41 | 42 | 43 | struct StateInputIndex{ 44 | int X = 0; 45 | int Y = 1; 46 | int phi = 2; 47 | int vx = 3; 48 | int vy = 4; 49 | int r = 5; 50 | int s = 6; 51 | int D = 7; 52 | int delta = 8; 53 | int vs = 9; 54 | 55 | int dD = 0; 56 | int dDelta = 1; 57 | int dVs = 2; 58 | 59 | int con_track = 0; 60 | int con_tire = 1; 61 | int con_alpha = 2; 62 | }; 63 | 64 | static const StateInputIndex si_index; 65 | 66 | } 67 | #endif //MPCC_CONFIG_H 68 | -------------------------------------------------------------------------------- /C++/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 eigne 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 57 | -------------------------------------------------------------------------------- /C++/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "Tests/spline_test.h" 18 | #include "Tests/model_integrator_test.h" 19 | #include "Tests/constratins_test.h" 20 | #include "Tests/cost_test.h" 21 | 22 | #include "MPC/mpc.h" 23 | #include "Model/integrator.h" 24 | #include "Params/track.h" 25 | #include "Plotting/plotting.h" 26 | 27 | #include 28 | using json = nlohmann::json; 29 | 30 | int main() { 31 | 32 | using namespace mpcc; 33 | std::ifstream iConfig("Params/config.json"); 34 | json jsonConfig; 35 | iConfig >> jsonConfig; 36 | 37 | PathToJson json_paths {jsonConfig["model_path"], 38 | jsonConfig["cost_path"], 39 | jsonConfig["bounds_path"], 40 | jsonConfig["track_path"], 41 | jsonConfig["normalization_path"]}; 42 | 43 | // std::cout << testSpline() << std::endl; 44 | // std::cout << testArcLengthSpline(json_paths) << std::endl; 45 | 46 | // std::cout << testIntegrator(json_paths) << std::endl; 47 | // std::cout << testLinModel(json_paths) << std::endl; 48 | 49 | // std::cout << testAlphaConstraint(json_paths) << std::endl; 50 | // std::cout << testTireForceConstraint(json_paths) << std::endl; 51 | // std::cout << testTrackConstraint(json_paths) << std::endl; 52 | 53 | // std::cout << testCost(json_paths) << std::endl; 54 | 55 | Integrator integrator = Integrator(jsonConfig["Ts"],json_paths); 56 | Plotting plotter = Plotting(jsonConfig["Ts"],json_paths); 57 | 58 | Track track = Track(json_paths.track_path); 59 | TrackPos track_xy = track.getTrack(); 60 | 61 | std::list log; 62 | MPC mpc(jsonConfig["n_sqp"],jsonConfig["n_reset"],jsonConfig["sqp_mixing"],jsonConfig["Ts"],json_paths); 63 | mpc.setTrack(track_xy.X,track_xy.Y); 64 | const double phi_0 = std::atan2(track_xy.Y(1) - track_xy.Y(0),track_xy.X(1) - track_xy.X(0)); 65 | State x0 = {track_xy.X(0),track_xy.Y(0),phi_0,jsonConfig["v0"],0,0,0,0.5,0,jsonConfig["v0"]}; 66 | for(int i=0;i max_time) 81 | max_time = log_i.time_total; 82 | } 83 | std::cout << "mean nmpc time " << mean_time/double(jsonConfig["n_sim"]) << std::endl; 84 | std::cout << "max nmpc time " << max_time << std::endl; 85 | return 0; 86 | } 87 | 88 | 89 | -------------------------------------------------------------------------------- /C++/types.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #include "types.h" 18 | namespace mpcc{ 19 | 20 | StateVector stateToVector(const State &x) 21 | { 22 | StateVector xk; 23 | xk(0) = x.X; 24 | xk(1) = x.Y; 25 | xk(2) = x.phi; 26 | xk(3) = x.vx; 27 | xk(4) = x.vy; 28 | xk(5) = x.r; 29 | xk(6) = x.s; 30 | xk(7) = x.D; 31 | xk(8) = x.delta; 32 | xk(9) = x.vs; 33 | return xk; 34 | } 35 | 36 | InputVector inputToVector(const Input &u) 37 | { 38 | InputVector uk = {u.dD,u.dDelta,u.dVs}; 39 | return uk; 40 | } 41 | 42 | State vectorToState(const StateVector &xk) 43 | { 44 | State x; 45 | x.X = xk(0); 46 | x.Y = xk(1); 47 | x.phi = xk(2); 48 | x.vx = xk(3); 49 | x.vy = xk(4); 50 | x.r = xk(5); 51 | x.s = xk(6); 52 | x.D = xk(7); 53 | x.delta = xk(8); 54 | x.vs = xk(9); 55 | 56 | return x; 57 | } 58 | 59 | Input vectorToInput(const InputVector &uk) 60 | { 61 | Input u; 62 | u.dD = uk(0); 63 | u.dDelta = uk(1); 64 | u.dVs = uk(2); 65 | 66 | return u; 67 | } 68 | 69 | State arrayToState(double *xk) 70 | { 71 | State x; 72 | x.X = xk[0]; 73 | x.Y = xk[1]; 74 | x.phi = xk[2]; 75 | x.vx = xk[3]; 76 | x.vy = xk[4]; 77 | x.r = xk[5]; 78 | x.s = xk[6]; 79 | x.D = xk[7]; 80 | x.delta = xk[8]; 81 | x.vs = xk[9]; 82 | 83 | return x; 84 | } 85 | 86 | Input arrayToInput(double *uk) 87 | { 88 | Input u; 89 | u.dD = uk[0]; 90 | u.dDelta = uk[1]; 91 | u.dVs = uk[2]; 92 | 93 | return u; 94 | } 95 | 96 | } -------------------------------------------------------------------------------- /C++/types.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Alexander Liniger 2 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | /////////////////////////////////////////////////////////////////////////// 15 | /////////////////////////////////////////////////////////////////////////// 16 | 17 | #ifndef MPCC_TYPES_H 18 | #define MPCC_TYPES_H 19 | 20 | #include "config.h" 21 | namespace mpcc{ 22 | struct State{ 23 | double X; 24 | double Y; 25 | double phi; 26 | double vx; 27 | double vy; 28 | double r; 29 | double s; 30 | double D; 31 | double delta; 32 | double vs; 33 | 34 | void setZero() 35 | { 36 | X = 0.0; 37 | Y = 0.0; 38 | phi = 0.0; 39 | vx = 0.0; 40 | vy = 0.0; 41 | r = 0.0; 42 | s = 0.0; 43 | D = 0.0; 44 | delta = 0.0; 45 | vs = 0.0; 46 | } 47 | 48 | void unwrap(double track_length) 49 | { 50 | if (phi > M_PI) 51 | phi -= 2.0 * M_PI; 52 | if (phi < -M_PI) 53 | phi += 2.0 * M_PI; 54 | 55 | if (s > track_length) 56 | s -= track_length; 57 | if (s < 0) 58 | s += track_length; 59 | } 60 | 61 | void vxNonZero(double vx_zero) 62 | { 63 | if(vx < vx_zero){ 64 | vx = vx_zero; 65 | vy = 0.0; 66 | r = 0.0; 67 | delta = 0.0; 68 | } 69 | } 70 | }; 71 | 72 | struct Input{ 73 | double dD; 74 | double dDelta; 75 | double dVs; 76 | 77 | void setZero() 78 | { 79 | dD = 0.0; 80 | dDelta = 0.0; 81 | dVs = 0.0; 82 | } 83 | }; 84 | 85 | struct PathToJson{ 86 | const std::string param_path; 87 | const std::string cost_path; 88 | const std::string bounds_path; 89 | const std::string track_path; 90 | const std::string normalization_path; 91 | }; 92 | 93 | typedef Eigen::Matrix StateVector; 94 | typedef Eigen::Matrix InputVector; 95 | 96 | typedef Eigen::Matrix A_MPC; 97 | typedef Eigen::Matrix B_MPC; 98 | typedef Eigen::Matrix g_MPC; 99 | 100 | typedef Eigen::Matrix Q_MPC; 101 | typedef Eigen::Matrix R_MPC; 102 | typedef Eigen::Matrix S_MPC; 103 | 104 | typedef Eigen::Matrix q_MPC; 105 | typedef Eigen::Matrix r_MPC; 106 | 107 | typedef Eigen::Matrix C_MPC; 108 | typedef Eigen::Matrix C_i_MPC; 109 | typedef Eigen::Matrix D_MPC; 110 | typedef Eigen::Matrix d_MPC; 111 | 112 | typedef Eigen::Matrix Z_MPC; 113 | typedef Eigen::Matrix z_MPC; 114 | 115 | typedef Eigen::Matrix TX_MPC; 116 | typedef Eigen::Matrix TU_MPC; 117 | typedef Eigen::Matrix TS_MPC; 118 | 119 | typedef Eigen::Matrix Bounds_x; 120 | typedef Eigen::Matrix Bounds_u; 121 | typedef Eigen::Matrix Bounds_s; 122 | 123 | StateVector stateToVector(const State &x); 124 | InputVector inputToVector(const Input &u); 125 | 126 | State vectorToState(const StateVector &xk); 127 | Input vectorToInput(const InputVector &uk); 128 | 129 | State arrayToState(double *xk); 130 | Input arrayToInput(double *uk); 131 | } 132 | #endif //MPCC_TYPES_H 133 | -------------------------------------------------------------------------------- /Images/MPC_sim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/MPC_sim.gif -------------------------------------------------------------------------------- /Images/Model.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/Model.jpg -------------------------------------------------------------------------------- /Images/NMPCC_problem.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/NMPCC_problem.jpg -------------------------------------------------------------------------------- /Images/TireModel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/TireModel.jpg -------------------------------------------------------------------------------- /Images/constraints_cpp.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/constraints_cpp.jpg -------------------------------------------------------------------------------- /Images/cost_cpp.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/cost_cpp.jpg -------------------------------------------------------------------------------- /Images/forces_cpp.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/forces_cpp.jpg -------------------------------------------------------------------------------- /Images/model_cpp.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/model_cpp.jpg -------------------------------------------------------------------------------- /Images/state-input.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/state-input.jpg -------------------------------------------------------------------------------- /Images/state_input_cpp.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Images/state_input_cpp.jpg -------------------------------------------------------------------------------- /Matlab/CVXInterface.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ X,U,dU,info ] = CVXInterface(stage,MPC_vars,ModelParams) 17 | nx = ModelParams.nx; 18 | nu = ModelParams.nu; 19 | tic 20 | 21 | cvx_begin 22 | variable x((nx+nu)*(MPC_vars.N+1)); 23 | variable u(nu*MPC_vars.N); 24 | 25 | objective = 0; 26 | 27 | for i = 1:MPC_vars.N 28 | objective = objective + 0.5*(quad_form(x((i-1)*(nx+nu)+[1:nx+nu]),stage(i).Qk) + quad_form(u((i-1)*nu+[1:nu]),stage(i).Rk)) + stage(i).fk'*x((i-1)*(nx+nu)+[1:nx+nu]); % cost 29 | end 30 | i = MPC_vars.N+1; 31 | objective = objective + 0.5*(quad_form(x((i-1)*(nx+nu)+[1:nx+nu]),stage(i).Qk)) + stage(i).fk'*x((i-1)*(nx+nu)+[1:nx+nu]); 32 | 33 | minimize( objective ) 34 | 35 | subject to 36 | x(1:nx+nu) == blkdiag(MPC_vars.Tx,MPC_vars.Tu)*[stage(1).x0;stage(1).u0]; % initialize initial state 37 | 38 | for i = 1:MPC_vars.N 39 | x((i)*(nx+nu)+[1:nx+nu]) == stage(i).Ak*x((i-1)*(nx+nu)+[1:nx+nu]) + stage(i).Bk*u((i-1)*nu+[1:nu]) + stage(i).gk ; %dynamics 40 | stage(i).lb <= [x((i-1)*(nx+nu)+[1:nx+nu]);u((i-1)*nu+[1:nu])] <= stage(i).ub; % bounds 41 | stage(i).lg <= stage(i).Ck*x((i-1)*(nx+nu)+[1:nx+nu]) <= stage(i).ug; %track constraints 42 | 43 | end 44 | i = MPC_vars.N+1; 45 | stage(i).lb(1:nx+nu) <= x((i-1)*(nx+nu)+[1:nx+nu]) <= stage(i).ub(1:nx+nu); %bounds 46 | stage(i).lg <= stage(i).Ck*x((i-1)*(nx+nu)+[1:nx+nu]) <= stage(i).ug; % track constraints 47 | 48 | cvx_end 49 | 50 | QPtime = toc(); 51 | 52 | x_opt = reshape(x,nx+nu,MPC_vars.N+1); 53 | u_opt = reshape(u,nu,MPC_vars.N); 54 | 55 | % rescale outputs 56 | X= MPC_vars.invTx*x_opt(1:nx,:); 57 | U = MPC_vars.invTu*x_opt(nx+1:end,2:end); 58 | dU = u_opt; 59 | 60 | if cvx_status == "Solved" 61 | info.exitflag = 0; 62 | else 63 | info.exitflag = 1; 64 | end 65 | info.QPtime = QPtime; 66 | 67 | cvx_clear 68 | 69 | end 70 | 71 | -------------------------------------------------------------------------------- /Matlab/DiscretizedLinearizedModel.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [Ad, Bd, gd]=DiscretizedLinearizedModel(Xbar_k,Ubar_k,ModelParams,Ts) 17 | % returns the discretized, linearized model about (Xbar_k,Ubar_k) 18 | % s.t. x(k+1) = Ad*x(k) + Bd*u(k) + gd 19 | % 20 | % Ad: [nx]x[nx], Bd: [nx]x[nu], gd: [nx]x[1] 21 | 22 | 23 | if ModelParams.ModelNo==1 || ModelParams.ModelNo==2 24 | sx=ModelParams.sx-1; 25 | su=ModelParams.su-1; 26 | 27 | 28 | Cm1=ModelParams.Cm1; 29 | Cm2=ModelParams.Cm2; 30 | Cr0=ModelParams.Cr0; 31 | Cr2=ModelParams.Cr2; 32 | B_r=ModelParams.Br; 33 | C_r=ModelParams.Cr; 34 | D_r=ModelParams.Dr; 35 | B_f=ModelParams.Bf; 36 | C_f=ModelParams.Cf; 37 | D_f=ModelParams.Df; 38 | m =ModelParams.m; 39 | Iz=ModelParams.Iz; 40 | l_f=ModelParams.lf; 41 | l_r=ModelParams.lr; 42 | 43 | 44 | phi =Xbar_k(ModelParams.stateindex_phi); 45 | v_x =Xbar_k(ModelParams.stateindex_vx); 46 | v_y =Xbar_k(ModelParams.stateindex_vy); 47 | omega =Xbar_k(ModelParams.stateindex_omega); 48 | 49 | 50 | D =Ubar_k(ModelParams.inputindex_D); 51 | delta =Ubar_k(ModelParams.inputindex_delta); 52 | vtheta=Ubar_k(ModelParams.inputindex_vtheta); 53 | 54 | if(v_x < 0.5) 55 | v_x = v_x; 56 | v_y = 0; 57 | omega = 0; 58 | delta = 0; 59 | if(v_x < 0.3) 60 | v_x = 0.3; 61 | end 62 | end 63 | 64 | alpha_f = -atan2(l_f*omega + v_y,v_x)+delta; 65 | alpha_r = atan2(l_r*omega - v_y,v_x); 66 | 67 | F_fy = D_f*sin(C_f*atan(B_f*alpha_f)); 68 | F_ry = D_r*sin(C_r*atan(B_r*alpha_r)); 69 | 70 | F_rx = (Cm1*D-Cm2*D*v_x-Cr0-Cr2*v_x^2); 71 | 72 | f=[v_x*cos(phi) - v_y*sin(phi); 73 | v_y*cos(phi) + v_x*sin(phi); 74 | omega; 75 | 1/m*(F_rx - F_fy*sin(delta) + m*v_y*omega); 76 | 1/m*(F_ry + F_fy*cos(delta) - m*v_x*omega); 77 | 1/Iz*(F_fy*l_f*cos(delta)- F_ry*l_r)]; 78 | %% Derivatives of the force laws 79 | % F_rx 80 | dFrx_dvx = -Cm2*D - 2*Cr2*v_x; 81 | dFrx_dD = Cm1 - Cm2*v_x; 82 | % F_ry 83 | dFry_dvx = ((B_r*C_r*D_r*cos(C_r*atan(B_r*alpha_r)))/(1+B_r^2*alpha_r^2))... 84 | *(-(l_r*omega - v_y)/((-l_r*omega + v_y)^2+v_x^2)); 85 | 86 | dFry_dvy = ((B_r*C_r*D_r*cos(C_r*atan(B_r*alpha_r)))/(1+B_r^2*alpha_r^2))... 87 | *((-v_x)/((-l_r*omega + v_y)^2+v_x^2)); 88 | 89 | dFry_domega = ((B_r*C_r*D_r*cos(C_r*atan(B_r*alpha_r)))/(1+B_r^2*alpha_r^2))... 90 | *((l_r*v_x)/((-l_r*omega + v_y)^2+v_x^2)); 91 | % F_fy 92 | 93 | dFfy_dvx = (B_f*C_f*D_f*cos(C_f*atan(B_f*alpha_f)))/(1+B_f^2*alpha_f^2)... 94 | *((l_f*omega + v_y)/((l_f*omega + v_y)^2+v_x^2)); 95 | 96 | dFfy_dvy = (B_f*C_f*D_f*cos(C_f*atan(B_f*alpha_f)))/(1+B_f^2*alpha_f^2)... 97 | *(-v_x/((l_f*omega + v_y)^2+v_x^2)); 98 | 99 | dFfy_domega = (B_f*C_f*D_f*cos(C_f*atan(B_f*alpha_f)))/(1+B_f^2*alpha_f^2)... 100 | *((-l_f*v_x)/((l_f*omega + v_y)^2+v_x^2)); 101 | 102 | dFfy_ddelta = (B_f*C_f*D_f*cos(C_f*atan(B_f*alpha_f)))/(1+B_f^2*alpha_f^2); 103 | %% 104 | % f1 = v_x*cos(phi) - v_y*sin(phi); 105 | df1_dphi = -v_x*sin(phi) - v_y*cos(phi); 106 | df1_dvx = cos(phi); 107 | df1_dvy = -sin(phi); 108 | 109 | % f2 = v_y*cos(phi) + v_x*sin(phi); 110 | df2_dphi = -v_y*sin(phi) + v_x*cos(phi); 111 | df2_dvx = sin(phi); 112 | df2_dvy = cos(phi); 113 | 114 | % f3 = omega; 115 | df3_domega = 1; 116 | 117 | % f4 = 1/m*(F_rx - F_fy*sin(delta) + m*v_y*omega); 118 | df4_dvx = 1/m*(dFrx_dvx - dFfy_dvx*sin(delta)); 119 | df4_dvy = 1/m*( - dFfy_dvy*sin(delta) + m*omega); 120 | df4_domega = 1/m*( - dFfy_domega*sin(delta) + m*v_y); 121 | df4_dD = 1/m* dFrx_dD; 122 | df4_ddelta = 1/m*( - dFfy_ddelta*sin(delta) - F_fy*cos(delta)); 123 | 124 | % f5 = 1/m*(F_ry + F_fy*cos(delta) - m*v_x*omega); 125 | df5_dvx = 1/m*(dFry_dvx + dFfy_dvx*cos(delta) - m*omega); 126 | df5_dvy = 1/m*(dFry_dvy + dFfy_dvy*cos(delta)); 127 | df5_domega = 1/m*(dFry_domega + dFfy_domega*cos(delta) - m*v_x); 128 | df5_ddelta = 1/m*( dFfy_ddelta*cos(delta) - F_fy*sin(delta)); 129 | 130 | % f6 = 1/Iz*(F_fy*l_f*cos(delta)- F_ry*l_r) 131 | df6_dvx = 1/Iz*(dFfy_dvx*l_f*cos(delta) - dFry_dvx*l_r); 132 | df6_dvy = 1/Iz*(dFfy_dvy*l_f*cos(delta) - dFry_dvy*l_r); 133 | df6_domega = 1/Iz*(dFfy_domega*l_f*cos(delta) - dFry_domega*l_r); 134 | df6_ddelta = 1/Iz*(dFfy_ddelta*l_f*cos(delta) - F_fy*l_f*sin(delta)); 135 | 136 | % Jacobians 137 | 138 | Ac=[0 0 df1_dphi df1_dvx df1_dvy 0 ; 139 | 0 0 df2_dphi df2_dvx df2_dvy 0 ; 140 | 0 0 0 0 0 df3_domega; 141 | 0 0 0 df4_dvx df4_dvy df4_domega; 142 | 0 0 0 df5_dvx df5_dvy df5_domega; 143 | 0 0 0 df6_dvx df6_dvy df6_domega]; 144 | 145 | Bc=[0 0 ; 146 | 0 0 ; 147 | 0 0 ; 148 | df4_dD df4_ddelta ; 149 | 0 df5_ddelta ; 150 | 0 df6_ddelta ]; 151 | 152 | 153 | 154 | gc=f-Ac*Xbar_k(1:sx)-Bc*Ubar_k(1:su); 155 | 156 | Bc_aug=[Bc gc]; 157 | 158 | %discretize 159 | 160 | % see report for proof of following method 161 | tmp = expm([Ac Bc_aug; zeros(su+1,sx+su+1)]*Ts); 162 | 163 | Ad = zeros(sx+1,sx+1); 164 | Bd = zeros(sx+1,su+1); 165 | gd = zeros(sx+1,1); 166 | Ad(1:sx,1:sx) =tmp(1:sx,1:sx); 167 | Bd(1:sx,1:su) =tmp(1:sx,sx+1:sx+su); 168 | gd(1:sx) =tmp(1:sx,sx+su+1); 169 | 170 | % following to avoid numerical errors 171 | Ad(end,end)=1; 172 | Bd(end,end)=Ts; 173 | 174 | end 175 | 176 | 177 | 178 | 179 | % in fact, the above can be done using only the physical states/inputs, 180 | % then can add Ad(end+1,end+1)=1; Bd(end+1,end+1)=Ts; 181 | 182 | end -------------------------------------------------------------------------------- /Matlab/ObstacelsState.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function Y = ObstacelsState(track,traj,trackWidth,n_cars) 17 | 18 | if n_cars == 1 19 | Y = []; 20 | elseif n_cars == 5 21 | startIdx_Op1 = 620; 22 | vx0_Op1 = 2; 23 | % find theta that coresponds to the 10th point on the centerline 24 | [theta_Op1, ~] = findTheta([track.center(1,startIdx_Op1),track.center(2,startIdx_Op1)],track.center,traj.ppx.breaks,trackWidth,startIdx_Op1); 25 | 26 | x0_Op1 = [track.center(1,startIdx_Op1),track.center(2,startIdx_Op1)+trackWidth/2,... % point on centerline 27 | atan2(ppval(traj.dppy,theta_Op1),ppval(traj.dppx,theta_Op1)),... % aligned with centerline 28 | vx0_Op1 ,0,0,theta_Op1]'; %driving straight with vx0, and correct theta progress 29 | 30 | startIdx_Op2 = 625; 31 | vx0_Op2 = 2; 32 | % find theta that coresponds to the 10th point on the centerline 33 | [theta_Op2, ~] = findTheta([track.center(1,startIdx_Op2),track.center(2,startIdx_Op2)],track.center,traj.ppx.breaks,trackWidth,startIdx_Op2); 34 | 35 | x0_Op2 = [track.center(1,startIdx_Op2),track.center(2,startIdx_Op2)+trackWidth/4,... % point on centerline 36 | atan2(ppval(traj.dppy,theta_Op2),ppval(traj.dppx,theta_Op2)),... % aligned with centerline 37 | vx0_Op2 ,0,0,theta_Op2]'; %driving straight with vx0, and correct theta progress 38 | 39 | startIdx_Op3 = 630; 40 | vx0_Op3 = 2; 41 | % find theta that coresponds to the 10th point on the centerline 42 | [theta_Op3, ~] = findTheta([track.center(1,startIdx_Op3),track.center(2,startIdx_Op3)],track.center,traj.ppx.breaks,trackWidth,startIdx_Op3); 43 | 44 | x0_Op3 = [track.center(1,startIdx_Op3),track.center(2,startIdx_Op3),... % point on centerline 45 | atan2(ppval(traj.dppy,theta_Op3),ppval(traj.dppx,theta_Op3)),... % aligned with centerline 46 | vx0_Op3 ,0,0,theta_Op3]'; %driving straight with vx0, and correct theta progress 47 | 48 | startIdx_Op4 = 645; 49 | vx0_Op4 = 2; 50 | % find theta that coresponds to the 10th point on the centerline 51 | [theta_Op4, ~] = findTheta([track.center(1,startIdx_Op4),track.center(2,startIdx_Op4)],track.center,traj.ppx.breaks,trackWidth,startIdx_Op4); 52 | 53 | x0_Op4 = [track.center(1,startIdx_Op4),track.center(2,startIdx_Op4)-trackWidth/3,... % point on centerline 54 | atan2(ppval(traj.dppy,theta_Op4),ppval(traj.dppx,theta_Op4)),... % aligned with centerline 55 | vx0_Op4 ,0,0,theta_Op4]'; %driving straight with vx0, and correct theta progress 56 | 57 | Y = [x0_Op1,x0_Op2,x0_Op3,x0_Op4]; 58 | else 59 | error('only 0 or 4 obstacles are pre programmed settings, feel free to change the obstacle constellation') 60 | end -------------------------------------------------------------------------------- /Matlab/PlotLog.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ ] = PlotLog( X,U,Y,track,track2,simN,Ts) 17 | 18 | figure(1); 19 | plot(track.outer(1,:),track.outer(2,:),'r') 20 | hold on 21 | plot(track.inner(1,:),track.inner(2,:),'r') 22 | plot(track2.outer(1,:),track2.outer(2,:),'k') 23 | plot(track2.inner(1,:),track2.inner(2,:),'k') 24 | plot(X(1,:),X(2,:),'b') 25 | if ~isempty(Y) 26 | for i=1:size(Y,2) 27 | carBox(Y(:,i),0.025,0.05) 28 | end 29 | end 30 | xlabel('X [m]') 31 | ylabel('Y [m]') 32 | axis equal 33 | hold off 34 | 35 | figure(2) 36 | subplot(3,1,1) 37 | plot([0:simN-1]*Ts,U(1,:)) 38 | xlabel('time [s]') 39 | ylabel('D [-]') 40 | subplot(3,1,2) 41 | plot([0:simN-1]*Ts,U(2,:)) 42 | xlabel('time [s]') 43 | ylabel('\delta [rad]') 44 | subplot(3,1,3) 45 | plot([0:simN-1]*Ts,U(3,:)) 46 | xlabel('time [s]') 47 | ylabel('v_{\Theta} [m/s]') 48 | 49 | figure(3) 50 | subplot(3,1,1) 51 | plot([0:simN-1]*Ts,X(4,:)) 52 | xlabel('time [s]') 53 | ylabel('\phi [rad]') 54 | subplot(3,1,2) 55 | plot([0:simN-1]*Ts,X(6,:)) 56 | xlabel('time [s]') 57 | ylabel('\omega [rad/s]') 58 | subplot(3,1,3) 59 | plot([0:simN-1]*Ts,180/pi*atan2(X(5,:),X(4,:))) 60 | xlabel('time [s]') 61 | ylabel('\beta [deg]') 62 | 63 | pause(0.001) 64 | end 65 | 66 | -------------------------------------------------------------------------------- /Matlab/PlotPrediction.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ ] = PlotPrediction( x,u,b,Y,track,track2,traj,MPC_vars,ModelParams ) 17 | 18 | N = MPC_vars.N; 19 | Ts = MPC_vars.Ts; 20 | 21 | tl = traj.ppx.breaks(end); 22 | 23 | figure(1); 24 | plot(track.outer(1,:),track.outer(2,:),'r') 25 | hold on 26 | plot(track.inner(1,:),track.inner(2,:),'r') 27 | plot(track2.outer(1,:),track2.outer(2,:),'k') 28 | plot(track2.inner(1,:),track2.inner(2,:),'k') 29 | plot(b(:,1),b(:,2),'g.') 30 | plot(b(:,3),b(:,4),'g.') 31 | plot(ppval(traj.ppx,mod(x(7,:),tl)),ppval(traj.ppy,mod(x(7,:),tl)),':k') 32 | plot(x(1,:),x(2,:),'-b') 33 | carBox(x(:,1),ModelParams.W/2,ModelParams.L/2) 34 | if ~isempty(Y) 35 | for i=1:size(Y,2) 36 | carBox(Y(:,i),ModelParams.W/2,ModelParams.L/2) 37 | end 38 | end 39 | xlabel('X [m]') 40 | ylabel('Y [m]') 41 | axis equal 42 | hold off 43 | 44 | figure(2) 45 | subplot(3,1,1) 46 | plot([0:N-1]*Ts,u(1,:)) 47 | xlabel('time [s]') 48 | ylabel('D [-]') 49 | subplot(3,1,2) 50 | plot([0:N-1]*Ts,u(2,:)) 51 | xlabel('time [s]') 52 | ylabel('\delta [rad]') 53 | subplot(3,1,3) 54 | plot([0:N-1]*Ts,u(3,:)) 55 | xlabel('time [s]') 56 | ylabel('v_{\Theta} [m/s]') 57 | 58 | figure(3) 59 | subplot(3,1,1) 60 | plot([0:N]*Ts,x(3,:)) 61 | xlabel('time [s]') 62 | ylabel('\phi [rad]') 63 | subplot(3,1,2) 64 | plot([0:N]*Ts,x(6,:)) 65 | xlabel('time [s]') 66 | ylabel('\omega [rad/s]') 67 | subplot(3,1,3) 68 | plot([0:N]*Ts,x(7,:)) 69 | xlabel('time [s]') 70 | ylabel('\Theta [m]') 71 | 72 | pause(0.001) 73 | 74 | end 75 | 76 | -------------------------------------------------------------------------------- /Matlab/QuadProgInterface.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ X,U,dU,info ] = QuadProgInterface(stage,MPC_vars,ModelParams) 17 | nx = ModelParams.nx; 18 | nu = ModelParams.nu; 19 | ng = 2; 20 | nz = nx+2*nu; 21 | nxu = nx+nu; 22 | N = MPC_vars.N; 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | H = zeros(nz*N+nxu,nz*N+nxu); 25 | f = zeros(nz*N+nxu,1); 26 | for i = 1:N+1 27 | if i1 104 | U(1:nu,i-1) = MPC_vars.invTu*z((i-1)*nz+nx+[1:nu]); 105 | end 106 | 107 | if i<=N 108 | dU(1:nu,i) = z((i-1)*nz+nxu+[1:nu]); 109 | end 110 | end 111 | end 112 | 113 | info.QPtime = QPtime; 114 | if exitflag == 1 115 | info.exitflag = 0; 116 | else 117 | info.exitflag = 1; 118 | end 119 | 120 | end 121 | -------------------------------------------------------------------------------- /Matlab/README.md: -------------------------------------------------------------------------------- 1 | # MPCC 2 | This is a Matlab implementation of the MPCC controller. The implementation is very similar to the approach described in [Optimization‐based autonomous racing of 1:43 scale RC cars](https://onlinelibrary.wiley.com/doi/abs/10.1002/oca.2123). 3 | 4 | ## How to run 5 | 6 | The simplest way to run the code is using the quadprog interface as this only requires Matlab. However, we recommend the hpipm interface as it reduces the solve times massively which makes working with the simulation more convenient. 7 | 8 | ### Before running code 9 | 1) Install [hpipm](https://github.com/giaf/hpipm) including the Matlab mex interface 10 | 2) alternatively install Yalmip or CVX, or use the quadprog interface 11 | ### Run code 12 | 0) in simulation.m change to the optimization framework you use (hpipm, Yalmip, CVX, quadprog) 13 | 1) run simulation.m 14 | 2) play with the tuning in getMPC_vars.m 15 | 3) change the car model between FullSize and ORCA 16 | 4) change the track layout between the ORCA and the RCP track 17 | 18 | ### Notes 19 | 1) If you use the RCP track the obstacle positions need to be changed by hand 20 | 21 | ## Options 22 | Beside the 1:43 RC cars, we also implemented a full sized car model as well as two possible tracks. 23 | 24 | -------------------------------------------------------------------------------- /Matlab/SimTimeStep.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function xp=SimTimeStep(x,u,Ts,ModelParams) 17 | %x state 18 | %u input 19 | %Ts sampling time 20 | x0=x; 21 | 22 | [~,inivt]=ode45(@(t,x)fx_bicycle(t,x,u,ModelParams),[0 Ts],x0); 23 | xp=inivt(end,:); 24 | return 25 | 26 | function xdot=fx_bicycle(t,x,u,ModelParams) 27 | 28 | Cm1=ModelParams.Cm1; 29 | Cm2=ModelParams.Cm2; 30 | Cr0=ModelParams.Cr0; 31 | Cr2=ModelParams.Cr2; 32 | 33 | 34 | B_r = ModelParams.Br; 35 | C_r = ModelParams.Cr; 36 | D_r = ModelParams.Dr; 37 | B_f = ModelParams.Bf; 38 | C_f = ModelParams.Cf; 39 | D_f = ModelParams.Df; 40 | 41 | m = ModelParams.m; 42 | Iz = ModelParams.Iz; 43 | l_f = ModelParams.lf; 44 | l_r = ModelParams.lr; 45 | 46 | 47 | phi =x(3); 48 | v_x =x(4); 49 | v_y =x(5); 50 | omega =x(6); 51 | D =u(1); 52 | delta =u(2); 53 | 54 | 55 | alpha_f = -atan2(l_f*omega + v_y,abs(v_x))+delta; 56 | alpha_r = atan2(l_r*omega - v_y,abs(v_x)); 57 | 58 | F_fy = D_f*sin(C_f*atan(B_f*alpha_f)); 59 | F_ry = D_r*sin(C_r*atan(B_r*alpha_r)); 60 | 61 | F_rx = (Cm1*D-Cm2*D*v_x-Cr0-Cr2*v_x^2); 62 | 63 | xdot=[v_x*cos(phi) - v_y*sin(phi); 64 | v_y*cos(phi) + v_x*sin(phi); 65 | omega; 66 | 1/m*(F_rx - F_fy*sin(delta) + m*v_y*omega); 67 | 1/m*(F_ry + F_fy*cos(delta) - m*v_x*omega); 68 | 1/Iz*(F_fy*l_f*cos(delta)- F_ry*l_r); 69 | u(3)]; 70 | 71 | 72 | return -------------------------------------------------------------------------------- /Matlab/Tracks/track2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Matlab/Tracks/track2.mat -------------------------------------------------------------------------------- /Matlab/Tracks/trackMobil.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexliniger/MPCC/bd331621ba47ae3326711922a863bdb1cdf2d2ea/Matlab/Tracks/trackMobil.mat -------------------------------------------------------------------------------- /Matlab/YalmipInterface.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ X,U,dU,info ] = YalmipInterface(stage,MPC_vars,ModelParams) 17 | nx = ModelParams.nx; 18 | nu = ModelParams.nu; 19 | tic 20 | x = sdpvar(nx+nu, MPC_vars.N+1); % states + initial state; fifth initial state for discretization 21 | u = sdpvar(nu, MPC_vars.N); % (front) steering angle 22 | objective = 0; 23 | constraints = [x(:,1) == blkdiag(MPC_vars.Tx,MPC_vars.Tu)*[stage(1).x0;stage(1).u0]]; % initialize initial state 24 | 25 | for i = 1:MPC_vars.N 26 | constraints = [constraints; 27 | x(:, i+1) == stage(i).Ak*x(:,i) + stage(i).Bk*u(:,i) + stage(i).gk ; %dynamics 28 | stage(i).lb <= [x(:,i);u(:,i)] <= stage(i).ub; % bounds 29 | stage(i).lg <= stage(i).Ck*x(:,i) <= stage(i).ug]; %track constraints 30 | 31 | objective = objective + 0.5*(x(:,i)'*stage(i).Qk*x(:,i) + u(:,i)'*stage(i).Rk*u(:,i)) ... 32 | + stage(i).fk'*x(:,i); % cost 33 | 34 | end 35 | i = MPC_vars.N+1; 36 | objective = objective + 0.5*(x(:,i)'*stage(i).Qk*x(:,i)) + stage(i).fk'*x(:,i); 37 | constraints = [constraints; 38 | stage(i).lb(1:nx+nu) <= x(:,i) <= stage(i).ub(1:nx+nu); %bounds 39 | stage(i).lg <= stage(i).Ck*x(:,i) <= stage(i).ug]; % track constraints 40 | yalmipTime = toc 41 | ops = sdpsettings('solver','ecos','verbose',2); 42 | 43 | % solve QP 44 | tic; 45 | exitflag = solvesdp(constraints,objective,ops); 46 | QPtime = toc; 47 | 48 | x_opt = double(x); 49 | u_opt = double(u); 50 | 51 | % rescale outputs 52 | X= MPC_vars.invTx*x_opt(1:nx,:); 53 | U = MPC_vars.invTu*x_opt(nx+1:end,2:end); 54 | dU = u_opt; 55 | 56 | info.exitflag = exitflag; 57 | info.QPtime = QPtime; 58 | 59 | end 60 | 61 | -------------------------------------------------------------------------------- /Matlab/augState.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [xTemp,uTemp] = augState(x,u,x0,MPC_vars,ModelParams,tl) 17 | 18 | nx = ModelParams.nx; 19 | nu = ModelParams.nu; 20 | N = MPC_vars.N; 21 | Ts = MPC_vars.Ts; 22 | indexPhi = ModelParams.stateindex_phi; 23 | indexTheta = ModelParams.stateindex_theta; 24 | 25 | xTemp = zeros(nx,N+1); 26 | uTemp = zeros(nu,N); 27 | 28 | xTemp(:,1) = x0; 29 | uTemp(:,1) = u(:,2); 30 | for j=2:N-1 31 | xTemp(:,j) = x(:,j+1); 32 | uTemp(:,j) = u(:,j+1); 33 | end 34 | j = N; 35 | xTemp(:,j) = x(:,j+1); 36 | uTemp(:,j) = u(:,j); 37 | 38 | j = N+1; 39 | xTemp(:,j) = SimTimeStep(x(:,N+1),u(:,N),Ts,ModelParams); 40 | 41 | if xTemp(indexPhi,1) - xTemp(indexPhi,2) > pi 42 | xTemp(indexPhi,2:end) = xTemp(indexPhi,2:end)+2*pi; 43 | end 44 | if xTemp(indexPhi,1) - xTemp(indexPhi,2) < -pi 45 | xTemp(indexPhi,2:end) = xTemp(indexPhi,2:end)-2*pi; 46 | end 47 | 48 | if xTemp(indexTheta,1) - xTemp(indexTheta,2) < -0.75*tl 49 | xTemp(indexTheta,2:end) = xTemp(indexTheta,2:end)-tl; 50 | end 51 | 52 | end -------------------------------------------------------------------------------- /Matlab/borderAdjustment.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [track,track2] = borderAdjustment(track2,ModelParams,safetyScaling) 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | %%%% shrink track on both sides with 'WidthCar' %%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | WidthCar = 0.5*ModelParams.W*safetyScaling; 21 | %scale track to car size 22 | track2.center = track2.center*ModelParams.Scale; 23 | track2.inner = track2.inner*ModelParams.Scale; 24 | track2.outer = track2.outer*ModelParams.Scale; 25 | 26 | % compute width of track (assumption track as a uniform width 27 | widthTrack = norm([track2.inner(1,1)-track2.outer(1,1),track2.inner(2,1)-track2.outer(2,1)]); 28 | 29 | for i = 1:length(track2.outer) 30 | x1 = track2.outer(1,i); 31 | y1 = track2.outer(2,i); 32 | x2 = track2.inner(1,i); 33 | y2 = track2.inner(2,i); 34 | % vector connecting right and left boundary 35 | numer=(x2-x1); 36 | denom=(y1-y2); 37 | 38 | % shrinking ratio 39 | c = WidthCar/widthTrack; 40 | d = -WidthCar/widthTrack; 41 | 42 | % shrink track 43 | track.outer(1,i) = x1 + c*numer; 44 | track.inner(1,i) = x2 - c*numer; 45 | track.outer(2,i) = y1 + d*denom; 46 | track.inner(2,i) = y2 - d*denom; 47 | end 48 | track.center = track2.center; 49 | 50 | end -------------------------------------------------------------------------------- /Matlab/carBox.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [] = carBox(x0,w,l) 17 | car1 = x0(1:2) + [cos(x0(3))*l;sin(x0(3))*l] + [sin(x0(3))*w;-cos(x0(3))*w]; 18 | car2 = x0(1:2) + [cos(x0(3))*l;sin(x0(3))*l] - [sin(x0(3))*w;-cos(x0(3))*w]; 19 | car3 = x0(1:2) - [cos(x0(3))*l;sin(x0(3))*l] + [sin(x0(3))*w;-cos(x0(3))*w]; 20 | car4 = x0(1:2) - [cos(x0(3))*l;sin(x0(3))*l] - [sin(x0(3))*w;-cos(x0(3))*w]; 21 | 22 | plot([car1(1),car2(1),car4(1),car3(1),car1(1)],[car1(2),car2(2),car4(2),car3(2),car1(2)],'k','LineWidth',1) 23 | end 24 | -------------------------------------------------------------------------------- /Matlab/env.sh: -------------------------------------------------------------------------------- 1 | #! /usr/bin/bash 2 | 3 | if [[ "${BASH_SOURCE[0]}" != "${0}" ]] 4 | then 5 | echo "Script is being sourced" 6 | else 7 | echo "ERROR: Script is a subshell" 8 | echo "To affect your current shell enviroment source this script with:" 9 | echo "source env.sh" 10 | exit 11 | fi 12 | 13 | # check that this file is run 14 | export ENV_RUN=true 15 | 16 | # if hpipm folder not specified assume alongside the parent of this folder 17 | HPIPM_MAIN_FOLDER=${HPIPM_MAIN_FOLDER:-"$(pwd)/../../hpipm"} 18 | export HPIPM_MAIN_FOLDER 19 | echo 20 | echo "HPIPM_MAIN_FOLDER=$HPIPM_MAIN_FOLDER" 21 | 22 | # if blasfeo folder not specified assume alongside the parent of this folder 23 | BLASFEO_MAIN_FOLDER=${BLASFEO_MAIN_FOLDER:-"$(pwd)/../../blasfeo"} 24 | export BLASFEO_MAIN_FOLDER 25 | echo 26 | echo "BLASFEO_MAIN_FOLDER=$BLASFEO_MAIN_FOLDER" 27 | 28 | # export matlab_octave_mex folder 29 | # MATLAB case 30 | export MATLABPATH=$MATLABPATH:$HPIPM_MAIN_FOLDER/interfaces/matlab_octave/ 31 | echo 32 | echo "MATLABPATH=$MATLABPATH" 33 | # Octave case 34 | export OCTAVE_PATH=$OCTAVE_PATH:$HPIPM_MAIN_FOLDER/interfaces/matlab_octave/ 35 | echo 36 | echo "OCTAVE_PATH=$OCTAVE_PATH" 37 | 38 | #export HPIPM_MEX_FLAGS="GCC=/usr/bin/gcc-4.9" 39 | 40 | # export LD_LIBRARY_PATH 41 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$HPIPM_MAIN_FOLDER/lib:$BLASFEO_MAIN_FOLDER/lib 42 | echo 43 | echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" 44 | 45 | -------------------------------------------------------------------------------- /Matlab/findTheta.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ theta, closestIdx] = findTheta(currentPosition,TrackCenter,traj_breaks,trackWidth,last_closestIdx) 17 | %FINDTHETA returns theta, the index of the point on the centerline which is the 18 | %closest to the current position 19 | 20 | %the algorithm first performs a local search around a guess where the 21 | %closest point on the center line is if the error is too big (more than 22 | %half the track width) a global search is performed. 23 | 24 | %then the projection is computed using the inner product and assuming the 25 | %track is picewise affine 26 | 27 | posX = currentPosition(1); 28 | posY = currentPosition(2); 29 | TrackX = TrackCenter(1,:); 30 | TrackY = TrackCenter(2,:); 31 | trackCenterline = TrackCenter; 32 | N_Track = length(TrackX); 33 | 34 | % length of local search region 35 | N_searchRegion = 26; 36 | searchRegion = zeros(1,N_searchRegion); 37 | % determin index which are investigated 38 | k = 1; 39 | for i = last_closestIdx-5:last_closestIdx+20 40 | searchRegion(k) = i; 41 | k = k+1; 42 | end 43 | % wrapping if necessary 44 | if last_closestIdx > N_Track - 20 || last_closestIdx <= 5 45 | i = find(searchRegion < 1); 46 | k = find(searchRegion > N_Track); 47 | searchRegion(i) = N_Track+searchRegion(i); 48 | searchRegion(k) = searchRegion(k)-N_Track; 49 | end 50 | 51 | % compute squared distance to the current location 52 | TrackXsmall = TrackX(searchRegion); 53 | TrackYsmall = TrackY(searchRegion); 54 | distanceX = TrackXsmall - posX*ones(1,N_searchRegion); 55 | distanceY = TrackYsmall - posY*ones(1,N_searchRegion); 56 | 57 | squared_distance_array = distanceX.^2 + distanceY.^2; 58 | % find closest point 59 | [e , minIndex] = min(squared_distance_array); 60 | minIndex = searchRegion(minIndex); 61 | 62 | % if distance is to large perform global search with respect to track width 63 | if sqrt(e) > (trackWidth*1.25)/2 64 | distanceX2 = TrackX - posX*ones(1,N_Track); 65 | distanceY2 = TrackY - posY*ones(1,N_Track); 66 | 67 | squared_distance_array2 = distanceX2.^2 + distanceY2.^2; 68 | 69 | [e , minIndex] = min(squared_distance_array2); 70 | end 71 | 72 | % circular edge conditions 73 | if minIndex == 1; 74 | nextIdx = 2; 75 | prevIdx = N_Track; 76 | elseif minIndex == N_Track; 77 | nextIdx = 1; 78 | prevIdx = N_Track-1; 79 | else 80 | nextIdx = minIndex + 1; 81 | prevIdx = minIndex - 1; 82 | end 83 | 84 | 85 | % compute theta based on inner product projection 86 | closestIdx = minIndex; 87 | cosinus = dot([posX;posY] - trackCenterline(:,minIndex), trackCenterline(:,prevIdx) - trackCenterline(:,minIndex)); 88 | if (cosinus > 0) 89 | minIndex2 = prevIdx; 90 | else 91 | minIndex2 = minIndex; 92 | minIndex = nextIdx; 93 | end 94 | 95 | 96 | if e ~= 0 97 | cosinus = dot([posX;posY] - trackCenterline(:,minIndex2), trackCenterline(:,minIndex) - trackCenterline(:,minIndex2))/... 98 | (norm([posX;posY] - trackCenterline(:,minIndex2))*norm(trackCenterline(:,minIndex) - trackCenterline(:,minIndex2))); 99 | else 100 | cosinus = 0; 101 | end 102 | theta = traj_breaks(1,minIndex2); 103 | theta = theta + cosinus*norm([posX;posY] - trackCenterline(:,minIndex2),2); 104 | return -------------------------------------------------------------------------------- /Matlab/getMPC_vars.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function MPC_vars = getMPC_vars(CarModel) 17 | 18 | 19 | if CarModel == "ORCA" 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | % MPC settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 23 | % prediction horizon 24 | MPC_vars.N = 40; 25 | % sampling time 26 | MPC_vars.Ts = 0.02; 27 | % used model (TODO incorparate new models) 28 | MPC_vars.ModelNo = 1; 29 | % use bounds on all opt variables (TODO implement selective bounds) 30 | MPC_vars.fullBound = 1; 31 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 32 | % state-input scaling %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 34 | % normalization matricies (scale states and inputs to ~ +/- 1 35 | MPC_vars.Tx = diag(1./[3,3,2*pi,4,2,7,30]); 36 | MPC_vars.Tu = diag(1./[1,0.35,6]); 37 | 38 | MPC_vars.invTx = diag([3,3,2*pi,4,2,7,30]); 39 | MPC_vars.invTu = diag([1,0.35,6]); 40 | 41 | MPC_vars.TDu = eye(3); 42 | MPC_vars.invTDu = eye(3); 43 | % identity matricies if inputs should not be normalized 44 | % MPC_vars.Tx = eye(7); 45 | % MPC_vars.Tu = eye(3); 46 | % 47 | % MPC_vars.invTx = eye(7); 48 | % MPC_vars.invTu = eye(3); 49 | % 50 | % MPC_vars.TDu = eye(3); 51 | % MPC_vars.invTDu = eye(3); 52 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 53 | % state-input bounds %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 54 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 55 | 56 | % bounds for non-nomalized state-inputs 57 | % MPC_vars.bounds = [-3,-3,-10,-0.1,-2,-7, 0, -0.1,-0.35,0 , -1 ,-1,-5; 58 | % 3, 3, 10, 4, 2, 7, 30, 1, 0.35,5 , 1 , 1, 5]'; 59 | % bounds for nomalized state-inputs (bounds can be changed by changing 60 | % % normalization) 61 | MPC_vars.bounds = [-1,-1,-3, 0,-1,-1, 0, -0.1,-1,0 , -1 ,-1,-5; 62 | 1, 1, 3, 1, 1, 1, 1, 1, 1,1 , 1 , 1, 5]'; 63 | 64 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 65 | % Cost Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 66 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 67 | MPC_vars.qC = 0.1; % contouring cost 68 | MPC_vars.qCNmult= 10; % increase of terminal contouring cost 69 | MPC_vars.qL= 1000; % lag cost 70 | MPC_vars.qVtheta= 0.02; % theta maximization cost 71 | MPC_vars.qOmega = 1e-5; % yaw rate regularization cost 72 | MPC_vars.qOmegaNmult = 10; % yaw rate regularization cost 73 | 74 | MPC_vars.rD= 1e-4; % cost on duty cycle (only used as regularization terms) 75 | MPC_vars.rDelta= 1e-4; % cost on steering 76 | MPC_vars.rVtheta= 1e-4; % cost on virtual velocity 77 | 78 | MPC_vars.rdD= 0.01; % cost on change of duty cycle 79 | MPC_vars.rdDelta= 1; % cost on change of steering 80 | MPC_vars.rdVtheta= 0.001; % cost on change of virtual velocity 81 | 82 | 83 | MPC_vars.q_eta = 250; % cost on soft constraints (TODO implement soft constraints) 84 | 85 | MPC_vars.costScale = 1; % scaling of the cost for better numerics 86 | 87 | elseif CarModel == "FullSize" 88 | 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 90 | % MPC settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | % prediction horizon 93 | MPC_vars.N = 120; 94 | % sampling time 95 | MPC_vars.Ts = 0.05; 96 | % used model (TODO incorparate new models) 97 | MPC_vars.ModelNo = 2; 98 | % use bounds on all opt variables (TODO implement selective bounds) 99 | MPC_vars.fullBound = 1; 100 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101 | % state-input scaling %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 102 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 103 | % normalization matricies (scale states and inputs to ~ +/- 1 104 | MPC_vars.Tx = diag(1./[1,1,2*pi,10,10,5,10]); 105 | MPC_vars.Tu = diag(1./[1,0.5,10]); 106 | 107 | MPC_vars.invTx = diag([1,1,2*pi,10,10,5,10]); 108 | MPC_vars.invTu = diag([1,0.5,10]); 109 | 110 | MPC_vars.TDu = eye(3); 111 | MPC_vars.invTDu = eye(3); 112 | % identity matricies if inputs should not be normalized 113 | % MPC_vars.Tx = eye(7); 114 | % MPC_vars.Tu = eye(3); 115 | % 116 | % MPC_vars.invTx = eye(7); 117 | % MPC_vars.invTu = eye(3); 118 | % 119 | % MPC_vars.TDu = eye(3); 120 | % MPC_vars.invTDu = eye(3); 121 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 122 | % state-input bounds %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 123 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 124 | 125 | % bounds for non-nomalized state-inputs 126 | % MPC_vars.bounds = [-3,-3,-10,-0.1,-2,-7, 0, -0.1,-0.35,0 , -1 ,-1,-5; 127 | % 3, 3, 10, 4, 2, 7, 30, 1, 0.35,5 , 1 , 1, 5]'; 128 | % bounds for nomalized state-inputs (bounds can be changed by changing 129 | % % normalization) 130 | MPC_vars.bounds = [-1e4,-1e4,-3, 0.25,-3,-1, 0, -1,-1, 0 , -0.25 ,-0.1,-10; 131 | 1e4, 1e4, 3, 10, 3, 1, 1e4, 1, 1,10 , 0.25 , 0.1, 10]'; 132 | 133 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | % Cost Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 135 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 136 | MPC_vars.qC = 0.01; % contouring cost 137 | MPC_vars.qCNmult= 10000; % increase of terminal contouring cost 138 | MPC_vars.qL= 1000; % lag cost 139 | MPC_vars.qVtheta= 0.5; % theta maximization cost 140 | MPC_vars.qOmega = 5e0; % yaw rate regularization cost 141 | MPC_vars.qOmegaNmult = 1; % yaw rate regularization cost 142 | 143 | MPC_vars.rD= 1e-4; % cost on duty cycle (only used as regularization terms) 144 | MPC_vars.rDelta= 1e-4; % cost on steering 145 | MPC_vars.rVtheta= 1e-6; % cost on virtual velocity 146 | 147 | MPC_vars.rdD= 0.1; % cost on change of duty cycle 148 | MPC_vars.rdDelta= 1; % cost on change of steering 149 | MPC_vars.rdVtheta= 0.1; % cost on change of virtual velocity 150 | 151 | MPC_vars.q_eta = 250; % cost on soft constraints (TODO implement soft constraints) 152 | 153 | MPC_vars.costScale = 0.01; % scaling of the cost for better numerics 154 | else 155 | error('invalid model name') 156 | end 157 | 158 | 159 | end 160 | -------------------------------------------------------------------------------- /Matlab/getModelParams.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function ModelParams=getModelParams(ModelNo) 17 | 18 | if ModelNo==1 19 | 20 | ModelParams.ModelNo=1; 21 | ModelParams.Scale=1;%scale of the car (1 is a 1:43 scale car) 22 | 23 | ModelParams.sx=7; %number of states 24 | ModelParams.su=3; %number of inputs 25 | ModelParams.nx=7; %number of states 26 | ModelParams.nu=3; %number of inputs 27 | 28 | ModelParams.stateindex_x=1; %x position 29 | ModelParams.stateindex_y=2; %y position 30 | ModelParams.stateindex_phi=3; %orientation 31 | ModelParams.stateindex_vx=4; %longitudinal velocity 32 | ModelParams.stateindex_vy=5; %lateral velocity 33 | ModelParams.stateindex_omega=6; %yaw rate 34 | ModelParams.stateindex_theta=7; %virtual position 35 | 36 | ModelParams.inputindex_D=1; %duty cycle 37 | ModelParams.inputindex_delta=2; %steering angle 38 | ModelParams.inputindex_vtheta=3; %virtual speed 39 | 40 | ModelParams.m = 0.041; 41 | ModelParams.Iz = 27.8e-6; 42 | ModelParams.lf = 0.029; 43 | ModelParams.lr = 0.033; 44 | 45 | ModelParams.Cm1=0.287; 46 | ModelParams.Cm2=0.0545; 47 | ModelParams.Cr0=0.0518; 48 | ModelParams.Cr2=0.00035; 49 | 50 | ModelParams.Br = 3.3852; 51 | ModelParams.Cr = 1.2691; 52 | ModelParams.Dr = 0.1737; 53 | 54 | ModelParams.Bf = 2.579; 55 | ModelParams.Cf = 1.2; 56 | ModelParams.Df = 0.192; 57 | 58 | ModelParams.L = 0.12; 59 | ModelParams.W = 0.06; 60 | 61 | elseif ModelNo==2 62 | ModelParams.ModelNo=2; 63 | ModelParams.Scale=43;%scale of the car (1 is a 1:43 scale car) 64 | 65 | ModelParams.sx=7; %number of states 66 | ModelParams.su=3; %number of inputs 67 | ModelParams.nx=7; %number of states 68 | ModelParams.nu=3; %number of inputs 69 | 70 | ModelParams.stateindex_x=1; %x position 71 | ModelParams.stateindex_y=2; %y position 72 | ModelParams.stateindex_phi=3; %orientation 73 | ModelParams.stateindex_vx=4; %longitudinal velocity 74 | ModelParams.stateindex_vy=5; %lateral velocity 75 | ModelParams.stateindex_omega=6; %yaw rate 76 | ModelParams.stateindex_theta=7; %virtual position 77 | 78 | ModelParams.inputindex_D=1; %duty cycle 79 | ModelParams.inputindex_delta=2; %steering angle 80 | ModelParams.inputindex_vtheta=3; %virtual speed 81 | 82 | ModelParams.m = 1573; 83 | ModelParams.Iz = 2873; 84 | ModelParams.lf = 1.35; 85 | ModelParams.lr = 1.35; 86 | 87 | ModelParams.Wight_f = ModelParams.lr/(ModelParams.lf+ModelParams.lr); 88 | ModelParams.Wight_r = ModelParams.lf/(ModelParams.lf+ModelParams.lr); 89 | 90 | ModelParams.Cm1=17303; 91 | ModelParams.Cm2=175; 92 | ModelParams.Cr0=120; 93 | ModelParams.Cr2=0.5*1.225*0.35*2.5;%0.5*rho*cd*A 94 | 95 | ModelParams.Br = 13; 96 | ModelParams.Cr = 2; 97 | ModelParams.Dr = ModelParams.Wight_f*ModelParams.m*9.81*1.2; 98 | 99 | ModelParams.Bf = 13; 100 | ModelParams.Cf = 2; 101 | ModelParams.Df = ModelParams.Wight_r*ModelParams.m*9.81*1.2; 102 | 103 | ModelParams.L = 5; 104 | ModelParams.W = 2.5; 105 | else 106 | error('ModelNo invalid'); 107 | end 108 | 109 | end 110 | -------------------------------------------------------------------------------- /Matlab/hpipmInterface.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ X,U,dU,info ] = hpipmInterface(stage,MPC_vars,ModelParams) 17 | 18 | 19 | % check that env.sh has been run 20 | env_run = getenv('ENV_RUN'); 21 | if (~strcmp(env_run, 'true')) 22 | disp('ERROR: env.sh has not been sourced! Before using the HPIPM solver, run in the shell:'); 23 | disp('source env.sh'); 24 | disp('and then launch matlab or octave from the same shell.'); 25 | disp('(tested on linux, untested on windows as of now).'); 26 | return; 27 | end 28 | 29 | 30 | nx = ModelParams.nx; 31 | nu = ModelParams.nu; 32 | qpTotal = tic; 33 | 34 | %import hpipm_matlab.* 35 | 36 | % dims 37 | N = MPC_vars.N; 38 | 39 | dims = hpipm_ocp_qp_dim(N); 40 | 41 | dims.set('nx', (nx+nu), 0, N); 42 | dims.set('nu', nu, 0, N-1); 43 | 44 | dims.set('nbx', (nx+nu), 0, N); 45 | dims.set('nbu', nu, 0, N-1); 46 | 47 | dims.set('ng', 0, 0); 48 | dims.set('ng', 1, 1, N); 49 | 50 | %dims.print_C_struct(); 51 | 52 | 53 | % qp 54 | qp = hpipm_ocp_qp(dims); 55 | %% Equility Constraints 56 | x0 = blkdiag(MPC_vars.Tx,MPC_vars.Tu)*[stage(1).x0;stage(1).u0]; 57 | for i = 0:N-1 58 | qp.set('A', stage(i+1).Ak, i); 59 | qp.set('B', stage(i+1).Bk, i); 60 | qp.set('b', stage(i+1).gk, i); 61 | end 62 | 63 | %% Cost 64 | for i = 0:N 65 | qp.set('Q', stage(i+1).Qk, i); 66 | qp.set('q', stage(i+1).fk, i); 67 | if i QP solved\n') 137 | % qp_sol.print_C_struct() 138 | else 139 | fprintf('-> Solver failed!\n') 140 | end 141 | 142 | 143 | % extract and print sol 144 | u_opt = zeros(nu,N); 145 | x_opt = zeros(nx+nu,N); 146 | for i=0:N-1 147 | u_opt(:,i+1) = qp_sol.get('u', i); 148 | end 149 | for i=0:N 150 | x_opt(:,i+1) = qp_sol.get('x', i); 151 | end 152 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 153 | QPtime = toc(qpTotal); 154 | 155 | % rescale outputs 156 | X = MPC_vars.invTx*x_opt(1:nx,:); 157 | U = MPC_vars.invTu*x_opt(nx+1:end,2:end); 158 | dU = u_opt; 159 | 160 | if return_flag == 0 161 | info.exitflag = 0; 162 | else 163 | info.exitflag = 1; 164 | end 165 | info.QPtime = tmp_time; 166 | 167 | 168 | 169 | if is_octave() 170 | % directly call destructor for octave 4.2.2 (ubuntu 18.04) + others ??? 171 | if strcmp(version(), '4.2.2') 172 | delete(dims); 173 | delete(qp); 174 | delete(qp_sol); 175 | delete(arg); 176 | delete(solver); 177 | end 178 | end 179 | 180 | 181 | 182 | end 183 | 184 | -------------------------------------------------------------------------------- /Matlab/optimizer_mpcc.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [x, u, new_b, exitflag,info] = optimizer_mpcc(TrackMPC,MPC_vars,ModelParams,n_cars, Y, x, u, x0, u0) 17 | 18 | % reshape X such that it can be used in DP 19 | X = reshape(x,(MPC_vars.N+1)*ModelParams.nx,1); 20 | % add other cars to X for DP 21 | if ~isempty(Y) 22 | Y = repmat(Y,MPC_vars.N+1,1); 23 | end 24 | X = [X,Y]; 25 | % get boarder points using DP based algorithm 26 | [new_b,~] = getNewBorders(TrackMPC.traj,TrackMPC.borders,TrackMPC.track_center,X,n_cars,1,MPC_vars,ModelParams); 27 | 28 | % formulate MPCC problem and solve it given the DP bundaries 29 | [xpred, upred,dupred,info] = getMPCmatrices(TrackMPC.traj,MPC_vars,ModelParams,new_b,x,u,x0,u0); 30 | 31 | if (info.exitflag == 0) 32 | exitflag = 0; 33 | x = xpred; 34 | u = upred; 35 | else 36 | exitflag = 1; 37 | % x and u stay identical to the initial guess 38 | end 39 | end -------------------------------------------------------------------------------- /Matlab/simulation.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %% MPCC Simulation Script 14 | clear 15 | close all 16 | clc 17 | 18 | %% add spline library 19 | addpath('splines'); 20 | %% Load Parameters 21 | CarModel = 'ORCA'; 22 | % CarModel = 'FullSize'; 23 | 24 | MPC_vars = getMPC_vars(CarModel); 25 | ModelParams=getModelParams(MPC_vars.ModelNo); 26 | % choose optimization interface options: 'Yalmip','CVX','hpipm','quadprog' 27 | MPC_vars.interface = 'hpipm'; 28 | 29 | 30 | nx = ModelParams.nx; 31 | nu = ModelParams.nu; 32 | N = MPC_vars.N; 33 | Ts = MPC_vars.Ts; 34 | %% import an plot track 35 | % use normal ORCA Track 36 | load Tracks/track2.mat 37 | % use RCP track 38 | % load Tracks/trackMobil.mat 39 | % track2 = trackMobil; 40 | % shrink track by half of the car widht plus safety margin 41 | % TODO implement orientation depending shrinking in the MPC constraints 42 | safteyScaling = 1.5; 43 | [track,track2] = borderAdjustment(track2,ModelParams,safteyScaling); 44 | 45 | trackWidth = norm(track.inner(:,1)-track.outer(:,1)); 46 | % plot shrinked and not shrinked track 47 | figure(1); 48 | plot(track.outer(1,:),track.outer(2,:),'r') 49 | hold on 50 | plot(track.inner(1,:),track.inner(2,:),'r') 51 | plot(track2.outer(1,:),track2.outer(2,:),'k') 52 | plot(track2.inner(1,:),track2.inner(2,:),'k') 53 | %% Simulation lenght and plotting 54 | simN = 500; 55 | %0=no plots, 1=plot predictions 56 | plotOn = 1; 57 | %0=real time iteration, 1=fixed number of QP iterations, 2=fixed number of damped QP iterations 58 | QP_iter = 2; 59 | % number of cars 60 | % there are two examples one with no other cars and one with 4 other cars 61 | % (inspired by the set up shown in the paper) 62 | % n_cars = 1; % no other car 63 | n_cars = 5; % 4 other cars 64 | %% Fit spline to track 65 | % TODO spline function only works with regular spaced points. 66 | % Fix add function which given any center line and bound generates equlally 67 | % space tracks. 68 | [traj, borders] =splinify(track); 69 | tl = traj.ppy.breaks(end); 70 | 71 | % store all data in one struct 72 | TrackMPC = struct('traj',traj,'borders',borders,'track_center',track.center,'tl',tl); 73 | %% Define starting position 74 | startIdx = 1; %point (in terms of track centerline array) allong the track 75 | % where the car starts, on the center line, aligned with the track, driving 76 | % straight with vx0 77 | %since the used bicycle model is not well defined for slow velocities use vx0 > 0.5 78 | if CarModel == "ORCA" 79 | vx0 = 1; 80 | elseif CarModel == "FullSize" 81 | vx0 = 15; 82 | end 83 | 84 | % find theta that coresponds to the 10th point on the centerline 85 | [theta, ~] = findTheta([track.center(1,startIdx),track.center(2,startIdx)],track.center,traj.ppx.breaks,trackWidth,startIdx); 86 | 87 | x0 = [track.center(1,startIdx),track.center(2,startIdx),... % point on centerline 88 | atan2(ppval(traj.dppy,theta),ppval(traj.dppx,theta)),... % aligned with centerline 89 | vx0 ,0,0,theta]'; %driving straight with vx0, and correct theta progress 90 | 91 | % the find theta function performs a local search to find the projection of 92 | % the position onto the centerline, therefore, we use the start index as an 93 | % starting point for this local search 94 | last_closestIdx = startIdx; 95 | %% First initial guess 96 | x = repmat(x0,1,N+1); % all points identical to current measurment 97 | % first inital guess, all points on centerline aligned with centerline 98 | % spaced as the car would drive with vx0 99 | for i = 2:N+1 100 | theta_next = x(ModelParams.stateindex_theta,i-1)+Ts*vx0; 101 | phi_next = atan2(ppval(traj.dppy,theta_next),ppval(traj.dppx,theta_next)); 102 | % phi_next can jump by two pi, make sure there are no jumps in the 103 | % initial guess 104 | if (x(ModelParams.stateindex_phi,i-1)-phi_next) < -pi 105 | phi_next = phi_next-2*pi; 106 | end 107 | if (x(ModelParams.stateindex_phi,i-1)-phi_next) > pi 108 | phi_next = phi_next+2*pi; 109 | end 110 | x(:,i) = [ppval(traj.ppx,theta_next),ppval(traj.ppy,theta_next),... % point on centerline 111 | phi_next,... % aligned with centerline 112 | vx0 ,0,0,theta_next]'; %driving straight with vx0, and correct theta progress 113 | end 114 | 115 | u = zeros(3,N); % zero inputs 116 | uprev = zeros(3,1); % last input is zero 117 | %% Ohter cars 118 | Y = ObstacelsState(track,traj,trackWidth,n_cars); 119 | 120 | if size(Y,2) ~= n_cars-1 121 | error('n_cars and the number of obstacles in "Y" does not match') 122 | end 123 | %% Initialize logging arrays 124 | X_log = zeros(nx*(N+1),simN); 125 | U_log = zeros(3*N,simN); 126 | B_log = zeros(4*N,simN); 127 | qpTime_log = zeros(1,simN); 128 | %% initializtion 129 | % solve problem 5 times without applying input 130 | % inspiered by sequential quadratic programming (SQP) 131 | for i = 1:5 132 | % formulate MPCC problem and solve it 133 | Iter_damping = 0.5; % 0 no damping 134 | [x_up, u_up, b, exitflag,info] = optimizer_mpcc(TrackMPC,MPC_vars,ModelParams, n_cars, Y, x, u, x0, uprev); 135 | x = Iter_damping*x + (1-Iter_damping)*x_up; 136 | u = Iter_damping*u + (1-Iter_damping)*u_up; 137 | 138 | if plotOn == 1 139 | % plot predictions 140 | PlotPrediction(x,u,b,Y,track,track2,traj,MPC_vars,ModelParams) 141 | end 142 | end 143 | %% Simulation 144 | for i = 1: simN 145 | 146 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 147 | %%%% MPCC-Call %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 148 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 149 | 150 | % augment state and inputs by shifting previus optimal solution 151 | [x,u] = augState(x,u,x0,MPC_vars,ModelParams,tl); 152 | % formulate MPCC problem and solve it 153 | if QP_iter == 0 154 | [x, u, b, exitflag,info] = optimizer_mpcc(TrackMPC,MPC_vars,ModelParams, n_cars, Y, x, u, x0, uprev); 155 | qpTime_log(i) = info.QPtime; 156 | elseif QP_iter == 1 157 | % doing multiple "SQP" steps 158 | for k = 1:2 159 | [x, u, b, exitflag,info] = optimizer_mpcc(TrackMPC,MPC_vars,ModelParams, n_cars, Y, x, u, x0, uprev); 160 | qpTime_log(i) = qpTime_log(i) + info.QPtime; 161 | end 162 | elseif QP_iter == 2 163 | % doing multiple damped "SQP" steps 164 | for k = 1:2 165 | Iter_damping = 0.75; % 0 no damping 166 | [x_up, u_up, b, exitflag,info] = optimizer_mpcc(TrackMPC,MPC_vars,ModelParams, n_cars, Y, x, u, x0, uprev); 167 | x = Iter_damping*x + (1-Iter_damping)*x_up; 168 | u = Iter_damping*u + (1-Iter_damping)*u_up; 169 | qpTime_log(i) = qpTime_log(i) + info.QPtime; 170 | end 171 | else 172 | error('invalid QP_iter value') 173 | end 174 | 175 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 176 | %%%% simulate system %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 177 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 178 | 179 | x0 = SimTimeStep(x(:,1),u(:,1),Ts,ModelParams)'; 180 | x0 = unWrapX0(x0); 181 | [ theta, last_closestIdx] = findTheta(x0,track.center,traj.ppx.breaks,trackWidth,last_closestIdx); 182 | x0(ModelParams.stateindex_theta) = theta; 183 | uprev = u(:,1); 184 | 185 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 186 | %%%% plotting and logging %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 187 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 188 | 189 | if plotOn == 1 190 | PlotPrediction(x,u,b,Y,track,track2,traj,MPC_vars,ModelParams) 191 | end 192 | 193 | % log predictions and time 194 | X_log(:,i) = reshape(x,(N+1)*7,1); 195 | U_log(:,i) = reshape(u,(N)*3,1); 196 | B_log(:,i) = reshape(b,N*4,1); 197 | 198 | 199 | end 200 | 201 | 202 | PlotLog( X_log,U_log,Y,track,track2,simN,Ts) 203 | 204 | %% Generating Stats 205 | a = 1; 206 | for i=1:simN-1 207 | if X_log(ModelParams.stateindex_theta,i+1) - X_log(ModelParams.stateindex_theta,i) < -0.9*tl 208 | LapStart(a) = i; 209 | a = a+1; 210 | end 211 | end 212 | 213 | if length(LapStart) > 1 214 | LapTime = diff(LapStart)*Ts; 215 | else 216 | LapTime = NaN; 217 | end 218 | 219 | disp('------------------------------------') 220 | disp(['Lap Time(s): ',num2str(LapTime)]) 221 | disp('------------------------------------') 222 | disp(['Mean Computation Time: ',num2str(mean(qpTime_log))]) 223 | disp(['Max Computation Time: ',num2str(max(qpTime_log))]) 224 | disp(['Min Computation Time: ',num2str(min(qpTime_log))]) 225 | disp('------------------------------------') 226 | -------------------------------------------------------------------------------- /Matlab/splines/computeCenter.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ppcx,ppcy]=computeCenter(ppx,ppy,Xtr,Ytr,Tsample) 17 | % Compute the spline approximation of the center of the track. 18 | % 19 | % Works by subdividing every two intervals, splining that interval, and 20 | % defining a new spline point as the point closest to the splined 21 | % subinterval. 22 | % 23 | % Modified by Samuel Zhao Oct.18,2012 so borders is same length as traj 24 | % This function assumes circular end-condition for track. 25 | 26 | 27 | refine=50;%factor of refinement to compute correspondance 28 | nbreaks=numel(ppx.breaks); 29 | Xtrs=[Xtr(1:end) Xtr(end)]; % wrap-around 30 | Ytrs=[Ytr(1:end) Ytr(end)]; % wrap-around 31 | t=zeros(nbreaks,1); 32 | Xj=ppval(ppx,ppx.breaks); 33 | Yj=ppval(ppy,ppx.breaks); 34 | 35 | t(1)=ppx.breaks(1); %enforce start and end points are same 36 | t(end)=ppx.breaks(end); 37 | 38 | for i=2:nbreaks-1 39 | 40 | cX=Xtrs(i); 41 | cY=Ytrs(i); 42 | 43 | [~,b]=min((Xj-cX).^2+(Yj-cY).^2);%find closest point 44 | 45 | % Pnext is spline parameter of next track coordinate 46 | % Pprev is spline parameter of previous track coordinate 47 | if(b==nbreaks) 48 | Pnext=ppx.breaks(b); 49 | else 50 | Pnext=ppx.breaks(b+1); 51 | end 52 | if(b==1) 53 | Pprev=ppx.breaks(1); 54 | else 55 | Pprev=ppx.breaks(b-1); 56 | end 57 | Xref=ppval(ppx,linspace(Pprev,Pnext,refine)); 58 | Yref=ppval(ppy,linspace(Pprev,Pnext,refine)); 59 | [~,d]=min((Xref-cX).^2+(Yref-cY).^2);%find closest point 60 | % bla(i)=Pnext; 61 | index=Pprev+d*(Pnext-Pprev)/refine; 62 | t(i)=index; 63 | 64 | end 65 | 66 | ppcx=spline(t,Xtrs); 67 | ppcy=spline(t,Ytrs); 68 | end 69 | -------------------------------------------------------------------------------- /Matlab/splines/getSplineDerivatives.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [dppx]=getSplineDerivatives(ppx) 17 | %[dppx]=getSplineDerivatives(ppx) 18 | %Compute the descriptor of the derivative of a spline 19 | %ppx descriptor of the spline 20 | if(ppx.order<1) 21 | error('order of spline should be more than 1'); 22 | end 23 | dppx=ppx; 24 | 25 | dppx.coefs=ppx.coefs(:,1:end-1).*kron(ones(size(ppx.coefs,1),1),(size(ppx.coefs,2)-1):-1:1); 26 | dppx.order = ppx.order-1; 27 | end 28 | -------------------------------------------------------------------------------- /Matlab/splines/normalizedSplineInterp.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [nppx, nppy, err]=normalizedSplineInterp(X,Y,Tsample,circular) 17 | %[nppx, nppy, err]=normalizedSplineInterp(X,Y,Tsample) 18 | %return a quasi-arc-parametrized spline 19 | %Tsample is optional and default value is 1 20 | %if circular is set to 'y', the function add a point to close the 21 | %trajectory and ensure derivability at closure. 22 | %checked for cubic splines 23 | if(nargin<3) 24 | Tsample=1; 25 | end 26 | if(nargin<4) 27 | circular='n'; 28 | end 29 | if(nargout>2) 30 | [nppx nppy err]=splineInterp(X,Y,Tsample,circular); 31 | else 32 | [nppx nppy]=splineInterp(X,Y,Tsample,circular); 33 | end 34 | l=splinelength(nppx, nppy,nppx.breaks(1:(end-1)),nppx.breaks(2:end)); 35 | cl=cumsum(l); 36 | 37 | %vandermonde matrix 38 | h=@(x) [x.^3 x.^2 x ones(numel(x),1)]; 39 | div=h(1./l); 40 | 41 | nppx.breaks = [0 cl']; 42 | nppy.breaks = [0 cl']; 43 | nppx.coefs = nppx.coefs .* div; 44 | nppy.coefs = nppy.coefs .* div; 45 | end -------------------------------------------------------------------------------- /Matlab/splines/regularindex.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function ind=regularindex(ppx,ppy,number) 17 | umax=ppx.breaks(end); 18 | l=splinelength(ppx, ppy,ppx.breaks(1:(end-1)),ppx.breaks(2:end)); 19 | cl=cumsum(l)/sum(l); 20 | ind=zeros(number,1); 21 | ind(1:floor(number*cl(1)))=linspace(ppx.breaks(1),ppx.breaks(2),floor(number*cl(1))); 22 | for i=2:(length(l)) 23 | ind(floor(number*cl(i-1)):floor(number*cl(i)))=linspace(ppx.breaks(i),ppx.breaks(i+1),floor(number*cl(i))-floor(number*cl(i-1))+1); 24 | end 25 | 26 | end -------------------------------------------------------------------------------- /Matlab/splines/spline5.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [Xcoefs Ycoefs] = spline5(X,Y,L) 17 | %X Y are coordinates of points (1D), length(X)==length(Y) 18 | %L is the length of each spline. By default L=ones(size(X)) 19 | %Xcoefs and Ycoefs are matrices length(X) 20 | assert(length(X)==length(Y),'X and Y should be the same length'); 21 | 22 | 23 | end -------------------------------------------------------------------------------- /Matlab/splines/splineInterp.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [ppx ppy err]=splineInterp(X,Y,Tsample,circular) 17 | %[ppx ppy err]=splineInterp(X,Y,Tsample) 18 | %Compute a 2D spline using X and Y. Optionally compute the error made by sampling (zero if Ts=1) 19 | %Tsample has to be an integer 1<=Tsample<=legnth(X), by default Tsample=1 20 | %if circular is set to 'y', the function add a point to close the 21 | %trajectory and ensure derivability at closure. 22 | %X and Y and 1D data, length(X)=length(Y) 23 | %err represent the squared error due to sampling 24 | capprox=50;%number of part to evaluate the spline when calculating the error 25 | if(nargin<3) 26 | Tsample=1; 27 | end 28 | if(nargin<4) 29 | circular='n'; 30 | end 31 | 32 | t=0:(ceil(length(X)/Tsample)-1); 33 | s=1:Tsample:length(X); 34 | if(length(X)/Tsample~=floor(length(X)/Tsample)) 35 | s=[s length(X)]; 36 | t = [t t(end)+1]; 37 | end 38 | 39 | if(strcmp(circular,'y')==1) 40 | t=[t t(end)+1];%add a point to close the trajectory; 41 | %this is not perfect because the value of the derivative should be 42 | %included in the unknowns of linear solver used for spline calculation. 43 | theta=atan2(Y(1)-Y(end-1),X(1)-X(end-1)); 44 | r=0; 45 | Xs=[X(s) X(s(1))]; 46 | Ys=[Y(s) Y(s(1))]; 47 | else 48 | Xs=X(s); 49 | Ys=Y(s); 50 | end 51 | % figure(3);plot(Xs,Ys,'xr-'); 52 | ppx=spline(t,Xs); 53 | ppy=spline(t,Ys); 54 | 55 | if(nargout>2)%calculate the error 56 | err=0; 57 | if(Tsample==1) 58 | return 59 | end 60 | for i=t(1:(end-1))%for each piece 61 | x=ppval(ppx,linspace(i,i+1,capprox)); 62 | y=ppval(ppy,linspace(i,i+1,capprox)); 63 | for j=1:Tsample%for each point 64 | if(i*floor(length(X)/Tsample)+j>length(X)) 65 | break; 66 | end 67 | err= err + min((x-X(i*floor(length(X)/Tsample)+j)).^2+(y-Y(i*floor(length(Y)/Tsample)+j)).^2); 68 | end 69 | end 70 | err = err / length(X); 71 | end 72 | 73 | end 74 | -------------------------------------------------------------------------------- /Matlab/splines/splinelength.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function l=splinelength(ppx, ppy,umin,umax) 17 | %l=splinelength(ppx, ppy,umin,umax) 18 | %compute the length of a 2D spline between parameter umin and umax 19 | %ppx and ppy are spline parameter. See spline function for more details 20 | %umin and umax are vectors of the same length 21 | %and for all i, umin(i)<=umax(i) 22 | %checked 23 | assert(length(umin)==length(umax)); 24 | 25 | dppx=getSplineDerivatives(ppx); 26 | dppy=getSplineDerivatives(ppy); 27 | 28 | h = @(u) sqrt(ppval(dppx,u).^2+ppval(dppy,u).^2); 29 | l=zeros(length(umin),1); 30 | for i=1:length(umin) 31 | l(i)=quad(h,umin(i),umax(i)); 32 | end 33 | 34 | end 35 | -------------------------------------------------------------------------------- /Matlab/splines/splinify.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function [traj, borders] = splinify(track) 17 | %[traj, borders] = splinify(track) 18 | %calculate the required spline and derivatives 19 | 20 | Tr=track.center;%(track.outer+track.inner)/2; 21 | Xt=Tr(1,:); 22 | Yt=Tr(2,:); 23 | % Tr = track.center; 24 | % Xt = track.center(1,:); 25 | % Yt = track.center(2,:); 26 | [traj.ppx, traj.ppy err]=normalizedSplineInterp(Xt,Yt,1,'y'); 27 | %traj.ppx = spline(1:length(Xt),Xt); 28 | 29 | %calculate derivatives of desired trajectory 30 | traj.dppx=getSplineDerivatives(traj.ppx); 31 | traj.dppy=getSplineDerivatives(traj.ppy); 32 | traj.ddppx=getSplineDerivatives(traj.dppx); 33 | traj.ddppy=getSplineDerivatives(traj.dppy); 34 | 35 | %compute borders of track spline approximation 36 | [borders.pplx,borders.pply]=computeCenter(traj.ppx,traj.ppy,track.inner(1,:),track.inner(2,:),1); 37 | [borders.pprx,borders.ppry]=computeCenter(traj.ppx,traj.ppy,track.outer(1,:),track.outer(2,:),1); 38 | 39 | % [borders.pplx,borders.pply]=computeCenter(traj.ppx,traj.ppy,track.inner(1,:),track.inner(2,:),1); 40 | % [borders.pprx,borders.ppry]=computeCenter(traj.ppx,traj.ppy,track.outer(1,:),track.outer(2,:),1); 41 | 42 | 43 | borders.dpplx=getSplineDerivatives(borders.pplx); 44 | borders.dpply=getSplineDerivatives(borders.pply); 45 | borders.dpprx=getSplineDerivatives(borders.pprx); 46 | borders.dppry=getSplineDerivatives(borders.ppry); 47 | 48 | %compute center (for compatibility and tests) 49 | [borders.ppcx,borders.ppcy]=computeCenter(traj.ppx,traj.ppy,Tr(1,:),Tr(2,:),1); 50 | borders.dppcx=getSplineDerivatives(borders.ppcx); 51 | borders.dppcy=getSplineDerivatives(borders.ppcy); 52 | borders.trackwidth=0.37; 53 | end -------------------------------------------------------------------------------- /Matlab/unWrapX0.m: -------------------------------------------------------------------------------- 1 | % Copyright (C) 2018, ETH Zurich, D-ITET, Kenneth Kuchera, Alexander Liniger 2 | % Licensed under the Apache License, Version 2.0 (the "License"); 3 | % you may not use this file except in compliance with the License. 4 | % You may obtain a copy of the License at 5 | % 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | 16 | function x0 = unWrapX0(x0) 17 | 18 | if ( x0(3) > pi) 19 | x0(3) = x0(3) - 2*pi; 20 | end 21 | if( x0(3) <= -pi) 22 | x0(3) = x0(3) + 2*pi; 23 | end 24 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MPCC 2 | Simulation environments in C++ and Matlab of the Model Predictive Contouring Controller (MPCC) for Autonomous Racing developed by the Automatic Control Lab (IfA) at ETH Zurich 3 | 4 | ## Formulation 5 | 6 | The MPCC is a model predictive path following controller which does follow a predefined reference path X^ref and Y^ref. This is achieved by augmenting the system with an integrator stated theta which approximates the progress along the reference path. The theta state is coupled to the real dynamics using the lag error e^l which is penalized in the cost. Additionally, the contouring error (lateral error to the reference path) is also penalized in the cost function. Finally, the progress along the reference path is maximized to achieve that the car does follow the path as fast as possible and the rate of the inputs is penalized. To guarantee that the car stays within the track, track constraints are imposed as well as bounds on all states, inputs, and the rate of inputs. The resulting optimization problem is shown in the following equation: 7 | 8 | 9 | The vehicle dynamics considered is a bicycle model with nonlinear magic formula tire models: 10 | 11 | 12 | with the tire model and drive train model given as follows: 13 | 14 | 15 | Finally, the state and inputs of the problem are given as follows: 16 | 17 | 18 | Where (X,Y) is the global position phi the heading of the car, v_x, and v_y the longitudinal respectively the lateral velocity and omega the yaw rate. Theta is the augmented state which approximates the progress. The inputs are the duty cycle d to the drive train, the steering angle delta and the velocity along the reference path v_theta (which is an approximation of the velocities projected onto the path) 19 | 20 | To achieve obstacle avoidance of other cars, before solving the MPCC problem grid search 1-D dynamic programming approach finds the best way to avoid the obstacles. This path is then converted into a corridor which modifies the track constraint. 21 | 22 | Finally, to solve the MPCC problem, we approximate the nonlinear problem as a time-varying quadratic program, by linearizing and discretizing the dynamics, approximating the lag and contouring errors using a first order Taylor approximation and approximating the track constraints with two linear half space aligned with the boundaries. The resulting quadratic program is then solved using [hpipm](https://github.com/giaf/hpipm), however, the Matlab implementation also offers a Yalmip, CVX, and direct QuadProg interface. 23 | 24 | Note that the C++ implementation has some additional features proposed in the following paper [AMZ Driverless: The Full Autonomous Racing System](https://arxiv.org/abs/1905.05150). However, obstacle avoidance is currently only supported in the Matlab implementation. 25 | 26 | For more details see our paper [Optimization‐based autonomous racing of 1:43 scale RC cars](https://onlinelibrary.wiley.com/doi/abs/10.1002/oca.2123) or the [Arxiv](https://arxiv.org/abs/1711.07300) version. 27 | 28 | ## How to run 29 | See the individual instruction for the C++ and Matlab code in the respective folders. 30 | 31 | ## Example 32 | 33 | 34 | ## Papers 35 | If you use the C++ or Matlab code please cite 36 | - [Optimization‐based autonomous racing of 1:43 scale RC cars](https://onlinelibrary.wiley.com/doi/abs/10.1002/oca.2123) 37 | 38 | and if you use the C++ version please also 39 | - [AMZ Driverless: The Full Autonomous Racing System](https://arxiv.org/abs/1905.05150) 40 | 41 | ## Related Papers for the interested reader 42 | ### Optimization‐based autonomous racing of 1:43 scale RC cars 43 | Liniger, A., Domahidi, A. and Morari, M. 44 | Optimization‐based autonomous racing of 1: 43 scale RC cars. 45 | Optimal Control Applications and Methods, 2015, 36(5), pp.628-647. 46 | 47 | 48 | Explains the MPCC problem for autonomous racing and the obstacle avoidance approach, as described above. 49 | 50 | ### Efficient implementation of randomized MPC for miniature race cars 51 | Carrau, J.V., Liniger, A., Zhang, X. and Lygeros, J. 52 | Efficient implementation of randomized MPC for miniature race cars. 53 | 2016 European Control Conference (ECC). 54 | 55 | 56 | Shows how to implement an efficient sampling based randomized MPC approach using the MPCC approach. First, the track constraints can be reformulated as chance constraints and approximated using randomized MPC. We then show that the problem can be split in a constraint tightening pre-processing step and solving a deterministic MPC problem with tight-end constraints. The advantage of this two step approach is that it only causes an overhead in the order of few milliseconds. 57 | 58 | ### Racing miniature cars: Enhancing performance using stochastic MPC and disturbance feedback 59 | Liniger, A., Zhang, X., Aeschbach, P., Georghiou, A. and Lygeros, J. 60 | Racing miniature cars: Enhancing performance using stochastic MPC and disturbance feedback. 61 | 2017 American Control Conference (ACC). 62 | 63 | 64 | Similar to the previous paper, the MPCC is augmented with probabilistic track constraints, and we show that it is beneficial to optimize over the feedback policies used in stochastic MPC, using the disturbance feedback approach. We also show that by using only 3 samples, the approach can be implemented in real-time and that even when using so few samples, it helps on the experimental platform. 65 | 66 | ### Cautious NMPC with Gaussian process dynamics for autonomous miniature race cars 67 | Hewing, L., Liniger, A. and Zeilinger, M.N. 68 | Cautious NMPC with Gaussian process dynamics for autonomous miniature race cars. 69 | 2018 European Control Conference (ECC). 70 | 71 | 72 | This paper proposes to learn the model mismatch between the real car and the MPC model using Gaussian Processes (GP). The GP is then considered when solving the MPC problem which helps to improve the performance. Furthermore, is the uncertainty in the GP considered by using probabilistic track constraints. The approach shows good results in simulation when the simulation model had a severe model mismatch. 73 | 74 | ### Learning-based model predictive control for autonomous racing 75 | Kabzan, J., Hewing, L., Liniger, A. and Zeilinger, M.N. 76 | Learning-based model predictive control for autonomous racing. 77 | IEEE Robotics and Automation Letters, 2019, 4(4), pp.3363-3370. 78 | 79 | 80 | This paper uses the previous paper as a basis but implements the approach on the AMZ Formula Student car. Additionally, online learning is implemented using a dictionary that is automatically updated based on how informative a data point is for the GP. The approach was tested on the real Formula Student car and reduced the lap time by 10% once the GP dictionary had converged. 81 | 82 | ### AMZ Driverless: The Full Autonomous Racing System 83 | Kabzan, J., et. al. 84 | AMZ Driverless: The Full Autonomous Racing System. 85 | arXiv preprint arXiv:1905.05150, 2019. 86 | 87 | 88 | This paper explains the full system used in the AMZ Driverless car for the 2017 and 2018 seasons. This also includes the MPCC including new features added in the C++ implementation of this GitHub repo, mainly the tire constraints and side slip angle cost. However, the paper also proposes an interesting mixed kinematic-dynamic model, which allows to get the benefits of kinematic models at slow speeds and the accuracy of dynamic models at high speeds. 89 | 90 | --------------------------------------------------------------------------------