From cd946f4de31a7040c8782e80d7e7a0ba6c770d4c Mon Sep 17 00:00:00 2001
From: rickstaa <rick.staa@outlook.com>
Date: Tue, 23 Nov 2021 22:32:36 +0100
Subject: [PATCH] fix(franka_gazebo): improve 'stone.world' models

---
 CHANGELOG.md                                 |   1 +
 franka_gazebo/models/pick_tray/model.config  |  11 +
 franka_gazebo/models/pick_tray/model.sdf     | 104 ++++++
 franka_gazebo/models/place_tray/model.config |  11 +
 franka_gazebo/models/place_tray/model.sdf    | 104 ++++++
 franka_gazebo/models/stone/model.config      |  11 +
 franka_gazebo/models/stone/model.sdf         | 104 ++++++
 franka_gazebo/models/table/model.config      |  11 +
 franka_gazebo/models/table/model.sdf         |  98 +++++
 franka_gazebo/package.xml                    |   4 +
 franka_gazebo/world/stone.sdf                | 364 -------------------
 franka_gazebo/worlds/stone.world             |  33 ++
 12 files changed, 492 insertions(+), 364 deletions(-)
 create mode 100644 franka_gazebo/models/pick_tray/model.config
 create mode 100644 franka_gazebo/models/pick_tray/model.sdf
 create mode 100644 franka_gazebo/models/place_tray/model.config
 create mode 100644 franka_gazebo/models/place_tray/model.sdf
 create mode 100644 franka_gazebo/models/stone/model.config
 create mode 100644 franka_gazebo/models/stone/model.sdf
 create mode 100644 franka_gazebo/models/table/model.config
 create mode 100644 franka_gazebo/models/table/model.sdf
 delete mode 100644 franka_gazebo/world/stone.sdf
 create mode 100644 franka_gazebo/worlds/stone.world

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0b64c74ca..ea328f77e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,7 @@
 * **BREAKING** Make `/panda` namespace of `franka_gazebo` optional
 * Add effort joint trajectory controller to be used by MoveIT
 * Make finger collisions primitive in `franka_gazebo`
+* Improve Gazebo 'stone' world objects
 
 ## 0.8.1 - 2021-09-08
 
