Commit aa5a887b authored by Justin Carpentier's avatar Justin Carpentier

serialization: add serialization structure + spatial classes

parent ced47931
......@@ -139,7 +139,7 @@ IF(BUILD_WITH_LUA_SUPPORT)
ENDIF(LUA5_2_FOUND)
ENDIF(BUILD_WITH_LUA_SUPPORT)
SET(BOOST_REQUIERED_COMPONENTS filesystem system)
SET(BOOST_REQUIERED_COMPONENTS filesystem serialization system)
SET(BOOST_BUILD_COMPONENTS unit_test_framework)
SET(BOOST_OPTIONAL_COMPONENTS "")
......@@ -215,6 +215,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/lua")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/utils")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/serialization")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/container")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/codegen")
......
......@@ -26,7 +26,7 @@ IF(UNIX)
TAG_LIBRARY_VERSION(${PROJECT_NAME})
ENDIF(BUILD_WITH_COMMIT_VERSION)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} eigen3)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_SERIALIZATION_LIBRARY})
IF(URDFDOM_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} urdfdom)
......
//
// Copyright (c) 2017-2019 CNRS INRIA
//
#ifndef __pinocchio_serialization_aligned_vector_hpp__
#define __pinocchio_serialization_aligned_vector_hpp__
#include "pinocchio/container/aligned-vector.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost
{
namespace serialization
{
template <class Archive, typename T>
void save(Archive & ar, const se3::container::aligned_vector<T> & v, const unsigned int version)
{
typedef typename se3::container::aligned_vector<T>::vector_base vector_base;
save(ar, *static_cast<const vector_base*>(&v), version);
}
template <class Archive, typename T>
void load(Archive & ar, se3::container::aligned_vector<T> & v, const unsigned int version)
{
typedef typename se3::container::aligned_vector<T>::vector_base vector_base;
load(ar, *static_cast<vector_base*>(&v), version);
}
template <class Archive, typename T>
void serialize(Archive & ar, se3::container::aligned_vector<T> & v, const unsigned int version)
{
split_free(ar,v,version);
}
}
}
#endif // ifndef __pinocchio_serialization_aligned_vector_hpp__
//
// Copyright (c) 2017-2019 CNRS INRIA
//
#ifndef __pinocchio_serialization_archive_hpp__
#define __pinocchio_serialization_archive_hpp__
#include "pinocchio/serialization/fwd.hpp"
#include <fstream>
#include <string>
#include <stdexcept>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
namespace pinocchio
{
namespace serialization
{
///
/// \brief Loads an object from a TXT file.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
///
template<typename T>
inline void loadFromText(T & object,
const std::string & filename) throw (std::invalid_argument)
{
std::ifstream ifs(filename.c_str());
if(ifs)
{
boost::archive::text_iarchive ia(ifs);
ia >> object;
}
else
{
const std::string exception_message(filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
}
///
/// \brief Saves an object inside a TXT file.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
///
template<typename T>
inline void saveToText(const T & object,
const std::string & filename) throw (std::invalid_argument)
{
std::ofstream ofs(filename.c_str());
if(ofs)
{
boost::archive::text_oarchive oa(ofs);
oa & object;
}
else
{
const std::string exception_message(filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
}
///
/// \brief Loads an object from a XML file.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
/// \param[in] tag_name XML Tag for the given object.
///
template<typename T>
inline void loadFromXML(T & object,
const std::string & filename,
const std::string & tag_name) throw (std::invalid_argument)
{
assert(!tag_name.empty());
std::ifstream ifs(filename.c_str());
if(ifs)
{
boost::archive::xml_iarchive ia(ifs);
ia >> boost::serialization::make_nvp(tag_name.c_str(),object);
}
else
{
const std::string exception_message(filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
}
///
/// \brief Saves an object inside a XML file.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
/// \param[in] tag_name XML Tag for the given object.
///
template<typename T>
inline void saveToXML(const T & object,
const std::string & filename,
const std::string & tag_name) throw (std::invalid_argument)
{
assert(!tag_name.empty());
std::ofstream ofs(filename.c_str());
if(ofs)
{
boost::archive::xml_oarchive oa(ofs);
oa & boost::serialization::make_nvp(tag_name.c_str(),object);
}
else
{
const std::string exception_message(filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
}
///
/// \brief Loads an object from a binary file.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
///
template<typename T>
inline void loadFromBinary(T & object,
const std::string & filename) throw (std::invalid_argument)
{
std::ifstream ifs(filename.c_str());
if(ifs)
{
boost::archive::binary_iarchive ia(ifs);
ia >> object;
}
else
{
const std::string exception_message(filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
}
///
/// \brief Saves an object inside a binary file.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
///
template<typename T>
void saveToBinary(const T & object,
const std::string & filename) throw (std::invalid_argument)
{
std::ofstream ofs(filename.c_str());
if(ofs)
{
boost::archive::binary_oarchive oa(ofs);
oa & object;
}
else
{
const std::string exception_message(filename + " does not seem to be a valid file.");
throw std::invalid_argument(exception_message);
}
}
}
} // namespace pinocchio
#endif // ifndef __pinocchio_serialization_archive_hpp__
//
// Copyright (c) 2017-2019 CNRS INRIA
//
/*
Code adapted from: https://gist.githubusercontent.com/mtao/5798888/raw/5be9fa9b66336c166dba3a92c0e5b69ffdb81501/eigen_boost_serialization.hpp
Copyright (c) 2015 Michael Tao
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef __pinocchio_serialization_eigen_matrix_hpp__
#define __pinocchio_serialization_eigen_matrix_hpp__
#include <Eigen/Dense>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
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*/)
{
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);
}
}
}
#endif // ifndef __pinocchio_serialization_eigen_matrix_hpp__
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_serialization_force_hpp__
#define __pinocchio_serialization_force_hpp__
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/serialization/fwd.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost
{
namespace serialization
{
template <class Archive, typename Scalar, int Options>
void save(Archive & ar,
const pinocchio::ForceTpl<Scalar,Options> & f,
const unsigned int /*version*/)
{
ar & make_nvp("linear",make_array(f.linear().data(),3));
ar & make_nvp("angular",make_array(f.angular().data(),3));
}
template <class Archive, typename Scalar, int Options>
void load(Archive & ar,
pinocchio::ForceTpl<Scalar,Options> & f,
const unsigned int /*version*/)
{
ar >> make_nvp("linear",make_array(f.linear().data(),3));
ar >> make_nvp("angular",make_array(f.angular().data(),3));
}
template <class Archive, typename Scalar, int Options>
void serialize(Archive & ar,
pinocchio::ForceTpl<Scalar,Options> & f,
const unsigned int version)
{
split_free(ar,f,version);
}
}
}
#endif // ifndef __pinocchio_serialization_force_hpp__
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_serialization_frame_hpp__
#define __pinocchio_serialization_frame_hpp__
#include "pinocchio/multibody/frame.hpp"
#include "pinocchio/serialization/fwd.hpp"
#include "pinocchio/serialization/se3.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost
{
namespace serialization
{
template <class Archive, typename Scalar, int Options>
void save(Archive & ar,
const pinocchio::FrameTpl<Scalar,Options> & f,
const unsigned int /*version*/)
{
ar & make_nvp("name",f.name);
ar & make_nvp("parent",f.parent);
ar & make_nvp("previousFrame",f.previousFrame);
ar & make_nvp("placement",f.placement);
ar & make_nvp("type",f.type);
}
template <class Archive, typename Scalar, int Options>
void load(Archive & ar,
pinocchio::FrameTpl<Scalar,Options> & f,
const unsigned int /*version*/)
{
ar >> make_nvp("name",f.name);
ar >> make_nvp("parent",f.parent);
ar >> make_nvp("previousFrame",f.previousFrame);
ar >> make_nvp("placement",f.placement);
ar >> make_nvp("type",f.type);
}
template <class Archive, typename Scalar, int Options>
void serialize(Archive & ar,
pinocchio::FrameTpl<Scalar,Options> & f,
const unsigned int version)
{
split_free(ar,f,version);
}
}
}
#endif // ifndef __pinocchio_serialization_frame_hpp__
//
// Copyright (c) 2017-2019 CNRS INRIA
//
#ifndef __pinocchio_serialization_fwd_hpp__
#define __pinocchio_serialization_fwd_hpp__
#include <boost/serialization/nvp.hpp>
#define BOOST_SERIALIZATION_MAKE_NVP(member) boost::serialization::make_nvp(##member,member)
#endif // ifndef __pinocchio_serialization_fwd_hpp__
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_serialization_inertia_hpp__
#define __pinocchio_serialization_inertia_hpp__
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/serialization/fwd.hpp"
#include "pinocchio/serialization/symmetric3.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost
{
namespace serialization
{
template <class Archive, typename Scalar, int Options>
void save(Archive & ar,
const pinocchio::InertiaTpl<Scalar,Options> & I,
const unsigned int /*version*/)
{
const Scalar mass = I.mass();
ar & make_nvp("mass",mass);
ar & make_nvp("lever",make_array(I.lever().data(),3));
ar & make_nvp("inertia",I.inertia());
}
template <class Archive, typename Scalar, int Options>
void load(Archive & ar,
pinocchio::InertiaTpl<Scalar,Options> & I,
const unsigned int /*version*/)
{
ar >> make_nvp("mass",I.mass());
ar >> make_nvp("lever",make_array(I.lever().data(),3));
ar >> make_nvp("inertia",I.inertia());
}
template <class Archive, typename Scalar, int Options>
void serialize(Archive & ar,
pinocchio::InertiaTpl<Scalar,Options> & I,
const unsigned int version)
{
split_free(ar,I,version);
}
}
}
#endif // ifndef __pinocchio_serialization_inertia_hpp__
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_serialization_motion_hpp__
#define __pinocchio_serialization_motion_hpp__
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/serialization/fwd.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost
{
namespace serialization
{
template <class Archive, typename Scalar, int Options>
void save(Archive & ar,
const pinocchio::MotionTpl<Scalar,Options> & m,
const unsigned int /*version*/)
{
ar & make_nvp("linear",make_array(m.linear().data(),3));
ar & make_nvp("angular",make_array(m.angular().data(),3));
}
template <class Archive, typename Scalar, int Options>
void load(Archive & ar,
pinocchio::MotionTpl<Scalar,Options> & m,
const unsigned int /*version*/)
{
ar >> make_nvp("linear",make_array(m.linear().data(),3));
ar >> make_nvp("angular",make_array(m.angular().data(),3));
}
template <class Archive, typename Scalar, int Options>
void serialize(Archive & ar,
pinocchio::MotionTpl<Scalar,Options> & m,
const unsigned int version)
{
split_free(ar,m,version);
}
}
}
#endif // ifndef __pinocchio_serialization_motion_hpp__
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_serialization_se3_hpp__
#define __pinocchio_serialization_se3_hpp__
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/serialization/fwd.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost
{
namespace serialization
{
template <class Archive, typename Scalar, int Options>
void save(Archive & ar,
const pinocchio::SE3Tpl<Scalar,Options> & M,
const unsigned int /*version*/)
{
ar & make_nvp("translation",make_array(M.translation().data(),3));
ar & make_nvp("rotation",make_array(M.rotation().data(),9));
}
template <class Archive, typename Scalar, int Options>
void load(Archive & ar,
pinocchio::SE3Tpl<Scalar,Options> & M,
const unsigned int /*version*/)
{
ar >> make_nvp("translation",make_array(M.translation().data(),3));
ar >> make_nvp("rotation",make_array(M.rotation().data(),9));
}
template <class Archive, typename Scalar, int Options>
void serialize(Archive & ar,
pinocchio::SE3Tpl<Scalar,Options> & M,
const unsigned int version)
{
split_free(ar,M,version);
}
}
}
#endif // ifndef __pinocchio_serialization_se3_hpp__
//
// Copyright (c) 2017-2019 CNRS INRIA
//
#ifndef __pinocchio_serialization_serializable_hpp__
#define __pinocchio_serialization_serializable_hpp__
#include "pinocchio/serialization/archive.hpp"
namespace pinocchio
{
namespace serialization
{
template<class Derived>
struct Serializable
{
private:
Derived & derived() { return *static_cast<Derived*>(this); }
const Derived & derived() const { return *static_cast<const Derived*>(this); }
public:
/// \brief Loads a Derived object from a text file.
void loadFromText(const std::string & filename) throw (std::invalid_argument)
{
loadFromText(derived(),filename);
}
/// \brief Saves a Derived object as a text file.
void saveToText(const std::string & filename) const throw (std::invalid_argument)
{
saveToText(derived(),filename);
}
/// \brief Loads a Derived object from an XML file.
void loadFromXML(const std::string & filename,
const std::string & tag_name) throw (std::invalid_argument)
{
loadFromXML(derived(),filename,tag_name);
}
/// \brief Saves a Derived object as an XML file.
void saveToXML(const std::string & filename,
const std::string & tag_name) const throw (std::invalid_argument)
{
saveToXML(derived(),filename,tag_name);
}
/// \brief Loads a Derived object from an binary file.
void loadFromBinary(const std::string & filename) throw (std::invalid_argument)
{
loadFromBinary(derived(),filename);
}
/// \brief Saves a Derived object as an binary file.
void saveToBinary(const std::string & filename) const throw (std::invalid_argument)
{
saveToBinary(derived(),filename);
}
};
}
} // namespace pinocchio
#endif // ifndef __pinocchio_serialization_serializable_hpp__
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_serialization_spatial_hpp__
#define __pinocchio_serialization_spatial_hpp__
#include "pinocchio/serialization/se3.hpp"
#include "pinocchio/serialization/motion.hpp"
#include "pinocchio/serialization/force.hpp"
#include "pinocchio/serialization/symmetric3.hpp"
#include "pinocchio/serialization/inertia.hpp"
#endif // ifndef __pinocchio_serialization_spatial_hpp__
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_serialization_symmetric3_hpp__
#define __pinocchio_serialization_symmetric3_hpp__
#include "pinocchio/spatial/symmetric3.hpp"
#include "pinocchio/serialization/fwd.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
namespace boost
{
namespace serialization
{
template <class Archive, typename Scalar, int Options>
void save(Archive & ar,
const pinocchio::Symmetric3Tpl<Scalar,Options> & S,
const unsigned int /*version*/)
{
ar & make_nvp("data",make_array(S.data().data(),6));
}
template <class Archive, typename Scalar, int Options>