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
Prev Previous commit
Next Next commit
Suggestions to #1297
Signed-off-by: Louise Poubel <[email protected]>
chapulina committed Jan 18, 2022
commit f2395c1ab3910c85c6f07c02d47acc63b3b0e07d
103 changes: 103 additions & 0 deletions examples/worlds/graded_buoyancy.sdf
Original file line number Diff line number Diff line change
@@ -43,6 +43,8 @@
<!-- enable by top level model name -->
<enable>balloon_lighter_than_air</enable>
<enable>box_neutral_buoyancy</enable>
<enable>box_neutral_buoyancy_collision_offset</enable>
<enable>multi_collision_neutral_buoyancy</enable>
<enable>box_negative_buoyancy</enable>
</plugin>

@@ -216,6 +218,107 @@
</link>
</model>

<!--
This box is neutrally buoyant and therefore should stay still.
Its link origin is above water, but its collision is below.
-->
<model name='box_neutral_buoyancy_collision_offset'>
<pose>-3 5 2 0 0 0</pose>
<link name='body'>
<inertial>
<mass>1000</mass>
<pose>0 0 -5.1 0 0 0</pose>
<inertia>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>86.28907821859966</iyy>
<iyz>0</iyz>
<izz>5.026548245743671</izz>
</inertia>
</inertial>

<visual name='link_origin'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
<visual name='collision_origin'>
<pose>0 0 -5 0 0 0</pose>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
<collision name='buoyancy_volume'>
<pose>0 0 -5 0 0 0</pose>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
</model>

<!--
This box is neutrally buoyant and therefore should stay still.
It has multiple collision shapes.
-->
<model name='multi_collision_neutral_buoyancy'>
<pose>-6 5 -3 0 0 0</pose>
<link name='body'>
<inertial>
<mass>1000</mass>
<pose>0 0 0.1 0 0 0</pose>
<inertia>
<ixx>86.28907821859966</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>86.28907821859966</iyy>
<iyz>0</iyz>
<izz>5.026548245743671</izz>
</inertia>
</inertial>

<visual name='collision_1_vis'>
<pose>-0.25 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.5 1 1</size>
</box>
</geometry>
</visual>
<visual name='collision_2_vis'>
<pose>0.25 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.5 1 1</size>
</box>
</geometry>
</visual>
<collision name='collision_1'>
<pose>-0.25 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.5 1 1</size>
</box>
</geometry>
</collision>
<collision name='collision_2'>
<pose>0.25 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.5 1 1</size>
</box>
</geometry>
</collision>
</link>
</model>

<!-- This box is negatively buoyant and therefore should sink -->
<model name='box_negative_buoyancy'>
<pose>0 -8 0 0 0 0</pose>
43 changes: 30 additions & 13 deletions src/systems/buoyancy/Buoyancy.cc
Original file line number Diff line number Diff line change
@@ -77,9 +77,10 @@ class ignition::gazebo::systems::BuoyancyPrivate
public: double UniformFluidDensity(const math::Pose3d &_pose) const;

/// \brief Get the resultant buoyant force on a shape.
/// \param[in] _pose The pose of the shape.
/// \param[in] _pose World pose of the shape's origin.
/// \param[in] _shape The collision mesh of a shape. Currently must
/// be one of box, cylinder or sphere.
/// be box or sphere.
/// \param[in] _gravity Gravity acceleration in the world frame.
/// Updates this->buoyancyForces containing {force, center_of_volume} to be
/// applied on the link.
public:
@@ -101,28 +102,36 @@ class ignition::gazebo::systems::BuoyancyPrivate
/// fluidDensity.
public: std::map<double, double> layers;

/// \brief Point from where to apply the force
/// \brief Holds information about forces contributed by a single collision
/// shape.
public: struct BuoyancyActionPoint
{
/// \brief The force to be applied
/// \brief The force to be applied, expressed in the world frame.
math::Vector3d force;

/// \brief The point from which the force will be applied
/// \brief The point from which the force will be applied, expressed in
/// the collision's frame.
math::Vector3d point;

/// \brief The pose of the link in question
/// \brief The world pose of the collision.
math::Pose3d pose;
};

/// \brief List of points from where the forces act.
/// This holds values refent to the current link being processed and must be
/// cleared between links.
/// \TODO(chapulina) It's dangerous to keep link-specific values in a member
/// variable. We should consider reducing the scope of this variable and pass
/// it across functions as needed.
public: std::vector<BuoyancyActionPoint> buoyancyForces;

/// \brief Resolve all forces as if they act as a Wrench from the give pose.
/// \param[in] _pose The point from which all poses are to be resolved.
/// \param[in] _linkInWorld The point from which all poses are to be resolved.
/// This is the link's origin in the world frame.
/// \return A pair of {force, torque} describing the wrench to be applied
/// at _pose.
/// at _pose, expressed in the world frame.
public: std::pair<math::Vector3d, math::Vector3d> ResolveForces(
const math::Pose3d &_pose);
const math::Pose3d &_linkInWorld);

/// \brief Scoped names of entities that buoyancy should apply to. If empty,
/// all links will receive buoyancy.
@@ -211,17 +220,25 @@ void BuoyancyPrivate::GradedFluidDensity(

//////////////////////////////////////////////////
std::pair<math::Vector3d, math::Vector3d> BuoyancyPrivate::ResolveForces(
const math::Pose3d &_pose)
const math::Pose3d &_linkInWorld)
{
auto force = math::Vector3d{0, 0, 0};
auto torque = math::Vector3d{0, 0, 0};

for (const auto &b : this->buoyancyForces)
{
force += b.force;
math::Pose3d localPoint{b.point, math::Quaterniond{1, 0, 0, 0}};
auto globalPoint = b.pose * localPoint;
auto offset = _pose.Pos() - globalPoint.Pos();

// Pose offset from application point (COV) to collision origin, expressed
// in the collision frame
math::Pose3d pointInCol{b.point, math::Quaterniond::Identity};

// Application point in the world frame
auto pointInWorld = b.pose * pointInCol;

// Offset between the link origin and the force application point
auto offset = _linkInWorld.Pos() - pointInWorld.Pos();

torque += force.Cross(offset);
}

8 changes: 4 additions & 4 deletions src/systems/buoyancy/Buoyancy.hh
Original file line number Diff line number Diff line change
@@ -33,8 +33,8 @@ namespace systems

/// \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.
/// world and this system will apply buoyancy to all links that have collision
/// shapes.
///
/// The volume and center of volume will be computed for each link, and
/// stored as components. During each iteration, Archimedes' principle is
@@ -48,9 +48,9 @@ namespace systems
/// * `<uniform_fluid_density>` sets the density of the fluid that surrounds
/// the buoyant object. [Units: kgm^-3]
/// * `<graded_buoyancy>` allows you to define a world where the buoyancy
/// changes with height. An example of such a world could be if we are
/// changes along the Z axis. An example of such a world could be if we are
/// simulating an open ocean with its surface and under water behaviour. This
/// mode slices the volume of the collision mesh according to where the water
/// mode slices the volume of each collision mesh according to where the water
/// line is set. When defining a `<graded_buoyancy>` tag, one must also define
/// `<default_density>` and `<density_change>` tags.
/// * `<default_density>` is the default fluid which the world should be