Add friction direction unit vector and frame parameters (#1427)

* Add friction direction vector and frame parameters * compute body-fixed friction directions * Fix handling of first frictional direction * test_Friction: test friction direction parameters * ContactConstraint.cpp use const & Co-Authored-By: Jeongseok Lee <[email protected]> * ContactConstraint.cpp: print transpose Co-Authored-By: Jeongseok Lee <[email protected]> * ContactConstraint.cpp: update comment * ContactConstraint: reverse tangent direction * add documentation of friction direction params * Format code * Update changelog * Fix typo
dartsim · Dec 7, 2019 · 31a8a0f · 31a8a0f
1 parent c64a78c
commit 31a8a0f
Show file tree

Hide file tree

Showing 7 changed files with 314 additions and 57 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -18,6 +18,7 @@
   * 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)
+  * Allowed to set friction direction per ShapeFrame: [#1427](https://github.com/dartsim/dart/pull/1427)
 
 * GUI
 

diff --git a/dart/constraint/ContactConstraint.cpp b/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;
+const Eigen::Vector3d DART_DEFAULT_FRICTION_DIR = Eigen::Vector3d::UnitZ();
 
 //==============================================================================
 ContactConstraint::ContactConstraint(
@@ -82,7 +83,7 @@ ContactConstraint::ContactConstraint(
                    ->getBodyNodePtr()
                    .get()),
     mContact(contact),
-    mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()),
+    mFirstFrictionalDirection(DART_DEFAULT_FRICTION_DIR),
     mIsFrictionOn(true),
     mAppliedImpulseIndex(dynamics::INVALID_INDEX),
     mIsBounceOn(false),
@@ -115,26 +116,62 @@ ContactConstraint::ContactConstraint(
   // TODO(JS): Assume the frictional coefficient can be changed during
   //           simulation steps.
   // Update mFrictionCoeff
-  const double primaryFrictionCoeffA =
-                       computePrimaryFrictionCoefficient(shapeNodeA);
-  const double primaryFrictionCoeffB =
-                       computePrimaryFrictionCoefficient(shapeNodeB);
-  const double secondaryFrictionCoeffA =
-                       computeSecondaryFrictionCoefficient(shapeNodeA);
-  const double secondaryFrictionCoeffB =
-                       computeSecondaryFrictionCoefficient(shapeNodeB);
+  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
-  mPrimaryFrictionCoeff =
-      std::min(primaryFrictionCoeffA, primaryFrictionCoeffB);
-  mSecondaryFrictionCoeff =
-      std::min(secondaryFrictionCoeffA, secondaryFrictionCoeffB);
-  if (mPrimaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD ||
-      mSecondaryFrictionCoeff > 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;
 
+    // Check shapeNodes for valid friction direction unit vectors
+    auto frictionDirA = computeWorldFirstFrictionDir(shapeNodeA);
+    auto frictionDirB = computeWorldFirstFrictionDir(shapeNodeB);
+
+    // check if the friction direction unit vectors have been set
+    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 (primaryFrictionCoeffA <= primaryFrictionCoeffB)
+        {
+          mFirstFrictionalDirection = frictionDirA.normalized();
+        }
+        else
+        {
+          mFirstFrictionalDirection = frictionDirB.normalized();
+        }
+      }
+      else if (nonzeroDirA)
+      {
+        mFirstFrictionalDirection = frictionDirA.normalized();
+      }
+      else
+      {
+        mFirstFrictionalDirection = frictionDirB.normalized();
+      }
+    }
+
     // Update frictional direction
     updateFirstFrictionalDirection();
   }
@@ -147,11 +184,6 @@ ContactConstraint::ContactConstraint(
   assert(mBodyNodeB->getSkeleton());
   mIsSelfCollision = (mBodyNodeA->getSkeleton() == mBodyNodeB->getSkeleton());
 
-  const math::Jacobian jacA
-      = mBodyNodeA->getSkeleton()->getJacobian(mBodyNodeA);
-  const math::Jacobian jacB
-      = mBodyNodeB->getSkeleton()->getJacobian(mBodyNodeB);
-
   // Compute local contact Jacobians expressed in body frame
   if (mIsFrictionOn)
   {
@@ -547,7 +579,7 @@ void ContactConstraint::applyUnitImpulse(std::size_t index)
       // Both bodies are not reactive
       else
       {
-        // This case should not be happed
+        // This case should not be happened
         assert(0);
       }
     }
@@ -754,6 +786,36 @@ double ContactConstraint::computeSecondaryFrictionCoefficient(
   return dynamicAspect->getSecondaryFrictionCoeff();
 }
 
+//==============================================================================
+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.transpose()
+           << ") will be used instead.\n";
+    return DART_DEFAULT_FRICTION_DIR;
+  }
+
+  auto frame = dynamicAspect->getFirstFrictionDirectionFrame();
+  const 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)
@@ -810,24 +872,24 @@ ContactConstraint::getTangentBasisMatrixODE(const Eigen::Vector3d& n)
 
   // Pick an arbitrary vector to take the cross product of (in this case,
   // Z-axis)
