crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-2d.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-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_2D_HPP_
10 #define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
11 
12 #include <pinocchio/spatial/motion.hpp>
13 #include <pinocchio/multibody/data.hpp>
14 #include <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
16 
17 #include "crocoddyl/multibody/fwd.hpp"
18 #include "crocoddyl/multibody/frames.hpp"
19 #include "crocoddyl/core/utils/exception.hpp"
20 #include "crocoddyl/multibody/contact-base.hpp"
21 
22 namespace crocoddyl {
23 
24 template <typename _Scalar>
25 class ContactModel2DTpl : 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  ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref, const std::size_t nu,
42  const Vector2s& gains = Vector2s::Zero());
43  ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref,
44  const Vector2s& gains = Vector2s::Zero());
45  virtual ~ContactModel2DTpl();
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 
60  virtual void print(std::ostream& os) const;
61 
62  protected:
63  using Base::nc_;
64  using Base::nu_;
65  using Base::state_;
66 
67  private:
68  FrameTranslation xref_;
69  Vector2s gains_;
70 };
71 
72 template <typename _Scalar>
73 struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
74  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 
76  typedef _Scalar Scalar;
79  typedef typename MathBase::Vector2s Vector2s;
80  typedef typename MathBase::Matrix3s Matrix3s;
81  typedef typename MathBase::Matrix6xs Matrix6xs;
82 
83  typedef typename MathBase::Vector3s Vector3s;
84  typedef typename MathBase::VectorXs VectorXs;
85  typedef typename MathBase::MatrixXs MatrixXs;
86 
87  template <template <typename Scalar> class Model>
88  ContactData2DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
89  : Base(model, data),
90  fJf(6, model->get_state()->get_nv()),
91  v_partial_dq(6, model->get_state()->get_nv()),
92  a_partial_dq(6, model->get_state()->get_nv()),
93  a_partial_dv(6, model->get_state()->get_nv()),
94  a_partial_da(6, model->get_state()->get_nv()),
95  fXjdv_dq(6, model->get_state()->get_nv()),
96  fXjda_dq(6, model->get_state()->get_nv()),
97  fXjda_dv(6, model->get_state()->get_nv()) {
98  frame = model->get_xref().id;
99  jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
100  fXj = jMf.inverse().toActionMatrix();
101  fJf.setZero();
102  v_partial_dq.setZero();
103  a_partial_dq.setZero();
104  a_partial_dv.setZero();
105  a_partial_da.setZero();
106  fXjdv_dq.setZero();
107  fXjda_dq.setZero();
108  fXjda_dv.setZero();
109  vv.setZero();
110  vw.setZero();
111  vv_skew.setZero();
112  vw_skew.setZero();
113  oRf.setZero();
114  }
115 
116  using Base::a0;
117  using Base::da0_dx;
118  using Base::df_du;
119  using Base::df_dx;
120  using Base::f;
121  using Base::frame;
122  using Base::fXj;
123  using Base::Jc;
124  using Base::jMf;
125  using Base::pinocchio;
126 
127  pinocchio::MotionTpl<Scalar> v;
128  pinocchio::MotionTpl<Scalar> a;
129  Matrix6xs fJf;
130  Matrix6xs v_partial_dq;
131  Matrix6xs a_partial_dq;
132  Matrix6xs a_partial_dv;
133  Matrix6xs a_partial_da;
134  Matrix6xs fXjdv_dq;
135  Matrix6xs fXjda_dq;
136  Matrix6xs fXjda_dv;
137  Vector3s vv;
138  Vector3s vw;
139  Matrix3s vv_skew;
140  Matrix3s vw_skew;
141  Matrix3s oRf;
142 };
143 
144 } // namespace crocoddyl
145 
146 /* --- Details -------------------------------------------------------------- */
147 /* --- Details -------------------------------------------------------------- */
148 /* --- Details -------------------------------------------------------------- */
149 #include "crocoddyl/multibody/contacts/contact-2d.hxx"
150 
151 #endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
State multibody representation.
Definition: fwd.hpp:300
virtual void print(std::ostream &os) const
Print relevant information of the 2d contact model.