From 1c18e3d14e23e01520f8e32193bd09735a82f5dd Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 20 Jan 2022 14:21:50 -0800 Subject: [PATCH 1/3] Fix center of volume's reference frame Signed-off-by: Louise Poubel --- .../gazebo/components/CenterOfVolume.hh | 2 +- src/systems/buoyancy/Buoyancy.cc | 11 +- test/integration/buoyancy.cc | 226 ++++++++++++++++++ test/worlds/center_of_volume.sdf | 160 +++++++++++++ 4 files changed, 392 insertions(+), 7 deletions(-) create mode 100644 test/worlds/center_of_volume.sdf diff --git a/include/ignition/gazebo/components/CenterOfVolume.hh b/include/ignition/gazebo/components/CenterOfVolume.hh index 4bd2389101..2096d2d5d0 100644 --- a/include/ignition/gazebo/components/CenterOfVolume.hh +++ b/include/ignition/gazebo/components/CenterOfVolume.hh @@ -33,7 +33,7 @@ namespace components /// \brief A component for an entity's center of volume. Units are in meters. /// The Vector3 value indicates center of volume of an entity. The /// position of the center of volume is relative to the pose of the parent - /// entity. + /// entity, which is usually a link. using CenterOfVolume = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CenterOfVolume", CenterOfVolume) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 81a880a21a..0e7a286583 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -151,7 +151,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, _entity, components::Collision()); double volumeSum = 0; - ignition::math::Vector3d weightedPosSum = + ignition::math::Vector3d weightedPosInLinkSum = ignition::math::Vector3d::Zero; // Compute the volume of the link by iterating over all the collision @@ -210,16 +210,15 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } volumeSum += volume; - math::Pose3d pose = worldPose(collision, _ecm); - weightedPosSum += volume * pose.Pos(); + auto poseInLink = _ecm.Component(collision)->Data(); + weightedPosInLinkSum += volume * poseInLink.Pos(); } if (volumeSum > 0) { - // Store the center of volume - math::Pose3d linkWorldPose = worldPose(_entity, _ecm); + // Store the center of volume expressed in the link frame _ecm.CreateComponent(_entity, components::CenterOfVolume( - weightedPosSum / volumeSum - linkWorldPose.Pos())); + weightedPosInLinkSum / volumeSum)); // Store the volume _ecm.CreateComponent(_entity, components::Volume(volumeSum)); diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 77445fdd61..030b84a11f 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -20,8 +20,10 @@ #include #include +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" #include "ignition/gazebo/components/CenterOfVolume.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" @@ -194,3 +196,227 @@ TEST_F(BuoyancyTest, Movement) server.Run(true, iterations, false); EXPECT_TRUE(finished); } + +///////////////////////////////////////////////// +TEST_F(BuoyancyTest, CenterOfVolume) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_BINARY_PATH), + "test", "worlds", "buoyancy.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::size_t iterations = 1000; + + bool finished = false; + test::Relay testSystem; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, + const gazebo::EntityComponentManager &_ecm) + { + // Check pose + Entity submarine = _ecm.EntityByComponents( + components::Model(), components::Name("submarine")); + + Entity submarineSinking = _ecm.EntityByComponents( + components::Model(), components::Name("submarine_sinking")); + + Entity submarineBuoyant = _ecm.EntityByComponents( + components::Model(), components::Name("submarine_buoyant")); + + Entity duck = _ecm.EntityByComponents( + components::Model(), components::Name("duck")); + + ASSERT_NE(submarine, kNullEntity); + ASSERT_NE(submarineSinking, kNullEntity); + ASSERT_NE(submarineBuoyant, kNullEntity); + ASSERT_NE(duck, kNullEntity); + + // Get the submarine link + auto submarineLink = _ecm.EntityByComponents( + components::ParentEntity(submarine), + components::Name("body"), + components::Link()); + + // Check the submarine buoyant volume and center of volume + auto submarineVolume = _ecm.Component( + submarineLink); + ASSERT_NE(submarineVolume , nullptr); + EXPECT_NEAR(0.25132741228718347, submarineVolume->Data(), 1e-3); + + auto submarineCenterOfVolume = + _ecm.Component(submarineLink); + ASSERT_NE(submarineCenterOfVolume, nullptr); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), + submarineCenterOfVolume->Data()); + + // Get the submarine buoyant link + auto submarineBuoyantLink = _ecm.EntityByComponents( + components::ParentEntity(submarineBuoyant), + components::Name("body"), + components::Link()); + + // Check the submarine buoyant volume and center of volume + auto submarineBuoyantVolume = _ecm.Component( + submarineBuoyantLink); + ASSERT_NE(submarineBuoyantVolume , nullptr); + EXPECT_NEAR(0.735133, submarineBuoyantVolume->Data(), 1e-3); + + auto submarineBuoyantCenterOfVolume = + _ecm.Component(submarineBuoyantLink); + ASSERT_NE(submarineBuoyantCenterOfVolume, nullptr); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), + submarineBuoyantCenterOfVolume->Data()); + + // Get the submarine sinking link + auto submarineSinkingLink = _ecm.EntityByComponents( + components::ParentEntity(submarineSinking), + components::Name("body"), + components::Link()); + + // Check the submarine sinking volume and center of volume + auto submarineSinkingVolume = _ecm.Component( + submarineSinkingLink); + ASSERT_NE(submarineSinkingVolume , nullptr); + EXPECT_NEAR(0.735133, submarineSinkingVolume->Data(), 1e-3); + + auto submarineSinkingCenterOfVolume = + _ecm.Component(submarineSinkingLink); + ASSERT_NE(submarineSinkingCenterOfVolume, nullptr); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), + submarineSinkingCenterOfVolume->Data()); + + // Get the duck link + auto duckLink = _ecm.EntityByComponents( + components::ParentEntity(duck), + components::Name("link"), + components::Link()); + + // Check the duck volume and center of volume + auto duckVolume = _ecm.Component(duckLink); + ASSERT_NE(duckVolume, nullptr); + EXPECT_NEAR(1.40186, duckVolume->Data(), 1e-3); + auto duckCenterOfVolume = + _ecm.Component(duckLink); + ASSERT_NE(duckCenterOfVolume, nullptr); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -0.4), + duckCenterOfVolume->Data()); + + auto submarinePose = _ecm.Component(submarine); + ASSERT_NE(submarinePose , nullptr); + + auto submarineSinkingPose = _ecm.Component( + submarineSinking); + ASSERT_NE(submarineSinkingPose , nullptr); + + auto submarineBuoyantPose = _ecm.Component( + submarineBuoyant); + ASSERT_NE(submarineSinkingPose , nullptr); + + auto duckPose = _ecm.Component(duck); + ASSERT_NE(duckPose , nullptr); + + // The "submarine" should stay in its starting location of 0, 0, 1.5 meters. + EXPECT_NEAR(0, submarinePose->Data().Pos().X(), 1e-2); + EXPECT_NEAR(0, submarinePose->Data().Pos().Y(), 1e-2); + EXPECT_NEAR(0, submarinePose->Data().Pos().Z(), 1e-2); + + if (_info.iterations > 10) + { + EXPECT_LT(submarineSinkingPose->Data().Pos().Z(), + submarinePose->Data().Pos().Z()); + EXPECT_GT(submarineBuoyantPose->Data().Pos().Z(), + submarinePose->Data().Pos().Z()); + EXPECT_GT(duckPose->Data().Pos().Z(), + submarinePose->Data().Pos().Z()); + } + + if (_info.iterations == iterations) + { + EXPECT_NEAR(-1.63, submarineSinkingPose->Data().Pos().Z(), 1e-2); + EXPECT_NEAR(4.90, submarineBuoyantPose->Data().Pos().Z(), 1e-2); + EXPECT_NEAR(171.4, duckPose->Data().Pos().Z(), 1e-2); + finished = true; + } + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, iterations, false); + EXPECT_TRUE(finished); +} + +///////////////////////////////////////////////// +TEST_F(BuoyancyTest, OffsetAndRotation) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "center_of_volume.sdf")); + + std::size_t iterations{0}; + fixture.OnPostUpdate([&]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get links + auto noOffsets = entitiesFromScopedName("no_offset::link", _ecm); + ASSERT_EQ(1u, noOffsets.size()); + auto noOffset = *noOffsets.begin(); + EXPECT_NE(kNullEntity, noOffset); + + auto noOffsetRotateds = entitiesFromScopedName("no_offset_rotated::link", _ecm); + ASSERT_EQ(1u, noOffsetRotateds.size()); + auto noOffsetRotated = *noOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, noOffsetRotated); + + auto withOffsets = entitiesFromScopedName("com_cov_offset::link", _ecm); + ASSERT_EQ(1u, withOffsets.size()); + auto withOffset = *withOffsets.begin(); + EXPECT_NE(kNullEntity, withOffset); + + auto withOffsetRotateds = entitiesFromScopedName("com_cov_offset_rotated::link", _ecm); + ASSERT_EQ(1u, withOffsetRotateds.size()); + auto withOffsetRotated = *withOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, withOffsetRotated); + + // Check CoVs have correct offsets + auto noOffsetCoV = _ecm.Component(noOffset); + ASSERT_NE(noOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetCoV->Data()); + + auto noOffsetRotatedCoV = _ecm.Component(noOffsetRotated); + ASSERT_NE(noOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetRotatedCoV->Data()); + + auto withOffsetCoV = _ecm.Component(withOffset); + ASSERT_NE(withOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetCoV->Data()); + + auto withOffsetRotatedCoV = _ecm.Component(withOffsetRotated); + ASSERT_NE(withOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetRotatedCoV->Data()); + + // Check that all objects are neutrally buoyant and stay still + auto noOffsetPose = worldPose(noOffset, _ecm); + EXPECT_EQ(math::Pose3d(), noOffsetPose); + + auto noOffsetRotatedPose = worldPose(noOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 0, 0, 0.1, 0.2, 0.3), noOffsetRotatedPose); + + auto withOffsetPose = worldPose(withOffset, _ecm); + EXPECT_EQ(math::Pose3d(0, 3, 0, 0, 0, 0), withOffsetPose); + + auto withOffsetRotatedPose = worldPose(withOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 3, 0, 0.1, 0.2, 0.3), withOffsetRotatedPose); + + iterations++; + }).Finalize(); + + std::size_t targetIterations{1000}; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations, iterations); +} diff --git a/test/worlds/center_of_volume.sdf b/test/worlds/center_of_volume.sdf new file mode 100644 index 0000000000..0972ca554d --- /dev/null +++ b/test/worlds/center_of_volume.sdf @@ -0,0 +1,160 @@ + + + + + + 0 + + + + + 1000 + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + -3 0 0 0.1 0.2 0.3 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + 0 0 0 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + 0 3 0 0 0 0 + + 0 0 0 0 0 0 + + 1 1 1 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + -3 3 0 0.1 0.2 0.3 + + 0 0 0 0 0 0 + + 1 1 1 0 0 0 + + 166.66 + 0 + 0 + 166.66 + 0 + 166.66 + + 1000.0 + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + 1 1 1 0 0 0 + + + 1.0 1.0 1.0 + + + + + + + + + From 97cc90285e0f059dbb0a2a2160347ab1bcf4ec90 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 20 Jan 2022 14:37:48 -0800 Subject: [PATCH 2/3] codecheck Signed-off-by: Louise Poubel --- test/integration/buoyancy.cc | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 030b84a11f..01ac694f11 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -368,7 +368,8 @@ TEST_F(BuoyancyTest, OffsetAndRotation) auto noOffset = *noOffsets.begin(); EXPECT_NE(kNullEntity, noOffset); - auto noOffsetRotateds = entitiesFromScopedName("no_offset_rotated::link", _ecm); + auto noOffsetRotateds = entitiesFromScopedName("no_offset_rotated::link", + _ecm); ASSERT_EQ(1u, noOffsetRotateds.size()); auto noOffsetRotated = *noOffsetRotateds.begin(); EXPECT_NE(kNullEntity, noOffsetRotated); @@ -378,7 +379,8 @@ TEST_F(BuoyancyTest, OffsetAndRotation) auto withOffset = *withOffsets.begin(); EXPECT_NE(kNullEntity, withOffset); - auto withOffsetRotateds = entitiesFromScopedName("com_cov_offset_rotated::link", _ecm); + auto withOffsetRotateds = entitiesFromScopedName( + "com_cov_offset_rotated::link", _ecm); ASSERT_EQ(1u, withOffsetRotateds.size()); auto withOffsetRotated = *withOffsetRotateds.begin(); EXPECT_NE(kNullEntity, withOffsetRotated); @@ -388,7 +390,8 @@ TEST_F(BuoyancyTest, OffsetAndRotation) ASSERT_NE(noOffsetCoV, nullptr); EXPECT_EQ(math::Vector3d::Zero, noOffsetCoV->Data()); - auto noOffsetRotatedCoV = _ecm.Component(noOffsetRotated); + auto noOffsetRotatedCoV = _ecm.Component( + noOffsetRotated); ASSERT_NE(noOffsetRotatedCoV, nullptr); EXPECT_EQ(math::Vector3d::Zero, noOffsetRotatedCoV->Data()); @@ -396,7 +399,8 @@ TEST_F(BuoyancyTest, OffsetAndRotation) ASSERT_NE(withOffsetCoV, nullptr); EXPECT_EQ(math::Vector3d::One, withOffsetCoV->Data()); - auto withOffsetRotatedCoV = _ecm.Component(withOffsetRotated); + auto withOffsetRotatedCoV = _ecm.Component( + withOffsetRotated); ASSERT_NE(withOffsetRotatedCoV, nullptr); EXPECT_EQ(math::Vector3d::One, withOffsetRotatedCoV->Data()); From 134056aac7dc95b4820b69746102afc4250ce922 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 20 Jan 2022 14:56:23 -0800 Subject: [PATCH 3/3] remove repeated test Signed-off-by: Louise Poubel --- test/integration/buoyancy.cc | 154 ----------------------------------- 1 file changed, 154 deletions(-) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 01ac694f11..7acfee7a5e 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -197,160 +197,6 @@ TEST_F(BuoyancyTest, Movement) EXPECT_TRUE(finished); } -///////////////////////////////////////////////// -TEST_F(BuoyancyTest, CenterOfVolume) -{ - // Start server - ServerConfig serverConfig; - const auto sdfFile = common::joinPaths(std::string(PROJECT_BINARY_PATH), - "test", "worlds", "buoyancy.sdf"); - serverConfig.SetSdfFile(sdfFile); - - Server server(serverConfig); - EXPECT_FALSE(server.Running()); - EXPECT_FALSE(*server.Running(0)); - - using namespace std::chrono_literals; - server.SetUpdatePeriod(1ns); - - std::size_t iterations = 1000; - - bool finished = false; - test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, - const gazebo::EntityComponentManager &_ecm) - { - // Check pose - Entity submarine = _ecm.EntityByComponents( - components::Model(), components::Name("submarine")); - - Entity submarineSinking = _ecm.EntityByComponents( - components::Model(), components::Name("submarine_sinking")); - - Entity submarineBuoyant = _ecm.EntityByComponents( - components::Model(), components::Name("submarine_buoyant")); - - Entity duck = _ecm.EntityByComponents( - components::Model(), components::Name("duck")); - - ASSERT_NE(submarine, kNullEntity); - ASSERT_NE(submarineSinking, kNullEntity); - ASSERT_NE(submarineBuoyant, kNullEntity); - ASSERT_NE(duck, kNullEntity); - - // Get the submarine link - auto submarineLink = _ecm.EntityByComponents( - components::ParentEntity(submarine), - components::Name("body"), - components::Link()); - - // Check the submarine buoyant volume and center of volume - auto submarineVolume = _ecm.Component( - submarineLink); - ASSERT_NE(submarineVolume , nullptr); - EXPECT_NEAR(0.25132741228718347, submarineVolume->Data(), 1e-3); - - auto submarineCenterOfVolume = - _ecm.Component(submarineLink); - ASSERT_NE(submarineCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), - submarineCenterOfVolume->Data()); - - // Get the submarine buoyant link - auto submarineBuoyantLink = _ecm.EntityByComponents( - components::ParentEntity(submarineBuoyant), - components::Name("body"), - components::Link()); - - // Check the submarine buoyant volume and center of volume - auto submarineBuoyantVolume = _ecm.Component( - submarineBuoyantLink); - ASSERT_NE(submarineBuoyantVolume , nullptr); - EXPECT_NEAR(0.735133, submarineBuoyantVolume->Data(), 1e-3); - - auto submarineBuoyantCenterOfVolume = - _ecm.Component(submarineBuoyantLink); - ASSERT_NE(submarineBuoyantCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), - submarineBuoyantCenterOfVolume->Data()); - - // Get the submarine sinking link - auto submarineSinkingLink = _ecm.EntityByComponents( - components::ParentEntity(submarineSinking), - components::Name("body"), - components::Link()); - - // Check the submarine sinking volume and center of volume - auto submarineSinkingVolume = _ecm.Component( - submarineSinkingLink); - ASSERT_NE(submarineSinkingVolume , nullptr); - EXPECT_NEAR(0.735133, submarineSinkingVolume->Data(), 1e-3); - - auto submarineSinkingCenterOfVolume = - _ecm.Component(submarineSinkingLink); - ASSERT_NE(submarineSinkingCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), - submarineSinkingCenterOfVolume->Data()); - - // Get the duck link - auto duckLink = _ecm.EntityByComponents( - components::ParentEntity(duck), - components::Name("link"), - components::Link()); - - // Check the duck volume and center of volume - auto duckVolume = _ecm.Component(duckLink); - ASSERT_NE(duckVolume, nullptr); - EXPECT_NEAR(1.40186, duckVolume->Data(), 1e-3); - auto duckCenterOfVolume = - _ecm.Component(duckLink); - ASSERT_NE(duckCenterOfVolume, nullptr); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -0.4), - duckCenterOfVolume->Data()); - - auto submarinePose = _ecm.Component(submarine); - ASSERT_NE(submarinePose , nullptr); - - auto submarineSinkingPose = _ecm.Component( - submarineSinking); - ASSERT_NE(submarineSinkingPose , nullptr); - - auto submarineBuoyantPose = _ecm.Component( - submarineBuoyant); - ASSERT_NE(submarineSinkingPose , nullptr); - - auto duckPose = _ecm.Component(duck); - ASSERT_NE(duckPose , nullptr); - - // The "submarine" should stay in its starting location of 0, 0, 1.5 meters. - EXPECT_NEAR(0, submarinePose->Data().Pos().X(), 1e-2); - EXPECT_NEAR(0, submarinePose->Data().Pos().Y(), 1e-2); - EXPECT_NEAR(0, submarinePose->Data().Pos().Z(), 1e-2); - - if (_info.iterations > 10) - { - EXPECT_LT(submarineSinkingPose->Data().Pos().Z(), - submarinePose->Data().Pos().Z()); - EXPECT_GT(submarineBuoyantPose->Data().Pos().Z(), - submarinePose->Data().Pos().Z()); - EXPECT_GT(duckPose->Data().Pos().Z(), - submarinePose->Data().Pos().Z()); - } - - if (_info.iterations == iterations) - { - EXPECT_NEAR(-1.63, submarineSinkingPose->Data().Pos().Z(), 1e-2); - EXPECT_NEAR(4.90, submarineBuoyantPose->Data().Pos().Z(), 1e-2); - EXPECT_NEAR(171.4, duckPose->Data().Pos().Z(), 1e-2); - finished = true; - } - }); - - server.AddSystem(testSystem.systemPtr); - server.Run(true, iterations, false); - EXPECT_TRUE(finished); -} - ///////////////////////////////////////////////// TEST_F(BuoyancyTest, OffsetAndRotation) {