Add serialization for Device and Joint.

CMakeLists.txt

@@ -86,6 +86,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/pinocchio/simple-device.hh
   include/hpp/pinocchio/util.hh
   include/hpp/pinocchio/pool.hh
+  include/hpp/pinocchio/serialization.hh
 
   include/hpp/pinocchio/liegroup.hh
   include/hpp/pinocchio/liegroup-element.hh

include/hpp/pinocchio/device.hh

@@ -28,6 +28,7 @@
 # include <boost/thread/condition_variable.hpp>
 
 # include <hpp/util/debug.hh>
+# include <hpp/util/serialization-fwd.hh>
 
 # include <hpp/pinocchio/fwd.hh>
 # include <hpp/pinocchio/frame.hh>
@@ -339,6 +340,9 @@ namespace hpp {
 
     private:
       Pool<DeviceData> datas_;
+
+      Device() {}
+      HPP_SERIALIZABLE_SPLIT();
     }; // class Device
 
     void replaceGeometryByConvexHull (GeomModel& geomModel,

include/hpp/pinocchio/joint.hh

@@ -24,6 +24,8 @@
 # include <hpp/pinocchio/fwd.hh>
 # include <hpp/pinocchio/config.hh>
 
+# include <hpp/util/serialization-fwd.hh>
+
 namespace hpp {
   namespace pinocchio {
     /// Robot joint
@@ -252,6 +254,9 @@ namespace hpp {
       void selfAssert() const;
 
       friend class Device;
+
+      Joint() {}
+      HPP_SERIALIZABLE();
     }; // class Joint
 
     inline std::ostream& operator<< (std::ostream& os, const Joint& joint) { return joint.display(os); }

src/device.cc

@@ -20,8 +20,12 @@
 #include <hpp/pinocchio/device.hh>
 
 #include <boost/foreach.hpp>
+#include <boost/serialization/export.hpp>
+
 #include <Eigen/Core>
 
+#include <hpp/util/serialization.hh>
+
 #include <hpp/fcl/BV/AABB.h>
 #include <hpp/fcl/BVH/BVH_model.h>
 
@@ -29,6 +33,7 @@
 #include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
 #include <pinocchio/multibody/geometry.hpp>
 #include <pinocchio/multibody/model.hpp>
+#include <pinocchio/serialization/model.hpp>
 
 #include <hpp/pinocchio/fwd.hh>
 #include <hpp/pinocchio/joint-collection.hh>
@@ -40,6 +45,10 @@
 #include <hpp/pinocchio/joint.hh>
 #include <hpp/pinocchio/liegroup.hh>
 #include <hpp/pinocchio/liegroup-space.hh>
+#include <hpp/pinocchio/serialization.hh>
+
+// Needed to allow inheritance.
+BOOST_CLASS_EXPORT(hpp::pinocchio::Device)
 
 namespace hpp {
   namespace pinocchio {
@@ -523,5 +532,84 @@ namespace hpp {
         }
       }
     }
+
+#if __cplusplus > 201103L
+    using boost::serialization::make_nvp;
+
+    template<typename To, typename Ti, typename UnaryOp>
+    inline std::vector<To> serialize_to (const std::vector<Ti>& in, UnaryOp op)
+    {
+      std::vector<To> out;
+      out.reserve(in.size());
+      std::transform(in.begin(), in.end(), out.begin(), op);
+      return out;
+    }
+
+    template<class Archive>
+    void Device::save(Archive & ar, const unsigned int version) const
+    {
+      (void) version;
+      ar & BOOST_SERIALIZATION_NVP(name_);
+      /*
+      // AbstractDevice
+      ar & BOOST_SERIALIZATION_NVP(model_);
+      //ar & BOOST_SERIALIZATION_NVP(geomModel_);
+
+      // Device
+      ar & BOOST_SERIALIZATION_NVP(name_);
+      // - grippers_
+      ar & make_nvp("grippers", serialize_to<FrameIndex>(grippers_,
+            [](const GripperPtr_t& g) -> FrameIndex { return g->frameIndex(); })
+          );
+      ar & BOOST_SERIALIZATION_NVP(jointConstraints_);
+      ar & BOOST_SERIALIZATION_NVP(weakPtr_);
+
+      ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.dimension_);
+      ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.lowerBounds_);
+      ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.upperBounds_);
+
+      ar & serialization::make_nvp("nbDeviceData", datas_.size());
+      */
+    }
+
+    template<class Archive>
+    void Device::load(Archive & ar, const unsigned int version)
+    {
+      using hpp::serialization::archive_device_wrapper;
+      archive_device_wrapper* adw = dynamic_cast<archive_device_wrapper*>(&ar);
+      if (!adw) throw std::logic_error("Not implemented.");
+      (void) version;
+      ar & BOOST_SERIALIZATION_NVP(name_);
+      // TODO if (adw->device->name() != name_) ?
+    }
+#else
+    template<class Archive>
+    void Device::save(Archive &, const unsigned int) const
+    {
+      throw std::logic_error("Not implemented without C++ 11.");
+    }
+
+    template<class Archive>
+    void Device::load(Archive &, const unsigned int)
+    {
+      throw std::logic_error("Not implemented without C++ 11.");
+    }
+#endif
+
+    HPP_SERIALIZATION_SPLIT_IMPLEMENT(Device);
   } // namespace pinocchio
 } // namespace hpp
+
+namespace boost {
+namespace serialization {
+template<class Archive>
+void serialize (Archive & ar, hpp::pinocchio::Device::JointLinearConstraint& c, const unsigned int version)
+{
+  (void) version;
+  ar & make_nvp("offset", c.offset);
+  ar & make_nvp("multiplier", c.multiplier);
+  ar & make_nvp("joint", c.joint);
+  ar & make_nvp("reference", c.reference);
+}
+}
+}

src/joint.cc

@@ -20,14 +20,19 @@
 # include <hpp/pinocchio/joint.hh>
 
 # include <limits>
+# include <boost/serialization/vector.hpp>
+
 # include <pinocchio/algorithm/jacobian.hpp>
 
+# include <hpp/util/serialization.hh>
+
 # include <hpp/pinocchio/device.hh>
 # include <hpp/pinocchio/device-data.hh>
 # include <hpp/pinocchio/body.hh>
 # include <hpp/pinocchio/frame.hh>
 # include <hpp/pinocchio/liegroup-space.hh>
 # include <hpp/pinocchio/joint-collection.hh>
+# include <hpp/pinocchio/serialization.hh>
 
 # include "joint/bound.hh"
 
@@ -539,5 +544,17 @@ namespace hpp {
     {
       return devicePtr.lock()->d();
     }
+
+    template<class Archive>
+    void Joint::serialize(Archive & ar, const unsigned int version)
+    {
+      (void) version;
+      ar & BOOST_SERIALIZATION_NVP(maximalDistanceToParent_);
+      ar & BOOST_SERIALIZATION_NVP(devicePtr);
+      ar & BOOST_SERIALIZATION_NVP(jointIndex);
+      ar & BOOST_SERIALIZATION_NVP(children);
+    }
+
+    HPP_SERIALIZATION_IMPLEMENT(Joint);
   } // namespace pinocchio
 } // namespace hpp

