Commit 26c9b5ef authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

[Serialization + cleaning] Fix serialization for contact-phase => Add Test OK...

[Serialization + cleaning] Fix serialization for contact-phase => Add Test OK / Comment references to contact-phase-humanoid
parent 59195485
......@@ -15,6 +15,8 @@
#include "multicontact-api/container/ref.hpp"
#include <curves/curve_abc.h>
#include <curves/piecewise_curve.h>
#include <curves/polynomial.h>
#include <curves/cubic_hermite_spline.h>
#include <map>
......@@ -63,17 +65,17 @@ namespace multicontact_api
std::pair<double,double> m_time_interval;
ConfigurationVector m_reference_configuration;
curve_abc_t* m_q;
curve_abc_t* m_dq;
curve_abc_t* m_ddq;
curve_abc_t* m_tau;
curve_abc_t* m_c;
curve_abc_t* m_dc;
curve_abc_t* m_ddc;
curve_abc_t* m_L;
curve_abc_t* m_dL;
curve_abc_t* m_wrench;
curve_abc_t* m_zmp;
curve_abc_t* m_q = NULL;
curve_abc_t* m_dq = NULL;
curve_abc_t* m_ddq = NULL;
curve_abc_t* m_tau = NULL;
curve_abc_t* m_c = NULL;
curve_abc_t* m_dc = NULL;
curve_abc_t* m_ddc = NULL;
curve_abc_t* m_L = NULL;
curve_abc_t* m_dL = NULL;
curve_abc_t* m_wrench = NULL;
curve_abc_t* m_zmp = NULL;
CurveMap m_contact_forces;
CurveMap m_contact_normal_force;
......@@ -82,10 +84,11 @@ namespace multicontact_api
StateVector m_init_state;
StateVector m_final_state;
CubicHermiteSpline3 * m_angular_momentum_ref;
CubicHermiteSpline3 * m_com_ref;
CubicHermiteSpline3 * m_vcom_ref;
CubicHermiteSpline24 * m_forces_ref;
CubicHermiteSpline3 m_angular_momentum_ref;
CubicHermiteSpline3 m_com_ref;
CubicHermiteSpline3 m_vcom_ref;
CubicHermiteSpline24 m_forces_ref;
/*Variables*/
......@@ -141,7 +144,10 @@ namespace multicontact_api
m_contact_patches == other.m_contact_patches
&& m_lwc == other.m_lwc
&& m_sowc == other.m_sowc
&& m_init_state == other.m_init_state
&& m_final_state == other.m_final_state
;
// TO DO : Test equality on curves
}
template<typename S2>
......@@ -189,6 +195,49 @@ namespace multicontact_api
private:
template <class Archive, typename Type >
void serialize_pointer(Archive & ar, std::string name, Type * ptr) const
{
std::cout << "start ser "<<name<<std::endl;
std::string name_boolean = name + std::string("_bool");
bool isNull = false;
std::cout<<"value ptr : "<<ptr<<std::endl;
if (ptr != NULL)
{
std::cout << "ptr not NULL"<<std::endl;
ar & boost::serialization::make_nvp(name_boolean.c_str(),isNull);
//std::cout << "Here 2"<<std::endl;
ar & boost::serialization::make_nvp(name.c_str(),ptr);
}
else
{
std::cout << "ptr NULL"<<std::endl;
isNull = true;
ar & boost::serialization::make_nvp(name_boolean.c_str(),isNull);
}
std::cout << "end"<<std::endl;
}
template <class Archive, typename Type >
void deserialize_pointer(Archive & ar, std::string name, Type ** ptr) const
{
std::cout << "start deser "<<name<<std::endl;
std::string name_boolean = name + std::string("_bool");
bool isNull;
ar >> boost::serialization::make_nvp(name_boolean.c_str(),isNull);
if (!isNull)
{
std::cout << "ptr not NULL"<<std::endl;
ar >> boost::serialization::make_nvp(name.c_str(),(*ptr));
}
else
{
std::cout << "ptr NULL"<<std::endl;
(*ptr) = NULL;
}
std::cout << "end"<<std::endl;
}
// Serialization of the class
friend class boost::serialization::access;
......@@ -200,29 +249,27 @@ namespace multicontact_api
ar & boost::serialization::make_nvp("time_interval",m_time_interval);
ar & boost::serialization::make_nvp("reference_configuration",m_reference_configuration);
ar & boost::serialization::make_nvp("m_q",m_q);
ar & boost::serialization::make_nvp("m_dq",m_dq);
ar & boost::serialization::make_nvp("m_ddq",m_ddq);
ar & boost::serialization::make_nvp("m_tau",m_tau);
ar & boost::serialization::make_nvp("m_c",m_c);
ar & boost::serialization::make_nvp("m_dc",m_dc);
ar & boost::serialization::make_nvp("m_ddc",m_ddc);
ar & boost::serialization::make_nvp("m_L",m_L);
ar & boost::serialization::make_nvp("m_dL",m_dL);
ar & boost::serialization::make_nvp("m_wrench",m_wrench);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_q"), m_q);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_dq"), m_dq);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_ddq"), m_ddq);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_tau"), m_tau);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_c"), m_c);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_dc"), m_dc);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_ddc"), m_ddc);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_L"), m_L);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_dL"), m_dL);
serialize_pointer<Archive, curve_abc_t>(ar, std::string("m_wrench"), m_wrench);
ar & boost::serialization::make_nvp("contact_forces",m_contact_forces);
ar & boost::serialization::make_nvp("contact_normal_force",m_contact_normal_force);
ar & boost::serialization::make_nvp("effector_trajectories",m_effector_trajectories);
ar & boost::serialization::make_nvp("init_state",m_init_state);
ar & boost::serialization::make_nvp("final_state",m_final_state);
ar & boost::serialization::make_nvp("m_angular_momentum_ref",m_angular_momentum_ref);
ar & boost::serialization::make_nvp("m_com_ref",m_com_ref);
ar & boost::serialization::make_nvp("m_vcom_ref",m_vcom_ref);
ar & boost::serialization::make_nvp("m_forces_ref",m_forces_ref);
ar & boost::serialization::make_nvp("m_angular_momentum_ref", m_angular_momentum_ref);
ar & boost::serialization::make_nvp("m_com_ref", m_com_ref);
ar & boost::serialization::make_nvp("m_vcom_ref", m_vcom_ref);
ar & boost::serialization::make_nvp("m_forces_ref", m_forces_ref);
ar & boost::serialization::make_nvp("lwc",m_lwc);
ar & boost::serialization::make_nvp("sowc",m_sowc);
......@@ -238,29 +285,27 @@ namespace multicontact_api
ar >> boost::serialization::make_nvp("time_interval",m_time_interval);
ar >> boost::serialization::make_nvp("reference_configuration",m_reference_configuration);
ar >> boost::serialization::make_nvp("m_q",m_q);
ar >> boost::serialization::make_nvp("m_dq",m_dq);
ar >> boost::serialization::make_nvp("m_ddq",m_ddq);
ar >> boost::serialization::make_nvp("m_tau",m_tau);
ar >> boost::serialization::make_nvp("m_c",m_c);
ar >> boost::serialization::make_nvp("m_dc",m_dc);
ar >> boost::serialization::make_nvp("m_ddc",m_ddc);
ar >> boost::serialization::make_nvp("m_L",m_L);
ar >> boost::serialization::make_nvp("m_dL",m_dL);
ar >> boost::serialization::make_nvp("m_wrench",m_wrench);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_q"), &m_q);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_dq"), &m_dq);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_ddq"), &m_ddq);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_tau"), &m_tau);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_c"), &m_c);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_dc"), &m_dc);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_ddc"), &m_ddc);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_L"), &m_L);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_dL"), &m_dL);
deserialize_pointer<Archive, curve_abc_t>(ar, std::string("m_wrench"), &m_wrench);
ar >> boost::serialization::make_nvp("contact_forces",m_contact_forces);
ar >> boost::serialization::make_nvp("contact_normal_force",m_contact_normal_force);
ar >> boost::serialization::make_nvp("effector_trajectories",m_effector_trajectories);
ar >> boost::serialization::make_nvp("init_state",m_init_state);
ar >> boost::serialization::make_nvp("final_state",m_final_state);
ar >> boost::serialization::make_nvp("m_angular_momentum_ref",m_angular_momentum_ref);
ar >> boost::serialization::make_nvp("m_com_ref",m_com_ref);
ar >> boost::serialization::make_nvp("m_vcom_ref",m_vcom_ref);
ar >> boost::serialization::make_nvp("m_forces_ref",m_forces_ref);
ar >> boost::serialization::make_nvp("m_angular_momentum_ref", m_angular_momentum_ref);
ar >> boost::serialization::make_nvp("m_com_ref", m_com_ref);
ar >> boost::serialization::make_nvp("m_vcom_ref", m_vcom_ref);
ar >> boost::serialization::make_nvp("m_forces_ref", m_forces_ref);
ar >> boost::serialization::make_nvp("lwc",m_lwc);
ar >> boost::serialization::make_nvp("sowc",m_sowc);
......
......@@ -7,55 +7,44 @@
#include "multicontact-api/geometry/fwd.hpp"
#include <pinocchio/spatial/fwd.hpp>
namespace multicontact_api {
namespace scenario {
template <class T>
struct traits {};
template <typename _Scalar>
struct ContactPatchTpl;
typedef ContactPatchTpl<double> ContactPatch;
template <typename _Scalar, int _dim>
struct ContactPhaseTpl;
typedef ContactPhaseTpl<double, 4> ContactPhase4;
template <typename _Scalar>
struct ContactPhaseHumanoidTpl;
typedef ContactPhaseHumanoidTpl<double> ContactPhaseHumanoid;
template <class _ContactPhase>
struct ContactSequenceTpl;
typedef ContactSequenceTpl<ContactPhase4> ContactSequence4;
typedef ContactSequenceTpl<ContactPhaseHumanoid> ContactSequenceHumanoid;
template <class SOC>
struct ContactConstraintSOC;
typedef ContactConstraintSOC<geometry::SOC6d> ContactConstraintSOC6;
template <typename Scalar>
struct ContactModelPlanarTpl;
typedef ContactModelPlanarTpl<double> ContactModelPlanar;
template <typename Scalar>
struct ContactConstraintPlanarTpl;
typedef ContactConstraintPlanarTpl<double> ContactConstraintPlanar;
template <class GMM>
struct ContactConstraintGMM;
enum HumanoidPhaseType {
SINGLE_SUPPORT,
DOUBLE_SUPPORT,
TRIPLE_SUPPORT,
QUADRUPLE_SUPPORT,
NO_SUPPORT,
HUMANOID_PHASE_UNDEFINED
};
enum ConicType { CONIC_SOWC, CONIC_DOUBLE_DESCRIPTION, CONIC_UNDEFINED };
} // namespace scenario
} // namespace multicontact_api
#endif // ifndef __multicontact_api_scenario_fwd_hpp__
namespace multicontact_api{
namespace scenario{
template<class T> struct traits {};
template<typename _Scalar> struct ContactPatchTpl;
typedef ContactPatchTpl<double> ContactPatch;
template<typename _Scalar, int _dim> struct ContactPhaseTpl;
typedef ContactPhaseTpl<double,4> ContactPhase4;
template<class _ContactPhase> struct ContactSequenceTpl;
typedef ContactSequenceTpl<ContactPhase4> ContactSequence4;
//typedef ContactSequenceTpl<ContactPhaseHumanoid> ContactSequenceHumanoid;
template<class SOC> struct ContactConstraintSOC;
typedef ContactConstraintSOC<geometry::SOC6d> ContactConstraintSOC6;
template<typename Scalar> struct ContactModelPlanarTpl;
typedef ContactModelPlanarTpl<double> ContactModelPlanar;
template<typename Scalar> struct ContactConstraintPlanarTpl;
typedef ContactConstraintPlanarTpl<double> ContactConstraintPlanar;
enum HumanoidPhaseType
{
SINGLE_SUPPORT,
DOUBLE_SUPPORT,
TRIPLE_SUPPORT,
QUADRUPLE_SUPPORT,
NO_SUPPORT,
HUMANOID_PHASE_UNDEFINED
};
enum ConicType { CONIC_SOWC, CONIC_DOUBLE_DESCRIPTION, CONIC_UNDEFINED };
}
}
#endif // ifndef __multicontact_api_scenario_fwd_hpp__
......@@ -32,35 +32,36 @@
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost {
namespace serialization {
namespace boost{
namespace serialization{
template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void save(Archive & ar, const Eigen::Matrix<_Scalar,_Rows,_Cols,_Options,_MaxRows,_MaxCols> & m, const unsigned int version)
{
if (version)
{
// Do something
}
Eigen::DenseIndex rows(m.rows()), cols(m.cols());
ar & BOOST_SERIALIZATION_NVP(rows);
ar & BOOST_SERIALIZATION_NVP(cols);
ar & make_nvp("data",make_array(m.data(), (size_t)m.size()));
}
template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void save(Archive& ar, const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m,
const unsigned int /*version*/) {
Eigen::DenseIndex rows(m.rows()), cols(m.cols());
ar& BOOST_SERIALIZATION_NVP(rows);
ar& BOOST_SERIALIZATION_NVP(cols);
ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
}
template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void load(Archive& ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m,
const unsigned int /*version*/) {
Eigen::DenseIndex rows, cols;
ar >> BOOST_SERIALIZATION_NVP(rows);
ar >> BOOST_SERIALIZATION_NVP(cols);
m.resize(rows, cols);
// if(m.size() > 0)
ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
}
template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void serialize(Archive& ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m,
const unsigned int version) {
split_free(ar, m, version);
}
template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void load(Archive & ar, Eigen::Matrix<_Scalar,_Rows,_Cols,_Options,_MaxRows,_MaxCols> & m, const unsigned int version)
{
if (version)
{
// Do something
}
Eigen::DenseIndex rows,cols;
ar >> BOOST_SERIALIZATION_NVP(rows);
ar >> BOOST_SERIALIZATION_NVP(cols);
m.resize(rows,cols);
//if(m.size() > 0)
ar >> make_nvp("data",make_array(m.data(), (size_t)m.size()));
}
} // namespace serialization
} // namespace boost
......
......@@ -17,10 +17,10 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(WrenchConeTest) {
using std::fabs;
typedef ForceCone::Matrix3x Matrix3x;
typedef WrenchCone::Matrix6x Matrix6x;
//typedef WrenchCone::Matrix6x Matrix6x;
typedef ForceCone::Index Index;
typedef ForceCone::SE3 SE3;
typedef ForceCone::Scalar Scalar;
//typedef ForceCone::Scalar Scalar;
const Index size = 10;
const Matrix3x rays(Matrix3x::Random(3, size));
......
......@@ -56,8 +56,9 @@ BOOST_AUTO_TEST_CASE(contact_patch) {
Ad a2(a1);
}
BOOST_AUTO_TEST_CASE(contact_phase) {
ContactPhase4 cp;
BOOST_AUTO_TEST_CASE(contact_phase)
{
ContactPhase4 cp, cp_test;
for(ContactPhase4::ContactPatchMap::iterator it = cp.contact_patches().begin();
it != cp.contact_patches().end(); ++it)
{
......@@ -70,26 +71,13 @@ BOOST_AUTO_TEST_CASE(contact_phase) {
ContactPhase4 cp2(cp);
BOOST_CHECK(cp == cp);
// BOOST_CHECK(cp == cp2);
BOOST_CHECK(cp == cp2);
// test serialization
cp.saveAsText("serialization_cp_test.test");
cp_test.loadFromText("serialization_cp_test.test");
remove("serialization_cp_test.test");
BOOST_CHECK(cp == cp_test);
}
//
// BOOST_AUTO_TEST_CASE(contact_phase_humanoid)
//{
// ContactPhaseHumanoid cp;
// for(ContactPhaseHumanoid::ContactPatchArray::iterator it = cp.contact_patches().begin();
// it != cp.contact_patches().end(); ++it)
// {
// it->contactModel().m_mu = 0.3;
// it->contactModel().m_ZMP_radius = 0.01;
// }
// ContactPhaseHumanoid cp2(cp);
//
// BOOST_CHECK(cp == cp);
// BOOST_CHECK(cp == cp2);
//
// cp.m_reference_configurations.push_back(Eigen::VectorXd::Zero(10));
//
//// BOOST_CHECK(cp != cp2);
//}
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