crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
box-qp.hpp
1
2// 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
17namespace crocoddyl {
18
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)
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
78class 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_
This class implements a Box QP solver based on a Projected Newton method.
Definition: box-qp.hpp:78
void set_reg(const double reg)
Modify the regularization value.
Definition: box-qp.cpp:231
void set_th_grad(const double th_grad)
Modify the gradient tolerance threshold.
Definition: box-qp.cpp:223
const BoxQPSolution & get_solution() const
Return the stored solution.
Definition: box-qp.cpp:191
double get_th_grad() const
Return the gradient tolerance threshold.
Definition: box-qp.cpp:199
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
std::size_t get_maxiter() const
Return the maximum allowed number of iterations.
Definition: box-qp.cpp:195
void set_maxiter(const std::size_t maxiter)
Modify the maximum allowed number of iterations.
Definition: box-qp.cpp:213
~BoxQP()
Destroy the Projected-Newton QP solver.
Definition: box-qp.cpp:56
void set_nx(const std::size_t nx)
Modify the decision vector dimension.
Definition: box-qp.cpp:205
const std::vector< double > & get_alphas() const
Return the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:203
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
void set_th_acceptstep(const double th_acceptstep)
Modify the acceptance step threshold.
Definition: box-qp.cpp:215
double get_th_acceptstep() const
Return the acceptance step threshold.
Definition: box-qp.cpp:197
double get_reg() const
Return the regularization value.
Definition: box-qp.cpp:201
std::size_t get_nx() const
Return the decision vector dimension.
Definition: box-qp.cpp:193
Box QP solution.
Definition: box-qp.hpp:28
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:50
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQPSolution()
Initialize the QP solution structure.
Definition: box-qp.hpp:34
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:48
Eigen::VectorXd x
Decision vector.
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:44
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:51