OneLevelSolver.cpp

Go to the documentation of this file.

#include "ocra/optim/OneLevelSolver.h"

using namespace ocra;




OneLevelSolver::OneLevelSolver()
    : ocra::Solver()
    , ocra::NamedInstance("OneLevelSolver")
    , ni(0),ne(0)
{
}

OneLevelSolver::~OneLevelSolver()
{

}

void OneLevelSolver::printValuesAtSolution()
{
    std::cout<<"TODO: 'printValuesAtSolution' method not implemented yet!"<<std::endl;
}

// void OneLevelSolver::addObjective(ObjectiveType& obj)
// {
//     internalAddObjective(obj);
//     _objectives.push_back(&obj);
// }

// void OneLevelSolver::removeObjective(ObjectiveType& obj)
// {
//     internalRemoveObjective(obj);
//     _objectives.erase(std::find(_objectives.begin(), _objectives.end(), &obj));
// }

void OneLevelSolver::addConstraint(ocra::LinearConstraint& constraint)
{
    internalAddConstraint(constraint);
    if(constraint.isEquality())
        _equalityConstraints.push_back(&constraint);
    else
        _inequalityConstraints.push_back(&constraint);
}

void OneLevelSolver::removeConstraint(ocra::LinearConstraint& constraint)
{
    internalRemoveConstraint(constraint);
    if(constraint.isEquality())
        _equalityConstraints.erase(std::find(_equalityConstraints.begin(), _equalityConstraints.end(), &constraint));
    else
        _inequalityConstraints.erase(std::find(_inequalityConstraints.begin(), _inequalityConstraints.end(), &constraint));
}




void OneLevelSolver::prepareMatrices()
{
    Xsolution.resize(n());
}

void OneLevelSolver::doPrepare()
{
    // prepareRecorder.initializeTime();

    prepareMatrices();
    updateObjectiveEquations();
    updateConstraintEquations();

    // prepareRecorder.saveRelativeTime();
}

void OneLevelSolver::doConclude()
{

}


void OneLevelSolver::writePerformanceInStream(std::ostream& outstream, bool addCommaAtEnd)
{
    // prepareRecorder.writeInStream("solver_prepare", outstream, true);
    // solveRecorder.writeInStream("solver_solve", outstream, addCommaAtEnd);
}


