[ROS Q&A] buoyancy neutral object goes up with hydrodynamics plugin

Written by Miguel Angel

07/11/2017

What will you learn in this post

  • Understand how to use the buoyancy plugin in gazebo

List of resources used in this post

Let’s get started

This post is an answer to a question related to buoyancy in Gazebo Answers.

To exemplify, we created a ros package called buoyancy_tests_pkg and a launch file called main.launch. We can launch that file with roslaunch buoyancy_tests_pkg main.launch.

In our package, we have the buoyancy_tests_pkg/urdf/simple_floating_sphere.urdf with the definition of a floating sphere.

The content of the file is as follows:

<robot name="simple_floating_sphere">

    <!-- Colours for RVIZ for geometric elements -->
    <material name="green">
        <color rgba="0 0.8 0 1"/>
    </material>


	<!-- * * * Link Definitions * * * -->

    <link name="simple_sphere_base_link">
 	    <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="7.23822947387" />
            <inertia ixx="0.00576" ixy="0.0" ixz="0.0" iyy="0.00576" iyz="0.0" izz="0.00576"/>
        </inertial>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <sphere radius="0.12"/>
            </geometry>
        </collision>
        <visual>
            <origin rpy="0.0 0 0" xyz="0 0 0"/>
            <geometry>
                <sphere radius="0.12"/>
            </geometry>
            <material name="green"/>
        </visual>
    </link>
    
    <gazebo reference="simple_sphere_base_link">
        <kp>100000.0</kp>
        <kd>100000.0</kd>
        <mu1>10.0</mu1>
        <mu2>10.0</mu2>
        <material>Gazebo/Green</material>
    </gazebo>
    
    <gazebo>
        <plugin name="buoyancy_sphere" filename="libBuoyancyPlugin.so">
          <fluid_density>1000</fluid_density>
        </plugin>
    </gazebo>

</robot>

The plugin used is libBuoyancyPlugin.so as can be seen at the end of the file.

<gazebo>
    <plugin name="buoyancy_sphere" filename="libBuoyancyPlugin.so">
      <fluid_density>1000</fluid_density>
    </plugin>
</gazebo>

To spawn the sphere in a gazebo simulation we have the spawn_simple_sphere.launch file with the following content:

<?xml version="1.0" encoding="UTF-8"?>

<launch>
    
    <arg name="x" default="3.0" />
    <arg name="y" default="-1.0" />
    <arg name="z" default="1.0" />
    <arg name="roll" default="0"/>
    <arg name="pitch" default="0"/>
    <arg name="yaw" default="0" />
    
    <arg name="urdf_robot_file" default="$(find buoyancy_tests_pkg)/urdf/simple_floating_sphere.urdf" />
    <arg name="robot_name" default="simple_floating_sphere" />
    
    <include file="$(find spawn_robot_tools_pkg)/launch/spawn_robot_urdf.launch">
        <arg name="x" value="$(arg x)" />
        <arg name="y" value="$(arg y)" />
        <arg name="z" value="$(arg z)" />
        <arg name="roll" value="$(arg roll)"/>
        <arg name="pitch" value="$(arg pitch)"/>
        <arg name="yaw" value="$(arg yaw)" />
        
        <arg name="urdf_robot_file" value="$(arg urdf_robot_file)" />
        <arg name="robot_name" value="$(arg robot_name)" />
    </include>
</launch>

Remember that the code to launch it would be:

roslaunch buoyancy_tests_pkg spawn_simple_sphere.launch

If everything went ok, you should have a sphere spawned like in the image below:

buoyancy sphere floating in ROSDS

buoyancy sphere floating in ROSDS

Bear in mind that the weight of the sphere applies forces in the Z direction from top to down (from the sky to the earth) and the buoyancy plugin applied the force in opposite direction. We will apply the same force in the opposite direction, which will be the mass of the sphere, basically.

Calculating the weight of the sphere

In order to calculate the mass of the sphere to then use the same value for buoyance, we created a file called volume_calculator.py on buoyancy_tests_pkg/scripts/volume_calculator.py with the following content:

#!/usr/bin/env python
import math


