crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/mathbase.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/force-base.hpp"
16 #include "crocoddyl/core/utils/to-string.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
22  public:
23  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 
25  typedef _Scalar Scalar;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nc, const std::size_t nu);
33  ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nc);
34  virtual ~ContactModelAbstractTpl();
35 
36  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
37  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
38 
39  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) = 0;
40  void updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx,
41  const MatrixXs& df_du) const;
42  void setZeroForce(const boost::shared_ptr<ContactDataAbstract>& data) const;
43  void setZeroForceDiff(const boost::shared_ptr<ContactDataAbstract>& data) const;
44 
45  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
46 
47  const boost::shared_ptr<StateMultibody>& get_state() const;
48  std::size_t get_nc() const;
49  std::size_t get_nu() const;
50 
54  pinocchio::FrameIndex get_id() const;
55 
59  void set_id(const pinocchio::FrameIndex id);
60 
64  template <class Scalar>
65  friend std::ostream& operator<<(std::ostream& os, const ContactModelAbstractTpl<Scalar>& model);
66 
72  virtual void print(std::ostream& os) const;
73 
74  protected:
75  boost::shared_ptr<StateMultibody> state_;
76  std::size_t nc_;
77  std::size_t nu_;
78  pinocchio::FrameIndex id_;
79 };
80 
81 template <typename _Scalar>
82 struct ContactDataAbstractTpl : public ForceDataAbstractTpl<_Scalar> {
83  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
84 
85  typedef _Scalar Scalar;
88  typedef typename MathBase::VectorXs VectorXs;
89  typedef typename MathBase::MatrixXs MatrixXs;
90 
91  template <template <typename Scalar> class Model>
92  ContactDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
93  : Base(model, data),
94  fXj(jMf.inverse().toActionMatrix()),
95  a0(model->get_nc()),
96  da0_dx(model->get_nc(), model->get_state()->get_ndx()) {
97  a0.setZero();
98  da0_dx.setZero();
99  }
100  virtual ~ContactDataAbstractTpl() {}
101 
102  using Base::df_du;
103  using Base::df_dx;
104  using Base::f;
105  using Base::frame;
106  using Base::Jc;
107  using Base::jMf;
108  using Base::pinocchio;
109  typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
110  VectorXs a0;
111  MatrixXs da0_dx;
112 };
113 
114 } // namespace crocoddyl
115 
116 /* --- Details -------------------------------------------------------------- */
117 /* --- Details -------------------------------------------------------------- */
118 /* --- Details -------------------------------------------------------------- */
119 #include "crocoddyl/multibody/contact-base.hxx"
120 
121 #endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
State multibody representation.
Definition: fwd.hpp:300
pinocchio::FrameIndex id_
Reference frame id of the contact.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
virtual void print(std::ostream &os) const
Print relevant information of the contact model.