crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
state.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_MULTIBODY_COSTS_STATE_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
11 
12 #include "crocoddyl/core/fwd.hpp"
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/core/state-base.hpp"
15 #include "crocoddyl/core/cost-base.hpp"
16 #include "crocoddyl/multibody/states/multibody.hpp"
17 #include "crocoddyl/core/utils/deprecate.hpp"
18 
19 namespace crocoddyl {
20 
38 template <typename _Scalar>
39 class CostModelStateTpl : public CostModelAbstractTpl<_Scalar> {
40  public:
41  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 
43  typedef _Scalar Scalar;
50  typedef typename MathBase::VectorXs VectorXs;
51  typedef typename MathBase::MatrixXs MatrixXs;
52 
61  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state,
62  boost::shared_ptr<ActivationModelAbstract> activation, const VectorXs& xref, const std::size_t nu);
63 
73  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state,
74  boost::shared_ptr<ActivationModelAbstract> activation, const VectorXs& xref);
75 
85  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref, const std::size_t nu);
86 
96  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref);
97 
107  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state,
108  boost::shared_ptr<ActivationModelAbstract> activation, const std::size_t nu);
109 
119  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const std::size_t nu);
120 
130  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state,
131  boost::shared_ptr<ActivationModelAbstract> activation);
132 
142  explicit CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state);
143  virtual ~CostModelStateTpl();
144 
152  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
153  const Eigen::Ref<const VectorXs>& u);
154 
162  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
163  const Eigen::Ref<const VectorXs>& u);
164 
168  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
169 
170  DEPRECATED("Use set_reference<MathBaseTpl<Scalar>::VectorXs>()", void set_xref(const VectorXs& xref_in));
171  DEPRECATED("Use get_reference<MathBaseTpl<Scalar>::VectorXs>()", const VectorXs& get_xref() const);
172 
173  protected:
177  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
178 
182  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
183 
184  using Base::activation_;
185  using Base::nu_;
186  using Base::state_;
187  using Base::unone_;
188 
189  private:
190  VectorXs xref_;
191  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
192 };
193 
194 template <typename _Scalar>
195 struct CostDataStateTpl : public CostDataAbstractTpl<_Scalar> {
196  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
197 
198  typedef _Scalar Scalar;
202  typedef typename MathBase::MatrixXs MatrixXs;
203 
204  template <template <typename Scalar> class Model>
205  CostDataStateTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
206  : Base(model, data), Arr_Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()) {
207  Arr_Rx.setZero();
208  }
209 
210  MatrixXs Arr_Rx;
211 
212  using Base::activation;
213  using Base::cost;
214  using Base::Lu;
215  using Base::Luu;
216  using Base::Lx;
217  using Base::Lxu;
218  using Base::Lxx;
219  using Base::r;
220  using Base::Ru;
221  using Base::Rx;
222  using Base::shared;
223 };
224 
225 } // namespace crocoddyl
226 
227 /* --- Details -------------------------------------------------------------- */
228 /* --- Details -------------------------------------------------------------- */
229 /* --- Details -------------------------------------------------------------- */
230 #include "crocoddyl/multibody/costs/state.hxx"
231 
232 #endif // CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the state cost.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
State multibody representation.
Definition: fwd.hpp:215
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the state reference.
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the state reference.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the state cost data.
CostModelStateTpl(boost::shared_ptr< typename Base::StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, const VectorXs &xref, const std::size_t nu)
Initialize the state cost model.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the state cost.