From 5a69b183cedf66357c5271180735331bb9732eba Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Sun, 9 Apr 2023 16:54:39 +0200
Subject: [PATCH] generate 'sortierkonfiguartion' for each robot as result, can
 be run with auerswald package, ToDo: generate launch files

---
 include/mediator/grasp_mediator.h             |  2 +-
 launch/config_routine.launch                  | 13 +++--
 .../-1234778859.yaml}                         |  0
 .../configs/-1234778859_panda_arm1.yaml       | 12 +++++
 .../configs/-1234778859_panda_arm2.yaml       | 12 +++++
 .../-1505208581.yaml}                         |  0
 .../configs/-1505208581_panda_arm1.yaml       | 12 +++++
 .../configs/-1505208581_panda_arm2.yaml       | 12 +++++
 .../-1775841698.yaml}                         |  0
 .../configs/-1775841698_panda_arm1.yaml       | 12 +++++
 .../configs/-1775841698_panda_arm2.yaml       | 12 +++++
 .../-1982814221.yaml}                         |  0
 .../configs/-1982814221_panda_arm1.yaml       | 12 +++++
 .../configs/-1982814221_panda_arm2.yaml       | 12 +++++
 .../-276051748.yaml}                          | 16 +++----
 .../configs/-276051748_panda_arm1.yaml        | 12 +++++
 .../configs/-276051748_panda_arm2.yaml        | 12 +++++
 .../-317753156.yaml}                          |  0
 .../configs/-317753156_panda_arm1.yaml        | 12 +++++
 .../configs/-317753156_panda_arm2.yaml        | 12 +++++
 .../-348871069.yaml}                          |  0
 .../configs/-348871069_panda_arm1.yaml        | 12 +++++
 .../configs/-348871069_panda_arm2.yaml        | 12 +++++
 .../-509770749.yaml}                          |  0
 .../configs/-509770749_panda_arm1.yaml        | 12 +++++
 .../configs/-509770749_panda_arm2.yaml        | 12 +++++
 .../-650013114.yaml}                          |  0
 .../configs/-650013114_panda_arm1.yaml        | 12 +++++
 .../configs/-650013114_panda_arm2.yaml        | 12 +++++
 .../1334033851.yaml}                          |  0
 .../configs/1334033851_panda_arm1.yaml        | 12 +++++
 .../configs/1334033851_panda_arm2.yaml        | 12 +++++
 .../1879793725.yaml}                          |  0
 .../configs/1879793725_panda_arm1.yaml        | 12 +++++
 .../configs/1879793725_panda_arm2.yaml        | 12 +++++
 .../1938185681.yaml}                          |  0
 .../configs/1938185681_panda_arm1.yaml        | 12 +++++
 .../configs/1938185681_panda_arm2.yaml        | 12 +++++
 .../1938901935.yaml}                          |  0
 .../configs/1938901935_panda_arm1.yaml        | 12 +++++
 .../configs/1938901935_panda_arm2.yaml        | 12 +++++
 .../2082550165.yaml}                          |  0
 .../configs/2082550165_panda_arm1.yaml        | 12 +++++
 .../configs/2082550165_panda_arm2.yaml        | 12 +++++
 .../493882106.yaml}                           |  0
 .../configs/493882106_panda_arm1.yaml         | 12 +++++
 .../configs/493882106_panda_arm2.yaml         | 12 +++++
 .../66805984.yaml}                            |  0
 .../66805984/configs/66805984_panda_arm1.yaml | 12 +++++
 .../66805984/configs/66805984_panda_arm2.yaml | 12 +++++
 .../812255525.yaml}                           | 16 +++----
 .../configs/812255525_panda_arm1.yaml         | 12 +++++
 .../configs/812255525_panda_arm2.yaml         | 12 +++++
 .../95613651.yaml}                            |  0
 .../95613651/configs/95613651_panda_arm1.yaml | 12 +++++
 .../95613651/configs/95613651_panda_arm2.yaml | 12 +++++
 src/config_routine.cpp                        |  5 +-
 src/mediator/base_calculation_mediator.cpp    | 47 ++++++++++++++++++-
 src/mediator/grasp_mediator.cpp               |  8 +++-
 59 files changed, 510 insertions(+), 29 deletions(-)
 rename results/dummy/{-1243119928.yaml => -1234778859/-1234778859.yaml} (100%)
 create mode 100644 results/dummy/-1234778859/configs/-1234778859_panda_arm1.yaml
 create mode 100644 results/dummy/-1234778859/configs/-1234778859_panda_arm2.yaml
 rename results/dummy/{877736799.yaml => -1505208581/-1505208581.yaml} (100%)
 create mode 100644 results/dummy/-1505208581/configs/-1505208581_panda_arm1.yaml
 create mode 100644 results/dummy/-1505208581/configs/-1505208581_panda_arm2.yaml
 rename results/dummy/{978579717.yaml => -1775841698/-1775841698.yaml} (100%)
 create mode 100644 results/dummy/-1775841698/configs/-1775841698_panda_arm1.yaml
 create mode 100644 results/dummy/-1775841698/configs/-1775841698_panda_arm2.yaml
 rename results/dummy/{516584596.yaml => -1982814221/-1982814221.yaml} (100%)
 create mode 100644 results/dummy/-1982814221/configs/-1982814221_panda_arm1.yaml
 create mode 100644 results/dummy/-1982814221/configs/-1982814221_panda_arm2.yaml
 rename results/dummy/{-243824774.yaml => -276051748/-276051748.yaml} (87%)
 create mode 100644 results/dummy/-276051748/configs/-276051748_panda_arm1.yaml
 create mode 100644 results/dummy/-276051748/configs/-276051748_panda_arm2.yaml
 rename results/dummy/{-1462055903.yaml => -317753156/-317753156.yaml} (100%)
 create mode 100644 results/dummy/-317753156/configs/-317753156_panda_arm1.yaml
 create mode 100644 results/dummy/-317753156/configs/-317753156_panda_arm2.yaml
 rename results/dummy/{-467577807.yaml => -348871069/-348871069.yaml} (100%)
 create mode 100644 results/dummy/-348871069/configs/-348871069_panda_arm1.yaml
 create mode 100644 results/dummy/-348871069/configs/-348871069_panda_arm2.yaml
 rename results/dummy/{-702828614.yaml => -509770749/-509770749.yaml} (100%)
 create mode 100644 results/dummy/-509770749/configs/-509770749_panda_arm1.yaml
 create mode 100644 results/dummy/-509770749/configs/-509770749_panda_arm2.yaml
 rename results/dummy/{-999728640.yaml => -650013114/-650013114.yaml} (100%)
 create mode 100644 results/dummy/-650013114/configs/-650013114_panda_arm1.yaml
 create mode 100644 results/dummy/-650013114/configs/-650013114_panda_arm2.yaml
 rename results/dummy/{1261526688.yaml => 1334033851/1334033851.yaml} (100%)
 create mode 100644 results/dummy/1334033851/configs/1334033851_panda_arm1.yaml
 create mode 100644 results/dummy/1334033851/configs/1334033851_panda_arm2.yaml
 rename results/dummy/{1473687055.yaml => 1879793725/1879793725.yaml} (100%)
 create mode 100644 results/dummy/1879793725/configs/1879793725_panda_arm1.yaml
 create mode 100644 results/dummy/1879793725/configs/1879793725_panda_arm2.yaml
 rename results/dummy/{1527063277.yaml => 1938185681/1938185681.yaml} (100%)
 create mode 100644 results/dummy/1938185681/configs/1938185681_panda_arm1.yaml
 create mode 100644 results/dummy/1938185681/configs/1938185681_panda_arm2.yaml
 rename results/dummy/{713225941.yaml => 1938901935/1938901935.yaml} (100%)
 create mode 100644 results/dummy/1938901935/configs/1938901935_panda_arm1.yaml
 create mode 100644 results/dummy/1938901935/configs/1938901935_panda_arm2.yaml
 rename results/dummy/{1963457492.yaml => 2082550165/2082550165.yaml} (100%)
 create mode 100644 results/dummy/2082550165/configs/2082550165_panda_arm1.yaml
 create mode 100644 results/dummy/2082550165/configs/2082550165_panda_arm2.yaml
 rename results/dummy/{-755583968.yaml => 493882106/493882106.yaml} (100%)
 create mode 100644 results/dummy/493882106/configs/493882106_panda_arm1.yaml
 create mode 100644 results/dummy/493882106/configs/493882106_panda_arm2.yaml
 rename results/dummy/{-720586033.yaml => 66805984/66805984.yaml} (100%)
 create mode 100644 results/dummy/66805984/configs/66805984_panda_arm1.yaml
 create mode 100644 results/dummy/66805984/configs/66805984_panda_arm2.yaml
 rename results/dummy/{-1704450111.yaml => 812255525/812255525.yaml} (86%)
 create mode 100644 results/dummy/812255525/configs/812255525_panda_arm1.yaml
 create mode 100644 results/dummy/812255525/configs/812255525_panda_arm2.yaml
 rename results/dummy/{-1851930455.yaml => 95613651/95613651.yaml} (100%)
 create mode 100644 results/dummy/95613651/configs/95613651_panda_arm1.yaml
 create mode 100644 results/dummy/95613651/configs/95613651_panda_arm2.yaml

