Commit 3ea4d75a authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Format] fix format

parent e30cdfec
...@@ -262,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth ...@@ -262,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
static bp::list getAllEffectorsInContactAsList(CS& self) { static bp::list getAllEffectorsInContactAsList(CS& self) {
return toPythonList<std::string>(self.getAllEffectorsInContact()); return toPythonList<std::string>(self.getAllEffectorsInContact());
} }
}; };
} // namespace python } // namespace python
} // namespace multicontact_api } // namespace multicontact_api
......
...@@ -13,22 +13,20 @@ namespace bp = boost::python; ...@@ -13,22 +13,20 @@ namespace bp = boost::python;
template <typename Derived> template <typename Derived>
struct cs_pickle_suite : bp::pickle_suite { struct cs_pickle_suite : bp::pickle_suite {
static bp::object getstate(const Derived& cs) {
std::ostringstream os;
boost::archive::text_oarchive oa(os);
oa << cs;
return bp::str(os.str());
}
static bp::object getstate (const Derived& cs) { static void setstate(Derived& cs, bp::object entries) {
std::ostringstream os; bp::str s = bp::extract<bp::str>(entries)();
boost::archive::text_oarchive oa(os); std::string st = bp::extract<std::string>(s)();
oa << cs; std::istringstream is(st);
return bp::str(os.str()); boost::archive::text_iarchive ia(is);
} ia >> cs;
}
static void
setstate(Derived& cs, bp::object entries) {
bp::str s = bp::extract<bp::str> (entries)();
std::string st = bp::extract<std::string> (s)();
std::istringstream is (st);
boost::archive::text_iarchive ia (is);
ia >> cs;
}
}; };
template <typename Derived> template <typename Derived>
......
...@@ -415,16 +415,16 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp ...@@ -415,16 +415,16 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "CoM acceleration trajectory not defined for phase : " << i << std::endl; std::cout << "CoM acceleration trajectory not defined for phase : " << i << std::endl;
return false; return false;
} }
if(phase.m_c->dim() != 3){ if (phase.m_c->dim() != 3) {
std::cout<<"CoM trajectory is not of dimension 3 for phase : " << i << std::endl; std::cout << "CoM trajectory is not of dimension 3 for phase : " << i << std::endl;
return false; return false;
} }
if(phase.m_dc->dim() != 3){ if (phase.m_dc->dim() != 3) {
std::cout<<"CoM velocity trajectory is not of dimension 3 for phase : " << i << std::endl; std::cout << "CoM velocity trajectory is not of dimension 3 for phase : " << i << std::endl;
return false; return false;
} }
if(phase.m_ddc->dim() != 3){ if (phase.m_ddc->dim() != 3) {
std::cout<<"CoM acceleration trajectory is not of dimension 3 for phase : " << i << std::endl; std::cout << "CoM acceleration trajectory is not of dimension 3 for phase : " << i << std::endl;
return false; return false;
} }
if (phase.m_c->min() != phase.timeInitial()) { if (phase.m_c->min() != phase.timeInitial()) {
...@@ -520,12 +520,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp ...@@ -520,12 +520,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "AM velocity trajectory not defined for phase : " << i << std::endl; std::cout << "AM velocity trajectory not defined for phase : " << i << std::endl;
return false; return false;
} }
if(phase.m_L->dim() != 3){ if (phase.m_L->dim() != 3) {
std::cout<<"AM trajectory is not of dimension 3 for phase : " << i << std::endl; std::cout << "AM trajectory is not of dimension 3 for phase : " << i << std::endl;
return false; return false;
} }
if(phase.m_dL->dim() != 3){ if (phase.m_dL->dim() != 3) {
std::cout<<"AM derivative trajectory is not of dimension 3 for phase : " << i << std::endl; std::cout << "AM derivative trajectory is not of dimension 3 for phase : " << i << std::endl;
return false; return false;
} }
if (phase.m_L->min() != phase.timeInitial()) { if (phase.m_L->min() != phase.timeInitial()) {
...@@ -622,8 +622,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp ...@@ -622,8 +622,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return false; return false;
} }
ContactPatch::SE3 pMax = ContactPatch::SE3((*traj)(traj->max()).matrix()); ContactPatch::SE3 pMax = ContactPatch::SE3((*traj)(traj->max()).matrix());
if ((use_rotation && ! pMax.isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement(), prec)) if ((use_rotation &&
|| (! pMax.translation().isApprox(m_contact_phases.at(i + 1).contactPatches().at(eeName).placement().translation(), prec))) { !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 std::cout << "Effector trajectory for " << eeName
<< " do not end at it's contact placement in the next phase, for phase " << i << std::endl; << " do not end at it's contact placement in the next phase, for phase " << i << std::endl;
std::cout << "Last point : " << std::endl std::cout << "Last point : " << std::endl
...@@ -634,9 +636,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp ...@@ -634,9 +636,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
} }
if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) { if (i > 0 && m_contact_phases.at(i - 1).isEffectorInContact(eeName)) {
ContactPatch::SE3 pMin = ContactPatch::SE3((*traj)(traj->min()).matrix()); ContactPatch::SE3 pMin = ContactPatch::SE3((*traj)(traj->min()).matrix());
if ((use_rotation && ! pMin.isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement(), prec)) if ((use_rotation &&
|| (! pMin.translation().isApprox(m_contact_phases.at(i - 1).contactPatches().at(eeName).placement().translation(), prec))) !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 std::cout << "Effector trajectory for " << eeName
<< " do not start at it's contact placement in the previous phase, for phase " << i << std::endl; << " do not start at it's contact placement in the previous phase, for phase " << i << std::endl;
std::cout << "First point : " << std::endl std::cout << "First point : " << std::endl
...@@ -853,8 +856,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp ...@@ -853,8 +856,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std::cout << "ZMP trajectory not defined for phase : " << i << std::endl; std::cout << "ZMP trajectory not defined for phase : " << i << std::endl;
return false; return false;
} }
if(phase.m_zmp->dim() != 3){ if (phase.m_zmp->dim() != 3) {
std::cout<<"ZMP trajectory is not of dimension 3 for phase : " << i << std::endl; std::cout << "ZMP trajectory is not of dimension 3 for phase : " << i << std::endl;
return false; return false;
} }
if (phase.m_zmp->min() != phase.timeInitial()) { if (phase.m_zmp->min() != phase.timeInitial()) {
......
...@@ -34,7 +34,6 @@ using curves::t_pointX_t; ...@@ -34,7 +34,6 @@ using curves::t_pointX_t;
using namespace multicontact_api::scenario; using namespace multicontact_api::scenario;
typedef ContactSequence::ContactPhaseVector ContactPhaseVector; typedef ContactSequence::ContactPhaseVector ContactPhaseVector;
typedef pinocchio::SE3Tpl<double> SE3; typedef pinocchio::SE3Tpl<double> SE3;
curve_ptr_t buildPiecewisePolynomialC2() { curve_ptr_t buildPiecewisePolynomialC2() {
...@@ -1815,25 +1814,25 @@ BOOST_AUTO_TEST_CASE(contact_sequence_phase_at_time) { ...@@ -1815,25 +1814,25 @@ BOOST_AUTO_TEST_CASE(contact_sequence_phase_at_time) {
BOOST_CHECK_THROW(cs1.phaseAtTime(10.), std::invalid_argument); BOOST_CHECK_THROW(cs1.phaseAtTime(10.), std::invalid_argument);
} }
BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory){ BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory) {
ContactSequence cs1 = ContactSequence(0); ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 2); ContactPhase cp0 = ContactPhase(0, 2);
cp0.m_c = buildRandomPolynomial3D(0,2); cp0.m_c = buildRandomPolynomial3D(0, 2);
cp0.m_c_init = cp0.m_c->operator()(0); cp0.m_c_init = cp0.m_c->operator()(0);
cp0.m_c_final = cp0.m_c->operator()(2); cp0.m_c_final = cp0.m_c->operator()(2);
cp0.m_dc = buildRandomPolynomial3D(0,2); cp0.m_dc = buildRandomPolynomial3D(0, 2);
cp0.m_dc_init = cp0.m_dc->operator()(0); cp0.m_dc_init = cp0.m_dc->operator()(0);
cp0.m_dc_final = cp0.m_dc->operator()(2); cp0.m_dc_final = cp0.m_dc->operator()(2);
cp0.m_ddc = buildRandomPolynomial3D(0,2); cp0.m_ddc = buildRandomPolynomial3D(0, 2);
cp0.m_ddc_init = cp0.m_ddc->operator()(0); cp0.m_ddc_init = cp0.m_ddc->operator()(0);
cp0.m_ddc_final = cp0.m_ddc->operator()(2); cp0.m_ddc_final = cp0.m_ddc->operator()(2);
cp0.m_L = buildRandomPolynomial3D(0,2); cp0.m_L = buildRandomPolynomial3D(0, 2);
cp0.m_L_init = cp0.m_L->operator()(0); cp0.m_L_init = cp0.m_L->operator()(0);
cp0.m_L_final = cp0.m_L->operator()(2); cp0.m_L_final = cp0.m_L->operator()(2);
cp0.m_dL = buildRandomPolynomial3D(0,2); cp0.m_dL = buildRandomPolynomial3D(0, 2);
cp0.m_dL_init = cp0.m_dL->operator()(0); cp0.m_dL_init = cp0.m_dL->operator()(0);
cp0.m_dL_final = cp0.m_dL->operator()(2); cp0.m_dL_final = cp0.m_dL->operator()(2);
cp0.m_zmp = buildRandomPolynomial3D(0,2); cp0.m_zmp = buildRandomPolynomial3D(0, 2);
cs1.append(cp0); cs1.append(cp0);
BOOST_CHECK(cs1.haveCOMtrajectories()); BOOST_CHECK(cs1.haveCOMtrajectories());
...@@ -1841,33 +1840,33 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory){ ...@@ -1841,33 +1840,33 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory){
BOOST_CHECK(cs1.haveZMPtrajectories()); BOOST_CHECK(cs1.haveZMPtrajectories());
} }
BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory_wrong_dimension){ BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory_wrong_dimension) {
ContactSequence cs1 = ContactSequence(0); ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 2); ContactPhase cp0 = ContactPhase(0, 2);
cp0.m_c = buildRandomPolynomial3D(0,2); cp0.m_c = buildRandomPolynomial3D(0, 2);
cp0.m_c_init = cp0.m_c->operator()(0); cp0.m_c_init = cp0.m_c->operator()(0);
cp0.m_c_final = cp0.m_c->operator()(2); cp0.m_c_final = cp0.m_c->operator()(2);
cp0.m_dc = buildRandomPolynomial12D(0,2); cp0.m_dc = buildRandomPolynomial12D(0, 2);
cp0.m_dc_init = cp0.m_dc->operator()(0).head<3>(); cp0.m_dc_init = cp0.m_dc->operator()(0).head<3>();
cp0.m_dc_final = cp0.m_dc->operator()(2).head<3>(); cp0.m_dc_final = cp0.m_dc->operator()(2).head<3>();
cp0.m_ddc = buildRandomPolynomial3D(0,2); cp0.m_ddc = buildRandomPolynomial3D(0, 2);
cp0.m_ddc_init = cp0.m_ddc->operator()(0); cp0.m_ddc_init = cp0.m_ddc->operator()(0);
cp0.m_ddc_final = cp0.m_ddc->operator()(2); cp0.m_ddc_final = cp0.m_ddc->operator()(2);
cp0.m_L = buildRandomPolynomial12D(0,2); cp0.m_L = buildRandomPolynomial12D(0, 2);
cp0.m_L_init = cp0.m_L->operator()(0).head<3>(); cp0.m_L_init = cp0.m_L->operator()(0).head<3>();
cp0.m_L_final = cp0.m_L->operator()(2).head<3>(); cp0.m_L_final = cp0.m_L->operator()(2).head<3>();
cp0.m_dL = buildRandomPolynomial3D(0,2); cp0.m_dL = buildRandomPolynomial3D(0, 2);
cp0.m_dL_init = cp0.m_dL->operator()(0); cp0.m_dL_init = cp0.m_dL->operator()(0);
cp0.m_dL_final = cp0.m_dL->operator()(2); cp0.m_dL_final = cp0.m_dL->operator()(2);
cp0.m_zmp = buildRandomPolynomial12D(0,2); cp0.m_zmp = buildRandomPolynomial12D(0, 2);
cs1.append(cp0); cs1.append(cp0);
BOOST_CHECK(! cs1.haveCOMtrajectories()); BOOST_CHECK(!cs1.haveCOMtrajectories());
BOOST_CHECK(! cs1.haveAMtrajectories()); BOOST_CHECK(!cs1.haveAMtrajectories());
BOOST_CHECK(! cs1.haveZMPtrajectories()); BOOST_CHECK(!cs1.haveZMPtrajectories());
} }
BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory){ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory) {
ContactSequence cs1 = ContactSequence(0); ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 1); ContactPhase cp0 = ContactPhase(0, 1);
ContactPhase cp1 = ContactPhase(1, 2); ContactPhase cp1 = ContactPhase(1, 2);
...@@ -1886,7 +1885,7 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory){ ...@@ -1886,7 +1885,7 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory){
BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-3, true)); BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-3, true));
} }
BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation){ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation) {
ContactSequence cs1 = ContactSequence(0); ContactSequence cs1 = ContactSequence(0);
ContactPhase cp0 = ContactPhase(0, 1); ContactPhase cp0 = ContactPhase(0, 1);
ContactPhase cp1 = ContactPhase(1, 2); ContactPhase cp1 = ContactPhase(1, 2);
...@@ -1912,5 +1911,4 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation){ ...@@ -1912,5 +1911,4 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation){
BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-6, false)); BOOST_CHECK(cs1.haveEffectorsTrajectories(1e-6, false));
} }
BOOST_AUTO_TEST_SUITE_END() 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