From 1a6a80003199c4c4503c73480118450eb52f35fe Mon Sep 17 00:00:00 2001
From: Giang Nguyen <hoanggia@uni-bremen.de>
Date: Thu, 13 Oct 2022 22:32:46 +0200
Subject: [PATCH] Fix issues #106

---
 iai_apartment/CMakeLists.txt                  |    4 +-
 iai_apartment/launch/apartment_bringup.launch |   12 +-
 iai_apartment/rviz_config/rviz_config.rviz    |  547 ++--
 iai_apartment/urdf/apartment.urdf             | 2551 ++++++++++++++---
 iai_apartment/urdf/apartment.xacro            |   19 +
 iai_apartment/urdf/furnitures.xacro           |  454 +++
 iai_apartment/urdf/iai-apartment.urdf         | 1992 -------------
 .../urdf/{kitchen.xarco => kitchen.xacro}     |    0
 8 files changed, 2806 insertions(+), 2773 deletions(-)
 create mode 100644 iai_apartment/urdf/apartment.xacro
 create mode 100644 iai_apartment/urdf/furnitures.xacro
 delete mode 100644 iai_apartment/urdf/iai-apartment.urdf
 rename iai_apartment/urdf/{kitchen.xarco => kitchen.xacro} (100%)

diff --git a/iai_apartment/CMakeLists.txt b/iai_apartment/CMakeLists.txt
index 825064b..4783213 100644
--- a/iai_apartment/CMakeLists.txt
+++ b/iai_apartment/CMakeLists.txt
@@ -19,10 +19,10 @@ find_package(xacro REQUIRED)
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 # catkin_python_setup()
 
-xacro_add_xacro_file(urdf/apartment.urdf urdf/iai-apartment.urdf)
+xacro_add_xacro_file(urdf/apartment.xacro urdf/apartment.urdf)
 # add target to trigger xacro
 add_custom_target(xacro ALL DEPENDS
-  urdf/iai-apartment.urdf)
+  urdf/apartment.urdf)
 install(DIRECTORY xacro
   DESTINATION share/${PROJECT_NAME})
 
diff --git a/iai_apartment/launch/apartment_bringup.launch b/iai_apartment/launch/apartment_bringup.launch
index d528681..900a9d8 100644
--- a/iai_apartment/launch/apartment_bringup.launch
+++ b/iai_apartment/launch/apartment_bringup.launch
@@ -1,12 +1,8 @@
 <launch>
 
-  <node pkg="tf"
-        type="static_transform_publisher"
-        name="iai_apartment_root_link_broadcaster" 
-        args="0 0 0 0 0 0 map iai_apartment/apartment_root 25">
-  </node>
+  <node pkg="tf" type="static_transform_publisher" name="iai_apartment_root_link_broadcaster" args="0 0 0 0 0 0 map iai_apartment/apartment_root 25"></node>
 
-  <arg name="model" default="$(find iai_apartment)/urdf/apartment.urdf" />
+  <arg name="model" default="$(find iai_apartment)/urdf/apartment.xacro" />
   <param name="apartment_description" command="$(find xacro)/xacro '$(arg model)'" />
 
   <node name="apartment_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
@@ -17,8 +13,8 @@
   </node>
 
   <node pkg="joint_state_publisher" type="joint_state_publisher" name="apartment_joint_state_publisher" output="screen">
-    <remap from="robot_description" to="apartment_description"/>
-    <param name="rate" value="25"/>
+    <remap from="robot_description" to="apartment_description" />
+    <param name="rate" value="25" />
     <remap from="/joint_states" to="/apartment_joint_states" />
     <rosparam param="source_list">
       - /iai_kitchen/cram_joint_states
diff --git a/iai_apartment/rviz_config/rviz_config.rviz b/iai_apartment/rviz_config/rviz_config.rviz
index e4e185d..b6c9116 100644
--- a/iai_apartment/rviz_config/rviz_config.rviz
+++ b/iai_apartment/rviz_config/rviz_config.rviz
@@ -6,13 +6,8 @@ Panels:
       Expanded:
         - /Grid1/Offset1
         - /RobotModel1
-        - /TF1
-        - /TF1/Frames1
-        - /TF1/Tree1
-        - /TF1/Tree1/map1
-        - /TF1/Tree1/map1/iai_apartment/apartment_root1
       Splitter Ratio: 0.6592742204666138
-    Tree Height: 874
+    Tree Height: 797
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -43,7 +38,7 @@ Visualization Manager:
       Cell Size: 1
       Class: rviz/Grid
       Color: 255; 53; 127
-      Enabled: false
+      Enabled: true
       Line Style:
         Line Width: 0.029999999329447746
         Value: Billboards
@@ -56,10 +51,10 @@ Visualization Manager:
       Plane: XY
       Plane Cell Count: 20
       Reference Frame: <Fixed Frame>
-      Value: false
+      Value: true
     - Alpha: 1
       Class: rviz/RobotModel
-      Collision Enabled: true
+      Collision Enabled: false
       Enabled: true
       Links:
         All Links Enabled: true
@@ -263,6 +258,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        cabinet7_drawer_middle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         cabinet8:
           Alpha: 1
           Show Axes: false
@@ -298,11 +298,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        coffe_machine:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         coffee_table:
           Alpha: 1
           Show Axes: false
@@ -323,11 +318,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        dishwasher_drawer_middle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         handle_cab10_b:
           Alpha: 1
           Show Axes: false
@@ -453,12 +443,12 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        island_back:
+        island:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        island_countertop:
+        island_back:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -481,11 +471,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        oven:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         side_A:
           Alpha: 1
           Show Axes: false
@@ -519,20 +504,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        tap_body:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        tap_handle:
+        tap:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        tap_handle_open:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         useless_underbar_a:
           Alpha: 1
           Show Axes: false
@@ -598,221 +574,211 @@ Visualization Manager:
       Enabled: true
       Frame Timeout: 15
       Frames:
-        All Enabled: false
+        All Enabled: true
         iai_apartment/apartment_root:
-          Value: false
+          Value: true
         iai_apartment/armchair:
-          Value: false
+          Value: true
         iai_apartment/bedside_table:
-          Value: false
+          Value: true
         iai_apartment/cabinet1:
-          Value: false
+          Value: true
         iai_apartment/cabinet10:
-          Value: false
+          Value: true
         iai_apartment/cabinet10_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet10_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/cabinet10_drawer_top:
-          Value: false
+          Value: true
         iai_apartment/cabinet11:
-          Value: false
+          Value: true
         iai_apartment/cabinet11_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet11_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/cabinet11_drawer_top:
-          Value: false
+          Value: true
         iai_apartment/cabinet1_coloksu_level4:
-          Value: false
+          Value: true
         iai_apartment/cabinet1_coloksu_level5:
-          Value: false
+          Value: true
         iai_apartment/cabinet1_door_top_left:
-          Value: false
+          Value: true
         iai_apartment/cabinet1_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet1_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/cabinet2:
-          Value: false
+          Value: true
         iai_apartment/cabinet2_door_left:
-          Value: false
+          Value: true
         iai_apartment/cabinet2_door_out_fancy:
-          Value: false
+          Value: true
         iai_apartment/cabinet2_drawer_big:
-          Value: false
+          Value: true
         iai_apartment/cabinet2_drawer_small:
-          Value: false
+          Value: true
         iai_apartment/cabinet3:
-          Value: false
+          Value: true
         iai_apartment/cabinet3_door_bottom_left:
-          Value: false
+          Value: true
         iai_apartment/cabinet3_door_bottom_out_fancy:
-          Value: false
+          Value: true
         iai_apartment/cabinet3_door_top_left:
-          Value: false
+          Value: true
         iai_apartment/cabinet3_door_top_out_fancy:
-          Value: false
+          Value: true
         iai_apartment/cabinet4:
-          Value: false
+          Value: true
         iai_apartment/cabinet4_door_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet4_door_top:
-          Value: false
+          Value: true
         iai_apartment/cabinet5:
-          Value: false
+          Value: true
         iai_apartment/cabinet5_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet5_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/cabinet5_drawer_top:
-          Value: false
+          Value: true
         iai_apartment/cabinet6:
-          Value: false
+          Value: true
         iai_apartment/cabinet6_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet6_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/cabinet6_drawer_top:
-          Value: false
+          Value: true
         iai_apartment/cabinet7:
-          Value: false
+          Value: true
         iai_apartment/cabinet7_door_bottom_left:
-          Value: false
+          Value: true
+        iai_apartment/cabinet7_drawer_middle:
+          Value: true
         iai_apartment/cabinet8:
-          Value: false
+          Value: true
         iai_apartment/cabinet8_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet8_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/cabinet9:
-          Value: false
+          Value: true
         iai_apartment/cabinet9_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/cabinet9_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/cabinet9_drawer_top:
-          Value: false
-        iai_apartment/coffe_machine:
-          Value: false
+          Value: true
         iai_apartment/coffee_table:
-          Value: false
+          Value: true
         iai_apartment/coffee_table_drawer:
-          Value: false
+          Value: true
         iai_apartment/cooktop:
-          Value: false
+          Value: true
         iai_apartment/countertop:
-          Value: false
-        iai_apartment/dishwasher_drawer_middle:
-          Value: false
+          Value: true
         iai_apartment/handle_cab10_b:
-          Value: false
+          Value: true
         iai_apartment/handle_cab10_m:
-          Value: false
+          Value: true
         iai_apartment/handle_cab10_t:
-          Value: false
+          Value: true
         iai_apartment/handle_cab11_b:
-          Value: false
+          Value: true
         iai_apartment/handle_cab11_m:
-          Value: false
+          Value: true
         iai_apartment/handle_cab11_t:
-          Value: false
+          Value: true
         iai_apartment/handle_cab1_drawer_bottom:
-          Value: false
+          Value: true
         iai_apartment/handle_cab1_drawer_mid:
-          Value: false
+          Value: true
         iai_apartment/handle_cab1_top_door:
-          Value: false
+          Value: true
         iai_apartment/handle_cab2_door:
-          Value: false
+          Value: true
         iai_apartment/handle_cab3_door_bottom:
-          Value: false
+          Value: true
         iai_apartment/handle_cab3_door_top:
-          Value: false
+          Value: true
         iai_apartment/handle_cab4_door_bottom:
-          Value: false
+          Value: true
         iai_apartment/handle_cab5_b:
-          Value: false
+          Value: true
         iai_apartment/handle_cab5_m:
-          Value: false
+          Value: true
         iai_apartment/handle_cab5_t:
-          Value: false
+          Value: true
         iai_apartment/handle_cab6_b:
-          Value: false
+          Value: true
         iai_apartment/handle_cab6_m:
-          Value: false
+          Value: true
         iai_apartment/handle_cab6_t:
-          Value: false
+          Value: true
         iai_apartment/handle_cab7:
-          Value: false
+          Value: true
         iai_apartment/handle_cab7_middle:
-          Value: false
+          Value: true
         iai_apartment/handle_cab8:
-          Value: false
+          Value: true
         iai_apartment/handle_cab9_b:
-          Value: false
+          Value: true
         iai_apartment/handle_cab9_m:
-          Value: false
+          Value: true
         iai_apartment/handle_cab9_t:
-          Value: false
+          Value: true
+        iai_apartment/island:
+          Value: true
         iai_apartment/island_back:
-          Value: false
-        iai_apartment/island_countertop:
-          Value: false
+          Value: true
         iai_apartment/island_waterfall_left:
-          Value: false
+          Value: true
         iai_apartment/island_waterfall_right:
-          Value: false
+          Value: true
         iai_apartment/kitchen_root:
-          Value: false
+          Value: true
         iai_apartment/middle_of_the_room:
-          Value: false
-        iai_apartment/oven:
-          Value: false
+          Value: true
         iai_apartment/side_A:
-          Value: false
+          Value: true
         iai_apartment/side_A_left:
-          Value: false
+          Value: true
         iai_apartment/side_A_rb:
-          Value: false
+          Value: true
         iai_apartment/side_A_rt:
-          Value: false
+          Value: true
         iai_apartment/side_B:
-          Value: false
+          Value: true
         iai_apartment/sink:
-          Value: false
+          Value: true
         iai_apartment/sofa:
-          Value: false
-        iai_apartment/tap_body:
-          Value: false
-        iai_apartment/tap_handle:
           Value: true
-        iai_apartment/tap_handle_open:
+        iai_apartment/tap:
           Value: true
         iai_apartment/useless_underbar_a:
-          Value: false
+          Value: true
         iai_apartment/useless_underbar_b:
-          Value: false
+          Value: true
         iai_apartment/wall_coloksu_wall1:
-          Value: false
+          Value: true
         iai_apartment/wall_coloksu_wall2:
-          Value: false
+          Value: true
         iai_apartment/wall_coloksu_wall3:
-          Value: false
+          Value: true
         iai_apartment/wall_coloksu_wall4:
-          Value: false
+          Value: true
         iai_apartment/walls:
-          Value: false
+          Value: true
         iai_apartment/wardrobe:
-          Value: false
+          Value: true
         iai_apartment/wardrobe_door_left:
-          Value: false
+          Value: true
         iai_apartment/wardrobe_door_right:
-          Value: false
+          Value: true
         iai_apartment/windows:
-          Value: false
-        map:
-          Value: false
+          Value: true
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -820,167 +786,108 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        map:
-          iai_apartment/apartment_root:
-            iai_apartment/kitchen_root:
-              iai_apartment/side_A:
-                iai_apartment/cabinet1:
-                  iai_apartment/cabinet1_coloksu_level4:
-                    {}
-                  iai_apartment/cabinet1_coloksu_level5:
-                    {}
-                  iai_apartment/cabinet1_door_top_left:
-                    iai_apartment/handle_cab1_top_door:
-                      {}
-                  iai_apartment/cabinet1_drawer_bottom:
-                    iai_apartment/handle_cab1_drawer_bottom:
-                      {}
-                  iai_apartment/cabinet1_drawer_middle:
-                    iai_apartment/handle_cab1_drawer_mid:
-                      {}
-                  iai_apartment/oven:
-                    {}
-                iai_apartment/cabinet2:
-                  iai_apartment/cabinet2_door_out_fancy:
-                    iai_apartment/cabinet2_door_left:
-                      iai_apartment/handle_cab2_door:
-                        {}
-                  iai_apartment/cabinet2_drawer_big:
-                    {}
-                  iai_apartment/cabinet2_drawer_small:
-                    {}
-                iai_apartment/cabinet3:
-                  iai_apartment/cabinet3_door_bottom_out_fancy:
-                    iai_apartment/cabinet3_door_bottom_left:
-                      iai_apartment/handle_cab3_door_bottom:
-                        {}
-                  iai_apartment/cabinet3_door_top_out_fancy:
-                    iai_apartment/cabinet3_door_top_left:
-                      iai_apartment/handle_cab3_door_top:
-                        {}
-                iai_apartment/cabinet4:
-                  iai_apartment/cabinet4_door_top:
-                    iai_apartment/cabinet4_door_bottom:
-                      iai_apartment/handle_cab4_door_bottom:
-                        {}
-                iai_apartment/cabinet5:
-                  iai_apartment/cabinet5_drawer_bottom:
-                    iai_apartment/handle_cab5_b:
-                      {}
-                  iai_apartment/cabinet5_drawer_middle:
-                    iai_apartment/handle_cab5_m:
-                      {}
-                  iai_apartment/cabinet5_drawer_top:
-                    iai_apartment/handle_cab5_t:
-                      {}
-                  iai_apartment/countertop:
-                    iai_apartment/coffe_machine:
-                      {}
-                iai_apartment/cabinet6:
-                  iai_apartment/cabinet6_drawer_bottom:
-                    iai_apartment/handle_cab6_b:
-                      {}
-                  iai_apartment/cabinet6_drawer_middle:
-                    iai_apartment/handle_cab6_m:
-                      {}
-                  iai_apartment/cabinet6_drawer_top:
-                    iai_apartment/handle_cab6_t:
-                      {}
-                iai_apartment/side_A_left:
-                  {}
-                iai_apartment/side_A_rb:
-                  {}
-                iai_apartment/side_A_rt:
-                  {}
-                iai_apartment/useless_underbar_a:
-                  {}
-              iai_apartment/side_B:
-                iai_apartment/cabinet10:
-                  iai_apartment/cabinet10_drawer_bottom:
-                    iai_apartment/handle_cab10_b:
-                      {}
-                  iai_apartment/cabinet10_drawer_middle:
-                    iai_apartment/handle_cab10_m:
-                      {}
-                  iai_apartment/cabinet10_drawer_top:
-                    iai_apartment/handle_cab10_t:
-                      {}
-                iai_apartment/cabinet11:
-                  iai_apartment/cabinet11_drawer_bottom:
-                    iai_apartment/handle_cab11_b:
-                      {}
-                  iai_apartment/cabinet11_drawer_middle:
-                    iai_apartment/handle_cab11_m:
-                      {}
-                  iai_apartment/cabinet11_drawer_top:
-                    iai_apartment/handle_cab11_t:
-                      {}
-                iai_apartment/cabinet7:
-                  iai_apartment/cabinet7_door_bottom_left:
-                    iai_apartment/handle_cab7:
-                      {}
-                  iai_apartment/dishwasher_drawer_middle:
-                    iai_apartment/handle_cab7_middle:
-                      {}
-                iai_apartment/cabinet8:
-                  iai_apartment/cabinet8_drawer_bottom:
-                    iai_apartment/handle_cab8:
-                      {}
-                  iai_apartment/cabinet8_drawer_middle:
-                    {}
-                iai_apartment/cabinet9:
-                  iai_apartment/cabinet9_drawer_bottom:
-                    iai_apartment/handle_cab9_b:
-                      {}
-                  iai_apartment/cabinet9_drawer_middle:
-                    iai_apartment/handle_cab9_m:
-                      {}
-                  iai_apartment/cabinet9_drawer_top:
-                    iai_apartment/handle_cab9_t:
-                      {}
-                iai_apartment/cooktop:
-                  {}
-                iai_apartment/island_back:
-                  {}
-                iai_apartment/island_countertop:
-                  iai_apartment/sink:
-                    iai_apartment/tap_body:
-                      iai_apartment/tap_handle_open:
-                        iai_apartment/tap_handle:
-                          {}
-                iai_apartment/island_waterfall_left:
-                  {}
-                iai_apartment/island_waterfall_right:
-                  {}
-                iai_apartment/useless_underbar_b:
-                  {}
-            iai_apartment/middle_of_the_room:
-              iai_apartment/armchair:
-                {}
-              iai_apartment/bedside_table:
-                {}
-              iai_apartment/coffee_table:
-                iai_apartment/coffee_table_drawer:
-                  {}
-              iai_apartment/sofa:
-                {}
-              iai_apartment/wall_coloksu_wall1:
-                {}
-              iai_apartment/wall_coloksu_wall2:
-                {}
-              iai_apartment/wall_coloksu_wall3:
-                {}
-              iai_apartment/wall_coloksu_wall4:
-                {}
-              iai_apartment/walls:
-                {}
-              iai_apartment/wardrobe:
-                iai_apartment/wardrobe_door_left:
-                  {}
-                iai_apartment/wardrobe_door_right:
-                  {}
-              iai_apartment/windows:
-                {}
+        iai_apartment/apartment_root:
+          iai_apartment/kitchen_root:
+            iai_apartment/side_A:
+              iai_apartment/cabinet1:
+                iai_apartment/cabinet1_coloksu_level4: {}
+                iai_apartment/cabinet1_coloksu_level5: {}
+                iai_apartment/cabinet1_door_top_left:
+                  iai_apartment/handle_cab1_top_door: {}
+                iai_apartment/cabinet1_drawer_bottom:
+                  iai_apartment/handle_cab1_drawer_bottom: {}
+                iai_apartment/cabinet1_drawer_middle:
+                  iai_apartment/handle_cab1_drawer_mid: {}
+              iai_apartment/cabinet2:
+                iai_apartment/cabinet2_door_out_fancy:
+                  iai_apartment/cabinet2_door_left:
+                    iai_apartment/handle_cab2_door: {}
+                iai_apartment/cabinet2_drawer_big: {}
+                iai_apartment/cabinet2_drawer_small: {}
+              iai_apartment/cabinet3:
+                iai_apartment/cabinet3_door_bottom_out_fancy:
+                  iai_apartment/cabinet3_door_bottom_left:
+                    iai_apartment/handle_cab3_door_bottom: {}
+                iai_apartment/cabinet3_door_top_out_fancy:
+                  iai_apartment/cabinet3_door_top_left:
+                    iai_apartment/handle_cab3_door_top: {}
+              iai_apartment/cabinet4:
+                iai_apartment/cabinet4_door_top:
+                  iai_apartment/cabinet4_door_bottom:
+                    iai_apartment/handle_cab4_door_bottom: {}
+              iai_apartment/cabinet5:
+                iai_apartment/cabinet5_drawer_bottom:
+                  iai_apartment/handle_cab5_b: {}
+                iai_apartment/cabinet5_drawer_middle:
+                  iai_apartment/handle_cab5_m: {}
+                iai_apartment/cabinet5_drawer_top:
+                  iai_apartment/handle_cab5_t: {}
+                iai_apartment/countertop: {}
+              iai_apartment/cabinet6:
+                iai_apartment/cabinet6_drawer_bottom:
+                  iai_apartment/handle_cab6_b: {}
+                iai_apartment/cabinet6_drawer_middle:
+                  iai_apartment/handle_cab6_m: {}
+                iai_apartment/cabinet6_drawer_top:
+                  iai_apartment/handle_cab6_t: {}
+              iai_apartment/side_A_left: {}
+              iai_apartment/side_A_rb: {}
+              iai_apartment/side_A_rt: {}
+              iai_apartment/useless_underbar_a: {}
+            iai_apartment/side_B:
+              iai_apartment/cabinet10:
+                iai_apartment/cabinet10_drawer_bottom:
+                  iai_apartment/handle_cab10_b: {}
+                iai_apartment/cabinet10_drawer_middle:
+                  iai_apartment/handle_cab10_m: {}
+                iai_apartment/cabinet10_drawer_top:
+                  iai_apartment/handle_cab10_t: {}
+              iai_apartment/cabinet11:
+                iai_apartment/cabinet11_drawer_bottom:
+                  iai_apartment/handle_cab11_b: {}
+                iai_apartment/cabinet11_drawer_middle:
+                  iai_apartment/handle_cab11_m: {}
+                iai_apartment/cabinet11_drawer_top:
+                  iai_apartment/handle_cab11_t: {}
+              iai_apartment/cabinet7:
+                iai_apartment/cabinet7_door_bottom_left:
+                  iai_apartment/handle_cab7: {}
+                iai_apartment/cabinet7_drawer_middle:
+                  iai_apartment/handle_cab7_middle: {}
+              iai_apartment/cabinet8:
+                iai_apartment/cabinet8_drawer_bottom:
+                  iai_apartment/handle_cab8: {}
+                iai_apartment/cabinet8_drawer_middle: {}
+              iai_apartment/cabinet9:
+                iai_apartment/cabinet9_drawer_bottom:
+                  iai_apartment/handle_cab9_b: {}
+                iai_apartment/cabinet9_drawer_middle:
+                  iai_apartment/handle_cab9_m: {}
+                iai_apartment/cabinet9_drawer_top:
+                  iai_apartment/handle_cab9_t: {}
+              iai_apartment/cooktop: {}
+              iai_apartment/island: {}
+              iai_apartment/island_back: {}
+              iai_apartment/island_waterfall_left: {}
+              iai_apartment/island_waterfall_right: {}
+              iai_apartment/sink: {}
+              iai_apartment/tap: {}
+              iai_apartment/useless_underbar_b: {}
+          iai_apartment/middle_of_the_room:
+            iai_apartment/armchair: {}
+            iai_apartment/bedside_table: {}
+            iai_apartment/coffee_table:
+              iai_apartment/coffee_table_drawer: {}
+            iai_apartment/sofa: {}
+            iai_apartment/wall_coloksu_wall1: {}
+            iai_apartment/wall_coloksu_wall2: {}
+            iai_apartment/wall_coloksu_wall3: {}
+            iai_apartment/wall_coloksu_wall4: {}
+            iai_apartment/walls: {}
+            iai_apartment/wardrobe:
+              iai_apartment/wardrobe_door_left: {}
+              iai_apartment/wardrobe_door_right: {}
+            iai_apartment/windows: {}
       Update Interval: 0
       Value: true
   Enabled: true
@@ -1011,7 +918,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.8237718343734741
+      Distance: 16.023786544799805
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -1019,25 +926,25 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 3.2305526733398438
-        Y: 3.0342557430267334
-        Z: 0.5103647112846375
+        X: 1.6595083475112915
+        Y: -0.08021938800811768
+        Z: -0.4785533547401428
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7547972202301025
+      Pitch: 0.4547962546348572
       Target Frame: <Fixed Frame>
-      Yaw: 2.5903995037078857
+      Yaw: 0.565395712852478
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1163
+  Height: 1086
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f200000431fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000431000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000003e4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003e4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000060fc0100000002fb0000000800540069006d0065000000000000000780000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005880000043100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f2000003e4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003e4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000003e4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003e4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000060fc0100000002fb0000000800540069006d0065000000000000000780000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000588000003e400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -1048,4 +955,4 @@ Window Geometry:
     collapsed: false
   Width: 1920
   X: 0
-  Y: 0
+  Y: 27
diff --git a/iai_apartment/urdf/apartment.urdf b/iai_apartment/urdf/apartment.urdf
index 510159b..654a7f6 100644
--- a/iai_apartment/urdf/apartment.urdf
+++ b/iai_apartment/urdf/apartment.urdf
@@ -1,462 +1,2111 @@
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="apartment">
-
-
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /home/giang/Workspace/map_ws/src/iai_maps/iai_apartment/urdf/apartment.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="apartment">
   <link name="apartment_root">
     <origin rpy="0 0 0" xyz="0 0 0"/>
   </link>
-
-  <joint name="middle_of_the_room_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="8.85 1.75 0.0"/>
+  <joint name="furnitures_base" type="fixed">
+    <origin xyz="8.85 1.75 0.0"/>
     <parent link="apartment_root"/>
-    <child link="middle_of_the_room"/>
+    <child link="furnitures_root"/>
   </joint>
-  <link name="middle_of_the_room"/>
-
+  <link name="furnitures_root"/>
   <!-- things and stuff -->
-    <joint name="walls_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 -0.2"/>
-      <parent link="middle_of_the_room"/>
-      <child link="walls"/>
-    </joint>
-    <link name="walls">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/walls.dae"/>
-        </geometry>
-      </visual>
-      <!--collision>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/collision/walls.stl" />
-        </geometry>
-      </collision-->
-    </link>
-    <joint name="windows_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 -0.2"/>
-      <parent link="middle_of_the_room"/>
-      <child link="windows"/>
-    </joint>
-    <link name="windows">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/windows.dae"/>
-        </geometry>
-      </visual>
-      <!--collision>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/collision/windows.obj"/>
-        </geometry>
-      </collision-->
-    </link>
-
-    <!-- Collision okay Walls -->
-    <!-- From the door side being wall 1, turn right and count the walls. -->
-      <joint name="wall_coloksu_wall1_joint" type="fixed">
-        <origin xyz="0.6 -1.615 1.53" rpy="0 0 -1.57"/>
-        <parent link="middle_of_the_room"/>
-        <child link="wall_coloksu_wall1"/>
-      </joint>
-      <link name="wall_coloksu_wall1">
-        <collision>
-          <origin rpy="0 1.57 0"/>
-          <geometry>
-            <box size="3.05 18.95 0.003"/>
-          </geometry>
-        </collision>
-      </link>
-
-      <joint name="wall_coloksu_wall2_joint" type="fixed">
-        <origin xyz="-8.855 0.782 1.53" rpy="0 0 3.14"/>
-        <parent link="middle_of_the_room"/>
-        <child link="wall_coloksu_wall2"/>
-      </joint>
-      <link name="wall_coloksu_wall2">
-        <collision>
-          <origin rpy="0 1.57 0"/>
-          <geometry>
-            <box size="3.05 4.808 0.003"/>
-          </geometry>
-        </collision>
-      </link>
-
-      <joint name="wall_coloksu_wall3_joint" type="fixed">
-        <origin xyz="0.6 3.180 1.53" rpy="0 0 1.57"/>
-        <parent link="middle_of_the_room"/>
-        <child link="wall_coloksu_wall3"/>
-      </joint>
-      <link name="wall_coloksu_wall3">
-        <collision>
-          <origin rpy="0 1.57 0"/>
+  <joint name="walls_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 -0.2"/>
+    <parent link="furnitures_root"/>
+    <child link="walls"/>
+  </joint>
+  <link name="walls">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/walls.dae"/>
+      </geometry>
+    </visual>
+    <!--collision>
           <geometry>
-            <box size="3.05 18.95 0.003"/>
+            <mesh filename="package://iai_apartment/meshes/collision/walls.stl" />
           </geometry>
-        </collision>
-      </link>
-
-      <joint name="wall_coloksu_wall4_joint" type="fixed">
-        <origin xyz="10.069 0.782 1.53" rpy="0 0 0"/>
-        <parent link="middle_of_the_room"/>
-        <child link="wall_coloksu_wall4"/>
-      </joint>
-      <link name="wall_coloksu_wall4">
-        <collision>
-          <origin rpy="0 1.57 0"/>
+        </collision-->
+  </link>
+  <joint name="windows_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 -0.2"/>
+    <parent link="furnitures_root"/>
+    <child link="windows"/>
+  </joint>
+  <link name="windows">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/windows.dae"/>
+      </geometry>
+    </visual>
+    <!--collision>
           <geometry>
