GCC Code Coverage Report

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