GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/activations/quadratic-barrier.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 81 0.0%
Branches: 0 240 0.0%
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