Crocoddyl
 
Loading...
Searching...
No Matches
quadratic-flat-exp.hpp
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.
9
10#ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
11#define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
12
13#include "crocoddyl/core/activation-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15
16namespace crocoddyl {
17
18/*
19 * @brief Quadratic-flat-exp activation
20 *
21 * This activation function describes a quadratic exponential activation
22 * depending on the square norm of a residual vector, i.e. \f[ \begin{equation}
23 * 1 - exp(\|\mathbf{r}\|^2 / \alpha) \end{equation} \f] where \f$\alpha\f$
24 * defines the width of the quadratic basin, \f$r\f$ is the scalar residual,
25 * \f$nr\f$ is the dimension of the residual vector. Far
26 * away from zero, the quadFlat activation is nearly flat.
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 */
33template <typename _Scalar>
35 : public ActivationModelAbstractTpl<_Scalar> {
36 public:
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39
40 typedef _Scalar Scalar;
45 typedef typename MathBase::VectorXs VectorXs;
46 typedef typename MathBase::MatrixXs MatrixXs;
47
48 /*
49 * @brief Initialize the quadratic-flat-exp activation model
50 *
51 * The default `alpha` value is defined as 1.
52 *
53 * @param[in] nr Dimension of the residual vector
54 * @param[in] alpha Width of quadratic basin (default: 1.)
55 */
56
57 explicit ActivationModelQuadFlatExpTpl(const std::size_t nr,
58 const Scalar alpha = Scalar(1.))
59 : Base(nr), alpha_(alpha) {
60 if (alpha < Scalar(0.)) {
61 throw_pretty("Invalid argument: " << "alpha should be a positive value");
62 }
63 };
64 virtual ~ActivationModelQuadFlatExpTpl() = default;
65
66 /*
67 * @brief Compute the quadratic-flat-exp function
68 *
69 * @param[in] data Quadratic-flat activation data
70 * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
71 */
72 virtual void calc(const std::shared_ptr<ActivationDataAbstract> &data,
73 const Eigen::Ref<const VectorXs> &r) override {
74 if (static_cast<std::size_t>(r.size()) != nr_) {
75 throw_pretty(
76 "Invalid argument: " << "r has wrong dimension (it should be " +
77 std::to_string(nr_) + ")");
78 }
79 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
80
81 d->a0 = exp(-r.squaredNorm() / alpha_);
82 data->a_value = Scalar(1.0) - d->a0;
83 };
84
85 /*
86 * @brief Compute the derivatives of the quadratic-flat-exp function
87 *
88 * @param[in] data Quadratic-flat activation data
89 * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
90 */
91 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract> &data,
92 const Eigen::Ref<const VectorXs> &r) override {
93 if (static_cast<std::size_t>(r.size()) != nr_) {
94 throw_pretty(
95 "Invalid argument: " << "r has wrong dimension (it should be " +
96 std::to_string(nr_) + ")");
97 }
98 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
99
100 d->a1 = Scalar(2.0) / alpha_ * d->a0;
101 data->Ar = d->a1 * r;
102 data->Arr.diagonal() = -Scalar(2.0) * d->a1 * r.array().square() / alpha_;
103 data->Arr.diagonal().array() += d->a1;
104 };
105
111 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
112 std::shared_ptr<Data> data =
113 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
114 return data;
115 };
116
117 template <typename NewScalar>
120 ReturnType res(nr_, scalar_cast<NewScalar>(alpha_));
121 return res;
122 }
123
124 Scalar get_alpha() const { return alpha_; };
125 void set_alpha(const Scalar alpha) { alpha_ = alpha; };
126
132 virtual void print(std::ostream &os) const override {
133 os << "ActivationModelQuadFlatExp {nr=" << nr_ << ", a=" << alpha_ << "}";
134 }
135
136 protected:
137 using Base::nr_;
138
139 private:
140 Scalar alpha_;
141};
142
143/*
144 * @brief Data structure of the quadratic-flat-exp activation
145 *
146 * @param[in] a0 computed in calc to avoid recomputation
147 * @param[in] a1 computed in calcDiff to avoid recomputation
148 */
149template <typename _Scalar>
151 : public ActivationDataAbstractTpl<_Scalar> {
152 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153
154 typedef _Scalar Scalar;
156 typedef typename MathBase::VectorXs VectorXs;
157 typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
159
160 template <typename Activation>
161 explicit ActivationDataQuadFlatExpTpl(Activation *const activation)
162 : Base(activation), a0(Scalar(0)), a1(Scalar(0)) {}
163 virtual ~ActivationDataQuadFlatExpTpl() = default;
164
165 Scalar a0;
166 Scalar a1;
167
168 using Base::a_value;
169 using Base::Ar;
170 using Base::Arr;
171};
172
173} // namespace crocoddyl
174
175CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
177CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
179
180#endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
virtual std::shared_ptr< ActivationDataAbstract > createData() override
Create the quadratic-flat-exp activation data.
virtual void print(std::ostream &os) const override
Print relevant information of the quadratic flat-exp model.