Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add friction direction unit vector and frame parameters #1427

Merged
merged 12 commits into from
Dec 7, 2019
Merged

Add friction direction unit vector and frame parameters #1427

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
ed3a488
Add friction direction vector and frame parameters
scpeters Jul 11, 2019
d1d8073
compute body-fixed friction directions
scpeters Jul 12, 2019
0195b7a
Fix handling of first frictional direction
azeey Jul 17, 2019
843b40b
test_Friction: test friction direction parameters
scpeters Nov 26, 2019
ed85373
ContactConstraint.cpp use const &
scpeters Dec 4, 2019
1962ef3
ContactConstraint.cpp: print transpose
scpeters Dec 4, 2019
a932d87
ContactConstraint.cpp: update comment
scpeters Dec 4, 2019
5fe40b7
ContactConstraint: reverse tangent direction
scpeters Dec 6, 2019
cddef7a
add documentation of friction direction params
scpeters Dec 6, 2019
b3438cb
Format code
Dec 6, 2019
691fa12
Update changelog
Dec 6, 2019
d2b9e0d
Fix typo
Dec 6, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
78 changes: 70 additions & 8 deletions dart/constraint/ContactConstraint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,8 @@ 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(
Expand All @@ -82,7 +84,7 @@ ContactConstraint::ContactConstraint(
->getBodyNodePtr()
.get()),
mContact(contact),
mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()),
mFirstFrictionalDirection(DART_DEFAULT_FRICTION_DIR),
mIsFrictionOn(true),
mAppliedImpulseIndex(dynamics::INVALID_INDEX),
mIsBounceOn(false),
Expand Down Expand Up @@ -135,6 +137,40 @@ ContactConstraint::ContactConstraint(
{
mIsFrictionOn = true;

// 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 (primaryFrictionCoeffA <= primaryFrictionCoeffB)
{
mFirstFrictionalDirection = frictionDirA.normalized();
}
else
{
mFirstFrictionalDirection = frictionDirB.normalized();
}
}
else if (nonzeroDirA)
{
mFirstFrictionalDirection = frictionDirA.normalized();
}
else
{
mFirstFrictionalDirection = frictionDirB.normalized();
}
}

// Update frictional direction
updateFirstFrictionalDirection();
}
Expand All @@ -147,11 +183,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)
{
Expand Down Expand Up @@ -754,6 +785,35 @@ 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 << ") will be used "
scpeters marked this conversation as resolved.
Show resolved Hide resolved
<< "instead.\n";
return DART_DEFAULT_FRICTION_DIR;
}

auto frame = dynamicAspect->getFirstFrictionDirectionFrame();
Eigen::Vector3d frictionDir = dynamicAspect->getFirstFrictionDirection();
scpeters marked this conversation as resolved.
Show resolved Hide resolved

// 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)
Expand Down Expand Up @@ -850,8 +910,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;
Comment on lines +915 to +918
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's not the part of this change, but I would like to negate the direction of tangent so that T.col(0).cross(T.col(1)) == n (currently, it's T.col(1).cross(T.col(0)) == n) because I believe it'd be more intuitive. This change wouldn't affect the behavior (but just flipping the sign of impulse for the second tangential friction).

It can be done by changing the order of all the cross products in this function. For example, line 813 should be changed to:

Eigen::Vector3d tangent = n.cross(mFirstFrictionalDirection);

or simply negating the final tangent. I'd prefer changing the order of the cross products though.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not opposed to that, but I'll bring @azeey into the conversation since he made this specific change

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Negating the direction of tangent sounds fine to me if it doesn't affect the behavior. I think the main change here is that T.col(0) has to be parallel to the first frictional direction.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

reversed in 5fe40b7

return T;
}

Expand Down
2 changes: 2 additions & 0 deletions dart/constraint/ContactConstraint.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
24 changes: 21 additions & 3 deletions dart/dynamics/ShapeFrame.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,9 @@ DynamicsAspectProperties::DynamicsAspectProperties(
const double frictionCoeff, const double restitutionCoeff)
: mFrictionCoeff(frictionCoeff),
mRestitutionCoeff(restitutionCoeff),
mSecondaryFrictionCoeff(frictionCoeff)
mSecondaryFrictionCoeff(frictionCoeff),
mFirstFrictionDirection(Eigen::Vector3d::Zero()),
Copy link
Member

@jslee02 jslee02 Dec 2, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Currently, the first friction direction will be set to z-direction in world coordinates (unless it's parallel to the contact normal). Maybe we want to set the default direction to non-zero direction (e.g., Eigen::Vector3d::UnitZ()) so that the default behavior is more consistent in terms of the friction direction?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

my intent was that an unset vector with zero length would use the global friction direction, and that only friction directions that are explicitly set to have a nonzero length would use their custom direction and frame. See the following logic in ContactConstraint.cpp

  • dart/dart/constraint/ContactConstraint.cpp

    Lines 148 to 172 in a932d87

    // 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();
    }
    }

does that make sense?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, I read the logic but was suggesting changing the default behavior to using local friction direction (by setting mFirstFrictionDirection to non-zero vector intentionally) because first/second friction coefficients would make more sense when they're aligned with local directions (if they have different values).

Downside of it is that it changes the default simulation behavior (from using global friction direction to local friction direction) though. I'm okay to leave as it is as long as the logic is documented.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

a tricky aspect of the friction directions is choosing which to use when custom directions are set for both bodies. I've added logic in this PR to choose the direction from the body with the smaller primary friction coefficient, since that would be the dominant parameter, which makes sense to me, but I haven't tested it, and I'd be wary of making a big behavior change like that without testing.

Is the logic sufficiently documented? If not, I'll add the necessary documentation.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Agreed to not changing the default behavior.

It's well documented in the implementation, but it'd be great if it's also documented in the header as well (maybe on the constructor?).

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

how does cddef7a look to you?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks awesome! Thank you for adding it.

mFirstFrictionDirectionFrame(nullptr)
{
// Do nothing
}
Expand All @@ -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
}
Expand Down Expand Up @@ -210,6 +216,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()
{
Expand Down
12 changes: 12 additions & 0 deletions dart/dynamics/ShapeFrame.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

//==============================================================================
Expand Down
10 changes: 9 additions & 1 deletion dart/dynamics/detail/ShapeFrameAspect.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,12 @@ struct DynamicsAspectProperties
/// Secondary coefficient of friction
double mSecondaryFrictionCoeff;

/// First friction direction unit vector
Eigen::Vector3d mFirstFrictionDirection;

/// First friction direction frame
const Frame* mFirstFrictionDirectionFrame;

/// Constructors
/// The frictionCoeff argument will be used for both primary and secondary friction
DynamicsAspectProperties(
Expand All @@ -105,7 +111,9 @@ struct DynamicsAspectProperties
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;
Expand Down
91 changes: 89 additions & 2 deletions unittests/comprehensive/test_Friction.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,15 @@ 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
Expand Down Expand Up @@ -121,6 +129,65 @@ TEST(Friction, FrictionPerShapeNode)
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)));
Expand All @@ -130,6 +197,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)
Expand All @@ -139,13 +208,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);
}
}
}