From fa7711225a910ce4fde3472c024f8c4e56387778 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 11 Aug 2020 03:03:39 +0200 Subject: [PATCH 1/4] [Gazebo9] Fixed fails for OSX: Added using namespace boost::placeholders (#2809) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Co-authored-by: Jose Luis Rivero Co-authored-by: Louise Poubel Co-authored-by: Steve Peters --- examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt | 6 ++++++ gazebo/gui/model/ModelTreeWidget.cc | 6 ++++++ gazebo/gui/model/SchematicViewWidget.cc | 6 ++++++ test/plugins/ForceTorqueModelRemovalTestPlugin.cc | 5 ++++- 4 files changed, 22 insertions(+), 1 deletion(-) diff --git a/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt b/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt index c99ca11eae..3b53c22df5 100644 --- a/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt +++ b/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt @@ -12,3 +12,9 @@ link_directories(${GAZEBO_LIBRARY_DIRS}) add_executable(custom_main custom_main.cc) target_link_libraries(custom_main ${GAZEBO_LIBRARIES}) + +if(${CMAKE_VERSION} VERSION_LESS "3.13.0") + link_directories(${Boost_LIBRARY_DIRS}) +else() + target_link_directories(custom_main PUBLIC ${Boost_LIBRARY_DIRS}) +endif() diff --git a/gazebo/gui/model/ModelTreeWidget.cc b/gazebo/gui/model/ModelTreeWidget.cc index 860b930a5f..14f52ca4cb 100644 --- a/gazebo/gui/model/ModelTreeWidget.cc +++ b/gazebo/gui/model/ModelTreeWidget.cc @@ -15,6 +15,8 @@ * */ +#include + #include "gazebo/common/Events.hh" #include "gazebo/gui/GuiEvents.hh" @@ -160,6 +162,10 @@ ModelTreeWidget::ModelTreeWidget(QWidget *_parent) this->layout()->setContentsMargins(0, 0, 0, 0); // Connections + #if BOOST_VERSION >= 107300 + using namespace boost::placeholders; + #endif + this->connections.push_back( gui::model::Events::ConnectSaveModel( boost::bind(&ModelTreeWidget::OnSaveModel, this, _1))); diff --git a/gazebo/gui/model/SchematicViewWidget.cc b/gazebo/gui/model/SchematicViewWidget.cc index 957a41f64b..8a40be569e 100644 --- a/gazebo/gui/model/SchematicViewWidget.cc +++ b/gazebo/gui/model/SchematicViewWidget.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include "gazebo/rendering/Material.hh" @@ -82,6 +84,10 @@ void SchematicViewWidget::Reset() ///////////////////////////////////////////////// void SchematicViewWidget::Init() { + #if BOOST_VERSION >= 107300 + using namespace boost::placeholders; + #endif + this->connections.push_back(gui::model::Events::ConnectLinkInserted( boost::bind(&SchematicViewWidget::AddNode, this, _1))); diff --git a/test/plugins/ForceTorqueModelRemovalTestPlugin.cc b/test/plugins/ForceTorqueModelRemovalTestPlugin.cc index 70d7d9ddbb..6cccd943b8 100644 --- a/test/plugins/ForceTorqueModelRemovalTestPlugin.cc +++ b/test/plugins/ForceTorqueModelRemovalTestPlugin.cc @@ -15,6 +15,8 @@ * */ +#include + #include "plugins/ForceTorqueModelRemovalTestPlugin.hh" #include "gazebo/sensors/ForceTorqueSensor.hh" @@ -53,7 +55,8 @@ void ForceTorqueModelRemovalTestPlugin::Load(sensors::SensorPtr _sensor, // Create connection this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin( - boost::bind(&ForceTorqueModelRemovalTestPlugin::onUpdate, this, _1)); + std::bind(&ForceTorqueModelRemovalTestPlugin::onUpdate, this, + std::placeholders::_1)); } void ForceTorqueModelRemovalTestPlugin::onUpdate( From cf4a7f05e43c542025ca602e9840f5083c623836 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 19 Aug 2020 10:54:48 -0700 Subject: [PATCH 2/4] DARTHeightmapShape: correctly load asymmetric terrains (#2818) The heightmap_valley.png used in the heightmap tests for ODE and DART is symmetric, which obscures the fact that DART is not treating asymmetric heightmaps properly. This adds a test using worlds/heightmap.world and heightmap_bowl.png to illustrate the bug. * DARTHeightmapShape: set flipY = false This variable was uninitialized and was causing problems with asymmetric heightmaps. Signed-off-by: Steve Peters --- gazebo/physics/dart/DARTHeightmapShape.cc | 1 + test/integration/heightmap.cc | 78 +++++++++++++++++++++++ worlds/heightmap.world | 5 ++ 3 files changed, 84 insertions(+) diff --git a/gazebo/physics/dart/DARTHeightmapShape.cc b/gazebo/physics/dart/DARTHeightmapShape.cc index 59fbe55f50..b86c9ba07d 100644 --- a/gazebo/physics/dart/DARTHeightmapShape.cc +++ b/gazebo/physics/dart/DARTHeightmapShape.cc @@ -30,6 +30,7 @@ DARTHeightmapShape::DARTHeightmapShape(DARTCollisionPtr _parent) : HeightmapShape(_parent), dataPtr(new DARTHeightmapShapePrivate()) { + this->flipY = false; } ////////////////////////////////////////////////// diff --git a/test/integration/heightmap.cc b/test/integration/heightmap.cc index ef8f1db5dd..4e574af08f 100644 --- a/test/integration/heightmap.cc +++ b/test/integration/heightmap.cc @@ -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(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 diff --git a/worlds/heightmap.world b/worlds/heightmap.world index 3cddc00edf..b102a80296 100644 --- a/worlds/heightmap.world +++ b/worlds/heightmap.world @@ -1,6 +1,11 @@ + + + bullet + + model://sun From 18b729b39dd6f3c0b8e0edfc0d8e00e25587f356 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 24 Sep 2020 09:38:03 -0700 Subject: [PATCH 3/4] Set friction params in DARTCollision for dart 6.10 (#2781) 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, including the DirectionNaN test. Signed-off-by: Steve Peters --- gazebo/physics/dart/DARTCollision.cc | 17 +++++++++++++- gazebo/physics/dart/DARTLink.cc | 2 ++ gazebo/physics/dart/DARTSurfaceParams.cc | 2 ++ gazebo/physics/dart/DARTSurfaceParams.hh | 4 +++- test/integration/physics_friction.cc | 30 ++++++++++++++++++++---- test/worlds/friction_dir_test.world | 5 ++++ test/worlds/friction_dir_test.world.erb | 5 ++++ 7 files changed, 58 insertions(+), 7 deletions(-) diff --git a/gazebo/physics/dart/DARTCollision.cc b/gazebo/physics/dart/DARTCollision.cc index c09331f71a..73f1282787 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 2447dd92e7..38a271139e 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 edd1590ab5..db8561488c 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..6197e6497d 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; } } @@ -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); 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 From e19438f5032b57e60baf1991673c273b036ae079 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 23 Jan 2021 23:07:33 -0800 Subject: [PATCH 4/4] changelog Signed-off-by: Steve Peters --- Changelog.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Changelog.md b/Changelog.md index 0dc748a8db..1bf471a6ad 100644 --- a/Changelog.md +++ b/Changelog.md @@ -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/)