Commit 67b2d585 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

haveEffectorTrajectories now take as input the precision treshold used in isApprox()

parent c86b64c8
......@@ -24,6 +24,7 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_createContact_overloads, CS::createContact, 2, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorToPlacement_overloads, CS::moveEffectorToPlacement, 2, 4)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_moveEffectorOf_overloads, CS::moveEffectorOf, 2, 4)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, CS::haveEffectorsTrajectories, 0, 1)
template <class PyClass>
void visit(PyClass& cl) const {
......@@ -128,12 +129,14 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
"Also check that the time interval of this trajectories matches the one of the phase.\n"
"and that the trajectories start and end and the correct values defined in each phase.")
.def("haveEffectorsTrajectories", &CS::haveEffectorsTrajectories,
cs_haveEffectorTrajectories_overloads(
(bp::args("precision_treshold") = Eigen::NumTraits<typename CS::Scalar>::dummy_precision()),
"check that for each phase preceeding a contact creation,"
"an SE3 trajectory is defined for the effector that will be in contact.\n"
"Also check that this trajectory is defined on the time-interval of the phase.\n"
"Also check that the trajectory correctly end at the placement defined for the contact in the next phase.\n"
"If this effector was in contact in the previous phase,"
"it check that the trajectory start at the previous contact placement.")
"it check that the trajectory start at the previous contact placement."))
.def("haveJointsTrajectories", &CS::haveJointsTrajectories,
"Check that a q trajectory is defined for each phases.\n"
"Also check that the time interval of this trajectories matches the one of the phase.\n"
......
......@@ -591,7 +591,7 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* If this effector was in contact in the previous phase, it check that the trajectory start at the previous contact placement.
* @return
*/
bool haveEffectorsTrajectories() const{
bool haveEffectorsTrajectories( const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const{
if(!haveTimings())
return false;
for(size_t i = 0 ; i < m_contact_phases.size() -1 ; ++i){
......@@ -610,14 +610,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return false;
}
ContactPatch::SE3 pMax = ContactPatch::SE3((*traj)(traj->max()).matrix());
if(!pMax.isApprox(m_contact_phases.at(i+1).contactPatches().at(eeName).placement())){
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;
return false;
}
if(i > 0 && m_contact_phases.at(i-1).isEffectorInContact(eeName)){
ContactPatch::SE3 pMin = ContactPatch::SE3((*traj)(traj->min()).matrix());
if(!pMin.isApprox(m_contact_phases.at(i-1).contactPatches().at(eeName).placement())){
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;
return false;
......
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