Creating tables in the disney usecase

CMakeLists.txt

@@ -94,6 +94,7 @@ if(QT_FOUND AND SOQT_FOUND AND BULLET_FOUND )
         src/collision_specification.cpp
         src/usecase_scene.cpp
         src/tabletop_scene.cpp
+        src/process_table.cpp
     )
 
 qt4_wrap_ui(UI_HEADERS src/mainwindow.ui)

examples/disney/surface_grasp_call1.sh

@@ -0,0 +1,98 @@
+#!/bin/bash
+
+# This script performs a surface grasp check with a PROB2 in a tabletop scene.
+# The target object is
+# represented by the green bounding box. This object is a terminating
+# collision. All other collisions are prohibited.
+
+# The initial goal pose is not feasible due to the collision with the red
+# object. The manifold includes Z rotations of the end effector in the range
+# [-1.5, 1.5] and the service is able to sample a feasible goal pose from 
+# the manifold.
+rosservice call /check_kinematics_tabletop "
+initial_configuration: [1.44862328, 0.40142573, 0.75049158, 0, 1.43116999, 0]
+goal_pose:
+  position: {x: 0, y: 0.52, z: 0.190}
+  orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
+table_surface_pose:
+  position:
+    x: -0.029503589496
+    y: 0.0248198062181
+    z: 0.786985516548
+  orientation:
+    x: -0.167932495475
+    y: -0.0869415700436
+    z: -0.327809333801
+    w: 0.925624549389
+table_from_edges: true
+edge_frames:
+-
+  position:
+    x: -0.362667769194
+    y: 0.0230741873384
+    z: 0.833642959595
+  orientation:
+    x: -0.105323456787
+    y: -0.157057728398
+    z: -0.732987024524
+    w: 0.653429294557
+-
+  position:
+    x: 0.0158419162035
+    y: 0.21796926856
+    z: 0.777151107788
+  orientation:
+    x: -0.0319034574641
+    y: 0.186392955899
+    z: 0.981449229674
+    w: 0.0315796422009
+-
+  position:
+    x: 0.33892339468
+    y: -0.0316337645054
+    z: 0.89374423027
+  orientation:
+    x: -0.156191844284
+    y: 0.106603363431
+    z: 0.659397143631
+    w: 0.727623056517
+-
+  position:
+    x: -0.0395862907171
+    y: -0.226528853178
+    z: 0.950236082077
+  orientation:
+    x: -0.185376567614
+    y: -0.0373589779824
+    z: -0.0603642594255
+    w: 0.980099984842
+goal_manifold_frame:
+  position: {x: 0, y: 0.52, z: 0.196}
+  orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
+goal_manifold_orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
+bounding_boxes_with_poses:
+- box:
+    type: 0
+    dimensions: [0.08, 0.08, 0.08]
+  pose:
+    position: {x: 0, y: 0.52, z: 0.196}
+    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+- box:
+    type: 0
+    dimensions: [0.08, 0.08, 0.05]
+  pose:
+    position: {x: 0.12, y: 0.51, z: 0.185}
+    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+- box:
+    type: 0
+    dimensions: [0.04, 0.15, 0.04]
+  pose:
+    position: {x: 0.2, y: 0.5, z: 0.190}
+    orientation: {x: 0, y: 0, z: 0.3826834, w: 0.9238795}
+min_position_deltas: [-0.05, -0.05, -0.07]
+max_position_deltas: [0.05, 0.05, 0.05]
+min_orientation_deltas: [0, 0, -1.5]
+max_orientation_deltas: [0, 0, 1.5]
+allowed_collisions:
+- {type: 1, box_id: 0, terminating: true}
+" 

examples/disney/surface_grasp_sampling_orientation.sh → examples/disney/surface_grasp_call3.sh

@@ -14,9 +14,17 @@ initial_configuration: [1.44862328, 0.40142573, 0.75049158, 0, 1.43116999, 0]
 goal_pose:
   position: {x: 0, y: 0.52, z: 0.190}
   orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
