Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/solvers/box-qp.hpp	Lines:	1	3	33.3 %
Date:	2024-02-13 11:12:33	Branches:	1	8	12.5 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
#define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_

#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <vector>

#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

/**
 * @brief Box QP solution
 *
 * It contains the Box QP solution data which consists of
 *  - the inverse of the free space Hessian
 *  - the optimal decision vector
 *  - the indexes for the free space
 *  - the indexes for the clamped (constrained) space
 */
struct BoxQPSolution {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the QP solution structure
   */
  BoxQPSolution() {}

  /**
   * @brief Initialize the QP solution structure
   *
   * @param[in] Hff_inv      Inverse of the free space Hessian
   * @param[in] x            Decision vector
   * @param[in] free_idx     Free space indexes
   * @param[in] clamped_idx  Clamped space indexes
   */
  BoxQPSolution(const Eigen::MatrixXd& Hff_inv, const Eigen::VectorXd& x,
                const std::vector<size_t>& free_idx,
                const std::vector<size_t>& clamped_idx)
      : Hff_inv(Hff_inv), x(x), free_idx(free_idx), clamped_idx(clamped_idx) {}

  Eigen::MatrixXd Hff_inv;          //!< Inverse of the free space Hessian
  Eigen::VectorXd x;                //!< Decision vector
  std::vector<size_t> free_idx;     //!< Free space indexes
  std::vector<size_t> clamped_idx;  //!< Clamped space indexes
};

/**
 * @brief This class implements a Box QP solver based on a Projected Newton
 * method.
 *
 * We consider a box QP problem of the form:
 * \f{eqnarray*}{
 *   \min_{\mathbf{x}} &= \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} +
 * \mathbf{q}^T\mathbf{x} \\
 *   \textrm{subject to} & \hspace{1em} \mathbf{\underline{b}} \leq \mathbf{x}
 * \leq \mathbf{\bar{b}} \\ \f} where \f$\mathbf{H}\f$, \f$\mathbf{q}\f$ are the
 * Hessian and gradient of the problem, respectively,
 * \f$\mathbf{\underline{b}}\f$, \f$\mathbf{\bar{b}}\f$ are lower and upper
 * bounds of the decision variable \f$\mathbf{x}\f$.
 *
 * The algorithm procees by iteratively identifying the active bounds, and then
 * performing a projected Newton step in the free sub-space.
 * The projection uses the Hessian of the free sub-space and is computed
 * efficiently using a Cholesky decomposition.
 * It uses a line search procedure with polynomial step length values in a
 * backtracking fashion.
 * The steps are checked using an Armijo condition together L2-norm gradient.
 *
 * For more details about this solver, we encourage you to read the following
 * article:
 * \include bertsekas-siam82.bib
 */
class BoxQP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the Projected-Newton QP for bound constraints
   *
   * @param[in] nx             Dimension of the decision vector
   * @param[in] maxiter        Maximum number of allowed iterations (default
   * 100)
   * @param[in] th_acceptstep  Acceptance step threshold (default 0.1)
   * @param[in] th_grad        Gradient tolerance threshold (default 1e-9)
   * @param[in] reg            Regularization value (default 1e-9)
   */
  BoxQP(const std::size_t nx, const std::size_t maxiter = 100,
        const double th_acceptstep = 0.1, const double th_grad = 1e-9,
        const double reg = 1e-9);
  /**
   * @brief Destroy the Projected-Newton QP solver
   */
  ~BoxQP();

  /**
   * @brief Compute the solution of bound-constrained QP based on Newton
   * projection
   *
   * @param[in] H      Hessian (dimension nx * nx)
   * @param[in] q      Gradient (dimension nx)
   * @param[in] lb     Lower bound (dimension nx)
   * @param[in] ub     Upper bound (dimension nx)
   * @param[in] xinit  Initial guess (dimension nx)
   * @return The solution of the problem
   */
  const BoxQPSolution& solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q,
                             const Eigen::VectorXd& lb,
                             const Eigen::VectorXd& ub,
                             const Eigen::VectorXd& xinit);

  /**
   * @brief Return the stored solution
   */
  const BoxQPSolution& get_solution() const;

  /**
   * @brief Return the decision vector dimension
   */
  std::size_t get_nx() const;

  /**
   * @brief Return the maximum allowed number of iterations
   */
  std::size_t get_maxiter() const;

  /**
   * @brief Return the acceptance step threshold
   */
  double get_th_acceptstep() const;

  /**
   * @brief Return the gradient tolerance threshold
   */
  double get_th_grad() const;

  /**
   * @brief Return the regularization value
   */
  double get_reg() const;

  /**
   * @brief Return the stack of step lengths using by the line-search procedure
   */
  const std::vector<double>& get_alphas() const;

  /**
   * @brief Modify the decision vector dimension
   */
  void set_nx(const std::size_t nx);

  /**
   * @brief Modify the maximum allowed number of iterations
   */
  void set_maxiter(const std::size_t maxiter);

  /**
   * @brief Modify the acceptance step threshold
   */
  void set_th_acceptstep(const double th_acceptstep);

  /**
   * @brief Modify the gradient tolerance threshold
   */
  void set_th_grad(const double th_grad);

  /**
   * @brief Modify the regularization value
   */
  void set_reg(const double reg);

  /**
   * @brief Modify the stack of step lengths using by the line-search procedure
   */
  void set_alphas(const std::vector<double>& alphas);

 private:
  std::size_t nx_;          //!< Decision variable dimension
  BoxQPSolution solution_;  //!< Solution of the Box QP
  std::size_t maxiter_;     //!< Allowed maximum number of iterations
  double th_acceptstep_;    //!< Threshold used for accepting step
  double
      th_grad_;  //!< Tolerance for stopping the algorithm (gradient threshold)
  double reg_;   //!< Current regularization value

  double fold_;     //!< Cost of previous iteration
  double fnew_;     //!< Cost of current iteration
  std::size_t nf_;  //!< Free space dimension
  std::size_t nc_;  //!< Constrained space dimension
  std::vector<double>
      alphas_;  //!< Set of step lengths using by the line-search procedure
  Eigen::VectorXd x_;     //!< Guess of the decision variable
  Eigen::VectorXd xnew_;  //!< New decision vector
  Eigen::VectorXd g_;     //!< Current gradient
  Eigen::VectorXd dx_;    //!< Current search direction

  Eigen::VectorXd xo_;  //!< Organized decision
  Eigen::VectorXd
      dxo_;  //!< Search direction organized by free and constrained subspaces
  Eigen::VectorXd
      qo_;  //!< Gradient organized by free and constrained subspaces
  Eigen::MatrixXd Ho_;  //!< Hessian organized by free and constrained subspaces

  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt_;  //!< Cholesky solver
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_


Generated by: GCOVR (Version 4.2)

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	✓✗	12	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_