crocoddyl  1.8.1
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  friend std::ostream& operator<<(std::ostream& os, const ActivationModelAbstractTpl<Scalar>& model) {
50  model.print(os);
51  return os;
52  }
53 
59  virtual void print(std::ostream& os) const { os << boost::core::demangle(typeid(*this).name()); }
60 
61  protected:
62  std::size_t nr_;
63 };
64 
65 template <typename _Scalar>
67  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 
69  typedef _Scalar Scalar;
71  typedef typename MathBase::VectorXs VectorXs;
72  typedef typename MathBase::MatrixXs MatrixXs;
73  typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
74 
75  template <template <typename Scalar> class Activation>
76  explicit ActivationDataAbstractTpl(Activation<Scalar>* const activation)
77  : a_value(0.), Ar(VectorXs::Zero(activation->get_nr())), Arr(DiagonalMatrixXs(activation->get_nr())) {
78  Arr.setZero();
79  }
80  virtual ~ActivationDataAbstractTpl() {}
81 
82  Scalar a_value;
83  VectorXs Ar;
84  DiagonalMatrixXs Arr;
85 
86  static MatrixXs getHessianMatrix(const ActivationDataAbstractTpl<Scalar>& data) {
87  return data.Arr.diagonal().asDiagonal();
88  }
89  static void setHessianMatrix(ActivationDataAbstractTpl<Scalar>& data, const MatrixXs& Arr) {
90  data.Arr.diagonal() = Arr.diagonal();
91  }
92 };
93 
94 } // namespace crocoddyl
95 
96 #endif // CROCODDYL_CORE_ACTIVATION_BASE_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the activation model.