Commit 7d67bfa8 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Serialization of HumanoidRobot

parent f8a71642
......@@ -341,7 +341,9 @@ namespace hpp {
private:
Pool<DeviceData> datas_;
protected:
Device() {}
private:
HPP_SERIALIZABLE_SPLIT();
}; // class Device
......
......@@ -122,6 +122,9 @@ namespace hpp {
JointPtr_t gazeJoint_;
vector3_t gazeOrigin_;
vector3_t gazeDirection_;
HumanoidRobot () {}
HPP_SERIALIZABLE();
}; // class HumanoidRobot
} // namespace pinocchio
} // namespace hpp
......
......@@ -47,9 +47,6 @@
#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 {
const ::pinocchio::FrameType all_joint_type =
......@@ -535,6 +532,7 @@ namespace hpp {
#if __cplusplus > 201103L
using boost::serialization::make_nvp;
using hpp::serialization::archive_device_wrapper;
template<typename To, typename Ti, typename UnaryOp>
inline std::vector<To> serialize_to (const std::vector<Ti>& in, UnaryOp op)
......@@ -549,38 +547,79 @@ namespace hpp {
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
archive_device_wrapper* adw = dynamic_cast<archive_device_wrapper*>(&ar);
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());
*/
bool written = (adw != NULL);
ar & BOOST_SERIALIZATION_NVP(written);
if (written) {
// AbstractDevice
ar & BOOST_SERIALIZATION_NVP(model_);
//ar & BOOST_SERIALIZATION_NVP(geomModel_);
// Device
ar & BOOST_SERIALIZATION_NVP(name_);
// - grippers_
std::vector<FrameIndex> grippers;
std::transform(grippers_.begin(), grippers_.end(), grippers.begin(),
[](const GripperPtr_t& g) -> FrameIndex { return g->frameId(); });
ar & BOOST_SERIALIZATION_NVP(grippers);
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_);
size_type nbDeviceData = numberDeviceData();
ar & BOOST_SERIALIZATION_NVP(nbDeviceData);
}
}
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_) ?
bool written;
ar & BOOST_SERIALIZATION_NVP(written);
archive_device_wrapper* adw = dynamic_cast<archive_device_wrapper*>(&ar);
if (written) {
// AbstractDevice
ar & BOOST_SERIALIZATION_NVP(model_);
//ar & BOOST_SERIALIZATION_NVP(geomModel_);
// Device
ar & BOOST_SERIALIZATION_NVP(name_);
// - grippers_
std::vector<FrameIndex> grippers;
ar & BOOST_SERIALIZATION_NVP(grippers);
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_);
size_type nbDeviceData;
ar & BOOST_SERIALIZATION_NVP(nbDeviceData);
if (!adw) { // if archive_device_wrapper, then this device will be
// thrown away. No need to initialize it cleanly.
grippers_.reserve(grippers.size());
std::transform(grippers.begin(), grippers.end(), grippers_.begin(),
[this](FrameIndex i) -> GripperPtr_t {
return Gripper::create (model_->frames[i].name, weakPtr_);
});
createData();
createGeomData();
numberDeviceData(nbDeviceData);
}
} else if (!adw) // && !written
throw std::logic_error("This archive does not contain a valid Device "
"and the archive is not of type archive_device_wrapper.");
// else TODO if (adw->device->name() != name_) ?
}
#else
template<class Archive>
......
......@@ -19,7 +19,12 @@
#include <hpp/pinocchio/humanoid-robot.hh>
#include <pinocchio/serialization/eigen.hpp>
#include <hpp/util/serialization.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/serialization.hh>
namespace hpp {
namespace pinocchio {
......@@ -181,5 +186,32 @@ namespace hpp {
gazeJoint_ = joint;
}
template<class Archive>
void HumanoidRobot::serialize(Archive & ar, const unsigned int version)
{
using hpp::serialization::archive_device_wrapper;
(void) version;
ar & boost::serialization::make_nvp("base", boost::serialization::base_object<Device>(*this));
archive_device_wrapper* adw = dynamic_cast<archive_device_wrapper*>(&ar);
bool written = (adw != NULL);
ar & BOOST_SERIALIZATION_NVP(written);
if (written) {
ar & BOOST_SERIALIZATION_NVP(weakPtr_);
ar & BOOST_SERIALIZATION_NVP(waist_);
ar & BOOST_SERIALIZATION_NVP(chest_);
ar & BOOST_SERIALIZATION_NVP(leftWrist_);
ar & BOOST_SERIALIZATION_NVP(rightWrist_);
ar & BOOST_SERIALIZATION_NVP(leftAnkle_);
ar & BOOST_SERIALIZATION_NVP(rightAnkle_);
ar & BOOST_SERIALIZATION_NVP(gazeJoint_);
ar & BOOST_SERIALIZATION_NVP(gazeOrigin_);
ar & BOOST_SERIALIZATION_NVP(gazeDirection_);
}
}
HPP_SERIALIZATION_IMPLEMENT(HumanoidRobot);
} // namespace pinocchio
} // namespace hpp
BOOST_CLASS_EXPORT(hpp::pinocchio::HumanoidRobot)
......@@ -154,7 +154,7 @@ struct iarchive :
BOOST_AUTO_TEST_CASE(serialization)
{
DevicePtr_t device = unittest::makeDevice (unittest::HumanoidSimple);
DevicePtr_t device = unittest::makeDevice (unittest::HumanoidRomeo);
JointPtr_t joint = Joint::create (device, 1);
std::stringstream ss;
......
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