From f9be775b97a2b6488f9ff8d14b45c766c8a9d1a9 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Tue, 6 Apr 2021 20:33:59 +0200
Subject: [PATCH 1/7] Generalize the way properties of a contact constraint are
 computed, allow for custom implementations.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 CONTRIBUTING.md                       |   1 +
 dart/constraint/ConstraintSolver.cpp  |  36 ++-
 dart/constraint/ConstraintSolver.hpp  |  12 +
 dart/constraint/ContactConstraint.cpp | 245 ++------------------
 dart/constraint/ContactConstraint.hpp |  27 ++-
 dart/constraint/ContactSurface.cpp    | 313 ++++++++++++++++++++++++++
 dart/constraint/ContactSurface.hpp    | 156 +++++++++++++
 dart/constraint/SmartPointer.hpp      |   1 +
 8 files changed, 546 insertions(+), 245 deletions(-)
 create mode 100644 dart/constraint/ContactSurface.cpp
 create mode 100644 dart/constraint/ContactSurface.hpp

diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 612c1e70981ee..52704efbdd074 100644
--- 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
index b56c7bf73e22f..3d855668e8c35 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/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;
 }
 
 //==============================================================================
@@ -542,8 +547,9 @@ void ConstraintSolver::updateConstraints()
       ++contactPairMap[std::make_pair(
           contact.collisionObject1, contact.collisionObject2)];
 
-      mContactConstraints.push_back(
-          std::make_shared<ContactConstraint>(contact, mTimeStep));
+      const auto params = mContactSurfaceHandler->createParams(contact);
+      mContactConstraints.push_back(std::make_shared<ContactConstraint>(
+          contact, mTimeStep, params));
     }
   }
 
@@ -559,10 +565,9 @@ void ConstraintSolver::updateConstraints()
     if (it != contactPairMap.end())
       numContacts = it->second;
 
-    contactConstraint->setPrimarySlipCompliance(
-        contactConstraint->getPrimarySlipCompliance() * numContacts);
-    contactConstraint->setSecondarySlipCompliance(
-        contactConstraint->getSecondarySlipCompliance() * numContacts);
+    mContactSurfaceHandler->updateConstraint(
+        *contactConstraint, contact, numContacts);
+
     contactConstraint->update();
 
     if (contactConstraint->isActive())
@@ -750,5 +755,20 @@ 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);
+}
+
 } // namespace constraint
 } // namespace dart
diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp
index ae2deb469df0a..afa5689cf690b 100644
--- a/dart/constraint/ConstraintSolver.hpp
+++ b/dart/constraint/ConstraintSolver.hpp
@@ -184,6 +184,15 @@ 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);
+
 protected:
   // TODO(JS): Docstring
   virtual void solveConstrainedGroup(ConstrainedGroup& group) = 0;
@@ -259,6 +268,9 @@ class ConstraintSolver
 
   /// Constraint group list
   std::vector<ConstrainedGroup> mConstrainedGroups;
+
+  /// Factory for ContactSurfaceParams for each contact
+  ContactSurfaceHandlerPtr mContactSurfaceHandler;
 };
 
 } // namespace constraint
diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp
index edbe88ea0b7ca..41920e098900c 100644
--- a/dart/constraint/ContactConstraint.cpp
+++ b/dart/constraint/ContactConstraint.cpp
@@ -49,12 +49,6 @@
 #define DART_CFM 1e-5
 // #define DART_MAX_NUMBER_OF_CONTACTS 32
 
