crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-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_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/core/utils/to-string.hpp"
16 
17 #include <pinocchio/multibody/data.hpp>
18 #include <pinocchio/spatial/force.hpp>
19 
20 namespace crocoddyl {
21 
22 template <typename _Scalar>
24  public:
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 
27  typedef _Scalar Scalar;
31  typedef typename MathBase::VectorXs VectorXs;
32  typedef typename MathBase::MatrixXs MatrixXs;
33 
34  ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nc, const std::size_t& nu);
35  ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nc);
36  virtual ~ContactModelAbstractTpl();
37 
38  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
39  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
40 
41  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) = 0;
42  void updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx,
43  const MatrixXs& df_du) const;
44  void setZeroForce(const boost::shared_ptr<ContactDataAbstract>& data) const;
45  void setZeroForceDiff(const boost::shared_ptr<ContactDataAbstract>& data) const;
46 
47  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
48 
49  const boost::shared_ptr<StateMultibody>& get_state() const;
50  const std::size_t& get_nc() const;
51  const std::size_t& get_nu() const;
52 
53  protected:
54  boost::shared_ptr<StateMultibody> state_;
55  std::size_t nc_;
56  std::size_t nu_;
57 };
58 
59 template <typename _Scalar>
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62 
63  typedef _Scalar Scalar;
65  typedef typename MathBase::VectorXs VectorXs;
66  typedef typename MathBase::MatrixXs MatrixXs;
67 
68  template <template <typename Scalar> class Model>
69  ContactDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
70  : pinocchio(data),
71  joint(0),
72  frame(0),
73  jMf(pinocchio::SE3Tpl<Scalar>::Identity()),
74  fXj(jMf.inverse().toActionMatrix()),
75  Jc(model->get_nc(), model->get_state()->get_nv()),
76  a0(model->get_nc()),
77  da0_dx(model->get_nc(), model->get_state()->get_ndx()),
78  f(pinocchio::ForceTpl<Scalar>::Zero()),
79  df_dx(model->get_nc(), model->get_state()->get_ndx()),
80  df_du(model->get_nc(), model->get_nu()) {
81  Jc.setZero();
82  a0.setZero();
83  da0_dx.setZero();
84  df_dx.setZero();
85  df_du.setZero();
86  }
87  virtual ~ContactDataAbstractTpl() {}
88 
89  typename pinocchio::DataTpl<Scalar>* pinocchio;
90  pinocchio::JointIndex joint;
91  pinocchio::FrameIndex frame;
92  typename pinocchio::SE3Tpl<Scalar> jMf;
93  typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
94  MatrixXs Jc;
95  VectorXs a0;
96  MatrixXs da0_dx;
97  pinocchio::ForceTpl<Scalar> f;
98  MatrixXs df_dx;
99  MatrixXs df_du;
100 };
101 
102 } // namespace crocoddyl
103 
104 /* --- Details -------------------------------------------------------------- */
105 /* --- Details -------------------------------------------------------------- */
106 /* --- Details -------------------------------------------------------------- */
107 #include "crocoddyl/multibody/contact-base.hxx"
108 
109 #endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_