Commit 88e67129 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Python] add api & tests for ContactPatch::contact_model

parent d5e383f7
......@@ -29,7 +29,6 @@ struct ContactModelPythonVisitor
.def(bp::init<ContactModel>(bp::args("other"), "Copy contructor."))
.def_readwrite("mu", &ContactModel::m_mu, "Friction coefficient.")
.def_readwrite("contact_type", &ContactModel::m_contact_type, "Enum that define the type of contact.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("copy", &copy, "Returns a copy of *this.");
......
......@@ -34,6 +34,7 @@ struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatc
"Placement of the patch represented as a pinocchio SE3 object.")
.add_property("friction", &getFriction, &setFriction,
"Friction coefficient between the robot and the environment for this contact.")
.def_readwrite("contact_model", &ContactPatch::m_contact_model, "The contact model defining this contact.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("copy", &copy, "Returns a copy of *this.");
......
......@@ -20,16 +20,16 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
typedef pinocchio::SE3Tpl<Scalar, 0> SE3;
/// \brief Default constructor.
ContactPatchTpl() : m_placement(SE3::Identity()), m_contact_model() {}
ContactPatchTpl() : m_contact_model(), m_placement(SE3::Identity()) {}
/// \brief Init contact patch from a given placement.
explicit ContactPatchTpl(const SE3& placement) : m_placement(placement), m_contact_model() {}
explicit ContactPatchTpl(const SE3& placement) : m_contact_model(), m_placement(placement) {}
/// \brief Init contact patch from a given placement and a friction coefficient
ContactPatchTpl(const SE3& placement, const Scalar mu) : m_placement(placement), m_contact_model(mu) {}
ContactPatchTpl(const SE3& placement, const Scalar mu) : m_contact_model(mu), m_placement(placement) {}
/// \brief Copy constructor
ContactPatchTpl(const ContactPatchTpl& other) : m_placement(other.m_placement), m_contact_model(other.m_contact_model) {}
ContactPatchTpl(const ContactPatchTpl& other) : m_contact_model(other.m_contact_model), m_placement(other.m_placement) {}
const SE3& placement() const { return m_placement; }
SE3& placement() { return m_placement; }
......@@ -57,11 +57,12 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
return os;
}
/// \brief Contact model of this contact
ContactModel m_contact_model;
protected:
/// \brief Placement of the contact patch
SE3 m_placement;
/// \brief friction coefficient for this contact
ContactModel m_contact_model;
private:
// Serialization of the class
......
......@@ -316,6 +316,22 @@ class ContactPatchTest(unittest.TestCase):
cp_from_pickle = pickle.loads(cp_pickled)
self.assertEqual(cp1, cp_from_pickle)
def test_contact_patch_model_accessor(self):
p = SE3()
p.setRandom()
cp1 = ContactPatch(p, 0.9)
cm = cp1.contact_model
self.assertEqual(cm.mu, 0.9)
cm.mu = 0.5
self.assertEqual(cp1.friction, 0.5)
cp1.contact_model.contact_type = ContactType.CONTACT_PLANAR
self.assertEqual(cp1.contact_model.contact_type, ContactType.CONTACT_PLANAR)
cp1.friction = 2
self.assertEqual(cp1.contact_model.mu, 2)
self.assertEqual(cm.mu, 2)
class ContactPhaseTest(unittest.TestCase):
def test_default_constructor(self):
......
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