Support setting max contacts in dart's bullet collision detector (#593)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
(cherry picked from commit a4eee2f)

# Conflicts:
#	dartsim/src/GzCollisionDetector.cc

Author: iche033

dartsim/src/EntityManagementFeatures.cc

@@ -28,7 +28,7 @@
 #include <dart/collision/CollisionFilter.hpp>
 #include <dart/collision/CollisionObject.hpp>
 
-#include "GzOdeCollisionDetector.hh"
+#include "GzCollisionDetector.hh"
 
 namespace gz {
 namespace physics {

dartsim/src/GzOdeCollisionDetector.cc dartsim/src/GzCollisionDetector.cc

@@ -22,18 +22,18 @@
 
 #include <dart/collision/CollisionObject.hpp>
 
-#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>
     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>
+    GzOdeCollisionDetector::mRegistrar{
+        GzOdeCollisionDetector::getStaticType(),
+        []() -> std::shared_ptr<GzOdeCollisionDetector> {
+          return GzOdeCollisionDetector::create();
+        }};
+
+/////////////////////////////////////////////////
+std::shared_ptr<GzOdeCollisionDetector> GzOdeCollisionDetector::create()
+{
+  return std::shared_ptr<GzOdeCollisionDetector>(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>
+    GzBulletCollisionDetector::mRegistrar{
+        GzBulletCollisionDetector::getStaticType(),
+        []() -> std::shared_ptr<GzBulletCollisionDetector> {
+          return GzBulletCollisionDetector::create();
+        }};
+
+/////////////////////////////////////////////////
+std::shared_ptr<GzBulletCollisionDetector> GzBulletCollisionDetector::create()
+{
+  return std::shared_ptr<GzBulletCollisionDetector>(
+      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;
+}

dartsim/src/GzOdeCollisionDetector.hh dartsim/src/GzCollisionDetector.hh

@@ -15,15 +15,51 @@
  *
 */
 
+#ifndef GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_
+#define GZ_PHYSICS_DARTSIM_SRC_GZCOLLISIONDETECTOR_HH_
+
+#include <cstdio>
 #include <limits>
 #include <memory>
 
+#include <dart/collision/CollisionResult.hpp>
+#include <dart/collision/bullet/BulletCollisionDetector.hpp>
 #include <dart/collision/ode/OdeCollisionDetector.hpp>
 
 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<std::size_t>::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<GzOdeCollisionDetector> 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<GzOdeCollisionDetector> mRegistrar;
+};
 
-  /// \brief Maximum number of contacts between a pair of collision objects.
-  private: std::size_t maxCollisionPairContacts =
-      std::numeric_limits<std::size_t>::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<GzOdeCollisionDetector> 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<GzBulletCollisionDetector> create();
+
+  /// Constructor
+  protected: GzBulletCollisionDetector();
+
+  private: static Registrar<GzBulletCollisionDetector> mRegistrar;
 };
 
 }
 }
+
+#endif

dartsim/src/WorldFeatures.cc

@@ -29,7 +29,7 @@
 
 #include <gz/common/Console.hh>
 
-#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<dart::collision::GzOdeCollisionDetector>(
+ auto gzCollisionDetector =
+    std::dynamic_pointer_cast<dart::collision::GzCollisionDetector>(
     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<dart::simulation::World>(_id);
   auto collisionDetector =
     world->getConstraintSolver()->getCollisionDetector();
-  auto odeCollisionDetector =
-    std::dynamic_pointer_cast<dart::collision::GzOdeCollisionDetector>(
+
+ auto gzCollisionDetector =
+    std::dynamic_pointer_cast<dart::collision::GzCollisionDetector>(
     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;

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<FeaturesCollisionPairMaxContacts>(
+          world, true, 1).first;
+      EXPECT_TRUE(checkedOutput);
+      contacts = world->GetContactsFromLastStep();
+      EXPECT_EQ(5u, contacts.size());
+    }
   }
 }