evaluation

CMakeLists.txt

@@ -78,6 +78,7 @@ src/bt/parallel_robot.cpp
 src/mediator/abstract_mediator.cpp
 src/mediator/base_calculation_mediator.cpp
 src/mediator/moveit_mediator.cpp
+src/mediator/grasp_mediator.cpp
 
 src/reader/abstract_param_reader.cpp
 src/reader/cuboid_reader.cpp
@@ -113,6 +114,44 @@ src/bt/parallel_robot.cpp
 src/mediator/abstract_mediator.cpp
 src/mediator/base_calculation_mediator.cpp
 src/mediator/moveit_mediator.cpp
+src/mediator/grasp_mediator.cpp
+
+
+src/reader/abstract_param_reader.cpp
+src/reader/cuboid_reader.cpp
+src/reader/job_reader.cpp
+src/reader/map_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/task_space_reader.cpp
+src/reader/ts_reader.cpp
+src/reader/wing_reader.cpp
+
+src/robot/decorators/panda_decorator.cpp
+src/robot/decorators/ur5_decorator.cpp
+src/robot/decorators/ur10_decorator.cpp
+src/robot/abstract_robot.cpp
+src/robot/ceti_robot.cpp
+
+src/robot_element/decorators/abstract_robot_element_decorator.cpp
+src/robot_element/decorators/log_decorator.cpp
+src/robot_element/observers/moveit_panel.cpp
+src/robot_element/observers/panel.cpp
+src/robot_element/observers/rviz_panel.cpp
+)
+
+add_executable(config_routine src/config_routine.cpp
+src/base_bridge/simple_base_implementation.cpp
+src/base_bridge/simple_base.cpp
+
+src/bt/execution.cpp
+src/bt/position_condition.cpp
+src/bt/parallel_robot.cpp
+
+src/mediator/abstract_mediator.cpp
+src/mediator/base_calculation_mediator.cpp
+src/mediator/moveit_mediator.cpp
+src/mediator/grasp_mediator.cpp
+
 
 src/reader/abstract_param_reader.cpp
 src/reader/cuboid_reader.cpp
@@ -150,6 +189,7 @@ src/bt/parallel_robot.cpp
 )
 
 
