/* primal-dual interior-point method, hard constraints, time variant matrices (mpc version) */ int d_ip2_hard_mpc(int *kk, int k_max, double mu0, double mu_tol, double alpha_min, int warm_start, double *sigma_par, double *stat, int nx, int nu, int N, int nb, int ng, int ngN, double **pBAbt, double **pQ, double **pDCt, double **d, double **ux, int compute_mult, double **pi, double **lam, double **t, double *work_memory) { int nbu = nu<nb ? nu : nb ; // indeces int jj, ll, ii, bs0; // constants const int bs = D_MR; //d_get_mr(); const int ncl = D_NCL; const int nal = bs*ncl; // number of doubles per cache line const int nz = nx+nu+1; const int nxu = nx+nu; const int pnz = bs*((nz+bs-1)/bs); const int pnx = bs*((nx+bs-1)/bs); const int pnb = bs*((nb+bs-1)/bs); // simd aligned number of two-sided box constraints !!!!!!!!!!!!!!!!!! const int png = bs*((ng+bs-1)/bs); // simd aligned number of two-sided general constraints !!!!!!!!!!!!!!!!!! const int pngN = bs*((ngN+bs-1)/bs); // simd aligned number of two-sided general constraints at stage N !!!!!!!!!!!!!!!!!! const int cnz = ncl*((nz+ncl-1)/ncl); const int cnx = ncl*((nx+ncl-1)/ncl); // const int cng = ncl*((ng+ncl-1)/ncl); const int cngN = ncl*((ngN+ncl-1)/ncl); const int cnxg = ncl*((ng+nx+ncl-1)/ncl); 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 anb = nal*((nb+nal-1)/nal); // cache aligned number of two-sided 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; const int cnl = cnz<cnx+ncl ? cnx+ncl : cnz; //printf("\n%d %d %d %d %d\n", N, nx, nu, nb, ng); //d_print_pmat(nz, nx, bs, pBAbt[0], cnx); //d_print_pmat(nz, nx, bs, pBAbt[1], cnx); //d_print_pmat(nz, nx, bs, pBAbt[N-1], cnx); //d_print_pmat(nz, nz, bs, pQ[0], cnz); //d_print_pmat(nz, nz, bs, pQ[1], cnz); //d_print_pmat(nz, nz, bs, pQ[N], cnz); //d_print_pmat(nx+nu, ng, bs, pDCt[0], cng); //d_print_pmat(nx+nu, ng, bs, pDCt[1], cng); //d_print_pmat(nx+nu, ng, bs, pDCt[N], cng); //d_print_mat(1, 2*pnb+2*png, d[0], 1); //d_print_mat(1, 2*pnb+2*png, d[1], 1); //d_print_mat(1, 2*pnb+2*png, d[N], 1); //d_print_mat(1, nx+nu, ux[0], 1); //d_print_mat(1, nx+nu, ux[1], 1); //d_print_mat(1, nx+nu, ux[N], 1); //exit(1); // initialize work space double *ptr; ptr = work_memory; double *(dux[N+1]); double *(dpi[N+1]); double *(pL[N+1]); double *(pd[N+1]); // pointer to diagonal of Hessian double *(pl[N+1]); // pointer to linear part of Hessian double *(bd[N+1]); // backup diagonal of Hessian double *(bl[N+1]); // backup linear part of Hessian double *work; double *diag; double *(dlam[N+1]); double *(dt[N+1]); double *(lamt[N+1]); double *(t_inv[N+1]); double *(Qx[N+1]); double *(qx[N+1]); double *(Pb[N]); // ptr += (N+1)*(pnx + pnz*cnl + 12*pnz) + 3*pnz; // inputs and states for(jj=0; jj<=N; jj++) { dux[jj] = ptr; ptr += anz; } // equality constr multipliers for(jj=0; jj<=N; jj++) { dpi[jj] = ptr; ptr += anx; } // Hessian for(jj=0; jj<=N; jj++) { pd[jj] = ptr; //pQ[jj]; pl[jj] = ptr + anz; //pQ[jj] + ((nu+nx)/bs)*bs*cnz + (nu+nx)%bs; bd[jj] = ptr + 2*anz; bl[jj] = ptr + 3*anz; ptr += 4*anz; // backup for(ll=0; ll<nx+nu; ll++) { bd[jj][ll] = pQ[jj][(ll/bs)*bs*cnz+ll%bs+ll*bs]; bl[jj][ll] = pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs]; } } // work space for(jj=0; jj<=N; jj++) { pL[jj] = ptr; ptr += pnz*cnl; } work = ptr; //ptr += 2*anz; if(cngN<=cnxg) ptr += pnz*cnxg; else ptr += pnz*cngN; diag = ptr; ptr += anz; // slack variables, Lagrangian multipliers for inequality constraints and work space (assume # box constraints <= 2*(nx+nu) < 2*pnz) for(jj=0; jj<N; jj++) { dlam[jj] = ptr; dt[jj] = ptr + 2*pnb+2*png; ptr += 4*pnb+4*png; } dlam[N] = ptr; dt[N] = ptr + 2*pnb+2*pngN; ptr += 4*pnb+4*pngN; for(jj=0; jj<N; jj++) { lamt[jj] = ptr; ptr += 2*pnb+2*png; } lamt[N] = ptr; ptr += 2*pnb+2*pngN; for(jj=0; jj<N; jj++) { t_inv[jj] = ptr; ptr += 2*pnb+2*png; } t_inv[N] = ptr; ptr += 2*pnb+2*pngN; for(jj=0; jj<N; jj++) { Qx[jj] = ptr; qx[jj] = ptr+png; ptr += 2*pnb+2*png; } Qx[N] = ptr; qx[N] = ptr+pngN; ptr += 2*pnb+2*pngN; // backup of P*b for(jj=0; jj<N; jj++) { Pb[jj] = ptr; ptr += anx; } double temp0, temp1; double alpha, mu, mu_aff; double mu_scal = N*2*(nb+ng)+2*ngN; //printf("\nmu_scal = %f\n", mu_scal); mu_scal = 1.0/mu_scal; //printf("\nmu_scal = %f\n", mu_scal); double sigma, sigma_decay, sigma_min; //printf("\n%d %d %d\n", ng, ngN, N*2*ng+2*ngN); //exit(1); sigma = sigma_par[0]; //0.4; sigma_decay = sigma_par[1]; //0.3; sigma_min = sigma_par[2]; //0.01; // initialize ux & t>0 (slack variable) d_init_var_hard_mpc(N, nx, nu, nb, ng, ngN, ux, pi, pDCt, d, t, lam, mu0, warm_start); #if 0 d_print_mat(1, 2*pnb+2*png, t[0], 1); d_print_mat(1, 2*pnb+2*png, t[1], 1); d_print_mat(1, 2*pnb+2*pngN, t[N], 1); d_print_mat(1, 2*pnb+2*png, lam[0], 1); d_print_mat(1, 2*pnb+2*png, lam[1], 1); d_print_mat(1, 2*pnb+2*pngN, lam[N], 1); exit(1); #endif // initialize pi for(jj=0; jj<=N; jj++) for(ll=0; ll<nx; ll++) dpi[jj][ll] = 0.0; // initialize dux for(ll=0; ll<nx; ll++) dux[0][nu+ll] = ux[0][nu+ll]; // compute the duality gap //alpha = 0.0; // needed to compute mu !!!!! //d_compute_mu_hard_mpc(N, nx, nu, nb, &mu, mu_scal, alpha, lam, dlam, t, dt); mu = mu0; // set to zero iteration count *kk = 0; // larger than minimum accepted step size alpha = 1.0; // update hessian in Riccati routine const int update_hessian = 1; int fast_rsqrt = 0; // IP loop while( *kk<k_max && mu>mu_tol && alpha>=alpha_min ) { //update cost function matrices and vectors (box constraints) d_update_hessian_hard_mpc(N, nx, nu, nb, ng, ngN, cnz, 0.0, t, t_inv, lam, lamt, dlam, Qx, qx, bd, bl, pd, pl, d); #if 0 d_print_mat(1, 2*pnb+2*png, pd[0], 1); d_print_mat(1, 2*pnb+2*png, pd[1], 1); d_print_mat(1, 2*pnb+2*png, pd[N], 1); d_print_mat(1, 2*pnb+2*png, pl[0], 1); d_print_mat(1, 2*pnb+2*png, pl[1], 1); d_print_mat(1, 2*pnb+2*png, pl[N], 1); #if 0 d_print_mat(1, 2*pnb+2*png, Qx[0], 1); d_print_mat(1, 2*pnb+2*png, Qx[1], 1); d_print_mat(1, 2*pnb+2*pngN, Qx[N], 1); d_print_mat(1, 2*pnb+2*png, qx[0], 1); d_print_mat(1, 2*pnb+2*png, qx[1], 1); d_print_mat(1, 2*pnb+2*pngN, qx[N], 1); #endif exit(1); #endif #if 0 for(ii=0; ii<=N; ii++) d_print_mat(1, nu+nx, pd[ii], 1); for(ii=0; ii<=N; ii++) d_print_mat(1, nu+nx, pl[ii], 1); for(ii=0; ii<N; ii++) d_print_mat(1, ng, Qx[ii], 1); d_print_mat(1, ngN, Qx[N], 1); for(ii=0; ii<N; ii++) d_print_mat(1, ng, qx[ii], 1); d_print_mat(1, ngN, qx[N], 1); if(*kk==1) exit(1); #endif // compute the search direction: factorize and solve the KKT system #if defined(FAST_RSQRT) if(mu>1e-2) fast_rsqrt = 2; else { if(mu>1e-4) fast_rsqrt = 1; else fast_rsqrt = 0; } #else fast_rsqrt = 0; #endif //printf("\n%d %f\n", fast_rsqrt, mu); d_back_ric_sv(N, nx, nu, pBAbt, pQ, update_hessian, pd, pl, 1, dux, pL, work, diag, 1, Pb, compute_mult, dpi, nb, ng, ngN, pDCt, Qx, qx); #if 0 for(ii=0; ii<=N; ii++) d_print_pmat(nz, nz, bs, pL[ii], cnl); exit(1); #endif #if 0 printf("\ndux\n"); for(ii=0; ii<=N; ii++) d_print_mat(1, nx+nu, dux[ii], 1); if(*kk==1) exit(1); #endif #if 1 // compute t_aff & dlam_aff & dt_aff & alpha for(jj=0; jj<=N; jj++) for(ll=0; ll<2*nb; ll++) dlam[jj][ll] = 0.0; alpha = 1.0; d_compute_alpha_hard_mpc(N, nx, nu, nb, ng, ngN, &alpha, t, dt, lam, dlam, lamt, dux, pDCt, d); stat[5*(*kk)] = sigma; stat[5*(*kk)+1] = alpha; alpha *= 0.995; // compute the affine duality gap d_compute_mu_hard_mpc(N, nx, nu, nb, ng, ngN, &mu_aff, mu_scal, alpha, lam, dlam, t, dt); stat[5*(*kk)+2] = mu_aff; //mu_aff = 1.346982; // TODO remove !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // compute sigma sigma = mu_aff/mu; sigma = sigma*sigma*sigma; // if(sigma<sigma_min) // sigma = sigma_min; d_update_gradient_hard_mpc(N, nx, nu, nb, ng, ngN, sigma*mu, dt, dlam, t_inv, pl, qx); #if 0 for(ii=0; ii<=N; ii++) d_print_mat(1, nu+nx, pl[ii], 1); //for(ii=0; ii<N; ii++) // d_print_mat(1, ng, qx[ii], 1); //d_print_mat(1, ngN, qx[N], 1); if(*kk==1) exit(1); #endif #if 0 // first stage for(ii=0; ii<2*nbu; ii+=2) { dlam[0][ii+0] = t_inv[0][ii+0]*(sigma*mu - dlam[0][ii+0]*dt[0][ii+0]); // !!!!! dlam[0][ii+1] = t_inv[0][ii+1]*(sigma*mu - dlam[0][ii+1]*dt[0][ii+1]); // !!!!! pl[0][ii/2] += dlam[0][ii+1] - dlam[0][ii+0]; } // middle stages for(jj=1; jj<N; jj++) { for(ii=0; ii<2*nb; ii+=2) { dlam[jj][ii+0] = t_inv[jj][ii+0]*(sigma*mu - dlam[jj][ii+0]*dt[jj][ii+0]); // !!!!! dlam[jj][ii+1] = t_inv[jj][ii+1]*(sigma*mu - dlam[jj][ii+1]*dt[jj][ii+1]); // !!!!! pl[jj][ii/2] += dlam[jj][ii+1] - dlam[jj][ii+0]; } } // last stages for(ii=2*nu; ii<2*nb; ii+=2) { dlam[jj][ii+0] = t_inv[jj][ii+0]*(sigma*mu - dlam[jj][ii+0]*dt[jj][ii+0]); // !!!!! dlam[jj][ii+1] = t_inv[jj][ii+1]*(sigma*mu - dlam[jj][ii+1]*dt[jj][ii+1]); // !!!!! pl[jj][ii/2] += dlam[jj][ii+1] - dlam[jj][ii+0]; } #endif // copy b into x for(ii=0; ii<N; ii++) for(jj=0; jj<nx; jj++) dux[ii+1][nu+jj] = pBAbt[ii][((nu+nx)/bs)*bs*cnx+(nu+nx)%bs+bs*jj]; // copy b // solve the system d_ric_trs_mpc(nx, nu, N, pBAbt, pL, pl, dux, work, 0, Pb, compute_mult, dpi, nb, ng, ngN, pDCt, qx); #if 0 printf("\ndux\n"); for(ii=0; ii<=N; ii++) d_print_mat(1, nx+nu, dux[ii], 1); if(*kk==1) exit(1); #endif #endif // compute t & dlam & dt & alpha alpha = 1.0; d_compute_alpha_hard_mpc(N, nx, nu, nb, ng, ngN, &alpha, t, dt, lam, dlam, lamt, dux, pDCt, d); stat[5*(*kk)] = sigma; stat[5*(*kk)+3] = alpha; alpha *= 0.995; // update x, u, lam, t & compute the duality gap mu d_update_var_hard_mpc(N, nx, nu, nb, ng, ngN, &mu, mu_scal, alpha, ux, dux, t, dt, lam, dlam, pi, dpi); stat[5*(*kk)+4] = mu; // update sigma /* sigma *= sigma_decay;*/ /* if(sigma<sigma_min)*/ /* sigma = sigma_min;*/ /* if(alpha<0.3)*/ /* sigma = sigma_par[0];*/ #if 0 d_print_mat(1, 2*pnb+2*png, lam[0], 1); d_print_mat(1, 2*pnb+2*png, lam[1], 1); d_print_mat(1, 2*pnb+2*png, lam[N], 1); d_print_mat(1, 2*pnb+2*png, t[0], 1); d_print_mat(1, 2*pnb+2*png, t[1], 1); d_print_mat(1, 2*pnb+2*png, t[N], 1); printf("\n%f\n", mu); exit(1); #endif //mu = 13.438997; // increment loop index (*kk)++; } // end of IP loop // restore Hessian for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { pQ[jj][(ll/bs)*bs*cnz+ll%bs+ll*bs] = bd[jj][ll]; pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs] = bl[jj][ll]; } } // successful exit if(mu<=mu_tol) return 0; // max number of iterations reached if(*kk>=k_max) return 1; // no improvement if(alpha<alpha_min) return 2; // impossible return -1; } // end of ipsolver
/* primal-dual interior-point method, box constraints, time invariant matrices (mpc version) */ void d_admm_soft_mpc(int *kk, int k_max, double tol_p, double tol_d, int warm_start, int compute_fact, double rho, double alpha, double *stat, int nx, int nu, int N, double **pBAbt, double **pQ, double **pS, double **lb, double **ub, double **ux_u, double **ux_v, double **ux_w, double **s_u, double **s_v, double **s_w, int compute_mult, double **pi, double *work_memory) // TODO return w ??? { //alpha = 1.0; // no relaxation for the moment TODO remove /*printf("\ncazzo\n");*/ /* int nbx = nb - nu;*/ /* if(nbx<0)*/ /* nbx = 0;*/ /* int nbu = nu<nb ? nu : nb ;*/ // indeces int jj, ll, ii, bs0; // constants const int bs = D_MR; //d_get_mr(); const int ncl = D_NCL; const int nal = bs*ncl; // number of doubles per cache line const int nz = nx+nu+1; const int nxu = nx+nu; const int pnz = bs*((nz+bs-1)/bs); const int pnx = bs*((nx+bs-1)/bs); const int cnz = ncl*((nz+ncl-1)/ncl); const int cnx = ncl*((nx+ncl-1)/ncl); 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; /* array or pointers */ double *(ux_r[N+1]); /* double *(ux_v[N+1]);*/ /* double *(ux_w[N+1]);*/ double *(pL[N+1]); double *(pl[N+1]); double *(bd[N+1]); double *(bl[N+1]); double *(bb[N]); double *work1; double *diag; double *(pSi[N+1]); // inverse of Hessian of soft constraints slack variables /* double *(s_u[N+1]); // soft constraints slack variable*/ /* double *(s_v[N+1]); // soft constraints slack variable*/ /* double *(s_w[N+1]); // soft constraints slack variable*/ double *(s_r[N+1]); // soft constraints slack variable double *(Pb[N]); double *ptr = work_memory; // TODO + 10*anx // inputs and states for(jj=0; jj<=N; jj++) { ux_r[jj] = ptr; ptr += anz; } /* for(jj=0; jj<=N; jj++)*/ /* {*/ /* ux_v[jj] = ptr;*/ /* ptr += anz;*/ /* }*/ /* for(jj=0; jj<=N; jj++)*/ /* {*/ /* ux_w[jj] = ptr;*/ /* ptr += anz;*/ /* }*/ // work space (matrices) for(jj=0; jj<=N; jj++) { pL[jj] = ptr; ptr += pnz*cnl; } // work space (vectors) for(jj=0; jj<=N; jj++) { pl[jj] = ptr; ptr += anz; } // work space work1 = ptr; ptr += 2*anz; diag = ptr; ptr += anz; // backup Hessian space for(jj=0; jj<=N; jj++) { bd[jj] = ptr; bl[jj] = ptr + anz; ptr += 2*anz; } // backup b for(jj=0; jj<N; jj++) { bb[jj] = ptr; ptr += anx; for(ll=0; ll<nx; ll++) { bb[jj][ll] = pBAbt[jj][((nx+nu)/bs)*bs*cnx+(nx+nu)%bs+ll*bs]; } } // inverse (of diagonal) of Hessian of soft constraints slack variables for(jj=0; jj<=N; jj++) { pSi[jj] = ptr; ptr += 2*anx; } /* // soft constraints slack variables*/ /* for(jj=0; jj<=N; jj++)*/ /* {*/ /* s_u[jj] = ptr;*/ /* ptr += 2*anx;*/ /* }*/ /* // soft constraints slack variables*/ /* for(jj=0; jj<=N; jj++)*/ /* {*/ /* s_v[jj] = ptr;*/ /* ptr += 2*anx;*/ /* }*/ /* // soft constraints slack variables*/ /* for(jj=0; jj<=N; jj++)*/ /* {*/ /* s_w[jj] = ptr;*/ /* ptr += 2*anx;*/ /* }*/ // soft constraints slack variables for(jj=0; jj<=N; jj++) { s_r[jj] = ptr; ptr += 2*anx; } // backup of P*b for(jj=0; jj<N; jj++) { Pb[jj] = ptr; ptr += anx; } double temp, v_temp, norm_p=1e3, norm_d=1e3, x_temp; // initialize u and x (cold start) if(warm_start==0) { // states and inputs for(ll=0; ll<nu; ll++) { ux_u[0][ll] = 0.0; } /* for(ll=0; ll<nx; ll++)*/ /* {*/ /* ux_u[jj][nu+ll] = ux[jj][nu+ll];*/ /* }*/ for(jj=1; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { ux_u[jj][ll] = 0.0; } } for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { ux_v[jj][ll] = 0.0; } } for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { ux_w[jj][ll] = 0.0; } } // slack variables of soft constraints for(jj=0; jj<=N; jj++) { for(ll=0; ll<2*anx; ll++) { s_u[jj][ll] = 0.0; } } for(jj=0; jj<=N; jj++) { for(ll=0; ll<2*anx; ll++) { s_v[jj][ll] = 0.0; } } for(jj=0; jj<=N; jj++) { for(ll=0; ll<2*anx; ll++) { s_w[jj][ll] = 0.0; } } } // first iteration (initial factorization) // reset iteration counter *kk = 0; if(compute_fact==1) // factorize hessina in the first iteration { // soft constraints cost function // invert Hessian of soft constraints slack variables for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx; ll++) { // upper pSi[jj][ll] = 1.0/(pS[jj][ll] + rho); s_u[jj][ll] = - pSi[jj][ll]*rho*(s_w[jj][ll] - s_v[jj][ll]); // lower pSi[jj][anx+ll] = 1.0/(pS[jj][anx+ll] + rho); s_u[jj][anx+ll] = - pSi[jj][anx+ll]*rho*(s_w[jj][anx+ll] - s_v[jj][anx+ll]); } } // dynamic // backup Hessian & add rho to diagonal for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { bd[jj][ll] = pQ[jj][(ll/bs)*bs*cnz+ll%bs+ll*bs]; pQ[jj][(ll/bs)*bs*cnz+ll%bs+ll*bs] = bd[jj][ll] + rho; bl[jj][ll] = pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs]; pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs] = bl[jj][ll] + rho*(ux_w[jj][ll] - ux_v[jj][ll]); } } // initial factorization d_ric_sv_mpc(nx, nu, N, pBAbt, pQ, ux_u, pL, work1, diag, compute_mult, pi); // constraints norm_p = 0; for(jj=0; jj<=N; jj++) { // hard constraints on inputs for(ll=0; ll<nu; ll++) { ux_r[jj][ll] = alpha*ux_u[jj][ll] + (1.0-alpha)*ux_v[jj][ll]; // relaxation /* v_temp = - ( - ux_w[jj][ll] - ux_u[jj][ll] );*/ v_temp = - ( - ux_w[jj][ll] - ux_r[jj][ll] ); v_temp = fmax(v_temp, lb[jj][ll]); v_temp = fmin(v_temp, ub[jj][ll]); temp = v_temp - ux_v[jj][ll]; norm_p += temp*temp; ux_v[jj][ll] = v_temp; } // soft constraints on states for(ll=0; ll<nx; ll++) { ux_r[jj][nu+ll] = alpha*ux_u[jj][nu+ll] + (1.0-alpha)*ux_v[jj][nu+ll]; // relaxation s_r[jj][ll] = alpha*s_u[jj][ll] + (1.0-alpha)*s_v[jj][ll]; // relaxation s_r[jj][anx+ll] = alpha*s_u[jj][anx+ll] + (1.0-alpha)*s_v[jj][anx+ll]; // relaxation /* x_temp = - ux_w[jj][nu+ll] - ux_u[jj][nu+ll];*/ x_temp = - ux_w[jj][nu+ll] - ux_r[jj][nu+ll]; v_temp = - ( x_temp ); v_temp = fmax(v_temp, lb[jj][nu+ll]); v_temp = fmin(v_temp, ub[jj][nu+ll]); /* s_v[jj][ll] = fmax( -0.5*( ub[jj][nu+ll] + x_temp + (- s_w[jj][ll] - s_u[jj][ll])), 0);*/ /* s_v[jj][anx+ll] = fmax( -0.5*( - lb[jj][nu+ll] - x_temp + (- s_w[jj][anx+ll] - s_u[jj][anx+ll])), 0);*/ s_v[jj][ll] = fmax( -0.5*( ub[jj][nu+ll] + x_temp + (- s_w[jj][ll] - s_r[jj][ll])), 0); s_v[jj][anx+ll] = fmax( -0.5*( - lb[jj][nu+ll] - x_temp + (- s_w[jj][anx+ll] - s_r[jj][anx+ll])), 0); v_temp = v_temp + s_v[jj][ll] - s_v[jj][anx+ll]; temp = v_temp - ux_v[jj][nu+ll]; norm_p += temp*temp; ux_v[jj][nu+ll] = v_temp; } } norm_p = sqrt(norm_p); stat[0+5*kk[0]] = norm_p; // integral of error norm_d = 0; for(jj=0; jj<=N; jj++) { for(ll=0; ll<nu; ll++) { /* temp = ux_u[jj][ll] - ux_v[jj][ll];*/ temp = ux_r[jj][ll] - ux_v[jj][ll]; // relaxation norm_d += temp*temp; ux_w[jj][ll] += temp; } for(ll=0; ll<nx; ll++) { /* temp = ux_u[jj][nu+ll] - ux_v[jj][nu+ll];*/ temp = ux_r[jj][nu+ll] - ux_v[jj][nu+ll]; // relaxation norm_d += temp*temp; ux_w[jj][nu+ll] += temp; } for(ll=0; ll<nx; ll++) { /* s_w[jj][ll] += s_u[jj][ll] - s_v[jj][ll];*/ /* s_w[jj][anx+ll] += s_u[jj][anx+ll] - s_v[jj][anx+ll];*/ s_w[jj][ll] += s_r[jj][ll] - s_v[jj][ll]; s_w[jj][anx+ll] += s_r[jj][anx+ll] - s_v[jj][anx+ll]; } } norm_d = rho*sqrt(norm_d); stat[1+5*kk[0]] = norm_d; // increment loop index (*kk)++; } // end of factorize hessian else { // backup Hessian for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { bl[jj][ll] = pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs]; } } } // ADMM loop int compute_Pb = compute_fact; while( (*kk<k_max && (norm_p>tol_p || norm_d>tol_d) ) || compute_Pb ) { // soft constraints cost function for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx; ll++) { // upper s_u[jj][ll] = - pSi[jj][ll]*rho*(s_w[jj][ll] - s_v[jj][ll]); // lower s_u[jj][anx+ll] = - pSi[jj][anx+ll]*rho*(s_w[jj][anx+ll] - s_v[jj][anx+ll]); } } // dynamic for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { pl[jj][ll] = bl[jj][ll] + rho*(ux_w[jj][ll] - ux_v[jj][ll]); } } // initialize x with b for(jj=0; jj<N; jj++) { for(ll=0; ll<nx; ll++) { ux_u[jj+1][nu+ll] = bb[jj][ll]; } } // Riccati solver d_ric_trs_mpc(nx, nu, N, pBAbt, pL, pl, ux_u, work1, compute_Pb, Pb, compute_mult, pi); compute_Pb = 0; /*for(jj=0; jj<=N; jj++)*/ /* d_print_mat(1, nu+nx, ux_u[jj], 1);*/ /*for(jj=0; jj<=N; jj++)*/ /* d_print_mat(1, 2*anx, s_u[jj], 1);*/ /*exit(1);*/ // constraints norm_p = 0; for(jj=0; jj<=N; jj++) { // hard constraints on inputs for(ll=0; ll<nu; ll++) { /* v_temp = - ( - ux_w[jj][ll] - ux_u[jj][ll] );*/ ux_r[jj][ll] = alpha*ux_u[jj][ll] + (1.0-alpha)*ux_v[jj][ll]; // relaxation v_temp = - ( - ux_w[jj][ll] - ux_r[jj][ll] ); v_temp = fmax(v_temp, lb[jj][ll]); v_temp = fmin(v_temp, ub[jj][ll]); temp = v_temp - ux_v[jj][ll]; norm_p += temp*temp; ux_v[jj][ll] = v_temp; } // soft constraints on states for(ll=0; ll<nx; ll++) { ux_r[jj][nu+ll] = alpha*ux_u[jj][nu+ll] + (1.0-alpha)*ux_v[jj][nu+ll]; // relaxation s_r[jj][ll] = alpha*s_u[jj][ll] + (1.0-alpha)*s_v[jj][ll]; // relaxation s_r[jj][anx+ll] = alpha*s_u[jj][anx+ll] + (1.0-alpha)*s_v[jj][anx+ll]; // relaxation /* x_temp = - ux_w[jj][nu+ll] - ux_u[jj][nu+ll];*/ x_temp = - ux_w[jj][nu+ll] - ux_r[jj][nu+ll]; v_temp = - ( x_temp ); v_temp = fmax(v_temp, lb[jj][nu+ll]); v_temp = fmin(v_temp, ub[jj][nu+ll]); /* s_v[jj][ll] = fmax( -0.5*( ub[jj][nu+ll] + x_temp + (- s_w[jj][ll] - s_u[jj][ll])), 0);*/ /* s_v[jj][anx+ll] = fmax( -0.5*( - lb[jj][nu+ll] - x_temp + (- s_w[jj][anx+ll] - s_u[jj][anx+ll])), 0);*/ s_v[jj][ll] = fmax( -0.5*( ub[jj][nu+ll] + x_temp + (- s_w[jj][ll] - s_r[jj][ll])), 0); s_v[jj][anx+ll] = fmax( -0.5*( - lb[jj][nu+ll] - x_temp + (- s_w[jj][anx+ll] - s_r[jj][anx+ll])), 0); v_temp = v_temp + s_v[jj][ll] - s_v[jj][anx+ll]; temp = v_temp - ux_v[jj][nu+ll]; norm_p += temp*temp; ux_v[jj][nu+ll] = v_temp; } } norm_p = sqrt(norm_p); stat[0+5*kk[0]] = norm_p; // integral of error norm_d = 0; for(jj=0; jj<=N; jj++) { for(ll=0; ll<nu; ll++) { /* temp = ux_u[jj][ll] - ux_v[jj][ll];*/ temp = ux_r[jj][ll] - ux_v[jj][ll]; // relaxation norm_d += temp*temp; ux_w[jj][ll] += temp; } for(ll=0; ll<nx; ll++) { /* temp = ux_u[jj][nu+ll] - ux_v[jj][nu+ll];*/ temp = ux_r[jj][nu+ll] - ux_v[jj][nu+ll]; // relaxation norm_d += temp*temp; ux_w[jj][nu+ll] += temp; } for(ll=0; ll<nx; ll++) { /* s_w[jj][ll] += s_u[jj][ll] - s_v[jj][ll];*/ /* s_w[jj][anx+ll] += s_u[jj][anx+ll] - s_v[jj][anx+ll];*/ s_w[jj][ll] += s_r[jj][ll] - s_v[jj][ll]; s_w[jj][anx+ll] += s_r[jj][anx+ll] - s_v[jj][anx+ll]; } } norm_d = rho*sqrt(norm_d); stat[1+5*kk[0]] = norm_d; // increment loop index (*kk)++; } // end of ADMM loop // restore Hessian if(compute_fact==1) { for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { pQ[jj][(ll/bs)*bs*cnz+ll%bs+ll*bs] = bd[jj][ll]; pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs] = bl[jj][ll]; } } } /*printf("\nfinal iteration %d, mu %f\n", *kk, mu);*/ return; }
/* primal-dual interior-point method, box constraints, time variant matrices (mpc version) */ int d_ip2_box_mpc(int *kk, int k_max, double mu_tol, double alpha_min, int warm_start, double *sigma_par, double *stat, int nx, int nu, int N, int nb, double **pBAbt, double **pQ, double **db, double **ux, int compute_mult, double **pi, double **lam, double **t, double *work_memory) { int nbu = nu<nb ? nu : nb ; // indeces int jj, ll, ii, bs0; // constants const int bs = D_MR; //d_get_mr(); const int ncl = D_NCL; const int nal = bs*ncl; // number of doubles per cache line const int nz = nx+nu+1; const int nxu = nx+nu; const int pnz = bs*((nz+bs-1)/bs); const int pnx = bs*((nx+bs-1)/bs); const int cnz = ncl*((nz+ncl-1)/ncl); const int cnx = ncl*((nx+ncl-1)/ncl); 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; // initialize work space double *ptr; ptr = work_memory; double *(dux[N+1]); double *(dpi[N+1]); double *(pL[N+1]); double *(pd[N+1]); // pointer to diagonal of Hessian double *(pl[N+1]); // pointer to linear part of Hessian double *(pl2[N+1]); // pointer to linear part of Hessian (backup) double *(bd[N+1]); // backup diagonal of Hessian double *(bl[N+1]); // backup linear part of Hessian double *work; double *diag; double *(dlam[N+1]); double *(dt[N+1]); double *(lamt[N+1]); double *(t_inv[N+1]); double *(Pb[N]); // ptr += (N+1)*(pnx + pnz*cnl + 12*pnz) + 3*pnz; // inputs and states for(jj=0; jj<=N; jj++) { dux[jj] = ptr; ptr += anz; } // equality constr multipliers for(jj=0; jj<=N; jj++) { dpi[jj] = ptr; ptr += anx; } // Hessian for(jj=0; jj<=N; jj++) { pd[jj] = pQ[jj]; pl[jj] = pQ[jj] + ((nu+nx)/bs)*bs*cnz + (nu+nx)%bs; bd[jj] = ptr; bl[jj] = ptr + anz; ptr += 2*anz; // backup for(ll=0; ll<nx+nu; ll++) { bd[jj][ll] = pQ[jj][(ll/bs)*bs*cnz+ll%bs+ll*bs]; bl[jj][ll] = pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs]; } } // work space for(jj=0; jj<=N; jj++) { pL[jj] = ptr; ptr += pnz*cnl; } for(jj=0; jj<=N; jj++) { pl2[jj] = ptr; ptr += anz; } work = ptr; ptr += 2*anz; diag = ptr; ptr += anz; // slack variables, Lagrangian multipliers for inequality constraints and work space (assume # box constraints <= 2*(nx+nu) < 2*pnz) for(jj=0; jj<=N; jj++) { dlam[jj] = ptr; dt[jj] = ptr + anb;; ptr += 2*anb; } for(jj=0; jj<=N; jj++) { lamt[jj] = ptr; ptr += anb; } for(jj=0; jj<=N; jj++) { t_inv[jj] = ptr; ptr += anb; } // backup of P*b for(jj=0; jj<N; jj++) { Pb[jj] = ptr; ptr += anx; } double temp0, temp1; double alpha, mu, mu_aff; double mu_scal = 1.0/(N*2*nb); double sigma, sigma_decay, sigma_min; sigma = sigma_par[0]; //0.4; sigma_decay = sigma_par[1]; //0.3; sigma_min = sigma_par[2]; //0.01; // initialize ux & t>0 (slack variable) d_init_ux_pi_t_box_mpc(N, nx, nu, nbu, nb, ux, pi, db, t, warm_start); // initialize lambda>0 (multiplier of the inequality constraint) d_init_lam_mpc(N, nu, nbu, nb, t, lam); // initialize pi for(jj=0; jj<=N; jj++) for(ll=0; ll<nx; ll++) dpi[jj][ll] = 0.0; // initialize dux for(ll=0; ll<nx; ll++) dux[0][nu+ll] = ux[0][nu+ll]; // compute the duality gap alpha = 0.0; // needed to compute mu !!!!! d_compute_mu_mpc(N, nbu, nu, nb, &mu, mu_scal, alpha, lam, dlam, t, dt); // set to zero iteration count *kk = 0; // larger than minimum accepted step size alpha = 1.0; // IP loop while( *kk<k_max && mu>mu_tol && alpha>=alpha_min ) { //update cost function matrices and vectors (box constraints) d_update_hessian_box_mpc(N, nbu, (nu/bs)*bs, nb, cnz, 0.0, t, t_inv, lam, lamt, dlam, bd, bl, pd, pl, pl2, db); // compute the search direction: factorize and solve the KKT system d_ric_sv_mpc(nx, nu, N, pBAbt, pQ, dux, pL, work, diag, compute_mult, dpi); // compute t_aff & dlam_aff & dt_aff & alpha for(jj=0; jj<=N; jj++) for(ll=0; ll<2*nb; ll++) dlam[jj][ll] = 0.0; alpha = 1.0; d_compute_alpha_box_mpc(N, 2*nbu, 2*nu, 2*nb, &alpha, t, dt, lam, dlam, lamt, dux, db); stat[5*(*kk)] = sigma; stat[5*(*kk)+1] = alpha; alpha *= 0.995; // compute the affine duality gap d_compute_mu_mpc(N, nbu, nu, nb, &mu_aff, mu_scal, alpha, lam, dlam, t, dt); stat[5*(*kk)+2] = mu_aff; // compute sigma sigma = mu_aff/mu; sigma = sigma*sigma*sigma; if(sigma<sigma_min) sigma = sigma_min; // first stage for(ii=0; ii<2*nbu; ii+=2) { dlam[0][ii+0] = t_inv[0][ii+0]*(sigma*mu - dlam[0][ii+0]*dt[0][ii+0]); // !!!!! dlam[0][ii+1] = t_inv[0][ii+1]*(sigma*mu - dlam[0][ii+1]*dt[0][ii+1]); // !!!!! pl2[0][ii/2] += dlam[0][ii+1] - dlam[0][ii+0]; } // middle stages for(jj=1; jj<N; jj++) { for(ii=0; ii<2*nb; ii+=2) { dlam[jj][ii+0] = t_inv[jj][ii+0]*(sigma*mu - dlam[jj][ii+0]*dt[jj][ii+0]); // !!!!! dlam[jj][ii+1] = t_inv[jj][ii+1]*(sigma*mu - dlam[jj][ii+1]*dt[jj][ii+1]); // !!!!! pl2[jj][ii/2] += dlam[jj][ii+1] - dlam[jj][ii+0]; } } // last stages for(ii=2*nu; ii<2*nb; ii+=2) { dlam[jj][ii+0] = t_inv[jj][ii+0]*(sigma*mu - dlam[jj][ii+0]*dt[jj][ii+0]); // !!!!! dlam[jj][ii+1] = t_inv[jj][ii+1]*(sigma*mu - dlam[jj][ii+1]*dt[jj][ii+1]); // !!!!! pl2[jj][ii/2] += dlam[jj][ii+1] - dlam[jj][ii+0]; } // copy b into x for(ii=0; ii<N; ii++) for(jj=0; jj<nx; jj++) dux[ii+1][nu+jj] = pBAbt[ii][((nu+nx)/bs)*bs*cnx+(nu+nx)%bs+bs*jj]; // copy b // solve the system d_ric_trs_mpc(nx, nu, N, pBAbt, pL, pl2, dux, work, 1, Pb, compute_mult, dpi); // compute t & dlam & dt & alpha alpha = 1.0; d_compute_alpha_box_mpc(N, 2*nbu, 2*nu, 2*nb, &alpha, t, dt, lam, dlam, lamt, dux, db); stat[5*(*kk)] = sigma; stat[5*(*kk)+3] = alpha; alpha *= 0.995; // update x, u, lam, t & compute the duality gap mu d_update_var_mpc(nx, nu, N, nb, nbu, &mu, mu_scal, alpha, ux, dux, t, dt, lam, dlam, pi, dpi); stat[5*(*kk)+4] = mu; // update sigma /* sigma *= sigma_decay;*/ /* if(sigma<sigma_min)*/ /* sigma = sigma_min;*/ /* if(alpha<0.3)*/ /* sigma = sigma_par[0];*/ // increment loop index (*kk)++; } // end of IP loop // restore Hessian for(jj=0; jj<=N; jj++) { for(ll=0; ll<nx+nu; ll++) { pQ[jj][(ll/bs)*bs*cnz+ll%bs+ll*bs] = bd[jj][ll]; pQ[jj][((nx+nu)/bs)*bs*cnz+(nx+nu)%bs+ll*bs] = bl[jj][ll]; } } // successful exit if(mu<=mu_tol) return 0; // max number of iterations reached if(*kk>=k_max) return 1; // no improvement if(alpha<alpha_min) return 2; // impossible return -1; } // end of ipsolver