crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
state.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_COSTS_STATE_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/state-base.hpp"
14 #include "crocoddyl/multibody/cost-base.hpp"
15 #include "crocoddyl/core/utils/deprecate.hpp"
16 
17 namespace crocoddyl {
18 
19 template <typename _Scalar>
20 class CostModelStateTpl : public CostModelAbstractTpl<_Scalar> {
21  public:
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  typedef _Scalar Scalar;
32  typedef typename MathBase::VectorXs VectorXs;
33  typedef typename MathBase::MatrixXs MatrixXs;
34 
35  CostModelStateTpl(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
36  const VectorXs& xref, const std::size_t& nu);
37  CostModelStateTpl(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
38  const VectorXs& xref);
39  CostModelStateTpl(boost::shared_ptr<StateMultibody> state, const VectorXs& xref, const std::size_t& nu);
40  CostModelStateTpl(boost::shared_ptr<StateMultibody> state, const VectorXs& xref);
41  CostModelStateTpl(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
42  const std::size_t& nu);
43  CostModelStateTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nu);
44  CostModelStateTpl(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation);
45  explicit CostModelStateTpl(boost::shared_ptr<StateMultibody> state);
46  virtual ~CostModelStateTpl();
47 
48  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
49  const Eigen::Ref<const VectorXs>& u);
50  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
51  const Eigen::Ref<const VectorXs>& u);
52  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
53 
54  DEPRECATED("Use set_reference<MathBaseTpl<Scalar>::VectorXs>()", void set_xref(const VectorXs& xref_in));
55  DEPRECATED("Use get_reference<MathBaseTpl<Scalar>::VectorXs>()", const VectorXs& get_xref() const);
56 
57  protected:
58  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
59  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
60 
61  using Base::activation_;
62  using Base::nu_;
63  using Base::state_;
64  using Base::unone_;
65 
66  private:
67  VectorXs xref_;
68 };
69 
70 template <typename _Scalar>
71 struct CostDataStateTpl : public CostDataAbstractTpl<_Scalar> {
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
74  typedef _Scalar Scalar;
78  typedef typename MathBase::VectorXs VectorXs;
79  typedef typename MathBase::MatrixXs MatrixXs;
80 
81  template <template <typename Scalar> class Model>
82  CostDataStateTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
83  : Base(model, data), Arr_Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()) {
84  Arr_Rx.setZero();
85  }
86 
87  MatrixXs Arr_Rx;
88 
89  using Base::activation;
90  using Base::cost;
91  using Base::Lu;
92  using Base::Luu;
93  using Base::Lx;
94  using Base::Lxu;
95  using Base::Lxx;
96  using Base::r;
97  using Base::Ru;
98  using Base::Rx;
99  using Base::shared;
100 };
101 
102 } // namespace crocoddyl
103 
104 /* --- Details -------------------------------------------------------------- */
105 /* --- Details -------------------------------------------------------------- */
106 /* --- Details -------------------------------------------------------------- */
107 #include "crocoddyl/multibody/costs/state.hxx"
108 
109 #endif // CROCODDYL_MULTIBODY_COSTS_STATE_HPP_