void OneLevelSolver::reduceConstraints(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, Eigen::MatrixXd& Ar, Eigen::VectorXd& br, double tolerance)
{
    if (A.rows()>0)
    {
        Eigen::JacobiSVD<Eigen::MatrixXd> svdOfMat(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::VectorXd S  = svdOfMat.singularValues();
        Eigen::MatrixXd Ur = svdOfMat.matrixU();
        Eigen::MatrixXd Vr = svdOfMat.matrixV();

        int r;
        for (r = 0; r<S.size(); r++)
        {
            if (S[r] <= tolerance)
                break;
        }

        Eigen::VectorXd Sr = S.segment(0,r);
        Ur.conservativeResize(Eigen::NoChange, r);
        Vr.conservativeResize(Eigen::NoChange, r);

        Ar = Sr.asDiagonal() * Vr.transpose();
        br = Ur.transpose() * b;
    }
    else
    {
        Ar = A;
        br = b;
    }
}


//#include <sstream>

std::string OneLevelSolver::toString()
{
    std::stringstream ss;
    ss<<"-----------------------------------------------\n";
    ss<<"probvar:\n";
    getProblemVariable().printSubTree(3,ss);
    ss<<"C:\n"<<_C<<"\n";
    ss<<"d:\n"<<-_d.transpose()<<"\n";
    ss<<"\n";
    ss<<"A:\n"<<_Atotal<<"\n";
    ss<<"b:\n"<<-_btotal.transpose()<<"\n";
    ss<<"\n";
    ss<<"G:\n"<<_G<<"\n";
    ss<<"h:\n"<<-_h.transpose()<<"\n";

    return ss.str();

}




//
// Hierarchy Solver with QuadProg sub-Solver
//

OneLevelSolverWithQuadProg::OneLevelSolverWithQuadProg()
    : ocra::NamedInstance("One Level Solver with QuadProg++ subSolver")
    , OneLevelSolver()
{
    std::cout << "-- " << getName() << " --" << std::endl;
}

OneLevelSolverWithQuadProg::~OneLevelSolverWithQuadProg()
{

}

void OneLevelSolverWithQuadProg::updateObjectiveEquations()
{
//    _C = _Pbase;
    _C.setZero(n(), n());
    _d.setZero(n());

    for(auto& obj : _objectives)
    {

        auto& f     = obj->getFunction();
        double weight                   = obj->getWeight();

        const std::vector<int>& objMap  = findMapping(f.getVariable());

        ocra::utils::addCompressed2d(   f.getPi(), _C, objMap, weight);
        ocra::utils::addCompressedByRow(f.getqi(), _d, objMap, weight);

    }

}

void OneLevelSolverWithQuadProg::updateConstraintEquations()
{
    this->mutex.lock();
    // UPDATE EQUALITY CONSTRAINTS
    this->ne = 0;
    int initialSize = (int) _equalityConstraints.size();
    for (int i=0; i<_equalityConstraints.size(); ++i)
        ne += _equalityConstraints[i]->getDimension();

//     OCRA_WARNING("Going to update constraints with " << ne << " equalities and " << n() <<" variables");
    
    _A.resize(ne, n()); _A.setZero();
    _b.resize(ne); _b.setZero();
    
    int initialRows = ne;

    int idx = 0;
    for (int i=0; i<_equalityConstraints.size(); ++i)
    {
        // WARNING The size of _equalityConstraints may change after _A has been resized before this for-loop
        // if a new request for a contact task deactivation arrives. The ocra::Task class would update the size
        // of _equalityConstraints thus making this for-loop access add more rows to _A than originally intended, 
        // thus causing a memory corruption and crashing the server.
//         std::cout << "Going to access constraint: " << i << std::endl;
        ocra::LinearConstraint* cstr = _equalityConstraints[i];
        int dim = cstr->getDimension();
//         std::cout << "Dimension obtained: " << dim << std::endl;
        if (dim > 0)
        {
            if (_equalityConstraints.size() != initialSize) {
                OCRA_ERROR("The size of equality constraints changed after _A had been resized.");
                std::cout << "Initial number of equality constraints was: " << initialSize << std::endl;
                std::cout << "New size of equality constraints is: " << _equalityConstraints.size() << std::endl;
                std::cout << "Resizing A accordingly... " << std::endl;
                // First copy current A with the initial size into a temporary
                Eigen::MatrixXd Atmp(initialRows, n());
                Atmp = _A.block(0,0,initialRows, n());
                // Then get the new number of rows
                for (int i=0; i<_equalityConstraints.size(); ++i)
                    ne += _equalityConstraints[i]->getDimension();
                // Resize _A accordingly
                _A.resize(ne, n());
                // Copy the previous content
                _A.block(0,0,initialRows, n()) = Atmp;
                initialSize = _equalityConstraints.size();
            }
            
            Eigen::Block<Eigen::MatrixXd> _A_block = _A.block(idx, 0, dim, n());
            Eigen::VectorBlock<Eigen::VectorXd> _b_segment = _b.segment(idx, dim);

            Eigen::VectorXd v; // ?? is it useless ??
            ocra::utils::convert(*cstr, findMapping(cstr->getVariable()), ocra::CSTR_PLUS_EQUAL, _A_block, _b_segment, v);

            if (_equalityConstraints.size() != 1 && i > 0) {
//                 std::cout << "Equality constraint (" << i << ") is of size: " << dim << "x" << n() << std::endl;
//                 std::cout << "_A equality: \n " << _A_block << std::endl;
            }

            idx += dim;
        }

    }

    reduceConstraints(_A, _b, _Atotal, _btotal);
    this->mutex.unlock();



    // UPDATE INEQUALITY CONSTRAINTS
    this->ni = 0;
    for (int i=0; i<_inequalityConstraints.size(); ++i)
    {
        ni += _inequalityConstraints[i]->getDimension();
    }

    _G.setZero(ni, n());
    _h.setZero(ni);

    idx = 0;
    for (int i=0; i<_inequalityConstraints.size(); ++i)
    {
        ocra::LinearConstraint* cstr = _inequalityConstraints[i];
        int dim = cstr->getDimension();

        if (dim > 0)
        {
            Eigen::Block<Eigen::MatrixXd > _G_block = _G.block(idx, 0, dim, n());
            Eigen::VectorBlock<Eigen::VectorXd> _h_segment = _h.segment(idx, dim);
            Eigen::VectorXd v;  // ?? is it useless ??
            ocra::utils::convert(*cstr, findMapping(cstr->getVariable()), ocra::CSTR_PLUS_GREATER, _G_block, _h_segment, v);

            idx += dim;
        }

    }
}


void OneLevelSolverWithQuadProg::doSolve()
{
    // solveRecorder.initializeTime();
    
    QuadProgPP::solve_quadprog(_C, -_d, _Atotal, _btotal, _G, _h, Xsolution);
    
    _result.solution = Xsolution;
    _result.info = ocra::RETURN_SUCCESS; //TODO: should be dfined through the result of the QuadProgPP::solve_quadprog

    // solveRecorder.saveRelativeTime();
}


//#ifdef USE_QPOASES

OneLevelSolverWithQPOASES::OneLevelSolverWithQPOASES()
: ocra::NamedInstance("One Level Solver with qpOASES")
, OneLevelSolver()
, _nWSR_every_run(200) // NOTE: totally random value
{
    std::cout << "-- " << getName() << " -- "<<this<< std::endl;
    sqp_options.setToMPC();
    sqp_options.enableRegularisation = qpOASES::BT_TRUE;
    sqp_options.enableEqualities = qpOASES::BT_TRUE;
}

void OneLevelSolverWithQPOASES::updateConstraintEquations()
{
    // UPDATE EQUALITY CONSTRAINTS
    this->ne = 0;
    for (int i=0; i<_equalityConstraints.size(); ++i)
        ne += _equalityConstraints[i]->getDimension();

    _A.setZero(ne, n());
    _lbA.setZero(ne);
    _ubA.setZero(ne);
    
    for (int i=0,idx=0; i<_equalityConstraints.size(); ++i)
    {
        ocra::LinearConstraint* cstr = _equalityConstraints[i];
        int dim = cstr->getDimension();

        if (dim > 0)
        {
            Eigen::Block<Eigen::MatrixXd> _A_block = _A.block(idx, 0, dim, n());
            Eigen::VectorBlock<Eigen::VectorXd> _ubA_segment = _ubA.segment(idx, dim);
            Eigen::VectorXd v;
            ocra::utils::convert(*cstr, findMapping(cstr->getVariable()), ocra::CSTR_MINUS_EQUAL, _A_block, _ubA_segment, v);

            idx += dim;
        }

    }
    
    _lbA = _ubA;
    
    // UPDATE INEQUALITY CONSTRAINTS
    this->ni = 0;
    for (int i=0; i<_inequalityConstraints.size(); ++i)
        ni += _inequalityConstraints[i]->getDimension();

    _G.setZero(ni, n());
    _lbG.setZero(ni);
    _ubG.setZero(ni);
    
    for (int i=0,idx=0; i<_inequalityConstraints.size(); ++i)
    {
        ocra::LinearConstraint* cstr = _inequalityConstraints[i];
        int dim = cstr->getDimension();

        if (dim > 0)
        {
            Eigen::Block<Eigen::MatrixXd > _G_block = _G.block(idx, 0, dim, n());
            
            Eigen::VectorBlock<Eigen::VectorXd> _lbG_segment = _lbG.segment(idx, dim);
            Eigen::VectorBlock<Eigen::VectorXd> _ubG_segment = _ubG.segment(idx, dim);
            
                // So we have _lbG_segment < _G_block.x < _ubG_segment
            ocra::utils::convert(*cstr,findMapping(cstr->getVariable()),ocra::CSTR_DOUBLE_BOUNDS,_G_block,_ubG_segment,_lbG_segment,1e6);

            idx += dim;
        }

    }
    
    // Verification 
    for(int i=0;i<_lbG.size();i++)
    {
        if(_lbG[i] > _ubG[i])
        {
     
            std::cout << "[ERROR] Lower bound "<<i<<"("<<_lbG[i]<<") higher than upper ("<<_ubG[i]<<")"<<std::endl;
            double tmp = _lbG[i];
            _lbG[i] = _ubG[i];
            _ubG[i] = tmp;
            
        }
    }
    // Combining Matrices
    if ( ( _AandG.rows() != _A.rows()+_G.rows() ) || ( _AandG.cols() != _A.cols() ) )
    {
        _ubAandG.resize(_ubA.size() + _ubG.size());
        _lbAandG.resize(_lbA.size() + _lbG.size());
        _AandG.resize(_A.rows()+_G.rows(),n());
    }
    

    _AandG.topRows(_A.rows())           = _A;
    _AandG.bottomRows(_G.rows())        = _G;

    _ubAandG.head(_ubA.size())           = _ubA;
    _ubAandG.tail(_ubG.size())           = _ubG;

    _lbAandG.head(_lbA.size())           = _lbA;
    _lbAandG.tail(_lbG.size())           = _lbG;

    if (_xl.size() != n())
    {
        _xl = - 1e6 * Eigen::VectorXd::Ones(n()); // X lower bound
        _xu =   1e6 * Eigen::VectorXd::Ones(n()); // X upper bound

    }
    
    A = _AandG.data();
    ubA = _ubAandG.data();
    lbA = _lbAandG.data();
    lb = _xl.data();
    ub = _xu.data();
    //std::cout << "G\n"<<_G<<std::endl;
//     std::cout << "A : \n"<<_A<<std::endl<<std::endl;
//     std::cout << "_AandG : \n"<<_AandG<<std::endl<<std::endl;
//     std::cout << "_ubAandG: "<<_ubAandG.transpose()<<std::endl;
//     std::cout << "_lbAandG: "<<_lbAandG.transpose()<<std::endl;
//     std::cout << "_ubG: "<<_ubG.transpose()<<std::endl;
//     std::cout << "_lbG: "<<_lbG.transpose()<<std::endl;
//     std::cout << "_xl: "<<_xl.transpose()<<std::endl;
//     std::cout << "_xu: "<<_xu.transpose()<<std::endl;
}

void OneLevelSolverWithQPOASES::updateObjectiveEquations()
{
    _C.setZero(n(), n());
    _d.setZero(n());
    
    for(int i=0; i<_objectives.size(); i++)
    {

        ocra::QuadraticFunction& obj     = _objectives[i]->getFunction();
        double weight                   = _objectives[i]->getWeight();
        const std::vector<int>& objMap  = findMapping(obj.getVariable());
        
        ocra::utils::addCompressed2d(   obj.getPi(), _C, objMap, weight);
        ocra::utils::addCompressedByRow(obj.getqi(), _d, objMap, weight);

    }


    if(H.size() != _C.rows()*_C.cols())
        H.resize(_C.rows()*_C.cols());
    
    if(_RegTerm.cols() != _C.cols() || _RegTerm.rows() != _C.rows())
        _RegTerm = 1.e-8*Eigen::MatrixXd::Identity(_C.rows(),_C.cols());
    
    Eigen::Map<MatrixXdRm>(H.data(),_C.rows(),_C.cols()) = _C /*+ _RegTerm*/;
    _d *= -1.0; // TODO: Find out why everybody's doing this :/
    g = _d.data();
}
eReturnInfo OneLevelSolverWithQPOASES::toOcraRetValue(const qpOASES::returnValue& ret)
{
  switch(ret)
  {
      case qpOASES::SUCCESSFUL_RETURN:
          return ocra::RETURN_SUCCESS;
      case qpOASES::RET_QP_INFEASIBLE:
          return ocra::RETURN_INFEASIBLE_PROBLEM;
      default:
          return ocra::RETURN_ERROR;
  }
}

void OneLevelSolverWithQPOASES::doSolve()
{
    static bool first_time;
    
    if(!sqp_prob || sqp_prob->getNV() != n() || sqp_prob->getNC() != (ni+ne))
    {
        std::cout << "Creating "<<getName()<<" Solver with params : n:"<<n()<<" ni:"<<ni<<" ne:"<<ne<<std::endl;
        sqp_prob.reset(new qpOASES::SQProblem(n(),ni+ne,qpOASES::HST_POSDEF));
        sqp_prob->setOptions( sqp_options );
        sqp_prob->setPrintLevel(qpOASES::PL_NONE);
        first_time = true;
    }
    
    
    //sqp_prob->printProperties();
    qpOASES::returnValue ret;
    nWSR = _nWSR_every_run; 
    if(first_time){
        ret = sqp_prob->init( H.data(),g,A,lb,ub,lbA,ubA, nWSR,NULL);
        if(ret == qpOASES::SUCCESSFUL_RETURN)
            first_time = false;
    }else{
        ret = sqp_prob->hotstart( H.data(),g,A,lb,ub,lbA,ubA, nWSR,NULL);
    }
    if(ret == qpOASES::SUCCESSFUL_RETURN)
        sqp_prob->getPrimalSolution(_result.solution.data());
//     else
//         _result.solution = Eigen::VectorXd::Zero(n());
    _result.info = toOcraRetValue(ret);
}

//#endif




OneLevelSolverWithQLD::OneLevelSolverWithQLD()
    : ocra::NamedInstance("One Level Solver with QLD subSolver")
    , OneLevelSolver()
    , _QLDsolver( new ocra::ObjQLD() )
    , MapP(NULL, 0,0)
    , Mapq(NULL,0)
    , MapAandG(NULL,0,0)
    , Mapbandh(NULL,0)
    , MapXsol(NULL,0)
    , MapXl(NULL,0)
    , MapXu(NULL,0)
{
    std::cout << "-- " << getName() << " --" << std::endl;
}

OneLevelSolverWithQLD::~OneLevelSolverWithQLD()
{

}



void OneLevelSolverWithQLD::updateObjectiveEquations()
{
//    _C = _Pbase;
    _C.setZero(n(), n());
    _d.setZero(n());

    if (MapP.size() != _C.size())
    {
        new (&MapP) MatrixMap(_C.data(), _C.rows(), _C.cols());
        new (&Mapq) VectorMap(_d.data(), _d.size());
    }

    for(int i=0; i<_objectives.size(); i++)
    {

        ocra::QuadraticFunction& obj     = _objectives[i]->getFunction();
        double weight                   = _objectives[i]->getWeight();
        const std::vector<int>& objMap  = findMapping(obj.getVariable());

        ocra::utils::addCompressed2d(   obj.getPi(), _C, objMap, weight);
        ocra::utils::addCompressedByRow(obj.getqi(), _d, objMap, weight);

    }

}

void OneLevelSolverWithQLD::updateConstraintEquations()
{
    // UPDATE EQUALITY CONSTRAINTS
    this->ne = 0;
    for (int i=0; i<_equalityConstraints.size(); ++i)
    {
        ne += _equalityConstraints[i]->getDimension();
    }

    _A.setZero(ne, n());
    _b.setZero(ne);

    int idx = 0;
    for (int i=0; i<_equalityConstraints.size(); ++i)
    {
        ocra::LinearConstraint* cstr = _equalityConstraints[i];
        int dim = cstr->getDimension();

        if (dim > 0)
        {
            Eigen::Block<Eigen::MatrixXd> _A_block = _A.block(idx, 0, dim, n());
            Eigen::VectorBlock<Eigen::VectorXd> _b_segment = _b.segment(idx, dim);

            Eigen::VectorXd v; // ?? is it useless ??
            ocra::utils::convert(*cstr, findMapping(cstr->getVariable()), ocra::CSTR_PLUS_EQUAL, _A_block, _b_segment, v);

            idx += dim;
        }

    }

    reduceConstraints(_A, _b, _Atotal, _btotal);



    // UPDATE INEQUALITY CONSTRAINTS
    this->ni = 0;
    for (int i=0; i<_inequalityConstraints.size(); ++i)
    {
        ni += _inequalityConstraints[i]->getDimension();
    }

    _G.setZero(ni, n());
    _h.setZero(ni);

    idx = 0;
    for (int i=0; i<_inequalityConstraints.size(); ++i)
    {
        ocra::LinearConstraint* cstr = _inequalityConstraints[i];
        int dim = cstr->getDimension();

        if (dim > 0)
        {
            Eigen::Block<Eigen::MatrixXd > _G_block = _G.block(idx, 0, dim, n());
            Eigen::VectorBlock<Eigen::VectorXd> _h_segment = _h.segment(idx, dim);
            Eigen::VectorXd v;  // ?? is it useless ??
            ocra::utils::convert(*cstr, findMapping(cstr->getVariable()), ocra::CSTR_PLUS_GREATER, _G_block, _h_segment, v);

            idx += dim;
        }

    }

    if ( ( AandG.rows() != _Atotal.rows()+_G.rows() ) || ( AandG.cols() != _Atotal.cols() ) )
    {
        AandG.resize(_Atotal.rows()+_G.rows(), n());
        bandh.resize(_btotal.size()+_h.size());
        new (&MapAandG) MatrixMap(AandG.data(), AandG.rows(), AandG.cols());
        new (&Mapbandh) VectorMap(bandh.data(), bandh.size());
    }
    AandG.topRows(_Atotal.rows())   = _Atotal;
    AandG.bottomRows(_G.rows())     = _G;
    bandh.head(_btotal.size())      = _btotal;
    bandh.tail(_h.size())           = _h;

    if (_xl.size() != n())
    {
        _xl = - 1e10 * Eigen::VectorXd::Ones(n()); // X lower bound
        _xu =   1e10 * Eigen::VectorXd::Ones(n()); // X upper bound
        new (&MapXl)    VectorMap(_xl.data(), _xl.size());
        new (&MapXu)    VectorMap(_xu.data(), _xu.size());
        new (&MapXsol)  VectorMap(Xsolution.data(), Xsolution.size()); // if bounds have changed, so has Xsolution
    }
}


void OneLevelSolverWithQLD::doSolve()
{
    // solveRecorder.initializeTime();


//    if (MapXsol.size() != Xsolution.size())                               // this test is done in #updateConstraintEquations()
//        new (&MapXsol) VectorMap(Xsolution.data(), Xsolution.size());

    _d = -_d; // this is to set -Mapq

    int res = _QLDsolver->solve(MapP, Mapq, MapAandG, Mapbandh, _Atotal.rows(), MapXsol, MapXl, MapXu, false);

    _result.solution = MapXsol;

    _result.info = ocra::RETURN_SUCCESS; //TODO: should be dfined through the result of the QuadProgPP::solve_quadprog

    // solveRecorder.saveRelativeTime();
}