diff --git a/franka_gazebo/models/pick_tray/model.config b/franka_gazebo/models/pick_tray/model.config
new file mode 100644
index 000000000..009e06bc6
--- /dev/null
+++ b/franka_gazebo/models/pick_tray/model.config
@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>pick_tray</name>
+    <version>1.0</version>
+    <sdf version="1.7">model.sdf</sdf>
+    <author>
+        <name>Franka Emika</name>
+        <email>support@franka.de</email>
+    </author>
+    <description>Simple blue coloured tray.</description>
+</model>
diff --git a/franka_gazebo/models/pick_tray/model.sdf b/franka_gazebo/models/pick_tray/model.sdf
new file mode 100644
index 000000000..15a85487b
--- /dev/null
+++ b/franka_gazebo/models/pick_tray/model.sdf
@@ -0,0 +1,104 @@
+<?xml version='1.0'?>
+<sdf version='1.7'>
+  <model name='pick_tray'>
+    <link name='link'>
+      <pose>0.0 0.0 0.011674 0 -0 0</pose>
+      <inertial>
+        <!--NOTE: Uses hard wood density (i.e 960 kg/m^3) to calculate the mass
+        see https://qtimber.daf.qld.gov.au/guides/wood-density-and-hardness
+        -->
+        <mass>0.7634836885523328</mass>
+        <inertia>
+          <ixx>0.00137101</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.00354935</iyy>
+          <iyz>0</iyz>
+          <izz>0.00485098</izz>
+        </inertia>
+        <pose>0 0 0 0 -0 0</pose>
+      </inertial>
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <pose>0 0 0 0 -0 0</pose>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.235035 0.144926 0.023348</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Blue</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+          <shader type='pixel'/>
+        </material>
+        <pose>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.235035 0.144926 0.023348</size>
+          </box>
+        </geometry>
+        <surface>
+          <!--NOTE: Uses dynamic friction of wood on a brick surface
+          see https://www.engineeringtoolbox.com/friction-coefficients-d_778.html
+          -->
+          <friction>
+            <ode>
+              <mu>0.6</mu>
+              <mu2>0.6</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>
diff --git a/franka_gazebo/models/place_tray/model.config b/franka_gazebo/models/place_tray/model.config
new file mode 100644
index 000000000..1b1689563
--- /dev/null
+++ b/franka_gazebo/models/place_tray/model.config
@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>place_tray</name>
+    <version>1.0</version>
+    <sdf version="1.7">model.sdf</sdf>
+    <author>
+        <name>Franka Emika</name>
+        <email>support@franka.de</email>
+    </author>
+    <description>A simple red coloured tray.</description>
+</model>
diff --git a/franka_gazebo/models/place_tray/model.sdf b/franka_gazebo/models/place_tray/model.sdf
new file mode 100644
index 000000000..41f91644c
--- /dev/null
+++ b/franka_gazebo/models/place_tray/model.sdf
@@ -0,0 +1,104 @@
+<?xml version='1.0'?>
+<sdf version='1.7'>
+  <model name='place_tray'>
+    <link name='link'>
+      <pose>0.0 0.0 0.011674 0 -0 0</pose>
+      <inertial>
+        <!--NOTE: Uses hard wood density (i.e 960 kg/m^3) to calculate the mass
+        see https://qtimber.daf.qld.gov.au/guides/wood-density-and-hardness
+        -->
+        <mass>0.7634836885523328</mass>
+        <inertia>
+          <ixx>0.00137101</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.00354935</iyy>
+          <iyz>0</iyz>
+          <izz>0.00485098</izz>
+        </inertia>
+        <pose>0 0 0 0 -0 0</pose>
+      </inertial>
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <pose>0 0 0 0 -0 0</pose>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.235035 0.144926 0.023348</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Red</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+          <shader type='pixel'/>
+        </material>
+        <pose>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.235035 0.144926 0.023348</size>
+          </box>
+        </geometry>
+        <surface>
+          <!--NOTE: Uses dynamic friction of wood on a brick surface
+          see https://www.engineeringtoolbox.com/friction-coefficients-d_778.html
+          -->
+          <friction>
+            <ode>
+              <mu>0.6</mu>
+              <mu2>0.6</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>
diff --git a/franka_gazebo/models/stone/model.config b/franka_gazebo/models/stone/model.config
new file mode 100644
index 000000000..4d3cefa04
--- /dev/null
+++ b/franka_gazebo/models/stone/model.config
@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>stone</name>
+    <version>1.0</version>
+    <sdf version="1.7">model.sdf</sdf>
+    <author>
+        <name>Franka Emika</name>
+        <email>support@franka.de</email>
+    </author>
+    <description>A simple stone.</description>
+</model>
diff --git a/franka_gazebo/models/stone/model.sdf b/franka_gazebo/models/stone/model.sdf
new file mode 100644
index 000000000..f1c058f85
--- /dev/null
+++ b/franka_gazebo/models/stone/model.sdf
@@ -0,0 +1,104 @@
+<?xml version='1.0'?>
+<sdf version='1.7'>
+  <model name='stone'>
+    <link name='link'>
+      <pose>0.0 0.0 0.032 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
+        -->
+        <mass>0.08192</mass>
+        <inertia>
+          <ixx>3.49525333e-05</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>3.06926933e-05</iyy>
+          <iyz>0</iyz>
+          <izz>9.72117333e-06</izz>
+        </inertia>
+        <pose>0 0 0 0 -0 0</pose>
+      </inertial>
+        <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <pose>0 0 0 0 -0 0</pose>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.02 0.032 0.064</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/White</name>
+          </script>
+          <shader type='pixel'/>
+        </material>
+        <pose>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.02 0.032 0.064</size>
+          </box>
+        </geometry>
+        <surface>
+          <!--NOTE: Uses dynamic friction of brick on a wood surface
+          see https://www.engineeringtoolbox.com/friction-coefficients-d_778.html
+          -->
+          <friction>
+            <ode>
+              <mu>0.6</mu>
+              <mu2>0.6</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0</max_vel>
+              <min_depth>0.003</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>
diff --git a/franka_gazebo/models/table/model.config b/franka_gazebo/models/table/model.config
new file mode 100644
index 000000000..aac78c9b8
--- /dev/null
+++ b/franka_gazebo/models/table/model.config
@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>table</name>
+    <version>1.0</version>
+    <sdf version="1.7">model.sdf</sdf>
+    <author>
+        <name>Franka Emika</name>
+        <email>support@franka.de</email>
+    </author>
+    <description>A simple table.</description>
+</model>
diff --git a/franka_gazebo/models/table/model.sdf b/franka_gazebo/models/table/model.sdf
new file mode 100644
index 000000000..ad3fa134a
--- /dev/null
+++ b/franka_gazebo/models/table/model.sdf
@@ -0,0 +1,98 @@
+<?xml version='1.0'?>
+<sdf version='1.7'>
+  <model name='table'>
+    <link name='link'>
+      <pose>0 0 0.2099115 0 -0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 -0 0</pose>
+        <mass>10</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+      </inertial>
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <visual name='visual'>
+        <pose>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.453157 0.642049 0.419823</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Grey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+          <shader type='pixel'/>
+        </material>
+        <pose>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.453157 0.642049 0.419823</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>1</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>
diff --git a/franka_gazebo/package.xml b/franka_gazebo/package.xml
index 901863799..11c41dbbd 100644
--- a/franka_gazebo/package.xml
+++ b/franka_gazebo/package.xml
@@ -43,5 +43,9 @@
   <export>
     <controller_interface plugin="${prefix}/franka_gripper_sim_plugin.xml"/>
     <gazebo_ros_control   plugin="${prefix}/franka_hw_sim_plugin.xml"/>
