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