-            <box size="3.05 4.808 0.003"/>
+            <mesh filename="package://iai_apartment/meshes/collision/windows.obj"/>
           </geometry>
-        </collision>
-      </link>
-
-    <joint name="wardrobe_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="9.7082 -0.6331 0.0"/>
-      <parent link="middle_of_the_room"/>
-      <child link="wardrobe"/>
-    </joint>
-    <link name="wardrobe">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/wardrobe.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.2693 -0.4482 0.0750"/>
-        <geometry>
-          <box size="0.08 0.08 0.15"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.10603 -0.4482 0.0750"/>
-        <geometry>
-          <box size="0.08 0.08 0.15"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.2693 0.4482 0.0750"/>
-        <geometry>
-          <box size="0.08 0.08 0.15"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.10603 0.4482 0.0750"/>
-        <geometry>
-          <box size="0.08 0.08 0.15"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.0805 0.0000 0.16345"/>
-        <geometry>
-          <box size="0.487 1.0000 0.0269"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.0805 0.48965 0.975"/>
-        <geometry>
-          <box size="0.487 0.0207 1.65"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.0896 0.0000 0.975"/>
-        <geometry>
-          <box size="0.4318 0.018568 1.65"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.0805 -0.48965 0.975"/>
-        <geometry>
-          <box size="0.487 0.0207 1.65"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975"/>
-        <geometry>
-          <box size="0.0367 1.0000 1.65"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975"/>
-        <geometry>
-          <box size="0.0367 1.0000 1.65"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.0805 0.0000 1.7865"/>
-        <geometry>
-          <box size="0.487 1.0000 0.027"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.5727"/>
-        <geometry>
-          <box size="0.4299 0.469957 0.0118"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.974836"/>
-        <geometry>
-          <box size="0.4299 0.469957 0.0118"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.08861 -0.244343 1.37686"/>
-        <geometry>
-          <box size="0.4299 0.469957 0.0118"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="1.5708 0 0" xyz="-0.086923 0.244373 1.66898"/>
-        <geometry>
-          <cylinder radius="0.0075" length="0.469897"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="wardrobe_door_left_joint" type="revolute">
-      <origin rpy="0 0 0" xyz="-0.3246 0.5 0.0000"/>
-      <parent link="wardrobe"/>
-      <child link="wardrobe_door_left"/>
-      <axis xyz="0 0 -1"/>
-      <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-    </joint>
-    <link name="wardrobe_door_left">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_left.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.006435 -0.2485 0.975"/>
-        <geometry>
-          <box size="0.012967 0.495438 1.64844"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="wardrobe_door_left_handle_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="-0.032089 -0.460513 0.973703"/>
-      <parent link="wardrobe_door_left"/>
-      <child link="wardrobe_door_left_handle"/>
-    </joint>
-    <link name="wardrobe_door_left_handle">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_handle.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.010421 0 0.059003"/>
-        <geometry>
-          <box size="0.033556 0.015082 0.014587"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.0136 0 0"/>
-        <geometry>
-          <box size="0.014486 0.015082 0.13"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.010421 0 -0.059003"/>
-        <geometry>
-          <box size="0.033556 0.015082 0.014587"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="wardrobe_door_right_joint" type="revolute">
-      <origin rpy="0 0 0" xyz="-0.3246 -0.5 0.0000"/>
-      <parent link="wardrobe"/>
-      <child link="wardrobe_door_right"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-    </joint>
-    <link name="wardrobe_door_right">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_right.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.006435 0.2485 0.975"/>
-        <geometry>
-          <box size="0.012967 0.495438 1.64844"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="wardrobe_door_right_handle_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="-0.032089 0.460513 0.973703"/>
-      <parent link="wardrobe_door_right"/>
-      <child link="wardrobe_door_right_handle"/>
-    </joint>
-    <link name="wardrobe_door_right_handle">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_handle.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.010421 0 0.059003"/>
-        <geometry>
-          <box size="0.033556 0.015082 0.014587"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.0136 0 0"/>
-        <geometry>
-          <box size="0.014486 0.015082 0.13"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.010421 0 -0.059003"/>
-        <geometry>
-          <box size="0.033556 0.015082 0.014587"/>
-        </geometry>
-      </collision>
-    </link>
-
-    <joint name="armchair_joint" type="fixed">
-      <origin rpy="0 0 1.9199" xyz="7.9688 -0.3791 0.0"/>
-      <parent link="middle_of_the_room"/>
-      <child link="armchair"/>
-    </joint>
-    <link name="armchair">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/armchair.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/collision/armchair.obj"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="sofa_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="8.1986 2.4096 0.0"/>
-      <parent link="middle_of_the_room"/>
-      <child link="sofa"/>
-    </joint>
-    <link name="sofa">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/sofa.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/collision/sofa.obj"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="coffee_table_joint" type="fixed">
-      <origin rpy="0 0 1.5708" xyz="7.8034 1.02957 0.0"/>
-      <parent link="middle_of_the_room"/>
-      <child link="coffee_table"/>
-    </joint>
-    <link name="coffee_table">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/coffee_table.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0000 0.0000 0.02977"/>
-        <geometry>
-          <box size="0.58 1.08 0.05954"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.022101 0.0000 0.06552"/>
-        <geometry>
-          <box size="0.6106 0.4 0.01196"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0000 0.39975 0.220625"/>
-        <geometry>
-          <box size="0.7 0.4 0.32"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0000 -0.39975 0.220625"/>
-        <geometry>
-          <box size="0.7 0.4 0.32"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.316343 0.0000 0.22"/>
-        <geometry>
-          <box size="0.0673 0.4 0.32"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0000 0.0000 0.39"/>
-        <geometry>
-          <box size="0.7 1.2 0.02"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="coffee_table_drawer_joint" type="prismatic">
-      <parent link="coffee_table"/>
-      <child link="coffee_table_drawer"/>
-      <axis xyz="-1 0 0"/>
-      <limit effort="1000.0" lower="0.0" upper="0.5" velocity="0.5"/>
-    </joint>
-    <link name="coffee_table_drawer">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/coffee_table_drawer.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.338806 0.0000 0.21892"/>
-        <geometry>
-          <box size="0.0214 0.396 0.31569"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.051066 0.0000 0.107979"/>
-        <geometry>
-          <box size="0.5521 0.3454 0.02144"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.207974 0.0000 0.238907"/>
-        <geometry>
-          <box size="0.0214 0.3486 0.2398"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.051066 -0.186206 0.226597"/>
-        <geometry>
-          <box size="0.5521 0.0214 0.26425"/>
-        </geometry>
-      </collision>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.051066 0.186206 0.226597"/>
-        <geometry>
-          <box size="0.5521 0.0214 0.26425"/>
-        </geometry>
-      </collision>
-    </link>
-    <joint name="bedside_table_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="7.2110 0.1296 0.0"/>
-      <parent link="middle_of_the_room"/>
-      <child link="bedside_table"/>
-    </joint>
-    <link name="bedside_table">
-      <visual>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/visual/bedside_table.dae"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0000 0.0000 0.230"/>
-        <geometry>
-          <box size="0.312 0.312 0.460"/>
-        </geometry>
-      </collision>
-    </link>
-
-  <!-- kitchen, kitchen on the walls -->
-    <xacro:include filename="$(find iai_apartment)/urdf/kitchen.xarco"/>
-    <xacro:kitchen_macro name="kitchen_base" parent="apartment_root">
-      <origin xyz="-0.03 0.75 0.005"/>
-    </xacro:kitchen_macro>
-
+        </collision-->
+  </link>
+  <!-- Collision okay Walls -->
+  <!-- From the door side being wall 1, turn right and count the walls. -->
+  <joint name="wall_coloksu_wall1_joint" type="fixed">
+    <origin rpy="0 0 -1.57" xyz="0.6 -1.615 1.53"/>
+    <parent link="furnitures_root"/>
+    <child link="wall_coloksu_wall1"/>
+  </joint>
+  <link name="wall_coloksu_wall1">
+    <collision>
+      <origin rpy="0 1.57 0"/>
+      <geometry>
+        <box size="3.05 18.95 0.003"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wall_coloksu_wall2_joint" type="fixed">
+    <origin rpy="0 0 3.14" xyz="-8.855 0.782 1.53"/>
+    <parent link="furnitures_root"/>
+    <child link="wall_coloksu_wall2"/>
+  </joint>
+  <link name="wall_coloksu_wall2">
+    <collision>
+      <origin rpy="0 1.57 0"/>
+      <geometry>
+        <box size="3.05 4.808 0.003"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wall_coloksu_wall3_joint" type="fixed">
+    <origin rpy="0 0 1.57" xyz="0.6 3.180 1.53"/>
+    <parent link="furnitures_root"/>
+    <child link="wall_coloksu_wall3"/>
+  </joint>
+  <link name="wall_coloksu_wall3">
+    <collision>
+      <origin rpy="0 1.57 0"/>
+      <geometry>
+        <box size="3.05 18.95 0.003"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wall_coloksu_wall4_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="10.069 0.782 1.53"/>
+    <parent link="furnitures_root"/>
+    <child link="wall_coloksu_wall4"/>
+  </joint>
+  <link name="wall_coloksu_wall4">
+    <collision>
+      <origin rpy="0 1.57 0"/>
+      <geometry>
+        <box size="3.05 4.808 0.003"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wardrobe_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="9.7082 -0.6331 0.0"/>
+    <parent link="furnitures_root"/>
+    <child link="wardrobe"/>
+  </joint>
+  <link name="wardrobe">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/wardrobe.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.2693 -0.4482 0.0750"/>
+      <geometry>
+        <box size="0.08 0.08 0.15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.10603 -0.4482 0.0750"/>
+      <geometry>
+        <box size="0.08 0.08 0.15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.2693 0.4482 0.0750"/>
+      <geometry>
+        <box size="0.08 0.08 0.15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.10603 0.4482 0.0750"/>
+      <geometry>
+        <box size="0.08 0.08 0.15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0805 0.0000 0.16345"/>
+      <geometry>
+        <box size="0.487 1.0000 0.0269"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0805 0.48965 0.975"/>
+      <geometry>
+        <box size="0.487 0.0207 1.65"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0896 0.0000 0.975"/>
+      <geometry>
+        <box size="0.4318 0.018568 1.65"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0805 -0.48965 0.975"/>
+      <geometry>
+        <box size="0.487 0.0207 1.65"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975"/>
+      <geometry>
+        <box size="0.0367 1.0000 1.65"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975"/>
+      <geometry>
+        <box size="0.0367 1.0000 1.65"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0805 0.0000 1.7865"/>
+      <geometry>
+        <box size="0.487 1.0000 0.027"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.5727"/>
+      <geometry>
+        <box size="0.4299 0.469957 0.0118"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.974836"/>
+      <geometry>
+        <box size="0.4299 0.469957 0.0118"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.08861 -0.244343 1.37686"/>
+      <geometry>
+        <box size="0.4299 0.469957 0.0118"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="1.5708 0 0" xyz="-0.086923 0.244373 1.66898"/>
+      <geometry>
+        <cylinder length="0.469897" radius="0.0075"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wardrobe_door_left_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="-0.3246 0.5 0.0000"/>
+    <parent link="wardrobe"/>
+    <child link="wardrobe_door_left"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="wardrobe_door_left">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_left.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.006435 -0.2485 0.975"/>
+      <geometry>
+        <box size="0.012967 0.495438 1.64844"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wardrobe_door_left_handle_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.032089 -0.460513 0.973703"/>
+    <parent link="wardrobe_door_left"/>
+    <child link="wardrobe_door_left_handle"/>
+  </joint>
+  <link name="wardrobe_door_left_handle">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_handle.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.010421 0 0.059003"/>
+      <geometry>
+        <box size="0.033556 0.015082 0.014587"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0136 0 0"/>
+      <geometry>
+        <box size="0.014486 0.015082 0.13"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.010421 0 -0.059003"/>
+      <geometry>
+        <box size="0.033556 0.015082 0.014587"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wardrobe_door_right_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="-0.3246 -0.5 0.0000"/>
+    <parent link="wardrobe"/>
+    <child link="wardrobe_door_right"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="wardrobe_door_right">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_right.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.006435 0.2485 0.975"/>
+      <geometry>
+        <box size="0.012967 0.495438 1.64844"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wardrobe_door_right_handle_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.032089 0.460513 0.973703"/>
+    <parent link="wardrobe_door_right"/>
+    <child link="wardrobe_door_right_handle"/>
+  </joint>
+  <link name="wardrobe_door_right_handle">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_handle.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.010421 0 0.059003"/>
+      <geometry>
+        <box size="0.033556 0.015082 0.014587"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0136 0 0"/>
+      <geometry>
+        <box size="0.014486 0.015082 0.13"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.010421 0 -0.059003"/>
+      <geometry>
+        <box size="0.033556 0.015082 0.014587"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="armchair_joint" type="fixed">
+    <origin rpy="0 0 1.9199" xyz="7.9688 -0.3791 0.0"/>
+    <parent link="furnitures_root"/>
+    <child link="armchair"/>
+  </joint>
+  <link name="armchair">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/armchair.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/collision/armchair.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="sofa_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="8.1986 2.4096 0.0"/>
+    <parent link="furnitures_root"/>
+    <child link="sofa"/>
+  </joint>
+  <link name="sofa">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/sofa.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/collision/sofa.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="coffee_table_joint" type="fixed">
+    <origin rpy="0 0 1.5708" xyz="7.8034 1.02957 0.0"/>
+    <parent link="furnitures_root"/>
+    <child link="coffee_table"/>
+  </joint>
+  <link name="coffee_table">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/coffee_table.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0000 0.0000 0.02977"/>
+      <geometry>
+        <box size="0.58 1.08 0.05954"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.022101 0.0000 0.06552"/>
+      <geometry>
+        <box size="0.6106 0.4 0.01196"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0000 0.39975 0.220625"/>
+      <geometry>
+        <box size="0.7 0.4 0.32"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0000 -0.39975 0.220625"/>
+      <geometry>
+        <box size="0.7 0.4 0.32"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.316343 0.0000 0.22"/>
+      <geometry>
+        <box size="0.0673 0.4 0.32"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0000 0.0000 0.39"/>
+      <geometry>
+        <box size="0.7 1.2 0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="coffee_table_drawer_joint" type="prismatic">
+    <parent link="coffee_table"/>
+    <child link="coffee_table_drawer"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.5" velocity="0.5"/>
+  </joint>
+  <link name="coffee_table_drawer">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/coffee_table_drawer.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.338806 0.0000 0.21892"/>
+      <geometry>
+        <box size="0.0214 0.396 0.31569"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.051066 0.0000 0.107979"/>
+      <geometry>
+        <box size="0.5521 0.3454 0.02144"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.207974 0.0000 0.238907"/>
+      <geometry>
+        <box size="0.0214 0.3486 0.2398"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.051066 -0.186206 0.226597"/>
+      <geometry>
+        <box size="0.5521 0.0214 0.26425"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.051066 0.186206 0.226597"/>
+      <geometry>
+        <box size="0.5521 0.0214 0.26425"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="bedside_table_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="7.2110 0.1296 0.0"/>
+    <parent link="furnitures_root"/>
+    <child link="bedside_table"/>
+  </joint>
+  <link name="bedside_table">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/visual/bedside_table.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0000 0.0000 0.230"/>
+      <geometry>
+        <box size="0.312 0.312 0.460"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="kitchen_base" type="fixed">
+    <origin xyz="-0.03 0.75 0.005"/>
+    <parent link="apartment_root"/>
+    <child link="kitchen_root"/>
+  </joint>
+  <link name="kitchen_root"/>
+  <!-- Frames, reference frames -->
+  <joint name="frame_A_joint" type="fixed">
+    <parent link="kitchen_root"/>
+    <origin xyz="0.49 0.696 0.080"/>
+    <child link="side_A"/>
+  </joint>
+  <!-- Bond, James Bond -->
+  <joint name="frame_B_joint" type="fixed">
+    <parent link="kitchen_root"/>
+    <origin rpy="0 0 3.14" xyz="2.91 3.7 0.11"/>
+    <child link="side_B"/>
+  </joint>
+  <link name="side_A"/>
+  <link name="side_B"/>
+  <!-- - - - - - - - - - - - - Cabinets side A - - - - - - - - - -->
+  <!-- Cabinet1 -->
+  <joint name="cabinet1_joint" type="fixed">
+    <origin xyz="0.01 0.01 1.02"/>
+    <parent link="side_A"/>
+    <child link="cabinet1"/>
+  </joint>
+  <link name="cabinet1">
+    <visual>
+      <origin xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAICabinet1W60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAICabinet1W60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet1_door_top_left_joint" type="revolute">
+    <origin rpy="0 0 3.14" xyz="0.103 -0.3 0.32"/>
+    <parent link="cabinet1"/>
+    <child link="cabinet1_door_top_left"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="cabinet1_door_top_left">
+    <visual>
+      <origin rpy="0 0 3.14" xyz="0.001 -0.305 0.35"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDoorW60H80.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14" xyz="0.001 -0.305 0.35"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDoorW60H80.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab1_top_door_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.055 -0.5 0.0"/>
+    <parent link="cabinet1_door_top_left"/>
+    <child link="handle_cab1_top_door"/>
+  </joint>
+  <link name="handle_cab1_top_door">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="oven_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.175 0.0 -0.025"/>
+    <!-- <origin xyz="-0.125 0.0 -0.025" rpy="0 0 0"/> -->
+    <parent link="cabinet1"/>
+    <child link="oven"/>
+  </joint>
+  <material name="oven_gray">
+    <color rgba="0.8 0.8 0.8 1"/>
+  </material>
+  <link name="oven">
+    <visual>
+      <geometry>
+        <!-- <box size="0.58 0.58 0.59"/> -->
+        <mesh filename="package://iai_apartment/meshes/cabinet01/oven.dae"/>
+      </geometry>
+      <!-- <material name="oven_gray"/> -->
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.58 0.58 0.59"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet1_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="-0.117 0.0 -0.43"/>
+    <parent link="cabinet1"/>
+    <child link="cabinet1_drawer_middle"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet1_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H26.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H26.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab1_drawer_mid_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.26 0.0 0.11"/>
+    <parent link="cabinet1_drawer_middle"/>
+    <child link="handle_cab1_drawer_mid"/>
+  </joint>
+  <link name="handle_cab1_drawer_mid">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet1_drawer_bottom_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="-0.094 0.0 -0.95"/>
+    <parent link="cabinet1"/>
+    <child link="cabinet1_drawer_bottom"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet1_drawer_bottom">
+    <visual>
+      <origin rpy="0 0 -3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H53.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H53.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab1_drawer_bottom_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.237 0.0 0.35"/>
+    <parent link="cabinet1_drawer_bottom"/>
+    <child link="handle_cab1_drawer_bottom"/>
+  </joint>
+  <link name="handle_cab1_drawer_bottom">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Collision okay surface for cabinet1-->
+  <material name="white">
+    <color rgba="1 1 1 1"/>
+  </material>
+  <joint name="cabinet1_coloksu_level4_joint" type="fixed">
+    <origin xyz="-0.172 0.005 0.281"/>
+    <parent link="cabinet1"/>
+    <child link="cabinet1_coloksu_level4"/>
+  </joint>
+  <link name="cabinet1_coloksu_level4">
+    <visual>
+      <geometry>
+        <box size="0.534 0.568 0.003"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.534 0.568 0.003"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet1_coloksu_level5_joint" type="fixed">
+    <!-- actually this should align with the CAD model but the CAD model is incorrect. -->
+    <!-- <origin xyz="-0.172 0.005 0.672"/> -->
+    <origin xyz="-0.172 0.005 0.56"/>
+    <parent link="cabinet1"/>
+    <child link="cabinet1_coloksu_level5"/>
+  </joint>
+  <link name="cabinet1_coloksu_level5">
+    <visual>
+      <geometry>
+        <box size="0.534 0.568 0.02"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.534 0.568 0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet2 -->
+  <joint name="cabinet2_joint" type="fixed">
+    <origin xyz="-0.163 0.51 0.975"/>
+    <parent link="side_A"/>
+    <child link="cabinet2"/>
+  </joint>
+  <link name="cabinet2">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAICabinet2W40H57.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAICabinet2W40H57.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet2_door_out_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.277 -0.19 0.0"/>
+    <parent link="cabinet2"/>
+    <child link="cabinet2_door_out_fancy"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1.0" lower="0.0" upper="0.02" velocity="1.5708"/>
+    <mimic joint="cabinet2_door_left_joint" multiplier="0.01"/>
+  </joint>
+  <link name="cabinet2_door_out_fancy"/>
+  <joint name="cabinet2_door_left_joint" type="revolute">
+    <parent link="cabinet2_door_out_fancy"/>
+    <child link="cabinet2_door_left"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="cabinet2_door_left">
+    <visual>
+      <origin rpy="0 0 3.14" xyz="0 -0.2 0.145"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDoorW40H212.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14" xyz="0 -0.2 0.145"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDoorW40H212.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab2_drawer_bottom_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.040 -0.29 0.05"/>
+    <parent link="cabinet2_door_left"/>
+    <child link="handle_cab2_door"/>
+  </joint>
+  <link name="handle_cab2_door">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet2_drawer_small_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="-0.01 0.0 0.17"/>
+    <parent link="cabinet2"/>
+    <child link="cabinet2_drawer_small"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="500.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet2_drawer_small">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H10.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H10.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet2_drawer_big_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.04 0.0 -0.12"/>
+    <parent link="cabinet2"/>
+    <child link="cabinet2_drawer_big"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="500.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet2_drawer_big">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H22.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H22.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet3 -->
+  <joint name="cabinet3_joint" type="fixed">
+    <origin xyz="-0.273 1.022 1.07"/>
+    <parent link="side_A"/>
+    <child link="cabinet3"/>
+  </joint>
+  <link name="cabinet3">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet03/IAICabinet3W60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet03/IAICabinet3W60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet3_door_top_out_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.39 -0.3 0.5"/>
+    <parent link="cabinet3"/>
+    <child link="cabinet3_door_top_out_fancy"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1.0" lower="0.0" upper="0.02" velocity="1.5708"/>
+    <mimic joint="cabinet3_door_top_left_joint" multiplier="0.01"/>
+  </joint>
+  <link name="cabinet3_door_top_out_fancy"/>
+  <joint name="cabinet3_door_top_left_joint" type="revolute">
+    <parent link="cabinet3_door_top_out_fancy"/>
+    <child link="cabinet3_door_top_left"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="cabinet3_door_top_left">
+    <visual>
+      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.1"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H133.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.1"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H133.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab3_door_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.036 -0.45 -0.65"/>
+    <parent link="cabinet3_door_top_left"/>
+    <child link="handle_cab3_door_top"/>
+  </joint>
+  <link name="handle_cab3_door_top">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet3_door_bottom_out_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.39 -0.3 -0.5"/>
+    <parent link="cabinet3"/>
+    <child link="cabinet3_door_bottom_out_fancy"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1.0" lower="0.0" upper="0.02" velocity="1.5708"/>
+    <mimic joint="cabinet3_door_bottom_left_joint" multiplier="0.01"/>
+  </joint>
+  <link name="cabinet3_door_bottom_out_fancy"/>
+  <joint name="cabinet3_door_bottom_left_joint" type="revolute">
+    <parent link="cabinet3_door_bottom_out_fancy"/>
+    <child link="cabinet3_door_bottom_left"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="cabinet3_door_bottom_left">
+    <visual>
+      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.165"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H80.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.165"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H80.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab3_door_bottom_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.036 -0.45 0.1"/>
+    <parent link="cabinet3_door_bottom_left"/>
+    <child link="handle_cab3_door_bottom"/>
+  </joint>
+  <link name="handle_cab3_door_bottom">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet4 -->
+  <joint name="cabinet4_joint" type="fixed">
+    <origin xyz="-0.175 1.825 1.72"/>
+    <parent link="side_A"/>
+    <child link="cabinet4"/>
+  </joint>
+  <link name="cabinet4">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIKitchenCabinet4W100.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIKitchenCabinet4W100.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet4_door_top_top_joint" type="revolute">
+    <origin rpy="0 0 3.14" xyz="0.07 0 0.42"/>
+    <parent link="cabinet4"/>
+    <child link="cabinet4_door_top"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="1000.0" lower="0" upper="2.0" velocity="1.5708"/>
+  </joint>
+  <link name="cabinet4_door_top">
+    <visual>
+      <origin rpy="0 0 3.14" xyz="0.0 0.0 -0.2"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor1W100H40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14" xyz="0.0 0.0 -0.2"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor1W100H40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet4_door_bottom_bottom_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0.0 0.0 -0.4"/>
+    <parent link="cabinet4_door_top"/>
+    <child link="cabinet4_door_bottom"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+    <mimic joint="cabinet4_door_top_top_joint" multiplier="1.2"/>
+  </joint>
+  <link name="cabinet4_door_bottom">
+    <visual>
+      <origin rpy="0 0 3.14" xyz="-0.014 0.0 -0.2"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor2W100H40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14" xyz="-.014 0.0 -0.2"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor2W100H40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab4_door_bottom_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.055 0 -0.35"/>
+    <parent link="cabinet4_door_bottom"/>
+    <child link="handle_cab4_door_bottom"/>
+  </joint>
+  <link name="handle_cab4_door_bottom">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet5 -->
+  <joint name="cabinet5_joint" type="fixed">
+    <origin xyz="-0.121 1.625 0.4"/>
+    <parent link="side_A"/>
+    <child link="cabinet5"/>
+  </joint>
+  <link name="cabinet5">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenCabinet5W60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenCabinet5W60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet5_drawer_top_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.327"/>
+    <parent link="cabinet5"/>
+    <child link="cabinet5_drawer_top"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet5_drawer_top">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H13.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H13.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab5_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.05"/>
+    <parent link="cabinet5_drawer_top"/>
+    <child link="handle_cab5_t"/>
+  </joint>
+  <link name="handle_cab5_t">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet5_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.027 0.0 0.107"/>
+    <parent link="cabinet5"/>
+    <child link="cabinet5_drawer_middle"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet5_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H26.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H26.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab5_mid_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
+    <parent link="cabinet5_drawer_middle"/>
+    <child link="handle_cab5_m"/>
+  </joint>
+  <link name="handle_cab5_m">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet5_drawer_bottom_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.037 0.0 -0.28"/>
+    <parent link="cabinet5"/>
+    <child link="cabinet5_drawer_bottom"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet5_drawer_bottom">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab5_bott_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.235 0 0.245"/>
+    <parent link="cabinet5_drawer_bottom"/>
+    <child link="handle_cab5_b"/>
+  </joint>
+  <link name="handle_cab5_b">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet6 -->
+  <joint name="cabinet6_joint" type="fixed">
+    <origin xyz="-0.121 2.127 0.4"/>
+    <parent link="side_A"/>
+    <child link="cabinet6"/>
+  </joint>
+  <link name="cabinet6">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenCabinet6W40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenCabinet6W40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet6_drawer_top_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.033 -0.0125 0.328"/>
+    <parent link="cabinet6"/>
+    <child link="cabinet6_drawer_top"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet6_drawer_top">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H13.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H13.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab6_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.238 0 0.05"/>
+    <parent link="cabinet6_drawer_top"/>
+    <child link="handle_cab6_t"/>
+  </joint>
+  <link name="handle_cab6_t">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet6_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.105"/>
+    <parent link="cabinet6"/>
+    <child link="cabinet6_drawer_middle"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet6_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H26.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H26.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab6_mid_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
+    <parent link="cabinet6_drawer_middle"/>
+    <child link="handle_cab6_m"/>
+  </joint>
+  <link name="handle_cab6_m">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet6_drawer_bottom_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.047 0.0 -0.28"/>
+    <parent link="cabinet6"/>
+    <child link="cabinet6_drawer_bottom"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet6_drawer_bottom">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab6_bott_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.227 0 0.245"/>
+    <parent link="cabinet6_drawer_bottom"/>
+    <child link="handle_cab6_b"/>
+  </joint>
+  <link name="handle_cab6_b">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- - - - - - - - - - - - - - - Cabinets side B - - - - - - - - - - - - - - -->
+  <!-- Dishwasher: Cabinet 7 -->
+  <joint name="cabinet7_joint" type="fixed">
+    <origin xyz="0.2 0.501 0.47"/>
+    <parent link="side_B"/>
+    <child link="cabinet7"/>
+  </joint>
+  <link name="cabinet7">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenCabinet7W60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenCabinet7W60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet7_door_bottom_left_joint" type="revolute">
+    <origin rpy="0 0 3.14" xyz="0.372 0 -0.45"/>
+    <parent link="cabinet7"/>
+    <child link="cabinet7_door_bottom_left"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="cabinet7_door_bottom_left">
+    <visual>
+      <origin xyz="0 0 0.385"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenDoorW60H80.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.385"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenDoorW60H80.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab7_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.06 0 0.74"/>
+    <parent link="cabinet7_door_bottom_left"/>
+    <child link="handle_cab7"/>
+  </joint>
+  <link name="handle_cab7">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- dishwasher rack -->
+  <joint name="dishwasher_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="-0.065 0.03 0.08"/>
+    <parent link="cabinet7"/>
+    <child link="dishwasher_drawer_middle"/>
+    <axis xyz="-1 0  0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="dishwasher_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet07/dishwasher_drawer_middle.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet07/dishwasher_drawer_middle.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab7_drawer_middle_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.315 0 0.068"/>
+    <parent link="dishwasher_drawer_middle"/>
+    <child link="handle_cab7_middle"/>
+  </joint>
+  <link name="handle_cab7_middle">
+    <visual>
+      <origin rpy="-1.57 0 0"/>
+      <geometry>
+        <box size="0.015 0.325 0.021"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <origin rpy="-1.57 0 0"/>
+      <geometry>
+        <box size="0.015 0.325 0.021"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet 8 -->
+  <joint name="cabinet8_joint" type="fixed">
+    <origin xyz="0.256 1.102 0.006"/>
+    <parent link="side_B"/>
+    <child link="cabinet8"/>
+  </joint>
+  <link name="cabinet8">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenCabinet8W60_v2.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenCabinet8W60_v2.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet8_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.05 0.0 0.48"/>
+    <parent link="cabinet8"/>
+    <child link="cabinet8_drawer_middle"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.40" velocity="0.5"/>
+  </joint>
+  <link name="cabinet8_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW56H10.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW56H10.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet8_drawer_bottom_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.114 0.0 0.105"/>
+    <parent link="cabinet8"/>
+    <child link="cabinet8_drawer_bottom"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet8_drawer_bottom">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW60H80.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW60H80.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab8_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.238 0 0.65"/>
+    <parent link="cabinet8_drawer_bottom"/>
+    <child link="handle_cab8"/>
+  </joint>
+  <link name="handle_cab8">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet9 -->
+  <joint name="cabinet9_joint" type="fixed">
+    <origin xyz="0.333 1.703 0.394"/>
+    <parent link="side_B"/>
+    <child link="cabinet9"/>
+  </joint>
+  <link name="cabinet9">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenCabinet9W60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenCabinet9W60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet9_drawer_top_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.327"/>
+    <parent link="cabinet9"/>
+    <child link="cabinet9_drawer_top"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet9_drawer_top">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H13.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H13.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab9_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.05"/>
+    <parent link="cabinet9_drawer_top"/>
+    <child link="handle_cab9_t"/>
+  </joint>
+  <link name="handle_cab9_t">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet9_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.027 0.0 0.106"/>
+    <parent link="cabinet9"/>
+    <child link="cabinet9_drawer_middle"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet9_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H26.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H26.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab9_mid_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
+    <parent link="cabinet9_drawer_middle"/>
+    <child link="handle_cab9_m"/>
+  </joint>
+  <link name="handle_cab9_m">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet9_drawer_bottom_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.036 0.0 -0.281"/>
+    <parent link="cabinet9"/>
+    <child link="cabinet9_drawer_bottom"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet9_drawer_bottom">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab9_bott_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.24 0 0.245"/>
+    <parent link="cabinet9_drawer_bottom"/>
+    <child link="handle_cab9_b"/>
+  </joint>
+  <link name="handle_cab9_b">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet 10 -->
+  <joint name="cabinet10_joint" type="fixed">
+    <origin xyz="0.333 2.204 0.394"/>
+    <parent link="side_B"/>
+    <child link="cabinet10"/>
+  </joint>
+  <link name="cabinet10">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenCabinet9W40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenCabinet9W40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet10_drawer_top_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.033 0.0 0.326"/>
+    <parent link="cabinet10"/>
+    <child link="cabinet10_drawer_top"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet10_drawer_top">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H13.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H13.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab10_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.243 0 0.05"/>
+    <parent link="cabinet10_drawer_top"/>
+    <child link="handle_cab10_t"/>
+  </joint>
+  <link name="handle_cab10_t">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet10_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.031 0.0 0.104"/>
+    <parent link="cabinet10"/>
+    <child link="cabinet10_drawer_middle"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet10_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H26.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H26.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab10_mid_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
+    <parent link="cabinet10_drawer_middle"/>
+    <child link="handle_cab10_m"/>
+  </joint>
+  <link name="handle_cab10_m">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet10_drawer_bottom_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.03 0.0 -0.279"/>
+    <parent link="cabinet10"/>
+    <child link="cabinet10_drawer_bottom"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet10_drawer_bottom">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab10_bott_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.245"/>
+    <parent link="cabinet10_drawer_bottom"/>
+    <child link="handle_cab10_b"/>
+  </joint>
+  <link name="handle_cab10_b">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cabinet 11 -->
+  <joint name="cabinet11_joint" type="fixed">
+    <origin xyz="0.333 2.855 0.394"/>
+    <parent link="side_B"/>
+    <child link="cabinet11"/>
+  </joint>
+  <link name="cabinet11">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenCabinet11W90.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenCabinet11W90.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet11_drawer_top_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.326"/>
+    <parent link="cabinet11"/>
+    <child link="cabinet11_drawer_top"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet11_drawer_top">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H13.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H13.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab11_top_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.243 0 0.05"/>
+    <parent link="cabinet11_drawer_top"/>
+    <child link="handle_cab11_t"/>
+  </joint>
+  <link name="handle_cab11_t">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet11_drawer_middle_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.027 0.0 0.106"/>
+    <parent link="cabinet11"/>
+    <child link="cabinet11_drawer_middle"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet11_drawer_middle">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H26.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H26.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab11_mid_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
+    <parent link="cabinet11_drawer_middle"/>
+    <child link="handle_cab11_m"/>
+  </joint>
+  <link name="handle_cab11_m">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="cabinet11_drawer_bottom_joint" type="prismatic">
+    <origin rpy="0 0 3.14" xyz="0.045 0.0 -0.286"/>
+    <parent link="cabinet11"/>
+    <child link="cabinet11_drawer_bottom"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
+  </joint>
+  <link name="cabinet11_drawer_bottom">
+    <visual>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H40.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 3.14"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H40.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="handle_cab11_bott_joint" type="fixed">
+    <origin rpy="1.57 0 0" xyz="-0.228 0 0.245"/>
+    <parent link="cabinet11_drawer_bottom"/>
+    <child link="handle_cab11_b"/>
+  </joint>
+  <link name="handle_cab11_b">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Miscellaneous -->
+  <!-- Island, Main Countertop -->
+  <joint name="island_countertop_joint" type="fixed">
+    <origin xyz="0.13 1.786 0.807"/>
+    <parent link="side_B"/>
+    <child link="island_countertop"/>
+  </joint>
+  <link name="island_countertop">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/island/IAIKitchenIslandAreaTop.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/island/IAIKitchenIslandAreaTop.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Cooktop -->
+  <joint name="cooktop_joint" type="fixed">
+    <origin xyz="0.130 1.074 0.023"/>
+    <parent link="island_countertop"/>
+    <child link="cooktop"/>
+  </joint>
+  <link name="cooktop">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cooktop/IAIKitchenCookTop.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/cooktop/IAIKitchenCookTop.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="hotplate1_joint" type="fixed">
+    <origin xyz="0.1069 -0.16199 0.005"/>
+    <parent link="cooktop"/>
+    <child link="hotplate1"/>
+  </joint>
+  <link name="hotplate1"/>
+  <joint name="hotplate2_joint" type="fixed">
+    <origin xyz="0.066902 0.13801 0.0055"/>
+    <parent link="cooktop"/>
+    <child link="hotplate2"/>
+  </joint>
+  <link name="hotplate2"/>
+  <joint name="hotplate3_joint" type="fixed">
+    <origin xyz="-0.155778 0.138013 0.0055"/>
+    <parent link="cooktop"/>
+    <child link="hotplate3"/>
+  </joint>
+  <link name="hotplate3"/>
+  <joint name="hotplate4_joint" type="fixed">
+    <origin xyz="-0.115778 -0.161987 0.005333"/>
+    <parent link="cooktop"/>
+    <child link="hotplate4"/>
+  </joint>
+  <link name="hotplate4"/>
+  <!-- Sink, Kitchen Sink -->
+  <joint name="sink_joint" type="fixed">
+    <origin xyz="0.198 -0.698 -0.038"/>
+    <parent link="island_countertop"/>
+    <child link="sink"/>
+  </joint>
+  <link name="sink">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/sink/IAIKitchenSink.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/sink/IAIKitchenSink.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Tap, Water Tap -->
+  <joint name="tap_joint" type="revolute">
+    <origin xyz="-0.228 -0.003 0.164"/>
+    <parent link="sink"/>
+    <child link="tap_body"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="-1.5708" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="tap_body">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/tap/IAIKitchenWaterTapBody.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5708 0 0"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/tap/IAIKitchenWaterTapBody.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="tap_handle_open_joint" type="revolute">
+    <origin xyz="0.001233 0.038489 -0.055395"/>
+    <parent link="tap_body"/>
+    <child link="tap_handle_open"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="1000.0" lower="-1.5708" upper="1.5708" velocity="1.5708"/>
+  </joint>
+  <link name="tap_handle_open"/>
+  <joint name="tap_handle_hot_cold_joint" type="revolute">
+    <origin rpy="0 0.5 0"/>
+    <parent link="tap_handle_open"/>
+    <child link="tap_handle"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="1000.0" lower="-0.5" upper="0" velocity="1.5708"/>
+  </joint>
+  <link name="tap_handle">
+    <visual>
+      <origin rpy="0 0.5 0"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/tap/IAIKitchenWaterTapHandle.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5708 0.5 0"/>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/tap/IAIKitchenWaterTapHandle.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- floor socket: A -->
+  <joint name="socket_a_joint" type="fixed">
+    <origin xyz="0.095 1.02 -0.03"/>
+    <parent link="side_A"/>
+    <child link="useless_underbar_a"/>
+  </joint>
+  <link name="useless_underbar_a">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/floor_socket_side_a/IAIKitchenPartSocketW260.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/floor_socket_side_a/IAIKitchenPartSocketW260.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- floor socket: B -->
+  <joint name="socket_b_joint" type="fixed">
+    <origin xyz="0.552 1.755 -0.045"/>
+    <parent link="side_B"/>
+    <child link="useless_underbar_b"/>
+  </joint>
+  <link name="useless_underbar_b">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/floor_socket_side_b/IAIKitchenPartSocketW310.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/floor_socket_side_b/IAIKitchenPartSocketW310.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Side A countertop -->
+  <joint name="countertop_joint" type="fixed">
+    <origin xyz="-0.027 0.21 0.423"/>
+    <parent link="cabinet5"/>
+    <child link="countertop"/>
+  </joint>
+  <link name="countertop">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/countertop/IAIKitchenPartW102H60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/countertop/IAIKitchenPartW102H60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="coffe_machine_joint" type="fixed">
+    <origin xyz="0 0.3 0.2"/>
+    <parent link="countertop"/>
+    <child link="coffe_machine"/>
+  </joint>
+  <link name="coffe_machine">
+    <visual>
+      <geometry>
+        <box size="0.58 0.33 0.41"/>
+      </geometry>
+      <material name="oven_gray"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.58 0.32 0.41"/>
+      </geometry>
+      <material name="oven_gray"/>
+    </collision>
+  </link>
+  <!-- island waterfall, left side -->
+  <joint name="island_waterfall_left_joint" type="fixed">
+    <origin rpy="1.57 0 1.57" xyz="0.285 0.191 0.354"/>
+    <parent link="side_B"/>
+    <child link="island_waterfall_left"/>
+  </joint>
+  <link name="island_waterfall_left">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- island waterfall, right side -->
+  <joint name="island_waterfall_right_joint" type="fixed">
+    <origin rpy="1.57 0 1.57" xyz="0.285 3.315 0.354"/>
+    <parent link="side_B"/>
+    <child link="island_waterfall_right"/>
+  </joint>
+  <link name="island_waterfall_right">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- island, back panel -->
+  <joint name="island_back_joint" type="fixed">
+    <origin rpy="0 1.57 0" xyz="-0.026 1.754 0.353"/>
+    <parent link="side_B"/>
+    <child link="island_back"/>
+  </joint>
+  <link name="island_back">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW314H90/IAIKitchenPartW314H90.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW314H90/IAIKitchenPartW314H90.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- side A left panel -->
+  <joint name="side_A_left_joint" type="fixed">
+    <origin rpy="0 0 1.57" xyz="-0.16 -0.296 1.030"/>
+    <parent link="side_A"/>
+    <child link="side_A_left"/>
+  </joint>
+  <link name="side_A_left">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW58H223/IAIKitchenPartW58H223.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW58H223/IAIKitchenPartW58H223.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- side A right panel up -->
+  <joint name="side_A_right_top_joint" type="fixed">
+    <origin rpy="0 0 1.57" xyz="-0.262 2.336 1.74"/>
+    <parent link="side_A"/>
+    <child link="side_A_rt"/>
+  </joint>
+  <link name="side_A_rt">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUppersideW80H39/IAIKitchenVisiblePartUppersideW80H39.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUppersideW80H39/IAIKitchenVisiblePartUppersideW80H39.obj"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- side A right panel bottom -->
+  <joint name="side_A_right_bott_joint" type="fixed">
+    <origin rpy="1.57 0 1.57" xyz="-0.159 2.338 0.361"/>
+    <parent link="side_A"/>
+    <child link="side_A_rb"/>
+  </joint>
+  <link name="side_A_rb">
+    <visual>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUnderSideW90H58/IAIKitchenVisiblePartUnderSideW90H58.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUnderSideW90H58/IAIKitchenVisiblePartUnderSideW90H58.obj"/>
+      </geometry>
+    </collision>
+  </link>
 </robot>
