fix(franka_gazebo): fix 'stone.world' models

[rickstaa]

As discussed in #172 (comment), here my attempt to improve the inertias of the models used in the stone.world. I added this as a draft since even though the inertias are calculated according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia and frictions and the density and friction taken from online sources, there still seem to be some problems.

It could be that my calculations or the density/friction sources are incorrect. However, due to some other pressing deadlines, I do not have more time to work on this right now.

old inertias:

new inertias:

Problems

Altough the Gazebo visualization of the inertias looks better now, there seem to be some strange problems. First, the stone seems to drift when it is on the platform:

Secondly, the platforms can not be placed on top of each other since they also drift away:

The platform drift problem is not present when the old inertias and masses are used.

Script used to calculate the mass and moment of inertias

"""Small script that calculates the inertia matrix for common shapes. See
`this wiki <https://en.wikipedia.org/wiki/List_of_moments_of_inertia>`_ for more
information.
"""
import math

import numpy as np

# Script settings
CUBE_WIDTH = 0.02
CUBE_DEPTH = 0.032
CUBE_HEIGHT = 0.064

CYLINDER_RADIUS = 0.025
CYLINDER_HEIGHT = 0.02

MATERIAL_DENSITY = 2000


def get_cylinder_mass(radius, height, rho):
    """Calculate the mass of a cylinder.

    Args:
        radius (float): The radius of the cylinder [m].
        height (float): The height of the cylinder [m].
        rho (float): The material density [kg/m^3].

    Returns:
        float: The mass [kg].
    """
    return rho * math.pi * radius ** 2 * height


def get_cube_inertia(height, width, depth, mass):
    """Calculates the moment of inertia matrix for a cube.

    Args:
        height (float): The height of the cube [m].
        width (float): The width of the cube [m].
        depth (float): The length of the cube [m].
        mass (float): The mass of the cube [kg].

    Returns:
        numpy.ndarray: The inertia matrix [kg*m^2].
    """

    return np.array(
        [
            [(1 / 12) * mass * (width ** 2 + depth ** 2), 0, 0],
            [0, (1 / 12) * mass * (depth ** 2 + height ** 2), 0],
            [0, 0, (1 / 12) * mass * (width ** 2 + height ** 2)],
        ]
    )


def get_cube_mass(height, width, depth, rho):
    """Calculate the mass of a cube.

    Args:
        radius (float): The radius of the cylinder [m].
        height (float): The height of the cylinder [m].
        depth (float): The depth of the cylinder [m].
        rho (float): The material density [kg/m^3].

    Returns:
        float: The mass [kg].
    """
    return rho * height * width * depth


def get_cylinder_inertia(radius, height, mass):
    """Calculates the moment of inertia matrix for a cube.

    Args:
        radius (float): The radius of the cylinder [m].
        height (float): The height of the cylinder [m].
        mass (float): The mass of the cylinder [kg].

    Returns:
        numpy.ndarray: The inertia matrix [kg*m^2].
    """

    return np.array(
        [
            [(1 / 12) * mass * (3 * radius ** 2 + height ** 2), 0, 0],
            [0, (1 / 12) * mass * (3 * radius ** 2 + height ** 2), 0],
            [0, 0, (1 / 2) * mass * radius ** 2],
        ]
    )


if __name__ == "__main__":

    # Cue calculations
    print("==Cube==")
    cube_mass = get_cube_mass(
        height=CUBE_HEIGHT, width=CUBE_WIDTH, depth=CUBE_DEPTH, rho=MATERIAL_DENSITY
    )
    I_cube = get_cube_inertia(
        height=CUBE_HEIGHT, width=CUBE_WIDTH, depth=CUBE_DEPTH, mass=cube_mass
    )
    print(f"mass = {cube_mass}")
    print("inertia =")
    print(I_cube)
    print("\n")

    # Cylinder calculations
    print("==Cylinder==")
    cylinder_mass = get_cylinder_mass(
        radius=CYLINDER_RADIUS, height=CYLINDER_HEIGHT, rho=MATERIAL_DENSITY
    )
    I_cylinder = get_cylinder_inertia(
        radius=CYLINDER_RADIUS, height=CYLINDER_HEIGHT, mass=cylinder_mass
    )
    print(f"mass = {cylinder_mass}")
    print("inertia =")
    print(I_cylinder)

