int main()
{
printf("\n");
printf("\n");
printf("\n");
printf(" HPMPC -- Library for High-Performance implementation of solvers for MPC.\n");
printf(" Copyright (C) 2014-2015 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");
int ii, jj, ll;
double **dummy;
int ** int_dummy;
const int bs = D_MR; //d_get_mr();
const int ncl = D_NCL;
const int nal = bs*ncl; // number of doubles per cache line
int nx, nu, N, nrep;
// timing variables
float time_ric_diag, time_ric_full, time_ric_full_tv, time_ip_diag, time_ip_full, time_ip_full_tv;
/************************************************
* test of riccati eye/diag & size-variant
************************************************/
#if 1
// horizon length
N = 11;
// base nx and nu
int nx0 = 2;
int nu0 = 1;
// size-varing
int nxx[N+1];
for(ii=0; ii<=N; ii++) nxx[ii] = (N+1-ii)*nx0 + nu0;
int pnxx[N+1];
for(ii=0; ii<=N; ii++) pnxx[ii] = (nxx[ii]+bs-1)/bs*bs;
int cnxx[N+1];
for(ii=0; ii<=N; ii++) cnxx[ii] = (nxx[ii]+ncl-1)/ncl*ncl;
int nuu[N+1];
for(ii=0; ii<N; ii++) nuu[ii] = nu0;
nuu[N] = 0; // !!!!!
int pnuu[N+1];
for(ii=0; ii<N; ii++) pnuu[ii] = (nuu[ii]+bs-1)/bs*bs;
pnuu[N] = 0; // !!!!!
int cnuu[N+1];
for(ii=0; ii<N; ii++) cnuu[ii] = (nuu[ii]+ncl-1)/ncl*ncl;
cnuu[N] = 0; // !!!!!
//for(ii=0; ii<=N; ii++) printf("\n%d %d %d\n", nxx[ii], pnxx[ii], cnxx[ii]);
//for(ii=0; ii<N; ii++) printf("\n%d %d %d\n", nuu[ii], pnuu[ii], cnuu[ii]);
// factorization
printf("\nRiccati diag\n\n");
// data memory space
double *hdA[N];
double *hpBt[N];
double *hpR[N];
double *hpS[N];
double *hpQ[N+1];
double *hpLK[N];
double *hpP[N+1];
double *pK;
for(ii=0; ii<N; ii++)
{
d_zeros_align(&hdA[ii], pnxx[ii], 1);
d_zeros_align(&hpBt[ii], pnuu[ii], cnxx[ii+1]);
d_zeros_align(&hpR[ii], pnuu[ii], cnuu[ii]);
d_zeros_align(&hpS[ii], pnxx[ii], cnuu[ii]);
d_zeros_align(&hpQ[ii], pnxx[ii], cnxx[ii]);
d_zeros_align(&hpLK[ii], pnuu[ii]+pnxx[ii], cnuu[ii]);
d_zeros_align(&hpP[ii], pnxx[ii], cnxx[ii]);
}
d_zeros_align(&hpQ[N], pnxx[N], cnxx[N]);
d_zeros_align(&hpP[N], pnxx[N], cnxx[N]);
d_zeros_align(&pK, pnxx[0], cnuu[0]); // max(nx) x nax(nu)
// dA
for(ii=0; ii<N; ii++)
for(jj=0; jj<nxx[ii+1]; jj++)
hdA[ii][jj] = 1.0;
//d_print_mat(1, cnxx[1], hdA[0], 1);
// B
double *eye_nu0; d_zeros(&eye_nu0, nu0, nu0);
for(jj=0; jj<nu0; jj++) eye_nu0[jj*(nu0+1)] = 1.0;
double *ptrB = BBB;
for(ii=0; ii<N; ii++)
{
d_cvt_mat2pmat(nuu[ii], nuu[ii], eye_nu0, nuu[ii], 0, hpBt[ii], cnxx[ii+1]);
d_cvt_tran_mat2pmat(nxx[ii+1]-nuu[ii], nuu[ii], ptrB, nxx[ii+1]-nuu[ii], 0, hpBt[ii]+nuu[ii]*bs, cnxx[ii+1]);
ptrB += nxx[ii+1] - nuu[ii];
}
free(eye_nu0);
//d_print_pmat(pnuu[0], cnxx[1], bs, hpBt[0], cnxx[0]);
//d_print_pmat(pnuu[1], cnxx[2], bs, hpBt[1], cnxx[1]);
//d_print_pmat(pnuu[2], cnxx[3], bs, hpBt[2], cnxx[2]);
//d_print_pmat(pnuu[N-1], cnxx[N-1], bs, hpBt[N-2], cnxx[N-2]);
//d_print_pmat(pnuu[N-1], cnxx[N], bs, hpBt[N-1], cnxx[N-1]);
// R
// penalty on du
for(ii=0; ii<N; ii++)
for(jj=0; jj<nuu[ii]; jj++)
hpR[ii][jj/bs*bs*cnuu[ii]+jj%bs+jj*bs] = 0.0;
//for(ii=0; ii<N; ii++)
// d_print_pmat(pnuu[ii], cnuu[ii], bs, hpR[ii], pnuu[ii]);
//d_print_pmat(pnuu[0], cnuu[0], bs, hpR[0], pnuu[0]);
// S (zero)
// Q
for(ii=0; ii<=N; ii++)
{
// penalty on u
for(jj=0; jj<nu0; jj++)
hpQ[ii][jj/bs*bs*cnxx[ii]+jj%bs+jj*bs] = 1.0;
// penalty on x
// for(jj==1; jj<nxx[ii]-nx0; jj++)
// hpQ[ii][jj/bs*bs*cnxx[ii]+jj%bs+jj*bs] = 0.0002;
for(jj=nxx[ii]-nx0; jj<nxx[ii]; jj++)
hpQ[ii][jj/bs*bs*cnxx[ii]+jj%bs+jj*bs] = 1.0;
}
//for(ii=0; ii<=N; ii++)
// d_print_pmat(pnxx[ii], cnxx[ii], bs, hpQ2[ii], cnxx[ii]);
//d_print_pmat(pnxx[0], cnxx[0], bs, hpQ2[0], cnxx[0]);
//d_print_pmat(pnxx[1], cnxx[1], bs, hpQ2[1], cnxx[1]);
//d_print_pmat(pnxx[N-1], cnxx[N-1], bs, hpQ2[N-1], cnxx[N-1]);
//d_print_pmat(pnxx[N], cnxx[N], bs, hpQ2[N], cnxx[N]);
//exit(1);
// work space
double *diag; d_zeros_align(&diag, pnxx[0]+pnuu[0], 1);
// factorization
printf("\nfactorization ...\n");
d_ric_diag_trf_mpc(N, nxx, nuu, hdA, hpBt, hpR, hpS, hpQ, hpLK, pK, hpP, diag);
printf("\nfactorization done\n\n");
#if 1
//d_print_pmat(nxx[0], nxx[0], bs, hpP[0], cnxx[0]);
//d_print_pmat(nxx[1], nxx[1], bs, hpP[1], cnxx[1]);
//d_print_pmat(nxx[N-2], nxx[N-2], bs, hpP[N-2], cnxx[N-2]);
//d_print_pmat(nxx[N-1], nxx[N-1], bs, hpP[N-1], cnxx[N-1]);
//d_print_pmat(nxx[N], nxx[N], bs, hpP[N], cnxx[N]);
//for(ii=0; ii<=N; ii++)
// d_print_pmat(pnuu[ii]+nxx[ii], nuu[ii], bs, hpLK[ii], cnuu[ii]);
//d_print_pmat(pnuu[0]+nxx[0], nuu[0], bs, hpLK[0], cnuu[0]);
//d_print_pmat(pnuu[1]+nxx[1], nuu[1], bs, hpLK[1], cnuu[1]);
//d_print_pmat(pnuu[2]+nxx[2], nuu[2], bs, hpLK[2], cnuu[2]);
//d_print_pmat(pnuu[N-3]+nxx[N-3], nuu[N-3], bs, hpLK[N-3], cnuu[N-3]);
//d_print_pmat(pnuu[N-2]+nxx[N-2], nuu[N-2], bs, hpLK[N-2], cnuu[N-2]);
//d_print_pmat(pnuu[N-1]+nxx[N-1], nuu[N-1], bs, hpLK[N-1], cnuu[N-1]);
#endif
// backward-forward solution
// data memory space
double *hrq[N+1];
double *hux[N+1];
double *hpi[N+1];
double *hPb[N];
double *hb[N];
for(ii=0; ii<N; ii++)
{
d_zeros_align(&hrq[ii], pnuu[ii]+pnxx[ii], 1);
d_zeros_align(&hux[ii], pnuu[ii]+pnxx[ii], 1);
d_zeros_align(&hpi[ii], pnxx[ii], 1);
d_zeros_align(&hPb[ii], pnxx[ii+1], 1);
d_zeros_align(&hb[ii], pnxx[ii+1], 1);
}
d_zeros_align(&hrq[N], pnuu[N]+pnxx[N], 1);
d_zeros_align(&hux[N], pnuu[N]+pnxx[N], 1);
d_zeros_align(&hpi[N], pnxx[N], 1);
double *work_diag; d_zeros_align(&work_diag, pnxx[0], 1);
for(ii=0; ii<=N; ii++)
for(jj=0; jj<nuu[ii]; jj++)
hrq[ii][jj] = 0.0;
for(ii=0; ii<=N; ii++)
for(jj=0; jj<nxx[ii]; jj++)
hrq[ii][nuu[ii]+jj] = 0.0;
for(ii=0; ii<N; ii++)
for(jj=0; jj<nxx[ii+1]; jj++)
hb[ii][jj] = 0.0;
// x0
for(jj=0; jj<nuu[0]; jj++)
{
hux[0][jj] = 0.0;
}
for(; jj<nuu[0]+nu0; jj++)
{
hux[0][jj] = 7.5097;
}
for(; jj<nxx[0]; jj+=2)
{
hux[0][jj+0] = 15.01940;
hux[0][jj+1] = 0.0;
}
//d_print_mat(1, nuu[0]+nxx[0], hux2[0], 1);
printf("\nbackward-forward solution ...\n");
d_ric_diag_trs_mpc(N, nxx, nuu, hdA, hpBt, hpLK, hpP, hb, hrq, hux, 1, hPb, 1, hpi, work_diag);
printf("\nbackward-forward solution done\n\n");
#if 1
printf("\nux\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hux[ii], 1);
#endif
// residuals
// data memory space
double *hres_rq[N+1];
double *hres_b[N];
for(ii=0; ii<N; ii++)
{
d_zeros_align(&hres_rq[ii], pnuu[ii]+pnxx[ii], 1);
d_zeros_align(&hres_b[ii], pnxx[ii+1], 1);
}
d_zeros_align(&hres_rq[N], pnuu[N]+pnxx[N], 1);
printf("\nresuduals ...\n");
d_res_diag_mpc(N, nxx, nuu, hdA, hpBt, hpR, hpS, hpQ, hb, hrq, hux, hpi, hres_rq, hres_b, work_diag);
printf("\nresiduals done\n\n");
#if 1
printf("\nres_q\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hres_rq[ii], 1);
printf("\nres_b\n");
for(ii=0; ii<N; ii++)
d_print_mat(1, nxx[ii+1], hres_b[ii], 1);
#endif
// timing
struct timeval tv20, tv21;
#if 1
printf("\ntiming ...\n\n");
gettimeofday(&tv20, NULL); // start
nrep = 10000;
for(ii=0; ii<nrep; ii++)
{
d_ric_diag_trf_mpc(N, nxx, nuu, hdA, hpBt, hpR, hpS, hpQ, hpLK, pK, hpP, diag);
d_ric_diag_trs_mpc(N, nxx, nuu, hdA, hpBt, hpLK, hpP, hb, hrq, hux, 1, hPb, 1, hpi, work_diag);
}
gettimeofday(&tv21, NULL); // start
time_ric_diag = (float) (tv21.tv_sec-tv20.tv_sec)/(nrep+0.0)+(tv21.tv_usec-tv20.tv_usec)/(nrep*1e6);
printf("\ntiming done\n\n");
#endif
#if 1
printf("\nRiccati full\n\n");
// size-variant full
int nzz[N+1];
for(ii=0; ii<=N; ii++) nzz[ii] = nuu[ii] + nxx[ii] + 1;
int pnzz[N+1];
for(ii=0; ii<=N; ii++) pnzz[ii] = (nzz[ii]+bs-1)/bs*bs;
int cnzz[N+1];
for(ii=0; ii<=N; ii++) cnzz[ii] = (nzz[ii]+ncl-1)/ncl*ncl;
int anzz[N+1];
for(ii=0; ii<=N; ii++) anzz[ii] = (nzz[ii]+nal-1)/nal*nal;
int cnll[N+1];
for(ii=0; ii<=N; ii++) cnll[ii] = cnzz[ll]<cnxx[ll]+ncl ? cnxx[ll]+ncl : cnzz[ll];
int nzero[N+1];
for(ii=0; ii<=N; ii++) nzero[ii] = 0;
double *hpBAbt_tv[N];
double *hpRSQ_tv[N+1];
double *hpL_tv[N+1];
double *hl[N+1];
for(ii=0; ii<N; ii++)
{
d_zeros_align(&hpBAbt_tv[ii], pnzz[ii], cnxx[ii+1]);
d_zeros_align(&hpRSQ_tv[ii], pnzz[ii], cnzz[ii]);
d_zeros_align(&hpL_tv[ii], pnzz[ii], cnll[ii]);
d_zeros_align(&hl[ii], anzz[ii], 1);
}
d_zeros_align(&hpRSQ_tv[N], pnzz[N], cnzz[N]);
d_zeros_align(&hpL_tv[N], pnzz[N], cnll[N]);
d_zeros_align(&hl[N], anzz[ii], 1);
double *work_ric_tv; d_zeros_align(&work_ric_tv, pnzz[0], cnxx[0]);
for(ii=0; ii<N; ii++)
{
d_copy_pmat(nuu[ii], nxx[ii+1], bs, hpBt[ii], cnxx[ii], hpBAbt_tv[ii], cnxx[ii+1]);
for(jj=0; jj<nxx[ii+1]; jj++) hpBAbt_tv[ii][(nuu[ii]+jj)/bs*bs*cnxx[ii+1]+(nuu[ii]+jj)%bs+jj*bs] = 1.0;
for(jj=0; jj<nxx[ii+1]; jj++) hpBAbt_tv[ii][(nuu[ii]+nxx[ii])/bs*bs*cnxx[ii+1]+(nuu[ii]+nxx[ii])%bs+jj*bs] = hb[ii][jj];
//d_print_pmat(nzz[ii], nxx[ii+1], bs, hpBAbt_tv[ii], cnxx[ii+1]);
}
for(ii=0; ii<=N; ii++)
{
// R
// penalty on du
for(jj=0; jj<nuu[ii]; jj++)
hpRSQ_tv[ii][jj/bs*bs*cnzz[ii]+jj%bs+jj*bs] = 0.0;
// Q
// penalty on u
for(; jj<nuu[ii]+nu0; jj++)
hpRSQ_tv[ii][jj/bs*bs*cnzz[ii]+jj%bs+jj*bs] = 1.0;
// penalty on x
for(jj=nuu[ii]+nxx[ii]-nx0; jj<nuu[ii]+nxx[ii]; jj++)
hpRSQ_tv[ii][jj/bs*bs*cnzz[ii]+jj%bs+jj*bs] = 1.0;
// r q
for(jj=0; jj<nuu[ii]+nxx[ii]; jj++) hpRSQ_tv[ii][(nuu[ii]+nxx[ii])/bs*bs*cnzz[ii]+(nuu[ii]+nxx[ii])%bs+jj*bs] = hrq[ii][jj];
//d_print_pmat(nzz[ii], nzz[ii], bs, hpRSQ_tv[ii], cnzz[ii]);
}
printf("\nfactorization and backward-forward solution ...\n");
d_ric_sv_mpc_tv(N, nxx, nuu, hpBAbt_tv, hpRSQ_tv, hux, hpL_tv, work_ric_tv, diag, COMPUTE_MULT, hpi, nzero, int_dummy, dummy, dummy, nzero, dummy, dummy, dummy, 0);
printf("\nfactorization and backward-forward solution done\n\n");
#if 0
for(ii=0; ii<=N; ii++)
d_print_pmat(nzz[ii], nzz[ii], bs, hpL_tv[ii], cnzz[ii]);
#endif
printf("\nux\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hux[ii], 1);
for(ii=0; ii<N; ii++)
for(jj=0; jj<nxx[ii+1]; jj++)
hux[ii+1][nuu[ii+1]+jj] = hb[ii][jj];
printf("\nbackward-forward solution ...\n");
d_ric_trs_mpc_tv(N, nxx, nuu, hpBAbt_tv, hpL_tv, hrq, hl, hux, work_ric_tv, 1, hPb, COMPUTE_MULT, hpi, nzero, int_dummy, dummy, nzero, dummy, dummy);
printf("\nbackward-forward solution done\n\n");
printf("\nux\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hux[ii], 1);
//exit(1);
printf("\nresuduals ...\n");
d_res_diag_mpc(N, nxx, nuu, hdA, hpBt, hpR, hpS, hpQ, hb, hrq, hux, hpi, hres_rq, hres_b, work_diag);
printf("\nresiduals done\n\n");
#if 1
printf("\nres_q\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hres_rq[ii], 1);
printf("\nres_b\n");
for(ii=0; ii<N; ii++)
d_print_mat(1, nxx[ii+1], hres_b[ii], 1);
#endif
#if 1
printf("\ntiming ...\n\n");
gettimeofday(&tv20, NULL); // start
nrep = 10000;
for(ii=0; ii<nrep; ii++)
{
d_ric_sv_mpc_tv(N, nxx, nuu, hpBAbt_tv, hpRSQ_tv, hux, hpL_tv, work_ric_tv, diag, COMPUTE_MULT, hpi, nzero, int_dummy, dummy, dummy, nzero, dummy, dummy, dummy, 0);
}
gettimeofday(&tv21, NULL); // start
time_ric_full_tv = (float) (tv21.tv_sec-tv20.tv_sec)/(nrep+0.0)+(tv21.tv_usec-tv20.tv_usec)/(nrep*1e6);
printf("\ntiming done\n\n");
#endif
#endif
#if 1
// IPM
printf("\nIPM diag\n\n");
int kk = -1;
int kmax = 50;
double mu0 = 1;
double mu_tol = 1e-8;
double alpha_min = 1e-12;
double sigma_par[] = {0.4, 0.3, 0.01};
double stat[5*50] = {};
int nbb[N+1];
nbb[0] = nu0;//nuu[0]; // XXX !!!!!!!!!!!!!!
for(ii=1; ii<N; ii++) nbb[ii] = 2*nu0 + nx0; //nuu[ii] + nxx[ii];
nbb[N] = nu0 + nx0;
int *(idxb[N+1]);
for(ii=0; ii<=N; ii++)
{
idxb[ii] = (int *) malloc(nbb[ii]*sizeof(int));
}
int pnbb[N+1];
for(ii=0; ii<=N; ii++) pnbb[ii] = (nbb[ii]+bs-1)/bs*bs;
// data memory space
double *hd[N+1];
double *hlam[N+1];
double *ht[N+1];
double *hres_d[N+1];
for(ii=0; ii<=N; ii++)
{
d_zeros_align(&hd[ii], 2*pnbb[ii], 1);
d_zeros_align(&hlam[ii], 2*pnbb[ii], 1);
d_zeros_align(&ht[ii], 2*pnbb[ii], 1);
d_zeros_align(&hres_d[ii], 2*pnbb[ii], 1);
}
double mu = -1;
//printf("\nbounds\n");
ii = 0; // initial stage
ll = 0;
for(jj=0; jj<nuu[ii]; jj++)
{
hd[ii][ll] = -20.5;
hd[ii][pnbb[ii]+ll] = -20.5;
idxb[ii][ll] = jj;
ll++;
}
//d_print_mat(1, 2*pnbb[ii], hd[ii], 1);
for(ii=1; ii<=N; ii++)
{
ll = 0;
for(jj=0; jj<nuu[ii]; jj++)
{
hd[ii][ll] = -20.5;
hd[ii][pnbb[ii]+ll] = -20.5;
idxb[ii][ll] = jj;
ll++;
}
for(; jj<nuu[ii]+nu0; jj++)
{
hd[ii][ll] = - 2.5; // -2.5
hd[ii][pnbb[ii]+ll] = -10.0; // -10
idxb[ii][ll] = jj;
ll++;
}
//for(; jj<nbb[ii]-nx0; jj++)
//for(; jj<nbb[ii]; jj++)
//{
//hd[ii][jj] = -100.0;
//hd[ii][pnbb[ii]+jj] = -100.0;
//idxb[ii][ll] = jj;
//ll++;
//}
jj += nx0*(N-ii);
hd[ii][ll+0] = - 0.0; // 0
hd[ii][pnbb[ii]+ll+0] = -20.0; // -20
idxb[ii][ll] = jj;
ll++;
jj++;
hd[ii][ll+0] = -10.0; // -10
hd[ii][pnbb[ii]+ll+0] = -10.0; // -10
idxb[ii][ll] = jj;
ll++;
jj++;
//d_print_mat(1, 2*pnbb[ii], hd[ii], 1);
}
#if 0
for(ii=0; ii<=N; ii++)
{
for(jj=0; jj<nbb[ii]; jj++)
printf("%d\t", idxb[ii][jj]);
printf("\n");
}
exit(1);
#endif
for(jj=0; jj<nuu[0]; jj++)
{
hux[0][jj] = 0.0;
}
for(; jj<nuu[0]+nu0; jj++)
{
hux[0][jj] = 7.5097;
}
for(; jj<nxx[0]; jj+=2)
{
hux[0][jj+0] = 15.01940;
hux[0][jj+1] = 0.0;
}
//d_print_mat(1, nuu[0]+nxx[0], hux2[0], 1);
int pnxM = pnxx[0];
int pnuM = pnuu[0];
int cnuM = cnuu[0];
int anxx[N+1];
for(ii=0; ii<=N; ii++) anxx[ii] = (nxx[ii]+nal-1)/nal*nal;
int anuu[N+1];
for(ii=0; ii<=N; ii++) anuu[ii] = (nuu[ii]+nal-1)/nal*nal;
int work_space_ip_double = 0;
for(ii=0; ii<=N; ii++)
work_space_ip_double += anuu[ii] + 3*anxx[ii] + (pnuu[ii]+pnxx[ii])*cnuu[ii] + pnxx[ii]*cnxx[ii] + 12*pnbb[ii];
work_space_ip_double += pnxM*cnuM + pnxM + pnuM;
int work_space_ip_int = (N+1)*7*sizeof(int);
work_space_ip_int = (work_space_ip_int+63)/64*64;
work_space_ip_int /= sizeof(int);
printf("\nIPM diag work space size: %d double + %d int\n\n", work_space_ip_double, work_space_ip_int);
double *work_space_ip; d_zeros_align(&work_space_ip, work_space_ip_double+(work_space_ip_int+1)/2, 1); // XXX assume sizeof(double) = 2 * sizeof(int) !!!!!
printf("\nIPM solution ...\n");
d_ip2_diag_mpc(&kk, kmax, mu0, mu_tol, alpha_min, 0, sigma_par, stat, N, nxx, nuu, nbb, idxb, hdA, hpBt, hpR, hpS, hpQ, hb, hd, hrq, hux, 1, hpi, hlam, ht, work_space_ip);
printf("\nIPM solution done\n");
printf("\nux\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hux[ii], 1);
printf("\nlam\n");
for(ii=0; ii<=N; ii++)
{
d_print_mat(1, nbb[ii], hlam[ii], 1);
d_print_mat(1, nbb[ii], hlam[ii]+pnbb[ii], 1);
}
printf("\nt\n");
for(ii=0; ii<=N; ii++)
{
d_print_mat(1, nbb[ii], ht[ii], 1);
d_print_mat(1, nbb[ii], ht[ii]+pnbb[ii], 1);
}
printf("\nstatistics\n\n");
for(ii=0; ii<kk; ii++)
printf("%d\t%f\t%f\t%f\t%e\t%f\t%f\t%e\n", ii+1, stat[5*ii+0], stat[5*ii+1], stat[5*ii+2], stat[5*ii+2], stat[5*ii+3], stat[5*ii+4], stat[5*ii+4]);
printf("\n\n");
// residuals
printf("\nresuduals IPM ...\n");
d_res_ip_diag_mpc(N, nxx, nuu, nbb, idxb, hdA, hpBt, hpR, hpS, hpQ, hb, hrq, hd, hux, hpi, hlam, ht, hres_rq, hres_b, hres_d, &mu, work_diag);
printf("\nresiduals IPM done\n");
printf("\nres_rq\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hres_rq[ii], 1);
printf("\nres_b\n");
for(ii=0; ii<N; ii++)
d_print_mat(1, nxx[ii+1], hres_b[ii], 1);
printf("\nres_d\n");
for(ii=0; ii<=N; ii++)
{
d_print_mat(1, nbb[ii], hres_d[ii], 1);
d_print_mat(1, nbb[ii], hres_d[ii]+pnbb[ii], 1);
}
printf("\nres_mu\n");
d_print_mat(1, 1, &mu, 1);
// timing
printf("\ntiming ...\n\n");
gettimeofday(&tv20, NULL); // start
nrep = 1000;
for(ii=0; ii<nrep; ii++)
{
d_ip2_diag_mpc(&kk, kmax, mu0, mu_tol, alpha_min, 0, sigma_par, stat, N, nxx, nuu, nbb, idxb, hdA, hpBt, hpR, hpS, hpQ, hb, hd, hrq, hux, 1, hpi, hlam, ht, work_space_ip);
}
gettimeofday(&tv21, NULL); // start
printf("\ntiming done\n\n");
time_ip_diag = (float) (tv21.tv_sec-tv20.tv_sec)/(nrep+0.0)+(tv21.tv_usec-tv20.tv_usec)/(nrep*1e6);
// simulation
printf("\nsimulation ...\n\n");
nrep = 15;
for(ii=0; ii<nrep; ii++)
{
d_ip2_diag_mpc(&kk, kmax, mu0, mu_tol, alpha_min, 0, sigma_par, stat, N, nxx, nuu, nbb, idxb, hdA, hpBt, hpR, hpS, hpQ, hb, hd, hrq, hux, 1, hpi, hlam, ht, work_space_ip);
dgemv_t_lib(nuu[0], nxx[0], hpBt[0], cnxx[0], hux[0], hux[0]+nuu[0], 1);
for(jj=0; jj<nxx[0]-nx0-nu0; jj++) hux[0][nuu[0]+nxx[0]-jj-1] = hux[0][nuu[0]+nxx[0]-jj-1-nx0];
printf("\nsimulation step = %d, IPM iterations = %d, mu = %e\n\n", ii, kk, stat[5*(kk-1)+4]);
d_print_mat(1, nuu[0]+nxx[0], hux[0], 1);
}
printf("\nsimulation done\n\n");
//exit(1);
#if 1
// IPM
printf("\nIPM full\n\n");
int ngg[N+1];
for(ii=0; ii<=N; ii++) ngg[ii] = 0;
int pngg[N+1];
for(ii=0; ii<=N; ii++) pngg[ii] = (ngg[ii]+bs-1)/bs*bs;
//int pnzM = pnzz[0]; // max
//int cnxgM = cnxx[0]; // max
//int work_space_int_size = 7*(N+1);
//int work_space_double_size = pnzM*cnxgM + pnzM;
//for(ii=0; ii<=N; ii++)
// work_space_double_size += pnzz[ii]*cnll[ii] + 3*anzz[ii] + 2*anxx[ii] + 14*pnbb[ii] + 10*pngg[ii];
//printf("\nIPM diag work space size: %d double + %d int\n\n", work_space_double_size, work_space_int_size);
//double *work_ipm_tv_double; d_zeros_align(&work_ipm_tv_double, work_space_double_size, 1);
double *work_ipm_tv_double; d_zeros_align(&work_ipm_tv_double, d_ip2_hard_mpc_tv_work_space_size_double(N, nxx, nuu, nbb, ngg), 1);
//int *work_ipm_tv_int = (int *) malloc(work_space_int_size*sizeof(int));
int *work_ipm_tv_int = (int *) malloc(d_ip2_hard_mpc_tv_work_space_size_int(N, nxx, nuu, nbb, ngg)*sizeof(int));
for(jj=0; jj<nuu[0]; jj++)
{
hux[0][jj] = 0.0;
}
for(; jj<nuu[0]+nu0; jj++)
{
hux[0][jj] = 7.5097;
}
for(; jj<nxx[0]; jj+=2)
{
hux[0][jj+0] = 15.01940;
hux[0][jj+1] = 0.0;
}
//d_print_mat(1, nuu[0]+nxx[0], hux2[0], 1);
printf("\nIPM solution ...\n");
d_ip2_hard_mpc_tv(&kk, kmax, mu0, mu_tol, alpha_min, 0, sigma_par, stat, N, nxx, nuu, nbb, idxb, ngg, hpBAbt_tv, hpRSQ_tv, dummy, hd, hux, 1, hpi, hlam, ht, work_ipm_tv_double, work_ipm_tv_int);
printf("\nIPM solution done\n");
printf("\nux\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hux[ii], 1);
printf("\nlam\n");
for(ii=0; ii<=N; ii++)
{
d_print_mat(1, nbb[ii], hlam[ii], 1);
d_print_mat(1, nbb[ii], hlam[ii]+pnbb[ii], 1);
}
printf("\nt\n");
for(ii=0; ii<=N; ii++)
{
d_print_mat(1, nbb[ii], ht[ii], 1);
d_print_mat(1, nbb[ii], ht[ii]+pnbb[ii], 1);
}
printf("\nstatistics\n\n");
for(ii=0; ii<kk; ii++)
printf("%d\t%f\t%f\t%f\t%e\t%f\t%f\t%e\n", ii+1, stat[5*ii+0], stat[5*ii+1], stat[5*ii+2], stat[5*ii+2], stat[5*ii+3], stat[5*ii+4], stat[5*ii+4]);
printf("\n\n");
printf("\nresiduals ...\n\n");
d_res_ip_hard_mpc_tv(N, nxx, nuu, nbb, idxb, ngg, hpBAbt_tv, hpRSQ_tv, hrq, hux, dummy, hd, hpi, hlam, ht, hres_rq, hres_b, hres_d, &mu);
printf("\nresiduals dones\n\n");
printf("\nres_rq\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nuu[ii]+nxx[ii], hres_rq[ii], 1);
printf("\nres_b\n");
for(ii=0; ii<N; ii++)
d_print_mat(1, nxx[ii+1], hres_b[ii], 1);
printf("\nres_d\n");
for(ii=0; ii<=N; ii++)
{
d_print_mat(1, nbb[ii], hres_d[ii], 1);
d_print_mat(1, nbb[ii], hres_d[ii]+pnbb[ii], 1);
}
printf("\nres_mu\n");
d_print_mat(1, 1, &mu, 1);
// timing
printf("\ntiming ...\n\n");
gettimeofday(&tv20, NULL); // start
nrep = 1000;
for(ii=0; ii<nrep; ii++)
{
d_ip2_hard_mpc_tv(&kk, kmax, mu0, mu_tol, alpha_min, 0, sigma_par, stat, N, nxx, nuu, nbb, idxb, ngg, hpBAbt_tv, hpRSQ_tv, dummy, hd, hux, 1, hpi, hlam, ht, work_ipm_tv_double, work_ipm_tv_int);
}
gettimeofday(&tv21, NULL); // start
printf("\ntiming done\n\n");
time_ip_full_tv = (float) (tv21.tv_sec-tv20.tv_sec)/(nrep+0.0)+(tv21.tv_usec-tv20.tv_usec)/(nrep*1e6);
free(work_ric_tv);
free(work_ipm_tv_double);
free(work_ipm_tv_int);
for(ii=0; ii<N; ii++)
{
free(hpBAbt_tv[ii]);
free(hpRSQ_tv[ii]);
free(hpL_tv[ii]);
free(hl[ii]);
}
free(hpRSQ_tv[N]);
free(hpL_tv[N]);
free(hl[N]);
//exit(1);
#endif
// free memory
for(ii=0; ii<=N; ii++)
{
free(idxb[ii]);
free(hd[ii]);
free(hlam[ii]);
free(ht[ii]);
}
free(work_space_ip);
#endif
for(ii=0; ii<N; ii++)
{
free(hdA[ii]);
free(hpBt[ii]);
free(hpR[ii]);
free(hpS[ii]);
free(hpQ[ii]);
free(hpLK[ii]);
free(hpP[ii]);
free(hrq[ii]);
free(hux[ii]);
free(hpi[ii]);
free(hPb[ii]);
free(hb[ii]);
free(hres_rq[ii]);
free(hres_b[ii]);
}
free(hpQ[N]);
free(hpP[N]);
free(pK);
free(hrq[N]);
free(hux[N]);
free(hpi[N]);
free(work_diag);
free(hres_rq[N]);
/************************************************
* test of normal riccati & IPM
************************************************/
printf("\nRiccati full\n\n");
nx = 25;
nu = 1;
N = 11;
int rep;
int nz = nx+nu+1;
int anz = nal*((nz+nal-1)/nal);
int anx = nal*((nx+nal-1)/nal);
int pnz = bs*((nz+bs-1)/bs);
int pnx = bs*((nx+bs-1)/bs);
int pnu = bs*((nu+bs-1)/bs);
int cnz = ncl*((nx+nu+1+ncl-1)/ncl);
int cnx = ncl*((nx+ncl-1)/ncl);
int cnu = ncl*((nu+ncl-1)/ncl);
int cnl = cnz<cnx+ncl ? cnx+ncl : cnz;
const int ncx = nx;
#if 1
double *BAb_temp; d_zeros(&BAb_temp, nx, nu+nx+1);
double *hpBAbt2[N];
ptrB = BBB;
for(ii=0; ii<N; ii++)
{
//printf("\n%d\n", ii);
d_zeros_align(&hpBAbt2[ii], pnz, cnx);
for(jj=0; jj<nx*(nx+nu+1); jj++) BAb_temp[jj] = 0.0;
for(jj=0; jj<nu; jj++) BAb_temp[jj*(nx+1)] = 1.0;
d_copy_mat(nxx[ii+1]-1, nuu[ii], ptrB, nxx[ii+1]-1, BAb_temp+1, nx);
ptrB += nxx[ii+1]-1;
for(jj=0; jj<nxx[ii+1]; jj++) BAb_temp[nuu[ii]*nx+jj*(nx+1)] = 1.0;
//for(jj=0; jj<nxx[ii+1]; jj++) BAb_temp[(nuu[ii]+nxx[ii+1])*nx+jj] = 1.0;
//d_print_mat(nx, nu+nx+1, BAb_temp, nx);
d_cvt_tran_mat2pmat(nx, nx+nu+1, BAb_temp, nx, 0, hpBAbt2[ii], cnx);
//d_print_pmat(nx+nu+1, nx, bs, hpBAbt2[ii], cnx);
}
double *RSQ; d_zeros(&RSQ, nz, nz);
double *hpRSQ[N+1];
for(ii=0; ii<=N; ii++)
{
//printf("\n%d\n", ii);
d_zeros_align(&hpRSQ[ii], pnz, cnz);
for(jj=0; jj<nz*nz; jj++) RSQ[jj] = 0.0;
for(jj=nu; jj<2*nu; jj++) RSQ[jj*(nz+1)] = 1.0;
for(jj=nu+nxx[ii]-nx0; jj<nu+nxx[ii]; jj++) RSQ[jj*(nz+1)] = 1.0;
d_cvt_mat2pmat(nz, nz, RSQ, nz, 0, hpRSQ[ii], cnz);
//d_print_pmat(nz, nz, bs, hpRSQ[ii], cnz);
}
double *hpL[N+1];
double *hq2[N+1];
double *hux2[N+1];
double *hpi2[N+1];
double *hPb2[N];
for(jj=0; jj<N; jj++)
{
d_zeros_align(&hq2[jj], pnz, 1); // it has to be pnz !!!
d_zeros_align(&hpL[jj], pnz, cnl);
d_zeros_align(&hux2[jj], pnz, 1); // it has to be pnz !!!
d_zeros_align(&hpi2[jj], pnx, 1);
d_zeros_align(&hPb2[jj], pnx, 1);
}
d_zeros_align(&hpL[N], pnz, cnl);
d_zeros_align(&hq2[N], pnz, 1); // it has to be pnz !!!
d_zeros_align(&hux2[N], pnz, 1); // it has to be pnz !!!
d_zeros_align(&hpi2[N], pnx, 1);
//double *work; d_zeros_align(&work, 2*anz, 1);
double *work; d_zeros_align(&work, pnz, cnx);
for(jj=0; jj<nx+nu; jj++) hux2[0][jj] = 0.0;
for(jj=0; jj<nu; jj++)
{
hux2[0][nu+jj] = 7.5097;
}
for(; jj<nx; jj+=2)
{
hux2[0][nu+jj+0] = 15.01940;
hux2[0][nu+jj+1] = 0.0;
}
printf("\nfactorization and backward-forward solution ...\n");
d_ric_sv_mpc(nx, nu, N, hpBAbt2, hpRSQ, 0, dummy, dummy, hux2, hpL, work, diag, COMPUTE_MULT, hpi2, 0, 0, 0, dummy, dummy, dummy, 0);
printf("\nfactorization and backward-forward solution done\n\n");
//for(ii=0; ii<=N; ii++)
// d_print_pmat(pnz, cnl-3, bs, hpL[ii], cnl);
//d_print_pmat(pnz, nu, bs, hpL[0], cnl);
//d_print_pmat(pnz, cnl-3, bs, hpL[1], cnl);
//d_print_pmat(pnz, cnl-3, bs, hpL[2], cnl);
//d_print_pmat(pnz, cnl-3, bs, hpL[N-3], cnl);
//d_print_pmat(pnz, cnl-3, bs, hpL[N-2], cnl);
//d_print_pmat(pnz, cnl-3, bs, hpL[N-1], cnl);
//d_print_pmat(pnz, cnl, bs, hpL[N], cnl);
#if 1
printf("\nux Riccati full\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nx+nu, hux2[ii], 1);
#endif
// residuals
double *hres_rq2[N+1];
double *hres_b2[N];
for(ii=0; ii<N; ii++)
{
d_zeros_align(&hres_rq2[ii], pnz, 1);
d_zeros_align(&hres_b2[ii], pnx, 1);
}
d_zeros_align(&hres_rq2[N], pnz, 1);
printf("\nresuduals ...\n");
d_res_mpc(nx, nu, N, hpBAbt2, hpRSQ, hq2, hux2, hpi2, hres_rq2, hres_b2);
printf("\nresiduals done\n\n");
printf("\nres_q full\n");
d_print_mat(1, nu, hres_rq2[ii], 1);
for(ii=0; ii<N; ii++)
d_print_mat(1, nx+nu, hres_rq2[ii], 1);
printf("\nres_b full\n");
for(ii=0; ii<N; ii++)
d_print_mat(1, nx, hres_b2[ii], 1);
// timing
//struct timeval tv20, tv21;
#if 1
printf("\ntiming ...\n\n");
gettimeofday(&tv20, NULL); // start
nrep = 10000;
for(ii=0; ii<nrep; ii++)
{
d_ric_sv_mpc(nx, nu, N, hpBAbt2, hpRSQ, 0, dummy, dummy, hux2, hpL, work, diag, COMPUTE_MULT, hpi2, 0, 0, 0, dummy, dummy, dummy, 0);
}
gettimeofday(&tv21, NULL); // start
time_ric_full = (float) (tv21.tv_sec-tv20.tv_sec)/(nrep+0.0)+(tv21.tv_usec-tv20.tv_usec)/(nrep*1e6);
printf("\ntiming done\n\n");
#endif
printf("\nIPM full\n\n");
int nb = nu+nx;
int ng = 0;
int ngN = 0;
int pnb = (nb+bs-1)/bs*bs;
int png = (ng+bs-1)/bs*bs;
int pngN = (ngN+bs-1)/bs*bs;
double *hd2[N+1];
double *hlam2[N+1];
double *ht2[N+1];
for(ii=0; ii<N; ii++)
{
d_zeros_align(&hd2[ii], 2*pnb+2*png, 1);
d_zeros_align(&hlam2[ii],2*pnb+2*png, 1);
d_zeros_align(&ht2[ii], 2*pnb+2*png, 1);
}
d_zeros_align(&hd2[N], 2*pnb+2*pngN, 1);
d_zeros_align(&hlam2[N],2*pnb+2*pngN, 1);
d_zeros_align(&ht2[N], 2*pnb+2*pngN, 1);
// work space // more than enought !!!!!
double *work_ipm_full; d_zeros_align(&work_ipm_full, hpmpc_ip_hard_mpc_dp_work_space(N, nx, nu, nb, ng, ngN), 1);
// bounds
for(ii=0; ii<=N; ii++)
{
for(jj=0; jj<nu; jj++)
{
hd2[ii][jj] = -20.5;
hd2[ii][pnb+jj] = -20.5;
}
for(; jj<2*nu; jj++)
{
hd2[ii][jj] = - 2.5;
hd2[ii][pnb+jj] = -10.0;
}
for(; jj<2*nu+(N-ii)*nx0; jj++)
{
hd2[ii][jj] = -100.0;
hd2[ii][pnb+jj] = -100.0;
}
hd2[ii][jj+0] = 0.0;
hd2[ii][pnb+jj+0] = -20.0;
hd2[ii][jj+1] = -10.0;
hd2[ii][pnb+jj+1] = -10.0;
jj += 2;
for(; jj<nu+nx; jj++)
{
hd2[ii][jj] = -100.0;
hd2[ii][pnb+jj] = -100.0;
}
//d_print_mat(1, nb, hd2[ii], 1);
//d_print_mat(1, nb, hd2[ii]+pnb, 1);
}
//exit(1);
printf("\nIPM full solve ...\n\n");
d_ip2_hard_mpc(&kk, kmax, mu0, mu_tol, alpha_min, 0, sigma_par, stat, nx, nu, N, nb, ng, ngN, hpBAbt2, hpRSQ, dummy, hd2, hux2, 1, hpi2, hlam2, ht2, work_ipm_full);
printf("\nIPM full solve done\n\n");
#if 1
printf("\nux IPM full\n");
for(ii=0; ii<=N; ii++)
d_print_mat(1, nx+nu, hux2[ii], 1);
#endif
printf("\nstatistics\n\n");
for(ii=0; ii<kk; ii++)
printf("%d\t%f\t%f\t%f\t%e\t%f\t%f\t%e\n", ii+1, stat[5*ii+0], stat[5*ii+1], stat[5*ii+2], stat[5*ii+2], stat[5*ii+3], stat[5*ii+4], stat[5*ii+4]);
printf("\n\n");
// timing
printf("\ntiming ...\n\n");
gettimeofday(&tv20, NULL); // start
nrep = 1000;
for(ii=0; ii<nrep; ii++)
{
d_ip2_hard_mpc(&kk, kmax, mu0, mu_tol, alpha_min, 0, sigma_par, stat, nx, nu, N, nb, ng, ngN, hpBAbt2, hpRSQ, dummy, hd2, hux2, 1, hpi2, hlam2, ht2, work_ipm_full);
}
gettimeofday(&tv21, NULL); // start
printf("\ntiming done\n\n");
time_ip_full = (float) (tv21.tv_sec-tv20.tv_sec)/(nrep+0.0)+(tv21.tv_usec-tv20.tv_usec)/(nrep*1e6);
// free memory
free(work_ipm_full);
for(ii=0; ii<N; ii++)
{
free(hd2[ii]);
free(hlam2[ii]);
free(ht2[ii]);
}
free(hd2[N]);
free(hlam2[N]);
free(ht2[N]);
// free memory
free(work);
free(RSQ);
free(BAb_temp);
for(ii=0; ii<N; ii++)
{
free(hpBAbt2[ii]);
free(hpRSQ[ii]);
free(hpL[ii]);
free(hux2[ii]);
free(hpi2[ii]);
free(hq2[ii]);
free(hPb2[ii]);
free(hres_rq2[ii]);
free(hres_b2[ii]);
}
free(hpRSQ[N]);
free(hpL[N]);
free(hux2[N]);
free(hpi2[N]);
free(hq2[N]);
free(hres_rq2[N]);
#endif
printf("\nric diag time = %e\t\tric full time = %e\t\tric full tv time = %e\t\tip diag time = %e\t\tip full time = %e\t\tip full tv time = %e\n\n", time_ric_diag, time_ric_full, time_ric_full_tv, time_ip_diag, time_ip_full, time_ip_full_tv);
#endif
}
/* 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