+
diff --git a/iai_apartment/urdf/apartment.xacro b/iai_apartment/urdf/apartment.xacro
new file mode 100644
index 0000000..cee763a
--- /dev/null
+++ b/iai_apartment/urdf/apartment.xacro
@@ -0,0 +1,19 @@
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="apartment">
+
+  <link name="apartment_root">
+    <origin rpy="0 0 0" xyz="0 0 0" />
+  </link>
+
+  <!-- furnitures, including wardrobe, sofa and coffee table -->
+  <xacro:include filename="$(find iai_apartment)/urdf/furnitures.xacro" />
+  <xacro:kitchen_macro name="furnitures_base" parent="apartment_root">
+    <origin xyz="8.85 1.75 0.0" />
+  </xacro:kitchen_macro>
+
+  <!-- kitchen, kitchen on the walls -->
+  <xacro:include filename="$(find iai_apartment)/urdf/kitchen.xacro" />
+  <xacro:kitchen_macro name="kitchen_base" parent="apartment_root">
+    <origin xyz="-0.03 0.75 0.005" />
+  </xacro:kitchen_macro>
+
+</robot>
\ No newline at end of file
diff --git a/iai_apartment/urdf/furnitures.xacro b/iai_apartment/urdf/furnitures.xacro
new file mode 100644
index 0000000..c80fce6
--- /dev/null
+++ b/iai_apartment/urdf/furnitures.xacro
@@ -0,0 +1,454 @@
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="furnitures">
+
+  <xacro:macro name="kitchen_macro" params="name parent *origin">
+
+    <joint name="${name}" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}" />
+      <child link="furnitures_root" />
+    </joint>
+    <link name="furnitures_root" />
+
+    <!-- things and stuff -->
+    <joint name="walls_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 -0.2" />
+      <parent link="furnitures_root" />
+      <child link="walls" />
+    </joint>
+    <link name="walls">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/walls.dae" />
+        </geometry>
+      </visual>
+      <!--collision>
+          <geometry>
+            <mesh filename="package://iai_apartment/meshes/collision/walls.stl" />
+          </geometry>
+        </collision-->
+    </link>
+    <joint name="windows_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 -0.2" />
+      <parent link="furnitures_root" />
+      <child link="windows" />
+    </joint>
+    <link name="windows">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/windows.dae" />
+        </geometry>
+      </visual>
+      <!--collision>
+          <geometry>
+            <mesh filename="package://iai_apartment/meshes/collision/windows.obj"/>
+          </geometry>
+        </collision-->
+    </link>
+
+    <!-- Collision okay Walls -->
+    <!-- From the door side being wall 1, turn right and count the walls. -->
+    <joint name="wall_coloksu_wall1_joint" type="fixed">
+      <origin xyz="0.6 -1.615 1.53" rpy="0 0 -1.57" />
+      <parent link="furnitures_root" />
+      <child link="wall_coloksu_wall1" />
+    </joint>
+    <link name="wall_coloksu_wall1">
+      <collision>
+        <origin rpy="0 1.57 0" />
+        <geometry>
+          <box size="3.05 18.95 0.003" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="wall_coloksu_wall2_joint" type="fixed">
+      <origin xyz="-8.855 0.782 1.53" rpy="0 0 3.14" />
+      <parent link="furnitures_root" />
+      <child link="wall_coloksu_wall2" />
+    </joint>
+    <link name="wall_coloksu_wall2">
+      <collision>
+        <origin rpy="0 1.57 0" />
+        <geometry>
+          <box size="3.05 4.808 0.003" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="wall_coloksu_wall3_joint" type="fixed">
+      <origin xyz="0.6 3.180 1.53" rpy="0 0 1.57" />
+      <parent link="furnitures_root" />
+      <child link="wall_coloksu_wall3" />
+    </joint>
+    <link name="wall_coloksu_wall3">
+      <collision>
+        <origin rpy="0 1.57 0" />
+        <geometry>
+          <box size="3.05 18.95 0.003" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="wall_coloksu_wall4_joint" type="fixed">
+      <origin xyz="10.069 0.782 1.53" rpy="0 0 0" />
+      <parent link="furnitures_root" />
+      <child link="wall_coloksu_wall4" />
+    </joint>
+    <link name="wall_coloksu_wall4">
+      <collision>
+        <origin rpy="0 1.57 0" />
+        <geometry>
+          <box size="3.05 4.808 0.003" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="wardrobe_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="9.7082 -0.6331 0.0" />
+      <parent link="furnitures_root" />
+      <child link="wardrobe" />
+    </joint>
+    <link name="wardrobe">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/wardrobe.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.2693 -0.4482 0.0750" />
+        <geometry>
+          <box size="0.08 0.08 0.15" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.10603 -0.4482 0.0750" />
+        <geometry>
+          <box size="0.08 0.08 0.15" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.2693 0.4482 0.0750" />
+        <geometry>
+          <box size="0.08 0.08 0.15" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.10603 0.4482 0.0750" />
+        <geometry>
+          <box size="0.08 0.08 0.15" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.0805 0.0000 0.16345" />
+        <geometry>
+          <box size="0.487 1.0000 0.0269" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.0805 0.48965 0.975" />
+        <geometry>
+          <box size="0.487 0.0207 1.65" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.0896 0.0000 0.975" />
+        <geometry>
+          <box size="0.4318 0.018568 1.65" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.0805 -0.48965 0.975" />
+        <geometry>
+          <box size="0.487 0.0207 1.65" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975" />
+        <geometry>
+          <box size="0.0367 1.0000 1.65" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975" />
+        <geometry>
+          <box size="0.0367 1.0000 1.65" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.0805 0.0000 1.7865" />
+        <geometry>
+          <box size="0.487 1.0000 0.027" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.5727" />
+        <geometry>
+          <box size="0.4299 0.469957 0.0118" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.974836" />
+        <geometry>
+          <box size="0.4299 0.469957 0.0118" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.08861 -0.244343 1.37686" />
+        <geometry>
+          <box size="0.4299 0.469957 0.0118" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="1.5708 0 0" xyz="-0.086923 0.244373 1.66898" />
+        <geometry>
+          <cylinder radius="0.0075" length="0.469897" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="wardrobe_door_left_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="-0.3246 0.5 0.0000" />
+      <parent link="wardrobe" />
+      <child link="wardrobe_door_left" />
+      <axis xyz="0 0 -1" />
+      <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708" />
+    </joint>
+    <link name="wardrobe_door_left">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_left.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.006435 -0.2485 0.975" />
+        <geometry>
+          <box size="0.012967 0.495438 1.64844" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="wardrobe_door_left_handle_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="-0.032089 -0.460513 0.973703" />
+      <parent link="wardrobe_door_left" />
+      <child link="wardrobe_door_left_handle" />
+    </joint>
+    <link name="wardrobe_door_left_handle">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_handle.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.010421 0 0.059003" />
+        <geometry>
+          <box size="0.033556 0.015082 0.014587" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.0136 0 0" />
+        <geometry>
+          <box size="0.014486 0.015082 0.13" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.010421 0 -0.059003" />
+        <geometry>
+          <box size="0.033556 0.015082 0.014587" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="wardrobe_door_right_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="-0.3246 -0.5 0.0000" />
+      <parent link="wardrobe" />
+      <child link="wardrobe_door_right" />
+      <axis xyz="0 0 1" />
+      <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708" />
+    </joint>
+    <link name="wardrobe_door_right">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_right.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.006435 0.2485 0.975" />
+        <geometry>
+          <box size="0.012967 0.495438 1.64844" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="wardrobe_door_right_handle_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="-0.032089 0.460513 0.973703" />
+      <parent link="wardrobe_door_right" />
+      <child link="wardrobe_door_right_handle" />
+    </joint>
+    <link name="wardrobe_door_right_handle">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_handle.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.010421 0 0.059003" />
+        <geometry>
+          <box size="0.033556 0.015082 0.014587" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.0136 0 0" />
+        <geometry>
+          <box size="0.014486 0.015082 0.13" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.010421 0 -0.059003" />
+        <geometry>
+          <box size="0.033556 0.015082 0.014587" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="armchair_joint" type="fixed">
+      <origin rpy="0 0 1.9199" xyz="7.9688 -0.3791 0.0" />
+      <parent link="furnitures_root" />
+      <child link="armchair" />
+    </joint>
+    <link name="armchair">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/armchair.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/collision/armchair.obj" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="sofa_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="8.1986 2.4096 0.0" />
+      <parent link="furnitures_root" />
+      <child link="sofa" />
+    </joint>
+    <link name="sofa">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/sofa.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/collision/sofa.obj" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="coffee_table_joint" type="fixed">
+      <origin rpy="0 0 1.5708" xyz="7.8034 1.02957 0.0" />
+      <parent link="furnitures_root" />
+      <child link="coffee_table" />
+    </joint>
+    <link name="coffee_table">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/coffee_table.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0000 0.0000 0.02977" />
+        <geometry>
+          <box size="0.58 1.08 0.05954" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.022101 0.0000 0.06552" />
+        <geometry>
+          <box size="0.6106 0.4 0.01196" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0000 0.39975 0.220625" />
+        <geometry>
+          <box size="0.7 0.4 0.32" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0000 -0.39975 0.220625" />
+        <geometry>
+          <box size="0.7 0.4 0.32" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.316343 0.0000 0.22" />
+        <geometry>
+          <box size="0.0673 0.4 0.32" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0000 0.0000 0.39" />
+        <geometry>
+          <box size="0.7 1.2 0.02" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="coffee_table_drawer_joint" type="prismatic">
+      <parent link="coffee_table" />
+      <child link="coffee_table_drawer" />
+      <axis xyz="-1 0 0" />
+      <limit effort="1000.0" lower="0.0" upper="0.5" velocity="0.5" />
+    </joint>
+    <link name="coffee_table_drawer">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/coffee_table_drawer.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.338806 0.0000 0.21892" />
+        <geometry>
+          <box size="0.0214 0.396 0.31569" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.051066 0.0000 0.107979" />
+        <geometry>
+          <box size="0.5521 0.3454 0.02144" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.207974 0.0000 0.238907" />
+        <geometry>
+          <box size="0.0214 0.3486 0.2398" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.051066 -0.186206 0.226597" />
+        <geometry>
+          <box size="0.5521 0.0214 0.26425" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin rpy="0 0 0" xyz="-0.051066 0.186206 0.226597" />
+        <geometry>
+          <box size="0.5521 0.0214 0.26425" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="bedside_table_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="7.2110 0.1296 0.0" />
+      <parent link="furnitures_root" />
+      <child link="bedside_table" />
+    </joint>
+    <link name="bedside_table">
+      <visual>
+        <geometry>
+          <mesh filename="package://iai_apartment/meshes/visual/bedside_table.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0.0000 0.0000 0.230" />
+        <geometry>
+          <box size="0.312 0.312 0.460" />
+        </geometry>
+      </collision>
+    </link>
+  </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/iai_apartment/urdf/iai-apartment.urdf b/iai_apartment/urdf/iai-apartment.urdf
deleted file mode 100644
index a9799b4..0000000
--- a/iai_apartment/urdf/iai-apartment.urdf
+++ /dev/null
@@ -1,1992 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from urdf/apartment.urdf            | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="apartment">
-  <link name="apartment_root">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-  </link>
-  <joint name="middle_of_the_room_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="8.85 1.75 0.0"/>
-    <parent link="apartment_root"/>
-    <child link="middle_of_the_room"/>
-  </joint>
-  <link name="middle_of_the_room"/>
-  <!-- things and stuff -->
-  <joint name="walls_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 -0.2"/>
-    <parent link="middle_of_the_room"/>
-    <child link="walls"/>
-  </joint>
-  <link name="walls">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/walls.dae"/>
-      </geometry>
-    </visual>
-    <!--collision>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/collision/walls.stl" />
-        </geometry>
-      </collision-->
-  </link>
-  <joint name="windows_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 -0.2"/>
-    <parent link="middle_of_the_room"/>
-    <child link="windows"/>
-  </joint>
-  <link name="windows">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/windows.dae"/>
-      </geometry>
-    </visual>
-    <!--collision>
-        <geometry>
-          <mesh filename="package://iai_apartment/meshes/collision/windows.obj"/>
-        </geometry>
-      </collision-->
-  </link>
-  <!-- Collision okay Walls -->
-  <!-- From the door side being wall 1, turn right and count the walls. -->
-  <joint name="wall_coloksu_wall1_joint" type="fixed">
-    <origin rpy="0 0 -1.57" xyz="0.6 -1.615 1.53"/>
-    <parent link="middle_of_the_room"/>
-    <child link="wall_coloksu_wall1"/>
-  </joint>
-  <link name="wall_coloksu_wall1">
-    <collision>
-      <origin rpy="0 1.57 0"/>
-      <geometry>
-        <box size="3.05 18.95 0.003"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="wall_coloksu_wall2_joint" type="fixed">
-    <origin rpy="0 0 3.14" xyz="-8.855 0.782 1.53"/>
-    <parent link="middle_of_the_room"/>
-    <child link="wall_coloksu_wall2"/>
-  </joint>
-  <link name="wall_coloksu_wall2">
-    <collision>
-      <origin rpy="0 1.57 0"/>
-      <geometry>
-        <box size="3.05 4.808 0.003"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="wall_coloksu_wall3_joint" type="fixed">
-    <origin rpy="0 0 1.57" xyz="0.6 3.180 1.53"/>
-    <parent link="middle_of_the_room"/>
-    <child link="wall_coloksu_wall3"/>
-  </joint>
-  <link name="wall_coloksu_wall3">
-    <collision>
-      <origin rpy="0 1.57 0"/>
-      <geometry>
-        <box size="3.05 18.95 0.003"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="wall_coloksu_wall4_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="10.069 0.782 1.53"/>
-    <parent link="middle_of_the_room"/>
-    <child link="wall_coloksu_wall4"/>
-  </joint>
-  <link name="wall_coloksu_wall4">
-    <collision>
-      <origin rpy="0 1.57 0"/>
-      <geometry>
-        <box size="3.05 4.808 0.003"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="wardrobe_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="9.7082 -0.6331 0.0"/>
-    <parent link="middle_of_the_room"/>
-    <child link="wardrobe"/>
-  </joint>
-  <link name="wardrobe">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/wardrobe.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.2693 -0.4482 0.0750"/>
-      <geometry>
-        <box size="0.08 0.08 0.15"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.10603 -0.4482 0.0750"/>
-      <geometry>
-        <box size="0.08 0.08 0.15"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.2693 0.4482 0.0750"/>
-      <geometry>
-        <box size="0.08 0.08 0.15"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.10603 0.4482 0.0750"/>
-      <geometry>
-        <box size="0.08 0.08 0.15"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.0805 0.0000 0.16345"/>
-      <geometry>
-        <box size="0.487 1.0000 0.0269"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.0805 0.48965 0.975"/>
-      <geometry>
-        <box size="0.487 0.0207 1.65"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.0896 0.0000 0.975"/>
-      <geometry>
-        <box size="0.4318 0.018568 1.65"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.0805 -0.48965 0.975"/>
-      <geometry>
-        <box size="0.487 0.0207 1.65"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975"/>
-      <geometry>
-        <box size="0.0367 1.0000 1.65"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.14465 0.0000 0.975"/>
-      <geometry>
-        <box size="0.0367 1.0000 1.65"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.0805 0.0000 1.7865"/>
-      <geometry>
-        <box size="0.487 1.0000 0.027"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.5727"/>
-      <geometry>
-        <box size="0.4299 0.469957 0.0118"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.08861 -0.244343 0.974836"/>
-      <geometry>
-        <box size="0.4299 0.469957 0.0118"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.08861 -0.244343 1.37686"/>
-      <geometry>
-        <box size="0.4299 0.469957 0.0118"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="1.5708 0 0" xyz="-0.086923 0.244373 1.66898"/>
-      <geometry>
-        <cylinder length="0.469897" radius="0.0075"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="wardrobe_door_left_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.3246 0.5 0.0000"/>
-    <parent link="wardrobe"/>
-    <child link="wardrobe_door_left"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-  </joint>
-  <link name="wardrobe_door_left">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_left.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/collision/wardrobe_door_left.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="wardrobe_door_right_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="-0.3246 -0.5 0.0000"/>
-    <parent link="wardrobe"/>
-    <child link="wardrobe_door_right"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-  </joint>
-  <link name="wardrobe_door_right">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/wardrobe_door_right.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/collision/wardrobe_door_right.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="armchair_joint" type="fixed">
-    <origin rpy="0 0 1.9199" xyz="7.9688 -0.3791 0.0"/>
-    <parent link="middle_of_the_room"/>
-    <child link="armchair"/>
-  </joint>
-  <link name="armchair">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/armchair.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/collision/armchair.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="sofa_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="8.1986 2.4096 0.0"/>
-    <parent link="middle_of_the_room"/>
-    <child link="sofa"/>
-  </joint>
-  <link name="sofa">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/sofa.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/collision/sofa.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="coffee_table_joint" type="fixed">
-    <origin rpy="0 0 1.5708" xyz="7.8034 1.02957 0.0"/>
-    <parent link="middle_of_the_room"/>
-    <child link="coffee_table"/>
-  </joint>
-  <link name="coffee_table">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/coffee_table.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0000 0.0000 0.02977"/>
-      <geometry>
-        <box size="0.58 1.08 0.05954"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.022101 0.0000 0.06552"/>
-      <geometry>
-        <box size="0.6106 0.4 0.01196"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0000 0.39975 0.220625"/>
-      <geometry>
-        <box size="0.7 0.4 0.32"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0000 -0.39975 0.220625"/>
-      <geometry>
-        <box size="0.7 0.4 0.32"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.316343 0.0000 0.22"/>
-      <geometry>
-        <box size="0.0673 0.4 0.32"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0000 0.0000 0.39"/>
-      <geometry>
-        <box size="0.7 1.2 0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="coffee_table_drawer_joint" type="prismatic">
-    <parent link="coffee_table"/>
-    <child link="coffee_table_drawer"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.5" velocity="0.5"/>
-  </joint>
-  <link name="coffee_table_drawer">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/coffee_table_drawer.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.338806 0.0000 0.21892"/>
-      <geometry>
-        <box size="0.0214 0.396 0.31569"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.051066 0.0000 0.107979"/>
-      <geometry>
-        <box size="0.5521 0.3454 0.02144"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.207974 0.0000 0.238907"/>
-      <geometry>
-        <box size="0.0214 0.3486 0.2398"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.051066 -0.186206 0.226597"/>
-      <geometry>
-        <box size="0.5521 0.0214 0.26425"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin rpy="0 0 0" xyz="-0.051066 0.186206 0.226597"/>
-      <geometry>
-        <box size="0.5521 0.0214 0.26425"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="bedside_table_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="7.2110 0.1296 0.0"/>
-    <parent link="middle_of_the_room"/>
-    <child link="bedside_table"/>
-  </joint>
-  <link name="bedside_table">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/visual/bedside_table.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0000 0.0000 0.230"/>
-      <geometry>
-        <box size="0.312 0.312 0.460"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="kitchen_base" type="fixed">
-    <origin xyz="-0.03 0.75 0.005"/>
-    <parent link="apartment_root"/>
-    <child link="kitchen_root"/>
-  </joint>
-  <link name="kitchen_root"/>
-  <!-- Frames, reference frames -->
-  <joint name="frame_A_joint" type="fixed">
-    <parent link="kitchen_root"/>
-    <origin xyz="0.49 0.696 0.080"/>
-    <child link="side_A"/>
-  </joint>
-  <!-- Bond, James Bond -->
-  <joint name="frame_B_joint" type="fixed">
-    <parent link="kitchen_root"/>
-    <origin rpy="0 0 3.14" xyz="2.91 3.7 0.11"/>
-    <child link="side_B"/>
-  </joint>
-  <link name="side_A"/>
-  <link name="side_B"/>
-  <!-- - - - - - - - - - - - - Cabinets side A - - - - - - - - - -->
-  <!-- Cabinet1 -->
-  <joint name="cabinet1_joint" type="fixed">
-    <origin xyz="0.01 0.01 1.02"/>
-    <parent link="side_A"/>
-    <child link="cabinet1"/>
-  </joint>
-  <link name="cabinet1">
-    <visual>
-      <origin xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAICabinet1W60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAICabinet1W60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet1_door_top_left_joint" type="revolute">
-    <origin rpy="0 0 3.14" xyz="0.103 -0.3 0.32"/>
-    <parent link="cabinet1"/>
-    <child link="cabinet1_door_top_left"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-  </joint>
-  <link name="cabinet1_door_top_left">
-    <visual>
-      <origin rpy="0 0 3.14" xyz="0.001 -0.305 0.35"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDoorW60H80.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14" xyz="0.001 -0.305 0.35"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDoorW60H80.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab1_top_door_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.055 -0.5 0.0"/>
-    <parent link="cabinet1_door_top_left"/>
-    <child link="handle_cab1_top_door"/>
-  </joint>
-  <link name="handle_cab1_top_door">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="oven_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="-0.175 0.0 -0.025"/>
-    <!-- <origin xyz="-0.125 0.0 -0.025" rpy="0 0 0"/> -->
-    <parent link="cabinet1"/>
-    <child link="oven"/>
-  </joint>
-  <material name="oven_gray">
-    <color rgba="0.8 0.8 0.8 1"/>
-  </material>
-  <link name="oven">
-    <visual>
-      <geometry>
-        <!-- <box size="0.58 0.58 0.59"/> -->
-        <mesh filename="package://iai_apartment/meshes/cabinet01/oven.dae"/>
-      </geometry>
-      <!-- <material name="oven_gray"/> -->
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.58 0.58 0.59"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet1_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="-0.117 0.0 -0.43"/>
-    <parent link="cabinet1"/>
-    <child link="cabinet1_drawer_middle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet1_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H26.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H26.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab1_drawer_mid_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.26 0.0 0.11"/>
-    <parent link="cabinet1_drawer_middle"/>
-    <child link="handle_cab1_drawer_mid"/>
-  </joint>
-  <link name="handle_cab1_drawer_mid">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet1_drawer_bottom_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="-0.094 0.0 -0.95"/>
-    <parent link="cabinet1"/>
-    <child link="cabinet1_drawer_bottom"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet1_drawer_bottom">
-    <visual>
-      <origin rpy="0 0 -3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H53.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 -3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet01/IAIDrawerW60H53.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab1_drawer_bottom_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.237 0.0 0.35"/>
-    <parent link="cabinet1_drawer_bottom"/>
-    <child link="handle_cab1_drawer_bottom"/>
-  </joint>
-  <link name="handle_cab1_drawer_bottom">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Collision okay surface for cabinet1-->
-  <material name="white">
-    <color rgba="1 1 1 1"/>
-  </material>
-  <joint name="cabinet1_coloksu_level4_joint" type="fixed">
-    <origin xyz="-0.172 0.005 0.281"/>
-    <parent link="cabinet1"/>
-    <child link="cabinet1_coloksu_level4"/>
-  </joint>
-  <link name="cabinet1_coloksu_level4">
-    <visual>
-      <geometry>
-        <box size="0.534 0.568 0.003"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.534 0.568 0.003"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet1_coloksu_level5_joint" type="fixed">
-    <!-- actually this should align with the CAD model but the CAD model is incorrect. -->
-    <!-- <origin xyz="-0.172 0.005 0.672"/> -->
-    <origin xyz="-0.172 0.005 0.56"/>
-    <parent link="cabinet1"/>
-    <child link="cabinet1_coloksu_level5"/>
-  </joint>
-  <link name="cabinet1_coloksu_level5">
-    <visual>
-      <geometry>
-        <box size="0.534 0.568 0.02"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.534 0.568 0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet2 -->
-  <joint name="cabinet2_joint" type="fixed">
-    <origin xyz="-0.163 0.51 0.975"/>
-    <parent link="side_A"/>
-    <child link="cabinet2"/>
-  </joint>
-  <link name="cabinet2">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAICabinet2W40H57.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAICabinet2W40H57.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet2_door_out_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.277 -0.19 0.0"/>
-    <parent link="cabinet2"/>
-    <child link="cabinet2_door_out_fancy"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1.0" lower="0.0" upper="0.02" velocity="1.5708"/>
-    <mimic joint="cabinet2_door_left_joint" multiplier="0.01"/>
-  </joint>
-  <link name="cabinet2_door_out_fancy"/>
-  <joint name="cabinet2_door_left_joint" type="revolute">
-    <parent link="cabinet2_door_out_fancy"/>
-    <child link="cabinet2_door_left"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-  </joint>
-  <link name="cabinet2_door_left">
-    <visual>
-      <origin rpy="0 0 3.14" xyz="0 -0.2 0.145"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDoorW40H212.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14" xyz="0 -0.2 0.145"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDoorW40H212.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab2_drawer_bottom_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.040 -0.29 0.05"/>
-    <parent link="cabinet2_door_left"/>
-    <child link="handle_cab2_door"/>
-  </joint>
-  <link name="handle_cab2_door">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet2_drawer_small_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="-0.01 0.0 0.17"/>
-    <parent link="cabinet2"/>
-    <child link="cabinet2_drawer_small"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="500.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet2_drawer_small">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H10.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H10.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet2_drawer_big_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.04 0.0 -0.12"/>
-    <parent link="cabinet2"/>
-    <child link="cabinet2_drawer_big"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="500.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet2_drawer_big">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H22.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet02/IAIDrawerW40H22.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet3 -->
-  <joint name="cabinet3_joint" type="fixed">
-    <origin xyz="-0.273 1.022 1.07"/>
-    <parent link="side_A"/>
-    <child link="cabinet3"/>
-  </joint>
-  <link name="cabinet3">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet03/IAICabinet3W60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet03/IAICabinet3W60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet3_door_top_out_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.39 -0.3 0.5"/>
-    <parent link="cabinet3"/>
-    <child link="cabinet3_door_top_out_fancy"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1.0" lower="0.0" upper="0.02" velocity="1.5708"/>
-    <mimic joint="cabinet3_door_top_left_joint" multiplier="0.01"/>
-  </joint>
-  <link name="cabinet3_door_top_out_fancy"/>
-  <joint name="cabinet3_door_top_left_joint" type="revolute">
-    <parent link="cabinet3_door_top_out_fancy"/>
-    <child link="cabinet3_door_top_left"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-  </joint>
-  <link name="cabinet3_door_top_left">
-    <visual>
-      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.1"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H133.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.1"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H133.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab3_door_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.036 -0.45 -0.65"/>
-    <parent link="cabinet3_door_top_left"/>
-    <child link="handle_cab3_door_top"/>
-  </joint>
-  <link name="handle_cab3_door_top">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet3_door_bottom_out_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.39 -0.3 -0.5"/>
-    <parent link="cabinet3"/>
-    <child link="cabinet3_door_bottom_out_fancy"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1.0" lower="0.0" upper="0.02" velocity="1.5708"/>
-    <mimic joint="cabinet3_door_bottom_left_joint" multiplier="0.01"/>
-  </joint>
-  <link name="cabinet3_door_bottom_out_fancy"/>
-  <joint name="cabinet3_door_bottom_left_joint" type="revolute">
-    <parent link="cabinet3_door_bottom_out_fancy"/>
-    <child link="cabinet3_door_bottom_left"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-  </joint>
-  <link name="cabinet3_door_bottom_left">
-    <visual>
-      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.165"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H80.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14" xyz="0.001 -0.3 -0.165"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet03/IAIDoorW60H80.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab3_door_bottom_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.036 -0.45 0.1"/>
-    <parent link="cabinet3_door_bottom_left"/>
-    <child link="handle_cab3_door_bottom"/>
-  </joint>
-  <link name="handle_cab3_door_bottom">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet4 -->
-  <joint name="cabinet4_joint" type="fixed">
-    <origin xyz="-0.175 1.825 1.72"/>
-    <parent link="side_A"/>
-    <child link="cabinet4"/>
-  </joint>
-  <link name="cabinet4">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIKitchenCabinet4W100.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIKitchenCabinet4W100.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet4_door_top_top_joint" type="revolute">
-    <origin rpy="0 0 3.14" xyz="0.07 0 0.42"/>
-    <parent link="cabinet4"/>
-    <child link="cabinet4_door_top"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="1000.0" lower="0" upper="2.0" velocity="1.5708"/>
-  </joint>
-  <link name="cabinet4_door_top">
-    <visual>
-      <origin rpy="0 0 3.14" xyz="0.0 0.0 -0.2"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor1W100H40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14" xyz="0.0 0.0 -0.2"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor1W100H40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet4_door_bottom_bottom_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0.0 0.0 -0.4"/>
-    <parent link="cabinet4_door_top"/>
-    <child link="cabinet4_door_bottom"/>
-    <axis xyz="0 -1 0"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-    <mimic joint="cabinet4_door_top_top_joint" multiplier="1.2"/>
-  </joint>
-  <link name="cabinet4_door_bottom">
-    <visual>
-      <origin rpy="0 0 3.14" xyz="-0.014 0.0 -0.2"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor2W100H40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14" xyz="-.014 0.0 -0.2"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet04/IAIDoor2W100H40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab4_door_bottom_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.055 0 -0.35"/>
-    <parent link="cabinet4_door_bottom"/>
-    <child link="handle_cab4_door_bottom"/>
-  </joint>
-  <link name="handle_cab4_door_bottom">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet5 -->
-  <joint name="cabinet5_joint" type="fixed">
-    <origin xyz="-0.121 1.625 0.4"/>
-    <parent link="side_A"/>
-    <child link="cabinet5"/>
-  </joint>
-  <link name="cabinet5">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenCabinet5W60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenCabinet5W60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet5_drawer_top_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.327"/>
-    <parent link="cabinet5"/>
-    <child link="cabinet5_drawer_top"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet5_drawer_top">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H13.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H13.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab5_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.05"/>
-    <parent link="cabinet5_drawer_top"/>
-    <child link="handle_cab5_t"/>
-  </joint>
-  <link name="handle_cab5_t">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet5_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.027 0.0 0.107"/>
-    <parent link="cabinet5"/>
-    <child link="cabinet5_drawer_middle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet5_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H26.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H26.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab5_mid_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
-    <parent link="cabinet5_drawer_middle"/>
-    <child link="handle_cab5_m"/>
-  </joint>
-  <link name="handle_cab5_m">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet5_drawer_bottom_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.037 0.0 -0.28"/>
-    <parent link="cabinet5"/>
-    <child link="cabinet5_drawer_bottom"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet5_drawer_bottom">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet05/IAIKitchenDrawerW60H40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab5_bott_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.235 0 0.245"/>
-    <parent link="cabinet5_drawer_bottom"/>
-    <child link="handle_cab5_b"/>
-  </joint>
-  <link name="handle_cab5_b">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet6 -->
-  <joint name="cabinet6_joint" type="fixed">
-    <origin xyz="-0.121 2.127 0.4"/>
-    <parent link="side_A"/>
-    <child link="cabinet6"/>
-  </joint>
-  <link name="cabinet6">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenCabinet6W40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenCabinet6W40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet6_drawer_top_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.033 -0.0125 0.328"/>
-    <parent link="cabinet6"/>
-    <child link="cabinet6_drawer_top"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet6_drawer_top">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H13.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H13.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab6_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.238 0 0.05"/>
-    <parent link="cabinet6_drawer_top"/>
-    <child link="handle_cab6_t"/>
-  </joint>
-  <link name="handle_cab6_t">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet6_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.105"/>
-    <parent link="cabinet6"/>
-    <child link="cabinet6_drawer_middle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet6_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H26.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H26.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab6_mid_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
-    <parent link="cabinet6_drawer_middle"/>
-    <child link="handle_cab6_m"/>
-  </joint>
-  <link name="handle_cab6_m">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet6_drawer_bottom_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.047 0.0 -0.28"/>
-    <parent link="cabinet6"/>
-    <child link="cabinet6_drawer_bottom"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet6_drawer_bottom">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet06/IAIKitchenDrawerW40H40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab6_bott_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.227 0 0.245"/>
-    <parent link="cabinet6_drawer_bottom"/>
-    <child link="handle_cab6_b"/>
-  </joint>
-  <link name="handle_cab6_b">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- - - - - - - - - - - - - - - Cabinets side B - - - - - - - - - - - - - - -->
-  <!-- Dishwasher: Cabinet 7 -->
-  <joint name="cabinet7_joint" type="fixed">
-    <origin xyz="0.2 0.501 0.47"/>
-    <parent link="side_B"/>
-    <child link="cabinet7"/>
-  </joint>
-  <link name="cabinet7">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenCabinet7W60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenCabinet7W60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet7_door_bottom_left_joint" type="revolute">
-    <origin rpy="0 0 3.14" xyz="0.372 0 -0.45"/>
-    <parent link="cabinet7"/>
-    <child link="cabinet7_door_bottom_left"/>
-    <axis xyz="0 -1 0"/>
-    <limit effort="1000.0" lower="0.0" upper="1.5708" velocity="1.5708"/>
-  </joint>
-  <link name="cabinet7_door_bottom_left">
-    <visual>
-      <origin xyz="0 0 0.385"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenDoorW60H80.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0.385"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet07/IAIKitchenDoorW60H80.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab7_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.06 0 0.74"/>
-    <parent link="cabinet7_door_bottom_left"/>
-    <child link="handle_cab7"/>
-  </joint>
-  <link name="handle_cab7">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- dishwasher rack -->
-  <joint name="dishwasher_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="-0.065 0.03 0.08"/>
-    <parent link="cabinet7"/>
-    <child link="dishwasher_drawer_middle"/>
-    <axis xyz="-1 0  0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="dishwasher_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet07/dishwasher_drawer_middle.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet07/dishwasher_drawer_middle.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab7_drawer_middle_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.315 0 0.068"/>
-    <parent link="dishwasher_drawer_middle"/>
-    <child link="handle_cab7_middle"/>
-  </joint>
-  <link name="handle_cab7_middle">
-    <visual>
-      <origin rpy="-1.57 0 0"/>
-      <geometry>
-        <box size="0.015 0.325 0.021"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <origin rpy="-1.57 0 0"/>
-      <geometry>
-        <box size="0.015 0.325 0.021"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet 8 -->
-  <joint name="cabinet8_joint" type="fixed">
-    <origin xyz="0.256 1.102 0.006"/>
-    <parent link="side_B"/>
-    <child link="cabinet8"/>
-  </joint>
-  <link name="cabinet8">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenCabinet8W60_v2.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenCabinet8W60_v2.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet8_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.05 0.0 0.48"/>
-    <parent link="cabinet8"/>
-    <child link="cabinet8_drawer_middle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.40" velocity="0.5"/>
-  </joint>
-  <link name="cabinet8_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW56H10.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW56H10.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet8_drawer_bottom_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.114 0.0 0.105"/>
-    <parent link="cabinet8"/>
-    <child link="cabinet8_drawer_bottom"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet8_drawer_bottom">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW60H80.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet08/IAIKitchenDrawerW60H80.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab8_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.238 0 0.65"/>
-    <parent link="cabinet8_drawer_bottom"/>
-    <child link="handle_cab8"/>
-  </joint>
-  <link name="handle_cab8">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet9 -->
-  <joint name="cabinet9_joint" type="fixed">
-    <origin xyz="0.333 1.703 0.394"/>
-    <parent link="side_B"/>
-    <child link="cabinet9"/>
-  </joint>
-  <link name="cabinet9">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenCabinet9W60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenCabinet9W60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet9_drawer_top_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.327"/>
-    <parent link="cabinet9"/>
-    <child link="cabinet9_drawer_top"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet9_drawer_top">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H13.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H13.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab9_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.05"/>
-    <parent link="cabinet9_drawer_top"/>
-    <child link="handle_cab9_t"/>
-  </joint>
-  <link name="handle_cab9_t">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet9_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.027 0.0 0.106"/>
-    <parent link="cabinet9"/>
-    <child link="cabinet9_drawer_middle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet9_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H26.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H26.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab9_mid_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
-    <parent link="cabinet9_drawer_middle"/>
-    <child link="handle_cab9_m"/>
-  </joint>
-  <link name="handle_cab9_m">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet9_drawer_bottom_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.036 0.0 -0.281"/>
-    <parent link="cabinet9"/>
-    <child link="cabinet9_drawer_bottom"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet9_drawer_bottom">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet09/IAIKitchenDrawerW60H40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab9_bott_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.24 0 0.245"/>
-    <parent link="cabinet9_drawer_bottom"/>
-    <child link="handle_cab9_b"/>
-  </joint>
-  <link name="handle_cab9_b">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet 10 -->
-  <joint name="cabinet10_joint" type="fixed">
-    <origin xyz="0.333 2.204 0.394"/>
-    <parent link="side_B"/>
-    <child link="cabinet10"/>
-  </joint>
-  <link name="cabinet10">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenCabinet9W40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenCabinet9W40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet10_drawer_top_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.033 0.0 0.326"/>
-    <parent link="cabinet10"/>
-    <child link="cabinet10_drawer_top"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet10_drawer_top">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H13.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H13.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab10_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.243 0 0.05"/>
-    <parent link="cabinet10_drawer_top"/>
-    <child link="handle_cab10_t"/>
-  </joint>
-  <link name="handle_cab10_t">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet10_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.031 0.0 0.104"/>
-    <parent link="cabinet10"/>
-    <child link="cabinet10_drawer_middle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet10_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H26.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H26.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab10_mid_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
-    <parent link="cabinet10_drawer_middle"/>
-    <child link="handle_cab10_m"/>
-  </joint>
-  <link name="handle_cab10_m">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet10_drawer_bottom_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.03 0.0 -0.279"/>
-    <parent link="cabinet10"/>
-    <child link="cabinet10_drawer_bottom"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet10_drawer_bottom">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet10/IAIKitchenDrawerW40H40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab10_bott_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.245"/>
-    <parent link="cabinet10_drawer_bottom"/>
-    <child link="handle_cab10_b"/>
-  </joint>
-  <link name="handle_cab10_b">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cabinet 11 -->
-  <joint name="cabinet11_joint" type="fixed">
-    <origin xyz="0.333 2.855 0.394"/>
-    <parent link="side_B"/>
-    <child link="cabinet11"/>
-  </joint>
-  <link name="cabinet11">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenCabinet11W90.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenCabinet11W90.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet11_drawer_top_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.03 0.0 0.326"/>
-    <parent link="cabinet11"/>
-    <child link="cabinet11_drawer_top"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet11_drawer_top">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H13.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H13.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab11_top_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.243 0 0.05"/>
-    <parent link="cabinet11_drawer_top"/>
-    <child link="handle_cab11_t"/>
-  </joint>
-  <link name="handle_cab11_t">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet11_drawer_middle_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.027 0.0 0.106"/>
-    <parent link="cabinet11"/>
-    <child link="cabinet11_drawer_middle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet11_drawer_middle">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H26.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H26.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab11_mid_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.245 0 0.13"/>
-    <parent link="cabinet11_drawer_middle"/>
-    <child link="handle_cab11_m"/>
-  </joint>
-  <link name="handle_cab11_m">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="cabinet11_drawer_bottom_joint" type="prismatic">
-    <origin rpy="0 0 3.14" xyz="0.045 0.0 -0.286"/>
-    <parent link="cabinet11"/>
-    <child link="cabinet11_drawer_bottom"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="0.0" upper="0.45" velocity="0.5"/>
-  </joint>
-  <link name="cabinet11_drawer_bottom">
-    <visual>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H40.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.14"/>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cabinet11/IAIKitchenDrawerW90H40.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="handle_cab11_bott_joint" type="fixed">
-    <origin rpy="1.57 0 0" xyz="-0.228 0 0.245"/>
-    <parent link="cabinet11_drawer_bottom"/>
-    <child link="handle_cab11_b"/>
-  </joint>
-  <link name="handle_cab11_b">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/handles/handle_horizontal.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Miscellaneous -->
-  <!-- Island, Main Countertop -->
-  <joint name="island_countertop_joint" type="fixed">
-    <origin xyz="0.13 1.786 0.807"/>
-    <parent link="side_B"/>
-    <child link="island_countertop"/>
-  </joint>
-  <link name="island_countertop">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/island/IAIKitchenIslandAreaTop.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/island/IAIKitchenIslandAreaTop.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Cooktop -->
-  <joint name="cooktop_joint" type="fixed">
-    <origin xyz="0.26 2.86 0.830"/>
-    <parent link="side_B"/>
-    <child link="cooktop"/>
-  </joint>
-  <link name="cooktop">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cooktop/IAIKitchenCookTop.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/cooktop/IAIKitchenCookTop.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Tap, Water Tap -->
-  <joint name="tap_joint" type="fixed">
-    <origin xyz="0.1 1.085 0.933"/>
-    <parent link="side_B"/>
-    <child link="tap"/>
-  </joint>
-  <link name="tap">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/tap/IAIKitchenWaterTap.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/tap/IAIKitchenWaterTap.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Sink, Kitchen Sink -->
-  <joint name="sink_joint" type="fixed">
-    <origin xyz="0.328 1.088 0.769"/>
-    <parent link="side_B"/>
-    <child link="sink"/>
-  </joint>
-  <link name="sink">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/sink/IAIKitchenSink.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/sink/IAIKitchenSink.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- floor socket: A -->
-  <joint name="socket_a_joint" type="fixed">
-    <origin xyz="0.095 1.02 -0.03"/>
-    <parent link="side_A"/>
-    <child link="useless_underbar_a"/>
-  </joint>
-  <link name="useless_underbar_a">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/floor_socket_side_a/IAIKitchenPartSocketW260.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/floor_socket_side_a/IAIKitchenPartSocketW260.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- floor socket: B -->
-  <joint name="socket_b_joint" type="fixed">
-    <origin xyz="0.552 1.755 -0.045"/>
-    <parent link="side_B"/>
-    <child link="useless_underbar_b"/>
-  </joint>
-  <link name="useless_underbar_b">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/floor_socket_side_b/IAIKitchenPartSocketW310.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/floor_socket_side_b/IAIKitchenPartSocketW310.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Side A countertop -->
-  <joint name="countertop_joint" type="fixed">
-    <origin xyz="-0.027 0.21 0.423"/>
-    <parent link="cabinet5"/>
-    <child link="countertop"/>
-  </joint>
-  <link name="countertop">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/countertop/IAIKitchenPartW102H60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/countertop/IAIKitchenPartW102H60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="coffe_machine_joint" type="fixed">
-    <origin xyz="0 0.3 0.2"/>
-    <parent link="countertop"/>
-    <child link="coffe_machine"/>
-  </joint>
-  <link name="coffe_machine">
-    <visual>
-      <geometry>
-        <box size="0.58 0.33 0.41"/>
-      </geometry>
-      <material name="oven_gray"/>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.58 0.32 0.41"/>
-      </geometry>
-      <material name="oven_gray"/>
-    </collision>
-  </link>
-  <!-- island waterfall, left side -->
-  <joint name="island_waterfall_left_joint" type="fixed">
-    <origin rpy="1.57 0 1.57" xyz="0.285 0.191 0.354"/>
-    <parent link="side_B"/>
-    <child link="island_waterfall_left"/>
-  </joint>
-  <link name="island_waterfall_left">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- island waterfall, right side -->
-  <joint name="island_waterfall_right_joint" type="fixed">
-    <origin rpy="1.57 0 1.57" xyz="0.285 3.315 0.354"/>
-    <parent link="side_B"/>
-    <child link="island_waterfall_right"/>
-  </joint>
-  <link name="island_waterfall_right">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartIslandUnderSideW90H60/IAIKitchenVisiblePartIslandUnderSideW90H60.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- island, back panel -->
-  <joint name="island_back_joint" type="fixed">
-    <origin rpy="0 1.57 0" xyz="-0.026 1.754 0.353"/>
-    <parent link="side_B"/>
-    <child link="island_back"/>
-  </joint>
-  <link name="island_back">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW314H90/IAIKitchenPartW314H90.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW314H90/IAIKitchenPartW314H90.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- side A left panel -->
-  <joint name="side_A_left_joint" type="fixed">
-    <origin rpy="0 0 1.57" xyz="-0.16 -0.296 1.030"/>
-    <parent link="side_A"/>
-    <child link="side_A_left"/>
-  </joint>
-  <link name="side_A_left">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW58H223/IAIKitchenPartW58H223.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenPartW58H223/IAIKitchenPartW58H223.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- side A right panel up -->
-  <joint name="side_A_right_top_joint" type="fixed">
-    <origin rpy="0 0 1.57" xyz="-0.262 2.336 1.74"/>
-    <parent link="side_A"/>
-    <child link="side_A_rt"/>
-  </joint>
-  <link name="side_A_rt">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUppersideW80H39/IAIKitchenVisiblePartUppersideW80H39.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUppersideW80H39/IAIKitchenVisiblePartUppersideW80H39.obj"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- side A right panel bottom -->
-  <joint name="side_A_right_bott_joint" type="fixed">
-    <origin rpy="1.57 0 1.57" xyz="-0.159 2.338 0.361"/>
-    <parent link="side_A"/>
-    <child link="side_A_rb"/>
-  </joint>
-  <link name="side_A_rb">
-    <visual>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUnderSideW90H58/IAIKitchenVisiblePartUnderSideW90H58.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <mesh filename="package://iai_apartment/meshes/IAIKitchenVisiblePartUnderSideW90H58/IAIKitchenVisiblePartUnderSideW90H58.obj"/>
-      </geometry>
-    </collision>
-  </link>
-</robot>
diff --git a/iai_apartment/urdf/kitchen.xarco b/iai_apartment/urdf/kitchen.xacro
similarity index 100%
rename from iai_apartment/urdf/kitchen.xarco
rename to iai_apartment/urdf/kitchen.xacro