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 template <
typename _Scalar>
117 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
119 typedef _Scalar Scalar;
122 typedef typename MathBase::VectorXs VectorXs;
123 typedef typename MathBase::MatrixXs MatrixXs;
125 template <
typename Activation>
127 : Base(activation), a(VectorXs::Zero(activation->get_nr())) {}
135 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_1NORM_HPP_
Scalar eps_
< Dimension of the residual vector
virtual boost::shared_ptr< ActivationDataAbstract > createData()
Create the smooth-abs activation data.
ActivationModelSmooth1NormTpl(const std::size_t &nr, const Scalar &eps=Scalar(1.))
Initialize the smooth-abs activation model.
virtual void calc(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the smooth-abs function.
virtual void calcDiff(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the derivatives of the smooth-abs function.