Skip to content

Commit

Permalink
Merge pull request #2086 from jcarpent/topic/fcl-update
Browse files Browse the repository at this point in the history
Update minimal version of hpp-fcl to 2.0.0
  • Loading branch information
jorisv authored Nov 16, 2023
2 parents b87ea95 + e58bb2d commit 033771a
Show file tree
Hide file tree
Showing 7 changed files with 7 additions and 58 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
9 changes: 1 addition & 8 deletions bindings/python/pinocchio/visualize/meshcat_visualizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
import os
import warnings
import numpy as np
from distutils.version import LooseVersion

try:
import hppfcl
Expand Down Expand Up @@ -154,13 +153,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:
Expand Down
17 changes: 1 addition & 16 deletions include/pinocchio/multibody/fcl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,30 +47,15 @@
#include <limits>
#include <assert.h>

#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 <memory>
#else
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#endif

#include <boost/foreach.hpp>

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<GeomIndex, GeomIndex>
{
Expand Down
4 changes: 0 additions & 4 deletions include/pinocchio/parsers/urdf.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,7 @@ namespace hpp
namespace fcl
{
class MeshLoader;
#ifdef PINOCCHIO_HPPFCL_USE_STD_SHARED_PTR
typedef std::shared_ptr<MeshLoader> MeshLoaderPtr;
#else
typedef boost::shared_ptr<MeshLoader> MeshLoaderPtr;
#endif
}
}
/// \endcond
Expand Down
18 changes: 0 additions & 18 deletions src/parsers/urdf/geometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -110,12 +110,6 @@ namespace pinocchio
}

#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
Expand Down Expand Up @@ -160,26 +154,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<fcl::CollisionGeometry>(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<fcl::CollisionGeometry>(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
Expand Down
13 changes: 3 additions & 10 deletions unittest/urdf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 033771a

Please sign in to comment.