Crocoddyl
quadratic-flat-log.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
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 
32 template <typename _Scalar>
34  : public ActivationModelAbstractTpl<_Scalar> {
35  public:
36  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 
38  typedef _Scalar Scalar;
43  typedef typename MathBase::VectorXs VectorXs;
44  typedef typename MathBase::MatrixXs MatrixXs;
45 
46  /*
47  * @brief Initialize the quadratic-flat-log activation model
48  *
49  * The default `alpha` value is defined as 1.
50  *
51  * @param[in] nr Dimension of the residual vector
52  * @param[in] alpha Width of quadratic basin (default: 1.)
53  */
54 
55  explicit ActivationModelQuadFlatLogTpl(const std::size_t &nr,
56  const Scalar &alpha = Scalar(1.))
57  : Base(nr), alpha_(alpha) {
58  if (alpha < Scalar(0.)) {
59  throw_pretty("Invalid argument: " << "alpha should be a positive value");
60  }
61  };
62  virtual ~ActivationModelQuadFlatLogTpl() {};
63 
64  /*
65  * @brief Compute the quadratic-flat-log function
66  *
67  * @param[in] data Quadratic-log activation data
68  * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
69  */
70  virtual void calc(const boost::shared_ptr<ActivationDataAbstract> &data,
71  const Eigen::Ref<const VectorXs> &r) {
72  if (static_cast<std::size_t>(r.size()) != nr_) {
73  throw_pretty(
74  "Invalid argument: " << "r has wrong dimension (it should be " +
75  std::to_string(nr_) + ")");
76  }
77  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
78  d->a0 = r.squaredNorm() / alpha_;
79  data->a_value = log(Scalar(1.0) + d->a0);
80  };
81 
82  /*
83  * @brief Compute the derivatives of the quadratic-flat-log function
84  *
85  * @param[in] data Quadratic-log activation data
86  * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
87  */
88  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract> &data,
89  const Eigen::Ref<const VectorXs> &r) {
90  if (static_cast<std::size_t>(r.size()) != nr_) {
91  throw_pretty(
92  "Invalid argument: " << "r has wrong dimension (it should be " +
93  std::to_string(nr_) + ")");
94  }
95  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
96 
97  d->a1 = Scalar(2.0) / (alpha_ + alpha_ * d->a0);
98  data->Ar = d->a1 * r;
99  data->Arr.diagonal() = -d->a1 * d->a1 * r.array().square();
100  data->Arr.diagonal().array() += d->a1;
101  };
102 
103  /*
104  * @brief Create the quadratic-flat-log activation data
105  *
106  * @return the activation data
107  */
108  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
109  boost::shared_ptr<Data> data =
110  boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
111  return data;
112  };
113 
114  Scalar get_alpha() const { return alpha_; };
115  void set_alpha(const Scalar alpha) { alpha_ = alpha; };
116 
122  virtual void print(std::ostream &os) const {
123  os << "ActivationModelQuadFlatLog {nr=" << nr_ << ", a=" << alpha_ << "}";
124  }
125 
126  protected:
127  using Base::nr_;
128 
129  private:
130  Scalar alpha_;
131 };
132 
133 /*
134  * @brief Data structure of the quadratic-flat-log activation
135  *
136  * @param[in] a0 computed in calc to avoid recomputation
137  * @param[in] a1 computed in calcDiff to avoid recomputation
138  */
139 template <typename _Scalar>
141  : public ActivationDataAbstractTpl<_Scalar> {
142  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 
144  typedef _Scalar Scalar;
147 
148  template <typename Activation>
149  explicit ActivationDataQuadFlatLogTpl(Activation *const activation)
150  : Base(activation), a0(0), a1(0) {}
151 
152  Scalar a0;
153  Scalar a1;
154 };
155 
156 } // namespace crocoddyl
157 
158 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the quadratic flat-log model.