├── data └── dummy.txt ├── configuremingw.p ├── MATMPC_SIMULINK.slx ├── MATMPC_SIMULINK_R2017b.slx ├── model_src └── readme.txt ├── solver ├── linux │ ├── osqp │ │ └── osqp_mex.mexa64 │ └── qpoases │ │ ├── qpOASES.mexa64 │ │ ├── qpOASES_sequence.mexa64 │ │ ├── qpOASES.m │ │ ├── qpOASES_auxInput.m │ │ ├── qpOASES_sequence.m │ │ └── qpOASES_options.m ├── win64 │ ├── osqp │ │ └── osqp_mex.mexw64 │ └── qpoases │ │ ├── qpOASES.mexw64 │ │ ├── qpOASES_sequence.mexw64 │ │ ├── qpOASES.m │ │ └── qpOASES_auxInput.m └── mac │ ├── qpalm │ ├── qpalm_mex.mexmaca64 │ ├── qpalm_mex.mexmaci64 │ └── qpalm.m │ └── qpoases │ ├── qpOASES.mexmaca64 │ ├── qpOASES.mexmaci64 │ ├── qpOASES_sequence.mexmaca64 │ ├── qpOASES_sequence.mexmaci64 │ ├── qpOASES.m │ ├── qpOASES_auxInput.m │ └── qpOASES_sequence.m ├── nmpc ├── Pcond.m ├── mpc_qp_solve_quadprog.m ├── mpc_qp_solve_ipopt_dense.m ├── Pcond_hpipm.m ├── mpc_qp_solve_osqp.m ├── mpc_qp_solve_qore.m ├── mpc_qp_solve_qpalm_sparse.m ├── mpc_qp_solve_qpalm_cond.m ├── mpc_qp_solve_osqp_partial.m ├── mpc_qp_solve_qpoases.m ├── mpc_nmpcsolver_simulink.m ├── mpc_qp_solve_ipopt_sparse.m ├── mpc_qp_solve_qpoases_mb.m ├── mpc_qp_solve_ipopt_partial_sparse.m └── mpc_nmpcsolver.m ├── mex_core ├── erk.h ├── sim.h ├── mpc_common.h ├── irk_ode.h ├── sim.c ├── Condensing_Blasfeo.h ├── irk_dae.h ├── casadi_wrapper.h ├── partial_condensing_routines.h ├── Compile_Mex.m ├── compile_hpipm.m ├── Recover.c ├── full2sparse.c ├── mpc_common.c ├── sparse2full.c ├── Simulate_System.c ├── adaptive_eta.c ├── partial_condensing_default.c ├── erk.c └── Condensing.c ├── examples ├── InvertedPendulum.m ├── ChainofMasses_Lin.m ├── ChainofMasses_NLin.m ├── TethUAV.m └── TurboEngine.m ├── doc ├── HPIPM-Tutorial.txt └── ref.bib ├── README.md ├── Initialization_Simulink.m ├── CMoN_Init.m ├── Simulation.m ├── InitData_ngrid.m └── Model_Generation.m /data/dummy.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /configuremingw.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/configuremingw.p -------------------------------------------------------------------------------- /MATMPC_SIMULINK.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/MATMPC_SIMULINK.slx -------------------------------------------------------------------------------- /MATMPC_SIMULINK_R2017b.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/MATMPC_SIMULINK_R2017b.slx -------------------------------------------------------------------------------- /model_src/readme.txt: -------------------------------------------------------------------------------- 1 | This folder contains part of the model src files generated by casadi during code_generation -------------------------------------------------------------------------------- /solver/linux/osqp/osqp_mex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/linux/osqp/osqp_mex.mexa64 -------------------------------------------------------------------------------- /solver/win64/osqp/osqp_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/win64/osqp/osqp_mex.mexw64 -------------------------------------------------------------------------------- /solver/linux/qpoases/qpOASES.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/linux/qpoases/qpOASES.mexa64 -------------------------------------------------------------------------------- /solver/mac/qpalm/qpalm_mex.mexmaca64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/mac/qpalm/qpalm_mex.mexmaca64 -------------------------------------------------------------------------------- /solver/mac/qpalm/qpalm_mex.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/mac/qpalm/qpalm_mex.mexmaci64 -------------------------------------------------------------------------------- /solver/mac/qpoases/qpOASES.mexmaca64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/mac/qpoases/qpOASES.mexmaca64 -------------------------------------------------------------------------------- /solver/mac/qpoases/qpOASES.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/mac/qpoases/qpOASES.mexmaci64 -------------------------------------------------------------------------------- /solver/win64/qpoases/qpOASES.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/win64/qpoases/qpOASES.mexw64 -------------------------------------------------------------------------------- /solver/linux/qpoases/qpOASES_sequence.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/linux/qpoases/qpOASES_sequence.mexa64 -------------------------------------------------------------------------------- /solver/win64/qpoases/qpOASES_sequence.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/win64/qpoases/qpOASES_sequence.mexw64 -------------------------------------------------------------------------------- /solver/mac/qpoases/qpOASES_sequence.mexmaca64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/mac/qpoases/qpOASES_sequence.mexmaca64 -------------------------------------------------------------------------------- /solver/mac/qpoases/qpOASES_sequence.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pi-cos/MATMPC/HEAD/solver/mac/qpoases/qpOASES_sequence.mexmaci64 -------------------------------------------------------------------------------- /nmpc/Pcond.m: -------------------------------------------------------------------------------- 1 | function [mem2] = Pcond(mem, settings, mem2, settings2) 2 | 3 | [Hp,gp,Ccx,Ap,Bp,ap,lxc,uxc,Ccg,lgc,ugc]=partial_condensing_default(mem, settings); 4 | 5 | sparse2full(mem2, mem, settings2, settings, Hp, gp, Ccx, Ccg, lxc, uxc, lgc, ugc); 6 | 7 | mem2.A=Ap; 8 | mem2.B=Bp; 9 | mem2.a=ap; 10 | mem2.lb_du=mem.lb_du; 11 | mem2.ub_du=mem.ub_du; 12 | mem2.ds0=mem.ds0; 13 | mem2.CgN=mem.CgN; 14 | mem2.Cx=mem.Cx; 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /mex_core/erk.h: -------------------------------------------------------------------------------- 1 | #ifndef ERK_H_ 2 | #define ERK_H_ 3 | 4 | #include "mex.h" 5 | #include "sim.h" 6 | 7 | typedef struct{ 8 | double *A; 9 | double *B; 10 | double *Sx; 11 | double *Su; 12 | 13 | double *xt; 14 | double *K; 15 | double *dKx; 16 | double *dKu; 17 | double *jacX_t; 18 | double *jacU_t; 19 | double *X_traj; 20 | double *K_lambda; 21 | double *lambda_t; 22 | }sim_erk_workspace; 23 | 24 | sim_erk_workspace* sim_erk_workspace_create(sim_opts *opts); 25 | 26 | void sim_erk_workspace_init(sim_opts *opts, const mxArray *mem, sim_erk_workspace *work); 27 | 28 | int sim_erk(sim_in *in, sim_out *out, sim_opts *opts, sim_erk_workspace *work); 29 | 30 | void sim_erk_workspace_free(sim_opts *opts, sim_erk_workspace *work); 31 | 32 | #endif -------------------------------------------------------------------------------- /mex_core/sim.h: -------------------------------------------------------------------------------- 1 | #ifndef SIM_H_ 2 | #define SIM_H_ 3 | 4 | #include "mex.h" 5 | 6 | typedef struct{ 7 | double h; 8 | size_t nx; 9 | size_t nu; 10 | size_t nz; 11 | size_t num_stages; 12 | size_t num_steps; 13 | bool forw_sens_flag; 14 | bool adj_sens_flag; 15 | }sim_opts; 16 | 17 | typedef struct{ 18 | double *x; 19 | double *u; 20 | double *p; 21 | double *z; 22 | double *lambda; 23 | }sim_in; 24 | 25 | typedef struct{ 26 | double *xn; 27 | double *zn; 28 | double *Sx; 29 | double *Su; 30 | double *adj_sens; 31 | }sim_out; 32 | 33 | sim_opts* sim_opts_create(const mxArray *mem); 34 | sim_in* sim_in_create(sim_opts *opts); 35 | sim_out* sim_out_create(sim_opts *opts); 36 | 37 | void sim_opts_free(sim_opts *opts); 38 | void sim_in_free(sim_in *in); 39 | void sim_out_free(sim_out *out); 40 | 41 | #endif -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_quadprog.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_quadprog(sizes,mem) 2 | 3 | nc=sizes.nc; 4 | ncN=sizes.ncN; 5 | N=sizes.N; 6 | nbx=sizes.nbx; 7 | 8 | warning off; 9 | 10 | tqp = tic; 11 | [mem.du,fval,exitflag,output,lambda] = quadprog(mem.Hc,mem.gc,[mem.Ccx;mem.Ccg;-mem.Ccx;-mem.Ccg],... 12 | [mem.uxc;mem.ucc;-mem.lxc;-mem.lcc],[],[],mem.lb_du, mem.ub_du,[],mem.quadprog_opt); 13 | 14 | mem.mu_u_new = lambda.upper - lambda.lower; 15 | mem.mu_x_new = lambda.ineqlin(1:N*nbx) - lambda.ineqlin(N*nbx+N*nc+ncN+1:N*nbx+N*nc+ncN+N*nbx); 16 | mem.mu_new = lambda.ineqlin(N*nbx+1:N*nbx+N*nc+ncN) - lambda.ineqlin(N*nbx+N*nc+ncN+N*nbx+1:end); 17 | cpt_qp = toc(tqp)*1e3; 18 | 19 | Recover(mem, sizes); 20 | end 21 | -------------------------------------------------------------------------------- /mex_core/mpc_common.h: -------------------------------------------------------------------------------- 1 | #ifndef MPC_COMMON_H_ 2 | #define MPC_COMMON_H_ 3 | 4 | #include "stdlib.h" 5 | 6 | void Block_Fill(size_t m, size_t n, double *Gi, double *G, 7 | size_t idm, size_t idn, size_t ldG); 8 | 9 | void Block_Fill_Trans(size_t m, size_t n, double *Gi, double *G, 10 | size_t idm, size_t idn, size_t ldG); 11 | 12 | void Block_Get(size_t m, size_t n, double *Gi, double *G, size_t idm, size_t idn, size_t ldG); 13 | 14 | void set_zeros(size_t dim, double *A); 15 | 16 | void print_matrix(double *A, size_t m, size_t n); 17 | 18 | void print_vector(double *x, size_t m); 19 | 20 | void regularization(size_t n, double *A, double reg); 21 | 22 | // void product_mb(double *P,double *M, size_t a, double *index, size_t r,size_t nu); // P = M*T, M is (a x (N*nu)) 23 | // 24 | // void product_mb_Trans(double *P,double *M, size_t b, double *index, size_t r,size_t nu); // P = T'* M, M is (N*nu x b) 25 | 26 | #endif -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_ipopt_dense.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_ipopt_dense(sizes,mem) 2 | N=sizes.N; 3 | nu=sizes.nu; 4 | nbx=sizes.nbx; 5 | 6 | H = sparse(mem.Hc); 7 | g = mem.gc; 8 | 9 | mem.ipopt.funcs.hessian = @(x,sigma,lambda) tril(sigma*H); 10 | mem.ipopt.funcs.hessianstructure = @() tril(H); 11 | mem.ipopt.funcs.objective=@(x) 0.5*x'*H*x + g'*x; 12 | mem.ipopt.funcs.gradient = @(x) (H*x + g)'; 13 | mem.ipopt.options.A= sparse([mem.Ccx;mem.Ccg]); 14 | mem.ipopt.options.ru= [mem.uxc;mem.ucc]; 15 | mem.ipopt.options.rl= [mem.lxc;mem.lcc]; 16 | mem.ipopt.options.ub = mem.ub_du; 17 | mem.ipopt.options.lb = mem.lb_du; 18 | [mem.du,fval,exitflag,info_ipopt] = opti_ipopt(mem.ipopt,[]); 19 | 20 | mem.mu_u_new = info_ipopt.Lambda.upper-info_ipopt.Lambda.lower; 21 | mem.mu_x_new = info_ipopt.Lambda.ineqlin(1:N*nbx); 22 | mem.mu_new = info_ipopt.Lambda.ineqlin(N*nbx+1:end); 23 | cpt_qp = info_ipopt.Time*1e3; 24 | 25 | Recover(mem, sizes); 26 | 27 | end 28 | 29 | -------------------------------------------------------------------------------- /nmpc/Pcond_hpipm.m: -------------------------------------------------------------------------------- 1 | function [mem2] = Pcond_hpipm(mem, settings, mem2, settings2) 2 | 3 | [Qp,Sp,Rp,qp,rp,Ap,Bp,bp,ubxp,lbxp,ubup,lbup,Cp,Dp,CNp,ugp,lgp,ds0p]=Partial_Condensing(mem,settings); 4 | 5 | mem2.gx=qp; 6 | mem2.gu=rp; 7 | mem2.A=Ap; 8 | mem2.B=Bp; 9 | mem2.a=bp; 10 | mem2.lb_dx=lbxp; 11 | mem2.ub_dx=ubxp; 12 | mem2.lb_du=lbup; 13 | mem2.ub_du=ubup; 14 | mem2.Cgx=Cp; 15 | mem2.Cgu=Dp; 16 | mem2.CgN=CNp; 17 | mem2.lc=lgp; 18 | mem2.uc=ugp; 19 | mem2.ds0=ds0p; 20 | 21 | for i=0:settings2.N-1 22 | mem2.Q(:,i*settings2.nx+1:(i+1)*settings2.nx) = Qp(:,i*settings2.nx+1:(i+1)*settings2.nx); 23 | 24 | mem2.R(:,i*settings2.nu+1:(i+1)*settings2.nu) = Rp(:,i*settings2.nu+1:(i+1)*settings2.nu); 25 | 26 | end 27 | mem2.Q(:,settings2.N*settings2.nx+1:(settings2.N+1)*settings2.nx) = Qp(:,settings2.N*settings2.nx+1:(settings2.N+1)*settings2.nx); 28 | 29 | for i=0:settings2.N-1 30 | mem2.S(:,i*settings2.nu+1:(i+1)*settings2.nu)=(Sp(:,i*settings2.nx+1:(i+1)*settings2.nx))'; 31 | end 32 | end 33 | 34 | -------------------------------------------------------------------------------- /mex_core/irk_ode.h: -------------------------------------------------------------------------------- 1 | #ifndef IRK_ODE_H_ 2 | #define IRK_ODE_H_ 3 | 4 | #include "mex.h" 5 | #include "sim.h" 6 | 7 | typedef struct{ 8 | double *A; 9 | double *B; 10 | double *Sx; 11 | double *Su; 12 | double *xt; 13 | double *K; 14 | double *rG; 15 | double *JGK; 16 | double *JGx; 17 | double *JGu; 18 | double *JKx; 19 | double *JKu; 20 | double *JFK; 21 | double *JGK_fact_traj; 22 | double *JGx_traj; 23 | double *JGu_traj; 24 | double *lambda_K; 25 | double **impl_ode_in; 26 | double **res_out; 27 | double **jac_x_out; 28 | double **jac_u_out; 29 | double **jac_xdot_out; 30 | size_t *IPIV; 31 | size_t *IPIV_traj; 32 | int newton_iter; 33 | double *tmp_nx_nx; 34 | }sim_irk_ode_workspace; 35 | 36 | sim_irk_ode_workspace* sim_irk_ode_workspace_create(sim_opts *opts); 37 | 38 | void sim_irk_ode_workspace_init(sim_opts *opts, const mxArray *mem, sim_irk_ode_workspace *work); 39 | 40 | int sim_irk_ode(sim_in *in, sim_out *out, sim_opts *opts, sim_irk_ode_workspace *work); 41 | 42 | void sim_irk_ode_workspace_free(sim_opts *opts, sim_irk_ode_workspace *work); 43 | 44 | #endif -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_osqp.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_osqp(sizes,mem) 2 | 3 | N=sizes.N; 4 | nbx=sizes.nbx; 5 | nx=sizes.nx; 6 | nu=sizes.nu; 7 | nc=sizes.nc; 8 | ncN=sizes.ncN; 9 | neq = (N+1)*nx; 10 | nw = (N+1)*nx+N*nu; 11 | nineq = N*nc+ncN; 12 | 13 | full2sparse(mem,sizes); 14 | A = [mem.sparse_dG; mem.sparse_dBu; mem.sparse_dBx; mem.sparse_dB]; 15 | l = [-mem.sparse_G; mem.sparse_lb]; 16 | u = [-mem.sparse_G; mem.sparse_ub]; 17 | 18 | v_H = mem.sparse_H(mem.sparse_H_idx); 19 | v_A = A(mem.sparse_A_idx); 20 | mem.qp_obj.update('Px',v_H,'Ax',v_A,'q',mem.sparse_g,'l',l,'u',u); 21 | sol = mem.qp_obj.solve(); 22 | 23 | assert(strcmp(sol.info.status,'solved'), ['QP Error: ' sol.info.status]); 24 | 25 | mem.dx(:) = reshape(sol.x(1:neq,1),[nx,N+1]); 26 | mem.du(:) = reshape(sol.x(neq+1:nw,1),[nu,N]); 27 | 28 | mem.lambda_new(:) = reshape(sol.y(1:neq,1),[nx,N+1]); 29 | mem.mu_u_new(:) = sol.y(neq+1:neq+N*nu,1); 30 | mem.mu_x_new(:) = sol.y(neq+N*nu+1:neq+N*nu+N*nbx,1); 31 | mem.mu_new(:) = sol.y(neq+N*nu+N*nbx+1:end,1); 32 | 33 | cpt_qp = sol.info.run_time*1e3; 34 | 35 | end 36 | -------------------------------------------------------------------------------- /mex_core/sim.c: -------------------------------------------------------------------------------- 1 | #include "mex.h" 2 | #include "sim.h" 3 | 4 | 5 | sim_opts* sim_opts_create(const mxArray *mem) 6 | { 7 | sim_opts* opts = (sim_opts*)mxMalloc(sizeof(sim_opts)); 8 | mexMakeMemoryPersistent(opts); 9 | 10 | opts->nx = mxGetScalar( mxGetField(mem, 0, "nx") ); 11 | opts->nu = mxGetScalar( mxGetField(mem, 0, "nu") ); 12 | opts->nz = mxGetScalar( mxGetField(mem, 0, "nz") ); 13 | opts->num_stages = mxGetScalar( mxGetField(mem, 0, "num_stages") ); 14 | opts->num_steps = mxGetScalar( mxGetField(mem, 0, "num_steps") ); 15 | opts->h = mxGetScalar( mxGetField(mem, 0, "h") ); 16 | 17 | return opts; 18 | } 19 | 20 | sim_in* sim_in_create(sim_opts *opts) 21 | { 22 | sim_in* in = (sim_in*)mxMalloc(sizeof(sim_in)); 23 | mexMakeMemoryPersistent(in); 24 | 25 | return in; 26 | } 27 | 28 | sim_out* sim_out_create(sim_opts *opts) 29 | { 30 | sim_out* out = (sim_out*)mxMalloc(sizeof(sim_out)); 31 | mexMakeMemoryPersistent(out); 32 | 33 | return out; 34 | } 35 | 36 | void sim_opts_free(sim_opts *opts) 37 | { 38 | mxFree(opts); 39 | } 40 | 41 | void sim_in_free(sim_in *in) 42 | { 43 | mxFree(in); 44 | } 45 | 46 | void sim_out_free(sim_out *out) 47 | { 48 | mxFree(out); 49 | } -------------------------------------------------------------------------------- /mex_core/Condensing_Blasfeo.h: -------------------------------------------------------------------------------- 1 | #ifndef CONDENSING_BLASFEO_H_ 2 | #define CONDENSING_BLASFEO_H_ 3 | 4 | typedef struct 5 | { 6 | 7 | struct blasfeo_dmat *StQ; 8 | struct blasfeo_dmat *BtAt; 9 | struct blasfeo_dmat *HtWt; 10 | struct blasfeo_dmat *Gt; 11 | struct blasfeo_dmat *Rt; 12 | struct blasfeo_dmat *Cgx_dmat; 13 | struct blasfeo_dmat *CgN_dmat; 14 | struct blasfeo_dmat *Cx_dmat; 15 | struct blasfeo_dmat *Hc_dmat; 16 | struct blasfeo_dmat *Ccg_dmat; 17 | struct blasfeo_dmat *Ccx_dmat; 18 | 19 | struct blasfeo_dvec *rq; 20 | struct blasfeo_dvec *a_dvec; 21 | struct blasfeo_dvec *L; 22 | struct blasfeo_dvec *gcw; 23 | struct blasfeo_dvec *bg; 24 | struct blasfeo_dvec *bx; 25 | 26 | } Condensing_Blasfeo_workspace; 27 | 28 | int Condensing_Blasfeo_workspace_calculate_size(int nx, int nu, int N, int nc, int ncN, int nbx); 29 | 30 | Condensing_Blasfeo_workspace* Condensing_Blasfeo_workspace_cast(int nx, int nu, int N, int nc, int ncN, int nbx, void *raw_memory); 31 | 32 | int align_char_to(int num, char **c_ptr); 33 | 34 | void assign_blasfeo_dmat_mem(int m, int n, struct blasfeo_dmat *sA, char **ptr); 35 | 36 | void assign_blasfeo_dvec_mem(int n, struct blasfeo_dvec *sv, char **ptr); 37 | 38 | #endif -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_qore.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_qore(sizes,mem) 2 | 3 | nu=sizes.nu; 4 | nc=sizes.nc; 5 | ncN=sizes.ncN; 6 | nbx=sizes.nbx; 7 | N=sizes.N; 8 | 9 | if mem.qore_id==-1 10 | [err, mem.qore_id] = QPDenseNew(N*nu, N*nbx+N*nc+ncN); 11 | QPDenseSetInt(mem.qore_id, 'prtfreq', -1); 12 | QPDenseSetData(mem.qore_id, [mem.Ccx;mem.Ccg]', mem.Hc); 13 | 14 | t1 = tic; 15 | QPDenseOptimize(mem.qore_id, [mem.lb_du;mem.lxc;mem.lcc], [mem.ub_du;mem.uxc;mem.ucc], mem.gc); 16 | cpt_qp = toc(t1)*1e3; 17 | else 18 | QPDenseUpdateMatrices(mem.qore_id, [mem.Ccx;mem.Ccg]', mem.Hc); 19 | 20 | t1 = tic; 21 | QPDenseOptimize(mem.qore_id, [mem.lb_du;mem.lxc;mem.lcc], [mem.ub_du;mem.uxc;mem.ucc], mem.gc); 22 | cpt_qp = toc(t1)*1e3; 23 | end 24 | 25 | pri_sol = QPDenseGetDblVector(mem.qore_id, 'primalsol'); 26 | dual_sol = QPDenseGetDblVector(mem.qore_id, 'dualsol'); 27 | 28 | mem.du = reshape(pri_sol(1:N*nu),[nu N]); 29 | mem.mu_u_new = -dual_sol(1:N*nu); 30 | mem.mu_x_new = -dual_sol(N*nu+1:N*nu+N*nbx); 31 | mem.mu_new = -dual_sol(N*nu+N*nbx+1:end); 32 | 33 | Recover(mem, sizes); 34 | end 35 | -------------------------------------------------------------------------------- /mex_core/irk_dae.h: -------------------------------------------------------------------------------- 1 | #ifndef IRK_DAE_H_ 2 | #define IRK_DAE_H_ 3 | 4 | #include "mex.h" 5 | #include "sim.h" 6 | 7 | typedef struct{ 8 | double *A; 9 | double *B; 10 | double *Sx; 11 | double *Su; 12 | double *xt; 13 | double *K; 14 | double *rG; 15 | double *JGK; 16 | double *JGx; 17 | double *JGu; 18 | double *JKx; 19 | double *JKu; 20 | double *JFK; 21 | double *JGK_fact_traj; 22 | double *JGx_traj; 23 | double *JGu_traj; 24 | double *lambda_K; 25 | double **impl_dae_in; 26 | double **res_out; 27 | double **jac_f_x_out; 28 | double **jac_f_u_out; 29 | double **jac_f_xdot_out; 30 | double **jac_f_z_out; 31 | double **jac_g_x_out; 32 | double **jac_g_u_out; 33 | double **jac_g_z_out; 34 | size_t *IPIV; 35 | size_t *IPIV_traj; 36 | int newton_iter; 37 | double *tmp_nx_nx; 38 | double *tmp_nx_nz; 39 | double *tmp_block; 40 | }sim_irk_dae_workspace; 41 | 42 | sim_irk_dae_workspace* sim_irk_dae_workspace_create(sim_opts *opts); 43 | 44 | void sim_irk_dae_workspace_init(sim_opts *opts, const mxArray *mem, sim_irk_dae_workspace *work); 45 | 46 | int sim_irk_dae(sim_in *in, sim_out *out, sim_opts *opts, sim_irk_dae_workspace *work); 47 | 48 | void sim_irk_dae_workspace_free(sim_opts *opts, sim_irk_dae_workspace *work); 49 | 50 | #endif -------------------------------------------------------------------------------- /mex_core/casadi_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef CASADI_WRAPPER_H_ 2 | #define CASADI_WRAPPER_H_ 3 | 4 | void f_Fun(double **in, double **out); 5 | void g_Fun(double **in, double **out); 6 | void vde_Fun(double **in, double **out); 7 | void adj_ERK_Fun(double **in, double **out); 8 | void impl_f_Fun(double **in, double **out); 9 | void impl_jac_f_x_Fun(double **in, double **out); 10 | void impl_jac_f_u_Fun(double **in, double **out); 11 | void impl_jac_f_xdot_Fun(double **in, double **out); 12 | void impl_jac_f_z_Fun(double **in, double **out); 13 | void impl_jac_g_x_Fun(double **in, double **out); 14 | void impl_jac_g_u_Fun(double **in, double **out); 15 | void impl_jac_g_z_Fun(double **in, double **out); 16 | // void F_Fun(double **in, double **out); 17 | // void D_Fun(double **in, double **out); 18 | void Ji_Fun(double **in, double **out); 19 | void Hi_Fun(double **in, double **out); 20 | void gi_Fun(double **in, double **out); 21 | void path_con_Fun(double **in, double **out); 22 | void Ci_Fun(double **in, double **out); 23 | void JN_Fun(double **in, double **out); 24 | void HN_Fun(double **in, double **out); 25 | void gN_Fun(double **in, double **out); 26 | void path_con_N_Fun(double **in, double **out); 27 | void CN_Fun(double **in, double **out); 28 | void adj_Fun(double **in, double **out); 29 | void adjN_Fun(double **in, double **out); 30 | // void adj_dG_Fun(double **in, double **out); 31 | void obji_Fun(double **in, double **out); 32 | void objN_Fun(double **in, double **out); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_qpalm_sparse.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_qpalm_sparse(sizes,mem) 2 | 3 | N=sizes.N; 4 | nbx=sizes.nbx; 5 | nx=sizes.nx; 6 | nu=sizes.nu; 7 | nc=sizes.nc; 8 | ncN=sizes.ncN; 9 | neq = (N+1)*nx; 10 | nw = (N+1)*nx+N*nu; 11 | nineq = N*nc+ncN; 12 | 13 | full2sparse(mem,sizes); 14 | A = [mem.sparse_dG; mem.sparse_dBu; mem.sparse_dBx; mem.sparse_dB]; 15 | l = [-mem.sparse_G; mem.sparse_lb]; 16 | u = [-mem.sparse_G; mem.sparse_ub]; 17 | 18 | mem.qpalm_solver=qpalm; 19 | mem.qpalm_settings=mem.qpalm_solver.default_settings(); 20 | 21 | mem.qpalm_solver.setup(mem.sparse_H,mem.sparse_g, A, l, u, ... 22 | [reshape(mem.dx,[(N+1)*nx,1]);reshape(mem.du,[N*nu,1])], [reshape(mem.lambda_new,[neq,1]);mem.mu_u_new;mem.mu_x_new;mem.mu_new], mem.qpalm_settings); 23 | S = 'mem.qpalm_solver.solve()'; 24 | [T,sol] = evalc(S); 25 | 26 | assert(strcmp(sol.info.status,'solved'), ['QP Error: ' sol.info.status]); 27 | 28 | mem.dx(:) = reshape(sol.x(1:neq,1),[nx,N+1]); 29 | mem.du(:) = reshape(sol.x(neq+1:nw,1),[nu,N]); 30 | 31 | mem.lambda_new(:) = reshape(sol.y(1:neq,1),[nx,N+1]); 32 | mem.mu_u_new(:) = sol.y(neq+1:neq+N*nu,1); 33 | mem.mu_x_new(:) = sol.y(neq+N*nu+1:neq+N*nu+N*nbx,1); 34 | mem.mu_new(:) = sol.y(neq+N*nu+N*nbx+1:end,1); 35 | 36 | cpt_qp = sol.info.run_time*1e3; 37 | 38 | mem.qpalm_solver.delete(); 39 | 40 | end 41 | -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_qpalm_cond.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_qpalm_cond(sizes,mem) 2 | 3 | nu=sizes.nu; 4 | N=sizes.N; 5 | nbx=sizes.nbx; 6 | 7 | % if mem.qpalm_settings.warm_start==0 8 | % mem.qpalm_solver.setup(mem.Hc,mem.gc, [eye(N*nu);mem.Ccx;mem.Ccg], [mem.lb_du;mem.lxc;mem.lcc],[mem.ub_du;mem.uxc;mem.ucc], mem.qpalm_settings); 9 | % sol = mem.qpalm_solver.solve(); 10 | % mem.qpalm_settings.warm_start=1; 11 | % else 12 | % mem.qpalm_solver.setup(mem.Hc,mem.gc, [eye(N*nu);mem.Ccx;mem.Ccg], [mem.lb_du;mem.lxc;mem.lcc],[mem.ub_du;mem.uxc;mem.ucc], ... 13 | % mem.du, [mem.mu_u_new;mem.mu_x_new;mem.mu_new], mem.qpalm_settings); 14 | % sol = mem.qpalm_solver.solve(); 15 | % end 16 | mem.qpalm_solver=qpalm; 17 | mem.qpalm_settings=mem.qpalm_solver.default_settings(); 18 | 19 | mem.qpalm_solver.setup(mem.Hc,mem.gc, [eye(N*nu);mem.Ccx;mem.Ccg], [mem.lb_du;mem.lxc;mem.lcc],[mem.ub_du;mem.uxc;mem.ucc], ... 20 | mem.du, [mem.mu_u_new;mem.mu_x_new;mem.mu_new], mem.qpalm_settings); 21 | S = 'mem.qpalm_solver.solve()'; 22 | [T,sol] = evalc(S); 23 | 24 | assert(strcmp(sol.info.status,'solved'), ['QP Error: ' sol.info.status]); 25 | 26 | mem.du(:) = reshape(sol.x, [nu,N]); 27 | mem.mu_u_new(:) = sol.y(1:N*nu); 28 | mem.mu_x_new(:) = sol.y(N*nu+1:N*nu+N*nbx); 29 | mem.mu_new(:) = sol.y(N*nu+N*nbx+1:end); 30 | cpt_qp = sol.info.run_time*1e3; 31 | 32 | Recover(mem, sizes); 33 | 34 | mem.qpalm_solver.delete(); 35 | end 36 | -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_osqp_partial.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_osqp_partial(settings,settings2,mem, mem2) 2 | 3 | N=settings2.N; 4 | Nc=settings2.Nc; 5 | nbx=settings2.nbx; 6 | nbx_idx=settings2.nbx_idx; 7 | nx=settings2.nx; 8 | nu=settings2.nu; 9 | nc=settings2.nc; 10 | ncN=settings2.ncN; 11 | neq = (N+1)*nx; 12 | nw = (N+1)*nx+N*nu; 13 | 14 | full2sparse(mem2,settings2); 15 | A = [mem2.sparse_dG; mem2.sparse_dBu; mem2.sparse_dBx; mem2.sparse_dB]; 16 | l = [-mem2.sparse_G; mem2.sparse_lb]; 17 | u = [-mem2.sparse_G; mem2.sparse_ub]; 18 | 19 | v_H = mem.mem2.sparse_H(mem.mem2.sparse_H_idx); 20 | v_A = A(mem.mem2.sparse_A_idx); 21 | mem.mem2.qp_obj.update('Px',v_H,'Ax',v_A,'q',mem.mem2.sparse_g,'l',l,'u',u); 22 | sol = mem.mem2.qp_obj.solve(); 23 | 24 | assert(strcmp(sol.info.status,'solved'), ['QP Error: QP is ' sol.info.status]); 25 | 26 | mem.du(:) = reshape(sol.x(neq+1:nw,1),[settings.nu,settings.N]); 27 | 28 | mem.mu_u_new(:) = sol.y(neq+1:neq+N*nu,1); 29 | mu_x = sol.y(neq+N*nu+1:neq+N*nu+N*nbx,1); 30 | mu = sol.y(neq+N*nu+N*nbx+1:end,1); 31 | 32 | for i=0:N-1 33 | mem.mu_x_new(i*Nc*settings.nbx+1:(i+1)*Nc*settings.nbx-settings.nbx) = mu(i*nc+1:i*nc+(Nc-1)*nbx); 34 | mem.mu_x_new((i+1)*Nc*settings.nbx-settings.nbx+1:(i+1)*Nc*settings.nbx) = mu_x(i*settings.nbx+1:(i+1)*settings.nbx); 35 | mem.mu_new(i*Nc*settings.nc+1:(i+1)*Nc*settings.nc) = mu(i*nc+(Nc-1)*nbx+1:(i+1)*nc); 36 | end 37 | 38 | cpt_qp = sol.info.run_time*1e3; 39 | 40 | Recover(mem, settings); 41 | end 42 | -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_qpoases.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_qpoases(sizes,mem) 2 | 3 | nu=sizes.nu; 4 | N=sizes.N; 5 | nbx=sizes.nbx; 6 | 7 | if mem.warm_start==0 8 | [mem.warm_start,sol,fval,exitflag,iterations,multiplier,auxOutput] = qpOASES_sequence('i',mem.Hc,mem.gc,[mem.Ccx;mem.Ccg],... 9 | mem.lb_du,mem.ub_du,[mem.lxc;mem.lcc],[mem.uxc;mem.ucc],mem.qpoases_opt); 10 | else 11 | if mem.hot_start==0 12 | [sol,fval,exitflag,iterations,multiplier,auxOutput] = qpOASES_sequence('m',mem.warm_start,mem.Hc,mem.gc,... 13 | [mem.Ccx;mem.Ccg],mem.lb_du,mem.ub_du,[mem.lxc;mem.lcc],[mem.uxc;mem.ucc],mem.qpoases_opt); 14 | end 15 | if mem.hot_start==1 16 | [sol,fval,exitflag,iterations,multiplier,auxOutput] = qpOASES_sequence('h',mem.warm_start,mem.gc,mem.lb_du,... 17 | mem.ub_du,[mem.lxc;mem.lcc],[mem.uxc;mem.ucc],mem.qpoases_opt); 18 | end 19 | end 20 | 21 | %The dual solution vector contains exactly one entry per lower/upper bound as well as exactly one entry per 22 | %lower/upper constraints bound. Positive entries correspond to active lower (constraints) bounds, negative 23 | %entries to active upper (constraints) bounds and a zero entry means that both corresponding (constraints) 24 | %bounds are inactive. 25 | 26 | mem.du(:) = reshape(sol, [nu,N]); 27 | mem.mu_u_new(:) = - multiplier(1:N*nu); 28 | mem.mu_x_new(:) = -multiplier(N*nu+1:N*nu+N*nbx); 29 | mem.mu_new(:) = - multiplier(N*nu+N*nbx+1:end); 30 | cpt_qp = auxOutput.cpuTime*1e3; 31 | 32 | Recover(mem, sizes); 33 | end 34 | -------------------------------------------------------------------------------- /nmpc/mpc_nmpcsolver_simulink.m: -------------------------------------------------------------------------------- 1 | function [input,mem,cpt,StopCrit,OBJ] = mpc_nmpcsolver_simulink(input, settings, mem, opt) 2 | 3 | mem.sqp_it=0; 4 | mem.alpha =1; 5 | mem.obj=0; 6 | StopCrit = 2*mem.kkt_lim; 7 | 8 | tic; 9 | while(mem.sqp_it < mem.sqp_maxit && StopCrit > mem.kkt_lim && mem.alpha>1E-8 ) 10 | 11 | if strcmp(opt.qpsolver, 'qpoases_mb') 12 | qp_generation_mb(input, settings, mem); 13 | else 14 | qp_generation(input, settings, mem); 15 | end 16 | 17 | switch opt.condensing 18 | case 'default_full' 19 | if ~strcmp(opt.qpsolver, 'qpoases_mb') 20 | Condensing(mem, settings); 21 | else 22 | Condensing_mb(mem, settings); 23 | end 24 | case 'blasfeo_full' 25 | Condensing_Blasfeo(mem, settings); 26 | case 'no' 27 | 28 | end 29 | 30 | switch opt.qpsolver 31 | case 'qpoases' 32 | [~,mem] = mpc_qp_solve_qpoases(settings,mem); 33 | case 'qpoases_mb' 34 | [~,mem] = mpc_qp_solve_qpoases_mb(settings,mem, opt); 35 | case 'hpipm_sparse' 36 | hpipm_sparse(mem,settings); 37 | case 'hpipm_pcond' 38 | hpipm_pcond(mem,settings); 39 | 40 | end 41 | 42 | Line_search(mem, input, settings); 43 | 44 | [eq_res, ineq_res, KKT, OBJ] = solution_info(input, settings, mem); 45 | 46 | StopCrit = max([eq_res, ineq_res, KKT]); 47 | 48 | mem.sqp_it=mem.sqp_it+1; 49 | 50 | end 51 | cpt=toc*1e3; 52 | 53 | end 54 | 55 | -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_ipopt_sparse.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_ipopt_sparse(sizes,mem) 2 | N=sizes.N; 3 | nbx=sizes.nbx; 4 | nbx_idx=sizes.nbx_idx; 5 | nx=sizes.nx; 6 | nu=sizes.nu; 7 | nc=sizes.nc; 8 | ncN=sizes.ncN; 9 | neq = (N+1)*nx; 10 | nw = (N+1)*nx+N*nu; 11 | 12 | ipopt.options = mem.ipopt.options; 13 | ipopt.x0 = mem.ipopt.x0; 14 | 15 | for i=0:N-1 16 | for j=1:nbx 17 | ipopt.options.ub((i+1)*nx+nbx_idx(j))= mem.ub_dx(i*nbx+j); 18 | ipopt.options.lb((i+1)*nx+nbx_idx(j))= mem.lb_dx(i*nbx+j); 19 | end 20 | end 21 | 22 | ipopt.options.ub(neq+1:end) = mem.ub_du; 23 | ipopt.options.lb(neq+1:end) = mem.lb_du; 24 | 25 | full2sparse(mem,sizes); 26 | 27 | H = sparse(mem.sparse_H); 28 | 29 | ipopt.funcs.hessian = @(x,sigma,lambda) tril(sigma*H); 30 | ipopt.funcs.hessianstructure = @() tril(H); 31 | ipopt.funcs.objective=@(x) 0.5*x'*H*x + (mem.sparse_g)'*x; 32 | ipopt.funcs.gradient = @(x) (H*x + mem.sparse_g)'; 33 | ipopt.options.A= sparse([mem.sparse_dB;mem.sparse_dG]); 34 | ipopt.options.ru= [mem.uc;-mem.sparse_G]; 35 | ipopt.options.rl= [mem.lc;-mem.sparse_G]; 36 | [sol,fval,exitflag,info_ipopt] = opti_ipopt(ipopt,[]); 37 | 38 | mem.dx=reshape(sol(1:neq,1),[nx,N+1]); 39 | mem.du=reshape(sol(neq+1:nw,1),[nu,N]); 40 | 41 | mem.mu_u_new = info_ipopt.Lambda.upper((N+1)*nx+1:end) - info_ipopt.Lambda.lower((N+1)*nx+1:end); 42 | mu_x = info_ipopt.Lambda.upper(1:(N+1)*nx) - info_ipopt.Lambda.lower(1:(N+1)*nx); 43 | for i=0:N-1 44 | for j=1:nbx 45 | mem.mu_x_new(i*nbx+j,1) = mu_x((i+1)*nx+nbx_idx(j)); 46 | end 47 | end 48 | mem.mu_new = info_ipopt.Lambda.ineqlin; 49 | mem.lambda_new = reshape(info_ipopt.Lambda.eqlin,[nx N+1]); 50 | cpt_qp = info_ipopt.Time*1e3; 51 | end 52 | 53 | -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_qpoases_mb.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_qpoases_mb(sizes,mem,opt) 2 | 3 | nu=sizes.nu; 4 | N=sizes.N; 5 | nbx=sizes.nbx; 6 | nc=sizes.nc; 7 | ncN=sizes.ncN; 8 | 9 | lb=[]; 10 | ub=[]; 11 | for i=1:mem.r 12 | lb=[lb; mem.lb_du(mem.index_T(i)*nu+1:(mem.index_T(i)+1)*nu,1)]; 13 | ub=[ub; mem.ub_du(mem.index_T(i)*nu+1:(mem.index_T(i)+1)*nu,1)]; 14 | end 15 | 16 | if mem.warm_start==0 17 | [mem.warm_start,sol,fval,exitflag,iterations,multiplier,auxOutput] = qpOASES_sequence('i',mem.Hc_r,mem.gc_r,[mem.Ccx_r;mem.Ccg_r],... 18 | lb,ub,[mem.lxc;mem.lcc],[mem.uxc;mem.ucc],mem.qpoases_opt); 19 | 20 | else 21 | if mem.hot_start==0 22 | [sol,fval,exitflag,iterations,multiplier,auxOutput] = qpOASES_sequence('m',mem.warm_start,mem.Hc_r,mem.gc_r,... 23 | [mem.Ccx_r;mem.Ccg_r],lb,ub,[mem.lxc;mem.lcc],[mem.uxc;mem.ucc],mem.qpoases_opt); 24 | end 25 | if mem.hot_start==1 26 | [sol,fval,exitflag,iterations,multiplier,auxOutput] = qpOASES_sequence('h',mem.warm_start,mem.gc_r,mem.lb_du,... 27 | mem.ub_du,[mem.lxc;mem.lcc],[mem.uxc;mem.ucc],mem.qpoases_opt); 28 | 29 | end 30 | end 31 | 32 | %The dual solution vector contains exactly one entry per lower/upper bound as well as exactly one entry per 33 | %lower/upper constraints bound. Positive entries correspond to active lower (constraints) bounds, negative 34 | %entries to active upper (constraints) bounds and a zero entry means that both corresponding (constraints) 35 | %bounds are inactive. 36 | sol = mem.T*sol; 37 | 38 | mem.du(:) = reshape(sol, [nu,N]); 39 | mem.mu_x_new(:) = - multiplier(mem.r*nu+1:mem.r*nu+N*nbx); 40 | mem.mu_new(:) = - multiplier(mem.r*nu+mem.r*nbx+1:mem.r*nu+mem.r*nbx+N*nc+ncN); 41 | mu_u_new = - multiplier(1:mem.r*nu); 42 | for i=1:mem.r 43 | mem.mu_u_new(mem.index_T(i)*nu+1:mem.index_T(i+1)*nu) = mu_u_new(i)*ones(nu*(mem.index_T(i+1)-mem.index_T(i)),1); 44 | end 45 | cpt_qp = auxOutput.cpuTime*1e3; 46 | 47 | Recover(mem, sizes); 48 | end 49 | -------------------------------------------------------------------------------- /mex_core/partial_condensing_routines.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTIAL_CONDENSING_ROUTINES_H_ 2 | #define PARTIAL_CONDENSING_ROUTINES_H_ 3 | 4 | typedef struct 5 | { 6 | double *G; 7 | double *L; 8 | double *I; 9 | 10 | double *block11; // nx,nx 11 | double *block12; // nx,nx 12 | double *block21; // nx,nu 13 | double *block22; // nx,nu 14 | double *block31; // nx,nu 15 | double *block32; // nu,nu 16 | double *block41; // nbx,nx 17 | double *block42; // nbx,nu 18 | double *block51; // nx,nx 19 | double *block52; // nx,nu 20 | double *block61; // nc,nx 21 | double *block62; // nc,nu 22 | double *vec1; 23 | double *vec2; 24 | }partial_condensing_workspace; 25 | 26 | partial_condensing_workspace* partial_condensing_workspace_allocate(size_t Npc, size_t nx, size_t nu, size_t nbx, 27 | size_t nc); 28 | 29 | void partial_condensing_workspace_free(partial_condensing_workspace *work); 30 | 31 | void compute_G(partial_condensing_workspace* work, double *Ap, double *Bp, double *A, double *B, size_t nx, size_t nu, size_t Npc); 32 | 33 | void compute_L(partial_condensing_workspace* work, double *ap, double *A, double *a, size_t nx, size_t Npc); 34 | 35 | void compute_H(double *H, double *Q, double *S, double *R, double *A, double *B, partial_condensing_workspace* work, 36 | size_t nx, size_t nu, size_t Npc); 37 | 38 | void compute_g(double *g, double *Q, double *S, double *A, double *B, partial_condensing_workspace* work, double *gx, double *gu, 39 | size_t nx, size_t nu, size_t Npc); 40 | 41 | void compute_Ccx(double *Ccx, double *Cx, partial_condensing_workspace* work, 42 | size_t nx, size_t nu, size_t nbx, size_t Npc); 43 | 44 | void compute_ccx(double *lxc, double *uxc, double *lb_dx, double *ub_dx, double *Cx, partial_condensing_workspace* work, 45 | size_t nx, size_t nu, size_t nbx, size_t Npc); 46 | 47 | void compute_Ccg(double *Ccg, double *Cgx, double *Cgu, partial_condensing_workspace* work, 48 | size_t nx, size_t nu, size_t nc, size_t Npc); 49 | 50 | void compute_ccg(double *lgc, double *ugc, double *lc, double *uc, double *Cgx, partial_condensing_workspace* work, 51 | size_t nx, size_t nu, size_t nc, size_t Npc); 52 | 53 | #endif -------------------------------------------------------------------------------- /nmpc/mpc_qp_solve_ipopt_partial_sparse.m: -------------------------------------------------------------------------------- 1 | function [cpt_qp, mem] = mpc_qp_solve_ipopt_partial_sparse(settings,settings2,mem, mem2) 2 | N=settings2.N; 3 | Nc=settings2.Nc; 4 | nbx=settings2.nbx; 5 | nbx_idx=settings2.nbx_idx; 6 | nx=settings2.nx; 7 | nu=settings2.nu; 8 | nc=settings2.nc; 9 | ncN=settings2.ncN; 10 | neq = (N+1)*nx; 11 | nw = (N+1)*nx+N*nu; 12 | 13 | ipopt.options = mem2.ipopt.options; 14 | ipopt.x0 = mem2.ipopt.x0; 15 | 16 | for i=0:N-1 17 | for j=1:nbx 18 | ipopt.options.ub((i+1)*nx+nbx_idx(j))= mem2.ub_dx(i*nbx+j); 19 | ipopt.options.lb((i+1)*nx+nbx_idx(j))= mem2.lb_dx(i*nbx+j); 20 | end 21 | end 22 | 23 | ipopt.options.ub(neq+1:end) = mem2.ub_du; 24 | ipopt.options.lb(neq+1:end) = mem2.lb_du; 25 | 26 | full2sparse(mem2,settings2); 27 | 28 | H = sparse(mem2.sparse_H); 29 | 30 | ipopt.funcs.hessian = @(x,sigma,lambda) tril(sigma*H); 31 | ipopt.funcs.hessianstructure = @() tril(H); 32 | ipopt.funcs.objective=@(x) 0.5*x'*H*x + (mem2.sparse_g)'*x; 33 | ipopt.funcs.gradient = @(x) (H*x + mem2.sparse_g)'; 34 | ipopt.options.A= sparse([mem2.sparse_dB;mem2.sparse_dG]); 35 | ipopt.options.ru= [mem2.uc;-mem2.sparse_G]; 36 | ipopt.options.rl= [mem2.lc;-mem2.sparse_G]; 37 | [sol,fval,exitflag,info_ipopt] = opti_ipopt(ipopt,[]); 38 | cpt_qp = info_ipopt.Time*1e3; 39 | 40 | mem.du(:) = reshape(sol(neq+1:nw,1),[settings.nu,settings.N]); 41 | mem.mu_u_new(:) = info_ipopt.Lambda.upper((N+1)*nx+1:end) - info_ipopt.Lambda.lower((N+1)*nx+1:end); 42 | mu_x_p = info_ipopt.Lambda.upper(1:(N+1)*nx) - info_ipopt.Lambda.lower(1:(N+1)*nx); 43 | mu = info_ipopt.Lambda.ineqlin; 44 | 45 | mu_x = zeros(N*nbx,1); 46 | for i=0:N-1 47 | for j=1:nbx 48 | mu_x(i*nbx+j,1) = mu_x_p((i+1)*nx+nbx_idx(j)); 49 | end 50 | end 51 | 52 | for i=0:N-1 53 | mem.mu_x_new(i*Nc*settings.nbx+1:(i+1)*Nc*settings.nbx-settings.nbx) = mu(i*nc+1:i*nc+(Nc-1)*nbx); 54 | mem.mu_x_new((i+1)*Nc*settings.nbx-settings.nbx+1:(i+1)*Nc*settings.nbx) = mu_x(i*settings.nbx+1:(i+1)*settings.nbx); 55 | end 56 | 57 | Recover(mem, settings); 58 | end 59 | 60 | -------------------------------------------------------------------------------- /examples/InvertedPendulum.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------% 2 | % Inverted Pendulum 3 | 4 | % "Autogenerating microsecond solvers for nonlinear MPC: A tutorial 5 | % using ACADO integrators", Quirynen, 2015 6 | 7 | % typical configuration: 1) N=80, Ts=Ts_st=0.025, no shifting 2) N=40, 8 | % Ts=Ts_st=0.05, shifting 9 | 10 | %------------------------------------------% 11 | 12 | 13 | %% Dimensions 14 | 15 | nx=4; % No. of differential states 16 | nu=1; % No. of controls 17 | nz=0; % No. of algebraic states 18 | ny=5; % No. of outputs 19 | nyN=4; % No. of outputs at the terminal point 20 | np=0; % No. of model parameters 21 | nc=0; % No. of general constraints 22 | ncN=0; % No. of general constraints at the terminal point 23 | nbx = 1; % No. of bounds on states 24 | nbu = 1; % No. of bounds on controls 25 | 26 | % state and control bounds 27 | nbx_idx = 1; % indexs of states which are bounded 28 | nbu_idx = 1; % indexs of controls which are bounded 29 | 30 | %% create variables 31 | 32 | import casadi.* 33 | 34 | states = SX.sym('states',nx,1); % differential states 35 | controls = SX.sym('controls',nu,1); % control input 36 | alg = SX.sym('alg',nz,1); % algebraic states 37 | params = SX.sym('paras',np,1); % parameters 38 | refs = SX.sym('refs',ny,1); % references of the first N stages 39 | refN = SX.sym('refs',nyN,1); % reference of the last stage 40 | Q = SX.sym('Q',ny,1); % weighting matrix of the first N stages 41 | QN = SX.sym('QN',nyN,1); % weighting matrix of the last stage 42 | aux = SX.sym('aux',ny,1); % auxilary variable 43 | auxN = SX.sym('auxN',nyN,1); % auxilary variable 44 | 45 | %% Dynamics 46 | 47 | M = 1; 48 | m = 0.1; 49 | l = 0.8; 50 | g = 9.81; 51 | 52 | p=states(1); 53 | theta=states(2); 54 | v=states(3); 55 | omega=states(4); 56 | u=controls(1); 57 | 58 | a=-m*l*sin(theta)*omega^2+m*g*cos(theta)*sin(theta)+u; 59 | b=-m*l*cos(theta)*sin(theta)*omega^2+u*cos(theta)+(M+m)*g*sin(theta); 60 | c=M+m-m*(cos(theta))^2; 61 | 62 | % explicit ODE RHS 63 | x_dot=[v;omega;a/c;b/(l*c)]; 64 | 65 | % algebraic function 66 | z_fun = []; 67 | 68 | % implicit ODE: impl_f = 0 69 | xdot = SX.sym('xdot',nx,1); 70 | impl_f = xdot - x_dot; 71 | 72 | %% Objectives and constraints 73 | 74 | % inner objectives 75 | h = [p;theta;v;omega;u]; 76 | hN = h(1:nyN); 77 | 78 | % outer objectives 79 | obji = 0.5*(h-refs)'*diag(Q)*(h-refs); 80 | objN = 0.5*(hN-refN)'*diag(QN)*(hN-refN); 81 | 82 | obji_GGN = 0.5*(aux-refs)'*(aux-refs); 83 | objN_GGN = 0.5*(auxN-refN)'*(auxN-refN); 84 | 85 | % general inequality constraints 86 | general_con = []; 87 | general_con_N = []; 88 | 89 | %% NMPC discretizing time length [s] 90 | 91 | Ts_st = 0.025; % shooting interval time 92 | -------------------------------------------------------------------------------- /doc/HPIPM-Tutorial.txt: -------------------------------------------------------------------------------- 1 | This is a tutorial for installing BLASFEO and HPIPM on Windows 64-bit system. BLASFEO is a linear algebra library and HPIPM is a sparse QP solver. 2 | 3 | Since they are developed in Linux systems, first you need to install the GCC compiler "mingw64" and the linux environment "msys" . 4 | 5 | 1. Download "mingw64" from http://www.mingw-w64.org/doku.php/download/mingw-builds. The website will lead you to Sourceforge repository where the installer will be automatically downloaded. 6 | 2. Open the installer. It is recommended to install mingw64 in disk D: or above (disk C: may have problem with the Administration account). Choose the configuration as follows: 7 | 1) GCC version=compatible with the MATLAB version (e.g. 6.3.0 for MATLAB R2020); 2) threads=posix; 3) exceptions=seh; 4) arch = x86_64; 5) rev = latest (biggest number). 8 | 3. add "D:\msys64\bin" and "C:\msys64\x86_64-w64-mingw32\bin" into system's environment path (not user path) 9 | 4. open a windows command window by using "cmd", type "gcc -v", if you get information about your gcc version, then you have installed mingw64 successfully. 10 | 5. Open the MATMPC folder in MATLAB, run "configuremingw" and select the mingw64 folder you just installed. You must see mingw64 successfully configured for MATLAB. 11 | 6. Download "msys" from https://sourceforge.net/projects/mingw-w64/files/External%20binary%20packages%20%28Win64%20hosted%29/MSYS%20%2832-bit%29/. Choose the "MSYS-20111123.zip". 12 | 7. Unzip the downloaded "msys" folder and add "msys/bin" to system's environment path. 13 | 14 | 15 | Now we can install BLASFEO and HPIPM. 16 | 17 | 1. download BLASFEO and HPIPM from https://github.com/chenyutao36/blasfeo and https://github.com/chenyutao36/hpipm 18 | 2. go to the BLASFEO directory, open Makefile.rule using a text editor 19 | 3. select "TARGET=" according to your CPU architecture (if not sure, use X64_INTEL_CORE if you have a INTEL CPU) 20 | 4. uncomment "OS=WINDOWS" for the operating system 21 | 5. choose C Compiler as "CC=x86_64-w64-mingw32-gcc" 22 | 6. Change the Installation directory to your desired path (recommend NOT to install in Disk C:) 23 | 7. open a command terminal at the current location 24 | 8. type "make" and run 25 | 9. type "make install_static" and run 26 | 27 | 9. go to the HPIPM directory, open Makefile.rule using a text editor 28 | 10. Change the "BLASFEO installation directory" to the exact path in which BLASFEO has been installed 29 | 11. do the same thing as in steps 4 to 8 30 | 31 | OK we are done! Let's compile mexFunction in MATMPC 32 | 33 | 1. Open Matlab and go to MATMPC/mex_core 34 | 2. open compile_hpipm.m 35 | 3. Click Run and you should have hpipm_sparse.mexw64, hpipm_pcond.mexw64 36 | 37 | In Simulation.m, try 38 | 39 | 1. opt.condensing = 'blasfeo_full', run simulations and it should be faster in terms of COND time than 'default_full' 40 | 2. opt.condensing = 'no', opt.qpsolver = 'hpipm_sparse' or 'hpipm_pcond', the time for solving QP should be much less. 41 | 42 | Good luck! 43 | 44 | Yutao Chen 45 | 46 | April 2021 -------------------------------------------------------------------------------- /mex_core/Compile_Mex.m: -------------------------------------------------------------------------------- 1 | %% set path for your computer 2 | 3 | %% detect OS type 4 | 5 | OS_MAC = 0; 6 | OS_LINUX = 0; 7 | OS_WIN = 0; 8 | 9 | gcc_version = 'n'; 10 | 11 | if ismac 12 | OS_MAC = 1; 13 | addpath(genpath('../solver/mac')); 14 | elseif isunix 15 | OS_LINUX = 1; 16 | addpath(genpath('../solver/linux')); 17 | elseif ispc 18 | OS_WIN = 1; 19 | addpath(genpath('../solver/win64')); 20 | else 21 | disp('Platform not supported') 22 | end 23 | 24 | %% Configure Compiler 25 | 26 | options = '-largeArrayDims'; 27 | 28 | if OS_WIN 29 | CC_FLAGS='CXXFLAGS="$CXXFLAGS -Wall"'; % use MinGW not VS studio 30 | end 31 | if OS_LINUX 32 | if strcmp(gcc_version, 'y') 33 | CC_FLAGS = 'GCC="/usr/bin/gcc-6.3"'; 34 | else 35 | CC_FLAGS = 'GCC="/usr/bin/gcc"'; 36 | end 37 | end 38 | if OS_MAC 39 | CC_FLAGS = ''; 40 | end 41 | 42 | OP_FLAGS='-O'; 43 | PRINT_FLAGS='-silent'; 44 | 45 | LIB1 = '-lmwblas'; 46 | LIB2 = '-lmwlapack'; 47 | 48 | LIB1_PATH = ''; 49 | LIB2_PATH = ''; 50 | 51 | HEAD1_PATH = ''; 52 | HEAD2_PATH = ''; 53 | 54 | %% These functions should be all compiled 55 | 56 | mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH, 'qp_generation.c','casadi_wrapper.c','sim.c','erk.c','irk_ode.c','irk_dae.c','casadi_src.c','mpc_common.c',LIB1, LIB2); 57 | 58 | mex(options, CC_FLAGS, OP_FLAGS, PRINT_FLAGS, 'Condensing.c','mpc_common.c', LIB1); 59 | 60 | mex(options, CC_FLAGS, OP_FLAGS, PRINT_FLAGS, 'full2sparse.c','mpc_common.c'); 61 | 62 | mex(options, CC_FLAGS, OP_FLAGS, PRINT_FLAGS, 'sparse2full.c','mpc_common.c'); 63 | 64 | mex(options, CC_FLAGS, OP_FLAGS, PRINT_FLAGS, 'partial_condensing_default.c','partial_condensing_routines.c','mpc_common.c', LIB1); 65 | 66 | mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH,'Recover.c', LIB1); 67 | 68 | mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH,'Line_search.c','casadi_wrapper.c','casadi_src.c','sim.c','erk.c','irk_ode.c','irk_dae.c','mpc_common.c', LIB1, LIB2); 69 | 70 | mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH,'solution_info.c','casadi_wrapper.c','casadi_src.c','sim.c','erk.c','irk_ode.c','irk_dae.c','mpc_common.c', LIB1, LIB2); 71 | 72 | mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH, 'Simulate_System.c','casadi_wrapper.c','sim.c','erk.c','irk_ode.c','irk_dae.c','casadi_src.c','mpc_common.c',LIB1, LIB2); 73 | 74 | %% Optional functions 75 | 76 | mex(options, CC_FLAGS, OP_FLAGS, PRINT_FLAGS, 'Condensing_mb.c','mpc_common.c', LIB1); 77 | 78 | mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH, 'qp_generation_ngrid.c','casadi_wrapper.c','sim.c','erk.c','irk_ode.c','irk_dae.c','casadi_src.c','mpc_common.c',LIB1, LIB2); 79 | 80 | mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH, 'qp_generation_mb.c','casadi_wrapper.c','sim.c','erk.c','irk_ode.c','irk_dae.c','casadi_src.c','mpc_common.c',LIB1, LIB2); 81 | 82 | % mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH, 'qp_generation_tac.c','casadi_wrapper.c','sim.c','erk.c','irk_ode.c','irk_dae.c','casadi_src.c','mpc_common.c',LIB1, LIB2); 83 | 84 | % mex(options, OP_FLAGS, CC_FLAGS, PRINT_FLAGS, HEAD1_PATH, LIB1_PATH, 'adaptive_eta.c',LIB1); 85 | -------------------------------------------------------------------------------- /doc/ref.bib: -------------------------------------------------------------------------------- 1 | @article{andersson2018casadi, 2 | title={CasADi: a software framework for nonlinear optimization and optimal control}, 3 | author={Andersson, Joel AE and Gillis, Joris and Horn, Greg and Rawlings, James B and Diehl, Moritz}, 4 | journal={Mathematical Programming Computation}, 5 | pages={1--36}, 6 | year={2018}, 7 | publisher={Springer} 8 | } 9 | 10 | @article{frison2018blasfeo, 11 | title={BLASFEO: Basic linear algebra subroutines for embedded optimization}, 12 | author={Frison, Gianluca and Kouzoupis, Dimitris and Sartor, Tommaso and Zanelli, Andrea and Diehl, Moritz}, 13 | journal={ACM Transactions on Mathematical Software (TOMS)}, 14 | volume={44}, 15 | number={4}, 16 | pages={42}, 17 | year={2018}, 18 | publisher={ACM} 19 | } 20 | 21 | @book{nocedal2006numerical, 22 | title={Numerical optimization}, 23 | author={Nocedal, Jorge and Wright, Stephen}, 24 | year={2006}, 25 | publisher={Springer Science \& Business Media} 26 | } 27 | 28 | @article{chen2018adaptive, 29 | title={An adaptive partial sensitivity updating scheme for fast nonlinear model predictive control}, 30 | author={Chen, Yutao and Bruschetta, Mattia and Cuccato, Davide and Beghi, Alessandro}, 31 | journal={IEEE Transactions on Automatic Control}, 32 | volume={64}, 33 | number={7}, 34 | pages={2712--2726}, 35 | year={2018}, 36 | publisher={IEEE} 37 | } 38 | 39 | @inproceedings{chen2017fast, 40 | title={A fast nonlinear model predictive control strategy for real-time motion control of mechanical systems}, 41 | author={Chen, Yutao and Cuccato, Davide and Bruschetta, Mattia and Beghi, Alessandro}, 42 | booktitle={2017 IEEE International Conference on Advanced Intelligent Mechatronics (AIM)}, 43 | pages={1780--1785}, 44 | year={2017}, 45 | organization={IEEE} 46 | } 47 | 48 | @article{ferreau2014qpoases, 49 | title={qpOASES: A parametric active-set algorithm for quadratic programming}, 50 | author={Ferreau, Hans Joachim and Kirches, Christian and Potschka, Andreas and Bock, Hans Georg and Diehl, Moritz}, 51 | journal={Mathematical Programming Computation}, 52 | volume={6}, 53 | number={4}, 54 | pages={327--363}, 55 | year={2014}, 56 | publisher={Springer} 57 | } 58 | 59 | @inproceedings{stellato2018osqp, 60 | title={OSQP: An operator splitting solver for quadratic programs}, 61 | author={Stellato, Bartolomeo and Banjac, Goran and Goulart, Paul and Bemporad, Alberto and Boyd, Stephen}, 62 | booktitle={2018 UKACC 12th International Conference on Control (CONTROL)}, 63 | pages={339--339}, 64 | year={2018}, 65 | organization={IEEE} 66 | } 67 | 68 | @article{wachter2006implementation, 69 | title={On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming}, 70 | author={W{\"a}chter, Andreas and Biegler, Lorenz T}, 71 | journal={Mathematical programming}, 72 | volume={106}, 73 | number={1}, 74 | pages={25--57}, 75 | year={2006}, 76 | publisher={Springer} 77 | } 78 | 79 | @article{chen2019efficient, 80 | title={Efficient move blocking strategy for multiple shooting-based non-linear model predictive control}, 81 | author={Chen, Yutao and Scarabottolo, Nicol{\'o} and Bruschetta, Mattia and Beghi, Alessandro}, 82 | journal={IET Control Theory \& Applications}, 83 | volume={14}, 84 | number={2}, 85 | pages={343--351}, 86 | year={2019}, 87 | publisher={IET} 88 | } -------------------------------------------------------------------------------- /examples/ChainofMasses_Lin.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------% 2 | % Chain of masses connected by linear springs 3 | 4 | % "Efficient direct multiple shooting for nonlinear model predictive 5 | % control on long horizons", Kirches, 2012 6 | 7 | % typical configuration: N=40(80,160), Ts=Ts_st=0.2, no shifting 8 | 9 | %------------------------------------------% 10 | 11 | %% Dimensions 12 | 13 | n=5; 14 | nx=n*3+(n-1)*3; 15 | nu=3; 16 | nz=0; 17 | np=0; 18 | ny=3*(n-1)+3+3; 19 | nyN=3*(n-1)+3; 20 | nc=0; 21 | ncN=0; 22 | nbx=0; 23 | nbu=3; 24 | 25 | % state and control bounds 26 | nbx_idx = 0; % indexs of states which are bounded 27 | nbu_idx = 1:3; % indexs of controls which are bounded 28 | 29 | %% create variables 30 | 31 | import casadi.* 32 | 33 | states = SX.sym('states',nx,1); % differential states 34 | controls = SX.sym('controls',nu,1); % control input 35 | alg = SX.sym('alg',nz,1); % algebraic states 36 | params = SX.sym('paras',np,1); % parameters 37 | refs = SX.sym('refs',ny,1); % references of the first N stages 38 | refN = SX.sym('refs',nyN,1); % reference of the last stage 39 | Q = SX.sym('Q',ny,1); % weighting matrix of the first N stages 40 | QN = SX.sym('QN',nyN,1); % weighting matrix of the last stage 41 | aux = SX.sym('aux',ny,1); % auxilary variable 42 | auxN = SX.sym('auxN',nyN,1); % auxilary variable 43 | 44 | %% Dynamics 45 | 46 | k=0.1; 47 | lr=0.55; 48 | m=0.45; 49 | g=9.81; 50 | 51 | p0x=0;p0y=0;p0z=0; 52 | xend=1;yend=0;zend=0; 53 | 54 | px=states(1:n,1); 55 | py=states(n+1:2*n,1); 56 | pz=states(2*n+1:3*n,1); 57 | vx=states(3*n+1:4*n-1,1); 58 | vy=states(4*n:5*n-2,1); 59 | vz=states(5*n-1:6*n-3,1); 60 | ux=controls(1); 61 | uy=controls(2); 62 | uz=controls(3); 63 | 64 | dist=SX(n,1); 65 | Fx=SX(n,1); 66 | Fy=SX(n,1); 67 | Fz=SX(n,1); 68 | ax=SX(n-1,1); 69 | ay=SX(n-1,1); 70 | az=SX(n-1,1); 71 | 72 | dist(1)= ((px(1)-p0x)^2+(py(1)-p0y)^2+(pz(1)-p0z)^2)^0.5 ; 73 | Fx(1)=(px(1)-p0x)*k*(n-lr/dist(1)); 74 | Fy(1)=(py(1)-p0y)*k*(n-lr/dist(1)); 75 | Fz(1)=(pz(1)-p0z)*k*(n-lr/dist(1)); 76 | for i=2:n 77 | dist(i)= ((px(i)-px(i-1))^2+(py(i)-py(i-1))^2+(pz(i)-pz(i-1))^2)^0.5 ; 78 | Fx(i)= (px(i)-px(i-1))*k*(n-lr/dist(i)) ; 79 | Fy(i)= (py(i)-py(i-1))*k*(n-lr/dist(i)) ; 80 | Fz(i)= (pz(i)-pz(i-1))*k*(n-lr/dist(i)) ; 81 | ax(i-1,1)= (Fx(i)-Fx(i-1))*n/m ; 82 | ay(i-1,1)= (Fy(i)-Fy(i-1))*n/m ; 83 | az(i-1,1)= (Fz(i)-Fz(i-1))*n/m-g ; 84 | end 85 | 86 | % explicit ODE RHS 87 | x_dot=[vx;ux;vy;uy;vz;uz;ax;ay;az]; 88 | 89 | % algebraic function 90 | z_fun = []; 91 | 92 | % implicit ODE: impl_f = 0 93 | xdot = SX.sym('xdot',nx,1); 94 | impl_f = xdot - x_dot; 95 | 96 | %% Objectives and constraints 97 | 98 | % inner objectives 99 | h = [px(n);py(n);pz(n);vx;vy;vz;ux;uy;uz]; 100 | hN = h(1:nyN); 101 | 102 | % outer objectives 103 | obji = 0.5*(h-refs)'*diag(Q)*(h-refs); 104 | objN = 0.5*(hN-refN)'*diag(QN)*(hN-refN); 105 | 106 | obji_GGN = 0.5*(aux-refs)'*(aux-refs); 107 | objN_GGN = 0.5*(auxN-refN)'*(auxN-refN); 108 | 109 | % general inequality path constraints 110 | general_con = []; 111 | general_con_N = []; 112 | 113 | %% NMPC discretizing time length [s] 114 | 115 | Ts_st = 0.2; % shooting interval time 116 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MATMPC 2 | ## MATMPC: MATLAB based nonlinear MPC tool 3 | 4 | This tool aims at providing an easy-to-use nonlinear MPC implementation. The optimal control problem (OCP) that should be solved is transcribed by multiple shooting and the resulting nonlinear program (NLP) is solved by Sequential Quadratic Programming (SQP) method. 5 | 6 | The tool supports fixed step (explicit/implicit) Runge-Kutta (RK) integrator for multiple shooting. The derivatives that are needed to perform optimization are obtained by CasADi (https://github.com/casadi/casadi/wiki), the state-of-the-art automatic/algorithmic differentiation toolbox. The Quadratic Programming (QP) problems can be solved by both dense and sparse solvers. By now, MATMPC supports interfaces with the following external solvers: qpOASES (https://projects.coin-or.org/qpOASES/wiki/QpoasesInstallation), Ipopt (https://projects.coin-or.org/Ipopt), hpipm (https://github.com/giaf/hpipm), osqp (https://osqp.org/) and qpalm (https://github.com/Benny44/QPALM). 7 | 8 | **The most unique feature of MATMPC is that it does not require to install any external libraries. The users need not to understand how to make, compile and link any library. Except for external QP solvers, all algorithmic routines are written directly using MATLAB C API and can be compiled into independent MEX functions using compilers that belong to GCC class (e.g. GCC, MinGW and Clang). MATMPC employs MATLAB built-in linear algebra library provided by Intel MKL. Therefore, MATMPC is able to provide compatible runtime performance as other libraries written directly in C/C++.** 9 | 10 | The mathematics and parameter tuning of MATMPC is explained at 11 | [matmpc-docs.readthedocs.io/en/latest/](https://matmpc-docs.readthedocs.io/en/latest/ "https://matmpc-docs.readthedocs.io/en/latest/") 12 | 13 | ### Windows: 14 | 15 | Please install Matlab supported MinGW compiler at https://www.mathworks.com/matlabcentral/fileexchange/52848-matlab-support-for-mingw-w64-c-c-compiler 16 | 17 | ### Linux: 18 | 19 | Install gcc by running "sudo apt-get install gcc" 20 | 21 | **Note:** MATLAB is incompatible with the latest releases of gcc, It is recommended to download the gcc version that is compatible with your MATLAB version. Go check [Compilers - MATLAB & Simulink (mathworks.com)](https://www.mathworks.com/support/requirements/supported-compilers.html). 22 | 23 | ### MacOS: 24 | 25 | Install Xcode from app store 26 | 27 | ### To use MATMPC, follow the steps below. 28 | 29 | 1. Download and install CasADi-3.3.0. MATMPC currently is compatible with CasADi 3.3.0. 30 | 31 | 2. Write your own model using following the styles given by examples, e.g. Inverted Pendulum. 32 | 33 | 3. In the model file you created, set your own sampling time and multiple shooting time. 34 | 35 | 4. Run Model_Generation.m 36 | 37 | 5. In InitData.m, set your own initialization data, e.g. data path, initial states, references and etc. 38 | 39 | 6. In Simulation.m, choose your integrator, set the prediction horizon and the solver options. You may also need to modify the reference type for constant or time-varying reference tracking problems. 40 | 41 | 7. In Draw.m, write your own plot functions to display your results. 42 | 43 | 8. Run Simulation.m and see the results! 44 | 45 | ### To use HPIPM as the QP solver 46 | please read /doc/HPIPM-tutorial for the detailed installation process 47 | -------------------------------------------------------------------------------- /examples/ChainofMasses_NLin.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------% 2 | % Chain of masses connected by nonlinear springs 3 | 4 | % "An Adaptive Partial Sensitivity Updating Scheme for Fast Nonlinear 5 | % Model Predictive Control", Yutao Chen, IEEE Transactions on Automatic Control, 2018 6 | 7 | % typical configuration: N=40(80,160), Ts=Ts_st=0.2, no shifting 8 | 9 | %------------------------------------------% 10 | 11 | %% Dimensions 12 | n=10; 13 | nx=n*3+(n-1)*3; 14 | nu=3; 15 | nz=0; 16 | np=0; 17 | ny=3*(n-1)+3+3; 18 | nyN=3*(n-1)+3; 19 | nc=0; 20 | ncN=0; 21 | nbx=0; 22 | nbu=3; 23 | 24 | % state and control bounds 25 | nbx_idx = 0; % indexs of states which are bounded 26 | nbu_idx = 1:3; % indexs of controls which are bounded 27 | 28 | %% create variables 29 | 30 | import casadi.* 31 | 32 | states = SX.sym('states',nx,1); % differential states 33 | controls = SX.sym('controls',nu,1); % control input 34 | alg = SX.sym('alg',nz,1); % algebraic states 35 | params = SX.sym('paras',np,1); % parameters 36 | refs = SX.sym('refs',ny,1); % references of the first N stages 37 | refN = SX.sym('refs',nyN,1); % reference of the last stage 38 | Q = SX.sym('Q',ny,1); % weighting matrix of the first N stages 39 | QN = SX.sym('QN',nyN,1); % weighting matrix of the last stage 40 | aux = SX.sym('aux',ny,1); % auxilary variable 41 | auxN = SX.sym('auxN',nyN,1); % auxilary variable 42 | 43 | %% Dynamics 44 | 45 | D=1; 46 | D1=-0.03; 47 | L=0.033; 48 | m=0.03; 49 | g=9.81; 50 | 51 | xend=1;yp=0;zend=0; 52 | p0x=0;p0y=0;p0z=0; 53 | 54 | px=states(1:n,1); 55 | py=states(n+1:2*n,1); 56 | pz=states(2*n+1:3*n,1); 57 | vx=states(3*n+1:4*n-1,1); 58 | vy=states(4*n:5*n-2,1); 59 | vz=states(5*n-1:6*n-3,1); 60 | ux=controls(1); 61 | uy=controls(2); 62 | uz=controls(3); 63 | 64 | dist=SX(n,1); 65 | Fx=SX(n,1); 66 | Fy=SX(n,1); 67 | Fz=SX(n,1); 68 | ax=SX(n-1,1); 69 | ay=SX(n-1,1); 70 | az=SX(n-1,1); 71 | 72 | dist(1)= ((px(1)-p0x)^2+(py(1)-p0y)^2+(pz(1)-p0z)^2)^0.5 ; 73 | Fx(1)=( D*(1-L/dist(1))+ D1*(dist(1)-L)^3/dist(1) )*(px(1)-p0x); 74 | Fy(1)=( D*(1-L/dist(1))+ D1*(dist(1)-L)^3/dist(1) )*(py(1)-p0y); 75 | Fz(1)=( D*(1-L/dist(1))+ D1*(dist(1)-L)^3/dist(1) )*(pz(1)-p0z); 76 | for i=2:n 77 | dist(i)= ((px(i)-px(i-1))^2+(py(i)-py(i-1))^2+(pz(i)-pz(i-1))^2)^0.5 ; 78 | Fx(i)= ( D*(1-L/dist(i))+ D1*(dist(i)-L)^3/dist(i) )*(px(i)-px(i-1)); 79 | Fy(i)= ( D*(1-L/dist(i))+ D1*(dist(i)-L)^3/dist(i) )*(py(i)-py(i-1)); 80 | Fz(i)= ( D*(1-L/dist(i))+ D1*(dist(i)-L)^3/dist(i) )*(pz(i)-pz(i-1)); 81 | ax(i-1,1)= (Fx(i)-Fx(i-1))/m ; 82 | ay(i-1,1)= (Fy(i)-Fy(i-1))/m ; 83 | az(i-1,1)= (Fz(i)-Fz(i-1))/m-g ; 84 | end 85 | 86 | % explicit ODE RHS 87 | x_dot=[vx;ux;vy;uy;vz;uz;ax;ay;az]; 88 | 89 | % algebraic function 90 | z_fun = []; 91 | 92 | % implicit ODE: impl_f = 0 93 | xdot = SX.sym('xdot',nx,1); 94 | impl_f = xdot - x_dot; 95 | 96 | %% Objectives and constraints 97 | 98 | % inner objectives 99 | h = [px(n);py(n);pz(n);vx;vy;vz;ux;uy;uz]; 100 | hN = h(1:nyN); 101 | 102 | % outer objectives 103 | obji = 0.5*(h-refs)'*diag(Q)*(h-refs); 104 | objN = 0.5*(hN-refN)'*diag(QN)*(hN-refN); 105 | 106 | obji_GGN = 0.5*(aux-refs)'*(aux-refs); 107 | objN_GGN = 0.5*(auxN-refN)'*(auxN-refN); 108 | 109 | % general inequality path constraints 110 | general_con = []; 111 | general_con_N = []; 112 | 113 | %% NMPC discretizing time length [s] 114 | 115 | Ts_st = 0.2; % shooting interval time 116 | -------------------------------------------------------------------------------- /mex_core/compile_hpipm.m: -------------------------------------------------------------------------------- 1 | 2 | %% detect OS type 3 | 4 | OS_MAC = 0; 5 | OS_LINUX = 0; 6 | OS_WIN = 0; 7 | 8 | %% Important! 9 | % Please revise the following codes using the correct 10 | % path for Blasfeo and Hpipm that have been installed in your PC. 11 | 12 | if ismac 13 | OS_MAC = 1; 14 | PREFIX = '/Users/xxx'; % xxx is the username of your MAC OS 15 | elseif isunix 16 | OS_LINUX = 1; 17 | elseif ispc 18 | OS_WIN = 1; 19 | PREFIX = 'D:\Tools'; 20 | else 21 | disp('Platform not supported') 22 | end 23 | 24 | %% hpipm sparse 25 | 26 | if OS_LINUX 27 | mexfiles_sp = ['hpipm_sparse.c ', ... 28 | ' /opt/hpipm/lib/libhpipm.a ', ... 29 | ' /opt/blasfeo/lib/libblasfeo.a ',... 30 | ]; % BLASFEO and HPIPM are in default installed at /opt/ 31 | elseif OS_WIN 32 | mexfiles_sp = ['hpipm_sparse.c ', ... 33 | PREFIX,'\hpipm\lib\libhpipm.a ', ... 34 | PREFIX,'\blasfeo\lib\libblasfeo.a ',... 35 | ]; % BLASFEO and HPIPM are in default installed at D:\Tools\ 36 | elseif OS_MAC 37 | mexfiles_sp = ['hpipm_sparse.c ', ... 38 | PREFIX,'/Documents/blasfeo/lib/libblasfeo.a ', ... 39 | PREFIX,'/Documents/hpipm/lib/libhpipm.a ',... 40 | ]; % BLASFEO and HPIPM are in default installed at Users/xxx/Documents/, where xxx is the username of your MAC OS 41 | end 42 | 43 | 44 | %% hpipm pcond 45 | 46 | if OS_LINUX 47 | mexfiles_pcondsol = ['hpipm_pcond.c ', ... 48 | ' /opt/hpipm/lib/libhpipm.a ', ... 49 | ' /opt/blasfeo/lib/libblasfeo.a ',... 50 | ]; 51 | elseif OS_WIN 52 | mexfiles_pcondsol = ['hpipm_pcond.c ', ... 53 | PREFIX,'\hpipm\lib\libhpipm.a ', ... 54 | PREFIX,'\blasfeo\lib\libblasfeo.a ',... 55 | ]; 56 | elseif OS_MAC 57 | mexfiles_pcondsol = ['hpipm_pcond.c ', ... 58 | PREFIX,'/Documents/blasfeo/lib/libblasfeo.a ', ... 59 | PREFIX,'/Documents/hpipm/lib/libhpipm.a ',... 60 | ]; 61 | end 62 | 63 | %% blasfeo condensing 64 | % 65 | % if OS_LINUX 66 | % mexfiles_bcond = ['Condensing_Blasfeo.c ', ... 67 | % ' /opt/blasfeo/lib/libblasfeo.a ',... 68 | % ]; 69 | % elseif OS_WIN 70 | % mexfiles_bcond = ['Condensing_Blasfeo.c ', ... 71 | % PREFIX,'\opt\blasfeo\lib\libblasfeo.a ',... 72 | % ]; 73 | % elseif OS_MAC 74 | % mexfiles_bcond = ['Condensing_Blasfeo.c ', ... 75 | % PREFIX,'/Documents/blasfeo_lib/blasfeo/lib/libblasfeo.a ',... 76 | % ]; 77 | % end 78 | 79 | %% Build mex command 80 | 81 | mexcmd = 'mex'; 82 | 83 | if OS_LINUX 84 | mexcmd = [mexcmd, ' -O -DINT64 CFLAGS="\$CFLAGS -std=c99" GCC="/usr/bin/gcc"']; 85 | mexcmd = [mexcmd, ' -I.. -I/opt/hpipm/include -I/opt/blasfeo/include']; 86 | elseif OS_WIN 87 | mexcmd = [mexcmd, ' -O -DINT64 CFLAGS="$CFLAGS -std=c99" ']; 88 | mexcmd = [mexcmd, ' -I.. -I' PREFIX '\hpipm\include -I' PREFIX '\blasfeo\include']; 89 | elseif OS_MAC 90 | mexcmd = [mexcmd, ' -O -DINT64 CFLAGS="\$CFLAGS -std=c99"']; 91 | mexcmd = [mexcmd, ' -I.. -I' PREFIX, '/Documents/hpipm/include -I' PREFIX '/Documents/blasfeo/include']; 92 | end 93 | 94 | %% 95 | 96 | mexcmd_sp = [mexcmd, ' ', mexfiles_sp]; 97 | mexcmd_pcondsol = [mexcmd, ' ', mexfiles_pcondsol]; 98 | % mexcmd_bcond = [mexcmd, ' ', mexfiles_bcond]; 99 | 100 | eval(mexcmd_sp); 101 | eval(mexcmd_pcondsol); 102 | % eval(mexcmd_bcond); -------------------------------------------------------------------------------- /solver/linux/qpoases/qpOASES.m: -------------------------------------------------------------------------------- 1 | %qpOASES -- An Implementation of the Online Active Set Strategy. 2 | %Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 3 | %Christian Kirches et al. All rights reserved. 4 | % 5 | %qpOASES is distributed under the terms of the 6 | %GNU Lesser General Public License 2.1 in the hope that it will be 7 | %useful, but WITHOUT ANY WARRANTY; without even the implied warranty 8 | %of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 9 | %See the GNU Lesser General Public License for more details. 10 | % 11 | %--------------------------------------------------------------------------------- 12 | % 13 | %qpOASES solves (a series) of quadratic programming (QP) problems of the 14 | %following form: 15 | % 16 | % min 1/2*x'Hx + x'g 17 | % s.t. lb <= x <= ub 18 | % lbA <= Ax <= ubA {optional} 19 | % 20 | %Call 21 | % 22 | % [x,fval,exitflag,iter,lambda,auxOutput] = 23 | % qpOASES( H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} ) 24 | % 25 | %for solving the above-mentioned QP. H must be a symmetric (but possibly 26 | %indefinite) matrix and all vectors g, lb, ub, lbA, ubA have to be given 27 | %as column vectors. Options can be generated using the qpOASES_options command, 28 | %otherwise default values are used. Optionally, further auxiliary inputs 29 | %may be generated using qpOASES_auxInput command and passed to the solver. 30 | %Both matrices H or A may be passed in sparse matrix format. 31 | % 32 | %Call 33 | % 34 | % [x,fval,exitflag,iter,lambda,auxOutput] = 35 | % qpOASES( H,g,lb,ub{,options{,auxInput}} ) 36 | % 37 | %for solving the above-mentioned QP without general constraints. 38 | % 39 | % 40 | %Optional outputs (only x is mandatory): 41 | % x - Optimal primal solution vector (if exitflag==0). 42 | % fval - Optimal objective function value (if exitflag==0). 43 | % exitflag - 0: QP problem solved, 44 | % 1: QP could not be solved within given number of iterations, 45 | % -1: QP could not be solved due to an internal error, 46 | % -2: QP is infeasible (and thus could not be solved), 47 | % -3: QP is unbounded (and thus could not be solved). 48 | % iter - Number of active set iterations actually performed. 49 | % lambda - Optimal dual solution vector (if exitflag==0). 50 | % auxOutput - Struct containing auxiliary outputs as described below. 51 | % 52 | %The auxOutput struct contains the following entries: 53 | % workingSetB - Working set of bounds at point x. 54 | % workingSetC - Working set of constraints at point x. 55 | % The working set is a subset of the active set (indices 56 | % of bounds/constraints that hold with equality) yielding 57 | % a set linearly independent of bounds/constraints. 58 | % The working sets are encoded as follows: 59 | % 1: bound/constraint at its upper bound 60 | % 0: bound/constraint not at any bound 61 | % -1: bound/constraint at its lower bound 62 | % cpuTime - Internally measured CPU time for solving QP problem. 63 | % 64 | % 65 | %If not a single QP but a sequence of QPs with varying vectors is to be solved, 66 | %the i-th QP is given by the i-th columns of the QP vectors g, lb, ub, lbA, ubA 67 | %(i.e. they are matrices in this case). Both matrices H and A remain constant. 68 | % 69 | %See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES_SEQUENCE 70 | % 71 | % 72 | %For additional information see the qpOASES User's Manual or 73 | %visit http://www.qpOASES.org/. 74 | % 75 | %Please send remarks and questions to support@qpOASES.org! 76 | -------------------------------------------------------------------------------- /solver/mac/qpoases/qpOASES.m: -------------------------------------------------------------------------------- 1 | %qpOASES -- An Implementation of the Online Active Set Strategy. 2 | %Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, 3 | %Christian Kirches et al. All rights reserved. 4 | % 5 | %qpOASES is distributed under the terms of the 6 | %GNU Lesser General Public License 2.1 in the hope that it will be 7 | %useful, but WITHOUT ANY WARRANTY; without even the implied warranty 8 | %of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 9 | %See the GNU Lesser General Public License for more details. 10 | % 11 | %--------------------------------------------------------------------------------- 12 | % 13 | %qpOASES solves (a series) of quadratic programming (QP) problems of the 14 | %following form: 15 | % 16 | % min 1/2*x'Hx + x'g 17 | % s.t. lb <= x <= ub 18 | % lbA <= Ax <= ubA {optional} 19 | % 20 | %Call 21 | % 22 | % [x,fval,exitflag,iter,lambda,auxOutput] = 23 | % qpOASES( H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} ) 24 | % 25 | %for solving the above-mentioned QP. H must be a symmetric (but possibly 26 | %indefinite) matrix and all vectors g, lb, ub, lbA, ubA have to be given 27 | %as column vectors. Options can be generated using the qpOASES_options command, 28 | %otherwise default values are used. Optionally, further auxiliary inputs 29 | %may be generated using qpOASES_auxInput command and passed to the solver. 30 | %Both matrices H or A may be passed in sparse matrix format. 31 | % 32 | %Call 33 | % 34 | % [x,fval,exitflag,iter,lambda,auxOutput] = 35 | % qpOASES( H,g,lb,ub{,options{,auxInput}} ) 36 | % 37 | %for solving the above-mentioned QP without general constraints. 38 | % 39 | % 40 | %Optional outputs (only x is mandatory): 41 | % x - Optimal primal solution vector (if exitflag==0). 42 | % fval - Optimal objective function value (if exitflag==0). 43 | % exitflag - 0: QP problem solved, 44 | % 1: QP could not be solved within given number of iterations, 45 | % -1: QP could not be solved due to an internal error, 46 | % -2: QP is infeasible (and thus could not be solved), 47 | % -3: QP is unbounded (and thus could not be solved). 48 | % iter - Number of active set iterations actually performed. 49 | % lambda - Optimal dual solution vector (if exitflag==0). 50 | % auxOutput - Struct containing auxiliary outputs as described below. 51 | % 52 | %The auxOutput struct contains the following entries: 53 | % workingSetB - Working set of bounds at point x. 54 | % workingSetC - Working set of constraints at point x. 55 | % The working set is a subset of the active set (indices 56 | % of bounds/constraints that hold with equality) yielding 57 | % a set linearly independent of bounds/constraints. 58 | % The working sets are encoded as follows: 59 | % 1: bound/constraint at its upper bound 60 | % 0: bound/constraint not at any bound 61 | % -1: bound/constraint at its lower bound 62 | % cpuTime - Internally measured CPU time for solving QP problem. 63 | % 64 | % 65 | %If not a single QP but a sequence of QPs with varying vectors is to be solved, 66 | %the i-th QP is given by the i-th columns of the QP vectors g, lb, ub, lbA, ubA 67 | %(i.e. they are matrices in this case). Both matrices H and A remain constant. 68 | % 69 | %See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES_SEQUENCE 70 | % 71 | % 72 | %For additional information see the qpOASES User's Manual or 73 | %visit http://www.qpOASES.org/. 74 | % 75 | %Please send remarks and questions to support@qpOASES.org! 76 | -------------------------------------------------------------------------------- /solver/win64/qpoases/qpOASES.m: -------------------------------------------------------------------------------- 1 | %qpOASES -- An Implementation of the Online Active Set Strategy. 2 | %Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, 3 | %Christian Kirches et al. All rights reserved. 4 | % 5 | %qpOASES is distributed under the terms of the 6 | %GNU Lesser General Public License 2.1 in the hope that it will be 7 | %useful, but WITHOUT ANY WARRANTY; without even the implied warranty 8 | %of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 9 | %See the GNU Lesser General Public License for more details. 10 | % 11 | %--------------------------------------------------------------------------------- 12 | % 13 | %qpOASES solves (a series) of quadratic programming (QP) problems of the 14 | %following form: 15 | % 16 | % min 1/2*x'Hx + x'g 17 | % s.t. lb <= x <= ub 18 | % lbA <= Ax <= ubA {optional} 19 | % 20 | %Call 21 | % 22 | % [x,fval,exitflag,iter,lambda,auxOutput] = 23 | % qpOASES( H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} ) 24 | % 25 | %for solving the above-mentioned QP. H must be a symmetric (but possibly 26 | %indefinite) matrix and all vectors g, lb, ub, lbA, ubA have to be given 27 | %as column vectors. Options can be generated using the qpOASES_options command, 28 | %otherwise default values are used. Optionally, further auxiliary inputs 29 | %may be generated using qpOASES_auxInput command and passed to the solver. 30 | %Both matrices H or A may be passed in sparse matrix format. 31 | % 32 | %Call 33 | % 34 | % [x,fval,exitflag,iter,lambda,auxOutput] = 35 | % qpOASES( H,g,lb,ub{,options{,auxInput}} ) 36 | % 37 | %for solving the above-mentioned QP without general constraints. 38 | % 39 | % 40 | %Optional outputs (only x is mandatory): 41 | % x - Optimal primal solution vector (if exitflag==0). 42 | % fval - Optimal objective function value (if exitflag==0). 43 | % exitflag - 0: QP problem solved, 44 | % 1: QP could not be solved within given number of iterations, 45 | % -1: QP could not be solved due to an internal error, 46 | % -2: QP is infeasible (and thus could not be solved), 47 | % -3: QP is unbounded (and thus could not be solved). 48 | % iter - Number of active set iterations actually performed. 49 | % lambda - Optimal dual solution vector (if exitflag==0). 50 | % auxOutput - Struct containing auxiliary outputs as described below. 51 | % 52 | %The auxOutput struct contains the following entries: 53 | % workingSetB - Working set of bounds at point x. 54 | % workingSetC - Working set of constraints at point x. 55 | % The working set is a subset of the active set (indices 56 | % of bounds/constraints that hold with equality) yielding 57 | % a set linearly independent of bounds/constraints. 58 | % The working sets are encoded as follows: 59 | % 1: bound/constraint at its upper bound 60 | % 0: bound/constraint not at any bound 61 | % -1: bound/constraint at its lower bound 62 | % cpuTime - Internally measured CPU time for solving QP problem. 63 | % 64 | % 65 | %If not a single QP but a sequence of QPs with varying vectors is to be solved, 66 | %the i-th QP is given by the i-th columns of the QP vectors g, lb, ub, lbA, ubA 67 | %(i.e. they are matrices in this case). Both matrices H and A remain constant. 68 | % 69 | %See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES_SEQUENCE 70 | % 71 | % 72 | %For additional information see the qpOASES User's Manual or 73 | %visit http://www.qpOASES.org/. 74 | % 75 | %Please send remarks and questions to support@qpOASES.org! 76 | -------------------------------------------------------------------------------- /mex_core/Recover.c: -------------------------------------------------------------------------------- 1 | #include "mex.h" 2 | #include "string.h" 3 | 4 | #include "blas.h" 5 | 6 | void 7 | mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) 8 | { 9 | 10 | double *Q = mxGetPr( mxGetField(prhs[0], 0, "Q") ); 11 | double *S = mxGetPr( mxGetField(prhs[0], 0, "S") ); 12 | double *R = mxGetPr( mxGetField(prhs[0], 0, "R") ); 13 | double *A = mxGetPr( mxGetField(prhs[0], 0, "A") ); 14 | double *B = mxGetPr( mxGetField(prhs[0], 0, "B") ); 15 | double *Cx = mxGetPr( mxGetField(prhs[0], 0, "Cx") ); 16 | double *Cgx = mxGetPr( mxGetField(prhs[0], 0, "Cgx") ); 17 | double *Cgu = mxGetPr( mxGetField(prhs[0], 0, "Cgu") ); 18 | double *CgN = mxGetPr( mxGetField(prhs[0], 0, "CgN") ); 19 | double *gx = mxGetPr( mxGetField(prhs[0], 0, "gx") ); 20 | double *a = mxGetPr( mxGetField(prhs[0], 0, "a") ); 21 | double *ds0 = mxGetPr( mxGetField(prhs[0], 0, "ds0") ); 22 | 23 | size_t nx = mxGetScalar( mxGetField(prhs[1], 0, "nx") ); 24 | size_t nu = mxGetScalar( mxGetField(prhs[1], 0, "nu") ); 25 | size_t nc = mxGetScalar( mxGetField(prhs[1], 0, "nc") ); 26 | size_t ncN = mxGetScalar( mxGetField(prhs[1], 0, "ncN") ); 27 | size_t nbx = mxGetScalar( mxGetField(prhs[1], 0, "nbx") ); 28 | size_t N = mxGetScalar( mxGetField(prhs[1], 0, "N") ); 29 | 30 | double *dx = mxGetPr( mxGetField(prhs[0], 0, "dx") ); 31 | double *du = mxGetPr( mxGetField(prhs[0], 0, "du") ); 32 | double *lambda = mxGetPr( mxGetField(prhs[0], 0, "lambda_new") ); 33 | double *mu = mxGetPr( mxGetField(prhs[0], 0, "mu_new") ); 34 | double *mu_x = mxGetPr( mxGetField(prhs[0], 0, "mu_x_new") ); 35 | 36 | int i,j; 37 | 38 | char *nTrans = "N", *Trans="T"; 39 | double one_d = 1.0, zero = 0.0, minus_one_d = -1.0; 40 | size_t one_i = 1; 41 | 42 | memcpy(dx, ds0, nx*sizeof(double)); 43 | 44 | for (i=0;i0) 54 | dgemv(Trans,&ncN,&nx,&one_d,CgN,&ncN,mu+N*nc,&one_i,&one_d,lambda+N*nx,&one_i); 55 | if (nbx>0) 56 | dgemv(Trans,&nbx,&nx,&one_d,Cx,&nbx,mu_x+(N-1)*nbx,&one_i,&one_d,lambda+N*nx,&one_i); 57 | 58 | for (i=N-1;i>0;i--){ 59 | memcpy(lambda+i*nx,gx+i*nx, nx*sizeof(double)); 60 | dgemv(nTrans,&nx,&nx,&one_d,Q+i*nx*nx,&nx,dx+i*nx,&one_i,&one_d,lambda+i*nx,&one_i); 61 | dgemv(nTrans,&nx,&nu,&one_d,S+i*nx*nu,&nx,du+i*nu,&one_i,&one_d,lambda+i*nx,&one_i); 62 | dgemv(Trans,&nx,&nx,&one_d,A+i*nx*nx,&nx,lambda+(i+1)*nx,&one_i,&one_d,lambda+i*nx,&one_i); 63 | 64 | if (nc>0) 65 | dgemv(Trans,&nc,&nx,&one_d,Cgx+i*nc*nx,&nc,mu+i*nc,&one_i,&one_d,lambda+i*nx,&one_i); 66 | if (nbx>0) 67 | dgemv(Trans,&nbx,&nx,&one_d,Cx,&nbx,mu_x+(i-1)*nbx,&one_i,&one_d,lambda+i*nx,&one_i); 68 | } 69 | 70 | // lambda_0 has a different sign 71 | for (j=0;j0) 78 | dgemv(Trans,&nc,&nx,&minus_one_d,Cgx,&nc,mu,&one_i,&one_d,lambda,&one_i); 79 | 80 | } -------------------------------------------------------------------------------- /mex_core/full2sparse.c: -------------------------------------------------------------------------------- 1 | #include "mex.h" 2 | #include "string.h" 3 | 4 | #include "mpc_common.h" 5 | 6 | void 7 | mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) 8 | { 9 | /*Inputs*/ 10 | double *Q = mxGetPr( mxGetField(prhs[0], 0, "Q") ); 11 | double *S = mxGetPr( mxGetField(prhs[0], 0, "S") ); 12 | double *R = mxGetPr( mxGetField(prhs[0], 0, "R") ); 13 | double *A = mxGetPr( mxGetField(prhs[0], 0, "A") ); 14 | double *B = mxGetPr( mxGetField(prhs[0], 0, "B") ); 15 | double *Cx = mxGetPr( mxGetField(prhs[0], 0, "Cx") ); 16 | double *Cgx = mxGetPr( mxGetField(prhs[0], 0, "Cgx") ); 17 | double *Cgu = mxGetPr( mxGetField(prhs[0], 0, "Cgu") ); 18 | double *CgN = mxGetPr( mxGetField(prhs[0], 0, "CgN") ); 19 | double *gx = mxGetPr( mxGetField(prhs[0], 0, "gx") ); 20 | double *gu = mxGetPr( mxGetField(prhs[0], 0, "gu") ); 21 | double *a = mxGetPr( mxGetField(prhs[0], 0, "a") ); 22 | double *ds0 = mxGetPr( mxGetField(prhs[0], 0, "ds0") ); 23 | double *lc = mxGetPr( mxGetField(prhs[0], 0, "lc") ); 24 | double *uc = mxGetPr( mxGetField(prhs[0], 0, "uc") ); 25 | double *lb_dx = mxGetPr( mxGetField(prhs[0], 0, "lb_dx") ); 26 | double *ub_dx = mxGetPr( mxGetField(prhs[0], 0, "ub_dx") ); 27 | double *lb_du = mxGetPr( mxGetField(prhs[0], 0, "lb_du") ); 28 | double *ub_du = mxGetPr( mxGetField(prhs[0], 0, "ub_du") ); 29 | 30 | size_t nx = mxGetScalar( mxGetField(prhs[1], 0, "nx") ); 31 | size_t nu = mxGetScalar( mxGetField(prhs[1], 0, "nu") ); 32 | size_t nc = mxGetScalar( mxGetField(prhs[1], 0, "nc") ); 33 | size_t ncN = mxGetScalar( mxGetField(prhs[1], 0, "ncN") ); 34 | size_t nbx = mxGetScalar( mxGetField(prhs[1], 0, "nbx") ); 35 | size_t N = mxGetScalar( mxGetField(prhs[1], 0, "N") ); 36 | 37 | double *H = mxGetPr( mxGetField(prhs[0], 0, "sparse_H") ); 38 | double *g = mxGetPr( mxGetField(prhs[0], 0, "sparse_g") ); 39 | double *G = mxGetPr( mxGetField(prhs[0], 0, "sparse_G") ); 40 | double *dG = mxGetPr( mxGetField(prhs[0], 0, "sparse_dG") ); 41 | double *dB = mxGetPr( mxGetField(prhs[0], 0, "sparse_dB") ); 42 | double *ub = mxGetPr( mxGetField(prhs[0], 0, "sparse_ub") ); 43 | double *lb = mxGetPr( mxGetField(prhs[0], 0, "sparse_lb") ); 44 | double *minus_eye = mxGetPr( mxGetField(prhs[0], 0, "sparse_minus_eye") ); 45 | 46 | int i,j; 47 | size_t nz= nx+nu; 48 | size_t nw = N*nu + (N+1)*nx; 49 | size_t neq = (N+1)*nx; 50 | size_t nineq = N*nc+ncN; 51 | 52 | for (i=0;iforw_sens_flag = false; 53 | opts->adj_sens_flag = false; 54 | in = sim_in_create(opts); 55 | out = sim_out_create(opts); 56 | switch(sim_method){ 57 | case 1: 58 | erk_workspace = sim_erk_workspace_create(opts); 59 | sim_erk_workspace_init(opts, prhs[4], erk_workspace); 60 | break; 61 | case 2: 62 | irk_ode_workspace = sim_irk_ode_workspace_create(opts); 63 | sim_irk_ode_workspace_init(opts, prhs[4], irk_ode_workspace); 64 | break; 65 | case 3: 66 | irk_dae_workspace = sim_irk_dae_workspace_create(opts); 67 | sim_irk_dae_workspace_init(opts, prhs[4], irk_dae_workspace); 68 | break; 69 | default: 70 | mexErrMsgTxt("Please choose a supported integrator"); 71 | break; 72 | } 73 | 74 | mem_alloc=true; 75 | mexAtExit(exitFcn); 76 | } 77 | 78 | 79 | // integration 80 | switch(sim_method){ 81 | case 1: 82 | in->x = x; 83 | in->u = u; 84 | in->p = od; 85 | out->xn = x_out; 86 | sim_erk(in, out, opts, erk_workspace); 87 | break; 88 | case 2: 89 | in->x = x; 90 | in->u = u; 91 | in->p = od; 92 | in->z = z; 93 | out->xn = x_out; 94 | sim_irk_ode(in, out, opts, irk_ode_workspace); 95 | break; 96 | case 3: 97 | in->x = x; 98 | in->u = u; 99 | in->p = od; 100 | in->z = z; 101 | out->xn = x_out; 102 | out->zn = z_out; 103 | sim_irk_dae(in, out, opts, irk_dae_workspace); 104 | break; 105 | default: 106 | mexErrMsgTxt("Please choose a supported integrator"); 107 | break; 108 | } 109 | 110 | } -------------------------------------------------------------------------------- /examples/TethUAV.m: -------------------------------------------------------------------------------- 1 | %------------------------------------------% 2 | % Tethered quadcopter for a safe and constrained maneuver 3 | 4 | % "Online Nonlinear Model Predictive Control for Tethered UAVs to 5 | % Perform a Safe and Constrained Maneuver", E.Rossi et,al., 2019 6 | 7 | % typical configuration: N=30, Ts=Ts_st=0.01, no shifting 8 | 9 | %------------------------------------------% 10 | 11 | 12 | %% Dimensions 13 | 14 | nx=6; % No. of differential states 15 | nu=4; % No. of controls 16 | nz=0; % No. of algebraic states 17 | ny=12; % No. of outputs 18 | nyN=4; % No. of outputs at the terminal point 19 | np=2; % No. of model parameters 20 | nc=3;%0; % No. of general inequality constraints 21 | ncN=1;%1; % No. of general inequality constraints 22 | nbx = 2; % No. of bounds on states 23 | nbu = 2; % No. of bounds on controls 24 | 25 | % state and control bounds 26 | nbx_idx = 5:6; % indexs of states which are bounded 27 | nbu_idx = 3:4; % indexs of controls which are bounded 28 | 29 | %% create variables 30 | 31 | import casadi.* 32 | 33 | states = SX.sym('states',nx,1); % differential states 34 | controls = SX.sym('controls',nu,1); % control input 35 | alg = SX.sym('alg',nz,1); % algebraic states 36 | params = SX.sym('paras',np,1); % parameters 37 | refs = SX.sym('refs',ny,1); % references of the first N stages 38 | refN = SX.sym('refs',nyN,1); % reference of the last stage 39 | Q = SX.sym('Q',ny,1); % weighting matrix of the first N stages 40 | QN = SX.sym('QN',nyN,1); % weighting matrix of the last stage 41 | aux = SX.sym('aux',ny,1); % auxilary variable 42 | auxN = SX.sym('auxN',nyN,1); % auxilary variable 43 | 44 | 45 | %% Dynamics 46 | 47 | %Constant 48 | 49 | g = 9.81; % gravity [m/s^2] 50 | 51 | l = 1; % link length = 1[m] 52 | mR = 1; % drone mass [Kg] 53 | JR = 0.15; % drone inertia 54 | a1 = -g/l; 55 | a2 = 1/(mR*l); 56 | a3 = 1/JR; 57 | d = 0.2; 58 | 59 | phi=states(1); 60 | phi_dot=states(2); 61 | theta=states(3); 62 | theta_dot=states(4); 63 | f1=states(5);%b*omega1^2 64 | f2=states(6);%b*omega2^2 65 | df1=controls(1);%b*omega1^2 66 | df2=controls(2);%b*omega2^2 67 | s1 = controls(3); 68 | s2 = controls(4); 69 | phi_ref = params(1); 70 | theta_ref = params(2); 71 | 72 | % explicit ODE RHS 73 | x_dot = [phi_dot; a1*cos(phi); theta_dot; 0] + ... 74 | [0 0; 75 | a2*cos(phi+theta) 0; 76 | 0 0; 77 | 0 a3] * [f1; f2];%* [1 1; -d d] 78 | 79 | x_dot = [x_dot; 80 | df1; 81 | df2]; 82 | 83 | % algebraic function 84 | z_fun = []; 85 | 86 | % implicit ODE: impl_f = 0 87 | xdot = SX.sym('xdot',nx,1); 88 | impl_f = xdot - x_dot; 89 | 90 | %% Objectives and constraints 91 | phi_lim_vel = phi_ref + 20*pi/180; 92 | phi_lim = phi_ref + 16*pi/180; 93 | gamma2_vel = 180/pi/0.8; 94 | gamma1 = 1; 95 | gamma2 = 180/pi/3; 96 | 97 | % inner objectives 98 | h = [phi-phi_ref;phi_dot;theta-theta_ref;theta_dot;df1;df2; 99 | gamma1*(theta-theta_ref)*(1/(1+exp(-gamma2*(phi_lim-phi)))); % to arrive close to the ground with attitude = surface slope 100 | phi_dot*(1/(1+exp(-gamma2_vel*(phi_lim_vel-phi)))); % elev velocity goes to zero as the drone is closer to the surface 101 | theta_dot*(1/(1+exp(-gamma2_vel*(phi_lim_vel-phi)))); % attitude velocity goes to zero as the drone is closer to the surface 102 | 1/(phi+theta-pi/2);... 103 | s1;s2]; 104 | hN = [phi-phi_ref;phi_dot;theta-theta_ref;theta_dot]; 105 | 106 | % outer objectives 107 | obji = 0.5*(h-refs)'*diag(Q)*(h-refs); 108 | objN = 0.5*(hN-refN)'*diag(QN)*(hN-refN); 109 | 110 | obji_GGN = 0.5*(aux-refs)'*(aux-refs); 111 | objN_GGN = 0.5*(auxN-refN)'*(auxN-refN); 112 | 113 | % general inequality path constraints 114 | general_con = [1/a2*phi_dot^2 + a1/a2*sin(phi) + sin(phi+theta)*(f1+f2); 115 | d*sin(theta-pi/6)-l*sin(pi/6+phi)-s1; 116 | -d*sin(theta-pi/6)-l*sin(pi/6+phi)-s2]; 117 | general_con_N = [1/a2*phi_dot^2 + a1/a2*sin(phi) + sin(phi+theta)*(f1+f2)]; 118 | 119 | %% NMPC discretizing time length [s] 120 | 121 | Ts_st = 0.01; % shooting interval time 122 | -------------------------------------------------------------------------------- /mex_core/adaptive_eta.c: -------------------------------------------------------------------------------- 1 | #include "mex.h" 2 | #include "string.h" 3 | #include 4 | #include 5 | 6 | #include "blas.h" 7 | 8 | static double *dy=NULL; 9 | static bool mem_alloc = false; 10 | void exitFcn(){ 11 | if (mem_alloc){ 12 | mxFree(dy); 13 | } 14 | } 15 | 16 | void 17 | mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) 18 | { 19 | 20 | double *x_pri = mxGetPr( mxGetField(prhs[0], 0, "dx") ); 21 | double *u_pri = mxGetPr( mxGetField(prhs[0], 0, "du") ); 22 | double *q_dual = mxGetPr( mxGetField(prhs[0], 0, "q_dual") ); 23 | double *A = mxGetPr( mxGetField(prhs[0], 0, "A") ); 24 | double *B = mxGetPr( mxGetField(prhs[0], 0, "B") ); 25 | double *dmu = mxGetPr( mxGetField(prhs[0], 0, "dmu") ); 26 | 27 | double *threshold_pri = mxGetPr( mxGetField(prhs[0], 0, "threshold_pri") ); 28 | double *threshold_dual = mxGetPr( mxGetField(prhs[0], 0, "threshold_dual") ); 29 | double *tol = mxGetPr( mxGetField(prhs[0], 0, "tol") ); 30 | 31 | double alpha = mxGetScalar( mxGetField(prhs[0], 0, "alpha_cmon") ); 32 | double beta = mxGetScalar( mxGetField(prhs[0], 0, "beta_cmon") ); 33 | double c1 = mxGetScalar( mxGetField(prhs[0], 0, "c1") ); 34 | double rho_cmon = mxGetScalar( mxGetField(prhs[0], 0, "rho_cmon") ); 35 | double gamma = mxGetScalar( mxGetField(prhs[0], 0, "gamma") ); 36 | double tol_abs = mxGetScalar( mxGetField(prhs[0], 0, "tol_abs") ); 37 | double tol_ref = mxGetScalar( mxGetField(prhs[0], 0, "tol_ref") ); 38 | 39 | double *shift_x = mxGetPr( mxGetField(prhs[0], 0, "shift_x") ); 40 | double *shift_u = mxGetPr( mxGetField(prhs[0], 0, "shift_u") ); 41 | double *V_pri = mxGetPr( mxGetField(prhs[0], 0, "V_pri") ); 42 | double *V_dual = mxGetPr( mxGetField(prhs[0], 0, "V_dual") ); 43 | 44 | size_t nx = mxGetScalar( mxGetField(prhs[1], 0, "nx") ); 45 | size_t nu = mxGetScalar( mxGetField(prhs[1], 0, "nu") ); 46 | size_t nc = mxGetScalar( mxGetField(prhs[1], 0, "nc") ); 47 | size_t ncN = mxGetScalar( mxGetField(prhs[1], 0, "ncN") ); 48 | size_t nbx = mxGetScalar( mxGetField(prhs[1], 0, "nbx") ); 49 | size_t N = mxGetScalar( mxGetField(prhs[1], 0, "N") ); 50 | size_t np = mxGetScalar( mxGetField(prhs[1], 0, "np") ); if(np==0) np++; 51 | size_t ny = mxGetScalar( mxGetField(prhs[1], 0, "ny") ); 52 | 53 | size_t nx_t = (N+1)*nx; 54 | size_t nu_t = N*nu; 55 | size_t m = N*nx; 56 | size_t nv = nx+nu; 57 | size_t n = N*nv; 58 | size_t ns = (n+nx)+(m+nx)+(N*nu)+(N*nbx)+(N*nc+ncN); 59 | char *nTrans = "N", *Trans="T"; 60 | double one_d = 1.0, zero = 0.0, minus_one_d = 1.0; 61 | size_t one_i = 1; 62 | 63 | if (!mem_alloc){ 64 | dy = (double *)mxMalloc( ns*sizeof(double) ); 65 | mexMakeMemoryPersistent(dy); 66 | 67 | mem_alloc = true; 68 | mexAtExit(exitFcn); 69 | } 70 | 71 | int i,j; 72 | 73 | daxpy(&nx_t, &one_d, shift_x, &one_i, x_pri, &one_i); 74 | daxpy(&nu_t, &one_d, shift_u, &one_i, u_pri, &one_i); 75 | 76 | for (i=0;i0 44 | C=blkdiag(C, [mem.Cgx(:,i*nx+1:(i+1)*nx), mem.Cgu(:,i*nu+1:(i+1)*nu); 45 | -mem.Cgx(:,i*nx+1:(i+1)*nx) , -mem.Cgu(:,i*nu+1:(i+1)*nu); 46 | zeros(nbu,nx), Iu; 47 | zeros(nbu,nx), -Iu; 48 | Ix, zeros(nbx,nu); 49 | -Ix, zeros(nbx,nu)]); 50 | else 51 | C=blkdiag(C, [mem.Cgx(:,i*nx+1:(i+1)*nx), mem.Cgu(:,i*nu+1:(i+1)*nu); 52 | -mem.Cgx(:,i*nx+1:(i+1)*nx) , -mem.Cgu(:,i*nu+1:(i+1)*nu); 53 | zeros(nbu,nx), Iu; 54 | zeros(nbu,nx), -Iu]); 55 | end 56 | end 57 | C=blkdiag(C,[mem.CgN;-mem.CgN; 58 | Ix; 59 | -Ix]); 60 | 61 | blk4 = []; 62 | no_mu = zeros(1,N+1); 63 | no_mu(1) = 2*nc+2*nbu; 64 | for i=2:N 65 | no_mu(i) = 2*(nc+nbu+nbx); 66 | end 67 | no_mu(N+1) = 2*(ncN+nbx); 68 | 69 | dmu = mem.dmu(N*nu+N*nbx+1:end); 70 | dmu_u = mem.dmu(1:N*nu); 71 | dmu_x= mem.dmu(N*nu+1:N*nu+N*nbx); 72 | % dmu = mem.mu_new; 73 | % dmu_u = mem.mu_u_new; 74 | % dmu_x = mem.mu_x_new; 75 | 76 | for i=0:N-1 77 | if i>0 78 | mu_tmp = [max(dmu(i*nc+1:(i+1)*nc),0);min(dmu(i*nc+1:(i+1)*nc),0); 79 | max(dmu_u(i*nbu+1:(i+1)*nbu),0);min(dmu_u(i*nbu+1:(i+1)*nbu),0); 80 | max(dmu_x((i-1)*nbx+1:(i)*nbx),0);min(dmu_x((i-1)*nbx+1:(i)*nbx),0)]; 81 | blk4=[blk4; -mu_tmp.*C(sum(no_mu(1:i))+1:sum(no_mu(1:i+1)),:)]; 82 | else 83 | mu_tmp = [max(dmu(i*nc+1:(i+1)*nc),0);min(dmu(i*nc+1:(i+1)*nc),0); 84 | max(dmu_u(i*nbu+1:(i+1)*nbu),0);min(dmu_u(i*nbu+1:(i+1)*nbu),0)]; 85 | blk4=[blk4; -mu_tmp.*C(1:no_mu(1),:)]; 86 | end 87 | 88 | end 89 | mu_tmp = [max(dmu(N*nc+1:N*nc+ncN),0);min(dmu(N*nc+1:N*nc+ncN),0); 90 | max(dmu_x((N-1)*nbx+1:N*nbx),0);min(dmu_x((N-1)*nbx+1:N*nbx),0)]; 91 | blk4=[blk4; -mu_tmp.*C(sum(no_mu(1:N))+1:sum(no_mu(1:N+1)),:)]; 92 | 93 | c=[]; 94 | tmp1=[]; 95 | tmp2=[]; 96 | for i=0:N-1 97 | if i>0 98 | c=[c;-mem.uc(i*nc+1:(i+1)*nc); 99 | mem.lc(i*nc+1:(i+1)*nc)]; 100 | if nbu_idx>0 101 | tmp1 = mem.ub_du(i*nu+1:(i+1)*nu); 102 | tmp2 = mem.lb_du(i*nu+1:(i+1)*nu); 103 | tmp1 = tmp1(nbu_idx); 104 | tmp2 = tmp2(nbu_idx); 105 | end 106 | c=[c;-tmp1; 107 | tmp2; 108 | -mem.ub_dx((i-1)*nbx+1:i*nbx); 109 | mem.lb_dx((i-1)*nbx+1:i*nbx)]; 110 | else 111 | c=[c;-mem.uc(i*nc+1:(i+1)*nc); 112 | mem.lc(i*nc+1:(i+1)*nc)]; 113 | if nbu_idx>0 114 | tmp1 = mem.ub_du(i*nu+1:(i+1)*nu); 115 | tmp2 = mem.lb_du(i*nu+1:(i+1)*nu); 116 | tmp1 = tmp1(nbu_idx); 117 | tmp2 = tmp2(nbu_idx); 118 | end 119 | c=[c;-tmp1; 120 | tmp2;]; 121 | end 122 | end 123 | c=[c;-mem.uc(N*nc+1:end); 124 | mem.lc(N*nc+1:end); 125 | -mem.ub_dx((N-1)*nbx+1:N*nbx); 126 | mem.lb_dx((N-1)*nbx+1:N*nbx)]; 127 | blk5 = -diag(c); 128 | 129 | M=[H,C',BT; 130 | blk4, blk5, zeros(nineq, neq); 131 | B, zeros(neq,nineq), zeros(neq, neq)]; 132 | 133 | s = svd(M).^-1; 134 | rho = max(s); 135 | c2 = std(s) +1; 136 | 137 | end 138 | -------------------------------------------------------------------------------- /nmpc/mpc_nmpcsolver.m: -------------------------------------------------------------------------------- 1 | function [output, mem] = mpc_nmpcsolver(input, settings, mem, opt) 2 | 3 | 4 | mem.sqp_it=0; 5 | mem.alpha =1; 6 | mem.obj=0; 7 | StopCrit = 2*mem.kkt_lim; 8 | 9 | CPT.SHOOT=0; 10 | CPT.COND=0; 11 | CPT.QP=0; 12 | 13 | tic; 14 | 15 | while(mem.sqp_it < mem.sqp_maxit && StopCrit > mem.kkt_lim && mem.alpha>1E-8 ) % RTI or multiple call 16 | 17 | %% ----------- QP Preparation 18 | 19 | tshoot = tic; 20 | if opt.nonuniform_grid 21 | qp_generation_ngrid(input, settings, mem); 22 | else 23 | if strcmp(opt.qpsolver, 'qpoases_mb') 24 | qp_generation_mb(input, settings, mem); 25 | else 26 | qp_generation(input, settings, mem); 27 | % qp_generation_tac(input, settings, mem); 28 | end 29 | end 30 | 31 | tSHOOT = toc(tshoot)*1e3; 32 | 33 | switch opt.condensing 34 | case 'default_full' 35 | tcond=tic; 36 | if ~strcmp(opt.qpsolver, 'qpoases_mb') 37 | Condensing(mem, settings); 38 | else 39 | Condensing_mb(mem, settings); 40 | end 41 | tCOND=toc(tcond)*1e3; 42 | 43 | case 'hpipm_full' 44 | tcond=tic; 45 | condensing_hpipm(mem, settings); 46 | tCOND=toc(tcond)*1e3; 47 | case 'blasfeo_full' 48 | tcond=tic; 49 | Condensing_Blasfeo(mem, settings); 50 | tCOND=toc(tcond)*1e3; 51 | case 'partial_condensing' 52 | tcond=tic; 53 | mem.mem2 = Pcond(mem, settings, mem.mem2, mem.settings2); 54 | tCOND=toc(tcond)*1e3; 55 | case 'no' 56 | tCOND = 0; 57 | end 58 | 59 | %% ---------- Solving QP 60 | switch opt.qpsolver 61 | case 'qpoases' 62 | [tQP,mem] = mpc_qp_solve_qpoases(settings,mem); 63 | 64 | case 'qpoases_mb' 65 | [tQP,mem] = mpc_qp_solve_qpoases_mb(settings,mem, opt); 66 | 67 | case 'quadprog_dense' 68 | [tQP,mem] = mpc_qp_solve_quadprog(settings,mem); 69 | 70 | case 'hpipm_sparse' 71 | tqp=tic; 72 | hpipm_sparse(mem, settings); 73 | tQP = toc(tqp)*1e3; 74 | 75 | case 'hpipm_pcond' 76 | tqp=tic; 77 | hpipm_pcond(mem,settings); 78 | tQP = toc(tqp)*1e3; 79 | 80 | case 'ipopt_dense' 81 | [tQP,mem] = mpc_qp_solve_ipopt_dense(settings,mem); 82 | 83 | case 'ipopt_sparse' 84 | [tQP,mem] = mpc_qp_solve_ipopt_sparse(settings,mem); 85 | 86 | case 'ipopt_partial_sparse' 87 | [tQP, mem] = mpc_qp_solve_ipopt_partial_sparse(settings,mem.settings2,mem, mem.mem2); 88 | 89 | case 'osqp_sparse' 90 | [tQP, mem] = mpc_qp_solve_osqp(settings,mem); 91 | 92 | case 'osqp_partial_sparse' 93 | [tQP, mem] = mpc_qp_solve_osqp_partial(settings,mem.settings2,mem,mem.mem2); 94 | 95 | case 'qpalm_cond' 96 | [tQP, mem] = mpc_qp_solve_qpalm_cond(settings,mem); 97 | 98 | case 'qpalm_sparse' 99 | [tQP, mem] = mpc_qp_solve_qpalm_sparse(settings,mem); 100 | end 101 | 102 | 103 | %% ---------- Line search 104 | 105 | Line_search(mem, input, settings); 106 | 107 | %% ---------- CMoN 108 | % if mem.iter==1 && mem.sqp_it==0 109 | % [mem.rho_cmon, mem.gamma] = CMoN_Init(settings, mem, input); 110 | % end 111 | % adaptive_eta(mem,settings); 112 | 113 | %% ---------- KKT calculation 114 | 115 | [eq_res, ineq_res, KKT, OBJ] = solution_info(input, settings, mem); 116 | 117 | StopCrit = max([eq_res, ineq_res, KKT]); 118 | 119 | %% ---------- Multiple call management and convergence check 120 | 121 | CPT.SHOOT=CPT.SHOOT+tSHOOT; 122 | CPT.COND=CPT.COND+tCOND; 123 | CPT.QP=CPT.QP+tQP; 124 | 125 | mem.sqp_it=mem.sqp_it+1; 126 | 127 | end 128 | 129 | output.info.cpuTime=toc*1e3; % Total CPU time for the current sampling instant 130 | 131 | output.x=input.x; 132 | output.u=input.u; 133 | output.z=input.z; 134 | output.lambda=input.lambda; 135 | output.mu=input.mu; 136 | output.mu_x=input.mu_x; 137 | output.mu_u=input.mu_u; 138 | 139 | output.info.iteration_num=mem.sqp_it; 140 | output.info.kktValue=KKT; 141 | output.info.objValue=OBJ; 142 | output.info.OptCrit = StopCrit; 143 | output.info.eq_res=eq_res; 144 | output.info.ineq_res=ineq_res; 145 | output.info.shootTime=CPT.SHOOT; 146 | output.info.condTime=CPT.COND; 147 | output.info.qpTime=CPT.QP; 148 | 149 | end 150 | 151 | -------------------------------------------------------------------------------- /solver/linux/qpoases/qpOASES_sequence.m: -------------------------------------------------------------------------------- 1 | %qpOASES -- An Implementation of the Online Active Set Strategy. 2 | %Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 3 | %Christian Kirches et al. All rights reserved. 4 | % 5 | %qpOASES is distributed under the terms of the 6 | %GNU Lesser General Public License 2.1 in the hope that it will be 7 | %useful, but WITHOUT ANY WARRANTY; without even the implied warranty 8 | %of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 9 | %See the GNU Lesser General Public License for more details. 10 | % 11 | %--------------------------------------------------------------------------------- 12 | % 13 | %qpOASES_sequence is intended to solve a sequence of quadratic 14 | %programming (QP) problems of the following form: 15 | % 16 | % min 1/2*x'Hx + x'g 17 | % s.t. lb <= x <= ub 18 | % lbA <= Ax <= ubA {optional} 19 | % 20 | %I) Call 21 | % 22 | % [QP,x,fval,exitflag,iter,lambda,auxOutput] = ... 23 | % qpOASES_sequence( 'i',H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} ) 24 | %or 25 | % [QP,x,fval,exitflag,iter,lambda,auxOutput] = ... 26 | % qpOASES_sequence( 'i',H,g,lb,ub{,options{,auxInput}} ) 27 | % 28 | %for initialising and solving the first above-mentioned QP of the sequence 29 | %starting from an initial guess x0. H must be a symmetric (possibly indefinite) 30 | %matrix and all vectors g, lb, ub, lbA, ubA have to be given as column vectors. 31 | %Options can be generated using the qpOASES_options command, otherwise default 32 | %values are used. Optionally, further auxiliary inputs may be generated 33 | %using qpOASES_auxInput command and passed to the solver. 34 | %Both matrices H or A may be passed in sparse matrix format. 35 | % 36 | %II) Call 37 | % 38 | % [x,fval,exitflag,iter,lambda,auxOutput] = ... 39 | % qpOASES_sequence( 'h',QP,g,lb,ub,lbA,ubA{,options} ) 40 | %or 41 | % [x,fval,exitflag,iter,lambda,auxOutput] = ... 42 | % qpOASES_sequence( 'h',QP,g,lb,ub{,options} ) 43 | % 44 | %for hotstarting from the previous QP solution to the one of the next QP 45 | %given by the vectors g, lb, ub, lbA, ubA. Options can be generated using the 46 | %qpOASES_options command, otherwise default values are used. 47 | % 48 | %III) Call 49 | % 50 | % [x,fval,exitflag,iter,lambda,auxOutput] = ... 51 | % qpOASES_sequence( 'm',QP,H,g,A,lb,ub,lbA,ubA{,options} ) 52 | % 53 | %for hotstarting from the previous QP solution to the one of the next QP 54 | %given by the matrices H, A and the vectors g, lb, ub, lbA, ubA. The previous 55 | %active set serves as a starting guess. If the new projected Hessian matrix 56 | %turns out to be not positive definite, qpOASES recedes to a safe initial active 57 | %set guess automatically. This can result in a high number of iterations iter. 58 | %Options can be generated using the qpOASES_options command, otherwise default 59 | %values are used. 60 | % 61 | %IV) Call 62 | % 63 | % [x,lambda,workingSetB,workingSetC] = ... 64 | % qpOASES_sequence( 'e',QP,g,lb,ub,lbA,ubA{,options} ) 65 | % 66 | %for solving the equality constrained QP with constraints determined by the 67 | %current active set. All inequalities and bounds which were not active in the 68 | %previous solution might be violated. This command does not alter the internal 69 | %state of qpOASES. Instead of calling this command multiple times, it is 70 | %possible to supply several columns simultaneously in g, lb, ub, lbA, and ubA. 71 | %Options can be generated using the qpOASES_options command, otherwise default 72 | %values are used. 73 | % 74 | %V) Having solved the last QP of your sequence, call 75 | % 76 | % qpOASES_sequence( 'c',QP ) 77 | % 78 | %in order to cleanup the internal memory. 79 | % 80 | % 81 | %Optional outputs (only x is mandatory): 82 | % x - Optimal primal solution vector (if exitflag==0). 83 | % fval - Optimal objective function value (if exitflag==0). 84 | % exitflag - 0: QP solved, 85 | % 1: QP could not be solved within given number of iterations, 86 | % -1: QP could not be solved due to an internal error, 87 | % -2: QP is infeasible (and thus could not be solved), 88 | % -3: QP is unbounded (and thus could not be solved). 89 | % iter - Number of active set iterations actually performed. 90 | % lambda - Optimal dual solution vector (if exitflag==0). 91 | % auxOutput - Struct containing auxiliary outputs as described below. 92 | % 93 | %The auxOutput struct contains the following entries: 94 | % workingSetB - Working set of bounds at point x. 95 | % workingSetC - Working set of constraints at point x. 96 | % The working set is a subset of the active set (indices 97 | % of bounds/constraints that hold with equality) yielding 98 | % a set linearly independent of bounds/constraints. 99 | % The working sets are encoded as follows: 100 | % 1: bound/constraint at its upper bound 101 | % 0: bound/constraint not at any bound 102 | % -1: bound/constraint at its lower bound 103 | % cpuTime - Internally measured CPU time for solving QP problem. 104 | % 105 | %See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES 106 | % 107 | % 108 | %For additional information see the qpOASES User's Manual or 109 | %visit http://www.qpOASES.org/. 110 | % 111 | %Please send remarks and questions to support@qpOASES.org! 112 | -------------------------------------------------------------------------------- /solver/mac/qpoases/qpOASES_sequence.m: -------------------------------------------------------------------------------- 1 | %qpOASES -- An Implementation of the Online Active Set Strategy. 2 | %Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, 3 | %Christian Kirches et al. All rights reserved. 4 | % 5 | %qpOASES is distributed under the terms of the 6 | %GNU Lesser General Public License 2.1 in the hope that it will be 7 | %useful, but WITHOUT ANY WARRANTY; without even the implied warranty 8 | %of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 9 | %See the GNU Lesser General Public License for more details. 10 | % 11 | %--------------------------------------------------------------------------------- 12 | % 13 | %qpOASES_sequence is intended to solve a sequence of quadratic 14 | %programming (QP) problems of the following form: 15 | % 16 | % min 1/2*x'Hx + x'g 17 | % s.t. lb <= x <= ub 18 | % lbA <= Ax <= ubA {optional} 19 | % 20 | %I) Call 21 | % 22 | % [QP,x,fval,exitflag,iter,lambda,auxOutput] = ... 23 | % qpOASES_sequence( 'i',H,g,A,lb,ub,lbA,ubA{,options{,auxInput}} ) 24 | %or 25 | % [QP,x,fval,exitflag,iter,lambda,auxOutput] = ... 26 | % qpOASES_sequence( 'i',H,g,lb,ub{,options{,auxInput}} ) 27 | % 28 | %for initialising and solving the first above-mentioned QP of the sequence 29 | %starting from an initial guess x0. H must be a symmetric (possibly indefinite) 30 | %matrix and all vectors g, lb, ub, lbA, ubA have to be given as column vectors. 31 | %Options can be generated using the qpOASES_options command, otherwise default 32 | %values are used. Optionally, further auxiliary inputs may be generated 33 | %using qpOASES_auxInput command and passed to the solver. 34 | %Both matrices H or A may be passed in sparse matrix format. 35 | % 36 | %II) Call 37 | % 38 | % [x,fval,exitflag,iter,lambda,auxOutput] = ... 39 | % qpOASES_sequence( 'h',QP,g,lb,ub,lbA,ubA{,options} ) 40 | %or 41 | % [x,fval,exitflag,iter,lambda,auxOutput] = ... 42 | % qpOASES_sequence( 'h',QP,g,lb,ub{,options} ) 43 | % 44 | %for hotstarting from the previous QP solution to the one of the next QP 45 | %given by the vectors g, lb, ub, lbA, ubA. Options can be generated using the 46 | %qpOASES_options command, otherwise default values are used. 47 | % 48 | %III) Call 49 | % 50 | % [x,fval,exitflag,iter,lambda,auxOutput] = ... 51 | % qpOASES_sequence( 'm',QP,H,g,A,lb,ub,lbA,ubA{,options} ) 52 | % 53 | %for hotstarting from the previous QP solution to the one of the next QP 54 | %given by the matrices H, A and the vectors g, lb, ub, lbA, ubA. The previous 55 | %active set serves as a starting guess. If the new projected Hessian matrix 56 | %turns out to be not positive definite, qpOASES recedes to a safe initial active 57 | %set guess automatically. This can result in a high number of iterations iter. 58 | %Options can be generated using the qpOASES_options command, otherwise default 59 | %values are used. 60 | % 61 | %IV) Call 62 | % 63 | % [x,lambda,workingSetB,workingSetC] = ... 64 | % qpOASES_sequence( 'e',QP,g,lb,ub,lbA,ubA{,options} ) 65 | % 66 | %for solving the equality constrained QP with constraints determined by the 67 | %current active set. All inequalities and bounds which were not active in the 68 | %previous solution might be violated. This command does not alter the internal 69 | %state of qpOASES. Instead of calling this command multiple times, it is 70 | %possible to supply several columns simultaneously in g, lb, ub, lbA, and ubA. 71 | %Options can be generated using the qpOASES_options command, otherwise default 72 | %values are used. 73 | % 74 | %V) Having solved the last QP of your sequence, call 75 | % 76 | % qpOASES_sequence( 'c',QP ) 77 | % 78 | %in order to cleanup the internal memory. 79 | % 80 | % 81 | %Optional outputs (only x is mandatory): 82 | % x - Optimal primal solution vector (if exitflag==0). 83 | % fval - Optimal objective function value (if exitflag==0). 84 | % exitflag - 0: QP solved, 85 | % 1: QP could not be solved within given number of iterations, 86 | % -1: QP could not be solved due to an internal error, 87 | % -2: QP is infeasible (and thus could not be solved), 88 | % -3: QP is unbounded (and thus could not be solved). 89 | % iter - Number of active set iterations actually performed. 90 | % lambda - Optimal dual solution vector (if exitflag==0). 91 | % auxOutput - Struct containing auxiliary outputs as described below. 92 | % 93 | %The auxOutput struct contains the following entries: 94 | % workingSetB - Working set of bounds at point x. 95 | % workingSetC - Working set of constraints at point x. 96 | % The working set is a subset of the active set (indices 97 | % of bounds/constraints that hold with equality) yielding 98 | % a set linearly independent of bounds/constraints. 99 | % The working sets are encoded as follows: 100 | % 1: bound/constraint at its upper bound 101 | % 0: bound/constraint not at any bound 102 | % -1: bound/constraint at its lower bound 103 | % cpuTime - Internally measured CPU time for solving QP problem. 104 | % 105 | %See also QPOASES_OPTIONS, QPOASES_AUXINPUT, QPOASES 106 | % 107 | % 108 | %For additional information see the qpOASES User's Manual or 109 | %visit http://www.qpOASES.org/. 110 | % 111 | %Please send remarks and questions to support@qpOASES.org! 112 | -------------------------------------------------------------------------------- /mex_core/erk.c: -------------------------------------------------------------------------------- 1 | #include "mex.h" 2 | #include 3 | #include "string.h" 4 | 5 | #include "sim.h" 6 | #include "erk.h" 7 | #include "mpc_common.h" 8 | #include "casadi_wrapper.h" 9 | #include "blas.h" 10 | 11 | sim_erk_workspace* sim_erk_workspace_create(sim_opts *opts) 12 | { 13 | sim_erk_workspace* work = (sim_erk_workspace*)mxMalloc(sizeof(sim_erk_workspace)); 14 | 15 | work->A = mxMalloc(opts->num_stages*opts->num_stages*sizeof(double)); 16 | work->B = mxMalloc(opts->num_stages*sizeof(double)); 17 | work->Sx = mxMalloc(opts->nx*opts->nx*sizeof(double)); 18 | work->Su = mxMalloc(opts->nx*opts->nu*sizeof(double)); 19 | work->xt = mxMalloc(opts->nx*sizeof(double)); 20 | work->K = mxMalloc(opts->num_stages*opts->nx*sizeof(double)); 21 | 22 | mexMakeMemoryPersistent(work->A); 23 | mexMakeMemoryPersistent(work->B); 24 | mexMakeMemoryPersistent(work->Sx); 25 | mexMakeMemoryPersistent(work->Su); 26 | mexMakeMemoryPersistent(work->xt); 27 | mexMakeMemoryPersistent(work->K); 28 | 29 | if (opts->forw_sens_flag){ 30 | work->dKx = mxMalloc(opts->num_stages*opts->nx*opts->nx*sizeof(double)); 31 | work->dKu = mxMalloc(opts->num_stages*opts->nx*opts->nu*sizeof(double)); 32 | work->jacX_t = mxMalloc(opts->nx*opts->nx*sizeof(double)); 33 | work->jacU_t = mxMalloc(opts->nx*opts->nu*sizeof(double)); 34 | 35 | mexMakeMemoryPersistent(work->dKx); 36 | mexMakeMemoryPersistent(work->dKu); 37 | mexMakeMemoryPersistent(work->jacX_t); 38 | mexMakeMemoryPersistent(work->jacU_t); 39 | } 40 | 41 | if (opts->adj_sens_flag){ 42 | work->X_traj = mxMalloc(opts->num_steps*opts->num_stages*opts->nx*sizeof(double)); 43 | work->K_lambda = mxMalloc(opts->num_stages*(opts->nx+opts->nu)*sizeof(double)); 44 | work->lambda_t = mxCalloc((opts->nx+opts->nu), sizeof(double)); 45 | 46 | mexMakeMemoryPersistent(work->X_traj); 47 | mexMakeMemoryPersistent(work->K_lambda); 48 | mexMakeMemoryPersistent(work->lambda_t); 49 | } 50 | 51 | mexMakeMemoryPersistent(work); 52 | 53 | return work; 54 | } 55 | 56 | void sim_erk_workspace_init(sim_opts *opts, const mxArray *mem, sim_erk_workspace *work) 57 | { 58 | size_t nx = opts->nx; 59 | size_t nu = opts->nu; 60 | size_t num_stages = opts->num_stages; 61 | 62 | double *A = mxGetPr( mxGetField(mem, 0, "A_tab")); 63 | double *B = mxGetPr( mxGetField(mem, 0, "B_tab")); 64 | double *Sx = mxGetPr( mxGetField(mem, 0, "Sx")); 65 | double *Su = mxGetPr( mxGetField(mem, 0, "Su")); 66 | 67 | memcpy(work->A, A, num_stages*num_stages*sizeof(double)); 68 | memcpy(work->B, B, num_stages*sizeof(double)); 69 | memcpy(work->Sx, Sx, nx*nx*sizeof(double)); 70 | memcpy(work->Su, Su, nx*nu*sizeof(double)); 71 | } 72 | 73 | int sim_erk(sim_in *in, sim_out *out, sim_opts *opts, sim_erk_workspace *workspace) 74 | { 75 | int istep, s, i, j; 76 | double a,b; 77 | 78 | double one_d = 1.0, zero = 0.0, minus_one_d = -1.0; 79 | mwSignedIndex one_i = 1; 80 | 81 | double h= opts->h; 82 | size_t nx = opts->nx; 83 | size_t nu = opts->nu; 84 | size_t num_stages = opts->num_stages; 85 | size_t num_steps = opts->num_steps; 86 | bool forw_sens_flag = opts->forw_sens_flag; 87 | bool adj_sens_flag = opts->adj_sens_flag; 88 | 89 | double *x = in->x; 90 | double *lambda=in->lambda; 91 | 92 | double *xn = out->xn; 93 | double *Jac_x = out->Sx; 94 | double *Jac_u = out->Su; 95 | double *adj_sens = out->adj_sens; 96 | 97 | double *A = workspace->A; 98 | double *B = workspace->B; 99 | double *Sx = workspace->Sx; 100 | double *Su = workspace->Su; 101 | double *xt = workspace->xt; 102 | double *K = workspace->K; 103 | double *dKx = workspace->dKx; 104 | double *dKu = workspace->dKu; 105 | double *jacX_t = workspace->jacX_t; 106 | double *jacU_t = workspace->jacU_t; 107 | double *X_traj = workspace->X_traj; 108 | double *K_lambda = workspace->K_lambda; 109 | double *lambda_t = workspace->lambda_t; 110 | 111 | size_t jx = nx*nx; 112 | size_t ju = nx*nu; 113 | size_t nw = nx+nu; 114 | 115 | double *vde_in[6]; 116 | double *vde_out[2]; 117 | double *ode_in[4]; 118 | double *ode_out[1]; 119 | double *adj_in[5]; 120 | double *adj_out[2]; 121 | 122 | // initialize 123 | memcpy(xn, x, nx*sizeof(double)); 124 | set_zeros(num_stages*nx, K); 125 | 126 | if (forw_sens_flag){ 127 | memcpy(Jac_x, Sx, nx*nx*sizeof(double)); 128 | memcpy(Jac_u, Su, nx*nu*sizeof(double)); 129 | } 130 | 131 | ode_in[1] = in->u; 132 | ode_in[2] = in->p; 133 | ode_in[3] = 0; 134 | 135 | if (forw_sens_flag){ 136 | vde_in[1] = in->u; 137 | vde_in[2] = in->p; 138 | vde_in[5] = 0; 139 | } 140 | 141 | if (adj_sens_flag){ 142 | adj_in[1] = in->u; 143 | adj_in[2] = in->p; 144 | adj_in[4] = 0; 145 | 146 | for (j=0;j -1; istep--) { 215 | for (s = num_stages-1; s > -1; s--){ 216 | memcpy(lambda_t, adj_sens, nx*sizeof(double)); 217 | if (sA); 240 | mxFree(work->B); 241 | mxFree(work->Sx); 242 | mxFree(work->Su); 243 | mxFree(work->xt); 244 | mxFree(work->K); 245 | 246 | if(opts->forw_sens_flag){ 247 | mxFree(work->dKx); 248 | mxFree(work->dKu); 249 | mxFree(work->jacX_t); 250 | mxFree(work->jacU_t); 251 | } 252 | 253 | if(opts->adj_sens_flag){ 254 | mxFree(work->X_traj); 255 | mxFree(work->K_lambda); 256 | mxFree(work->lambda_t); 257 | } 258 | 259 | mxFree(work); 260 | } -------------------------------------------------------------------------------- /Simulation.m: -------------------------------------------------------------------------------- 1 | clear all; clear mex; close all;clc; 2 | 3 | disp( ' ' ); 4 | disp( 'MATMPC -- A (MAT)LAB based Model(M) Predictive(P) Control(C) Package.' ); 5 | disp( 'Copyright (C) 2016-2019 by Yutao Chen, University of Padova' ); 6 | disp( 'All rights reserved.' ); 7 | disp( ' ' ); 8 | disp( 'MATMPC is distributed under the terms of the' ); 9 | disp( 'GNU General Public License 3.0 in the hope that it will be' ); 10 | disp( 'useful, but WITHOUT ANY WARRANTY; without even the implied warranty' ); 11 | disp( 'of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.' ); 12 | disp( 'See the GNU General Public License for more details.' ); 13 | disp( ' ' ); 14 | disp( ' ' ); 15 | disp('---------------------------------------------------------------------------------'); 16 | 17 | %% Configuration (complete your configuration here...) 18 | addpath([pwd,'/nmpc']); 19 | addpath([pwd,'/model_src']); 20 | addpath([pwd,'/mex_core']); 21 | addpath(genpath([pwd,'/data'])); 22 | if ismac 23 | addpath(genpath([pwd,'/solver/mac'])); 24 | elseif isunix 25 | addpath(genpath([pwd,'/solver/linux'])); 26 | elseif ispc 27 | addpath(genpath([pwd,'/solver/win64'])); 28 | else 29 | disp('Platform not supported') 30 | end 31 | 32 | cd data; 33 | if exist('settings','file')==2 34 | load settings 35 | cd .. 36 | else 37 | cd .. 38 | error('No setting data is detected!'); 39 | end 40 | 41 | Ts = settings.Ts_st; % Closed-loop sampling time (usually = shooting interval) 42 | 43 | Ts_st = settings.Ts_st; % Shooting interval 44 | nx = settings.nx; % No. of states 45 | nu = settings.nu; % No. of controls 46 | ny = settings.ny; % No. of outputs (references) 47 | nyN= settings.nyN; % No. of outputs at terminal stage 48 | np = settings.np; % No. of parameters (on-line data) 49 | nc = settings.nc; % No. of constraints 50 | ncN = settings.ncN; % No. of constraints at terminal stage 51 | nbx = settings.nbx; % No. of state bounds 52 | 53 | %% solver configurations 54 | 55 | N = 80; % No. of shooting points 56 | settings.N = N; 57 | 58 | N2 = N/5; 59 | settings.N2 = N2; % No. of horizon length after partial condensing (N2=1 means full condensing) 60 | 61 | r = 10; 62 | settings.r = r; % No. of input blocks (go to InitMemory.m, line 441 to configure) 63 | 64 | opt.hessian = 'Gauss_Newton'; % 'Gauss_Newton', 'Generalized_Gauss_Newton' 65 | opt.integrator = 'ERK4'; % 'ERK4','IRK3','IRK3-DAE' 66 | opt.condensing = 'default_full'; %'default_full','no','blasfeo_full(require blasfeo installed)','partial_condensing' 67 | opt.qpsolver = 'qpoases'; 68 | opt.hotstart = 'no'; %'yes','no' (only for qpoases, use 'no' for nonlinear systems) 69 | opt.shifting = 'no'; % 'yes','no' 70 | opt.ref_type = 0; % 0-time invariant, 1-time varying(no preview), 2-time varying (preview) 71 | opt.nonuniform_grid = 0; % if use non-uniform grid discretization (go to InitMemory.m, line 459 to configure) 72 | opt.RTI = 'yes'; % if use Real-time Iteration 73 | %% available qpsolver 74 | 75 | %'qpoases' (condensing is needed) 76 | %'qpoases_mb' (move blocking strategy) 77 | %'quadprog_dense' (for full condensing) 78 | %'hpipm_sparse' (run mex_core/compile_hpipm.m first; set opt.condensing='no') 79 | %'hpipm_pcond' (run mex_core/compile_hpipm.m first; set opt.condensing='no') 80 | %'ipopt_dense' (install OPTI Toolbox first; for full condensing) 81 | %'ipopt_sparse' (install OPTI Toolbox first; set opt.condensing='no') 82 | %'ipopt_partial_sparse'(set opt.condensing='partial_condensing'; only for state and control bounded problems) 83 | %'osqp_sparse' (set opt.condensing='no') 84 | %'osqp_partial_sparse' (set opt.condensing='partial_condensing') 85 | %'qpalm_cond' (condensing is needed) 86 | %'qpalm_sparse'(set opt.condensing='no') 87 | 88 | %% Initialize Data (all users have to do this) 89 | if opt.nonuniform_grid 90 | [input, data] = InitData_ngrid(settings); 91 | N = r; 92 | settings.N = N; 93 | else 94 | [input, data] = InitData(settings); 95 | end 96 | 97 | %% Initialize Solvers (only for advanced users) 98 | 99 | mem = InitMemory(settings, opt, input); 100 | 101 | %% Simulation (start your simulation...) 102 | 103 | mem.iter = 1; time = 0.0; 104 | Tf = 4; % simulation time 105 | state_sim= input.x0'; 106 | controls_MPC = input.u0'; 107 | y_sim = []; 108 | constraints = []; 109 | CPT = []; 110 | ref_traj = []; 111 | KKT = []; 112 | OBJ=[]; 113 | numIT=[]; 114 | 115 | while time(end) < Tf 116 | 117 | % the reference input.y is a ny by N matrix 118 | % the reference input.yN is a nyN by 1 vector 119 | switch opt.ref_type 120 | case 0 % time-invariant reference 121 | input.y = repmat(data.REF',1,N); 122 | input.yN = data.REF(1:nyN)'; 123 | case 1 % time-varying reference (no reference preview) 124 | input.y = repmat(data.REF(mem.iter,:)',1,N); 125 | input.yN = data.REF(mem.iter,1:nyN)'; 126 | case 2 %time-varying reference (reference preview) 127 | input.y = data.REF(mem.iter:mem.iter+N-1,:)'; 128 | input.yN = data.REF(mem.iter+N,1:nyN)'; 129 | end 130 | 131 | % obtain the state measurement 132 | input.x0 = state_sim(end,:)'; 133 | 134 | % call the NMPC solver 135 | [output, mem] = mpc_nmpcsolver(input, settings, mem, opt); 136 | 137 | % obtain the solution and update the data 138 | switch opt.shifting 139 | case 'yes' 140 | input.x=[output.x(:,2:end),output.x(:,end)]; 141 | input.u=[output.u(:,2:end),output.u(:,end)]; 142 | input.z=[output.z(:,2:end),output.z(:,end)]; 143 | input.lambda=[output.lambda(:,2:end),output.lambda(:,end)]; 144 | input.mu=[output.mu(nc+1:end);output.mu(end-nc+1:end)]; 145 | input.mu_x=[output.mu_x(nbx+1:end);output.mu_x(end-nbx+1:end)]; 146 | input.mu_u=[output.mu_u(nu+1:end);output.mu_u(end-nu+1:end)]; 147 | 148 | % for CMoN-RTI 149 | % mem.A=[mem.A(:,nx+1:end),mem.A(:,end-nx+1:end)]; 150 | % mem.B=[mem.B(:,nu+1:end),mem.B(:,end-nu+1:end)]; 151 | % mem.F_old = [mem.F_old(:,2:end),mem.F_old(:,end)]; 152 | % mem.V_pri = [mem.V_pri(:,2:end),mem.V_pri(:,end)]; 153 | % mem.V_dual = [mem.V_dual(:,2:end),mem.V_dual(:,end)]; 154 | % mem.q_dual = [mem.q_dual(:,2:end),mem.q_dual(:,end)]; 155 | % mem.shift_x = input.x-output.x; 156 | % mem.shift_u = input.u-output.u; 157 | case 'no' 158 | input.x=output.x; 159 | input.u=output.u; 160 | input.z=output.z; 161 | input.lambda=output.lambda; 162 | input.mu=output.mu; 163 | input.mu_x=output.mu_x; 164 | input.mu_u=output.mu_u; 165 | end 166 | 167 | % collect the statistics 168 | cpt=output.info.cpuTime; 169 | tshooting=output.info.shootTime; 170 | tcond=output.info.condTime; 171 | tqp=output.info.qpTime; 172 | OptCrit=output.info.OptCrit; 173 | 174 | % Simulate system dynamics 175 | sim_input.x = state_sim(end,:).'; 176 | sim_input.u = output.u(:,1); 177 | sim_input.z = input.z(:,1); 178 | sim_input.p = input.od(:,1); 179 | 180 | [xf, zf] = Simulate_System(sim_input.x, sim_input.u, sim_input.z, sim_input.p, mem, settings); 181 | xf = full(xf); 182 | 183 | % Collect outputs 184 | y_sim = [y_sim; full(h_fun('h_fun', xf, sim_input.u, sim_input.p))']; 185 | 186 | % Collect constraints 187 | constraints=[constraints; full( path_con_fun('path_con_fun', xf, sim_input.u, sim_input.p) )']; 188 | 189 | % store the optimal solution and states 190 | controls_MPC = [controls_MPC; output.u(:,1)']; 191 | state_sim = [state_sim; xf']; 192 | KKT= [KKT;OptCrit]; 193 | OBJ= [OBJ;output.info.objValue]; 194 | CPT = [CPT; cpt, tshooting, tcond, tqp]; 195 | numIT = [numIT; output.info.iteration_num]; 196 | 197 | % go to the next sampling instant 198 | nextTime = mem.iter*Ts; 199 | mem.iter = mem.iter+1; 200 | disp(['current time:' num2str(nextTime) ' CPT:' num2str(cpt) 'ms SHOOTING:' num2str(tshooting) 'ms COND:' num2str(tcond) 'ms QP:' num2str(tqp) 'ms Opt:' num2str(OptCrit) ' OBJ:' num2str(OBJ(end)) ' SQP_IT:' num2str(output.info.iteration_num)]); 201 | % disp(['current time:' num2str(nextTime) ' CPT:' num2str(cpt) 'ms SHOOTING:' num2str(tshooting) 'ms COND:' num2str(tcond) 'ms QP:' num2str(tqp) 'ms Opt:' num2str(OptCrit) ' OBJ:' num2str(OBJ(end)) ' SQP_IT:' num2str(output.info.iteration_num) ' Perc:' num2str(mem.perc)]); 202 | time = [time nextTime]; 203 | end 204 | 205 | %% 206 | if strcmp(opt.qpsolver, 'qpoases') 207 | qpOASES_sequence( 'c', mem.warm_start); 208 | end 209 | % if strcmp(opt.qpsolver, 'qpalm') 210 | % mem.qpalm_solver.delete(); 211 | % end 212 | clear mex; 213 | 214 | %% draw pictures (optional) 215 | disp(['Average CPT: ', num2str(mean(CPT(2:end,:),1)) ]); 216 | disp(['Maximum CPT: ', num2str(max(CPT(2:end,:))) ]); 217 | 218 | Draw; 219 | -------------------------------------------------------------------------------- /mex_core/Condensing.c: -------------------------------------------------------------------------------- 1 | 2 | #include "mex.h" 3 | #include 4 | #include "string.h" 5 | 6 | #include "mpc_common.h" 7 | 8 | #include "blas.h" 9 | 10 | static double *L = NULL, *G=NULL, *W_mat=NULL, *w_vec=NULL; 11 | static double *Hi = NULL, *Cci = NULL, *Ccxi = NULL, *CcN = NULL; 12 | static bool mem_alloc = false; 13 | 14 | void exitFcn(){ 15 | if (mem_alloc){ 16 | mxFree(G); 17 | mxFree(W_mat); 18 | mxFree(w_vec); 19 | mxFree(L); 20 | mxFree(Hi); 21 | mxFree(Cci); 22 | mxFree(CcN); 23 | mxFree(Ccxi); 24 | } 25 | } 26 | 27 | void 28 | mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) 29 | { 30 | /*Inputs*/ 31 | double *Q = mxGetPr( mxGetField(prhs[0], 0, "Q") ); 32 | double *S = mxGetPr( mxGetField(prhs[0], 0, "S") ); 33 | double *R = mxGetPr( mxGetField(prhs[0], 0, "R") ); 34 | double *A = mxGetPr( mxGetField(prhs[0], 0, "A") ); 35 | double *B = mxGetPr( mxGetField(prhs[0], 0, "B") ); 36 | double *Cx = mxGetPr( mxGetField(prhs[0], 0, "Cx") ); 37 | double *Cgx = mxGetPr( mxGetField(prhs[0], 0, "Cgx") ); 38 | double *Cgu = mxGetPr( mxGetField(prhs[0], 0, "Cgu") ); 39 | double *CgN = mxGetPr( mxGetField(prhs[0], 0, "CgN") ); 40 | double *gx = mxGetPr( mxGetField(prhs[0], 0, "gx") ); 41 | double *gu = mxGetPr( mxGetField(prhs[0], 0, "gu") ); 42 | double *a = mxGetPr( mxGetField(prhs[0], 0, "a") ); 43 | double *ds0 = mxGetPr( mxGetField(prhs[0], 0, "ds0") ); 44 | double *lc = mxGetPr( mxGetField(prhs[0], 0, "lc") ); 45 | double *uc = mxGetPr( mxGetField(prhs[0], 0, "uc") ); 46 | double *lb_dx = mxGetPr( mxGetField(prhs[0], 0, "lb_dx") ); 47 | double *ub_dx = mxGetPr( mxGetField(prhs[0], 0, "ub_dx") ); 48 | 49 | size_t nx = mxGetScalar( mxGetField(prhs[1], 0, "nx") ); 50 | size_t nu = mxGetScalar( mxGetField(prhs[1], 0, "nu") ); 51 | size_t nc = mxGetScalar( mxGetField(prhs[1], 0, "nc") ); 52 | size_t ncN = mxGetScalar( mxGetField(prhs[1], 0, "ncN") ); 53 | size_t nbx = mxGetScalar( mxGetField(prhs[1], 0, "nbx") ); 54 | size_t N = mxGetScalar( mxGetField(prhs[1], 0, "N") ); 55 | 56 | /*Outputs*/ 57 | double *Hc, *gc, *Ccg, *Ccx, *lcc, *ucc, *lxc, *uxc; 58 | 59 | Hc = mxGetPr( mxGetField(prhs[0], 0, "Hc") ); 60 | gc = mxGetPr( mxGetField(prhs[0], 0, "gc") ); 61 | Ccg = mxGetPr( mxGetField(prhs[0], 0, "Ccg") ); 62 | Ccx = mxGetPr( mxGetField(prhs[0], 0, "Ccx") ); 63 | lcc = mxGetPr( mxGetField(prhs[0], 0, "lcc") ); 64 | ucc = mxGetPr( mxGetField(prhs[0], 0, "ucc") ); 65 | lxc = mxGetPr( mxGetField(prhs[0], 0, "lxc") ); 66 | uxc = mxGetPr( mxGetField(prhs[0], 0, "uxc") ); 67 | 68 | int iter = mxGetScalar( mxGetField(prhs[0], 0, "iter") ); 69 | int hot_start = mxGetScalar( mxGetField(prhs[0], 0, "hot_start") ); 70 | 71 | /*Allocate memory*/ 72 | int i=0,j=0; 73 | 74 | if (!mem_alloc){ 75 | G = (double *)mxMalloc(nx*N*N*nu * sizeof(double)); 76 | mexMakeMemoryPersistent(G); 77 | W_mat = (double *)mxMalloc(nx*N*N*nu * sizeof(double)); 78 | mexMakeMemoryPersistent(W_mat); 79 | w_vec = (double *)mxMalloc(nx*N * sizeof(double)); 80 | mexMakeMemoryPersistent(w_vec); 81 | L = (double *)mxMalloc((N+1)*nx * sizeof(double)); 82 | mexMakeMemoryPersistent(L); 83 | Hi = (double *)mxMalloc(nu*nu * sizeof(double)); 84 | mexMakeMemoryPersistent(Hi); 85 | Cci = (double *)mxMalloc(nc*nu * sizeof(double)); 86 | mexMakeMemoryPersistent(Cci); 87 | CcN = (double *)mxMalloc(ncN*nu * sizeof(double)); 88 | mexMakeMemoryPersistent(CcN); 89 | Ccxi = (double *)mxMalloc(nbx*nu * sizeof(double)); 90 | mexMakeMemoryPersistent(Ccxi); 91 | 92 | mem_alloc = true; 93 | mexAtExit(exitFcn); 94 | } 95 | 96 | char *nTrans = "N", *Trans="T", *SIDE = "L", *UPLO = "L"; 97 | double one_d = 1.0, zero = 0.0, minus_one = -1.0; 98 | size_t one_i = 1; 99 | 100 | /*Start the loop*/ 101 | 102 | /* compute G */ 103 | for(i=0;ii;j--){ 114 | dgemm(Trans, nTrans, &nu, &nu, &nx, &one_d, S+j*nx*nu, &nx, G+(i*N+j-1)*nx*nu, &nx, &zero, Hi, &nu); 115 | dgemm(Trans, nTrans, &nu, &nu, &nx, &one_d, B+j*nx*nu, &nx, W_mat+(i*N+j)*nx*nu, &nx, &one_d, Hi, &nu); 116 | Block_Fill(nu, nu, Hi, Hc, j*nu, i*nu, N*nu); 117 | Block_Fill_Trans(nu, nu, Hi, Hc, i*nu, j*nu, N*nu); 118 | dgemm(Trans, nTrans, &nx, &nu, &nx, &one_d, A+j*nx*nx, &nx, W_mat+(i*N+j)*nx*nu, &nx, &zero, W_mat+(i*N+j-1)*nx*nu, &nx); 119 | dsymm(SIDE, UPLO, &nx, &nu, &one_d, Q+j*nx*nx, &nx, G+(i*N+j-1)*nx*nu, &nx, &one_d, W_mat+(i*N+j-1)*nx*nu, &nx); 120 | } 121 | memcpy(Hi,R+i*nu*nu,nu*nu*sizeof(double)); 122 | dgemm(Trans, nTrans, &nu, &nu, &nx, &one_d, B+i*nx*nu, &nx, W_mat+(i*N+i)*nx*nu, &nx, &one_d, Hi, &nu); 123 | Block_Fill(nu, nu, Hi, Hc, i*nu, i*nu, N*nu); 124 | } 125 | 126 | /* Compute Cc */ 127 | if (nc>0){ 128 | for(i=0;i0){ 139 | for(i=0;i0){ 147 | for(i=0;i0;i--){ 167 | memcpy(gc+i*nu,gu+i*nu,nu*sizeof(double)); 168 | dgemv(Trans,&nx,&nu,&one_d,S+i*nx*nu,&nx,L+i*nx,&one_i,&one_d,gc+i*nu,&one_i); 169 | dgemv(Trans,&nx,&nu,&one_d,B+i*nx*nu,&nx,w_vec+i*nx,&one_i,&one_d,gc+i*nu,&one_i); 170 | 171 | memcpy(w_vec+(i-1)*nx, gx+i*nx, nx*sizeof(double)); 172 | dsymv(UPLO, &nx, &one_d, Q+i*nx*nx, &nx, L+i*nx, &one_i, &one_d, w_vec+(i-1)*nx, &one_i); 173 | dgemv(Trans,&nx,&nx,&one_d,A+i*nx*nx,&nx,w_vec+i*nx,&one_i,&one_d,w_vec+(i-1)*nx,&one_i); 174 | } 175 | memcpy(gc,gu,nu*sizeof(double)); 176 | dgemv(Trans,&nx,&nu,&one_d,S,&nx,L,&one_i,&one_d,gc,&one_i); 177 | dgemv(Trans,&nx,&nu,&one_d,B,&nx,w_vec,&one_i,&one_d,gc,&one_i); 178 | 179 | /* Compute cc */ 180 | if (nc>0){ 181 | for(i=0;i0){ 192 | dgemv(nTrans,&ncN,&nx,&minus_one,CgN,&ncN,L+N*nx,&one_i,&zero,lcc+N*nc,&one_i); 193 | for(j=0;j0){ 201 | for(i=0;i