GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/activations/quadratic-flat-log.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 34 39 87.2%
Branches: 13 68 19.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, LAAS-CNRS, University of Edinburgh,
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_QUADRATIC_FLAT_LOG_HPP_
11 #define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
12
13 #include "crocoddyl/core/activation-base.hpp"
14 #include "crocoddyl/core/fwd.hpp"
15
16 namespace crocoddyl {
17
18 /**
19 * @brief Quadratic-flat-log activation
20 *
21 * This activation function describes a logarithmic quadratic activation
22 * depending on the quadratic norm of a residual vector, i.e. \f[
23 * \begin{equation} log(1 + \|\mathbf{r}\|^2 / \alpha) \end{equation} \f] where
24 * \f$\alpha\f$ defines the width of the quadratic basin, \f$r\f$ is the scalar
25 * residual, \f$nr\f$ is the dimension of the residual vector.
26 *
27 * The computation of the function and it derivatives are carried out in
28 * `calc()` and `caldDiff()`, respectively.
29 *
30 * \sa `calc()`, `calcDiff()`, `createData()`
31 */
32 template <typename _Scalar>
33 class ActivationModelQuadFlatLogTpl
34 : public ActivationModelAbstractTpl<_Scalar> {
35 public:
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 16 CROCODDYL_DERIVED_CAST(ActivationModelBase, ActivationModelQuadFlatLogTpl)
38
39 typedef _Scalar Scalar;
40 typedef MathBaseTpl<Scalar> MathBase;
41 typedef ActivationModelAbstractTpl<Scalar> Base;
42 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
43 typedef ActivationDataQuadFlatLogTpl<Scalar> Data;
44 typedef typename MathBase::VectorXs VectorXs;
45 typedef typename MathBase::MatrixXs MatrixXs;
46
47 /*
48 * @brief Initialize the quadratic-flat-log activation model
49 *
50 * The default `alpha` value is defined as 1.
51 *
52 * @param[in] nr Dimension of the residual vector
53 * @param[in] alpha Width of quadratic basin (default: 1.)
54 */
55
56 227 explicit ActivationModelQuadFlatLogTpl(const std::size_t nr,
57 const Scalar alpha = Scalar(1.))
58 227 : Base(nr), alpha_(alpha) {
59
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 227 times.
 227 if (alpha < Scalar(0.)) {
60 ✗ throw_pretty("Invalid argument: " << "alpha should be a positive value");
61 }
62 227 };
63 466 virtual ~ActivationModelQuadFlatLogTpl() = default;
64
65 /*
66 * @brief Compute the quadratic-flat-log function
67 *
68 * @param[in] data Quadratic-log activation data
69 * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
70 */
71 9677 virtual void calc(const std::shared_ptr<ActivationDataAbstract> &data,
72 const Eigen::Ref<const VectorXs> &r) override {
73
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9677 times.
 9677 if (static_cast<std::size_t>(r.size()) != nr_) {
74 ✗ throw_pretty(
75 "Invalid argument: " << "r has wrong dimension (it should be " +
76 std::to_string(nr_) + ")");
77 }
78 9677 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
79
1/2
✓ Branch 1 taken 9677 times.
✗ Branch 2 not taken.
 9677 d->a0 = r.squaredNorm() / alpha_;
80 9677 data->a_value = log(Scalar(1.0) + d->a0);
81 9677 };
82
83 /*
84 * @brief Compute the derivatives of the quadratic-flat-log function
85 *
86 * @param[in] data Quadratic-log activation data
87 * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
88 */
89 305 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract> &data,
90 const Eigen::Ref<const VectorXs> &r) override {
91
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 305 times.
 305 if (static_cast<std::size_t>(r.size()) != nr_) {
92 ✗ throw_pretty(
93 "Invalid argument: " << "r has wrong dimension (it should be " +
94 std::to_string(nr_) + ")");
95 }
96 305 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
97
98 305 d->a1 = Scalar(2.0) / (alpha_ + alpha_ * d->a0);
99
2/4
✓ Branch 2 taken 305 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 305 times.
✗ Branch 7 not taken.
 305 data->Ar = d->a1 * r;
100
4/8
✓ Branch 1 taken 305 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 305 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 305 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 305 times.
✗ Branch 15 not taken.
 305 data->Arr.diagonal() = -d->a1 * d->a1 * r.array().square();
101
2/4
✓ Branch 4 taken 305 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 305 times.
✗ Branch 8 not taken.
 305 data->Arr.diagonal().array() += d->a1;
102 305 };
103
104 /*
105 * @brief Create the quadratic-flat-log activation data
106 *
107 * @return the activation data
108 */
109 7840 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
110
1/2
✓ Branch 1 taken 7840 times.
✗ Branch 2 not taken.
 7840 std::shared_ptr<Data> data =
111 7840 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
112 15680 return data;
113 7840 };
114
115 template <typename NewScalar>
116 4 ActivationModelQuadFlatLogTpl<NewScalar> cast() const {
117 typedef ActivationModelQuadFlatLogTpl<NewScalar> ReturnType;
118 4 ReturnType res(nr_, scalar_cast<NewScalar>(alpha_));
119 4 return res;
120 }
121
122 ✗ Scalar get_alpha() const { return alpha_; };
123 ✗ void set_alpha(const Scalar alpha) { alpha_ = alpha; };
124
125 /**
126 * @brief Print relevant information of the quadratic flat-log model
127 *
128 * @param[out] os Output stream object
129 */
130 37 virtual void print(std::ostream &os) const override {
131 37 os << "ActivationModelQuadFlatLog {nr=" << nr_ << ", a=" << alpha_ << "}";
132 37 }
133
134 protected:
135 using Base::nr_; //!< Dimension of the residual vector
136
137 private:
138 Scalar alpha_; //!< Width of quadratic basin
139 };
140
141 /*
142 * @brief Data structure of the quadratic-flat-log activation
143 *
144 * @param[in] a0 computed in calc to avoid recomputation
145 * @param[in] a1 computed in calcDiff to avoid recomputation
146 */
147 template <typename _Scalar>
148 struct ActivationDataQuadFlatLogTpl
149 : public ActivationDataAbstractTpl<_Scalar> {
150 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
151
152 typedef _Scalar Scalar;
153 typedef MathBaseTpl<Scalar> MathBase;
154 typedef typename MathBase::VectorXs VectorXs;
155 typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
156 typedef ActivationDataAbstractTpl<Scalar> Base;
157
158 template <typename Activation>
159 7840 explicit ActivationDataQuadFlatLogTpl(Activation *const activation)
160 7840 : Base(activation), a0(Scalar(0)), a1(Scalar(0)) {}
161 15680 virtual ~ActivationDataQuadFlatLogTpl() = default;
162
163 Scalar a0;
164 Scalar a1;
165
166 using Base::a_value;
167 using Base::Ar;
168 using Base::Arr;
169 };
170
171 } // namespace crocoddyl
172
173 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
174 crocoddyl::ActivationModelQuadFlatLogTpl)
175 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
176 crocoddyl::ActivationDataQuadFlatLogTpl)
177
178 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
179