From bb7c32c275fe37fef531441f1f60a2af40065816 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 8 Dec 2022 14:15:07 +0100 Subject: [PATCH 01/17] core: remove memory leak --- include/hpp/fcl/shape/details/convex.hxx | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx index c660b858a..f25e17676 100644 --- a/include/hpp/fcl/shape/details/convex.hxx +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -230,7 +230,9 @@ void Convex::fillNeighbors() { } } + if (nneighbors_) delete nneighbors_; nneighbors_ = new unsigned int[c_nneighbors]; + unsigned int* p_nneighbors = nneighbors_; for (unsigned int i = 0; i < num_points; ++i) { Neighbors& n = neighbors[i]; From 4ab4667d0c3a856e4c0921bd9695e0fb57b4ab56 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 8 Dec 2022 14:54:01 +0100 Subject: [PATCH 02/17] test/serialization: add missing test for Ellipsoid --- test/serialization.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/test/serialization.cpp b/test/serialization.cpp index e9bfb6af7..aa4998d43 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2021 INRIA. + * Copyright (c) 2021-2022 INRIA. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -243,6 +243,11 @@ BOOST_AUTO_TEST_CASE(test_shapes) { test_serialization(sphere, sphere_copy); } + { + Ellipsoid ellipsoid(1., 2., 3.), ellipsoid_copy(0., 0., 0.); + test_serialization(ellipsoid, ellipsoid_copy); + } + { Capsule capsule(1., 2.), capsule_copy(10., 10.); test_serialization(capsule, capsule_copy); From 36ec86a321e6fcefcea9919e989c138c976cb643 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 8 Dec 2022 15:01:06 +0100 Subject: [PATCH 03/17] serialization: move Triangle to a dedicated file --- CMakeLists.txt | 1 + include/hpp/fcl/serialization/BVH_model.h | 11 ++-------- include/hpp/fcl/serialization/triangle.h | 25 +++++++++++++++++++++++ 3 files changed, 28 insertions(+), 9 deletions(-) create mode 100644 include/hpp/fcl/serialization/triangle.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 334892727..37b9f9071 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -256,6 +256,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/serialization/RSS.h include/hpp/fcl/serialization/OBBRSS.h include/hpp/fcl/serialization/hfield.h + include/hpp/fcl/serialization/triangle.h include/hpp/fcl/timings.h ) diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/hpp/fcl/serialization/BVH_model.h index 78fd175c7..d48d20b2a 100644 --- a/include/hpp/fcl/serialization/BVH_model.h +++ b/include/hpp/fcl/serialization/BVH_model.h @@ -1,5 +1,5 @@ // -// Copyright (c) 2021 INRIA +// Copyright (c) 2021-2022 INRIA // #ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H @@ -12,6 +12,7 @@ #include "hpp/fcl/serialization/BV_splitter.h" #include "hpp/fcl/serialization/collision_object.h" #include "hpp/fcl/serialization/memory.h" +#include "hpp/fcl/serialization/triangle.h" namespace boost { namespace serialization { @@ -24,14 +25,6 @@ struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase { }; } // namespace internal -template -void serialize(Archive &ar, hpp::fcl::Triangle &triangle, - const unsigned int /*version*/) { - ar &make_nvp("p0", triangle[0]); - ar &make_nvp("p1", triangle[1]); - ar &make_nvp("p2", triangle[2]); -} - template void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model, const unsigned int /*version*/) { diff --git a/include/hpp/fcl/serialization/triangle.h b/include/hpp/fcl/serialization/triangle.h new file mode 100644 index 000000000..44c532e72 --- /dev/null +++ b/include/hpp/fcl/serialization/triangle.h @@ -0,0 +1,25 @@ +// +// Copyright (c) 2021-2022 INRIA +// + +#ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H +#define HPP_FCL_SERIALIZATION_TRIANGLE_H + +#include "hpp/fcl/data_types.h" +#include "hpp/fcl/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive &ar, hpp::fcl::Triangle &triangle, + const unsigned int /*version*/) { + ar &make_nvp("p0", triangle[0]); + ar &make_nvp("p1", triangle[1]); + ar &make_nvp("p2", triangle[2]); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H From 7052a6fbc95f648d62209cb9906b2efba340aed3 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 8 Dec 2022 15:01:57 +0100 Subject: [PATCH 04/17] serialization: support quadrilateral --- CMakeLists.txt | 3 ++- include/hpp/fcl/serialization/quadrilateral.h | 26 +++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) create mode 100644 include/hpp/fcl/serialization/quadrilateral.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 37b9f9071..0da548424 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ # # Software License Agreement (BSD License) # -# Copyright (c) 2014-2020 CNRS-LAAS, INRIA +# Copyright (c) 2014-2022 CNRS-LAAS, INRIA # Author: Florent Lamiraux, Joseph Mirabel # All rights reserved. # @@ -256,6 +256,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/serialization/RSS.h include/hpp/fcl/serialization/OBBRSS.h include/hpp/fcl/serialization/hfield.h + include/hpp/fcl/serialization/quadrilateral.h include/hpp/fcl/serialization/triangle.h include/hpp/fcl/timings.h ) diff --git a/include/hpp/fcl/serialization/quadrilateral.h b/include/hpp/fcl/serialization/quadrilateral.h new file mode 100644 index 000000000..3694607ea --- /dev/null +++ b/include/hpp/fcl/serialization/quadrilateral.h @@ -0,0 +1,26 @@ +// +// Copyright (c) 2022 INRIA +// + +#ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H +#define HPP_FCL_SERIALIZATION_QUADRILATERAL_H + +#include "hpp/fcl/data_types.h" +#include "hpp/fcl/serialization/fwd.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive &ar, hpp::fcl::Quadrilateral &quadrilateral, + const unsigned int /*version*/) { + ar &make_nvp("p0", quadrilateral[0]); + ar &make_nvp("p1", quadrilateral[1]); + ar &make_nvp("p2", quadrilateral[2]); + ar &make_nvp("p3", quadrilateral[3]); +} + +} // namespace serialization +} // namespace boost + +#endif // ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H From 08653ad304b014bcc1a1f33b3425782ff3611a4d Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 8 Dec 2022 15:02:27 +0100 Subject: [PATCH 05/17] python: activate EigenPy first --- python/fcl.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/fcl.cc b/python/fcl.cc index 59aec9b95..a0061d6fe 100644 --- a/python/fcl.cc +++ b/python/fcl.cc @@ -1,7 +1,7 @@ // // Software License Agreement (BSD License) // -// Copyright (c) 2019-2020 CNRS-LAAS INRIA +// Copyright (c) 2019-2022 CNRS-LAAS INRIA // Author: Joseph Mirabel // All rights reserved. // @@ -87,6 +87,7 @@ BOOST_PYTHON_MODULE(hppfcl) { namespace bp = boost::python; PyImport_ImportModule("warnings"); + eigenpy::enableEigenPy(); exposeVersion(); exposeMaths(); From 3b2204edad0ab4177dc8b7ff955a54bf95cd3c70 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 9 Dec 2022 10:19:28 +0100 Subject: [PATCH 06/17] core: fix deallocation --- include/hpp/fcl/shape/details/convex.hxx | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx index f25e17676..2a53de928 100644 --- a/include/hpp/fcl/shape/details/convex.hxx +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -202,6 +202,7 @@ FCL_REAL Convex::computeVolume() const { template void Convex::fillNeighbors() { + if (neighbors) delete[] neighbors; neighbors = new Neighbors[num_points]; typedef typename PolygonT::size_type size_type; @@ -230,7 +231,7 @@ void Convex::fillNeighbors() { } } - if (nneighbors_) delete nneighbors_; + if (nneighbors_) delete[] nneighbors_; nneighbors_ = new unsigned int[c_nneighbors]; unsigned int* p_nneighbors = nneighbors_; From 9575791fdc5c3d6d3c4fb9ba0923166c56c94e60 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 9 Dec 2022 10:20:11 +0100 Subject: [PATCH 07/17] serialization: add convex support --- CMakeLists.txt | 1 + include/hpp/fcl/serialization/convex.h | 113 +++++++++++++++++++++++++ 2 files changed, 114 insertions(+) create mode 100644 include/hpp/fcl/serialization/convex.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0da548424..73e023181 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -249,6 +249,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/serialization/BVH_model.h include/hpp/fcl/serialization/collision_data.h include/hpp/fcl/serialization/collision_object.h + include/hpp/fcl/serialization/convex.h include/hpp/fcl/serialization/eigen.h include/hpp/fcl/serialization/geometric_shapes.h include/hpp/fcl/serialization/memory.h diff --git a/include/hpp/fcl/serialization/convex.h b/include/hpp/fcl/serialization/convex.h new file mode 100644 index 000000000..dc027bc84 --- /dev/null +++ b/include/hpp/fcl/serialization/convex.h @@ -0,0 +1,113 @@ +// +// Copyright (c) 2022 INRIA +// + +#ifndef HPP_FCL_SERIALIZATION_CONVEX_H +#define HPP_FCL_SERIALIZATION_CONVEX_H + +#include "hpp/fcl/shape/convex.h" + +#include "hpp/fcl/serialization/fwd.h" +#include "hpp/fcl/serialization/geometric_shapes.h" +#include "hpp/fcl/serialization/memory.h" +#include "hpp/fcl/serialization/triangle.h" +#include "hpp/fcl/serialization/quadrilateral.h" + +namespace boost { +namespace serialization { + +namespace internal { +struct ConvexBaseAccessor : hpp::fcl::ConvexBase { + typedef hpp::fcl::ConvexBase Base; + using Base::own_storage_; +}; + +} // namespace internal + +template +void serialize(Archive &ar, hpp::fcl::ConvexBase &convex_base, + const unsigned int /*version*/) { + using namespace hpp::fcl; + + typedef internal::ConvexBaseAccessor Accessor; + Accessor &accessor = reinterpret_cast(convex_base); + + ar &make_nvp("base", boost::serialization::base_object( + convex_base)); + const unsigned int num_points_previous = convex_base.num_points; + ar &make_nvp("num_points", convex_base.num_points); + + if (Archive::is_loading::value) { + if (num_points_previous != convex_base.num_points || + !accessor.own_storage_) { + delete[] convex_base.points; + convex_base.points = new hpp::fcl::Vec3f[convex_base.num_points]; + accessor.own_storage_ = true; + } + } + + { + typedef Eigen::Matrix MatrixPoints; + Eigen::Map points_map( + reinterpret_cast(convex_base.points), 3, + convex_base.num_points); + ar &make_nvp("points", points_map); + } + + ar &make_nvp("center", convex_base.center); + // We don't save neighbors as they will be computed directly by calling + // fillNeighbors. +} + +namespace internal { +template +struct ConvexAccessor : hpp::fcl::Convex { + typedef hpp::fcl::Convex Base; + using Base::fillNeighbors; +}; + +} // namespace internal + +template +void serialize(Archive &ar, hpp::fcl::Convex &convex_, + const unsigned int /*version*/) { + using namespace hpp::fcl; + typedef internal::ConvexAccessor Accessor; + + Accessor &convex = reinterpret_cast(convex_); + ar &make_nvp("base", boost::serialization::base_object(convex)); + + const unsigned int num_polygons_previous = convex.num_polygons; + ar &make_nvp("num_polygons", convex.num_polygons); + + if (Archive::is_loading::value) { + if (num_polygons_previous != convex.num_polygons) { + delete[] convex.polygons; + convex.polygons = new PolygonT[convex.num_polygons]; + } + } + + ar &make_array(convex.polygons, convex.num_polygons); + + if (Archive::is_loading::value) convex.fillNeighbors(); +} + +} // namespace serialization +} // namespace boost + +namespace hpp { +namespace fcl { + +// namespace internal { +// template +// struct memory_footprint_evaluator< ::hpp::fcl::BVHModel > { +// static size_t run(const ::hpp::fcl::BVHModel &bvh_model) { +// return static_cast(bvh_model.memUsage(false)); +// } +// }; +// } // namespace internal + +} // namespace fcl +} // namespace hpp + +#endif // ifndef HPP_FCL_SERIALIZATION_CONVEX_H From 30e1065d1fd3ffc25bc7974049316752e1be391f Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 9 Dec 2022 10:20:51 +0100 Subject: [PATCH 08/17] test/serialization: test convex --- test/serialization.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/test/serialization.cpp b/test/serialization.cpp index aa4998d43..69dac1b69 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include "utility.h" @@ -207,6 +208,30 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) { } } +BOOST_AUTO_TEST_CASE(test_Convex) { + std::vector p1; + std::vector t1; + boost::filesystem::path path(TEST_RESOURCES_DIR); + + loadOBJFile((path / "env.obj").string().c_str(), p1, t1); + + BVHModel m1; + + m1.beginModel(); + m1.addSubModel(p1, t1); + m1.endModel(); + + m1.buildConvexHull(true); + + Convex& convex = static_cast&>(*m1.convex.get()); + + // Test Convex + { + Convex convex_copy; + test_serialization(convex, convex_copy); + } +} + BOOST_AUTO_TEST_CASE(test_HeightField) { const FCL_REAL min_altitude = -1.; const FCL_REAL x_dim = 1., y_dim = 2.; From f2aa9628fec3114cf734017e9955a7334dbb7fb2 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 9 Dec 2022 10:22:09 +0100 Subject: [PATCH 09/17] python: add Pickling support for geometries --- python/collision-geometries.cc | 85 ++++++++++++++++++++++++++++------ 1 file changed, 72 insertions(+), 13 deletions(-) diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 5abc2e872..5b99c2d78 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -35,6 +35,10 @@ #include #include +#include +#include +#include + #include "fcl.hh" #include "deprecation.hh" @@ -43,8 +47,13 @@ #include #include #include + #include +#include #include +#include +#include +#include #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC // FIXME for a reason I do not understand, doxygen fails to understand that @@ -69,6 +78,43 @@ using boost::noncopyable; typedef std::vector Vec3fs; typedef std::vector Triangles; +template +struct PickleObject : bp::pickle_suite { + static bp::tuple getinitargs(const T&) { return bp::make_tuple(); } + + static bp::tuple getstate(const T& obj) { + std::stringstream ss; + boost::archive::text_oarchive oa(ss); + oa& obj; + + return bp::make_tuple(bp::str(ss.str())); + } + + static void setstate(T& obj, bp::tuple tup) { + if (bp::len(tup) == 0 || bp::len(tup) > 1) { + throw eigenpy::Exception( + "Pickle was not able to reconstruct the object from the loaded " + "data.\n" + "The pickle data structure contains too many elements."); + } + + bp::object py_obj = tup[0]; + boost::python::extract obj_as_string(py_obj.ptr()); + if (obj_as_string.check()) { + const std::string str = obj_as_string; + std::istringstream is(str); + boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); + ia >> obj; + } else { + throw eigenpy::Exception( + "Pickle was not able to reconstruct the model from the loaded data.\n" + "The entry is not a string."); + } + } + + static bool getstate_manages_dict() { return false; } +}; + struct BVHModelBaseWrapper { typedef Eigen::Matrix RowMatrixX3; typedef Eigen::Map MapRowMatrixX3; @@ -102,7 +148,8 @@ void exposeBVHModel(const std::string& bvname) { .DEF_CLASS_FUNC(BVH, makeParentRelative) .DEF_CLASS_FUNC(BVHModelBase, memUsage) .def("clone", &BVH::clone, doxygen::member_func_doc(&BVH::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); } template @@ -141,7 +188,8 @@ void exposeHeightField(const std::string& bvname) { .def("getBV", (Node & (Geometry::*)(unsigned int)) & Geometry::getBV, doxygen::member_func_doc((Node & (Geometry::*)(unsigned int)) & Geometry::getBV), - bp::return_internal_reference<>()); + bp::return_internal_reference<>()) + .def_pickle(PickleObject()); } struct ConvexBaseWrapper { @@ -231,7 +279,8 @@ void exposeShapes() { .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Box, halfSide) .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr >( "Capsule", doxygen::class_doc(), no_init) @@ -239,7 +288,8 @@ void exposeShapes() { .DEF_RW_CLASS_ATTRIB(Capsule, radius) .DEF_RW_CLASS_ATTRIB(Capsule, halfLength) .def("clone", &Capsule::clone, doxygen::member_func_doc(&Capsule::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr >( "Cone", doxygen::class_doc(), no_init) @@ -247,7 +297,8 @@ void exposeShapes() { .DEF_RW_CLASS_ATTRIB(Cone, radius) .DEF_RW_CLASS_ATTRIB(Cone, halfLength) .def("clone", &Cone::clone, doxygen::member_func_doc(&Cone::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr, noncopyable>( "ConvexBase", doxygen::class_doc(), no_init) @@ -280,7 +331,8 @@ void exposeShapes() { no_init) .def("__init__", make_constructor(&ConvexWrapper::constructor)) .DEF_RO_CLASS_ATTRIB(Convex, num_polygons) - .def("polygons", &ConvexWrapper::polygons); + .def("polygons", &ConvexWrapper::polygons) + .def_pickle(PickleObject >()); class_, shared_ptr >( "Cylinder", doxygen::class_doc(), no_init) @@ -289,7 +341,8 @@ void exposeShapes() { .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength) .def("clone", &Cylinder::clone, doxygen::member_func_doc(&Cylinder::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr >( "Halfspace", doxygen::class_doc(), no_init) @@ -300,7 +353,8 @@ void exposeShapes() { .DEF_RW_CLASS_ATTRIB(Halfspace, d) .def("clone", &Halfspace::clone, doxygen::member_func_doc(&Halfspace::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr >( "Plane", doxygen::class_doc(), no_init) @@ -310,14 +364,16 @@ void exposeShapes() { .DEF_RW_CLASS_ATTRIB(Plane, n) .DEF_RW_CLASS_ATTRIB(Plane, d) .def("clone", &Plane::clone, doxygen::member_func_doc(&Plane::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr >( "Sphere", doxygen::class_doc(), no_init) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Sphere, radius) .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr >( "Ellipsoid", doxygen::class_doc(), no_init) @@ -326,7 +382,8 @@ void exposeShapes() { .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, doxygen::member_func_doc(&Ellipsoid::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); class_, shared_ptr >( "TriangleP", doxygen::class_doc(), no_init) @@ -336,7 +393,8 @@ void exposeShapes() { .DEF_RW_CLASS_ATTRIB(TriangleP, c) .def("clone", &TriangleP::clone, doxygen::member_func_doc(&TriangleP::clone), - return_value_policy()); + return_value_policy()) + .def_pickle(PickleObject()); } boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) { @@ -484,7 +542,8 @@ void exposeCollisionGeometries() { // Vec3f &)>(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), - bp::return_internal_reference<>()); + bp::return_internal_reference<>()) + .def_pickle(PickleObject()); def("translate", (AABB(*)(const AABB&, const Vec3f&)) & translate, bp::args("aabb", "t"), "Translate the center of AABB by t"); From 10619a7f2a5f50320a5fbcbd5156d5dbdbb60030 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 9 Dec 2022 17:25:58 +0100 Subject: [PATCH 10/17] test: fix qhull --- test/serialization.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/serialization.cpp b/test/serialization.cpp index 69dac1b69..c5eaffb1c 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -208,6 +208,7 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) { } } +#ifdef HPP_FCL_HAS_QHULL BOOST_AUTO_TEST_CASE(test_Convex) { std::vector p1; std::vector t1; @@ -231,6 +232,7 @@ BOOST_AUTO_TEST_CASE(test_Convex) { test_serialization(convex, convex_copy); } } +#endif BOOST_AUTO_TEST_CASE(test_HeightField) { const FCL_REAL min_altitude = -1.; From d17b569234a20829acf55057d3ae16d85ce4d6b2 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sat, 10 Dec 2022 08:18:37 +0100 Subject: [PATCH 11/17] python: move pickle in a dedicated file --- python/CMakeLists.txt | 3 +- python/collision-geometries.cc | 43 ++----------------------- python/pickle.hh | 57 ++++++++++++++++++++++++++++++++++ 3 files changed, 61 insertions(+), 42 deletions(-) create mode 100644 python/pickle.hh diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f41f2f74d..88f9589ed 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -1,7 +1,7 @@ # # Software License Agreement (BSD License) # -# Copyright (c) 2019-2021 CNRS-LAAS INRIA +# Copyright (c) 2019-2022 CNRS-LAAS INRIA # Author: Joseph Mirabel # All rights reserved. # @@ -47,6 +47,7 @@ SET(${LIBRARY_NAME}_HEADERS broadphase/fwd.hh broadphase/broadphase_collision_manager.hh broadphase/broadphase_callbacks.hh + pickle.hh ) SET(ENABLE_PYTHON_DOXYGEN_AUTODOC TRUE CACHE BOOL "Enable automatic documentation of Python bindings from Doxygen documentation") diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 5b99c2d78..474bcf617 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -35,10 +35,6 @@ #include #include -#include -#include -#include - #include "fcl.hh" #include "deprecation.hh" @@ -55,6 +51,8 @@ #include #include +#include "pickle.hh" + #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC // FIXME for a reason I do not understand, doxygen fails to understand that // BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h @@ -78,43 +76,6 @@ using boost::noncopyable; typedef std::vector Vec3fs; typedef std::vector Triangles; -template -struct PickleObject : bp::pickle_suite { - static bp::tuple getinitargs(const T&) { return bp::make_tuple(); } - - static bp::tuple getstate(const T& obj) { - std::stringstream ss; - boost::archive::text_oarchive oa(ss); - oa& obj; - - return bp::make_tuple(bp::str(ss.str())); - } - - static void setstate(T& obj, bp::tuple tup) { - if (bp::len(tup) == 0 || bp::len(tup) > 1) { - throw eigenpy::Exception( - "Pickle was not able to reconstruct the object from the loaded " - "data.\n" - "The pickle data structure contains too many elements."); - } - - bp::object py_obj = tup[0]; - boost::python::extract obj_as_string(py_obj.ptr()); - if (obj_as_string.check()) { - const std::string str = obj_as_string; - std::istringstream is(str); - boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); - ia >> obj; - } else { - throw eigenpy::Exception( - "Pickle was not able to reconstruct the model from the loaded data.\n" - "The entry is not a string."); - } - } - - static bool getstate_manages_dict() { return false; } -}; - struct BVHModelBaseWrapper { typedef Eigen::Matrix RowMatrixX3; typedef Eigen::Map MapRowMatrixX3; diff --git a/python/pickle.hh b/python/pickle.hh new file mode 100644 index 000000000..e2a2be652 --- /dev/null +++ b/python/pickle.hh @@ -0,0 +1,57 @@ +// +// Copyright (c) 2022 INRIA +// + +#ifndef HPP_FCL_PYTHON_PICKLE_H +#define HPP_FCL_PYTHON_PICKLE_H + +#include +#include + +#include +#include +#include + +using namespace boost::python; +using namespace hpp::fcl; +// +template +struct PickleObject : boost::python::pickle_suite { + static boost::python::tuple getinitargs(const T&) { + return boost::python::make_tuple(); + } + + static boost::python::tuple getstate(const T& obj) { + std::stringstream ss; + boost::archive::text_oarchive oa(ss); + oa& obj; + + return boost::python::make_tuple(boost::python::str(ss.str())); + } + + static void setstate(T& obj, boost::python::tuple tup) { + if (boost::python::len(tup) == 0 || boost::python::len(tup) > 1) { + throw eigenpy::Exception( + "Pickle was not able to reconstruct the object from the loaded " + "data.\n" + "The pickle data structure contains too many elements."); + } + + boost::python::object py_obj = tup[0]; + boost::python::extract obj_as_string(py_obj.ptr()); + if (obj_as_string.check()) { + const std::string str = obj_as_string; + std::istringstream is(str); + boost::archive::text_iarchive ia(is, boost::archive::no_codecvt); + ia >> obj; + } else { + throw eigenpy::Exception( + "Pickle was not able to reconstruct the model from the loaded data.\n" + "The entry is not a string."); + } + } + + static bool getstate_manages_dict() { return false; } +}; + +#endif // ifndef HPP_FCL_PYTHON_PICKLE_H From 7b71853b4388cb5e67ba3131770174ffac13e6d5 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sat, 10 Dec 2022 08:38:57 +0100 Subject: [PATCH 12/17] shapes: add missing explicit keyword --- include/hpp/fcl/shape/geometric_shapes.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index 54ef80922..645f411b7 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -193,7 +193,7 @@ class HPP_FCL_DLLAPI Box : public ShapeBase { /// @brief Center at zero point sphere class HPP_FCL_DLLAPI Sphere : public ShapeBase { public: - Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {} + explicit Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {} Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {} @@ -255,7 +255,7 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) : ShapeBase(), radii(rx, ry, rz) {} - Ellipsoid(const Vec3f& radii) : radii(radii) {} + explicit Ellipsoid(const Vec3f& radii) : radii(radii) {} Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} From c9aa5e0b1ffa3e35ac1262e690e197f480a64634 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sat, 10 Dec 2022 09:13:04 +0100 Subject: [PATCH 13/17] core: add missing default constructors for pickling --- include/hpp/fcl/shape/geometric_shapes.h | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index 645f411b7..c469bd972 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -70,6 +70,8 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { /// @brief Triangle stores the points instead of only indices of points class HPP_FCL_DLLAPI TriangleP : public ShapeBase { public: + TriangleP(){}; + TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_) {} @@ -193,6 +195,9 @@ class HPP_FCL_DLLAPI Box : public ShapeBase { /// @brief Center at zero point sphere class HPP_FCL_DLLAPI Sphere : public ShapeBase { public: + /// @brief Default constructor + Sphere() {} + explicit Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {} Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {} @@ -252,6 +257,9 @@ class HPP_FCL_DLLAPI Sphere : public ShapeBase { /// @brief Ellipsoid centered at point zero class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { public: + /// @brief Default constructor + Ellipsoid() {} + Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) : ShapeBase(), radii(rx, ry, rz) {} @@ -324,6 +332,9 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { /// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$. class HPP_FCL_DLLAPI Capsule : public ShapeBase { public: + /// @brief Default constructor + Capsule() {} + Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } @@ -402,6 +413,9 @@ class HPP_FCL_DLLAPI Capsule : public ShapeBase { /// \f$ z = halfLength \f$. class HPP_FCL_DLLAPI Cone : public ShapeBase { public: + /// @brief Default constructor + Cone() {} + Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } @@ -486,6 +500,9 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase { /// The cylinder is defined at its centroid. class HPP_FCL_DLLAPI Cylinder : public ShapeBase { public: + /// @brief Default constructor + Cylinder() {} + Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } From 98703110fbdc50a2f57d51382f867b635d89b898 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sat, 10 Dec 2022 09:13:27 +0100 Subject: [PATCH 14/17] python: expose missing contructors for Pickling --- python/collision-geometries.cc | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 474bcf617..9801851f6 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -236,6 +236,7 @@ void exposeShapes() { class_, shared_ptr >( "Box", doxygen::class_doc(), no_init) .def(dv::init()) + .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Box, halfSide) @@ -245,7 +246,9 @@ void exposeShapes() { class_, shared_ptr >( "Capsule", doxygen::class_doc(), no_init) + .def(dv::init()) .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Capsule, radius) .DEF_RW_CLASS_ATTRIB(Capsule, halfLength) .def("clone", &Capsule::clone, doxygen::member_func_doc(&Capsule::clone), @@ -254,7 +257,9 @@ void exposeShapes() { class_, shared_ptr >( "Cone", doxygen::class_doc(), no_init) + .def(dv::init()) .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cone, radius) .DEF_RW_CLASS_ATTRIB(Cone, halfLength) .def("clone", &Cone::clone, doxygen::member_func_doc(&Cone::clone), @@ -291,13 +296,17 @@ void exposeShapes() { noncopyable>("Convex", doxygen::class_doc >(), no_init) .def("__init__", make_constructor(&ConvexWrapper::constructor)) + .def(dv::init >()) + .def(dv::init, const Convex&>()) .DEF_RO_CLASS_ATTRIB(Convex, num_polygons) .def("polygons", &ConvexWrapper::polygons) .def_pickle(PickleObject >()); class_, shared_ptr >( "Cylinder", doxygen::class_doc(), no_init) + .def(dv::init()) .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cylinder, radius) .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength) .def("clone", &Cylinder::clone, @@ -308,6 +317,7 @@ void exposeShapes() { class_, shared_ptr >( "Halfspace", doxygen::class_doc(), no_init) .def(dv::init()) + .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Halfspace, n) @@ -320,6 +330,7 @@ void exposeShapes() { class_, shared_ptr >( "Plane", doxygen::class_doc(), no_init) .def(dv::init()) + .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Plane, n) @@ -330,6 +341,8 @@ void exposeShapes() { class_, shared_ptr >( "Sphere", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Sphere, radius) .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone), @@ -338,8 +351,10 @@ void exposeShapes() { class_, shared_ptr >( "Ellipsoid", doxygen::class_doc(), no_init) + .def(dv::init()) .def(dv::init()) .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, doxygen::member_func_doc(&Ellipsoid::clone), @@ -348,7 +363,9 @@ void exposeShapes() { class_, shared_ptr >( "TriangleP", doxygen::class_doc(), no_init) + .def(dv::init()) .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(TriangleP, a) .DEF_RW_CLASS_ATTRIB(TriangleP, b) .DEF_RW_CLASS_ATTRIB(TriangleP, c) From 4b4ec87a12485ad90b0d7e105151ee93d8b8b733 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sat, 10 Dec 2022 09:13:46 +0100 Subject: [PATCH 15/17] test: add pickling test --- test/python_unit/CMakeLists.txt | 2 ++ test/python_unit/pickle.py | 59 +++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+) create mode 100644 test/python_unit/pickle.py diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt index 6bad8ec8a..542886c60 100644 --- a/test/python_unit/CMakeLists.txt +++ b/test/python_unit/CMakeLists.txt @@ -2,6 +2,8 @@ SET(${PROJECT_NAME}_PYTHON_TESTS geometric_shapes api collision + collision_manager + pickle ) ADD_DEPENDENCIES(build_tests hppfcl) diff --git a/test/python_unit/pickle.py b/test/python_unit/pickle.py new file mode 100644 index 000000000..d2bb137b3 --- /dev/null +++ b/test/python_unit/pickle.py @@ -0,0 +1,59 @@ +import unittest +from test_case import TestCase +import hppfcl + +import numpy as np +import pickle + + +def tetahedron(): + pts = hppfcl.StdVec_Vec3f() + pts.append(np.array((0, 0, 0))) + pts.append(np.array((0, 1, 0))) + pts.append(np.array((1, 0, 0))) + pts.append(np.array((0, 0, 1))) + tri = hppfcl.StdVec_Triangle() + tri.append(hppfcl.Triangle(0, 1, 2)) + tri.append(hppfcl.Triangle(0, 1, 3)) + tri.append(hppfcl.Triangle(0, 2, 3)) + tri.append(hppfcl.Triangle(1, 2, 3)) + return hppfcl.Convex(pts, tri) + + +class TestGeometryPickling(TestCase): + def pickling(self, obj): + with open("save.p", "wb") as f: + pickle.dump(obj, f) + with open("save.p", "rb") as f: + obj2 = pickle.load(f) + + self.assertTrue(obj == obj2) + + def test_all_shapes(self): + box = hppfcl.Box(1.0, 2.0, 3.0) + self.pickling(box) + + sphere = hppfcl.Sphere(1.0) + self.pickling(sphere) + + ellipsoid = hppfcl.Ellipsoid(1.0, 2.0, 3.0) + self.pickling(ellipsoid) + + convex = tetahedron() + self.pickling(convex) + + capsule = hppfcl.Capsule(1.0, 2.0) + self.pickling(capsule) + + cylinder = hppfcl.Cylinder(1.0, 2.0) + self.pickling(cylinder) + + plane = hppfcl.Plane(np.array([0.0, 0.0, 1.0]), 2.0) + self.pickling(plane) + + half_space = hppfcl.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0) + self.pickling(half_space) + + +if __name__ == "__main__": + unittest.main() From 3b0c94ae2115a66d8d65c4bbf84b890482db7254 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sat, 10 Dec 2022 10:05:43 +0100 Subject: [PATCH 16/17] test: rename python test --- test/python_unit/CMakeLists.txt | 2 +- test/python_unit/{pickle.py => pickling.py} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename test/python_unit/{pickle.py => pickling.py} (100%) diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt index 542886c60..8f7e098e0 100644 --- a/test/python_unit/CMakeLists.txt +++ b/test/python_unit/CMakeLists.txt @@ -3,7 +3,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS api collision collision_manager - pickle + pickling ) ADD_DEPENDENCIES(build_tests hppfcl) diff --git a/test/python_unit/pickle.py b/test/python_unit/pickling.py similarity index 100% rename from test/python_unit/pickle.py rename to test/python_unit/pickling.py From 013649714816bb69f8d3b8cc4b3708d8294fc478 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sat, 10 Dec 2022 10:08:11 +0100 Subject: [PATCH 17/17] test/python: fix import ordering --- test/python_unit/pickling.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/python_unit/pickling.py b/test/python_unit/pickling.py index d2bb137b3..1e20fe393 100644 --- a/test/python_unit/pickling.py +++ b/test/python_unit/pickling.py @@ -2,8 +2,8 @@ from test_case import TestCase import hppfcl -import numpy as np import pickle +import numpy as np def tetahedron():