crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
box-qp.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
10 #define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
11 
12 #include <vector>
13 #include <Eigen/Dense>
14 #include <Eigen/Cholesky>
15 
16 #include "crocoddyl/core/utils/exception.hpp"
17 
18 namespace crocoddyl {
19 
29 struct BoxQPSolution {
30  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 
36 
45  BoxQPSolution(const Eigen::MatrixXd& Hff_inv, const Eigen::VectorXd& x, const std::vector<size_t>& free_idx,
46  const std::vector<size_t>& clamped_idx)
48 
49  Eigen::MatrixXd Hff_inv;
50  Eigen::VectorXd x;
51  std::vector<size_t> free_idx;
52  std::vector<size_t> clamped_idx;
53 };
54 
79 class BoxQP {
80  public:
81  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
82 
92  BoxQP(const std::size_t nx, std::size_t maxiter = 100, const double th_acceptstep = 0.1, const double th_grad = 1e-9,
93  const double reg = 1e-9);
97  ~BoxQP();
98 
109  const BoxQPSolution& solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& lb,
110  const Eigen::VectorXd& ub, const Eigen::VectorXd& xinit);
111 
115  const BoxQPSolution& get_solution() const;
116 
120  const std::size_t& get_nx() const;
121 
125  const std::size_t& get_maxiter() const;
126 
130  const double& get_th_acceptstep() const;
131 
135  const double& get_th_grad() const;
136 
140  const double& get_reg() const;
141 
145  const std::vector<double>& get_alphas() const;
146 
150  void set_nx(const std::size_t& nx);
151 
155  void set_maxiter(const std::size_t& maxiter);
156 
160  void set_th_acceptstep(const double& th_acceptstep);
161 
165  void set_th_grad(const double& th_grad);
166 
170  void set_reg(const double& reg);
171 
175  void set_alphas(const std::vector<double>& alphas);
176 
177  private:
178  std::size_t nx_;
179  BoxQPSolution solution_;
180  std::size_t maxiter_;
181  double th_acceptstep_;
182  double th_grad_;
183  double reg_;
184 
185  double fold_;
186  double fnew_;
187  std::size_t nf_;
188  std::size_t nc_;
189  std::vector<double> alphas_;
190  Eigen::VectorXd x_;
191  Eigen::VectorXd xnew_;
192  Eigen::VectorXd g_;
193  Eigen::VectorXd dx_;
194 
195  Eigen::VectorXd qf_;
196  Eigen::VectorXd xf_;
197  Eigen::VectorXd xc_;
198  Eigen::VectorXd dxf_;
199  Eigen::MatrixXd Hff_;
200  Eigen::MatrixXd Hfc_;
201  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt_;
202 };
203 
204 } // namespace crocoddyl
205 
206 #endif // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
crocoddyl::BoxQP::solve
const BoxQPSolution & solve(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::VectorXd &xinit)
Compute the solution of bound-constrained QP based on Newton projection.
Definition: box-qp.cpp:58
crocoddyl::BoxQP::get_reg
const double & get_reg() const
Return the regularization value.
Definition: box-qp.cpp:201
crocoddyl::BoxQP::set_alphas
void set_alphas(const std::vector< double > &alphas)
Modify the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:239
crocoddyl::BoxQPSolution::free_idx
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:51
crocoddyl::BoxQPSolution::Hff_inv
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:49
crocoddyl::BoxQP
This class implements a Box QP solver based on a Projected Newton method.
Definition: box-qp.hpp:79
crocoddyl::BoxQP::get_th_acceptstep
const double & get_th_acceptstep() const
Return the acceptance step threshold.
Definition: box-qp.cpp:197
crocoddyl::BoxQP::set_maxiter
void set_maxiter(const std::size_t &maxiter)
Modify the maximum allowed number of iterations.
Definition: box-qp.cpp:213
crocoddyl::BoxQP::get_solution
const BoxQPSolution & get_solution() const
Return the stored solution.
Definition: box-qp.cpp:191
crocoddyl::BoxQP::~BoxQP
~BoxQP()
Destroy the Projected-Newton QP solver.
Definition: box-qp.cpp:56
crocoddyl::BoxQP::BoxQP
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQP(const std::size_t nx, std::size_t maxiter=100, const double th_acceptstep=0.1, const double th_grad=1e-9, const double reg=1e-9)
Initialize the Projected-Newton QP for bound constraints.
Definition: box-qp.cpp:15
crocoddyl::BoxQPSolution::BoxQPSolution
BoxQPSolution(const Eigen::MatrixXd &Hff_inv, const Eigen::VectorXd &x, const std::vector< size_t > &free_idx, const std::vector< size_t > &clamped_idx)
Initialize the QP solution structure.
Definition: box-qp.hpp:45
crocoddyl::BoxQP::set_th_acceptstep
void set_th_acceptstep(const double &th_acceptstep)
Modify the acceptance step threshold.
Definition: box-qp.cpp:215
crocoddyl::BoxQP::get_th_grad
const double & get_th_grad() const
Return the gradient tolerance threshold.
Definition: box-qp.cpp:199
crocoddyl::BoxQP::set_nx
void set_nx(const std::size_t &nx)
Modify the decision vector dimension.
Definition: box-qp.cpp:205
crocoddyl::BoxQP::get_maxiter
const std::size_t & get_maxiter() const
Return the maximum allowed number of iterations.
Definition: box-qp.cpp:195
crocoddyl::BoxQP::set_reg
void set_reg(const double &reg)
Modify the regularization value.
Definition: box-qp.cpp:231
crocoddyl::BoxQPSolution
Box QP solution.
Definition: box-qp.hpp:29
crocoddyl::BoxQP::get_alphas
const std::vector< double > & get_alphas() const
Return the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:203
crocoddyl::BoxQPSolution::clamped_idx
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:52
crocoddyl::BoxQPSolution::x
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:50
crocoddyl::BoxQP::get_nx
const std::size_t & get_nx() const
Return the decision vector dimension.
Definition: box-qp.cpp:193
crocoddyl::BoxQPSolution::BoxQPSolution
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQPSolution()
Initialize the QP solution structure.
Definition: box-qp.hpp:35
crocoddyl::BoxQP::set_th_grad
void set_th_grad(const double &th_grad)
Modify the gradient tolerance threshold.
Definition: box-qp.cpp:223