Crocoddyl
 
Loading...
Searching...
No Matches
quadratic-flat-log.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_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
16namespace crocoddyl {
17
32template <typename _Scalar>
34 : public ActivationModelAbstractTpl<_Scalar> {
35 public:
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38
39 typedef _Scalar Scalar;
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 explicit ActivationModelQuadFlatLogTpl(const std::size_t nr,
57 const Scalar alpha = Scalar(1.))
58 : Base(nr), alpha_(alpha) {
59 if (alpha < Scalar(0.)) {
60 throw_pretty("Invalid argument: " << "alpha should be a positive value");
61 }
62 };
63 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 virtual void calc(const std::shared_ptr<ActivationDataAbstract> &data,
72 const Eigen::Ref<const VectorXs> &r) override {
73 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 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
79 d->a0 = r.squaredNorm() / alpha_;
80 data->a_value = log(Scalar(1.0) + d->a0);
81 };
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 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract> &data,
90 const Eigen::Ref<const VectorXs> &r) override {
91 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 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
97
98 d->a1 = Scalar(2.0) / (alpha_ + alpha_ * d->a0);
99 data->Ar = d->a1 * r;
100 data->Arr.diagonal() = -d->a1 * d->a1 * r.array().square();
101 data->Arr.diagonal().array() += d->a1;
102 };
103
104 /*
105 * @brief Create the quadratic-flat-log activation data
106 *
107 * @return the activation data
108 */
109 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
110 std::shared_ptr<Data> data =
111 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
112 return data;
113 };
114
115 template <typename NewScalar>
118 ReturnType res(nr_, scalar_cast<NewScalar>(alpha_));
119 return res;
120 }
121
122 Scalar get_alpha() const { return alpha_; };
123 void set_alpha(const Scalar alpha) { alpha_ = alpha; };
124
130 virtual void print(std::ostream &os) const override {
131 os << "ActivationModelQuadFlatLog {nr=" << nr_ << ", a=" << alpha_ << "}";
132 }
133
134 protected:
135 using Base::nr_;
136
137 private:
138 Scalar alpha_;
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 */
147template <typename _Scalar>
149 : public ActivationDataAbstractTpl<_Scalar> {
150 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
151
152 typedef _Scalar Scalar;
154 typedef typename MathBase::VectorXs VectorXs;
155 typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
157
158 template <typename Activation>
159 explicit ActivationDataQuadFlatLogTpl(Activation *const activation)
160 : Base(activation), a0(Scalar(0)), a1(Scalar(0)) {}
161 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
173CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
175CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
177
178#endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
virtual void print(std::ostream &os) const override
Print relevant information of the quadratic flat-log model.