From 2cd3bc076038b7a490d0f251837150814d8f45ba Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 15 Nov 2023 08:44:49 +0100 Subject: [PATCH 1/6] cmake: sync submodule --- cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake b/cmake index 47b924b476..570915059b 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 47b924b4760b239a75474462c1d6d66f74ba42d8 +Subproject commit 570915059b50f7dead7dae4c7f782ad3612fdc6e From 4ad140f361eca804d08df27aa93183a1f71bc917 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 15 Nov 2023 08:53:21 +0100 Subject: [PATCH 2/6] fcl: set minimal version to 2.0.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 80b616b8d2..9204d02c0c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -180,7 +180,7 @@ ENDIF(BUILD_PYTHON_INTERFACE) IF(BUILD_WITH_HPP_FCL_SUPPORT) ADD_DEFINITIONS(-DPINOCCHIO_WITH_HPP_FCL) LIST(APPEND CFLAGS_DEPENDENCIES "-DPINOCCHIO_WITH_HPP_FCL") - ADD_PROJECT_DEPENDENCY(hpp-fcl 1.7.3 REQUIRED PKG_CONFIG_REQUIRES "hpp-fcl >= 1.7.3") + ADD_PROJECT_DEPENDENCY(hpp-fcl 2.0.0 REQUIRED PKG_CONFIG_REQUIRES "hpp-fcl >= 2.0.0") # Check whether hpp-fcl python bindings are available. SET(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS FALSE) IF(BUILD_PYTHON_INTERFACE) From fe32adec2a30a1847e89a463b92431a8ac5ef125 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 15 Nov 2023 08:53:51 +0100 Subject: [PATCH 3/6] fcl: remove outdated code --- .../pinocchio/visualize/meshcat_visualizer.py | 8 +------- include/pinocchio/multibody/fcl.hpp | 17 +---------------- include/pinocchio/parsers/urdf.hpp | 4 ---- src/parsers/urdf/geometry.cpp | 19 ------------------- unittest/urdf.cpp | 13 +++---------- 5 files changed, 5 insertions(+), 56 deletions(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index c8f7c75454..90ef554976 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -154,13 +154,7 @@ def loadMesh(mesh): tri = call_triangles(k) faces[k] = [tri[i] for i in range(3)] - if LooseVersion(hppfcl.__version__) >= LooseVersion("1.7.7"): - vertices = call_vertices() - else: - vertices = np.empty((num_vertices, 3)) - for k in range(num_vertices): - vertices[k] = call_vertices(k) - + vertices = call_vertices() vertices = vertices.astype(np.float32) if num_tris > 0: diff --git a/include/pinocchio/multibody/fcl.hpp b/include/pinocchio/multibody/fcl.hpp index afc88131c2..2fcebac0fe 100644 --- a/include/pinocchio/multibody/fcl.hpp +++ b/include/pinocchio/multibody/fcl.hpp @@ -47,30 +47,15 @@ #include #include -#ifdef PINOCCHIO_WITH_HPP_FCL -#if HPP_FCL_VERSION_AT_LEAST(2,0,0) -#define PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR -#endif -#endif - -#ifdef PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR #include -#else -#include -#include -#endif #include namespace pinocchio { -#ifdef PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR using std::shared_ptr; using std::make_shared; -#else - using boost::shared_ptr; - using boost::make_shared; -#endif + struct CollisionPair : public std::pair { diff --git a/include/pinocchio/parsers/urdf.hpp b/include/pinocchio/parsers/urdf.hpp index f88bb6b5af..6d59edb061 100644 --- a/include/pinocchio/parsers/urdf.hpp +++ b/include/pinocchio/parsers/urdf.hpp @@ -24,11 +24,7 @@ namespace hpp namespace fcl { class MeshLoader; -#ifdef PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR typedef std::shared_ptr MeshLoaderPtr; -#else - typedef boost::shared_ptr MeshLoaderPtr; -#endif } } /// \endcond diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp index 7b5e32c8ad..f75b087370 100644 --- a/src/parsers/urdf/geometry.cpp +++ b/src/parsers/urdf/geometry.cpp @@ -109,13 +109,6 @@ namespace pinocchio mesh->scale.z; } -#ifdef PINOCCHIO_WITH_HPP_FCL -# if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \ - ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \ - HPP_FCL_PATCH_VERSION>3)))) -# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 -# endif - /** * @brief Get a fcl::CollisionObject from a URDF geometry, searching * for it in specified package directories @@ -160,26 +153,14 @@ namespace pinocchio retrieveMeshScale(urdf_mesh, meshScale); // Create FCL mesh by parsing Collada file. -#ifdef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale); bool convex = tree.isMeshConvex (linkName, geomName); if (convex) { bvh->buildConvexRepresentation (false); -#if HPP_FCL_MAJOR_VERSION < 2 - geometry = bvh->convex; -#else geometry = shared_ptr(bvh->convex.get(), [bvh](...) mutable { bvh->convex.reset(); }); -#endif // HPP_FCL_MAJOR_VERSION } else { -#if HPP_FCL_MAJOR_VERSION < 2 - geometry = bvh; -#else geometry = shared_ptr(bvh.get(), [bvh](...) mutable { bvh.reset(); }); -#endif // HPP_FCL_MAJOR_VERSION } -#else - geometry = meshLoader->load (meshPath, scale); -#endif } // Handle the case where collision geometry is a cylinder diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp index 5813add1a8..0ae9e88011 100644 --- a/unittest/urdf.cpp +++ b/unittest/urdf.cpp @@ -56,18 +56,11 @@ BOOST_AUTO_TEST_CASE ( build_model_simple_humanoid ) BOOST_CHECK_EQUAL(geomModel.geometryObjects[0].geometry->getNodeType(), hpp::fcl::GEOM_CAPSULE); #endif -#if ( HPP_FCL_MAJOR_VERSION>1 || ( HPP_FCL_MAJOR_VERSION==1 && \ - ( HPP_FCL_MINOR_VERSION>1 || ( HPP_FCL_MINOR_VERSION==1 && \ - HPP_FCL_PATCH_VERSION>3)))) -# define PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 -#endif - -#if defined(PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3) && !defined(PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME) +#ifndef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getNodeType(), hpp::fcl::GEOM_CONVEX); -#undef PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 -#else // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME +#else PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getObjectType(), hpp::fcl::OT_BVH); -#endif // PINOCCHIO_HPP_FCL_SUPERIOR_TO_1_1_3 && !PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME +#endif #endif // PINOCCHIO_WITH_HPP_FCL pinocchio::Model model_ff; From c515d1e25c27f5e804fab50b4c09bc1e2a20486d Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 15 Nov 2023 10:08:41 +0100 Subject: [PATCH 4/6] fcl: remove missing #endif --- src/parsers/urdf/geometry.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp index f75b087370..5dfd685a57 100644 --- a/src/parsers/urdf/geometry.cpp +++ b/src/parsers/urdf/geometry.cpp @@ -109,6 +109,7 @@ namespace pinocchio mesh->scale.z; } +#ifdef PINOCCHIO_WITH_HPP_FCL /** * @brief Get a fcl::CollisionObject from a URDF geometry, searching * for it in specified package directories From 7b36e323ad3434daadf2ab7a9983deace6782a2b Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 15 Nov 2023 13:48:02 +0100 Subject: [PATCH 5/6] python: remove LooseVersion for Python > 3.12 --- bindings/python/pinocchio/visualize/meshcat_visualizer.py | 1 - 1 file changed, 1 deletion(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 90ef554976..12305f6864 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -6,7 +6,6 @@ import os import warnings import numpy as np -from distutils.version import LooseVersion try: import hppfcl From e58bb2de266ed0996b7ccb1bbdc8de9e99268964 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 16 Nov 2023 15:53:55 +0100 Subject: [PATCH 6/6] test: add missing comment Co-authored-by: Joris Vaillant --- unittest/urdf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp index 0ae9e88011..b0cdf2ffed 100644 --- a/unittest/urdf.cpp +++ b/unittest/urdf.cpp @@ -58,7 +58,7 @@ BOOST_AUTO_TEST_CASE ( build_model_simple_humanoid ) #ifndef PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getNodeType(), hpp::fcl::GEOM_CONVEX); -#else PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME +#else // PINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME BOOST_CHECK_EQUAL(geomModel.geometryObjects[1].geometry->getObjectType(), hpp::fcl::OT_BVH); #endif #endif // PINOCCHIO_WITH_HPP_FCL