Minor optimization for picking contact points in dartsim's ODE collision detector

dartsim/src/EntityManagementFeatures.cc

@@ -22,13 +22,14 @@
 #include <unordered_map>
 
 #include <dart/config.hpp>
-#include <dart/collision/ode/OdeCollisionDetector.hpp>
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
 
 #include <dart/collision/CollisionFilter.hpp>
 #include <dart/collision/CollisionObject.hpp>
 
+#include "GzOdeCollisionDetector.hh"
+
 namespace gz {
 namespace physics {
 namespace dartsim {
@@ -724,11 +725,12 @@ Identity EntityManagementFeatures::ConstructEmptyWorld(
     const Identity &/*_engineID*/, const std::string &_name)
 {
   const auto &world = std::make_shared<dart::simulation::World>(_name);
-  world->getConstraintSolver()->setCollisionDetector(
-        dart::collision::OdeCollisionDetector::create());
+  auto collisionDetector = dart::collision::GzOdeCollisionDetector::create();
+  world->getConstraintSolver()->setCollisionDetector(collisionDetector);
 
-  // TODO(anyone) We need a machanism to configure maxNumContacts at runtime.
   auto &collOpt = world->getConstraintSolver()->getCollisionOption();
+  // Set the max number of contacts for all collision objects
+  // in the world
   collOpt.maxNumContacts = 10000;
 
   world->getConstraintSolver()->getCollisionOption().collisionFilter =

dartsim/src/GzOdeCollisionDetector.cc

@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+
+#include <dart/collision/CollisionObject.hpp>
+
+#include "GzOdeCollisionDetector.hh"
+
+using namespace dart;
+using namespace collision;
+
+/////////////////////////////////////////////////
+GzOdeCollisionDetector::GzOdeCollisionDetector()
+  : OdeCollisionDetector()
+{
+}
+
+/////////////////////////////////////////////////
+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;
+}
+
+/////////////////////////////////////////////////
+void GzOdeCollisionDetector::SetCollisionPairMaxContacts(
+    std::size_t _maxContacts)
+{
+  this->maxCollisionPairContacts = _maxContacts;
+}
+
+/////////////////////////////////////////////////
+std::size_t GzOdeCollisionDetector::GetCollisionPairMaxContacts() const
+{
+  return this->maxCollisionPairContacts;
+}
+
+/////////////////////////////////////////////////
+void GzOdeCollisionDetector::LimitCollisionPairMaxContacts(
+    CollisionResult *_result)
+{
+  if (this->maxCollisionPairContacts ==
+    std::numeric_limits<std::size_t>::max())
+    return;
+
+  auto allContacts = _result->getContacts();
+  _result->clear();
+
+
+  if (this->maxCollisionPairContacts == 0u)
+    return;
+
+  // A map of collision pairs and their their contact info
+  // Contact info is stored in std::pair. The elements are:
+  // <contact count, index of last contact point (in _result)>
+  std::unordered_map<dart::collision::CollisionObject *,
+      std::unordered_map<dart::collision::CollisionObject *,
+      std::pair<std::size_t, std::size_t>>>
+      contactMap;
+
+  for (auto &contact : allContacts)
+  {
+    auto &[count, lastContactIdx] =
+        contactMap[contact.collisionObject1][contact.collisionObject2];
+    count++;
+    auto &[otherCount, otherLastContactIdx] =
+      contactMap[contact.collisionObject2][contact.collisionObject1];
+
+    std::size_t total =  count + otherCount;
+    if (total <= this->maxCollisionPairContacts)
+    {
+      if (total == this->maxCollisionPairContacts)
+      {
+        lastContactIdx = _result->getNumContacts();
+        otherLastContactIdx = lastContactIdx;
+      }
+      _result->addContact(contact);
+    }
+    else
+    {
+      // If too many contacts were generated, replace the last contact point
+      // of the collision pair with one that has a larger penetration depth
+      auto &c = _result->getContact(lastContactIdx);
+      if (contact.penetrationDepth > c.penetrationDepth)
+      {
+        c = contact;
+      }
+    }
+  }
+}

dartsim/src/GzOdeCollisionDetector.hh

@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <limits>
+#include <memory>
+
+#include <dart/collision/ode/OdeCollisionDetector.hpp>
+
+namespace dart {
+namespace collision {
+
+class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector
+{
+  // Documentation inherited
+  public: bool collide(
+      CollisionGroup* group,
+      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
+      CollisionResult* result = nullptr) override;
+
+  // Documentation inherited
+  public: bool collide(
+      CollisionGroup* group1,
+      CollisionGroup* group2,
+      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);
+
+  /// \brief Maximum number of contacts between a pair of collision objects.
+  private: std::size_t maxCollisionPairContacts =
+      std::numeric_limits<std::size_t>::max();
+
+  private: static Registrar<GzOdeCollisionDetector> mRegistrar;
+};
+
+}
+}

dartsim/src/WorldFeatures.cc

@@ -21,7 +21,6 @@
 #include <dart/collision/bullet/BulletCollisionDetector.hpp>
 #include <dart/collision/dart/DARTCollisionDetector.hpp>
 #include <dart/collision/fcl/FCLCollisionDetector.hpp>
-#include <dart/collision/ode/OdeCollisionDetector.hpp>
 #include <dart/constraint/BoxedLcpConstraintSolver.hpp>
 #include <dart/constraint/ConstraintSolver.hpp>
 #include <dart/constraint/DantzigBoxedLcpSolver.hpp>
@@ -30,8 +29,10 @@
 
 #include <gz/common/Console.hh>
 
+#include "GzOdeCollisionDetector.hh"
 #include "WorldFeatures.hh"
 
+
 namespace gz {
 namespace physics {
 namespace dartsim {
@@ -53,7 +54,7 @@ void WorldFeatures::SetWorldCollisionDetector(
   }
   else if (_collisionDetector == "ode")
   {
-    collisionDetector = dart::collision::OdeCollisionDetector::create();
+    collisionDetector = dart::collision::GzOdeCollisionDetector::create();
   }
   else if (_collisionDetector == "dart")
   {
@@ -96,6 +97,46 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity(
   return world->getGravity();
 }
 
+/////////////////////////////////////////////////
+void WorldFeatures::SetWorldCollisionPairMaxContacts(
+    const Identity &_id, std::size_t _maxContacts)
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+  auto collisionDetector =
+    world->getConstraintSolver()->getCollisionDetector();
+
+  auto odeCollisionDetector =
+    std::dynamic_pointer_cast<dart::collision::GzOdeCollisionDetector>(
+    collisionDetector);
+  if (odeCollisionDetector)
+  {
+    odeCollisionDetector->SetCollisionPairMaxContacts(_maxContacts);
+  }
+  else
+  {
+    gzwarn << "Currently max contacts feature is only supported by the "
+           << "ode collision detector in dartsim." << std::endl;
+  }
+}
+
+/////////////////////////////////////////////////
+std::size_t WorldFeatures::GetWorldCollisionPairMaxContacts(
+    const Identity &_id) const
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+  auto collisionDetector =
+    world->getConstraintSolver()->getCollisionDetector();
+  auto odeCollisionDetector =
+    std::dynamic_pointer_cast<dart::collision::GzOdeCollisionDetector>(
+    collisionDetector);
+  if (odeCollisionDetector)
+  {
+    return odeCollisionDetector->GetCollisionPairMaxContacts();
+  }
+
+  return 0u;
+}
+
 /////////////////////////////////////////////////
 void WorldFeatures::SetWorldSolver(const Identity &_id,
     const std::string &_solver)

dartsim/src/WorldFeatures.hh

@@ -30,6 +30,7 @@ namespace dartsim {
 
 struct WorldFeatureList : FeatureList<
   CollisionDetector,
+  CollisionPairMaxContacts,
   Gravity,
   Solver
 > { };
@@ -53,6 +54,14 @@ class WorldFeatures :
   // Documentation inherited
   public: LinearVectorType GetWorldGravity(const Identity &_id) const override;
 
+  // Documentation inherited
+  public: void SetWorldCollisionPairMaxContacts(
+      const Identity &_id, std::size_t _maxContacts) override;
+
+  // Documentation inherited
+  public: std::size_t GetWorldCollisionPairMaxContacts(const Identity &_id)
+      const override;
+
   // Documentation inherited
   public: void SetWorldSolver(const Identity &_id, const std::string &_solver)
       override;

include/gz/physics/World.hh

@@ -130,6 +130,45 @@ namespace gz
       };
     };
 
+    /////////////////////////////////////////////////
+    class GZ_PHYSICS_VISIBLE CollisionPairMaxContacts:
+        public virtual Feature
+    {
+      /// \brief The World API for getting and setting max contacts.
+      public: template <typename PolicyT, typename FeaturesT>
+      class World : public virtual Feature::World<PolicyT, FeaturesT>
+      {
+        /// \brief Set the maximum number of contacts allowed between two
+        /// entities.
+        /// \param[in] _maxContacts Maximum number of contacts.
+        public: void SetCollisionPairMaxContacts(std::size_t _maxContacts);
+
+        /// \brief Set the maximum number of contacts allowed between two
+        /// entities.
+        /// \return Maximum number of contacts.
+        public: std::size_t GetCollisionPairMaxContacts() const;
+      };
+
+      /// \private The implementation API for getting and setting max contacts.
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        /// \brief Implementation API for setting the maximum number of
+        /// contacts between two entities.
+        /// \param[in] _id Identity of the world.
+        /// \param[in] _maxContacts Maximum number of contacts.
+        public: virtual void SetWorldCollisionPairMaxContacts(
+            const Identity &_id, std::size_t _maxContacts) = 0;
+
+        /// \brief Implementation API for getting the maximum number of
+        /// contacts between two entities.
+        /// \param[in] _id Identity of the world.
+        /// \param[in] _maxContacts Maximum number of contacts.
+        public: virtual std::size_t GetWorldCollisionPairMaxContacts(
+            const Identity &_id) const = 0;
+      };
+    };
+
     /////////////////////////////////////////////////
     class GZ_PHYSICS_VISIBLE Solver : public virtual Feature
     {