class BuoyancyCalculator(object):

    def __init__(self):
        print "Buoyancy Force Calculator Initialised..."

    def start_ask_loop(self):

        selection = "START"

        while selection != "Q":
            print "#############################"
            print "Select Geometry to Calculate Buoyancy Force:"
            print "[1]Sphere radius(r)"
            print "[2]Cylinder radius(r)"
            print "[3]Box dimensions(x_size*y_size*z_size)"
            print "[Q]END program"
            selection = raw_input(">>")
            self.select_action(selection)

        print "InertialCaluclator Quit...Thank you"

    def select_action(self, selection):
        if selection == "1":
            mass = float(raw_input("mass>>"))
            radius = float(raw_input("radius>>"))
            fluid_density = float(raw_input("fluid_density [kg/m^3]>>"))
            self.calculate_sphere_buoyancy_force(mass, radius, fluid_density)
            
        elif selection == "2":
            mass = float(raw_input("mass>>"))
            radius = float(raw_input("radius>>"))
            height = float(raw_input("height>>"))
            fluid_density = float(raw_input("fluid_density [kg/m^3]>>"))
            self.calculate_cylinder_buoyancy_force(mass, radius, height, fluid_density)
            
        elif selection == "3":
            mass = float(raw_input("mass>>"))
            x_size = float(raw_input("x_size>>"))
            y_size = float(raw_input("y_size>>"))
            z_size = float(raw_input("z_size>>"))
            
            fluid_density = float(raw_input("fluid_density [kg/m^3]>>"))
            self.calculate_box_buoyancy_force(mass, x_size, y_size, z_size, fluid_density, gravity=9.81)
            
        elif selection == "Q":
            print "Selected Quit"
        else:
            print "Usage: Select one of the give options"

    def calculate_sphere_volume(self, m, r):
        volume = (4.0/3.0)*math.pi*math.pow(r,3)
        return volume
        
    def calculate_cylinder_volume(self, m, r, h):
        # volume = pi*r^2*h
        volume = math.pi*math.pow(r,2)*h
        return volume
        
    def calculate_cylinder_area(self, r, h):
        # area = 2*pi*r^2 + 2*pi*r*h
        area = 2*math.pi*math.pow(r,2) + 2*math.pi*r*h
        return area
        
    def calculate_box_volume(self,x, y, z):
        # Volume of box is x*y*z
        volume = x*y*z
        return volume
    
    def calculate_sphere_buoyancy_force(self, mass, radius, fluid_density, gravity=9.81):
        volume = self.calculate_sphere_volume(m=mass, r=radius)
        fluid_eq_mass = fluid_density * volume
        buoyancy_force = fluid_eq_mass * gravity
        sphere_weight = mass * gravity
        print "Volume Sphere ="+str(volume)
        print "Mass of Fluid for Sphere Volume Sphere, This should be the mass of sphere for neutral buoyancy ="+str(fluid_eq_mass)
        print "Buoyancy Force ="+str(buoyancy_force)
        print "Weight Force of Sphere ="+str(sphere_weight)
        
        final_weight_force = buoyancy_force - sphere_weight
        
        print "RESULT OF WEIGHT AND BUOYANCY FORCE =="+str(final_weight_force)
        if final_weight_force < 0:
            print "The Sphere is going to SINK"
        elif final_weight_force == 0:
            print "The Sphere is going to have NEUTRAL Buoyancy"
        else:
            print "The Sphere is going to FLOAT"
            
            
    def calculate_cylinder_buoyancy_force(self, mass, radius, height, fluid_density, gravity=9.81):
        
        volume = self.calculate_cylinder_volume(m=mass, r=radius, h=height)
        area = self.calculate_cylinder_area(r=radius, h=height)
        
        fluid_eq_mass = fluid_density * volume
        buoyancy_force = fluid_eq_mass * gravity
        cylinder_weight = mass * gravity
        print "Volume Cylinder ="+str(volume)
        print "Area Cylinder ="+str(area)
        print "Mass of Fluid for Cylinder Volume Cylinder, This should be the mass of sphere for neutral buoyancy ="+str(fluid_eq_mass)
        print "Buoyancy Force ="+str(buoyancy_force)
        print "Weight Force of Cylinder ="+str(cylinder_weight)
        
        final_weight_force = buoyancy_force - cylinder_weight
        
        print "RESULT OF WEIGHT AND BUOYANCY FORCE =="+str(final_weight_force)
        if final_weight_force < 0:
            print "The Cylinder is going to SINK"
        elif final_weight_force == 0:
            print "The Cylinder is going to have NEUTRAL Buoyancy"
        else:
            print "The Cylinder is going to FLOAT"
            
            
    def calculate_box_buoyancy_force(self, mass, x_size, y_size, z_size, fluid_density, gravity=9.81):
        
        volume = self.calculate_box_volume(x=x_size, y=y_size, z=z_size)
        
        fluid_eq_mass = fluid_density * volume
        buoyancy_force = fluid_eq_mass * gravity
        box_weight = mass * gravity
        print "Volume Box ="+str(volume)
        print "Mass of Fluid for Box Volume, This should be the mass of Box for neutral buoyancy ="+str(fluid_eq_mass)
        print "Buoyancy Force ="+str(buoyancy_force)
        print "Weight Force of Box ="+str(box_weight)
        
        final_weight_force = buoyancy_force - box_weight
        
        print "RESULT OF WEIGHT AND BUOYANCY FORCE =="+str(final_weight_force)
        if final_weight_force < 0:
            print "The Box is going to SINK"
        elif final_weight_force == 0:
            print "The Box is going to have NEUTRAL Buoyancy"
        else:
            print "The Box is going to FLOAT"
        


