[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

include/multicontact-api/scenario/contact-phase.hpp

@@ -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);

include/multicontact-api/scenario/fwd.hpp

@@ -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__

include/multicontact-api/serialization/eigen-matrix.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

unittest/geometry.cpp

@@ -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));

unittest/scenario.cpp

@@ -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()