qpOASESsolver.cpp

#include <iostream>

#include "qpOASESsolver.h"


qpOASESsolver::qpOASESsolver(int numVariable, int numConstraint, int numWorkingsetComputation)
{
    /** Initialize class variables */
    _numVariable                = numVariable;
    _numIneqConstraint          = numConstraint;
    _numWorkingsetComputation   = (int_t) numWorkingsetComputation;
    _qpOASESInitialized         = false;
}

qpOASESsolver::qpOASESsolver()
{
    /** Initialize class variables */
    _numVariable                = 0;
    _numIneqConstraint          = 0;
    _numWorkingsetComputation   = 100;
    _qpOASESInitialized         = false;
}

qpOASESsolver::~qpOASESsolver()
{
    _qpOASESInitialized = false;
}

bool qpOASESsolver::initProblem()
{
    /** Check the problem has been initialized */
    if (!_numVariable || !_numIneqConstraint)
    {
        std::cout << "Number of variable/constraint not initialized" << std::endl;
        return false;
    }

    /** Create a qpOASES problem */
    _problem = SQProblem(_numVariable, _numIneqConstraint);
    _qpOASESInitialized = true;

    /** Initialize the qpOASES problem */
    const size_t hessian_size       = _numVariable*_numVariable;
    const size_t gradient_size      = _numVariable;
    const size_t A_size             = _numIneqConstraint*_numVariable;
    const size_t B_size             = _numIneqConstraint;
    const size_t lowerBound_size    = _numVariable;
    const size_t upperBound_size    = _numVariable;
    real_t hessian[hessian_size], gradient[gradient_size], A[A_size], B[B_size],
           lowerBound[lowerBound_size], upperBound[upperBound_size];
    int_t nWSR = _numWorkingsetComputation;

    // Generic initialization
    for (int i=0; i<hessian_size; i++)
        hessian[i] = 1.0;
    for (int i=0; i<gradient_size; i++)
        gradient[i] = 1.0;
    for (int i=0; i<A_size; i++)
        A[i] = 0.0;
    for (int i=0; i<B_size; i++)
        B[i] = 0.0;
    for (int i=0; i<lowerBound_size; i++)
        lowerBound[i] = -100.0;
    for (int i=0; i<upperBound_size; i++)
        upperBound[i] = 100.0;

    returnValue res = _problem.init(hessian, gradient, A, lowerBound, upperBound, NULL, B, nWSR, 0);

    switch (res)
    {
    case SUCCESSFUL_RETURN:
        // do nothing
        break;

    case RET_MAX_NWSR_REACHED:
        std::cout << "Maximum number of working set recalculations reached" << std::endl;
        return false;
        break;

    case RET_INIT_FAILED:
        std::cout << "qpOASES initialization failed" << std::endl;
        return false;
        break;

    default:
        std::cout << "Unknown error code generated by qpOASES initialization" << std::endl;
        return false;
    }

    return true;
}

bool qpOASESsolver::setProblem(const Ref<const VectorXd> lowerBound, const Ref<const VectorXd> upperBound, const Ref<const MatrixXd> hessian,
                               const Ref<const VectorXd> gradient, const Ref<const MatrixXd> A, const Ref<const VectorXd> B)
// Cost function min 0.5*xT*H*x+xT*f
// Constraints Ax <= B
{
    /** Check that the problem has been initialized */
    if (!_qpOASESInitialized)
    {
        std::cout << "Call initProblem before setProblem" << std::endl;
        return false;
    }

    /** Convert matrices in row-major vectors */
    const double* lowerBound_ = lowerBound.data();
    const double* upperBound_ = upperBound.data();
    const double* gradient_   = gradient.data();
    const double* B_          = B.data();

    MatrixXd At            = A.transpose();
    const double *A_       = At.data();
    MatrixXd hessiant      = hessian.transpose();
    const double *hessian_ = hessiant.data();

    /** Set and solve the QP problem */
    int_t nWSR = _numWorkingsetComputation;
    returnValue res = _problem.hotstart(hessian_, gradient_, A_, lowerBound_, upperBound_, NULL, B_, nWSR, 0);

    switch (res)
    {
    case SUCCESSFUL_RETURN:
        // do nothing
        break;

    case RET_MAX_NWSR_REACHED:
        std::cout << "Maximum number of working set recalculations reached" << std::endl;
        return false;
        break;

    case RET_HOTSTART_FAILED:
        std::cout << "qpOASES hot restart failed" << std::endl;
        return false;
        break;

    default:
        std::cout << "Unknown error code generated by qpOASES initialization" << std::endl;
        return false;
    }

    return true;
}

bool qpOASESsolver::solveProblem(Ref<VectorXd> result, int& optimizerStatus)
{
    /** Check that the problem has been initialized */
    if (!_qpOASESInitialized)
    {
        std::cout << "Call initProblem before solveProblem" << std::endl;
        return false;
    }

    /** Get a solution */
    const size_t solution_size = _numVariable;
    real_t solution[solution_size];

    returnValue res = _problem.getPrimalSolution(solution);

    switch (res)
    {
    case SUCCESSFUL_RETURN:
        for (int i=0; i<_numVariable; i++)
            result[i] = solution[i];
        optimizerStatus = 1;
        break;

    case RET_QP_NOT_SOLVED:
        std::cout << "QP problem has not been solved" << std::endl;
        optimizerStatus = 0;
        return false;
        break;

    default:
        std::cout << "Unknown error code generated by qpOASES initialization" << std::endl;
        optimizerStatus = 0;
        return false;
    }

    return true;
}

void qpOASESsolver::set_printLevel(printLevelType printLevel)
{
    /** Check that the problem has been initialized */
    if (!_qpOASESInitialized)
    {
        std::cout << "Call initProblem before set_printLevel" << std::endl;
        return;
    }

    switch (printLevel)
    {
    case NONE:
        _problem.setPrintLevel(PL_NONE);
        break;

    case LOW:
        _problem.setPrintLevel(PL_LOW);
        break;

    case MEDIUM:
        _problem.setPrintLevel(PL_MEDIUM);
        break;

    case HIGH:
        _problem.setPrintLevel(PL_HIGH);
        break;

    default:
        _problem.setPrintLevel(PL_NONE);
    }
}