contact-model.hpp
Go to the documentation of this file.
1 // Copyright (c) 2015-2018, CNRS
2 // Authors: Justin Carpentier <jcarpent@laas.fr>
3 
4 #ifndef __multicontact_api_scenario_contact_model_planar_hpp__
5 #define __multicontact_api_scenario_contact_model_planar_hpp__
6 
7 #include <Eigen/Dense>
8 #include <iostream>
9 #include <pinocchio/fwd.hpp>
10 #include <pinocchio/spatial/skew.hpp>
11 
15 
16 namespace multicontact_api {
17 namespace scenario {
18 
19 template <typename _Scalar>
21  : public serialization::Serializable<ContactModelTpl<_Scalar> > {
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  typedef _Scalar Scalar;
25  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
26  typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3X;
27  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic> Matrix6X;
28 
31  : m_mu(-1.),
33  m_num_contact_points(1),
34  m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
35 
38  : m_mu(mu),
40  m_num_contact_points(1),
41  m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
42 
44  ContactModelTpl(const Scalar mu, const ContactType contact_type)
45  : m_mu(mu),
46  m_contact_type(contact_type),
47  m_num_contact_points(1),
48  m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
49 
51  template <typename S2>
52  explicit ContactModelTpl(const ContactModelTpl<S2>& other)
53  : m_mu(other.mu),
55  m_contact_points_positions(other.m_contact_points_positions),
56  m_num_contact_points(other.m_num_contact_points) {}
57 
58  template <typename S2>
59  bool operator==(const ContactModelTpl<S2>& other) const {
60  return m_mu == other.m_mu && m_contact_type == other.m_contact_type &&
61  m_num_contact_points == other.m_num_contact_points &&
62  m_contact_points_positions == other.m_contact_points_positions;
63  }
64 
65  template <typename S2>
66  bool operator!=(const ContactModelTpl<S2>& other) const {
67  return !(*this == other);
68  }
69 
70  void disp(std::ostream& os) const {
71  os << "ContactType: " << m_contact_type << ", mu: " << m_mu << std::endl
72  << "Number of contact points: " << m_num_contact_points
73  << ", positions: " << std::endl
74  << m_contact_points_positions << std::endl;
75  }
76 
77  template <typename S2>
78  friend std::ostream& operator<<(std::ostream& os,
79  const ContactModelTpl<S2>& cp) {
80  cp.disp(os);
81  return os;
82  }
83 
84  size_t num_contact_points() const { return m_num_contact_points; }
85 
86  void num_contact_points(const size_t num) {
87  m_num_contact_points = num;
88  m_contact_points_positions = Matrix3X::Zero(3, num);
89  }
90 
92  return m_contact_points_positions;
93  }
94 
95  void contact_points_positions(const Matrix3X& positions) {
96  m_contact_points_positions = positions;
97  m_num_contact_points = positions.cols();
98  }
99 
106  Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3);
107  for (size_t i = 0; i < m_num_contact_points; i++) {
108  gen.block(0, i * 3, 3, 3) = Matrix3::Identity();
109  gen.block(3, i * 3, 3, 3) =
110  pinocchio::skew(m_contact_points_positions.col(i));
111  }
112  return gen;
113  }
114 
119 
120  private:
122  size_t m_num_contact_points;
125  Matrix3X m_contact_points_positions;
126 
127  // Serialization of the class
129 
130  template <class Archive>
131  void save(Archive& ar, const unsigned int /*version*/) const {
132  ar& boost::serialization::make_nvp("mu", m_mu);
133  ar& boost::serialization::make_nvp("contact_type", m_contact_type);
134  ar& boost::serialization::make_nvp("num_contact_points",
135  m_num_contact_points);
136  ar& boost::serialization::make_nvp("contact_points_positions",
137  m_contact_points_positions);
138  }
139 
140  template <class Archive>
141  void load(Archive& ar, const unsigned int /*version*/) {
142  ar >> boost::serialization::make_nvp("mu", m_mu);
143  ar >> boost::serialization::make_nvp("contact_type", m_contact_type);
144  ar >> boost::serialization::make_nvp("num_contact_points",
145  m_num_contact_points);
146  ar >> boost::serialization::make_nvp("contact_points_positions",
147  m_contact_points_positions);
148  }
149 
150  BOOST_SERIALIZATION_SPLIT_MEMBER()
151 };
152 } // namespace scenario
153 } // namespace multicontact_api
154 
157 
158 #endif // ifndef __multicontact_api_scenario_contact_model_planar_hpp__
multicontact_api::scenario::ContactModelTpl::Matrix3X
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3X
Definition: contact-model.hpp:26
multicontact_api::scenario::ContactModelTpl::ContactModelTpl
ContactModelTpl(const Scalar mu)
Constructor with friction.
Definition: contact-model.hpp:37
multicontact_api::scenario::ContactModelTpl::ContactModelTpl
ContactModelTpl()
Default constructor.
Definition: contact-model.hpp:30
multicontact_api::scenario::ContactModelTpl::m_contact_type
ContactType m_contact_type
ZMP radius.
Definition: contact-model.hpp:118
multicontact_api::scenario::ContactModelTpl::Matrix6X
Eigen::Matrix< Scalar, 6, Eigen::Dynamic > Matrix6X
Definition: contact-model.hpp:27
eigen-matrix.hpp
multicontact_api::scenario::ContactModelTpl::operator!=
bool operator!=(const ContactModelTpl< S2 > &other) const
Definition: contact-model.hpp:66
archive.hpp
fwd.hpp
multicontact_api::scenario::ContactModelTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: contact-model.hpp:24
multicontact_api::scenario::ContactModelTpl::Matrix3
Eigen::Matrix< Scalar, 3, 3 > Matrix3
Definition: contact-model.hpp:25
multicontact_api::scenario::ContactModelTpl::ContactModelTpl
ContactModelTpl(const Scalar mu, const ContactType contact_type)
Full constructor.
Definition: contact-model.hpp:44
multicontact_api
Definition: ellipsoid.hpp:12
multicontact_api::scenario::ContactModelTpl
Definition: contact-model.hpp:20
multicontact_api::scenario::ContactModelTpl::contact_points_positions
Matrix3X contact_points_positions() const
Definition: contact-model.hpp:91
multicontact_api::scenario::CONTACT_UNDEFINED
@ CONTACT_UNDEFINED
Definition: fwd.hpp:33
multicontact_api::scenario::ContactModelTpl::ContactModelTpl
ContactModelTpl(const ContactModelTpl< S2 > &other)
Copy constructor.
Definition: contact-model.hpp:52
multicontact_api::scenario::ContactModelTpl::num_contact_points
size_t num_contact_points() const
Definition: contact-model.hpp:84
boost::serialization::save
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16
multicontact_api::scenario::ContactModelTpl::disp
void disp(std::ostream &os) const
Definition: contact-model.hpp:70
multicontact_api::scenario::ContactModelTpl::access
friend class boost::serialization::access
Definition: contact-model.hpp:128
boost::serialization::load
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
multicontact_api::scenario::ContactType
ContactType
Definition: fwd.hpp:33
multicontact_api::scenario::ContactModelTpl::num_contact_points
void num_contact_points(const size_t num)
Definition: contact-model.hpp:86
DEFINE_CLASS_TEMPLATE_VERSION
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:22
multicontact_api::scenario::ContactModelTpl::operator==
bool operator==(const ContactModelTpl< S2 > &other) const
Definition: contact-model.hpp:59
multicontact_api::serialization::Serializable
Definition: archive.hpp:38
multicontact_api::scenario::ContactModelTpl::operator<<
friend std::ostream & operator<<(std::ostream &os, const ContactModelTpl< S2 > &cp)
Definition: contact-model.hpp:78
multicontact_api::scenario::ContactModelTpl::m_mu
Scalar m_mu
Friction coefficient.
Definition: contact-model.hpp:116
multicontact_api::scenario::ContactModelTpl::generatorMatrix
Matrix6X generatorMatrix() const
generatorMatrix Return a 6x(num_contact_points*3) matrix containing the generator used to compute con...
Definition: contact-model.hpp:105
multicontact_api::scenario::ContactModelTpl::contact_points_positions
void contact_points_positions(const Matrix3X &positions)
Definition: contact-model.hpp:95