diff --git a/include/mediator/grasp_mediator.h b/include/mediator/grasp_mediator.h
index 2428b23..e0e6d34 100644
--- a/include/mediator/grasp_mediator.h
+++ b/include/mediator/grasp_mediator.h
@@ -18,12 +18,12 @@ class GraspMediator : public AbstractMediator{
     std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
     std::unique_ptr<moveit_grasps_demo::GraspPipelineDemo> grasp_pipeline_demo_; //!< Auerswald grasp pipeline
     std::string referenceRobot_; //!< Reference Robot
+    bool autoMapping_;//!< override graspmap config
 
 
     public:
     GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh);
     void mediate() override;
-    inline void setReferenceRobot(std::string& str) {referenceRobot_ = str;}
     void connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) override;
     void setPanel() override;
 
diff --git a/launch/config_routine.launch b/launch/config_routine.launch
index c397a43..0364939 100644
--- a/launch/config_routine.launch
+++ b/launch/config_routine.launch
@@ -1,15 +1,18 @@
 
 <launch>
   <!-- specify the planning pipeline -->
-  <arg name="reference_robot" default="panda_arm1" />
-  <arg name="referenceXYZ" default="-0.320002 -0.000003 0.89" />
+  <arg name="referenceRobot" default="panda_arm1" />
+  <arg name="autoMapping" default="true" />
+  <arg name="referenceXYZ" default="-0.22 -0.2 0.89" />
   <arg name="referenceRPY" default="0 0 0" />
 