-#define DART_RESTITUTION_COEFF_THRESHOLD 1e-3
-#define DART_FRICTION_COEFF_THRESHOLD 1e-3
-#define DART_BOUNCING_VELOCITY_THRESHOLD 1e-1
-#define DART_MAX_BOUNCING_VELOCITY 1e+2
-#define DART_CONTACT_CONSTRAINT_EPSILON_SQUARED 1e-12
-
 namespace dart {
 namespace constraint {
 
@@ -63,17 +57,18 @@ double ContactConstraint::mErrorReductionParameter = DART_ERP;
 double ContactConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV;
 double ContactConstraint::mConstraintForceMixing = DART_CFM;
 
-constexpr double DART_DEFAULT_FRICTION_COEFF = 1.0;
-constexpr double DART_DEFAULT_RESTITUTION_COEFF = 0.0;
-// slip compliance is combined through addition,
-// so set to half the global default value
-constexpr double DART_DEFAULT_SLIP_COMPLIANCE = 0.0;
-const Eigen::Vector3d DART_DEFAULT_FRICTION_DIR =
-    Eigen::Vector3d::UnitZ();
+//==============================================================================
+ContactConstraint::ContactConstraint(
+    collision::Contact& contact, double timeStep) :
+    ContactConstraint(contact, timeStep,
+                      DefaultContactSurfaceHandler().createParams(contact))
+{
+}
 
 //==============================================================================
 ContactConstraint::ContactConstraint(
-    collision::Contact& contact, double timeStep)
+    collision::Contact& contact, double timeStep,
+    const ContactSurfaceParams& contactSurfaceParams)
   : ConstraintBase(),
     mTimeStep(timeStep),
     mBodyNodeA(const_cast<dynamics::ShapeFrame*>(
@@ -98,19 +93,10 @@ ContactConstraint::ContactConstraint(
   assert(
       contact.normal.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED);
 
-  const auto* shapeNodeA = const_cast<dynamics::ShapeFrame*>(
-                               contact.collisionObject1->getShapeFrame())
-                               ->asShapeNode();
-  const auto* shapeNodeB = const_cast<dynamics::ShapeFrame*>(
-                               contact.collisionObject2->getShapeFrame())
-                               ->asShapeNode();
-
   //----------------------------------------------
   // Bounce
   //----------------------------------------------
-  const double restitutionCoeffA = computeRestitutionCoefficient(shapeNodeA);
-  const double restitutionCoeffB = computeRestitutionCoefficient(shapeNodeB);
-  mRestitutionCoeff = restitutionCoeffA * restitutionCoeffB;
+  mRestitutionCoeff = contactSurfaceParams.mRestitutionCoeff;
   if (mRestitutionCoeff > DART_RESTITUTION_COEFF_THRESHOLD)
     mIsBounceOn = true;
   else
@@ -119,71 +105,18 @@ ContactConstraint::ContactConstraint(
   //----------------------------------------------
   // Friction
   //----------------------------------------------
-  // TODO(JS): Assume the frictional coefficient can be changed during
-  //           simulation steps.
-  // Update mFrictionCoeff
-  const double frictionCoeffA = computeFrictionCoefficient(shapeNodeA);
-  const double frictionCoeffB = computeFrictionCoefficient(shapeNodeB);
-  const double secondaryFrictionCoeffA =
-                       computeSecondaryFrictionCoefficient(shapeNodeA);
-  const double secondaryFrictionCoeffB =
-                       computeSecondaryFrictionCoefficient(shapeNodeB);
-
-  // TODO(JS): Consider providing various ways of the combined friction or
-  // allowing to override this method by a custom method
-  mFrictionCoeff = std::min(frictionCoeffA, frictionCoeffB);
-  mSecondaryFrictionCoeff =
-      std::min(secondaryFrictionCoeffA, secondaryFrictionCoeffB);
+  mFrictionCoeff = contactSurfaceParams.mFrictionCoeff;
+  mSecondaryFrictionCoeff = contactSurfaceParams.mSecondaryFrictionCoeff;
   if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD ||
       mSecondaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD)
   {
     mIsFrictionOn = true;
 
-    // Compute slip compliance
-    const double slipComplianceA = computeSlipCompliance(shapeNodeA);
-    const double slipComplianceB = computeSlipCompliance(shapeNodeB);
-    const double secondarySlipComplianceA =
-                         computeSecondarySlipCompliance(shapeNodeA);
-    const double secondarySlipComplianceB =
-                         computeSecondarySlipCompliance(shapeNodeB);
     // Combine slip compliances through addition
-    mSlipCompliance = slipComplianceA + slipComplianceB;
-    mSecondarySlipCompliance =
-        secondarySlipComplianceA + secondarySlipComplianceB;
+    mSlipCompliance = contactSurfaceParams.mSlipCompliance;
+    mSecondarySlipCompliance = contactSurfaceParams.mSecondarySlipCompliance;
 
-    // Check shapeNodes for valid friction direction unit vectors
-    auto frictionDirA = computeWorldFirstFrictionDir(shapeNodeA);
-    auto frictionDirB = computeWorldFirstFrictionDir(shapeNodeB);
-
-    // resulting friction direction unit vector
-    bool nonzeroDirA = frictionDirA.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED;
-    bool nonzeroDirB = frictionDirB.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED;
-
-    // only consider custom friction direction if one has nonzero length
-    if (nonzeroDirA || nonzeroDirB)
-    {
-      // if A and B are both set, choose one with smaller friction coefficient
-      // since it's friction properties will dominate
-      if (nonzeroDirA && nonzeroDirB)
-      {
-        if (frictionCoeffA <= frictionCoeffB)
-        {
-          mFirstFrictionalDirection = frictionDirA.normalized();
-        }
-        else
-        {
-          mFirstFrictionalDirection = frictionDirB.normalized();
-        }
-      }
-      else if (nonzeroDirA)
-      {
-        mFirstFrictionalDirection = frictionDirA.normalized();
-      }
-      else
-      {
-        mFirstFrictionalDirection = frictionDirB.normalized();
-      }
-    }
+    mFirstFrictionalDirection = contactSurfaceParams.mFirstFrictionalDirection;
 
     // Update frictional direction
     updateFirstFrictionalDirection();
@@ -193,6 +126,9 @@ ContactConstraint::ContactConstraint(
     mIsFrictionOn = false;
   }
 
+  mContactSurfaceMotionVelocity =
+      contactSurfaceParams.mContactSurfaceMotionVelocity;
+
   assert(mBodyNodeA->getSkeleton());
   assert(mBodyNodeB->getSkeleton());
   mIsSelfCollision = (mBodyNodeA->getSkeleton() == mBodyNodeB->getSkeleton());
@@ -475,6 +411,9 @@ void ContactConstraint::getInformation(ConstraintInfo* info)
     }
 
     info->b[0] += bouncingVelocity;
+    info->b[0] += mContactSurfaceMotionVelocity.x();
+    info->b[1] += mContactSurfaceMotionVelocity.y();
+    info->b[2] += mContactSurfaceMotionVelocity.z();
 
     // TODO(JS): Initial guess
     // x
@@ -530,6 +469,7 @@ void ContactConstraint::getInformation(ConstraintInfo* info)
     }
 
     info->b[0] += bouncingVelocity;
+    info->b[0] += mContactSurfaceMotionVelocity.x();
 
     // TODO(JS): Initial guess
     // x
@@ -737,147 +677,6 @@ bool ContactConstraint::isActive() const
   return mActive;
 }
 
-//==============================================================================
-double ContactConstraint::computeFrictionCoefficient(
-    const dynamics::ShapeNode* shapeNode)
-{
-  assert(shapeNode);
-
-  auto dynamicAspect = shapeNode->getDynamicsAspect();
-
-  if (dynamicAspect == nullptr)
-  {
-    dtwarn << "[ContactConstraint] Attempt to extract friction coefficient "
-           << "from a ShapeNode that doesn't have DynamicAspect. The default "
-           << "value (" << DART_DEFAULT_FRICTION_COEFF << ") will be used "
-           << "instead.\n";
-    return DART_DEFAULT_FRICTION_COEFF;
-  }
-
-  return dynamicAspect->getFrictionCoeff();
-}
-
-//==============================================================================
-double ContactConstraint::computeSecondaryFrictionCoefficient(
-    const dynamics::ShapeNode* shapeNode)
-{
-  assert(shapeNode);
-
-  auto dynamicAspect = shapeNode->getDynamicsAspect();
-
-  if (dynamicAspect == nullptr)
-  {
-    dtwarn << "[ContactConstraint] Attempt to extract "
-           << "secondary friction coefficient "
-           << "from a ShapeNode that doesn't have DynamicAspect. The default "
-           << "value (" << DART_DEFAULT_FRICTION_COEFF << ") will be used "
-           << "instead.\n";
-    return DART_DEFAULT_FRICTION_COEFF;
-  }
-
-  return dynamicAspect->getSecondaryFrictionCoeff();
-}
-
-//==============================================================================
-double ContactConstraint::computeSlipCompliance(
-    const dynamics::ShapeNode* shapeNode)
-{
-  assert(shapeNode);
-
-  auto dynamicAspect = shapeNode->getDynamicsAspect();
-
-  if (dynamicAspect == nullptr)
-  {
-    dtwarn << "[ContactConstraint] Attempt to extract slip compliance "
-           << "from a ShapeNode that doesn't have DynamicAspect. The default "
-           << "value (" << DART_DEFAULT_SLIP_COMPLIANCE << ") will be used "
-           << "instead.\n";
-    return DART_DEFAULT_SLIP_COMPLIANCE;
-  }
-
-  double slipCompliance = dynamicAspect->getSlipCompliance();
-  if (slipCompliance < 0)
-  {
-    return DART_DEFAULT_SLIP_COMPLIANCE;
-  }
-  return slipCompliance;
-}
-
-//==============================================================================
-double ContactConstraint::computeSecondarySlipCompliance(
-    const dynamics::ShapeNode* shapeNode)
-{
-  assert(shapeNode);
-
-  auto dynamicAspect = shapeNode->getDynamicsAspect();
-
-  if (dynamicAspect == nullptr)
-  {
-    dtwarn << "[ContactConstraint] Attempt to extract "
-           << "secondary slip compliance "
-           << "from a ShapeNode that doesn't have DynamicAspect. The default "
-           << "value (" << DART_DEFAULT_SLIP_COMPLIANCE << ") will be used "
-           << "instead.\n";
-    return DART_DEFAULT_SLIP_COMPLIANCE;
-  }
-
-  double slipCompliance = dynamicAspect->getSecondarySlipCompliance();
-  if (slipCompliance < 0)
-  {
-    return DART_DEFAULT_SLIP_COMPLIANCE;
-  }
-  return slipCompliance;
-}
-
-//==============================================================================
-Eigen::Vector3d ContactConstraint::computeWorldFirstFrictionDir(
-    const dynamics::ShapeNode* shapeNode)
-{
-  assert(shapeNode);
-
-  auto dynamicAspect = shapeNode->getDynamicsAspect();
-
-  if (dynamicAspect == nullptr)
-  {
-    dtwarn << "[ContactConstraint] Attempt to extract friction direction "
-           << "from a ShapeNode that doesn't have DynamicAspect. The default "
-           << "value (" << DART_DEFAULT_FRICTION_DIR << ") will be used "
-           << "instead.\n";
-    return DART_DEFAULT_FRICTION_DIR;
-  }
-
-  auto frame = dynamicAspect->getFirstFrictionDirectionFrame();
-  Eigen::Vector3d frictionDir = dynamicAspect->getFirstFrictionDirection();
-
-  // rotate using custom frame if it is specified
-  if (frame)
-  {
-    return frame->getWorldTransform().linear() * frictionDir;
-  }
-  // otherwise rotate using shapeNode
-  return shapeNode->getWorldTransform().linear() * frictionDir;
-}
-
-//==============================================================================
-double ContactConstraint::computeRestitutionCoefficient(
-    const dynamics::ShapeNode* shapeNode)
-{
-  assert(shapeNode);
-
-  auto dynamicAspect = shapeNode->getDynamicsAspect();
-
-  if (dynamicAspect == nullptr)
-  {
-    dtwarn << "[ContactConstraint] Attempt to extract restitution coefficient "
-           << "from a ShapeNode that doesn't have DynamicAspect. The default "
-           << "value (" << DART_DEFAULT_RESTITUTION_COEFF << ") will be used "
-           << "instead.\n";
-    return DART_DEFAULT_RESTITUTION_COEFF;
-  }
-
-  return dynamicAspect->getRestitutionCoeff();
-}
-
 //==============================================================================
 dynamics::SkeletonPtr ContactConstraint::getRootSkeleton() const
 {
diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp
index a3c2f8a240442..82bd4be90dfbe 100644
--- a/dart/constraint/ContactConstraint.hpp
+++ b/dart/constraint/ContactConstraint.hpp
@@ -35,6 +35,7 @@
 
 #include "dart/collision/CollisionDetector.hpp"
 #include "dart/constraint/ConstraintBase.hpp"
+#include "dart/constraint/ContactSurface.hpp"
 #include "dart/math/MathTypes.hpp"
 
 namespace dart {
@@ -50,6 +51,10 @@ namespace constraint {
 class ContactConstraint : public ConstraintBase
 {
 public:
+  /// Constructor
+  ContactConstraint(collision::Contact& contact, double timeStep,
+                    const ContactSurfaceParams& contactSurfaceParams);
+
   /// Constructor
   ContactConstraint(collision::Contact& contact, double timeStep);
 
@@ -96,6 +101,7 @@ class ContactConstraint : public ConstraintBase
 
   friend class ConstraintSolver;
   friend class ConstrainedGroup;
+  friend class DefaultContactSurfaceHandler;
 
 protected:
   //----------------------------------------------------------------------------
@@ -132,19 +138,6 @@ class ContactConstraint : public ConstraintBase
   // Documentation inherited
   bool isActive() const override;
 
-  static double computeFrictionCoefficient(
-      const dynamics::ShapeNode* shapeNode);
-  static double computeSecondaryFrictionCoefficient(
-      const dynamics::ShapeNode* shapeNode);
-  static double computeSlipCompliance(
-      const dynamics::ShapeNode* shapeNode);
-  static double computeSecondarySlipCompliance(
-      const dynamics::ShapeNode* shapeNode);
-  static Eigen::Vector3d computeWorldFirstFrictionDir(
-      const dynamics::ShapeNode* shapenode);
-  static double computeRestitutionCoefficient(
-      const dynamics::ShapeNode* shapeNode);
-
 private:
   using TangentBasisMatrix = Eigen::Matrix<double, 3, 2>;
 
@@ -198,7 +191,7 @@ class ContactConstraint : public ConstraintBase
   /// Primary Coefficient of Friction
   double mFrictionCoeff;
 
-  /// Primary Coefficient of Friction
+  /// Secondary Coefficient of Friction
   double mSecondaryFrictionCoeff;
 
   /// Primary Coefficient of Slip Compliance
@@ -210,6 +203,12 @@ class ContactConstraint : public ConstraintBase
   /// Coefficient of restitution
   double mRestitutionCoeff;
 
+  /// Velocity of the contact independent of friction
+  /// x = vel. in direction of contact normal
+  /// y = vel. in first friction direction
+  /// z = vel. in second friction direction
+  Eigen::Vector3d mContactSurfaceMotionVelocity;
+
   /// Whether this contact is self-collision.
   bool mIsSelfCollision;
 
diff --git a/dart/constraint/ContactSurface.cpp b/dart/constraint/ContactSurface.cpp
new file mode 100644
index 0000000000000..2d04c30837f20
--- /dev/null
+++ b/dart/constraint/ContactSurface.cpp
@@ -0,0 +1,313 @@
+/*
+ * Copyright (c) 2021, 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 "ContactSurface.hpp"
+
+#include <utility>
+
+#include <dart/collision/Contact.hpp>
+#include <dart/constraint/ContactSurface.hpp>
+
+#include "dart/collision/CollisionObject.hpp"
+
+#include "ContactConstraint.hpp"
+
+namespace dart {
+namespace constraint {
+
+//==============================================================================
+ContactSurfaceHandler::ContactSurfaceHandler(ContactSurfaceHandlerPtr parent)
+  : mParent(std::move(parent))
+{
+}
+
+//==============================================================================
+void ContactSurfaceHandler::setParent(ContactSurfaceHandlerPtr parent)
+{
+  if (parent.get() != this)
+    this->mParent = std::move(parent);
+}
+
+//==============================================================================
+ContactSurfaceParams constraint::DefaultContactSurfaceHandler::createParams(
+    const collision::Contact& contact) const
+{
+  ContactSurfaceParams params
+      = (this->mParent ? this->mParent->createParams(contact)
+                       : ContactSurfaceParams());
+
+  const auto* shapeNodeA = const_cast<dynamics::ShapeFrame*>(
+                               contact.collisionObject1->getShapeFrame())
+                               ->asShapeNode();
+  const auto* shapeNodeB = const_cast<dynamics::ShapeFrame*>(
+                               contact.collisionObject2->getShapeFrame())
+                               ->asShapeNode();
+
+  const double restitutionCoeffA = computeRestitutionCoefficient(shapeNodeA);
+  const double restitutionCoeffB = computeRestitutionCoefficient(shapeNodeB);
+
+  params.mRestitutionCoeff = restitutionCoeffA * restitutionCoeffB;
+
+  const double frictionCoeffA = computeFrictionCoefficient(shapeNodeA);
+  const double frictionCoeffB = computeFrictionCoefficient(shapeNodeB);
+  const double secondaryFrictionCoeffA
+      = computeSecondaryFrictionCoefficient(shapeNodeA);
+  const double secondaryFrictionCoeffB
+      = computeSecondaryFrictionCoefficient(shapeNodeB);
+
+  params.mFrictionCoeff = (std::min)(frictionCoeffA, frictionCoeffB);
+  params.mSecondaryFrictionCoeff
+      = (std::min)(secondaryFrictionCoeffA, secondaryFrictionCoeffB);
+
+  if (params.mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD
+      || params.mSecondaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD)
+  {
+    const double slipComplianceA = computeSlipCompliance(shapeNodeA);
+    const double slipComplianceB = computeSlipCompliance(shapeNodeB);
+
+    // Combine slip compliances through addition
+    params.mSlipCompliance = slipComplianceA + slipComplianceB;
+
+    const double secondarySlipComplianceA
+        = computeSecondarySlipCompliance(shapeNodeA);
+    const double secondarySlipComplianceB
+        = computeSecondarySlipCompliance(shapeNodeB);
+
+    // Combine slip compliances through addition
+    params.mSecondarySlipCompliance
+        = secondarySlipComplianceA + secondarySlipComplianceB;
+
+    // Check shapeNodes for valid friction direction unit vectors
+    auto frictionDirA = computeWorldFirstFrictionDir(shapeNodeA);
+    auto frictionDirB = computeWorldFirstFrictionDir(shapeNodeB);
+
+    // resulting friction direction unit vector
+    bool nonzeroDirA
+        = frictionDirA.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED;
+    bool nonzeroDirB
+        = frictionDirB.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED;
+
+    // only consider custom friction direction if one has nonzero length
+    if (nonzeroDirA || nonzeroDirB)
+    {
+      // if A and B are both set, choose one with smaller friction coefficient
+      // since it's friction properties will dominate
+      if (nonzeroDirA && nonzeroDirB)
+      {
+        if (frictionCoeffA <= frictionCoeffB)
+        {
+          params.mFirstFrictionalDirection = frictionDirA.normalized();
+        }
+        else
+        {
+          params.mFirstFrictionalDirection = frictionDirB.normalized();
+        }
+      }
+      else if (nonzeroDirA)
+      {
+        params.mFirstFrictionalDirection = frictionDirA.normalized();
+      }
+      else
+      {
+        params.mFirstFrictionalDirection = frictionDirB.normalized();
+      }
+    }
+    else
+    {
+      params.mFirstFrictionalDirection = DART_DEFAULT_FRICTION_DIR;
+    }
+  }
+
+  params.mContactSurfaceMotionVelocity = Eigen::Vector3d::Zero();
+
+  return params;
+}
+
+//==============================================================================
+void constraint::DefaultContactSurfaceHandler::updateConstraint(
+    ContactConstraint& constraint,
+    const collision::Contact& contact,
+    size_t numContactsOnCollisionObject) const
+{
+  if (this->mParent)
+    this->mParent->updateConstraint(
+        constraint, contact, numContactsOnCollisionObject);
+
+  constraint.setPrimarySlipCompliance(
+      constraint.getPrimarySlipCompliance() * numContactsOnCollisionObject);
+  constraint.setSecondarySlipCompliance(
+      constraint.getSecondarySlipCompliance() * numContactsOnCollisionObject);
+}
+
+//==============================================================================
+double DefaultContactSurfaceHandler::computeFrictionCoefficient(
+    const dynamics::ShapeNode* shapeNode)
+{
+  assert(shapeNode);
+
+  auto dynamicAspect = shapeNode->getDynamicsAspect();
+
+  if (dynamicAspect == nullptr)
+  {
+    dtwarn << "[ContactConstraint] Attempt to extract friction coefficient "
+           << "from a ShapeNode that doesn't have DynamicAspect. The default "
+           << "value (" << DART_DEFAULT_FRICTION_COEFF << ") will be used "
+           << "instead.\n";
+    return DART_DEFAULT_FRICTION_COEFF;
+  }
+
+  return dynamicAspect->getFrictionCoeff();
+}
+
+//==============================================================================
+double DefaultContactSurfaceHandler::computeSecondaryFrictionCoefficient(
+    const dynamics::ShapeNode* shapeNode)
+{
+  assert(shapeNode);
+
+  auto dynamicAspect = shapeNode->getDynamicsAspect();
+
+  if (dynamicAspect == nullptr)
+  {
+    dtwarn << "[ContactConstraint] Attempt to extract "
+           << "secondary friction coefficient "
+           << "from a ShapeNode that doesn't have DynamicAspect. The default "
+           << "value (" << DART_DEFAULT_FRICTION_COEFF << ") will be used "
+           << "instead.\n";
+    return DART_DEFAULT_FRICTION_COEFF;
+  }
+
+  return dynamicAspect->getSecondaryFrictionCoeff();
+}
+
+//==============================================================================
+double DefaultContactSurfaceHandler::computeSlipCompliance(
+    const dynamics::ShapeNode* shapeNode)
+{
+  assert(shapeNode);
+
+  auto dynamicAspect = shapeNode->getDynamicsAspect();
+
+  if (dynamicAspect == nullptr)
+  {
+    dtwarn << "[ContactConstraint] Attempt to extract slip compliance "
+           << "from a ShapeNode that doesn't have DynamicAspect. The default "
+           << "value (" << DART_DEFAULT_SLIP_COMPLIANCE << ") will be used "
+           << "instead.\n";
+    return DART_DEFAULT_SLIP_COMPLIANCE;
+  }
+
+  double slipCompliance = dynamicAspect->getSlipCompliance();
+  if (slipCompliance < 0)
+  {
+    return DART_DEFAULT_SLIP_COMPLIANCE;
+  }
+  return slipCompliance;
+}
+
+//==============================================================================
+double DefaultContactSurfaceHandler::computeSecondarySlipCompliance(
+    const dynamics::ShapeNode* shapeNode)
+{
+  assert(shapeNode);
+
+  auto dynamicAspect = shapeNode->getDynamicsAspect();
+
+  if (dynamicAspect == nullptr)
+  {
+    dtwarn << "[ContactConstraint] Attempt to extract "
+           << "secondary slip compliance "
+           << "from a ShapeNode that doesn't have DynamicAspect. The default "
+           << "value (" << DART_DEFAULT_SLIP_COMPLIANCE << ") will be used "
+           << "instead.\n";
+    return DART_DEFAULT_SLIP_COMPLIANCE;
+  }
+
+  double slipCompliance = dynamicAspect->getSecondarySlipCompliance();
+  if (slipCompliance < 0)
+  {
+    return DART_DEFAULT_SLIP_COMPLIANCE;
+  }
+  return slipCompliance;
+}
+
+//==============================================================================
+Eigen::Vector3d DefaultContactSurfaceHandler::computeWorldFirstFrictionDir(
+    const dynamics::ShapeNode* shapeNode)
+{
+  assert(shapeNode);
+
+  auto dynamicAspect = shapeNode->getDynamicsAspect();
+
+  if (dynamicAspect == nullptr)
+  {
+    dtwarn << "[ContactConstraint] Attempt to extract friction direction "
+           << "from a ShapeNode that doesn't have DynamicAspect. The default "
+           << "value (" << DART_DEFAULT_FRICTION_DIR << ") will be used "
+           << "instead.\n";
+    return DART_DEFAULT_FRICTION_DIR;
+  }
+
+  auto frame = dynamicAspect->getFirstFrictionDirectionFrame();
+  Eigen::Vector3d frictionDir = dynamicAspect->getFirstFrictionDirection();
+
+  // rotate using custom frame if it is specified
+  if (frame)
+  {
+    return frame->getWorldTransform().linear() * frictionDir;
+  }
+  // otherwise rotate using shapeNode
+  return shapeNode->getWorldTransform().linear() * frictionDir;
+}
+
+//==============================================================================
+double DefaultContactSurfaceHandler::computeRestitutionCoefficient(
+    const dynamics::ShapeNode* shapeNode)
+{
+  assert(shapeNode);
+
+  auto dynamicAspect = shapeNode->getDynamicsAspect();
+
+  if (dynamicAspect == nullptr)
+  {
+    dtwarn << "[ContactConstraint] Attempt to extract restitution coefficient "
+           << "from a ShapeNode that doesn't have DynamicAspect. The default "
+           << "value (" << DART_DEFAULT_RESTITUTION_COEFF << ") will be used "
+           << "instead.\n";
+    return DART_DEFAULT_RESTITUTION_COEFF;
+  }
+
+  return dynamicAspect->getRestitutionCoeff();
+}
+
+} // namespace constraint
+} // namespace dart
\ No newline at end of file
diff --git a/dart/constraint/ContactSurface.hpp b/dart/constraint/ContactSurface.hpp
new file mode 100644
index 0000000000000..61400822bc2e1
--- /dev/null
+++ b/dart/constraint/ContactSurface.hpp
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2021, 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.
+ */
+
+#ifndef DART_CONSTRAINT_CONTACTSURFACE_HPP
+#define DART_CONSTRAINT_CONTACTSURFACE_HPP
+
+#include <Eigen/Core>
+#include "dart/collision/Contact.hpp"
+#include "dart/dynamics/ShapeNode.hpp"
+#include "dart/constraint/SmartPointer.hpp"
+
+namespace dart {
+namespace constraint {
+
+#define DART_RESTITUTION_COEFF_THRESHOLD 1e-3
+#define DART_FRICTION_COEFF_THRESHOLD 1e-3
+#define DART_BOUNCING_VELOCITY_THRESHOLD 1e-1
+#define DART_MAX_BOUNCING_VELOCITY 1e+2
+#define DART_CONTACT_CONSTRAINT_EPSILON_SQUARED 1e-12
+
+constexpr double DART_DEFAULT_FRICTION_COEFF = 1.0;
+constexpr double DART_DEFAULT_RESTITUTION_COEFF = 0.0;
+// slip compliance is combined through addition,
+// so set to half the global default value
+constexpr double DART_DEFAULT_SLIP_COMPLIANCE = 0.0;
+const Eigen::Vector3d DART_DEFAULT_FRICTION_DIR = Eigen::Vector3d::UnitZ();
+const Eigen::Vector3d DART_DEFAULT_CONTACT_SURFACE_MOTION_VELOCITY = Eigen::Vector3d::Zero();
+
+/// Computed parameters of the contact surface
+struct ContactSurfaceParams
+{
+  /// Primary Coefficient of Friction
+  double mFrictionCoeff {DART_DEFAULT_FRICTION_COEFF};
+
+  /// Secondary Coefficient of Friction
+  double mSecondaryFrictionCoeff {DART_DEFAULT_FRICTION_COEFF};
+
+  /// Coefficient of restitution
+  double mRestitutionCoeff {DART_DEFAULT_RESTITUTION_COEFF};
+
+  /// Primary Coefficient of Slip Compliance
+  double mSlipCompliance {DART_DEFAULT_SLIP_COMPLIANCE};
+
+  /// Secondary Coefficient of Slip Compliance
+  double mSecondarySlipCompliance {DART_DEFAULT_SLIP_COMPLIANCE};
+
+  /// First frictional direction (in world frame)
+  Eigen::Vector3d mFirstFrictionalDirection {DART_DEFAULT_FRICTION_DIR};
+
+  /// Velocity of the contact independent of friction
+  /// x = vel. in direction of contact normal
+  /// y = vel. in first friction direction
+  /// z = vel. in second friction direction
+  Eigen::Vector3d mContactSurfaceMotionVelocity {DART_DEFAULT_CONTACT_SURFACE_MOTION_VELOCITY};
+};
+
+class ContactConstraint;
+
+/// Class used to determine the properties of a contact constraint based on the
+/// two colliding bodies and information about their contact.
+class ContactSurfaceHandler
+{
+public:
+  /// Constructor
+  /// \param[in] parent Optional parent handler. It suggested that new handlers
+  ///                   should not overwrite the previous ones, but rather
+  ///                   "extend" them.
+  explicit ContactSurfaceHandler(ContactSurfaceHandlerPtr parent = nullptr);
+
+  /// Create parameters of the contact constraint. This method should combine
+  /// the collision properties of the two colliding bodies and write the
+  /// combined result in the returned object.
+  virtual ContactSurfaceParams createParams(
+      const collision::Contact& contact) const = 0;
+
+  /// Called after the contact constrain object is constructed to allow
+  /// additional configuration. It is also passed the total number of contact
+  /// points reported on the same collision body - that is useful e.g. for
+  /// normalization of forces by the number of contact points.
+  virtual void updateConstraint(
+      ContactConstraint& constraint,
+      const collision::Contact& contact,
+      size_t numContactsOnCollisionObject) const = 0;
+
+  /// Set the optional parent handler (ignored if parent.get() == this)
+  void setParent(ContactSurfaceHandlerPtr parent);
+
+protected:
+  /// The optional parent handler
+  ContactSurfaceHandlerPtr mParent;
+};
+
+/// Default contact surface handler. It chooses friction direction of the body
+/// with lower friction coefficient. It also adjusts slip compliance by
+/// mutliplying it with the number of contact points.
+class DefaultContactSurfaceHandler : public ContactSurfaceHandler
+{
+public:
+  // Documentation inherited
+  ContactSurfaceParams createParams(
+      const collision::Contact& contact) const override;
+
+  // Documentation inherited
+  void updateConstraint(
+      ContactConstraint& constraint,
+      const collision::Contact& contact,
+      size_t numContactsOnCollisionObject) const override;
+
+protected:
+  static double computeFrictionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+  static double computeSecondaryFrictionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+  static double computeSlipCompliance(
+      const dynamics::ShapeNode* shapeNode);
+  static double computeSecondarySlipCompliance(
+      const dynamics::ShapeNode* shapeNode);
+  static Eigen::Vector3d computeWorldFirstFrictionDir(
+      const dynamics::ShapeNode* shapenode);
+  static double computeRestitutionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+};
+
+} // namespace constraint
+} // namespace dart
+
+#endif // DART_CONSTRAINT_CONTACTSURFACE_HPP
diff --git a/dart/constraint/SmartPointer.hpp b/dart/constraint/SmartPointer.hpp
index 313dc0cb65e8b..b7718fb8df952 100644
--- a/dart/constraint/SmartPointer.hpp
+++ b/dart/constraint/SmartPointer.hpp
@@ -45,6 +45,7 @@ DART_COMMON_DECLARE_SHARED_WEAK(ConstrainedGroup)
 DART_COMMON_DECLARE_SHARED_WEAK(ConstraintBase)
 DART_COMMON_DECLARE_SHARED_WEAK(ClosedLoopConstraint)
 DART_COMMON_DECLARE_SHARED_WEAK(ContactConstraint)
+DART_COMMON_DECLARE_SHARED_WEAK(ContactSurfaceHandler)
 DART_COMMON_DECLARE_SHARED_WEAK(SoftContactConstraint)
 DART_COMMON_DECLARE_SHARED_WEAK(JointLimitConstraint)
 DART_COMMON_DECLARE_SHARED_WEAK(ServoMotorConstraint)

From 0a442a63ae4d74ddf4404a663d7aab6603ff4b6e Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Thu, 10 Jun 2021 11:17:42 +0200
Subject: [PATCH 2/7] Add possibility to remove contact surface handlers.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 dart/constraint/ConstraintSolver.cpp | 29 ++++++++++++++++++++++++++++
 dart/constraint/ConstraintSolver.hpp |  9 +++++++++
 dart/constraint/ContactSurface.hpp   |  2 ++
 3 files changed, 40 insertions(+)

diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 9a21c048479cf..cae663e875557 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -770,5 +770,34 @@ void ConstraintSolver::setContactSurfaceHandler(
   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
diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp
index afa5689cf690b..9b41e66f879f6 100644
--- a/dart/constraint/ConstraintSolver.hpp
+++ b/dart/constraint/ConstraintSolver.hpp
@@ -193,6 +193,15 @@ class ConstraintSolver
   /// 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;
diff --git a/dart/constraint/ContactSurface.hpp b/dart/constraint/ContactSurface.hpp
index 61400822bc2e1..85c3a5b4aa9a3 100644
--- a/dart/constraint/ContactSurface.hpp
+++ b/dart/constraint/ContactSurface.hpp
@@ -114,6 +114,8 @@ class ContactSurfaceHandler
   /// Set the optional parent handler (ignored if parent.get() == this)
   void setParent(ContactSurfaceHandlerPtr parent);
 
+  friend class dart::constraint::ConstraintSolver;
+
 protected:
   /// The optional parent handler
   ContactSurfaceHandlerPtr mParent;

From 723121287fb7ed2bd05c526e4d3ec0c3598afbe3 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Thu, 10 Jun 2021 19:00:25 +0200
Subject: [PATCH 3/7] Better generalization of contact constraint generation.
 Now it allows customizing the whole constraint generation process.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 dart/constraint/ConstraintSolver.cpp  | 17 ++++-----
 dart/constraint/ContactConstraint.cpp |  2 +-
 dart/constraint/ContactSurface.cpp    | 55 +++++++++++++++++++--------
 dart/constraint/ContactSurface.hpp    | 32 ++++++++--------
 4 files changed, 64 insertions(+), 42 deletions(-)

diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index cae663e875557..312cfd54ed94f 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -505,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)
@@ -547,26 +548,22 @@ void ConstraintSolver::updateConstraints()
       ++contactPairMap[std::make_pair(
           contact.collisionObject1, contact.collisionObject2)];
 
-      const auto params = mContactSurfaceHandler->createParams(contact);
-      mContactConstraints.push_back(std::make_shared<ContactConstraint>(
-          contact, mTimeStep, params));
+      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;
 
-    mContactSurfaceHandler->updateConstraint(
-        *contactConstraint, contact, numContacts);
+    auto contactConstraint = mContactSurfaceHandler->createConstraint(
+        *contact, numContacts, mTimeStep);
+    mContactConstraints.push_back(contactConstraint);
 
     contactConstraint->update();
 
diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp
index 41920e098900c..a8c4fc3642de9 100644
--- a/dart/constraint/ContactConstraint.cpp
+++ b/dart/constraint/ContactConstraint.cpp
@@ -61,7 +61,7 @@ double ContactConstraint::mConstraintForceMixing = DART_CFM;
 ContactConstraint::ContactConstraint(
     collision::Contact& contact, double timeStep) :
     ContactConstraint(contact, timeStep,
-                      DefaultContactSurfaceHandler().createParams(contact))
+                      DefaultContactSurfaceHandler().createParams(contact, 1u))
 {
 }
 
diff --git a/dart/constraint/ContactSurface.cpp b/dart/constraint/ContactSurface.cpp
index 2d04c30837f20..abccc86a9fbf1 100644
--- a/dart/constraint/ContactSurface.cpp
+++ b/dart/constraint/ContactSurface.cpp
@@ -57,13 +57,33 @@ void ContactSurfaceHandler::setParent(ContactSurfaceHandlerPtr parent)
     this->mParent = std::move(parent);
 }
 
+//==============================================================================
+ContactSurfaceParams ContactSurfaceHandler::createParams(
+    const collision::Contact& contact,
+    size_t numContactsOnCollisionObject) const
+{
+  if (mParent != nullptr)
+    return mParent->createParams(contact, numContactsOnCollisionObject);
+  return {};
+}
+
+//==============================================================================
+ContactConstraintPtr ContactSurfaceHandler::createConstraint(
+    collision::Contact& contact,
+    size_t numContactsOnCollisionObject,
+    const double timeStep) const
+{
+  auto params = this->createParams(contact, numContactsOnCollisionObject);
+  return std::make_shared<ContactConstraint>(contact, timeStep, params);
+}
+
 //==============================================================================
 ContactSurfaceParams constraint::DefaultContactSurfaceHandler::createParams(
-    const collision::Contact& contact) const
+    const collision::Contact& contact,
+    size_t numContactsOnCollisionObject) const
 {
-  ContactSurfaceParams params
-      = (this->mParent ? this->mParent->createParams(contact)
-                       : ContactSurfaceParams());
+  ContactSurfaceParams params = ContactSurfaceHandler::createParams(
+      contact, numContactsOnCollisionObject);
 
   const auto* shapeNodeA = const_cast<dynamics::ShapeFrame*>(
                                contact.collisionObject1->getShapeFrame())
@@ -153,19 +173,22 @@ ContactSurfaceParams constraint::DefaultContactSurfaceHandler::createParams(
 }
 
 //==============================================================================
-void constraint::DefaultContactSurfaceHandler::updateConstraint(
-    ContactConstraint& constraint,
-    const collision::Contact& contact,
-    size_t numContactsOnCollisionObject) const
+ContactConstraintPtr DefaultContactSurfaceHandler::createConstraint(
+    collision::Contact& contact,
+    const size_t numContactsOnCollisionObject,
+    const double timeStep) const
 {
-  if (this->mParent)
-    this->mParent->updateConstraint(
-        constraint, contact, numContactsOnCollisionObject);
-
-  constraint.setPrimarySlipCompliance(
-      constraint.getPrimarySlipCompliance() * numContactsOnCollisionObject);
-  constraint.setSecondarySlipCompliance(
-      constraint.getSecondarySlipCompliance() * numContactsOnCollisionObject);
+  auto constraint = ContactSurfaceHandler::createConstraint(
+      contact, numContactsOnCollisionObject, timeStep);
+
+  constraint->setPrimarySlipCompliance(
+      constraint->getPrimarySlipCompliance() *
+      static_cast<double>(numContactsOnCollisionObject));
+  constraint->setSecondarySlipCompliance(
+      constraint->getSecondarySlipCompliance() *
+      static_cast<double>(numContactsOnCollisionObject));
+
+  return constraint;
 }
 
 //==============================================================================
diff --git a/dart/constraint/ContactSurface.hpp b/dart/constraint/ContactSurface.hpp
index 85c3a5b4aa9a3..3707fefb1807f 100644
--- a/dart/constraint/ContactSurface.hpp
+++ b/dart/constraint/ContactSurface.hpp
@@ -98,18 +98,19 @@ class ContactSurfaceHandler
 
   /// Create parameters of the contact constraint. This method should combine
   /// the collision properties of the two colliding bodies and write the
-  /// combined result in the returned object.
+  /// combined result in the returned object. It is also passed the total number
+  /// of contact points reported on the same collision body - that is useful
+  /// e.g. for normalization of forces by the number of contact points.
   virtual ContactSurfaceParams createParams(
-      const collision::Contact& contact) const = 0;
-
-  /// Called after the contact constrain object is constructed to allow
-  /// additional configuration. It is also passed the total number of contact
-  /// points reported on the same collision body - that is useful e.g. for
-  /// normalization of forces by the number of contact points.
-  virtual void updateConstraint(
-      ContactConstraint& constraint,
       const collision::Contact& contact,
-      size_t numContactsOnCollisionObject) const = 0;
+      size_t numContactsOnCollisionObject) const;
+
+  /// Create the constraint that represents contact between two collision
+  /// objects.
+  virtual ContactConstraintPtr createConstraint(
+      collision::Contact& contact,
+      size_t numContactsOnCollisionObject,
+      double timeStep) const;
 
   /// Set the optional parent handler (ignored if parent.get() == this)
   void setParent(ContactSurfaceHandlerPtr parent);
@@ -129,14 +130,15 @@ class DefaultContactSurfaceHandler : public ContactSurfaceHandler
 public:
   // Documentation inherited
   ContactSurfaceParams createParams(
-      const collision::Contact& contact) const override;
-
-  // Documentation inherited
-  void updateConstraint(
-      ContactConstraint& constraint,
       const collision::Contact& contact,
       size_t numContactsOnCollisionObject) const override;
 
+  // Documentation inherited
+  ContactConstraintPtr createConstraint(
+      collision::Contact& contact,
+      size_t numContactsOnCollisionObject,
+      double timeStep) const override;
+
 protected:
   static double computeFrictionCoefficient(
       const dynamics::ShapeNode* shapeNode);

From e95a63e2fd138b5e13295c3871bddf1b3109fbb5 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Sat, 3 Jul 2021 10:20:11 +0200
Subject: [PATCH 4/7] Retain ABI compatibility.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 dart/constraint/ConstraintSolver.cpp  | 64 +++++++++++++++++-----
 dart/constraint/ConstraintSolver.hpp  |  5 +-
 dart/constraint/ContactConstraint.cpp | 79 +++++++++++++++++++++++++--
 dart/constraint/ContactConstraint.hpp | 24 +++++++-
 dart/constraint/ContactSurface.hpp    |  2 +
 5 files changed, 151 insertions(+), 23 deletions(-)

diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 312cfd54ed94f..93027ea39ed90 100644
--- 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"
@@ -59,14 +61,17 @@ namespace constraint {
 
 using namespace dynamics;
 
+std::mutex gContactSurfaceHandlersMutex;
+std::unordered_map<const ConstraintSolver*, ContactSurfaceHandlerPtr>
+    gContactSurfaceHandlers;
+
 //==============================================================================
 ConstraintSolver::ConstraintSolver(double timeStep)
   : mCollisionDetector(collision::FCLCollisionDetector::create()),
     mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()),
     mCollisionOption(collision::CollisionOption(
         true, 1000u, std::make_shared<collision::BodyNodeCollisionFilter>())),
-    mTimeStep(timeStep),
-    mContactSurfaceHandler(std::make_shared<DefaultContactSurfaceHandler>())
+    mTimeStep(timeStep)
 {
   assert(timeStep > 0.0);
 
@@ -77,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>();
+  }
 }
 
 //==============================================================================
@@ -85,8 +96,7 @@ ConstraintSolver::ConstraintSolver()
     mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()),
     mCollisionOption(collision::CollisionOption(
         true, 1000u, std::make_shared<collision::BodyNodeCollisionFilter>())),
-    mTimeStep(0.001),
-    mContactSurfaceHandler(std::make_shared<DefaultContactSurfaceHandler>())
+    mTimeStep(0.001)
 {
   auto cd = std::static_pointer_cast<collision::FCLCollisionDetector>(
       mCollisionDetector);
@@ -95,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);
 }
 
 //==============================================================================
