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

ContactSequence::haveEffectorTrajectory take an additionnal argument to ignore rotation

parent 9a0216d6
......@@ -24,7 +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)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(cs_haveEffectorTrajectories_overloads, CS::haveEffectorsTrajectories, 0, 2)
template <class PyClass>
void visit(PyClass& cl) const {
......@@ -130,14 +130,16 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
"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()),
(bp::args("precision_treshold") = Eigen::NumTraits<typename CS::Scalar>::dummy_precision(),
bp::args("use_rotation") = true),
"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.\n"
"If use_rotation == false, only the translation part of the transforms are compared. "))
.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"
......
......@@ -602,7 +602,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* placement.
* @return
*/
bool haveEffectorsTrajectories(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
bool haveEffectorsTrajectories(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(),
const bool use_rotation = true) const {
if (!haveTimings()) return false;
for (size_t i = 0; i < m_contact_phases.size() - 1; ++i) {
for (std::string eeName : m_contact_phases.at(i).getContactsCreated(m_contact_phases.at(i + 1))) {
......@@ -621,7 +622,8 @@ 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(), prec)) {
if ((use_rotation && ! pMax.isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement(), prec))
|| (! pMax.translation().isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement().translation(), 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
......@@ -632,7 +634,9 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
}
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(), prec)) {
if ((use_rotation && ! pMin.isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement(), prec))
|| (! pMin.translation().isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement().translation(), 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
......
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