merged with refactoring branch

config/config.yaml

-connector_node_ros_cgv:
 cgv_address: "tcp://*:6576"
-scene_controller:
 max_grasp_approach_velocity: 0.2
 max_grasp_approach_acceleration: 0.2
 max_grasp_transition_velocity: 0.2

config/config_scene.yaml

-{ "objects": [
-  { "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
-  { "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
-  { "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
-  { "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
-  { "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
-  { "id": "binBlue","type": "BIN","pos": { "x": -0.34,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
-  { "id": "binRed","type": "BIN","pos": { "x": 0.06,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
-  { "id": "binGreen","type": "BIN","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } },
-  { "id": "objectRed1","type": "BOX","pos": { "x": 0.5,"y": -0.1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
-  { "id": "objectRed2","type": "BOX","pos": { "x": 0.25,"y": -0.2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } },
-  { "id": "objectRed3","type": "BOX","pos": { "x": -0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
-  { "id": "objectGreen1","type": "BOX","pos": { "x": 0.45,"y": -0.3,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
-  { "id": "objectGreen2","type": "BOX","pos": { "x": -0.45,"y": -0.2,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 1,"w": 0.92388 },"color": { "g": 1 } },
-  { "id": "objectGreen3","type": "BOX","pos": { "x": 0.1,"y": -0.3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
-  { "id": "objectBlue1","type": "BOX","pos": { "x": 0.25,"y": -0.4,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
-  { "id": "objectBlue2","type": "BOX","pos": { "x": -0.3,"y": -0.3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
-  { "id": "objectBlue3","type": "BOX","pos": { "x": 0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
-  { "id": "arm","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
-] }
+"{ 'objects': [
+  { 'id': 'tablePillar1','pos': { 'x': 0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.46,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+  { 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.25,'y': -0.2,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'objectRed3','type': 'BOX','pos': { 'x': -0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+  { 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+  { 'id': 'objectGreen2','type': 'BOX','pos': { 'x': -0.45,'y': -0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+  { 'id': 'objectGreen3','type': 'BOX','pos': { 'x': 0.1,'y': -0.3,'z': 0.819 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+  { 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+  { 'id': 'objectBlue2','type': 'BOX','pos': { 'x': -0.3,'y': -0.3,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+  { 'id': 'objectBlue3','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+  { 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }"

src/ccf/util/scene_controller_node.cpp

@@ -40,7 +40,7 @@ namespace world_state {
     std::string current_picked_object_id;
     std::string current_bin_id;
     geometry_msgs::Point current_transform;
-    double open_amount = 0.08;
+    double open_amount = 0.078;
     bool is_initialized = false;
     bool is_table_configured = false;
     bool is_simulated_robot = false;