-table_pose:
-  position: {x: 0.18, y: 0.6, z: 0.146}
-  orientation: {x: 0.0, y: 0.0, z: 0.7071068, w: 0.7071068}
+table_surface_pose:
+  position:
+    x: -0.142048403621
+    y: -0.157964363694
+    z: 0.673826873302
+  orientation:
+    x: -0.101292222738
+    y: -0.0305242687464
+    z: 0.00310935568996
+    w: 0.994383513927
+table_from_edges: true
 goal_manifold_frame:
   position: {x: 0, y: 0.52, z: 0.196}
   orientation: {x: 0.997, y: 0, z: 0.071, w: 0}

examples/disney/surface_grasp_call5.sh

@@ -0,0 +1,88 @@
+#!/bin/bash
+
+# This script performs a surface grasp check with a PROB2 in a tabletop scene.
+# The target object is
+# represented by the green bounding box. This object is a terminating
+# collision. All other collisions are prohibited.
+
+# The initial goal pose is not feasible due to the collision with the red
+# object. The manifold includes Z rotations of the end effector in the range
+# [-1.5, 1.5] and the service is able to sample a feasible goal pose from 
+# the manifold.
+rosservice call /check_kinematics_tabletop "
+initial_configuration: [1.44862328, 0.40142573, 0.75049158, 0, 1.43116999, 0]
+goal_pose:
+  position: {x: 0, y: 0.52, z: 0.190}
+  orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
+table_surface_pose:
+  position:
+    x: -0.0985574275255
+    y: 0.0364626981318
+    z: 0.718236863613
+  orientation:
+    x: -0.227255776525
+    y: 0.0417324751616
+    z: 0.255886077881
+    w: 0.938688218594
+table_from_edges: true
+edge_frames:
+-
+  position:
+    x: 0.23587718606
+    y: 0.0118581131101
+    z: 0.912215173244
+  orientation:
+    x: -0.199806234162
+    y: 0.116035575477
+    z: 0.557890539934
+    w: 0.797101895436
+-
+  position:
+    x: 0.0669989585876
+    y: -0.159065335989
+    z: 0.990766584873
+  orientation:
+    x: -0.23079088776
+    y: -0.0110614857981
+    z: 0.0356289074289
+    w: 0.972287924094
+-
+  position:
+    x: -0.226289525628
+    y: -0.112215168774
+    z: 0.954806029797
+  orientation:
+    x: -0.224340699888
+    y: -0.0552995889972
+    z: -0.152278669899
+    w: 0.960949765879
+goal_manifold_frame:
+  position: {x: 0, y: 0.52, z: 0.196}
+  orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
+goal_manifold_orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
+bounding_boxes_with_poses:
+- box:
+    type: 0
+    dimensions: [0.08, 0.08, 0.08]
+  pose:
+    position: {x: 0, y: 0.52, z: 0.196}
+    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+- box:
+    type: 0
+    dimensions: [0.08, 0.08, 0.05]
+  pose:
+    position: {x: 0.12, y: 0.51, z: 0.185}
+    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+- box:
+    type: 0
+    dimensions: [0.04, 0.15, 0.04]
+  pose:
+    position: {x: 0.2, y: 0.5, z: 0.190}
+    orientation: {x: 0, y: 0, z: 0.3826834, w: 0.9238795}
+min_position_deltas: [-0.05, -0.05, -0.07]
+max_position_deltas: [0.05, 0.05, 0.05]
+min_orientation_deltas: [0, 0, -1.5]
+max_orientation_deltas: [0, 0, 1.5]
+allowed_collisions:
+- {type: 1, box_id: 0, terminating: true}
+" 

