crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-3d.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_3D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/frames.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
15 #include "crocoddyl/multibody/contact-base.hpp"
16 
17 #include <pinocchio/spatial/motion.hpp>
18 #include <pinocchio/multibody/data.hpp>
19 #include <pinocchio/algorithm/frames.hpp>
20 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
21 
22 namespace crocoddyl {
23 
24 template <typename _Scalar>
25 class ContactModel3DTpl : public ContactModelAbstractTpl<_Scalar> {
26  public:
27  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 
29  typedef _Scalar Scalar;
36  typedef typename MathBase::Vector2s Vector2s;
37  typedef typename MathBase::Vector3s Vector3s;
38  typedef typename MathBase::VectorXs VectorXs;
39  typedef typename MathBase::MatrixXs MatrixXs;
40 
41  ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref, const std::size_t& nu,
42  const Vector2s& gains = Vector2s::Zero());
43  ContactModel3DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref,
44  const Vector2s& gains = Vector2s::Zero());
45  virtual ~ContactModel3DTpl();
46 
47  virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
48  virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
49  virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
50  virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
51 
52  const FrameTranslation& get_xref() const;
53  const Vector2s& get_gains() const;
54 
55  protected:
56  using Base::nc_;
57  using Base::nu_;
58  using Base::state_;
59 
60  private:
61  FrameTranslation xref_;
62  Vector2s gains_;
63 };
64 
65 template <typename _Scalar>
66 struct ContactData3DTpl : public ContactDataAbstractTpl<_Scalar> {
67  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 
69  typedef _Scalar Scalar;
72  typedef typename MathBase::Vector2s Vector2s;
73  typedef typename MathBase::Matrix3s Matrix3s;
74  typedef typename MathBase::Matrix6xs Matrix6xs;
75 
76  typedef typename MathBase::Vector3s Vector3s;
77  typedef typename MathBase::VectorXs VectorXs;
78  typedef typename MathBase::MatrixXs MatrixXs;
79 
80  template <template <typename Scalar> class Model>
81  ContactData3DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
82  : Base(model, data),
83  fJf(6, model->get_state()->get_nv()),
84  v_partial_dq(6, model->get_state()->get_nv()),
85  a_partial_dq(6, model->get_state()->get_nv()),
86  a_partial_dv(6, model->get_state()->get_nv()),
87  a_partial_da(6, model->get_state()->get_nv()),
88  fXjdv_dq(6, model->get_state()->get_nv()),
89  fXjda_dq(6, model->get_state()->get_nv()),
90  fXjda_dv(6, model->get_state()->get_nv()) {
91  frame = model->get_xref().frame;
92  joint = model->get_state()->get_pinocchio()->frames[frame].parent;
93  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
94  fXj = jMf.inverse().toActionMatrix();
95  fJf.setZero();
96  v_partial_dq.setZero();
97  a_partial_dq.setZero();
98  a_partial_dv.setZero();
99  a_partial_da.setZero();
100  fXjdv_dq.setZero();
101  fXjda_dq.setZero();
102  fXjda_dv.setZero();
103  vv.setZero();
104  vw.setZero();
105  vv_skew.setZero();
106  vw_skew.setZero();
107  oRf.setZero();
108  }
109 
110  using Base::a0;
111  using Base::da0_dx;
112  using Base::df_du;
113  using Base::df_dx;
114  using Base::f;
115  using Base::frame;
116  using Base::fXj;
117  using Base::Jc;
118  using Base::jMf;
119  using Base::joint;
120  using Base::pinocchio;
121 
122  pinocchio::MotionTpl<Scalar> v;
123  pinocchio::MotionTpl<Scalar> a;
124  Matrix6xs fJf;
125  Matrix6xs v_partial_dq;
126  Matrix6xs a_partial_dq;
127  Matrix6xs a_partial_dv;
128  Matrix6xs a_partial_da;
129  Matrix6xs fXjdv_dq;
130  Matrix6xs fXjda_dq;
131  Matrix6xs fXjda_dv;
132  Vector3s vv;
133  Vector3s vw;
134  Matrix3s vv_skew;
135  Matrix3s vw_skew;
136  Matrix3s oRf;
137 };
138 
139 } // namespace crocoddyl
140 
141 /* --- Details -------------------------------------------------------------- */
142 /* --- Details -------------------------------------------------------------- */
143 /* --- Details -------------------------------------------------------------- */
144 #include "crocoddyl/multibody/contacts/contact-3d.hxx"
145 
146 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_