Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Buoyancy: fix center of volume's reference frame #1302

Merged
merged 3 commits into from
Jan 21, 2022
Merged

Buoyancy: fix center of volume's reference frame #1302

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1c18e3d
Fix center of volume's reference frame
chapulina Jan 20, 2022
97cc902
codecheck
chapulina Jan 20, 2022
134056a
remove repeated test
chapulina Jan 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/ignition/gazebo/components/CenterOfVolume.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<math::Vector3d, class CenterOfVolumeTag>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CenterOfVolume",
CenterOfVolume)
Expand Down
11 changes: 5 additions & 6 deletions src/systems/buoyancy/Buoyancy.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<components::Pose>(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));
Expand Down
76 changes: 76 additions & 0 deletions test/integration/buoyancy.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,10 @@
#include <ignition/common/Console.hh>
#include <ignition/common/Util.hh>

#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"
Expand Down Expand Up @@ -194,3 +196,77 @@ TEST_F(BuoyancyTest, Movement)
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<components::CenterOfVolume>(noOffset);
ASSERT_NE(noOffsetCoV, nullptr);
EXPECT_EQ(math::Vector3d::Zero, noOffsetCoV->Data());

auto noOffsetRotatedCoV = _ecm.Component<components::CenterOfVolume>(
noOffsetRotated);
ASSERT_NE(noOffsetRotatedCoV, nullptr);
EXPECT_EQ(math::Vector3d::Zero, noOffsetRotatedCoV->Data());

auto withOffsetCoV = _ecm.Component<components::CenterOfVolume>(withOffset);
ASSERT_NE(withOffsetCoV, nullptr);
EXPECT_EQ(math::Vector3d::One, withOffsetCoV->Data());

auto withOffsetRotatedCoV = _ecm.Component<components::CenterOfVolume>(
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);
}
160 changes: 160 additions & 0 deletions test/worlds/center_of_volume.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,160 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="center_of_volume">

<physics name="fast" type="ignored">
<real_time_factor>0</real_time_factor>
</physics>
<plugin
filename="libignition-gazebo-physics-system.so"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="libignition-gazebo-buoyancy-system.so"
name="ignition::gazebo::systems::Buoyancy">
<uniform_fluid_density>1000</uniform_fluid_density>
</plugin>

<model name='no_offset'>
<pose>0 0 0 0 0 0</pose>
<link name='link'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>166.66</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>166.66</iyy>
<iyz>0</iyz>
<izz>166.66</izz>
</inertia>
<mass>1000.0</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</visual>
<collision name='collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</collision>
</link>
</model>

<model name='no_offset_rotated'>
<pose>-3 0 0 0.1 0.2 0.3</pose>
<link name='link'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>166.66</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>166.66</iyy>
<iyz>0</iyz>
<izz>166.66</izz>
</inertia>
<mass>1000.0</mass>
</inertial>
<visual name='visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</visual>
<collision name='collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</collision>
</link>
</model>

<model name='com_cov_offset'>
<pose>0 3 0 0 0 0</pose>
<link name='link'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>1 1 1 0 0 0</pose>
<inertia>
<ixx>166.66</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>166.66</iyy>
<iyz>0</iyz>
<izz>166.66</izz>
</inertia>
<mass>1000.0</mass>
</inertial>
<visual name='visual'>
<pose>1 1 1 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</visual>
<collision name='collision'>
<pose>1 1 1 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</collision>
</link>
</model>

<model name='com_cov_offset_rotated'>
<pose>-3 3 0 0.1 0.2 0.3</pose>
<link name='link'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>1 1 1 0 0 0</pose>
<inertia>
<ixx>166.66</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>166.66</iyy>
<iyz>0</iyz>
<izz>166.66</izz>
</inertia>
<mass>1000.0</mass>
</inertial>
<visual name='visual'>
<pose>1 1 1 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</visual>
<collision name='collision'>
<pose>1 1 1 0 0 0</pose>
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</collision>
</link>
</model>

</world>
</sdf>