From 1d58bb7a5626955aaeb9d617a811d55006d7ecac Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Sun, 10 Nov 2019 23:22:41 -0800 Subject: [PATCH 01/11] ShapeFrame DynamicsAspect: add secondary friction --- dart/dynamics/ShapeFrame.cpp | 6 ++++-- dart/dynamics/ShapeFrame.hpp | 3 +++ dart/dynamics/detail/ShapeFrameAspect.hpp | 8 ++++++-- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index ac35a13ff336b..b563da91f2d5e 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -54,8 +54,10 @@ CollisionAspectProperties::CollisionAspectProperties(const bool collidable) //============================================================================== DynamicsAspectProperties::DynamicsAspectProperties( - const double frictionCoeff, const double restitutionCoeff) - : mFrictionCoeff(frictionCoeff), mRestitutionCoeff(restitutionCoeff) + const double frictionCoeff, const double restitutionCoeff, + const double secondaryFrictionCoeff) + : mFrictionCoeff(frictionCoeff), mRestitutionCoeff(restitutionCoeff), + mSecondaryFrictionCoeff(secondaryFrictionCoeff) { // Do nothing } diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index 0d2a489149250..1c162170cdb4b 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -149,6 +149,9 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties< DART_COMMON_SET_GET_ASPECT_PROPERTY(double, RestitutionCoeff) // void setRestitutionCoeff(const double& value); // const double& getRestitutionCoeff() const; + DART_COMMON_SET_GET_ASPECT_PROPERTY( double, SecondaryFrictionCoeff ) + // void setSecondaryFrictionCoeff(const double& value); + // const double& getSecondaryFrictionCoeff() const; }; //============================================================================== diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp index 0100c805b8e04..7cd4c696c26de 100644 --- a/dart/dynamics/detail/ShapeFrameAspect.hpp +++ b/dart/dynamics/detail/ShapeFrameAspect.hpp @@ -88,15 +88,19 @@ struct CollisionAspectProperties struct DynamicsAspectProperties { - /// Coefficient of friction + /// Primary coefficient of friction double mFrictionCoeff; /// Coefficient of restitution double mRestitutionCoeff; + /// Secondary coefficient of friction + double mSecondaryFrictionCoeff; + /// Constructor DynamicsAspectProperties( - const double frictionCoeff = 1.0, const double restitutionCoeff = 0.0); + const double frictionCoeff = 1.0, const double restitutionCoeff = 0.0, + const double secondaryFrictionCoeff = 1.0); /// Destructor virtual ~DynamicsAspectProperties() = default; From 40767368ecbf7c694a21a1b75c51d9f67bc8ef5f Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Fri, 12 Jul 2019 14:36:40 -0700 Subject: [PATCH 02/11] ContactConstraint: use secondary friction coeff --- dart/constraint/ContactConstraint.cpp | 35 ++++++++++++++++++++++++--- dart/constraint/ContactConstraint.hpp | 7 +++++- 2 files changed, 37 insertions(+), 5 deletions(-) diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 0a24a3e6ab877..9e8c7e02405c1 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -114,14 +114,21 @@ ContactConstraint::ContactConstraint( //---------------------------------------------- // TODO(JS): Assume the frictional coefficient can be changed during // simulation steps. - // Update mFrictionalCoff + // 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); - if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD) + mSecondaryFrictionCoeff = + std::min(secondaryFrictionCoeffA, secondaryFrictionCoeffB); + if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD || + mSecondaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD) { mIsFrictionOn = true; @@ -392,8 +399,8 @@ void ContactConstraint::getInformation(ConstraintInfo* info) 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 +709,26 @@ double ContactConstraint::computeFrictionCoefficient( return dynamicAspect->getFrictionCoeff(); } +//============================================================================== +double ContactConstraint::computeSecondaryFrictionCoefficient( + 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->getSecondaryFrictionCoeff(); +} + //============================================================================== double ContactConstraint::computeRestitutionCoefficient( const dynamics::ShapeNode* shapeNode) diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index 7a9632c75e473..a9c60475d1bc1 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -140,6 +140,8 @@ class ContactConstraint : public ConstraintBase static double computeFrictionCoefficient( const dynamics::ShapeNode* shapeNode); + static double computeSecondaryFrictionCoefficient( + const dynamics::ShapeNode* shapeNode); static double computeRestitutionCoefficient( const dynamics::ShapeNode* shapeNode); @@ -173,9 +175,12 @@ class ContactConstraint : public ConstraintBase /// First frictional direction Eigen::Vector3d mFirstFrictionalDirection; - /// Coefficient of Friction + /// Primary Coefficient of Friction double mFrictionCoeff; + /// Primary Coefficient of Friction + double mSecondaryFrictionCoeff; + /// Coefficient of restitution double mRestitutionCoeff; From b14467849aa3b07fee6c1ab0de7a82a568bc594b Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 12 Nov 2019 12:12:54 -0800 Subject: [PATCH 03/11] ShapeFrame: set both coeffs with setFrictionCoeff Rename mFrictionCoeff to mPrimaryFrictionCoeff and replace auto-generated (set|get)FrictionCoeff with custom functions that set both coefficients and return the average of the coefficients. --- dart/dynamics/ShapeFrame.cpp | 19 +++++++++++++++++-- dart/dynamics/ShapeFrame.hpp | 17 +++++++++++------ dart/dynamics/detail/ShapeFrameAspect.hpp | 5 +++-- 3 files changed, 31 insertions(+), 10 deletions(-) diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index b563da91f2d5e..883ecf7c9b16d 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -54,9 +54,11 @@ CollisionAspectProperties::CollisionAspectProperties(const bool collidable) //============================================================================== DynamicsAspectProperties::DynamicsAspectProperties( - const double frictionCoeff, const double restitutionCoeff, + const double primaryFrictionCoeff, + const double restitutionCoeff, const double secondaryFrictionCoeff) - : mFrictionCoeff(frictionCoeff), mRestitutionCoeff(restitutionCoeff), + : mPrimaryFrictionCoeff(primaryFrictionCoeff), + mRestitutionCoeff(restitutionCoeff), mSecondaryFrictionCoeff(secondaryFrictionCoeff) { // Do nothing @@ -175,6 +177,19 @@ DynamicsAspect::DynamicsAspect(const PropertiesData& properties) // Do nothing } +void DynamicsAspect::setFrictionCoeff(const double& value) +{ + mProperties.mPrimaryFrictionCoeff = value; + mProperties.mSecondaryFrictionCoeff = value; +} + +double DynamicsAspect::getFrictionCoeff() const +{ + return 0.5 * ( + mProperties.mPrimaryFrictionCoeff + + mProperties.mSecondaryFrictionCoeff); +} + //============================================================================== ShapeFrame::~ShapeFrame() { diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index 1c162170cdb4b..3d4e1e5ab2865 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -143,15 +143,20 @@ 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; - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, RestitutionCoeff) - // void setRestitutionCoeff(const double& value); - // const double& getRestitutionCoeff() 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; }; //============================================================================== diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp index 7cd4c696c26de..1c6f23de7d306 100644 --- a/dart/dynamics/detail/ShapeFrameAspect.hpp +++ b/dart/dynamics/detail/ShapeFrameAspect.hpp @@ -89,7 +89,7 @@ struct CollisionAspectProperties struct DynamicsAspectProperties { /// Primary coefficient of friction - double mFrictionCoeff; + double mPrimaryFrictionCoeff; /// Coefficient of restitution double mRestitutionCoeff; @@ -99,7 +99,8 @@ struct DynamicsAspectProperties /// Constructor DynamicsAspectProperties( - const double frictionCoeff = 1.0, const double restitutionCoeff = 0.0, + const double primaryFrictionCoeff = 1.0, + const double restitutionCoeff = 0.0, const double secondaryFrictionCoeff = 1.0); /// Destructor From 0ade9ca50840ecdb5276915d01f50645f9287517 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 12 Nov 2019 12:28:20 -0800 Subject: [PATCH 04/11] test_Friction: reduce expectation line length --- unittests/comprehensive/test_Friction.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/unittests/comprehensive/test_Friction.cpp b/unittests/comprehensive/test_Friction.cpp index 41d1d9ed35d24..3f44849ada2df 100644 --- a/unittests/comprehensive/test_Friction.cpp +++ b/unittests/comprehensive/test_Friction.cpp @@ -80,15 +80,18 @@ 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()); 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()); + // 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()); // Create a world and add the rigid body auto world = simulation::World::create(); From 20aa3a0d839a9baa722adaaaeb8fc8851ba8669c Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 12 Nov 2019 12:28:52 -0800 Subject: [PATCH 05/11] test_Friction: coverage for new functions --- unittests/comprehensive/test_Friction.cpp | 28 +++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/unittests/comprehensive/test_Friction.cpp b/unittests/comprehensive/test_Friction.cpp index 3f44849ada2df..7c7c3ffc7b346 100644 --- a/unittests/comprehensive/test_Friction.cpp +++ b/unittests/comprehensive/test_Friction.cpp @@ -83,15 +83,43 @@ TEST(Friction, FrictionPerShapeNode) // 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(); // 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(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(); From 727ca635938199d71e3aad95cee51e7afe988002 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Sun, 17 Nov 2019 23:25:46 -0800 Subject: [PATCH 06/11] ShapeFrame DynamicsAspect: override constructor --- dart/dynamics/ShapeFrame.cpp | 14 ++++++++++++-- dart/dynamics/detail/ShapeFrameAspect.hpp | 12 ++++++++---- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 883ecf7c9b16d..3f0058f518ad9 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -52,11 +52,21 @@ CollisionAspectProperties::CollisionAspectProperties(const bool collidable) // Do nothing } +//============================================================================== +DynamicsAspectProperties::DynamicsAspectProperties( + const double frictionCoeff, const double restitutionCoeff) + : mPrimaryFrictionCoeff(frictionCoeff), + mRestitutionCoeff(restitutionCoeff), + mSecondaryFrictionCoeff(frictionCoeff) +{ + // Do nothing +} + //============================================================================== DynamicsAspectProperties::DynamicsAspectProperties( const double primaryFrictionCoeff, - const double restitutionCoeff, - const double secondaryFrictionCoeff) + const double secondaryFrictionCoeff, + const double restitutionCoeff) : mPrimaryFrictionCoeff(primaryFrictionCoeff), mRestitutionCoeff(restitutionCoeff), mSecondaryFrictionCoeff(secondaryFrictionCoeff) diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp index 1c6f23de7d306..9189b31440aa5 100644 --- a/dart/dynamics/detail/ShapeFrameAspect.hpp +++ b/dart/dynamics/detail/ShapeFrameAspect.hpp @@ -97,11 +97,15 @@ struct DynamicsAspectProperties /// Secondary coefficient of friction double mSecondaryFrictionCoeff; - /// Constructor + /// 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 = 1.0, - const double restitutionCoeff = 0.0, - const double secondaryFrictionCoeff = 1.0); + const double primaryFrictionCoeff, + const double secondaryFrictionCoeff, + const double restitutionCoeff); /// Destructor virtual ~DynamicsAspectProperties() = default; From 760cf3bc9a048aad3aab7d38d0ac4422d17c2f16 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Sun, 17 Nov 2019 23:28:24 -0800 Subject: [PATCH 07/11] ContactConstraint: add computePrimaryFrictionCoefficient --- dart/constraint/ContactConstraint.cpp | 24 +++++++++++++++++++++++- dart/constraint/ContactConstraint.hpp | 2 ++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 9e8c7e02405c1..97cc380b67ea1 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -709,6 +709,27 @@ 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) @@ -719,7 +740,8 @@ double ContactConstraint::computeSecondaryFrictionCoefficient( if (dynamicAspect == nullptr) { - dtwarn << "[ContactConstraint] Attempt to extract friction coefficient " + 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"; diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index a9c60475d1bc1..a21b6aa6f422b 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -140,6 +140,8 @@ 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( From 2a0934faabd1eee92be1233227a7d74ca23a103f Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Sun, 17 Nov 2019 23:29:12 -0800 Subject: [PATCH 08/11] ContactConstraint: use primary in variable names --- dart/constraint/ContactConstraint.cpp | 15 +++++++++------ dart/constraint/ContactConstraint.hpp | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 97cc380b67ea1..c0e0785101560 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -115,8 +115,10 @@ ContactConstraint::ContactConstraint( // 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 primaryFrictionCoeffA = + computePrimaryFrictionCoefficient(shapeNodeA); + const double primaryFrictionCoeffB = + computePrimaryFrictionCoefficient(shapeNodeB); const double secondaryFrictionCoeffA = computeSecondaryFrictionCoefficient(shapeNodeA); const double secondaryFrictionCoeffB = @@ -124,10 +126,11 @@ ContactConstraint::ContactConstraint( // 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); + mPrimaryFrictionCoeff = + std::min(primaryFrictionCoeffA, primaryFrictionCoeffB); mSecondaryFrictionCoeff = std::min(secondaryFrictionCoeffA, secondaryFrictionCoeffB); - if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD || + if (mPrimaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD || mSecondaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD) { mIsFrictionOn = true; @@ -394,8 +397,8 @@ 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 diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index a21b6aa6f422b..1d45e5996f2da 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -178,7 +178,7 @@ class ContactConstraint : public ConstraintBase Eigen::Vector3d mFirstFrictionalDirection; /// Primary Coefficient of Friction - double mFrictionCoeff; + double mPrimaryFrictionCoeff; /// Primary Coefficient of Friction double mSecondaryFrictionCoeff; From 05ddc81f194ee7ae666a1f42062199b39f0415e6 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 19 Nov 2019 11:43:36 -0800 Subject: [PATCH 09/11] Update dart/dynamics/ShapeFrame.hpp Co-Authored-By: Jeongseok Lee --- dart/dynamics/ShapeFrame.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index 3d4e1e5ab2865..f21ec7c01d833 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -151,7 +151,7 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties< 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 ) + 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) From 2c506f3145c561d1cc80088029dc10cd7842a79c Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 19 Nov 2019 14:11:16 -0800 Subject: [PATCH 10/11] restore property name, add explicit get/set --- dart/dynamics/ShapeFrame.cpp | 18 ++++++++++++++---- dart/dynamics/ShapeFrame.hpp | 7 ++++--- dart/dynamics/detail/ShapeFrameAspect.hpp | 2 +- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 3f0058f518ad9..3623122ed16b8 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -55,7 +55,7 @@ CollisionAspectProperties::CollisionAspectProperties(const bool collidable) //============================================================================== DynamicsAspectProperties::DynamicsAspectProperties( const double frictionCoeff, const double restitutionCoeff) - : mPrimaryFrictionCoeff(frictionCoeff), + : mFrictionCoeff(frictionCoeff), mRestitutionCoeff(restitutionCoeff), mSecondaryFrictionCoeff(frictionCoeff) { @@ -67,7 +67,7 @@ DynamicsAspectProperties::DynamicsAspectProperties( const double primaryFrictionCoeff, const double secondaryFrictionCoeff, const double restitutionCoeff) - : mPrimaryFrictionCoeff(primaryFrictionCoeff), + : mFrictionCoeff(primaryFrictionCoeff), mRestitutionCoeff(restitutionCoeff), mSecondaryFrictionCoeff(secondaryFrictionCoeff) { @@ -189,17 +189,27 @@ DynamicsAspect::DynamicsAspect(const PropertiesData& properties) void DynamicsAspect::setFrictionCoeff(const double& value) { - mProperties.mPrimaryFrictionCoeff = value; + mProperties.mFrictionCoeff = value; mProperties.mSecondaryFrictionCoeff = value; } double DynamicsAspect::getFrictionCoeff() const { return 0.5 * ( - mProperties.mPrimaryFrictionCoeff + + mProperties.mFrictionCoeff + mProperties.mSecondaryFrictionCoeff); } +void DynamicsAspect::setPrimaryFrictionCoeff(const double& value) +{ + mProperties.mFrictionCoeff = value; +} + +const double& DynamicsAspect::getPrimaryFrictionCoeff() const +{ + return mProperties.mFrictionCoeff; +} + //============================================================================== ShapeFrame::~ShapeFrame() { diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index f21ec7c01d833..1cccd4c3646a9 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -148,9 +148,10 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties< /// 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, 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; diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp index 9189b31440aa5..493085358652a 100644 --- a/dart/dynamics/detail/ShapeFrameAspect.hpp +++ b/dart/dynamics/detail/ShapeFrameAspect.hpp @@ -89,7 +89,7 @@ struct CollisionAspectProperties struct DynamicsAspectProperties { /// Primary coefficient of friction - double mPrimaryFrictionCoeff; + double mFrictionCoeff; /// Coefficient of restitution double mRestitutionCoeff; From af03aed64493f51a2813a5f3e47a10eb6576a4d1 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Nov 2019 13:14:51 -0800 Subject: [PATCH 11/11] Update changelog for #1424 --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 76dbfa30b3311..b0c42b967e260 100644 --- a/CHANGELOG.md +++ b/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