solver/qp_ip.cpp

Go to the documentation of this file.

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


#include <cmath> // log, fabs

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


//==============================================
// qp_ip

qp_ip::qp_ip(
        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_)
{
    g = new double[2*N];
    i2hess = new double[2*N];
    i2hess_grad = new double[N*SMPC_NUM_VAR];
    grad = new double[N*SMPC_NUM_VAR];

    Q[0] = Beta/2;
    Q[1] = Alpha/2;
    Q[2] = regularization;
    P = Gamma/2;
}


qp_ip::~qp_ip()
{
    if (g != NULL)
        delete g;
    if (i2hess != NULL)
        delete i2hess;
    if (i2hess_grad != NULL)
        delete i2hess_grad;
    if (grad != NULL)
        delete grad;
}


void qp_ip::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_)
{
    h_initial = h_initial_;
    set_state_parameters (T, h, h_initial_, angle);

    lb = lb_;
    ub = ub_;

    form_g (zref_x, zref_y);
}



void qp_ip::form_g (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
        g[i*2] = -(cosA*p0 + sinA*p1)*gain_beta;
        g[i*2 + 1] = -(-sinA*p0 + cosA*p1)*gain_beta; 
    }
}



void qp_ip::form_grad_i2hess_logbar (const double kappa)
{
    phi_X = 0;
    int i,j;


    // grad = H*X + g + kappa * b;
    // initialize grad = H*X
    for (i = 0; i < N*SMPC_NUM_STATE_VAR; i++)
    {
        grad[i] = X[i] / i2Q[i%3];
    }
    for (; i < N*SMPC_NUM_VAR; i++)
    {
        grad[i] = X[i] / i2P;
    }

    // finish initalization of grad 
    // initialize inverted hessian
    // initialize logarithmic barrier in the function
    for (i = 0, j = 0; i < 2*N; i++, j += 3)
    {
        double lb_diff = -lb[i] + X[j];
        double ub_diff =  ub[i] - X[j];

        // logarithmic barrier
        phi_X -= log(lb_diff) + log(ub_diff);

        lb_diff = 1/lb_diff;
        ub_diff = 1/ub_diff;

        // grad += g + kappa * (ub_diff - lb_diff)
        grad[j] += g[i] + kappa * (ub_diff - lb_diff);

        // only elements 1:3:N*SMPC_NUM_STATE_VAR on the diagonal of hessian 
        // can change
        // hess = H + kappa * (ub_diff^2 - lb_diff^2)
        i2hess[i] = 1/(2*Q[0] + kappa * (ub_diff*ub_diff + lb_diff*lb_diff));
    }
    phi_X *= kappa;
}



void qp_ip::form_i2hess_grad ()
{
    int i,j;
    for (i = 0, j = 0; i < N*2; i++)
    {
        i2hess_grad[j] = - grad[j] * i2hess[i]; 
        j++;
        i2hess_grad[j] = - grad[j] * i2Q[1]; 
        j++;
        i2hess_grad[j] = - grad[j] * i2Q[2]; 
        j++;
    }
    for (i = N*SMPC_NUM_STATE_VAR; i < N*SMPC_NUM_VAR; i++)
    {
        i2hess_grad[i] = - grad[i] * i2P;
    }
}



void qp_ip::form_phi_X ()
{
    int i;

    // phi_X += X'*H*X
    for (i = 0; i < N*SMPC_NUM_STATE_VAR; i++)
    {
        phi_X += Q[i%3] * X[i] * X[i];
    }
    for (; i < N*SMPC_NUM_VAR; i++)
    {
        phi_X += P * X[i] * X[i];
    }

    // phi_X += g'*X
    for (i = 0; i < 2*N; i++)
    {
        phi_X += g[i] * X[i*3];
    }
}


