crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-6d.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_CONTACTS_CONTACT_6D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/contact-base.hpp"
14 #include "crocoddyl/multibody/frames.hpp"
15 
16 #include <pinocchio/spatial/motion.hpp>
17 #include <pinocchio/multibody/data.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 
52  protected:
53  using Base::nc_;
54  using Base::nu_;
55  using Base::state_;
56 
57  private:
58  FramePlacement Mref_;
59  Vector2s gains_;
60 };
61 
62 template <typename _Scalar>
63 struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
64  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 
66  typedef _Scalar Scalar;
69  typedef typename MathBase::Vector2s Vector2s;
70  typedef typename MathBase::Matrix3s Matrix3s;
71  typedef typename MathBase::Matrix6xs Matrix6xs;
72  typedef typename MathBase::Matrix6s Matrix6s;
73 
74  typedef typename MathBase::Vector3s Vector3s;
75  typedef typename MathBase::VectorXs VectorXs;
76  typedef typename MathBase::MatrixXs MatrixXs;
77 
78  template <template <typename Scalar> class Model>
79  ContactData6DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
80  : Base(model, data),
81  rMf(pinocchio::SE3Tpl<Scalar>::Identity()),
82  v_partial_dq(6, model->get_state()->get_nv()),
83  a_partial_dq(6, model->get_state()->get_nv()),
84  a_partial_dv(6, model->get_state()->get_nv()),
85  a_partial_da(6, model->get_state()->get_nv()) {
86  frame = model->get_Mref().id;
87  joint = model->get_state()->get_pinocchio()->frames[frame].parent;
88  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
89  fXj = jMf.inverse().toActionMatrix();
90  v_partial_dq.setZero();
91  a_partial_dq.setZero();
92  a_partial_dv.setZero();
93  a_partial_da.setZero();
94  rMf_Jlog6.setZero();
95  }
96 
97  using Base::a0;
98  using Base::da0_dx;
99  using Base::df_du;
100  using Base::df_dx;
101  using Base::f;
102  using Base::frame;
103  using Base::fXj;
104  using Base::Jc;
105  using Base::jMf;
106  using Base::joint;
107  using Base::pinocchio;
108 
109  pinocchio::SE3Tpl<Scalar> rMf;
110  pinocchio::MotionTpl<Scalar> v;
111  pinocchio::MotionTpl<Scalar> a;
112  Matrix6xs v_partial_dq;
113  Matrix6xs a_partial_dq;
114  Matrix6xs a_partial_dv;
115  Matrix6xs a_partial_da;
116  Matrix6s rMf_Jlog6;
117 };
118 
119 } // namespace crocoddyl
120 /* --- Details -------------------------------------------------------------- */
121 /* --- Details -------------------------------------------------------------- */
122 /* --- Details -------------------------------------------------------------- */
123 #include "crocoddyl/multibody/contacts/contact-6d.hxx"
124 
125 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_