crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (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/fwd.hpp"
13 #include "crocoddyl/core/activation-base.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
15 
16 namespace crocoddyl {
17 
32 template <typename _Scalar>
34  public:
35  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 
37  typedef _Scalar Scalar;
42  typedef typename MathBase::VectorXs VectorXs;
43  typedef typename MathBase::MatrixXs MatrixXs;
44 
45  /*
46  * @brief Initialize the quadratic-flat-log activation model
47  *
48  * The default `alpha` value is defined as 1.
49  *
50  * @param[in] nr Dimension of the residual vector
51  * @param[in] alpha Width of quadratic basin (default: 1.)
52  */
53 
54  explicit ActivationModelQuadFlatLogTpl(const std::size_t &nr, const Scalar &alpha = Scalar(1.))
55  : Base(nr), alpha_(alpha) {
56  if (alpha < Scalar(0.)) {
57  throw_pretty("Invalid argument: "
58  << "alpha should be a positive value");
59  }
60  };
61  virtual ~ActivationModelQuadFlatLogTpl(){};
62 
63  /*
64  * @brief Compute the quadratic-flat-log function
65  *
66  * @param[in] data Quadratic-log activation data
67  * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
68  */
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_) + ")");
73  }
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);
77  };
78 
79  /*
80  * @brief Compute the derivatives of the quadratic-flat-log function
81  *
82  * @param[in] data Quadratic-log activation data
83  * @param[in] r Residual vector \f$\mathbf{r}\in\mathbb{R}^{nr}\f$
84  */
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_) + ")");
89  }
90  boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
91 
92  d->a1 = Scalar(2.0) / (alpha_ + alpha_ * d->a0);
93  data->Ar = d->a1 * r;
94  data->Arr.diagonal() = -d->a1 * d->a1 * r.array().square();
95  data->Arr.diagonal().array() += d->a1;
96  };
97 
98  /*
99  * @brief Create the quadratic-flat-log activation data
100  *
101  * @return the activation data
102  */
103  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
104  boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
105  return data;
106  };
107 
108  Scalar get_alpha() const { return alpha_; };
109  void set_alpha(const Scalar alpha) { alpha_ = alpha; };
110 
111  protected:
112  using Base::nr_;
113 
114  private:
115  Scalar alpha_;
116 };
117 
118 /*
119  * @brief Data structure of the quadratic-flat-log activation
120  *
121  * @param[in] a0 computed in calc to avoid recomputation
122  * @param[in] a1 computed in calcDiff to avoid recomputation
123  */
124 template <typename _Scalar>
126  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
127 
128  typedef _Scalar Scalar;
131 
132  template <typename Activation>
133  explicit ActivationDataQuadFlatLogTpl(Activation *const activation) : Base(activation), a0(0), a1(0) {}
134 
135  Scalar a0;
136  Scalar a1;
137 };
138 
139 } // namespace crocoddyl
140 
141 #endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_FLAT_LOG_HPP_