int bigint_free(bigint_t *a) {
if(a->allocated_W) {
int_free(a->wordv);
}
memset(a, 0, sizeof(bigint_t));
return 0;
}
int main() {
printf("\n");
printf("\n");
printf("\n");
printf(
" HPMPC -- Library for High-Performance implementation of solvers for "
"MPC.\n");
printf(
" Copyright (C) 2014-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");
#if defined(TARGET_X64_INTEL_HASWELL) || \
defined(TARGET_X64_INTEL_SABDY_BRIDGE) || \
defined(TARGET_X64_INTEL_CORE) || defined(TARGET_X86_AMD_BULLDOZER)
_MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); // flush to zero subnormals !!!
// works only with one thread
// !!!
#endif
int ii, jj;
int rep, nrep = NREP;
int nx = 8; // number of states (it has to be even for the mass-spring
// system test problem)
int nu = 3; // number of inputs (controllers) (it has to be at least 1 and
// at most nx/2 for the mass-spring system test problem)
int N = 15; // horizon length
int nb = 11; // number of box constrained inputs and states
int ng = 0; // 4; // number of general constraints
int ngN = 4; // 4; // number of general constraints at the last stage
// int N2 = 3; // horizon length of partially condensed problem
int nbu = nu < nb ? nu : nb;
int nbx = nb - nu > 0 ? nb - nu : 0;
// stage-wise variant size
int nxx[N + 1];
#if defined(ELIMINATE_X0)
nxx[0] = 0;
#else
nxx[0] = nx;
#endif
for (ii = 1; ii <= N; ii++) nxx[ii] = nx;
int nuu[N + 1];
for (ii = 0; ii < N; ii++) nuu[ii] = nu;
nuu[N] = 0;
int nbb[N + 1];
#if defined(ELIMINATE_X0)
nbb[0] = nbu;
#else
nbb[0] = nb;
#endif
for (ii = 1; ii < N; ii++) nbb[ii] = nb;
nbb[N] = nbx;
int ngg[N + 1];
for (ii = 0; ii < N; ii++) ngg[ii] = ng;
ngg[N] = ngN;
printf(
" Test problem: mass-spring system with %d masses and %d controls.\n",
nx / 2, nu);
printf("\n");
printf(
" MPC problem size: %d states, %d inputs, %d horizon length, %d "
"two-sided box constraints, %d two-sided general constraints.\n",
nx, nu, N, nb, ng);
printf("\n");
printf(
" IP method parameters: predictor-corrector IP, double precision, %d "
"maximum iterations, %5.1e exit tolerance in duality measure.\n",
MAXITER, TOL);
printf("\n");
#if defined(TARGET_X64_AVX2)
printf(" HPMPC built for the AVX2 architecture\n");
#endif
#if defined(TARGET_X64_AVX)
printf(" HPMPC built for the AVX architecture\n");
#endif
printf("\n");
/************************************************
* dynamical system
************************************************/
// state space matrices & initial state
double *A;
d_zeros(&A, nx, nx); // states update matrix
double *B;
d_zeros(&B, nx, nu); // inputs matrix
double *b;
d_zeros(&b, nx, 1); // states offset
double *x0;
d_zeros(&x0, nx, 1); // initial state
// mass-spring system
double Ts = 0.5; // sampling time
mass_spring_system(Ts, nx, nu, A, B, b, x0);
for (jj = 0; jj < nx; jj++) b[jj] = 0.1;
for (jj = 0; jj < nx; jj++) x0[jj] = 0;
x0[0] = 2.5;
x0[1] = 2.5;
// d_print_mat(nx, nx, A, nx);
// d_print_mat(nx, nu, B, nx);
// d_print_mat(nx, 1, b, nx);
// d_print_mat(nx, 1, x0, nx);
#if defined(ELIMINATE_X0)
// compute b0 = b + A*x0
double *b0;
d_zeros(&b0, nx, 1);
dcopy_3l(nx, b, 1, b0, 1);
dgemv_n_3l(nx, nx, A, nx, x0, b0);
// d_print_mat(nx, 1, b, nx);
// d_print_mat(nx, 1, b0, nx);
// then A0 is a matrix of size 0x0
double *A0;
d_zeros(&A0, 0, 0);
#endif
/************************************************
* box constraints
************************************************/
int jj_end;
int *idxb0;
int_zeros(&idxb0, nbb[0], 1);
double *lb0;
d_zeros(&lb0, nbb[0], 1);
double *ub0;
d_zeros(&ub0, nbb[0], 1);
#if defined(ELIMINATE_X0)
for (jj = 0; jj < nbb[0]; jj++) {
lb0[jj] = -0.5; // umin
ub0[jj] = +0.5; // umin
idxb0[jj] = jj;
}
#else
jj_end = nbx < nbb[0] ? nbx : nbb[0];
for (jj = 0; jj < jj_end; jj++) {
// lb0[jj] = x0[jj - nbu]; // initial state
// ub0[jj] = x0[jj - nbu]; // initial state
lb0[jj] = x0[jj]; // initial state
ub0[jj] = x0[jj]; // initial state
idxb0[jj] = jj;
}
for (; jj < nbb[0]; jj++) {
lb0[jj] = -0.5; // umin
ub0[jj] = +0.5; // umax
idxb0[jj] = jj;
}
#endif
// int_print_mat(nbb[0], 1, idxb0, nbb[0]);
// d_print_mat(nbb[0], 1, lb0, nbb[0]);
int *idxb1;
int_zeros(&idxb1, nbb[1], 1);
double *lb1;
d_zeros(&lb1, nbb[1], 1);
double *ub1;
d_zeros(&ub1, nbb[1], 1);
jj_end = nbx < nbb[1] ? nbx : nbb[1];
for (jj = 0; jj < jj_end; jj++) {
lb1[jj] = -4.0; // xmin
ub1[jj] = +4.0; // xmax
idxb1[jj] = jj;
}
for (; jj < nbb[1]; jj++) {
lb1[jj] = -0.5; // umin
ub1[jj] = +0.5; // umax
idxb1[jj] = jj;
}
// int_print_mat(nbb[1], 1, idxb1, nbb[1]);
// d_print_mat(nbb[1], 1, lb1, nbb[1]);
int *idxbN;
int_zeros(&idxbN, nbb[N], 1);
double *lbN;
d_zeros(&lbN, nbb[N], 1);
double *ubN;
d_zeros(&ubN, nbb[N], 1);
jj_end = nbx < nbb[N] ? nbx : nbb[N];
for (jj = 0; jj < jj_end; jj++) {
lbN[jj] = -4.0; // xmin
ubN[jj] = +4.0; // xmax
idxbN[jj] = jj;
}
for (; jj < nbb[N]; jj++) {
lbN[jj] = -0.5; // umin
ubN[jj] = +0.5; // umax
idxbN[jj] = jj;
}
// int_print_mat(nbb[N], 1, idxbN, nbb[N]);
// d_print_mat(nbb[N], 1, lbN, nbb[N]);
/************************************************
* general constraints
************************************************/
double *C;
d_zeros(&C, ng, nx);
double *D;
d_zeros(&D, ng, nu);
double *lg;
d_zeros(&lg, ng, 1);
double *ug;
d_zeros(&ug, ng, 1);
double *CN;
d_zeros(&CN, ngN, nx);
for (ii = 0; ii < ngN; ii++) CN[ii * (ngN + 1)] = 1.0;
// d_print_mat(ngN, nx, CN, ngN);
double *lgN;
d_zeros(&lgN, ngN, 1); // force all states to 0 at the last stage
double *ugN;
d_zeros(&ugN, ngN, 1); // force all states to 0 at the last stage
/************************************************
* cost function
************************************************/
double *Q;
d_zeros(&Q, nx, nx);
for (ii = 0; ii < nx; ii++) Q[ii * (nx + 1)] = 1.0;
double *R;
d_zeros(&R, nu, nu);
for (ii = 0; ii < nu; ii++) R[ii * (nu + 1)] = 2.0;
double *S;
d_zeros(&S, nu, nx);
double *q;
d_zeros(&q, nx, 1);
for (ii = 0; ii < nx; ii++) q[ii] = 0.1;
double *r;
d_zeros(&r, nu, 1);
for (ii = 0; ii < nu; ii++) r[ii] = 0.2;
#if defined(ELIMINATE_X0)
// Q0 and q0 are matrices of size 0
double *Q0;
d_zeros(&Q0, 0, 0);
double *q0;
d_zeros(&q0, 0, 1);
// compute r0 = r + S*x0
double *r0;
d_zeros(&r0, nu, 1);
dcopy_3l(nu, r, 1, r0, 1);
dgemv_n_3l(nu, nx, S, nu, x0, r0);
// then S0 is a matrix of size nux0
double *S0;
d_zeros(&S0, nu, 0);
#endif
/************************************************
* problems data
************************************************/
double *hA[N];
double *hB[N];
double *hb[N];
double *hQ[N + 1];
double *hS[N];
double *hR[N];
double *hq[N + 1];
double *hr[N];
double *hlb[N + 1];
double *hub[N + 1];
int *hidxb[N + 1];
double *hC[N + 1];
double *hD[N];
double *hlg[N + 1];
double *hug[N + 1];
#if defined(ELIMINATE_X0)
hA[0] = A0;
hb[0] = b0;
hQ[0] = Q0;
hS[0] = S0;
hq[0] = q0;
hr[0] = r0;
#else
hA[0] = A;
hb[0] = b;
hQ[0] = Q;
hS[0] = S;
hq[0] = q;
hr[0] = r;
#endif
hB[0] = B;
hR[0] = R;
hlb[0] = lb0;
hub[0] = ub0;
hidxb[0] = idxb0;
hC[0] = C;
hD[0] = D;
hlg[0] = lg;
hug[0] = ug;
for (ii = 1; ii < N; ii++) {
hA[ii] = A;
hB[ii] = B;
hb[ii] = b;
hQ[ii] = Q;
hS[ii] = S;
hR[ii] = R;
hq[ii] = q;
hr[ii] = r;
hlb[ii] = lb1;
hub[ii] = ub1;
hidxb[ii] = idxb1;
hC[ii] = C;
hD[ii] = D;
hlg[ii] = lg;
hug[ii] = ug;
}
hQ[N] = Q; // or maybe initialize to the solution of the DARE???
hq[N] = q; // or maybe initialize to the solution of the DARE???
hlb[N] = lbN;
hub[N] = ubN;
hidxb[N] = idxbN;
hC[N] = CN;
hlg[N] = lgN;
hug[N] = ugN;
/************************************************
* solution
************************************************/
double *hx[N + 1];
double *hu[N];
double *hpi[N];
double *hlam[N + 1];
double *ht[N + 1];
for (ii = 0; ii < N; ii++) {
d_zeros(&hx[ii], nxx[ii], 1);
d_zeros(&hu[ii], nuu[ii], 1);
d_zeros(&hpi[ii], nxx[ii + 1], 1);
d_zeros(&hlam[ii], 2 * nbb[ii] + 2 * ngg[ii], 1);
d_zeros(&ht[ii], 2 * nbb[ii] + 2 * ngg[ii], 1);
}
d_zeros(&hx[N], nxx[N], 1);
d_zeros(&hlam[N], 2 * nbb[N] + 2 * ngg[N], 1);
d_zeros(&ht[N], 2 * nbb[N] + 2 * ngg[N], 1);
/************************************************
* create the in and out struct
************************************************/
ocp_qp_in qp_in;
qp_in.N = N;
qp_in.nx = (const int *)nxx;
qp_in.nu = (const int *)nuu;
qp_in.nb = (const int *)nbb;
qp_in.nc = (const int *)ngg;
qp_in.A = (const double **)hA;
qp_in.B = (const double **)hB;
qp_in.b = (const double **)hb;
qp_in.Q = (const double **)hQ;
qp_in.S = (const double **)hS;
qp_in.R = (const double **)hR;
qp_in.q = (const double **)hq;
qp_in.r = (const double **)hr;
qp_in.idxb = (const int **)hidxb;
qp_in.lb = (const double **)hlb;
qp_in.ub = (const double **)hub;
qp_in.Cx = (const double **)hC;
qp_in.Cu = (const double **)hD;
qp_in.lc = (const double **)hlg;
qp_in.uc = (const double **)hug;
ocp_qp_out qp_out;
qp_out.x = hx;
qp_out.u = hu;
qp_out.pi = hpi;
qp_out.lam = hlam;
qp_out.t = ht; // XXX why also the slack variables ???
/************************************************
* solver arguments (fully sparse)
************************************************/
// solver arguments
ocp_qp_condensing_hpipm_args *hpipm_args = ocp_qp_condensing_hpipm_create_arguments(&qp_in);
// hpipm_args->mu_max = TOL;
// hpipm_args->iter_max = MAXITER;
// hpipm_args->alpha_min = MINSTEP;
hpipm_args->mu0 = 1.0; // 0.0
/************************************************
* work space (fully sparse)
************************************************/
int work_space_size =
ocp_qp_condensing_hpipm_calculate_workspace_size(&qp_in, hpipm_args);
printf("\nwork space size: %d bytes\n", work_space_size);
void *workspace = malloc(work_space_size);
// void *mem;
// ocp_qp_hpipm_create_memory(&qp_in, hpipm_args, &mem);
int memory_size =
ocp_qp_condensing_hpipm_calculate_memory_size(&qp_in, hpipm_args);
printf("\nmemory: %d bytes\n", memory_size);
void *memory = malloc(memory_size);
ocp_qp_condensing_hpipm_memory *hpipm_memory =
ocp_qp_condensing_hpipm_create_memory(&qp_in, hpipm_args);
/************************************************
* call the solver (fully sparse)
************************************************/
int return_value;
acados_timer timer;
acados_tic(&timer);
// nrep = 1;
for (rep = 0; rep < nrep; rep++) {
// call the QP OCP solver
// return_value = ocp_qp_hpipm(&qp_in, &qp_out, hpipm_args,
// workspace);
return_value =
ocp_qp_condensing_hpipm(&qp_in, &qp_out, hpipm_args, hpipm_memory, workspace);
}
real_t time = acados_toc(&timer)/nrep;
if (return_value == ACADOS_SUCCESS)
printf("\nACADOS status: solution found in %d iterations\n",
hpipm_memory->iter);
if (return_value == ACADOS_MAXITER)
printf("\nACADOS status: maximum number of iterations reached\n");
if (return_value == ACADOS_MINSTEP)
printf("\nACADOS status: below minimum step size length\n");
printf("\nu = \n");
for (ii = 0; ii < N; ii++) d_print_mat(1, nuu[ii], hu[ii], 1);
printf("\nx = \n");
for (ii = 0; ii <= N; ii++) d_print_mat(1, nxx[ii], hx[ii], 1);
printf("\npi = \n");
for (ii = 0; ii < N; ii++) d_print_mat(1, nxx[ii+1], hpi[ii], 1);
printf("\nlam = \n");
for (ii = 0; ii <= N; ii++) d_print_mat(1, 2*nbb[ii]+2*ngg[ii], hlam[ii], 1);
printf("\n");
printf(" inf norm res: %e, %e, %e, %e, %e\n", hpipm_memory->inf_norm_res[0],
hpipm_memory->inf_norm_res[1], hpipm_memory->inf_norm_res[2],
hpipm_memory->inf_norm_res[3], hpipm_memory->inf_norm_res[4]);
printf("\n");
printf(
" Solution time for %d IPM iterations, averaged over %d runs: %5.2e "
"seconds\n", hpipm_memory->iter, nrep, time);
printf("\n\n");
/************************************************
* free memory
************************************************/
d_free(A);
d_free(B);
d_free(b);
d_free(x0);
d_free(Q);
d_free(S);
d_free(R);
d_free(q);
d_free(r);
#if defined(ELIMINATE_X0)
d_free(A0);
d_free(b0);
d_free(Q0);
d_free(S0);
d_free(q0);
d_free(r0);
#endif
int_free(idxb0);
d_free(lb0);
d_free(ub0);
int_free(idxb1);
d_free(lb1);
d_free(ub1);
int_free(idxbN);
d_free(lbN);
d_free(ubN);
d_free(C);
d_free(D);
d_free(lg);
d_free(ug);
d_free(CN);
d_free(lgN);
d_free(ugN);
for (ii = 0; ii < N; ii++) {
d_free(hx[ii]);
d_free(hu[ii]);
d_free(hpi[ii]);
d_free(hlam[ii]);
d_free(ht[ii]);
}
d_free(hx[N]);
d_free(hlam[N]);
d_free(ht[N]);
free(workspace);
free(memory);
return 0;
}
int main()
{
printf("\n");
printf("\n");
printf("\n");
printf(" HPMPC -- Library for High-Performance implementation of solvers for MPC.\n");
printf(" Copyright (C) 2014-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");
#if defined(TARGET_X64_AVX2) || defined(TARGET_X64_AVX) || defined(TARGET_X64_SSE3) || defined(TARGET_X86_ATOM) || defined(TARGET_AMD_SSE3)
_MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); // flush to zero subnormals !!! works only with one thread !!!
#endif
int ii, jj;
int rep, nrep=1000; //000;//NREP;
int nx_ = 8;//NX; // number of states (it has to be even for the mass-spring system test problem)
int nu_ = 3;//NU; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)
int N = 10;//NN; // horizon lenght
// int nb = nu+nx; // number of box constrained inputs and states
// int ng = nx; //4; // number of general constraints
// int ngN = nx; // number of general constraints at the last stage
printf("\nN = %d, nx = %d, nu = %d\n\n", N, nx_, nu_);
#define MHE 0
// int nbu = nu<nb ? nu : nb ;
// int nbx = nb-nu>0 ? nb-nu : 0;
// stage-wise variant size
int nx[N+1];
#if MHE==1
nx[0] = nx_;
#else
nx[0] = 0;
#endif
for(ii=1; ii<=N; ii++)
nx[ii] = nx_;
int nu[N+1];
for(ii=0; ii<N; ii++)
nu[ii] = nu_;
nu[N] = 0; // XXX
int nb[N+1];
nb[0] = nu[0] + nx[0]/2;
for(ii=1; ii<N; ii++)
nb[ii] = nu[1] + nx[ii]/2;
nb[N] = nu[N] + nx[N]/2;
int ng[N+1];
for(ii=0; ii<N; ii++)
ng[ii] = 0; //ng;
ng[N] = 0; //ngN;
// ng[M] = nx_; // XXX
/************************************************
* IPM common arguments
************************************************/
int hpmpc_status;
int kk = -1;
int k_max = 10;
double mu0 = 2.0;
double mu_tol = 1e-20;
double alpha_min = 1e-8;
int warm_start = 0; // read initial guess from x and u
double *stat; d_zeros(&stat, k_max, 5);
int compute_res = 1;
int compute_mult = 1;
/************************************************
* dynamical system
************************************************/
double *A; d_zeros(&A, nx_, nx_); // states update matrix
double *B; d_zeros(&B, nx_, nu_); // inputs matrix
double *b; d_zeros_align(&b, nx_, 1); // states offset
double *x0; d_zeros_align(&x0, nx_, 1); // initial state
double Ts = 0.5; // sampling time
mass_spring_system(Ts, nx_, nu_, N, A, B, b, x0);
for(jj=0; jj<nx_; jj++)
b[jj] = 0.1;
for(jj=0; jj<nx_; jj++)
x0[jj] = 0;
x0[0] = 2.5;
x0[1] = 2.5;
#if MHE!=1
struct blasfeo_dvec sx0;
blasfeo_allocate_dvec(nx_, &sx0);
blasfeo_pack_dvec(nx_, x0, &sx0, 0);
struct blasfeo_dvec sb;
blasfeo_allocate_dvec(nx_, &sb);
blasfeo_pack_dvec(nx_, b, &sb, 0);
struct blasfeo_dmat sA;
blasfeo_allocate_dmat(nx_, nx_, &sA);
blasfeo_pack_dmat(nx_, nx_, A, nx_, &sA, 0, 0);
struct blasfeo_dvec sb0;
blasfeo_allocate_dvec(nx_, &sb0);
blasfeo_dgemv_n(nx_, nx_, 1.0, &sA, 0, 0, &sx0, 0, 1.0, &sb, 0, &sb0, 0);
struct blasfeo_dmat sBAbt0;
blasfeo_allocate_dmat(nu[0]+1, nx[1], &sBAbt0);
blasfeo_pack_tran_dmat(nx_, nu_, B, nx_, &sBAbt0, 0, 0);
blasfeo_drowin(nx[1], 1.0, &sb0, 0, &sBAbt0, nu[0], 0);
// d_print_strmat(nu[0]+1, nx[1], &sBAbt0, 0, 0);
#endif
struct blasfeo_dmat sBAbt1;
if(N>1)
{
blasfeo_allocate_dmat(nu[1]+nx[1]+1, nx[2], &sBAbt1);
blasfeo_pack_tran_dmat(nx_, nu_, B, nx_, &sBAbt1, 0, 0);
blasfeo_pack_tran_dmat(nx_, nx_, A, nx_, &sBAbt1, nu[1], 0);
blasfeo_pack_tran_dmat(nx_, 1, b, nx_, &sBAbt1, nu[1]+nx[1], 0);
// d_print_strmat(nu[1]+nx[1]+1, nx[2], &sBAbt1, 0, 0);
}
/************************************************
* cost function
************************************************/
double *R; d_zeros(&R, nu_, nu_);
for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 2.0;
double *S; d_zeros(&S, nu_, nx_);
double *Q; d_zeros(&Q, nx_, nx_);
for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 1.0;
double *r; d_zeros(&r, nu_, 1);
for(ii=0; ii<nu_; ii++) r[ii] = 0.2;
double *q; d_zeros(&q, nx_, 1);
for(ii=0; ii<nx_; ii++) q[ii] = 0.1;
#if MHE!=1
struct blasfeo_dvec sr;
blasfeo_allocate_dvec(nu_, &sr);
blasfeo_pack_dvec(nu_, r, &sr, 0);
struct blasfeo_dmat sS;
blasfeo_allocate_dmat(nu_, nx_, &sS);
blasfeo_pack_dmat(nu_, nx_, S, nu_, &sS, 0, 0);
struct blasfeo_dvec sr0;
blasfeo_allocate_dvec(nu_, &sr0);
blasfeo_dgemv_n(nu_, nx_, 1.0, &sS, 0, 0, &sx0, 0, 1.0, &sr, 0, &sr0, 0);
struct blasfeo_dmat sRSQrq0;
blasfeo_allocate_dmat(nu[0]+nx[0]+1, nu[0]+nx[0], &sRSQrq0);
blasfeo_pack_dmat(nu_, nu_, R, nu_, &sRSQrq0, 0, 0);
blasfeo_drowin(nu[0], 1.0, &sr0, 0, &sRSQrq0, nu[0], 0);
// d_print_strmat(nu[0]+nx[0]+1, nu[0]+nx[0], &sRSQrq0, 0, 0);
struct blasfeo_dvec srq0;
blasfeo_allocate_dvec(nu[0]+nx[0], &srq0);
blasfeo_dveccp(nu[0], 1.0, &sr0, 0, &srq0, 0);
#endif
struct blasfeo_dmat sRSQrq1;
struct blasfeo_dvec srq1;
if(N>1)
{
blasfeo_allocate_dmat(nu[1]+nx[1]+1, nu[1]+nx[1], &sRSQrq1);
blasfeo_pack_dmat(nu_, nu_, R, nu_, &sRSQrq1, 0, 0);
blasfeo_pack_tran_dmat(nu_, nx_, S, nu_, &sRSQrq1, nu[1], 0);
blasfeo_pack_dmat(nx_, nx_, Q, nx_, &sRSQrq1, nu[1], nu[1]);
blasfeo_pack_tran_dmat(nu_, 1, r, nu_, &sRSQrq1, nu[1]+nx[1], 0);
blasfeo_pack_tran_dmat(nx_, 1, q, nx_, &sRSQrq1, nu[1]+nx[1], nu[1]);
// d_print_strmat(nu[1]+nx[1]+1, nu[1]+nx[1], &sRSQrq1, 0, 0);
blasfeo_allocate_dvec(nu[1]+nx[1], &srq1);
blasfeo_pack_dvec(nu_, r, &srq1, 0);
blasfeo_pack_dvec(nx_, q, &srq1, nu[1]);
}
struct blasfeo_dmat sRSQrqN;
blasfeo_allocate_dmat(nx[N]+1, nx[N], &sRSQrqN);
blasfeo_pack_dmat(nx_, nx_, Q, nx_, &sRSQrqN, 0, 0);
blasfeo_pack_tran_dmat(nx_, 1, q, nx_, &sRSQrqN, nx[1], 0);
// d_print_strmat(nu[N]+nx[N]+1, nu[N]+nx[N], &sRSQrqN, 0, 0);
struct blasfeo_dvec srqN;
blasfeo_allocate_dvec(nx[N], &srqN);
blasfeo_pack_dvec(nx_, q, &srqN, 0);
/************************************************
* constraints
************************************************/
#if MHE!=1
double *d0; d_zeros(&d0, 2*nb[0]+2*ng[0], 1);
int *idxb0; int_zeros(&idxb0, nb[0], 1);
// inputs
for(ii=0; ii<nu[0]; ii++)
{
d0[ii] = - 0.5; // u_min
d0[nb[0]+ng[0]+ii] = + 0.5; // u_max
idxb0[ii] = ii;
}
// states
for( ; ii<nb[0]; ii++)
{
d0[ii] = - 4.0; // x_min
d0[nb[0]+ng[0]+ii] = + 4.0; // x_max
idxb0[ii] = ii;
}
#endif
double *d1;
int *idxb1;
if(N>1)
{
d_zeros(&d1, 2*nb[1]+2*ng[1], 1);
int_zeros(&idxb1, nb[1], 1);
// inputs
for(ii=0; ii<nu[1]; ii++)
{
d1[ii] = - 0.5; // u_min
d1[nb[1]+ng[1]+ii] = + 0.5; // u_max
idxb1[ii] = ii;
}
// states
for( ; ii<nb[1]; ii++)
{
d1[ii] = - 4.0; // x_min
d1[nb[1]+ng[1]+ii] = + 4.0; // x_max
idxb1[ii] = ii;
}
}
double *dN; d_zeros(&dN, 2*nb[N]+2*ng[N], 1);
int *idxbN; int_zeros(&idxbN, nb[N], 1);
// no inputs
// states
for(ii=0 ; ii<nb[N]; ii++)
{
dN[ii] = - 4.0; // x_min
dN[nb[N]+ng[N]+ii] = + 4.0; // x_max
idxbN[ii] = ii;
}
struct blasfeo_dvec sd0;
blasfeo_allocate_dvec(2*nb[0]+2*ng[0], &sd0);
blasfeo_pack_dvec(2*nb[0]+2*ng[0], d0, &sd0, 0);
// blasfeo_print_tran_dvec(2*nb[0], &sd0, 0);
struct blasfeo_dvec sd1;
blasfeo_allocate_dvec(2*nb[1]+2*ng[1], &sd1);
blasfeo_pack_dvec(2*nb[1]+2*ng[1], d1, &sd1, 0);
// blasfeo_print_tran_dvec(2*nb[1], &sd1, 0);
struct blasfeo_dvec sdN;
blasfeo_allocate_dvec(2*nb[N]+2*ng[N], &sdN);
blasfeo_pack_dvec(2*nb[N]+2*ng[N], dN, &sdN, 0);
// blasfeo_print_tran_dvec(2*nb[N], &sdN, 0);
/************************************************
* array of data matrices
************************************************/
// original MPC
struct blasfeo_dmat hsBAbt[N];
struct blasfeo_dvec hsb[N];
struct blasfeo_dmat hsRSQrq[N+1];
struct blasfeo_dvec hsrq[N+1];
struct blasfeo_dmat hsDCt[N+1]; // XXX
struct blasfeo_dvec hsd[N+1];
int *hidxb[N+1];
ii = 0;
#if MHE!=1
hsBAbt[ii] = sBAbt0;
hsb[ii] = sb0;
hsRSQrq[ii] = sRSQrq0;
hsrq[ii] = srq0;
hsd[ii] = sd0;
hidxb[0] = idxb0;
#else
hsBAbt[ii] = sBAbt1;
hsb[ii] = sb;
hsRSQrq[ii] = sRSQrq1;
hsrq[ii] = srq1;
hsd[ii] = sd1;
hidxb[0] = idxb1;
#endif
for(ii=1; ii<N; ii++)
{
hsBAbt[ii] = sBAbt1;
hsb[ii] = sb;
hsRSQrq[ii] = sRSQrq1;
hsrq[ii] = srq1;
hsd[ii] = sd1;
hidxb[ii] = idxb1;
}
hsRSQrq[ii] = sRSQrqN;
hsrq[ii] = srqN;
hsd[ii] = sdN;
hidxb[N] = idxbN;
/************************************************
* solve full spase system using Riccati / IPM
************************************************/
// result vectors
struct blasfeo_dvec hsux[N+1];
struct blasfeo_dvec hspi[N+1];
struct blasfeo_dvec hslam[N+1];
struct blasfeo_dvec hst[N+1];
for(ii=0; ii<=N; ii++)
{
blasfeo_allocate_dvec(nu[ii]+nx[ii], &hsux[ii]);
blasfeo_allocate_dvec(nx[ii], &hspi[ii]);
blasfeo_allocate_dvec(2*nb[ii]+2*ng[ii], &hslam[ii]);
blasfeo_allocate_dvec(2*nb[ii]+2*ng[ii], &hst[ii]);
}
// work space
void *work_space_ipm;
v_zeros_align(&work_space_ipm, d_ip2_res_mpc_hard_work_space_size_bytes_libstr(N, nx, nu, nb, ng));
struct timeval tv0, tv1;
printf("\nsolving... (full space system)\n");
gettimeofday(&tv0, NULL); // stop
for(rep=0; rep<nrep; rep++)
{
hpmpc_status = d_ip2_res_mpc_hard_libstr(&kk, k_max, mu0, mu_tol, alpha_min, warm_start, stat, N, nx, nu, nb, hidxb, ng, hsBAbt, hsRSQrq, hsDCt, hsd, hsux, 1, hspi, hslam, hst, work_space_ipm);
}
gettimeofday(&tv1, NULL); // stop
printf("\n... done\n");
float time_ipm_full = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
printf("\nstatistics from last run\n\n");
for(jj=0; jj<kk; jj++)
printf("k = %d\tsigma = %f\talpha = %f\tmu = %f\t\tmu = %e\talpha = %f\tmu = %f\tmu = %e\n", jj, stat[5*jj], stat[5*jj+1], stat[5*jj+2], stat[5*jj+2], stat[5*jj+3], stat[5*jj+4], stat[5*jj+4]);
printf("\n");
printf("\nux =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(nu[ii]+nx[ii], &hsux[ii], 0);
printf("\npi =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(nx[ii], &hspi[ii], 0);
printf("\nlam =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii], &hslam[ii], 0);
printf("\nt =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii], &hst[ii], 0);
// residuals vectors
struct blasfeo_dvec hsrrq[N+1];
struct blasfeo_dvec hsrb[N+1];
struct blasfeo_dvec hsrd[N+1];
struct blasfeo_dvec hsrm[N+1];
double mu;
for(ii=0; ii<N; ii++)
{
blasfeo_allocate_dvec(nu[ii]+nx[ii], &hsrrq[ii]);
blasfeo_allocate_dvec(nx[ii+1], &hsrb[ii]);
blasfeo_allocate_dvec(2*nb[ii]+2*ng[ii], &hsrd[ii]);
blasfeo_allocate_dvec(2*nb[ii]+2*ng[ii], &hsrm[ii]);
}
blasfeo_allocate_dvec(nu[N]+nx[N], &hsrrq[N]);
blasfeo_allocate_dvec(2*nb[N]+2*ng[N], &hsrd[N]);
blasfeo_allocate_dvec(2*nb[N]+2*ng[N], &hsrm[N]);
int ngM = ng[0];
for(ii=1; ii<=N; ii++)
{
ngM = ng[ii]>ngM ? ng[ii] : ngM;
}
void *work_space_res;
v_zeros_align(&work_space_res, d_res_res_mpc_hard_work_space_size_bytes_libstr(N, nx, nu, nb, ng));
d_res_res_mpc_hard_libstr(N, nx, nu, nb, hidxb, ng, hsBAbt, hsb, hsRSQrq, hsrq, hsux, hsDCt, hsd, hspi, hslam, hst, hsrrq, hsrb, hsrd, hsrm, &mu, work_space_res);
printf("\nres_rq\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], &hsrrq[ii], 0);
printf("\nres_b\n");
for(ii=0; ii<N; ii++)
blasfeo_print_exp_tran_dvec(nx[ii+1], &hsrb[ii], 0);
printf("\nres_d\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], &hsrd[ii], 0);
printf("\nres_m\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], &hsrm[ii], 0);
/************************************************
* full condensing
************************************************/
// condensed problem size
int N2 = 1;
int nx2[N2+1];
int nu2[N2+1];
int nb2[N2+1];
int ng2[N2+1];
d_cond_compute_problem_size_libstr(N, nx, nu, nb, hidxb, ng, nx2, nu2, nb2, ng2);
#if 0
for(ii=0; ii<=N2; ii++)
printf("\n%d %d %d %d\n", nx2[ii], nu2[ii], nb2[ii], ng2[ii]);
#endif
int work_sizes_cond[5];
int work_size_cond = d_cond_work_space_size_bytes_libstr(N, nx, nu, nb, hidxb, ng, nx2, nu2, nb2, ng2, work_sizes_cond);
int memo_size_cond = d_cond_memory_space_size_bytes_libstr(N, nx, nu, nb, hidxb, ng, nx, nu2, nb2, ng2);
int work_size_ipm_cond = d_ip2_res_mpc_hard_work_space_size_bytes_libstr(N2, nx2, nu2, nb2, ng2);
int work_sizes_expa[2];
int work_size_expa = d_expand_work_space_size_bytes_libstr(N, nx, nu, nb, ng, work_sizes_expa);
// work space
void *work_cond;
void *memo_cond;
void *work_ipm_cond;
void *work_expa;
v_zeros_align(&work_cond, work_size_cond);
v_zeros_align(&memo_cond, memo_size_cond);
v_zeros_align(&work_ipm_cond, work_size_ipm_cond);
v_zeros_align(&work_expa, work_size_expa);
// data matrices
struct blasfeo_dmat hsBAbt2[N2];
struct blasfeo_dvec hsb2[N2];
struct blasfeo_dmat hsRSQrq2[N2+1];
struct blasfeo_dvec hsrq2[N2+1];
struct blasfeo_dmat hsDCt2[N2+1];
struct blasfeo_dvec hsd2[N2+1];
int *hidxb2[N2+1];
for(ii=0; ii<N2; ii++)
blasfeo_allocate_dmat(nu2[ii]+nx2[ii]+1, nx2[ii+1], &hsBAbt2[ii]);
for(ii=0; ii<N2; ii++)
blasfeo_allocate_dvec(nx2[ii+1], &hsb2[ii]);
for(ii=0; ii<=N2; ii++)
blasfeo_allocate_dmat(nu2[ii]+nx2[ii]+1, nu2[ii]+nx2[ii], &hsRSQrq2[ii]);
for(ii=0; ii<=N2; ii++)
blasfeo_allocate_dvec(nu2[ii]+nx2[ii], &hsrq2[ii]);
for(ii=0; ii<=N2; ii++)
blasfeo_allocate_dmat(nu2[ii]+nx2[ii]+1, ng2[ii], &hsDCt2[ii]);
for(ii=0; ii<=N2; ii++)
blasfeo_allocate_dvec(2*nb2[ii]+2*ng2[ii], &hsd2[ii]);
for(ii=0; ii<=N2; ii++)
int_zeros(&hidxb2[ii], nb2[ii], 1);
// result vectors
struct blasfeo_dvec hsux2[N2+1];
struct blasfeo_dvec hspi2[N2+1];
struct blasfeo_dvec hslam2[N2+1];
struct blasfeo_dvec hst2[N2+1];
for(ii=0; ii<=N2; ii++)
{
blasfeo_allocate_dvec(nu2[ii]+nx2[ii], &hsux2[ii]);
blasfeo_allocate_dvec(nx2[ii], &hspi2[ii]);
blasfeo_allocate_dvec(2*nb2[ii]+2*ng2[ii], &hslam2[ii]);
blasfeo_allocate_dvec(2*nb2[ii]+2*ng2[ii], &hst2[ii]);
}
d_cond_libstr(N, nx, nu, nb, hidxb, ng, hsBAbt, hsRSQrq, hsDCt, hsd, nx2, nu2, nb2, hidxb2, ng2, hsBAbt2, hsRSQrq2, hsDCt2, hsd2, memo_cond, work_cond, work_sizes_cond);
#if 0
printf("\nBAbt2\n");
for(ii=0; ii<N2; ii++)
d_print_strmat(nu2[ii]+nx2[ii]+1, nx2[ii+1], &hsBAbt2[ii], 0, 0);
printf("\nRSQrq2\n");
for(ii=0; ii<=N2; ii++)
d_print_strmat(nu2[ii]+nx2[ii]+1, nu2[ii]+nx2[ii], &hsRSQrq2[ii], 0, 0);
printf("\nDCt2\n");
for(ii=0; ii<=N2; ii++)
d_print_strmat(nu2[ii]+nx2[ii], ng2[ii], &hsDCt2[ii], 0, 0);
printf("\nd2\n");
for(ii=0; ii<=N2; ii++)
blasfeo_print_tran_dvec(2*nb2[ii]+2*ng2[ii], &hsd2[ii], 0);
#endif
/************************************************
* solve condensed system using IPM
************************************************/
// zero solution
for(ii=0; ii<=N; ii++)
blasfeo_dvecse(nu[ii]+nx[ii], 0.0, &hsux[ii], 0);
for(ii=0; ii<=N; ii++)
blasfeo_dvecse(nx[ii], 0.0, &hspi[ii], 0);
for(ii=0; ii<=N; ii++)
blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, &hslam[ii], 0);
for(ii=0; ii<=N; ii++)
blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, &hst[ii], 0);
printf("\nsolving... (condensed system)\n");
gettimeofday(&tv0, NULL); // stop
for(rep=0; rep<nrep; rep++)
{
d_cond_libstr(N, nx, nu, nb, hidxb, ng, hsBAbt, hsRSQrq, hsDCt, hsd, nx2, nu2, nb2, hidxb2, ng2, hsBAbt2, hsRSQrq2, hsDCt2, hsd2, memo_cond, work_cond, work_sizes_cond);
hpmpc_status = d_ip2_res_mpc_hard_libstr(&kk, k_max, mu0, mu_tol, alpha_min, warm_start, stat, N2, nx2, nu2, nb2, hidxb2, ng2, hsBAbt2, hsRSQrq2, hsDCt2, hsd2, hsux2, 1, hspi2, hslam2, hst2, work_ipm_cond);
d_expand_solution_libstr(N, nx, nu, nb, hidxb, ng, hsBAbt, hsb, hsRSQrq, hsrq, hsDCt, hsux, hspi, hslam, hst, nx2, nu2, nb2, hidxb2, ng2, hsux2, hspi2, hslam2, hst2, work_expa, work_sizes_expa);
}
gettimeofday(&tv1, NULL); // stop
printf("\n... done\n");
float time_ipm_cond = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
printf("\nstatistics from last run\n\n");
for(jj=0; jj<kk; jj++)
printf("k = %d\tsigma = %f\talpha = %f\tmu = %f\t\tmu = %e\talpha = %f\tmu = %f\tmu = %e\n", jj, stat[5*jj], stat[5*jj+1], stat[5*jj+2], stat[5*jj+2], stat[5*jj+3], stat[5*jj+4], stat[5*jj+4]);
printf("\n");
#if 0
printf("\nux2 =\n\n");
for(ii=0; ii<=N2; ii++)
blasfeo_print_tran_dvec(nu2[ii]+nx2[ii], &hsux2[ii], 0);
printf("\npi2 =\n\n");
for(ii=0; ii<=N2; ii++)
blasfeo_print_tran_dvec(nx2[ii], &hspi2[ii], 0);
printf("\nlam2 =\n\n");
for(ii=0; ii<=N2; ii++)
blasfeo_print_tran_dvec(2*nb2[ii]+2*ng2[ii], &hslam2[ii], 0);
printf("\nt2 =\n\n");
for(ii=0; ii<=N2; ii++)
blasfeo_print_tran_dvec(2*nb2[ii]+2*ng2[ii], &hst2[ii], 0);
#endif
printf("\nux =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(nu[ii]+nx[ii], &hsux[ii], 0);
printf("\npi =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(nx[ii], &hspi[ii], 0);
printf("\nlam =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii], &hslam[ii], 0);
printf("\nt =\n\n");
for(ii=0; ii<=N; ii++)
blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii], &hst[ii], 0);
/************************************************
* free memory full space
************************************************/
// TODO
d_free(A);
d_free(B);
d_free(b);
d_free(x0);
d_free(R);
d_free(S);
d_free(Q);
d_free(r);
d_free(q);
d_free(d0);
int_free(idxb0);
d_free(d1);
int_free(idxb1);
d_free(dN);
int_free(idxbN);
v_free_align(work_space_ipm);
blasfeo_free_dvec(&sx0);
blasfeo_free_dvec(&sb);
blasfeo_free_dmat(&sA);
blasfeo_free_dvec(&sb0);
blasfeo_free_dmat(&sBAbt0);
if(N>1)
blasfeo_free_dmat(&sBAbt1);
blasfeo_free_dvec(&sr);
blasfeo_free_dmat(&sS);
blasfeo_free_dvec(&sr0);
blasfeo_free_dmat(&sRSQrq0);
blasfeo_free_dvec(&srq0);
if(N>1)
blasfeo_free_dmat(&sRSQrq1);
if(N>1)
blasfeo_free_dvec(&srq1);
blasfeo_free_dmat(&sRSQrqN);
blasfeo_free_dvec(&srqN);
blasfeo_free_dvec(&sd0);
blasfeo_free_dvec(&sd1);
blasfeo_free_dvec(&sdN);
for(ii=0; ii<N; ii++)
{
blasfeo_free_dvec(&hsux[ii]);
blasfeo_free_dvec(&hspi[ii]);
blasfeo_free_dvec(&hslam[ii]);
blasfeo_free_dvec(&hst[ii]);
blasfeo_free_dvec(&hsrrq[ii]);
blasfeo_free_dvec(&hsrb[ii]);
blasfeo_free_dvec(&hsrd[ii]);
blasfeo_free_dvec(&hsrm[ii]);
}
ii = N;
blasfeo_free_dvec(&hsux[ii]);
blasfeo_free_dvec(&hspi[ii]);
blasfeo_free_dvec(&hslam[ii]);
blasfeo_free_dvec(&hst[ii]);
blasfeo_free_dvec(&hsrrq[ii]);
blasfeo_free_dvec(&hsrd[ii]);
blasfeo_free_dvec(&hsrm[ii]);
v_free_align(work_space_res);
/************************************************
* print timings
************************************************/
printf("\ntime ipm full (in sec): %e", time_ipm_full);
printf("\ntime ipm cond (in sec): %e\n\n", time_ipm_cond);
/************************************************
* return
************************************************/
return 0;
}
ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(void *config, ocp_qp_dims *dims)
{
int ii;
/************************************************
* extract dims
************************************************/
int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
int *nb = dims->nb;
int *ng = dims->ng;
int *ns = dims->ns;
int nx_ = nx[1];
int nu_ = nu[1];
int ng_ = ng[1];
int ngN = ng[N];
/************************************************
* dynamical system
************************************************/
// state space matrices & initial state
double *A;
d_zeros(&A, nx_, nx_); // states update matrix
double *B;
d_zeros(&B, nx_, nu_); // inputs matrix
double *b;
d_zeros(&b, nx_, 1); // states offset
double *x0;
d_zeros(&x0, nx_, 1); // initial state
// mass-spring system
double Ts = 0.5;
mass_spring_system(Ts, nx_, nu_, A, B, b);
// TODO(dimitris): @giaf, why do we overwrite b here?
for (int jj = 0; jj < nx_; jj++) b[jj] = 0.0;
// initial state
for (int jj = 0; jj < nx_; jj++) x0[jj] = 0;
x0[0] = 2.5;
x0[1] = 2.5;
// d_print_mat(nx_, nx_, A, nx_);
// d_print_mat(nx_, nu_, B, nx_);
// d_print_mat(nx_, 1, b, nx_);
// d_print_mat(nx_, 1, x0, nx_);
#if defined(ELIMINATE_X0)
// compute b0 = b + A*x0
double *b0;
d_zeros(&b0, nx_, 1);
dcopy_3l(nx_, b, 1, b0, 1);
dgemv_n_3l(nx_, nx_, A, nx_, x0, b0);
// d_print_mat(nx_, 1, b, nx_);
// d_print_mat(nx_, 1, b0, nx_);
// then A0 is a matrix of size 0x0
double *A0;
d_zeros(&A0, 0, 0);
#endif
/************************************************
* box constraints
************************************************/
int jj_end;
int *idxb0;
int_zeros(&idxb0, nb[0], 1);
double *lb0;
d_zeros(&lb0, nb[0], 1);
double *ub0;
d_zeros(&ub0, nb[0], 1);
#if defined(ELIMINATE_X0)
for (int jj = 0; jj < nb[0]; jj++) {
lb0[jj] = -0.5; // umin
ub0[jj] = +0.5; // umin
idxb0[jj] = jj;
}
#else
jj_end = nu[0] < nb[0] ? nu[0] : nb[0];
for (int jj = 0; jj < jj_end; jj++) {
lb0[jj] = -0.5; // umin
ub0[jj] = 0.5; // umax
idxb0[jj] = jj;
}
for (int jj = jj_end; jj < nb[0]; jj++) {
lb0[jj] = x0[jj-jj_end]; // initial state
ub0[jj] = x0[jj-jj_end]; // initial state
idxb0[jj] = jj;
}
#endif
int *idxb1;
int_zeros(&idxb1, nb[1], 1);
double *lb1;
d_zeros(&lb1, nb[1], 1);
double *ub1;
d_zeros(&ub1, nb[1], 1);
jj_end = nu[1] < nb[1] ? nu[1] : nb[1];
for (int jj = 0; jj < jj_end; jj++) {
lb1[jj] = -0.5; // umin
ub1[jj] = +0.5; // umax
idxb1[jj] = jj;
}
for (int jj = jj_end; jj < nb[1]; jj++) {
lb1[jj] = -1.0; // xmin
ub1[jj] = +1.0; // xmax
idxb1[jj] = jj;
}
// int_print_mat(nb[1], 1, idxb1, nb[1]);
// d_print_mat(nb[1], 1, lb1, nb[1]);
int *idxbN;
int_zeros(&idxbN, nb[N], 1);
double *lbN;
d_zeros(&lbN, nb[N], 1);
double *ubN;
d_zeros(&ubN, nb[N], 1);
jj_end = nu[N] < nb[N] ? nu[N] : nb[N];
for (int jj = 0; jj < jj_end; jj++) {
lbN[jj] = -0.5; // umin
ubN[jj] = +0.5; // umax
idxbN[jj] = jj;
}
for (int jj = jj_end; jj < nb[N]; jj++)
{
lbN[jj] = -1.0; // xmin
ubN[jj] = +1.0; // xmax
idxbN[jj] = jj;
}
#ifndef GENERAL_CONSTRAINT_AT_TERMINAL_STAGE
for (int jj = nu[N]; jj < ngN; jj++)
{
lbN[jj] = 0.0;
ubN[jj] = 0.0;
idxbN[jj] = jj;
}
#endif
// int_print_mat(nb[N], 1, idxbN, nb[N]);
// d_print_mat(nb[N], 1, lbN, nb[N]);
/************************************************
* general constraints
************************************************/
double *C;
d_zeros(&C, ng_, nx_);
double *D;
d_zeros(&D, ng_, nu_);
double *lg;
d_zeros(&lg, ng_, 1);
double *ug;
d_zeros(&ug, ng_, 1);
double *CN;
d_zeros(&CN, ngN, nx_);
for (int ii = 0; ii < ngN; ii++) CN[ii * (ngN + 1)] = 1.0;
// d_print_mat(ngN, nx_, CN, ngN);
double *lgN;
d_zeros(&lgN, ngN, 1); // force all states to 0 at the last stage
double *ugN;
d_zeros(&ugN, ngN, 1); // force all states to 0 at the last stage
/************************************************
* soft constraints
************************************************/
double *Zl0; d_zeros(&Zl0, ns[0], 1);
for(ii=0; ii<ns[0]; ii++)
Zl0[ii] = 1e3;
double *Zu0; d_zeros(&Zu0, ns[0], 1);
for(ii=0; ii<ns[0]; ii++)
Zu0[ii] = 1e3;
double *zl0; d_zeros(&zl0, ns[0], 1);
for(ii=0; ii<ns[0]; ii++)
zl0[ii] = 1e2;
double *zu0; d_zeros(&zu0, ns[0], 1);
for(ii=0; ii<ns[0]; ii++)
zu0[ii] = 1e2;
int *idxs0; int_zeros(&idxs0, ns[0], 1);
for(ii=0; ii<ns[0]; ii++)
idxs0[ii] = nu[0]+ii;
double *d_ls0; d_zeros(&d_ls0, ns[0], 1);
for(ii=0; ii<ns[0]; ii++)
d_ls0[ii] = 0.0;
double *d_us0; d_zeros(&d_us0, ns[0], 1);
for(ii=0; ii<ns[0]; ii++)
d_us0[ii] = 0.0;
double *Zl1; d_zeros(&Zl1, ns[1], 1);
for(ii=0; ii<ns[1]; ii++)
Zl1[ii] = 1e3;
double *Zu1; d_zeros(&Zu1, ns[1], 1);
for(ii=0; ii<ns[1]; ii++)
Zu1[ii] = 1e3;
double *zl1; d_zeros(&zl1, ns[1], 1);
for(ii=0; ii<ns[1]; ii++)
zl1[ii] = 1e2;
double *zu1; d_zeros(&zu1, ns[1], 1);
for(ii=0; ii<ns[1]; ii++)
zu1[ii] = 1e2;
int *idxs1; int_zeros(&idxs1, ns[1], 1);
for(ii=0; ii<ns[1]; ii++)
idxs1[ii] = nu[1]+ii;
double *d_ls1; d_zeros(&d_ls1, ns[1], 1);
for(ii=0; ii<ns[1]; ii++)
d_ls1[ii] = 0.0;
double *d_us1; d_zeros(&d_us1, ns[1], 1);
for(ii=0; ii<ns[1]; ii++)
d_us1[ii] = 0.0;
double *ZlN; d_zeros(&ZlN, ns[N], 1);
for(ii=0; ii<ns[N]; ii++)
ZlN[ii] = 1e3;
double *ZuN; d_zeros(&ZuN, ns[N], 1);
for(ii=0; ii<ns[N]; ii++)
ZuN[ii] = 1e3;
double *zlN; d_zeros(&zlN, ns[N], 1);
for(ii=0; ii<ns[N]; ii++)
zlN[ii] = 1e2;
double *zuN; d_zeros(&zuN, ns[N], 1);
for(ii=0; ii<ns[N]; ii++)
zuN[ii] = 1e2;
int *idxsN; int_zeros(&idxsN, ns[N], 1);
for(ii=0; ii<ns[N]; ii++)
idxsN[ii] = nu[N]+ii;
double *d_lsN; d_zeros(&d_lsN, ns[N], 1);
for(ii=0; ii<ns[N]; ii++)
d_lsN[ii] = 0.0;
double *d_usN; d_zeros(&d_usN, ns[N], 1);
for(ii=0; ii<ns[N]; ii++)
d_usN[ii] = 0.0;
/************************************************
* cost function
************************************************/
double *Q;
d_zeros(&Q, nx_, nx_);
for (int ii = 0; ii < nx_; ii++) Q[ii * (nx_ + 1)] = 0.0;
double *R;
d_zeros(&R, nu_, nu_);
for (int ii = 0; ii < nu_; ii++) R[ii * (nu_ + 1)] = 2.0;
double *S;
d_zeros(&S, nu_, nx_);
double *q;
d_zeros(&q, nx_, 1);
for (int ii = 0; ii < nx_; ii++) q[ii] = 0.0;
double *r;
d_zeros(&r, nu_, 1);
for (int ii = 0; ii < nu_; ii++) r[ii] = 0.0;
#if defined(ELIMINATE_X0)
// Q0 and q0 are matrices of size 0
double *Q0;
d_zeros(&Q0, 0, 0);
double *q0;
d_zeros(&q0, 0, 1);
// compute r0 = r + S*x0
double *r0;
d_zeros(&r0, nu_, 1);
dcopy_3l(nu_, r, 1, r0, 1);
dgemv_n_3l(nu_, nx_, S, nu_, x0, r0);
// then S0 is a matrix of size nux0
double *S0;
d_zeros(&S0, nu_, 0);
#endif
/************************************************
* problem data
************************************************/
double *hA[N];
double *hB[N];
double *hb[N];
double *hQ[N+1];
double *hS[N+1];
double *hR[N+1];
double *hq[N+1];
double *hr[N+1];
double *hlb[N+1];
double *hub[N+1];
int *hidxb[N+1];
double *hC[N+1];
double *hD[N+1];
double *hlg[N+1];
double *hug[N+1];
double *hZl[N+1];
double *hZu[N+1];
double *hzl[N+1];
double *hzu[N+1];
int *hidxs[N+1]; // XXX
double *hd_ls[N+1];
double *hd_us[N+1];
#if defined(ELIMINATE_X0)
hA[0] = A0;
hb[0] = b0;
hQ[0] = Q0;
hS[0] = S0;
hq[0] = q0;
hr[0] = r0;
#else
hA[0] = A;
hb[0] = b;
hQ[0] = Q;
hS[0] = S;
hq[0] = q;
hr[0] = r;
#endif
hB[0] = B;
hR[0] = R;
hlb[0] = lb0;
hub[0] = ub0;
hidxb[0] = idxb0;
hC[0] = C;
hD[0] = D;
hlg[0] = lg;
hug[0] = ug;
hZl[0] = Zl0;
hZu[0] = Zu0;
hzl[0] = zl0;
hzu[0] = zu0;
hidxs[0] = idxs0;
hd_ls[0] = d_ls0;
hd_us[0] = d_us0;
for (int ii = 1; ii < N; ii++) {
hA[ii] = A;
hB[ii] = B;
hb[ii] = b;
hQ[ii] = Q;
hS[ii] = S;
hR[ii] = R;
hq[ii] = q;
hr[ii] = r;
hlb[ii] = lb1;
hub[ii] = ub1;
hidxb[ii] = idxb1;
hC[ii] = C;
hD[ii] = D;
hlg[ii] = lg;
hug[ii] = ug;
hZl[ii] = Zl1;
hZu[ii] = Zu1;
hzl[ii] = zl1;
hzu[ii] = zu1;
hidxs[ii] = idxs1;
hd_ls[ii] = d_ls1;
hd_us[ii] = d_us1;
}
hQ[N] = Q; // or maybe initialize to the solution of the DARE???
hq[N] = q; // or maybe initialize to the solution of the DARE???
hlb[N] = lbN;
hub[N] = ubN;
hidxb[N] = idxbN;
hC[N] = CN;
hlg[N] = lgN;
hug[N] = ugN;
hZl[N] = ZlN;
hZu[N] = ZuN;
hzl[N] = zlN;
hzu[N] = zuN;
hidxs[N] = idxsN;
hd_ls[N] = d_lsN;
hd_us[N] = d_usN;
ocp_qp_in *qp_in = ocp_qp_in_create(config, dims);
d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hlb, hub, hC, hD, hlg, hug, hZl, hZu, hzl, hzu, hidxs, hd_ls, hd_us, qp_in);
// free objective
free(Q);
free(S);
free(R);
free(q);
free(r);
#if defined(ELIMINATE_X0)
free(Q0);
free(q0);
free(r0);
free(S0);
#endif
// free dynamics
free(A);
free(B);
free(b);
free(x0);
#if defined(ELIMINATE_X0)
free(b0);
free(A0);
#endif
// free constraints
free(C);
free(D);
free(lg);
free(ug);
free(CN);
free(lgN);
free(ugN);
free(idxb0);
free(idxb1);
free(idxbN);
free(lb0);
free(lb1);
free(lbN);
free(ub0);
free(ub1);
free(ubN);
d_free(Zl0);
d_free(Zu0);
d_free(zl0);
d_free(zu0);
int_free(idxs0);
d_free(d_ls0);
d_free(d_us0);
d_free(Zl1);
d_free(Zu1);
d_free(zl1);
d_free(zu1);
int_free(idxs1);
d_free(d_ls1);
d_free(d_us1);
d_free(ZlN);
d_free(ZuN);
d_free(zlN);
d_free(zuN);
int_free(idxsN);
d_free(d_lsN);
d_free(d_usN);
return qp_in;
}
int main() {
#if defined(TARGET_X64_INTEL_HASWELL) || defined(TARGET_X64_INTEL_SABDY_BRIDGE) || \
defined(TARGET_X64_INTEL_CORE) || defined(TARGET_X86_AMD_BULLDOZER)
_MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON); // flush to zero subnormals !!!
// works only with one thread
// !!!
#endif
int ii, jj;
int rep, nrep = NREP;
int nx = 8; // number of states (it has to be even for the mass-spring
// system test problem)
int nu = 3; // number of inputs (controllers) (it has to be at least 1 and
// at most nx/2 for the mass-spring system test problem)
int N = 15; // horizon length
int nb = 11; // number of box constrained inputs and states
int ng = 0; // 4; // number of general constraints
int ngN = 4; // 4; // number of general constraints at the last stage
int nbu = nu < nb ? nu : nb;
int nbx = nb - nu > 0 ? nb - nu : 0;
// stage-wise variant size
int nxx[N + 1];
#if defined(ELIMINATE_X0)
nxx[0] = 0;
#else
nxx[0] = nx;
#endif
for (ii = 1; ii <= N; ii++) nxx[ii] = nx;
int nuu[N + 1];
for (ii = 0; ii < N; ii++) nuu[ii] = nu;
nuu[N] = 0;
int nbb[N + 1];
#if defined(ELIMINATE_X0)
nbb[0] = nbu;
#else
nbb[0] = nb;
#endif
for (ii = 1; ii < N; ii++) nbb[ii] = nb;
nbb[N] = nbx;
int ngg[N + 1];
for (ii = 0; ii < N; ii++) ngg[ii] = ng;
ngg[N] = ngN;
printf(
" Test problem: mass-spring system with %d masses and %d controls.\n",
nx / 2, nu);
printf("\n");
printf(
" MPC problem size: %d states, %d inputs, %d horizon length, %d "
"two-sided box constraints, %d two-sided general constraints.\n",
nx, nu, N, nb, ng);
printf("\n");
printf("qpDUNES\n");
printf("\n");
/************************************************
* dynamical system
************************************************/
// state space matrices & initial state
double *A;
d_zeros(&A, nx, nx); // states update matrix
double *B;
d_zeros(&B, nx, nu); // inputs matrix
double *b;
d_zeros(&b, nx, 1); // states offset
double *x0;
d_zeros(&x0, nx, 1); // initial state
// mass-spring system
double Ts = 0.5; // sampling time
mass_spring_system(Ts, nx, nu, A, B, b, x0);
for (jj = 0; jj < nx; jj++) b[jj] = 0.1;
for (jj = 0; jj < nx; jj++) x0[jj] = 0;
x0[0] = 2.5;
x0[1] = 2.5;
// d_print_mat(nx, nx, A, nx);
// d_print_mat(nx, nu, B, nx);
// d_print_mat(nx, 1, b, nx);
// d_print_mat(nx, 1, x0, nx);
#if defined(ELIMINATE_X0)
// compute b0 = b + A*x0
double *b0;
d_zeros(&b0, nx, 1);
dcopy_3l(nx, b, 1, b0, 1);
dgemv_n_3l(nx, nx, A, nx, x0, b0);
// d_print_mat(nx, 1, b, nx);
// d_print_mat(nx, 1, b0, nx);
// then A0 is a matrix of size 0x0
double *A0;
d_zeros(&A0, 0, 0);
#endif
/************************************************
* box constraints
************************************************/
#if defined (FLIP_BOUNDS)
int jj_end;
#endif
int *idxb0;
int_zeros(&idxb0, nbb[0], 1);
double *lb0;
d_zeros(&lb0, nbb[0], 1);
double *ub0;
d_zeros(&ub0, nbb[0], 1);
#if defined(ELIMINATE_X0)
for (jj = 0; jj < nbb[0]; jj++) {
lb0[jj] = - 0.5; // umin
ub0[jj] = + 0.5; // umin
idxb0[jj] = jj;
}
#else
#if defined (FLIP_BOUNDS)
jj_end = nbu < nbb[0] ? nbu : nbb[0];
for (jj = 0; jj < jj_end; jj++) {
lb0[jj] = - 0.5; // umin
ub0[jj] = + 0.5; // umax
idxb0[jj] = jj;
}
for ( ; jj < nbb[0]; jj++) {
lb0[jj] = x0[jj-nbu]; // initial state
ub0[jj] = x0[jj-nbu]; // initial state
idxb0[jj] = jj;
}
#else
for (jj = 0; jj < nxx[0]; jj++) {
lb0[jj] = x0[jj]; // initial state
ub0[jj] = x0[jj]; // initial state
idxb0[jj] = jj;
}
for (jj = 0; jj < nbu; jj++) {
lb0[jj+nxx[0]] = -0.5; // umin
ub0[jj+nxx[0]] = 0.5; // umax
idxb0[jj+nxx[0]] = nxx[0]+jj;
}
#endif
#endif
// int_print_mat(nbb[0], 1, idxb0, nbb[0]);
// d_print_mat(nbb[0], 1, lb0, nbb[0]);
int *idxb1;
int_zeros(&idxb1, nbb[1], 1);
double *lb1;
d_zeros(&lb1, nbb[1], 1);
double *ub1;
d_zeros(&ub1, nbb[1], 1);
#if defined (FLIP_BOUNDS)
jj_end = nbu < nbb[1] ? nbu : nbb[1];
for (jj = 0; jj < jj_end; jj++) {
lb1[jj] = - 0.5; // umin
ub1[jj] = + 0.5; // umax
idxb1[jj] = jj;
}
for ( ; jj < nbb[1]; jj++) {
lb1[jj] = - 4.0; // xmin
ub1[jj] = + 4.0; // xmax
idxb1[jj] = jj;
}
#else
for (jj = 0; jj < nbx; jj++) {
lb1[jj] = -4.0; // xmin
ub1[jj] = 4.0; // xmax
idxb1[jj] = jj;
}
for (; jj < nb; jj++) {
lb1[jj] = -0.5; // umin
ub1[jj] = 0.5; // umax
idxb1[jj] = jj;
}
#endif
// int_print_mat(nbb[1], 1, idxb1, nbb[1]);
// d_print_mat(nbb[1], 1, lb1, nbb[1]);
int *idxbN;
int_zeros(&idxbN, nbb[N], 1);
double *lbN;
d_zeros(&lbN, nbb[N], 1);
double *ubN;
d_zeros(&ubN, nbb[N], 1);
#if defined (FLIP_BOUNDS)
jj_end = nbu < nbb[N] ? nbu : nbb[N];
for (jj = 0; jj < jj_end; jj++) {
lbN[jj] = - 0.5; // umin
ubN[jj] = + 0.5; // umax
idxbN[jj] = jj;
}
for ( ; jj < nbb[N]; jj++) {
lbN[jj] = - 4.0; // xmin
ubN[jj] = + 4.0; // xmax
idxbN[jj] = jj;
}
#else
for (jj = 0; jj < nbx; jj++) {
lbN[jj] = -4.0; // xmin
ubN[jj] = 4.0; // xmax
idxbN[jj] = jj;
}
#endif
// int_print_mat(nbb[N], 1, idxbN, nbb[N]);
// d_print_mat(nbb[N], 1, lbN, nbb[N]);
/************************************************
* general constraints
************************************************/
double *C;
d_zeros(&C, ng, nx);
double *D;
d_zeros(&D, ng, nu);
double *lg;
d_zeros(&lg, ng, 1);
double *ug;
d_zeros(&ug, ng, 1);
double *CN;
d_zeros(&CN, ngN, nx);
for (ii = 0; ii < ngN; ii++) CN[ii * (ngN + 1)] = 1.0;
// d_print_mat(ngN, nx, CN, ngN);
double *lgN;
d_zeros(&lgN, ngN, 1); // force all states to 0 at the last stage
double *ugN;
d_zeros(&ugN, ngN, 1); // force all states to 0 at the last stage
/************************************************
* cost function
************************************************/
double *Q;
d_zeros(&Q, nx, nx);
for (ii = 0; ii < nx; ii++) Q[ii * (nx + 1)] = 1.0;
double *R;
d_zeros(&R, nu, nu);
for (ii = 0; ii < nu; ii++) R[ii * (nu + 1)] = 2.0;
double *S;
d_zeros(&S, nu, nx);
double *q;
d_zeros(&q, nx, 1);
for (ii = 0; ii < nx; ii++) q[ii] = 0.1;
double *r;
d_zeros(&r, nu, 1);
for (ii = 0; ii < nu; ii++) r[ii] = 0.2;
#if defined(ELIMINATE_X0)
// Q0 and q0 are matrices of size 0
double *Q0;
d_zeros(&Q0, 0, 0);
double *q0;
d_zeros(&q0, 0, 1);
// compute r0 = r + S*x0
double *r0;
d_zeros(&r0, nu, 1);
dcopy_3l(nu, r, 1, r0, 1);
dgemv_n_3l(nu, nx, S, nu, x0, r0);
// then S0 is a matrix of size nux0
double *S0;
d_zeros(&S0, nu, 0);
#endif
/************************************************
* problems data
************************************************/
double *hA[N];
double *hB[N];
double *hb[N];
double *hQ[N + 1];
double *hS[N];
double *hR[N];
double *hq[N + 1];
double *hr[N];
double *hlb[N + 1];
double *hub[N + 1];
int *hidxb[N + 1];
double *hC[N + 1];
double *hD[N];
double *hlg[N + 1];
double *hug[N + 1];
#if defined(ELIMINATE_X0)
hA[0] = A0;
hb[0] = b0;
hQ[0] = Q0;
hS[0] = S0;
hq[0] = q0;
hr[0] = r0;
#else
hA[0] = A;
hb[0] = b;
hQ[0] = Q;
hS[0] = S;
hq[0] = q;
hr[0] = r;
#endif
hB[0] = B;
hR[0] = R;
hlb[0] = lb0;
hub[0] = ub0;
hidxb[0] = idxb0;
hC[0] = C;
hD[0] = D;
hlg[0] = lg;
hug[0] = ug;
for (ii = 1; ii < N; ii++) {
hA[ii] = A;
hB[ii] = B;
hb[ii] = b;
hQ[ii] = Q;
hS[ii] = S;
hR[ii] = R;
hq[ii] = q;
hr[ii] = r;
hlb[ii] = lb1;
hub[ii] = ub1;
hidxb[ii] = idxb1;
hC[ii] = C;
hD[ii] = D;
hlg[ii] = lg;
hug[ii] = ug;
}
hQ[N] = Q; // or maybe initialize to the solution of the DARE???
hq[N] = q; // or maybe initialize to the solution of the DARE???
hlb[N] = lbN;
hub[N] = ubN;
hidxb[N] = idxbN;
hC[N] = CN;
hlg[N] = lgN;
hug[N] = ugN;
/************************************************
* solution
************************************************/
double *hx[N + 1];
double *hu[N];
double *hpi[N];
double *hlam[N+1];
double *ht[N+1];
for (ii = 0; ii < N; ii++) {
d_zeros(&hx[ii], nxx[ii], 1);
d_zeros(&hu[ii], nuu[ii], 1);
d_zeros(&hpi[ii], nxx[ii+1], 1);
d_zeros(&hlam[ii], 2*nbb[ii]+2*ngg[ii], 1);
d_zeros(&ht[ii], 2*nbb[ii]+2*ngg[ii], 1);
}
d_zeros(&hx[N], nxx[N], 1);
d_zeros(&hlam[N], 2*nbb[N]+2*ngg[N], 1);
d_zeros(&ht[N], 2*nbb[N]+2*ngg[N], 1);
/************************************************
* XXX initial guess
************************************************/
double *hux_in[N+1];
double *hlam_in[N+1];
double *ht_in[N+1];
for (ii = 0; ii <= N; ii++) {
d_zeros(&hux_in[ii], nuu[ii]+nxx[ii], 1);
d_zeros(&hlam_in[ii], 2*nbb[ii]+2*ngg[ii], 1);
d_zeros(&ht_in[ii], 2*nbb[ii]+2*ngg[ii], 1);
}
/************************************************
* create the in and out struct
************************************************/
ocp_qp_in qp_in;
qp_in.N = N;
qp_in.nx = (const int *) nxx;
qp_in.nu = (const int *) nuu;
qp_in.nb = (const int *) nbb;
qp_in.nc = (const int *) ngg;
qp_in.A = (const double **) hA;
qp_in.B = (const double **) hB;
qp_in.b = (const double **) hb;
qp_in.Q = (const double **) hQ;
qp_in.S = (const double **) hS;
qp_in.R = (const double **) hR;
qp_in.q = (const double **) hq;
qp_in.r = (const double **) hr;
qp_in.idxb = (const int **) hidxb;
qp_in.lb = (const double **) hlb;
qp_in.ub = (const double **) hub;
qp_in.Cx = (const double **) hC;
qp_in.Cu = (const double **) hD;
qp_in.lc = (const double **) hlg;
qp_in.uc = (const double **) hug;
ocp_qp_out qp_out;
qp_out.x = hx;
qp_out.u = hu;
qp_out.pi = hpi;
qp_out.lam = hlam;
qp_out.t = ht; // XXX why also the slack variables ???
/************************************************
* solver arguments
************************************************/
ocp_qp_qpdunes_args *args = ocp_qp_qpdunes_create_arguments(QPDUNES_LINEAR_MPC);
/************************************************
* workspace
************************************************/
ocp_qp_qpdunes_memory *mem = NULL;
int_t work_space_size = ocp_qp_qpdunes_calculate_workspace_size(&qp_in, args);
void *work = (void*)malloc(work_space_size);
/************************************************
* call the solver
************************************************/
int return_value = 0;
acados_timer timer;
acados_tic(&timer);
// nrep = 1;
for (rep = 0; rep < nrep; rep++) {
// NOTE(dimitris): creating memory again to avoid warm start
mem = ocp_qp_qpdunes_create_memory(&qp_in, args);
// call the QP OCP solver
ocp_qp_qpdunes(&qp_in, &qp_out, args, mem, work);
}
real_t time = acados_toc(&timer)/nrep;
if (return_value == ACADOS_SUCCESS)
printf("\nACADOS status: solution found\n");
if (return_value == ACADOS_MAXITER)
printf("\nACADOS status: maximum number of iterations reached\n");
if (return_value == ACADOS_MINSTEP)
printf("\nACADOS status: below minimum step size length\n");
printf("\nu = \n");
for (ii = 0; ii < N; ii++) d_print_mat(1, nuu[ii], hu[ii], 1);
printf("\nx = \n");
for (ii = 0; ii <= N; ii++) d_print_mat(1, nxx[ii], hx[ii], 1);
printf("\n");
printf(" Average solution time over %d runs: %5.2e seconds\n", nrep, time);
printf("\n\n");
/************************************************
* free memory
************************************************/
d_free(A);
d_free(B);
d_free(b);
d_free(x0);
d_free(Q);
d_free(S);
d_free(R);
d_free(q);
d_free(r);
#if defined(ELIMINATE_X0)
d_free(A0);
d_free(b0);
d_free(Q0);
d_free(S0);
d_free(q0);
d_free(r0);
#endif
int_free(idxb0);
d_free(lb0);
d_free(ub0);
int_free(idxb1);
d_free(lb1);
d_free(ub1);
int_free(idxbN);
d_free(lbN);
d_free(ubN);
d_free(C);
d_free(D);
d_free(lg);
d_free(ug);
d_free(CN);
d_free(lgN);
d_free(ugN);
for (ii = 0; ii < N; ii++) {
d_free(hx[ii]);
d_free(hu[ii]);
d_free(hpi[ii]);
d_free(hlam[ii]);
d_free(ht[ii]);
}
d_free(hx[N]);
d_free(hlam[N]);
d_free(ht[N]);
ocp_qp_qpdunes_free_memory(mem);
free(work);
return 0;
}
