Skip to content

Commit

Permalink
Add new example and fix it shooting up
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <[email protected]>
  • Loading branch information
@chapulina
chapulina committed Sep 21, 2021
1 parent 80b27ae commit 2abcabd
Show file tree
Hide file tree
Showing 2 changed files with 62 additions and 21 deletions.
35 changes: 34 additions & 1 deletion examples/worlds/graded_buoyancy.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
<camera_pose>-10 -2 5.6 0 0.5 0</camera_pose>
</plugin>

<!-- World control -->
Expand Down Expand Up @@ -169,6 +169,39 @@
</link>
</model>

<!-- This box should float. -->
<model name='box_lighter_than_water'>
<pose>3 5 0 0.3 0.2 0.1</pose>
<link name='body'>
<inertial>
<mass>200</mass>
<inertia>
<ixx>33.33</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>33.33</iyy>
<iyz>0</iyz>
<izz>33.33</izz>
</inertia>
</inertial>

<visual name='body_visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
<collision name='body_collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
</model>

<!-- This balloon should shoot up -->
<model name='balloon_lighter_than_air'>
<pose>0 -5 -5 0 0 0</pose>
Expand Down
48 changes: 28 additions & 20 deletions src/systems/buoyancy/Buoyancy.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ class ignition::gazebo::systems::BuoyancyPrivate
{
/// \brief The force to be applied
math::Vector3d force;

/// \brief The point from which the force will be applied
math::Vector3d point;

Expand Down Expand Up @@ -138,26 +138,29 @@ void BuoyancyPrivate::GradedFluidDensity(
auto prevLayerVol = 0.0;
auto centerOfBuoyancy = math::Vector3d{0, 0, 0};

for(auto [height, currFluidDensity] : this->layers)
for (const auto &[height, currFluidDensity] : this->layers)
{
// TODO(arjo): Transform plane and slice the shape
math::Planed plane{math::Vector3d{0, 0, 1}, height - _pose.Pos().Z()};
auto vol = _shape.VolumeBelow(plane);

// Short circuit.
if (vol <= 0)
{
prevLayerFluidDensity = currFluidDensity;
continue;
}

// Calculate point from which force is applied
auto cov = _shape.CenterOfVolumeBelow(plane);

if(!cov.has_value())
if (!cov.has_value())
{
prevLayerFluidDensity = currFluidDensity;
continue;
}

// Archimedes principal for this layer
// Archimedes principle for this layer
auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity;

// Accumulate layers.
Expand All @@ -172,17 +175,18 @@ void BuoyancyPrivate::GradedFluidDensity(
cob,
_pose
};
buoyancyForces.push_back(buoyancyAction);
this->buoyancyForces.push_back(buoyancyAction);

prevLayerVol = vol;
}
// For the rest of the layers.
auto vol = _shape.Volume();

// No force contributed by this layer.
if (abs(vol - prevLayerVol) < 1e-10) return;
if (std::abs(vol - prevLayerVol) < 1e-10)
return;

// Archimedes principal for this layer
// Archimedes principle for this layer
auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity;

// Calculate centre of buoyancy
Expand All @@ -196,7 +200,7 @@ void BuoyancyPrivate::GradedFluidDensity(
cob,
_pose
};
buoyancyForces.push_back(buoyancyAction);
this->buoyancyForces.push_back(buoyancyAction);
}

//////////////////////////////////////////////////
Expand All @@ -206,7 +210,7 @@ std::pair<math::Vector3d, math::Vector3d> BuoyancyPrivate::ResolveForces(
auto force = math::Vector3d{0, 0, 0};
auto torque = math::Vector3d{0, 0, 0};

for(auto b : this->buoyancyForces)
for (const auto &b : this->buoyancyForces)
{
force += b.force;
math::Pose3d localPoint{b.point, math::Quaterniond{1, 0, 0, 0}};
Expand Down Expand Up @@ -247,37 +251,41 @@ void Buoyancy::Configure(const Entity &_entity,
{
this->dataPtr->fluidDensity = _sdf->Get<double>("uniform_fluid_density");
}
else if(_sdf->HasElement("graded_buoyancy"))
else if (_sdf->HasElement("graded_buoyancy"))
{
this->dataPtr->buoyancyType =
BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY;

auto gradedElement = _sdf->GetFirstElement();
if(gradedElement == nullptr)
if (gradedElement == nullptr)
{
ignerr << "Unable to get element description" << std::endl;
return;
}

auto argument = gradedElement->GetFirstElement();
while(argument != nullptr)
while (argument != nullptr)
{
if(argument->GetName() == "default_density")
if (argument->GetName() == "default_density")
{
argument->GetValue()->Get<double>(this->dataPtr->fluidDensity);
igndbg << "Default density set to "
<< this->dataPtr->fluidDensity << std::endl;
}
if(argument->GetName() == "density_change")
if (argument->GetName() == "density_change")
{
auto depth = argument->Get<double>("above_depth", 0.0);
auto density = argument->Get<double>("density", 0.0);
if(!depth.second)
if (!depth.second)
{
ignwarn << "No <above_depth> tag was found as a "
<< "child of <density_change>" << std::endl;
if(!density.second)
}
if (!density.second)
{
ignwarn << "No <density> tag was found as a "
<< "child of <density_change>" << std::endl;
}
this->dataPtr->layers[depth.first] = density.first;
igndbg << "Added layer at " << depth.first << ", "
<< density.first << std::endl;
Expand Down Expand Up @@ -422,7 +430,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
// By Archimedes' principle,
// buoyancy = -(mass*gravity)*fluid_density/object_density
// object_density = mass/volume, so the mass term cancels.
if(this->dataPtr->buoyancyType
if (this->dataPtr->buoyancyType
== BuoyancyPrivate::BuoyancyType::UNIFORM_BUOYANCY)
{
buoyancy =
Expand All @@ -440,14 +448,14 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
// Physics System.
link.AddWorldWrench(_ecm, buoyancy, torque);
}
else if(this->dataPtr->buoyancyType
else if (this->dataPtr->buoyancyType
== BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY)
{
std::vector<Entity> collisions = _ecm.ChildrenByComponents(
_entity, components::Collision());
this->dataPtr->buoyancyForces.clear();

for(auto e : collisions)
for (auto e : collisions)
{
const components::CollisionElement *coll =
_ecm.Component<components::CollisionElement>(e);
Expand Down Expand Up @@ -481,7 +489,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
}
}
}
auto [force, torque]= this->dataPtr->ResolveForces(
auto [force, torque] = this->dataPtr->ResolveForces(
link.WorldInertialPose(_ecm).value());
// Apply the wrench to the link. This wrench is applied in the
// Physics System.
Expand Down

0 comments on commit 2abcabd

Please sign in to comment.