GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files. All
7			// rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_BARRIER_HPP_
11			#define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_BARRIER_HPP_
12
13			#include "crocoddyl/core/activation-base.hpp"
14			#include "crocoddyl/core/fwd.hpp"
15
16			namespace crocoddyl {
17
18			template <typename _Scalar>
19			struct ActivationBoundsTpl {
20			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21
22			typedef _Scalar Scalar;
23			typedef MathBaseTpl<Scalar> MathBase;
24			typedef typename MathBase::VectorXs VectorXs;
25			typedef typename MathBase::MatrixXs MatrixXs;
26
27		✗	ActivationBoundsTpl(const VectorXs& lower, const VectorXs& upper,
28			const Scalar b = Scalar(1.))
29		✗	: lb(lower), ub(upper), beta(b) {
30		✗	if (lb.size() != ub.size()) {
31		✗	throw_pretty("Invalid argument: "
32			<< "The lower and upper bounds don't have the same "
33			"dimension (lb,ub dimensions equal to " +
34			std::to_string(lb.size()) + "," +
35			std::to_string(ub.size()) + ", respectively)");
36			}
37		✗	if (beta < Scalar(0) || beta > Scalar(1.)) {
38		✗	throw_pretty(
39			"Invalid argument: " << "The range of beta is between 0 and 1");
40			}
41		✗	for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) {
42		✗	if (isfinite(lb(i)) && isfinite(ub(i))) {
43		✗	if (lb(i) - ub(i) > Scalar(0)) {
44		✗	throw_pretty("Invalid argument: "
45			<< "The lower and upper bounds are badly defined; ub "
46			"has to be bigger / equals to lb");
47			}
48			}
49			// Assign the maximum value for infinity/nan values
50		✗	if (!isfinite(lb(i))) {
51		✗	lb(i) = -std::numeric_limits<Scalar>::max();
52			}
53		✗	if (!isfinite(ub(i))) {
54		✗	ub(i) = std::numeric_limits<Scalar>::max();
55			}
56			}
57
58		✗	if (beta >= Scalar(0) && beta <= Scalar(1.)) {
59		✗	for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) {
60			// do not use beta when one of the bounds is inf
61		✗	if (lb(i) != (-std::numeric_limits<Scalar>::max()) &&
62		✗	ub(i) != (std::numeric_limits<Scalar>::max())) {
63		✗	Scalar m = Scalar(0.5) * (lb(i) + ub(i));
64		✗	Scalar d = Scalar(0.5) * (ub(i) - lb(i));
65		✗	lb(i) = m - beta * d;
66		✗	ub(i) = m + beta * d;
67			}
68			}
69		✗	} else {
70		✗	beta = Scalar(1.);
71			}
72		✗	}
73		✗	ActivationBoundsTpl(const ActivationBoundsTpl& other)
74		✗	: lb(other.lb), ub(other.ub), beta(other.beta) {}
75		✗	ActivationBoundsTpl() : beta(Scalar(1.)) {}
76
77			template <typename NewScalar>
78		✗	ActivationBoundsTpl<NewScalar> cast() const {
79			typedef ActivationBoundsTpl<NewScalar> ReturnType;
80		✗	ReturnType res(lb.template cast<NewScalar>(), ub.template cast<NewScalar>(),
81		✗	scalar_cast<NewScalar>(beta));
82		✗	return res;
83			}
84
85		✗	ActivationBoundsTpl& operator=(const ActivationBoundsTpl& other) {
86		✗	if (this != &other) {
87		✗	lb = other.lb;
88		✗	ub = other.ub;
89		✗	beta = other.beta;
90			}
91		✗	return *this;
92			}
93
94			/**
95			* @brief Print information on the activation bounds
96			*/
97		✗	friend std::ostream& operator<<(std::ostream& os,
98			const ActivationBoundsTpl& bounds) {
99		✗	bounds.print(os);
100		✗	return os;
101			}
102
103		✗	void print(std::ostream& os) const {
104		✗	os << "ActivationBounds {lb=" << lb.transpose() << ", ub=" << ub.transpose()
105		✗	<< ", beta=" << beta << "}";
106		✗	}
107
108			VectorXs lb;
109			VectorXs ub;
110			Scalar beta;
111			};
112
113			template <typename _Scalar>
114			class ActivationModelQuadraticBarrierTpl
115			: public ActivationModelAbstractTpl<_Scalar> {
116			public:
117			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
118		✗	CROCODDYL_DERIVED_CAST(ActivationModelBase,
119			ActivationModelQuadraticBarrierTpl)
120
121			typedef _Scalar Scalar;
122			typedef MathBaseTpl<Scalar> MathBase;
123			typedef ActivationModelAbstractTpl<Scalar> Base;
124			typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
125			typedef ActivationDataQuadraticBarrierTpl<Scalar> Data;
126			typedef ActivationBoundsTpl<Scalar> ActivationBounds;
127			typedef typename MathBase::VectorXs VectorXs;
128			typedef typename MathBase::MatrixXs MatrixXs;
129
130		✗	explicit ActivationModelQuadraticBarrierTpl(const ActivationBounds& bounds)
131		✗	: Base(bounds.lb.size()), bounds_(bounds) {};
132		✗	virtual ~ActivationModelQuadraticBarrierTpl() = default;
133
134		✗	virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
135			const Eigen::Ref<const VectorXs>& r) override {
136		✗	if (static_cast<std::size_t>(r.size()) != nr_) {
137		✗	throw_pretty(
138			"Invalid argument: " << "r has wrong dimension (it should be " +
139			std::to_string(nr_) + ")");
140			}
141
142		✗	std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
143
144		✗	d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.));
145		✗	d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.));
146		✗	data->a_value = Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() +
147		✗	Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
148		✗	};
149
150		✗	virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
151			const Eigen::Ref<const VectorXs>& r) override {
152		✗	if (static_cast<std::size_t>(r.size()) != nr_) {
153		✗	throw_pretty(
154			"Invalid argument: " << "r has wrong dimension (it should be " +
155			std::to_string(nr_) + ")");
156			}
157
158		✗	std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
159		✗	data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
160
161			using pinocchio::internal::if_then_else;
162		✗	for (Eigen::Index i = 0; i < data->Arr.cols(); i++) {
163		✗	data->Arr.diagonal()[i] = if_then_else(
164		✗	pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.),
165		✗	if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i],
166		✗	Scalar(0.), Scalar(1.), Scalar(0.)));
167			}
168		✗	};
169
170		✗	virtual std::shared_ptr<ActivationDataAbstract> createData() override {
171		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
172			};
173
174			template <typename NewScalar>
175		✗	ActivationModelQuadraticBarrierTpl<NewScalar> cast() const {
176			typedef ActivationModelQuadraticBarrierTpl<NewScalar> ReturnType;
177		✗	ReturnType res(bounds_.template cast<NewScalar>());
178		✗	return res;
179			}
180
181		✗	const ActivationBounds& get_bounds() const { return bounds_; };
182		✗	void set_bounds(const ActivationBounds& bounds) { bounds_ = bounds; };
183
184			/**
185			* @brief Print relevant information of the quadratic barrier model
186			*
187			* @param[out] os Output stream object
188			*/
189		✗	virtual void print(std::ostream& os) const override {
190		✗	os << "ActivationModelQuadraticBarrier {nr=" << nr_ << "}";
191		✗	}
192
193			protected:
194			using Base::nr_;
195
196			private:
197			ActivationBounds bounds_;
198			};
199
200			template <typename _Scalar>
201			struct ActivationDataQuadraticBarrierTpl
202			: public ActivationDataAbstractTpl<_Scalar> {
203			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204
205			typedef _Scalar Scalar;
206			typedef MathBaseTpl<Scalar> MathBase;
207			typedef typename MathBase::ArrayXs ArrayXs;
208			typedef typename MathBase::VectorXs VectorXs;
209			typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
210			typedef ActivationDataAbstractTpl<Scalar> Base;
211
212			template <typename Activation>
213		✗	explicit ActivationDataQuadraticBarrierTpl(Activation* const activation)
214			: Base(activation),
215		✗	rlb_min_(activation->get_nr()),
216		✗	rub_max_(activation->get_nr()) {
217		✗	rlb_min_.setZero();
218		✗	rub_max_.setZero();
219		✗	}
220
221			ArrayXs rlb_min_;
222			ArrayXs rub_max_;
223
224			using Base::a_value;
225			using Base::Ar;
226			using Base::Arr;
227			};
228
229			} // namespace crocoddyl
230
231			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActivationBoundsTpl)
232			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
233			crocoddyl::ActivationModelQuadraticBarrierTpl)
234			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
235			crocoddyl::ActivationDataQuadraticBarrierTpl)
236
237			#endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_BARRIER_HPP_
238