@@ -393,7 +416,10 @@ void ConstraintSolver::setFromOtherConstraintSolver(
   addSkeletons(other.getSkeletons());
   mManualConstraints = other.mManualConstraints;
 
-  mContactSurfaceHandler = other.mContactSurfaceHandler;
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    gContactSurfaceHandlers[this] = gContactSurfaceHandlers[&other];
+  }
 }
 
 //==============================================================================
@@ -561,8 +587,13 @@ void ConstraintSolver::updateConstraints()
     if (it != contactPairMap.end())
       numContacts = it->second;
 
-    auto contactConstraint = mContactSurfaceHandler->createConstraint(
-        *contact, numContacts, mTimeStep);
+    ContactConstraintPtr contactConstraint;
+    {
+      std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+      contactConstraint = gContactSurfaceHandlers[this]->createConstraint(
+          *contact, numContacts, mTimeStep);
+    }
+
     mContactConstraints.push_back(contactConstraint);
 
     contactConstraint->update();
@@ -756,15 +787,21 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const
 ContactSurfaceHandlerPtr
 ConstraintSolver::getContactSurfaceHandler() const
 {
-  return mContactSurfaceHandler;
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    return gContactSurfaceHandlers[this];
+  }
 }
 
 //==============================================================================
 void ConstraintSolver::setContactSurfaceHandler(
     ContactSurfaceHandlerPtr handler)
 {
-  handler->setParent(mContactSurfaceHandler);
-  mContactSurfaceHandler = std::move(handler);
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    handler->setParent(gContactSurfaceHandlers[this]);
+    gContactSurfaceHandlers[this] = std::move(handler);
+  }
 }
 
 //==============================================================================