TODOS

• Update the documentation so that it reflects the new world file (i.e. worlds/stone.world instead of world/stone.sdf).

[rickstaa]

@gollth Because of the limited time, I can assign to this issue I created a hotfix. By setting the trays to static the problem does not appear. Nevertheless, it would be interesting to know what causes the drifting problem.

[marcbone]

Thanks for this PR. I am currently writing a set of gripper tests in gazebo and I tested your changes against it.

For your convenience I created a patch with my requested changes that you can apply with "git apply":

Index: franka_gazebo/models/pick_tray/model.config
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/pick_tray/model.config b/franka_gazebo/models/pick_tray/model.config
--- a/franka_gazebo/models/pick_tray/model.config	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/pick_tray/model.config	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -2,7 +2,7 @@
 <model>
     <name>pick_tray</name>
     <version>1.0</version>
-    <sdf version="1.7">model.sdf</sdf>
+    <sdf version="1.6">model.sdf</sdf>
     <author>
         <name>Franka Emika</name>
         <email>[email protected]</email>
Index: franka_gazebo/models/pick_tray/model.sdf
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/pick_tray/model.sdf b/franka_gazebo/models/pick_tray/model.sdf
--- a/franka_gazebo/models/pick_tray/model.sdf	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/pick_tray/model.sdf	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -1,5 +1,5 @@
 <?xml version='1.0'?>
-<sdf version='1.7'>
+<sdf version='1.6'>
   <model name='pick_tray'>
     <link name='link'>
       <pose>0.0 0.0 0.011674 0 -0 0</pose>
Index: franka_gazebo/models/place_tray/model.config
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/place_tray/model.config b/franka_gazebo/models/place_tray/model.config
--- a/franka_gazebo/models/place_tray/model.config	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/place_tray/model.config	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -2,7 +2,7 @@
 <model>
     <name>place_tray</name>
     <version>1.0</version>
-    <sdf version="1.7">model.sdf</sdf>
+    <sdf version="1.6">model.sdf</sdf>
     <author>
         <name>Franka Emika</name>
         <email>[email protected]</email>
Index: franka_gazebo/models/place_tray/model.sdf
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/place_tray/model.sdf b/franka_gazebo/models/place_tray/model.sdf
--- a/franka_gazebo/models/place_tray/model.sdf	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/place_tray/model.sdf	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -1,5 +1,5 @@
 <?xml version='1.0'?>
-<sdf version='1.7'>
+<sdf version='1.6'>
   <model name='place_tray'>
     <link name='link'>
       <pose>0.0 0.0 0.011674 0 -0 0</pose>
Index: franka_gazebo/models/stone/model.config
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/stone/model.config b/franka_gazebo/models/stone/model.config
--- a/franka_gazebo/models/stone/model.config	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/stone/model.config	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -2,7 +2,7 @@
 <model>
     <name>stone</name>
     <version>1.0</version>
-    <sdf version="1.7">model.sdf</sdf>
+    <sdf version="1.6">model.sdf</sdf>
     <author>
         <name>Franka Emika</name>
         <email>[email protected]</email>
Index: franka_gazebo/models/stone/model.sdf
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/stone/model.sdf b/franka_gazebo/models/stone/model.sdf
--- a/franka_gazebo/models/stone/model.sdf	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/stone/model.sdf	(revision 8c905d293eb7c0bafe4c304676f3c810874d502d)
@@ -1,8 +1,8 @@
 <?xml version='1.0'?>