-  Eigen::Vector3d tangent = mFirstFrictionalDirection.cross(n);
+  Eigen::Vector3d tangent = n.cross(mFirstFrictionalDirection);
 
   // TODO(JS): Modify following lines once _updateFirstFrictionalDirection() is
   //           implemented.
   // If they're too close (or opposing directions, or one of the vectors 0),
   // pick another tangent (use X-axis as arbitrary vector)
   if (tangent.squaredNorm() < DART_CONTACT_CONSTRAINT_EPSILON_SQUARED)
   {
-    tangent = Eigen::Vector3d::UnitX().cross(n);
+    tangent = n.cross(Eigen::Vector3d::UnitX());
 
     // Make sure this is not zero length, otherwise normalization will lead to
     // NaN values.
     if (tangent.squaredNorm() < DART_CONTACT_CONSTRAINT_EPSILON_SQUARED)
     {
-      tangent = Eigen::Vector3d::UnitY().cross(n);
+      tangent = n.cross(Eigen::Vector3d::UnitY());
       if (tangent.squaredNorm() < DART_CONTACT_CONSTRAINT_EPSILON_SQUARED)
       {
-        tangent = Eigen::Vector3d::UnitZ().cross(n);
+        tangent = n.cross(Eigen::Vector3d::UnitZ());
 
         // Now tangent shouldn't be zero-length unless the normal is
         // zero-length, which shouldn't the case because ConstraintSolver
@@ -850,8 +912,10 @@ ContactConstraint::getTangentBasisMatrixODE(const Eigen::Vector3d& n)
   // Note: a possible speedup is in place for mNumDir % 2 = 0
   // Each basis and its opposite belong in the matrix, so we iterate half as
   // many times
-  T.col(0) = tangent;
-  T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(0.5_pi, n)) * tangent;
+  // The first column is the same as mFirstFrictionalDirection unless
+  // mFirstFrictionalDirection is parallel to the normal
+  T.col(0) = Eigen::Quaterniond(Eigen::AngleAxisd(0.5_pi, n)) * tangent;
+  T.col(1) = tangent;
   return T;
 }
 

diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp
@@ -144,6 +144,8 @@ class ContactConstraint : public ConstraintBase
       const dynamics::ShapeNode* shapeNode);
   static double computeSecondaryFrictionCoefficient(
       const dynamics::ShapeNode* shapeNode);
+  static Eigen::Vector3d computeWorldFirstFrictionDir(
+      const dynamics::ShapeNode* shapenode);
   static double computeRestitutionCoefficient(
       const dynamics::ShapeNode* shapeNode);
 

diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp
@@ -57,7 +57,9 @@ DynamicsAspectProperties::DynamicsAspectProperties(
     const double frictionCoeff, const double restitutionCoeff)
   : mFrictionCoeff(frictionCoeff),
     mRestitutionCoeff(restitutionCoeff),
-    mSecondaryFrictionCoeff(frictionCoeff)
+    mSecondaryFrictionCoeff(frictionCoeff),
+    mFirstFrictionDirection(Eigen::Vector3d::Zero()),
+    mFirstFrictionDirectionFrame(nullptr)
 {
   // Do nothing
 }
@@ -66,10 +68,14 @@ DynamicsAspectProperties::DynamicsAspectProperties(
 DynamicsAspectProperties::DynamicsAspectProperties(
     const double primaryFrictionCoeff,
     const double secondaryFrictionCoeff,
-    const double restitutionCoeff)
+    const double restitutionCoeff,
+    const Eigen::Vector3d& firstFrictionDirection,
+    const Frame* firstFrictionDirectionFrame)
   : mFrictionCoeff(primaryFrictionCoeff),
     mRestitutionCoeff(restitutionCoeff),