if __name__ == "__main__":
    inertial_object = BuoyancyCalculator()
    inertial_object.start_ask_loop()

If you need further details at this file, you can have a look at this video: https://youtu.be/NgmvhSEM5SQ?t=560

By looking at the video you can also see that we can apply buoyancy even for complex-shaped robots like a fish:

 

More code

More code is used in this post. The list of files and their content is as follows:

main.launch

<?xml version="1.0" encoding="UTF-8"?>

<launch>  
  <arg name="robot" default="machines"/>
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="pause" default="true"/>  <!-- Start Gazebo with a blank world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find buoyancy_tests_pkg)/worlds/ocean.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg pause)"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="headless" value="$(arg headless)"/>
    <env name="GAZEBO_MODEL_PATH" value="$(find simulation_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
  </include>
<!--  Include launch.xml if needed -->
</launch>

spawn_simple_sphere.launch

<?xml version="1.0" encoding="UTF-8"?>

<launch>
    
    <arg name="x" default="3.0" />
    <arg name="y" default="-1.0" />
    <arg name="z" default="1.0" />
    <arg name="roll" default="0"/>
    <arg name="pitch" default="0"/>
    <arg name="yaw" default="0" />
    
    <arg name="urdf_robot_file" default="$(find buoyancy_tests_pkg)/urdf/simple_floating_sphere.urdf" />
    <arg name="robot_name" default="simple_floating_sphere" />
    
    <include file="$(find spawn_robot_tools_pkg)/launch/spawn_robot_urdf.launch">
        <arg name="x" value="$(arg x)" />
        <arg name="y" value="$(arg y)" />
        <arg name="z" value="$(arg z)" />
        <arg name="roll" value="$(arg roll)"/>
        <arg name="pitch" value="$(arg pitch)"/>
        <arg name="yaw" value="$(arg yaw)" />
        
        <arg name="urdf_robot_file" value="$(arg urdf_robot_file)" />
        <arg name="robot_name" value="$(arg robot_name)" />
    </include>
</launch>

spawn_robot_urdf.launch

<?xml version="1.0" encoding="UTF-8"?>

    <launch>
    
        <arg name="x" default="0.0" />
        <arg name="y" default="0.0" />
        <arg name="z" default="0.0" />
        <arg name="roll" default="0"/>
        <arg name="pitch" default="0"/>
        <arg name="yaw" default="0.0" />
        
        <arg name="urdf_robot_file" default="" />
        <arg name="robot_name" default="" />

        <!-- This Version was created due to some errors seen in the V1 that crashed GAzebo or went too slow in spawn -->
        <!-- Load the URDF into the ROS Parameter Server -->
        <param name="robot_description" command="cat $(arg urdf_robot_file)" />
        
        <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
        <node name="$(arg robot_name)_urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -model $(arg robot_name) -param robot_description"/>
    </launch>

ocean.world

