Commit e563c371 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

ContactModel add members to define the contact points used

parent 88e67129
......@@ -6,7 +6,7 @@
#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include <iostream>
#include <Eigen/Dense>
......@@ -18,24 +18,42 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3X;
/// \brief Default constructor.
ContactModelTpl() : m_mu(-1.), m_contact_type(ContactType::CONTACT_UNDEFINED) {}
ContactModelTpl()
: m_mu(-1.),
m_contact_type(ContactType::CONTACT_UNDEFINED),
m_num_contact_points(1),
m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
/// \brief Constructor with friction
ContactModelTpl(const Scalar mu) : m_mu(mu), m_contact_type(ContactType::CONTACT_UNDEFINED) {}
ContactModelTpl(const Scalar mu)
: m_mu(mu),
m_contact_type(ContactType::CONTACT_UNDEFINED),
m_num_contact_points(1),
m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
/// \brief Full constructor
ContactModelTpl(const Scalar mu, const ContactType contact_type) : m_mu(mu), m_contact_type(contact_type) {}
ContactModelTpl(const Scalar mu, const ContactType contact_type)
: m_mu(mu),
m_contact_type(contact_type),
m_num_contact_points(1),
m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
/// \brief Copy constructor
template <typename S2>
explicit ContactModelTpl(const ContactModelTpl<S2>& other)
: m_mu(other.mu), m_contact_type(other.m_contact_type) {}
: m_mu(other.mu),
m_contact_type(other.m_contact_type),
m_contact_points_positions(other.m_contact_points_positions),
m_num_contact_points(other.m_num_contact_points) {}
template <typename S2>
bool operator==(const ContactModelTpl<S2>& other) const {
return m_mu == other.m_mu && m_contact_type == other.m_contact_type;
return m_mu == other.m_mu && m_contact_type == other.m_contact_type &&
m_num_contact_points == other.m_num_contact_points &&
m_contact_points_positions == other.m_contact_points_positions;
}
template <typename S2>
......@@ -44,7 +62,9 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
}
void disp(std::ostream& os) const {
os << "ContactType : " << m_contact_type << ", mu: " << m_mu << std::endl;
os << "ContactType: " << m_contact_type << ", mu: " << m_mu << std::endl
<< "Number of contact points: " << m_num_contact_points << ", positions: " << std::endl
<< m_contact_points_positions << std::endl;
}
template <typename S2>
......@@ -53,12 +73,31 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
return os;
}
size_t num_contact_points() const { return m_num_contact_points; }
void num_contact_points(const size_t num) {
m_num_contact_points = num;
m_contact_points_positions = Matrix3X::Zero(3, num);
}
Matrix3X contact_points_positions() const { return m_contact_points_positions; }
void contact_points_positions(const Matrix3X& positions) {
m_contact_points_positions = positions;
m_num_contact_points = positions.cols();
}
/// \brief Friction coefficient.
Scalar m_mu;
/// \brief ZMP radius.
ContactType m_contact_type;
private:
/// \brief The number of contact points used to model this contact
size_t m_num_contact_points;
/// \brief 3xnum_contact_points matrix defining the contact points positions in the frame of the contact placement
Matrix3X m_contact_points_positions;
// Serialization of the class
friend class boost::serialization::access;
......@@ -66,12 +105,16 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
void save(Archive& ar, const unsigned int /*version*/) const {
ar& boost::serialization::make_nvp("mu", m_mu);
ar& boost::serialization::make_nvp("contact_type", m_contact_type);
ar& boost::serialization::make_nvp("num_contact_points", m_num_contact_points);
ar& boost::serialization::make_nvp("contact_points_positions", m_contact_points_positions);
}
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp("mu", m_mu);
ar >> boost::serialization::make_nvp("contact_type", m_contact_type);
ar >> boost::serialization::make_nvp("num_contact_points", m_num_contact_points);
ar >> boost::serialization::make_nvp("contact_points_positions", m_contact_points_positions);
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment