GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/constraint-base.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 23 0.0%
Branches: 0 78 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, 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_CONSTRAINT_BASE_HPP_
10 #define CROCODDYL_CORE_CONSTRAINT_BASE_HPP_
11
12 #include "crocoddyl/core/fwd.hpp"
13 //
14 #include "crocoddyl/core/data-collector-base.hpp"
15 #include "crocoddyl/core/residual-base.hpp"
16 #include "crocoddyl/core/state-base.hpp"
17
18 namespace crocoddyl {
19
20 enum ConstraintType { Inequality = 0, Equality, Both };
21
22 class ConstraintModelBase {
23 public:
24 ✗ virtual ~ConstraintModelBase() = default;
25
26 ✗ CROCODDYL_BASE_CAST(ConstraintModelBase, ConstraintModelAbstractTpl)
27 };
28
29 /**
30 * @brief Abstract class for constraint models
31 *
32 * A constraint model defines both: inequality \f$\mathbf{g}(\mathbf{x},
33 * \mathbf{u})\in\mathbb{R}^{ng}\f$ and equality \f$\mathbf{h}(\mathbf{x},
34 * \mathbf{u})\in\mathbb{R}^{nh}\f$ constraints. The constraint function depends
35 * on the state point \f$\mathbf{x}\in\mathcal{X}\f$, which lies in the state
36 * manifold described with a `nx`-tuple, its velocity \f$\dot{\mathbf{x}}\in
37 * T_{\mathbf{x}}\mathcal{X}\f$ that belongs to the tangent space with `ndx`
38 * dimension, and the control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$.
39 *
40 * The main computations are carried out in `calc()` and `calcDiff()` routines.
41 * `calc()` computes the constraint residual and `calcDiff()` computes the
42 * Jacobians of the constraint function. Concretely speaking, `calcDiff()`
43 * builds a linear approximation of the constraint function with the form:
44 * \f$\mathbf{g_x}\in\mathbb{R}^{ng\times ndx}\f$,
45 * \f$\mathbf{g_u}\in\mathbb{R}^{ng\times nu}\f$,
46 * \f$\mathbf{h_x}\in\mathbb{R}^{nh\times ndx}\f$
47 * \f$\mathbf{h_u}\in\mathbb{R}^{nh\times nu}\f$. Additionally, it is important
48 * to note that `calcDiff()` computes the derivatives using the latest stored
49 * values by `calc()`. Thus, we need to first run `calc()`.
50 *
51 * \sa `calc()`, `calcDiff()`, `createData()`
52 */
53 template <typename _Scalar>
54 class ConstraintModelAbstractTpl : public ConstraintModelBase {
55 public:
56 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57
58 typedef _Scalar Scalar;
59 typedef MathBaseTpl<Scalar> MathBase;
60 typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
61 typedef StateAbstractTpl<Scalar> StateAbstract;
62 typedef ResidualModelAbstractTpl<Scalar> ResidualModelAbstract;
63 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
64 typedef typename MathBase::VectorXs VectorXs;
65
66 /**
67 * @brief Initialize the constraint model
68 *
69 * @param[in] state State of the multibody system
70 * @param[in] residual Residual model
71 * @param[in] ng Number of inequality constraints
72 * @param[in] nh Number of equality constraints
73 */
74 ConstraintModelAbstractTpl(std::shared_ptr<StateAbstract> state,
75 std::shared_ptr<ResidualModelAbstract> residual,
76 const std::size_t ng, const std::size_t nh);
77
78 /**
79 * @copybrief Initialize the constraint model
80 *
81 * @param[in] state State of the multibody system
82 * @param[in] nu Dimension of control vector
83 * @param[in] ng Number of inequality constraints
84 * @param[in] nh Number of equality constraints
85 * @param[in] T_const True if this is a constraint in both running and
86 * terminal nodes. False if it is a constraint on running nodes only (default
87 * true)
88 */
89 ConstraintModelAbstractTpl(std::shared_ptr<StateAbstract> state,
90 const std::size_t nu, const std::size_t ng,
91 const std::size_t nh, const bool T_const = true);
92
93 /**
94 * @copybrief ConstraintModelAbstractTpl()
95 *
96 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
97 *
98 * @param[in] state State of the multibody system
99 * @param[in] ng Number of inequality constraints
100 * @param[in] nh Number of equality constraints
101 * @param[in] T_const True if this is a constraint in both running and
102 * terminal nodes. False if it is a constraint on running nodes only (default
103 * true)
104 */
105 ConstraintModelAbstractTpl(std::shared_ptr<StateAbstract> state,
106 const std::size_t ng, const std::size_t nh,
107 const bool T_const = true);
108 ✗ virtual ~ConstraintModelAbstractTpl() = default;
109
110 /**
111 * @brief Compute the constraint value
112 *
113 * @param[in] data Constraint data
114 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
115 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
116 */
117 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
118 const Eigen::Ref<const VectorXs>& x,
119 const Eigen::Ref<const VectorXs>& u) = 0;
120
121 /**
122 * @brief Compute the constraint value for nodes that depends only on the
123 * state
124 *
125 * It updates the constraint based on the state only. This function is
126 * commonly used in the terminal nodes of an optimal control problem.
127 *
128 * @param[in] data Constraint data
129 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
130 */
131 virtual void calc(const std::shared_ptr<ConstraintDataAbstract>& data,
132 const Eigen::Ref<const VectorXs>& x);
133
134 /**
135 * @brief Compute the Jacobian of the constraint
136 *
137 * It computes the Jacobian of the constraint function. It assumes that
138 * `calc()` has been run first.
139 *
140 * @param[in] data Constraint data
141 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
142 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
143 */
144 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
145 const Eigen::Ref<const VectorXs>& x,
146 const Eigen::Ref<const VectorXs>& u) = 0;
147
148 /**
149 * @brief Compute the Jacobian of the constraint with respect to the state
150 * only
151 *
152 * It computes the Jacobian of the constraint function based on the state
153 * only. This function is commonly used in the terminal nodes of an optimal
154 * control problem.
155 *
156 * @param[in] data Constraint data
157 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
158 */
159 virtual void calcDiff(const std::shared_ptr<ConstraintDataAbstract>& data,
160 const Eigen::Ref<const VectorXs>& x);
161
162 /**
163 * @brief Create the constraint data
164 *
165 * The default data contains objects to store the values of the constraint,
166 * residual vector and their first derivatives. However, it is possible to
167 * specialize this function is we need to create additional data, for
168 * instance, to avoid dynamic memory allocation.
169 *
170 * @param data Data collector
171 * @return the constraint data
172 */
173 virtual std::shared_ptr<ConstraintDataAbstract> createData(
174 DataCollectorAbstract* const data);
175
176 /**
177 * @brief Update the lower and upper bounds the upper bound of constraint
178 */
179 void update_bounds(const VectorXs& lower, const VectorXs& upper);
180
181 /**
182 * @brief Remove the bounds of the constraint
183 */
184 void remove_bounds();
185
186 /**
187 * @brief Return the state
188 */
189 const std::shared_ptr<StateAbstract>& get_state() const;
190
191 /**
192 * @brief Return the residual model
193 */
194 const std::shared_ptr<ResidualModelAbstract>& get_residual() const;
195
196 /**
197 * @brief Return the type of constraint
198 */
199 ConstraintType get_type() const;
200
201 /**
202 * @brief Return the lower bound of the constraint
203 */
204 const VectorXs& get_lb() const;
205
206 /**
207 * @brief Return the upper bound of the constraint
208 */
209 const VectorXs& get_ub() const;
210
211 /**
212 * @brief Return the dimension of the control input
213 */
214 std::size_t get_nu() const;
215
216 /**
217 * @brief Return the number of inequality constraints
218 */
219 std::size_t get_ng() const;
220
221 /**
222 * @brief Return the number of equality constraints
223 */
224 std::size_t get_nh() const;
225
226 /**
227 * @brief Return true if the constraint is imposed in terminal nodes as well.
228 */
229 bool get_T_constraint() const;
230
231 /**
232 * @brief Print information on the constraint model
233 */
234 template <class Scalar>
235 friend std::ostream& operator<<(std::ostream& os,
236 const CostModelAbstractTpl<Scalar>& model);
237
238 /**
239 * @brief Print relevant information of the constraint model
240 *
241 * @param[out] os Output stream object
242 */
243 virtual void print(std::ostream& os) const;
244
245 private:
246 std::size_t ng_internal_; //!< Number of inequality constraints defined at
247 //!< construction time
248 std::size_t nh_internal_; //!< Number of equality constraints defined at
249 //!< construction time
250
251 protected:
252 std::shared_ptr<StateAbstract> state_; //!< State description
253 std::shared_ptr<ResidualModelAbstract> residual_; //!< Residual model
254 ConstraintType
255 type_; //!< Type of constraint: inequality=0, equality=1, both=2
256 VectorXs lb_; //!< Lower bound of the constraint
257 VectorXs ub_; //!< Upper bound of the constraint
258 std::size_t nu_; //!< Control dimension
259 std::size_t ng_; //!< Number of inequality constraints
260 std::size_t nh_; //!< Number of equality constraints
261 bool T_constraint_; //!< Label that indicates if the constraint is imposed in
262 //!< terminal nodes as well
263 VectorXs unone_; //!< No control vector
264 ✗ ConstraintModelAbstractTpl()
265 ✗ : state_(nullptr), residual_(nullptr), nu_(0), ng_(0), nh_(0) {}
266 };
267
268 template <typename _Scalar>
269 struct ConstraintDataAbstractTpl {
270 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
271
272 typedef _Scalar Scalar;
273 typedef MathBaseTpl<Scalar> MathBase;
274 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
275 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
276 typedef typename MathBase::VectorXs VectorXs;
277 typedef typename MathBase::MatrixXs MatrixXs;
278
279 template <template <typename Scalar> class Model>
280 ✗ ConstraintDataAbstractTpl(Model<Scalar>* const model,
281 DataCollectorAbstract* const data)
282 ✗ : shared(data),
283 ✗ residual(model->get_residual()->createData(data)),
284 ✗ g(model->get_ng()),
285 ✗ Gx(model->get_ng(), model->get_state()->get_ndx()),
286 ✗ Gu(model->get_ng(), model->get_nu()),
287 ✗ h(model->get_nh()),
288 ✗ Hx(model->get_nh(), model->get_state()->get_ndx()),
289 ✗ Hu(model->get_nh(), model->get_nu()) {
290 ✗ if (model->get_ng() == 0 && model->get_nh() == 0) {
291 ✗ throw_pretty("Invalid argument: " << "ng and nh cannot be equals to 0");
292 }
293 ✗ g.setZero();
294 ✗ Gx.setZero();
295 ✗ Gu.setZero();
296 ✗ h.setZero();
297 ✗ Hx.setZero();
298 ✗ Hu.setZero();
299 }
300 ✗ virtual ~ConstraintDataAbstractTpl() = default;
301
302 DataCollectorAbstract* shared; //!< Shared data
303 std::shared_ptr<ResidualDataAbstract> residual; //!< Residual data
304 VectorXs g; //!< Inequality constraint values
305 MatrixXs Gx; //!< Jacobian of the inequality constraint
306 MatrixXs Gu; //!< Jacobian of the inequality constraint
307 VectorXs h; //!< Equality constraint values
308 MatrixXs Hx; //!< Jacobian of the equality constraint
309 MatrixXs Hu; //!< Jacobian of the equality constraint
310 };
311
312 } // namespace crocoddyl
313
314 /* --- Details -------------------------------------------------------------- */
315 /* --- Details -------------------------------------------------------------- */
316 /* --- Details -------------------------------------------------------------- */
317 #include "crocoddyl/core/constraint-base.hxx"
318
319 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ConstraintModelAbstractTpl)
320 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintDataAbstractTpl)
321
322 #endif // CROCODDYL_CORE_CONSTRAINT_BASE_HPP_
323