9 #ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
12 #include "crocoddyl/core/fwd.hpp"
13 #include "crocoddyl/core/activation-base.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
32 template <
typename _Scalar>
35 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 typedef _Scalar Scalar;
42 typedef typename MathBase::VectorXs VectorXs;
43 typedef typename MathBase::MatrixXs MatrixXs;
55 :
Base(nr), alpha_(alpha) {
56 if (alpha < Scalar(0.)) {
57 throw_pretty(
"Invalid argument: "
58 <<
"alpha should be a positive value");
69 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract> &data,
const Eigen::Ref<const VectorXs> &r) {
70 if (
static_cast<std::size_t
>(r.size()) != nr_) {
71 throw_pretty(
"Invalid argument: "
72 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
74 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
75 d->a0 = r.squaredNorm() / alpha_;
76 data->a_value = log(Scalar(1.0) + d->a0);
85 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract> &data,
const Eigen::Ref<const VectorXs> &r) {
86 if (
static_cast<std::size_t
>(r.size()) != nr_) {
87 throw_pretty(
"Invalid argument: "
88 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
90 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
92 d->a1 = Scalar(2.0) / (alpha_ + alpha_ * d->a0);
94 data->Arr.diagonal() = -d->a1 * d->a1 * r.array().square();
95 data->Arr.diagonal().array() += d->a1;
103 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
104 boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
108 Scalar get_alpha()
const {
return alpha_; };
109 void set_alpha(
const Scalar alpha) { alpha_ = alpha; };
116 virtual void print(std::ostream &os)
const {
117 os <<
"ActivationModelQuadFlatLog {nr=" << nr_ <<
", a=" << alpha_ <<
"}";
133 template <
typename _Scalar>
135 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
137 typedef _Scalar Scalar;
141 template <
typename Activation>
150 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_