crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-6d.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_CONTACTS_CONTACT_6D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
11 
12 #include <pinocchio/spatial/motion.hpp>
13 #include <pinocchio/multibody/data.hpp>
14 
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/multibody/contact-base.hpp"
17 #include "crocoddyl/multibody/frames.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
22 class ContactModel6DTpl : public ContactModelAbstractTpl<_Scalar> {
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
33  typedef typename MathBase::Vector2s Vector2s;
34  typedef typename MathBase::Vector3s Vector3s;
35  typedef typename MathBase::VectorXs VectorXs;
36  typedef typename MathBase::MatrixXs MatrixXs;
37 
38  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& xref, const std::size_t nu,
39  const Vector2s& gains = Vector2s::Zero());
40  ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& xref,
41  const Vector2s& gains = Vector2s::Zero());
42  virtual ~ContactModel6DTpl();
43 
44  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
45  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
46  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
47  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
48 
49  const FramePlacement& get_Mref() const;
50  const Vector2s& get_gains() const;
51 
57  virtual void print(std::ostream& os) const;
58 
59  protected:
60  using Base::nc_;
61  using Base::nu_;
62  using Base::state_;
63 
64  private:
65  FramePlacement Mref_;
66  Vector2s gains_;
67 };
68 
69 template <typename _Scalar>
70 struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
71  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 
73  typedef _Scalar Scalar;
76  typedef typename MathBase::Vector2s Vector2s;
77  typedef typename MathBase::Matrix3s Matrix3s;
78  typedef typename MathBase::Matrix6xs Matrix6xs;
79  typedef typename MathBase::Matrix6s Matrix6s;
80 
81  typedef typename MathBase::Vector3s Vector3s;
82  typedef typename MathBase::VectorXs VectorXs;
83  typedef typename MathBase::MatrixXs MatrixXs;
84 
85  template <template <typename Scalar> class Model>
86  ContactData6DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
87  : Base(model, data),
88  rMf(pinocchio::SE3Tpl<Scalar>::Identity()),
89  v_partial_dq(6, model->get_state()->get_nv()),
90  a_partial_dq(6, model->get_state()->get_nv()),
91  a_partial_dv(6, model->get_state()->get_nv()),
92  a_partial_da(6, model->get_state()->get_nv()) {
93  frame = model->get_Mref().id;
94  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
95  fXj = jMf.inverse().toActionMatrix();
96  v_partial_dq.setZero();
97  a_partial_dq.setZero();
98  a_partial_dv.setZero();
99  a_partial_da.setZero();
100  rMf_Jlog6.setZero();
101  }
102 
103  using Base::a0;
104  using Base::da0_dx;
105  using Base::df_du;
106  using Base::df_dx;
107  using Base::f;
108  using Base::frame;
109  using Base::fXj;
110  using Base::Jc;
111  using Base::jMf;
112  using Base::pinocchio;
113 
114  pinocchio::SE3Tpl<Scalar> rMf;
115  pinocchio::MotionTpl<Scalar> v;
116  pinocchio::MotionTpl<Scalar> a;
117  Matrix6xs v_partial_dq;
118  Matrix6xs a_partial_dq;
119  Matrix6xs a_partial_dv;
120  Matrix6xs a_partial_da;
121  Matrix6s rMf_Jlog6;
122 };
123 
124 } // namespace crocoddyl
125 /* --- Details -------------------------------------------------------------- */
126 /* --- Details -------------------------------------------------------------- */
127 /* --- Details -------------------------------------------------------------- */
128 #include "crocoddyl/multibody/contacts/contact-6d.hxx"
129 
130 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
State multibody representation.
Definition: fwd.hpp:300
virtual void print(std::ostream &os) const
Print relevant information of the 6d contact model.