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() {
#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;
}
// Simple SQP example for acados
int main() {
int_t nil;
int_t NMF_MAX = 4; // data exist up to 9 masses
int_t IMPL_MAX = 2; // was originally 4, reduced to run the ctest faster
for (int_t implicit = 0; implicit < IMPL_MAX; implicit++) {
if (implicit == 0) {
printf(
"\n\n--------------------------------------------------------------------\n");
printf(
"------------------ Explicit Runge-Kutta of order 4 -----------------\n");
printf(
"--------------------------------------------------------------------\n");
} else {
printf(
"\n\n--------------------------------------------------------------------\n");
printf(
"-------------- Lifted Implicit Runge-Kutta of order %d --------------\n",
2 * implicit);
printf(
"--------------------------------------------------------------------\n");
}
for (int_t NMF = 1; NMF < NMF_MAX; NMF++) {
printf("\n------------ NUMBER OF FREE MASSES = %d ------------\n",
NMF);
int_t NX = 6 * NMF;
int_t NU = 3;
int_t jj;
// Problem data
int_t N = NN;
real_t *x0;
real_t *w; // states and controls stacked
real_t *Q;
real_t *R;
real_t *xref;
real_t *uref;
int_t max_sqp_iters = 20;
int_t max_iters = 1;
real_t *x_end;
real_t *u_end;
FILE *initStates, *refStates;
real_t feas, stepX, stepU;
d_zeros(&x0, NX, 1);
d_zeros(&xref, NX, 1);
d_zeros(&uref, NU, 1);
d_zeros(&x_end, NX, 1);
d_zeros(&u_end, NU, 1);
d_zeros(&w, NN * (NX + NU) + NX, 1);
d_zeros(&Q, NX, NX);
d_zeros(&R, NU, NU);
switch (NMF) {
case 1:
initStates = fopen(X0_NM2_FILE, "r");
break;
case 2:
initStates = fopen(X0_NM3_FILE, "r");
break;
case 3:
initStates = fopen(X0_NM4_FILE, "r");
break;
case 4:
initStates = fopen(X0_NM5_FILE, "r");
break;
case 5:
initStates = fopen(X0_NM6_FILE, "r");
break;
case 6:
initStates = fopen(X0_NM7_FILE, "r");
break;
case 7:
initStates = fopen(X0_NM8_FILE, "r");
break;
default:
initStates = fopen(X0_NM9_FILE, "r");
break;
}
for (int_t i = 0; i < NX; i++) {
nil = fscanf(initStates, "%lf", &x0[i]);
}
fclose(initStates);
switch (NMF) {
case 1:
refStates = fopen(XN_NM2_FILE, "r");
break;
case 2:
refStates = fopen(XN_NM3_FILE, "r");
break;
case 3:
refStates = fopen(XN_NM4_FILE, "r");
break;
case 4:
refStates = fopen(XN_NM5_FILE, "r");
break;
case 5:
refStates = fopen(XN_NM6_FILE, "r");
break;
case 6:
refStates = fopen(XN_NM7_FILE, "r");
break;
case 7:
refStates = fopen(XN_NM8_FILE, "r");
break;
default:
refStates = fopen(XN_NM9_FILE, "r");
break;
}
for (int_t i = 0; i < NX; i++) {
nil = fscanf(refStates, "%lf", &xref[i]);
}
fclose(refStates);
for (int_t i = 0; i < NN; i++) {
for (int_t j = 0; j < NX; j++) w[i * (NX + NU) + j] = xref[j];
}
for (int_t j = 0; j < NX; j++) w[NN * (NX + NU) + j] = xref[j];
for (int_t i = 0; i < NX; i++) Q[i * (NX + 1)] = 1.0;
for (int_t i = 0; i < NU; i++) R[i * (NU + 1)] = 0.01;
// Integrator structs
real_t T = 0.2;
sim_in sim_in[NN];
sim_out sim_out[NN];
sim_info info[NN];
sim_RK_opts rk_opts[NN];
void *sim_work = NULL;
sim_lifted_irk_memory irk_mem[NN];
for (jj = 0; jj < NN; jj++) {
sim_in[jj].num_steps = 2;
sim_in[jj].step = T / sim_in[jj].num_steps;
sim_in[jj].nx = NX;
sim_in[jj].nu = NU;
sim_in[jj].sens_forw = true;
sim_in[jj].sens_adj = false;
sim_in[jj].sens_hess = false;
sim_in[jj].num_forw_sens = NX + NU;
switch (NMF) {
case 1:
sim_in[jj].vde = &vde_chain_nm2;
sim_in[jj].forward_vde_wrapper = &vde_fun;
sim_in[jj].jac = &jac_chain_nm2;
sim_in[jj].jacobian_wrapper = &jac_fun;
break;
case 2:
sim_in[jj].vde = &vde_chain_nm3;
sim_in[jj].forward_vde_wrapper = &vde_fun;
sim_in[jj].jac = &jac_chain_nm2;
sim_in[jj].jacobian_wrapper = &jac_fun;
break;
case 3:
sim_in[jj].vde = &vde_chain_nm4;
sim_in[jj].forward_vde_wrapper = &vde_fun;
sim_in[jj].jac = &jac_chain_nm4;
sim_in[jj].jacobian_wrapper = &jac_fun;
break;
// case 4:
// sim_in[jj].vde = &vde_chain_nm5;
// sim_in[jj].VDE_forw = &vde_fun;
// sim_in[jj].jac = &jac_chain_nm5;
// sim_in[jj].jac_fun = &jac_fun;
// break;
// case 5:
// sim_in[jj].vde = &vde_chain_nm6;
// sim_in[jj].VDE_forw = &vde_fun;
// sim_in[jj].jac = &jac_chain_nm6;
// sim_in[jj].jac_fun = &jac_fun;
// break;
// case 6:
// sim_in[jj].vde = &vde_chain_nm7;
// sim_in[jj].VDE_forw = &vde_fun;
// sim_in[jj].jac = &jac_chain_nm7;
// sim_in[jj].jac_fun = &jac_fun;
// break;
// case 7:
// sim_in[jj].vde = &vde_chain_nm8;
// sim_in[jj].VDE_forw = &vde_fun;
// sim_in[jj].jac = &jac_chain_nm8;
// sim_in[jj].jac_fun = &jac_fun;
// break;
// default:
// sim_in[jj].vde = &vde_chain_nm9;
// sim_in[jj].VDE_forw = &vde_fun;
// sim_in[jj].jac = &jac_chain_nm9;
// sim_in[jj].jac_fun = &jac_fun;
// break;
}
sim_in[jj].x = malloc(sizeof(*sim_in[jj].x) * (NX));
sim_in[jj].u = malloc(sizeof(*sim_in[jj].u) * (NU));
sim_in[jj].S_forw = malloc(sizeof(*sim_in[jj].S_forw) * (NX * (NX + NU)));
for (int_t i = 0; i < NX * (NX + NU); i++)
sim_in[jj].S_forw[i] = 0.0;
for (int_t i = 0; i < NX; i++)
sim_in[jj].S_forw[i * (NX + 1)] = 1.0;
sim_out[jj].xn = malloc(sizeof(*sim_out[jj].xn) * (NX));
sim_out[jj].S_forw = malloc(sizeof(*sim_out[jj].S_forw) * (NX * (NX + NU)));
sim_out[jj].info = &info[jj];
int_t workspace_size;
if (implicit > 0) {
sim_irk_create_arguments(&rk_opts[jj], implicit, "Gauss");
workspace_size =
sim_lifted_irk_calculate_workspace_size(&sim_in[jj], &rk_opts[jj]);
sim_lifted_irk_create_memory(&sim_in[jj], &rk_opts[jj], &irk_mem[jj]);
} else {
sim_erk_create_arguments(&rk_opts[jj], 4);
workspace_size =
sim_erk_calculate_workspace_size(&sim_in[jj], &rk_opts[jj]);
}
if (jj == 0) sim_work = (void *)malloc(workspace_size);
}
int_t nx[NN + 1] = {0};
int_t nu[NN + 1] = {0};
int_t nb[NN + 1] = {0};
int_t nc[NN + 1] = {0};
nx[0] = NX;
nu[0] = NU;
nb[0] = NX + NU;
nc[0] = 0;
for (int_t i = 1; i < N; i++) {
nx[i] = NX;
nu[i] = NU;
nb[i] = NU;
nc[i] = 0;
}
nx[N] = NX;
nu[N] = 0;
nb[N] = 0;
nc[N] = 0;
/************************************************
* box constraints
************************************************/
int *idxb0;
int_zeros(&idxb0, NX + NU, 1);
real_t *lb0;
d_zeros(&lb0, NX + NU, 1);
real_t *ub0;
d_zeros(&ub0, NX + NU, 1);
for (int_t j = 0; j < NX; j++) {
lb0[j] = x0[j]; // xmin
ub0[j] = x0[j]; // xmax
idxb0[j] = j;
}
for (int_t j = 0; j < NU; j++) {
lb0[j + NX] = -UMAX; // umin
ub0[j + NX] = UMAX; // umax
idxb0[j + NX] = j + NX;
}
int_t *idxb1;
int_zeros(&idxb1, NU, 1);
real_t *lb1[N - 1];
real_t *ub1[N - 1];
for (int_t i = 0; i < N - 1; i++) {
d_zeros(&lb1[i], NU, 1);
d_zeros(&ub1[i], NU, 1);
for (int_t j = 0; j < NU; j++) {
lb1[i][j] = -UMAX; // umin
ub1[i][j] = UMAX; // umax
idxb1[j] = j + NX;
}
}
real_t *pA[N];
real_t *pB[N];
real_t *pb[N];
real_t *pQ[N + 1];
real_t *pS[N];
real_t *pR[N];
real_t *pq[N + 1];
real_t *pr[N];
real_t *px[N + 1];
real_t *pu[N];
real_t *ppi[N];
real_t *plam[N + 1];
real_t *pCx[N + 1];
real_t *pCu[N];
real_t *plc[N + 1];
real_t *puc[N + 1];
for (int_t i = 0; i < N; i++) {
d_zeros(&pA[i], nx[i + 1], nx[i]);
d_zeros(&pB[i], nx[i + 1], nu[i]);
d_zeros(&pb[i], nx[i + 1], 1);
d_zeros(&pS[i], nu[i], nx[i]);
d_zeros(&pq[i], nx[i], 1);
d_zeros(&pr[i], nu[i], 1);
d_zeros(&px[i], nx[i], 1);
d_zeros(&pu[i], nu[i], 1);
d_zeros(&ppi[i], nx[i + 1], 1);
d_zeros(&plam[i], 2*nb[i] + 2*nc[i], 1);
d_zeros(&pCx[i], nc[i], nx[i]);
d_zeros(&pCu[i], nc[i], nu[i]);
d_zeros(&plc[i], nc[i], 1);
d_zeros(&puc[i], nc[i], 1);
}
d_zeros(&pq[N], nx[N], 1);
d_zeros(&px[N], nx[N], 1);
d_zeros(&pCx[N], nc[N], nx[N]);
d_zeros(&plc[N], nc[N], 1);
d_zeros(&puc[N], nc[N], 1);
d_zeros(&plam[N], 2*nb[N] + 2*nc[N], 1);
real_t *hlb[N + 1];
real_t *hub[N + 1];
int *hidxb[N + 1];
hlb[0] = lb0;
hub[0] = ub0;
hidxb[0] = idxb0;
for (int_t i = 1; i < N; i++) {
hlb[i] = lb1[i - 1];
hub[i] = ub1[i - 1];
hidxb[i] = idxb1;
}
// Allocate OCP QP variables
ocp_qp_in qp_in;
qp_in.N = N;
ocp_qp_out qp_out;
qp_in.nx = nx;
qp_in.nu = nu;
qp_in.nb = nb;
qp_in.nc = nc;
for (int_t i = 0; i < N; i++) {
pQ[i] = Q;
pR[i] = R;
}
pQ[N] = Q;
qp_in.Q = (const real_t **)pQ;
qp_in.S = (const real_t **)pS;
qp_in.R = (const real_t **)pR;
qp_in.q = (const real_t **)pq;
qp_in.r = (const real_t **)pr;
qp_in.A = (const real_t **)pA;
qp_in.B = (const real_t **)pB;
qp_in.b = (const real_t **)pb;
// TODO(dimitris): rename all to p
qp_in.lb = (const real_t **)hlb;
qp_in.ub = (const real_t **)hub;
qp_in.idxb = (const int **)hidxb;
qp_in.Cx = (const real_t **)pCx;
qp_in.Cu = (const real_t **)pCu;
qp_in.lc = (const real_t **)plc;
qp_in.uc = (const real_t **)puc;;
qp_out.x = px;
qp_out.u = pu;
qp_out.pi = ppi;
qp_out.lam = plam;
// Initialize solver
#ifdef USE_QPOASES
ocp_qp_condensing_qpoases_args *qpsolver_args =
ocp_qp_condensing_qpoases_create_arguments(&qp_in);
ocp_qp_condensing_qpoases_memory *qpsolver_memory =
ocp_qp_condensing_qpoases_create_memory(&qp_in, qpsolver_args);
int_t qpsolver_workspace_size =
ocp_qp_condensing_qpoases_calculate_workspace_size(&qp_in, qpsolver_args);
int_t qpsolver_memory_size =
ocp_qp_condensing_qpoases_calculate_memory_size(&qp_in, qpsolver_args);
#else
ocp_qp_condensing_hpipm_args *qpsolver_args =
ocp_qp_condensing_hpipm_create_arguments(qp_in);
ocp_qp_condensing_hpipm_memory qpsolver_memory;
qpsolver_args->mu_max = 1e-8;
qpsolver_args->iter_max = 20;
qpsolver_args->alpha_min = 1e-8;
qpsolver_args->mu0 = 1.0;
int_t qpsolver_workspace_size =
ocp_qp_condensing_hpipm_calculate_workspace_size(&qp_in, qpsolver_args);
int_t qpsolver_memory_size =
ocp_qp_condensing_hpipm_calculate_memory_size(&qp_in, qpsolver_args);
#endif
void *qpsolver_work = calloc(qpsolver_workspace_size, sizeof(char));
#ifdef USE_QPOASES
(void) qpsolver_memory_size;
#else
ocp_qp_condensing_hpipm_assign_memory(&qp_in, qpsolver_args, &qpsolver_memory,
qpsolver_mem);
#endif
acados_timer timer;
real_t timings = 0;
real_t timings_sim = 0;
real_t timings_la = 0;
real_t timings_ad = 0;
// for (int_t iter = 0; iter < max_iters; iter++) {
// printf("\n------ TIME STEP %d ------\n", iter);
acados_tic(&timer);
for (int_t sqp_iter = 0; sqp_iter < max_sqp_iters; sqp_iter++) {
feas = -1e10;
stepX = -1e10;
stepU = -1e10;
#if PARALLEL
#pragma omp parallel for
#endif
for (int_t i = 0; i < N; i++) {
// Pass state and control to integrator
for (int_t j = 0; j < NX; j++)
sim_in[i].x[j] = w[i * (NX + NU) + j];
for (int_t j = 0; j < NU; j++)
sim_in[i].u[j] = w[i * (NX + NU) + NX + j];
if (implicit > 0) {
sim_lifted_irk(&sim_in[i], &sim_out[i], &rk_opts[i],
&irk_mem[i], sim_work);
} else {
sim_erk(&sim_in[i], &sim_out[i], &rk_opts[i], 0,
sim_work);
}
for (int_t j = 0; j < NX; j++) {
pb[i][j] = sim_out[i].xn[j] - w[(i + 1) * (NX + NU) + j];
if (fabs(pb[i][j]) > feas) feas = fabs(pb[i][j]);
for (int_t k = 0; k < NX; k++)
pA[i][j * NX + k] = sim_out[i].S_forw[j * NX + k]; // COLUMN MAJOR
// FROM CASADI
}
for (int_t j = 0; j < NU; j++)
for (int_t k = 0; k < NX; k++)
pB[i][j * NX + k] = sim_out[i].S_forw[(NX + j) * NX + k];
timings_sim += sim_out[i].info->CPUtime;
timings_la += sim_out[i].info->LAtime;
timings_ad += sim_out[i].info->ADtime;
}
for (int_t i = 0; i < N; i++) {
// Update bounds:
if (i == 0) {
for (int_t j = 0; j < NU; j++) {
lb0[NX + j] = -UMAX - w[NX + j];
ub0[NX + j] = UMAX - w[NX + j];
}
} else {
for (int_t j = 0; j < NU; j++) {
lb1[i - 1][j] = -UMAX - w[i * (NX + NU) + NX + j];
ub1[i - 1][j] = UMAX - w[i * (NX + NU) + NX + j];
}
}
// Construct QP matrices:
for (int_t j = 0; j < NX; j++)
pq[i][j] = Q[j * (NX + 1)] * (w[i * (NX + NU) + j] - xref[j]);
for (int_t j = 0; j < NU; j++)
pr[i][j] = R[j * (NU + 1)] * (w[i * (NX + NU) + NX + j] - uref[j]);
}
for (int_t j = 0; j < NX; j++) {
lb0[j] = (x0[j] - w[j]);
ub0[j] = (x0[j] - w[j]);
}
for (int_t j = 0; j < NX; j++)
pq[N][j] = Q[j * (NX + 1)] * (w[N * (NX + NU) + j] - xref[j]);
// Set updated bounds:
hlb[0] = lb0;
hub[0] = ub0;
for (int_t i = 1; i < N; i++) {
hlb[i] = lb1[i - 1];
hub[i] = ub1[i - 1];
}
int status = 0;
#ifdef USE_QPOASES
status = ocp_qp_condensing_qpoases(&qp_in, &qp_out, qpsolver_args,
qpsolver_memory, qpsolver_work);
#else
status = ocp_qp_condensing_hpipm(&qp_in, &qp_out, qpsolver_args,
&qpsolver_memory, qpsolver_work);
#endif
if (status) {
printf("qpOASES returned error status %d\n", status);
return -1;
}
for (int_t i = 0; i < N; i++) {
for (int_t j = 0; j < NX; j++) {
w[i * (NX + NU) + j] += qp_out.x[i][j];
if (fabs(qp_out.x[i][j]) > stepX)
stepX = fabs(qp_out.x[i][j]);
}
for (int_t j = 0; j < NU; j++) {
w[i * (NX + NU) + NX + j] += qp_out.u[i][j];
if (fabs(qp_out.u[i][j]) > stepU)
stepU = fabs(qp_out.u[i][j]);
}
}
for (int_t j = 0; j < NX; j++) {
w[N * (NX + NU) + j] += qp_out.x[N][j];
if (fabs(qp_out.x[N][j]) > stepX)
stepX = fabs(qp_out.x[N][j]);
}
if (sqp_iter == max_sqp_iters - 1) {
fprintf(stdout,
"--- ITERATION %d, Infeasibility: %+.3e , step X: "
"%+.3e, "
"step U: %+.3e \n",
sqp_iter, feas, stepX, stepU);
}
}
// for (int_t i = 0; i < NX; i++) x0[i] = w[NX+NU+i];
// shift_states(w, x_end, N);
// shift_controls(w, u_end, N);
timings += acados_toc(&timer);
// }
printf("\nAverage of %.3f ms in the integrator,\n",
1e3 * timings_sim / (max_sqp_iters * max_iters));
printf(" of which %.3f ms spent in CasADi and\n",
1e3 * timings_ad / (max_sqp_iters * max_iters));
printf(" of which %.3f ms spent in BLASFEO.\n",
1e3 * timings_la / (max_sqp_iters * max_iters));
printf("--Total of %.3f ms per SQP iteration.--\n",
1e3 * timings / (max_sqp_iters * max_iters));
// #ifdef DEBUG
// print_matrix_name("stdout", "sol", w, NX+NU, N);
// #endif // DEBUG
// TODO(dimitris): this program is leaking memory
}
}
return 0 * nil;
}
int sim_lifted_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_,
void *work_)
{
// typecasting
sim_config *config = config_;
sim_opts *opts = opts_;
sim_lifted_irk_memory *memory = (sim_lifted_irk_memory *) mem_;
void *dims_ = in->dims;
sim_lifted_irk_dims *dims = (sim_lifted_irk_dims *) dims_;
sim_lifted_irk_workspace *workspace =
(sim_lifted_irk_workspace *) sim_lifted_irk_cast_workspace(config, dims, opts,
work_);
int nx = dims->nx;
int nu = dims->nu;
int nz = dims->nz;
int ns = opts->ns;
if ( opts->ns != opts->tableau_size )
{
printf("Error in sim_lifted_irk: the Butcher tableau size does not match ns");
return ACADOS_FAILURE;
}
// assert - only use supported features
if (nz != 0)
{
printf("nz should be zero - DAEs are not supported by the lifted IRK integrator");
return ACADOS_FAILURE;
}
if (opts->output_z)
{
printf("opts->output_z should be false - DAEs are not supported for the lifted IRK integrator");
return ACADOS_FAILURE;
}
if (opts->sens_algebraic)
{
printf("opts->sens_algebraic should be false - DAEs are not supported for the lifted IRK integrator");
return ACADOS_FAILURE;
}
int ii, jj, ss;
double a;
double *x = in->x;
double *u = in->u;
double *S_forw_in = in->S_forw;
// int newton_iter = opts->newton_iter; // not used; always 1 in lifted
double *A_mat = opts->A_mat;
double *b_vec = opts->b_vec;
int num_steps = opts->num_steps;
double step = in->T / num_steps;
// TODO(FreyJo): this should be an option!
int update_sens = memory->update_sens;
int *ipiv = workspace->ipiv;
struct blasfeo_dmat *JGK = memory->JGK;
struct blasfeo_dmat *S_forw = memory->S_forw;
struct blasfeo_dmat *J_temp_x = workspace->J_temp_x;
struct blasfeo_dmat *J_temp_xdot = workspace->J_temp_xdot;
struct blasfeo_dmat *J_temp_u = workspace->J_temp_u;
struct blasfeo_dvec *rG = workspace->rG;
struct blasfeo_dvec *K = memory->K;
struct blasfeo_dmat *JGf = memory->JGf;
struct blasfeo_dmat *JKf = memory->JKf;
struct blasfeo_dvec *xt = workspace->xt;
struct blasfeo_dvec *xn = workspace->xn;
struct blasfeo_dvec *xn_out = workspace->xn_out;
struct blasfeo_dvec *dxn = workspace->dxn;
struct blasfeo_dvec *w = workspace->w;
double *x_out = out->xn;
double *S_forw_out = out->S_forw;
struct blasfeo_dvec_args ext_fun_in_K;
ext_fun_arg_t ext_fun_type_in[3];
void *ext_fun_in[3];
struct blasfeo_dvec_args ext_fun_out_rG;
ext_fun_arg_t ext_fun_type_out[5];
void *ext_fun_out[5];
lifted_irk_model *model = in->model;
acados_timer timer, timer_ad;
double timing_ad = 0.0;
if (opts->sens_adj)
{
printf("LIFTED_IRK with ADJOINT SENSITIVITIES - NOT IMPLEMENTED YET - EXITING.");
exit(1);
}
blasfeo_dgese(nx, nx, 0.0, J_temp_x, 0, 0);
blasfeo_dgese(nx, nx, 0.0, J_temp_xdot, 0, 0);
blasfeo_dgese(nx, nu, 0.0, J_temp_x, 0, 0);
blasfeo_dvecse(nx * ns, 0.0, rG, 0);
// TODO(dimitris): shouldn't this be NF instead of nx+nu??
if (update_sens) blasfeo_pack_dmat(nx, nx + nu, S_forw_in, nx, S_forw, 0, 0);
blasfeo_dvecse(nx * ns, 0.0, rG, 0);
blasfeo_pack_dvec(nx, x, xn, 0);
blasfeo_pack_dvec(nx, x, xn_out, 0);
blasfeo_dvecse(nx, 0.0, dxn, 0);
// start the loop
acados_tic(&timer);
for (ss = 0; ss < num_steps; ss++)
{
// initialize
blasfeo_dgese(nx * ns, nx * ns, 0.0, JGK, 0, 0);
blasfeo_dgese(nx * ns, nx + nu, 0.0, JGf, 0, 0);
// expansion step (K variables)
// compute x and u step
blasfeo_pack_dvec(nx, in->x, w, 0);
blasfeo_pack_dvec(nu, in->u, w, nx);
blasfeo_daxpy(nx, -1.0, memory->x, 0, w, 0, w, 0);
blasfeo_daxpy(nu, -1.0, memory->u, 0, w, nx, w, nx);
blasfeo_dgemv_n(nx * ns, nx + nu, 1.0, &JKf[ss], 0, 0, w, 0, 1.0, &K[ss], 0, &K[ss], 0);
blasfeo_pack_dvec(nx, in->x, memory->x, 0);
blasfeo_pack_dvec(nu, in->u, memory->u, 0);
// reset value of JKf
blasfeo_dgese(nx * ns, nx + nu, 0.0, &JKf[ss], 0, 0);
for (ii = 0; ii < ns; ii++) // ii-th row of tableau
{
// take x(n); copy a strvec into a strvec
blasfeo_dveccp(nx, xn, 0, xt, 0);
for (jj = 0; jj < ns; jj++)
{ // jj-th col of tableau
a = A_mat[ii + ns * jj];
if (a != 0)
{
// xt = xt + T_int * a[i,j]*K_j
a *= step;
blasfeo_daxpy(nx, a, &K[ss], jj * nx, xt, 0, xt, 0);
}
}
if (!update_sens)
{
// compute the residual of implicit ode at time t_ii, store value in rGt
acados_tic(&timer_ad);
ext_fun_type_in[0] = BLASFEO_DVEC;
ext_fun_in[0] = xt; // x: nx
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in_K.xi = ii * nx;
ext_fun_in_K.x = &K[ss];
ext_fun_in[1] = &ext_fun_in_K; // K[ii*nx]: nx
ext_fun_type_in[2] = COLMAJ;
ext_fun_in[2] = u; // u: nu
ext_fun_type_out[0] = BLASFEO_DVEC_ARGS;
ext_fun_out_rG.x = rG;
ext_fun_out_rG.xi = ii * nx;
ext_fun_out[0] = &ext_fun_out_rG; // fun: nx
model->impl_ode_fun->evaluate(model->impl_ode_fun, ext_fun_type_in, ext_fun_in,
ext_fun_type_out, ext_fun_out);
timing_ad += acados_toc(&timer_ad);
}
else
{
// compute the jacobian of implicit ode
acados_tic(&timer_ad);
ext_fun_type_in[0] = BLASFEO_DVEC;
ext_fun_in[0] = xt; // x: nx
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in_K.xi = ii * nx; // K[ii*nx]: nx
ext_fun_in_K.x = &K[ss];
ext_fun_in[1] = &ext_fun_in_K;
ext_fun_type_in[2] = COLMAJ;
ext_fun_in[2] = u; // u: nu
ext_fun_type_out[0] = BLASFEO_DVEC_ARGS;
ext_fun_out_rG.x = rG;
ext_fun_out_rG.xi = ii * nx;
ext_fun_out[0] = &ext_fun_out_rG; // fun: nx
ext_fun_type_out[1] = BLASFEO_DMAT;
ext_fun_out[1] = J_temp_x; // jac_x: nx*nx
ext_fun_type_out[2] = BLASFEO_DMAT;
ext_fun_out[2] = J_temp_xdot; // jac_xdot: nx*nx
ext_fun_type_out[3] = BLASFEO_DMAT;
ext_fun_out[3] = J_temp_u; // jac_u: nx*nu
model->impl_ode_fun_jac_x_xdot_u->evaluate(model->impl_ode_fun_jac_x_xdot_u,
ext_fun_type_in, ext_fun_in,
ext_fun_type_out, ext_fun_out);
timing_ad += acados_toc(&timer_ad);
blasfeo_dgecp(nx, nx, J_temp_x, 0, 0, JGf, ii * nx, 0);
blasfeo_dgecp(nx, nu, J_temp_u, 0, 0, JGf, ii * nx, nx);
for (jj = 0; jj < ns; jj++)
{
// compute the block (ii,jj)th block of JGK
a = A_mat[ii + ns * jj];
if (a != 0)
{
a *= step;
blasfeo_dgead(nx, nx, a, J_temp_x, 0, 0, JGK, ii * nx, jj * nx);
}
if (jj == ii)
{
blasfeo_dgead(nx, nx, 1, J_temp_xdot, 0, 0, JGK, ii * nx, jj * nx);
}
} // end jj
}
} // end ii
// obtain x(n+1) before updating K(n)
for (ii = 0; ii < ns; ii++)
blasfeo_daxpy(nx, step * b_vec[ii], &K[ss], ii * nx, xn, 0, xn, 0);
// DGETRF computes an LU factorization of a general M-by-N matrix A
// using partial pivoting with row interchanges.
if (update_sens)
{
blasfeo_dgetrf_rp(nx * ns, nx * ns, JGK, 0, 0, JGK, 0, 0, ipiv);
}
// update r.h.s (6.23, Quirynen2017)
blasfeo_dgemv_n(nx * ns, nx, 1.0, JGf, 0, 0, dxn, 0, 1.0, rG, 0, rG, 0);
// permute also the r.h.s
blasfeo_dvecpe(nx * ns, ipiv, rG, 0);
// solve JGK * y = rG, JGK on the (l)eft, (l)ower-trian, (n)o-trans
// (u)nit trian
blasfeo_dtrsv_lnu(nx * ns, JGK, 0, 0, rG, 0, rG, 0);
// solve JGK * x = rG, JGK on the (l)eft, (u)pper-trian, (n)o-trans
// (n)o unit trian , and store x in rG
blasfeo_dtrsv_unn(nx * ns, JGK, 0, 0, rG, 0, rG, 0);
// scale and add a generic strmat into a generic strmat // K = K - rG, where rG is DeltaK
blasfeo_daxpy(nx * ns, -1.0, rG, 0, &K[ss], 0, &K[ss], 0);
// obtain dx(n)
for (ii = 0; ii < ns; ii++)
blasfeo_daxpy(nx, -step * b_vec[ii], rG, ii * nx, dxn, 0, dxn, 0);
// update JKf
blasfeo_dgemm_nn(nx * ns, nx + nu, nx, 1.0, JGf, 0, 0, S_forw, 0, 0, 0.0, &JKf[ss], 0, 0,
&JKf[ss], 0, 0);
blasfeo_dgead(nx * ns, nu, 1.0, JGf, 0, nx, &JKf[ss], 0, nx);
blasfeo_drowpe(nx * ns, ipiv, &JKf[ss]);
blasfeo_dtrsm_llnu(nx * ns, nx + nu, 1.0, JGK, 0, 0, &JKf[ss], 0, 0, &JKf[ss], 0, 0);
blasfeo_dtrsm_lunn(nx * ns, nx + nu, 1.0, JGK, 0, 0, &JKf[ss], 0, 0, &JKf[ss], 0, 0);
// update forward sensitivity
for (jj = 0; jj < ns; jj++)
blasfeo_dgead(nx, nx + nu, -step * b_vec[jj], &JKf[ss], jj * nx, 0, S_forw, 0, 0);
// obtain x(n+1)
for (ii = 0; ii < ns; ii++)
blasfeo_daxpy(nx, step * b_vec[ii], &K[ss], ii * nx, xn_out, 0, xn_out, 0);
} // end int step ss
// extract output
blasfeo_unpack_dvec(nx, xn_out, 0, x_out);
blasfeo_unpack_dmat(nx, nx + nu, S_forw, 0, 0, S_forw_out, nx);
out->info->CPUtime = acados_toc(&timer);
out->info->LAtime = 0.0;
out->info->ADtime = timing_ad;
return 0;
}
