diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 9c124a498fecf..f0141c4bc868d 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -32,6 +32,8 @@ #include "dart/constraint/ConstraintSolver.hpp" +#include + #include "dart/collision/CollisionFilter.hpp" #include "dart/collision/CollisionGroup.hpp" #include "dart/collision/CollisionObject.hpp" @@ -478,6 +480,29 @@ void ConstraintSolver::updateConstraints() // Destroy previous soft contact constraints mSoftContactConstraints.clear(); + // Create a mapping of contact pairs to the number of contacts between them + using ContactPair + = std::pair; + + // Compare contact pairs while ignoring their order in the pair. + struct ContactPairCompare + { + ContactPair getSortedPair(const ContactPair& a) const + { + if (a.first < a.second) + return std::make_pair(a.second, a.first); + return a; + } + + bool operator()(const ContactPair& a, const ContactPair& b) const + { + // Sort each pair and then do a lexicographical comparison + return getSortedPair(a) < getSortedPair(b); + } + }; + + std::map contactPairMap; + // Create new contact constraints for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i) { @@ -515,6 +540,10 @@ void ConstraintSolver::updateConstraints() } else { + // Increment the count of contacts between the two collision objects + ++contactPairMap[std::make_pair( + contact.collisionObject1, contact.collisionObject2)]; + mContactConstraints.push_back( std::make_shared(contact, mTimeStep)); } @@ -523,6 +552,23 @@ void ConstraintSolver::updateConstraints() // 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; + + // The slip compliance acts like a damper at each contact point so the total + // damping for each collision is multiplied by the number of contact points + // (numContacts). To eliminate this dependence on numContacts, the inverse + // damping is multiplied by numContacts. + contactConstraint->setPrimarySlipCompliance( + contactConstraint->getPrimarySlipCompliance() * numContacts); + contactConstraint->setSecondarySlipCompliance( + contactConstraint->getSecondarySlipCompliance() * numContacts); contactConstraint->update(); if (contactConstraint->isActive()) diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 125dc0b4aa4fc..623d13e7cc8be 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -65,6 +65,7 @@ double ContactConstraint::mConstraintForceMixing = DART_CFM; constexpr double DART_DEFAULT_FRICTION_COEFF = 1.0; constexpr double DART_DEFAULT_RESTITUTION_COEFF = 0.0; +constexpr double DART_DEFAULT_SLIP_COMPLIANCE = 0.0; const Eigen::Vector3d DART_DEFAULT_FRICTION_DIR = Eigen::Vector3d::UnitZ(); //============================================================================== @@ -84,6 +85,8 @@ ContactConstraint::ContactConstraint( .get()), mContact(contact), mFirstFrictionalDirection(DART_DEFAULT_FRICTION_DIR), + mPrimarySlipCompliance(DART_DEFAULT_SLIP_COMPLIANCE), + mSecondarySlipCompliance(DART_DEFAULT_SLIP_COMPLIANCE), mIsFrictionOn(true), mAppliedImpulseIndex(dynamics::INVALID_INDEX), mIsBounceOn(false), @@ -136,6 +139,20 @@ ContactConstraint::ContactConstraint( { mIsFrictionOn = true; + // Compute slip compliance + const double primarySlipComplianceA + = computePrimarySlipCompliance(shapeNodeA); + const double primarySlipComplianceB + = computePrimarySlipCompliance(shapeNodeB); + const double secondarySlipComplianceA + = computeSecondarySlipCompliance(shapeNodeA); + const double secondarySlipComplianceB + = computeSecondarySlipCompliance(shapeNodeB); + // Combine slip compliances through addition + mPrimarySlipCompliance = primarySlipComplianceA + primarySlipComplianceB; + mSecondarySlipCompliance + = secondarySlipComplianceA + secondarySlipComplianceB; + // Check shapeNodes for valid friction direction unit vectors auto frictionDirA = computeWorldFirstFrictionDir(shapeNodeA); auto frictionDirB = computeWorldFirstFrictionDir(shapeNodeB); @@ -627,6 +644,17 @@ void ContactConstraint::getVelocityChange(double* vel, bool withCfm) { vel[mAppliedImpulseIndex] += vel[mAppliedImpulseIndex] * mConstraintForceMixing; + switch (mAppliedImpulseIndex) + { + case 1: + vel[1] += (mPrimarySlipCompliance / mTimeStep); + break; + case 2: + vel[2] += (mSecondarySlipCompliance / mTimeStep); + break; + default: + break; + } } } @@ -786,6 +814,57 @@ double ContactConstraint::computeSecondaryFrictionCoefficient( return dynamicAspect->getSecondaryFrictionCoeff(); } +//============================================================================== +double ContactConstraint::computePrimarySlipCompliance( + 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->getPrimarySlipCompliance(); + 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) @@ -950,5 +1029,35 @@ void ContactConstraint::uniteSkeletons() } } +//============================================================================== +double ContactConstraint::getPrimarySlipCompliance() const +{ + return mPrimarySlipCompliance; +} + +//============================================================================== +void ContactConstraint::setPrimarySlipCompliance(double slip) +{ + mPrimarySlipCompliance = slip; +} + +//============================================================================== +double ContactConstraint::getSecondarySlipCompliance() const +{ + return mSecondarySlipCompliance; +} + +//============================================================================== +void ContactConstraint::setSecondarySlipCompliance(double slip) +{ + mSecondarySlipCompliance = slip; +} + +//============================================================================== +const collision::Contact& ContactConstraint::getContact() const +{ + return mContact; +} + } // namespace constraint } // namespace dart diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index b54f08f5b2dc4..c7a766dc5788a 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -144,6 +144,10 @@ class ContactConstraint : public ConstraintBase const dynamics::ShapeNode* shapeNode); static double computeSecondaryFrictionCoefficient( const dynamics::ShapeNode* shapeNode); + static double computePrimarySlipCompliance( + const dynamics::ShapeNode* shapeNode); + static double computeSecondarySlipCompliance( + const dynamics::ShapeNode* shapeNode); static Eigen::Vector3d computeWorldFirstFrictionDir( const dynamics::ShapeNode* shapenode); static double computeRestitutionCoefficient( @@ -163,6 +167,26 @@ class ContactConstraint : public ConstraintBase /// TangentBasisMatrix getTangentBasisMatrixODE(const Eigen::Vector3d& n); + // The following functions for getting and setting slip compliance and + // accessing the contact object are meant to be used by ConstraintSolver to + // update the slip compliances based on the number of contacts between the + // collision objects. + // + /// Get primary slip compliance + double getPrimarySlipCompliance() const; + + /// Set primary slip compliance + void setPrimarySlipCompliance(double slip); + + /// Get secondary slip compliance + double getSecondarySlipCompliance() const; + + /// Set secondary slip compliance + void setSecondarySlipCompliance(double slip); + + /// Get contact object associated witht this constraint + const collision::Contact& getContact() const; + private: /// Time step double mTimeStep; @@ -185,6 +209,12 @@ class ContactConstraint : public ConstraintBase /// Primary Coefficient of Friction double mSecondaryFrictionCoeff; + /// Primary Coefficient of Slip Compliance + double mPrimarySlipCompliance; + + /// Secondary Coefficient of Slip Compliance + double mSecondarySlipCompliance; + /// Coefficient of restitution double mRestitutionCoeff; diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 22685d5ac00ca..4b0dfb114ac8d 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -58,6 +58,8 @@ DynamicsAspectProperties::DynamicsAspectProperties( : mFrictionCoeff(frictionCoeff), mRestitutionCoeff(restitutionCoeff), mSecondaryFrictionCoeff(frictionCoeff), + mPrimarySlipCompliance(-1.0), + mSecondarySlipCompliance(-1.0), mFirstFrictionDirection(Eigen::Vector3d::Zero()), mFirstFrictionDirectionFrame(nullptr) { @@ -69,11 +71,15 @@ DynamicsAspectProperties::DynamicsAspectProperties( const double primaryFrictionCoeff, const double secondaryFrictionCoeff, const double restitutionCoeff, + const double primarySlipCompliance, + const double secondarySlipCompliance, const Eigen::Vector3d& firstFrictionDirection, const Frame* firstFrictionDirectionFrame) : mFrictionCoeff(primaryFrictionCoeff), mRestitutionCoeff(restitutionCoeff), mSecondaryFrictionCoeff(secondaryFrictionCoeff), + mPrimarySlipCompliance(primarySlipCompliance), + mSecondarySlipCompliance(secondarySlipCompliance), mFirstFrictionDirection(firstFrictionDirection), mFirstFrictionDirectionFrame(firstFrictionDirectionFrame) { diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index 1bfc8bb08a941..68491da239a7b 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -159,6 +159,18 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties< // void setRestitutionCoeff(const double& value); // const double& getRestitutionCoeff() const; + /// Slip compliance parameters act as constraint force mixing (cfm) + /// for the friction constraints. + /// They start with a default value of -1.0 and will be ignored + /// in favor of the global default value unless explicitly + /// set to a positive value. + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, PrimarySlipCompliance) + // void sePrimarytSlipCompliance(const double& value); + // const double& getPrimarySlipCompliance() const; + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, SecondarySlipCompliance) + // void setSecondarySlipCompliance(const double& value); + // const double& getSecondarySlipCompliance() const; + /// Set the frame for interpreting the first friction direction vector. /// The frame pointer defaults to nullptr, which is interpreted as this /// ShapeFrame. diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp index 3909af437c13f..a804d79c5b874 100644 --- a/dart/dynamics/detail/ShapeFrameAspect.hpp +++ b/dart/dynamics/detail/ShapeFrameAspect.hpp @@ -97,6 +97,12 @@ struct DynamicsAspectProperties /// Secondary coefficient of friction double mSecondaryFrictionCoeff; + /// Primary slip compliance coefficient + double mPrimarySlipCompliance; + + /// Secondary slip compliance coefficient + double mSecondarySlipCompliance; + /// First friction direction unit vector Eigen::Vector3d mFirstFrictionDirection; @@ -128,6 +134,8 @@ struct DynamicsAspectProperties const double primaryFrictionCoeff, const double secondaryFrictionCoeff, const double restitutionCoeff, + const double primarySlipCompliance = -1.0, + const double secondarySlipCompliance = -1.0, const Eigen::Vector3d& firstFrictionDirection = Eigen::Vector3d::Zero(), const Frame* firstFrictionDirectionFrame = nullptr); diff --git a/unittests/comprehensive/CMakeLists.txt b/unittests/comprehensive/CMakeLists.txt index c9025a178e8dd..b068dce7b0ab1 100644 --- a/unittests/comprehensive/CMakeLists.txt +++ b/unittests/comprehensive/CMakeLists.txt @@ -22,6 +22,13 @@ if(TARGET dart-collision-bullet) target_link_libraries(test_Raycast dart-collision-bullet) endif() +if(TARGET dart-collision-ode) + # This test doesn't work with FCL because it converts simple shapes to meshes, which makes + # it difficult to come up with correct test expectations. + dart_add_test("comprehensive" test_ForceDependentSlip) + target_link_libraries(test_ForceDependentSlip dart-collision-ode) +endif() + if(TARGET dart-utils) dart_add_test("comprehensive" test_Collision) diff --git a/unittests/comprehensive/test_ForceDependentSlip.cpp b/unittests/comprehensive/test_ForceDependentSlip.cpp new file mode 100644 index 0000000000000..02bfab3974267 --- /dev/null +++ b/unittests/comprehensive/test_ForceDependentSlip.cpp @@ -0,0 +1,285 @@ +/* + * Copyright (c) 2011-2019, 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 + +#include "dart/collision/ode/OdeCollisionDetector.hpp" +#include "dart/constraint/ConstraintSolver.hpp" +#include "dart/dynamics/SimpleFrame.hpp" +#include "dart/math/Geometry.hpp" +#include "dart/math/Helpers.hpp" +#include "dart/math/Random.hpp" + +#include "TestHelpers.hpp" + +using namespace dart; +using namespace dynamics; + +//============================================================================== +std::shared_ptr createWorld() +{ + auto world = simulation::World::create(); + world->getConstraintSolver()->setCollisionDetector( + collision::OdeCollisionDetector::create()); + return world; +} + +//============================================================================== +dynamics::SkeletonPtr createFloor() +{ + auto floor = dynamics::Skeleton::create("floor"); + + // Give the floor a body + auto body + = floor->createJointAndBodyNodePair(nullptr).second; + + // Give the body a shape + double floorWidth = 10000.0; + double floorHeight = 0.01; + auto box = std::make_shared( + Eigen::Vector3d(floorWidth, floorWidth, floorHeight)); + auto* shapeNode = body->createShapeNodeWith< + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>(box); + shapeNode->getVisualAspect()->setColor(dart::Color::LightGray()); + shapeNode->getDynamicsAspect()->setPrimarySlipCompliance(0); + shapeNode->getDynamicsAspect()->setSecondarySlipCompliance(0); + + // Put the body into position + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = Eigen::Vector3d(0.0, 0.0, -floorHeight / 2.0); + body->getParentJoint()->setTransformFromParentBodyNode(tf); + + return floor; +} + +//============================================================================== +SkeletonPtr createCylinder( + double _radius, + double _height, + const Eigen::Vector3d& _position = Eigen::Vector3d::Zero(), + const Eigen::Vector3d& _orientation = Eigen::Vector3d::Zero()) +{ + SkeletonPtr cylinder = createObject(_position, _orientation); + + BodyNode* bn = cylinder->getBodyNode(0); + std::shared_ptr cylinderShape(new CylinderShape(_radius, _height)); + bn->createShapeNodeWith( + cylinderShape); + + return cylinder; +} + +//============================================================================== +TEST(ForceDependentSlip, BoxSlipVelocity) +{ + using Eigen::Vector3d; + const double mass = 5.0; + const double slip = 0.02; + auto skeleton1 = createBox({0.3, 0.3, 0.3}, {0, -0.5, 0.15}); + skeleton1->setName("Skeleton1"); + auto skeleton2 = createBox({0.3, 0.3, 0.3}, {0, +0.5, 0.15}); + skeleton2->setName("Skeleton2"); + + auto body1 = skeleton1->getRootBodyNode(); + body1->setMass(mass); + auto body1Dynamics = body1->getShapeNode(0)->getDynamicsAspect(); + + EXPECT_DOUBLE_EQ(body1Dynamics->getFrictionCoeff(), 1.0); + + body1Dynamics->setPrimarySlipCompliance(slip); + body1Dynamics->setFirstFrictionDirection(Vector3d::UnitX()); + EXPECT_DOUBLE_EQ(body1Dynamics->getPrimarySlipCompliance(), slip); + EXPECT_EQ(body1Dynamics->getFirstFrictionDirection(), Vector3d::UnitX()); + + auto body2 = skeleton2->getRootBodyNode(); + body2->setMass(mass); + auto body2Dynamics = body2->getShapeNode(0)->getDynamicsAspect(); + body2Dynamics->setFirstFrictionDirection(Vector3d::UnitX()); + EXPECT_EQ(body2Dynamics->getFirstFrictionDirection(), Vector3d::UnitX()); + EXPECT_DOUBLE_EQ(body2Dynamics->getFrictionCoeff(), 1.0); + + // Create a world and add the rigid bodies + auto world = createWorld(); + world->addSkeleton(createFloor()); + world->addSkeleton(skeleton1); + world->addSkeleton(skeleton2); + + const auto numSteps = 2000; + const double extForce = 10.0; + for (auto i = 0u; i < numSteps; ++i) + { + body1->addExtForce({extForce, 0, 0}); + body2->addExtForce({extForce, 0, 0}); + world->step(); + + if (i > 1000) + { + // The velocity of body1 should stabilize at F_ext * slip = 0.2 m/s + EXPECT_NEAR(extForce * slip, body1->getLinearVelocity().x(), 2e-5); + EXPECT_NEAR(0.0, body1->getLinearVelocity().y(), 2e-5); + + // The second box should remain at rest because of friction + EXPECT_NEAR(0.0, body2->getLinearVelocity().x(), 2e-5); + EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 2e-5); + } + } + + const double slip2 = 0.03; + // Test slip compliance in the secondary friction direction + body1Dynamics->setPrimarySlipCompliance(0); + body1Dynamics->setSecondarySlipCompliance(slip2); + + EXPECT_DOUBLE_EQ(body1Dynamics->getPrimarySlipCompliance(), 0.0); + EXPECT_DOUBLE_EQ(body1Dynamics->getSecondarySlipCompliance(), slip2); + + // Step without external force so the body stop moving + for (auto i = 0u; i < 500; ++i) + { + world->step(); + } + EXPECT_NEAR(0.0, body1->getLinearVelocity().x(), 2e-5); + EXPECT_NEAR(0.0, body1->getLinearVelocity().y(), 2e-5); + + // The second box should remain at rest because of friction + EXPECT_NEAR(0.0, body2->getLinearVelocity().x(), 2e-5); + EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 2e-5); + + // Apply force in the +y direction + for (auto i = 0u; i < numSteps; ++i) + { + body1->addExtForce({0, extForce, 0}); + body2->addExtForce({0, extForce, 0}); + world->step(); + + if (i > 1500) + { + // The velocity of body1 should stabilize at F_ext * slip2 = 0.3 m/s + EXPECT_NEAR(0.0, body1->getLinearVelocity().x(), 2e-5); + EXPECT_NEAR(extForce * slip2, body1->getLinearVelocity().y(), 2e-5); + + // The second box should remain at rest because of friction + EXPECT_NEAR(0.0, body2->getLinearVelocity().x(), 2e-5); + EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 2e-5); + } + } +} + +//============================================================================== +// Test two cylinders, one with its z axis pointing in the z axis of the world +// so it's purely slipping, and the other with its z axis pointing in the y axis +// of the world so it's rolling and slipping. +TEST(ForceDependentSlip, CylinderSlipVelocity) +{ + using Eigen::Vector3d; + const double mass = 2.0; + const double radius = 0.5; + const double slip = 0.02; + auto skeleton1 = createCylinder(radius, 0.3, {0, -5, radius}); + skeleton1->setName("Skeleton1"); + + auto skeleton2 = createCylinder( + radius, 0.8, {0, 5, radius}, {math::constantsd::half_pi(), 0, 0}); + skeleton2->setName("Skeleton2"); + + auto body1 = skeleton1->getRootBodyNode(); + body1->setMass(5.0); + auto body1Dynamics = body1->getShapeNode(0)->getDynamicsAspect(); + + EXPECT_DOUBLE_EQ(body1Dynamics->getFrictionCoeff(), 1.0); + + body1Dynamics->setPrimarySlipCompliance(slip); + body1Dynamics->setFirstFrictionDirection(Vector3d::UnitX()); + EXPECT_DOUBLE_EQ(body1Dynamics->getPrimarySlipCompliance(), slip); + EXPECT_EQ(body1Dynamics->getFirstFrictionDirection(), Vector3d::UnitX()); + + auto body2 = skeleton2->getRootBodyNode(); + body2->setMass(mass); + auto body2Dynamics = body2->getShapeNode(0)->getDynamicsAspect(); + EXPECT_DOUBLE_EQ(body2Dynamics->getFrictionCoeff(), 1.0); + + // Set the friction direction to +z in the shape frame because it will always + // be orthogonal to the floor's normal. + body2Dynamics->setFirstFrictionDirection(Vector3d::UnitZ()); + // Since we want to test rolling with slipping for body2, we want to set a + // non-zero slip parameter in the direction orthogonal to the axis of rotation + // of the cylinder. The axis of rotation is in the body's +z direction, so we + // make that the first friction direction and set the non-zero slip parameter + // in the secondary direction. + body2Dynamics->setSecondarySlipCompliance(slip); + EXPECT_DOUBLE_EQ(body2Dynamics->getSecondarySlipCompliance(), slip); + EXPECT_EQ(body2Dynamics->getFirstFrictionDirection(), Vector3d::UnitZ()); + + // Create a world and add the rigid bodies + auto world = createWorld(); + + world->addSkeleton(createFloor()); + world->addSkeleton(skeleton1); + world->addSkeleton(skeleton2); + + const double dt = 0.001; + const auto numSteps = 2000; + const double extForceX = 1.0; + const double extTorqueY = 2.0; + + auto lastVel = body2->getLinearVelocity(); + for (auto i = 0u; i < numSteps; ++i) + { + body1->addExtForce({extForceX, 0, 0}); + body2->addExtTorque({0, extTorqueY, 0.0}, false); + world->step(); + + if (i > 1000) + { + // The velocity of body1 should stabilize at F_ext * slip + EXPECT_NEAR(extForceX * slip, body1->getLinearVelocity().x(), 1e-4); + EXPECT_NEAR(0.0, body1->getLinearVelocity().y(), 1e-4); + + // body2 rolls with sliding. The difference between the linear velocity + // and the expected non-sliding velocity (angular velocity * radius) is + // equal to F_fr * slip, where F_fr is the friction force. We compute the + // friction force from the linear acceleration since it's the only linear + // force on the body. + auto linVel = body2->getLinearVelocity().x(); + auto spinVel = body2->getAngularVelocity().y() * radius; + // There appears to be a bug in DART in obtaining the linear acceleration + // of the body using (BodyNode::getLinearAcceleration), so we compute it + // here via finite difference. + auto accel = (body2->getLinearVelocity() - lastVel) / dt; + EXPECT_NEAR(mass * accel.x() * slip, spinVel - linVel, 2e-4); + EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 1e-4); + } + + lastVel = body2->getLinearVelocity(); + } +} diff --git a/unittests/regression/test_Issue1445.cpp b/unittests/regression/test_Issue1445.cpp index 8fa5c6e89edf0..2ba3a5a8cb394 100644 --- a/unittests/regression/test_Issue1445.cpp +++ b/unittests/regression/test_Issue1445.cpp @@ -81,9 +81,11 @@ TEST(Issue1445, Collision) bn->createShapeNodeWith< dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>( - std::make_shared( - Eigen::Vector3d::UnitZ(), 0.0)); - + std::make_shared( + Eigen::Vector3d(10, 10, 0.2))); + auto sn = bn->getShapeNode(0); + ASSERT_TRUE(sn); + sn->setRelativeTranslation({0, 0, -0.1}); world->addSkeleton(ground); }