contact-patch.hpp 3.18 KB
Newer Older
Guilhem Saurel's avatar
Guilhem Saurel committed
1
2
3
4
5

#ifndef __multicontact_api_scenario_contact_patch_hpp__
#define __multicontact_api_scenario_contact_patch_hpp__

#include "multicontact-api/scenario/fwd.hpp"
6
#include "multicontact-api/scenario/contact-model.hpp"
Pierre Fernbach's avatar
Pierre Fernbach committed
7
#include <pinocchio/spatial/se3.hpp>
Guilhem Saurel's avatar
Guilhem Saurel committed
8
9
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/spatial.hpp"
Justin Carpentier's avatar
Justin Carpentier committed
10

Guilhem Saurel's avatar
Guilhem Saurel committed
11
12
13
14
15
16
namespace multicontact_api {
namespace scenario {

template <typename _Scalar>
struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Scalar> > {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Pierre Fernbach's avatar
Pierre Fernbach committed
17

Guilhem Saurel's avatar
Guilhem Saurel committed
18
  typedef _Scalar Scalar;
19
  typedef ContactModelTpl<_Scalar> ContactModel;
Guilhem Saurel's avatar
Guilhem Saurel committed
20
21
22
  typedef pinocchio::SE3Tpl<Scalar, 0> SE3;

  /// \brief Default constructor.
23
  ContactPatchTpl() : m_contact_model(), m_placement(SE3::Identity()) {}
Guilhem Saurel's avatar
Guilhem Saurel committed
24
25

  /// \brief Init contact patch from a given placement.
26
  explicit ContactPatchTpl(const SE3& placement) : m_contact_model(), m_placement(placement) {}
Pierre Fernbach's avatar
Pierre Fernbach committed
27
28

  /// \brief Init contact patch from a given placement and a friction coefficient
29
  ContactPatchTpl(const SE3& placement, const Scalar mu) : m_contact_model(mu), m_placement(placement) {}
Guilhem Saurel's avatar
Guilhem Saurel committed
30
31

  /// \brief Copy constructor
Pierre Fernbach's avatar
Pierre Fernbach committed
32
33
  ContactPatchTpl(const ContactPatchTpl& other)
      : m_contact_model(other.m_contact_model), m_placement(other.m_placement) {}
Guilhem Saurel's avatar
Guilhem Saurel committed
34
35
36
37

  const SE3& placement() const { return m_placement; }
  SE3& placement() { return m_placement; }

38
39
  const Scalar& friction() const { return m_contact_model.m_mu; }
  Scalar& friction() { return m_contact_model.m_mu; }
Guilhem Saurel's avatar
Guilhem Saurel committed
40
41
42

  template <typename S2>
  bool operator==(const ContactPatchTpl<S2>& other) const {
43
    return m_placement == other.m_placement && m_contact_model == other.m_contact_model;
Justin Carpentier's avatar
Justin Carpentier committed
44
45
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
46
47
  template <typename S2>
  bool operator!=(const ContactPatchTpl<S2>& other) const {
Pierre Fernbach's avatar
Pierre Fernbach committed
48
    return !(*this == other);
Guilhem Saurel's avatar
Guilhem Saurel committed
49
50
51
  }

  void disp(std::ostream& os) const {
52
    os << "Placement:\n" << m_placement << std::endl << "ContactModel : " << m_contact_model << std::endl;
Guilhem Saurel's avatar
Guilhem Saurel committed
53
54
55
56
57
58
59
60
  }

  template <typename S2>
  friend std::ostream& operator<<(std::ostream& os, const ContactPatchTpl<S2>& cp) {
    cp.disp(os);
    return os;
  }

61
62
63
  /// \brief Contact model of this contact
  ContactModel m_contact_model;

Pierre Fernbach's avatar
Pierre Fernbach committed
64
65
66
 protected:
  /// \brief Placement of the contact patch
  SE3 m_placement;
Guilhem Saurel's avatar
Guilhem Saurel committed
67

Pierre Fernbach's avatar
Pierre Fernbach committed
68
69
70
 private:
  // Serialization of the class
  friend class boost::serialization::access;
Pierre Fernbach's avatar
Pierre Fernbach committed
71

Pierre Fernbach's avatar
Pierre Fernbach committed
72
73
74
  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
    ar& boost::serialization::make_nvp("placement", m_placement);
75
    ar& boost::serialization::make_nvp("contact_model", m_contact_model);
Pierre Fernbach's avatar
Pierre Fernbach committed
76
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
77

Pierre Fernbach's avatar
Pierre Fernbach committed
78
79
80
  template <class Archive>
  void load(Archive& ar, const unsigned int /*version*/) {
    ar >> boost::serialization::make_nvp("placement", m_placement);
81
    ar >> boost::serialization::make_nvp("contact_model", m_contact_model);
Pierre Fernbach's avatar
Pierre Fernbach committed
82
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
83

Pierre Fernbach's avatar
Pierre Fernbach committed
84
85
  BOOST_SERIALIZATION_SPLIT_MEMBER()  // why is it required ? using only serialize() lead to compilation error,
                                      // probably because of the SE3
Guilhem Saurel's avatar
Guilhem Saurel committed
86

Pierre Fernbach's avatar
Pierre Fernbach committed
87
88
89
};  // struct ContactPatchTpl
}  // namespace scenario
}  // namespace multicontact_api
Guilhem Saurel's avatar
Guilhem Saurel committed
90

91
92
DEFINE_CLASS_TEMPLATE_VERSION(typename Scalar, multicontact_api::scenario::ContactPatchTpl<Scalar>)

Pierre Fernbach's avatar
Pierre Fernbach committed
93
#endif  // __multicontact_api_scenario_contact_patch_hpp__