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_ <<
"}";
117 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_