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.

Sign up for GitHub

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

Jump to bottom

Documentation and more examples for #1297 #1301

Merged
merged 3 commits into from
Jan 19, 2022
Merged

Documentation and more examples for #1297 #1301

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f2395c1
Suggestions to #1297
chapulina Jan 18, 2022
60d013a
offset
chapulina Jan 19, 2022
bad001b
Fix torque for multiple collisions
chapulina Jan 19, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
103 changes: 103 additions & 0 deletions examples/worlds/graded_buoyancy.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down Expand Up @@ -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 -4.9 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>
Expand Down
45 changes: 31 additions & 14 deletions src/systems/buoyancy/Buoyancy.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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.
Expand Down Expand Up @@ -211,18 +220,26 @@ 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();
torque += force.Cross(offset);

// 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 += b.force.Cross(offset);
}

return {force, torque};
Expand Down
8 changes: 4 additions & 4 deletions src/systems/buoyancy/Buoyancy.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down