crawll back scenario

CMakeLists.txt

@@ -64,14 +64,16 @@ install(FILES
   data/urdf/box_rom.urdf
   data/urdf/box.urdf
   data/urdf/scene.urdf
-  data/urdf/scene2.urdf
+  data/urdf/scene_wall.urdf
+  #~ data/urdf/scene2.urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
   )
 install(FILES
   data/srdf/box_rom.srdf
   data/srdf/box.srdf
   data/srdf/scene.srdf
-  data/srdf/scene2.srdf
+  data/srdf/scene_wall.srdf
+  #~ data/srdf/scene2.srdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
   )
 install(FILES
@@ -82,6 +84,7 @@ install(FILES
 	data/meshes/robot_box.stl
 	data/meshes/chair_simple.stl
 	data/meshes/ground.stl
+	data/meshes/ground_table.stl
 	#~ data/meshes/pedal.stl
 	#~ data/meshes/board.stl
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes

data/srdf/scene_wall.srdf

+<?xml version="1.0"?>
+<robot name="scene_wall">
+  <handle name="handle">
+    <position> 0 0 0 1 0 0 0 </position>
+    <link name="base_link"/>
+  </handle>
+  <handle name="handle2">
+    <position> 0 0 0
+      0 0 0.7071067811865476 0.7071067811865476 </position>
+    <link name="base_link"/>
+  </handle>
+  <contact name="box_surface">
+    <link name="base_link"/>
+    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025
+           +0.025 -0.025 -0.025   +0.025 0.025 -0.025   +0.025 -0.025 0.025   +0.025 0.025 0.025 </point>
+    <triangle> 0 2 1 4 5 6</triangle>
+  </contact>
+</robot>