-    mSecondaryFrictionCoeff(secondaryFrictionCoeff)
+    mSecondaryFrictionCoeff(secondaryFrictionCoeff),
+    mFirstFrictionDirection(firstFrictionDirection),
+    mFirstFrictionDirectionFrame(firstFrictionDirectionFrame)
 {
   // Do nothing
 }
@@ -195,9 +201,8 @@ void DynamicsAspect::setFrictionCoeff(const double& value)
 
 double DynamicsAspect::getFrictionCoeff() const
 {
-  return 0.5 * (
-      mProperties.mFrictionCoeff +
-      mProperties.mSecondaryFrictionCoeff);
+  return 0.5
+         * (mProperties.mFrictionCoeff + mProperties.mSecondaryFrictionCoeff);
 }
 
 void DynamicsAspect::setPrimaryFrictionCoeff(const double& value)
@@ -210,6 +215,18 @@ const double& DynamicsAspect::getPrimaryFrictionCoeff() const
   return mProperties.mFrictionCoeff;
 }
 
+//==============================================================================
+void DynamicsAspect::setFirstFrictionDirectionFrame(const Frame* value)
+{
+  mProperties.mFirstFrictionDirectionFrame = value;
+}
+
+//==============================================================================
+const Frame* DynamicsAspect::getFirstFrictionDirectionFrame() const
+{
+  return mProperties.mFirstFrictionDirectionFrame;
+}
+
 //==============================================================================
 ShapeFrame::~ShapeFrame()
 {

diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp
@@ -158,6 +158,18 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties<
   DART_COMMON_SET_GET_ASPECT_PROPERTY(double, RestitutionCoeff)
   // void setRestitutionCoeff(const double& value);
   // const double& getRestitutionCoeff() const;
+
+  /// Set the frame for interpreting the first friction direction vector.
+  /// The frame pointer defaults to nullptr, which is interpreted as this
+  /// ShapeFrame.
+  void setFirstFrictionDirectionFrame(const Frame* value);
+
+  /// Get the frame for the first friction direction vector.
+  const Frame* getFirstFrictionDirectionFrame() const;
+
+  DART_COMMON_SET_GET_ASPECT_PROPERTY(Eigen::Vector3d, FirstFrictionDirection)
+  // void setFirstFrictionDirection(const Eigen::Vector3d& value);
+  // const Eigen::Vector3d& getFirstFrictionDirection() const;
 };
 
 //==============================================================================

diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp
@@ -97,15 +97,39 @@ struct DynamicsAspectProperties
   /// Secondary coefficient of friction
   double mSecondaryFrictionCoeff;
 
+  /// First friction direction unit vector
+  Eigen::Vector3d mFirstFrictionDirection;
+
+  /// First friction direction frame
+  /// The first friction direction unit vector is expressed in this frame
+  const Frame* mFirstFrictionDirectionFrame;
+
   /// Constructors
-  /// The frictionCoeff argument will be used for both primary and secondary friction
+  /// The frictionCoeff argument will be used for both primary and secondary
+  /// friction
   DynamicsAspectProperties(
       const double frictionCoeff = 1.0, const double restitutionCoeff = 0.0);
 
+  /// Set primary and secondary friction and restitution coefficients.
+  /// The first friction direction vector and frame may optionally be set.
+  /// The vector defaults to a zero-vector, which will cause it to be ignored
+  /// and the global friction directions used instead.
+  /// If the vector is set to a non-zero vector, the first friction direction
+  /// for this shape is computed from this vector expressed in the frame
+  /// given by the Frame pointer. THe Frame pointer defaults to nullptr,
+  /// which is interpreted as the body-fixed frame of this Shape.
+  /// Note that if two objects with custom friction directions come into
+  /// contact, only one of the directions can be chosen.
+  /// One approach is to use the first friction direction for the ShapeNode
+  /// with the smaller primary friction coefficient, since that has the
+  /// dominant effect. See the ContactConstraint implementation for
+  /// further details.
   DynamicsAspectProperties(
       const double primaryFrictionCoeff,
       const double secondaryFrictionCoeff,
-      const double restitutionCoeff);
+      const double restitutionCoeff,
+      const Eigen::Vector3d& firstFrictionDirection = Eigen::Vector3d::Zero(),
+      const Frame* firstFrictionDirectionFrame = nullptr);
 
   /// Destructor
   virtual ~DynamicsAspectProperties() = default;