diff --git a/Changelog.md b/Changelog.md
index 1873ce0957..637767d02f 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -2,6 +2,10 @@
### Ignition Gazebo 3.x.x (20xx-xx-xx)
+1. Added buoyancy system plugin.
+ * [pull request 252](https://github.com/ignitionrobotics/ign-gazebo/pull/252)
+
+
1. Backport collision bitmask changes
* [pull request 223](https://github.com/ignitionrobotics/ign-gazebo/pull/223)
diff --git a/examples/worlds/buoyancy.sdf b/examples/worlds/buoyancy.sdf
new file mode 100644
index 0000000000..f9da7e090c
--- /dev/null
+++ b/examples/worlds/buoyancy.sdf
@@ -0,0 +1,644 @@
+
+
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+
+ 1000
+
+
+
+
+
+
+
+ 3D View
+ false
+ docked
+
+
+ ogre2
+ scene
+ 0.4 0.4 0.4
+ 0.8 0.8 0.8
+ -6 0 6 0 0.5 0
+
+
+
+
+
+ World control
+ false
+ false
+ 72
+ 121
+ 1
+
+ floating
+
+
+
+
+
+
+ true
+ true
+ true
+ /world/buoyancy/control
+ /world/buoyancy/stats
+
+
+
+
+
+
+ World stats
+ false
+ false
+ 110
+ 290
+ 1
+
+ floating
+
+
+
+
+
+
+ true
+ true
+ true
+ true
+ /world/buoyancy/stats
+
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ 0 0 0 1.5707963267948966 0 0
+
+ 0 0 0 0 0 0
+
+ 251.32741228718348
+
+ 86.28907821859966
+ 0
+ 0
+ 86.28907821859966
+ 0
+ 5.026548245743671
+
+
+
+
+
+
+ 0.2
+ 2
+
+
+
+
+
+
+ 0.2
+ 2
+
+
+
+
+
+ 0 0 1.05855 0 0 0
+
+
+ 0.0933402
+ 0.127211
+
+
+
+
+
+
+ 0 0 1.09 3.14159 0 0
+
+ 21.820000000000004
+
+ 0.45999415237916674
+ 0
+ 0
+ 0.45999415237916674
+ 0
+ 0.9091666666666668
+
+
+
+
+
+ 0.3433402 0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ 0.3433402 0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+ 0.0 0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ 0.0 0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+ -0.3433402 -0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ -0.3433402 -0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+ -0.0 -0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ -0.0 -0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+
+
+ body
+ propeller
+
+ 0 0 1
+ 1
+
+ -1e+12
+ 1e+12
+ -1
+ -1
+
+
+
+
+
+ propeller_joint
+
+
+
+ 1000
+ 1.2535816618911175
+ -1.4326647564469914
+ 0
+ 1.4326647564469914
+ 1.396
+ 0
+ 0.27637
+ 0 -0.7071067811865476 -0.7071067811865475
+ 0 -0.7071067811865475 0.7071067811865476
+ propeller
+ 0.35 0 0
+
+
+
+ 1000
+ 1.2535816618911175
+ -1.4326647564469914
+ 0
+ 1.4326647564469914
+ 1.396
+ 0
+ 0.27637
+ -0.7071067811865475 0 -0.7071067811865476
+ -0.7071067811865476 0 0.7071067811865475
+ propeller
+ 0 -0.35 0
+
+
+
+
+ 1000
+ 1.2535816618911175
+ -1.4326647564469914
+ 0
+ 1.4326647564469914
+ 1.396
+ 0
+ 0.27637
+ 0 0.7071067811865475 -0.7071067811865476
+ 0 -0.7071067811865476 -0.7071067811865475
+ propeller
+ -0.35 0 0
+
+
+
+
+ 1000
+ 1.2535816618911175
+ -1.4326647564469914
+ 0
+ 1.4326647564469914
+ 1.396
+ 0
+ 0.27637
+ 0.7071067811865476 0 -0.7071067811865475
+ 0.7071067811865475 0 0.7071067811865476
+ propeller
+ 0 0.35 0
+
+
+
+
+
+ 5 0 0 1.5707963267948966 0 0
+
+ 0 0 -1.07935 0 0 0
+
+ 1102.6990214100174
+
+ 645.9978433760353
+ 0
+ 0
+ 645.9978433760353
+ 0
+ 49.62145596345078
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+ 0 0 1.378546 0 0 0
+
+
+ 0.0933402
+ 0.127211
+
+
+
+
+
+
+ 0 0 0.3455 3.14159 0 0
+
+ 25.2312297
+
+ 0.5319073381913637
+ 0
+ 0
+ 0.5319073381913637
+ 0
+ 1.0513012375
+
+
+
+
+
+ 0.3433402 0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.3433402 0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ 0.0 0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.0 0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.3433402 -0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.3433402 -0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.0 -0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+
+
+ body
+ propeller
+
+ 0 0 1
+ 1
+
+ -1e+12
+ 1e+12
+ -1
+ -1
+
+
+
+
+
+
+
+ -5 0 0 1.5707963267948966 0 0
+
+ 0 0 -1.07935 0 0 0
+
+ 367.5663404700058
+
+ 215.33261445867842
+ 0
+ 0
+ 215.33261445867842
+ 0
+ 16.54048532115026
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+ 0 0 1.378546 0 0 0
+
+
+ 0.0933402
+ 0.127211
+
+
+
+
+
+
+ 0 0 0.3455 3.14159 0 0
+
+ 8.410409900000001
+
+ 0.17730244606378792
+ 0
+ 0
+ 0.17730244606378792
+ 0
+ 0.35043374583333337
+
+
+
+
+
+ 0.3433402 0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.3433402 0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ 0.0 0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.0 0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.3433402 -0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.3433402 -0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.0 -0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.0 -0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+
+
+ body
+ propeller
+
+ 0 0 1
+ 1
+
+ -1e+12
+ 1e+12
+ -1
+ -1
+
+
+
+
+
+
+
+
diff --git a/include/ignition/gazebo/components/CenterOfVolume.hh b/include/ignition/gazebo/components/CenterOfVolume.hh
new file mode 100644
index 0000000000..4bd2389101
--- /dev/null
+++ b/include/ignition/gazebo/components/CenterOfVolume.hh
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_CENTEROFVOLUME_HH_
+#define IGNITION_GAZEBO_COMPONENTS_CENTEROFVOLUME_HH_
+
+#include
+#include
+#include
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+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.
+ using CenterOfVolume = Component;
+ IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CenterOfVolume",
+ CenterOfVolume)
+}
+}
+}
+}
+
+#endif
diff --git a/include/ignition/gazebo/components/Volume.hh b/include/ignition/gazebo/components/Volume.hh
new file mode 100644
index 0000000000..75a0ee8135
--- /dev/null
+++ b/include/ignition/gazebo/components/Volume.hh
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_VOLUME_HH_
+#define IGNITION_GAZEBO_COMPONENTS_VOLUME_HH_
+
+#include
+#include
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+ /// \brief A volume component where the units are m^3.
+ /// Double value indicates volume of an entity.
+ using Volume = Component;
+ IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Volume", Volume)
+}
+}
+}
+}
+
+#endif
diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index 5f5619d2b2..ab085ca807 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -75,6 +75,7 @@ add_subdirectory(altimeter)
add_subdirectory(apply_joint_force)
add_subdirectory(battery_plugin)
add_subdirectory(breadcrumbs)
+add_subdirectory(buoyancy)
add_subdirectory(contact)
add_subdirectory(detachable_joint)
add_subdirectory(diff_drive)
diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc
new file mode 100644
index 0000000000..f3489f6f56
--- /dev/null
+++ b/src/systems/buoyancy/Buoyancy.cc
@@ -0,0 +1,273 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#include
+
+#include
+
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+
+#include
+
+#include
+
+#include "ignition/gazebo/components/CenterOfVolume.hh"
+#include "ignition/gazebo/components/Collision.hh"
+#include "ignition/gazebo/components/Gravity.hh"
+#include "ignition/gazebo/components/Inertial.hh"
+#include "ignition/gazebo/components/Link.hh"
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/Volume.hh"
+#include "ignition/gazebo/components/World.hh"
+#include "ignition/gazebo/Link.hh"
+#include "ignition/gazebo/Model.hh"
+#include "ignition/gazebo/Util.hh"
+
+#include "Buoyancy.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace systems;
+
+class ignition::gazebo::systems::BuoyancyPrivate
+{
+ /// \brief Get the fluid density based on a pose. This function can be
+ /// used to adjust the fluid density based on the pose of an object in the
+ /// world. This function currently returns a constant value, see the todo
+ /// in the function implementation.
+ /// \param[in] _pose The pose to use when computing the fluid density. The
+ /// pose frame is left undefined because this function currently returns
+ /// a constant value, see the todo in the function implementation.
+ /// \return The fluid density at the givein pose.
+ public: double FluidDensity(const math::Pose3d &_pose) const;
+
+ /// \brief Model interface
+ public: Entity world{kNullEntity};
+
+ /// \brief The density of the fluid in which the object is submerged in
+ /// kg/m^3. Defaults to 1000, the fluid density of water.
+ public: double fluidDensity{1000};
+};
+
+//////////////////////////////////////////////////
+double BuoyancyPrivate::FluidDensity(const math::Pose3d & /*_pose*/) const
+{
+ // \todo(nkoenig) Adjust the fluid density based on the provided pose.
+ // This could take into acount:
+ // 1. Transition from water to air. Currently this function is used for
+ // a whole link, but when transitioning between mediums a link may span
+ // both mediums. Surface tension could also be a factor.
+ // 2. Fluid density changes based on depth below the water surface and
+ // height above water surface.
+ return this->fluidDensity;
+}
+
+//////////////////////////////////////////////////
+Buoyancy::Buoyancy()
+ : dataPtr(std::make_unique())
+{
+}
+
+//////////////////////////////////////////////////
+void Buoyancy::Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &/*_eventMgr*/)
+{
+ // Store the world.
+ this->dataPtr->world = _entity;
+
+ // Get the gravity (defined in world frame)
+ const components::Gravity *gravity = _ecm.Component(
+ this->dataPtr->world);
+ if (!gravity)
+ {
+ ignerr << "Unable to get the gravity vector. Make sure this plugin is "
+ << "attached to a , not a ." << std::endl;
+ return;
+ }
+
+ if (_sdf->HasElement("uniform_fluid_density"))
+ {
+ this->dataPtr->fluidDensity = _sdf->Get("uniform_fluid_density");
+ }
+}
+
+//////////////////////////////////////////////////
+void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
+ ignition::gazebo::EntityComponentManager &_ecm)
+{
+ IGN_PROFILE("Buoyancy::PreUpdate");
+ const components::Gravity *gravity = _ecm.Component(
+ this->dataPtr->world);
+ if (!gravity)
+ {
+ ignerr << "Unable to get the gravity vector. Has gravity been defined?"
+ << std::endl;
+ return;
+ }
+
+ // Compute the volume and center of volume for each new link
+ _ecm.EachNew(
+ [&](const Entity &_entity,
+ const components::Link *,
+ const components::Inertial *) -> bool
+ {
+ // Skip if the entity already has a volume and center of volume
+ if (_ecm.EntityHasComponentType(_entity,
+ components::CenterOfVolume().TypeId()) &&
+ _ecm.EntityHasComponentType(_entity,
+ components::Volume().TypeId()))
+ {
+ return true;
+ }
+
+ Link link(_entity);
+
+ std::vector collisions = _ecm.ChildrenByComponents(
+ _entity, components::Collision());
+
+ double volumeSum = 0;
+ ignition::math::Vector3d weightedPosSum =
+ ignition::math::Vector3d::Zero;
+
+ // Compute the volume of the link by iterating over all the collision
+ // elements and storing each geometry's volume.
+ for (const Entity &collision : collisions)
+ {
+ double volume = 0;
+ const components::CollisionElement *coll =
+ _ecm.Component(collision);
+
+ if (!coll)
+ {
+ ignerr << "Invalid collision pointer. This shouldn't happen\n";
+ continue;
+ }
+
+ switch (coll->Data().Geom()->Type())
+ {
+ case sdf::GeometryType::BOX:
+ volume = coll->Data().Geom()->BoxShape()->Shape().Volume();
+ break;
+ case sdf::GeometryType::SPHERE:
+ volume = coll->Data().Geom()->SphereShape()->Shape().Volume();
+ break;
+ case sdf::GeometryType::CYLINDER:
+ volume = coll->Data().Geom()->CylinderShape()->Shape().Volume();
+ break;
+ case sdf::GeometryType::PLANE:
+ // Ignore plane shapes. They have no volume and are not expected
+ // to be buoyant.
+ break;
+ case sdf::GeometryType::MESH:
+ {
+ std::string file = asFullPath(
+ coll->Data().Geom()->MeshShape()->Uri(),
+ coll->Data().Geom()->MeshShape()->FilePath());
+ if (common::MeshManager::Instance()->IsValidFilename(file))
+ {
+ const common::Mesh *mesh =
+ common::MeshManager::Instance()->Load(file);
+ if (mesh)
+ volume = mesh->Volume();
+ else
+ ignerr << "Unable to load mesh[" << file << "]\n";
+ }
+ else
+ {
+ ignerr << "Invalid mesh filename[" << file << "]\n";
+ }
+ break;
+ }
+ default:
+ ignerr << "Unsupported collision geometry["
+ << static_cast(coll->Data().Geom()->Type()) << "]\n";
+ break;
+ }
+
+ volumeSum += volume;
+ math::Pose3d pose = worldPose(collision, _ecm);
+ weightedPosSum += volume * pose.Pos();
+ }
+
+ if (volumeSum > 0)
+ {
+ // Store the center of volume
+ math::Pose3d linkWorldPose = worldPose(_entity, _ecm);
+ _ecm.CreateComponent(_entity, components::CenterOfVolume(
+ weightedPosSum / volumeSum - linkWorldPose.Pos()));
+
+ // Store the volume
+ _ecm.CreateComponent(_entity, components::Volume(volumeSum));
+ }
+
+ return true;
+ });
+
+ // Only update if not paused.
+ if (_info.paused)
+ return;
+
+ _ecm.Each(
+ [&](const Entity &_entity,
+ const components::Link *,
+ const components::Volume *_volume,
+ const components::CenterOfVolume *_centerOfVolume) -> bool
+ {
+ // World pose of the link.
+ math::Pose3d linkWorldPose = worldPose(_entity, _ecm);
+
+ // By Archimedes' principle,
+ // buoyancy = -(mass*gravity)*fluid_density/object_density
+ // object_density = mass/volume, so the mass term cancels.
+ math::Vector3d buoyancy =
+ -this->dataPtr->FluidDensity(linkWorldPose) *
+ _volume->Data() * gravity->Data();
+
+ // Convert the center of volume to the world frame
+ math::Vector3d offsetWorld = linkWorldPose.Rot().RotateVector(
+ _centerOfVolume->Data());
+ // Compute the torque that should be applied due to buoyancy and
+ // the center of volume.
+ math::Vector3d torque = offsetWorld.Cross(buoyancy);
+
+ // Apply the wrench to the link. This wrench is applied in the
+ // Physics System.
+ Link link(_entity);
+ link.AddWorldWrench(_ecm, buoyancy, torque);
+
+ return true;
+ });
+}
+
+IGNITION_ADD_PLUGIN(Buoyancy,
+ ignition::gazebo::System,
+ Buoyancy::ISystemConfigure,
+ Buoyancy::ISystemPreUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(Buoyancy,
+ "ignition::gazebo::systems::Buoyancy")
diff --git a/src/systems/buoyancy/Buoyancy.hh b/src/systems/buoyancy/Buoyancy.hh
new file mode 100644
index 0000000000..3a9c42418e
--- /dev/null
+++ b/src/systems/buoyancy/Buoyancy.hh
@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef IGNITION_GAZEBO_SYSTEMS_BUOYANCY_HH_
+#define IGNITION_GAZEBO_SYSTEMS_BUOYANCY_HH_
+
+#include
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems
+{
+ // Forward declaration
+ class BuoyancyPrivate;
+
+ /// \brief A system that simulates buoyancy of objects immersed in fluid.
+ /// All SDF parameters are optional. This system must be attached to the
+ /// world and this system will apply buoyancy to all links that have inertia
+ /// and collision shapes.
+ ///
+ /// The volume and center of volume will be computed for each link, and
+ /// stored as components. During each iteration, Archimedes' principle is
+ /// applied to each link with a volume and center of volume component.
+ ///
+ /// Plane shapes are not handled by this plugin, and will not be affected
+ /// by buoyancy.
+ ///
+ /// ## System Parameters
+ ///
+ /// * sets the density of the fluid that surrounds
+ /// the buoyant object.
+ ///
+ /// ## Example
+ ///
+ /// The `buoyancy.sdf` SDF file contains three submarines. The first
+ /// submarine is neutrally buoyant, the second sinks, and the third
+ /// floats. To run:
+ ///
+ /// ```
+ /// ign gazebo -v 4 buoyancy.sdf
+ /// ```
+ class IGNITION_GAZEBO_VISIBLE Buoyancy
+ : public System,
+ public ISystemConfigure,
+ public ISystemPreUpdate
+ {
+ /// \brief Constructor
+ public: Buoyancy();
+
+ /// \brief Destructor
+ public: ~Buoyancy() override = default;
+
+ // Documentation inherited
+ public: void Configure(const Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &_eventMgr) override;
+
+ // Documentation inherited
+ public: void PreUpdate(
+ const ignition::gazebo::UpdateInfo &_info,
+ ignition::gazebo::EntityComponentManager &_ecm) override;
+
+ /// \brief Private data pointer
+ private: std::unique_ptr dataPtr;
+ };
+ }
+}
+}
+}
+
+#endif
diff --git a/src/systems/buoyancy/CMakeLists.txt b/src/systems/buoyancy/CMakeLists.txt
new file mode 100644
index 0000000000..882344ac1b
--- /dev/null
+++ b/src/systems/buoyancy/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_add_system(buoyancy
+ SOURCES
+ Buoyancy.cc
+ PUBLIC_LINK_LIBS
+ ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 1997467bd4..c506039bc6 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -129,3 +129,4 @@ add_subdirectory(integration)
add_subdirectory(performance)
add_subdirectory(plugins)
add_subdirectory(regression)
+add_subdirectory(worlds)
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 9f306f8c41..7521ea7770 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -6,6 +6,7 @@ set(tests
apply_joint_force_system.cc
battery_plugin.cc
breadcrumbs.cc
+ buoyancy.cc
components.cc
contact_system.cc
detachable_joint.cc
diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc
new file mode 100644
index 0000000000..034346b007
--- /dev/null
+++ b/test/integration/buoyancy.cc
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include
+
+#include
+
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/components/CenterOfVolume.hh"
+#include "ignition/gazebo/components/Link.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/ParentEntity.hh"
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Volume.hh"
+
+#include "ignition/gazebo/test_config.hh"
+#include "../helpers/Relay.hh"
+
+using namespace ignition;
+using namespace gazebo;
+
+class BuoyancyTest : public ::testing::Test
+{
+ // Documentation inherited
+ protected: void SetUp() override
+ {
+ ignition::common::Console::SetVerbosity(4);
+ setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH",
+ (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1);
+ }
+};
+
+/////////////////////////////////////////////////
+TEST_F(BuoyancyTest, Movement)
+{
+ // Start server
+ ServerConfig serverConfig;
+ const auto sdfFile = 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);
+}
diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt
new file mode 100644
index 0000000000..ea81f11e65
--- /dev/null
+++ b/test/worlds/CMakeLists.txt
@@ -0,0 +1 @@
+configure_file (buoyancy.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/buoyancy.sdf)
diff --git a/test/worlds/buoyancy.sdf.in b/test/worlds/buoyancy.sdf.in
new file mode 100644
index 0000000000..cdb2651c84
--- /dev/null
+++ b/test/worlds/buoyancy.sdf.in
@@ -0,0 +1,590 @@
+
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+
+
+ 1000
+
+
+
+
+
+
+
+ 3D View
+ false
+ docked
+
+
+ ogre2
+ scene
+ 0.4 0.4 0.4
+ 0.8 0.8 0.8
+ -6 0 6 0 0.5 0
+
+
+
+
+
+ World control
+ false
+ false
+ 72
+ 121
+ 1
+
+ floating
+
+
+
+
+
+
+ true
+ true
+ true
+ /world/buoyancy/control
+ /world/buoyancy/stats
+
+
+
+
+
+
+ World stats
+ false
+ false
+ 110
+ 290
+ 1
+
+ floating
+
+
+
+
+
+
+ true
+ true
+ true
+ true
+ /world/buoyancy/stats
+
+
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ 0 0 0 1.5707963267948966 0 0
+
+ 0 0 0 0 0 0
+
+ 251.32741228718348
+
+ 86.28907821859966
+ 0
+ 0
+ 86.28907821859966
+ 0
+ 5.026548245743671
+
+
+
+
+
+
+ 0.2
+ 2
+
+
+
+
+
+
+ 0.2
+ 2
+
+
+
+
+
+ 0 0 1.05855 0 0 0
+
+
+ 0.0933402
+ 0.127211
+
+
+
+
+
+
+ 0 0 1.09 3.14159 0 0
+
+ 21.820000000000004
+
+ 0.45999415237916674
+ 0
+ 0
+ 0.45999415237916674
+ 0
+ 0.9091666666666668
+
+
+
+
+
+ 0.3433402 0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ 0.3433402 0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+ 0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ 0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+ -0.3433402 -0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ -0.3433402 -0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+ -0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+ -0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.2 0.05455
+
+
+
+
+
+
+
+ body
+ propeller
+
+ 0 0 1
+ 1
+
+ -1e+12
+ 1e+12
+ -1
+ -1
+
+
+
+
+ 1
+
+
+
+
+ 5 0 0 1.5707963267948966 0 0
+
+ 0 0 -1.07935 0 0 0
+
+ 1102.6990214100174
+
+ 645.9978433760353
+ 0
+ 0
+ 645.9978433760353
+ 0
+ 49.62145596345078
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+ 0 0 1.378546 0 0 0
+
+
+ 0.0933402
+ 0.127211
+
+
+
+
+
+
+ 0 0 0.3455 3.14159 0 0
+
+ 25.2312297
+
+ 0.5319073381913637
+ 0
+ 0
+ 0.5319073381913637
+ 0
+ 1.0513012375
+
+
+
+
+ 0.3433402 0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.3433402 0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ 0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.3433402 -0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.3433402 -0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+
+
+ body
+ propeller
+
+ 0 0 1
+ 1
+
+ -1e+12
+ 1e+12
+ -1
+ -1
+
+
+
+
+ 1
+
+
+
+ -5 0 0 1.5707963267948966 0 0
+
+ 0 0 -1.07935 0 0 0
+
+ 367.5663404700058
+
+ 215.33261445867842
+ 0
+ 0
+ 215.33261445867842
+ 0
+ 16.54048532115026
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+
+ 0.3
+ 2.6
+
+
+
+
+
+ 0 0 1.378546 0 0 0
+
+
+ 0.0933402
+ 0.127211
+
+
+
+
+
+
+ 0 0 0.3455 3.14159 0 0
+
+ 8.410409900000001
+
+ 0.17730244606378792
+ 0
+ 0
+ 0.17730244606378792
+ 0
+ 0.35043374583333337
+
+
+
+
+
+ 0.3433402 0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.3433402 0.0 0 0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ 0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ 0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.3433402 -0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.3433402 -0.0 0 -0.7853981633974483 0 0.0
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+ -0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+ -0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966
+
+
+ 0.5 0.154178 0.05455
+
+
+
+
+
+
+
+ body
+ propeller
+
+ 0 0 1
+ 1
+
+ -1e+12
+ 1e+12
+ -1
+ -1
+
+
+
+
+ 1
+
+
+
+ 2 0 0 0 0 0
+
+
+
+ 3.92
+ 0
+ 0
+ 3.92
+ 0
+ 3.92
+
+ 39
+
+
+ 0 0 -0.4 1.57 0.0 0.0
+
+
+ 0.5 0.5 0.5
+ file://@CMAKE_SOURCE_DIR@/test/media/duck_collider.dae
+
+
+
+
+
+ 0 0 -0.4 1.57 0.0 0.0
+
+
+ 0.5 0.5 0.5
+ file://@CMAKE_SOURCE_DIR@/test/media/duck.dae
+
+
+
+
+
+
+
+
+