Set friction params in DARTCollision for dart 6.10

They are currently being set in DARTLink; set them in DARTCollision instead if dart 6.10 is in use. Also set the DART friction direction and enable corresponding tests Signed-off-by: Steve Peters <scpeters@openrobotics.org>
gazebosim · Jul 8, 2020 · 49b5421 · 49b5421
1 parent 293ede8
commit 49b5421
Show file tree

Hide file tree

Showing 7 changed files with 44 additions and 5 deletions.
diff --git 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
@@ -204,6 +204,7 @@ void DARTLink::Init()
   // Gravity mode
   this->SetGravityMode(this->sdf->Get<bool>("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
@@ -64,6 +64,8 @@ void DARTSurfaceParams::Load(sdf::ElementPtr _sdf)
           frictionOdeElem->Get<double>("mu"));
         this->dataPtr->frictionPyramid->SetMuSecondary(
           frictionOdeElem->Get<double>("mu2"));
+        this->dataPtr->frictionPyramid->direction1 =
+          frictionOdeElem->Get<ignition::math::Vector3d>("fdir1");
       }
     }
   }

diff --git 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
@@ -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
@@ -9,6 +9,11 @@
       <uri>model://ground_plane</uri>
     </include>
     <gravity>0 1 -9.81</gravity>
+    <physics type="ode">
+      <dart>
+        <collision_detector>bullet</collision_detector>
+      </dart>
+    </physics>
 
     <model name='spheres'>
       <pose>0 3 0.25  0 0 0</pose>

diff --git a/test/worlds/friction_dir_test.world.erb b/test/worlds/friction_dir_test.world.erb
@@ -9,6 +9,11 @@
       <uri>model://ground_plane</uri>
     </include>
     <gravity>0 1 -9.81</gravity>
+    <physics type="ode">
+      <dart>
+        <collision_detector>bullet</collision_detector>
+      </dart>
+    </physics>
 <%
   # Test pyramid friction model
   # Set asymmetric friction coefficients and friction direction