Skip to content

Commit 9c1c1e1

Browse files
caguerocaguero
committed
Add cylinder, capsule and ellipsoid support to graded buoyancy (#3423)
Backport of #3423. Cone support is omitted because ign-gazebo6 depends on sdformat12 and ign-math6, which do not provide cone geometry. (cherry picked from commit 9c925b6) Signed-off-by: Carlos Agüero <caguero@honurobotics.com>
1 parent cf69b2e commit 9c1c1e1

4 files changed

Lines changed: 376 additions & 4 deletions

File tree

‎src/systems/buoyancy/Buoyancy.cc‎

Lines changed: 28 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -78,8 +78,7 @@ class ignition::gazebo::systems::BuoyancyPrivate
7878

7979
/// \brief Get the resultant buoyant force on a shape.
8080
/// \param[in] _pose World pose of the shape's origin.
81-
/// \param[in] _shape The collision mesh of a shape. Currently must
82-
/// be box or sphere.
81+
/// \param[in] _shape The collision shape.
8382
/// \param[in] _gravity Gravity acceleration in the world frame.
8483
/// Updates this->buoyancyForces containing {force, center_of_volume} to be
8584
/// applied on the link.
@@ -419,6 +418,12 @@ void Buoyancy::PreUpdate(const UpdateInfo &_info,
419418
case sdf::GeometryType::CYLINDER:
420419
volume = coll->Data().Geom()->CylinderShape()->Shape().Volume();
421420
break;
421+
case sdf::GeometryType::CAPSULE:
422+
volume = coll->Data().Geom()->CapsuleShape()->Shape().Volume();
423+
break;
424+
case sdf::GeometryType::ELLIPSOID:
425+
volume = coll->Data().Geom()->EllipsoidShape()->Shape().Volume();
426+
break;
422427
case sdf::GeometryType::PLANE:
423428
// Ignore plane shapes. They have no volume and are not expected
424429
// to be buoyant.
@@ -540,13 +545,32 @@ void Buoyancy::PreUpdate(const UpdateInfo &_info,
540545
coll->Data().Geom()->SphereShape()->Shape(),
541546
gravity->Data());
542547
break;
548+
case sdf::GeometryType::CYLINDER:
549+
this->dataPtr->GradedFluidDensity<math::Cylinderd>(
550+
pose,
551+
coll->Data().Geom()->CylinderShape()->Shape(),
552+
gravity->Data());
553+
break;
554+
case sdf::GeometryType::CAPSULE:
555+
this->dataPtr->GradedFluidDensity<math::Capsuled>(
556+
pose,
557+
coll->Data().Geom()->CapsuleShape()->Shape(),
558+
gravity->Data());
559+
break;
560+
case sdf::GeometryType::ELLIPSOID:
561+
this->dataPtr->GradedFluidDensity<math::Ellipsoidd>(
562+
pose,
563+
coll->Data().Geom()->EllipsoidShape()->Shape(),
564+
gravity->Data());
565+
break;
543566
default:
544567
{
545568
static bool warned{false};
546569
if (!warned)
547570
{
548-
ignwarn << "Only <box> and <sphere> collisions are supported "
549-
<< "by the graded buoyancy option." << std::endl;
571+
ignwarn << "Unsupported collision geometry for graded buoyancy["
572+
<< static_cast<int>(coll->Data().Geom()->Type())
573+
<< "]" << std::endl;
550574
warned = true;
551575
}
552576
break;

‎test/integration/buoyancy.cc‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -623,3 +623,114 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(GradedDivideByZero))
623623
server.Run(true, iterations, false);
624624
EXPECT_TRUE(finished);
625625
}
626+
627+
/////////////////////////////////////////////////
628+
// Test that cylinder, capsule, and ellipsoid shapes work with graded
629+
// buoyancy. All shapes are neutrally buoyant and fully submerged, so they
630+
// should stay at their initial positions. Uses graded buoyancy which
631+
// exercises both CheckForNewEntities (volume) and GradedFluidDensity.
632+
TEST_F(BuoyancyTest,
633+
IGN_UTILS_TEST_DISABLED_ON_WIN32(GradedBuoyancyCylinderCapsuleEllipsoid))
634+
{
635+
ServerConfig serverConfig;
636+
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
637+
"test", "worlds", "buoyancy_shapes.sdf");
638+
serverConfig.SetSdfFile(sdfFile);
639+
640+
Server server(serverConfig);
641+
EXPECT_FALSE(server.Running());
642+
EXPECT_FALSE(*server.Running(0));
643+
644+
using namespace std::chrono_literals;
645+
server.SetUpdatePeriod(1ns);
646+
647+
std::size_t iterations = 1000;
648+
649+
bool finished = false;
650+
test::Relay testSystem;
651+
testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info,
652+
const sim::EntityComponentManager &_ecm)
653+
{
654+
Entity cylinder = _ecm.EntityByComponents(
655+
components::Model(), components::Name("cylinder_neutral"));
656+
Entity capsule = _ecm.EntityByComponents(
657+
components::Model(), components::Name("capsule_neutral"));
658+
Entity ellipsoid = _ecm.EntityByComponents(
659+
components::Model(), components::Name("ellipsoid_neutral"));
660+
661+
ASSERT_NE(cylinder, kNullEntity);
662+
ASSERT_NE(capsule, kNullEntity);
663+
ASSERT_NE(ellipsoid, kNullEntity);
664+
665+
// Check Volume components have correct values
666+
auto cylinderLink = _ecm.EntityByComponents(
667+
components::ParentEntity(cylinder),
668+
components::Name("body"),
669+
components::Link());
670+
auto capsuleLink = _ecm.EntityByComponents(
671+
components::ParentEntity(capsule),
672+
components::Name("body"),
673+
components::Link());
674+
auto ellipsoidLink = _ecm.EntityByComponents(
675+
components::ParentEntity(ellipsoid),
676+
components::Name("body"),
677+
components::Link());
678+
679+
if (cylinderLink != kNullEntity)
680+
{
681+
auto vol = _ecm.Component<components::Volume>(cylinderLink);
682+
if (vol)
683+
{
684+
// Cylinder: pi*r^2*l with r=0.2, l=2.0
685+
EXPECT_NEAR(0.2513, vol->Data(), 1e-2);
686+
}
687+
}
688+
689+
if (capsuleLink != kNullEntity)
690+
{
691+
auto vol = _ecm.Component<components::Volume>(capsuleLink);
692+
if (vol)
693+
{
694+
// Capsule: pi*r^2*l + (4/3)*pi*r^3 with r=0.5, l=1.0
695+
EXPECT_NEAR(1.309, vol->Data(), 1e-2);
696+
}
697+
}
698+
699+
if (ellipsoidLink != kNullEntity)
700+
{
701+
auto vol = _ecm.Component<components::Volume>(ellipsoidLink);
702+
if (vol)
703+
{
704+
// Ellipsoid: (4/3)*pi*0.5*0.4*0.3
705+
EXPECT_NEAR(0.2513, vol->Data(), 1e-2);
706+
}
707+
}
708+
709+
// All neutrally buoyant and fully submerged — should stay in place.
710+
auto cylinderPose = _ecm.Component<components::Pose>(cylinder);
711+
auto capsulePose = _ecm.Component<components::Pose>(capsule);
712+
auto ellipsoidPose = _ecm.Component<components::Pose>(ellipsoid);
713+
714+
ASSERT_NE(cylinderPose, nullptr);
715+
ASSERT_NE(capsulePose, nullptr);
716+
ASSERT_NE(ellipsoidPose, nullptr);
717+
718+
EXPECT_NEAR(0, cylinderPose->Data().Pos().X(), 1e-1);
719+
EXPECT_NEAR(0, cylinderPose->Data().Pos().Y(), 1e-1);
720+
EXPECT_NEAR(-3, cylinderPose->Data().Pos().Z(), 1e-1);
721+
722+
EXPECT_NEAR(3, capsulePose->Data().Pos().X(), 1e-1);
723+
EXPECT_NEAR(0, capsulePose->Data().Pos().Y(), 1e-1);
724+
EXPECT_NEAR(-3, capsulePose->Data().Pos().Z(), 1e-1);
725+
726+
EXPECT_NEAR(-3, ellipsoidPose->Data().Pos().X(), 1e-1);
727+
EXPECT_NEAR(0, ellipsoidPose->Data().Pos().Y(), 1e-1);
728+
EXPECT_NEAR(-3, ellipsoidPose->Data().Pos().Z(), 1e-1);
729+
730+
finished = _info.iterations == iterations;
731+
});
732+
733+
server.AddSystem(testSystem.systemPtr);
734+
server.Run(true, iterations, false);
735+
EXPECT_TRUE(finished);
736+
}

