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

[C++] small fixes and format in new helpers in contact sequence

parent 69ddd986
......@@ -84,19 +84,18 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* @param eeName the name of the effector to remove from contact
* @param phaseDuration if provided, the duration of the previous last phase of the sequence is set to this value
* (it is thus the duration BEFORE breaking the contact)
* @return true if the last phase had eeName in contact, false otherwise
* @throw invalid_argument if the phaseDuration is provided but the last phase do not have a time-range defined
* @throw invalid_argument if eeName is not in contact in the last phase of the sequence
*/
void breakContact(const std::string& eeName, const double phaseDuration = -1){
void breakContact(const std::string& eeName, const double phaseDuration = -1) {
ContactPhase& lastPhase = m_contact_phases.back();
if(!lastPhase.isEffectorInContact(eeName))
throw std::invalid_argument("In breakContact : effector is not currently in contact : "+eeName);
if(phaseDuration > 0){
if(lastPhase.timeInitial() < 0){
throw std::invalid_argument("In breakContact : duration is specified but current phase interval in not initialised.");
}
else {
if (!lastPhase.isEffectorInContact(eeName))
throw std::invalid_argument("In breakContact : effector is not currently in contact : " + eeName);
if (phaseDuration > 0) {
if (lastPhase.timeInitial() < 0) {
throw std::invalid_argument(
"In breakContact : duration is specified but current phase interval in not initialised.");
} else {
lastPhase.duration(phaseDuration);
}
}
......@@ -111,9 +110,9 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
phase.m_q_init = lastPhase.m_q_final;
// copy contact patchs :
for(std::string name : lastPhase.effectorsInContact()){
if(name != eeName){
phase.addContact(name,lastPhase.contactPatch(name));
for (std::string name : lastPhase.effectorsInContact()) {
if (name != eeName) {
phase.addContact(name, lastPhase.contactPatch(name));
}
}
// add new phase at the end of the sequence
......@@ -132,15 +131,15 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* @throw invalid_argument if the phaseDuration is provided but the last phase do not have a time-range defined
* @throw invalid_argument if eeName is already in contact in the last phase of the sequence
*/
void createContact(const std::string& eeName, const ContactPatch& patch, const double phaseDuration = -1){
void createContact(const std::string& eeName, const ContactPatch& patch, const double phaseDuration = -1) {
ContactPhase& lastPhase = m_contact_phases.back();
if(lastPhase.isEffectorInContact(eeName))
throw std::invalid_argument("In createContact : effector is already in contact : "+eeName);
if(phaseDuration > 0){
if(lastPhase.timeInitial() < 0){
throw std::invalid_argument("In createContact : duration is specified but current phase interval in not initialised.");
}
else {
if (lastPhase.isEffectorInContact(eeName))
throw std::invalid_argument("In createContact : effector is already in contact : " + eeName);
if (phaseDuration > 0) {
if (lastPhase.timeInitial() < 0) {
throw std::invalid_argument(
"In createContact : duration is specified but current phase interval in not initialised.");
} else {
lastPhase.duration(phaseDuration);
}
}
......@@ -155,17 +154,16 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
phase.m_q_init = lastPhase.m_q_final;
// copy contact patchs :
for(std::string name : lastPhase.effectorsInContact()){
phase.addContact(name,lastPhase.contactPatch(name));
for (std::string name : lastPhase.effectorsInContact()) {
phase.addContact(name, lastPhase.contactPatch(name));
}
// add new contact to new phase :
phase.addContact(eeName,patch);
phase.addContact(eeName, patch);
// add new phase at the end of the sequence
append(phase);
return;
}
/**
* @brief moveEffectorToPlacement Add two new phases at the end of the current ContactSequence,
* - it break the contact with eeName
......@@ -182,12 +180,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* @throw invalid_argument if eeName is not in contact in the last phase of the sequence
*/
void moveEffectorToPlacement(const std::string& eeName, const ContactPatch::SE3& placement,
const double durationBreak = -1, const double durationCreate = -1 ){
if(!m_contact_phases.back().isEffectorInContact(eeName))
throw std::invalid_argument("In moveEffectorToPlacement : effector is not currently in contact : "+eeName);
const double durationBreak = -1, const double durationCreate = -1) {
if (!m_contact_phases.back().isEffectorInContact(eeName))
throw std::invalid_argument("In moveEffectorToPlacement : effector is not currently in contact : " + eeName);
ContactPatch target(m_contact_phases.back().contactPatch(eeName));
target.placement() = placement;
breakContact(eeName,durationBreak);
breakContact(eeName, durationBreak);
// copy all "init" value to "final" for the current last phase :
ContactPhase& lastPhase = m_contact_phases.back();
lastPhase.m_c_final = lastPhase.m_c_init;
......@@ -196,15 +194,15 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
lastPhase.m_L_final = lastPhase.m_L_init;
lastPhase.m_dL_final = lastPhase.m_dL_init;
lastPhase.m_q_final = lastPhase.m_q_init;
createContact(eeName,target,durationCreate);
createContact(eeName, target, durationCreate);
return;
}
/**
/**
* @brief moveEffectorOf similar to moveEffectorToPlacement
* exept that the new placement is defined from the previous placement and a given transform applied.
* @param eeName the name of the effector used to create contact
* @param transform the new placement for the contact of eeName
* @param transform the transform applied to the placement of the contact in the last phase of the sequence
* @param durationBreak the duration of the previous last phase of the sequence
* (it is thus the duration BEFORE breaking the contact)
* @param durationCreate the duration of the first new ContactPhase
......@@ -212,16 +210,15 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* @throw invalid_argument if the phaseDuration is provided but the last phase do not have a time-range defined
* @throw invalid_argument if eeName is not in contact in the last phase of the sequence
*/
void moveEffectorOf(const std::string& eeName, const ContactPatch::SE3& transform,
const double durationBreak = -1, const double durationCreate = -1 ){
if(!m_contact_phases.back().isEffectorInContact(eeName))
throw std::invalid_argument("In moveEffectorToPlacement : effector is not currently in contact : "+eeName);
void moveEffectorOf(const std::string& eeName, const ContactPatch::SE3& transform, const double durationBreak = -1,
const double durationCreate = -1) {
if (!m_contact_phases.back().isEffectorInContact(eeName))
throw std::invalid_argument("In moveEffectorToPlacement : effector is not currently in contact : " + eeName);
ContactPatch::SE3 previous(m_contact_phases.back().contactPatch(eeName).placement());
ContactPatch::SE3 target = previous.act(transform);
return moveEffectorToPlacement(eeName,target,durationBreak,durationCreate);
return moveEffectorToPlacement(eeName, target, durationBreak, durationCreate);
}
/* End Helpers */
/*Public Attributes*/
......
......@@ -1115,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(0,2);
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;
......@@ -1369,13 +1369,13 @@ BOOST_AUTO_TEST_CASE(contact_sequence) {
ContactSequence cs4;
BOOST_CHECK(cs3 == cs4);
ContactPhase cp3_0 = buildRandomContactPhase(0,2);
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(0,2);
ContactPhase cp3_1 = buildRandomContactPhase(0, 2);
cs3.append(cp3_1);
BOOST_CHECK(cs3 != cs4);
cs4.append(cp3_1);
......@@ -1392,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(0,2);
ContactPhase cp6 = buildRandomContactPhase(0, 2);
cs6.append(cp6);
}
BOOST_CHECK(cs6.size() == 10);
......@@ -1430,9 +1430,9 @@ BOOST_AUTO_TEST_CASE(contact_sequence_helper) {
// # 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
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);
......@@ -1446,9 +1446,9 @@ BOOST_AUTO_TEST_CASE(contact_sequence_helper) {
// # 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
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);
......@@ -1462,27 +1462,26 @@ BOOST_AUTO_TEST_CASE(contact_sequence_helper) {
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
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
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
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();
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 );
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);
......@@ -1518,12 +1517,10 @@ BOOST_AUTO_TEST_CASE(contact_sequence_helper) {
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();
SE3 target_transform = SE3::Identity().setRandom();
cs1.contactPhase(7).contactPatch("left_foot").friction() = 10.;
cs1.moveEffectorOf("left_foot",target_transform,1.,1.5);
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"));
......@@ -1533,8 +1530,6 @@ BOOST_AUTO_TEST_CASE(contact_sequence_helper) {
// 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) {
......@@ -1548,25 +1543,22 @@ BOOST_AUTO_TEST_CASE(contact_sequence_helper_throw) {
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_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_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_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_THROW(cs1.createContact("left_foot", target, 2.), invalid_argument); // time interval not defined
BOOST_CHECK(cs1.size() == 4);
}
BOOST_AUTO_TEST_SUITE_END()
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