/* 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
