Crocoddyl
 
Loading...
Searching...
No Matches
smooth-2norm.hpp
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.
8
9#ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
10#define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
11
12#include "crocoddyl/core/activation-base.hpp"
13#include "crocoddyl/core/fwd.hpp"
14
15namespace crocoddyl {
16
31template <typename _Scalar>
33 : public ActivationModelAbstractTpl<_Scalar> {
34 public:
35 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37
38 typedef _Scalar Scalar;
42 typedef typename MathBase::VectorXs VectorXs;
43 typedef typename MathBase::MatrixXs MatrixXs;
44
53 explicit ActivationModelSmooth2NormTpl(const std::size_t nr,
54 const Scalar eps = Scalar(1.))
55 : Base(nr), eps_(eps) {
56 if (eps < Scalar(0.)) {
57 throw_pretty("Invalid argument: " << "eps should be a positive value");
58 }
59 };
60 virtual ~ActivationModelSmooth2NormTpl() = default;
61
68 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>& r) override {
70 if (static_cast<std::size_t>(r.size()) != nr_) {
71 throw_pretty(
72 "Invalid argument: " << "r has wrong dimension (it should be " +
73 std::to_string(nr_) + ")");
74 }
75 using std::sqrt;
76 data->a_value = sqrt(r.squaredNorm() + eps_);
77 };
78
85 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
86 const Eigen::Ref<const VectorXs>& r) override {
87 if (static_cast<std::size_t>(r.size()) != nr_) {
88 throw_pretty(
89 "Invalid argument: " << "r has wrong dimension (it should be " +
90 std::to_string(nr_) + ")");
91 }
92
93 data->Ar = r / data->a_value;
94 data->Arr.diagonal().array() = Scalar(1) / pow(data->a_value, Scalar(3));
95 };
96
102 virtual std::shared_ptr<ActivationDataAbstract> createData() override {
103 return std::allocate_shared<ActivationDataAbstract>(
104 Eigen::aligned_allocator<ActivationDataAbstract>(), this);
105 };
106
107 template <typename NewScalar>
110 ReturnType res(nr_, scalar_cast<NewScalar>(eps_));
111 return res;
112 }
113
114 protected:
120 virtual void print(std::ostream& os) const override {
121 os << "ActivationModelSmooth2Norm {nr=" << nr_ << ", eps=" << eps_ << "}";
122 }
123
124 using Base::nr_;
125 Scalar eps_;
126};
127
128} // namespace crocoddyl
129
130CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
132
133#endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
virtual void calcDiff(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r) override
Compute the derivatives of the smooth-2Norm function.
ActivationModelSmooth2NormTpl(const std::size_t nr, const Scalar eps=Scalar(1.))
Initialize the smooth-2Norm activation model.
virtual std::shared_ptr< ActivationDataAbstract > createData() override
Create the smooth-2norm activation data.
Scalar eps_
< Dimension of the residual vector
virtual void calc(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r) override
Compute the smooth-2Norm function.
virtual void print(std::ostream &os) const override
Print relevant information of the smooth-1norm model.