void qp_ip::init_alpha()
{
    double min_alpha = 1;
    alpha = 1;

    for (int i = 0; i < 2*N; i++)
    {
        // lower bound may be violated
        if (dX[i*3] < 0)
        {
            double tmp_alpha = (lb[i]-X[i*3])/dX[i*3];
            if (tmp_alpha < min_alpha)
            {
                min_alpha = tmp_alpha;
            }
        }
        // upper bound may be violated
        else if (dX[i*3] > 0)
        {
            double tmp_alpha = (ub[i]-X[i*3])/dX[i*3];
            if (tmp_alpha < min_alpha)
            {
                min_alpha = tmp_alpha;
            }
        }
    }

    if (min_alpha > tol)
    {
        while (alpha > min_alpha)
        {
            alpha *= bs_beta;
        }
    }
    else
    {
        alpha = 0;
    }
}



double qp_ip::form_bs_alpha_grad_dX ()
{
    double res = 0;
    
    for (int i = 0; i < N*SMPC_NUM_VAR; i++)
    {
        res += grad[i]*dX[i];
    }

    return (res*bs_alpha);
}


double qp_ip::form_phi_X_tmp (const double kappa)
{
    int i,j;
    double res = 0;
    double X_tmp;


    // phi_X += X'*H*X
    for (i = 0,j = 0; i < 2*N; i++)
    {
        X_tmp = X[j] + alpha * dX[j];
        j++;

        // logarithmic barrier
        res -= kappa * (log(-lb[i] + X_tmp) + log(ub[i] - X_tmp));

        // phi_X += g'*X
        res += g[i] * X_tmp;


        // phi_X += X'*H*X // states
        res += Q[0] * X_tmp*X_tmp;

        X_tmp = X[j] + alpha * dX[j];
        j++;
        res += Q[1] * X_tmp*X_tmp;

        X_tmp = X[j] + alpha * dX[j];
        j++;
        res += Q[2] * X_tmp*X_tmp;
    }
    // phi_X += X'*H*X // controls
    for (i = N*SMPC_NUM_STATE_VAR; i < N*SMPC_NUM_VAR; i++)
    {
        X_tmp = X[i] + alpha * dX[i];
        res += P * X_tmp * X_tmp;
    }

    return (res);
}



void qp_ip::set_ip_parameters (
        const double t_, 
        const double mu_, 
        const double bs_alpha_, 
        const double bs_beta_, 
        const int max_iter_,
        const double tol_out_)
{
    t = t_;
    mu = mu_;
    bs_alpha = bs_alpha_;
    bs_beta = bs_beta_;
    max_iter = max_iter_;
    tol_out = tol_out_;
}



int qp_ip::solve()
{
    double kappa = 1/t;
    double duality_gap = 2*N*kappa;
    int i;


    while (duality_gap > tol_out)
    {
        for (i = 0; (i < max_iter) && solve_onestep(kappa); i++);
        if (i == max_iter)
        {
            return (-1);
        }

        kappa /= mu;
        duality_gap = 2*N*kappa;
    }
    return (0);
}


bool qp_ip::solve_onestep (const double kappa)
{
    int i,j;


    form_grad_i2hess_logbar (kappa);
    form_phi_X ();
    form_i2hess_grad ();

    chol.solve (*this, i2hess_grad, i2hess, X, dX);


    // stopping criterion (decrement)
    double decrement = 0;
    for (i = 0, j = 0; i < N*2; i++)
    {
        decrement += dX[j] * dX[j] / i2hess[i];
        j++;
        decrement += dX[j] * dX[j] / i2Q[1];
        j++;
        decrement += dX[j] * dX[j] / i2Q[2];
        j++;
    }
    for (i = N*SMPC_NUM_STATE_VAR; i < N*SMPC_NUM_VAR; i++)
    {
        decrement += dX[i] * dX[i] / i2P;
    }
    if (decrement < tol)
    {
        return (false);
    }


    init_alpha ();
    // stopping criterion (step size)
    if (alpha < tol)
    {
        return (false); // done
    }


    // backtracking search
    double bs_alpha_grad_dX = form_bs_alpha_grad_dX ();
    for (;;)
    {
        if (form_phi_X_tmp (kappa) <= phi_X + alpha * bs_alpha_grad_dX)
        {
            break;
        }

        alpha = bs_beta * alpha;

        // stopping criterion (step size)
        if (alpha < tol)
        {
            return (false); // done
        }
    }


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

    return (true);
}