From 47369ea2a2f070873084f64d5d04db8555193bcd Mon Sep 17 00:00:00 2001 From: Joseph Mirabel <jmirabel@laas.fr> Date: Thu, 31 Jan 2019 23:55:21 +0100 Subject: [PATCH] [FCL] Update to change of namespace --- .../multibody/fcl/collision-geometry.hpp | 56 +++++++++---------- .../python/multibody/fcl/collision-result.hpp | 2 +- bindings/python/multibody/fcl/contact.hpp | 4 +- .../python/multibody/fcl/distance-result.hpp | 6 +- bindings/python/multibody/fcl/mesh-loader.hpp | 4 +- src/multibody/fcl.hpp | 4 ++ src/parsers/urdf.hpp | 21 ++++--- src/spatial/fcl-pinocchio-conversions.hpp | 6 +- 8 files changed, 55 insertions(+), 48 deletions(-) diff --git a/bindings/python/multibody/fcl/collision-geometry.hpp b/bindings/python/multibody/fcl/collision-geometry.hpp index 190ed89bc..343e021c2 100644 --- a/bindings/python/multibody/fcl/collision-geometry.hpp +++ b/bindings/python/multibody/fcl/collision-geometry.hpp @@ -23,7 +23,7 @@ namespace pinocchio struct CollisionGeometryPythonVisitor : public bp::def_visitor<CollisionGeometryPythonVisitor> { - typedef ::fcl::CollisionGeometry CollisionGeometry; + typedef ::hpp::fcl::CollisionGeometry CollisionGeometry; template<class PyClass> void visit(PyClass& cl) const @@ -49,35 +49,35 @@ namespace pinocchio .def(CollisionGeometryPythonVisitor()) ; - bp::enum_< ::fcl::OBJECT_TYPE >("OBJECT_TYPE") - .value("OT_UNKNOWN",::fcl::OT_UNKNOWN) - .value("OT_BVH",::fcl::OT_BVH) - .value("OT_GEOM",::fcl::OT_GEOM) - .value("OT_OCTREE",::fcl::OT_OCTREE) - .value("OT_COUNT",::fcl::OT_COUNT) + bp::enum_< ::hpp::fcl::OBJECT_TYPE >("OBJECT_TYPE") + .value("OT_UNKNOWN",::hpp::fcl::OT_UNKNOWN) + .value("OT_BVH",::hpp::fcl::OT_BVH) + .value("OT_GEOM",::hpp::fcl::OT_GEOM) + .value("OT_OCTREE",::hpp::fcl::OT_OCTREE) + .value("OT_COUNT",::hpp::fcl::OT_COUNT) ; - bp::enum_< ::fcl::NODE_TYPE >("NODE_TYPE") - .value("BV_UNKNOWN",::fcl::BV_UNKNOWN) - .value("BV_AABB",::fcl::BV_AABB) - .value("BV_OBB",::fcl::BV_OBB) - .value("BV_RSS",::fcl::BV_RSS) - .value("BV_kIOS",::fcl::BV_kIOS) - .value("BV_OBBRSS",::fcl::BV_OBBRSS) - .value("BV_KDOP16",::fcl::BV_KDOP16) - .value("BV_KDOP18",::fcl::BV_KDOP18) - .value("BV_KDOP24",::fcl::BV_KDOP24) - .value("GEOM_BOX",::fcl::GEOM_BOX) - .value("GEOM_SPHERE",::fcl::GEOM_SPHERE) - .value("GEOM_CAPSULE",::fcl::GEOM_CAPSULE) - .value("GEOM_CONE",::fcl::GEOM_CONE) - .value("GEOM_CYLINDER",::fcl::GEOM_CYLINDER) - .value("GEOM_CONVEX",::fcl::GEOM_CONVEX) - .value("GEOM_PLANE",::fcl::GEOM_PLANE) - .value("GEOM_HALFSPACE",::fcl::GEOM_HALFSPACE) - .value("GEOM_TRIANGLE",::fcl::GEOM_TRIANGLE) - .value("GEOM_OCTREE",::fcl::GEOM_OCTREE) - .value("NODE_COUNT",::fcl::NODE_COUNT) + bp::enum_< ::hpp::fcl::NODE_TYPE >("NODE_TYPE") + .value("BV_UNKNOWN",::hpp::fcl::BV_UNKNOWN) + .value("BV_AABB",::hpp::fcl::BV_AABB) + .value("BV_OBB",::hpp::fcl::BV_OBB) + .value("BV_RSS",::hpp::fcl::BV_RSS) + .value("BV_kIOS",::hpp::fcl::BV_kIOS) + .value("BV_OBBRSS",::hpp::fcl::BV_OBBRSS) + .value("BV_KDOP16",::hpp::fcl::BV_KDOP16) + .value("BV_KDOP18",::hpp::fcl::BV_KDOP18) + .value("BV_KDOP24",::hpp::fcl::BV_KDOP24) + .value("GEOM_BOX",::hpp::fcl::GEOM_BOX) + .value("GEOM_SPHERE",::hpp::fcl::GEOM_SPHERE) + .value("GEOM_CAPSULE",::hpp::fcl::GEOM_CAPSULE) + .value("GEOM_CONE",::hpp::fcl::GEOM_CONE) + .value("GEOM_CYLINDER",::hpp::fcl::GEOM_CYLINDER) + .value("GEOM_CONVEX",::hpp::fcl::GEOM_CONVEX) + .value("GEOM_PLANE",::hpp::fcl::GEOM_PLANE) + .value("GEOM_HALFSPACE",::hpp::fcl::GEOM_HALFSPACE) + .value("GEOM_TRIANGLE",::hpp::fcl::GEOM_TRIANGLE) + .value("GEOM_OCTREE",::hpp::fcl::GEOM_OCTREE) + .value("NODE_COUNT",::hpp::fcl::NODE_COUNT) ; } diff --git a/bindings/python/multibody/fcl/collision-result.hpp b/bindings/python/multibody/fcl/collision-result.hpp index 21bf625ea..8e756de7b 100644 --- a/bindings/python/multibody/fcl/collision-result.hpp +++ b/bindings/python/multibody/fcl/collision-result.hpp @@ -23,7 +23,7 @@ namespace pinocchio struct CollisionResultPythonVisitor : public bp::def_visitor<CollisionResultPythonVisitor> { - typedef ::fcl::CollisionResult CollisionResult; + typedef ::hpp::fcl::CollisionResult CollisionResult; template<class PyClass> void visit(PyClass& cl) const diff --git a/bindings/python/multibody/fcl/contact.hpp b/bindings/python/multibody/fcl/contact.hpp index fff7eb022..12a0ae212 100644 --- a/bindings/python/multibody/fcl/contact.hpp +++ b/bindings/python/multibody/fcl/contact.hpp @@ -23,8 +23,8 @@ namespace pinocchio struct ContactPythonVisitor : public bp::def_visitor<ContactPythonVisitor> { - typedef ::fcl::Contact Contact; - typedef ::fcl::CollisionGeometry CollisionGeometry; + typedef ::hpp::fcl::Contact Contact; + typedef ::hpp::fcl::CollisionGeometry CollisionGeometry; template<class PyClass> void visit(PyClass& cl) const diff --git a/bindings/python/multibody/fcl/distance-result.hpp b/bindings/python/multibody/fcl/distance-result.hpp index 7b9c793a0..57399b789 100644 --- a/bindings/python/multibody/fcl/distance-result.hpp +++ b/bindings/python/multibody/fcl/distance-result.hpp @@ -23,7 +23,7 @@ namespace pinocchio struct DistanceResultPythonVisitor : public bp::def_visitor<DistanceResultPythonVisitor> { - typedef ::fcl::DistanceResult DistanceResult; + typedef ::hpp::fcl::DistanceResult DistanceResult; template<class PyClass> void visit(PyClass& cl) const @@ -54,8 +54,8 @@ namespace pinocchio private: - static ::fcl::Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; } - static ::fcl::Vec3f getNearestPoint2(const DistanceResult & res) { return res.nearest_points[1]; } + static ::hpp::fcl::Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; } + static ::hpp::fcl::Vec3f getNearestPoint2(const DistanceResult & res) { return res.nearest_points[1]; } }; diff --git a/bindings/python/multibody/fcl/mesh-loader.hpp b/bindings/python/multibody/fcl/mesh-loader.hpp index ef73a13e0..6ca8e93d2 100644 --- a/bindings/python/multibody/fcl/mesh-loader.hpp +++ b/bindings/python/multibody/fcl/mesh-loader.hpp @@ -23,10 +23,10 @@ namespace pinocchio struct MeshLoaderPythonVisitor : public bp::def_visitor<MeshLoaderPythonVisitor> { - typedef ::fcl::MeshLoader MeshLoader; + typedef ::hpp::fcl::MeshLoader MeshLoader; typedef boost::shared_ptr<MeshLoader> MeshLoaderPtr_t; - typedef ::fcl::CachedMeshLoader CachedMeshLoader; + typedef ::hpp::fcl::CachedMeshLoader CachedMeshLoader; typedef boost::shared_ptr<CachedMeshLoader> CachedMeshLoaderPtr_t; template <typename T> diff --git a/src/multibody/fcl.hpp b/src/multibody/fcl.hpp index 42036af43..c718ccfc1 100644 --- a/src/multibody/fcl.hpp +++ b/src/multibody/fcl.hpp @@ -61,6 +61,10 @@ namespace pinocchio } +#else + + namespace fcl = hpp::fcl; + #endif // PINOCCHIO_WITH_HPP_FCL enum GeometryType diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp index 1a0758829..5433488a4 100644 --- a/src/parsers/urdf.hpp +++ b/src/parsers/urdf.hpp @@ -13,10 +13,13 @@ /// \cond // Commented because this is needed in function prototypes. //#ifdef PINOCCHIO_WITH_HPP_FCL +namespace hpp +{ namespace fcl { class MeshLoader; - typedef boost::shared_ptr<fcl::MeshLoader> MeshLoaderPtr; + typedef boost::shared_ptr<MeshLoader> MeshLoaderPtr; +} } //#endif // PINOCCHIO_WITH_HPP_FCL /// \endcond @@ -144,7 +147,7 @@ namespace pinocchio * obtained from calling pinocchio::rosPaths() * * @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION) - * @param[in] meshLoader object used to load meshes: fcl::MeshLoader [default] or fcl::CachedMeshLoader. + * @param[in] meshLoader object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader. * @param[out] geomModel Reference where to put the parsed information. * * @return Returns the reference on geom model for convenience. @@ -158,7 +161,7 @@ namespace pinocchio const GeometryType type, GeometryModel & geomModel, const std::vector<std::string> & packageDirs = std::vector<std::string> (), - fcl::MeshLoaderPtr meshLoader = fcl::MeshLoaderPtr()) + ::hpp::fcl::MeshLoaderPtr meshLoader = ::hpp::fcl::MeshLoaderPtr()) throw (std::invalid_argument); /** @@ -173,7 +176,7 @@ namespace pinocchio * typically obtained from calling pinocchio::rosPaths(). * * @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION) - * @param[in] meshLoader object used to load meshes: fcl::MeshLoader [default] or fcl::CachedMeshLoader. + * @param[in] meshLoader object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader. * @param[out] geomModel Reference where to put the parsed information. * * @return Returns the reference on geom model for convenience. @@ -187,7 +190,7 @@ namespace pinocchio const GeometryType type, GeometryModel & geomModel, const std::string & packageDir, - fcl::MeshLoaderPtr meshLoader = fcl::MeshLoaderPtr()) + hpp::fcl::MeshLoaderPtr meshLoader = hpp::fcl::MeshLoaderPtr()) throw (std::invalid_argument) { const std::vector<std::string> dirs(1,packageDir); @@ -207,7 +210,7 @@ namespace pinocchio * obtained from calling pinocchio::rosPaths() * * @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION) - * @param[in] meshLoader object used to load meshes: fcl::MeshLoader [default] or fcl::CachedMeshLoader. + * @param[in] meshLoader object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader. * @param[out] geomModel Reference where to put the parsed information. * * @return Returns the reference on geom model for convenience. @@ -221,7 +224,7 @@ namespace pinocchio const GeometryType type, GeometryModel & geomModel, const std::vector<std::string> & packageDirs = std::vector<std::string> (), - fcl::MeshLoaderPtr meshLoader = fcl::MeshLoaderPtr()) + hpp::fcl::MeshLoaderPtr meshLoader = hpp::fcl::MeshLoaderPtr()) throw (std::invalid_argument); /** @@ -236,7 +239,7 @@ namespace pinocchio * typically obtained from calling pinocchio::rosPaths(). * * @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION) - * @param[in] meshLoader object used to load meshes: fcl::MeshLoader [default] or fcl::CachedMeshLoader. + * @param[in] meshLoader object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader. * @param[out] geomModel Reference where to put the parsed information. * * @return Returns the reference on geom model for convenience. @@ -250,7 +253,7 @@ namespace pinocchio const GeometryType type, GeometryModel & geomModel, const std::string & packageDir, - fcl::MeshLoaderPtr meshLoader = fcl::MeshLoaderPtr()) + hpp::fcl::MeshLoaderPtr meshLoader = hpp::fcl::MeshLoaderPtr()) throw (std::invalid_argument) { const std::vector<std::string> dirs(1,packageDir); diff --git a/src/spatial/fcl-pinocchio-conversions.hpp b/src/spatial/fcl-pinocchio-conversions.hpp index e85a2909b..9632429d2 100644 --- a/src/spatial/fcl-pinocchio-conversions.hpp +++ b/src/spatial/fcl-pinocchio-conversions.hpp @@ -10,12 +10,12 @@ namespace pinocchio { - inline fcl::Transform3f toFclTransform3f(const SE3 & m) + inline hpp::fcl::Transform3f toFclTransform3f(const SE3 & m) { - return fcl::Transform3f(m.rotation(), m.translation()); + return hpp::fcl::Transform3f(m.rotation(), m.translation()); } - inline SE3 toPinocchioSE3(const fcl::Transform3f & tf) + inline SE3 toPinocchioSE3(const hpp::fcl::Transform3f & tf) { return SE3(tf.getRotation(), tf.getTranslation()); } -- GitLab