Commit 16bcdaff authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

ContactSequence::haveEffectorsTrajectories output the placement when the test fail

parent 7988f958
......@@ -613,6 +613,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
if(!pMax.isApprox(m_contact_phases.at(i+1).contactPatches().at(eeName).placement(), prec)){
std::cout<<"Effector trajectory for "<<eeName
<< " do not end at it's contact placement in the next phase, for phase "<<i<<std::endl;
std::cout<<"Last point : "<<std::endl<<pMax<<std::endl<<"Next contact : "
<<std::endl<<m_contact_phases.at(i+1).contactPatches().at(eeName).placement()<<std::endl;
return false;
}
if(i > 0 && m_contact_phases.at(i-1).isEffectorInContact(eeName)){
......@@ -620,6 +622,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
if(!pMin.isApprox(m_contact_phases.at(i-1).contactPatches().at(eeName).placement(), prec)){
std::cout<<"Effector trajectory for "<<eeName
<< " do not start at it's contact placement in the previous phase, for phase "<<i<<std::endl;
std::cout<<"First point : "<<std::endl<<pMin<<std::endl<<"Previous contact : "
<<std::endl<<m_contact_phases.at(i-1).contactPatches().at(eeName).placement()<<std::endl;
return false;
}
}
......
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