1% compile the C code 2mex HPIPM_d_solve_ipm2_hard_ocp_qp.c /opt/hpipm/lib/libhpipm.a /opt/blasfeo/lib/libblasfeo.a -I/opt/hpipm/include -I/opt/blasfeo/include % linear algebra in BLASFEO 3 4% import cool graphic toolkit if in octave 5if is_octave() 6 graphics_toolkit('gnuplot') 7end 8 9 10 11% test problem 12 13nx = 8; % number of states 14nu = 3; % number of inputs (controls) 15N = 30; % horizon length 16 17nb = nu+nx/2; % number of two-sided box constraints 18ng = 0; % number of two-sided general constraints 19ngN = nx; % number of two-sided general constraints on last stage 20 21time_invariant = 0; % time_invariant (0) vs time_variant (1) problems 22 23nbu = min(nb, nu); 24 25Ts = 0.5; % sampling time 26 27Ac = [zeros(nx/2), eye(nx/2); diag(-2*ones(nx/2,1))+diag(ones(nx/2-1,1),-1)+diag(ones(nx/2-1,1),1), zeros(nx/2) ]; 28Bc = [zeros(nx/2,nu); eye(nu); zeros(nx/2-nu, nu)]; 29 30M = expm([Ts*Ac, Ts*Bc; zeros(nu, 2*nx/2+nu)]); 31 32% dynamica system 33A = M(1:nx,1:nx); 34B = M(1:nx,nx+1:end); 35b = 0.0*ones(nx,1); 36x0 = zeros(nx, 1); 37x0(1) = 3.5; 38x0(2) = 3.5; 39AA = repmat(A, 1, N); 40BB = repmat(B, 1, N); 41bb = repmat(b, 1, N); 42 43% cost function 44Q = eye(nx); 45Qf = Q; 46R = 2*eye(nu); 47S = zeros(nx, nu); 48q = zeros(nx,1); 49%q = 1*Q*[ones(nx/2,1); zeros(nx/2,1)]; 50qf = q; 51r = zeros(nu,1); 52%r = 1*R*ones(nu,1); 53QQ = repmat(Q, 1, N); 54SS = repmat(S, 1, N); 55RR = repmat(R, 1, N); 56qq = repmat(q, 1, N); 57rr = repmat(r, 1, N); 58 59% general constraints 60C = zeros(ng, nx); 61D = zeros(ng, nu); 62CN = zeros(ngN, nx); 63%if(ng>0) 64% if(nb==0) 65% for ii=1:min(nu,ng) 66% D(ii,ii) = 1.0; 67% end 68% for ii=min(nu,ng)+1:ng 69% C(ii,ii-nu) = 1.0; 70% end 71% else 72% for ii=1:ng 73% C(ii,ii) = 1.0; 74% end 75% end 76%end 77if(ngN>0) 78 for ii=1:ngN 79 CN(ii,ii) = 1.0; 80 end 81end 82CC = [zeros(ng,nx), repmat(C, 1, N-1)]; 83DD = [repmat(D, 1, N)]; 84 85% box constraints 86lb = zeros(nb,1); 87ub = zeros(nb,1); 88for ii=1:nbu 89 lb(ii) = -0.5; % lower bound 90 ub(ii) = 0.5; % - upper bound 91end 92for ii=nbu+1:nb 93 lb(ii) = -10; 94 ub(ii) = 10; 95end 96llb = repmat(lb, 1, N+1); 97uub = repmat(ub, 1, N+1); 98% general constraints 99lg = zeros(ng,1); 100ug = zeros(ng,1); 101%if(ng>0) 102% if(nb==0) 103% for ii=1:min(nu,ng) 104% lg(ii) = -2.5; % lower bound 105% ug(ii) = -0.1; % - upper bound 106% end 107% for ii=min(nu,ng)+1:ng 108% lg(ii) = -10; 109% ug(ii) = 10; 110% end 111% else 112% for ii=1:ng 113% lg(ii) = -10; 114% ug(ii) = 10; 115% end 116% end 117%end 118%db(2*nu+1:end) = -4; 119llg = repmat(lg, 1, N); 120uug = repmat(ug, 1, N); 121% general constraints last stage 122lgN = zeros(ngN,1); 123ugN = zeros(ngN,1); 124if(ngN>0) 125 if(nb==0) 126% for ii=1:min(nu,ng) 127% lg(ii) = -2.5; % lower bound 128% ug(ii) = -0.1; % - upper bound 129% end 130 for ii=min(nu,ng)+1:ng 131 lgN(ii) = 0; 132 ugN(ii) = 0; 133 end 134 else 135 for ii=1:ngN 136 lgN(ii) = 0; 137 ugN(ii) = 0; 138 end 139 end 140end 141%if(ng>0) 142% if(nb==0) 143% uub(1:min(nu,ng),N+1) = 0.0; 144% end 145%end 146 147% initial guess for states and inputs 148x = zeros(nx, N+1); x(:,1) = x0; % initial condition 149u = zeros(nu, N); 150mult_pi = zeros(nx,N); 151mult_lam = zeros(2*(nb+ng)*N+2*(nb+ngN),1); 152mult_t = zeros(2*(nb+ng)*N+2*(nb+ngN),1); 153 154free_x0 = 0; % consider x0 as optimization variable 155warm_start = 0; % read initial guess from x and u 156 157mu0 = 2; % max element in cost function as estimate of max multiplier 158kk = -1; % actual number of performed iterations 159k_max = 20; % maximim number of iterations 160tol = 1e-8; % tolerance in the duality measure 161infos = zeros(5, k_max); 162inf_norm_res = zeros(1, 4); 163 164nrep = 1000; % number of calls to IPM solver for better timings 165 166 167tic 168 169if time_invariant==0 % time-variant interface 170 171 for ii=1:nrep 172 HPIPM_d_solve_ipm2_hard_ocp_qp(kk, k_max, mu0, tol, N, nx, nu, nb, ng, ngN, time_invariant, free_x0, warm_start, AA, BB, bb, QQ, Qf, RR, SS, qq, qf, rr, llb, uub, CC, DD, llg, uug, CN, lgN, ugN, x, u, mult_pi, mult_lam, inf_norm_res, infos); 173 end 174 175else % time-invariant interface 176 177 for ii=1:nrep 178 HPIPM_d_solve_ipm2_hard_ocp_qp(kk, k_max, mu0, tol, N, nx, nu, nb, ng, ngN, time_invariant, free_x0, warm_start, A, B, b, Q, Qf, R, S, q, qf, r, lb, ub, C, D, lg, ug, CN, lgN, ugN, x, u, mult_pi, mult_lam, inf_norm_res, infos); 179 end 180 181end 182 183solution_time = toc/nrep 184 185kk 186fprintf('\n alpha_aff mu_aff sigma alpha mu\n'); 187infos(:,1:kk)' 188inf_norm_res 189 190u 191x 192 193 194f1 = figure() 195plot([0:N], x(:,:)) 196title('states') 197xlabel('N') 198 199% print figure to file 200if is_octave() 201 W = 4; H = 3; 202 set(f1,'PaperUnits','inches') 203 set(f1,'PaperOrientation','portrait'); 204 set(f1,'PaperSize',[H,W]) 205 set(f1,'PaperPosition',[0,0,W,H]) 206 FN = findall(f1,'-property','FontName'); 207 set(FN,'FontName','/usr/share/fonts/truetype/ttf-dejavu/DejaVuSerifCondensed.ttf'); 208 FS = findall(f1,'-property','FontSize'); 209 set(FS,'FontSize',10); 210 file_name = ['states.eps']; 211 print(f1, file_name, '-depsc') 212end 213 214 215f1 = figure() 216plot([1:N], u(:,:)) 217title('controls') 218xlabel('N') 219 220% print figure to file 221if is_octave() 222 W = 4; H = 3; 223 set(f1,'PaperUnits','inches') 224 set(f1,'PaperOrientation','portrait'); 225 set(f1,'PaperSize',[H,W]) 226 set(f1,'PaperPosition',[0,0,W,H]) 227 FN = findall(f1,'-property','FontName'); 228 set(FN,'FontName','/usr/share/fonts/truetype/ttf-dejavu/DejaVuSerifCondensed.ttf'); 229 FS = findall(f1,'-property','FontSize'); 230 set(FS,'FontSize',10); 231 file_name = ['inputs.eps']; 232 print(f1, file_name, '-depsc') 233end 234 235 236