Skip to content

Commit

Permalink
Merge 134056a into 650b746
Browse files Browse the repository at this point in the history
  • Loading branch information
@chapulina
chapulina authored Jan 20, 2022
2 parents 650b746 + 134056a commit 0bb551d
Show file tree
Hide file tree
Showing 4 changed files with 242 additions and 7 deletions.
2 changes: 1 addition & 1 deletion include/ignition/gazebo/components/CenterOfVolume.hh
View file
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
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
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
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>

0 comments on commit 0bb551d

Please sign in to comment.