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.

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

Fix graded buoyancy problems #1297

Merged
merged 15 commits into from
Jan 20, 2022
15 changes: 2 additions & 13 deletions src/systems/buoyancy/Buoyancy.cc
Original file line number Diff line number Diff line change
@@ -221,7 +221,7 @@ std::pair<math::Vector3d, math::Vector3d> BuoyancyPrivate::ResolveForces(
force += b.force;
math::Pose3d localPoint{b.point, math::Quaterniond{1, 0, 0, 0}};
auto globalPoint = b.pose * localPoint;
auto offset = globalPoint.Pos() - _pose.Pos();
auto offset = _pose.Pos() - globalPoint.Pos();
torque += force.Cross(offset);
}

@@ -439,9 +439,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
const components::Volume *_volume,
const components::CenterOfVolume *_centerOfVolume) -> bool
{
auto newPose = enableComponent<components::Inertial>(_ecm, _entity);
newPose |= enableComponent<components::WorldPose>(_ecm, _entity);

// World pose of the link.
math::Pose3d linkWorldPose = worldPose(_entity, _ecm);

@@ -472,13 +469,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
else if (this->dataPtr->buoyancyType
== BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY)
{
if (newPose)
{
// Skip entity if WorldPose and inertial are not yet ready
// TODO(arjo): Find a way of disabling gravity effects for
// this first iteration.
return true;
}
std::vector<Entity> collisions = _ecm.ChildrenByComponents(
_entity, components::Collision());
this->dataPtr->buoyancyForces.clear();
@@ -523,8 +513,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
}
}
}
auto [force, torque] = this->dataPtr->ResolveForces(
link.WorldInertialPose(_ecm).value());
auto [force, torque] = this->dataPtr->ResolveForces(linkWorldPose);
// Apply the wrench to the link. This wrench is applied in the
// Physics System.
link.AddWorldWrench(_ecm, force, torque);
115 changes: 115 additions & 0 deletions test/integration/buoyancy.cc
Original file line number Diff line number Diff line change
@@ -42,6 +42,121 @@ class BuoyancyTest : public InternalFixture<::testing::Test>
{
};

/////////////////////////////////////////////////
TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RestoringMoments))
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
{
// This test checks if the restoring moments are correctly calculated accross
// both uniform and graded modes when the vehicle is fully submerged.
std::size_t iterations{10000};
std::vector<math::Pose3d> poses_uniform, poses_graded;
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
{
// Start server
ServerConfig serverConfig;
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"/test/worlds/buoyancy_uniform_restoring_moments.sdf");
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
serverConfig.SetSdfFile(sdfFile);

test::Relay testSystem;
testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*unused*/,
const gazebo::EntityComponentManager &_ecm)
{
// Get Submarine
Entity submarine = _ecm.EntityByComponents(
components::Model(), components::Name("submarine"));

// Get pose
auto submarinePose = _ecm.Component<components::Pose>(submarine);
ASSERT_NE(submarinePose, nullptr);

poses_uniform.push_back(submarinePose->Data());
});

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

server.AddSystem(testSystem.systemPtr);
server.Run(true, iterations, false);
}

{
// Start server
ServerConfig serverConfig;
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"/test/worlds/buoyancy_graded_restoring_moments.sdf");
serverConfig.SetSdfFile(sdfFile);

test::Relay testSystem;
testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*unused*/,
const gazebo::EntityComponentManager &_ecm)
{
// Get Submarine
Entity submarine = _ecm.EntityByComponents(
components::Model(), components::Name("submarine"));

// Get pose
auto submarinePose = _ecm.Component<components::Pose>(submarine);
ASSERT_NE(submarinePose, nullptr);

poses_graded.push_back(submarinePose->Data());
});

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

server.AddSystem(testSystem.systemPtr);
server.Run(true, iterations, false);
}

double max_uniform_roll{0}, max_graded_roll{0};
double min_uniform_roll{0}, min_graded_roll{0};

