GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/solvers/box-qp.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 1 3 33.3%
Branches: 1 8 12.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
10 #define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
11
12 #include <Eigen/Cholesky>
13 #include <Eigen/Dense>
14 #include <vector>
15
16 #include "crocoddyl/core/utils/exception.hpp"
17
18 namespace crocoddyl {
19
20 /**
21 * @brief Box QP solution
22 *
23 * It contains the Box QP solution data which consists of
24 * - the inverse of the free space Hessian
25 * - the optimal decision vector
26 * - the indexes for the free space
27 * - the indexes for the clamped (constrained) space
28 */
29 struct BoxQPSolution {
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32 /**
33 * @brief Initialize the QP solution structure
34 */
35
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 16 BoxQPSolution() {}
36
37 /**
38 * @brief Initialize the QP solution structure
39 *
40 * @param[in] Hff_inv Inverse of the free space Hessian
41 * @param[in] x Decision vector
42 * @param[in] free_idx Free space indexes
43 * @param[in] clamped_idx Clamped space indexes
44 */
45 ✗ BoxQPSolution(const Eigen::MatrixXd& Hff_inv, const Eigen::VectorXd& x,
46 const std::vector<size_t>& free_idx,
47 const std::vector<size_t>& clamped_idx)
48 ✗ : Hff_inv(Hff_inv), x(x), free_idx(free_idx), clamped_idx(clamped_idx) {}
49
50 Eigen::MatrixXd Hff_inv; //!< Inverse of the free space Hessian
51 Eigen::VectorXd x; //!< Decision vector
52 std::vector<size_t> free_idx; //!< Free space indexes
53 std::vector<size_t> clamped_idx; //!< Clamped space indexes
54 };
55
56 /**
57 * @brief This class implements a Box QP solver based on a Projected Newton
58 * method.
59 *
60 * We consider a box QP problem of the form:
61 * \f{eqnarray*}{
62 * \min_{\mathbf{x}} &= \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} +
63 * \mathbf{q}^T\mathbf{x} \\
64 * \textrm{subject to} & \hspace{1em} \mathbf{\underline{b}} \leq \mathbf{x}
65 * \leq \mathbf{\bar{b}} \\ \f} where \f$\mathbf{H}\f$, \f$\mathbf{q}\f$ are the
66 * Hessian and gradient of the problem, respectively,
67 * \f$\mathbf{\underline{b}}\f$, \f$\mathbf{\bar{b}}\f$ are lower and upper
68 * bounds of the decision variable \f$\mathbf{x}\f$.
69 *
70 * The algorithm procees by iteratively identifying the active bounds, and then
71 * performing a projected Newton step in the free sub-space.
72 * The projection uses the Hessian of the free sub-space and is computed
73 * efficiently using a Cholesky decomposition.
74 * It uses a line search procedure with polynomial step length values in a
75 * backtracking fashion.
76 * The steps are checked using an Armijo condition together L2-norm gradient.
77 *
78 * For more details about this solver, we encourage you to read the following
79 * article:
80 * \include bertsekas-siam82.bib
81 */
82 class BoxQP {
83 public:
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85
86 /**
87 * @brief Initialize the Projected-Newton QP for bound constraints
88 *
89 * @param[in] nx Dimension of the decision vector
90 * @param[in] maxiter Maximum number of allowed iterations (default
91 * 100)
92 * @param[in] th_acceptstep Acceptance step threshold (default 0.1)
93 * @param[in] th_grad Gradient tolerance threshold (default 1e-9)
94 * @param[in] reg Regularization value (default 1e-9)
95 */
96 BoxQP(const std::size_t nx, const std::size_t maxiter = 100,
97 const double th_acceptstep = 0.1, const double th_grad = 1e-9,
98 const double reg = 1e-9);
99 /**
100 * @brief Destroy the Projected-Newton QP solver
101 */
102 ~BoxQP();
103
104 /**
105 * @brief Compute the solution of bound-constrained QP based on Newton
106 * projection
107 *
108 * @param[in] H Hessian (dimension nx * nx)
109 * @param[in] q Gradient (dimension nx)
110 * @param[in] lb Lower bound (dimension nx)
111 * @param[in] ub Upper bound (dimension nx)
112 * @param[in] xinit Initial guess (dimension nx)
113 * @return The solution of the problem
114 */
115 const BoxQPSolution& solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q,
116 const Eigen::VectorXd& lb,
117 const Eigen::VectorXd& ub,
118 const Eigen::VectorXd& xinit);
119
120 /**
121 * @brief Return the stored solution
122 */
123 const BoxQPSolution& get_solution() const;
124
125 /**
126 * @brief Return the decision vector dimension
127 */
128 std::size_t get_nx() const;
129
130 /**
131 * @brief Return the maximum allowed number of iterations
132 */
133 std::size_t get_maxiter() const;
134
135 /**
136 * @brief Return the acceptance step threshold
137 */
138 double get_th_acceptstep() const;
139
140 /**
141 * @brief Return the gradient tolerance threshold
142 */
143 double get_th_grad() const;
144
145 /**
146 * @brief Return the regularization value
147 */
148 double get_reg() const;
149
150 /**
151 * @brief Return the stack of step lengths using by the line-search procedure
152 */
153 const std::vector<double>& get_alphas() const;
154
155 /**
156 * @brief Modify the decision vector dimension
157 */
158 void set_nx(const std::size_t nx);
159
160 /**
161 * @brief Modify the maximum allowed number of iterations
162 */
163 void set_maxiter(const std::size_t maxiter);
164
165 /**
166 * @brief Modify the acceptance step threshold
167 */
168 void set_th_acceptstep(const double th_acceptstep);
169
170 /**
171 * @brief Modify the gradient tolerance threshold
172 */
173 void set_th_grad(const double th_grad);
174
175 /**
176 * @brief Modify the regularization value
177 */
178 void set_reg(const double reg);
179
180 /**
181 * @brief Modify the stack of step lengths using by the line-search procedure
182 */
183 void set_alphas(const std::vector<double>& alphas);
184
185 private:
186 std::size_t nx_; //!< Decision variable dimension
187 BoxQPSolution solution_; //!< Solution of the Box QP
188 std::size_t maxiter_; //!< Allowed maximum number of iterations
189 double th_acceptstep_; //!< Threshold used for accepting step
190 double
191 th_grad_; //!< Tolerance for stopping the algorithm (gradient threshold)
192 double reg_; //!< Current regularization value
193
194 double fold_; //!< Cost of previous iteration
195 double fnew_; //!< Cost of current iteration
196 std::size_t nf_; //!< Free space dimension
197 std::size_t nc_; //!< Constrained space dimension
198 std::vector<double>
199 alphas_; //!< Set of step lengths using by the line-search procedure
200 Eigen::VectorXd x_; //!< Guess of the decision variable
201 Eigen::VectorXd xnew_; //!< New decision vector
202 Eigen::VectorXd g_; //!< Current gradient
203 Eigen::VectorXd dx_; //!< Current search direction
204
205 Eigen::VectorXd xo_; //!< Organized decision
206 Eigen::VectorXd
207 dxo_; //!< Search direction organized by free and constrained subspaces
208 Eigen::VectorXd
209 qo_; //!< Gradient organized by free and constrained subspaces
210 Eigen::MatrixXd Ho_; //!< Hessian organized by free and constrained subspaces
211
212 Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt_; //!< Cholesky solver
213 };
214
215 } // namespace crocoddyl
216
217 #endif // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
218