From 2a158b1d22be3632cb39add2058a537f069c5c77 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 14 Jan 2022 15:13:16 -0800 Subject: [PATCH] Drop supporting FCL < 0.5 (#1647) --- CHANGELOG.md | 4 ++ cmake/DARTFindBoost.cmake | 2 +- cmake/DARTFindassimp.cmake | 9 ++-- cmake/DARTFindoctomap.cmake | 2 +- dart/collision/fcl/BackwardCompatibility.hpp | 28 ++++------ dart/collision/fcl/FCLCollisionDetector.cpp | 13 ++--- dart/collision/fcl/FCLCollisionDetector.hpp | 6 +-- dart/collision/fcl/FCLCollisionObject.cpp | 8 +-- dart/collision/fcl/FCLCollisionObject.hpp | 2 +- dart/constraint/ConstraintSolver.cpp | 2 +- dart/constraint/ContactSurface.hpp | 4 +- dart/dynamics/VoxelGridShape.cpp | 15 +++--- dart/dynamics/VoxelGridShape.hpp | 10 ++-- .../integration/test_ConstraintSolver.cpp | 54 +++++++++---------- 14 files changed, 73 insertions(+), 86 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c7c5393550ce8..2b175a4630c0c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,10 @@ ### [DART 6.13.0 (TBD)](https://github.com/dartsim/dart/milestone/69?closed=1) +* Build + + * Dropped supporting FCL < 0.5: [#1647](https://github.com/dartsim/dart/pull/1647) + * Common * Added Castable class: [#1634](https://github.com/dartsim/dart/pull/1634) diff --git a/cmake/DARTFindBoost.cmake b/cmake/DARTFindBoost.cmake index 266f33825b08c..0c1663cf6dbc0 100644 --- a/cmake/DARTFindBoost.cmake +++ b/cmake/DARTFindBoost.cmake @@ -6,7 +6,7 @@ # # This file is provided under the "BSD-style" License -set(DART_MIN_BOOST_VERSION 1.58.0 CACHE INTERNAL "Boost min version requirement" FORCE) +set(DART_MIN_BOOST_VERSION 1.65.1 CACHE INTERNAL "Boost min version requirement" FORCE) if(MSVC) add_definitions(-DBOOST_ALL_NO_LIB) endif() diff --git a/cmake/DARTFindassimp.cmake b/cmake/DARTFindassimp.cmake index 3057f903b9b9a..61db74aab0d64 100644 --- a/cmake/DARTFindassimp.cmake +++ b/cmake/DARTFindassimp.cmake @@ -10,10 +10,11 @@ find_package(assimp REQUIRED MODULE) # Manually check version because the upstream version compatibility policy # doesn't allow different major number while DART is compatible any version -# greater than or equal to 3.2. -if(ASSIMP_VERSION AND ASSIMP_VERSION VERSION_LESS 3.2) - message(SEND_ERROR "Found Assimp ${ASSIMP_VERSION}, but Assimp >= 3.2 is " - "required" +# greater than or equal to 4.1. +set(DART_ASSIMP_VERSION 4.1) +if(ASSIMP_VERSION AND ASSIMP_VERSION VERSION_LESS ${DART_ASSIMP_VERSION}) + message(SEND_ERROR "Found Assimp ${ASSIMP_VERSION}, but Assimp >= ${DART_ASSIMP_VERSION} + is required" ) endif() diff --git a/cmake/DARTFindoctomap.cmake b/cmake/DARTFindoctomap.cmake index 240c72f903056..e84bcb32be5c7 100644 --- a/cmake/DARTFindoctomap.cmake +++ b/cmake/DARTFindoctomap.cmake @@ -6,7 +6,7 @@ # # This file is provided under the "BSD-style" License -find_package(octomap 1.6.8 QUIET CONFIG) +find_package(octomap 1.8.1 QUIET CONFIG) if(octomap_FOUND AND NOT TARGET octomap) add_library(octomap INTERFACE IMPORTED) diff --git a/dart/collision/fcl/BackwardCompatibility.hpp b/dart/collision/fcl/BackwardCompatibility.hpp index eabb1c47a55a0..5de6c58065fc4 100644 --- a/dart/collision/fcl/BackwardCompatibility.hpp +++ b/dart/collision/fcl/BackwardCompatibility.hpp @@ -35,6 +35,7 @@ #include +#include "dart/common/Deprecated.hpp" #include "dart/config.hpp" // clang-format off @@ -88,30 +89,23 @@ #endif // FCL_VERSION_AT_LEAST(0,6,0) #endif // HAVE_OCTOMAP && FCL_HAVE_OCTOMAP -#if FCL_VERSION_AT_LEAST(0, 5, 0) - #include +#include + +/// \deprecated Use std::shared_ptr() instead template using fcl_shared_ptr = std::shared_ptr; + +/// \deprecated Use std::weak_ptr() instead template using fcl_weak_ptr = std::weak_ptr; + +/// \deprecated Use std::make_shared() instead template -fcl_shared_ptr fcl_make_shared(Args&&... args) +DART_DEPRECATED(6.13) +std::shared_ptr fcl_make_shared(Args&&... args) { return std::make_shared(std::forward(args)...); } -#else - #include - #include -template -using fcl_shared_ptr = boost::shared_ptr; -template -using fcl_weak_ptr = boost::weak_ptr; -template -fcl_shared_ptr fcl_make_shared(Args&&... args) -{ - return boost::make_shared(std::forward(args)...); -} -#endif // FCL_VERSION_AT_LEAST(0,5,0) namespace dart { namespace collision { @@ -169,7 +163,7 @@ using DistanceResult = ::fcl::DistanceResult; using Contact = ::fcl::Contact; #endif -#if FCL_VERSION_AT_LEAST(0, 4, 0) && !FCL_VERSION_AT_LEAST(0, 6, 0) +#if !FCL_VERSION_AT_LEAST(0, 6, 0) using Ellipsoid = ::fcl::Ellipsoid; #endif diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 2fe6bf7fcdecf..bbb1351bbb1a6 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -857,7 +857,7 @@ void FCLCollisionDetector::refreshCollisionObject(CollisionObject* const object) } //============================================================================== -fcl_shared_ptr +std::shared_ptr FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { @@ -885,7 +885,7 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } //============================================================================== -fcl_shared_ptr +std::shared_ptr FCLCollisionDetector::createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, @@ -942,12 +942,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( if (FCLCollisionDetector::PRIMITIVE == type) { -#if FCL_VERSION_AT_LEAST(0, 4, 0) geom = new fcl::Ellipsoid(FCLTypes::convertVector3(radii)); -#else - geom = createEllipsoid( - radii[0] * 2.0, radii[1] * 2.0, radii[2] * 2.0); -#endif } else { @@ -1079,7 +1074,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( geom = createEllipsoid(0.1, 0.1, 0.1); } - return fcl_shared_ptr(geom, deleter); + return std::shared_ptr(geom, deleter); } //============================================================================== @@ -1657,9 +1652,7 @@ void convertOption( { request.num_max_contacts = option.maxNumContacts; request.enable_contact = option.enableContact; -#if FCL_VERSION_AT_LEAST(0, 3, 0) request.gjk_solver_type = ::fcl::GST_LIBCCD; -#endif } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 9fdcd4e44e2b2..27218bef7397c 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -147,7 +147,7 @@ class FCLCollisionDetector : public CollisionDetector /// Return fcl::CollisionGeometry associated with give Shape. New /// fcl::CollisionGeome will be created if it hasn't created yet. - fcl_shared_ptr + std::shared_ptr claimFCLCollisionGeometry(const dynamics::ConstShapePtr& shape); protected: @@ -176,7 +176,7 @@ class FCLCollisionDetector : public CollisionDetector struct ShapeInfo final { /// A weak reference to the shape - fcl_weak_ptr mShape; + std::weak_ptr mShape; /// The last version of the shape, as known by this collision detector std::size_t mLastKnownVersion; @@ -184,7 +184,7 @@ class FCLCollisionDetector : public CollisionDetector /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter - fcl_shared_ptr + std::shared_ptr createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index 20f0b2279a58b..179fafefa0f57 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -57,7 +57,7 @@ FCLCollisionObject::getFCLCollisionObject() const FCLCollisionObject::FCLCollisionObject( CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, - const fcl_shared_ptr& fclCollGeom) + const std::shared_ptr& fclCollGeom) : CollisionObject(collisionDetector, shapeFrame), mFCLCollisionObject(new dart::collision::fcl::CollisionObject(fclCollGeom)) { @@ -83,14 +83,8 @@ void FCLCollisionObject::updateEngineData() const_cast(softMeshShape)->update(); // TODO(JS): update function be called by somewhere out of here. -#if FCL_VERSION_AT_LEAST(0, 3, 0) auto collGeom = const_cast( mFCLCollisionObject->collisionGeometry().get()); -#else - dart::collision::fcl::CollisionGeometry* collGeom - = const_cast( - mFCLCollisionObject->getCollisionGeometry()); -#endif assert( dynamic_cast<::fcl::BVHModel*>(collGeom)); auto bvhModel diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp index 7e18457ed1bc9..b563581e41f51 100644 --- a/dart/collision/fcl/FCLCollisionObject.hpp +++ b/dart/collision/fcl/FCLCollisionObject.hpp @@ -55,7 +55,7 @@ class FCLCollisionObject : public CollisionObject FCLCollisionObject( CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, - const fcl_shared_ptr& + const std::shared_ptr& fclCollGeom); // Documentation inherited diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 902a6b8088b00..215be7b0d4342 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -757,7 +757,7 @@ void ConstraintSolver::addContactSurfaceHandler( if (handler == mContactSurfaceHandler) { dterr << "Adding the same contact surface handler for the second time, " - "ignoring." << std::endl; + << "ignoring.\n"; return; } handler->setParent(mContactSurfaceHandler); diff --git a/dart/constraint/ContactSurface.hpp b/dart/constraint/ContactSurface.hpp index 0ae79343b694c..e48596c95bfa1 100644 --- a/dart/constraint/ContactSurface.hpp +++ b/dart/constraint/ContactSurface.hpp @@ -113,7 +113,7 @@ class ContactSurfaceHandler /// when adding a new one. It is suggested to keep this /// paradigm if used elsewhere. explicit ContactSurfaceHandler(ContactSurfaceHandlerPtr parent = nullptr); - + virtual ~ContactSurfaceHandler() = default; /// Create parameters of the contact constraint. This method should combine @@ -152,7 +152,7 @@ class DefaultContactSurfaceHandler : public ContactSurfaceHandler { public: virtual ~DefaultContactSurfaceHandler() = default; - + // Documentation inherited ContactSurfaceParams createParams( const collision::Contact& contact, diff --git a/dart/dynamics/VoxelGridShape.cpp b/dart/dynamics/VoxelGridShape.cpp index 2ed39ac20a731..9537411f1a80c 100644 --- a/dart/dynamics/VoxelGridShape.cpp +++ b/dart/dynamics/VoxelGridShape.cpp @@ -79,19 +79,20 @@ octomap::pose6d toPose6d(const Eigen::Isometry3d& frame) //============================================================================== VoxelGridShape::VoxelGridShape(double resolution) : Shape() { - setOctree(fcl_make_shared(resolution)); + setOctree(std::make_shared(resolution)); mVariance = DYNAMIC_ELEMENTS; } //============================================================================== -VoxelGridShape::VoxelGridShape(fcl_shared_ptr octree) : Shape() +VoxelGridShape::VoxelGridShape(std::shared_ptr octree) + : Shape() { if (!octree) { dtwarn << "[VoxelGridShape] Attempting to assign null octree. Creating an " << "empty octree with resolution 0.01 instead.\n"; - setOctree(fcl_make_shared(0.01)); + setOctree(std::make_shared(0.01)); return; } @@ -112,7 +113,7 @@ const std::string& VoxelGridShape::getStaticType() } //============================================================================== -void VoxelGridShape::setOctree(fcl_shared_ptr octree) +void VoxelGridShape::setOctree(std::shared_ptr octree) { if (!octree) { @@ -134,13 +135,13 @@ void VoxelGridShape::setOctree(fcl_shared_ptr octree) } //============================================================================== -fcl_shared_ptr VoxelGridShape::getOctree() +std::shared_ptr VoxelGridShape::getOctree() { return mOctree; } //============================================================================== -fcl_shared_ptr VoxelGridShape::getOctree() const +std::shared_ptr VoxelGridShape::getOctree() const { return mOctree; } @@ -220,7 +221,7 @@ void VoxelGridShape::notifyColorUpdated(const Eigen::Vector4d& /*color*/) ShapePtr VoxelGridShape::clone() const { return std::make_shared( - fcl_make_shared(*mOctree)); + std::make_shared(*mOctree)); } //============================================================================== diff --git a/dart/dynamics/VoxelGridShape.hpp b/dart/dynamics/VoxelGridShape.hpp index 0cdf85f437f36..7e4d1ba8b4e8e 100644 --- a/dart/dynamics/VoxelGridShape.hpp +++ b/dart/dynamics/VoxelGridShape.hpp @@ -56,7 +56,7 @@ class VoxelGridShape : public Shape /// Constructs from a octomap::OcTree. /// \param[in] octree Octree. - explicit VoxelGridShape(fcl_shared_ptr octree); + explicit VoxelGridShape(std::shared_ptr octree); /// Destructor. ~VoxelGridShape() override = default; @@ -68,13 +68,13 @@ class VoxelGridShape : public Shape static const std::string& getStaticType(); /// Sets octree. - void setOctree(fcl_shared_ptr octree); + void setOctree(std::shared_ptr octree); /// Returns octree. - fcl_shared_ptr getOctree(); + std::shared_ptr getOctree(); /// Returns octree. - fcl_shared_ptr getOctree() const; + std::shared_ptr getOctree() const; /// Updates the occupancy probability of the voxels where \c point is located. /// @@ -158,7 +158,7 @@ class VoxelGridShape : public Shape void updateVolume() const override; /// Octree. - fcl_shared_ptr mOctree; + std::shared_ptr mOctree; // TODO(JS): Use std::shared_ptr once we drop supporting FCL (< 0.5) }; diff --git a/unittests/integration/test_ConstraintSolver.cpp b/unittests/integration/test_ConstraintSolver.cpp index edbf43155a0f3..e2ed11a19cf26 100644 --- a/unittests/integration/test_ConstraintSolver.cpp +++ b/unittests/integration/test_ConstraintSolver.cpp @@ -71,43 +71,44 @@ TEST(ConstraintSolver, CustomConstactSurfaceHandler) return params; } }; - + auto world = createWorld(); auto solver = world->getConstraintSolver(); auto defaultHandler = solver->getLastContactSurfaceHandler(); EXPECT_EQ(nullptr, defaultHandler->getParent()); - + auto customHandler = std::make_shared(); solver->addContactSurfaceHandler(customHandler); - + ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler()); EXPECT_EQ(nullptr, defaultHandler->getParent()); EXPECT_EQ(defaultHandler, customHandler->getParent()); - + // try to remove nonexisting handler - EXPECT_FALSE(solver->removeContactSurfaceHandler(std::make_shared())); + EXPECT_FALSE( + solver->removeContactSurfaceHandler(std::make_shared())); EXPECT_TRUE(solver->removeContactSurfaceHandler(defaultHandler)); EXPECT_EQ(nullptr, customHandler->getParent()); EXPECT_EQ(customHandler, solver->getLastContactSurfaceHandler()); - + // removing last handler should not be done, but we test it anyways // a printed error message is expected EXPECT_TRUE(solver->removeContactSurfaceHandler(customHandler)); EXPECT_EQ(nullptr, customHandler->getParent()); EXPECT_EQ(nullptr, solver->getLastContactSurfaceHandler()); - + solver->addContactSurfaceHandler(defaultHandler); ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler()); EXPECT_EQ(defaultHandler, solver->getLastContactSurfaceHandler()); - + solver->addContactSurfaceHandler(customHandler); ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler()); EXPECT_EQ(customHandler, solver->getLastContactSurfaceHandler()); EXPECT_EQ(nullptr, defaultHandler->getParent()); EXPECT_EQ(defaultHandler, customHandler->getParent()); - + auto customHandler2 = std::make_shared(); auto customHandler3 = std::make_shared(); solver->addContactSurfaceHandler(customHandler2); @@ -118,7 +119,7 @@ TEST(ConstraintSolver, CustomConstactSurfaceHandler) EXPECT_EQ(defaultHandler, customHandler->getParent()); EXPECT_EQ(customHandler, customHandler2->getParent()); EXPECT_EQ(customHandler2, customHandler3->getParent()); - + EXPECT_TRUE(solver->removeContactSurfaceHandler(customHandler)); ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler()); EXPECT_EQ(customHandler3, solver->getLastContactSurfaceHandler()); @@ -126,7 +127,7 @@ TEST(ConstraintSolver, CustomConstactSurfaceHandler) EXPECT_EQ(defaultHandler, customHandler->getParent()); EXPECT_EQ(defaultHandler, customHandler2->getParent()); EXPECT_EQ(customHandler2, customHandler3->getParent()); - + EXPECT_TRUE(solver->removeContactSurfaceHandler(customHandler3)); ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler()); EXPECT_EQ(customHandler2, solver->getLastContactSurfaceHandler()); @@ -134,7 +135,7 @@ TEST(ConstraintSolver, CustomConstactSurfaceHandler) EXPECT_EQ(defaultHandler, customHandler->getParent()); EXPECT_EQ(defaultHandler, customHandler2->getParent()); EXPECT_EQ(customHandler2, customHandler3->getParent()); - + // after we break the chain at handler 2, default handler is no longer // reachable customHandler2->setParent(nullptr); @@ -160,43 +161,43 @@ TEST(ConstraintSolver, ConstactSurfaceHandlerIsCalled) contact, numContactsOnCollisionObject); mCalled = true; params.mPrimaryFrictionCoeff = mValue; - + return params; } mutable bool mCalled{false}; int mValue{0}; }; - + auto world = createWorld(); auto solver = world->getConstraintSolver(); auto defaultHandler = solver->getLastContactSurfaceHandler(); EXPECT_EQ(nullptr, defaultHandler->getParent()); - + auto customHandler = std::make_shared(1); solver->addContactSurfaceHandler(customHandler); solver->removeContactSurfaceHandler(defaultHandler); - + customHandler->mCalled = false; auto params = solver->getLastContactSurfaceHandler()->createParams({}, 0); EXPECT_TRUE(customHandler->mCalled); EXPECT_EQ(1, params.mPrimaryFrictionCoeff); - + auto customHandler2 = std::make_shared(2); solver->addContactSurfaceHandler(customHandler2); - + customHandler->mCalled = customHandler2->mCalled = false; params = solver->getLastContactSurfaceHandler()->createParams({}, 0); EXPECT_TRUE(customHandler->mCalled); EXPECT_TRUE(customHandler2->mCalled); EXPECT_EQ(2, params.mPrimaryFrictionCoeff); - + // Try once more adding the same handler instance; this should be ignored. // If it were added, the createParams() call could get into an infinite loop // calling the last handler as its parent, so rather check for it. solver->addContactSurfaceHandler(customHandler2); - + customHandler->mCalled = customHandler2->mCalled = false; params = solver->getLastContactSurfaceHandler()->createParams({}, 0); EXPECT_TRUE(customHandler->mCalled); @@ -222,36 +223,35 @@ TEST(ConstraintSolver, ConstactSurfaceHandlerIgnoreParent) auto params = constraint::ContactSurfaceParams{}; mCalled = true; params.mPrimaryFrictionCoeff = mValue; - + return params; } mutable bool mCalled{false}; int mValue{0}; }; - + auto world = createWorld(); auto solver = world->getConstraintSolver(); auto defaultHandler = solver->getLastContactSurfaceHandler(); EXPECT_EQ(nullptr, defaultHandler->getParent()); - + auto customHandler = std::make_shared(1); solver->addContactSurfaceHandler(customHandler); solver->removeContactSurfaceHandler(defaultHandler); - + customHandler->mCalled = false; auto params = solver->getLastContactSurfaceHandler()->createParams({}, 0); EXPECT_TRUE(customHandler->mCalled); EXPECT_EQ(1, params.mPrimaryFrictionCoeff); - + auto customHandler2 = std::make_shared(2); solver->addContactSurfaceHandler(customHandler2); - + customHandler->mCalled = customHandler2->mCalled = false; params = solver->getLastContactSurfaceHandler()->createParams({}, 0); EXPECT_FALSE(customHandler->mCalled); EXPECT_TRUE(customHandler2->mCalled); EXPECT_EQ(2, params.mPrimaryFrictionCoeff); } -