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