+add_dependencies(config_routine ${config_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(cell_routine ${cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(mtc2taskspace ${mtc2taskspace_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(robot_base_calculation_approach ${robot_base_calculation_approach_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -163,6 +203,14 @@ target_link_libraries(robot_base_calculation_approach
   yaml-cpp
 )
 
+target_link_libraries(config_routine 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  ${PCL_LIBRARY_DIRS}
+  ${BEHAVIOR_TREE_LIBRARY}
+  yaml-cpp
+)
+
 target_link_libraries(cell_routine 
   ${catkin_LIBRARIES}
   ${OCTOMAP_LIBRARIES}

descriptions/dummy.yaml

-task:
-  groups:
-    panda_arm1:
-      - -0.300000 -0.700000 0.890000
-      - -0.300000 -0.600000 0.890000
-      - -0.200000 -0.700000 0.890000
-      - -0.200000 -0.600000 0.890000
-      - 0.100000 -0.700000 0.890000
-      - 0.100000 -0.600000 0.890000
-      - 0.100000 -0.300000 0.890000
-      - 0.100000 -0.200000 0.890000
-      - 0.100000 -0.100000 0.890000
-      - 0.200000 -0.300000 0.890000
-      - 0.200000 -0.200000 0.890000
-      - 0.200000 -0.100000 0.890000
-      - 0.300000 -0.300000 0.890000
-      - 0.300000 -0.200000 0.890000
-      - 0.300000 -0.100000 0.890000
-      - 0.100000 0.100000 0.890000
-      - 0.100000 0.200000 0.890000
-      - 0.100000 0.300000 0.890000
-      - 0.200000 0.100000 0.890000
-      - 0.200000 0.200000 0.890000
-      - 0.200000 0.300000 0.890000
-      - 0.300000 0.100000 0.890000
-      - 0.300000 0.200000 0.890000
-      - 0.300000 0.300000 0.890000
-      - -0.300000 0.600000 0.890000
-      - -0.300000 0.700000 0.890000
-      - -0.200000 0.600000 0.890000
-      - -0.200000 0.700000 0.890000
-      - 0.100000 0.600000 0.890000
-      - 0.100000 0.700000 0.890000
-    panda_arm2:
-      - 0.100000 1.010000 0.890000
-      - 0.100000 1.110000 0.890000
-      - 0.100000 1.210000 0.890000
-      - 0.200000 1.010000 0.890000
-      - 0.200000 1.110000 0.890000
-      - 0.200000 1.210000 0.890000
-      - 0.300000 1.010000 0.890000
-      - 0.300000 1.110000 0.890000
-      - 0.300000 1.210000 0.890000
-      - 0.100000 1.410000 0.890000
-      - 0.100000 1.510000 0.890000
-      - 0.100000 1.610000 0.890000
-      - 0.200000 1.410000 0.890000
-      - 0.200000 1.510000 0.890000
-      - 0.200000 1.610000 0.890000
-      - 0.300000 1.410000 0.890000
-      - 0.300000 1.510000 0.890000
-      - 0.300000 1.610000 0.890000
-      - -0.300000 1.910000 0.890000
-      - -0.300000 2.010000 0.890000
-      - -0.200000 1.910000 0.890000
-      - -0.200000 2.010000 0.890000
-      - 0.100000 1.910000 0.890000
-      - 0.100000 2.010000 0.890000
-      - -0.300000 0.600000 0.890000
-      - -0.300000 0.700000 0.890000
-      - -0.200000 0.600000 0.890000
-      - -0.200000 0.700000 0.890000
-      - 0.100000 0.600000 0.890000
-      - 0.100000 0.700000 0.890000
\ No newline at end of file

descriptions/dummy2.yaml

-task:
-  groups:
-    panda_arm1:
-      - -0.300000 -0.700000 0.890000
-      - -0.300000 -0.600000 0.890000
-      - -0.200000 -0.700000 0.890000
-      - -0.200000 -0.600000 0.890000
-      - 0.100000 -0.700000 0.890000
-      - 0.100000 -0.600000 0.890000
-      - 0.100000 -0.300000 0.890000
-      - 0.100000 -0.200000 0.890000
-      - 0.100000 -0.100000 0.890000
-      - 0.200000 -0.300000 0.890000
-      - 0.200000 -0.200000 0.890000
-      - 0.200000 -0.100000 0.890000
-      - 0.300000 -0.300000 0.890000
-      - 0.300000 -0.200000 0.890000
-      - 0.300000 -0.100000 0.890000
-      - 0.100000 0.100000 0.890000
-      - 0.100000 0.200000 0.890000
-      - 0.100000 0.300000 0.890000
-      - 0.200000 0.100000 0.890000
-      - 0.200000 0.200000 0.890000
-      - 0.200000 0.300000 0.890000
-      - 0.300000 0.100000 0.890000
-      - 0.300000 0.200000 0.890000
-      - 0.300000 0.300000 0.890000
-    panda_arm2:
-      - 0.100000 1.110000 0.890000
-      - 0.100000 1.210000 0.890000
-      - 0.200000 1.010000 0.890000
-      - 0.200000 1.110000 0.890000
-      - 0.200000 1.210000 0.890000
-      - 0.300000 1.010000 0.890000
-      - 0.300000 1.110000 0.890000
-      - 0.300000 1.210000 0.890000
-      - 0.100000 1.410000 0.890000
-      - 0.100000 1.510000 0.890000
-      - 0.100000 1.610000 0.890000
-      - 0.200000 1.410000 0.890000
-      - 0.200000 1.510000 0.890000
-      - 0.200000 1.610000 0.890000
-      - 0.300000 1.410000 0.890000
-      - 0.300000 1.510000 0.890000
-      - 0.300000 1.610000 0.890000
-      - -0.300000 1.910000 0.890000
-      - -0.300000 2.010000 0.890000
-      - -0.200000 1.910000 0.890000
-      - -0.200000 2.010000 0.890000
-      - 0.100000 1.910000 0.890000
-      - 0.100000 2.010000 0.890000
\ No newline at end of file

include/mediator/grasp_mediator.h

+#ifndef GRASP_MEDIATOR_
+#define GRASP_MEDIATOR_
+
+#include "abstract_mediator.h"
+#include "moveit_mediator.h"
+#include "gb_grasp/MapConfigLoader.h"
+#include "gb_grasp/GraspMap.h"
+#include "gb_grasp/Cuboid.h"
+#include "gb_grasp/MapGenerator.h"
+#include "gb_grasp/VoxelManager.h"
+
+
+class GraspMediator : public MoveitMediator{
+    protected:
+    std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools
+    std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
+
+
+    public:
+    GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh);
+    void mediate() override;
+    void generateGraspMap();
+};
+
+#endif
\ No newline at end of file

include/mediator/moveit_mediator.h

@@ -58,12 +58,10 @@
 class MoveitMediator : public AbstractMediator{
     protected:
     std::shared_ptr<moveit::core::RobotModel> robot_model_; //!< Moveit Robot-Model as specified in SDF
-    std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools
     std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_; //!< Move Group Interface of the whole multi-cell
     std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_; //!< PlanningSceneInteface to manage Scene Objects
     std::shared_ptr<planning_scene::PlanningScene> ps_; //!< Shared Planning Scene
     std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_; //!< Publisher to manage PlanningScene diffs
-    std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
     std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_; //!< Moveit task Constructior simple planner
     std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_; //!< Moveit task Constructior cartesian planner
     std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_; //!< Tasks mapped to Robot

jobs/eval.yaml

+{ 'tasks': 
+  {'groups' : [
+    { 'name': 'panda_arm1', 'jobs':[
+      [
+        { 'pos': { 'x': 0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+      ], 
+      [
+        { 'pos': { 'x': 0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+      ], 
+      ]
+    }
+  ]
+  }
+}

jobs/eval_dual.yaml

+{ 'tasks': 
+  {'groups' : [
+    { 'name': 'panda_arm1', 'jobs':[
+      [
+        { 'pos': { 'x': 0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+      ]]},
+    { 'name': 'panda_arm2', 'jobs':[
+      [
+        { 'pos': { 'x': 0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+        { 'pos': { 'x': -0.2 ,'y': -0.1,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+      ], 
+      ]
+    }
+  ]
+  }
+}

launch/base_routine.launch

-<launch>
-    <arg name="map" default="dummy.yaml" />
-    <arg name="description" default="dummy.yaml" />
-    <arg name="resource" default="dummy" />
-
-
-    <rosparam command="load" file="$(find multi_cell_builder)/maps/$(arg map)"/>
-    <rosparam command="load" file="$(find multi_cell_builder)/descriptions/$(arg description)"/>
-    <rosparam command="load" file="$(find multi_cell_builder)/resources/$(arg resource).yaml"/>
-
-    <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
-    <node pkg="multi_cell_builder" type="base_routine" name="base_routine" output="screen" required="true"/>
-</launch>
\ No newline at end of file

launch/cell_routine.launch

 <launch>    
-    <arg name="result" default="dummy/-277257222.yaml" />
-    <arg name="jobs" default="jobs/dummy.yaml" />
+    <arg name="result" default="evaluation/double_eval.yaml" />
+    <arg name="jobs" default="jobs/eval_dual.yaml" />
 
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
 
@@ -8,7 +8,6 @@
     <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
     <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
-    <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
     <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
 
 

launch/config_routine.launch

+<launch>    
+    <arg name="result" default="dummy/-277257222.yaml" />
+    <arg name="jobs" default="jobs/dummy.yaml" />
+
+    <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+
+    <!-- this is to change-->
+    <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
+    <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
+
+    <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+
+    <include file="$(find ceti_double)/launch/demo.launch"> 
+        <arg name="use_rviz" value="false"/>
+        <arg name="scene" value="$(arg result)" />
+    </include> 
+    
+    <include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+    <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
+    <include file="$(find ceti_double)/launch/moveit_rviz.launch">
+        <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+    </include>
+
+    <node pkg="multi_cell_builder" type="config_routine" name="config_routine" output="screen" required="true">
+        <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
+    </node>
+</launch>
+
+    

launch/single_cell_routine.launch

+<launch>    
+    <arg name="result" default="evaluation/single_eval.yaml" />
+    <arg name="jobs" default="jobs/eval.yaml" />
+
+    <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+
+    <!-- this is to change-->
+    <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
+    <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
+    <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
+
+    <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+
+    <include file="$(find ceti_single)/launch/demo.launch"> 
+        <arg name="use_rviz" value="false"/>
+        <arg name="scene" value="$(arg result)" />
+    </include> 
+    
+    <include ns="cell_routine" file="$(find ceti_single)/launch/fake_moveit_controller_manager.launch.xml" />
+    <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
+    <include file="$(find ceti_single)/launch/moveit_rviz.launch">
+        <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+    </include>
+
+    <node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+    </node>
+
+    <node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+    </node>
+</launch>
+
+    

resources/dummy_single.yaml

+# create json file using
+# yq eval -o=json config_scene_ceti-table-placeworld.yaml > config_scene_ceti-table-placeworld.json
+{ 'objects': [
+  # table
+  # height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total
+  # wheels width/depth/height 12*12*12
+  # body width/depth/height 69*69*76
+  # table-top width/depth/height 80*80*1
+  # panels are 50*70, with 0.5 cm distance to the table-top
+  # height of both is 12 + 76 + .5 = 88.5
+  # their centre is at 40 + 0.25 + 25 = 65.25
+  # a filled table and a backplate going all the way up is not possible, because of the oversized collision model of the robot
+  #  { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_wheel_1',     'pos': { 'x': 0.28,'y': 0.28,'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': 'table1_wheel_2',     'pos': { 'x': -0.28,'y': -0.28,'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': 'table1_wheel_3',     'pos': { 'x': -0.28,'y': 0.28,'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': 'table1_wheel_4',     'pos': { 'x': 0.28,'y': -0.28,'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': 'table1_body_front',  'pos': { 'x': .345, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_body_back',   'pos': { 'x': -.345, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_body_left',   'pos': { 'y': .345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_body_right',  'pos': { 'y': -.345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_table_top',   'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_left_panel',  'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_right_panel',  'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'shared_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+  # table 2 is shifted in y direction by the size of the base, the size of a side panel and two 5mm spaces:
+  # 0.8 + 0.5 + 2*0.05 = 1.31
+
+  { 'id': 'A1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'A2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'A3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'A4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'B1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'B2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'B3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'B4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'C1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'C4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'C7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'D1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'D4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'D7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'E1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'E2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'E3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'E4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'F1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'F2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'F3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'F4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'G1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'G4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'G7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'H1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'H4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'H7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'I1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'I2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'I3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'I4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'J1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'J2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'J3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'J4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  # the collaboration zone has height 120cm
+  { 'id': 'cz1', 'type': 'COLLABORATION_ZONE', 'pos': { 'z': 1.49, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.3,'height': 1.2 },'orientation': { 'w': 1 },'color': { 'r': 1,'g': 0.627,'b': 0 } },
+
+  { 'id': 'blue1',  'type': 'BOX', 'pos': { 'x':  .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+  { 'id': 'blue2',  'type': 'BOX', 'pos': { 'x':  .2, 'y':  .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+  { 'id': 'blue3',  'type': 'BOX', 'pos': { 'x':  .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
+  { 'id': 'green1', 'type': 'BOX', 'pos': { 'x':  .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'green2', 'type': 'BOX', 'pos': { 'x':  .1, 'y':  1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'red1',   'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'red2',   'type': 'BOX', 'pos': { 'x':  .3, 'y':  1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+
+  { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.22,            'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+] }

results/dummy/-359616072.yaml

@@ -22,4 +22,6 @@
 { 'id': 'table2_right_panel' , 'pos': { 'x': -0.000005 , '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': 'arm1','type': 'ARM','pos': { 'x': 0.219998, 'y': -0.000002, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 1.000000, 'w': -0.000001 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
 { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220004, 'y': 1.304998, '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': 'blue1',  'type': 'BOX', 'pos': { 'x':  -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2',  'type': 'BOX', 'pos': { 'x':  0.1, 'y':  1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
 ]}
\ No newline at end of file

results/dummy/-548755835.yaml

@@ -23,4 +23,6 @@
 { 'id': 'table2_right_panel' , 'pos': { 'x': -0.000007 , 'y': 1.857498 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.100003, '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.220005, 'y': 1.204997, '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': 'blue1',  'type': 'BOX', 'pos': { 'x':  -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2',  'type': 'BOX', 'pos': { 'x':  0.1, 'y':  1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
 ]}
\ No newline at end of file

results/dummy/-564062842.yaml

@@ -22,4 +22,6 @@
 { 'id': 'table2_right_panel' , 'pos': { 'x': -0.000007 , 'y': 2.057498 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, '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.220005, 'y': 1.404997, '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': 'blue1',  'type': 'BOX', 'pos': { 'x':  -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2',  'type': 'BOX', 'pos': { 'x':  0.1, 'y':  1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
 ]}
\ No newline at end of file

results/evaluation/double_eval.yaml

+{ 'objects' : [ 
+{ 'id' : 'table1_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' : 'table1_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' : 'table1_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 }}, 
+{ 'id' : 'table1_wheel_4', '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' : 'table1_body_front', '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_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.099998 , '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 }}, 
+{ 'id' : 'table2_wheel_4', '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_body_front', '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_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.000005 , 'y': -0.75 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, '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': 'blue1',  'type': 'BOX', 'pos': { 'x':  0.2, 'y': 0.4, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2',  'type': 'BOX', 'pos': { 'x':  0.2, 'y': -0.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+]}
\ No newline at end of file

results/evaluation/single_eval.yaml

+{ 'objects' : [ 
+{ 'id' : 'table1_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' : 'table1_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' : 'table1_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 }}, 
+{ 'id' : 'table1_wheel_4', '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' : 'table1_body_front', '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_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.099998 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, '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': 'blue1',  'type': 'BOX', 'pos': { 'x':  0.2, 'y': 0.4, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2',  'type': 'BOX', 'pos': { 'x':  0.2, 'y': -0.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+]}
\ No newline at end of file

src/config_routine.cpp

+#include "mediator/abstract_mediator.h"
+#include "mediator/grasp_mediator.h"
+#include "robot/decorators/panda_decorator.h"
+
+
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "config_routine");
+    std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
+
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    std::shared_ptr<GraspMediator> mediator = std::make_shared<GraspMediator>(n);
+
+    auto rd = mediator->robotReader()->robotData();
+    for (int i = 0; i < rd.size() ;i++){
+        std::unique_ptr<AbstractRobot> ceti = std::make_unique<CetiRobot>(rd[i].name_, rd[i].pose_, rd[i].size_);
+        std::unique_ptr<PandaDecorator> ceti_panda = std::make_unique<PandaDecorator>(std::move(ceti));
+        mediator->connectRobots(std::move(ceti_panda));
+    }
+
+    //mediator->set_dirname(filename);
+    
+    mediator->mediate();
+
+    while (ros::ok()){
+        ros::spinOnce();
+    }
+}
\ No newline at end of file

src/mediator/grasp_mediator.cpp

+#include "mediator/grasp_mediator.h"
+
+GraspMediator::GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh) 
+    : MoveitMediator(nh)
+    , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
+    {
+        if (planning_scene_monitor_->getPlanningScene()){
+            planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
+                                                              "planning_scene");
+            planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
+        } else {
+            ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
+            return;
+        }
+
+        visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
+        robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
+        visual_tools_->loadMarkerPub();
+        visual_tools_->loadRemoteControl();
+        visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
+
+        visual_tools_->loadRobotStatePub("/display_robot_state");
+        visual_tools_->setRobotStateTopic("/robot_state_publisher");
+        visual_tools_->loadTrajectoryPub("/display_planned_path");
+
+        visual_tools_->loadSharedRobotState();
+        visual_tools_->enableBatchPublishing();
+        visual_tools_->deleteAllMarkers();
+        visual_tools_->removeAllCollisionObjects();
+        //visual_tools_->hideRobot();
+
+        visual_tools_->trigger();
+
+        // Publish the global frame
+        visual_tools_->publishAxis(Eigen::Isometry3d::Identity());
+        visual_tools_->trigger();
+    }
+
+void GraspMediator::mediate() {
+    setPanel();
+    publishTables();
+    generateGraspMap();
+	ros::waitForShutdown();
+}
+
+void GraspMediator::generateGraspMap() {   
+    auto cd = cuboid_reader_->cuboidBin();
+
+	//std::vector<std::string> objs =  {"bottle1", "bottle2"};
+
+	for (int i = 0; i < cd.size(); i ++){
+		std::stringstream ss;
+		ss << "box_" << i;
+
+		moveit_msgs::CollisionObject item;
+		item.id = ss.str();
+		item.header.frame_id = "world";
+		item.header.stamp = ros::Time::now();
+		item.primitives.resize(1);
+		item.primitives[0].type = item.primitives[0].BOX;
+		item.primitives[0].dimensions.resize(3);
+		item.primitives[0].dimensions[0] = cd[i].x_depth;
+		item.primitives[0].dimensions[1] = cd[i].y_width;
+		item.primitives[0].dimensions[2] = cd[i].z_heigth;
+
+		item.primitive_poses.resize(1);
+		item.primitive_poses[0].position.x = cd[i].Pose.position.x;
+		item.primitive_poses[0].position.y = cd[i].Pose.position.y;
+		item.primitive_poses[0].position.z = cd[i].Pose.position.z;
+		item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
+		item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
+		item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
+		item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
+		item.operation = item.ADD;
+
+		psi_->applyCollisionObject(item);
+		// Could also safe id's somewhere
+	}
+
+    ROS_INFO("Debug1!");
+    auto arm_jmg_ = robot_model_->getJointModelGroup(robots_.at("panda_arm1")->name());
+    ROS_INFO("Debug1.5!");
+    auto voxel_manager_ = std::make_shared<VoxelManager>(*nh_,"",visual_tools_,planning_scene_monitor_);
+
+    ROS_INFO("%f", robots_.at("panda_arm1")->tf().getOrigin().getZ());
+    ROS_INFO("Debug2!");
+    Cuboid voxelSpace("VS");
+    voxelSpace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
+    voxelSpace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
+    voxelSpace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.1f;
+    voxelSpace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
+    voxelSpace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
+    voxelSpace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
+    voxelSpace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
+    voxelSpace.x_depth = 0.3f;
+    voxelSpace.y_width = 0.3f;
+    voxelSpace.z_heigth = 0.2f;
+
+    voxel_manager_->setVerboseLevel(0);
+    voxel_manager_->setVoxelSize(0.02f);
+    voxel_manager_->setVoxelSpace(voxelSpace);
+    ROS_INFO("Debug2.5!");
+
+    XmlRpc::XmlRpcValue data;
+	nh_->getParam("/", data);
+	
+	std::regex rx("panda_arm([0-9]+)");
+    std::smatch match;
+	std::regex_match(robots_.at("panda_arm1")->name(), match, rx);
+	data["base_link"]= "world";
+
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_name"] = robots_.at("panda_arm1")->map().at("eef_name").c_str();
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_type"] = "finger";
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["angle_resolution"] = 45;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005f;
+
+	std::stringstream ss;
+	ss << "panda_" <<  match[1] << "_tool_center_point";
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853f;
+
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["joints"][0] = robots_.at("panda_arm1")->mgiHand()->getJoints()[1].c_str();
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_finger_width"] = 0.085f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["min_finger_width"] = 0.06f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012f;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
+	data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
+	nh_->setParam("/", data);
+
+    MapGenerator::MapGeneratorPtr grasp_map_generator_ = std::make_shared<MapGenerator>(*nh_,
+                                                      visual_tools_,
+                                                      planning_scene_monitor_,
+                                                      robots_.at("panda_arm1")->map().at("eef_name"),
+                                                      robots_.at("panda_arm1")->name(),
+                                                      arm_jmg_,
+                                                      robots_.at("panda_arm1")->mgi()->getCurrentState(),
+                                                      voxel_manager_);
+    auto cb = cuboid_reader_->cuboidBin();
+
+    ROS_INFO("Debug3!");
+
+    std::vector<GraspMap> maps;
+
+    Cuboid workspace("WS");
+    workspace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
+    workspace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
+    workspace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.02f;
+    workspace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
+    workspace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
+    workspace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
+    workspace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
+    workspace.x_depth = 0.2f;
+    workspace.y_width = 0.2f;
+    workspace.z_heigth = 0.04f;
+
+    ROS_INFO("Debug4!");
+
+    double cube_clearence, translation_rate, rotation_rate, max_rotation;
+    cube_clearence = 0.01f;
+    translation_rate = 0.03f;
+    rotation_rate = 45.f * (M_PI/180.f);
+    max_rotation = 90.f * (M_PI/180.f);
+
+    for (auto& object: cb) {
+        GraspMap map(workspace,object,cube_clearence,translation_rate,rotation_rate,max_rotation);
+        maps.push_back(map);
+    }
+
+    ROS_INFO("Debug5!");
+
+    // Create Maps
+    {
+        ROS_INFO("creating map ...");
+        auto start_time = ros::Time::now();
+        for (GraspMap &m: maps) { grasp_map_generator_->SampleMap(m); }
+        double durration = (start_time - ros::Time::now()).toSec();
+        ROS_INFO_STREAM(maps.size() << " GraspMaps created in " << durration << " seconds");
+    }
+    ROS_INFO("Debug6!");
+
+}

src/mediator/moveit_mediator.cpp

@@ -16,19 +16,9 @@ MoveitMediator::MoveitMediator(std::shared_ptr<ros::NodeHandle> const& nh)
     , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
     , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
     , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
-    , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
 	, job_reader_(std::make_unique<JobReader>(nh_))
     , cuboid_reader_(std::make_unique<CuboidReader>(nh)){
 
-        if (planning_scene_monitor_->getPlanningScene()){
-            planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
-                                                              "planning_scene");
-            planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
-        } else {
-            ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
-            return;
-        }
-
         robot_model_loader::RobotModelLoaderPtr robot_model_loader;
         robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
         robot_model_ = robot_model_loader->getModel();
@@ -124,9 +114,7 @@ void MoveitMediator::mediate(){
     publishTables();
 	taskPlanner();
 	ros::waitForShutdown();
-
-
-};
+}
 
 void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::PlanningScene& ps){
 	// First find ID from panda to start with
@@ -477,6 +465,7 @@ void MoveitMediator::taskPlanner(){
 	BT::PublisherZMQ zmq(tree);
 	ros::Duration sleep(1);
 	BT::NodeStatus status = BT::NodeStatus::RUNNING;
+	auto t_start = std::chrono::high_resolution_clock::now();
 	while( status == BT::NodeStatus::RUNNING){
 		status = tree.tickRoot();
 		
@@ -527,7 +516,9 @@ void MoveitMediator::taskPlanner(){
 
 		
 	}
-
+	auto t_end = std::chrono::high_resolution_clock::now();
+	double elapsed_time_ms = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+	ROS_INFO("%f", elapsed_time_ms);
 
 	// clean up
 	for (int i = 0; i < cd.size(); i ++){