crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
activation-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIVATION_BASE_HPP_
10 #define CROCODDYL_CORE_ACTIVATION_BASE_HPP_
11 
12 #include <stdexcept>
13 #include <boost/shared_ptr.hpp>
14 #include <boost/make_shared.hpp>
15 #include <boost/core/demangle.hpp>
16 
17 #include "crocoddyl/core/fwd.hpp"
18 #include "crocoddyl/core/mathbase.hpp"
19 #include "crocoddyl/core/utils/to-string.hpp"
20 
21 namespace crocoddyl {
22 
23 template <typename _Scalar>
25  public:
26  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 
28  typedef _Scalar Scalar;
31  typedef typename MathBase::VectorXs VectorXs;
32  typedef typename MathBase::MatrixXs MatrixXs;
33 
34  explicit ActivationModelAbstractTpl(const std::size_t nr) : nr_(nr){};
35  virtual ~ActivationModelAbstractTpl(){};
36 
37  virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) = 0;
38  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
39  const Eigen::Ref<const VectorXs>& r) = 0;
40  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
41  return boost::allocate_shared<ActivationDataAbstract>(Eigen::aligned_allocator<ActivationDataAbstract>(), this);
42  };
43 
44  std::size_t get_nr() const { return nr_; };
45 
49  template <class Scalar>
50  friend std::ostream& operator<<(std::ostream& os, const ActivationModelAbstractTpl<Scalar>& model) {
51  model.print(os);
52  return os;
53  }
54 
60  virtual void print(std::ostream& os) const { os << boost::core::demangle(typeid(*this).name()); }
61 
62  protected:
63  std::size_t nr_;
64 };
65 
66 template <typename _Scalar>
68  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 
70  typedef _Scalar Scalar;
72  typedef typename MathBase::VectorXs VectorXs;
73  typedef typename MathBase::MatrixXs MatrixXs;
74  typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
75 
76  template <template <typename Scalar> class Activation>
77  explicit ActivationDataAbstractTpl(Activation<Scalar>* const activation)
78  : a_value(0.), Ar(VectorXs::Zero(activation->get_nr())), Arr(DiagonalMatrixXs(activation->get_nr())) {
79  Arr.setZero();
80  }
81  virtual ~ActivationDataAbstractTpl() {}
82 
83  Scalar a_value;
84  VectorXs Ar;
85  DiagonalMatrixXs Arr;
86 
87  static MatrixXs getHessianMatrix(const ActivationDataAbstractTpl<Scalar>& data) {
88  return data.Arr.diagonal().asDiagonal();
89  }
90  static void setHessianMatrix(ActivationDataAbstractTpl<Scalar>& data, const MatrixXs& Arr) {
91  data.Arr.diagonal() = Arr.diagonal();
92  }
93 };
94 
95 } // namespace crocoddyl
96 
97 #endif // CROCODDYL_CORE_ACTIVATION_BASE_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the activation model.