gazebosim · scpeters · Jan 25, 2021 · Aug 11, 2020 · Aug 19, 2020 · Sep 24, 2020
@@ -2,6 +2,12 @@
 
 ## Gazebo 11.x.x (202x-xx-xx)
 
+1. DARTHeightmapShape: correctly load asymmetric terrains
+    * [Pull request #2818](https://github.com/osrf/gazebo/pull/2818)
+
+1. Set friction params in DARTCollision for dart 6.10
+    * [Pull request #2781](https://github.com/osrf/gazebo/pull/2781)
+
 ## Gazebo 11.3.0 (2020-11-26)
 
 1. Added profiler to gazebo::rendering and gzclient
@@ -188,6 +194,12 @@
 
 ## Gazebo 10.x.x (202x-xx-xx)
 
+1. DARTHeightmapShape: correctly load asymmetric terrains
+    * [Pull request #2818](https://github.com/osrf/gazebo/pull/2818)
+
+1. Set friction params in DARTCollision for dart 6.10
+    * [Pull request #2781](https://github.com/osrf/gazebo/pull/2781)
+
 1. Fix problem with automoc in CMake 3.17
     * [BitBucket pull request 3206](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/3206/)
 

@@ -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
   }
 }
 

@@ -30,6 +30,7 @@ DARTHeightmapShape::DARTHeightmapShape(DARTCollisionPtr _parent)
   : HeightmapShape(_parent),
     dataPtr(new DARTHeightmapShapePrivate<HeightmapShape::HeightType>())
 {
+  this->flipY = false;
 }
 
 //////////////////////////////////////////////////

@@ -205,6 +205,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
@@ -247,6 +248,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

@@ -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");
       }
     }
   }

@@ -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.

@@ -69,6 +69,9 @@ class HeightmapTest : public ServerFixture,
   //  is the collision detector to use in DART. Can be fcl, dart, bullet or ode.
   public: void TerrainCollision(const std::string &_physicsEngine,
                                 const std::string &_dartCollision = "");
+  // \brief Test dropping boxes on asymmetric terrain
+  // \param[in] _physics the physics engine to test
+  public: void TerrainCollisionAsymmetric(const std::string &_physics);
 
   /// \brief Test loading a heightmap that has no visuals
   public: void NoVisual();
@@ -884,6 +887,75 @@ void HeightmapTest::TerrainCollision(const std::string &_physicsEngine,
   EXPECT_GE(spherePose.Pos().Z(), (minHeight + radius*0.99));
 }
 
+/////////////////////////////////////////////////
+void HeightmapTest::TerrainCollisionAsymmetric(const std::string &_physics)
+{
+  if (_physics == "bullet")
+  {
+    gzerr << "Skipping test for bullet. See issue #2506" << std::endl;
+    return;
+  }
+
+  if (_physics == "simbody")
+  {
+    // SimbodyHeightmapShape unimplemented.
+    gzerr << "Aborting test for " << _physics << std::endl;
+    return;
+  }
+
+  Load("worlds/heightmap.world", true, _physics);
+
+  physics::WorldPtr world = physics::get_world("default");
+  ASSERT_NE(world, nullptr);
+
+  if (_physics == "dart")
+  {
+#ifdef HAVE_DART
+    physics::PhysicsEnginePtr engine = world->Physics();
+    ASSERT_NE(engine, nullptr);
+    physics::DARTPhysicsPtr dartEngine
+      = boost::dynamic_pointer_cast<physics::DARTPhysics>(engine);
+    ASSERT_NE(dartEngine, nullptr);
+    std::string cd = dartEngine->CollisionDetectorInUse();
+    ASSERT_FALSE(cd.empty());
+    if (cd != "bullet")
+    {
+      // the test only works if DART uses bullet as a collision detector at the
+      // moment.
+      gzerr << "Aborting test for dart, see issue #909 and pull request #2956"
+            << std::endl;
+      return;
+    }
+#else
+    gzerr << "Have no DART installed, skipping test for DART." << std::endl;
+    return;
+#endif
+  }
+
+  // each box has an initial z position of 10. meters
+  physics::ModelPtr box1 = GetModel("box1");
+  physics::ModelPtr box2 = GetModel("box2");
+  physics::ModelPtr box3 = GetModel("box3");
+  physics::ModelPtr box4 = GetModel("box4");
+  ASSERT_NE(box1, nullptr);
+  ASSERT_NE(box2, nullptr);
+  ASSERT_NE(box3, nullptr);
+  ASSERT_NE(box4, nullptr);
+  EXPECT_GE(box1->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box2->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box3->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box4->WorldPose().Pos().Z(), 9.9);
+
+  // step the world and verify that only box2 falls
+  world->Step(1000);
+
+  EXPECT_GE(box1->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box3->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box4->WorldPose().Pos().Z(), 9.9);
+
+  EXPECT_LT(box2->WorldPose().Pos().Z(), 5.5);
+}
+
 /////////////////////////////////////////////////
 TEST_F(HeightmapTest, NotSquareImage)
 {
@@ -1055,6 +1127,12 @@ TEST_F(HeightmapTest, TerrainCollisionDartBullet)
   TerrainCollision("dart", "bullet");
 }
 
+/////////////////////////////////////////////////
+TEST_P(HeightmapTest, TerrainCollisionAsymmetric)
+{
+  TerrainCollisionAsymmetric(GetParam());
+}
+
 /////////////////////////////////////////////////
 //
 // Disabled: segfaults ocassionally

@@ -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;
   }
 }
 
@@ -477,13 +485,17 @@ void PhysicsFrictionTest::DirectionNaN(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/empty.world", true, _physicsEngine);
@@ -495,6 +507,14 @@ void PhysicsFrictionTest::DirectionNaN(const std::string &_physicsEngine)
   ASSERT_TRUE(physics != NULL);
   EXPECT_EQ(physics->GetType(), _physicsEngine);
 
+#ifdef HAVE_DART
+  // Use bullet collision detector with DART
+  if (_physicsEngine == "dart")
+  {
+    physics->SetParam("collision_detector", std::string("bullet"));
+  }
+#endif
+
   // set the gravity vector
   // small positive y component
   ignition::math::Vector3d g(0.0, 1.5, -1.0);

@@ -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>

@@ -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

@@ -1,6 +1,11 @@
 <?xml version="1.0" ?>
 <sdf version="1.5">
   <world name="default">
+    <physics type="ode">
+      <dart>
+        <collision_detector>bullet</collision_detector>
+      </dart>
+    </physics>
     <!-- A global light source -->
     <include>
       <uri>model://sun</uri>