int main() { printf("\n"); printf("\n"); printf("\n"); printf(" HPMPC -- Library for High-Performance implementation of solvers for MPC.\n"); printf(" Copyright (C) 2014 by Technical University of Denmark. All rights reserved.\n"); printf("\n"); printf(" HPMPC is distributed in the hope that it will be useful,\n"); printf(" but WITHOUT ANY WARRANTY; without even the implied warranty of\n"); printf(" MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\n"); printf(" See the GNU Lesser General Public License for more details.\n"); printf("\n"); printf("\n"); printf("\n"); printf("Riccati solver performance test - single precision\n"); printf("\n"); // maximum frequency of the processor const float GHz_max = 2.9; //3.6; //2.9; printf("Frequency used to compute theoretical peak: %5.1f GHz (edit test_dricposv.c to modify this value).\n", GHz_max); printf("\n"); // maximum flops per cycle, single precision #if defined(TARGET_X64_AVX) const float flops_max = 16; printf("Testing solvers for AVX instruction set, 64 bit: theoretical peak %5.1f Gflops\n", flops_max*GHz_max); #elif defined(TARGET_X64_SSE3) || defined(TARGET_AMD_SSE3) const float flops_max = 8; printf("Testing solvers for SSE3 instruction set, 64 bit: theoretical peak %5.1f Gflops\n", flops_max*GHz_max); #elif defined(TARGET_CORTEXA9) const float flops_max = 4; printf("Testing solvers for ARMv7a NEON instruction set: theoretical peak %5.1f Gflops\n", flops_max*GHz_max); #elif defined(TARGET_X86_ATOM) const float flops_max = 4; printf("Testing solvers for SSE3 instruction set, 32 bit, optimized for Intel Atom: theoretical peak %5.1f Gflops\n", flops_max*GHz_max); #elif defined(TARGET_POWERPC_G2) const float flops_max = 2; printf("Testing solvers for POWERPC instruction set, 32 bit: theoretical peak %5.1f Gflops\n", flops_max*GHz_max); #elif defined(TARGET_C99_4X4) const float flops_max = 2; printf("Testing reference solvers, 4x4 kernel: theoretical peak %5.1f Gflops\n", flops_max*GHz_max); #elif defined(TARGET_C99_2X2) const float flops_max = 2; printf("Testing reference solvers, 2x2 kernel: theoretical peak %5.1f Gflops\n", flops_max*GHz_max); #endif printf("\n"); printf("Tested solvers:\n"); printf("-sv : Riccati factorization and system solution (prediction step in IP methods)\n"); printf("-trs: system solution after a previous call to Riccati factorization (correction step in IP methods)\n"); printf("\n"); printf("\n"); #if defined(TARGET_X64_AVX) || defined(TARGET_X64_SSE3) || defined(TARGET_X86_ATOM) || defined(TARGET_AMD_SSE3) printf("\nflush to zero on\n"); _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); // flush to zero subnormals !!! works only with one thread !!! #endif // to throw floating-point exception /*#ifndef __APPLE__*/ /* feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);*/ /*#endif*/ int err; int i, j, ii, jj, idx; const int bsd = D_MR; //d_get_mr(); const int bss = S_MR; //s_get_mr(); int info = 0; int nn[] = {4, 6, 8, 10, 12, 16, 20, 24, 28, 32, 36, 40, 44, 48, 52, 56, 60, 64, 68, 72, 76, 80, 84, 88, 92, 96, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144, 148, 152, 156, 160, 164, 168, 172, 176, 180, 184, 188, 192, 196, 200, 204, 208, 212, 216, 220, 224, 228, 232, 236, 240, 244, 248, 252, 256, 260, 264, 268, 272, 276, 280, 284, 288, 292, 296, 300}; int nnrep[] = {10000, 10000, 10000, 10000, 10000, 4000, 4000, 2000, 2000, 1000, 1000, 400, 400, 400, 200, 200, 200, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 40, 40, 40, 40, 40, 20, 20, 20, 20, 20, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10}; int vnx[] = {8, 12, 16, 24, 32, 48, 64, 96, 128, 192, 256, 512, 1024}; int vnrep[] = {100, 100, 100, 100, 100, 100, 50, 50, 50, 20, 10, 10}; int vN[] = {4, 8, 12, 16, 24, 32, 48, 64, 96, 128, 192, 256}; int ll; for(ll=0; ll<77; ll++) /* for(ll=0; ll<1; ll++)*/ { int nx = nn[ll];//NX;//16;//nn[ll]; // number of states (it has to be even for the mass-spring system test problem) int nu = 2;//NU;//5; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem) int N = 10;//NN;//10; // horizon lenght int nrep = nnrep[ll]; /* int nx = NX;//16;//nn[ll]; // number of states (it has to be even for the mass-spring system test problem)*/ /* int nu = NU;//5; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)*/ /* int N = NN;//10; // horizon lenght*/ /* int nrep = NREP;*/ int rep; int nz = nx+nu+1; int pnz = bss*((nz+bss-nu%bss+bss-1)/bss); /************************************************ * dynamical system ************************************************/ double *A; d_zeros(&A, nx, nx); // states update matrix double *B; d_zeros(&B, nx, nu); // inputs matrix double *b; d_zeros(&b, nx, 1); // states offset double *x0; d_zeros(&x0, nx, 1); // initial state double Ts = 0.5; // sampling time mass_spring_system(Ts, nx, nu, N, A, B, b, x0); for(jj=0; jj<nx; jj++) b[jj] = 0.1; for(jj=0; jj<nx; jj++) x0[jj] = 0; x0[0] = 3.5; x0[1] = 3.5; // d_print_mat(nx, nx, A, nx); // d_print_mat(nx, nu, B, nx); // d_print_mat(nx, 1, b, nx); // d_print_mat(nx, 1, x0, nx); /* packed */ double *BAb; d_zeros(&BAb, nx, nz); dmcopy(nx, nu, B, nx, BAb, nx); dmcopy(nx, nx, A, nx, BAb+nu*nx, nx); dmcopy(nx, 1 , b, nx, BAb+(nu+nx)*nx, nx); // d_print_mat(nx, nx+nu+1, BAb, nx); /* transposed */ double *BAbt; d_zeros_align(&BAbt, pnz, pnz); for(ii=0; ii<nx; ii++) for(jj=0; jj<nz; jj++) { BAbt[jj+pnz*ii] = BAb[ii+nx*jj]; } // d_print_mat(nz, nx+1, BAbt, pnz); // s_print_mat(nz, nx+1, sBAbt, pnz); // return 0; /* packed into contiguous memory */ double *pBAbt; d_zeros_align(&pBAbt, pnz, pnz); d_cvt_mat2pmat(nz, nx, 0, bsd, BAbt, pnz, pBAbt, pnz); float *psBAbt; s_zeros_align(&psBAbt, pnz, pnz); s_cvt_d2s_pmat(nz, nx, bsd, pBAbt, pnz, bss, psBAbt, pnz); // d_print_pmat(nz, nx, bsd, pBAbt, pnz); // s_print_pmat(nz, nx, bss, spBAbt, pnz); /************************************************ * cost function ************************************************/ double *Q; d_zeros_align(&Q, pnz, pnz); for(ii=0; ii<nu; ii++) Q[ii*(pnz+1)] = 2.0; for(; ii<pnz; ii++) Q[ii*(pnz+1)] = 1.0; for(ii=0; ii<nz; ii++) Q[nx+nu+ii*pnz] = 1.0; Q[(nx+nu)*(pnz+1)] = 1e6; /* packed into contiguous memory */ float *pQ; s_zeros_align(&pQ, pnz, pnz); cvt_d2s_mat2pmat(nz, nz, 0, bss, Q, pnz, pQ, pnz); /* matrices series */ float *(hpQ[N+1]); float *(hq[N+1]); float *(hux[N+1]); float *(hpi[N+1]); float *(hpBAbt[N]); float *(hrb[N]); float *(hrq[N+1]); for(jj=0; jj<N; jj++) { s_zeros_align(&hpQ[jj], pnz, pnz); s_zeros_align(&hq[jj], pnz, 1); s_zeros_align(&hux[jj], pnz, 1); s_zeros_align(&hpi[jj], nx, 1); hpBAbt[jj] = psBAbt; s_zeros_align(&hrb[jj], nx, 1); s_zeros_align(&hrq[jj], nx+nu, 1); } s_zeros_align(&hpQ[N], pnz, pnz); s_zeros_align(&hq[N], pnz, 1); s_zeros_align(&hux[N], pnz, 1); s_zeros_align(&hpi[N], nx, 1); s_zeros_align(&hrq[N], nx+nu, 1); // starting guess for(jj=0; jj<nx; jj++) hux[0][nu+jj] = (float) x0[jj]; float *pL; s_zeros_align(&pL, pnz, pnz); float *pBAbtL; s_zeros_align(&pBAbtL, pnz, pnz); /************************************************ * riccati-like iteration ************************************************/ // predictor // restore cost function for(ii=0; ii<N; ii++) { for(jj=0; jj<pnz*pnz; jj++) hpQ[ii][jj]=pQ[jj]; } for(jj=0; jj<pnz*pnz; jj++) hpQ[N][jj]=pQ[jj]; // call the solver sricposv_mpc(nx, nu, N, pnz, hpBAbt, hpQ, hux, pL, pBAbtL, COMPUTE_MULT, hpi, &info); if(PRINTRES==1) { /* print result */ printf("\n\nsv\n\n"); for(ii=0; ii<N; ii++) s_print_mat(1, nu, hux[ii], 1); } if(PRINTRES==1 && COMPUTE_MULT==1) { // print result printf("\n\nsv\n\n"); for(ii=0; ii<N; ii++) s_print_mat(1, nx, hpi[ii+1], 1); } // corrector // clear solution for(ii=0; ii<N; ii++) { for(jj=0; jj<nu; jj++) hux[ii][jj] = 0; for(jj=0; jj<nx; jj++) hux[ii+1][nu+jj] = 0; } // restore linear part of cost function for(ii=0; ii<N; ii++) { for(jj=0; jj<nx+nu; jj++) hq[ii][jj] = Q[nx+nu+pnz*jj]; } for(jj=0; jj<nx+nu; jj++) hq[N][jj] = Q[nx+nu+pnz*jj]; // call the solver sricpotrs_mpc(nx, nu, N, pnz, hpBAbt, hpQ, hq, hux, pBAbtL, COMPUTE_MULT, hpi); if(PRINTRES==1) { // print result printf("\n\ntrs\n\n"); for(ii=0; ii<N; ii++) s_print_mat(1, nu, hux[ii], 1); } if(PRINTRES==1 && COMPUTE_MULT==1) { // print result printf("\n\ntrs\n\n"); for(ii=0; ii<N; ii++) s_print_mat(1, nx, hpi[ii+1], 1); } // restore cost function for(ii=0; ii<N; ii++) { for(jj=0; jj<pnz*pnz; jj++) hpQ[ii][jj]=pQ[jj]; } for(jj=0; jj<pnz*pnz; jj++) hpQ[N][jj]=pQ[jj]; // restore linear part of cost function for(ii=0; ii<N; ii++) { for(jj=0; jj<nx+nu; jj++) hq[ii][jj] = Q[nx+nu+pnz*jj]; } for(jj=0; jj<nx+nu; jj++) hq[N][jj] = Q[nx+nu+pnz*jj]; // residuals computation sres(nx, nu, N, pnz, hpBAbt, hpQ, hq, hux, hpi, hrq, hrb); if(PRINTRES==1 && COMPUTE_MULT==1) { // print result printf("\n\nres\n\n"); for(ii=0; ii<+N; ii++) s_print_mat(1, nx+nu, hrq[ii], 1); for(ii=0; ii<N; ii++) s_print_mat(1, nx, hrb[ii], 1); } // timing struct timeval tv0, tv1, tv2; gettimeofday(&tv0, NULL); // start // double precision for(rep=0; rep<nrep; rep++) { // restore cost function for(ii=0; ii<N; ii++) { for(jj=0; jj<pnz*pnz; jj++) hpQ[ii][jj]=pQ[jj]; } for(jj=0; jj<pnz*pnz; jj++) hpQ[N][jj]=pQ[jj]; // call the solver sricposv_mpc(nx, nu, N, pnz, hpBAbt, hpQ, hux, pL, pBAbtL, COMPUTE_MULT, hpi, &info); } gettimeofday(&tv1, NULL); // start for(rep=0; rep<nrep; rep++) { // clear solution for(ii=0; ii<N; ii++) { for(jj=0; jj<nu; jj++) hux[ii][jj] = 0; for(jj=0; jj<nx; jj++) hux[ii+1][nu+jj] = 0; } // restore linear part of cost function for(ii=0; ii<N; ii++) { for(jj=0; jj<nx+nu; jj++) hq[ii][jj] = Q[nx+nu+pnz*jj]; } for(jj=0; jj<nx+nu; jj++) hq[N][jj] = Q[nx+nu+pnz*jj]; // call the solver sricpotrs_mpc(nx, nu, N, pnz, hpBAbt, hpQ, hq, hux, pBAbtL, COMPUTE_MULT, hpi); } gettimeofday(&tv2, NULL); // start float time_sv = (float) (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6); float flop_sv = (1.0/3.0*nx*nx*nx+3.0/2.0*nx*nx) + N*(7.0/3.0*nx*nx*nx+4.0*nx*nx*nu+2.0*nx*nu*nu+1.0/3.0*nu*nu*nu+13.0/2.0*nx*nx+9.0*nx*nu+5.0/2.0*nu*nu); if(COMPUTE_MULT==1) flop_sv += N*2*nx*nx; float Gflops_sv = 1e-9*flop_sv/time_sv; float time_trs = (float) (tv2.tv_sec-tv1.tv_sec)/(nrep+0.0)+(tv2.tv_usec-tv1.tv_usec)/(nrep*1e6); float flop_trs = N*(8.0*nx*nx+8.0*nx*nu+2.0*nu*nu); if(COMPUTE_MULT==1) flop_trs += N*2*nx*nx; float Gflops_trs = 1e-9*flop_trs/time_trs; float Gflops_max = flops_max * GHz_max; if(ll==0) printf("\nnx\tnu\tN\tsv time\t\tsv Gflops\tsv \%\t\ttrs time\ttrs Gflops\ttrs \%\n\n"); printf("%d\t%d\t%d\t%e\t%f\t%f\t%e\t%f\t%f\n", nx, nu, N, time_sv, Gflops_sv, 100.0*Gflops_sv/Gflops_max, time_trs, Gflops_trs, 100.0*Gflops_trs/Gflops_max); /************************************************ * return ************************************************/ free(A); free(B); free(b); free(x0); free(BAb); free(BAbt); free(pBAbt); free(Q); free(pQ); free(pL); free(pBAbtL); for(jj=0; jj<N; jj++) { free(hpQ[jj]); free(hq[jj]); free(hux[jj]); free(hpi[jj]); } free(hpQ[N]); free(hq[N]); free(hux[N]); free(hpi[N]); } // increase size printf("\n"); printf("\n"); printf("\n"); return 0; }
int main() { printf("\n"); printf("\n"); printf("\n"); printf(" HPMPC -- Library for High-Performance implementation of solvers for MPC.\n"); printf(" Copyright (C) 2014 by Technical University of Denmark. All rights reserved.\n"); printf("\n"); printf(" HPMPC is distributed in the hope that it will be useful,\n"); printf(" but WITHOUT ANY WARRANTY; without even the implied warranty of\n"); printf(" MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\n"); printf(" See the GNU Lesser General Public License for more details.\n"); printf("\n"); printf("\n"); printf("\n"); #if defined(TARGET_X64_AVX2) || defined(TARGET_X64_AVX) || defined(TARGET_X64_SSE3) || defined(TARGET_X86_ATOM) || defined(TARGET_AMD_SSE3) /* printf("\nflush subnormals to zero\n\n");*/ _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); // flush to zero subnormals !!! works only with one thread !!! #endif int ii, jj, idx; int rep, nrep=NREP; int nx = NX; // number of states (it has to be even for the mass-spring system test problem) int nu = NU; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem) int N = NN; // horizon lenght int nb = NB; // number of box constrained inputs and states printf(" Test problem: mass-spring system with %d masses and %d controls.\n", nx/2, nu); printf("\n"); printf(" MPC problem size: %d states, %d inputs, %d horizon length, %d two-sided box constraints.\n", nx, nu, N, nb); printf("\n"); printf(" ADMM method parameters: single precision, %d maximum iterations, %5.1e exit tolerance in primal and duality measure (edit file test_admm_ip_box.c to change them).\n", K_MAX_ADMM, TOL); int info = 0; const int bs = S_MR; //d_get_mr(); const int ncl = S_NCL; const int nal = bs*ncl; // number of doubles per cache line const int nz = nx+nu+1; const int pnz = bs*((nz+bs-1)/bs); const int pnx = bs*((nx+bs-1)/bs); const int cnz = ncl*((nx+nu+1+ncl-1)/ncl); const int cnx = ncl*((nx+ncl-1)/ncl); const int pnb = bs*((2*nb+bs-1)/bs); // packed number of box constraints const int anz = nal*((nz+nal-1)/nal); const int anx = nal*((nx+nal-1)/nal); const int anb = nal*((2*nb+nal-1)/nal); // cache aligned number of box constraints const int pad = (ncl-nx%ncl)%ncl; // packing between BAbtL & P const int cnl = cnz<cnx+ncl ? nx+pad+cnx+ncl : nx+pad+cnz; /************************************************ * dynamical system ************************************************/ double *A; d_zeros(&A, nx, nx); // states update matrix double *B; d_zeros(&B, nx, nu); // inputs matrix double *b; d_zeros(&b, nx, 1); // states offset double *x0; d_zeros(&x0, nx, 1); // initial state double Ts = 0.5; // sampling time mass_spring_system(Ts, nx, nu, N, A, B, b, x0); for(jj=0; jj<nx; jj++) b[jj] = 0.1; for(jj=0; jj<nx; jj++) x0[jj] = 0; x0[0] = 3.5; x0[1] = 3.5; // d_print_mat(nx, nx, A, nx); // d_print_mat(nx, nu, B, nx); // d_print_mat(nx, 1, b, nx); // d_print_mat(nx, 1, x0, nx); /* packed */ /* double *BAb; d_zeros(&BAb, nx, nz);*/ /* dmcopy(nx, nu, B, nx, BAb, nx);*/ /* dmcopy(nx, nx, A, nx, BAb+nu*nx, nx);*/ /* dmcopy(nx, 1 , b, nx, BAb+(nu+nx)*nx, nx);*/ /* transposed */ /* double *BAbt; d_zeros_align(&BAbt, pnz, pnz);*/ /* for(ii=0; ii<nx; ii++)*/ /* for(jj=0; jj<nz; jj++)*/ /* {*/ /* BAbt[jj+pnz*ii] = BAb[ii+nx*jj];*/ /* }*/ /* packed into contiguous memory */ float *pBAbt; s_zeros_align(&pBAbt, pnz, cnx); /* d_cvt_mat2pmat(nz, nx, 0, bs, BAbt, pnz, pBAbt, cnx);*/ /* d_cvt_tran_mat2pmat(nx, nz, 0, bs, BAb, nx, pBAbt, cnx);*/ cvt_tran_d2s_mat2pmat(nx, nu, 0, bs, B, nx, pBAbt, cnx); cvt_tran_d2s_mat2pmat(nx, nx, nu, bs, A, nx, pBAbt+nu/bs*cnx*bs+nu%bs, cnx); for (jj = 0; jj<nx; jj++) pBAbt[(nx+nu)/bs*cnx*bs+(nx+nu)%bs+jj*bs] = (float) b[jj]; /* s_print_pmat (nz, nx, bs, pBAbt, cnx);*/ /* exit(1);*/ /************************************************ * box constraints ************************************************/ /* double *db; d_zeros_align(&db, 2*nb, 1);*/ /* for(jj=0; jj<2*nu; jj++)*/ /* db[jj] = - 0.5; // umin*/ /* for(; jj<2*nb; jj++)*/ /* db[jj] = - 4.0; // xmin*/ float *lb; s_zeros_align(&lb, nx+nu, 1); for(jj=0; jj<nu; jj++) lb[jj] = - 0.5; // umin for(; jj<nu+nx; jj++) lb[jj] = - 4.0; // xmin float *ub; s_zeros_align(&ub, nx+nu, 1); for(jj=0; jj<nu; jj++) ub[jj] = 0.5; // uman for(; jj<nu+nx; jj++) ub[jj] = 4.0; // xman /************************************************ * cost function ************************************************/ float *Q; s_zeros_align(&Q, pnz, pnz); for(ii=0; ii<nu; ii++) Q[ii*(pnz+1)] = 2.0; for(; ii<pnz; ii++) Q[ii*(pnz+1)] = 1.0; for(ii=0; ii<nz; ii++) Q[nx+nu+ii*pnz] = 0.1; /* Q[(nx+nu)*(pnz+1)] = 1e35; // large enough (not needed any longer) */ /* packed into contiguous memory */ float *pQ; s_zeros_align(&pQ, pnz, cnz); s_cvt_mat2pmat(nz, nz, 0, bs, Q, pnz, pQ, cnz); /************************************************ * matrices series ************************************************/ float *(hpQ[N+1]); float *(hq[N+1]); float *(hux[N+1]); float *(hpi[N+1]); float *(hlam[N+1]); float *(ht[N+1]); float *(hpBAbt[N]); float *(hlb[N+1]); float *(hub[N+1]); float *(hrb[N]); float *(hrq[N+1]); float *(hrd[N+1]); float *(hux_v[N+1]); float *(hux_w[N+1]); for(jj=0; jj<N; jj++) { s_zeros_align(&hpQ[jj], pnz, cnz); } s_zeros_align(&hpQ[N], pnz, pnz); for(jj=0; jj<N; jj++) { s_zeros_align(&hq[jj], anz, 1); s_zeros_align(&hux[jj], anz, 1); s_zeros_align(&hpi[jj], anx, 1); s_zeros_align(&hlam[jj],anb, 1); // TODO pnb s_zeros_align(&ht[jj], anb, 1); // TODO pnb hpBAbt[jj] = pBAbt; hlb[jj] = lb; hub[jj] = ub; s_zeros_align(&hrb[jj], anx, 1); s_zeros_align(&hrq[jj], anz, 1); s_zeros_align(&hrd[jj], anb, 1); // TODO pnb s_zeros_align(&hux_v[jj], anz, 1); s_zeros_align(&hux_w[jj], anz, 1); } s_zeros_align(&hq[N], anz, 1); s_zeros_align(&hux[N], anz, 1); s_zeros_align(&hpi[N], anx, 1); s_zeros_align(&hlam[N], anb, 1); // TODO pnb s_zeros_align(&ht[N], anb, 1); // TODO pnb hlb[N] = lb; hub[N] = ub; s_zeros_align(&hrq[N], anz, 1); s_zeros_align(&hrd[N], anb, 1); // TODO pnb s_zeros_align(&hux_v[N], anz, 1); s_zeros_align(&hux_w[N], anz, 1); // starting guess // for(jj=0; jj<nx; jj++) hux[0][nu+jj]=x0[jj]; /************************************************ * riccati-like iteration ************************************************/ float *work; s_zeros_align(&work, (N+1)*(pnz*cnl + 4*anz + 2*anx) + 3*anz, 1); // work space int kk = 0; // acutal number of iterations /* char prec = PREC; // double/single precision*/ /* float sp_thr = SP_THR; // threshold to switch between double and single precision*/ int k_max = K_MAX_ADMM; // maximum number of iterations in the ADMM method float tol = TOL*sqrt(N*(nx+nu));//TOL; // tolerance in the duality measure /* float sigma[] = {0.4, 0.3, 0.01}; // control primal-dual IP behaviour*/ float rho = 2.0; // penalty parameter float alpha = 1.5; // relaxation parameter float *stat; s_zeros(&stat, 5, k_max); // stats from the ADMM routine int compute_mult = COMPUTE_MULT_ADMM; int warm_start = 0;//WARM_START; /* float mu = -1.0;*/ /* initizile the cost function */ for(ii=0; ii<N; ii++) { for(jj=0; jj<pnz*cnz; jj++) hpQ[ii][jj]=pQ[jj]; } for(jj=0; jj<pnz*cnz; jj++) hpQ[N][jj]=pQ[jj]; // initial states float xx0[] = {3.5, 3.5, 3.66465, 2.15833, 1.81327, -0.94207, 1.86531, -2.35760, 2.91534, 1.79890, -1.49600, -0.76600, -2.60268, 1.92456, 1.66630, -2.28522, 3.12038, 1.83830, 1.93519, -1.87113}; /* warm up */ // initialize states and inputs for(ii=0; ii<=N; ii++) for(jj=0; jj<nx+nu; jj++) hux[ii][jj] = 0; hux[0][nu+0] = xx0[0]; hux[0][nu+1] = xx0[1]; // call the ADMM solver // if(FREE_X0==0) // { s_admm_box_mpc(&kk, k_max, tol, tol, warm_start, 1, rho, alpha, stat, nx, nu, N, hpBAbt, hpQ, hlb, hub, hux, hux_v, hux_w, compute_mult, hpi, work); // } // else // { ///* d_ip_box_mhe(&kk, k_max, tol, warm_start, sigma, stat, nx, nu, N, nb, hpBAbt, hpQ, hdb, hux, compute_mult, hpi, hlam, ht, work);*/ // } int kk_avg = 0; /* timing */ struct timeval tv0, tv1; gettimeofday(&tv0, NULL); // start for(rep=0; rep<nrep; rep++) { idx = rep%10; x0[0] = xx0[2*idx]; x0[1] = xx0[2*idx+1]; // initialize states and inputs for(ii=0; ii<=N; ii++) for(jj=0; jj<nx+nu; jj++) hux[ii][jj] = 0; hux[0][nu+0] = xx0[2*idx]; hux[0][nu+1] = xx0[2*idx+1]; // call the ADMM solver // if(FREE_X0==0) // { s_admm_box_mpc(&kk, k_max, tol, tol, warm_start, 0, rho, alpha, stat, nx, nu, N, hpBAbt, hpQ, hlb, hub, hux, hux_v, hux_w, compute_mult, hpi, work); // } // else // { ///* d_ip_box_mhe(&kk, k_max, tol, warm_start, sigma, stat, nx, nu, N, nb, hpBAbt, hpQ, hdb, hux, compute_mult, hpi, hlam, ht, work);*/ // } kk_avg += kk; } gettimeofday(&tv1, NULL); // stop float time = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6); /* printf("\nnx\tnu\tN\tkernel\n\n");*/ /* printf("\n%d\t%d\t%d\t%e\n\n", nx, nu, N, time);*/ printf("\n"); printf(" Average number of iterations over %d runs: %5.1f\n", nrep, kk_avg / (float) nrep); /* printf(" Average number of iterations over %d runs: %d\n", nrep, kk);*/ printf("\n"); printf(" Average solution time over %d runs: %5.2e seconds\n", nrep, time); printf("\n"); // restore linear part of cost function for(ii=0; ii<N; ii++) { for(jj=0; jj<nx+nu; jj++) hq[ii][jj] = Q[nx+nu+pnz*jj]; } for(jj=0; jj<nx+nu; jj++) hq[N][jj] = Q[nx+nu+pnz*jj]; // residuals computation /* if(FREE_X0==0)*/ /* d_res_ip_box_mpc(nx, nu, N, nb, hpBAbt, hpQ, hq, hux, hdb, hpi, hlam, ht, hrq, hrb, hrd, &mu);*/ /* else*/ /* d_res_ip_box_mhe(nx, nu, N, nb, hpBAbt, hpQ, hq, hux, hdb, hpi, hlam, ht, hrq, hrb, hrd, &mu);*/ if(PRINTSTAT==1) { printf("\n"); printf("\n"); printf(" Print ADMM statistics of the last run\n"); printf("\n"); for(jj=0; jj<kk; jj++) printf("k = %d\t\tp_res = %f\td_res = %f\n", jj, stat[5*jj], stat[5*jj+1]); printf("\n"); } if(PRINTRES==1) { printf("\n"); printf("\n"); printf(" Print solution\n"); printf("\n"); printf("\nu = \n\n"); for(ii=0; ii<N; ii++) s_print_mat(1, nu, hux[ii], 1); } if(0 && PRINTRES==1 && COMPUTE_MULT_ADMM==1) { // print result // print result printf("\n"); printf("\n"); printf(" Print residuals\n\n"); printf("\n"); printf("\n"); printf("rq = \n\n"); // if(FREE_X0==0) // { s_print_mat(1, nu, hrq[0], 1); for(ii=1; ii<=N; ii++) /* s_print_mat_e(1, nx+nu, hrq[ii], 1);*/ s_print_mat(1, nx+nu, hrq[ii], 1); // } // else // { // for(ii=0; ii<=N; ii++) ///* s_print_mat_e(1, nx+nu, hrq[ii], 1);*/ // s_print_mat(1, nx+nu, hrq[ii], 1); // } printf("\n"); printf("\n"); printf("rb = \n\n"); for(ii=0; ii<N; ii++) /* s_print_mat_e(1, nx, hrb[ii], 1);*/ s_print_mat(1, nx, hrb[ii], 1); printf("\n"); printf("\n"); printf("rd = \n\n"); for(ii=0; ii<=N; ii++) /* s_print_mat_e(1, 2*nb, hrd[ii], 1);*/ s_print_mat(1, 2*nb, hrd[ii], 1); printf("\n"); printf("\n"); /* printf("mu = %e\n\n", mu);*/ } /* printf("\nnx\tnu\tN\tkernel\n\n");*/ /* printf("\n%d\t%d\t%d\t%e\n\n", nx, nu, N, time);*/ /************************************************ * free memory and return ************************************************/ free(A); free(B); free(b); free(x0); /* free(BAb);*/ /* free(BAbt);*/ free(pBAbt); free(lb); free(ub); free(Q); free(pQ); free(work); free(stat); for(jj=0; jj<N; jj++) { free(hpQ[jj]); free(hq[jj]); free(hux[jj]); free(hpi[jj]); free(hlam[jj]); free(ht[jj]); free(hrb[jj]); free(hrq[jj]); free(hrd[jj]); free(hux_v[jj]); free(hux_w[jj]); } free(hpQ[N]); free(hq[N]); free(hux[N]); free(hpi[N]); free(hlam[N]); free(ht[N]); free(hrq[N]); free(hrd[N]); free(hux_v[N]); free(hux_w[N]); return 0; }