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;
}
Beispiel #2
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;
}
Beispiel #3
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;
}
Beispiel #4
0
// the gateway function 
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
	{
		
	// get data 
	int k_max;
	double mu0, tol, *A, *B, *b, *Q, *Qf, *R, *S, *q, *qf, *r, *x, *u, *lb, *ub, *C, *D, *lg, *ug, *CN, *lgN, *ugN, *stat, *kkk, *inf_norm_res, *pi, *lam, *t;
	
	kkk   = mxGetPr(prhs[0]);
	k_max = (int) mxGetScalar(prhs[1]);
	mu0   = mxGetScalar(prhs[2]);
	tol   = mxGetScalar(prhs[3]);
	const int N   = (int) mxGetScalar(prhs[4]);
	const int nx  = (int) mxGetScalar(prhs[5]);
	const int nu  = (int) mxGetScalar(prhs[6]);
	const int nb  = (int) mxGetScalar(prhs[7]);
	const int ng  = (int) mxGetScalar(prhs[8]);
	const int ngN = (int) mxGetScalar(prhs[9]);
	const int time_invariant = (int) mxGetScalar(prhs[10]);
	const int free_x0 = (int) mxGetScalar(prhs[11]);
	const int warm_start = (int) mxGetScalar(prhs[12]);

	A    = mxGetPr(prhs[13]);
	B    = mxGetPr(prhs[14]);
	b    = mxGetPr(prhs[15]);
	Q    = mxGetPr(prhs[16]);
	Qf   = mxGetPr(prhs[17]);
	R    = mxGetPr(prhs[18]);
	S    = mxGetPr(prhs[19]);
	q    = mxGetPr(prhs[20]);
	qf   = mxGetPr(prhs[21]);
	r    = mxGetPr(prhs[22]);
	lb   = mxGetPr(prhs[23]);
	ub   = mxGetPr(prhs[24]);
	C    = mxGetPr(prhs[25]);
	D    = mxGetPr(prhs[26]);
	lg   = mxGetPr(prhs[27]);
	ug   = mxGetPr(prhs[28]);
	CN   = mxGetPr(prhs[29]);
	lgN  = mxGetPr(prhs[30]);
	ugN  = mxGetPr(prhs[31]);
	x    = mxGetPr(prhs[32]);
	u    = mxGetPr(prhs[33]);
	pi  = mxGetPr(prhs[34]);
	lam = mxGetPr(prhs[35]);
	t   = mxGetPr(prhs[36]);
	inf_norm_res = mxGetPr(prhs[37]);
	stat = mxGetPr(prhs[38]);
	
	int kk = -1;



	// 
	int ii;



	int nb0 = nb<nu ? nb : nu;
	int nbN = nb-nu>0 ? nb-nu : 0;



	int nx_v[N+1];
	nx_v[0] = 0;
	for(ii=1; ii<=N; ii++)
		nx_v[ii] = nx;

	int nu_v[N+1];
	for(ii=0; ii<N; ii++)
		nu_v[ii] = nu;
	nu_v[N] = 0;

	int nb_v[N+1];
	nb_v[0] = nb0;
	for(ii=1; ii<N; ii++)
		nb_v[ii] = nb;
	nb_v[N] = nbN;

	int ng_v[N+1];
	for(ii=0; ii<N; ii++)
		ng_v[ii] = ng;
	ng_v[N] = ngN;



	double b0[nx];
	dgemv_n_3l(nx, nx, 1.0, A, nx, x, 1.0, b, b0);

	double r0[nu];
	dgemv_n_3l(nu, nx, 1.0, S, nu, x, 1.0, r, r0);

	double lb0[nb0];
	for(ii=0; ii<nb0; ii++)
		lb0[ii] = lb[ii];

	double ub0[nb0];
	for(ii=0; ii<nb0; ii++)
		ub0[ii] = ub[ii];

	double lbN[nbN];
	double ubN[nbN];

	double lg0[ng];
	dgemv_n_3l(ng, nx, -1.0, C, ng, x, 1.0, lg, lg0);

	double ug0[ng];
	dgemv_n_3l(ng, nx, -1.0, C, ng, x, 1.0, ug, ug0);



	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];
	double *hC[N+1];
	double *hD[N];
	double *hlg[N+1];
	double *hug[N+1];
	double *hx[N+1];
	double *hu[N+1];
	double *hpi[N];
	double *hlam[N+1];
	double *ht[N+1];



	if(time_invariant)
		{

		for(ii=1; ii<N; ii++)
			hA[ii] = A;

		for(ii=0; ii<N; ii++)
			hB[ii] = B;

		hb[0] = b0;
		for(ii=1; ii<N; ii++)
			hb[ii] = b;

		for(ii=1; ii<N; ii++)
			hQ[ii] = Q;
		hQ[N] = Qf;

		for(ii=1; ii<N; ii++)
			hS[ii] = S;

		for(ii=0; ii<N; ii++)
			hR[ii] = R;

		for(ii=1; ii<N; ii++)
			hq[ii] = q;
		hq[N] = qf;
		
		hr[0] = r0;
		for(ii=1; ii<N; ii++)
			hr[ii] = r;

		for(ii=0; ii<nbN; ii++)
			lbN[ii] = lb[nu+ii];

		hlb[0] = lb0;
		for(ii=1; ii<N; ii++)
			hlb[ii] = lb;
		hlb[N] = lbN;

		for(ii=0; ii<nbN; ii++)
			ubN[ii] = ub[nu+ii];

		hub[0] = ub0;
		for(ii=1; ii<N; ii++)
			hub[ii] = ub;
		hub[N] = ubN;

		for(ii=1; ii<N; ii++)
			hC[ii] = C;
		hC[N] = CN;

		for(ii=0; ii<N; ii++)
			hD[ii] = D;

		hlg[0] = lg0;
		for(ii=0; ii<N; ii++)
			hlg[ii] = lg;
		hlg[N] = lgN;

		hug[0] = ug0;
		for(ii=0; ii<N; ii++)
			hug[ii] = ug;
		hug[N] = ugN;

		}
	else
		{

		for(ii=1; ii<N; ii++)
			hA[ii] = A+ii*nx*nx;

		for(ii=0; ii<N; ii++)
			hB[ii] = B+ii*nx*nu;

		hb[0] = b0;
		for(ii=1; ii<N; ii++)
			hb[ii] = b+ii*nx;

		for(ii=1; ii<N; ii++)
			hQ[ii] = Q+ii*nx*nx;
		hQ[N] = Qf;

		for(ii=1; ii<N; ii++)
			hS[ii] = S+ii*nu*nx;

		for(ii=0; ii<N; ii++)
			hR[ii] = R+ii*nu*nu;

		for(ii=1; ii<N; ii++)
			hq[ii] = q+ii*nx;
		hq[N] = qf;
		
		hr[0] = r0;
		for(ii=1; ii<N; ii++)
			hr[ii] = r+ii*nu;

		for(ii=0; ii<nbN; ii++)
			lbN[ii] = lb[nu+ii];
		hlb[0] = lb0;
		for(ii=1; ii<N; ii++)
			hlb[ii] = lb+ii*nb;
		hlb[N] = lbN;

		for(ii=0; ii<nbN; ii++)
			ubN[ii] = ub[nu+ii];
		hub[0] = ub0;
		for(ii=1; ii<N; ii++)
			hub[ii] = ub+ii*nb;
		hub[N] = ubN;

		for(ii=1; ii<N; ii++)
			hC[ii] = C+ii*ng*nx;
		hC[N] = CN;

		for(ii=0; ii<N; ii++)
			hD[ii] = D+ii*ng*nu;

		hlg[0] = lg0;
		for(ii=0; ii<N; ii++)
			hlg[ii] = lg+ii*ng;
		hlg[N] = lgN;

		hug[0] = ug0;
		for(ii=0; ii<N; ii++)
			hug[ii] = ug+ii*ng;
		hug[N] = ugN;

		}
	
	for(ii=0; ii<=N; ii++)
		hx[ii] = x+ii*nx;

	for(ii=0; ii<N; ii++)
		hu[ii] = u+ii*nu;
	
	for(ii=0; ii<N; ii++)
		hpi[ii] = pi+ii*nx;
	
	for(ii=0; ii<=N; ii++)
		hlam[ii] = lam+ii*(2*nb+2*ng);

	for(ii=0; ii<=N; ii++)
		ht[ii] = t+ii*(2*nb+2*ng);



	int work_space_size = hpmpc_d_ip_ocp_hard_tv_work_space_size_bytes(N, nx_v, nu_v, nb_v, ng_v);
	void *work = malloc( work_space_size );



	// call solver 
	fortran_order_d_ip_ocp_hard_tv(&kk, k_max, mu0, tol, N, nx_v, nu_v, nb_v, ng_v, warm_start, hA, hB, hb, hQ, hS, hR, hq, hr, hlb, hub, hC, hD, hlg, hug, hx, hu, hpi, hlam, ht, inf_norm_res, work, stat);


	*kkk = (double) kk;


	free(work);


	return;

	}