9 #ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/activation-base.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
32 template <
typename _Scalar>
35 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 typedef _Scalar Scalar;
41 typedef typename MathBase::VectorXs VectorXs;
42 typedef typename MathBase::MatrixXs MatrixXs;
53 if (eps < Scalar(0.)) {
54 throw_pretty(
"Invalid argument: "
55 <<
"eps should be a positive value");
66 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
67 if (
static_cast<std::size_t
>(r.size()) != nr_) {
68 throw_pretty(
"Invalid argument: "
69 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
72 data->a_value = sqrt(r.squaredNorm() +
eps_);
81 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
82 if (
static_cast<std::size_t
>(r.size()) != nr_) {
83 throw_pretty(
"Invalid argument: "
84 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
87 data->Ar = r / data->a_value;
89 data->Arr.diagonal().array() = Scalar(1) / pow(data->a_value, 3);
97 virtual boost::shared_ptr<ActivationDataAbstract>
createData() {
98 return boost::allocate_shared<ActivationDataAbstract>(Eigen::aligned_allocator<ActivationDataAbstract>(),
this);
107 virtual void print(std::ostream& os)
const {
108 os <<
"ActivationModelSmooth2Norm {nr=" << nr_ <<
", eps=" <<
eps_ <<
"}";
virtual boost::shared_ptr< ActivationDataAbstract > createData()
Create the smooth-2Norm activation data.
ActivationModelSmooth2NormTpl(const std::size_t nr, const Scalar eps=Scalar(1.))
Initialize the smooth-2Norm activation model.
virtual void calc(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the smooth-2Norm function.
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 boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the derivatives of the smooth-2Norm function.