examples/disney/surface_grasp_reference.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+
+# This script performs a surface grasp check. The target object is
+# represented by the green bounding box. This object is a terminating
+# collision. All other collisions (except the bottom of the container) are prohibited.
+
+# This will check an easy go down movement. Collisions are only allowed with 
+# the bottom of the container and the object (collision is required). This movement
+# is feasible and thus the planner does not have to generate an alternative 
+# trajectory.
+rosservice call /check_kinematics_tabletop "
+initial_configuration: [-0.4857, 0.4931, 1.259, 1.604, 1.786, -1.804]
+goal_pose:
+  position: [0.498, 0.129, 0.164]
+  orientation: {x: 0.685114499881, y: 0.663267754151, z: 0.217253436334, w: -0.208554435956}
+table_surface_pose:
+  orientation: [0.0, 0.986, 0.16700000000000007, 0.0]
+  position: [0.498, 0.129, 0.134]
+bounding_boxes_with_poses:
+- box: 
+    type: 1
+    dimensions: [0.11955147981643677, 0.0790453553199768, 0.06020838022232056]
+  pose: 
+    position: [0.498, 0.129, 0.164]
+    orientation: {x: 0.000448179713371, y: -0.00122113167373, z: -0.0165826804441, w: 0.999861651771}
+goal_manifold_frame:
+    position: [0.498, 0.129, 0.134]
+    orientation: {x: 0.000448179713371, y: -0.00122113167373, z: -0.0165826804441, w: 0.999861651771}
+goal_manifold_orientation: {x: 0.685114499881, y: 0.663267754151, z: 0.217253436334, w: -0.208554435956}
+min_position_deltas: [-0.04, -0.07, 0.03]
+max_position_deltas: [0.04, -0.01, 0.07]
+min_orientation_deltas: [0, 0, -0.5]
+max_orientation_deltas: [0, 0, 0.5]
+allowed_collisions:
+- {type: 1, box_id: 0, terminating: true}
+- {type: 2, constraint_name: 'bottom', terminating: false}
+"

examples/disney/surface_grasp_sampling_simple.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+
+# This script performs a surface grasp check. The target object is
+# represented by the green bounding box. This object is a terminating
+# collision. All other collisions (except the bottom of the container) are prohibited.
+
+# This will check an easy go down movement. Collisions are only allowed with 
+# the bottom of the container and the object (collision is required). This movement
+# is feasible and thus the planner does not have to generate an alternative 
+# trajectory.
+rosservice call /check_kinematics_tabletop "
+initial_configuration: [0.42180752185581194, 0.5116913273945006, 1.592272893703166, 0.424190933949368, 1.2527738218055204, -1.7908341943112134]
+goal_pose:
+  position: {x: 0.524368202392, y: -0.110797924543, z: -0.044818284}
+  orientation: {x: 0.685114499881, y: 0.663267754151, z: 0.217253436334, w: -0.208554435956}
+table_surface_pose:
+  position: {x: 0.500191347818, y: -0.0999358441943, z: -0.065988522}
+  orientation: {x: -0.000566068955935, y: -0.00117116004218, z: 0.707109752686, w: 0.707102613209}
+bounding_boxes_with_poses:
+- box: 
+    type: 1
+    dimensions: [0.11955147981643677, 0.0790453553199768, 0.06020838022232056]
+  pose: 
+    position: {x: 0.548410148325, y: -0.0112594768472, z: 0.005324942}
+    orientation: {x: 0.000448179713371, y: -0.00122113167373, z: -0.0165826804441, w: 0.999861651771}
+goal_manifold_frame:
+    position: {x: 0.548410148325, y: -0.0112594768472, z: 0.005324942}
+    orientation: {x: 0.000448179713371, y: -0.00122113167373, z: -0.0165826804441, w: 0.999861651771}
+goal_manifold_orientation: {x: 0.685114499881, y: 0.663267754151, z: 0.217253436334, w: -0.208554435956}
+min_position_deltas: [-0.04, -0.07, 0.03]
+max_position_deltas: [0.04, -0.01, 0.07]
+min_orientation_deltas: [0, 0, -0.5]
+max_orientation_deltas: [0, 0, 0.5]
+allowed_collisions:
+- {type: 1, box_id: 0, terminating: true}
+- {type: 2, constraint_name: 'bottom', terminating: false}
+"