@@ -772,7 +809,8 @@ bool ConstraintSolver::removeContactSurfaceHandler(
     const ContactSurfaceHandlerPtr& handler)
 {
   bool found = false;
-  ContactSurfaceHandlerPtr current = mContactSurfaceHandler;
+  std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+  ContactSurfaceHandlerPtr current = gContactSurfaceHandlers[this];
   ContactSurfaceHandlerPtr previous = nullptr;
   while (current != nullptr)
   {
@@ -781,7 +819,7 @@ bool ConstraintSolver::removeContactSurfaceHandler(
       if (previous != nullptr)
         previous->mParent = current->mParent;
       else
-        mContactSurfaceHandler = current->mParent;
+        gContactSurfaceHandlers[this] = current->mParent;
       found = true;
       break;
     }
@@ -789,7 +827,7 @@ bool ConstraintSolver::removeContactSurfaceHandler(
     current = current->mParent;
   }
 
-  if (mContactSurfaceHandler == nullptr)
+  if (gContactSurfaceHandlers[this] == nullptr)
     dterr << "No contact surface handler remained. This is an error. Add at "
           << "least DefaultContactSurfaceHandler." << std::endl;
 
diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp
index 9b41e66f879f6..55c147b7094bc 100644
--- 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);
@@ -279,7 +279,8 @@ class ConstraintSolver
   std::vector<ConstrainedGroup> mConstrainedGroups;
 
   /// Factory for ContactSurfaceParams for each contact
-  ContactSurfaceHandlerPtr mContactSurfaceHandler;
+  // Moved to gContactSurfaceHandlers in .cpp file to preserve ABI
+  // ContactSurfaceHandlerPtr mContactSurfaceHandler;
 };
 
 } // namespace constraint
diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp
index a8c4fc3642de9..3f0cae37f8e86 100644
--- a/dart/constraint/ContactConstraint.cpp
+++ b/dart/constraint/ContactConstraint.cpp
@@ -33,6 +33,8 @@
 #include "dart/constraint/ContactConstraint.hpp"
 
 #include <iostream>
+#include <mutex>
+#include <unordered_map>
 
 #include "dart/external/odelcpsolver/lcp.h"
 
@@ -57,6 +59,10 @@ double ContactConstraint::mErrorReductionParameter = DART_ERP;
 double ContactConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV;
 double ContactConstraint::mConstraintForceMixing = DART_CFM;
 
+std::mutex gContactSurfaceMotionVelocitiesMutex;
+std::unordered_map<ContactConstraint*, Eigen::Vector3d>
+    gContactSurfaceMotionVelocities;
+
 //==============================================================================
 ContactConstraint::ContactConstraint(
     collision::Contact& contact, double timeStep) :
@@ -126,8 +132,11 @@ ContactConstraint::ContactConstraint(
     mIsFrictionOn = false;
   }
 
-  mContactSurfaceMotionVelocity =
-      contactSurfaceParams.mContactSurfaceMotionVelocity;
+  {
+    std::lock_guard<std::mutex> lock(gContactSurfaceMotionVelocitiesMutex);
+    gContactSurfaceMotionVelocities[this] =
+        contactSurfaceParams.mContactSurfaceMotionVelocity;
+  }
 
   assert(mBodyNodeA->getSkeleton());
   assert(mBodyNodeB->getSkeleton());
