GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/activations/weighted-quadratic-barrier.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 35 45 77.8%
Branches: 44 136 32.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
11 #define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
12
13 #include <pinocchio/utils/static-if.hpp>
14
15 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
16 #include "crocoddyl/core/fwd.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
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 16 CROCODDYL_DERIVED_CAST(ActivationModelBase,
26 ActivationModelWeightedQuadraticBarrierTpl)
27
28 typedef _Scalar Scalar;
29 typedef MathBaseTpl<Scalar> MathBase;
30 typedef ActivationModelAbstractTpl<Scalar> Base;
31 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
32 typedef ActivationDataQuadraticBarrierTpl<Scalar> Data;
33 typedef ActivationBoundsTpl<Scalar> ActivationBounds;
34 typedef typename MathBase::VectorXs VectorXs;
35 typedef typename MathBase::MatrixXs MatrixXs;
36
37 188 explicit ActivationModelWeightedQuadraticBarrierTpl(
38 const ActivationBounds& bounds, const VectorXs& weights)
39
2/4
✓ Branch 3 taken 188 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 188 times.
✗ Branch 7 not taken.
 188 : Base(bounds.lb.size()), bounds_(bounds), weights_(weights) {};
40 388 virtual ~ActivationModelWeightedQuadraticBarrierTpl() = default;
41
42 3797 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
43 const Eigen::Ref<const VectorXs>& r) override {
44
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3797 times.
 3797 if (static_cast<std::size_t>(r.size()) != nr_) {
45 ✗ throw_pretty(
46 "Invalid argument: " << "r has wrong dimension (it should be " +
47 std::to_string(nr_) + ")");
48 }
49 3797 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
50
51
4/8
✓ Branch 1 taken 3797 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3797 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3797 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3797 times.
✗ Branch 12 not taken.
 3797 d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.));
52
4/8
✓ Branch 1 taken 3797 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3797 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3797 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3797 times.
✗ Branch 12 not taken.
 3797 d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.));
53
2/4
✓ Branch 1 taken 3797 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3797 times.
✗ Branch 7 not taken.
 3797 d->rlb_min_.array() *= weights_.array();
54
2/4
✓ Branch 1 taken 3797 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3797 times.
✗ Branch 7 not taken.
 3797 d->rub_max_.array() *= weights_.array();
55
2/4
✓ Branch 2 taken 3797 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3797 times.
✗ Branch 6 not taken.
 3797 data->a_value = Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() +
56
2/4
✓ Branch 2 taken 3797 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3797 times.
✗ Branch 6 not taken.
 3797 Scalar(0.5) * d->rub_max_.matrix().squaredNorm();
57 3797 };
58
59 213 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
60 const Eigen::Ref<const VectorXs>& r) override {
61
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 213 times.
 213 if (static_cast<std::size_t>(r.size()) != nr_) {
62 ✗ throw_pretty(
63 "Invalid argument: " << "r has wrong dimension (it should be " +
64 std::to_string(nr_) + ")");
65 }
66 213 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
67
3/6
✓ Branch 3 taken 213 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 213 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 213 times.
✗ Branch 11 not taken.
 213 data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
68
3/6
✓ Branch 1 taken 213 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 213 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 213 times.
✗ Branch 9 not taken.
 213 data->Ar.array() *= weights_.array();
69
70 using pinocchio::internal::if_then_else;
71
3/4
✓ Branch 2 taken 2629 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2416 times.
✓ Branch 5 taken 213 times.
 2629 for (Eigen::Index i = 0; i < data->Arr.cols(); i++) {
72
1/2
✓ Branch 3 taken 2416 times.
✗ Branch 4 not taken.
 2416 data->Arr.diagonal()[i] = if_then_else(
73
2/4
✓ Branch 1 taken 2416 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2416 times.
✗ Branch 5 not taken.
 2416 pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.),
74
3/6
✓ Branch 1 taken 2416 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2416 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2416 times.
✗ Branch 8 not taken.
 2416 if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i],
75
1/2
✓ Branch 1 taken 2416 times.
✗ Branch 2 not taken.
 4832 Scalar(0.), Scalar(1.), Scalar(0.)));
76 }
77
78
3/6
✓ Branch 1 taken 213 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 213 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 213 times.
✗ Branch 10 not taken.
 213 data->Arr.diagonal().array() *= weights_.array();
79 213 };
80
81 4668 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
82
1/2
✓ Branch 2 taken 4668 times.
✗ Branch 3 not taken.
 4668 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
83 };
84
85 template <typename NewScalar>
86 4 ActivationModelWeightedQuadraticBarrierTpl<NewScalar> cast() const {
87 typedef ActivationModelWeightedQuadraticBarrierTpl<NewScalar> ReturnType;
88
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 ReturnType res(bounds_.template cast<NewScalar>(),
89 4 weights_.template cast<NewScalar>());
90 4 return res;
91 }
92
93 ✗ const ActivationBounds& get_bounds() const { return bounds_; };
94 ✗ const VectorXs& get_weights() const { return weights_; };
95 ✗ void set_bounds(const ActivationBounds& bounds) { bounds_ = bounds; };
96 ✗ void set_weights(const VectorXs& weights) {
97 ✗ if (weights.size() != weights_.size()) {
98 ✗ throw_pretty("Invalid argument: "
99 << "weight vector has wrong dimension (it should be " +
100 std::to_string(weights_.size()) + ")");
101 }
102 ✗ weights_ = weights;
103 ✗ };
104
105 /**
106 * @brief Print relevant information of the quadratic barrier model
107 *
108 * @param[out] os Output stream object
109 */
110 37 virtual void print(std::ostream& os) const override {
111 37 os << "ActivationModelWeightedQuadraticBarrier {nr=" << nr_ << "}";
112 37 }
113
114 protected:
115 using Base::nr_;
116
117 private:
118 ActivationBounds bounds_;
119 VectorXs weights_;
120 };
121
122 } // namespace crocoddyl
123
124 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
125 crocoddyl::ActivationModelWeightedQuadraticBarrierTpl<double>;
126 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
127 crocoddyl::ActivationModelWeightedQuadraticBarrierTpl<float>;
128
129 #endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_BARRIER_HPP_
130