eiquadprog-fast.hpp

Go to the documentation of this file.

//
// Copyright (c) 2017 CNRS
//
// This file is part of eiquadprog.
//
// eiquadprog is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
//(at your option) any later version.
 
// eiquadprog is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
 
// You should have received a copy of the GNU Lesser General Public License
// along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef EIQUADPROGFAST_HPP_
#define EIQUADPROGFAST_HPP_
 
#include <Eigen/Dense>
 
#define OPTIMIZE_STEP_1_2  // compute s(x) = ci^T * x + ci0
#define OPTIMIZE_COMPUTE_D
#define OPTIMIZE_UPDATE_Z
#define OPTIMIZE_HESSIAN_INVERSE
#define OPTIMIZE_UNCONSTR_MINIM
 
// #define USE_WARM_START
// #define PROFILE_EIQUADPROG
 
// #define DEBUG_STREAM(msg) std::cout<<msg;
#define DEBUG_STREAM(msg)
 
#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
#else
#define START_PROFILER_EIQUADPROG_FAST(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x)
#endif
 
#define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
#define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
  "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
 
#define DEFAULT_MAX_ITER 1000
 
#include "eiquadprog/eiquadprog-utils.hxx"
 
namespace eiquadprog {
 
namespace solvers {
 
enum EiquadprogFast_status {
  EIQUADPROG_FAST_OPTIMAL = 0,
  EIQUADPROG_FAST_INFEASIBLE = 1,
  EIQUADPROG_FAST_UNBOUNDED = 2,
  EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
  EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
};
 
class EiquadprogFast {
  typedef Eigen::MatrixXd MatrixXd;
  typedef Eigen::VectorXd VectorXd;
  typedef Eigen::VectorXi VectorXi;
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  EiquadprogFast();
  virtual ~EiquadprogFast();
 
  void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
 
  int getMaxIter() const { return m_maxIter; }
 
  bool setMaxIter(int maxIter) {
    if (maxIter < 0) return false;
    m_maxIter = maxIter;
    return true;
  }
 
  size_t getActiveSetSize() const { return q; }
 
  int getIteratios() const { return iter; }
 
  double getObjValue() const { return f_value; }
 
  const VectorXd& getLagrangeMultipliers() const { return u; }
 
  const VectorXi& getActiveSet() const { return A; }
 
  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
                                       const MatrixXd& CE, const VectorXd& ce0,
                                       const MatrixXd& CI, const VectorXd& ci0,
                                       VectorXd& x);
 
  MatrixXd m_J;  // J * J' = Hessian <nVars,nVars>::d
  bool is_inverse_provided_;
 
 private:
  size_t m_nVars;
  size_t m_nEqCon;
  size_t m_nIneqCon;
 
  int m_maxIter;   
  double f_value;  
 
  Eigen::LLT<MatrixXd, Eigen::Lower> chol_;  // <nVars,nVars>::d
 
  MatrixXd R;  // <nVars,nVars>::d
 
  VectorXd s;  // <nIneqCon>::d
 
  VectorXd r;  // <nIneqCon+nEqCon>::d
 
  VectorXd u;  // <nIneqCon+nEqCon>::d
 
  VectorXd z;  // <nVars>::d
 
  VectorXd d;  //<nVars>::d
 
  VectorXd np;  //<nVars>::d
 
  VectorXi A;  // <nIneqCon+nEqCon>
 
  VectorXi iai;  // <nIneqCon>::i
 
  VectorXi iaexcl;  //<nIneqCon>::i
 
  VectorXd x_old;  // old value of x <nVars>::d
  VectorXd u_old;  // old value of u <nIneqCon+nEqCon>::d
  VectorXi A_old;  // old value of A <nIneqCon+nEqCon>::i
 
#ifdef OPTIMIZE_ADD_CONSTRAINT
  VectorXd T1;  
#endif
 
  size_t q;
 
  int iter;
 
  inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
#ifdef OPTIMIZE_COMPUTE_D
    d.noalias() = J.adjoint() * np;
#else
    d = J.adjoint() * np;
#endif
  }
 
  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,
                       size_t iq) {
#ifdef OPTIMIZE_UPDATE_Z
    z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
#else
    z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
#endif
  }
 
  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
                       size_t iq) {
    r.head(iq) = d.head(iq);
    R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
        r.head(iq));
  }
 
  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq,
                             double& R_norm);
 
  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
                                VectorXd& u, size_t nEqCon, size_t& iq,
                                size_t l);
};
 
} /* namespace solvers */
} /* namespace eiquadprog */
 
#endif /* EIQUADPROGFAST_HPP_ */