Crocoddyl
 
Loading...
Searching...
No Matches
smooth-1norm.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-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_SMOOTH_1NORM_HPP_
11#define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
12
13#include "crocoddyl/core/activation-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15
16namespace crocoddyl {
17
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
56 explicit ActivationModelSmooth1NormTpl(const std::size_t nr,
57 const Scalar eps = Scalar(1.))
58 : Base(nr), eps_(eps) {
59 if (eps < Scalar(0.)) {
60 throw_pretty("Invalid argument: " << "eps should be a positive value");
61 }
62 if (eps == Scalar(0.)) {
63 std::cerr << "Warning: eps=0 leads to derivatives discontinuities in the "
64 "origin, it becomes the absolute function"
65 << std::endl;
66 }
67 };
68 virtual ~ActivationModelSmooth1NormTpl() = default;
69
76 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
77 const Eigen::Ref<const VectorXs>& r) override {
78 if (static_cast<std::size_t>(r.size()) != nr_) {
79 throw_pretty(
80 "Invalid argument: " << "r has wrong dimension (it should be " +
81 std::to_string(nr_) + ")");
82 }
83 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
84
85 d->a = (r.array().cwiseAbs2().array() + eps_).array().cwiseSqrt();
86 data->a_value = d->a.sum();
87 };
88
95 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
96 const Eigen::Ref<const VectorXs>& r) override {
97 if (static_cast<std::size_t>(r.size()) != nr_) {
98 throw_pretty(
99 "Invalid argument: " << "r has wrong dimension (it should be " +
100 std::to_string(nr_) + ")");
101 }
102
103 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
104 data->Ar = r.cwiseProduct(d->a.cwiseInverse());
105 data->Arr.diagonal() =
106 d->a.cwiseProduct(d->a).cwiseProduct(d->a).cwiseInverse();
107 };
108
114 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
115 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
116 };
117
118 template <typename NewScalar>
121 ReturnType res(nr_, scalar_cast<NewScalar>(eps_));
122 return res;
123 }
124
130 virtual void print(std::ostream& os) const override {
131 os << "ActivationModelSmooth1Norm {nr=" << nr_ << ", eps=" << eps_ << "}";
132 }
133
134 protected:
135 using Base::nr_;
136 Scalar eps_;
137};
138
139template <typename _Scalar>
141 : public ActivationDataAbstractTpl<_Scalar> {
142 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143
144 typedef _Scalar Scalar;
147 typedef typename MathBase::VectorXs VectorXs;
148 typedef typename MathBase::MatrixXs MatrixXs;
149 typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
150
151 template <typename Activation>
152 explicit ActivationDataSmooth1NormTpl(Activation* const activation)
153 : Base(activation), a(VectorXs::Zero(activation->get_nr())) {}
154 virtual ~ActivationDataSmooth1NormTpl() = default;
155
156 VectorXs a;
157
158 using Base::a_value;
159 using Base::Ar;
160 using Base::Arr;
161};
162
163} // namespace crocoddyl
164
165CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
167CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
169
170#endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
virtual void calcDiff(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r) override
Compute the derivatives of the smooth-abs function.
virtual std::shared_ptr< ActivationDataAbstract > createData() override
Create the smooth-abs activation data.
Scalar eps_
< Dimension of the residual vector
ActivationModelSmooth1NormTpl(const std::size_t nr, const Scalar eps=Scalar(1.))
Initialize the smooth-abs activation model.
virtual void calc(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r) override
Compute the smooth-abs function.
virtual void print(std::ostream &os) const override
Print relevant information of the smooth-1norm model.