GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/activations/smooth-2norm.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 23 26 88.5%
Branches: 7 56 12.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, LAAS-CNRS, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
11
12 #include <stdexcept>
13
14 #include "crocoddyl/core/activation-base.hpp"
15 #include "crocoddyl/core/fwd.hpp"
16
17 namespace crocoddyl {
18
19 /**
20 * @brief Smooth-2Norm activation
21 *
22 * This activation function describes a smooth representation of a 2-norm of a
23 * residual vector, i.e. \f[ \begin{equation} \sqrt{\epsilon + sum^nr_{i=0}
24 * \|r_i\|^2} \end{equation} \f] where \f$\epsilon\f$ defines the smoothing
25 * factor, \f$r_i\f$ is the scalar residual for the \f$i\f$ constraints,
26 * \f$nr\f$ is the dimension of the residual vector.
27 *
28 * The computation of the function and it derivatives are carried out in
29 * `calc()` and `caldDiff()`, respectively.
30 *
31 * \sa `calc()`, `calcDiff()`, `createData()`
32 */
33 template <typename _Scalar>
34 class ActivationModelSmooth2NormTpl
35 : public ActivationModelAbstractTpl<_Scalar> {
36 public:
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 16 CROCODDYL_DERIVED_CAST(ActivationModelBase, ActivationModelSmooth2NormTpl)
39
40 typedef _Scalar Scalar;
41 typedef MathBaseTpl<Scalar> MathBase;
42 typedef ActivationModelAbstractTpl<Scalar> Base;
43 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
44 typedef typename MathBase::VectorXs VectorXs;
45 typedef typename MathBase::MatrixXs MatrixXs;
46
47 /**
48 * @brief Initialize the smooth-2Norm activation model
49 *
50 * The default `eps` value is defined as 1.
51 *
52 * @param[in] nr Dimension of the residual vector
53 * @param[in] eps Smoothing factor (default: 1.)
54 */
55 227 explicit ActivationModelSmooth2NormTpl(const std::size_t nr,
56 const Scalar eps = Scalar(1.))
57 227 : Base(nr), eps_(eps) {
58
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 227 times.
 227 if (eps < Scalar(0.)) {
59 ✗ throw_pretty("Invalid argument: " << "eps should be a positive value");
60 }
61 227 };
62 466 virtual ~ActivationModelSmooth2NormTpl() = default;
63
64 /**
65 * @brief Compute the smooth-2Norm function
66 *
67 * @param[in] data Smooth-2Norm activation data
68 * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
69 */
70 9677 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
71 const Eigen::Ref<const VectorXs>& r) override {
72
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9677 times.
 9677 if (static_cast<std::size_t>(r.size()) != nr_) {
73 ✗ throw_pretty(
74 "Invalid argument: " << "r has wrong dimension (it should be " +
75 std::to_string(nr_) + ")");
76 }
77 using std::sqrt;
78 9677 data->a_value = sqrt(r.squaredNorm() + eps_);
79 9677 };
80
81 /**
82 * @brief Compute the derivatives of the smooth-2Norm function
83 *
84 * @param[in] data Smooth-2Norm activation data
85 * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
86 */
87 305 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
88 const Eigen::Ref<const VectorXs>& r) override {
89
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 305 times.
 305 if (static_cast<std::size_t>(r.size()) != nr_) {
90 ✗ throw_pretty(
91 "Invalid argument: " << "r has wrong dimension (it should be " +
92 std::to_string(nr_) + ")");
93 }
94
95
1/2
✓ Branch 4 taken 305 times.
✗ Branch 5 not taken.
 305 data->Ar = r / data->a_value;
96
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() = Scalar(1) / pow(data->a_value, Scalar(3));
97 305 };
98
99 /**
100 * @brief Create the smooth-2norm activation data
101 *
102 * @return the activation data
103 */
104 7840 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
105 return std::allocate_shared<ActivationDataAbstract>(
106
1/2
✓ Branch 2 taken 7840 times.
✗ Branch 3 not taken.
 15680 Eigen::aligned_allocator<ActivationDataAbstract>(), this);
107 };
108
109 template <typename NewScalar>
110 4 ActivationModelSmooth2NormTpl<NewScalar> cast() const {
111 typedef ActivationModelSmooth2NormTpl<NewScalar> ReturnType;
112 4 ReturnType res(nr_, scalar_cast<NewScalar>(eps_));
113 4 return res;
114 }
115
116 protected:
117 /**
118 * @brief Print relevant information of the smooth-1norm model
119 *
120 * @param[out] os Output stream object
121 */
122 37 virtual void print(std::ostream& os) const override {
123 37 os << "ActivationModelSmooth2Norm {nr=" << nr_ << ", eps=" << eps_ << "}";
124 37 }
125
126 using Base::nr_; //!< Dimension of the residual vector
127 Scalar eps_; //!< Smoothing factor
128 };
129
130 } // namespace crocoddyl
131
132 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
133 crocoddyl::ActivationModelSmooth2NormTpl<double>;
134 extern template class CROCODDYL_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI
135 crocoddyl::ActivationModelSmooth2NormTpl<float>;
136
137 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
138