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);
108 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_2NORM_HPP_ Scalar eps_
< Dimension of the residual vector
ActivationModelSmooth2NormTpl(const std::size_t &nr, const Scalar &eps=Scalar(1.))
Initialize the smooth-2Norm activation model.
virtual boost::shared_ptr< ActivationDataAbstract > createData()
Create the smooth-2Norm activation data.
virtual void calc(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the smooth-2Norm function.
virtual void calcDiff(const boost::shared_ptr< ActivationDataAbstract > &data, const Eigen::Ref< const VectorXs > &r)
Compute the derivatives of the smooth-2Norm function.