examples/tabletop_surface_grasp_sampling_orientation.sh

@@ -13,9 +13,9 @@ initial_configuration: [0, 0.1, 0, 2.3, 0, 0.5, 0]
 goal_pose:
   position: {x: 0.4, y: -0.02, z: -0.096}
   orientation: {x: 0.997, y: 0, z: 0.071, w: 0}
-table_pose:
+table_surface_pose:
   position: {x: 0.48, y: 0, z: -0.146}
-  orientation: {x: 0.0, y: 0.0, z: 0, w: 1}
+  orientation: {x: 1.0, y: 0.0, z: 0, w: 0}
 goal_manifold_frame:
   position: {x: 0.4, y: -0.02, z: -0.096}
   orientation: {x: 0.997, y: 0, z: 0.071, w: 0}

launch/disney_feasibility_check.launch

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <launch>
+    <rosparam param="/feasibility_check/table_dimensions">[0.6, 0.8, 0.1]</rosparam>
     <node name="feasibility_check" pkg="tub_feasibility_check" type="tub_feasibility_check" output="screen">
         <param name="ifco_scene_graph_file" value="$(find tub_feasibility_check)/model/rlsg/prob-rbohand2-ifco.convex.xml" />
         <param name="tabletop_scene_graph_file" value="$(find tub_feasibility_check)/model/rlsg/prob-rbohand2-tabletop.convex.xml" />

launch/disney_feasibility_check_reference.launch

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+    <rosparam param="/feasibility_check/table_dimensions">[0.3, 0.3, 0.3]</rosparam>
+    <node name="feasibility_check" pkg="tub_feasibility_check" type="tub_feasibility_check" output="screen">
+        <param name="ifco_scene_graph_file" value="$(find tub_feasibility_check)/model/rlsg/prob-rbohand2-ifco.convex.xml" />
+        <param name="tabletop_scene_graph_file" value="$(find tub_feasibility_check)/model/rlsg/prob-rbohand2-tabletop.convex.xml" />
+        <param name="kinematics_file" value="$(find tub_feasibility_check)/model/rlkin/prob-rbohand2.xml"/>
+        <param name="hide_window" type="bool" value="false" /> 
+    </node>
+</launch>
+
+

launch/tub_feasibility_check.launch

@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
-    <node name="feasibility_check" pkg="tub_feasibility_check" type="tub_feasibility_check">
+    <rosparam param="/feasibility_check/table_dimensions">[0.8, 0.6, 0.1]</rosparam>
+    <node name="feasibility_check" pkg="tub_feasibility_check" type="tub_feasibility_check" output="screen">
         <param name="ifco_scene_graph_file" value="$(find tub_feasibility_check)/model/rlsg/wam-rbohand2-with-camera-ifco.convex.xml" />
         <param name="tabletop_scene_graph_file" value="$(find tub_feasibility_check)/model/rlsg/wam-rbohand2-with-camera-tabletop.convex.xml" />
         <param name="kinematics_file" value="$(find tub_feasibility_check)/model/rlkin/barrett-wam-rbohand2.xml"/>

model/rlsg/prob-rbohand2-tabletop.convex.wrl

@@ -8,9 +8,12 @@ Transform {
 				}
 			]
 		}
-		DEF tabletop Inline {
-			url "tabletop.wrl"
-		}
+		# This is needed so the corresponding xml file is processed right
+		# This empty shape will be removed during the tabletop
+		# service calls and a new shape will be created
+    DEF tabletop Shape
+    {
+    }
 	]
 }
 

model/rlsg/wam-rbohand2-with-camera-tabletop.convex.wrl

@@ -8,9 +8,12 @@ Transform {
 				}
 			]
 		}
-		DEF tabletop Inline {
-			url "tabletop.wrl"
-		}
+		# This is needed so the corresponding xml file is processed right
+		# This empty shape will be removed during the tabletop
+		# service calls and a new shape will be created
+    DEF tabletop Shape
+    {
+    }
 	]
 }