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)
47  : Hff_inv(Hff_inv), x(x), free_idx(free_idx), clamped_idx(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_
Box QP solution.
Definition: box-qp.hpp:29
This class implements a Box QP solver based on a Projected Newton method.
Definition: box-qp.hpp:79
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:49
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
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:50
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQPSolution()
Initialize the QP solution structure.
Definition: box-qp.hpp:35
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:52
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:51