-<sdf version='1.7'>
+<sdf version='1.6'>
   <model name='stone'>
     <link name='link'>
-      <pose>0.0 0.0 0.032 0 -0 0</pose>
+      <pose>0.0 0.0 0.0 0 -0 0</pose>
       <inertial>
         <!--NOTE: Uses limestone density (i.e 2000 kg/m^3) to calculate the mass
         see https://en.wikipedia.org/wiki/Limestone
Index: franka_gazebo/models/table/model.config
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/table/model.config b/franka_gazebo/models/table/model.config
--- a/franka_gazebo/models/table/model.config	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/table/model.config	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -2,7 +2,7 @@
 <model>
     <name>table</name>
     <version>1.0</version>
-    <sdf version="1.7">model.sdf</sdf>
+    <sdf version="1.6">model.sdf</sdf>
     <author>
         <name>Franka Emika</name>
         <email>[email protected]</email>
Index: franka_gazebo/models/table/model.sdf
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/models/table/model.sdf b/franka_gazebo/models/table/model.sdf
--- a/franka_gazebo/models/table/model.sdf	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/models/table/model.sdf	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -1,5 +1,5 @@
 <?xml version='1.0'?>
-<sdf version='1.7'>
+<sdf version='1.6'>
   <model name='table'>
     <link name='link'>
       <pose>0 0 0.2099115 0 -0 0</pose>
Index: franka_gazebo/package.xml
IDEA additional info:
Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
<+>UTF-8
===================================================================
diff --git a/franka_gazebo/package.xml b/franka_gazebo/package.xml
--- a/franka_gazebo/package.xml	(revision de59a8b0b3625451f22e2cea5f1bac4722781d69)
+++ b/franka_gazebo/package.xml	(revision 74237b9680246d9ea1401483ce03c422e60722a6)
@@ -47,6 +47,6 @@
 
     <!--Export gazebo models and worlds-->
     <gazebo_ros gazebo_model_path="${prefix}/models"/>
-    <gazebo_ros gazebo_resource_path="${prefix}/worlds"/>
+    <gazebo_ros gazebo_resource_path="${prefix}/world"/>
   </export>
 </package>
diff --git a/franka_gazebo/worlds/stone.world b/franka_gazebo/world/stone.sdf
rename from franka_gazebo/worlds/stone.world
rename to franka_gazebo/world/stone.sdf
index 9f2315654a0df182e820ebd7a15b4e9e32055737..38f9c25c312aad8dd04bfde0c3ddc16cb03e4756
GIT binary patch
literal 992
zc$~eH&2obv6h`-Xid(xjp!F{uW2T!<AE0}OD6vclIDl#T_61Bj$uw!~rW@yihwFC)
z%GG|$VMmo_QY?dLjssX#vr@O4)7dPjW&q`fRJ;X2wsaZvM6{&tS@T}os_r>ysHZG<
z7lu#Jak(ZOIP2aFXgR2chT#EVqTxeJ$B`Wmid9?LQvQgL9z==U#E7CZ_Rw0%L9}4M
zDW>%t7u0^it>&*}&8KxWjy!2+U-wee3eRzrC2;|^z&I@uR{)04K1$?~Bd-lHtf`_B
z9jfcO+^+_s$mgVNeq$VcIUg?4ys&}hag^rR1x%tvo<EE!!<#XErWa}8neD+y=oQ^j
zVW3rPW0>ebQwsMn&c4H5RC0&r4^9sMO)817HLy;LEIm49$^SZOBPSk8PhK&>agq7$
cu`YS+RPr=(#ZQ&96eaR^)ZSoZuW&W{0;4++HUIzs

[rickstaa]

@marcbone Great I will take a look and apply the changes. Also, feel free to push changes to any of my pull requests using the gh pr checkout 206 feature.

[rickstaa]

@marcbone I rebased on the current develop branch. Let me know if you want me to change other things.

[marcbone]

No its fine thanks.