crocoddyl  1.7.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 #include "crocoddyl/core/utils/exception.hpp"
16 
17 namespace crocoddyl {
18 
28 struct BoxQPSolution {
29  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 
35 
44  BoxQPSolution(const Eigen::MatrixXd& Hff_inv, const Eigen::VectorXd& x, const std::vector<size_t>& free_idx,
45  const std::vector<size_t>& clamped_idx)
46  : Hff_inv(Hff_inv), x(x), free_idx(free_idx), clamped_idx(clamped_idx) {}
47 
48  Eigen::MatrixXd Hff_inv;
49  Eigen::VectorXd x;
50  std::vector<size_t> free_idx;
51  std::vector<size_t> clamped_idx;
52 };
53 
78 class BoxQP {
79  public:
80  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81 
91  BoxQP(const std::size_t nx, const std::size_t maxiter = 100, const double th_acceptstep = 0.1,
92  const double th_grad = 1e-9, const double reg = 1e-9);
96  ~BoxQP();
97 
108  const BoxQPSolution& solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& lb,
109  const Eigen::VectorXd& ub, const Eigen::VectorXd& xinit);
110 
114  const BoxQPSolution& get_solution() const;
115 
119  std::size_t get_nx() const;
120 
124  std::size_t get_maxiter() const;
125 
129  double get_th_acceptstep() const;
130 
134  double get_th_grad() const;
135 
139  double get_reg() const;
140 
144  const std::vector<double>& get_alphas() const;
145 
149  void set_nx(const std::size_t nx);
150 
154  void set_maxiter(const std::size_t maxiter);
155 
159  void set_th_acceptstep(const double th_acceptstep);
160 
164  void set_th_grad(const double th_grad);
165 
169  void set_reg(const double reg);
170 
174  void set_alphas(const std::vector<double>& alphas);
175 
176  private:
177  std::size_t nx_;
178  BoxQPSolution solution_;
179  std::size_t maxiter_;
180  double th_acceptstep_;
181  double th_grad_;
182  double reg_;
183 
184  double fold_;
185  double fnew_;
186  std::size_t nf_;
187  std::size_t nc_;
188  std::vector<double> alphas_;
189  Eigen::VectorXd x_;
190  Eigen::VectorXd xnew_;
191  Eigen::VectorXd g_;
192  Eigen::VectorXd dx_;
193 
194  Eigen::VectorXd qf_;
195  Eigen::VectorXd xf_;
196  Eigen::VectorXd xc_;
197  Eigen::VectorXd dxf_;
198  Eigen::MatrixXd Hff_;
199  Eigen::MatrixXd Hfc_;
200  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt_;
201 };
202 
203 } // namespace crocoddyl
204 
205 #endif // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
Box QP solution.
Definition: box-qp.hpp:28
This class implements a Box QP solver based on a Projected Newton method.
Definition: box-qp.hpp:78
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:48
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:44
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:49
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQPSolution()
Initialize the QP solution structure.
Definition: box-qp.hpp:34
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:51
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:50