Add force dependent slip (#1505)

Co-authored-by: Jeongseok Lee <jslee02@users.noreply.github.com>

dart/constraint/ConstraintSolver.cpp

@@ -32,6 +32,8 @@
 
 #include "dart/constraint/ConstraintSolver.hpp"
 
+#include <algorithm>
+
 #include "dart/collision/CollisionFilter.hpp"
 #include "dart/collision/CollisionGroup.hpp"
 #include "dart/collision/CollisionObject.hpp"
@@ -478,6 +480,29 @@ void ConstraintSolver::updateConstraints()
   // Destroy previous soft contact constraints
   mSoftContactConstraints.clear();
 
+  // Create a mapping of contact pairs to the number of contacts between them
+  using ContactPair
+      = std::pair<collision::CollisionObject*, collision::CollisionObject*>;
+
+  // Compare contact pairs while ignoring their order in the pair.
+  struct ContactPairCompare
+  {
+    ContactPair getSortedPair(const ContactPair& a) const
+    {
+      if (a.first < a.second)
+        return std::make_pair(a.second, a.first);
+      return a;
+    }
+
+    bool operator()(const ContactPair& a, const ContactPair& b) const
+    {
+      // Sort each pair and then do a lexicographical comparison
+      return getSortedPair(a) < getSortedPair(b);
+    }
+  };
+
+  std::map<ContactPair, size_t, ContactPairCompare> contactPairMap;
+
   // Create new contact constraints
   for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i)
   {
@@ -515,6 +540,10 @@ void ConstraintSolver::updateConstraints()
     }
     else
     {
+      // Increment the count of contacts between the two collision objects
+      ++contactPairMap[std::make_pair(
+          contact.collisionObject1, contact.collisionObject2)];
+
       mContactConstraints.push_back(
           std::make_shared<ContactConstraint>(contact, mTimeStep));
     }
@@ -523,6 +552,23 @@ void ConstraintSolver::updateConstraints()
   // 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;
+
+    // The slip compliance acts like a damper at each contact point so the total
+    // damping for each collision is multiplied by the number of contact points
+    // (numContacts). To eliminate this dependence on numContacts, the inverse
+    // damping is multiplied by numContacts.
+    contactConstraint->setPrimarySlipCompliance(
+        contactConstraint->getPrimarySlipCompliance() * numContacts);
+    contactConstraint->setSecondarySlipCompliance(
+        contactConstraint->getSecondarySlipCompliance() * numContacts);
     contactConstraint->update();
 
     if (contactConstraint->isActive())

dart/constraint/ContactConstraint.cpp

@@ -65,6 +65,7 @@ double ContactConstraint::mConstraintForceMixing = DART_CFM;
 
 constexpr double DART_DEFAULT_FRICTION_COEFF = 1.0;
 constexpr double DART_DEFAULT_RESTITUTION_COEFF = 0.0;
+constexpr double DART_DEFAULT_SLIP_COMPLIANCE = 0.0;
 const Eigen::Vector3d DART_DEFAULT_FRICTION_DIR = Eigen::Vector3d::UnitZ();
 
 //==============================================================================
@@ -84,6 +85,8 @@ ContactConstraint::ContactConstraint(
                    .get()),
     mContact(contact),
     mFirstFrictionalDirection(DART_DEFAULT_FRICTION_DIR),
+    mPrimarySlipCompliance(DART_DEFAULT_SLIP_COMPLIANCE),
+    mSecondarySlipCompliance(DART_DEFAULT_SLIP_COMPLIANCE),
     mIsFrictionOn(true),
     mAppliedImpulseIndex(dynamics::INVALID_INDEX),
     mIsBounceOn(false),
