9 #ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/activation-base.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
33 template <
typename _Scalar>
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 typedef _Scalar Scalar;
43 typedef typename MathBase::VectorXs VectorXs;
44 typedef typename MathBase::MatrixXs MatrixXs;
55 if (eps < Scalar(0.)) {
56 throw_pretty(
"Invalid argument: "
57 <<
"eps should be a positive value");
59 if (eps == Scalar(0.)) {
61 <<
"Warning: eps=0 leads to derivatives discontinuities in the origin, it becomes the absolute function"
73 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
74 if (
static_cast<std::size_t
>(r.size()) != nr_) {
75 throw_pretty(
"Invalid argument: "
76 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
78 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
80 d->a = (r.array().cwiseAbs2().array() +
eps_).array().cwiseSqrt();
81 data->a_value = d->a.sum();
90 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
91 if (
static_cast<std::size_t
>(r.size()) != nr_) {
92 throw_pretty(
"Invalid argument: "
93 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
96 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
97 data->Ar = r.cwiseProduct(d->a.cwiseInverse());
98 data->Arr.diagonal() = d->a.cwiseProduct(d->a).cwiseProduct(d->a).cwiseInverse();
106 virtual boost::shared_ptr<ActivationDataAbstract>
createData() {
107 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
115 virtual void print(std::ostream& os)
const {
116 os <<
"ActivationModelSmooth1Norm {nr=" << nr_ <<
", eps=" <<
eps_ <<
"}";
124 template <
typename _Scalar>
126 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
128 typedef _Scalar Scalar;
131 typedef typename MathBase::VectorXs VectorXs;
132 typedef typename MathBase::MatrixXs MatrixXs;
134 template <
typename Activation>
136 :
Base(activation), a(VectorXs::Zero(activation->get_nr())) {}
144 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_