9 #ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_
12 #include "crocoddyl/core/fwd.hpp"
13 #include "crocoddyl/core/activation-base.hpp"
14 #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;
56 :
Base(nr), alpha_(alpha) {
57 if (alpha < Scalar(0.)) {
58 throw_pretty(
"Invalid argument: "
59 <<
"alpha should be a positive value");
70 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract> &data,
const Eigen::Ref<const VectorXs> &r) {
71 if (
static_cast<std::size_t
>(r.size()) != nr_) {
72 throw_pretty(
"Invalid argument: "
73 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
75 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
77 d->a0 = exp(-r.squaredNorm() / alpha_);
78 data->a_value = Scalar(1.0) - d->a0;
87 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract> &data,
const Eigen::Ref<const VectorXs> &r) {
88 if (
static_cast<std::size_t
>(r.size()) != nr_) {
89 throw_pretty(
"Invalid argument: "
90 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
92 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
94 d->a1 = Scalar(2.0) / alpha_ * d->a0;
96 data->Arr.diagonal() = -Scalar(2.0) * d->a1 * r.array().square() / alpha_;
97 data->Arr.diagonal().array() += d->a1;
105 virtual boost::shared_ptr<ActivationDataAbstract>
createData() {
106 boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
110 Scalar get_alpha()
const {
return alpha_; };
111 void set_alpha(
const Scalar alpha) { alpha_ = alpha; };
118 virtual void print(std::ostream &os)
const {
119 os <<
"ActivationModelQuadFlatExp {nr=" << nr_ <<
", a=" << alpha_ <<
"}";
135 template <
typename _Scalar>
137 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139 typedef _Scalar Scalar;
143 template <
typename Activation>
152 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_EXP_HPP_