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

…allow for custom implementations (#22) * Generalize the way properties of a contact constraint are computed, allow for custom implementations. Signed-off-by: Martin Pecka <[email protected]> * Add possibility to remove contact surface handlers. Signed-off-by: Martin Pecka <[email protected]> * Better generalization of contact constraint generation. Now it allows customizing the whole constraint generation process. Signed-off-by: Martin Pecka <[email protected]> * Retain ABI compatibility. Signed-off-by: Martin Pecka <[email protected]> * Improve comment. Signed-off-by: Martin Pecka <[email protected]> * Added comments to ABI hacks. Signed-off-by: Martin Pecka <[email protected]> * Addressed comments from review. Signed-off-by: Martin Pecka <[email protected]>
gazebo-forks · Aug 2, 2021 · 11876d5 · 11876d5
1 parent 71c7cfb
commit 11876d5
Show file tree

Hide file tree

Showing 8 changed files with 709 additions and 210 deletions.
diff --git a/CONTRIBUTING.md b/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).
 

diff --git a/dart/constraint/ConstraintSolver.cpp b/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,12 @@ namespace constraint {
 
 using namespace dynamics;
 
+// These two globals are a hack made to retain ABI compatibility.
+// TODO(anyone): Revert e95a6 in a future ABI-breaking version.
+std::mutex gContactSurfaceHandlersMutex;
+std::unordered_map<const ConstraintSolver*, ContactSurfaceHandlerPtr>
+    gContactSurfaceHandlers;
+
 //==============================================================================
 ConstraintSolver::ConstraintSolver(double timeStep)
   : mCollisionDetector(collision::FCLCollisionDetector::create()),
@@ -75,6 +84,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 +107,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 +417,11 @@ void ConstraintSolver::setFromOtherConstraintSolver(
 
   addSkeletons(other.getSkeletons());
   mManualConstraints = other.mManualConstraints;
+
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    gContactSurfaceHandlers[this] = gContactSurfaceHandlers[&other];
+  }
 }
 
 //==============================================================================
@@ -500,6 +533,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,31 +576,31 @@ 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)
   {
-    // 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));
-    if (it != contactPairMap.end())
-      numContacts = it->second;
-
-    contactConstraint->setPrimarySlipCompliance(
-        contactConstraint->getPrimarySlipCompliance() * numContacts);
-    contactConstraint->setSecondarySlipCompliance(
-        contactConstraint->getSecondarySlipCompliance() * numContacts);
-    contactConstraint->update();
-
-    if (contactConstraint->isActive())
-      mActiveConstraints.push_back(contactConstraint);
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    for (auto* contact : contacts)
+    {
+      std::size_t numContacts = 1;
+      auto it = contactPairMap.find(
+          std::make_pair(contact->collisionObject1, contact->collisionObject2));
+      if (it != contactPairMap.end())
+        numContacts = it->second;
+
+      auto contactConstraint = gContactSurfaceHandlers[this]->createConstraint(
+        *contact, numContacts, mTimeStep);
+
+      mContactConstraints.push_back(contactConstraint);
+
+      contactConstraint->update();
+
+      if (contactConstraint->isActive())
+        mActiveConstraints.push_back(contactConstraint);
+    }
   }
 
   // Add the new soft contact constraints to dynamic constraint list
@@ -750,5 +784,56 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const
   return bodyNode1IsSoft || bodyNode2IsSoft;
 }
 
+//==============================================================================
+ContactSurfaceHandlerPtr
+ConstraintSolver::getLastContactSurfaceHandler() const
+{
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    return gContactSurfaceHandlers[this];
+  }
+}
+
+//==============================================================================
+void ConstraintSolver::addContactSurfaceHandler(
+    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
diff --git a/dart/constraint/ConstraintSolver.hpp b/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 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;
@@ -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