diff --git a/gazebo/physics/dart/DARTCollision.cc b/gazebo/physics/dart/DARTCollision.cc index 1748c2303a..ff8eb7eecb 100644 --- a/gazebo/physics/dart/DARTCollision.cc +++ b/gazebo/physics/dart/DARTCollision.cc @@ -26,6 +26,7 @@ #include "gazebo/physics/dart/DARTCollision.hh" #include "gazebo/physics/dart/DARTPlaneShape.hh" #include "gazebo/physics/dart/DARTSurfaceParams.hh" +#include "gazebo/physics/dart/DARTTypes.hh" #include "gazebo/physics/dart/DARTCollisionPrivate.hh" @@ -69,7 +70,7 @@ void DARTCollision::Init() // Init(), because the BodyNode will only have been created in Load() // and is not guaranteed to exist before. - // Set the pose offset. + // Set the pose offset and friction parameters. if (this->dataPtr->dtCollisionShape) { // TODO: Remove type check once DART completely supports plane shape. @@ -82,6 +83,20 @@ void DARTCollision::Init() Eigen::Isometry3d tf = DARTTypes::ConvPose(this->RelativePose()); this->dataPtr->dtCollisionShape->setRelativeTransform(tf); } + +#if DART_MAJOR_MINOR_VERSION_AT_LEAST(6, 10) + // Set friction parameters. + auto surf = this->DARTSurface(); + GZ_ASSERT(surf, "Surface pointer is invalid"); + FrictionPyramidPtr friction = surf->FrictionPyramid(); + GZ_ASSERT(friction, "Friction pointer is invalid"); + auto aspect = this->dataPtr->dtCollisionShape->getDynamicsAspect(); + GZ_ASSERT(aspect, "DynamicsAspect pointer is invalid"); + aspect->setFrictionCoeff(friction->MuPrimary()); + aspect->setSecondaryFrictionCoeff(friction->MuSecondary()); + aspect->setFirstFrictionDirection( + DARTTypes::ConvVec3(friction->direction1)); +#endif } } diff --git a/gazebo/physics/dart/DARTLink.cc b/gazebo/physics/dart/DARTLink.cc index 6f17f90d42..bf2c6da3ec 100644 --- a/gazebo/physics/dart/DARTLink.cc +++ b/gazebo/physics/dart/DARTLink.cc @@ -204,6 +204,7 @@ void DARTLink::Init() // Gravity mode this->SetGravityMode(this->sdf->Get("gravity")); +#if DART_MAJOR_MINOR_VERSION_AT_MOST(6, 9) // Friction coefficient /// \todo FIXME: Friction Parameters @@ -246,6 +247,7 @@ void DARTLink::Init() // friction coefficient may not be negative in DART coeff = std::max(0.0f, coeff); this->dataPtr->dtBodyNode->setFrictionCoeff(coeff); +#endif // We don't add dart body node to the skeleton here because dart body node // should be set its parent joint before being added. This body node will be diff --git a/gazebo/physics/dart/DARTSurfaceParams.cc b/gazebo/physics/dart/DARTSurfaceParams.cc index daca5ea75f..8a9ad432b1 100644 --- a/gazebo/physics/dart/DARTSurfaceParams.cc +++ b/gazebo/physics/dart/DARTSurfaceParams.cc @@ -64,6 +64,8 @@ void DARTSurfaceParams::Load(sdf::ElementPtr _sdf) frictionOdeElem->Get("mu")); this->dataPtr->frictionPyramid->SetMuSecondary( frictionOdeElem->Get("mu2")); + this->dataPtr->frictionPyramid->direction1 = + frictionOdeElem->Get("fdir1"); } } } diff --git a/gazebo/physics/dart/DARTSurfaceParams.hh b/gazebo/physics/dart/DARTSurfaceParams.hh index 98203894ac..a7083261d7 100644 --- a/gazebo/physics/dart/DARTSurfaceParams.hh +++ b/gazebo/physics/dart/DARTSurfaceParams.hh @@ -34,7 +34,9 @@ namespace gazebo /// Forward declare private data class class DARTSurfaceParamsPrivate; - /// \brief DART surface parameters. + /// \brief Data structure containing DART surface parameters. + /// Updating the parameters in this class doesn't update the + /// actual DART objects. class GZ_PHYSICS_VISIBLE DARTSurfaceParams : public SurfaceParams { /// \brief Constructor. diff --git a/test/integration/physics_friction.cc b/test/integration/physics_friction.cc index 6ffff04c4f..14072b0b4d 100644 --- a/test/integration/physics_friction.cc +++ b/test/integration/physics_friction.cc @@ -29,6 +29,10 @@ #include "gazebo/test/helper_physics_generator.hh" #include "gazebo/gazebo_config.h" +#ifdef HAVE_DART +#include "gazebo/physics/dart/DARTTypes.hh" +#endif + using namespace gazebo; const double g_friction_tolerance = 1e-3; @@ -380,13 +384,17 @@ void PhysicsFrictionTest::BoxDirectionRing(const std::string &_physicsEngine) << std::endl; return; } +#ifdef HAVE_DART +#if DART_MAJOR_MINOR_VERSION_AT_MOST(6, 9) if (_physicsEngine == "dart") { - gzerr << "Aborting test since there's an issue with dart's friction" - << " parameters (#1000)" + gzerr << "Aborting test since dart 6.9 and earlier doesn't support" + << " body-fixed friction directions (#1000)." << std::endl; return; } +#endif +#endif // Load an empty world Load("worlds/friction_dir_test.world", true, _physicsEngine); @@ -454,7 +462,7 @@ void PhysicsFrictionTest::BoxDirectionRing(const std::string &_physicsEngine) // so spinning the spheres should cause them to start rolling // check that spheres are spinning about the X axis auto w = link->WorldAngularVel(); - EXPECT_LT(w.X(), -4) << "Checking " << link->GetScopedName() << std::endl; + EXPECT_LT(w.X(), -3) << "Checking " << link->GetScopedName() << std::endl; } } diff --git a/test/worlds/friction_dir_test.world b/test/worlds/friction_dir_test.world index 2b59fe1e64..7374f14ad0 100644 --- a/test/worlds/friction_dir_test.world +++ b/test/worlds/friction_dir_test.world @@ -9,6 +9,11 @@ model://ground_plane 0 1 -9.81 + + + bullet + + 0 3 0.25 0 0 0 diff --git a/test/worlds/friction_dir_test.world.erb b/test/worlds/friction_dir_test.world.erb index 43e5d51bc5..8c1b2724f5 100644 --- a/test/worlds/friction_dir_test.world.erb +++ b/test/worlds/friction_dir_test.world.erb @@ -9,6 +9,11 @@ model://ground_plane 0 1 -9.81 + + + bullet + + <% # Test pyramid friction model # Set asymmetric friction coefficients and friction direction