From f9be775b97a2b6488f9ff8d14b45c766c8a9d1a9 Mon Sep 17 00:00:00 2001 From: Martin Pecka 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 --- 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())), - mTimeStep(timeStep) + mTimeStep(timeStep), + mContactSurfaceHandler(std::make_shared()) { assert(timeStep > 0.0); @@ -83,7 +85,8 @@ ConstraintSolver::ConstraintSolver() mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()), mCollisionOption(collision::CollisionOption( true, 1000u, std::make_shared())), - mTimeStep(0.001) + mTimeStep(0.001), + mContactSurfaceHandler(std::make_shared()) { auto cd = std::static_pointer_cast( 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(contact, mTimeStep)); + const auto params = mContactSurfaceHandler->createParams(contact); + mContactConstraints.push_back(std::make_shared( + 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 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( @@ -98,19 +93,10 @@ ContactConstraint::ContactConstraint( assert( contact.normal.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED); - const auto* shapeNodeA = const_cast( - contact.collisionObject1->getShapeFrame()) - ->asShapeNode(); - const auto* shapeNodeB = const_cast( - 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; @@ -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 + +#include +#include + +#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( + contact.collisionObject1->getShapeFrame()) + ->asShapeNode(); + const auto* shapeNodeB = const_cast( + 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 +#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 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 --- 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 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 --- 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 contactPairMap; + std::vector 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( - 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(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( 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(numContactsOnCollisionObject)); + constraint->setSecondarySlipCompliance( + constraint->getSecondarySlipCompliance() * + static_cast(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 Date: Sat, 3 Jul 2021 10:20:11 +0200 Subject: [PATCH 4/7] Retain ABI compatibility. Signed-off-by: Martin Pecka --- 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 +#include +#include #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 + gContactSurfaceHandlers; + //============================================================================== ConstraintSolver::ConstraintSolver(double timeStep) : mCollisionDetector(collision::FCLCollisionDetector::create()), mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()), mCollisionOption(collision::CollisionOption( true, 1000u, std::make_shared())), - mTimeStep(timeStep), - mContactSurfaceHandler(std::make_shared()) + 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 lock(gContactSurfaceHandlersMutex); + gContactSurfaceHandlers[this] = + std::make_shared(); + } } //============================================================================== @@ -85,8 +96,7 @@ ConstraintSolver::ConstraintSolver() mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()), mCollisionOption(collision::CollisionOption( true, 1000u, std::make_shared())), - mTimeStep(0.001), - mContactSurfaceHandler(std::make_shared()) + mTimeStep(0.001) { auto cd = std::static_pointer_cast( 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 lock(gContactSurfaceHandlersMutex); + gContactSurfaceHandlers[this] = + std::make_shared(); + } +} + +//============================================================================== +ConstraintSolver::~ConstraintSolver() +{ + std::lock_guard lock(gContactSurfaceHandlersMutex); + gContactSurfaceHandlers.erase(this); } //============================================================================== @@ -393,7 +416,10 @@ void ConstraintSolver::setFromOtherConstraintSolver( addSkeletons(other.getSkeletons()); mManualConstraints = other.mManualConstraints; - mContactSurfaceHandler = other.mContactSurfaceHandler; + { + std::lock_guard 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 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 lock(gContactSurfaceHandlersMutex); + return gContactSurfaceHandlers[this]; + } } //============================================================================== void ConstraintSolver::setContactSurfaceHandler( ContactSurfaceHandlerPtr handler) { - handler->setParent(mContactSurfaceHandler); - mContactSurfaceHandler = std::move(handler); + { + std::lock_guard 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 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 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 +#include +#include #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 + gContactSurfaceMotionVelocities; + //============================================================================== ContactConstraint::ContactConstraint( collision::Contact& contact, double timeStep) : @@ -126,8 +132,11 @@ ContactConstraint::ContactConstraint( mIsFrictionOn = false; } - mContactSurfaceMotionVelocity = - contactSurfaceParams.mContactSurfaceMotionVelocity; + { + std::lock_guard lock(gContactSurfaceMotionVelocitiesMutex); + gContactSurfaceMotionVelocities[this] = + contactSurfaceParams.mContactSurfaceMotionVelocity; + } assert(mBodyNodeA->getSkeleton()); assert(mBodyNodeB->getSkeleton()); @@ -233,6 +242,13 @@ ContactConstraint::ContactConstraint( } } +//============================================================================== +ContactConstraint::~ContactConstraint() +{ + std::lock_guard 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 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 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; @@ -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 Date: Tue, 13 Jul 2021 11:31:49 +0200 Subject: [PATCH 5/7] Improve comment. Signed-off-by: Martin Pecka --- 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 Date: Tue, 13 Jul 2021 12:19:37 +0200 Subject: [PATCH 6/7] Added comments to ABI hacks. Signed-off-by: Martin Pecka --- 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 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 gContactSurfaceMotionVelocities; From e28afda01f42e55e1df0c1996e14892a8ca8da4b Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Sat, 31 Jul 2021 09:14:30 +0200 Subject: [PATCH 7/7] Addressed comments from review. Signed-off-by: Martin Pecka --- 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 lock(gContactSurfaceHandlersMutex); + for (auto* contact : contacts) { - std::lock_guard 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 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