9 #ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_ 10 #define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_ 14 #include "crocoddyl/core/fwd.hpp" 15 #include "crocoddyl/core/activation-base.hpp" 16 #include "crocoddyl/core/utils/exception.hpp" 20 template <
typename _Scalar>
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
31 typedef typename MathBase::MatrixXs MatrixXs;
36 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
37 if (static_cast<std::size_t>(r.size()) != nr_) {
38 throw_pretty(
"Invalid argument: " 39 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
41 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
43 d->a = (r.array().cwiseAbs2().array() + Scalar(1)).array().cwiseSqrt();
44 data->a_value = d->a.sum();
47 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
48 if (static_cast<std::size_t>(r.size()) != nr_) {
49 throw_pretty(
"Invalid argument: " 50 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
53 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
54 data->Ar = r.cwiseProduct(d->a.cwiseInverse());
55 data->Arr.diagonal() = d->a.cwiseProduct(d->a).cwiseProduct(d->a).cwiseInverse();
58 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
59 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
66 template <
typename _Scalar>
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 typedef _Scalar Scalar;
73 typedef typename MathBase::VectorXs VectorXs;
74 typedef typename MathBase::MatrixXs MatrixXs;
76 template <
typename Activation>
78 : Base(activation), a(VectorXs::Zero(activation->get_nr())) {
79 Arr.diagonal().array() = Scalar(2);
88 #endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_