crocoddyl  1.6.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,
63  const std::size_t& nu);
64 
74  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state,
75  boost::shared_ptr<ActivationModelAbstract> activation, const VectorXs& xref);
76 
86  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref,
87  const std::size_t& nu);
88 
98  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref);
99 
109  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state,
110  boost::shared_ptr<ActivationModelAbstract> activation, const std::size_t& nu);
111 
121  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state, const std::size_t& nu);
122 
132  CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state,
133  boost::shared_ptr<ActivationModelAbstract> activation);
134 
144  explicit CostModelStateTpl(boost::shared_ptr<typename Base::StateAbstract> state);
145  virtual ~CostModelStateTpl();
146 
154  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
155  const Eigen::Ref<const VectorXs>& u);
156 
164  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
165  const Eigen::Ref<const VectorXs>& u);
166 
170  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
171 
172  DEPRECATED("Use set_reference<MathBaseTpl<Scalar>::VectorXs>()", void set_xref(const VectorXs& xref_in));
173  DEPRECATED("Use get_reference<MathBaseTpl<Scalar>::VectorXs>()", const VectorXs& get_xref() const);
174 
175  protected:
179  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
180 
184  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
185 
186  using Base::activation_;
187  using Base::nu_;
188  using Base::state_;
189  using Base::unone_;
190 
191  private:
192  VectorXs xref_;
193  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
194 };
195 
196 template <typename _Scalar>
197 struct CostDataStateTpl : public CostDataAbstractTpl<_Scalar> {
198  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
199 
200  typedef _Scalar Scalar;
204  typedef typename MathBase::MatrixXs MatrixXs;
205 
206  template <template <typename Scalar> class Model>
207  CostDataStateTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
208  : Base(model, data), Arr_Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()) {
209  Arr_Rx.setZero();
210  }
211 
212  MatrixXs Arr_Rx;
213 
214  using Base::activation;
215  using Base::cost;
216  using Base::Lu;
217  using Base::Luu;
218  using Base::Lx;
219  using Base::Lxu;
220  using Base::Lxx;
221  using Base::r;
222  using Base::Ru;
223  using Base::Rx;
224  using Base::shared;
225 };
226 
227 } // namespace crocoddyl
228 
229 /* --- Details -------------------------------------------------------------- */
230 /* --- Details -------------------------------------------------------------- */
231 /* --- Details -------------------------------------------------------------- */
232 #include "crocoddyl/multibody/costs/state.hxx"
233 
234 #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
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.
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.
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.