Generalize the way properties of a contact constraints are computed, allow for custom implementations

CONTRIBUTING.md

@@ -48,6 +48,7 @@ The code doesn't need to be perfect right away, feel free to post work-in-progre
  [pchorak](https://github.com/pchorak)              | bug fixes
  [acxz](https://github.com/acxz)                    | doxygen warning fix
  [Addisu Taddese](https://github.com/azeey)         | bug fix in ode collision detector
+ [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

@@ -32,6 +32,8 @@
 
 #include "dart/constraint/ConstraintSolver.hpp"
 #include <algorithm>
+#include <mutex>
+#include <unordered_map>
 
 #include "dart/collision/CollisionFilter.hpp"
 #include "dart/collision/CollisionGroup.hpp"
@@ -42,6 +44,7 @@
 #include "dart/common/Console.hpp"
 #include "dart/constraint/ConstrainedGroup.hpp"
 #include "dart/constraint/ContactConstraint.hpp"
+#include "dart/constraint/ContactSurface.hpp"
 #include "dart/constraint/JointCoulombFrictionConstraint.hpp"
 #include "dart/constraint/JointLimitConstraint.hpp"
 #include "dart/constraint/LCPSolver.hpp"
@@ -58,6 +61,10 @@ namespace constraint {
 
 using namespace dynamics;
 
+std::mutex gContactSurfaceHandlersMutex;
+std::unordered_map<const ConstraintSolver*, ContactSurfaceHandlerPtr>
+    gContactSurfaceHandlers;
+
 //==============================================================================
 ConstraintSolver::ConstraintSolver(double timeStep)
   : mCollisionDetector(collision::FCLCollisionDetector::create()),
@@ -75,6 +82,12 @@ ConstraintSolver::ConstraintSolver(double timeStep)
   // TODO(JS): Consider using FCL's primitive shapes once FCL addresses
   // incorrect contact point computation.
   // (see: https://github.com/flexible-collision-library/fcl/issues/106)
+
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    gContactSurfaceHandlers[this] =
+        std::make_shared<DefaultContactSurfaceHandler>();
+  }
 }
 
 //==============================================================================
@@ -92,6 +105,19 @@ ConstraintSolver::ConstraintSolver()
   // TODO(JS): Consider using FCL's primitive shapes once FCL addresses
   // incorrect contact point computation.
   // (see: https://github.com/flexible-collision-library/fcl/issues/106)
+
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    gContactSurfaceHandlers[this] =
+        std::make_shared<DefaultContactSurfaceHandler>();
+  }
+}
+
+//==============================================================================
+ConstraintSolver::~ConstraintSolver()
+{
+  std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+  gContactSurfaceHandlers.erase(this);
 }
 
 //==============================================================================
@@ -389,6 +415,11 @@ void ConstraintSolver::setFromOtherConstraintSolver(
 
   addSkeletons(other.getSkeletons());
   mManualConstraints = other.mManualConstraints;
+
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    gContactSurfaceHandlers[this] = gContactSurfaceHandlers[&other];
+  }
 }
 
 //==============================================================================
@@ -500,6 +531,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)
@@ -542,27 +574,28 @@ 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;
 
-    contactConstraint->setPrimarySlipCompliance(
-        contactConstraint->getPrimarySlipCompliance() * numContacts);
-    contactConstraint->setSecondarySlipCompliance(
-        contactConstraint->getSecondarySlipCompliance() * numContacts);
+    ContactConstraintPtr contactConstraint;
+    {
+      std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+      contactConstraint = gContactSurfaceHandlers[this]->createConstraint(
+          *contact, numContacts, mTimeStep);
+    }
+
+    mContactConstraints.push_back(contactConstraint);
+
     contactConstraint->update();
 
     if (contactConstraint->isActive())
@@ -750,5 +783,56 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const
   return bodyNode1IsSoft || bodyNode2IsSoft;
 }
 
+//==============================================================================
+ContactSurfaceHandlerPtr
+ConstraintSolver::getContactSurfaceHandler() const
+{
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    return gContactSurfaceHandlers[this];
+  }
+}
+
+//==============================================================================
+void ConstraintSolver::setContactSurfaceHandler(
+    ContactSurfaceHandlerPtr handler)
+{
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    handler->setParent(gContactSurfaceHandlers[this]);
+    gContactSurfaceHandlers[this] = std::move(handler);
+  }
+}
+
+//==============================================================================
+bool ConstraintSolver::removeContactSurfaceHandler(
+    const ContactSurfaceHandlerPtr& handler)
+{
+  bool found = false;
+  std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+  ContactSurfaceHandlerPtr current = gContactSurfaceHandlers[this];
+  ContactSurfaceHandlerPtr previous = nullptr;
+  while (current != nullptr)
+  {
+    if (current == handler)
+    {
+      if (previous != nullptr)
+        previous->mParent = current->mParent;
+      else
+        gContactSurfaceHandlers[this] = current->mParent;
+      found = true;
+      break;
+    }
+    previous = current;
+    current = current->mParent;
+  }
+
+  if (gContactSurfaceHandlers[this] == 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

@@ -77,7 +77,7 @@ class ConstraintSolver
   ConstraintSolver(const ConstraintSolver& other) = delete;
 
   /// Destructor
-  virtual ~ConstraintSolver() = default;
+  virtual ~ConstraintSolver();
 
   /// Add single skeleton
   void addSkeleton(const dynamics::SkeletonPtr& skeleton);
@@ -184,6 +184,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 getContactSurfaceHandler() 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 setContactSurfaceHandler(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;
@@ -259,6 +277,10 @@ class ConstraintSolver
 
   /// Constraint group list
   std::vector<ConstrainedGroup> mConstrainedGroups;
+
+  /// Factory for ContactSurfaceParams for each contact
+  // Moved to gContactSurfaceHandlers in .cpp file to preserve ABI
+  // ContactSurfaceHandlerPtr mContactSurfaceHandler;
 };
 
 } // namespace constraint