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; }