<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <gui>
      <camera name="user_camera">
        <pose>10 11 10 0 0 -2.356</pose>
      </camera>
    </gui>

    <scene>
      <ambient> 0.25 0.3 0.5 1</ambient>
      <grid>false</grid>
    </scene>

    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 100 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="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>200 200</size>
            </plane>
          </geometry>
          <surface>
            <contact>
              <collide_bitmask>0xffff</collide_bitmask>
            </contact>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <cast_shadows>false</cast_shadows>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>200 200</size>
            </plane>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
      </link>
    </model>


    <model name="ceiling_plane">
      <static>true</static>
      <pose>0 0 40 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>200 200 .1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <cast_shadows>false</cast_shadows>
          <geometry>
            <box>
              <size>200 200 .1</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/BlueTransparent</name>
            </script>
          </material>
        </visual>

        <visual name="visual_sideA">
          <pose>100.05 0 -20 0 0 0</pose>
          <cast_shadows>false</cast_shadows>
          <geometry>
            <box>
              <size>0.1 200 39.9</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/BlueTransparent</name>
            </script>
          </material>
        </visual>

        <visual name="visual_sideB">
          <pose>-100.05 0 -20 0 0 0</pose>
          <cast_shadows>false</cast_shadows>
          <geometry>
            <box>
              <size>0.1 200 39.9</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/BlueTransparent</name>
            </script>
          </material>
        </visual>

        <visual name="visual_sideC">
          <pose>0 100.05 -20 0 0 0</pose>
          <cast_shadows>false</cast_shadows>
          <geometry>
            <box>
              <size>200 0.1 39.9</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/BlueTransparent</name>
            </script>
          </material>
        </visual>

        <visual name="visual_sideD">
          <pose>0 -100.05 -20 0 0 0</pose>
          <cast_shadows>false</cast_shadows>
          <geometry>
            <box>
              <size>200 0.1 39.9</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/BlueTransparent</name>
            </script>
          </material>
        </visual>

      </link>
    </model>

  </world>
</sdf>

inertial_calculator.py

#!/usr/bin/env python
import math


class InertialCalculator(object):

    def __init__(self):
        print "InertialCaluclator Initialised..."

    def start_ask_loop(self):

        selection = "START"

        while selection != "Q":
            print "#############################"
            print "Select Geometry to Calculate:"
            print "[1]Box width(w)*depth(d)*height(h)"
            print "[2]Sphere radius(r)"
            print "[3]Cylinder radius(r)*height(h)"
            print "[Q]END program"
            selection = raw_input(">>")
            self.select_action(selection)

        print "InertialCaluclator Quit...Thank you"

    def select_action(self, selection):
        if selection == "1":
            mass = float(raw_input("mass>>"))
            width = float(raw_input("width>>"))
            depth = float(raw_input("depth>>"))
            height = float(raw_input("height>>"))
            self.calculate_box_inertia(m=mass, w=width, d=depth, h=height)
        elif selection == "2":
            mass = float(raw_input("mass>>"))
            radius = float(raw_input("radius>>"))
            self.calculate_sphere_inertia(m=mass, r=radius)
        elif selection == "3":
            mass = float(raw_input("mass>>"))
            radius = float(raw_input("radius>>"))
            height = float(raw_input("height>>"))
            self.calculate_cylinder_inertia(m=mass, r=radius, h=height)
        elif selection == "Q":
            print "Selected Quit"
        else:
            print "Usage: Select one of the give options"


    def calculate_box_inertia(self, m, w, d, h):
        Iw = (m/12.0)*(pow(d,2)+pow(h,2))
        Id = (m / 12.0) * (pow(w, 2) + pow(h, 2))
        Ih = (m / 12.0) * (pow(w, 2) + pow(d, 2))
        print "BOX w*d*h, Iw = "+str(Iw)+",Id = "+str(Id)+",Ih = "+str(Ih)

    def calculate_sphere_inertia(self, m, r):
        I = (2*m*pow(r,2))/5.0
        print "SPHERE Ix,y,z = "+str(I)

    def calculate_cylinder_inertia(self, m, r, h):
        Ix = (m/12.0)*(3*pow(r,2)+pow(h,2))
        Iy = Ix
        Iz = (m*pow(r,2))/2.0
        print "Cylinder Ix,y = "+str(Ix)+",Iz = "+str(Iz)

if __name__ == "__main__":
    inertial_object = InertialCalculator()
    inertial_object.start_ask_loop()

So this is the post for today. We hope it made you better understand how the buoyancy plugin works in the gazebo simulator.

Remember that we have a live version of this post on YouTube. If you liked the post and the video, please consider subscribing to our channel and Keep Pushing your ROS Learning.

You can also find more buoyancy-related explanations in the next post: https://www.theconstruct.ai/ros-qa-066-buoyancy-neutral-object-goes-hydrodynamics-plugin-part2/

 

Topics:
Masterclass 2023 batch2 blog banner

Check Out These Related Posts

129. ros2ai

129. ros2ai

I would like to dedicate this episode to all the ROS Developers who believe that ChatGPT or...

read more

0 Comments

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This