Crocoddyl
 
Loading...
Searching...
No Matches
smooth-2norm.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020, LAAS-CNRS
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 <stdexcept>
13
14#include "crocoddyl/core/activation-base.hpp"
15#include "crocoddyl/core/fwd.hpp"
16#include "crocoddyl/core/utils/exception.hpp"
17
18namespace crocoddyl {
19
34template <typename _Scalar>
36 : public ActivationModelAbstractTpl<_Scalar> {
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39
40 typedef _Scalar Scalar;
44 typedef typename MathBase::VectorXs VectorXs;
45 typedef typename MathBase::MatrixXs MatrixXs;
46
55 explicit ActivationModelSmooth2NormTpl(const std::size_t nr,
56 const Scalar eps = Scalar(1.))
57 : Base(nr), eps_(eps) {
58 if (eps < Scalar(0.)) {
59 throw_pretty("Invalid argument: " << "eps should be a positive value");
60 }
61 };
63
70 virtual void calc(const std::shared_ptr<ActivationDataAbstract>& data,
71 const Eigen::Ref<const VectorXs>& r) {
72 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 data->a_value = sqrt(r.squaredNorm() + eps_);
79 };
80
87 virtual void calcDiff(const std::shared_ptr<ActivationDataAbstract>& data,
88 const Eigen::Ref<const VectorXs>& r) {
89 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 data->Ar = r / data->a_value;
96 using std::pow;
97 data->Arr.diagonal().array() = Scalar(1) / pow(data->a_value, 3);
98 };
99
105 virtual std::shared_ptr<ActivationDataAbstract> createData() {
106 return std::allocate_shared<ActivationDataAbstract>(
107 Eigen::aligned_allocator<ActivationDataAbstract>(), this);
108 };
109
110 protected:
116 virtual void print(std::ostream& os) const {
117 os << "ActivationModelSmooth2Norm {nr=" << nr_ << ", eps=" << eps_ << "}";
118 }
119
120 using Base::nr_;
121 Scalar eps_;
122};
123
124} // namespace crocoddyl
125
126#endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
ActivationModelSmooth2NormTpl(const std::size_t nr, const Scalar eps=Scalar(1.))
Initialize the smooth-2Norm activation model.
virtual void print(std::ostream &os) const
Print relevant information of the smooth-1norm model.
Scalar eps_
< Dimension of the residual vector
virtual void calcDiff(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the derivatives of the smooth-2Norm function.
virtual void calc(const std::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the smooth-2Norm function.
virtual std::shared_ptr< ActivationDataAbstract > createData()
Create the smooth-2Norm activation data.