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

@@ -42,6 +42,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"
@@ -64,7 +65,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);
 
@@ -83,7 +85,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);
@@ -389,6 +392,8 @@ void ConstraintSolver::setFromOtherConstraintSolver(
 
   addSkeletons(other.getSkeletons());
   mManualConstraints = other.mManualConstraints;
+
+  mContactSurfaceHandler = other.mContactSurfaceHandler;
 }
 
 //==============================================================================
@@ -500,6 +505,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 +548,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;
 
-    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())
@@ -750,5 +752,49 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const
   return bodyNode1IsSoft || bodyNode2IsSoft;
 }
 
+//==============================================================================
+ContactSurfaceHandlerPtr
+ConstraintSolver::getContactSurfaceHandler() const
+{
+  return mContactSurfaceHandler;
+}
+
+//==============================================================================
+void ConstraintSolver::setContactSurfaceHandler(
+    ContactSurfaceHandlerPtr handler)
+{
+  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

@@ -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,9 @@ class ConstraintSolver
 
   /// Constraint group list
   std::vector<ConstrainedGroup> mConstrainedGroups;
+
+  /// Factory for ContactSurfaceParams for each contact
+  ContactSurfaceHandlerPtr mContactSurfaceHandler;
 };
 
 } // namespace constraint