Accumulate contact points to prevent objects sinking when using ODE - Waiting for comments

dart/collision/ode/OdeCollisionDetector.cpp

@@ -33,6 +33,7 @@
 #include "dart/collision/ode/OdeCollisionDetector.hpp"
 
 #include "dart/collision/CollisionFilter.hpp"
+#include "dart/collision/Contact.hpp"
 #include "dart/collision/ode/OdeCollisionGroup.hpp"
 #include "dart/collision/ode/OdeCollisionObject.hpp"
 #include "dart/collision/ode/OdeTypes.hpp"
@@ -49,6 +50,10 @@
 
 #include <ode/ode.h>
 
+#include <deque>
+#include <unordered_map>
+#include <utility>
+
 namespace dart {
 namespace collision {
 
@@ -70,6 +75,37 @@ Contact convertContact(
     OdeCollisionObject* b2,
     const CollisionOption& option);
 
+using CollObjPair = std::pair<CollisionObject*, CollisionObject*>;
+
+struct ContactHistoryItem
+{
+  CollObjPair pair;
+  std::deque<Contact> history;
+  ContactHistoryItem() = delete;
+};
+
+// using ContactManifold = std::unordered_map<CollObjPair,
+// std::deque<Contact>,obj_pair_hash>;
+std::vector<ContactHistoryItem> pastContacts;
+
+CollObjPair MakeNewPair(CollisionObject* o1, CollisionObject* o2)
+{
+  return std::make_pair(std::min(o1, o2), std::max(o1, o2));
+}
+
+std::deque<Contact>& FindPairInHist(const CollObjPair& pair)
+{
+  for (auto& item : pastContacts) {
+    if (pair.first == item.pair.first && pair.second == item.pair.second) {
+      return item.history;
+    }
+  }
+  auto newItem = ContactHistoryItem{pair, std::deque<Contact>()};
+  pastContacts.push_back(newItem);
+
+  return pastContacts.back().history;
+}
+
 struct OdeCollisionCallbackData
 {
   dContactGeom* contactGeoms;
@@ -161,6 +197,21 @@ bool OdeCollisionDetector::collide(
 
   dSpaceCollide(odeGroup->getOdeSpaceId(), &data, CollisionCallback);
 
+  for (auto& past_contact : pastContacts) {
+    bool clear = true;
+    for (const auto& curr_result : result->getContacts()) {
+      auto current_pair = MakeNewPair(
+          curr_result.collisionObject1, curr_result.collisionObject2);
+      if (past_contact.pair == current_pair) {
+        clear = false;
+        break;
+      }
+    }
+    if (clear) {
+      past_contact.history.clear();
+    }
+  }
+
   return data.numContacts > 0;
 }
 
@@ -285,8 +336,9 @@ void CollisionCallback(void* data, dGeomID o1, dGeomID o2)
 
   cdData->numContacts += numc;
 
-  if (result)
+  if (result) {
     reportContacts(numc, odeResult, collObj1, collObj2, option, *result);
+  }
 }
 
 //==============================================================================
@@ -305,15 +357,53 @@ void reportContacts(
   // without the checkings of repeatidity and co-linearity.
   if (1u == option.maxNumContacts) {
     result.addContact(convertContact(contactGeoms[0], b1, b2, option));
-
     return;
   }
 
   for (auto i = 0; i < numContacts; ++i) {
     result.addContact(convertContact(contactGeoms[i], b1, b2, option));
+  }
+
+  auto missing = 3 - numContacts;
+  if (missing <= 0) {
+    return;
+  }
+
+  const auto pair = MakeNewPair(b1, b2);
+  auto& pastContacsVec = FindPairInHist(pair);
+  auto results_vec_copy = result.getContacts();
+
+  for (auto it = pastContacsVec.rbegin(); it != pastContacsVec.rend(); ++it) {
+    if (missing <= 0)
+      break;
+    auto past_cont = *it;
+    for (const auto& curr_cont : results_vec_copy) {
+      const auto res_pair
+          = MakeNewPair(curr_cont.collisionObject1, curr_cont.collisionObject2);
+      if (res_pair != pair) {
+        continue;
+      }
+      auto dist_v = past_cont.point - curr_cont.point;
+      const auto dist_m = (dist_v.transpose() * dist_v).coeff(0, 0);
+      if (dist_m < 0.01) {
+        continue;
+      } else {
+        --missing;
+        result.addContact(past_cont);
+      }
+    }
+  }
+  for (const auto& item : results_vec_copy) {
+    const auto res_pair
+        = MakeNewPair(item.collisionObject1, item.collisionObject2);
+    if (res_pair == pair) {
+      pastContacsVec.push_back(item);
+    }
+  }
 
-    if (result.getNumContacts() >= option.maxNumContacts)
-      return;
+  const auto size = pastContacsVec.size();
+  if (size > 11) {
+    pastContacsVec.erase(pastContacsVec.begin(), pastContacsVec.end() - 5);
   }
 }
 

tests/integration/test_Collision.cpp

@@ -482,10 +482,10 @@ void testSphereSphere(
     EXPECT_TRUE(group->collide(option, &result));
     // TODO(JS): BulletCollisionDetector includes a bug related to this.
     // (see #876)
-#if HAVE_BULLET
-    if (cd->getType() != BulletCollisionDetector::getStaticType())
-#endif
-    {
+
+    constexpr auto hasOde = (HAVE_ODE == 1);
+    constexpr auto hasBullet = (HAVE_BULLET == 1);
+    if constexpr (!hasOde && !hasBullet) {
       EXPECT_EQ(result.getNumContacts(), 1u);
     }
     for (auto i = 0u; i < result.getNumContacts(); ++i) {

tests/regression/CMakeLists.txt

@@ -48,6 +48,14 @@ if(TARGET dart-collision-bullet AND TARGET dart-collision-ode)
     dart-utils)
 endif()
 
+
+  dart_add_test("regression" test_Issue1654)
+  target_link_libraries(test_Issue1654
+    dart-collision-bullet
+    dart-collision-ode
+    dart-gui-osg
+    dart-utils)
+
 if(TARGET dart-utils)
   dart_add_test("regression" test_Issue1193)
   target_link_libraries(test_Issue1193 dart-utils)

tests/regression/test_Issue1654.cpp

@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2011-2022, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dart/gui/osg/osg.hpp>
+
+#include <dart/utils/DartResourceRetriever.hpp>
+
+#include <dart/simulation/World.hpp>
+
+#include <dart/constraint/ConstraintSolver.hpp>
+
+#include <dart/collision/bullet/BulletCollisionDetector.hpp>
+#include <dart/collision/ode/OdeCollisionDetector.hpp>
+
+#include <dart/dynamics/BoxShape.hpp>
+#include <dart/dynamics/FreeJoint.hpp>
+#include <dart/dynamics/MeshShape.hpp>
+#include <dart/dynamics/PlaneShape.hpp>
+#include <dart/dynamics/Skeleton.hpp>
+#include <dart/dynamics/WeldJoint.hpp>
+
+#include <TestHelpers.hpp>
+#include <gtest/gtest.h>
+
+#include <cstring>
+
+using namespace dart;
+
+class CapsuleTest : public testing::Test,
+                    public testing::WithParamInterface<const char*>
+{
+};
+TEST_P(CapsuleTest, CapsuleCollision)
+{
+  const double capsuleRadius = 0.2;
+  const double capsuleHeight = 0.6;
+
+  auto world = dart::simulation::World::create();
+
+  if (strcmp(GetParam(), "bullet") == 0) {
+    world->getConstraintSolver()->setCollisionDetector(
+        dart::collision::BulletCollisionDetector::create());
+  } else if (strcmp(GetParam(), "ode") == 0) {
+    world->getConstraintSolver()->setCollisionDetector(
+        dart::collision::OdeCollisionDetector::create());
+  } else {
+    ASSERT_TRUE(false);
+  }
+
+  auto ground = createBox({10000, 1000, 0.1}, {0, 0, -0.05});
+  ground->setMobile(false);
+  world->addSkeleton(ground);
+
+  ::osg::ref_ptr<gui::osg::RealTimeWorldNode> node
+      = new gui::osg::RealTimeWorldNode(world);
+
+  auto skeleton = dart::dynamics::Skeleton::create("test");
+  dart::dynamics::FreeJoint* joint;
+  BodyNode* bn;
+  std::tie(joint, bn)
+      = skeleton->createJointAndBodyNodePair<dart::dynamics::FreeJoint>();
+
+  Eigen::Isometry3d T_capsule;
+  T_capsule = Eigen::Translation3d(0, 0, 0.5)
+              * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0, 1, 0));
+  joint->setRelativeTransform(T_capsule);
+
+  auto capsuleNode = bn->createShapeNodeWith<
+      dart::dynamics::CollisionAspect,
+      dart::dynamics::DynamicsAspect>(
+      std::make_shared<dart::dynamics::CapsuleShape>(
+          capsuleRadius, capsuleHeight));
+
+  dart::dynamics::Inertia inertia;
+  inertia.setMass(1.0);
+  inertia.setMoment(capsuleNode->getShape()->computeInertia(inertia.getMass()));
+  bn->setInertia(inertia);
+  // std::cout << "Inertia: " <<
+  // bn->getBodyNodeProperties().mInertia.getMoment()
+  //           << std::endl;
+
+  world->addSkeleton(skeleton);
+  const double tolerance = 0.05;
+  for (int i = 1; i < 50000; ++i) {
+    world->step();
+    auto capsulePos = bn->getWorldTransform().translation();
+    ASSERT_GT(capsulePos.z(), capsuleRadius - tolerance);
+  }
+}
+
+INSTANTIATE_TEST_CASE_P(CollisionEngine, CapsuleTest, testing::Values("ode"));
+// testing::Values("bullet", "ode"));