GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/activations/weighted-quadratic-barrier.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 30 40 75.0%
Branches: 40 128 31.2%
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
2/4
✓ Branch 3 taken 184 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 184 times.
✗ Branch 7 not taken.
 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3795 times.
 3795 if (static_cast<std::size_t>(r.size()) != nr_) {
43 ✗ throw_pretty(
44 "Invalid argument: " << "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
4/8
✓ Branch 1 taken 3795 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3795 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3795 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3795 times.
✗ Branch 12 not taken.
 3795 d->rlb_min_ = (r - bounds_.lb).array().min(Scalar(0.));
50
4/8
✓ Branch 1 taken 3795 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3795 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3795 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3795 times.
✗ Branch 12 not taken.
 3795 d->rub_max_ = (r - bounds_.ub).array().max(Scalar(0.));
51
2/4
✓ Branch 1 taken 3795 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3795 times.
✗ Branch 7 not taken.
 3795 d->rlb_min_.array() *= weights_.array();
52
2/4
✓ Branch 1 taken 3795 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3795 times.
✗ Branch 7 not taken.
 3795 d->rub_max_.array() *= weights_.array();
53
2/4
✓ Branch 2 taken 3795 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3795 times.
✗ Branch 6 not taken.
 3795 data->a_value = Scalar(0.5) * d->rlb_min_.matrix().squaredNorm() +
54
2/4
✓ Branch 2 taken 3795 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3795 times.
✗ Branch 6 not taken.
 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 212 times.
 212 if (static_cast<std::size_t>(r.size()) != nr_) {
60 ✗ throw_pretty(
61 "Invalid argument: " << "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
3/6
✓ Branch 3 taken 212 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 212 times.
✗ Branch 11 not taken.
 212 data->Ar = (d->rlb_min_ + d->rub_max_).matrix();
66
3/6
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 212 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 212 times.
✗ Branch 9 not taken.
 212 data->Ar.array() *= weights_.array();
67
68 using pinocchio::internal::if_then_else;
69
3/4
✓ Branch 2 taken 2623 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2411 times.
✓ Branch 5 taken 212 times.
 2623 for (Eigen::Index i = 0; i < data->Arr.cols(); i++) {
70
1/2
✓ Branch 3 taken 2411 times.
✗ Branch 4 not taken.
 2411 data->Arr.diagonal()[i] = if_then_else(
71
2/4
✓ Branch 1 taken 2411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2411 times.
✗ Branch 5 not taken.
 2411 pinocchio::internal::LE, r[i] - bounds_.lb[i], Scalar(0.), Scalar(1.),
72
3/6
✓ Branch 1 taken 2411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2411 times.
✗ Branch 8 not taken.
 2411 if_then_else(pinocchio::internal::GE, r[i] - bounds_.ub[i],
73
1/2
✓ Branch 1 taken 2411 times.
✗ Branch 2 not taken.
 4822 Scalar(0.), Scalar(1.), Scalar(0.)));
74 }
75
76
3/6
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 212 times.
✗ Branch 10 not taken.
 212 data->Arr.diagonal().array() *= weights_.array();
77 212 };
78
79 4665 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
80
1/2
✓ Branch 2 taken 4665 times.
✗ Branch 3 not taken.
 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_
115