@@ -233,6 +242,13 @@ ContactConstraint::ContactConstraint(
   }
 }
 
+//==============================================================================
+ContactConstraint::~ContactConstraint()
+{
+  std::lock_guard<std::mutex> lock(gContactSurfaceMotionVelocitiesMutex);
+  gContactSurfaceMotionVelocities.erase(this);
+}
+
 //==============================================================================
 void ContactConstraint::setErrorAllowance(double allowance)
 {
@@ -411,9 +427,13 @@ void ContactConstraint::getInformation(ConstraintInfo* info)
     }
 
     info->b[0] += bouncingVelocity;
-    info->b[0] += mContactSurfaceMotionVelocity.x();
-    info->b[1] += mContactSurfaceMotionVelocity.y();
-    info->b[2] += mContactSurfaceMotionVelocity.z();
+    {
+      std::lock_guard<std::mutex> lock(gContactSurfaceMotionVelocitiesMutex);
+      const auto& surfaceVelocity = gContactSurfaceMotionVelocities[this];
+      info->b[0] += surfaceVelocity.x();
+      info->b[1] += surfaceVelocity.y();
+      info->b[2] += surfaceVelocity.z();
+    }
 
     // TODO(JS): Initial guess
     // x
@@ -469,7 +489,10 @@ void ContactConstraint::getInformation(ConstraintInfo* info)
     }
 
     info->b[0] += bouncingVelocity;