@@ -136,6 +139,20 @@ ContactConstraint::ContactConstraint(
   {
     mIsFrictionOn = true;
 
+    // Compute slip compliance
+    const double primarySlipComplianceA
+        = computePrimarySlipCompliance(shapeNodeA);
+    const double primarySlipComplianceB
+        = computePrimarySlipCompliance(shapeNodeB);
+    const double secondarySlipComplianceA
+        = computeSecondarySlipCompliance(shapeNodeA);
+    const double secondarySlipComplianceB
+        = computeSecondarySlipCompliance(shapeNodeB);
+    // Combine slip compliances through addition
+    mPrimarySlipCompliance = primarySlipComplianceA + primarySlipComplianceB;
+    mSecondarySlipCompliance
+        = secondarySlipComplianceA + secondarySlipComplianceB;
+
     // Check shapeNodes for valid friction direction unit vectors
     auto frictionDirA = computeWorldFirstFrictionDir(shapeNodeA);
     auto frictionDirB = computeWorldFirstFrictionDir(shapeNodeB);
@@ -627,6 +644,17 @@ void ContactConstraint::getVelocityChange(double* vel, bool withCfm)
   {
     vel[mAppliedImpulseIndex]
         += vel[mAppliedImpulseIndex] * mConstraintForceMixing;
+    switch (mAppliedImpulseIndex)
+    {
+      case 1:
+        vel[1] += (mPrimarySlipCompliance / mTimeStep);
+        break;
+      case 2:
+        vel[2] += (mSecondarySlipCompliance / mTimeStep);
+        break;
+      default:
+        break;
+    }
   }
 }
 
@@ -786,6 +814,57 @@ double ContactConstraint::computeSecondaryFrictionCoefficient(
   return dynamicAspect->getSecondaryFrictionCoeff();
 }
 
+//==============================================================================
+double ContactConstraint::computePrimarySlipCompliance(
+    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->getPrimarySlipCompliance();
+  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)
@@ -950,5 +1029,35 @@ void ContactConstraint::uniteSkeletons()
   }
 }
 
+//==============================================================================
+double ContactConstraint::getPrimarySlipCompliance() const
+{
+  return mPrimarySlipCompliance;
+}
+
+//==============================================================================
+void ContactConstraint::setPrimarySlipCompliance(double slip)
+{
+  mPrimarySlipCompliance = slip;
+}
+
+//==============================================================================
+double ContactConstraint::getSecondarySlipCompliance() const
+{
+  return mSecondarySlipCompliance;
+}
+
+//==============================================================================
+void ContactConstraint::setSecondarySlipCompliance(double slip)
+{
+  mSecondarySlipCompliance = slip;
+}
+
+//==============================================================================
+const collision::Contact& ContactConstraint::getContact() const
+{
+  return mContact;
+}
+
 } // namespace constraint
 } // namespace dart

dart/constraint/ContactConstraint.hpp

@@ -144,6 +144,10 @@ class ContactConstraint : public ConstraintBase
       const dynamics::ShapeNode* shapeNode);
   static double computeSecondaryFrictionCoefficient(
       const dynamics::ShapeNode* shapeNode);
