GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
1999 |
ActivationBoundsTpl(const VectorXs& lower, const VectorXs& upper, |
|
33 |
const Scalar b = (Scalar)1.) |
||
34 |
✓✗ | 1999 |
: lb(lower), ub(upper), beta(b) { |
35 |
✓✗✓✗ ✗✓ |
1999 |
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 |
✓✗✗✓ |
1999 |
if (beta < Scalar(0) || beta > Scalar(1.)) { |
43 |
throw_pretty("Invalid argument: " |
||
44 |
<< "The range of beta is between 0 and 1"); |
||
45 |
} |
||
46 |
using std::isfinite; |
||
47 |
✓✗✓✓ |
24210 |
for (std::size_t i = 0; i < static_cast<std::size_t>(lb.size()); ++i) { |
48 |
✓✗✓✓ ✓✗✓✓ ✓✓ |
22211 |
if (isfinite(lb(i)) && isfinite(ub(i))) { |
49 |
✓✗✓✗ ✗✓ |
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 |
✓✗✓✓ |
22211 |
if (!isfinite(lb(i))) { |
57 |
✓✗ | 16368 |
lb(i) = -std::numeric_limits<Scalar>::max(); |
58 |
} |
||
59 |
✓✗✓✓ |
22211 |
if (!isfinite(ub(i))) { |
60 |
✓✗ | 1631 |
ub(i) = std::numeric_limits<Scalar>::max(); |
61 |
} |
||
62 |
} |
||
63 |
|||
64 |
✓✗✓✗ |
1999 |
if (beta >= Scalar(0) && beta <= Scalar(1.)) { |
65 |
✓✗✓✓ |
24210 |
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 |
✓✗✓✓ ✓✓✓✓ |
28054 |
if (lb(i) != (-std::numeric_limits<Scalar>::max()) && |
68 |
✓✗ | 5843 |
ub(i) != (std::numeric_limits<Scalar>::max())) { |
69 |
✓✗✓✗ |
4212 |
Scalar m = Scalar(0.5) * (lb(i) + ub(i)); |
70 |
✓✗✓✗ |
4212 |
Scalar d = Scalar(0.5) * (ub(i) - lb(i)); |
71 |
✓✗ | 4212 |
lb(i) = m - beta * d; |
72 |
✓✗ | 4212 |
ub(i) = m + beta * d; |
73 |
} |
||
74 |
1999 |
} |
|
75 |
} else { |
||
76 |
beta = Scalar(1.); |
||
77 |
} |
||
78 |
1999 |
} |
|
79 |
2005 |
ActivationBoundsTpl(const ActivationBoundsTpl& other) |
|
80 |
✓✗ | 2005 |
: 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 |
1815 |
explicit ActivationModelQuadraticBarrierTpl(const ActivationBounds& bounds) |
|
113 |
✓✗ | 1815 |
: Base(bounds.lb.size()), bounds_(bounds){}; |
114 |
3634 |
virtual ~ActivationModelQuadraticBarrierTpl(){}; |
|
115 |
|||
116 |
82440 |
virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, |
|
117 |
const Eigen::Ref<const VectorXs>& r) { |
||
118 |
✓✗✗✓ |
82440 |
if (static_cast<std::size_t>(r.size()) != nr_) { |
119 |
throw_pretty("Invalid argument: " |
||
120 |
<< "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 |
✓✗✓✗ ✓✗✓✗ |
82440 |
d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.)); |
127 |
✓✗✓✗ ✓✗✓✗ |
82440 |
d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.)); |
128 |
✓✗✓✗ |
82440 |
data->a_value = Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() + |
129 |
✓✗✓✗ |
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 |
✓✗✗✓ |
10465 |
if (static_cast<std::size_t>(r.size()) != nr_) { |
135 |
throw_pretty("Invalid argument: " |
||
136 |
<< "r has wrong dimension (it should be " + |
||
137 |
std::to_string(nr_) + ")"); |
||
138 |
} |
||
139 |
|||
140 |
20930 |
boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data); |
|
141 |
✓✗✓✗ ✓✗ |
10465 |
data->Ar = (d->rlb_min_ + d->rub_max_).matrix(); |
142 |
|||
143 |
using pinocchio::internal::if_then_else; |
||
144 |
✓✗✓✓ |
81085 |
for (Eigen::Index i = 0; i < data->Arr.cols(); i++) { |
145 |
✓✗✓✗ |
70620 |
data->Arr.diagonal()[i] = if_then_else( |
146 |
✓✗ | 70620 |
pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.), |
147 |
✓✗✓✗ ✓✗ |
70620 |
if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i], |
148 |
✓✗ | 141240 |
Scalar(0.), Scalar(1.), Scalar(0.))); |
149 |
} |
||
150 |
10465 |
}; |
|
151 |
|||
152 |
87100 |
virtual boost::shared_ptr<ActivationDataAbstract> createData() { |
|
153 |
✓✗ | 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 |
rlb_min_(activation->get_nr()), |
||
189 |
✓✗✓✗ |
96428 |
rub_max_(activation->get_nr()) { |
190 |
✓✗ | 96428 |
rlb_min_.setZero(); |
191 |
✓✗ | 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_ |
Generated by: GCOVR (Version 4.2) |