-    info->b[0] += mContactSurfaceMotionVelocity.x();
+    {
+      std::lock_guard<std::mutex> lock(gContactSurfaceMotionVelocitiesMutex);
+      info->b[0] += gContactSurfaceMotionVelocities[this].x();
+    }
 
     // TODO(JS): Initial guess
     // x
@@ -677,6 +700,50 @@ bool ContactConstraint::isActive() const
   return mActive;
 }
 
+//==============================================================================
+double ContactConstraint::computeFrictionCoefficient(
+    const dynamics::ShapeNode* shapeNode)
+{
+  return DefaultContactSurfaceHandler::computeFrictionCoefficient(shapeNode);
+}
+
+//==============================================================================
+double ContactConstraint::computeSecondaryFrictionCoefficient(
+    const dynamics::ShapeNode* shapeNode)
+{
+  return DefaultContactSurfaceHandler::computeSecondaryFrictionCoefficient(
+      shapeNode);
+}
+
+//==============================================================================
+double ContactConstraint::computeSlipCompliance(
+    const dynamics::ShapeNode* shapeNode)
+{
+  return DefaultContactSurfaceHandler::computeSlipCompliance(shapeNode);
+}
+
+//==============================================================================
+double ContactConstraint::computeSecondarySlipCompliance(
+    const dynamics::ShapeNode* shapeNode)
+{
+  return DefaultContactSurfaceHandler::computeSecondarySlipCompliance(
+      shapeNode);
+}
+
+//==============================================================================
+Eigen::Vector3d ContactConstraint::computeWorldFirstFrictionDir(
+    const dynamics::ShapeNode* shapeNode)
+{
+  return DefaultContactSurfaceHandler::computeWorldFirstFrictionDir(shapeNode);
+}
+
+//==============================================================================
+double ContactConstraint::computeRestitutionCoefficient(
+    const dynamics::ShapeNode* shapeNode)
+{
+  return DefaultContactSurfaceHandler::computeRestitutionCoefficient(shapeNode);
+}
+
 //==============================================================================
 dynamics::SkeletonPtr ContactConstraint::getRootSkeleton() const
 {
diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp
index 82bd4be90dfbe..56149c6f62c45 100644
--- a/dart/constraint/ContactConstraint.hpp
+++ b/dart/constraint/ContactConstraint.hpp
@@ -59,7 +59,7 @@ class ContactConstraint : public ConstraintBase
   ContactConstraint(collision::Contact& contact, double timeStep);
 
   /// Destructor
-  ~ContactConstraint() override = default;
+  ~ContactConstraint() override;
 
   //----------------------------------------------------------------------------
   // Property settings
@@ -138,6 +138,25 @@ class ContactConstraint : public ConstraintBase
   // Documentation inherited
   bool isActive() const override;
 
+  DART_DEPRECATED(6.10)
+  static double computeFrictionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+  DART_DEPRECATED(6.10)
+  static double computeSecondaryFrictionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+  DART_DEPRECATED(6.10)
+  static double computeSlipCompliance(
+      const dynamics::ShapeNode* shapeNode);
+  DART_DEPRECATED(6.10)
+  static double computeSecondarySlipCompliance(
+      const dynamics::ShapeNode* shapeNode);
+  DART_DEPRECATED(6.10)
+  static Eigen::Vector3d computeWorldFirstFrictionDir(
+      const dynamics::ShapeNode* shapenode);
+  DART_DEPRECATED(6.10)
+  static double computeRestitutionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+
 private:
   using TangentBasisMatrix = Eigen::Matrix<double, 3, 2>;
 
@@ -207,7 +226,8 @@ class ContactConstraint : public ConstraintBase
   /// x = vel. in direction of contact normal
   /// y = vel. in first friction direction
   /// z = vel. in second friction direction
-  Eigen::Vector3d mContactSurfaceMotionVelocity;
+  // Moved to gContactSurfaceMotionVelocities in .cpp to preserve ABI
+  //Eigen::Vector3d mContactSurfaceMotionVelocity;
 
   /// Whether this contact is self-collision.
   bool mIsSelfCollision;
diff --git a/dart/constraint/ContactSurface.hpp b/dart/constraint/ContactSurface.hpp
index 3707fefb1807f..751994e29e03d 100644
--- a/dart/constraint/ContactSurface.hpp
+++ b/dart/constraint/ContactSurface.hpp
@@ -152,6 +152,8 @@ class DefaultContactSurfaceHandler : public ContactSurfaceHandler
       const dynamics::ShapeNode* shapenode);
   static double computeRestitutionCoefficient(
       const dynamics::ShapeNode* shapeNode);
