Skip to content

GitLab

Commit 41868547 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

format

parent 326738d2
Expand all Hide whitespace changes
Inline Side-by-side
include/multicontact-api/bindings/python/scenario/contact-phase.hpp
View file @ 41868547
......@@ -36,7 +36,6 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
typedef curves::pointX_list_t pointX_list_t;
typedef curves::time_waypoints_t time_waypoints_t;
// call macro for all ContactPhase methods that can be overloaded
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(isConsistent_overloads, ContactPhase::isConsistent, 0, 1)
......@@ -179,14 +178,15 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
.def("setCOMtrajectoryFromPoints", &setCOMtrajectoryFromPoints,
bp::args("COM_positions", "COM_velocities", "COM_accelerations", "times"),
"set the c,dc and ddc curves from a list of discrete"
"COM positions, velocity and accelerations.\n"
"The trajectories are build with first order polynomials connecting each discrete points given.\n"
"This method also set the initial/final values for c, dc and ddc from the first and last discrete point given.")
.def("setAMtrajectoryFromPoints", &setAMtrajectoryFromPoints,
bp::args("AM_values", "AM_derivatives", "times"),
"COM positions, velocity and accelerations.\n"
"The trajectories are build with first order polynomials connecting each discrete points given.\n"
"This method also set the initial/final values for c, dc and ddc from the first and last discrete point "
"given.")
.def("setAMtrajectoryFromPoints", &setAMtrajectoryFromPoints, bp::args("AM_values", "AM_derivatives", "times"),
"set the L and d_L curves from a list of discrete Angular velocity values and their derivatives.\n"
"The trajectories are build with first order polynomials connecting each discrete points given.\n"
"This method also set the initial/final values for L, and dL from the first and last discrete point given.")
"This method also set the initial/final values for L, and dL from the first and last discrete point "
"given.")
.def("setJointsTrajectoryFromPoints", &setJointsTrajectoryFromPoints,
bp::args("Joints_poisitions", "Joints_velocities", "Joints_accelerations", "times"),
"set the q,dq and ddq curves from a list of discrete joints positions, velocity and accelerations."
......@@ -263,7 +263,6 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
return toPythonList<std::string>(self.getContactsVariations(to));
}
// Converts a C++ map to a python dict
// Note : lot of overhead, should not be used for large map and/or operations called frequently.
// prefer the direct bindings with std_map_* for this cases.
......@@ -288,42 +287,43 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
return toPythonDict<curve_SE3_ptr>(self.effectorTrajectories());
}
static void setCOMtrajectoryFromPoints(ContactPhase& self, const pointX_list_t& points,
const pointX_list_t& points_derivative,
const pointX_list_t& points_second_derivative,
const time_waypoints_t& time_points) {
t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative);
t_pointX_t points_second_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative);
t_pointX_t points_second_derivative_list =
curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative);
t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
self.setCOMtrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list, time_points_list);
return ;
self.setCOMtrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list,
time_points_list);
return;
}
static void setJointsTrajectoryFromPoints(ContactPhase& self, const pointX_list_t& points,
const pointX_list_t& points_derivative,
const pointX_list_t& points_second_derivative,
const time_waypoints_t& time_points) {
const pointX_list_t& points_derivative,
const pointX_list_t& points_second_derivative,
const time_waypoints_t& time_points) {
t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative);
t_pointX_t points_second_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative);
t_pointX_t points_second_derivative_list =
curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative);
t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
self.setJointsTrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list, time_points_list);
return ;
self.setJointsTrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list,
time_points_list);
return;
}
static void setAMtrajectoryFromPoints(ContactPhase& self, const pointX_list_t& points,
const pointX_list_t& points_derivative,
const time_waypoints_t& time_points) {
const pointX_list_t& points_derivative, const time_waypoints_t& time_points) {
t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative);
t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
self.setAMtrajectoryFromPoints(points_list, points_derivative_list, time_points_list);
return ;
return;
}
static ContactPhase copy(const ContactPhase& self) { return ContactPhase(self); }
};
} // namespace python
......
include/multicontact-api/bindings/python/scenario/contact-sequence.hpp
View file @ 41868547
......@@ -130,13 +130,14 @@ 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()),
"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."))
(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."))
.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"
......@@ -161,7 +162,7 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
"a friction coefficient initialized.")
.def("haveZMPtrajectories", &CS::haveZMPtrajectories,
"check that all the contact phases have a ZMP trajectory.")
.def("getAllEffectorsInContact",&getAllEffectorsInContactAsList,
.def("getAllEffectorsInContact", &getAllEffectorsInContactAsList,
"return a list of names of all the effectors used to create contacts during the sequence")
.def("concatenateCtrajectories", &CS::concatenateCtrajectories,
"Return a piecewise curve wchich is the concatenation of the m_c curves"
......@@ -199,35 +200,30 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
.def("concatenateRootTrajectories", &CS::concatenateRootTrajectories,
"Return a piecewise curve wchich is the concatenation of the m_root curves"
" for each contact phases in the sequence.")
.def("concatenateEffectorTrajectories", &CS::concatenateEffectorTrajectories,
bp::arg("eeName"),
.def("concatenateEffectorTrajectories", &CS::concatenateEffectorTrajectories, bp::arg("eeName"),
"Return a piecewise curve which is the concatenation"
"of the effectors trajectories curves for the given effector"
"for each contact phases in the sequence.\n"
"During the phases where no effector trajectories are defined,"
"the trajectory is constant with the value of"
"the last phase where it was defined.")
.def("concatenateContactForceTrajectories", &CS::concatenateContactForceTrajectories,
bp::arg("eeName"),
.def("concatenateContactForceTrajectories", &CS::concatenateContactForceTrajectories, bp::arg("eeName"),
"Return a piecewise curve which"
"is the concatenation of the contact forces for the given effector"
"for each contact phases in the sequence.\n"
"During the phases where no contact forces are defined,"
"the trajectory is constant with the value of 0.")
.def("concatenateNormalForceTrajectories", &CS::concatenateNormalForceTrajectories,
bp::arg("eeName"),
.def("concatenateNormalForceTrajectories", &CS::concatenateNormalForceTrajectories, bp::arg("eeName"),
"Return a piecewise curve which"
"is the concatenation of the contact normal forces for the given effector"
"for each contact phases in the sequence.\n"
"During the phases where no contact normal forces are defined,"
"the trajectory is constant with the value of 0.")
.def("phaseIdAtTime", &CS::phaseIdAtTime,
bp::arg("time"),
.def("phaseIdAtTime", &CS::phaseIdAtTime, bp::arg("time"),
"return the index of a phase in the sequence such that "
"phase.timeInitial <= t < phase.timeFinal \n"
"if t equal to the last phase timeFinal, this index is returned.")
.def("phaseAtTime", &CS::phaseAtTime,
bp::arg("time"), bp::return_internal_reference<>(),
.def("phaseAtTime", &CS::phaseAtTime, bp::arg("time"), bp::return_internal_reference<>(),
"return a phase of the sequence such that "
"phase.timeInitial <= t < phase.timeFinal \n"
"if t equal to the last phase timeFinal, this index is returned.")
......
include/multicontact-api/scenario/contact-phase.hpp
View file @ 41868547
......@@ -462,13 +462,13 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
m_dc = curve_ptr(c_t.compute_derivate_ptr(1));
m_ddc = curve_ptr(c_t.compute_derivate_ptr(2));
*/
m_c = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points, time_points)));
m_dc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points_derivative, time_points)));
m_c = curve_ptr(new piecewise_t(
piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
m_dc = curve_ptr(new piecewise_t(
piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points_second_derivative, time_points)));
if(m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3)
points_second_derivative, time_points)));
if (m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3)
throw std::invalid_argument("Dimension of the points must be 3.");
m_c_init = point3_t(points.front());
......@@ -498,12 +498,11 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
m_L = curve_ptr(new piecewise_t(L_t));
m_dL = curve_ptr(L_t.compute_derivate_ptr(1));
*/
m_L = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points, time_points)));
m_dL = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points_derivative, time_points)));
if(m_L->dim() != 3 || m_dL->dim() != 3 )
throw std::invalid_argument("Dimension of the points must be 3.");
m_L = curve_ptr(new piecewise_t(
piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
m_dL = curve_ptr(new piecewise_t(
piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
if (m_L->dim() != 3 || m_dL->dim() != 3) throw std::invalid_argument("Dimension of the points must be 3.");
m_L_init = point3_t(points.front());
m_L_final = point3_t(points.back());
......@@ -531,12 +530,12 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
m_dq = curve_ptr(q_t.compute_derivate_ptr(1));
m_ddq = curve_ptr(q_t.compute_derivate_ptr(2));
*/
m_q = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points, time_points)));
m_dq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points_derivative, time_points)));
m_q = curve_ptr(new piecewise_t(
piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
m_dq = curve_ptr(new piecewise_t(
piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
points_second_derivative, time_points)));
points_second_derivative, time_points)));
m_q_init = points.front();
m_q_final = points.back();
return;
......@@ -549,9 +548,8 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
*/
t_strings getContactsBroken(const ContactPhase& to) const {
t_strings res;
for(std::string eeName : m_effector_in_contact){
if(!to.isEffectorInContact(eeName))
res.push_back(eeName);
for (std::string eeName : m_effector_in_contact) {
if (!to.isEffectorInContact(eeName)) res.push_back(eeName);
}
return res;
}
......@@ -563,9 +561,8 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
*/
t_strings getContactsCreated(const ContactPhase& to) const {
t_strings res;
for(std::string eeName : to.effectorsInContact()){
if(!isEffectorInContact(eeName))
res.push_back(eeName);
for (std::string eeName : to.effectorsInContact()) {
if (!isEffectorInContact(eeName)) res.push_back(eeName);
}
return res;
}
......@@ -579,10 +576,9 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
t_strings getContactsRepositioned(const ContactPhase& to) const {
t_strings res;
const ContactPatchMap& to_patch_map = to.contactPatches();
for(std::string eeName : m_effector_in_contact){
if(to.isEffectorInContact(eeName))
if(m_contact_patches.at(eeName).placement() != to_patch_map.at(eeName).placement())
res.push_back(eeName);
for (std::string eeName : m_effector_in_contact) {
if (to.isEffectorInContact(eeName))
if (m_contact_patches.at(eeName).placement() != to_patch_map.at(eeName).placement()) res.push_back(eeName);
}
return res;
}
......@@ -593,20 +589,19 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
* @return a list of string containing the effectors names
*/
t_strings getContactsVariations(const ContactPhase& to) const {
std::set<std::string> set_res; // use intermediate set to guarantee uniqueness of element
for(std::string eeName : getContactsBroken(to)){
std::set<std::string> set_res; // use intermediate set to guarantee uniqueness of element
for (std::string eeName : getContactsBroken(to)) {
set_res.insert(eeName);
}
for(std::string eeName : getContactsCreated(to)){
for (std::string eeName : getContactsCreated(to)) {
set_res.insert(eeName);
}
for(std::string eeName : getContactsRepositioned(to)){
for (std::string eeName : getContactsRepositioned(to)) {
set_res.insert(eeName);
}
return t_strings(set_res.begin(), set_res.end());
}
/* End Helpers */
void disp(std::ostream& os) const {
......
unittest/examples.cpp
View file @ 41868547
......@@ -19,11 +19,11 @@
#include <curves/cubic_hermite_spline.h>
/**
* This unit test try to deserialize the ContactSequences in the examples folder
* and check if they have the given data set.
* If this test fail, it probably mean that an update of multicontact-api broke the backward compatibility with serialized objects
* The objects need to be re-generated.
*/
* This unit test try to deserialize the ContactSequences in the examples folder
* and check if they have the given data set.
* If this test fail, it probably mean that an update of multicontact-api broke the backward compatibility with
* serialized objects The objects need to be re-generated.
*/
using namespace multicontact_api::scenario;
......@@ -32,7 +32,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(com_motion_above_feet_COM) {
ContactSequence cs;
cs.loadFromBinary(path+"com_motion_above_feet_COM.cs");
cs.loadFromBinary(path + "com_motion_above_feet_COM.cs");
BOOST_CHECK_EQUAL(cs.size(), 1);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -40,10 +40,9 @@ BOOST_AUTO_TEST_CASE(com_motion_above_feet_COM) {
BOOST_CHECK(cs.haveCentroidalTrajectories());
}
BOOST_AUTO_TEST_CASE(com_motion_above_feet_WB) {
ContactSequence cs;
cs.loadFromBinary(path+"com_motion_above_feet_WB.cs");
cs.loadFromBinary(path + "com_motion_above_feet_WB.cs");
BOOST_CHECK_EQUAL(cs.size(), 1);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -55,17 +54,16 @@ BOOST_AUTO_TEST_CASE(com_motion_above_feet_WB) {
BOOST_CHECK(cs.haveZMPtrajectories());
}
BOOST_AUTO_TEST_CASE(step_in_place) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place.cs");
cs.loadFromBinary(path + "step_in_place.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
}
BOOST_AUTO_TEST_CASE(step_in_place_COM) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place_COM.cs");
cs.loadFromBinary(path + "step_in_place_COM.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -75,7 +73,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_COM) {
BOOST_AUTO_TEST_CASE(step_in_place_REF) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place_REF.cs");
cs.loadFromBinary(path + "step_in_place_REF.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -86,7 +84,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_REF) {
BOOST_AUTO_TEST_CASE(step_in_place_WB) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place_WB.cs");
cs.loadFromBinary(path + "step_in_place_WB.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -101,14 +99,14 @@ BOOST_AUTO_TEST_CASE(step_in_place_WB) {
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place_quasistatic.cs");
cs.loadFromBinary(path + "step_in_place_quasistatic.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
}
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_COM) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place_quasistatic_COM.cs");
cs.loadFromBinary(path + "step_in_place_quasistatic_COM.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -118,7 +116,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_COM) {
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_REF) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place_quasistatic_REF.cs");
cs.loadFromBinary(path + "step_in_place_quasistatic_REF.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -129,7 +127,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_REF) {
BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
ContactSequence cs;
cs.loadFromBinary(path+"step_in_place_quasistatic_WB.cs");
cs.loadFromBinary(path + "step_in_place_quasistatic_WB.cs");
BOOST_CHECK_EQUAL(cs.size(), 9);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -144,14 +142,14 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
BOOST_AUTO_TEST_CASE(walk_20cm) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm.cs");
cs.loadFromBinary(path + "walk_20cm.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
}
BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm_COM.cs");
cs.loadFromBinary(path + "walk_20cm_COM.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -161,7 +159,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm_REF.cs");
cs.loadFromBinary(path + "walk_20cm_REF.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -172,7 +170,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
BOOST_AUTO_TEST_CASE(walk_20cm_WB) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm_WB.cs");
cs.loadFromBinary(path + "walk_20cm_WB.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -187,14 +185,14 @@ BOOST_AUTO_TEST_CASE(walk_20cm_WB) {
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm_quasistatic.cs");
cs.loadFromBinary(path + "walk_20cm_quasistatic.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
}
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_COM) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm_quasistatic_COM.cs");
cs.loadFromBinary(path + "walk_20cm_quasistatic_COM.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -204,7 +202,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_COM) {
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_REF) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm_quasistatic_REF.cs");
cs.loadFromBinary(path + "walk_20cm_quasistatic_REF.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -215,7 +213,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_REF) {
BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_WB) {
ContactSequence cs;
cs.loadFromBinary(path+"walk_20cm_quasistatic_WB.cs");
cs.loadFromBinary(path + "walk_20cm_quasistatic_WB.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
......@@ -228,6 +226,4 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_WB) {
BOOST_CHECK(cs.haveZMPtrajectories());
}
BOOST_AUTO_TEST_SUITE_END()
unittest/scenario.cpp
View file @ 41868547
......@@ -27,9 +27,9 @@ using curves::point6_t;
typedef Eigen::Matrix<double, 12, 1> point12_t;
using curves::curve_ptr_t;
using curves::curve_SE3_ptr_t;
using curves::piecewise_t;
using curves::piecewise_SE3_t;
using curves::matrix3_t;
using curves::piecewise_SE3_t;
using curves::piecewise_t;
using curves::pointX_t;
using curves::quaternion_t;
using curves::t_point3_t;
......@@ -103,7 +103,7 @@ curve_ptr_t buildRandomPolynomial(double min = -1, double max = -1) {
pointX_t b = Point::Random();
pointX_t c = Point::Random();
pointX_t d = Point::Random();
if(min < 0 && max < 0){
if (min < 0 && max < 0) {
const double t1 = Eigen::internal::random<double>(0., 10.);
const double t2 = Eigen::internal::random<double>(0., 10.);
min = std::min(t1, t2);
......@@ -118,14 +118,17 @@ curve_ptr_t buildRandomPolynomial(double min = -1, double max = -1) {
return res;
}
curve_ptr_t buildRandomPolynomial3D(double min = -1, double max = -1)
{ return buildRandomPolynomial<point3_t>(min,max); }
curve_ptr_t buildRandomPolynomial3D(double min = -1, double max = -1) {
return buildRandomPolynomial<point3_t>(min, max);
}
curve_ptr_t buildRandomPolynomial12D(double min = -1, double max = -1)
{ return buildRandomPolynomial<point12_t>(min, max); }
curve_ptr_t buildRandomPolynomial12D(double min = -1, double max = -1) {
return buildRandomPolynomial<point12_t>(min, max);
}
curve_ptr_t buildRandomPolynomial1D(double min = -1, double max = -1)
{ return buildRandomPolynomial<point1_t>(min, max); }
curve_ptr_t buildRandomPolynomial1D(double min = -1, double max = -1) {
return buildRandomPolynomial<point1_t>(min, max);
}
curve_SE3_ptr_t buildPiecewiseSE3() {
const double min = 0.5;
......@@ -1123,27 +1126,27 @@ BOOST_AUTO_TEST_CASE(contact_phase_helpers_tarjectories) {
// check that init/final are not modified if they ere already set :
// NOT THE CORRECT BEHAVIOR ANYMORE
// ContactPhase cp2 = buildRandomContactPhase(0, 2);
// point3_t c_init, c_final, dc_init, dc_final, ddc_init, ddc_final;
// c_init = cp2.m_c_init;
// dc_init = cp2.m_dc_init;
// ddc_init = cp2.m_ddc_init;
// c_final = cp2.m_c_final;
// dc_final = cp2.m_dc_final;
// ddc_final = cp2.m_ddc_final;
// cp2.setCOMtrajectoryFromPoints(points, points_derivative, points_second_derivative, time_points);
// BOOST_CHECK(cp2.m_c_init.isApprox(c_init));
// BOOST_CHECK(cp2.m_c_final.isApprox(c_final));
// BOOST_CHECK(cp2.m_dc_init.isApprox(dc_init));
// BOOST_CHECK(cp2.m_dc_final.isApprox(dc_final));
// BOOST_CHECK(cp2.m_ddc_init.isApprox(ddc_init));
// BOOST_CHECK(cp2.m_ddc_final.isApprox(ddc_final));
// BOOST_CHECK(!cp2.m_c_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_c_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dc_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dc_final.isApprox(d3));
// BOOST_CHECK(!cp2.m_ddc_init.isApprox(dd0));
// BOOST_CHECK(!cp2.m_ddc_final.isApprox(dd3));
// ContactPhase cp2 = buildRandomContactPhase(0, 2);
// point3_t c_init, c_final, dc_init, dc_final, ddc_init, ddc_final;
// c_init = cp2.m_c_init;
// dc_init = cp2.m_dc_init;
// ddc_init = cp2.m_ddc_init;
// c_final = cp2.m_c_final;
// dc_final = cp2.m_dc_final;
// ddc_final = cp2.m_ddc_final;
// cp2.setCOMtrajectoryFromPoints(points, points_derivative, points_second_derivative, time_points);
// BOOST_CHECK(cp2.m_c_init.isApprox(c_init));
// BOOST_CHECK(cp2.m_c_final.isApprox(c_final));
// BOOST_CHECK(cp2.m_dc_init.isApprox(dc_init));
// BOOST_CHECK(cp2.m_dc_final.isApprox(dc_final));
// BOOST_CHECK(cp2.m_ddc_init.isApprox(ddc_init));
// BOOST_CHECK(cp2.m_ddc_final.isApprox(ddc_final));
// BOOST_CHECK(!cp2.m_c_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_c_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dc_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dc_final.isApprox(d3));
// BOOST_CHECK(!cp2.m_ddc_init.isApprox(dd0));
// BOOST_CHECK(!cp2.m_ddc_final.isApprox(dd3));
// check AM trajectory
......@@ -1169,21 +1172,21 @@ BOOST_AUTO_TEST_CASE(contact_phase_helpers_tarjectories) {
BOOST_CHECK(cp.m_dL_final.isApprox(d3));
// check that init/final are not modified if they ere already set :
//NOT THE CORRECT BEHAVIOR ANYMORE
// point3_t L_init, L_final, dL_init, dL_final;
// L_init = cp2.m_L_init;
// dL_init = cp2.m_dL_init;
// L_final = cp2.m_L_final;
// dL_final = cp2.m_dL_final;
// cp2.setAMtrajectoryFromPoints(points, points_derivative, time_points);
// BOOST_CHECK(cp2.m_L_init.isApprox(L_init));
// BOOST_CHECK(cp2.m_L_final.isApprox(L_final));
// BOOST_CHECK(cp2.m_dL_init.isApprox(dL_init));
// BOOST_CHECK(cp2.m_dL_final.isApprox(dL_final));
// BOOST_CHECK(!cp2.m_L_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_L_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dL_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dL_final.isApprox(d3));
// NOT THE CORRECT BEHAVIOR ANYMORE
// point3_t L_init, L_final, dL_init, dL_final;
// L_init = cp2.m_L_init;
// dL_init = cp2.m_dL_init;
// L_final = cp2.m_L_final;
// dL_final = cp2.m_dL_final;
// cp2.setAMtrajectoryFromPoints(points, points_derivative, time_points);
// BOOST_CHECK(cp2.m_L_init.isApprox(L_init));
// BOOST_CHECK(cp2.m_L_final.isApprox(L_final));
// BOOST_CHECK(cp2.m_dL_init.isApprox(dL_init));
// BOOST_CHECK(cp2.m_dL_final.isApprox(dL_final));
// BOOST_CHECK(!cp2.m_L_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_L_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dL_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dL_final.isApprox(d3));
// check q trajectory :
pointX_t q0 = point12_t::Random();
......@@ -1242,14 +1245,14 @@ BOOST_AUTO_TEST_CASE(contact_phase_helpers_tarjectories) {
// check that init/final are not modified if they ere already set :