+
+    <!--Export gazebo models and worlds-->
+    <gazebo_ros gazebo_model_path="${prefix}/models"/>
+    <gazebo_ros gazebo_resource_path="${prefix}/worlds"/>
   </export>
 </package>
diff --git a/franka_gazebo/world/stone.sdf b/franka_gazebo/world/stone.sdf
deleted file mode 100644
index e3911a288..000000000
--- a/franka_gazebo/world/stone.sdf
+++ /dev/null
@@ -1,364 +0,0 @@
-<sdf version='1.6'>
-  <world name='default'>
-    <state world_name='default'>
-      <sim_time>0 0</sim_time>
-      <real_time>0 0</real_time>
-      <wall_time>1624534445 711523457</wall_time>
-      <iterations>0</iterations>
-      <model name='ground_plane'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
-        <scale>1 1 1</scale>
-        <link name='link'>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-          <velocity>0 0 0 0 -0 0</velocity>
-          <acceleration>0 0 0 0 -0 0</acceleration>
-          <wrench>0 0 0 0 -0 0</wrench>
-        </link>
-      </model>
-      <model name='place_tray'>
-        <pose frame=''>-0.004103 0.219654 0.431426 -0.000112 -9.6e-05 0.01087</pose>
-        <scale>1 1 1</scale>
-        <link name='link'>
-          <pose frame=''>-0.004103 0.219654 0.431426 -0.000112 -9.6e-05 0.01087</pose>
-          <velocity>0 0 0 0 -0 0</velocity>
-          <acceleration>0 0 -9.8 0 -0 0</acceleration>
-          <wrench>0 0 -9.8 0 -0 0</wrench>
-        </link>
-      </model>
-      <model name='pick_tray'>
-        <pose frame=''>0.009179 -0.219554 0.430776 3.5e-05 -1.8e-05 -0.006066</pose>
-        <scale>1 1 1</scale>
-        <link name='link'>
-          <pose frame=''>0.009179 -0.219554 0.430776 3.5e-05 -1.8e-05 -0.006066</pose>
-          <velocity>0 0 0 0 -0 0</velocity>
-          <acceleration>0 -0 -10.78 0.181358 -0.160514 0</acceleration>
-          <wrench>0 -0 -10.78 0 -0 0</wrench>
-        </link>
-      </model>
-      <model name='stone'>
-        <pose frame=''>0.000286 -0.221972 0.475172 7.5e-05 3.5e-05 0.003179</pose>
-        <scale>1 1 1</scale>
-        <link name='link'>
-          <pose frame=''>0.000286 -0.221972 0.475172 7.5e-05 3.5e-05 0.003179</pose>
-          <velocity>0 0 0 0 -0 0</velocity>
-          <acceleration>0 0 -9.8 0 -0 0</acceleration>
-          <wrench>0 0 -0.49 0 -0 0</wrench>
-        </link>
-      </model>
-      <model name='table'>
-        <pose frame=''>-0.016428 0.000584 0.209863 -4.5e-05 -1.6e-05 1.8e-05</pose>
-        <scale>0.148528 0.465925 0.163403</scale>
-        <link name='link'>
-          <pose frame=''>-0.016428 0.000584 0.209863 -4.5e-05 -1.6e-05 1.8e-05</pose>
-          <velocity>0 0 0 0 -0 0</velocity>
-          <acceleration>0.545345 -1.28786 -23.3914 2.21988 -1.56308 -2.85983</acceleration>
-          <wrench>0.545345 -1.28786 -23.3914 0 -0 0</wrench>
-        </link>
-      </model>
-      <light name='sun'>
-        <pose frame=''>0 0 2 0 0 0</pose>
-      </light>
-    </state>
-    <light name='sun' type='directional'>
-      <cast_shadows>1</cast_shadows>
-      <diffuse>1 1 1 1</diffuse>
-      <specular>0.8 0.8 0.8 1</specular>
-      <direction>-0.5 -0.5 -1</direction>
-        <pose frame=''>0 0 2 0 0 0</pose>
-      <attenuation>
-        <range>1000</range>
-        <constant>1</constant>
-        <linear>1</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-    </light>
-    <model name='ground_plane'>
-      <static>1</static>
-      <link name='link'>
-        <collision name='collision'>
-          <geometry>
-            <plane>
-              <size>100 100</size>
-              <normal>0 0 1</normal>
-            </plane>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='visual'>
-          <geometry>
-            <plane>
-              <size>100 100</size>
-              <normal>0 0 1</normal>
-            </plane>
-          </geometry>
-          <material>
-            <script>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-              <name>Gazebo/Grey</name>
-            </script>
-          </material>
-        </visual>
-        <kinematic>1</kinematic>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-      </link>
-    </model>
-    <model name='stone'>
-      <link name='link'>
-        <inertial>
-          <mass>0.05</mass>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0</iyz>
-            <izz>0.001</izz>
-          </inertia>
-        </inertial>
-        <collision name='collision'>
-          <geometry>
-            <box>
-              <size>0.02 0.032 0.064</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode>
-                <max_vel>0</max_vel>
-                <min_depth>0.003</min_depth>
-              </ode>
-            </contact>
-            <friction>
-              <ode>
-                <mu>1.13</mu>
-                <mu2>1.13</mu2>
-              </ode>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <bounce/>
-          </surface>
-        </collision>
-        <visual name='visual'>
-          <geometry>
-            <box>
-              <size>0.02 0.032 0.064</size>
-            </box>
-          </geometry>
-          <material>
-            <script>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-              <name>Gazebo/White</name>
-            </script>
-          </material>
-        </visual>
-        <self_collide>0</self_collide>
-        <kinematic>0</kinematic>
-        <enable_wind>0</enable_wind>
-      </link>
-    </model>
-    <gravity>0 0 -9.8</gravity>
-    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
-    <atmosphere type='adiabatic'/>
-    <physics name='default_physics' default='0' type='ode'>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <real_time_update_rate>1000</real_time_update_rate>
-    </physics>
-    <scene>
-      <ambient>0.4 0.4 0.4 1</ambient>
-      <background>0.7 0.7 0.7 1</background>
-      <shadows>1</shadows>
-    </scene>
-    <audio>
-      <device>default</device>
-    </audio>
-    <wind/>
-    <spherical_coordinates>
-      <surface_model>EARTH_WGS84</surface_model>
-      <latitude_deg>0</latitude_deg>
-      <longitude_deg>0</longitude_deg>
-      <elevation>0</elevation>
-      <heading_deg>0</heading_deg>
-    </spherical_coordinates>
-    <model name='place_tray'>
-      <pose frame=''>-0.045922 0.225094 0.5 0 -0 0</pose>
-      <link name='link'>
-        <inertial>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.166667</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.166667</iyy>
-            <iyz>0</iyz>
-            <izz>0.166667</izz>
-          </inertia>
-        </inertial>
-        <collision name='collision'>
-          <geometry>
-            <box>
-              <size>0.235035 0.144926 0.023348</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='visual'>
-          <geometry>
-            <box>
-              <size>0.235035 0.144926 0.023348</size>
-            </box>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Red</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-    </model>
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose frame=''>1.21935 -0.642908 0.9243 0 0.381797 2.69376</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
-    <model name='table'>
-      <static>1</static>
-      <pose frame=''>0 0 0.5 0 -0 0</pose>
-      <link name='link'>
-        <inertial>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.166667</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.166667</iyy>
-            <iyz>0</iyz>
-            <izz>0.166667</izz>
-          </inertia>
-        </inertial>
-        <collision name='collision'>
-          <geometry>
-            <box>
-              <size>3.05099 1.37801 2.56925</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='visual'>
-          <geometry>
-            <box>
-              <size>3.05099 1.37801 2.56925</size>
-            </box>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Grey</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-    </model>
-    <model name='pick_tray'>
-      <pose frame=''>0.009381 -0.218515 0.445328 -0.000167 -7.4e-05 -0.002119</pose>
-      <link name='link'>
-        <inertial>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.166667</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.166667</iyy>
-            <iyz>0</iyz>
-            <izz>0.166667</izz>
-          </inertia>
-        </inertial>
-        <collision name='collision'>
-          <geometry>
-            <box>
-              <size>0.235035 0.144926 0.023348</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='visual'>
-          <geometry>
-            <box>
-              <size>0.235035 0.144926 0.023348</size>
-            </box>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Blue</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-    </model>
-  </world>
-</sdf>
diff --git a/franka_gazebo/worlds/stone.world b/franka_gazebo/worlds/stone.world
new file mode 100644
index 000000000..bd6dbd307
--- /dev/null
+++ b/franka_gazebo/worlds/stone.world
@@ -0,0 +1,33 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <world name="panda_pick_and_place">
+    <!--A global light source-->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <!--A ground plane-->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <!--A Table-->
+    <include>
+      <uri>model://table</uri>
+      <pose>0.016428 0.000584 0.0 0 -0 0</pose>
+    </include>
+    <!--A pick tray-->
+    <include>
+      <uri>model://pick_tray</uri>
+      <pose>0.008578 -0.215704 0.419773 0 -0 -0</pose>
+    </include>
+    <!--A place tray-->
+    <include>
+      <uri>model://place_tray</uri>
+      <pose>0.004103 0.219654 0.419764 0 -0 -0</pose>
+    </include>
+    <!--A stone-->
+    <include>
+      <uri>model://stone</uri>
+      <pose>0.000286 -0.221972 0.475172 0 -0 -0</pose>
+    </include>
+  </world>
+</sdf>