ASSERT_EQ(poses_graded.size(), poses_uniform.size());
ASSERT_EQ(poses_uniform.size(), iterations);
for (std::size_t i = 0; i < poses_graded.size(); i++)
{
// The two modes should track a similar course when fully submerged
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
// Ignore roll for now as that changes a lot and the two sims are not
// perfectly in sync.
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
EXPECT_NEAR(poses_graded[i].Rot().Euler().Y(),
poses_uniform[i].Rot().Euler().Y(), 1e-1);
EXPECT_NEAR(poses_graded[i].Rot().Euler().Z(),
poses_uniform[i].Rot().Euler().Z(), 1e-1);

EXPECT_NEAR(poses_graded[i].Pos().X(), poses_uniform[i].Pos().X(), 1e-1);
EXPECT_NEAR(poses_graded[i].Pos().Y(), poses_uniform[i].Pos().Y(), 1e-1);
EXPECT_NEAR(poses_graded[i].Pos().Z(), poses_uniform[i].Pos().Z(), 1e-1);

// The box should stay around zero
EXPECT_NEAR(poses_graded[i].Pos().X(), 0, 1e-1);
EXPECT_NEAR(poses_graded[i].Pos().Y(), 0, 1e-1);
EXPECT_NEAR(poses_graded[i].Pos().Z(), 0, 1e-1);

// The box should not yaw or pitch
EXPECT_NEAR(poses_graded[i].Rot().Euler().Y(), 0, 1e-1);
EXPECT_NEAR(poses_graded[i].Rot().Euler().Z(), 0, 1e-1);

// The roll should not exceed its equilibrium position
EXPECT_LT(poses_graded[i].Rot().Euler().X(), 0.11 + 0.01);
EXPECT_LT(poses_uniform[i].Rot().Euler().X(), 0.11 + 0.01);

// Get the maximum roll
max_uniform_roll = std::max(max_uniform_roll, poses_uniform[i].Rot().X());
max_graded_roll = std::max(max_graded_roll, poses_graded[i].Rot().X());

// And the minimum roll
min_uniform_roll = std::min(min_uniform_roll, poses_uniform[i].Rot().X());
min_graded_roll = std::min(min_graded_roll, poses_graded[i].Rot().X());
}
// The max and min roll should be similar
EXPECT_NEAR(max_uniform_roll, max_graded_roll, 1e-1);
EXPECT_NEAR(max_uniform_roll, 0.11, 1e-1);
EXPECT_NEAR(min_uniform_roll, min_graded_roll, 1e-1);
// Emperically derived
EXPECT_NEAR(min_uniform_roll, -0.15, 1e-1);
}

/////////////////////////////////////////////////
// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175
TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(UniformWorldMovement))
79 changes: 79 additions & 0 deletions test/worlds/buoyancy_graded_restoring_moments.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
<sdf version="1.6">
<world name="buoyancy">

<physics name="1ms" type="ode">
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
<max_step_size>0.001</max_step_size>
<real_time_factor>0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-user-commands-system"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
arjo129 marked this conversation as resolved.
Show resolved Hide resolved
<plugin
filename="ignition-gazebo-buoyancy-system"
name="ignition::gazebo::systems::Buoyancy">
<graded_buoyancy>
<default_density>1000</default_density>
<density_change>
<above_depth>100</above_depth>
<density>1</density>
</density_change>
</graded_buoyancy>
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name='submarine'>
<pose>0 0 0 0.1 0 0</pose>
<link name='body'>

<inertial>
<mass>1000</mass>
<inertia>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>86.28907821859966</iyy>
<iyz>0</iyz>
<izz>5.026548245743671</izz>
</inertia>
</inertial>

<collision name='body_collision'>
<pose>0 0 0.08 0 0 0</pose>
<geometry>
<box>1 1 1</box>
</geometry>
</collision>

<visual name='body_visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>
</model>
</world>
</sdf>
73 changes: 73 additions & 0 deletions test/worlds/buoyancy_uniform_restoring_moments.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
<sdf version="1.6">
<world name="buoyancy">

<physics name="1ms" type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-user-commands-system"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<plugin
filename="ignition-gazebo-buoyancy-system"
name="ignition::gazebo::systems::Buoyancy">
<uniform_fluid_density>1000</uniform_fluid_density>
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name='submarine'>
<pose>0 0 0 0.1 0 0</pose>
<link name='body'>

<inertial>
<mass>1000</mass>
<inertia>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>86.28907821859966</iyy>
<iyz>0</iyz>
<izz>5.026548245743671</izz>
</inertia>
</inertial>

<collision name='body_collision'>
<pose>0 0 0.08 0 0 0</pose>
<geometry>
<box>1 1 1</box>
</geometry>
</collision>

<visual name='body_visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>
</model>
</world>
</sdf>