Skip to content

GitLab

Unverified Commit c0b034e8 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #958 from jcarpent/devel 

Add pickling of Model
parents d4105897 f00f4b09
Hide whitespace changes
Inline Side-by-side
.gitlab-ci.yml
View file @ c0b034e8
......@@ -77,7 +77,6 @@ robotpkg-pinocchio-18.04-debug:
#robotpkg-py-pinocchio-py3-14.04-release:
# <<: *robotpkg-py-pinocchio
# image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:14.04
# allow_failure: true
#robotpkg-py-pinocchio-14.04-debug:
# <<: *robotpkg-py-pinocchio
......@@ -88,7 +87,6 @@ robotpkg-pinocchio-18.04-debug:
#robotpkg-py-pinocchio-py3-14.04-debug:
# <<: *robotpkg-py-pinocchio
# image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:14.04
# allow_failure: true
# before_script:
# - echo PKG_OPTIONS.py-pinocchio=debug >> /opt/openrobots/etc/robotpkg.conf
......@@ -99,7 +97,6 @@ robotpkg-py-pinocchio-16.04-release:
robotpkg-py-pinocchio-py3-16.04-release:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:16.04
allow_failure: true
robotpkg-py-pinocchio-16.04-debug:
<<: *robotpkg-py-pinocchio
......@@ -110,13 +107,11 @@ robotpkg-py-pinocchio-16.04-debug:
robotpkg-py-pinocchio-py3-16.04-debug:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:16.04
allow_failure: true
before_script:
- echo PKG_OPTIONS.py-pinocchio=debug >> /opt/openrobots/etc/robotpkg.conf
robotpkg-py-pinocchio-py3-18.04-release:
<<: *robotpkg-py-pinocchio
allow_failure: true
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:18.04
robotpkg-py-pinocchio-18.04-release:
......@@ -132,7 +127,6 @@ robotpkg-py-pinocchio-18.04-debug:
robotpkg-py-pinocchio-py3-18.04-debug:
<<: *robotpkg-py-pinocchio
image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:18.04
allow_failure: true
before_script:
- echo PKG_OPTIONS.py-pinocchio=debug >> /opt/openrobots/etc/robotpkg.conf
......
.travis.yml
View file @ c0b034e8
language: generic
python:
- "2.7"
sudo: required
git:
depth: false
......@@ -32,10 +30,16 @@ jobs:
include:
- dist: xenial
env: BUILD_WITH_COLLISION_SUPPORT=OFF
python: "2.7"
- dist: trusty
env: BUILD_WITH_COLLISION_SUPPORT=ON
python: "2.7"
- dist: xenial
env: BUILD_WITH_COLLISION_SUPPORT=ON
python: "2.7"
- dist: xenial
env: BUILD_WITH_COLLISION_SUPPORT=ON
python: "3.5"
allow_failures:
- compiler:
......
bindings/python/algorithm/expose-joints.cpp
View file @ c0b034e8
......@@ -84,14 +84,6 @@ namespace pinocchio
"Velocity v (size model.nv)"),
"Integrate the model for a tangent vector during one unit time .");
bp::enum_<ArgumentPosition>("ArgumentPosition")
.value("ARG0",ARG0)
.value("ARG1",ARG1)
.value("ARG2",ARG2)
.value("ARG3",ARG3)
.value("ARG4",ARG4)
;
bp::def("dIntegrate",
&dIntegrate_proxy,
bp::args("Model",
......
bindings/python/module.cpp
View file @ c0b034e8
......@@ -21,8 +21,10 @@ using namespace pinocchio::python;
BOOST_PYTHON_MODULE(libpinocchio_pywrap)
{
bp::scope().attr("__version__") = pinocchio::printVersion();
bp::docstring_options module_docstring_options(true,true,false);
bp::scope().attr("__version__") = pinocchio::printVersion();
bp::scope().attr("__raw_version__") = bp::str(PINOCCHIO_VERSION);
eigenpy::enableEigenPy();
if(not register_symbolic_link_to_registered_type<Eigen::Quaterniond>())
......@@ -52,6 +54,14 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
.value("LOCAL",::pinocchio::LOCAL)
.value("LOCAL_WORLD_ALIGNED",::pinocchio::LOCAL_WORLD_ALIGNED)
;
bp::enum_< ::pinocchio::ArgumentPosition>("ArgumentPosition")
.value("ARG0",::pinocchio::ARG0)
.value("ARG1",::pinocchio::ARG1)
.value("ARG2",::pinocchio::ARG2)
.value("ARG3",::pinocchio::ARG3)
.value("ARG4",::pinocchio::ARG4)
;
exposeModel();
exposeFrame();
......
bindings/python/multibody/expose-model.cpp
View file @ c0b034e8
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2019 CNRS INRIA
//
#include "pinocchio/bindings/python/fwd.hpp"
......@@ -12,7 +12,7 @@ namespace pinocchio
void exposeModel()
{
ModelPythonVisitor::expose();
ModelPythonVisitor<Model>::expose();
}
} // namespace python
......
bindings/python/multibody/model.hpp
View file @ c0b034e8
......@@ -24,6 +24,17 @@
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Model)
#if PY_MAJOR_VERSION >= 3
#ifndef PyString_Check
#define PyString_Check_notDefined
#define PyString_Check PyBytes_Check
#endif
#ifndef PyString_AsString
#define PyString_AsString_notDefined
#define PyString_AsString PyBytes_AsString
#endif
#endif
namespace pinocchio
{
namespace python
......@@ -32,17 +43,67 @@ namespace pinocchio
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getFrameId_overload,Model::getFrameId,1,2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(existFrame_overload,Model::existFrame,1,2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addJointFrame_overload,Model::addJointFrame,1,2)
template<typename Model>
struct PickleModel : bp::pickle_suite
{
static bp::tuple getinitargs(const Model &)
{
return bp::make_tuple();
}
static bp::tuple getstate(const Model & model)
{
const std::string str(model.saveToString());
return bp::make_tuple(bp::str(str));
}
static void setstate(Model & model, bp::tuple tup)
{
if(bp::len(tup) == 0 || bp::len(tup) > 1)
{
throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
"The pickle data structure contains too many elements.");
}
bp::object py_obj = tup[0];
if(!PyString_Check(py_obj.ptr()))
{
throw eigenpy::Exception("Pickle was not able to reconstruct the model from the loaded data.\n"
"The entry is not a string.");
}
const std::string str(PyString_AsString(py_obj.ptr()));
model.loadFromString(str);
}
};
template<typename Model>
struct ModelPythonVisitor
: public bp::def_visitor< ModelPythonVisitor >
: public bp::def_visitor< ModelPythonVisitor<Model> >
{
public:
typedef Model::Index Index;
typedef Model::JointIndex JointIndex;
typedef Model::FrameIndex FrameIndex;
typedef typename Model::Scalar Scalar;
typedef typename Model::Index Index;
typedef typename Model::JointIndex JointIndex;
typedef typename Model::FrameIndex FrameIndex;
typedef typename Model::IndexVector IndexVector;
typedef typename Model::SE3 SE3;
typedef typename Model::Motion Motion;
typedef typename Model::Force Force;
typedef typename Model::Frame Frame;
typedef typename Model::Inertia Inertia;
typedef typename Model::Data Data;
typedef typename Model::VectorXs VectorXs;
protected:
struct addJointVisitor : public boost::static_visitor<Model::Index>
struct addJointVisitor
: public boost::static_visitor<Index>
{
Model & m_model;
const JointIndex m_parent_id;
......@@ -66,25 +127,26 @@ namespace pinocchio
}
}; // struct addJointVisitor
struct addJointWithLimitsVisitor : public boost::static_visitor<Model::Index>
struct addJointWithLimitsVisitor
: public boost::static_visitor<Index>
{
Model & m_model;
const JointIndex m_parent_id;
const SE3 & m_joint_placement;
const std::string & m_joint_name;
const Eigen::VectorXd & m_max_effort;
const Eigen::VectorXd & m_max_velocity;
const Eigen::VectorXd & m_min_config;
const Eigen::VectorXd & m_max_config;
const VectorXs & m_max_effort;
const VectorXs & m_max_velocity;
const VectorXs & m_min_config;
const VectorXs & m_max_config;
addJointWithLimitsVisitor(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const Eigen::VectorXd & max_effort,
const Eigen::VectorXd & max_velocity,
const Eigen::VectorXd & min_config,
const Eigen::VectorXd & max_config)
const VectorXs & max_effort,
const VectorXs & max_velocity,
const VectorXs & min_config,
const VectorXs & max_config)
: m_model(model)
, m_parent_id(parent_id)
, m_joint_placement(joint_placement)
......@@ -109,7 +171,8 @@ namespace pinocchio
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Default constructor. Constructs an empty model."))
.def(bp::init<>(bp::arg("self"),"Default constructor. Constructs an empty model."))
// Class Members
.add_property("nq", &Model::nq)
.add_property("nv", &Model::nv)
......@@ -162,10 +225,20 @@ namespace pinocchio
.def_readwrite("gravity",&Model::gravity,"Motion vector corresponding to the gravity field expressed in the world Frame.")
// Class Methods
.def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
.def("addJoint",&ModelPythonVisitor::addJointWithLimits,bp::args("parent_id","joint_model","joint_placement","joint_name","max_effort","max_velocity","min_config","max_config"),"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.")
.def("addJointFrame", &Model::addJointFrame, bp::args("jointIndex", "frameIndex"), "add the joint at index jointIndex as a frame to the frame tree")
.def("appendBodyToJoint",&Model::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
.def("addJoint",&ModelPythonVisitor::addJoint,
bp::args("parent_id","joint_model","joint_placement","joint_name"),
"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
.def("addJoint",&ModelPythonVisitor::addJointWithLimits,
bp::args("parent_id","joint_model","joint_placement","joint_name",
"max_effort","max_velocity","min_config","max_config"),
"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.")
.def("addJointFrame", &Model::addJointFrame,
addJointFrame_overload(bp::args("joint_id", "frame_id"),
"Add the joint provided by its joint_id as a frame to the frame tree.\n"
"The frame_id may be optionally provided."))
.def("appendBodyToJoint",&Model::appendBodyToJoint,
bp::args("joint_id","body_inertia","body_placement"),
"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
.def("addBodyFrame", &Model::addBodyFrame, bp::args("body_name", "parentJoint", "body_placement", "previous_frame(parent frame)"), "add a body to the frame tree")
.def("getBodyId",&Model::getBodyId, bp::args("name"), "Return the index of a frame of type BODY given by its name")
......@@ -191,7 +264,6 @@ namespace pinocchio
;
}
static JointIndex addJoint(Model & model,
JointIndex parent_id,
bp::object jmodel,
......@@ -207,10 +279,10 @@ namespace pinocchio
bp::object jmodel,
const SE3 & joint_placement,
const std::string & joint_name,
const Eigen::VectorXd & max_effort,
const Eigen::VectorXd & max_velocity,
const Eigen::VectorXd & min_config,
const Eigen::VectorXd & max_config)
const VectorXs & max_effort,
const VectorXs & max_velocity,
const VectorXs & min_config,
const VectorXs & max_config)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointWithLimitsVisitor(model,parent_id,joint_placement,joint_name,max_effort,max_velocity,min_config,max_config), jmodel_variant);
......@@ -229,10 +301,10 @@ namespace pinocchio
/// no element is found, return the size of the vector.
///
template<typename T>
static Model::Index index(std::vector<T> const& x,
static Index index(std::vector<T> const& x,
typename std::vector<T>::value_type const& v)
{
Model::Index i = 0;
Index i = 0;
for(typename std::vector<T>::const_iterator it = x.begin(); it != x.end(); ++it, ++i)
{
if(*it == v)
......@@ -247,13 +319,13 @@ namespace pinocchio
static void expose()
{
StdVectorPythonVisitor<Index>::expose("StdVec_Index");
StdVectorPythonVisitor<Model::IndexVector>::expose("StdVec_IndexVector");
StdVectorPythonVisitor<IndexVector>::expose("StdVec_IndexVector");
StdVectorPythonVisitor<std::string>::expose("StdVec_StdString");
StdVectorPythonVisitor<bool>::expose("StdVec_Bool");
StdVectorPythonVisitor<double>::expose("StdVec_double");
bp::class_< std::map<std::string, Eigen::VectorXd> >("StdMap_String_EigenVectorXd")
.def(bp::map_indexing_suite< std::map<std::string, Eigen::VectorXd>, true >())
.def_pickle(PickleMap<std::map<std::string, Eigen::VectorXd> >());
StdVectorPythonVisitor<Scalar>::expose("StdVec_double");
bp::class_<typename Model::ConfigVectorMap>("StdMap_String_EigenVectorXd")
.def(bp::map_indexing_suite< typename Model::ConfigVectorMap, true >())
.def_pickle(PickleMap<typename Model::ConfigVectorMap>());
bp::class_<Model>("Model",
"Articulated Rigid Body model",
......@@ -262,16 +334,23 @@ namespace pinocchio
.def(SerializableVisitor<Model>())
.def(PrintableVisitor<Model>())
.def(CopyableVisitor<Model>())
.def_pickle(PickleModel<Model>())
;
}
};
}} // namespace pinocchio::python
#if PY_MAJOR_VERSION >= 3
#ifdef PyString_Check_notDefined
#undef PyString_Check
#undef PyString_Check_notDefined
#endif
#ifdef PyString_AsString_notDefined
#undef PyString_AsString
#undef PyString_AsString_notDefined
#endif
#endif
#endif // ifndef __pinocchio_python_model_hpp__
bindings/python/serialization/serializable.hpp
View file @ c0b034e8
......@@ -29,6 +29,11 @@ namespace pinocchio
bp::arg("filename"),"Saves *this inside a text file.")
.def("loadFromText",&Derived::loadFromText,
bp::arg("filename"),"Loads *this from a text file.")
.def("saveToString",&Derived::saveToString,
"Parses the current object to a string.")
.def("loadFromString",&Derived::loadFromString,
bp::arg("string"),
"Parses from the input string the content of the current object.")
.def("saveToXML",&Derived::saveToXML,
bp::args("filename","tag_name"),"Saves *this inside a XML file.")
.def("loadFromXML",&Derived::loadFromXML,
......
src/serialization/archive.hpp
View file @ c0b034e8
......@@ -58,7 +58,7 @@ namespace pinocchio
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
///
template<typename T>
......@@ -77,6 +77,71 @@ namespace pinocchio
throw std::invalid_argument(exception_message);
}
}
///
/// \brief Loads an object from a std::stringstream.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] is string stream constaining the serialized content of the object.
///
template<typename T>
inline void loadFromStringStream(T & object,
std::istringstream & is)
{
boost::archive::text_iarchive ia(is,boost::archive::no_codecvt);
ia >> object;
}
///
/// \brief Saves an object inside a std::stringstream.
///
/// \tparam T Type of the object to deserialize.
///
/// \param[in] object Object in which the loaded data are copied.
/// \param[out] ss String stream constaining the serialized content of the object.
///
template<typename T>
inline void saveToStringStream(const T & object,
std::stringstream & ss)
{
boost::archive::text_oarchive oa(ss);
oa & object;
}
///
/// \brief Loads an object from a std::string
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] str string constaining the serialized content of the object.
///
template<typename T>
inline void loadFromString(T & object,
const std::string & str)
{
std::istringstream is(str);
loadFromStringStream(object,is);
}
///
/// \brief Saves an object inside a std::string
///
/// \tparam T Type of the object to deserialize.
///
/// \param[in] object Object in which the loaded data are copied.
///
/// \returns a string constaining the serialized content of the object.
///
template<typename T>
inline std::string saveToString(const T & object)
{
std::stringstream ss;
saveToStringStream(object,ss);
return ss.str();
}
///
/// \brief Loads an object from a XML file.
......@@ -114,7 +179,7 @@ namespace pinocchio
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] 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.
///
......@@ -168,7 +233,7 @@ namespace pinocchio
///
/// \tparam T Type of the object to deserialize.
///
/// \param[out] object Object in which the loaded data are copied.
/// \param[in] object Object in which the loaded data are copied.
/// \param[in] filename Name of the file containing the serialized data.
///
template<typename T>
......
src/serialization/serializable.hpp
View file @ c0b034e8
......@@ -34,6 +34,30 @@ namespace pinocchio
pinocchio::serialization::saveToText(derived(),filename);
}
/// \brief Loads a Derived object from a stream string.
void loadFromStringStream(std::istringstream & is)
{
pinocchio::serialization::loadFromStringStream(derived(),is);
}
/// \brief Saves a Derived object to a string stream.
void saveToStringStream(std::stringstream & ss) const
{
pinocchio::serialization::saveToStringStream(derived(),ss);
}
/// \brief Loads a Derived object from a string.
void loadFromString(const std::string & str)
{
pinocchio::serialization::loadFromString(derived(),str);
}
/// \brief Saves a Derived object to a string.
std::string saveToString() const
{
return pinocchio::serialization::saveToString(derived());
}
/// \brief Loads a Derived object from an XML file.
void loadFromXML(const std::string & filename,
const std::string & tag_name)
......
unittest/python/CMakeLists.txt
View file @ c0b034e8
SET(${PROJECT_NAME}_PYTHON_TESTS
bindings
bindings_model
bindings_data
bindings_com
bindings_com_velocity_derivatives
......
unittest/python/bindings_model.py 0 → 100644
View file @ c0b034e8
import unittest
import pinocchio as pin
pin.switchToNumpyMatrix()
from test_case import TestCase
class TestModel(TestCase):
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
def test_pickle(self):
import pickle
model = self.model
pickle.dump(model, open( "save.p", "wb" ) )
model_copy = pickle.load( open( "save.p", "rb" ) )
self.assertTrue(model == model_copy)
if __name__ == '__main__':
unittest.main()
unittest/serialization.cpp
View file @ c0b034e8
......@@ -38,6 +38,31 @@ void generic_test(const T & object,
BOOST_CHECK(object_loaded == object);
}
// Load and save as string stream
std::stringstream ss_out;
saveToStringStream(object,ss_out);
{
T object_loaded;
std::istringstream is(ss_out.str());
loadFromStringStream(object_loaded,is);
// Check
BOOST_CHECK(object_loaded == object);
}
// Load and save as string
std::string str_out = saveToString(object);
{
T object_loaded;
std::string str_in(str_out);
loadFromString(object_loaded,str_in);
// Check
BOOST_CHECK(object_loaded == object);
}
// Load and save as XML
const std::string xml_filename = filename + ".xml";
saveToXML(object,xml_filename,tag_name);
......
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