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

Fluid added mass #1592

Merged
merged 30 commits into from
Feb 2, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
30 commits
Select commit Hold shift + click to select a range
cdcdf77
Added mass example world
chapulina Jul 13, 2022
ed827e0
conversion, start GUI
chapulina Jul 14, 2022
e703948
display added mass
chapulina Jul 14, 2022
aac59b8
New Apply Link Wrench system
chapulina Jul 14, 2022
34dcd90
cherry-picking
chapulina Jul 18, 2022
5593281
Fix crash
chapulina Jul 15, 2022
aed1fd1
fix demo
chapulina Jul 15, 2022
b3b537e
starting tests
chapulina Jul 16, 2022
909594f
iterating on tests
chapulina Jul 18, 2022
a04ff60
fix cherry-pick
chapulina Jul 18, 2022
4281f41
fix cherry-pick
chapulina Jul 18, 2022
ecebb84
apply wrench in example world
chapulina Jul 19, 2022
8ea4f60
Use conversion from msgs
chapulina Jul 19, 2022
0f1daa4
test world
chapulina Jul 28, 2022
55ab535
Merge branch 'main' into chapulina/7/added_mass
chapulina Aug 3, 2022
65841ee
Merge branch 'gz-sim7' into chapulina/7/added_mass
mjcarroll Sep 27, 2022
7721e01
Update Util_TEST.cc to match the one in gz-sim7.
JoanAguilar Oct 6, 2022
ebf0d62
Add added mass integration test.
JoanAguilar Oct 7, 2022
5bfdbfa
Merge remote-tracking branch 'origin/gz-sim7' into chapulina/7/added_…
JoanAguilar Nov 14, 2022
42492dc
Fix badly resolved merge conflict.
JoanAguilar Nov 15, 2022
004b8e3
Change the name of the added mass ellipsoids world.
JoanAguilar Nov 15, 2022
31d4e4a
Add added mass unit test.
JoanAguilar Nov 15, 2022
6aeeeb8
Merge branch 'gz-sim7' into chapulina/7/added_mass
mjcarroll Nov 28, 2022
cb4e97d
Remove extra line.
JoanAguilar Nov 29, 2022
a2473b5
Change use of math library in order to fix Windows errors.
JoanAguilar Dec 5, 2022
57bba83
Merge branch 'gz-sim7' into chapulina/7/added_mass
mjcarroll Dec 6, 2022
74dfd2e
Merge branch 'gz-sim7' into chapulina/7/added_mass
mjcarroll Dec 8, 2022
fdd3588
Merge branch 'gz-sim7' into chapulina/7/added_mass
mjcarroll Jan 25, 2023
9e9893d
Temporary test debug output
mjcarroll Jan 30, 2023
0ba6831
Loosen test tolerances for focal
mjcarroll Feb 1, 2023
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
346 changes: 346 additions & 0 deletions examples/worlds/fluid_added_mass.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,346 @@
<?xml version="1.0" ?>
<!--

Demo of fluid added mass SDF parameter.

-->
<sdf version="1.6">
<world name="fluid_added_mass">
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-apply-link-wrench-system"
name="gz::sim::systems::ApplyLinkWrench">

<persistent>
<entity_name>sphere_no_added_mass</entity_name>
<entity_type>model</entity_type>
<force>1 0 0</force>
</persistent>

<persistent>
<entity_name>sphere_in_water</entity_name>
<entity_type>model</entity_type>
<force>1 0 0</force>
</persistent>

<persistent>
<entity_name>sphere_in_air</entity_name>
<entity_type>model</entity_type>
<force>1 0 0</force>
</persistent>

<persistent>
<entity_name>box_diagonal</entity_name>
<entity_type>model</entity_type>
<force>1 0 0</force>
</persistent>

<persistent>
<entity_name>box_xx_xy_xz</entity_name>
<entity_type>model</entity_type>
<force>1 0 0</force>
</persistent>

<persistent>
<entity_name>box_xy</entity_name>
<entity_type>model</entity_type>
<force>1 0 0</force>
</persistent>

</plugin>

<gravity>0 0 0</gravity>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="sphere_no_added_mass">
<pose>0 0 0.5 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<collision name="collision">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</collision>

<visual name="visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 0 1</ambient>
<diffuse>0 1 0 1</diffuse>
<specular>0 1 0 1</specular>
</material>
</visual>
</link>
</model>

<model name="sphere_in_water">
<pose>0 1.5 0.5 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
<mass>1.0</mass>
<!--
Added mass matrix for a sphere is mostly zeros, with just the translational diagonal set.

Each element is half the displaced fluid mass.

Assuming the sphere is fully submerged.

Sv: Sphere volume
Fd: Fluid density
Fm: Fluid mass
a: Added mass

Sv = 4 / 3 * pi * r ^ 3

Fm = Sv * Fd

a = Fm / 2 = 2 / 3 * pi * r ^ 3 * Fd

The values below use 1000 kg/m^3 for the fluid density.
-->
<fluid_added_mass>
<xx>261.666</xx>
<yy>261.666</yy>
<zz>261.666</zz>
</fluid_added_mass>
</inertial>
<collision name="collision">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</collision>

<visual name="visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1 1</ambient>
<diffuse>0 0 1 1</diffuse>
<specular>0 0 1 1</specular>
</material>
</visual>
</link>
</model>

<model name="sphere_in_air">
<pose>0 -1.5 0.5 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
<mass>1.0</mass>
<!--
The values below use 1 kg/m^3 for the fluid density.
-->
<fluid_added_mass>
<xx>0.261666</xx>
<yy>0.261666</yy>
<zz>0.261666</zz>
</fluid_added_mass>
</inertial>
<collision name="collision">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</collision>

<visual name="visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 1</ambient>
<diffuse>0 1 1 1</diffuse>
<specular>1 0 1 1</specular>
</material>
</visual>
</link>
</model>

<model name="box_diagonal">
<pose>0 4.0 0.5 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
<mass>1.0</mass>
<fluid_added_mass>
<xx>0.261666</xx>
<yy>0.261666</yy>
<zz>0.261666</zz>
<!--xx>0.261666</xx>
<yy>0.261666</yy>
<zz>0.261666</zz-->
</fluid_added_mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</collision>

<visual name="visual">
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
<material>
<ambient>0 1 1 1</ambient>
<diffuse>0 1 1 1</diffuse>
<specular>1 0 1 1</specular>
</material>
</visual>
</link>
</model>

<model name="box_xx_xy_xz">
<pose>0 6.0 0.5 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
<mass>1.0</mass>
<fluid_added_mass>
<xx>0.261666</xx>
<xy>0.261666</xy>
<xz>0.261666</xz>
</fluid_added_mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</collision>

<visual name="visual">
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
<material>
<ambient>0 1 1 1</ambient>
<diffuse>0 1 1 1</diffuse>
<specular>1 0 1 1</specular>
</material>
</visual>
</link>
</model>

<model name="box_xy">
<pose>0 8.0 0.5 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
<mass>1.0</mass>
<fluid_added_mass>
<xy>0.261666</xy>
</fluid_added_mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
</collision>

<visual name="visual">
<geometry>
<box>
<size>1.0 1.0 1.0</size>
</box>
</geometry>
<material>
<ambient>0 1 1 1</ambient>
<diffuse>0 1 1 1</diffuse>
<specular>1 0 1 1</specular>
</material>
</visual>
</link>
</model>
</world>
</sdf>
Loading