+
+  friend class ContactConstraint;
 };
 
 } // namespace constraint

From 11bc2dbc53376b81310b2649291c36c84fc91b0b Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Tue, 13 Jul 2021 11:31:49 +0200
Subject: [PATCH 5/7] Improve comment.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 dart/constraint/ContactSurface.hpp | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/dart/constraint/ContactSurface.hpp b/dart/constraint/ContactSurface.hpp
index 751994e29e03d..29c2c4b9eb082 100644
--- a/dart/constraint/ContactSurface.hpp
+++ b/dart/constraint/ContactSurface.hpp
@@ -83,17 +83,16 @@ struct ContactSurfaceParams
   Eigen::Vector3d mContactSurfaceMotionVelocity {DART_DEFAULT_CONTACT_SURFACE_MOTION_VELOCITY};
 };
 
-class ContactConstraint;
-
 /// Class used to determine the properties of a contact constraint based on the
 /// two colliding bodies and information about their contact.
 class ContactSurfaceHandler
 {
 public:
   /// Constructor
-  /// \param[in] parent Optional parent handler. It suggested that new handlers
-  ///                   should not overwrite the previous ones, but rather
-  ///                   "extend" them.
+  /// \param[in] parent Optional parent handler. In ConstraintSolver, the parent
+  ///                   handler is automatically set to the previous handler
+  ///                   when adding a new one. It is suggested to keep this
+  ///                   paradigm if used elsewhere.
   explicit ContactSurfaceHandler(ContactSurfaceHandlerPtr parent = nullptr);
 
   /// Create parameters of the contact constraint. This method should combine

From 8f7c9fccc5d104503f5489d6a9aa324861dc14ab Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Tue, 13 Jul 2021 12:19:37 +0200
Subject: [PATCH 6/7] Added comments to ABI hacks.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 dart/constraint/ConstraintSolver.cpp  | 2 ++
 dart/constraint/ContactConstraint.cpp | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 93027ea39ed90..fbe0fa7045b68 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -61,6 +61,8 @@ 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;
diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp
index 3f0cae37f8e86..a350e85d31deb 100644
--- a/dart/constraint/ContactConstraint.cpp
+++ b/dart/constraint/ContactConstraint.cpp
@@ -59,6 +59,8 @@ double ContactConstraint::mErrorReductionParameter = DART_ERP;
 double ContactConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV;
 double ContactConstraint::mConstraintForceMixing = DART_CFM;
 
+// These two globals are a hack made to retain ABI compatibility.
+// TODO(anyone): Revert e95a6 in a future ABI-breaking version.
 std::mutex gContactSurfaceMotionVelocitiesMutex;
 std::unordered_map<ContactConstraint*, Eigen::Vector3d>
     gContactSurfaceMotionVelocities;

From e28afda01f42e55e1df0c1996e14892a8ca8da4b Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Sat, 31 Jul 2021 09:14:30 +0200
Subject: [PATCH 7/7] Addressed comments from review.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
---
 dart/constraint/ConstraintSolver.cpp | 35 ++++++++++++++--------------
 dart/constraint/ConstraintSolver.hpp |  4 ++--
 dart/constraint/ContactSurface.hpp   |  6 +++++
 3 files changed, 25 insertions(+), 20 deletions(-)

diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index fbe0fa7045b68..8d5565fd2543a 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -581,27 +581,26 @@ void ConstraintSolver::updateConstraints()
   }
 
   // Add the new contact constraints to dynamic constraint list
-  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;
-
-    ContactConstraintPtr contactConstraint;
+    std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
+    for (auto* contact : contacts)
     {
-      std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
-      contactConstraint = gContactSurfaceHandlers[this]->createConstraint(
-          *contact, numContacts, mTimeStep);
-    }
+      std::size_t numContacts = 1;
+      auto it = contactPairMap.find(
+          std::make_pair(contact->collisionObject1, contact->collisionObject2));
+      if (it != contactPairMap.end())
+        numContacts = it->second;
 
-    mContactConstraints.push_back(contactConstraint);
+      auto contactConstraint = gContactSurfaceHandlers[this]->createConstraint(
+        *contact, numContacts, mTimeStep);
 
-    contactConstraint->update();
+      mContactConstraints.push_back(contactConstraint);
 
-    if (contactConstraint->isActive())
-      mActiveConstraints.push_back(contactConstraint);
+      contactConstraint->update();
+
+      if (contactConstraint->isActive())
+        mActiveConstraints.push_back(contactConstraint);
+    }
   }
 
   // Add the new soft contact constraints to dynamic constraint list
@@ -787,7 +786,7 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const
 
 //==============================================================================
 ContactSurfaceHandlerPtr
-ConstraintSolver::getContactSurfaceHandler() const
+ConstraintSolver::getLastContactSurfaceHandler() const
 {
   {
     std::lock_guard<std::mutex> lock(gContactSurfaceHandlersMutex);
@@ -796,7 +795,7 @@ ConstraintSolver::getContactSurfaceHandler() const
 }
 
 //==============================================================================
-void ConstraintSolver::setContactSurfaceHandler(
+void ConstraintSolver::addContactSurfaceHandler(
     ContactSurfaceHandlerPtr handler)
 {
   {
diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp
index 55c147b7094bc..3d61bbddc1bb6 100644
--- a/dart/constraint/ConstraintSolver.hpp
+++ b/dart/constraint/ConstraintSolver.hpp
@@ -186,12 +186,12 @@ class ConstraintSolver
 
   /// Get the handler used for computing contact surface parameters based on
   /// the contact properties of the two colliding bodies.
-  ContactSurfaceHandlerPtr getContactSurfaceHandler() const;
+  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 setContactSurfaceHandler(ContactSurfaceHandlerPtr 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
diff --git a/dart/constraint/ContactSurface.hpp b/dart/constraint/ContactSurface.hpp
index 29c2c4b9eb082..ee709ff4ac1a0 100644
--- a/dart/constraint/ContactSurface.hpp
+++ b/dart/constraint/ContactSurface.hpp
@@ -81,6 +81,12 @@ struct ContactSurfaceParams
   /// y = vel. in first friction direction
   /// z = vel. in second friction direction
   Eigen::Vector3d mContactSurfaceMotionVelocity {DART_DEFAULT_CONTACT_SURFACE_MOTION_VELOCITY};
+
+private:
+  /// Usued for future-compatibility. Add any newly added fields here so that
+  /// ABI doesn't change. The data should be accessed via non-virtual getters
+  /// and setters added to this struct.
+  void* mExtraData {nullptr};
 };
 
 /// Class used to determine the properties of a contact constraint based on the