GCC Code Coverage Report

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