Unverified Commit b4a2569d authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #19 from pFernbach/devel

Add jupyter notebooks
parents 50bdf623 ff7175c3
Pipeline #17108 passed with stage
in 6 minutes and 24 seconds
......@@ -41,13 +41,13 @@ make test
## Main classes
This package define 3 main classes representing physical concepts of legged locomotion. Contact Patch, Contact Phase and Contact Sequence.
This package define 4 main classes representing physical concepts of legged locomotion. Contact Model, Contact Patch, Contact Phase and Contact Sequence.
### Contact Patch
A Contact Model define the physical properties of a contact. A Contact Patch define completely a contact between a part of the robot and the environment, it contain a Contact Model. A Contact Phase is defined by a constant set of contacts, it contains one or more Contact Patches. Finally, a Contact Sequence is a sequence of Contact Phases.
A contact patch define the placement (in SE(3), in the world frame) of a contact as long as the friction coefficient for this contact.
### Contact Patch
Future work will also include the type of contact (eg planar, punctual, bilateral ...), a representation of the friction cone or additional information about the contact.
A contact patch define the placement (in SE(3), in the world frame) of a contact between a part of the robot and the environment. It contains a ContactModel.
### Contact Phase
......
......@@ -19,6 +19,7 @@ template <typename ContactPatch>
struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatchPythonVisitor<ContactPatch> > {
typedef typename ContactPatch::Scalar Scalar;
typedef typename ContactPatch::SE3 SE3;
typedef typename ContactPatch::ContactModel ContactModel;
template <class PyClass>
void visit(PyClass& cl) const {
......@@ -26,6 +27,8 @@ struct ContactPatchPythonVisitor : public boost::python::def_visitor<ContactPatc
.def(bp::init<SE3>(bp::arg("placement"), "Init with a given placement."))
.def(bp::init<SE3, Scalar>(bp::args("placement", "friction"),
"Init with a given placement and friction coefficient."))
.def(bp::init<SE3, ContactModel>(bp::args("placement", "contact_model"),
"Init with a given placement and contact model."))
.def(bp::init<ContactPatch>(bp::arg("other"), "Copy contructor."))
.add_property("placement",
// getter require to use "make_function" to pass the return_internal_reference policy (return ref
......
......@@ -28,6 +28,9 @@ struct ContactPatchTpl : public serialization::Serializable<ContactPatchTpl<_Sca
/// \brief Init contact patch from a given placement and a friction coefficient
ContactPatchTpl(const SE3& placement, const Scalar mu) : m_contact_model(mu), m_placement(placement) {}
/// \brief Init contact patch from a given placement and a contact model
ContactPatchTpl(const SE3& placement, const ContactModel contact_model) : m_contact_model(contact_model), m_placement(placement) {}
/// \brief Copy constructor
ContactPatchTpl(const ContactPatchTpl& other)
: m_contact_model(other.m_contact_model), m_placement(other.m_placement) {}
......
This diff is collapsed.
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -289,6 +289,20 @@ class ContactPatchTest(unittest.TestCase):
self.assertTrue(cp.friction == 0.9)
self.assertTrue(cp.placement == p)
def test_constructor_with_contact_model(self):
cm = ContactModel(0.5, ContactType.CONTACT_PLANAR)
cm.num_contact_points = 4
p = SE3()
p.setRandom()
cp = ContactPatch(p, cm)
self.assertTrue(cp.friction == 0.5)
self.assertTrue(cp.placement == p)
self.assertTrue(cp.contact_model.num_contact_points == 4)
# check that the value have been copied and it's not the same pointer anymore :
cm.num_contact_points = 6
self.assertTrue(cp.contact_model.num_contact_points == 4)
def test_operator_equal(self):
cp1 = ContactPatch()
cp2 = ContactPatch()
......
......@@ -469,6 +469,15 @@ BOOST_AUTO_TEST_CASE(contact_patch) {
BOOST_CHECK(cp2.placement() == p);
BOOST_CHECK(cp2.friction() == 0.9);
// constructor with contact_model
ContactModel cm(0.5, ContactType::CONTACT_PLANAR);
cm.num_contact_points(4);
ContactPatch cp_with_model(p, cm);
BOOST_CHECK(cp_with_model.placement() == p);
BOOST_CHECK(cp_with_model.friction() == 0.5);
BOOST_CHECK(cp_with_model.m_contact_model.num_contact_points() == 4);
// check comparison operator
BOOST_CHECK(cp1 != cp2);
ContactPatch cp3(p, 0.9);
......
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