crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
activation-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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_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 
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/mathbase.hpp"
18 #include "crocoddyl/core/utils/to-string.hpp"
19 
20 namespace crocoddyl {
21 
22 template <typename _Scalar>
24  public:
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 
27  typedef _Scalar Scalar;
30  typedef typename MathBase::VectorXs VectorXs;
31  typedef typename MathBase::MatrixXs MatrixXs;
32 
33  explicit ActivationModelAbstractTpl(const std::size_t nr) : nr_(nr){};
34  virtual ~ActivationModelAbstractTpl(){};
35 
36  virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const VectorXs>& r) = 0;
37  virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
38  const Eigen::Ref<const VectorXs>& r) = 0;
39  virtual boost::shared_ptr<ActivationDataAbstract> createData() {
40  return boost::allocate_shared<ActivationDataAbstract>(Eigen::aligned_allocator<ActivationDataAbstract>(), this);
41  };
42 
43  std::size_t get_nr() const { return nr_; };
44 
45  protected:
46  std::size_t nr_;
47 };
48 
49 template <typename _Scalar>
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 
53  typedef _Scalar Scalar;
55  typedef typename MathBase::VectorXs VectorXs;
56  typedef typename MathBase::MatrixXs MatrixXs;
57  typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
58 
59  template <template <typename Scalar> class Activation>
60  explicit ActivationDataAbstractTpl(Activation<Scalar>* const activation)
61  : a_value(0.), Ar(VectorXs::Zero(activation->get_nr())), Arr(DiagonalMatrixXs(activation->get_nr())) {
62  Arr.setZero();
63  }
64  virtual ~ActivationDataAbstractTpl() {}
65 
66  Scalar a_value;
67  VectorXs Ar;
68  DiagonalMatrixXs Arr;
69 
70  static MatrixXs getHessianMatrix(const ActivationDataAbstractTpl<Scalar>& data) {
71  return data.Arr.diagonal().asDiagonal();
72  }
73  static void setHessianMatrix(ActivationDataAbstractTpl<Scalar>& data, const MatrixXs& Arr) {
74  data.Arr.diagonal() = Arr.diagonal();
75  }
76 };
77 
78 } // namespace crocoddyl
79 
80 #endif // CROCODDYL_CORE_ACTIVATION_BASE_HPP_