solver/qp_as.cpp

Go to the documentation of this file.

/****************************************
 * INCLUDES 
 ****************************************/
#include "qp_as.h"
#include "state_handling.h"


/****************************************
 * FUNCTIONS
 ****************************************/
//==============================================
// bound
void bound::set(const int var_num_, const double lb_, const double ub_, const bool active)
{
    var_num = var_num_;
    lb = lb_;
    ub = ub_;
    isActive = active;
}



//==============================================
// qp_as

qp_as::qp_as(
        const int N_, 
        const double Alpha, 
        const double Beta, 
        const double Gamma, 
        const double regularization, 
        const double tol_) : 
    qp_solver (N_, Alpha, Beta, Gamma, regularization, tol_),
    chol (N_)
{
    iHg = new double[2*N];

    // there are no inequality constraints in the initial working set (no hot-starting for the moment)
    W = new int[2*N];
    W_sign = new int[2*N];

    Bounds.resize(2*N);
}


qp_as::~qp_as()
{
    if (W  != NULL)
        delete W;

    if (iHg != NULL)
        delete iHg;

    if (W_sign != NULL)
        delete W_sign;
}


void qp_as::set_parameters(
        const double* T_, 
        const double* h_, 
        const double h_initial_,
        const double* angle,
        const double* zref_x,
        const double* zref_y,
        const double* lb,
        const double* ub)
{
    nW = 0;

    h_initial = h_initial_;
    set_state_parameters (T_, h_, h_initial_, angle);
    form_iHg (zref_x, zref_y);
    form_bounds(lb, ub);
}



void qp_as::form_iHg(const double *zref_x, const double *zref_y)
{
    double p0, p1;
    double cosA, sinA;

    for (int i = 0; i < N; i++)
    {
        cosA = spar[i].cos;
        sinA = spar[i].sin;

        // zref
        p0 = zref_x[i];
        p1 = zref_y[i];

        // inv (2*H) * R' * Cp' * zref
        iHg[i*2] =     -i2Q[0] * (cosA*p0 + sinA*p1)*gain_beta;
        iHg[i*2 + 1] = -i2Q[0] * (-sinA*p0 + cosA*p1)*gain_beta; 
    }
}



void qp_as::form_bounds(const double *lb, const double *ub)
{
    for (int i=0; i < N; i++)
    {
        Bounds[i*2].set(SMPC_NUM_STATE_VAR*i, lb[i*2], ub[i*2], false);
        Bounds[i*2+1].set(SMPC_NUM_STATE_VAR*i+3, lb[i*2+1], ub[i*2+1], false);
    }
}



int qp_as::check_blocking_bounds()
{
    alpha = 1;

    /* Index to include in the working set, -1 if no constraint have to be included. */
    int activated_var_num = -1;


    for (int i = 0; i < 2*N; i++)
    {
        // Check only inactive constraints for violation. 
        // The constraints in the working set will not be violated regardless of 
        // the depth of descent
        if (!Bounds[i].isActive)
        {
            int ind = Bounds[i].var_num;
            int sign = 1;
            double t = 1;

            if ( dX[ind] < -tol )
            {
                t = (Bounds[i].lb - X[ind])/dX[ind];
                sign = -1;
            }
            else if ( dX[ind] > tol ) 
            {
                t = (Bounds[i].ub - X[ind])/dX[ind];
            }

            if (t < alpha)
            {
                alpha = t;
                activated_var_num = i;
                W_sign[nW] = sign;
            }
        }
    }
    if (activated_var_num != -1)
    {
        W[nW] = activated_var_num;    
        nW++;
        Bounds[activated_var_num].isActive = true;
    }

    return (activated_var_num);
}  


int qp_as::choose_excl_constr (const double *lambda)
{
    double min_lambda = -tol;
    int ind_exclude = -1;

    // find the constraint with the smallest lambda
    for (int i = 0; i < nW; i++)
    {
        if (lambda[i] * W_sign[i] < min_lambda)
        {
            min_lambda = lambda[i] * W_sign[i];
            ind_exclude = i;
        }
    }

    if (ind_exclude != -1)
    {
        Bounds[W[ind_exclude]].isActive = false;
        for (int i = ind_exclude; i < nW-1; i++)
        {
            W[i] = W[i + 1];
            W_sign[i] = W_sign[i + 1];
        }
        nW--;
    }

    return (ind_exclude);
}


int qp_as::solve ()
{
    // obtain dX
    chol.solve(*this, iHg, X, dX);

    for (;;)
    {
        int activated_var_num = check_blocking_bounds();

        // Move in the feasible descent direction
        for (int i = 0; i < N*SMPC_NUM_VAR ; i++)
        {
            X[i] += alpha * dX[i];
        }

        // no new inequality constraints
        if (activated_var_num == -1)
        {
            int ind_exclude = choose_excl_constr (chol.get_lambda(*this));
            if (ind_exclude != -1)
            {
                chol.down_resolve (*this, iHg, nW, W, ind_exclude, X, dX);
            }
            else
            {
                break;
            }
        }
        else
        {
            // add row to the L matrix and find new dX
            chol.up_resolve (*this, iHg, nW, W, X, dX);
        }
    }

    return (nW);
}