-  <arg name="result" default="dummy/-243824774.yaml" />
+  <arg name="result" default="dummy/-276051748/-276051748.yaml" />
 
   <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
 
-  <rosparam param="reference_robot" subst_value="True"> $(arg reference_robot)</rosparam>
+  <rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
+  <rosparam param="autoMapping" subst_value="True"> $(arg autoMapping)</rosparam>
+
 
   <arg name="pipeline" default="ompl" />
 
@@ -86,7 +89,7 @@
     <param name="planning_group_name" value="panda_arm_hand"/>
     <rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
     <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-    <rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-276051748/configs/-276051748_panda_arm1.yaml"/>
   </node>
 
   <!-- Planning Functionality -->
diff --git a/results/dummy/-1243119928.yaml b/results/dummy/-1234778859/-1234778859.yaml
similarity index 100%
rename from results/dummy/-1243119928.yaml
rename to results/dummy/-1234778859/-1234778859.yaml
diff --git a/results/dummy/-1234778859/configs/-1234778859_panda_arm1.yaml b/results/dummy/-1234778859/configs/-1234778859_panda_arm1.yaml
new file mode 100644
index 0000000..3fe15e1
--- /dev/null
+++ b/results/dummy/-1234778859/configs/-1234778859_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, 0.099998, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, 0.099998, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-1234778859/configs/-1234778859_panda_arm2.yaml b/results/dummy/-1234778859/configs/-1234778859_panda_arm2.yaml
new file mode 100644
index 0000000..f727ffe
--- /dev/null
+++ b/results/dummy/-1234778859/configs/-1234778859_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100005, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100005, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/877736799.yaml b/results/dummy/-1505208581/-1505208581.yaml
similarity index 100%
rename from results/dummy/877736799.yaml
rename to results/dummy/-1505208581/-1505208581.yaml
diff --git a/results/dummy/-1505208581/configs/-1505208581_panda_arm1.yaml b/results/dummy/-1505208581/configs/-1505208581_panda_arm1.yaml
new file mode 100644
index 0000000..df52dd3
--- /dev/null
+++ b/results/dummy/-1505208581/configs/-1505208581_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, 0.099998, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, 0.099998, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-1505208581/configs/-1505208581_panda_arm2.yaml b/results/dummy/-1505208581/configs/-1505208581_panda_arm2.yaml
new file mode 100644
index 0000000..72a394f
--- /dev/null
+++ b/results/dummy/-1505208581/configs/-1505208581_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/978579717.yaml b/results/dummy/-1775841698/-1775841698.yaml
similarity index 100%
rename from results/dummy/978579717.yaml
rename to results/dummy/-1775841698/-1775841698.yaml
diff --git a/results/dummy/-1775841698/configs/-1775841698_panda_arm1.yaml b/results/dummy/-1775841698/configs/-1775841698_panda_arm1.yaml
new file mode 100644
index 0000000..5ca9e7e
--- /dev/null
+++ b/results/dummy/-1775841698/configs/-1775841698_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -0.100002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -0.100002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-1775841698/configs/-1775841698_panda_arm2.yaml b/results/dummy/-1775841698/configs/-1775841698_panda_arm2.yaml
new file mode 100644
index 0000000..0ae335f
--- /dev/null
+++ b/results/dummy/-1775841698/configs/-1775841698_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.205, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.205, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/516584596.yaml b/results/dummy/-1982814221/-1982814221.yaml
similarity index 100%
rename from results/dummy/516584596.yaml
rename to results/dummy/-1982814221/-1982814221.yaml
diff --git a/results/dummy/-1982814221/configs/-1982814221_panda_arm1.yaml b/results/dummy/-1982814221/configs/-1982814221_panda_arm1.yaml
new file mode 100644
index 0000000..bbf69db
--- /dev/null
+++ b/results/dummy/-1982814221/configs/-1982814221_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 0.91], 'rot': [0, -0, -3.14159], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -2.04146e-06, 0.99], 'rot': [0, -0, -3.14159]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-1982814221/configs/-1982814221_panda_arm2.yaml b/results/dummy/-1982814221/configs/-1982814221_panda_arm2.yaml
new file mode 100644
index 0000000..9d1f0b5
--- /dev/null
+++ b/results/dummy/-1982814221/configs/-1982814221_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-3.67233e-06, 1.305, 0.91], 'rot': [0, -0, 1.33714e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-3.67233e-06, 1.305, 0.99], 'rot': [0, -0, 1.33714e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-243824774.yaml b/results/dummy/-276051748/-276051748.yaml
similarity index 87%
rename from results/dummy/-243824774.yaml
rename to results/dummy/-276051748/-276051748.yaml
index 841590a..0a68d83 100644
--- a/results/dummy/-243824774.yaml
+++ b/results/dummy/-276051748/-276051748.yaml
@@ -7,7 +7,7 @@
 { 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
+{ 'id' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': -0.200002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
 { 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
 { 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
@@ -16,11 +16,11 @@
 { 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005 , 'y': 1.304998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.100007 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id' : 'table2_table_top', 'pos': { 'x': -0.000006 , 'y': 1.402498 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
+{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.000008 , 'y': 2.054998 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
 { 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.9725 },  '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.9725 },    '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.9725 },  'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
@@ -30,6 +30,6 @@
 { 'id': 'object4','type': 'BOX','pos': { 'x': -0.6,'y': -0.4,'z': 0.996 },   'size': { 'length': 0.031,'width': 0.062,'height': 0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
 { 'id': 'object5','type': 'BOX','pos': { 'x': -0.4,'y': -0.5,'z': 0.959 },  'size': { 'length': 0.031,'width': 0.031,'height': 0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
 { 'id': 'obstacle1','pos': { 'x': 0.34,'y': -0.11,'z': 0.94 },  'size': { 'length': 0.3,'width': 0.3,'height': 0.1 },'orientation': { 'z': 0.452123,'w': 1 },'color': { 'b': 1 } },
-{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
-{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220006, 'y': 1.402497, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
 ]}
\ No newline at end of file
diff --git a/results/dummy/-276051748/configs/-276051748_panda_arm1.yaml b/results/dummy/-276051748/configs/-276051748_panda_arm1.yaml
new file mode 100644
index 0000000..f294e99
--- /dev/null
+++ b/results/dummy/-276051748/configs/-276051748_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -0.200002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -0.200002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-276051748/configs/-276051748_panda_arm2.yaml b/results/dummy/-276051748/configs/-276051748_panda_arm2.yaml
new file mode 100644
index 0000000..9bc7e19
--- /dev/null
+++ b/results/dummy/-276051748/configs/-276051748_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-6.04679e-06, 1.4025, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-6.04679e-06, 1.4025, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-1462055903.yaml b/results/dummy/-317753156/-317753156.yaml
similarity index 100%
rename from results/dummy/-1462055903.yaml
rename to results/dummy/-317753156/-317753156.yaml
diff --git a/results/dummy/-317753156/configs/-317753156_panda_arm1.yaml b/results/dummy/-317753156/configs/-317753156_panda_arm1.yaml
new file mode 100644
index 0000000..4359a90
--- /dev/null
+++ b/results/dummy/-317753156/configs/-317753156_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, -0.100002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, -0.100002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-317753156/configs/-317753156_panda_arm2.yaml b/results/dummy/-317753156/configs/-317753156_panda_arm2.yaml
new file mode 100644
index 0000000..5e62f59
--- /dev/null
+++ b/results/dummy/-317753156/configs/-317753156_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100006, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100006, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-467577807.yaml b/results/dummy/-348871069/-348871069.yaml
similarity index 100%
rename from results/dummy/-467577807.yaml
rename to results/dummy/-348871069/-348871069.yaml
diff --git a/results/dummy/-348871069/configs/-348871069_panda_arm1.yaml b/results/dummy/-348871069/configs/-348871069_panda_arm1.yaml
new file mode 100644
index 0000000..3be7bc7
--- /dev/null
+++ b/results/dummy/-348871069/configs/-348871069_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, -2.04146e-06, 0.91], 'rot': [0, -0, -3.14159], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, -2.04146e-06, 0.99], 'rot': [0, -0, -3.14159]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-348871069/configs/-348871069_panda_arm2.yaml b/results/dummy/-348871069/configs/-348871069_panda_arm2.yaml
new file mode 100644
index 0000000..b351ce4
--- /dev/null
+++ b/results/dummy/-348871069/configs/-348871069_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100004, 1.305, 0.91], 'rot': [0, -0, 1.33714e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100004, 1.305, 0.99], 'rot': [0, -0, 1.33714e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-702828614.yaml b/results/dummy/-509770749/-509770749.yaml
similarity index 100%
rename from results/dummy/-702828614.yaml
rename to results/dummy/-509770749/-509770749.yaml
diff --git a/results/dummy/-509770749/configs/-509770749_panda_arm1.yaml b/results/dummy/-509770749/configs/-509770749_panda_arm1.yaml
new file mode 100644
index 0000000..5ca9e7e
--- /dev/null
+++ b/results/dummy/-509770749/configs/-509770749_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -0.100002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -0.100002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-509770749/configs/-509770749_panda_arm2.yaml b/results/dummy/-509770749/configs/-509770749_panda_arm2.yaml
new file mode 100644
index 0000000..0ae335f
--- /dev/null
+++ b/results/dummy/-509770749/configs/-509770749_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.205, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.205, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-999728640.yaml b/results/dummy/-650013114/-650013114.yaml
similarity index 100%
rename from results/dummy/-999728640.yaml
rename to results/dummy/-650013114/-650013114.yaml
diff --git a/results/dummy/-650013114/configs/-650013114_panda_arm1.yaml b/results/dummy/-650013114/configs/-650013114_panda_arm1.yaml
new file mode 100644
index 0000000..3714fe6
--- /dev/null
+++ b/results/dummy/-650013114/configs/-650013114_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-650013114/configs/-650013114_panda_arm2.yaml b/results/dummy/-650013114/configs/-650013114_panda_arm2.yaml
new file mode 100644
index 0000000..532e48d
--- /dev/null
+++ b/results/dummy/-650013114/configs/-650013114_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1261526688.yaml b/results/dummy/1334033851/1334033851.yaml
similarity index 100%
rename from results/dummy/1261526688.yaml
rename to results/dummy/1334033851/1334033851.yaml
diff --git a/results/dummy/1334033851/configs/1334033851_panda_arm1.yaml b/results/dummy/1334033851/configs/1334033851_panda_arm1.yaml
new file mode 100644
index 0000000..4040d4a
--- /dev/null
+++ b/results/dummy/1334033851/configs/1334033851_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1334033851/configs/1334033851_panda_arm2.yaml b/results/dummy/1334033851/configs/1334033851_panda_arm2.yaml
new file mode 100644
index 0000000..54ae0dc
--- /dev/null
+++ b/results/dummy/1334033851/configs/1334033851_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100005, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100005, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1473687055.yaml b/results/dummy/1879793725/1879793725.yaml
similarity index 100%
rename from results/dummy/1473687055.yaml
rename to results/dummy/1879793725/1879793725.yaml
diff --git a/results/dummy/1879793725/configs/1879793725_panda_arm1.yaml b/results/dummy/1879793725/configs/1879793725_panda_arm1.yaml
new file mode 100644
index 0000000..df52dd3
--- /dev/null
+++ b/results/dummy/1879793725/configs/1879793725_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, 0.099998, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, 0.099998, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1879793725/configs/1879793725_panda_arm2.yaml b/results/dummy/1879793725/configs/1879793725_panda_arm2.yaml
new file mode 100644
index 0000000..72a394f
--- /dev/null
+++ b/results/dummy/1879793725/configs/1879793725_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1527063277.yaml b/results/dummy/1938185681/1938185681.yaml
similarity index 100%
rename from results/dummy/1527063277.yaml
rename to results/dummy/1938185681/1938185681.yaml
diff --git a/results/dummy/1938185681/configs/1938185681_panda_arm1.yaml b/results/dummy/1938185681/configs/1938185681_panda_arm1.yaml
new file mode 100644
index 0000000..bbf69db
--- /dev/null
+++ b/results/dummy/1938185681/configs/1938185681_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 0.91], 'rot': [0, -0, -3.14159], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -2.04146e-06, 0.99], 'rot': [0, -0, -3.14159]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1938185681/configs/1938185681_panda_arm2.yaml b/results/dummy/1938185681/configs/1938185681_panda_arm2.yaml
new file mode 100644
index 0000000..9d1f0b5
--- /dev/null
+++ b/results/dummy/1938185681/configs/1938185681_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-3.67233e-06, 1.305, 0.91], 'rot': [0, -0, 1.33714e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-3.67233e-06, 1.305, 0.99], 'rot': [0, -0, 1.33714e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/713225941.yaml b/results/dummy/1938901935/1938901935.yaml
similarity index 100%
rename from results/dummy/713225941.yaml
rename to results/dummy/1938901935/1938901935.yaml
diff --git a/results/dummy/1938901935/configs/1938901935_panda_arm1.yaml b/results/dummy/1938901935/configs/1938901935_panda_arm1.yaml
new file mode 100644
index 0000000..3be7bc7
--- /dev/null
+++ b/results/dummy/1938901935/configs/1938901935_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, -2.04146e-06, 0.91], 'rot': [0, -0, -3.14159], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, -2.04146e-06, 0.99], 'rot': [0, -0, -3.14159]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1938901935/configs/1938901935_panda_arm2.yaml b/results/dummy/1938901935/configs/1938901935_panda_arm2.yaml
new file mode 100644
index 0000000..b351ce4
--- /dev/null
+++ b/results/dummy/1938901935/configs/1938901935_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100004, 1.305, 0.91], 'rot': [0, -0, 1.33714e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100004, 1.305, 0.99], 'rot': [0, -0, 1.33714e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/1963457492.yaml b/results/dummy/2082550165/2082550165.yaml
similarity index 100%
rename from results/dummy/1963457492.yaml
rename to results/dummy/2082550165/2082550165.yaml
diff --git a/results/dummy/2082550165/configs/2082550165_panda_arm1.yaml b/results/dummy/2082550165/configs/2082550165_panda_arm1.yaml
new file mode 100644
index 0000000..f294e99
--- /dev/null
+++ b/results/dummy/2082550165/configs/2082550165_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -0.200002, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -0.200002, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/2082550165/configs/2082550165_panda_arm2.yaml b/results/dummy/2082550165/configs/2082550165_panda_arm2.yaml
new file mode 100644
index 0000000..9bc7e19
--- /dev/null
+++ b/results/dummy/2082550165/configs/2082550165_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-6.04679e-06, 1.4025, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-6.04679e-06, 1.4025, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-755583968.yaml b/results/dummy/493882106/493882106.yaml
similarity index 100%
rename from results/dummy/-755583968.yaml
rename to results/dummy/493882106/493882106.yaml
diff --git a/results/dummy/493882106/configs/493882106_panda_arm1.yaml b/results/dummy/493882106/configs/493882106_panda_arm1.yaml
new file mode 100644
index 0000000..3fe15e1
--- /dev/null
+++ b/results/dummy/493882106/configs/493882106_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, 0.099998, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, 0.099998, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/493882106/configs/493882106_panda_arm2.yaml b/results/dummy/493882106/configs/493882106_panda_arm2.yaml
new file mode 100644
index 0000000..f727ffe
--- /dev/null
+++ b/results/dummy/493882106/configs/493882106_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100005, 1.405, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100005, 1.405, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-720586033.yaml b/results/dummy/66805984/66805984.yaml
similarity index 100%
rename from results/dummy/-720586033.yaml
rename to results/dummy/66805984/66805984.yaml
diff --git a/results/dummy/66805984/configs/66805984_panda_arm1.yaml b/results/dummy/66805984/configs/66805984_panda_arm1.yaml
new file mode 100644
index 0000000..74bd7bb
--- /dev/null
+++ b/results/dummy/66805984/configs/66805984_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.200002, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.200002, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/66805984/configs/66805984_panda_arm2.yaml b/results/dummy/66805984/configs/66805984_panda_arm2.yaml
new file mode 100644
index 0000000..0908f3f
--- /dev/null
+++ b/results/dummy/66805984/configs/66805984_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.200005, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.200005, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-1704450111.yaml b/results/dummy/812255525/812255525.yaml
similarity index 86%
rename from results/dummy/-1704450111.yaml
rename to results/dummy/812255525/812255525.yaml
index 649ef1f..6060605 100644
--- a/results/dummy/-1704450111.yaml
+++ b/results/dummy/812255525/812255525.yaml
@@ -7,7 +7,7 @@
 { 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': -0.200002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
+{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
 { 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
 { 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
@@ -16,11 +16,11 @@
 { 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
 { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_table_top', 'pos': { 'x': -0.000006 , 'y': 1.402498 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.000008 , 'y': 2.054998 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005 , 'y': 1.304998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
+{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.100007 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
 { 'id': 'blue1',  'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'blue2',  'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'blue3',  'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
@@ -28,6 +28,6 @@
 { 'id': 'green2',  'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.91, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'red1',  'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.6, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
 { 'id': 'red2',  'type': 'BOX', 'pos': { 'x': 0.3, 'y': 1.41, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
-{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
-{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220006, 'y': 1.402497, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
 ]}
\ No newline at end of file
diff --git a/results/dummy/812255525/configs/812255525_panda_arm1.yaml b/results/dummy/812255525/configs/812255525_panda_arm1.yaml
new file mode 100644
index 0000000..4040d4a
--- /dev/null
+++ b/results/dummy/812255525/configs/812255525_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100002, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100002, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/812255525/configs/812255525_panda_arm2.yaml b/results/dummy/812255525/configs/812255525_panda_arm2.yaml
new file mode 100644
index 0000000..54ae0dc
--- /dev/null
+++ b/results/dummy/812255525/configs/812255525_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-0.100005, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-0.100005, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/-1851930455.yaml b/results/dummy/95613651/95613651.yaml
similarity index 100%
rename from results/dummy/-1851930455.yaml
rename to results/dummy/95613651/95613651.yaml
diff --git a/results/dummy/95613651/configs/95613651_panda_arm1.yaml b/results/dummy/95613651/configs/95613651_panda_arm1.yaml
new file mode 100644
index 0000000..3714fe6
--- /dev/null
+++ b/results/dummy/95613651/configs/95613651_panda_arm1.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-2.04146e-06, -2.04146e-06, 0.91], 'rot': [0, -0, 2.49943e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-2.04146e-06, -2.04146e-06, 0.99], 'rot': [0, -0, 2.49943e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/results/dummy/95613651/configs/95613651_panda_arm2.yaml b/results/dummy/95613651/configs/95613651_panda_arm2.yaml
new file mode 100644
index 0000000..532e48d
--- /dev/null
+++ b/results/dummy/95613651/configs/95613651_panda_arm2.yaml
@@ -0,0 +1,12 @@
+mapspace: {'dim': [0.4, 0.4, 0.04], 'pos': [-5.30321e-06, 1.305, 0.91], 'rot': [0, -0, 2.67427e-06], 'verbose': false }
+voxel_size: 0.02 
+voxel_space: {'dim': [0.45, 0.45, 0.2], 'pos': [-5.30321e-06, 1.305, 0.99], 'rot': [0, -0, 2.67427e-06]}
+voxel_verbose_level: 0
+translation_rate: 0.03
+rotation_rate: 45.0
+rotation_max: 90.0
+clearance: 0.01
+min_quality: 0.1
+max_candidate_count: 10
+allow_dependencies: true
+top_grasps_only: true
diff --git a/src/config_routine.cpp b/src/config_routine.cpp
index cf9976e..c333c3e 100644
--- a/src/config_routine.cpp
+++ b/src/config_routine.cpp
@@ -7,7 +7,7 @@
 
 int main(int argc, char *argv[]) {
 
-    ros::init(argc, argv, "config_routine");
+    ros::init(argc, argv, "config_routine"); 
     std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
 
     // Allow the action server to recieve and send ros messages
@@ -23,9 +23,6 @@ int main(int argc, char *argv[]) {
         mediator->connectRobots(std::move(ceti_panda));
     }
 
-    std::string demo_robot;
-    n->getParam("/reference_robot", demo_robot);
-    mediator->setReferenceRobot(demo_robot);
     mediator->mediate();
 
     while (ros::ok()){
diff --git a/src/mediator/base_calculation_mediator.cpp b/src/mediator/base_calculation_mediator.cpp
index e13a242..a8f79de 100644
--- a/src/mediator/base_calculation_mediator.cpp
+++ b/src/mediator/base_calculation_mediator.cpp
@@ -553,6 +553,12 @@ void Base_calculation_mediator::write_file(){
 */
 
 void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution){
+
+    std::string resultFile = std::to_string(static_cast<int>(ros::Time::now().toNSec()));
+    std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" +  filename_ + "/" + resultFile);
+    std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" +  filename_ + "/" + resultFile + "/configs");
+    std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" +  filename_ + "/" + resultFile + "/launch");
+
     std::stringstream ss;
     std::stringstream panel_ss;
     std::stringstream root_ss;
@@ -604,6 +610,45 @@ void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution
             ss << "{ 'id' : 'table" << match[1] << "_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
             ss << "{ 'id' : 'table" << match[1] << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << " , 'y': "<< std::to_string(pos_y) << " , 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << " , 'p': " << std::to_string(p) << " , 'y': " << std::to_string(y) << " } },\n";
 
+            std::stringstream config;
+            config << "mapspace: {'dim': [";
+            config << size_x/2 << ", ";
+            config << size_y/2 << ", 0.04], ";
+            config << "'pos': [";
+            config << pos_x << ", ";
+            config << pos_y << ", ";
+            config << (pos_z + size_z/2.0f + 0.04f/2.0f) << "], ";
+            config << "'rot': [";
+            config << r << ", " << p << ", " << y <<"], ";
+            config << "'verbose': false }\n";
+
+            config << "voxel_size: 0.02 \n";
+
+            config << "voxel_space: {'dim': [";
+            config << (size_x + 0.1f)/2 << ", ";
+            config << (size_y + 0.1f)/2 << ", 0.2], ";
+            config << "'pos': [";
+            config << pos_x << ", ";
+            config << pos_y << ", ";
+            config << (pos_z + size_z/2.0f + 0.2f/2.0f) << "], ";
+            config << "'rot': [";
+            config << r << ", " << p << ", " << y <<"]}\n";
+
+            config << "voxel_verbose_level: 0\n";
+            config << "translation_rate: 0.03\n";
+            config << "rotation_rate: 45.0\n";
+            config << "rotation_max: 90.0\n";
+            config << "clearance: 0.01\n";
+            config << "min_quality: 0.1\n";
+            config << "max_candidate_count: 10\n";
+            config << "allow_dependencies: true\n";
+            config << "top_grasps_only: true\n";
+
+            std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/configs/" + resultFile + "_"+ wc_solution[c].name_+".yaml");
+            o << config.str();
+            o.close();
+
+
             std::bitset<3> panel_mask(wc_solution[c].wing_config_);
             for(std::size_t i = 0; i < ceti->observers().size(); i++){
                 if(ceti->observerMask()[i] & panel_mask[i]){
@@ -645,7 +690,7 @@ void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution
    
 
 
-    std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
+    std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/" + resultFile + ".yaml");
     ss << panel_ss.str();
     ss << box_ss.str();
     ss << root_ss.str();
diff --git a/src/mediator/grasp_mediator.cpp b/src/mediator/grasp_mediator.cpp
index dfa9bd8..cfce4fd 100644
--- a/src/mediator/grasp_mediator.cpp
+++ b/src/mediator/grasp_mediator.cpp
@@ -2,9 +2,12 @@
 
 GraspMediator::GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh) 
     : AbstractMediator(nh)
-    , grasp_pipeline_demo_(std::make_unique<moveit_grasps_demo::GraspPipelineDemo>())
     , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
+    , grasp_pipeline_demo_(std::make_unique<moveit_grasps_demo::GraspPipelineDemo>())
     {
+        nh->getParam("/referenceRobot", referenceRobot_); 
+        nh->getParam("/autoMapping", autoMapping_);
+        
     }
 
 void GraspMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) {
@@ -51,6 +54,7 @@ void GraspMediator::setPanel(){
 
 void GraspMediator::mediate() {
     setPanel();
+    //if(autoMapping_)mapReconfigure();
     ros::Duration(2).sleep();
     std::vector<Cuboid> relevant_box;
     std::vector<Cuboid> relevant_obstacle;
@@ -115,4 +119,4 @@ void GraspMediator::mediate() {
     grasp_pipeline_demo_->getVoxelizedEnv();
     grasp_pipeline_demo_->computeConfig();
     grasp_pipeline_demo_->build();
-}
+}
\ No newline at end of file
-- 
GitLab