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 bac4c39d6655c..8d5565fd2543a 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" @@ -42,6 +44,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" @@ -58,6 +61,12 @@ 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; + //============================================================================== ConstraintSolver::ConstraintSolver(double timeStep) : mCollisionDetector(collision::FCLCollisionDetector::create()), @@ -75,6 +84,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(); + } } //============================================================================== @@ -92,6 +107,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); } //============================================================================== @@ -389,6 +417,11 @@ void ConstraintSolver::setFromOtherConstraintSolver( addSkeletons(other.getSkeletons()); mManualConstraints = other.mManualConstraints; + + { + std::lock_guard lock(gContactSurfaceHandlersMutex); + gContactSurfaceHandlers[this] = gContactSurfaceHandlers[&other]; + } } //============================================================================== @@ -500,6 +533,7 @@ void ConstraintSolver::updateConstraints() }; std::map contactPairMap; + std::vector contacts; // Create new contact constraints for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i) @@ -542,31 +576,31 @@ void ConstraintSolver::updateConstraints() ++contactPairMap[std::make_pair( contact.collisionObject1, contact.collisionObject2)]; - mContactConstraints.push_back( - std::make_shared(contact, mTimeStep)); + contacts.push_back(&contact); } } // Add the new contact constraints to dynamic constraint list - for (const auto& contactConstraint : mContactConstraints) { - // update the slip compliances of the contact constraints based on the - // number of contacts between the collision objects. - auto& contact = contactConstraint->getContact(); - std::size_t numContacts = 1; - auto it = contactPairMap.find( - std::make_pair(contact.collisionObject1, contact.collisionObject2)); - if (it != contactPairMap.end()) - numContacts = it->second; - - contactConstraint->setPrimarySlipCompliance( - contactConstraint->getPrimarySlipCompliance() * numContacts); - contactConstraint->setSecondarySlipCompliance( - contactConstraint->getSecondarySlipCompliance() * numContacts); - contactConstraint->update(); - - if (contactConstraint->isActive()) - mActiveConstraints.push_back(contactConstraint); + std::lock_guard lock(gContactSurfaceHandlersMutex); + 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; + + auto contactConstraint = gContactSurfaceHandlers[this]->createConstraint( + *contact, numContacts, mTimeStep); + + mContactConstraints.push_back(contactConstraint); + + contactConstraint->update(); + + if (contactConstraint->isActive()) + mActiveConstraints.push_back(contactConstraint); + } } // Add the new soft contact constraints to dynamic constraint list @@ -750,5 +784,56 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const return bodyNode1IsSoft || bodyNode2IsSoft; } +//============================================================================== +ContactSurfaceHandlerPtr +ConstraintSolver::getLastContactSurfaceHandler() const +{ + { + std::lock_guard lock(gContactSurfaceHandlersMutex); + return gContactSurfaceHandlers[this]; + } +} + +//============================================================================== +void ConstraintSolver::addContactSurfaceHandler( + ContactSurfaceHandlerPtr handler) +{ + { + std::lock_guard lock(gContactSurfaceHandlersMutex); + handler->setParent(gContactSurfaceHandlers[this]); + gContactSurfaceHandlers[this] = std::move(handler); + } +} + +//============================================================================== +bool ConstraintSolver::removeContactSurfaceHandler( + const ContactSurfaceHandlerPtr& handler) +{ + bool found = false; + std::lock_guard lock(gContactSurfaceHandlersMutex); + ContactSurfaceHandlerPtr current = gContactSurfaceHandlers[this]; + ContactSurfaceHandlerPtr previous = nullptr; + while (current != nullptr) + { + if (current == handler) + { + if (previous != nullptr) + previous->mParent = current->mParent; + else + gContactSurfaceHandlers[this] = current->mParent; + found = true; + break; + } + previous = current; + current = current->mParent; + } + + if (gContactSurfaceHandlers[this] == 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 ae2deb469df0a..3d61bbddc1bb6 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); @@ -184,6 +184,24 @@ 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 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 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 + /// 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; @@ -259,6 +277,10 @@ class ConstraintSolver /// Constraint group list std::vector mConstrainedGroups; + + /// Factory for ContactSurfaceParams for each contact + // 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 edbe88ea0b7ca..a350e85d31deb 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" @@ -49,12 +51,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 +59,24 @@ 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(); +// 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; //============================================================================== ContactConstraint::ContactConstraint( - collision::Contact& contact, double timeStep) + collision::Contact& contact, double timeStep) : + ContactConstraint(contact, timeStep, + DefaultContactSurfaceHandler().createParams(contact, 1u)) +{ +} + +//============================================================================== +ContactConstraint::ContactConstraint( + collision::Contact& contact, double timeStep, + const ContactSurfaceParams& contactSurfaceParams) : ConstraintBase(), mTimeStep(timeStep), mBodyNodeA(const_cast( @@ -98,19 +101,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 +113,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 +134,12 @@ ContactConstraint::ContactConstraint( mIsFrictionOn = false; } + { + std::lock_guard lock(gContactSurfaceMotionVelocitiesMutex); + gContactSurfaceMotionVelocities[this] = + contactSurfaceParams.mContactSurfaceMotionVelocity; + } + assert(mBodyNodeA->getSkeleton()); assert(mBodyNodeB->getSkeleton()); mIsSelfCollision = (mBodyNodeA->getSkeleton() == mBodyNodeB->getSkeleton()); @@ -297,6 +244,13 @@ ContactConstraint::ContactConstraint( } } +//============================================================================== +ContactConstraint::~ContactConstraint() +{ + std::lock_guard lock(gContactSurfaceMotionVelocitiesMutex); + gContactSurfaceMotionVelocities.erase(this); +} + //============================================================================== void ContactConstraint::setErrorAllowance(double allowance) { @@ -475,6 +429,13 @@ void ContactConstraint::getInformation(ConstraintInfo* info) } info->b[0] += bouncingVelocity; + { + 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 @@ -530,6 +491,10 @@ void ContactConstraint::getInformation(ConstraintInfo* info) } info->b[0] += bouncingVelocity; + { + std::lock_guard lock(gContactSurfaceMotionVelocitiesMutex); + info->b[0] += gContactSurfaceMotionVelocities[this].x(); + } // TODO(JS): Initial guess // x @@ -741,141 +706,44 @@ bool ContactConstraint::isActive() const 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(); + return DefaultContactSurfaceHandler::computeFrictionCoefficient(shapeNode); } //============================================================================== 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(); + return DefaultContactSurfaceHandler::computeSecondaryFrictionCoefficient( + shapeNode); } //============================================================================== 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; + return DefaultContactSurfaceHandler::computeSlipCompliance(shapeNode); } //============================================================================== 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; + return DefaultContactSurfaceHandler::computeSecondarySlipCompliance( + shapeNode); } //============================================================================== 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; + return DefaultContactSurfaceHandler::computeWorldFirstFrictionDir(shapeNode); } //============================================================================== 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(); + return DefaultContactSurfaceHandler::computeRestitutionCoefficient(shapeNode); } //============================================================================== diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index a3c2f8a240442..56149c6f62c45 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,11 +51,15 @@ namespace constraint { class ContactConstraint : public ConstraintBase { public: + /// Constructor + ContactConstraint(collision::Contact& contact, double timeStep, + const ContactSurfaceParams& contactSurfaceParams); + /// Constructor ContactConstraint(collision::Contact& contact, double timeStep); /// Destructor - ~ContactConstraint() override = default; + ~ContactConstraint() override; //---------------------------------------------------------------------------- // Property settings @@ -96,6 +101,7 @@ class ContactConstraint : public ConstraintBase friend class ConstraintSolver; friend class ConstrainedGroup; + friend class DefaultContactSurfaceHandler; protected: //---------------------------------------------------------------------------- @@ -132,16 +138,22 @@ 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); @@ -198,7 +210,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 +222,13 @@ 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 + // 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.cpp b/dart/constraint/ContactSurface.cpp new file mode 100644 index 0000000000000..abccc86a9fbf1 --- /dev/null +++ b/dart/constraint/ContactSurface.cpp @@ -0,0 +1,336 @@ +/* + * 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 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, + size_t numContactsOnCollisionObject) const +{ + ContactSurfaceParams params = ContactSurfaceHandler::createParams( + contact, numContactsOnCollisionObject); + + 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; +} + +//============================================================================== +ContactConstraintPtr DefaultContactSurfaceHandler::createConstraint( + collision::Contact& contact, + const size_t numContactsOnCollisionObject, + const double timeStep) const +{ + auto constraint = ContactSurfaceHandler::createConstraint( + contact, numContactsOnCollisionObject, timeStep); + + constraint->setPrimarySlipCompliance( + constraint->getPrimarySlipCompliance() * + static_cast(numContactsOnCollisionObject)); + constraint->setSecondarySlipCompliance( + constraint->getSecondarySlipCompliance() * + static_cast(numContactsOnCollisionObject)); + + return constraint; +} + +//============================================================================== +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..ee709ff4ac1a0 --- /dev/null +++ b/dart/constraint/ContactSurface.hpp @@ -0,0 +1,167 @@ +/* + * 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}; + +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 +/// two colliding bodies and information about their contact. +class ContactSurfaceHandler +{ +public: + /// Constructor + /// \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 + /// the collision properties of the two colliding bodies and write the + /// 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, + 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); + + friend class dart::constraint::ConstraintSolver; + +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, + 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); + 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); + + friend class ContactConstraint; +}; + +} // 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)