crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
impulse-base.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_IMPULSE_BASE_HPP_
10 #define CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/states/multibody.hpp"
14 #include "crocoddyl/multibody/force-base.hpp"
15 #include "crocoddyl/core/utils/to-string.hpp"
16 #include "crocoddyl/core/utils/deprecate.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
21 class ImpulseModelAbstractTpl {
22  public:
23  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 
25  typedef _Scalar Scalar;
26  typedef MathBaseTpl<Scalar> MathBase;
27  typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
28  typedef StateMultibodyTpl<Scalar> StateMultibody;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  ImpulseModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nc);
33  virtual ~ImpulseModelAbstractTpl();
34 
35  virtual void calc(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
36  virtual void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
37 
38  virtual void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force) = 0;
39  void updateForceDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const MatrixXs& df_dx) const;
40  void setZeroForce(const boost::shared_ptr<ImpulseDataAbstract>& data) const;
41  void setZeroForceDiff(const boost::shared_ptr<ImpulseDataAbstract>& data) const;
42 
43  virtual boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
44 
45  const boost::shared_ptr<StateMultibody>& get_state() const;
46  std::size_t get_nc() const;
47  DEPRECATED("Use get_nc().", std::size_t get_ni() const;)
48  std::size_t get_nu() const;
49 
53  template <class Scalar>
54  friend std::ostream& operator<<(std::ostream& os, const ImpulseModelAbstractTpl<Scalar>& model);
55 
61  virtual void print(std::ostream& os) const;
62 
63  protected:
64  boost::shared_ptr<StateMultibody> state_;
65  std::size_t nc_;
66 };
67 
68 template <typename _Scalar>
69 struct ImpulseDataAbstractTpl : public ForceDataAbstractTpl<_Scalar> {
70  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 
72  typedef _Scalar Scalar;
73  typedef MathBaseTpl<Scalar> MathBase;
74  typedef ForceDataAbstractTpl<Scalar> Base;
75  typedef typename MathBase::VectorXs VectorXs;
76  typedef typename MathBase::MatrixXs MatrixXs;
77 
78  template <template <typename Scalar> class Model>
79  ImpulseDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
80  : Base(model, data), dv0_dq(model->get_nc(), model->get_state()->get_nv()) {
81  dv0_dq.setZero();
82  }
83  virtual ~ImpulseDataAbstractTpl() {}
84 
85  using Base::df_dx;
86  using Base::f;
87  using Base::frame;
88  using Base::Jc;
89  using Base::jMf;
90  using Base::pinocchio;
91  MatrixXs dv0_dq;
92 };
93 
94 } // namespace crocoddyl
95 
96 /* --- Details -------------------------------------------------------------- */
97 /* --- Details -------------------------------------------------------------- */
98 /* --- Details -------------------------------------------------------------- */
99 #include "crocoddyl/multibody/impulse-base.hxx"
100 
101 #endif // CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the impulse model.