| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2019-2020, University of Edinburgh, LAAS-CNRS |
||
5 |
// Copyright note valid unless otherwise stated in individual files. |
||
6 |
// All rights reserved. |
||
7 |
/////////////////////////////////////////////////////////////////////////////// |
||
8 |
|||
9 |
#ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_ |
||
10 |
#define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_ |
||
11 |
|||
12 |
#include <pinocchio/utils/static-if.hpp> |
||
13 |
|||
14 |
#include "crocoddyl/core/activations/quadratic-barrier.hpp" |
||
15 |
#include "crocoddyl/core/fwd.hpp" |
||
16 |
#include "crocoddyl/core/utils/exception.hpp" |
||
17 |
|||
18 |
namespace crocoddyl { |
||
19 |
|||
20 |
template <typename _Scalar> |
||
21 |
class ActivationModelWeightedQuadraticBarrierTpl |
||
22 |
: public ActivationModelAbstractTpl<_Scalar> { |
||
23 |
public: |
||
24 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
25 |
|||
26 |
typedef _Scalar Scalar; |
||
27 |
typedef MathBaseTpl<Scalar> MathBase; |
||
28 |
typedef ActivationModelAbstractTpl<Scalar> Base; |
||
29 |
typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract; |
||
30 |
typedef ActivationDataQuadraticBarrierTpl<Scalar> Data; |
||
31 |
typedef ActivationBoundsTpl<Scalar> ActivationBounds; |
||
32 |
typedef typename MathBase::VectorXs VectorXs; |
||
33 |
typedef typename MathBase::MatrixXs MatrixXs; |
||
34 |
|||
35 |
184 |
explicit ActivationModelWeightedQuadraticBarrierTpl( |
|
36 |
const ActivationBounds& bounds, const VectorXs& weights) |
||
37 |
✓✗✓✗ |
184 |
: Base(bounds.lb.size()), bounds_(bounds), weights_(weights){}; |
38 |
372 |
virtual ~ActivationModelWeightedQuadraticBarrierTpl(){}; |
|
39 |
|||
40 |
3795 |
virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, |
|
41 |
const Eigen::Ref<const VectorXs>& r) { |
||
42 |
✓✗✗✓ |
3795 |
if (static_cast<std::size_t>(r.size()) != nr_) { |
43 |
throw_pretty("Invalid argument: " |
||
44 |
<< "r has wrong dimension (it should be " + |
||
45 |
std::to_string(nr_) + ")"); |
||
46 |
} |
||
47 |
3795 |
boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data); |
|
48 |
|||
49 |
✓✗✓✗ ✓✗✓✗ |
3795 |
d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.)); |
50 |
✓✗✓✗ ✓✗✓✗ |
3795 |
d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.)); |
51 |
✓✗✓✗ |
3795 |
d->rlb_min_.array() *= weights_.array(); |
52 |
✓✗✓✗ |
3795 |
d->rub_max_.array() *= weights_.array(); |
53 |
✓✗✓✗ |
3795 |
data->a_value = Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() + |
54 |
✓✗✓✗ |
3795 |
Scalar(0.5) * d->rub_max_.matrix().squaredNorm(); |
55 |
3795 |
}; |
|
56 |
|||
57 |
212 |
virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, |
|
58 |
const Eigen::Ref<const VectorXs>& r) { |
||
59 |
✓✗✗✓ |
212 |
if (static_cast<std::size_t>(r.size()) != nr_) { |
60 |
throw_pretty("Invalid argument: " |
||
61 |
<< "r has wrong dimension (it should be " + |
||
62 |
std::to_string(nr_) + ")"); |
||
63 |
} |
||
64 |
212 |
boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data); |
|
65 |
✓✗✓✗ ✓✗ |
212 |
data->Ar = (d->rlb_min_ + d->rub_max_).matrix(); |
66 |
✓✗✓✗ ✓✗ |
212 |
data->Ar.array() *= weights_.array(); |
67 |
|||
68 |
using pinocchio::internal::if_then_else; |
||
69 |
✓✗✓✓ |
2623 |
for (Eigen::Index i = 0; i < data->Arr.cols(); i++) { |
70 |
✓✗✓✗ |
2411 |
data->Arr.diagonal()[i] = if_then_else( |
71 |
✓✗ | 2411 |
pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.), |
72 |
✓✗✓✗ ✓✗ |
2411 |
if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i], |
73 |
✓✗ | 4822 |
Scalar(0.), Scalar(1.), Scalar(0.))); |
74 |
} |
||
75 |
|||
76 |
✓✗✓✗ ✓✗ |
212 |
data->Arr.diagonal().array() *= weights_.array(); |
77 |
212 |
}; |
|
78 |
|||
79 |
4665 |
virtual boost::shared_ptr<ActivationDataAbstract> createData() { |
|
80 |
✓✗ | 4665 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); |
81 |
}; |
||
82 |
|||
83 |
const ActivationBounds& get_bounds() const { return bounds_; }; |
||
84 |
const VectorXs& get_weights() const { return weights_; }; |
||
85 |
void set_bounds(const ActivationBounds& bounds) { bounds_ = bounds; }; |
||
86 |
void set_weights(const VectorXs& weights) { |
||
87 |
if (weights.size() != weights_.size()) { |
||
88 |
throw_pretty("Invalid argument: " |
||
89 |
<< "weight vector has wrong dimension (it should be " + |
||
90 |
std::to_string(weights_.size()) + ")"); |
||
91 |
} |
||
92 |
weights_ = weights; |
||
93 |
}; |
||
94 |
|||
95 |
/** |
||
96 |
* @brief Print relevant information of the quadratic barrier model |
||
97 |
* |
||
98 |
* @param[out] os Output stream object |
||
99 |
*/ |
||
100 |
37 |
virtual void print(std::ostream& os) const { |
|
101 |
37 |
os << "ActivationModelWeightedQuadraticBarrier {nr=" << nr_ << "}"; |
|
102 |
37 |
} |
|
103 |
|||
104 |
protected: |
||
105 |
using Base::nr_; |
||
106 |
|||
107 |
private: |
||
108 |
ActivationBounds bounds_; |
||
109 |
VectorXs weights_; |
||
110 |
}; |
||
111 |
|||
112 |
} // namespace crocoddyl |
||
113 |
|||
114 |
#endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_ |
| Generated by: GCOVR (Version 4.2) |