GCC Code Coverage Report

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