Generalize the way properties of a contact constraints are computed, …

…allow for custom implementations (#1626)

Co-authored-by: Jeongseok Lee <[email protected]>

CHANGELOG.md

@@ -15,6 +15,7 @@
   * Added iterator methods to container-like classes: [#1644](https://github.com/dartsim/dart/pull/1644)
   * Fixed grouping of constraints: [#1624](https://github.com/dartsim/dart/pull/1624), [#1628](https://github.com/dartsim/dart/pull/1628)
   * Fixed issue with removing skeletons without shapes: [#1625](https://github.com/dartsim/dart/pull/1625)
+  * Added support for custom contact surface handlers: [#1626](https://github.com/dartsim/dart/pull/1626)
 
 * GUI
 

CONTRIBUTING.md

@@ -51,6 +51,7 @@ The code doesn't need to be perfect right away, feel free to post work-in-progre
  [Christoph Hinze](https://github.com/chhinze)      | python bindings
  [Erwin Coumans](https://github.com/erwincoumans)   | build fix on Windows/MSVC
  [Silvio Traversaro](https://github.com/traversaro) | build fix on Windows/MSVC, vcpkg packaging
+ [Martin Pecka](https://github.com/peci1)           | contact surface generalization
 
 You can find the complete contribution history in [here](https://github.com/dartsim/dart/graphs/contributors).
 

dart/constraint/ConstraintSolver.cpp

@@ -44,6 +44,7 @@
 #include "dart/common/Macros.hpp"
 #include "dart/constraint/ConstrainedGroup.hpp"
 #include "dart/constraint/ContactConstraint.hpp"
+#include "dart/constraint/ContactSurface.hpp"
 #include "dart/constraint/JointConstraint.hpp"
 #include "dart/constraint/JointCoulombFrictionConstraint.hpp"
 #include "dart/constraint/LCPSolver.hpp"
@@ -65,7 +66,8 @@ ConstraintSolver::ConstraintSolver(double timeStep)
     mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()),
     mCollisionOption(collision::CollisionOption(
         true, 1000u, std::make_shared<collision::BodyNodeCollisionFilter>())),
-    mTimeStep(timeStep)
+    mTimeStep(timeStep),
+    mContactSurfaceHandler(std::make_shared<DefaultContactSurfaceHandler>())
 {
   assert(timeStep > 0.0);
 
@@ -84,7 +86,8 @@ ConstraintSolver::ConstraintSolver()
     mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()),
     mCollisionOption(collision::CollisionOption(
         true, 1000u, std::make_shared<collision::BodyNodeCollisionFilter>())),
-    mTimeStep(0.001)
+    mTimeStep(0.001),
+    mContactSurfaceHandler(std::make_shared<DefaultContactSurfaceHandler>())
 {
   auto cd = std::static_pointer_cast<collision::FCLCollisionDetector>(
       mCollisionDetector);
@@ -390,6 +393,8 @@ void ConstraintSolver::setFromOtherConstraintSolver(
 
   addSkeletons(other.getSkeletons());
   mManualConstraints = other.mManualConstraints;
+
+  mContactSurfaceHandler = other.mContactSurfaceHandler;
 }
 
 //==============================================================================
@@ -506,6 +511,7 @@ void ConstraintSolver::updateConstraints()
   };
 
   std::map<ContactPair, size_t, ContactPairCompare> contactPairMap;
+  std::vector<collision::Contact*> contacts;
 
   // Create new contact constraints
   for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i)
@@ -548,31 +554,23 @@ void ConstraintSolver::updateConstraints()
       ++contactPairMap[std::make_pair(
           contact.collisionObject1, contact.collisionObject2)];
 
-      mContactConstraints.push_back(
-          std::make_shared<ContactConstraint>(contact, mTimeStep));
+      contacts.push_back(&contact);
     }
   }
 
   // Add the new contact constraints to dynamic constraint list
-  for (const auto& contactConstraint : mContactConstraints)
+  for (auto* contact : contacts)
   {
-    // update the slip compliances of the contact constraints based on the
-    // number of contacts between the collision objects.
-    auto& contact = contactConstraint->getContact();
     std::size_t numContacts = 1;
     auto it = contactPairMap.find(
-        std::make_pair(contact.collisionObject1, contact.collisionObject2));
+        std::make_pair(contact->collisionObject1, contact->collisionObject2));
     if (it != contactPairMap.end())
       numContacts = it->second;
 
-    // The slip compliance acts like a damper at each contact point so the total
-    // damping for each collision is multiplied by the number of contact points
-    // (numContacts). To eliminate this dependence on numContacts, the inverse
-    // damping is multiplied by numContacts.
-    contactConstraint->setPrimarySlipCompliance(
-        contactConstraint->getPrimarySlipCompliance() * numContacts);
-    contactConstraint->setSecondarySlipCompliance(
-        contactConstraint->getSecondarySlipCompliance() * numContacts);
+    auto contactConstraint = mContactSurfaceHandler->createConstraint(
+        *contact, numContacts, mTimeStep);
+    mContactConstraints.push_back(contactConstraint);
+
     contactConstraint->update();
 
     if (contactConstraint->isActive())
@@ -745,5 +743,55 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const
   return bodyNode1IsSoft || bodyNode2IsSoft;
 }
 
+//==============================================================================
+ContactSurfaceHandlerPtr ConstraintSolver::getLastContactSurfaceHandler() const
+{
+  return mContactSurfaceHandler;
+}
+
+//==============================================================================
+void ConstraintSolver::addContactSurfaceHandler(
+    ContactSurfaceHandlerPtr handler)
+{
+  // sanity check, do not add the same handler twice
+  if (handler == mContactSurfaceHandler)
+  {
+    dterr << "Adding the same contact surface handler for the second time, "
+             "ignoring." << std::endl;
+    return;
+  }
+  handler->setParent(mContactSurfaceHandler);
+  mContactSurfaceHandler = std::move(handler);
+}
+
+//==============================================================================
+bool ConstraintSolver::removeContactSurfaceHandler(
+    const ContactSurfaceHandlerPtr& handler)
+{
+  bool found = false;
+  ContactSurfaceHandlerPtr current = mContactSurfaceHandler;
+  ContactSurfaceHandlerPtr previous = nullptr;
+  while (current != nullptr)
+  {
+    if (current == handler)
+    {
+      if (previous != nullptr)
+        previous->mParent = current->mParent;
+      else
+        mContactSurfaceHandler = current->mParent;
+      found = true;
+      break;
+    }
+    previous = current;
+    current = current->mParent;
+  }
+
+  if (mContactSurfaceHandler == nullptr)
+    dterr << "No contact surface handler remained. This is an error. Add at "
+          << "least DefaultContactSurfaceHandler." << std::endl;
+
+  return found;
+}
+
 } // namespace constraint
 } // namespace dart

dart/constraint/ConstraintSolver.hpp

@@ -214,6 +214,24 @@ class ConstraintSolver
   /// properties and registered skeletons and constraints will be copied over.
   virtual void setFromOtherConstraintSolver(const ConstraintSolver& other);
 
+  /// Get the handler used for computing contact surface parameters based on
+  /// the contact properties of the two colliding bodies.
+  ContactSurfaceHandlerPtr getLastContactSurfaceHandler() const;
+
+  /// Set the handler used for computing contact surface parameters based on
+  /// the contact properties of the two colliding bodies. This function
+  /// automatically sets the previous handler as parent of the given handler.
+  void addContactSurfaceHandler(ContactSurfaceHandlerPtr handler);
+
+  /// Remove the given contact surface handler. If it is not the last in the
+  /// chain of handlers, the neighbor handlers are automatically connected
+  /// when the given handler is removed. This function returns true when the
+  /// given handler was found. It returns false when the handler is not found.
+  /// The search procedure utilizes pointer equality (i.e. the shared pointers
+  /// have to point to the same address to be treated equal). Take special care
+  /// to make sure at least one handler is always available.
+  bool removeContactSurfaceHandler(const ContactSurfaceHandlerPtr& handler);
+
 protected:
   // TODO(JS): Docstring
   virtual void solveConstrainedGroup(ConstrainedGroup& group) = 0;
@@ -292,6 +310,9 @@ class ConstraintSolver
 
   /// Constraint group list
   std::vector<ConstrainedGroup> mConstrainedGroups;
+
+  /// Factory for ContactSurfaceParams for each contact
+  ContactSurfaceHandlerPtr mContactSurfaceHandler;
 };
 
 } // namespace constraint