GCC Code Coverage Report

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_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
10			#define CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
11
12			#include "crocoddyl/core/constraint-base.hpp"
13
14			namespace crocoddyl {
15
16			template <typename _Scalar>
17			struct ConstraintItemTpl {
18			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
19
20			typedef _Scalar Scalar;
21			typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract;
22
23		✗	ConstraintItemTpl() {}
24		✗	ConstraintItemTpl(const std::string& name,
25			std::shared_ptr<ConstraintModelAbstract> constraint,
26			bool active = true)
27		✗	: name(name), constraint(constraint), active(active) {}
28
29			template <typename NewScalar>
30		✗	ConstraintItemTpl<NewScalar> cast() const {
31			typedef ConstraintItemTpl<NewScalar> ReturnType;
32		✗	ReturnType ret(name, constraint->template cast<NewScalar>(), active);
33		✗	return ret;
34			}
35
36			/**
37			* @brief Print information on the constraint item
38			*/
39		✗	friend std::ostream& operator<<(std::ostream& os,
40			const ConstraintItemTpl<Scalar>& model) {
41		✗	os << "{" << *model.constraint << "}";
42		✗	return os;
43			}
44
45			std::string name;
46			std::shared_ptr<ConstraintModelAbstract> constraint;
47			bool active;
48			};
49
50			/**
51			* @brief Manage the individual constraint models
52			*
53			* This class serves to manage a set of added constraint models. The constraint
54			* functions might active or inactive, with this approach we avoid dynamic
55			* allocation of memory. Each constraint model is added through `addConstraint`,
56			* where its status can be defined.
57			*
58			* The main computations are carring out in `calc` and `calcDiff` routines.
59			* `calc` computes the constraint residuals and `calcDiff` computes the
60			* Jacobians of the constraint functions. Concretely speaking, `calcDiff` builds
61			* a linear approximation of the total constraint function (both inequality and
62			* equality) with the form: \f$\mathbf{g_u}\in\mathbb{R}^{ng\times nu}\f$,
63			* \f$\mathbf{h_x}\in\mathbb{R}^{nh\times ndx}\f$
64			* \f$\mathbf{h_u}\in\mathbb{R}^{nh\times nu}\f$.
65			*
66			* \sa `StateAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
67			*/
68			template <typename _Scalar>
69			class ConstraintModelManagerTpl {
70			public:
71			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72
73			typedef _Scalar Scalar;
74			typedef MathBaseTpl<Scalar> MathBase;
75			typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager;
76			typedef StateAbstractTpl<Scalar> StateAbstract;
77			typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract;
78			typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract;
79			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
80			typedef ConstraintItemTpl<Scalar> ConstraintItem;
81			typedef typename MathBase::VectorXs VectorXs;
82			typedef typename MathBase::MatrixXs MatrixXs;
83
84			typedef std::map<std::string, std::shared_ptr<ConstraintItem> >
85			ConstraintModelContainer;
86			typedef std::map<std::string, std::shared_ptr<ConstraintDataAbstract> >
87			ConstraintDataContainer;
88
89			/**
90			* @brief Initialize the constraint-manager model
91			*
92			* @param[in] state State of the multibody system
93			* @param[in] nu Dimension of control vector
94			*/
95			ConstraintModelManagerTpl(std::shared_ptr<StateAbstract> state,
96			const std::size_t nu);
97
98			/**
99			* @brief Initialize the constraint-manager model
100			*
101			* The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
102			*
103			* @param[in] state State of the multibody system
104			*/
105			explicit ConstraintModelManagerTpl(std::shared_ptr<StateAbstract> state);
106			~ConstraintModelManagerTpl();
107
108			/**
109			* @brief Add a constraint item
110			*
111			* @param[in] name Constraint name
112			* @param[in] constraint Constraint model
113			* @param[in] weight Constraint weight
114			* @param[in] active True if the constraint is activated (default true)
115			*/
116			void addConstraint(const std::string& name,
117			std::shared_ptr<ConstraintModelAbstract> constraint,
118			const bool active = true);
119
120			/**
121			* @brief Remove a constraint item
122			*
123			* @param[in] name Constraint name
124			*/
125			void removeConstraint(const std::string& name);
126
127			/**
128			* @brief Change the constraint status
129			*
130			* @param[in] name Constraint name
131			* @param[in] active Constraint status (true for active and false for
132			* inactive)
133			*/
134			void changeConstraintStatus(const std::string& name, bool active);
135
136			/**
137			* @brief Compute the total constraint value
138			*
139			* @param[in] data Constraint data
140			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
141			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
142			*/
143			void calc(const std::shared_ptr<ConstraintDataManager>& data,
144			const Eigen::Ref<const VectorXs>& x,
145			const Eigen::Ref<const VectorXs>& u);
146
147			/**
148			* @brief Compute the total constraint value for nodes that depends only on
149			* the state
150			*
151			* It updates the constraint based on the state only. This function is
152			* commonly used in the terminal nodes of an optimal control problem.
153			*
154			* @param[in] data Constraint data
155			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
156			*/
157			void calc(const std::shared_ptr<ConstraintDataManager>& data,
158			const Eigen::Ref<const VectorXs>& x);
159
160			/**
161			* @brief Compute the Jacobian of the total constraint
162			*
163			* @param[in] data Constraint data
164			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
165			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
166			*/
167			void calcDiff(const std::shared_ptr<ConstraintDataManager>& data,
168			const Eigen::Ref<const VectorXs>& x,
169			const Eigen::Ref<const VectorXs>& u);
170
171			/**
172			* @brief Compute the Jacobian of the total constraint with respect to the
173			* state only
174			*
175			* It computes the Jacobian of the constraint function based on the state
176			* only. This function is commonly used in the terminal nodes of an optimal
177			* control problem.
178			*
179			* @param[in] data Constraint data
180			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
181			*/
182			void calcDiff(const std::shared_ptr<ConstraintDataManager>& data,
183			const Eigen::Ref<const VectorXs>& x);
184
185			/**
186			* @brief Create the constraint data
187			*
188			* The default data contains objects to store the values of the constraint and
189			* their derivatives (i.e. Jacobians). However, it is possible to specialize
190			* this function is we need to create additional data, for instance, to avoid
191			* dynamic memory allocation.
192			*
193			* @param data Data collector
194			* @return the constraint data
195			*/
196			std::shared_ptr<ConstraintDataManager> createData(
197			DataCollectorAbstract* const data);
198
199			/**
200			* @brief Cast the constraint-manager model to a different scalar type.
201			*
202			* It is useful for operations requiring different precision or scalar types.
203			*
204			* @tparam NewScalar The new scalar type to cast to.
205			* @return ConstraintModelManagerTpl<NewScalar> A constraint-manager model
206			* with the new scalar type.
207			*/
208			template <typename NewScalar>
209			ConstraintModelManagerTpl<NewScalar> cast() const;
210
211			/**
212			* @brief Return the state
213			*/
214			const std::shared_ptr<StateAbstract>& get_state() const;
215
216			/**
217			* @brief Return the stack of constraint models
218			*/
219			const ConstraintModelContainer& get_constraints() const;
220
221			/**
222			* @brief Return the dimension of the control input
223			*/
224			std::size_t get_nu() const;
225
226			/**
227			* @brief Return the number of active inequality constraints
228			*/
229			std::size_t get_ng() const;
230
231			/**
232			* @brief Return the number of active equality constraints
233			*/
234			std::size_t get_nh() const;
235
236			/**
237			* @brief Return the number of active inequality terminal constraints
238			*/
239			std::size_t get_ng_T() const;
240
241			/**
242			* @brief Return the number of active equality terminal constraints
243			*/
244			std::size_t get_nh_T() const;
245
246			/**
247			* @brief Return the names of the set of active constraints
248			*/
249			const std::set<std::string>& get_active_set() const;
250
251			/**
252			* @brief Return the names of the set of inactive constraints
253			*/
254			const std::set<std::string>& get_inactive_set() const;
255
256			/**
257			* @brief Return the state lower bound
258			*/
259			const VectorXs& get_lb() const;
260
261			/**
262			* @brief Return the state upper bound
263			*/
264			const VectorXs& get_ub() const;
265
266			/**
267			* @brief Return the status of a given constraint name
268			*
269			* @param[in] name Constraint name
270			*/
271			bool getConstraintStatus(const std::string& name) const;
272
273			/**
274			* @brief Print information on the stack of constraints
275			*/
276			template <class Scalar>
277			friend std::ostream& operator<<(
278			std::ostream& os, const ConstraintModelManagerTpl<Scalar>& model);
279
280			private:
281			std::shared_ptr<StateAbstract> state_; //!< State description
282			ConstraintModelContainer constraints_; //!< Stack of constraint items
283			VectorXs lb_; //!< Lower bound of the constraint
284			VectorXs ub_; //!< Upper bound of the constraint
285			std::size_t nu_; //!< Dimension of the control input
286			std::size_t ng_; //!< Number of the active inequality constraints
287			std::size_t nh_; //!< Number of the active equality constraints
288			std::size_t ng_T_; //!< Number of the active inequality terminal constraints
289			std::size_t nh_T_; //!< Number of the active equality terminal constraints
290			std::set<std::string> active_set_; //!< Names of the active constraint items
291			std::set<std::string>
292			inactive_set_; //!< Names of the inactive constraint items
293			};
294
295			template <typename _Scalar>
296			struct ConstraintDataManagerTpl {
297			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
298
299			typedef _Scalar Scalar;
300			typedef MathBaseTpl<Scalar> MathBase;
301			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
302			typedef ConstraintItemTpl<Scalar> ConstraintItem;
303			typedef typename MathBase::VectorXs VectorXs;
304			typedef typename MathBase::MatrixXs MatrixXs;
305
306			template <template <typename Scalar> class Model>
307		✗	ConstraintDataManagerTpl(Model<Scalar>* const model,
308			DataCollectorAbstract* const data)
309		✗	: g_internal(model->get_ng()),
310		✗	Gx_internal(model->get_ng(), model->get_state()->get_ndx()),
311		✗	Gu_internal(model->get_ng(), model->get_nu()),
312		✗	h_internal(model->get_nh()),
313		✗	Hx_internal(model->get_nh(), model->get_state()->get_ndx()),
314		✗	Hu_internal(model->get_nh(), model->get_nu()),
315		✗	shared(data),
316		✗	g(g_internal.data(), model->get_ng()),
317		✗	Gx(Gx_internal.data(), model->get_ng(), model->get_state()->get_ndx()),
318		✗	Gu(Gu_internal.data(), model->get_ng(), model->get_nu()),
319		✗	h(h_internal.data(), model->get_nh()),
320		✗	Hx(Hx_internal.data(), model->get_nh(), model->get_state()->get_ndx()),
321		✗	Hu(Hu_internal.data(), model->get_nh(), model->get_nu()) {
322		✗	g.setZero();
323		✗	Gx.setZero();
324		✗	Gu.setZero();
325		✗	h.setZero();
326		✗	Hx.setZero();
327		✗	Hu.setZero();
328		✗	for (typename ConstraintModelManagerTpl<
329			Scalar>::ConstraintModelContainer::const_iterator it =
330		✗	model->get_constraints().begin();
331		✗	it != model->get_constraints().end(); ++it) {
332		✗	const std::shared_ptr<ConstraintItem>& item = it->second;
333		✗	constraints.insert(
334		✗	std::make_pair(item->name, item->constraint->createData(data)));
335			}
336		✗	}
337
338			template <class ActionData>
339		✗	void shareMemory(ActionData* const data) {
340			// Save memory by setting the internal variables with null dimension
341		✗	g_internal.resize(0);
342		✗	Gx_internal.resize(0, 0);
343		✗	Gu_internal.resize(0, 0);
344		✗	h_internal.resize(0);
345		✗	Hx_internal.resize(0, 0);
346		✗	Hu_internal.resize(0, 0);
347			// Share memory with the differential action data
348		✗	new (&g) Eigen::Map<VectorXs>(data->g.data(), data->g.size());
349		✗	new (&Gx)
350			Eigen::Map<MatrixXs>(data->Gx.data(), data->Gx.rows(), data->Gx.cols());
351		✗	new (&Gu)
352			Eigen::Map<MatrixXs>(data->Gu.data(), data->Gu.rows(), data->Gu.cols());
353		✗	new (&h) Eigen::Map<VectorXs>(data->h.data(), data->h.size());
354		✗	new (&Hx)
355			Eigen::Map<MatrixXs>(data->Hx.data(), data->Hx.rows(), data->Hx.cols());
356		✗	new (&Hu)
357			Eigen::Map<MatrixXs>(data->Hu.data(), data->Hu.rows(), data->Hu.cols());
358		✗	}
359
360			template <class Model>
361		✗	void resize(Model* const model, const bool running_node = true) {
362		✗	const std::size_t ndx = model->get_state()->get_ndx();
363		✗	const std::size_t nu = model->get_nu();
364		✗	const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
365		✗	const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
366		✗	new (&g) Eigen::Map<VectorXs>(g_internal.data(), ng);
367		✗	new (&Gx) Eigen::Map<MatrixXs>(Gx_internal.data(), ng, ndx);
368		✗	new (&Gu) Eigen::Map<MatrixXs>(Gu_internal.data(), ng, nu);
369		✗	new (&h) Eigen::Map<VectorXs>(h_internal.data(), nh);
370		✗	new (&Hx) Eigen::Map<MatrixXs>(Hx_internal.data(), nh, ndx);
371		✗	new (&Hu) Eigen::Map<MatrixXs>(Hu_internal.data(), nh, nu);
372		✗	}
373
374			template <class ActionModel, class ActionData>
375		✗	void resize(ActionModel* const model, ActionData* const data,
376			const bool running_node = true) {
377		✗	const std::size_t ndx = model->get_state()->get_ndx();
378		✗	const std::size_t nu = model->get_nu();
379		✗	const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
380		✗	const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
381		✗	data->g.conservativeResize(ng);
382		✗	data->Gx.conservativeResize(ng, ndx);
383		✗	data->Gu.conservativeResize(ng, nu);
384		✗	data->h.conservativeResize(nh);
385		✗	data->Hx.conservativeResize(nh, ndx);
386		✗	data->Hu.conservativeResize(nh, nu);
387		✗	new (&g) Eigen::Map<VectorXs>(data->g.data(), ng);
388		✗	new (&Gx) Eigen::Map<MatrixXs>(data->Gx.data(), ng, ndx);
389		✗	new (&Gu) Eigen::Map<MatrixXs>(data->Gu.data(), ng, nu);
390		✗	new (&h) Eigen::Map<VectorXs>(data->h.data(), nh);
391		✗	new (&Hx) Eigen::Map<MatrixXs>(data->Hx.data(), nh, ndx);
392		✗	new (&Hu) Eigen::Map<MatrixXs>(data->Hu.data(), nh, nu);
393		✗	}
394
395		✗	VectorXs get_g() const { return g; }
396		✗	MatrixXs get_Gx() const { return Gx; }
397		✗	MatrixXs get_Gu() const { return Gu; }
398		✗	VectorXs get_h() const { return h; }
399		✗	MatrixXs get_Hx() const { return Hx; }
400		✗	MatrixXs get_Hu() const { return Hu; }
401
402		✗	void set_g(const VectorXs& _g) {
403		✗	if (g.size() != _g.size()) {
404		✗	throw_pretty(
405			"Invalid argument: " << "g has wrong dimension (it should be " +
406			std::to_string(g.size()) + ")");
407			}
408		✗	g = _g;
409		✗	}
410		✗	void set_Gx(const MatrixXs& _Gx) {
411		✗	if (Gx.rows() != _Gx.rows() || Gx.cols() != _Gx.cols()) {
412		✗	throw_pretty(
413			"Invalid argument: " << "Gx has wrong dimension (it should be " +
414			std::to_string(Gx.rows()) + ", " +
415			std::to_string(Gx.cols()) + ")");
416			}
417		✗	Gx = _Gx;
418		✗	}
419		✗	void set_Gu(const MatrixXs& _Gu) {
420		✗	if (Gu.rows() != _Gu.rows() || Gu.cols() != _Gu.cols()) {
421		✗	throw_pretty(
422			"Invalid argument: " << "Gu has wrong dimension (it should be " +
423			std::to_string(Gu.rows()) + ", " +
424			std::to_string(Gu.cols()) + ")");
425			}
426		✗	Gu = _Gu;
427		✗	}
428		✗	void set_h(const VectorXs& _h) {
429		✗	if (h.size() != _h.size()) {
430		✗	throw_pretty(
431			"Invalid argument: " << "h has wrong dimension (it should be " +
432			std::to_string(h.size()) + ")");
433			}
434		✗	h = _h;
435		✗	}
436		✗	void set_Hx(const MatrixXs& _Hx) {
437		✗	if (Hx.rows() != _Hx.rows() || Hx.cols() != _Hx.cols()) {
438		✗	throw_pretty(
439			"Invalid argument: " << "Hx has wrong dimension (it should be " +
440			std::to_string(Hx.rows()) + ", " +
441			std::to_string(Hx.cols()) + ")");
442			}
443		✗	Hx = _Hx;
444		✗	}
445		✗	void set_Hu(const MatrixXs& _Hu) {
446		✗	if (Hu.rows() != _Hu.rows() || Hu.cols() != _Hu.cols()) {
447		✗	throw_pretty(
448			"Invalid argument: " << "Hu has wrong dimension (it should be " +
449			std::to_string(Hu.rows()) + ", " +
450			std::to_string(Hu.cols()) + ")");
451			}
452		✗	Hu = _Hu;
453		✗	}
454
455			// Creates internal data in case we don't share it externally
456			VectorXs g_internal;
457			MatrixXs Gx_internal;
458			MatrixXs Gu_internal;
459			VectorXs h_internal;
460			MatrixXs Hx_internal;
461			MatrixXs Hu_internal;
462
463			typename ConstraintModelManagerTpl<Scalar>::ConstraintDataContainer
464			constraints;
465			DataCollectorAbstract* shared;
466			Eigen::Map<VectorXs> g;
467			Eigen::Map<MatrixXs> Gx;
468			Eigen::Map<MatrixXs> Gu;
469			Eigen::Map<VectorXs> h;
470			Eigen::Map<MatrixXs> Hx;
471			Eigen::Map<MatrixXs> Hu;
472			};
473
474			} // namespace crocoddyl
475
476			/* --- Details -------------------------------------------------------------- */
477			/* --- Details -------------------------------------------------------------- */
478			/* --- Details -------------------------------------------------------------- */
479			#include "crocoddyl/core/constraints/constraint-manager.hxx"
480
481			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintItemTpl)
482			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ConstraintModelManagerTpl)
483			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintDataManagerTpl)
484
485			#endif // CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_
486