Crocoddyl
3 //
4 // Copyright (C) 2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
8
11
12 #include "crocoddyl/core/activation-base.hpp"
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
15
16 namespace crocoddyl {
17
18 /*
20  *
21  * This activation function describes a quadratic exponential activation
22  * depending on the square norm of a residual vector, i.e. \f[
23  * 1 - exp(\|\mathbf{r}\|^2 / \alpha) \f] where \f$\alpha\f$
24  * defines the width of the quadratic basin, \f$r\f$ is the scalar residual,
25  * \f$nr\f$ is the dimension of the residual vector. Far
26  * away from zero, the quadFlat activation is nearly flat.
27  *
28  * The computation of the function and it derivatives are carried out in
29  * calc() and caldDiff(), respectively.
30  *
31  * \sa calc(), calcDiff(), createData()
32  */
33 template <typename _Scalar>
35  : public ActivationModelAbstractTpl<_Scalar> {
36  public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38
39  typedef _Scalar Scalar;
44  typedef typename MathBase::VectorXs VectorXs;
45  typedef typename MathBase::MatrixXs MatrixXs;
46
47  /*
48  * @brief Initialize the quadratic-flat-exp activation model
49  *
50  * The default alpha value is defined as 1.
51  *
52  * @param[in] nr Dimension of the residual vector
53  * @param[in] alpha Width of quadratic basin (default: 1.)
54  */
55
57  const Scalar &alpha = Scalar(1.))
58  : Base(nr), alpha_(alpha) {
59  if (alpha < Scalar(0.)) {
60  throw_pretty("Invalid argument: "
61  << "alpha should be a positive value");
62  }
63  };
65
66  /*
67  * @brief Compute the quadratic-flat-exp function
68  *
69  * @param[in] data Quadratic-flat activation data
70  * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
71  */
72  virtual void calc(const boost::shared_ptr<ActivationDataAbstract> &data,
73  const Eigen::Ref<const VectorXs> &r) {
74  if (static_cast<std::size_t>(r.size()) != nr_) {
75  throw_pretty("Invalid argument: "
76  << "r has wrong dimension (it should be " +
77  std::to_string(nr_) + ")");
78  }
79  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
80
81  d->a0 = exp(-r.squaredNorm() / alpha_);
82  data->a_value = Scalar(1.0) - d->a0;
83  };
84
85  /*
86  * @brief Compute the derivatives of the quadratic-flat-exp function
87  *
88  * @param[in] data Quadratic-flat activation data
89  * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
90  */
91  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract> &data,
92  const Eigen::Ref<const VectorXs> &r) {
93  if (static_cast<std::size_t>(r.size()) != nr_) {
94  throw_pretty("Invalid argument: "
95  << "r has wrong dimension (it should be " +
96  std::to_string(nr_) + ")");
97  }
98  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
99
100  d->a1 = Scalar(2.0) / alpha_ * d->a0;
101  data->Ar = d->a1 * r;
102  data->Arr.diagonal() = -Scalar(2.0) * d->a1 * r.array().square() / alpha_;
103  data->Arr.diagonal().array() += d->a1;
104  };
105
111  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
112  boost::shared_ptr<Data> data =
113  boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
114  return data;
115  };
116
117  Scalar get_alpha() const { return alpha_; };
118  void set_alpha(const Scalar alpha) { alpha_ = alpha; };
119
125  virtual void print(std::ostream &os) const {
126  os << "ActivationModelQuadFlatExp {nr=" << nr_ << ", a=" << alpha_ << "}";
127  }
128
129  protected:
130  using Base::nr_;
131
132  private:
133  Scalar alpha_;
134 };
135
136 /*
137  * @brief Data structure of the quadratic-flat-exp activation
138  *
139  * @param[in] a0 computed in calc to avoid recomputation
140  * @param[in] a1 computed in calcDiff to avoid recomputation
141  */
142 template <typename _Scalar>
144  : public ActivationDataAbstractTpl<_Scalar> {
145  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146
147  typedef _Scalar Scalar;
150
151  template <typename Activation>