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

dartsim: Fix sign convention error with contact surface motion velocities #556

Merged
merged 2 commits into from
Sep 28, 2023
Merged

dartsim: Fix sign convention error with contact surface motion velocities #556

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1f2da25
dartsim: Fix sign convention error with contact surface motion veloci…
azeey Sep 28, 2023
6de0392
Update to pre4
azeey Sep 28, 2023
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
32 changes: 30 additions & 2 deletions dartsim/src/SimulationFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,20 @@
// See https://github.com/dartsim/dart/pull/1626 and
// https://github.com/gazebo-forks/dart/pull/22 for more info.
#define DART_HAS_UPSTREAM_FRICTION_VARIABLE_NAMES
// There's a sign difference between our fork and upstream dart in the
// implementation of surface velocities. Here, we assume that 6.13 is the
// upstream version. There's a potential that a user might update our fork to
// version 6.13.0 or later. To support that case, we'll assume
// DART_HAS_POSITIVE_CONTACT_SURFACE_MOTION_VELOCITY will be defined in the
// fork. Note, if we simply check for
// `DART_HAS_POSITIVE_CONTACT_SURFACE_MOTION_VELOCITY ` without the 6.13
// condition above, we might break users that are building with our 6.10 fork
// without updating or adding the
// DART_HAS_POSITIVE_CONTACT_SURFACE_MOTION_VELOCITY define in their version of
// dart.
#ifndef DART_HAS_POSITIVE_CONTACT_SURFACE_MOTION_VELOCITY
#define DART_HAS_NEGATIVE_CONTACT_SURFACE_MOTION_VELOCITY
#endif
#endif

namespace gz {
Expand Down Expand Up @@ -257,6 +271,17 @@ dart::constraint::ContactSurfaceParams GzContactSurfaceHandler::createParams(
typedef FeaturePolicy3d P;
typename F::ContactSurfaceParams<P> pGz;

auto convertMotionVelocity = [](Eigen::Vector3d _input) {
#ifdef DART_HAS_NEGATIVE_CONTACT_SURFACE_MOTION_VELOCITY
// The y and z components correspond to the velocities in the first and
// second friction directions. These have to be inverted in the upstream
// version of DART. https://github.com/gazebo-forks/dart/pull/33
_input.y() = -_input.y();
_input.z() = -_input.z();
#endif
return _input;
};

#ifdef DART_HAS_UPSTREAM_FRICTION_VARIABLE_NAMES
pGz.frictionCoeff = pDart.mPrimaryFrictionCoeff;
#else
Expand All @@ -271,7 +296,8 @@ dart::constraint::ContactSurfaceParams GzContactSurfaceHandler::createParams(
pGz.secondarySlipCompliance = pDart.mSecondarySlipCompliance;
pGz.restitutionCoeff = pDart.mRestitutionCoeff;
pGz.firstFrictionalDirection = pDart.mFirstFrictionalDirection;
pGz.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity;
pGz.contactSurfaceMotionVelocity =
convertMotionVelocity(pDart.mContactSurfaceMotionVelocity);

auto contactInternal = this->convertContact(_contact);
if (contactInternal)
Expand Down Expand Up @@ -304,8 +330,10 @@ dart::constraint::ContactSurfaceParams GzContactSurfaceHandler::createParams(
if (pGz.firstFrictionalDirection)
pDart.mFirstFrictionalDirection = pGz.firstFrictionalDirection.value();
if (pGz.contactSurfaceMotionVelocity)
{
pDart.mContactSurfaceMotionVelocity =
pGz.contactSurfaceMotionVelocity.value();
convertMotionVelocity(pGz.contactSurfaceMotionVelocity.value());
}

static bool warnedRollingFrictionCoeff = false;
if (!warnedRollingFrictionCoeff && pGz.rollingFrictionCoeff)
Expand Down
4 changes: 4 additions & 0 deletions test/common_test/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,10 @@ foreach(test ${tests})
"GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\""
)

if (DART_HAS_CONTACT_SURFACE_HEADER)
target_compile_definitions(${test_executable} PRIVATE DART_HAS_CONTACT_SURFACE)
endif()

install(TARGETS ${test_executable} DESTINATION ${TEST_INSTALL_DIR})

configure_common_test("bullet" ${test_executable})
Expand Down
1 change: 1 addition & 0 deletions test/common_test/simulation_features.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@

#include "gz/physics/BoxShape.hh"
#include <gz/physics/GetContacts.hh>
#include "gz/physics/ContactProperties.hh"
#include "gz/physics/CylinderShape.hh"
#include "gz/physics/CapsuleShape.hh"
#include "gz/physics/EllipsoidShape.hh"
Expand Down