Add secondary friction coefficient parameter

CHANGELOG.md

@@ -17,6 +17,7 @@
   * Added joint velocity limit constraint support: [#1407](https://github.com/dartsim/dart/pull/1407)
   * Added type property to constrain classes: [#1415](https://github.com/dartsim/dart/pull/1415)
   * Allowed to set joint rest position out of joint limits: [#1418](https://github.com/dartsim/dart/pull/1418)
+  * Added secondary friction coefficient parameter: [#1424](https://github.com/dartsim/dart/pull/1424)
 
 * GUI
 

dart/constraint/ContactConstraint.cpp

@@ -114,14 +114,24 @@ ContactConstraint::ContactConstraint(
   //----------------------------------------------
   // TODO(JS): Assume the frictional coefficient can be changed during
   //           simulation steps.
-  // Update mFrictionalCoff
-  const double frictionCoeffA = computeFrictionCoefficient(shapeNodeA);
-  const double frictionCoeffB = computeFrictionCoefficient(shapeNodeB);
+  // Update mFrictionCoeff
+  const double primaryFrictionCoeffA =
+                       computePrimaryFrictionCoefficient(shapeNodeA);
+  const double primaryFrictionCoeffB =
+                       computePrimaryFrictionCoefficient(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);
-  if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD)
+  mPrimaryFrictionCoeff =
+      std::min(primaryFrictionCoeffA, primaryFrictionCoeffB);
+  mSecondaryFrictionCoeff =
+      std::min(secondaryFrictionCoeffA, secondaryFrictionCoeffB);
+  if (mPrimaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD ||
+      mSecondaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD)
   {
     mIsFrictionOn = true;
 
@@ -387,13 +397,13 @@ void ContactConstraint::getInformation(ConstraintInfo* info)
     assert(info->findex[0] == -1);
 
     // Upper and lower bounds of tangential direction-1 impulsive force
-    info->lo[1] = -mFrictionCoeff;
-    info->hi[1] = mFrictionCoeff;
+    info->lo[1] = -mPrimaryFrictionCoeff;
+    info->hi[1] = mPrimaryFrictionCoeff;
     info->findex[1] = 0;
 
     // Upper and lower bounds of tangential direction-2 impulsive force
-    info->lo[2] = -mFrictionCoeff;
-    info->hi[2] = mFrictionCoeff;
+    info->lo[2] = -mSecondaryFrictionCoeff;
+    info->hi[2] = mSecondaryFrictionCoeff;
     info->findex[2] = 0;
 
     //------------------------------------------------------------------------
@@ -702,6 +712,48 @@ double ContactConstraint::computeFrictionCoefficient(
   return dynamicAspect->getFrictionCoeff();
 }
 
+//==============================================================================
+double ContactConstraint::computePrimaryFrictionCoefficient(
+    const dynamics::ShapeNode* shapeNode)
+{
+  assert(shapeNode);
+
+  auto dynamicAspect = shapeNode->getDynamicsAspect();
+
+  if (dynamicAspect == nullptr)
+  {
+    dtwarn << "[ContactConstraint] Attempt to extract "
+           << "primary 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->getPrimaryFrictionCoeff();
+}
+
+//==============================================================================
+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::computeRestitutionCoefficient(
     const dynamics::ShapeNode* shapeNode)

dart/constraint/ContactConstraint.hpp

@@ -140,6 +140,10 @@ class ContactConstraint : public ConstraintBase
 
   static double computeFrictionCoefficient(
       const dynamics::ShapeNode* shapeNode);
+  static double computePrimaryFrictionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
+  static double computeSecondaryFrictionCoefficient(
+      const dynamics::ShapeNode* shapeNode);
   static double computeRestitutionCoefficient(
       const dynamics::ShapeNode* shapeNode);
 
@@ -173,8 +177,11 @@ class ContactConstraint : public ConstraintBase
   /// First frictional direction
   Eigen::Vector3d mFirstFrictionalDirection;
 
-  /// Coefficient of Friction
-  double mFrictionCoeff;
+  /// Primary Coefficient of Friction
+  double mPrimaryFrictionCoeff;
+
+  /// Primary Coefficient of Friction
+  double mSecondaryFrictionCoeff;
 
   /// Coefficient of restitution
   double mRestitutionCoeff;

dart/dynamics/ShapeFrame.cpp

@@ -55,7 +55,21 @@ CollisionAspectProperties::CollisionAspectProperties(const bool collidable)
 //==============================================================================
 DynamicsAspectProperties::DynamicsAspectProperties(
     const double frictionCoeff, const double restitutionCoeff)
-  : mFrictionCoeff(frictionCoeff), mRestitutionCoeff(restitutionCoeff)
+  : mFrictionCoeff(frictionCoeff),
+    mRestitutionCoeff(restitutionCoeff),
+    mSecondaryFrictionCoeff(frictionCoeff)
+{
+  // Do nothing
+}
+
+//==============================================================================
+DynamicsAspectProperties::DynamicsAspectProperties(
+    const double primaryFrictionCoeff,
+    const double secondaryFrictionCoeff,
+    const double restitutionCoeff)
+  : mFrictionCoeff(primaryFrictionCoeff),
+    mRestitutionCoeff(restitutionCoeff),
+    mSecondaryFrictionCoeff(secondaryFrictionCoeff)
 {
   // Do nothing
 }
@@ -173,6 +187,29 @@ DynamicsAspect::DynamicsAspect(const PropertiesData& properties)
   // Do nothing
 }
 
+void DynamicsAspect::setFrictionCoeff(const double& value)
+{
+  mProperties.mFrictionCoeff = value;
+  mProperties.mSecondaryFrictionCoeff = value;
+}
+
+double DynamicsAspect::getFrictionCoeff() const
+{
+  return 0.5 * (
+      mProperties.mFrictionCoeff +
+      mProperties.mSecondaryFrictionCoeff);
+}
+
+void DynamicsAspect::setPrimaryFrictionCoeff(const double& value)
+{
+  mProperties.mFrictionCoeff = value;
+}
+
+const double& DynamicsAspect::getPrimaryFrictionCoeff() const
+{
+  return mProperties.mFrictionCoeff;
+}
+
 //==============================================================================
 ShapeFrame::~ShapeFrame()
 {

dart/dynamics/ShapeFrame.hpp

@@ -143,9 +143,18 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties<
 
   DynamicsAspect(const PropertiesData& properties = PropertiesData());
 
-  DART_COMMON_SET_GET_ASPECT_PROPERTY(double, FrictionCoeff)
-  // void setFrictionCoeff(const double& value);
-  // const double& getFrictionCoeff() const;
+  /// Set both primary and secondary friction coefficients to the same value.
+  void setFrictionCoeff(const double& value);
+  /// Get average of primary and secondary friction coefficients.
+  double getFrictionCoeff() const;
+
+  // DART_COMMON_SET_GET_ASPECT_PROPERTY(double, PrimaryFrictionCoeff)
+  void setPrimaryFrictionCoeff(const double& value);
+  const double& getPrimaryFrictionCoeff() const;
+
+  DART_COMMON_SET_GET_ASPECT_PROPERTY(double, SecondaryFrictionCoeff)
+  // void setSecondaryFrictionCoeff(const double& value);
+  // const double& getSecondaryFrictionCoeff() const;
   DART_COMMON_SET_GET_ASPECT_PROPERTY(double, RestitutionCoeff)
   // void setRestitutionCoeff(const double& value);
   // const double& getRestitutionCoeff() const;

dart/dynamics/detail/ShapeFrameAspect.hpp

@@ -88,16 +88,25 @@ struct CollisionAspectProperties
 
 struct DynamicsAspectProperties
 {
-  /// Coefficient of friction
+  /// Primary coefficient of friction
   double mFrictionCoeff;
 
   /// Coefficient of restitution
   double mRestitutionCoeff;
 
-  /// Constructor
+  /// Secondary coefficient of friction
+  double mSecondaryFrictionCoeff;
+
+  /// Constructors
+  /// The frictionCoeff argument will be used for both primary and secondary friction
   DynamicsAspectProperties(
       const double frictionCoeff = 1.0, const double restitutionCoeff = 0.0);
 
+  DynamicsAspectProperties(
+      const double primaryFrictionCoeff,
+      const double secondaryFrictionCoeff,
+      const double restitutionCoeff);
+
   /// Destructor
   virtual ~DynamicsAspectProperties() = default;
 };

unittests/comprehensive/test_Friction.cpp

@@ -80,15 +80,46 @@ TEST(Friction, FrictionPerShapeNode)
   skeleton1->setName("Skeleton2");
 
   auto body1 = skeleton1->getRootBodyNode();
-  EXPECT_DOUBLE_EQ(
-      body1->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff(), 1.0);
+  // default friction values
+  EXPECT_DOUBLE_EQ(1.0,
+      body1->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff());
+  EXPECT_DOUBLE_EQ(1.0,
+      body1->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff());
+  EXPECT_DOUBLE_EQ(1.0,
+      body1->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff());
 
   auto body2 = skeleton2->getRootBodyNode();
-  EXPECT_DOUBLE_EQ(
-      body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff(), 1.0);
+  // default friction values
+  EXPECT_DOUBLE_EQ(1.0,
+      body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff());
+  EXPECT_DOUBLE_EQ(1.0,
+      body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff());
+  EXPECT_DOUBLE_EQ(1.0,
+      body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff());
+  // test setting primary friction
+  body2->getShapeNode(0)->getDynamicsAspect()->setPrimaryFrictionCoeff(0.5);
+  EXPECT_DOUBLE_EQ(0.75,
+      body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff());
+  EXPECT_DOUBLE_EQ(0.5,
+      body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff());
+  EXPECT_DOUBLE_EQ(1.0,
+      body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff());
+  // test setting secondary friction
+  body2->getShapeNode(0)->getDynamicsAspect()->setSecondaryFrictionCoeff(0.25);
+  EXPECT_DOUBLE_EQ(0.375,
+      body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff());
+  EXPECT_DOUBLE_EQ(0.5,
+      body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff());
+  EXPECT_DOUBLE_EQ(0.25,
+      body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff());
+  // set all friction coeffs to 0.0
   body2->getShapeNode(0)->getDynamicsAspect()->setFrictionCoeff(0.0);
-  EXPECT_DOUBLE_EQ(
-      body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff(), 0.0);
+  EXPECT_DOUBLE_EQ(0.0,
+      body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff());
+  EXPECT_DOUBLE_EQ(0.0,
+      body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff());
+  EXPECT_DOUBLE_EQ(0.0,
+      body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff());
 
   // Create a world and add the rigid body
   auto world = simulation::World::create();