+  static double computePrimarySlipCompliance(
+      const dynamics::ShapeNode* shapeNode);
+  static double computeSecondarySlipCompliance(
+      const dynamics::ShapeNode* shapeNode);
   static Eigen::Vector3d computeWorldFirstFrictionDir(
       const dynamics::ShapeNode* shapenode);
   static double computeRestitutionCoefficient(
@@ -163,6 +167,26 @@ class ContactConstraint : public ConstraintBase
   ///
   TangentBasisMatrix getTangentBasisMatrixODE(const Eigen::Vector3d& n);
 
+  // The following functions for getting and setting slip compliance and
+  // accessing the contact object are meant to be used by ConstraintSolver to
+  // update the slip compliances based on the number of contacts between the
+  // collision objects.
+  //
+  /// Get primary slip compliance
+  double getPrimarySlipCompliance() const;
+
+  /// Set primary slip compliance
+  void setPrimarySlipCompliance(double slip);
+
+  /// Get secondary slip compliance
+  double getSecondarySlipCompliance() const;
+
+  /// Set secondary slip compliance
+  void setSecondarySlipCompliance(double slip);
+
+  /// Get contact object associated witht this constraint
+  const collision::Contact& getContact() const;
+
 private:
   /// Time step
   double mTimeStep;
@@ -185,6 +209,12 @@ class ContactConstraint : public ConstraintBase
   /// Primary Coefficient of Friction
   double mSecondaryFrictionCoeff;
 
+  /// Primary Coefficient of Slip Compliance
+  double mPrimarySlipCompliance;
+
+  /// Secondary Coefficient of Slip Compliance
+  double mSecondarySlipCompliance;
+
   /// Coefficient of restitution
   double mRestitutionCoeff;
 

dart/dynamics/ShapeFrame.cpp

@@ -58,6 +58,8 @@ DynamicsAspectProperties::DynamicsAspectProperties(
   : mFrictionCoeff(frictionCoeff),
     mRestitutionCoeff(restitutionCoeff),
     mSecondaryFrictionCoeff(frictionCoeff),
+    mPrimarySlipCompliance(-1.0),
+    mSecondarySlipCompliance(-1.0),
     mFirstFrictionDirection(Eigen::Vector3d::Zero()),
     mFirstFrictionDirectionFrame(nullptr)
 {
@@ -69,11 +71,15 @@ DynamicsAspectProperties::DynamicsAspectProperties(
     const double primaryFrictionCoeff,
     const double secondaryFrictionCoeff,
     const double restitutionCoeff,
+    const double primarySlipCompliance,
+    const double secondarySlipCompliance,
     const Eigen::Vector3d& firstFrictionDirection,
     const Frame* firstFrictionDirectionFrame)
   : mFrictionCoeff(primaryFrictionCoeff),
     mRestitutionCoeff(restitutionCoeff),
     mSecondaryFrictionCoeff(secondaryFrictionCoeff),
+    mPrimarySlipCompliance(primarySlipCompliance),
+    mSecondarySlipCompliance(secondarySlipCompliance),
     mFirstFrictionDirection(firstFrictionDirection),
     mFirstFrictionDirectionFrame(firstFrictionDirectionFrame)
 {

dart/dynamics/ShapeFrame.hpp

@@ -159,6 +159,18 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties<
   // void setRestitutionCoeff(const double& value);
   // const double& getRestitutionCoeff() const;
 
+  /// Slip compliance parameters act as constraint force mixing (cfm)
+  /// for the friction constraints.
+  /// They start with a default value of -1.0 and will be ignored
+  /// in favor of the global default value unless explicitly
+  /// set to a positive value.
+  DART_COMMON_SET_GET_ASPECT_PROPERTY(double, PrimarySlipCompliance)
+  // void sePrimarytSlipCompliance(const double& value);
+  // const double& getPrimarySlipCompliance() const;
+  DART_COMMON_SET_GET_ASPECT_PROPERTY(double, SecondarySlipCompliance)
+  // void setSecondarySlipCompliance(const double& value);
+  // const double& getSecondarySlipCompliance() const;
+
   /// Set the frame for interpreting the first friction direction vector.
   /// The frame pointer defaults to nullptr, which is interpreted as this
   /// ShapeFrame.

dart/dynamics/detail/ShapeFrameAspect.hpp

@@ -97,6 +97,12 @@ struct DynamicsAspectProperties
   /// Secondary coefficient of friction
   double mSecondaryFrictionCoeff;
 
+  /// Primary slip compliance coefficient
+  double mPrimarySlipCompliance;
+
+  /// Secondary slip compliance coefficient
+  double mSecondarySlipCompliance;
+
   /// First friction direction unit vector
   Eigen::Vector3d mFirstFrictionDirection;
 
@@ -128,6 +134,8 @@ struct DynamicsAspectProperties
       const double primaryFrictionCoeff,
       const double secondaryFrictionCoeff,
       const double restitutionCoeff,
+      const double primarySlipCompliance = -1.0,
+      const double secondarySlipCompliance = -1.0,
       const Eigen::Vector3d& firstFrictionDirection = Eigen::Vector3d::Zero(),
       const Frame* firstFrictionDirectionFrame = nullptr);
 

unittests/comprehensive/CMakeLists.txt

@@ -22,6 +22,13 @@ if(TARGET dart-collision-bullet)
   target_link_libraries(test_Raycast dart-collision-bullet)
 endif()
 
+if(TARGET dart-collision-ode)
+  # This test doesn't work with FCL because it converts simple shapes to meshes, which makes
+  # it difficult to come up with correct test expectations.
+  dart_add_test("comprehensive" test_ForceDependentSlip)
+  target_link_libraries(test_ForceDependentSlip dart-collision-ode)
+endif()
+
 if(TARGET dart-utils)
 
   dart_add_test("comprehensive" test_Collision)