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			namespace crocoddyl {
10
11			template <typename Scalar>
12		✗	ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
13			std::shared_ptr<StateAbstract> state,
14			std::shared_ptr<ResidualModelAbstract> residual, const std::size_t ng,
15			const std::size_t nh)
16		✗	: ng_internal_(ng),
17		✗	nh_internal_(nh),
18		✗	state_(state),
19		✗	residual_(residual),
20		✗	type_((ng > 0 && nh > 0) ? ConstraintType::Both
21		✗	: (ng > 0 ? ConstraintType::Inequality
22			: ConstraintType::Equality)),
23		✗	lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
24		✗	ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
25		✗	nu_(residual->get_nu()),
26		✗	ng_(ng),
27		✗	nh_(nh),
28		✗	T_constraint_(residual->get_q_dependent() || residual->get_v_dependent()
29		✗	? true
30			: false),
31		✗	unone_(VectorXs::Zero(residual->get_nu())) {
32		✗	if (nh_ > residual_->get_nr()) {
33		✗	throw_pretty("Invalid argument: "
34			<< "the number of equality constraints (nh) is wrong as it is "
35			"bigger than the residual dimension.")
36			}
37		✗	std::size_t max_ng = 2 * (residual_->get_nr() - nh_);
38		✗	if (ng_ > max_ng) {
39		✗	throw_pretty("Invalid argument: "
40			<< "the number of inequality constraints (ng) is wrong as it "
41			"should be in the range [0, " +
42			std::to_string(max_ng) + "]");
43			}
44		✗	}
45
46			template <typename Scalar>
47		✗	ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
48			std::shared_ptr<StateAbstract> state, const std::size_t nu,
49			const std::size_t ng, const std::size_t nh, const bool T_const)
50		✗	: ng_internal_(ng),
51		✗	nh_internal_(nh),
52		✗	state_(state),
53		✗	residual_(std::make_shared<ResidualModelAbstractTpl<Scalar>>(
54		✗	state, ng + nh, nu)),
55		✗	type_((ng > 0 && nh > 0) ? ConstraintType::Both
56		✗	: (ng > 0 ? ConstraintType::Inequality
57			: ConstraintType::Equality)),
58		✗	lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
59		✗	ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
60		✗	nu_(nu),
61		✗	ng_(ng),
62		✗	nh_(nh),
63		✗	T_constraint_(T_const),
64		✗	unone_(VectorXs::Zero(nu)) {}
65
66			template <typename Scalar>
67		✗	ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
68			std::shared_ptr<StateAbstract> state, const std::size_t ng,
69			const std::size_t nh, const bool T_const)
70		✗	: ng_internal_(ng),
71		✗	nh_internal_(nh),
72		✗	state_(state),
73		✗	residual_(
74		✗	std::make_shared<ResidualModelAbstractTpl<Scalar>>(state, ng + nh)),
75		✗	type_((ng > 0 && nh > 0) ? ConstraintType::Both
76		✗	: (ng > 0 ? ConstraintType::Inequality
77			: ConstraintType::Equality)),
78		✗	lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
79		✗	ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
80		✗	nu_(state->get_nv()),
81		✗	ng_(ng),
82		✗	nh_(nh),
83		✗	T_constraint_(T_const),
84		✗	unone_(VectorXs::Zero(state->get_nv())) {}
85
86			template <typename Scalar>
87		✗	void ConstraintModelAbstractTpl<Scalar>::calc(
88			const std::shared_ptr<ConstraintDataAbstract>& data,
89			const Eigen::Ref<const VectorXs>& x) {
90		✗	calc(data, x, unone_);
91		✗	}
92
93			template <typename Scalar>
94		✗	void ConstraintModelAbstractTpl<Scalar>::calcDiff(
95			const std::shared_ptr<ConstraintDataAbstract>& data,
96			const Eigen::Ref<const VectorXs>& x) {
97		✗	calcDiff(data, x, unone_);
98		✗	}
99
100			template <typename Scalar>
101			std::shared_ptr<ConstraintDataAbstractTpl<Scalar>>
102		✗	ConstraintModelAbstractTpl<Scalar>::createData(
103			DataCollectorAbstract* const data) {
104			return std::allocate_shared<ConstraintDataAbstract>(
105		✗	Eigen::aligned_allocator<ConstraintDataAbstract>(), this, data);
106			}
107
108			template <typename Scalar>
109		✗	void ConstraintModelAbstractTpl<Scalar>::update_bounds(const VectorXs& lower,
110			const VectorXs& upper) {
111		✗	if (static_cast<std::size_t>(upper.size()) != ng_internal_ ||
112		✗	static_cast<std::size_t>(lower.size()) != ng_internal_) {
113		✗	throw_pretty(
114			"Invalid argument: the dimension of the lower/upper bound is not the "
115			"same to ng.")
116			}
117		✗	if (((upper - lower).array() <= Scalar(0.)).any()) {
118		✗	throw_pretty(
119			"Invalid argument: the upper bound is not higher than the lower bound.")
120			}
121		✗	if ((lb_.array() == std::numeric_limits<Scalar>::infinity()).any() ||
122		✗	(lb_.array() == std::numeric_limits<Scalar>::max()).any()) {
123		✗	throw_pretty(
124			"Invalid argument: the lower bound cannot contain a positive "
125			"infinity/max value");
126			}
127		✗	if ((ub_.array() == -std::numeric_limits<Scalar>::infinity()).any() ||
128		✗	(ub_.array() == -std::numeric_limits<Scalar>::infinity()).any()) {
129		✗	throw_pretty(
130			"Invalid argument: the lower bound cannot contain a negative "
131			"infinity/min value");
132			}
133		✗	ng_ = ng_internal_;
134		✗	nh_ = nh_internal_;
135		✗	lb_ = lower;
136		✗	ub_ = upper;
137		✗	if (nh_ == 0) {
138		✗	type_ = ConstraintType::Inequality;
139			} else {
140		✗	type_ = ConstraintType::Both;
141			}
142		✗	}
143
144			template <typename Scalar>
145		✗	void ConstraintModelAbstractTpl<Scalar>::remove_bounds() {
146		✗	ng_ = 0;
147		✗	nh_ = nh_internal_ + ng_internal_;
148		✗	lb_.setConstant(-std::numeric_limits<Scalar>::infinity());
149		✗	ub_.setConstant(std::numeric_limits<Scalar>::infinity());
150		✗	type_ = ConstraintType::Equality;
151		✗	}
152
153			template <typename Scalar>
154		✗	void ConstraintModelAbstractTpl<Scalar>::print(std::ostream& os) const {
155		✗	os << boost::core::demangle(typeid(*this).name());
156		✗	}
157
158			template <typename Scalar>
159			const std::shared_ptr<StateAbstractTpl<Scalar>>&
160		✗	ConstraintModelAbstractTpl<Scalar>::get_state() const {
161		✗	return state_;
162			}
163
164			template <typename Scalar>
165			const std::shared_ptr<ResidualModelAbstractTpl<Scalar>>&
166		✗	ConstraintModelAbstractTpl<Scalar>::get_residual() const {
167		✗	return residual_;
168			}
169
170			template <typename Scalar>
171		✗	ConstraintType ConstraintModelAbstractTpl<Scalar>::get_type() const {
172		✗	return type_;
173			}
174
175			template <typename Scalar>
176			const typename MathBaseTpl<Scalar>::VectorXs&
177		✗	ConstraintModelAbstractTpl<Scalar>::get_lb() const {
178		✗	return lb_;
179			}
180
181			template <typename Scalar>
182			const typename MathBaseTpl<Scalar>::VectorXs&
183		✗	ConstraintModelAbstractTpl<Scalar>::get_ub() const {
184		✗	return ub_;
185			}
186
187			template <typename Scalar>
188		✗	std::size_t ConstraintModelAbstractTpl<Scalar>::get_nu() const {
189		✗	return nu_;
190			}
191
192			template <typename Scalar>
193		✗	std::size_t ConstraintModelAbstractTpl<Scalar>::get_ng() const {
194		✗	return ng_;
195			}
196
197			template <typename Scalar>
198		✗	std::size_t ConstraintModelAbstractTpl<Scalar>::get_nh() const {
199		✗	return nh_;
200			}
201
202			template <typename Scalar>
203		✗	bool ConstraintModelAbstractTpl<Scalar>::get_T_constraint() const {
204		✗	return T_constraint_;
205			}
206
207			template <class Scalar>
208		✗	std::ostream& operator<<(std::ostream& os,
209			const ConstraintModelAbstractTpl<Scalar>& model) {
210		✗	model.print(os);
211		✗	return os;
212			}
213
214			} // namespace crocoddyl
215