Commit 595788a7 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Tests] add test cases for new helpers in contact-sequence

parent 2ac06049
......@@ -237,7 +237,7 @@ void addRandomEffectorTrajectories(ContactPhase& cp) {
cp.addEffectorTrajectory("right_foot", buildRandomSE3LinearTraj(0, 2));
}
ContactPhase buildRandomContactPhase(const double min = 0., const double max = 2.) {
ContactPhase buildRandomContactPhase(const double min, const double max) {
ContactPhase cp(min, max);
addRandomPointsValues(cp);
addRandomCurvesValues(cp);
......@@ -247,6 +247,16 @@ ContactPhase buildRandomContactPhase(const double min = 0., const double max = 2
return cp;
}
ContactPhase buildRandomContactPhase() {
ContactPhase cp;
addRandomPointsValues(cp);
addRandomCurvesValues(cp);
addRandomContacts(cp);
addRandomForcesTrajs(cp);
addRandomEffectorTrajectories(cp);
return cp;
}
void explicitContactPhaseAssertEqual(ContactPhase& cp1, ContactPhase& cp2) {
BOOST_CHECK(cp1.m_c_init == cp2.m_c_init);
BOOST_CHECK(cp1.m_dc_init == cp2.m_dc_init);
......@@ -1105,7 +1115,7 @@ BOOST_AUTO_TEST_CASE(contact_phase_helpers) {
BOOST_CHECK(cp.m_ddc_final.isApprox(dd3));
// check that init/final are not modified if they ere already set :
ContactPhase cp2 = buildRandomContactPhase();
ContactPhase cp2 = buildRandomContactPhase(0,2);
point3_t c_init, c_final, dc_init, dc_final, ddc_init, ddc_final;
c_init = cp2.m_c_init;
dc_init = cp2.m_dc_init;
......@@ -1359,13 +1369,13 @@ BOOST_AUTO_TEST_CASE(contact_sequence) {
ContactSequence cs4;
BOOST_CHECK(cs3 == cs4);
ContactPhase cp3_0 = buildRandomContactPhase();
ContactPhase cp3_0 = buildRandomContactPhase(0,2);
cs3.append(cp3_0);
BOOST_CHECK(cs3 != cs4);
BOOST_CHECK(!(cs3 == cs4));
cs4.append(cp3_0);
BOOST_CHECK(cs3 == cs4);
ContactPhase cp3_1 = buildRandomContactPhase();
ContactPhase cp3_1 = buildRandomContactPhase(0,2);
cs3.append(cp3_1);
BOOST_CHECK(cs3 != cs4);
cs4.append(cp3_1);
......@@ -1382,7 +1392,7 @@ BOOST_AUTO_TEST_CASE(contact_sequence) {
// test copy constructor :
ContactSequence cs6;
for (size_t i = 0; i < 10; ++i) {
ContactPhase cp6 = buildRandomContactPhase();
ContactPhase cp6 = buildRandomContactPhase(0,2);
cs6.append(cp6);
}
BOOST_CHECK(cs6.size() == 10);
......@@ -1410,4 +1420,153 @@ BOOST_AUTO_TEST_CASE(contact_sequence) {
BOOST_CHECK(cs6 == cs_from_bin);
}
BOOST_AUTO_TEST_CASE(contact_sequence_helper) {
ContactSequence cs1 = ContactSequence(0);
BOOST_CHECK(cs1.size() == 0);
ContactPhase cp0 = buildRandomContactPhase(0, 2);
ContactPhase cp1 = buildRandomContactPhase(2, 4.);
cs1.append(cp0);
cs1.append(cp1);
// # test break contact :
BOOST_CHECK(cs1.size() == 2);
cs1.breakContact("left_foot");
BOOST_CHECK(cs1.size() == 3 );
BOOST_CHECK( ! cs1.contactPhase(2).isEffectorInContact("left_foot") );
BOOST_CHECK(cs1.contactPhase(1).timeFinal() == 4.); // time final of previous phase should not have been modified
// check that the final value of the previous phase have been copied in the initial value of the new one
BOOST_CHECK(cs1.contactPhase(1).m_c_final == cs1.contactPhase(2).m_c_init);
BOOST_CHECK(cs1.contactPhase(1).m_dc_final == cs1.contactPhase(2).m_dc_init);
BOOST_CHECK(cs1.contactPhase(1).m_ddc_final == cs1.contactPhase(2).m_ddc_init);
BOOST_CHECK(cs1.contactPhase(1).m_L_final == cs1.contactPhase(2).m_L_init);
BOOST_CHECK(cs1.contactPhase(1).m_dL_final == cs1.contactPhase(2).m_dL_init);
BOOST_CHECK(cs1.contactPhase(1).m_q_final == cs1.contactPhase(2).m_q_init);
BOOST_CHECK(cs1.contactPhase(1).timeFinal() == cs1.contactPhase(2).timeInitial());
// check that the other contactPatch have been copied :
BOOST_CHECK(cs1.contactPhase(1).contactPatch("right_hand") == cs1.contactPhase(2).contactPatch("right_hand"));
// # test create contact :
ContactPatch target(SE3::Identity().setRandom());
cs1.createContact("left_foot",target,2.5);
BOOST_CHECK(cs1.size() == 4 );
BOOST_CHECK(cs1.contactPhase(2).timeFinal() == 6.5); // time final of previous phase should have been modified
BOOST_CHECK(cs1.contactPhase(3).contactPatch("left_foot") == target);
// check that the final value of the previous phase have been copied in the initial value of the new one
BOOST_CHECK(cs1.contactPhase(2).m_c_final == cs1.contactPhase(3).m_c_init);
BOOST_CHECK(cs1.contactPhase(2).m_dc_final == cs1.contactPhase(3).m_dc_init);
BOOST_CHECK(cs1.contactPhase(2).m_ddc_final == cs1.contactPhase(3).m_ddc_init);
BOOST_CHECK(cs1.contactPhase(2).m_L_final == cs1.contactPhase(3).m_L_init);
BOOST_CHECK(cs1.contactPhase(2).m_dL_final == cs1.contactPhase(3).m_dL_init);
BOOST_CHECK(cs1.contactPhase(2).m_q_final == cs1.contactPhase(3).m_q_init);
BOOST_CHECK(cs1.contactPhase(2).timeFinal() == cs1.contactPhase(3).timeInitial());
// check that the other contactPatch have been copied :
BOOST_CHECK(cs1.contactPhase(2).contactPatch("right_hand") == cs1.contactPhase(3).contactPatch("right_hand"));
// # test break with duration :
cs1.breakContact("left_foot",1.);
BOOST_CHECK(cs1.size() == 5 );
BOOST_CHECK( ! cs1.contactPhase(4).isEffectorInContact("left_foot") );
BOOST_CHECK(cs1.contactPhase(3).timeFinal() == 7.5); // time final of previous phase should have been modified
// # test create contact with no duration:
cs1.contactPhase(4).duration(1.);
BOOST_CHECK(cs1.contactPhase(4).timeFinal() == 8.5); // time final of previous phase should have been modified
target = ContactPatch(SE3::Identity().setRandom());
cs1.createContact("left_foot",target);
BOOST_CHECK(cs1.size() == 6 );
BOOST_CHECK(cs1.contactPhase(4).timeFinal() == 8.5); // time final of previous phase should have been modified
BOOST_CHECK(cs1.contactPhase(5).timeInitial() == 8.5); // time final of previous phase should have been modified
// # test move effector to placement :
SE3 target_placement =SE3::Identity().setRandom();
addRandomPointsValues(cs1.contactPhase(5));
cs1.contactPhase(5).contactPatch("right_hand").friction() = 2.;
cs1.moveEffectorToPlacement("right_hand",target_placement,1.,1.5);
BOOST_CHECK(cs1.size() == 8 );
BOOST_CHECK(!cs1.contactPhase(6).isEffectorInContact("right_hand"));
BOOST_CHECK(cs1.contactPhase(7).isEffectorInContact("right_hand"));
BOOST_CHECK(cs1.contactPhase(7).contactPatch("right_hand").placement() == target_placement);
// check that previous patch have not been modified :
BOOST_CHECK(cs1.contactPhase(5).contactPatch("right_hand").placement() != target_placement);
BOOST_CHECK(cs1.contactPhase(7).contactPatch("right_hand").friction() == 2.);
BOOST_CHECK(cs1.contactPhase(5).timeFinal() == 9.5);
BOOST_CHECK(cs1.contactPhase(6).timeInitial() == 9.5);
BOOST_CHECK(cs1.contactPhase(6).timeFinal() == 11.);
BOOST_CHECK(cs1.contactPhase(7).timeInitial() == 11.);
// check that the final value of the previous phase have been copied in the initial value of the new one
BOOST_CHECK(cs1.contactPhase(5).m_c_final == cs1.contactPhase(6).m_c_init);
BOOST_CHECK(cs1.contactPhase(5).m_dc_final == cs1.contactPhase(6).m_dc_init);
BOOST_CHECK(cs1.contactPhase(5).m_ddc_final == cs1.contactPhase(6).m_ddc_init);
BOOST_CHECK(cs1.contactPhase(5).m_L_final == cs1.contactPhase(6).m_L_init);
BOOST_CHECK(cs1.contactPhase(5).m_dL_final == cs1.contactPhase(6).m_dL_init);
BOOST_CHECK(cs1.contactPhase(5).m_q_final == cs1.contactPhase(6).m_q_init);
// with MoveEffector, the middle phase should have the same initial and final point :
BOOST_CHECK(cs1.contactPhase(6).m_c_final == cs1.contactPhase(6).m_c_init);
BOOST_CHECK(cs1.contactPhase(6).m_dc_final == cs1.contactPhase(6).m_dc_init);
BOOST_CHECK(cs1.contactPhase(6).m_ddc_final == cs1.contactPhase(6).m_ddc_init);
BOOST_CHECK(cs1.contactPhase(6).m_L_final == cs1.contactPhase(6).m_L_init);
BOOST_CHECK(cs1.contactPhase(6).m_dL_final == cs1.contactPhase(6).m_dL_init);
BOOST_CHECK(cs1.contactPhase(6).m_q_final == cs1.contactPhase(6).m_q_init);
// check that the final value of the previous phase have been copied in the initial value of the new one
BOOST_CHECK(cs1.contactPhase(6).m_c_final == cs1.contactPhase(7).m_c_init);
BOOST_CHECK(cs1.contactPhase(6).m_dc_final == cs1.contactPhase(7).m_dc_init);
BOOST_CHECK(cs1.contactPhase(6).m_ddc_final == cs1.contactPhase(7).m_ddc_init);
BOOST_CHECK(cs1.contactPhase(6).m_L_final == cs1.contactPhase(7).m_L_init);
BOOST_CHECK(cs1.contactPhase(6).m_dL_final == cs1.contactPhase(7).m_dL_init);
BOOST_CHECK(cs1.contactPhase(6).m_q_final == cs1.contactPhase(7).m_q_init);
// check that the other contactPatch have been copied :
BOOST_CHECK(cs1.contactPhase(5).contactPatch("left_foot") == cs1.contactPhase(6).contactPatch("left_foot"));
BOOST_CHECK(cs1.contactPhase(6).contactPatch("left_foot") == cs1.contactPhase(7).contactPatch("left_foot"));
// # test move effector of:
SE3 target_transform =SE3::Identity().setRandom();
cs1.contactPhase(7).contactPatch("left_foot").friction() = 10.;
cs1.moveEffectorOf("left_foot",target_transform,1.,1.5);
BOOST_CHECK(cs1.size() == 10);
BOOST_CHECK(!cs1.contactPhase(8).isEffectorInContact("left_foot"));
BOOST_CHECK(cs1.contactPhase(9).isEffectorInContact("left_foot"));
target_placement = cs1.contactPhase(7).contactPatch("left_foot").placement().act(target_transform);
BOOST_CHECK(cs1.contactPhase(9).contactPatch("left_foot").placement() == target_placement);
BOOST_CHECK(cs1.contactPhase(9).contactPatch("left_foot").friction() == 10.);
// check that the other contactPatch have been copied :
BOOST_CHECK(cs1.contactPhase(7).contactPatch("right_hand") == cs1.contactPhase(8).contactPatch("right_hand"));
BOOST_CHECK(cs1.contactPhase(8).contactPatch("right_hand") == cs1.contactPhase(9).contactPatch("right_hand"));
}
BOOST_AUTO_TEST_CASE(contact_sequence_helper_throw) {
using std::invalid_argument;
// # check that break contact correctly throw error when needed :
ContactSequence cs1 = ContactSequence(0);
BOOST_CHECK(cs1.size() == 0);
ContactPhase cp0 = buildRandomContactPhase(0, 2);
ContactPhase cp1 = buildRandomContactPhase(2, 4.);
cp1.removeContact("left_foot");
cs1.append(cp0);
cs1.append(cp1);
BOOST_CHECK(cs1.size() == 2);
BOOST_CHECK_THROW(cs1.breakContact("left_foot"), invalid_argument); // contact do not exist
BOOST_CHECK(cs1.size() == 2);
ContactPhase cp2 = buildRandomContactPhase();
cs1.append(cp2);
BOOST_CHECK(cs1.size() == 3);
BOOST_CHECK_THROW(cs1.breakContact("left_foot",1.5), invalid_argument); // time interval not defined for last phase
BOOST_CHECK(cs1.size() == 3);
// # check that create contact correctly throw error when needed :
ContactPatch target(SE3::Identity().setRandom());
BOOST_CHECK_THROW(cs1.createContact("left_foot",target), invalid_argument); // contact already exist
BOOST_CHECK(cs1.size() == 3);
cs1.breakContact("left_foot");
BOOST_CHECK(cs1.size() == 4);
BOOST_CHECK_THROW(cs1.createContact("left_foot",target,2.), invalid_argument); // time interval not defined
BOOST_CHECK(cs1.size() == 4);
}
BOOST_AUTO_TEST_SUITE_END()
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