qp_as.cpp

Go to the documentation of this file.

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

#include <cmath> //cos,sin


using namespace AS;

/****************************************
 * FUNCTIONS
 ****************************************/

qp_as::qp_as(
        const int N_, 
        const double gain_position, 
        const double gain_velocity, 
        const double gain_acceleration, 
        const double gain_jerk, 
        const double tol_,
        const bool obj_computation_on_,
        const unsigned int max_added_constraints_num_,
        const bool constraint_removal_on_) : 
    problem_parameters (N_, gain_position, gain_velocity, gain_acceleration, gain_jerk),
    chol (N_)
{
    dX = new double[SMPC_NUM_VAR*N]();

    constraints.resize(2*N);

    tol = tol_,
    obj_computation_on = obj_computation_on_;
    constraint_removal_on = constraint_removal_on_;

    max_added_constraints_num = max_added_constraints_num_;
    if (max_added_constraints_num == 0)
    {
        max_added_constraints_num = N*2;
    }
}


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


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)
{
    set_state_parameters (T_, h_, h_initial_);

    zref_x = zref_x_;
    zref_y = zref_y_;

    active_set.clear();

    added_constraints_num = 0;
    removed_constraints_num = 0;


    // form inv(2*H) *g and initialize constraints
    // inv(2*H) * g  =  inv (2*(beta/2)) * beta * Cp' * zref  =  zref
    for (int i = 0, cind = 0; i < N; ++i)
    {
        double cosR = cos(angle[i]);
        double sinR = sin(angle[i]);
        double RTzref_x = (cosR*zref_x[i] + sinR*zref_y[i]);
        double RTzref_y = (-sinR*zref_x[i] + cosR*zref_y[i]);

        constraints[cind].set(
                cind, cosR, sinR, 
                lb[cind] - RTzref_x, 
                ub[cind] - RTzref_x, 
                false);
        ++cind;

        constraints[cind].set(
                cind, -sinR, cosR, 
                lb[cind] - RTzref_y, 
                ub[cind] - RTzref_y, 
                false);
        ++cind;
    }
}



void qp_as::form_init_fp (
        const double *x_coord, 
        const double *y_coord, 
        const double *init_state,
        const bool tilde_state,
        double* X_)
{
    X = X_;
    form_init_fp_tilde<problem_parameters>(*this, x_coord, y_coord, init_state, tilde_state, X);
}



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

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


    for (unsigned int i = 0; i < constraints.size(); ++i)
    {
        // Check only inactive constraints for violation. 
        // The constraints in the working set will not be violated regardless of 
        // the depth of descent
        if (!constraints[i].isActive)
        {
            const constraint c = constraints[i];

            const double constr = X[c.ind]*c.coef_x + X[c.ind+3]*c.coef_y;
            const double d_constr = dX[c.ind]*c.coef_x + dX[c.ind+3]*c.coef_y;

            if ( d_constr < -tol )
            {
                const double t = (c.lb - constr)/d_constr;
                if (t < alpha)
                {
                    alpha = t;
                    activated_var_num = i;
                    sign = -1;
                }
            }
            else if ( d_constr > tol )
            {
                const double t = (c.ub - constr)/d_constr;
                if (t < alpha)
                {
                    alpha = t;
                    activated_var_num = i;
                    sign = 1;
                }
            }
        }
    }
    if (activated_var_num != -1)
    {
        constraints[activated_var_num].sign = sign;
        constraints[activated_var_num].isActive = true;
        active_set.push_back(constraints[activated_var_num]);
    }

    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 (unsigned int i = 0; i < active_set.size(); ++i)
    {
        if (lambda[i] * active_set[i].sign < min_lambda)
        {
            min_lambda = lambda[i] * active_set[i].sign;
            ind_exclude = i;
        }
    }

    if (ind_exclude != -1)
    {
        constraints[active_set[ind_exclude].cind].isActive = false;
        active_set.erase(active_set.begin()+ind_exclude);
    }

    return (ind_exclude);
}


void qp_as::solve (vector<double> &obj_log)
{
    for (int i = 0; i < N; ++i)
    {
        const int ind = i*SMPC_NUM_STATE_VAR;
        X[ind]   -= zref_x[i];
        X[ind+3] -= zref_y[i];
    }
    if (obj_computation_on)
    {
        obj_log.clear();
        obj_log.push_back(compute_obj());
    }

    // obtain dX
    chol.solve(*this, X, dX);

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

        // Move in the feasible descent direction
        for (int i = 0; i < N*SMPC_NUM_VAR; i += SMPC_NUM_VAR)
        {
            X[i]   += alpha * dX[i];
            X[i+1] += alpha * dX[i+1];
            X[i+2] += alpha * dX[i+2];
            X[i+3] += alpha * dX[i+3];
            X[i+4] += alpha * dX[i+4];
            X[i+5] += alpha * dX[i+5];
            X[i+6] += alpha * dX[i+6];
            X[i+7] += alpha * dX[i+7];
        }

        if (obj_computation_on)
        {
            obj_log.push_back(compute_obj());
        }

        if (activated_var_num != -1)
        {
            ++added_constraints_num;
            if (added_constraints_num == max_added_constraints_num)
            {
                break;
            }

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

            chol.down_resolve (*this, active_set, ind_exclude, X, dX);
            ++removed_constraints_num;
        }
        else
        {
            break;
        }
    }

    for (int i = 0; i < N; ++i)
    {
        const int ind = i*SMPC_NUM_STATE_VAR;
        X[ind]   += zref_x[i];
        X[ind+3] += zref_y[i];
    }

    active_set_size = active_set.size();
}



double qp_as::compute_obj()
{
    int i,j;
    double obj_pos = 0;
    double obj_vel = 0;
    double obj_acc = 0;
    double obj_jerk = 0;

    // phi_X = X'*H*X + g'*X
    for(i = 0, j = 0;
        i < N*SMPC_NUM_STATE_VAR;
        i += SMPC_NUM_STATE_VAR, j += 2)
    {
        const double X_copy[6] = {X[i], X[i+1], X[i+2], X[i+3], X[i+4], X[i+5]};

        // X'*H*X
        obj_pos += X_copy[0]*X_copy[0] + X_copy[3]*X_copy[3];
        obj_vel += X_copy[1]*X_copy[1] + X_copy[4]*X_copy[4];
        obj_acc += X_copy[2]*X_copy[2] + X_copy[5]*X_copy[5];
    }
    for (; i < N*SMPC_NUM_VAR; i += SMPC_NUM_CONTROL_VAR)
    {
        // X'*H*X
        obj_jerk += X[i] * X[i] + X[i+1] * X[i+1];
    }

    return (0.5*(obj_pos/i2Q[0] + obj_vel/i2Q[1] + obj_acc/i2Q[2] + obj_jerk/i2P));
}