tests/device.cc

@@ -18,6 +18,10 @@
 
 #include <boost/test/unit_test.hpp>
 
+#include <sstream>
+#include <boost/archive/polymorphic_xml_iarchive.hpp>
+#include <boost/archive/polymorphic_xml_oarchive.hpp>
+
 #include <pinocchio/fwd.hpp>
 #include <hpp/pinocchio/fwd.hh>
 #include <hpp/pinocchio/joint-collection.hh>
@@ -31,6 +35,7 @@
 #include <hpp/pinocchio/urdf/util.hh>
 #include <hpp/pinocchio/liegroup-space.hh>
 #include <hpp/pinocchio/configuration.hh>
+#include <hpp/pinocchio/serialization.hh>
 static bool verbose = true;
 
 using namespace hpp::pinocchio;
@@ -141,3 +146,34 @@ BOOST_AUTO_TEST_CASE(load_neutral_configuration)
       */
 }
 
+struct iarchive :
+  boost::archive::polymorphic_xml_iarchive, hpp::serialization::archive_device_wrapper
+{
+  iarchive(std::istream& is) : boost::archive::polymorphic_xml_iarchive (is) {}
+};
+
+BOOST_AUTO_TEST_CASE(serialization)
+{
+  DevicePtr_t device = unittest::makeDevice (unittest::HumanoidSimple);
+  JointPtr_t joint = Joint::create (device, 1);
+
+  std::stringstream ss;
+  {
+    boost::archive::polymorphic_xml_oarchive oa(ss);
+    oa << boost::serialization::make_nvp("device", device);
+    oa << boost::serialization::make_nvp("joint", joint);
+  }
+
+  DevicePtr_t device2;
+  JointPtr_t joint2;
+  {
+    iarchive ia(ss);
+    ia.device = device;
+    //boost::archive::polymorphic_xml_iarchive ia(ss);
+    ia >> boost::serialization::make_nvp("device", device2);
+    ia >> boost::serialization::make_nvp("joint", joint2);
+  }
+
+  BOOST_CHECK(device == device2);
+  BOOST_CHECK(joint2->robot() == device2);
+}