FSQPSolver.h

Go to the documentation of this file.

#ifndef _OCRABASE_FSQP_SOLVER_H_
#define _OCRABASE_FSQP_SOLVER_H_

// includes
#include "Solver.h"
#include "OFSQP.h"
#include "FSQPConstraintManager.h"
#include "ocra/optim/Buffer.h"


namespace ocra
{
  class FSQPSolver : public Solver, public OFSQPProblem
  {
    // ------------------------ structures --------------------------------------
  public:
    typedef Map<VectorXd> VectorMap;
    typedef Map<MatrixXd> MatrixMap;


    enum eFsqpProblemType
    {
      NORMAL_PB=0,      //A=0: min sum f_i
      INFINITE_PB       //A=1: min |sum f_i|
    };

    enum eFsqpAlgo
    {
      AL=0,             //B=0: algorithm FSQP-AL, decrease of the (modified) objectif function at each step
      NL                //B=1: algorithm FSQP-NL, decrease of the (modified) objectif function after at most 4 steps
    };

    enum eFsqpEvaluationDomainPolicy
    {
      LOOSE=1,          //C=1: objectives and constraints can be evaluated outside the feasible space
      STRICT            //C=2: objectives and constraints can not be evaluated outside the feasible space
    };

    enum eFsqpPrintOption
    {
      NONE,
      FINAL,
      EACH,
      FULL
    };

  protected:
  private:

    // ------------------------ public static members ---------------------------
  public:

    // ------------------------ constructors ------------------------------------
  private:
    FSQPSolver(const FSQPSolver&);
    FSQPSolver& operator=(const FSQPSolver&);
  protected:
  public:
    FSQPSolver();

    // ------------------------ public interface --------------------------------
  public:
    void addObjective(GenericObjective& obj);
    void removeObjective(Function& obj);
    void removeObjective(GenericObjective& obj);

    void addConstraint(LinearConstraint& constraint);
    void addConstraint(GenericConstraint& constraint);
    void removeConstraint(LinearConstraint& constraint);
    void removeConstraint(GenericConstraint& constraint);
    
    void addBounds(BoundConstraint& constraint);
    void addBounds(IdentityConstraint& constraint);
    void removeBounds(BoundConstraint& constraint);
    void removeBounds(IdentityConstraint& constraint);

    void clearObjectives();
    void clearConstraints();
    void clearEqualityConstraints();
    void clearNonLinearEqualityConstraints();
    void clearLinearEqualityConstraints();
    void clearInequalityConstraints();
    void clearNonLinearInequalityConstraints();
    void clearLinearInequalityConstraints();
    void clearBounds();

    void set_x0(const VectorXd& x0, const Variable& ref);

    void setA(eFsqpProblemType type);
    void setB(eFsqpAlgo type);
    void setC(eFsqpEvaluationDomainPolicy type);
    void setMode(int m);
    void setPrintMode(eFsqpPrintOption m);
    void setPrintStep(int N);
    void setPrintOption(int option);
    void setMaxIter(int n);
    void setInfinity(double infinity);
    void setEps(double eps);
    void setEqnViol(double eps);
    void setUDelta(double udelta);

    eFsqpProblemType            getA()            const;
    eFsqpAlgo                   getB()            const;
    eFsqpEvaluationDomainPolicy getC()            const;
    int                         getMode()         const;
    eFsqpPrintOption            getPrintMode()    const;
    int                         getPrintStep()    const;
    int                         getPrintOption()  const;
    int                         getMaxIter()      const;
    double                      getInfinity()     const;
    double                      setEps()          const;
    double                      setEqnViol()      const;
    double                      setUDelta()       const;


    void printValuesAtSolution();

    std::string toString();

    // ------------------------ public methods ----------------------------------
  public:
    void obj(int nparam, int j, double* x, double* fj, void* cd);
    void constr(int nparam, int j, double* x, double* gj, void* cd);
    void gradob(int nparam, int j, double* x, double* gradfj, void* cd);
    void gradcn(int nparam, int j, double* x, double* gradgj, void* cd);

    // ------------------------ public static methods ---------------------------
  public:

    // ------------------------ protected methods -------------------------------
  protected:
    void doPrepare();
    void doSolve();
    void doConclude();

    virtual void onConstraintResize(int timestamp);

    virtual void onObjectiveResize(int timestamp);

    // ------------------------ protected static methods ------------------------
  protected:

    // ------------------------ private methods ---------------------------------
  private:
    void resize();
    void updateBounds();
    eReturnInfo translateReturnInfo() const;

    void addBounds_(DiagonalLinearConstraint& constraint);
    void removeBounds_(DiagonalLinearConstraint& constraint);
    void checkNewX(int nparam, double* x);

    // ------------------------ private static methods --------------------------
  private:

    // ------------------------ protected members -------------------------------
  protected:

    // ------------------------ protected static members ------------------------
  protected:

    // ------------------------ private members ---------------------------------
  private:
    OFSQP     solver;

    int       nparam;
    int       nf;
    int       nfsr;
    int       nineqn; 
    int       nineq; 
    int       neqn; 
    int       neq;
    int       ncsrl;
    int       ncsrn;
    int       mode;
    int       iprint;
    int       miter;
    int       inform;
    double    bigbnd;
    double    eps;
    double    epseqn;
    double    udelta;

    VectorXd  x0;
    VectorMap bl;
    VectorMap bu;
    VectorMap f;
    VectorMap g;
    VectorMap lambda;

    Map<VectorXi>   mesh_pts;

    Buffer<double>  _buffer;

    bool _allObjectivesProvideAGradient;
    std::vector<GenericObjective*>          _objectives;
    std::vector<DiagonalLinearConstraint*>  _bounds;
    FSQPConstraintManager                   _constraints;

    // ------------------------ private static members --------------------------
  private:

    // ------------------------ friendship declarations -------------------------
  };

  void testFSQPSolver01();
  void testFSQPSolver02();
}

#endif  //_OCRABASE_FSQP_SOLVER_H_