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

add method numCOntacts in contactPhase

parent c130eaf2
...@@ -397,6 +397,10 @@ struct ContactPhaseTpl : public serialization::Serializable< ContactPhaseTpl<_Sc ...@@ -397,6 +397,10 @@ struct ContactPhaseTpl : public serialization::Serializable< ContactPhaseTpl<_Sc
return existed; return existed;
} }
std::size_t numContacts() const{
return m_effector_in_contact.size();
}
std::set<std::string> effectorsInContact() const{ std::set<std::string> effectorsInContact() const{
return m_effector_in_contact; return m_effector_in_contact;
} }
......
...@@ -362,6 +362,8 @@ BOOST_AUTO_TEST_CASE(contact_phase) ...@@ -362,6 +362,8 @@ BOOST_AUTO_TEST_CASE(contact_phase)
BOOST_CHECK(cp1.timeInitial() == 0.5); BOOST_CHECK(cp1.timeInitial() == 0.5);
BOOST_CHECK(cp1.timeFinal() == 3.5); BOOST_CHECK(cp1.timeFinal() == 3.5);
BOOST_CHECK(cp1.duration() == 3.); BOOST_CHECK(cp1.duration() == 3.);
BOOST_CHECK(cp1.numContacts() == 0);
// check exeptions related to wrong timing // check exeptions related to wrong timing
BOOST_CHECK_THROW(cp1.timeFinal(0.1),std::invalid_argument); BOOST_CHECK_THROW(cp1.timeFinal(0.1),std::invalid_argument);
BOOST_CHECK_THROW(cp1.duration(-2.),std::invalid_argument); BOOST_CHECK_THROW(cp1.duration(-2.),std::invalid_argument);
...@@ -382,6 +384,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) ...@@ -382,6 +384,7 @@ BOOST_AUTO_TEST_CASE(contact_phase)
BOOST_CHECK(cp2.isEffectorInContact("left_leg")); BOOST_CHECK(!cp2.isEffectorInContact("other")); BOOST_CHECK(cp2.isEffectorInContact("left_leg")); BOOST_CHECK(!cp2.isEffectorInContact("other"));
BOOST_CHECK(cp2.contactPatch("left_leg") == patchL); BOOST_CHECK(cp2.contactPatch("left_leg") == patchL);
BOOST_CHECK(cp2.contactPatches()["left_leg"] == patchL); BOOST_CHECK(cp2.contactPatches()["left_leg"] == patchL);
BOOST_CHECK(cp2.numContacts() == 1);
BOOST_CHECK_THROW(cp2.contactPatch("other"),std::invalid_argument); BOOST_CHECK_THROW(cp2.contactPatch("other"),std::invalid_argument);
ContactPatch patchR(SE3::Identity().setRandom()); ContactPatch patchR(SE3::Identity().setRandom());
...@@ -405,11 +408,13 @@ BOOST_AUTO_TEST_CASE(contact_phase) ...@@ -405,11 +408,13 @@ BOOST_AUTO_TEST_CASE(contact_phase)
BOOST_CHECK(cp2.isEffectorInContact("left_leg")); BOOST_CHECK(cp2.isEffectorInContact("left_leg"));
BOOST_CHECK(cp2.isEffectorInContact("right_leg")); BOOST_CHECK(cp2.isEffectorInContact("right_leg"));
BOOST_CHECK(cp2.contactPatch("left_leg") != patchL); BOOST_CHECK(cp2.contactPatch("left_leg") != patchL);
BOOST_CHECK(cp2.numContacts() == 2);
bool exist = cp2.removeContact("right_leg"); bool exist = cp2.removeContact("right_leg");
BOOST_CHECK(exist); BOOST_CHECK(exist);
BOOST_CHECK(cp2.isEffectorInContact("left_leg")); BOOST_CHECK(cp2.isEffectorInContact("left_leg"));
BOOST_CHECK(!cp2.isEffectorInContact("right_leg")); BOOST_CHECK(!cp2.isEffectorInContact("right_leg"));
BOOST_CHECK(cp2.numContacts() == 1);
exist = cp2.removeContact("other"); exist = cp2.removeContact("other");
BOOST_CHECK(!exist); BOOST_CHECK(!exist);
exist = cp2.removeContact("right_leg"); exist = cp2.removeContact("right_leg");
......
Supports Markdown
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