Unverified Commit 8c81b3e9 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #10 from pFernbach/topic/fix_checks

fix contactSequence check methods
parents 026241bc 506ad2f2
Pipeline #9601 passed with stage
in 14 minutes and 32 seconds
......@@ -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"
......
......@@ -415,6 +415,18 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "CoM acceleration trajectory not defined for phase : " << i << std::endl;
return false;
}
if (phase.m_c->dim() != 3) {
std::cout << "CoM trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_dc->dim() != 3) {
std::cout << "CoM velocity trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_ddc->dim() != 3) {
std::cout << "CoM acceleration trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_c->min() != phase.timeInitial()) {
std::cout << "CoM trajectory do not start at t_init for phase : " << i << std::endl;
return false;
......@@ -508,6 +520,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "AM velocity trajectory not defined for phase : " << i << std::endl;
return false;
}
if (phase.m_L->dim() != 3) {
std::cout << "AM trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_dL->dim() != 3) {
std::cout << "AM derivative trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_L->min() != phase.timeInitial()) {
std::cout << "AM trajectory do not start at t_init for phase : " << i << std::endl;
return false;
......@@ -582,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))) {
......@@ -601,7 +622,10 @@ 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
......@@ -612,7 +636,10 @@ 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
......@@ -847,6 +874,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "ZMP trajectory not defined for phase : " << i << std::endl;
return false;
}
if (phase.m_zmp->dim() != 3) {
std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i << std::endl;
return false;
}
if (phase.m_zmp->min() != phase.timeInitial()) {
std::cout << "ZMP trajectory do not start at t_init for phase : " << i << std::endl;
return false;
......
......@@ -80,6 +80,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_REF) {
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories(1e-6, false));
}
BOOST_AUTO_TEST_CASE(step_in_place_WB) {
......
......@@ -149,6 +149,7 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_step_in_place_WB(self):
......
......@@ -1974,4 +1974,101 @@ BOOST_AUTO_TEST_CASE(contact_sequence_phase_at_time) {
BOOST_CHECK_THROW(cs1.phaseAtTime(10.), std::invalid_argument);
}
BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory) {
ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 2);
cp0.m_c = buildRandomPolynomial3D(0, 2);
cp0.m_c_init = cp0.m_c->operator()(0);
cp0.m_c_final = cp0.m_c->operator()(2);
cp0.m_dc = buildRandomPolynomial3D(0, 2);
cp0.m_dc_init = cp0.m_dc->operator()(0);
cp0.m_dc_final = cp0.m_dc->operator()(2);
cp0.m_ddc = buildRandomPolynomial3D(0, 2);
cp0.m_ddc_init = cp0.m_ddc->operator()(0);
cp0.m_ddc_final = cp0.m_ddc->operator()(2);
cp0.m_L = buildRandomPolynomial3D(0, 2);
cp0.m_L_init = cp0.m_L->operator()(0);
cp0.m_L_final = cp0.m_L->operator()(2);
cp0.m_dL = buildRandomPolynomial3D(0, 2);
cp0.m_dL_init = cp0.m_dL->operator()(0);
cp0.m_dL_final = cp0.m_dL->operator()(2);
cp0.m_zmp = buildRandomPolynomial3D(0, 2);
cs1.append(cp0);
BOOST_CHECK(cs1.haveCOMtrajectories());
BOOST_CHECK(cs1.haveAMtrajectories());
BOOST_CHECK(cs1.haveZMPtrajectories());
}
BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory_wrong_dimension) {
ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 2);
cp0.m_c = buildRandomPolynomial3D(0, 2);
cp0.m_c_init = cp0.m_c->operator()(0);
cp0.m_c_final = cp0.m_c->operator()(2);
cp0.m_dc = buildRandomPolynomial12D(0, 2);
cp0.m_dc_init = cp0.m_dc->operator()(0).head<3>();
cp0.m_dc_final = cp0.m_dc->operator()(2).head<3>();
cp0.m_ddc = buildRandomPolynomial3D(0, 2);
cp0.m_ddc_init = cp0.m_ddc->operator()(0);
cp0.m_ddc_final = cp0.m_ddc->operator()(2);
cp0.m_L = buildRandomPolynomial12D(0, 2);
cp0.m_L_init = cp0.m_L->operator()(0).head<3>();
cp0.m_L_final = cp0.m_L->operator()(2).head<3>();
cp0.m_dL = buildRandomPolynomial3D(0, 2);
cp0.m_dL_init = cp0.m_dL->operator()(0);
cp0.m_dL_final = cp0.m_dL->operator()(2);
cp0.m_zmp = buildRandomPolynomial12D(0, 2);
cs1.append(cp0);
BOOST_CHECK(!cs1.haveCOMtrajectories());
BOOST_CHECK(!cs1.haveAMtrajectories());
BOOST_CHECK(!cs1.haveZMPtrajectories());
}
BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory) {
ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 1);
ContactPhase cp1 = ContactPhase(1, 2);
ContactPhase cp2 = ContactPhase(2, 3);
cp1.addEffectorTrajectory("left_foot", buildRandomSE3LinearTraj(1, 2));
cp0.addContact("left_foot", ContactPatch(SE3(cp1.effectorTrajectories("left_foot")->operator()(1.).matrix())));
cp2.addContact("left_foot", ContactPatch(SE3(cp1.effectorTrajectories("left_foot")->operator()(2.).matrix())));
cs1.append(cp0);
cs1.append(cp1);
cs1.append(cp2);
BOOST_CHECK(cs1.haveEffectorsTrajectories());
BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-3));
BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-3, true));
}
BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation) {
ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 1);
ContactPhase cp1 = ContactPhase(1, 2);
ContactPhase cp2 = ContactPhase(2, 3);
cp1.addEffectorTrajectory("left_foot", buildRandomSE3LinearTraj(1, 2));
SE3 p0(cp1.effectorTrajectories("left_foot")->operator()(1.).matrix());
SE3 p1(cp1.effectorTrajectories("left_foot")->operator()(2.).matrix());
SE3 p0_r = SE3::Identity().setRandom();
SE3 p1_r = SE3::Identity().setRandom();
p0.rotation() = p0_r.rotation();
p1.rotation() = p1_r.rotation();
cp0.addContact("left_foot", ContactPatch(p0));
cp2.addContact("left_foot", ContactPatch(p1));
cs1.append(cp0);
cs1.append(cp1);
cs1.append(cp2);
BOOST_CHECK(!cs1.haveEffectorsTrajectories());
BOOST_CHECK(!cs1.haveEffectorsTrajectories(1e-6));
BOOST_CHECK(!cs1.haveEffectorsTrajectories(1e-6, true));
BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-6, false));
}
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