From f9e7e160285cfc1943ad5189cf2d98dbe288a73f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 21 Dec 2024 01:51:15 +0800 Subject: [PATCH] Support setting max contacts in dart's bullet collision detector (#593) Signed-off-by: Ian Chen (cherry picked from commit a4eee2fdbf4cb75230871ef35cdeea9b1dd91955) # Conflicts: # dartsim/src/GzCollisionDetector.cc --- dartsim/src/EntityManagementFeatures.cc | 2 +- ...sionDetector.cc => GzCollisionDetector.cc} | 101 +++++++++++++++++- ...sionDetector.hh => GzCollisionDetector.hh} | 86 +++++++++++---- dartsim/src/WorldFeatures.cc | 28 +++-- test/common_test/simulation_features.cc | 15 +++ 5 files changed, 193 insertions(+), 39 deletions(-) rename dartsim/src/{GzOdeCollisionDetector.cc => GzCollisionDetector.cc} (58%) rename dartsim/src/{GzOdeCollisionDetector.hh => GzCollisionDetector.hh} (58%) diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 990ca5631..59fafe22b 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -28,7 +28,7 @@ #include #include -#include "GzOdeCollisionDetector.hh" +#include "GzCollisionDetector.hh" namespace gz { namespace physics { diff --git a/dartsim/src/GzOdeCollisionDetector.cc b/dartsim/src/GzCollisionDetector.cc similarity index 58% rename from dartsim/src/GzOdeCollisionDetector.cc rename to dartsim/src/GzCollisionDetector.cc index 13e96e980..80be800c3 100644 --- a/dartsim/src/GzOdeCollisionDetector.cc +++ b/dartsim/src/GzCollisionDetector.cc @@ -22,18 +22,18 @@ #include -#include "GzOdeCollisionDetector.hh" +#include "GzCollisionDetector.hh" using namespace dart; using namespace collision; ///////////////////////////////////////////////// -GzOdeCollisionDetector::GzOdeCollisionDetector() - : OdeCollisionDetector() +GzCollisionDetector::GzCollisionDetector() { } ///////////////////////////////////////////////// +<<<<<<< HEAD:dartsim/src/GzOdeCollisionDetector.cc GzOdeCollisionDetector::Registrar GzOdeCollisionDetector::mRegistrar{ GzOdeCollisionDetector::getStaticType(), @@ -79,19 +79,22 @@ bool GzOdeCollisionDetector::collide( ///////////////////////////////////////////////// void GzOdeCollisionDetector::SetCollisionPairMaxContacts( +======= +void GzCollisionDetector::SetCollisionPairMaxContacts( +>>>>>>> a4eee2f (Support setting max contacts in dart's bullet collision detector (#593)):dartsim/src/GzCollisionDetector.cc std::size_t _maxContacts) { this->maxCollisionPairContacts = _maxContacts; } ///////////////////////////////////////////////// -std::size_t GzOdeCollisionDetector::GetCollisionPairMaxContacts() const +std::size_t GzCollisionDetector::GetCollisionPairMaxContacts() const { return this->maxCollisionPairContacts; } ///////////////////////////////////////////////// -void GzOdeCollisionDetector::LimitCollisionPairMaxContacts( +void GzCollisionDetector::LimitCollisionPairMaxContacts( CollisionResult *_result) { if (this->maxCollisionPairContacts == @@ -143,3 +146,91 @@ void GzOdeCollisionDetector::LimitCollisionPairMaxContacts( } } } + +///////////////////////////////////////////////// +GzOdeCollisionDetector::GzOdeCollisionDetector() + : OdeCollisionDetector(), GzCollisionDetector() +{ +} + +///////////////////////////////////////////////// +GzOdeCollisionDetector::Registrar + GzOdeCollisionDetector::mRegistrar{ + GzOdeCollisionDetector::getStaticType(), + []() -> std::shared_ptr { + return GzOdeCollisionDetector::create(); + }}; + +///////////////////////////////////////////////// +std::shared_ptr GzOdeCollisionDetector::create() +{ + return std::shared_ptr(new GzOdeCollisionDetector()); +} + +///////////////////////////////////////////////// +bool GzOdeCollisionDetector::collide( + CollisionGroup *_group, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = OdeCollisionDetector::collide(_group, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +bool GzOdeCollisionDetector::collide( + CollisionGroup *_group1, + CollisionGroup *_group2, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +GzBulletCollisionDetector::GzBulletCollisionDetector() + : BulletCollisionDetector(), GzCollisionDetector() +{ +} + +///////////////////////////////////////////////// +GzBulletCollisionDetector::Registrar + GzBulletCollisionDetector::mRegistrar{ + GzBulletCollisionDetector::getStaticType(), + []() -> std::shared_ptr { + return GzBulletCollisionDetector::create(); + }}; + +///////////////////////////////////////////////// +std::shared_ptr GzBulletCollisionDetector::create() +{ + return std::shared_ptr( + new GzBulletCollisionDetector()); +} + +///////////////////////////////////////////////// +bool GzBulletCollisionDetector::collide( + CollisionGroup *_group, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = BulletCollisionDetector::collide(_group, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} + +///////////////////////////////////////////////// +bool GzBulletCollisionDetector::collide( + CollisionGroup *_group1, + CollisionGroup *_group2, + const CollisionOption &_option, + CollisionResult *_result) +{ + bool ret = BulletCollisionDetector::collide( + _group1, _group2, _option, _result); + this->LimitCollisionPairMaxContacts(_result); + return ret; +} diff --git a/dartsim/src/GzOdeCollisionDetector.hh b/dartsim/src/GzCollisionDetector.hh similarity index 58% rename from dartsim/src/GzOdeCollisionDetector.hh rename to dartsim/src/GzCollisionDetector.hh index de8233e93..f5c502c03 100644 --- a/dartsim/src/GzOdeCollisionDetector.hh +++ b/dartsim/src/GzCollisionDetector.hh @@ -15,15 +15,51 @@ * */ +#ifndef GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_ +#define GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_ + +#include #include #include +#include +#include #include namespace dart { namespace collision { -class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector +class GzCollisionDetector +{ + /// \brief Set the maximum number of contacts between a pair of collision + /// objects + /// \param[in] _maxContacts Maximum number of contacts between a pair of + /// collision objects. + public: virtual void SetCollisionPairMaxContacts(std::size_t _maxContacts); + + /// \brief Get the maximum number of contacts between a pair of collision + /// objects + /// \return Maximum number of contacts between a pair of collision objects. + public: virtual std::size_t GetCollisionPairMaxContacts() const; + + /// Constructor + protected: GzCollisionDetector(); + + /// \brief Limit max number of contacts between a pair of collision objects. + /// The function modifies the contacts vector inside the CollisionResult + /// object to cap the number of contacts for each collision pair based on the + /// maxCollisionPairContacts value + protected: virtual void LimitCollisionPairMaxContacts( + CollisionResult *_result); + + /// \brief Maximum number of contacts between a pair of collision objects. + protected: std::size_t maxCollisionPairContacts = + std::numeric_limits::max(); +}; + +class GzOdeCollisionDetector : + public dart::collision::OdeCollisionDetector, + public dart::collision::GzCollisionDetector { // Documentation inherited public: bool collide( @@ -38,36 +74,42 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; - /// \brief Set the maximum number of contacts between a pair of collision - /// objects - /// \param[in] _maxContacts Maximum number of contacts between a pair of - /// collision objects. - public: void SetCollisionPairMaxContacts(std::size_t _maxContacts); - - /// \brief Get the maximum number of contacts between a pair of collision - /// objects - /// \return Maximum number of contacts between a pair of collision objects. - public: std::size_t GetCollisionPairMaxContacts() const; - - /// \brief Create the GzOdeCollisionDetector public: static std::shared_ptr create(); /// Constructor protected: GzOdeCollisionDetector(); - /// \brief Limit max number of contacts between a pair of collision objects. - /// The function modifies the contacts vector inside the CollisionResult - /// object to cap the number of contacts for each collision pair based on the - /// maxCollisionPairContacts value - private: void LimitCollisionPairMaxContacts(CollisionResult *_result); + private: static Registrar mRegistrar; +}; - /// \brief Maximum number of contacts between a pair of collision objects. - private: std::size_t maxCollisionPairContacts = - std::numeric_limits::max(); +class GzBulletCollisionDetector : + public dart::collision::BulletCollisionDetector, + public dart::collision::GzCollisionDetector +{ + // Documentation inherited + public: bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; - private: static Registrar mRegistrar; + // Documentation inherited + public: bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; + + /// \brief Create the GzBulletCollisionDetector + public: static std::shared_ptr create(); + + /// Constructor + protected: GzBulletCollisionDetector(); + + private: static Registrar mRegistrar; }; } } + +#endif diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index da7088580..c55fd1e55 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -29,7 +29,7 @@ #include -#include "GzOdeCollisionDetector.hh" +#include "GzCollisionDetector.hh" #include "WorldFeatures.hh" @@ -46,7 +46,7 @@ void WorldFeatures::SetWorldCollisionDetector( world->getConstraintSolver()->getCollisionDetector(); if (_collisionDetector == "bullet") { - collisionDetector = dart::collision::BulletCollisionDetector::create(); + collisionDetector = dart::collision::GzBulletCollisionDetector::create(); } else if (_collisionDetector == "fcl") { @@ -105,17 +105,17 @@ void WorldFeatures::SetWorldCollisionPairMaxContacts( auto collisionDetector = world->getConstraintSolver()->getCollisionDetector(); - auto odeCollisionDetector = - std::dynamic_pointer_cast( + auto gzCollisionDetector = + std::dynamic_pointer_cast( collisionDetector); - if (odeCollisionDetector) + if (gzCollisionDetector) { - odeCollisionDetector->SetCollisionPairMaxContacts(_maxContacts); + gzCollisionDetector->SetCollisionPairMaxContacts(_maxContacts); } else { gzwarn << "Currently max contacts feature is only supported by the " - << "ode collision detector in dartsim." << std::endl; + << "bullet and ode collision detector in dartsim." << std::endl; } } @@ -126,12 +126,18 @@ std::size_t WorldFeatures::GetWorldCollisionPairMaxContacts( auto world = this->ReferenceInterface(_id); auto collisionDetector = world->getConstraintSolver()->getCollisionDetector(); - auto odeCollisionDetector = - std::dynamic_pointer_cast( + + auto gzCollisionDetector = + std::dynamic_pointer_cast( collisionDetector); - if (odeCollisionDetector) + if (gzCollisionDetector) { - return odeCollisionDetector->GetCollisionPairMaxContacts(); + return gzCollisionDetector->GetCollisionPairMaxContacts(); + } + else + { + gzwarn << "Currently max contacts feature is only supported by the " + << "bullet and ode collision detector in dartsim." << std::endl; } return 0u; diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index d5537f1cd..17d629874 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -228,6 +228,7 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts) // The features that an engine must have to be loaded by this loader. struct FeaturesCollisionPairMaxContacts : gz::physics::FeatureList< gz::physics::sdf::ConstructSdfWorld, + gz::physics::CollisionDetector, gz::physics::CollisionPairMaxContacts, gz::physics::FindFreeGroupFeature, gz::physics::ForwardStep, @@ -282,6 +283,20 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest, contacts = world->GetContactsFromLastStep(); EXPECT_EQ(0u, contacts.size()); + + if (name == "gz::physics::dartsim::Plugin") + { + EXPECT_EQ("ode", world->GetCollisionDetector()); + world->SetCollisionDetector("bullet"); + EXPECT_EQ("bullet", world->GetCollisionDetector()); + world->SetCollisionPairMaxContacts(1u); + EXPECT_EQ(1u, world->GetCollisionPairMaxContacts()); + checkedOutput = StepWorld( + world, true, 1).first; + EXPECT_TRUE(checkedOutput); + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(5u, contacts.size()); + } } }