crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_COST_BASE_HPP_
10 #define CROCODDYL_MULTIBODY_COST_BASE_HPP_
11 
12 #include <boost/shared_ptr.hpp>
13 #include <boost/make_shared.hpp>
14 
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/multibody/states/multibody.hpp"
17 #include "crocoddyl/core/data-collector-base.hpp"
18 #include "crocoddyl/core/activation-base.hpp"
19 #include "crocoddyl/core/utils/to-string.hpp"
20 #include "crocoddyl/core/activations/quadratic.hpp"
21 
22 namespace crocoddyl {
23 
24 template <typename _Scalar>
26  public:
27  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 
29  typedef _Scalar Scalar;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
38 
39  CostModelAbstractTpl(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
40  const std::size_t& nu);
41  CostModelAbstractTpl(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation);
42  CostModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nr, const std::size_t& nu);
43  CostModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nr);
45 
46  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
47  const Eigen::Ref<const VectorXs>& u) = 0;
48  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
49  const Eigen::Ref<const VectorXs>& u) = 0;
50  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
51 
52  void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
53  void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
54 
55  const boost::shared_ptr<StateMultibody>& get_state() const;
56  const boost::shared_ptr<ActivationModelAbstract>& get_activation() const;
57  const std::size_t& get_nu() const;
58 
59  template <class ReferenceType>
60  void set_reference(ReferenceType ref);
61 
62  template <class ReferenceType>
63  ReferenceType get_reference() const;
64 
65  protected:
66  virtual void set_referenceImpl(const std::type_info&, const void*);
67  virtual void get_referenceImpl(const std::type_info&, void*) const;
68 
69  boost::shared_ptr<StateMultibody> state_;
70  boost::shared_ptr<ActivationModelAbstract> activation_;
71  std::size_t nu_;
72  VectorXs unone_;
73 };
74 
75 template <typename _Scalar>
77  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78 
79  typedef _Scalar Scalar;
83  typedef typename MathBase::VectorXs VectorXs;
84  typedef typename MathBase::MatrixXs MatrixXs;
85 
86  template <template <typename Scalar> class Model>
87  CostDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
88  : shared(data),
89  activation(model->get_activation()->createData()),
90  cost(Scalar(0.)),
91  Lx(model->get_state()->get_ndx()),
92  Lu(model->get_nu()),
93  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
94  Lxu(model->get_state()->get_ndx(), model->get_nu()),
95  Luu(model->get_nu(), model->get_nu()),
96  r(model->get_activation()->get_nr()),
97  Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()),
98  Ru(model->get_activation()->get_nr(), model->get_nu()) {
99  Lx.setZero();
100  Lu.setZero();
101  Lxx.setZero();
102  Lxu.setZero();
103  Luu.setZero();
104  r.setZero();
105  Rx.setZero();
106  Ru.setZero();
107  }
108  virtual ~CostDataAbstractTpl() {}
109 
110  DataCollectorAbstract* shared;
111  boost::shared_ptr<ActivationDataAbstract> activation;
112  Scalar cost;
113  VectorXs Lx;
114  VectorXs Lu;
115  MatrixXs Lxx;
116  MatrixXs Lxu;
117  MatrixXs Luu;
118  VectorXs r;
119  MatrixXs Rx;
120  MatrixXs Ru;
121 };
122 
123 } // namespace crocoddyl
124 
125 /* --- Details -------------------------------------------------------------- */
126 /* --- Details -------------------------------------------------------------- */
127 /* --- Details -------------------------------------------------------------- */
128 #include "crocoddyl/multibody/cost-base.hxx"
129 
130 #endif // CROCODDYL_MULTIBODY_COST_BASE_HPP_