‎test/worlds/buoyancy_shapes.sdf‎

Lines changed: 138 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,138 @@
1+
<?xml version="1.0" ?>
2+
<!--
3+
Test world for graded buoyancy with capsule, ellipsoid, and cylinder.
4+
All objects are neutrally buoyant (density = default_density = 1000 kg/m^3)
5+
and fully submerged at z=-3, so they should stay at their initial positions.
6+
This exercises the GradedFluidDensity template for each new shape type.
7+
-->
8+
<sdf version="1.6">
9+
<world name="buoyancy">
10+
11+
<physics name="1ms" type="ode">
12+
<max_step_size>0.001</max_step_size>
13+
<real_time_factor>0</real_time_factor>
14+
</physics>
15+
<plugin
16+
filename="gz-sim-physics-system"
17+
name="gz::sim::systems::Physics">
18+
</plugin>
19+
<plugin
20+
filename="gz-sim-buoyancy-system"
21+
name="gz::sim::systems::Buoyancy">
22+
<graded_buoyancy>
23+
<default_density>1000</default_density>
24+
<density_change>
25+
<above_depth>0</above_depth>
26+
<density>1</density>
27+
</density_change>
28+
</graded_buoyancy>
29+
</plugin>
30+
31+
<!-- Cylinder: radius=0.2, length=2.0
32+
Volume = pi*r^2*l = pi*0.04*2 = 0.25133 m^3
33+
mass = 1000 * 0.25133 = 251.33 kg -->
34+
<model name='cylinder_neutral'>
35+
<pose>0 0 -3 0 0 0</pose>
36+
<link name='body'>
37+
<inertial>
38+
<mass>251.33</mass>
39+
<inertia>
40+
<ixx>86.29</ixx>
41+
<ixy>0</ixy>
42+
<ixz>0</ixz>
43+
<iyy>86.29</iyy>
44+
<iyz>0</iyz>
45+
<izz>5.03</izz>
46+
</inertia>
47+
</inertial>
48+
<collision name='body_collision'>
49+
<geometry>
50+
<cylinder>
51+
<radius>0.2</radius>
52+
<length>2.0</length>
53+
</cylinder>
54+
</geometry>
55+
</collision>
56+
<visual name='body_visual'>
57+
<geometry>
58+
<cylinder>
59+
<radius>0.2</radius>
60+
<length>2.0</length>
61+
</cylinder>
62+
</geometry>
63+
</visual>
64+
</link>
65+
</model>
66+
67+
<!-- Capsule: radius=0.5, length=1.0
68+
Volume = pi*r^2*l + (4/3)*pi*r^3 = 1.30900 m^3
69+
mass = 1000 * 1.30900 = 1309.00 kg -->
70+
<model name='capsule_neutral'>
71+
<pose>3 0 -3 0 0 0</pose>
72+
<link name='body'>
73+
<inertial>
74+
<mass>1309.00</mass>
75+
<inertia>
76+
<ixx>200</ixx>
77+
<ixy>0</ixy>
78+
<ixz>0</ixz>
79+
<iyy>200</iyy>
80+
<iyz>0</iyz>
81+
<izz>200</izz>
82+
</inertia>
83+
</inertial>
84+
<collision name='body_collision'>
85+
<geometry>
86+
<capsule>
87+
<radius>0.5</radius>
88+
<length>1.0</length>
89+
</capsule>
90+
</geometry>
91+
</collision>
92+
<visual name='body_visual'>
93+
<geometry>
94+
<capsule>
95+
<radius>0.5</radius>
96+
<length>1.0</length>
97+
</capsule>
98+
</geometry>
99+
</visual>
100+
</link>
101+
</model>
102+
103+
<!-- Ellipsoid: radii=(0.5, 0.4, 0.3)
104+
Volume = (4/3)*pi*0.5*0.4*0.3 = 0.25133 m^3
105+
mass = 1000 * 0.25133 = 251.33 kg -->
106+
<model name='ellipsoid_neutral'>
107+
<pose>-3 0 -3 0 0 0</pose>
108+
<link name='body'>
109+
<inertial>
110+
<mass>251.33</mass>
111+
<inertia>
112+
<ixx>50</ixx>
113+
<ixy>0</ixy>
114+
<ixz>0</ixz>
115+
<iyy>50</iyy>
116+
<iyz>0</iyz>
117+
<izz>50</izz>
118+
</inertia>
119+
</inertial>
120+
<collision name='body_collision'>
121+
<geometry>
122+
<ellipsoid>
123+
<radii>0.5 0.4 0.3</radii>
124+
</ellipsoid>
125+
</geometry>
126+
</collision>
127+
<visual name='body_visual'>
128+
<geometry>
129+
<ellipsoid>
130+
<radii>0.5 0.4 0.3</radii>
131+
</ellipsoid>
132+
</geometry>
133+
</visual>
134+
</link>
135+
</model>
136+
137+
</world>
138+
</sdf>

0 commit comments

Comments
 (0)