diff --git a/CHANGELOG.md b/CHANGELOG.md index b0c42b967e260..94c97cb0eac8c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ * Added type property to constrain classes: [#1415](https://github.com/dartsim/dart/pull/1415) * Allowed to set joint rest position out of joint limits: [#1418](https://github.com/dartsim/dart/pull/1418) * Added secondary friction coefficient parameter: [#1424](https://github.com/dartsim/dart/pull/1424) + * Allowed to set friction direction per ShapeFrame: [#1427](https://github.com/dartsim/dart/pull/1427) * GUI diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index c0e0785101560..125dc0b4aa4fc 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; +const Eigen::Vector3d DART_DEFAULT_FRICTION_DIR = Eigen::Vector3d::UnitZ(); //============================================================================== ContactConstraint::ContactConstraint( @@ -82,7 +83,7 @@ ContactConstraint::ContactConstraint( ->getBodyNodePtr() .get()), mContact(contact), - mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), + mFirstFrictionalDirection(DART_DEFAULT_FRICTION_DIR), mIsFrictionOn(true), mAppliedImpulseIndex(dynamics::INVALID_INDEX), mIsBounceOn(false), @@ -115,26 +116,62 @@ ContactConstraint::ContactConstraint( // TODO(JS): Assume the frictional coefficient can be changed during // simulation steps. // Update mFrictionCoeff - const double primaryFrictionCoeffA = - computePrimaryFrictionCoefficient(shapeNodeA); - const double primaryFrictionCoeffB = - computePrimaryFrictionCoefficient(shapeNodeB); - const double secondaryFrictionCoeffA = - computeSecondaryFrictionCoefficient(shapeNodeA); - const double secondaryFrictionCoeffB = - computeSecondaryFrictionCoefficient(shapeNodeB); + const double primaryFrictionCoeffA + = computePrimaryFrictionCoefficient(shapeNodeA); + const double primaryFrictionCoeffB + = computePrimaryFrictionCoefficient(shapeNodeB); + const double secondaryFrictionCoeffA + = computeSecondaryFrictionCoefficient(shapeNodeA); + const double secondaryFrictionCoeffB + = computeSecondaryFrictionCoefficient(shapeNodeB); // TODO(JS): Consider providing various ways of the combined friction or // allowing to override this method by a custom method - mPrimaryFrictionCoeff = - std::min(primaryFrictionCoeffA, primaryFrictionCoeffB); - mSecondaryFrictionCoeff = - std::min(secondaryFrictionCoeffA, secondaryFrictionCoeffB); - if (mPrimaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD || - mSecondaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD) + mPrimaryFrictionCoeff + = std::min(primaryFrictionCoeffA, primaryFrictionCoeffB); + mSecondaryFrictionCoeff + = std::min(secondaryFrictionCoeffA, secondaryFrictionCoeffB); + if (mPrimaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD + || mSecondaryFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD) { mIsFrictionOn = true; + // Check shapeNodes for valid friction direction unit vectors + auto frictionDirA = computeWorldFirstFrictionDir(shapeNodeA); + auto frictionDirB = computeWorldFirstFrictionDir(shapeNodeB); + + // check if the friction direction unit vectors have been set + bool nonzeroDirA + = frictionDirA.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED; + bool nonzeroDirB + = frictionDirB.squaredNorm() >= DART_CONTACT_CONSTRAINT_EPSILON_SQUARED; + + // only consider custom friction direction if one has nonzero length + if (nonzeroDirA || nonzeroDirB) + { + // if A and B are both set, choose one with smaller friction coefficient + // since it's friction properties will dominate + if (nonzeroDirA && nonzeroDirB) + { + if (primaryFrictionCoeffA <= primaryFrictionCoeffB) + { + mFirstFrictionalDirection = frictionDirA.normalized(); + } + else + { + mFirstFrictionalDirection = frictionDirB.normalized(); + } + } + else if (nonzeroDirA) + { + mFirstFrictionalDirection = frictionDirA.normalized(); + } + else + { + mFirstFrictionalDirection = frictionDirB.normalized(); + } + } + // Update frictional direction updateFirstFrictionalDirection(); } @@ -147,11 +184,6 @@ ContactConstraint::ContactConstraint( assert(mBodyNodeB->getSkeleton()); mIsSelfCollision = (mBodyNodeA->getSkeleton() == mBodyNodeB->getSkeleton()); - const math::Jacobian jacA - = mBodyNodeA->getSkeleton()->getJacobian(mBodyNodeA); - const math::Jacobian jacB - = mBodyNodeB->getSkeleton()->getJacobian(mBodyNodeB); - // Compute local contact Jacobians expressed in body frame if (mIsFrictionOn) { @@ -547,7 +579,7 @@ void ContactConstraint::applyUnitImpulse(std::size_t index) // Both bodies are not reactive else { - // This case should not be happed + // This case should not be happened assert(0); } } @@ -754,6 +786,36 @@ double ContactConstraint::computeSecondaryFrictionCoefficient( return dynamicAspect->getSecondaryFrictionCoeff(); } +//============================================================================== +Eigen::Vector3d ContactConstraint::computeWorldFirstFrictionDir( + const dynamics::ShapeNode* shapeNode) +{ + assert(shapeNode); + + auto dynamicAspect = shapeNode->getDynamicsAspect(); + + if (dynamicAspect == nullptr) + { + dtwarn << "[ContactConstraint] Attempt to extract friction direction " + << "from a ShapeNode that doesn't have DynamicAspect. The default " + << "value (" << DART_DEFAULT_FRICTION_DIR.transpose() + << ") will be used instead.\n"; + return DART_DEFAULT_FRICTION_DIR; + } + + auto frame = dynamicAspect->getFirstFrictionDirectionFrame(); + const Eigen::Vector3d& frictionDir + = dynamicAspect->getFirstFrictionDirection(); + + // rotate using custom frame if it is specified + if (frame) + { + return frame->getWorldTransform().linear() * frictionDir; + } + // otherwise rotate using shapeNode + return shapeNode->getWorldTransform().linear() * frictionDir; +} + //============================================================================== double ContactConstraint::computeRestitutionCoefficient( const dynamics::ShapeNode* shapeNode) @@ -810,7 +872,7 @@ ContactConstraint::getTangentBasisMatrixODE(const Eigen::Vector3d& n) // Pick an arbitrary vector to take the cross product of (in this case, // Z-axis) - Eigen::Vector3d tangent = mFirstFrictionalDirection.cross(n); + Eigen::Vector3d tangent = n.cross(mFirstFrictionalDirection); // TODO(JS): Modify following lines once _updateFirstFrictionalDirection() is // implemented. @@ -818,16 +880,16 @@ ContactConstraint::getTangentBasisMatrixODE(const Eigen::Vector3d& n) // pick another tangent (use X-axis as arbitrary vector) if (tangent.squaredNorm() < DART_CONTACT_CONSTRAINT_EPSILON_SQUARED) { - tangent = Eigen::Vector3d::UnitX().cross(n); + tangent = n.cross(Eigen::Vector3d::UnitX()); // Make sure this is not zero length, otherwise normalization will lead to // NaN values. if (tangent.squaredNorm() < DART_CONTACT_CONSTRAINT_EPSILON_SQUARED) { - tangent = Eigen::Vector3d::UnitY().cross(n); + tangent = n.cross(Eigen::Vector3d::UnitY()); if (tangent.squaredNorm() < DART_CONTACT_CONSTRAINT_EPSILON_SQUARED) { - tangent = Eigen::Vector3d::UnitZ().cross(n); + tangent = n.cross(Eigen::Vector3d::UnitZ()); // Now tangent shouldn't be zero-length unless the normal is // zero-length, which shouldn't the case because ConstraintSolver @@ -850,8 +912,10 @@ ContactConstraint::getTangentBasisMatrixODE(const Eigen::Vector3d& n) // Note: a possible speedup is in place for mNumDir % 2 = 0 // Each basis and its opposite belong in the matrix, so we iterate half as // many times - T.col(0) = tangent; - T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(0.5_pi, n)) * tangent; + // The first column is the same as mFirstFrictionalDirection unless + // mFirstFrictionalDirection is parallel to the normal + T.col(0) = Eigen::Quaterniond(Eigen::AngleAxisd(0.5_pi, n)) * tangent; + T.col(1) = tangent; return T; } diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index 1d45e5996f2da..b54f08f5b2dc4 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/dart/constraint/ContactConstraint.hpp @@ -144,6 +144,8 @@ class ContactConstraint : public ConstraintBase const dynamics::ShapeNode* shapeNode); static double computeSecondaryFrictionCoefficient( const dynamics::ShapeNode* shapeNode); + static Eigen::Vector3d computeWorldFirstFrictionDir( + const dynamics::ShapeNode* shapenode); static double computeRestitutionCoefficient( const dynamics::ShapeNode* shapeNode); diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 3623122ed16b8..22685d5ac00ca 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -57,7 +57,9 @@ DynamicsAspectProperties::DynamicsAspectProperties( const double frictionCoeff, const double restitutionCoeff) : mFrictionCoeff(frictionCoeff), mRestitutionCoeff(restitutionCoeff), - mSecondaryFrictionCoeff(frictionCoeff) + mSecondaryFrictionCoeff(frictionCoeff), + mFirstFrictionDirection(Eigen::Vector3d::Zero()), + mFirstFrictionDirectionFrame(nullptr) { // Do nothing } @@ -66,10 +68,14 @@ DynamicsAspectProperties::DynamicsAspectProperties( DynamicsAspectProperties::DynamicsAspectProperties( const double primaryFrictionCoeff, const double secondaryFrictionCoeff, - const double restitutionCoeff) + const double restitutionCoeff, + const Eigen::Vector3d& firstFrictionDirection, + const Frame* firstFrictionDirectionFrame) : mFrictionCoeff(primaryFrictionCoeff), mRestitutionCoeff(restitutionCoeff), - mSecondaryFrictionCoeff(secondaryFrictionCoeff) + mSecondaryFrictionCoeff(secondaryFrictionCoeff), + mFirstFrictionDirection(firstFrictionDirection), + mFirstFrictionDirectionFrame(firstFrictionDirectionFrame) { // Do nothing } @@ -195,9 +201,8 @@ void DynamicsAspect::setFrictionCoeff(const double& value) double DynamicsAspect::getFrictionCoeff() const { - return 0.5 * ( - mProperties.mFrictionCoeff + - mProperties.mSecondaryFrictionCoeff); + return 0.5 + * (mProperties.mFrictionCoeff + mProperties.mSecondaryFrictionCoeff); } void DynamicsAspect::setPrimaryFrictionCoeff(const double& value) @@ -210,6 +215,18 @@ const double& DynamicsAspect::getPrimaryFrictionCoeff() const return mProperties.mFrictionCoeff; } +//============================================================================== +void DynamicsAspect::setFirstFrictionDirectionFrame(const Frame* value) +{ + mProperties.mFirstFrictionDirectionFrame = value; +} + +//============================================================================== +const Frame* DynamicsAspect::getFirstFrictionDirectionFrame() const +{ + return mProperties.mFirstFrictionDirectionFrame; +} + //============================================================================== ShapeFrame::~ShapeFrame() { diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index 1cccd4c3646a9..1bfc8bb08a941 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -158,6 +158,18 @@ class DynamicsAspect final : public common::AspectWithVersionedProperties< DART_COMMON_SET_GET_ASPECT_PROPERTY(double, RestitutionCoeff) // void setRestitutionCoeff(const double& value); // const double& getRestitutionCoeff() const; + + /// Set the frame for interpreting the first friction direction vector. + /// The frame pointer defaults to nullptr, which is interpreted as this + /// ShapeFrame. + void setFirstFrictionDirectionFrame(const Frame* value); + + /// Get the frame for the first friction direction vector. + const Frame* getFirstFrictionDirectionFrame() const; + + DART_COMMON_SET_GET_ASPECT_PROPERTY(Eigen::Vector3d, FirstFrictionDirection) + // void setFirstFrictionDirection(const Eigen::Vector3d& value); + // const Eigen::Vector3d& getFirstFrictionDirection() const; }; //============================================================================== diff --git a/dart/dynamics/detail/ShapeFrameAspect.hpp b/dart/dynamics/detail/ShapeFrameAspect.hpp index 493085358652a..3909af437c13f 100644 --- a/dart/dynamics/detail/ShapeFrameAspect.hpp +++ b/dart/dynamics/detail/ShapeFrameAspect.hpp @@ -97,15 +97,39 @@ struct DynamicsAspectProperties /// Secondary coefficient of friction double mSecondaryFrictionCoeff; + /// First friction direction unit vector + Eigen::Vector3d mFirstFrictionDirection; + + /// First friction direction frame + /// The first friction direction unit vector is expressed in this frame + const Frame* mFirstFrictionDirectionFrame; + /// Constructors - /// The frictionCoeff argument will be used for both primary and secondary friction + /// The frictionCoeff argument will be used for both primary and secondary + /// friction DynamicsAspectProperties( const double frictionCoeff = 1.0, const double restitutionCoeff = 0.0); + /// Set primary and secondary friction and restitution coefficients. + /// The first friction direction vector and frame may optionally be set. + /// The vector defaults to a zero-vector, which will cause it to be ignored + /// and the global friction directions used instead. + /// If the vector is set to a non-zero vector, the first friction direction + /// for this shape is computed from this vector expressed in the frame + /// given by the Frame pointer. THe Frame pointer defaults to nullptr, + /// which is interpreted as the body-fixed frame of this Shape. + /// Note that if two objects with custom friction directions come into + /// contact, only one of the directions can be chosen. + /// One approach is to use the first friction direction for the ShapeNode + /// with the smaller primary friction coefficient, since that has the + /// dominant effect. See the ContactConstraint implementation for + /// further details. DynamicsAspectProperties( const double primaryFrictionCoeff, const double secondaryFrictionCoeff, - const double restitutionCoeff); + const double restitutionCoeff, + const Eigen::Vector3d& firstFrictionDirection = Eigen::Vector3d::Zero(), + const Frame* firstFrictionDirectionFrame = nullptr); /// Destructor virtual ~DynamicsAspectProperties() = default; diff --git a/unittests/comprehensive/test_Friction.cpp b/unittests/comprehensive/test_Friction.cpp index 7c7c3ffc7b346..e797c8ceafa2e 100644 --- a/unittests/comprehensive/test_Friction.cpp +++ b/unittests/comprehensive/test_Friction.cpp @@ -77,50 +77,167 @@ TEST(Friction, FrictionPerShapeNode) skeleton1->setName("Skeleton1"); auto skeleton2 = createBox(Eigen::Vector3d(0.3, 0.3, 0.3), Eigen::Vector3d(+0.5, 0, 0)); - skeleton1->setName("Skeleton2"); + skeleton2->setName("Skeleton2"); + auto skeleton3 = createBox( + Eigen::Vector3d(0.3, 0.3, 0.3), + Eigen::Vector3d(+1.5, 0, 0), + Eigen::Vector3d(0, 0, 0.7853981633974483)); + skeleton3->setName("Skeleton3"); + auto skeleton4 = createBox( + Eigen::Vector3d(0.3, 0.3, 0.3), + Eigen::Vector3d(-1.5, 0, 0), + Eigen::Vector3d(0, 0, 0.7853981633974483)); + skeleton4->setName("Skeleton4"); auto body1 = skeleton1->getRootBodyNode(); // default friction values - EXPECT_DOUBLE_EQ(1.0, - body1->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); - EXPECT_DOUBLE_EQ(1.0, + EXPECT_DOUBLE_EQ( + 1.0, body1->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, body1->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); - EXPECT_DOUBLE_EQ(1.0, + EXPECT_DOUBLE_EQ( + 1.0, body1->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); auto body2 = skeleton2->getRootBodyNode(); // default friction values - EXPECT_DOUBLE_EQ(1.0, - body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); - EXPECT_DOUBLE_EQ(1.0, + EXPECT_DOUBLE_EQ( + 1.0, body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); - EXPECT_DOUBLE_EQ(1.0, + EXPECT_DOUBLE_EQ( + 1.0, body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); // test setting primary friction body2->getShapeNode(0)->getDynamicsAspect()->setPrimaryFrictionCoeff(0.5); - EXPECT_DOUBLE_EQ(0.75, - body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); - EXPECT_DOUBLE_EQ(0.5, + EXPECT_DOUBLE_EQ( + 0.75, body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 0.5, body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); - EXPECT_DOUBLE_EQ(1.0, + EXPECT_DOUBLE_EQ( + 1.0, body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); // test setting secondary friction body2->getShapeNode(0)->getDynamicsAspect()->setSecondaryFrictionCoeff(0.25); - EXPECT_DOUBLE_EQ(0.375, - body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); - EXPECT_DOUBLE_EQ(0.5, + EXPECT_DOUBLE_EQ( + 0.375, body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 0.5, body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); - EXPECT_DOUBLE_EQ(0.25, + EXPECT_DOUBLE_EQ( + 0.25, body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); // set all friction coeffs to 0.0 body2->getShapeNode(0)->getDynamicsAspect()->setFrictionCoeff(0.0); - EXPECT_DOUBLE_EQ(0.0, - body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); - EXPECT_DOUBLE_EQ(0.0, + EXPECT_DOUBLE_EQ( + 0.0, body2->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 0.0, body2->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); - EXPECT_DOUBLE_EQ(0.0, + EXPECT_DOUBLE_EQ( + 0.0, body2->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); + auto body3 = skeleton3->getRootBodyNode(); + // default friction values + EXPECT_DOUBLE_EQ( + 1.0, body3->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body3->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body3->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 0.0, + body3->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirection() + .squaredNorm()); + EXPECT_EQ( + nullptr, + body3->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirectionFrame()); + // this body is rotated by 45 degrees, so set friction direction in body frame + // along Y axis so that gravity pushes it in x and y + body3->getShapeNode(0)->getDynamicsAspect()->setPrimaryFrictionCoeff(0.0); + body3->getShapeNode(0)->getDynamicsAspect()->setFirstFrictionDirection( + Eigen::Vector3d(0, 1, 0)); + // check friction values + EXPECT_DOUBLE_EQ( + 0.5, body3->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 0.0, + body3->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body3->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body3->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirection() + .squaredNorm()); + EXPECT_EQ( + nullptr, + body3->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirectionFrame()); + + auto body4 = skeleton4->getRootBodyNode(); + // default friction values + EXPECT_DOUBLE_EQ( + 1.0, body4->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body4->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body4->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 0.0, + body4->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirection() + .squaredNorm()); + EXPECT_EQ( + nullptr, + body4->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirectionFrame()); + // this body is rotated by 45 degrees, but set friction direction according to + // world frame so that the body orientation doesn't matter. thus a diagonal + // axis is needed to push it in x and y + body4->getShapeNode(0)->getDynamicsAspect()->setPrimaryFrictionCoeff(0.0); + body4->getShapeNode(0)->getDynamicsAspect()->setFirstFrictionDirectionFrame( + Frame::World()); + body4->getShapeNode(0)->getDynamicsAspect()->setFirstFrictionDirection( + Eigen::Vector3d(0.5 * std::sqrt(2), 0.5 * std::sqrt(2), 0)); + // check friction values + EXPECT_DOUBLE_EQ( + 0.5, body4->getShapeNode(0)->getDynamicsAspect()->getFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 0.0, + body4->getShapeNode(0)->getDynamicsAspect()->getPrimaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body4->getShapeNode(0)->getDynamicsAspect()->getSecondaryFrictionCoeff()); + EXPECT_DOUBLE_EQ( + 1.0, + body4->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirection() + .squaredNorm()); + EXPECT_EQ( + Frame::World(), + body4->getShapeNode(0) + ->getDynamicsAspect() + ->getFirstFrictionDirectionFrame()); + // Create a world and add the rigid body auto world = simulation::World::create(); EXPECT_TRUE(equals(world->getGravity(), ::Eigen::Vector3d(0, 0, -9.81))); @@ -130,6 +247,8 @@ TEST(Friction, FrictionPerShapeNode) world->addSkeleton(createFloor()); world->addSkeleton(skeleton1); world->addSkeleton(skeleton2); + world->addSkeleton(skeleton3); + world->addSkeleton(skeleton4); const auto numSteps = 500; for (auto i = 0u; i < numSteps; ++i) @@ -139,13 +258,31 @@ TEST(Friction, FrictionPerShapeNode) // Wait until the first box settle-in on the ground if (i > 300) { + const auto x1 = body1->getTransform().translation()[0]; const auto y1 = body1->getTransform().translation()[1]; + EXPECT_NEAR(x1, -0.5, 0.00001); EXPECT_NEAR(y1, -0.17889, 0.001); - // The second box still moves even after landed on the ground because its + // The second box still moves even after landing on the ground because its // friction is zero. + const auto x2 = body2->getTransform().translation()[0]; const auto y2 = body2->getTransform().translation()[1]; + EXPECT_NEAR(x2, 0.5, 0.00001); EXPECT_LE(y2, -0.17889); + + // The third box still moves even after landing on the ground because its + // friction is zero along the first friction direction. + const auto x3 = body3->getTransform().translation()[0]; + const auto y3 = body3->getTransform().translation()[1]; + EXPECT_GE(x3, 1.5249); + EXPECT_LE(y3, -0.20382); + + // The fourth box still moves even after landing on the ground because its + // friction is zero along the first friction direction. + const auto x4 = body4->getTransform().translation()[0]; + const auto y4 = body4->getTransform().translation()[1]; + EXPECT_LE(x4, -1.5249); + EXPECT_LE(y4, -0.20382); } } }