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

dartsim/src/GzOdeCollisionDetector.cc

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <unordered_map>
+#include <utility>
 
 #include <dart/collision/CollisionObject.hpp>
 
@@ -92,21 +93,45 @@ void GzOdeCollisionDetector::LimitCollisionPairMaxContacts(
   auto allContacts = _result->getContacts();
   _result->clear();
 
+
+  if (this->maxCollisionPairContacts == 0u)
+    return;
+
+  // A map of collision pairs and 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::size_t>>
+      std::unordered_map<dart::collision::CollisionObject *,
+      std::pair<std::size_t, std::size_t>>>
       contactMap;
 
   for (auto &contact : allContacts)
   {
-    auto &count =
-      contactMap[contact.collisionObject1][contact.collisionObject2];
+    auto &[count, lastContactIdx] =
+        contactMap[contact.collisionObject1][contact.collisionObject2];
     count++;
-    auto &otherCount =
+    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;
+      }
+    }
   }
 }

test/common_test/Worlds.hh

@@ -29,6 +29,8 @@ inline std::string CommonTestWorld(const std::string &_world)
 
 namespace common_test::worlds
 {
+const auto kCollisionPairContactPointSdf =
+  CommonTestWorld("collision_pair_contact_point.sdf");
 const auto kContactSdf = CommonTestWorld("contact.sdf");
 const auto kDetachableJointWorld = CommonTestWorld("detachable_joint.world");
 const auto kEmptySdf = CommonTestWorld("empty.sdf");

test/common_test/simulation_features.cc

@@ -223,9 +223,13 @@ 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::GetContactsFromLastStepFeature,
+  gz::physics::CollisionPairMaxContacts,
+  gz::physics::FindFreeGroupFeature,
   gz::physics::ForwardStep,
-  gz::physics::CollisionPairMaxContacts
+  gz::physics::FreeGroupFrameSemantics,
+  gz::physics::GetContactsFromLastStepFeature,
+  gz::physics::GetModelFromWorld,
+  gz::physics::SetFreeGroupWorldPose
 > {};
 
 template <class T>
@@ -276,6 +280,92 @@ TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest,
   }
 }
 
+/////////////////////////////////////////////////
+TYPED_TEST(SimulationFeaturesCollisionPairMaxContactsTest,
+    CollisionPairMaxContactsSelection)
+{
+  for (const std::string &name : this->pluginNames)
+  {
+    auto world = LoadPluginAndWorld<FeaturesCollisionPairMaxContacts>(
+      this->loader,
+      name,
+      common_test::worlds::kCollisionPairContactPointSdf);
+    auto checkedOutput = StepWorld<FeaturesCollisionPairMaxContacts>(
+        world, true, 1).first;
+    EXPECT_TRUE(checkedOutput);
+
+    // Verify initial pose
+    const gz::math::Pose3d initialPose = gz::math::Pose3d::Zero;
+    auto ellipsoid = world->GetModel("ellipsoid");
+    ASSERT_NE(nullptr, ellipsoid);
+    auto ellipsoidFreeGroup = ellipsoid->FindFreeGroup();
+    ASSERT_NE(nullptr, ellipsoidFreeGroup);
+    auto box = world->GetModel("box");
+    ASSERT_NE(nullptr, box);
+    auto boxFreeGroup = box->FindFreeGroup();
+    ASSERT_NE(nullptr, boxFreeGroup);
+    auto ellipsoidFrameData = ellipsoidFreeGroup->FrameDataRelativeToWorld();
+    auto boxFrameData = boxFreeGroup->FrameDataRelativeToWorld();
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(ellipsoidFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(boxFrameData.pose));
+
+    // Get all contacts between box and ellipsoid
+    auto contacts = world->GetContactsFromLastStep();
+    EXPECT_EQ(std::numeric_limits<std::size_t>::max(),
+              world->GetCollisionPairMaxContacts());
+    EXPECT_GT(contacts.size(), 30u);
+
+    // Find contact point with max penetration depth
+    double maxDepth = 0;
+    for (const auto &contact : contacts)
+    {
+      const auto* extraContactData =
+          contact.template Query<
+          gz::physics::World3d<
+          FeaturesCollisionPairMaxContacts>::ExtraContactData>();
+      ASSERT_NE(nullptr, extraContactData);
+      if (extraContactData->depth > maxDepth)
+        maxDepth = extraContactData->depth;
+    }
+    EXPECT_GT(maxDepth, 0.0);
+
+    // Reset pose back to initial pose
+    ellipsoidFreeGroup->SetWorldPose(
+      gz::math::eigen3::convert(initialPose));
+    boxFreeGroup->SetWorldPose(
+      gz::math::eigen3::convert(initialPose));
+    ellipsoidFrameData = ellipsoidFreeGroup->FrameDataRelativeToWorld();
+    boxFrameData = boxFreeGroup->FrameDataRelativeToWorld();
+
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(ellipsoidFrameData.pose));
+    EXPECT_EQ(initialPose,
+              gz::math::eigen3::convert(boxFrameData.pose));
+
+    // Set max contact between collision pairs to be 1
+    world->SetCollisionPairMaxContacts(1u);
+    EXPECT_EQ(1u, world->GetCollisionPairMaxContacts());
+    checkedOutput = StepWorld<FeaturesCollisionPairMaxContacts>(
+        world, true, 1).first;
+    EXPECT_TRUE(checkedOutput);
+
+    contacts = world->GetContactsFromLastStep();
+    EXPECT_EQ(1u, contacts.size());
+
+    // Verify that the physics engine picked the contact with max penetration
+    // depth
+    auto contact = contacts[0];
+    const auto* extraContactData =
+        contact.template Query<
+        gz::physics::World3d<
+        FeaturesCollisionPairMaxContacts>::ExtraContactData>();
+    ASSERT_NE(nullptr, extraContactData);
+    EXPECT_FLOAT_EQ(maxDepth, extraContactData->depth);
+  }
+}
+
 // The features that an engine must have to be loaded by this loader.
 struct FeaturesStep : gz::physics::FeatureList<
   gz::physics::sdf::ConstructSdfWorld,

test/common_test/worlds/collision_pair_contact_point.sdf

@@ -0,0 +1,68 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+
+   <model name="box">
+      <static>true</static>
+      <pose>0 0 0.0 0 0 0</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>100 100 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>100 100 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
+    <model name="ellipsoid">
+      <pose>0 -0 0.0 0 0 0</pose>
+      <link name="ellipsoid_link">
+        <inertial>
+          <inertia>
+            <ixx>0.068</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.058</iyy>
+            <iyz>0</iyz>
+            <izz>0.026</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="ellipsoid_collision">
+          <geometry>
+            <ellipsoid>
+              <radii>0.2 0.3 0.5</radii>
+            </ellipsoid>
+          </geometry>
+        </collision>
+        <visual name="ellipsoid_visual">
+          <geometry>
+            <ellipsoid>
+              <radii>0.2 0.3 0.5</radii>
+            </ellipsoid>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>