diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 2887213cc991ef8f5e56bc786443b3740ee825cd..a99b3e8c806533a16d0b11b6a5ede667d211d6d1 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 17431
+/home/matteo/reachability/devel/./cmake.lock 21138
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/.catkin_tools/profiles/default/packages/mtc/package.xml b/.catkin_tools/profiles/default/packages/mtc/package.xml
index cd7cd58d6af1f909d2e16230737f9a0386f8c3b8..79cef2f50eeaaf2fe9620c2581950b1cfe090300 100644
--- a/.catkin_tools/profiles/default/packages/mtc/package.xml
+++ b/.catkin_tools/profiles/default/packages/mtc/package.xml
@@ -35,11 +35,14 @@
   <build_depend>moveit_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>YAML_CPP</build_depend> 
+  <build_depend>filesystem</build_depend> 
+
+
   <build_depend>pcl_ros</build_depend>
   <build_depend>yaml_to_mtc</build_depend>
 
   
-
+  <run_depend>filesystem</run_depend> 
   <run_depend>YAML_CPP</run_depend> 
   <run_depend>pcl_ros</run_depend>
   <run_depend>std_msgs</run_depend>
diff --git a/.catkin_tools/profiles/default/packages/reachability/package.xml b/.catkin_tools/profiles/default/packages/reachability/package.xml
index 9cae359fd37493df56f1910cfdf8b819b97f229d..865c04a1827cd5aaf20aace8eb14c15ebe657c08 100644
--- a/.catkin_tools/profiles/default/packages/reachability/package.xml
+++ b/.catkin_tools/profiles/default/packages/reachability/package.xml
@@ -35,6 +35,7 @@
   <exec_depend>interactive_markers</exec_depend>
   <exec_depend>eigen_conversions</exec_depend>
   <exec_depend>YAML_CPP</exec_depend>
+  
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json
index 29c1085f158e1999d108c43eb18e56da583ebc68..73b94dd7c5066c90b3c9ad8baa14adcda17b8395 100644
--- a/src/.vscode/c_cpp_properties.json
+++ b/src/.vscode/c_cpp_properties.json
@@ -41,7 +41,9 @@
                 "${workspaceFolder}/moveit_task_constructor/core/include",
                 "${workspaceFolder}/moveit_task_constructor/"
             ],
-            "name": "ROS"
+            "name": "ROS",
+            "cStandard": "c17",
+            "cppStandard": "c++17"
         }
     ],
     "version": 4
diff --git a/src/.vscode/settings.json b/src/.vscode/settings.json
index dbabd7d94b6bbfbea5f37d517acef7732ae7abef..e92b5d44b1caecf09f2ca5f708c459ae61b4b3bc 100644
--- a/src/.vscode/settings.json
+++ b/src/.vscode/settings.json
@@ -79,6 +79,8 @@
         "typeindex": "cpp",
         "typeinfo": "cpp",
         "variant": "cpp",
-        "any": "cpp"
+        "any": "cpp",
+        "filesystem": "cpp",
+        "codecvt": "cpp"
     }
 }
\ No newline at end of file
diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt
index 7ae8bc5797c21a0ea3456e6734059f662cacd51f..91d4b6df4c08961ac1f05245f1e59740a68e2644 100644
--- a/src/mtc/CMakeLists.txt
+++ b/src/mtc/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(mtc)
 # C++ 11
-add_compile_options(-std=c++14)
+add_compile_options(-std=c++17)
 
 # Warnings
 add_definitions(-W -Wall -Wextra
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index 6417f3a0cc9f0a841926c83d4d5dd0a8477da7d2..091b26360a96a3a22ed5bb8b701ae97591ce12ea 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -33,7 +33,8 @@ class Abstract_mediator {
     std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
     ros::Publisher* pub_;
     std::vector<std::vector<pcl::PointXYZ>> result_vector_;
-    std::vector<std::vector<wing_BP>> wings_;
+    std::vector<std::vector<Abstract_robot_element*>> wings_;
+    std::string dirname_;
 
 
 
@@ -41,25 +42,24 @@ class Abstract_mediator {
 
     public:
     Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
-        std::vector<wing_BP> wings;
-        wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
-        wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
-        wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
-        wings_.push_back(wings);
-        wings_.push_back(wings);
     }
 
     inline void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
     inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
-    inline std::vector<std::vector<wing_BP>> wings() {return wings_;}
-    inline void set_wings(std::vector<std::vector<wing_BP>>& w) {wings_ = w;}
+    inline std::vector<std::vector<Abstract_robot_element*>> wings() {return wings_;}
     inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
     inline std::vector<Abstract_robot*> robots(){return robots_;}
 
 
+    inline void set_dirname(std::string& dirn) {dirname_ = dirn;}
+    inline std::string& dirname() {return dirname_;}
+
+
+
     pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
     std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
     
+    virtual void set_wings(std::vector<std::vector<wing_BP>>& wbp)=0;
     virtual bool check_collision( const int& robot)=0;
     virtual void mediate()=0;
     virtual void build_wings(std::bitset<3>& wing, int& robot)=0;
diff --git a/src/mtc/include/impl/abstract_robot.h b/src/mtc/include/impl/abstract_robot.h
index 50be7a180a9cbf5e3289da4c6e213271e59efa6b..9a3430e742ee735a4a61b6be90ebd3b6f4447c12 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/src/mtc/include/impl/abstract_robot.h
@@ -35,7 +35,7 @@ class Abstract_robot {
 
 
     public:
-    Abstract_robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
+    Abstract_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
         root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));
 
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, 0.4f, tf.getOrigin().getZ()))); // ++
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index 7573cc3879d12f91811ea700dc96d50fb8cb9f9b..28f9003152f7c25bbdc867d155df2e787decc4f0 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/src/mtc/include/impl/mediator.h
@@ -10,9 +10,6 @@
 #include "impl/robot.h"
 
 class Mediator : public Abstract_mediator{
-    protected:
-    visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray;
-
     public:
     Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
 
@@ -20,8 +17,12 @@ class Mediator : public Abstract_mediator{
     std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
     void approximation(Robot* robot);
     void write_file(Robot* A, Robot* B);
+    void publish(Robot* r);
 
-
+    void setup_rviz();
+    void calculate(std::vector<tf2::Transform>& ground_per_robot);
+    
+    void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
     bool check_collision(const int& robot) override;
     void mediate() override;
     void build_wings(std::bitset<3>& wings, int& robot) override;
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
index e1edb2300e824b7c822ed47d45de4ed080d15fee..9d75774ec6f564df90f106dd71fa3b10fbdf62d6 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -23,6 +23,7 @@ class Moveit_mediator : public Abstract_mediator{
     robot_model_loader::RobotModelLoader* robot_model_loader_;
     robot_model::RobotModelPtr kinematic_model_;
     planning_scene::PlanningScene* ps_;
+    XmlRpc::XmlRpcValue properties_;
 
     public:
     Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh){
@@ -30,13 +31,17 @@ class Moveit_mediator : public Abstract_mediator{
         planning_scene_diff_publisher_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
     };
 
+    void setup_task();
+
     bool check_collision(const int& robot) override;
     void mediate() override;
     void build_wings(std::bitset<3>& wing,int& robot) override;
+    void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
 
+    
     void publish_tables(moveit::planning_interface::PlanningSceneInterface& psi);
     void load_robot_description();
-    void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& target);
+    void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
 };
 
 #endif
\ No newline at end of file
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
index 4178d1a128653d972f23b6c8a024c8db6d0167fb..387d9b26cc207ff3be503e835dc81c4365cdd2de 100644
--- a/src/mtc/include/impl/moveit_robot.h
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -6,15 +6,37 @@
 
 
 class Moveit_robot : public Robot{
-    private:
+    protected:
     moveit::planning_interface::MoveGroupInterface* mgi_;
+    std::map<std::string, std::pair <std::string, std::string>>  map_; 
+
+
+
 
     public:
-    Moveit_robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
+    Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
         mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
-        ROS_INFO("planning frame: %s", mgi_->getPlanningFrame().c_str());
+        std::stringstream group, eef, hand_grasping_frame, ik_frame, hand, hand_n, ik_frame_n, name_n;
+        name_n << name;
+        group << "group_" << name;
+        eef << "eef_" << name;
+        hand_grasping_frame << "hand_grasping_frame_" << name;
+        ik_frame << "ik_frame_" << name;
+        hand << "hand_" << name;
+        hand_n << "hand_" << name.back();
+        ik_frame_n << "panda_" << name.back() << "_link8";
+
+
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("group", std::make_pair<std::string, std::string>(group.str(), name_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("eef", std::make_pair<std::string, std::string>(eef.str(), hand_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand_grasping_frame", std::make_pair<std::string, std::string>(hand_grasping_frame.str(), ik_frame_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("ik_frame", std::make_pair<std::string, std::string>(ik_frame.str(), ik_frame_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand", std::make_pair<std::string, std::string>(hand.str(), hand_n.str())));
     }
 
+    inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
+    inline std::map<std::string, std::pair <std::string, std::string>>&  map(){return map_;};
+
 
 };
 
diff --git a/src/mtc/include/impl/robot.h b/src/mtc/include/impl/robot.h
index 95b69339920f50f53ef398a464a4cbea52c50b16..d09d2850bc881f7670690f91e821846222217b47 100644
--- a/src/mtc/include/impl/robot.h
+++ b/src/mtc/include/impl/robot.h
@@ -20,17 +20,12 @@ class Robot : public Abstract_robot{
     protected:
     std::vector<Abstract_robot_element*> observers_;
     std::vector<Abstract_robot_element*> access_fields_;
-    std::vector<visualization_msgs::MarkerArray*> rviz_markers_;
     std::vector<moveit_msgs::CollisionObject*> coll_markers_;
 
 
     public:
-    Robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){ 
-        generate_access_fields();
-    }
+    Robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){generate_access_fields();}
     
-    inline void add_rviz_markers(visualization_msgs::MarkerArray* marker) {rviz_markers_.push_back(marker);}
-    inline std::vector<visualization_msgs::MarkerArray*> rviz_markers(){return rviz_markers_;}
     inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
     inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
 
diff --git a/src/mtc/include/impl/wing.h b/src/mtc/include/impl/wing.h
index 9a920c21776a74a1e5de5beb38b0611b75e07c63..6fc94d09ef38523433c6790bf5933dfd33324570 100644
--- a/src/mtc/include/impl/wing.h
+++ b/src/mtc/include/impl/wing.h
@@ -11,16 +11,19 @@ class Wing  : public Abstract_robot_element{
     std::vector<tf2::Transform> bounds_;
 
     public:
-    Wing(std::string& name, tf2::Transform& tf, tf2::Vector3& size) : size_(size), name_(name){ 
+    Wing(std::string name, tf2::Transform tf, tf2::Vector3 size) : size_(size), name_(name){ 
         relative_tf_ = tf;
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/2.f,0))); //++
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/-2.f,0))); //+-
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/-2.f,0))); //--
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/2.f,0))); //-+
         }
+    virtual ~Wing() {}
         
     inline std::string& name() {return name_;}
-    inline void set_name(std::string& str) {name_ = str;}
+    inline void set_name(std::string str) {name_ = str;}
+    inline void set_set(tf2::Vector3& vec) {size_ = vec;}
+
     inline tf2::Vector3 size(){ return size_;}
     inline std::vector<tf2::Transform>& bounds() {return bounds_;}
     void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
diff --git a/src/mtc/include/impl/wing_moveit_decorator.h b/src/mtc/include/impl/wing_moveit_decorator.h
index 0db53ef50ad7b92747a87dea705277a658db2aa5..7f315d51035240a6fbc929be3fce106f62b918e2 100644
--- a/src/mtc/include/impl/wing_moveit_decorator.h
+++ b/src/mtc/include/impl/wing_moveit_decorator.h
@@ -12,11 +12,11 @@
 
 class Wing_moveit_decorator  : public Abstract_robot_element_decorator{
     private:
-    moveit_msgs::CollisionObject* markers_;
+    moveit_msgs::CollisionObject* markers_ = new moveit_msgs::CollisionObject();
 
 
     public:
-    Wing_moveit_decorator(Abstract_robot_element* next, moveit_msgs::CollisionObject* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
+    Wing_moveit_decorator(Abstract_robot_element* next) : Abstract_robot_element_decorator(next){};
     inline void set_markers(moveit_msgs::CollisionObject* markers) { markers_= markers;}
     inline moveit_msgs::CollisionObject* markers() { return markers_;}
     
diff --git a/src/mtc/include/impl/wing_rviz_decorator.h b/src/mtc/include/impl/wing_rviz_decorator.h
index 823af759a1a13982b0c71355ef363bb832f37bee..54313c73c480f1a0a2086c139414d2ef7624c1fd 100644
--- a/src/mtc/include/impl/wing_rviz_decorator.h
+++ b/src/mtc/include/impl/wing_rviz_decorator.h
@@ -8,13 +8,13 @@
 
 class Wing_rviz_decorator  : public Abstract_robot_element_decorator{
     private:
-    visualization_msgs::MarkerArray* markers_;
+    visualization_msgs::Marker* marker_ = new visualization_msgs::Marker();
 
 
     public:
-    Wing_rviz_decorator(Abstract_robot_element* next, visualization_msgs::MarkerArray* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
-    inline void set_markers(visualization_msgs::MarkerArray* markers) { markers_= markers;}
-    inline visualization_msgs::MarkerArray* markers() { return markers_;}
+    Wing_rviz_decorator(Abstract_robot_element* next) : Abstract_robot_element_decorator(next){};
+    inline void set_markers(visualization_msgs::Marker* markers) { marker_= markers;}
+    inline visualization_msgs::Marker* markers() { return marker_;}
     void update(tf2::Transform& tf) override;
     void input_filter(tf2::Transform& tf) override;
     void output_filter() override;
diff --git a/src/mtc/launch/base_routine.launch b/src/mtc/launch/base_routine.launch
index 6c5facb67a03d5b4f1b72472e6fcdc4d0e7be131..f13e2e65e2aee76f16eeed237a626bbec2bd319c 100644
--- a/src/mtc/launch/base_routine.launch
+++ b/src/mtc/launch/base_routine.launch
@@ -1,8 +1,13 @@
 <launch>
-    <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/resources/dummy.yaml"/>
+    <arg name="map" default="dummy" />
+    <arg name="description" default="dummy" />
+    <arg name="resource" default="dummy" />
 
 
+    <rosparam command="load" file="$(find mtc)/maps/$(arg map).yaml"/>
+    <rosparam command="load" file="$(find mtc)/descriptions/$(arg description).yaml"/>
+    <rosparam command="load" file="$(find mtc)/resources/$(arg resource).yaml"/>
+
+    <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
     <node pkg="mtc" type="base_routine" name="base_routine" output="screen" required="true"/>
 </launch>
\ No newline at end of file
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index bc0e5c3dcf5f874a1412c580514a742ae85073e9..f8795d144b6153e7c8120e2ac417151b2a6dc4cb 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -1,7 +1,7 @@
 <launch>    
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
     <rosparam command="load" file="$(find mtc)/results/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/mtc_task_file/dummy.yaml" />
+    <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
     <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
     
diff --git a/src/mtc/mtc_task_file/-1406488393.yaml b/src/mtc/mtc_templates/-1406488393.yaml
similarity index 100%
rename from src/mtc/mtc_task_file/-1406488393.yaml
rename to src/mtc/mtc_templates/-1406488393.yaml
diff --git a/src/mtc/mtc_task_file/-854508939.yaml b/src/mtc/mtc_templates/-854508939.yaml
similarity index 100%
rename from src/mtc/mtc_task_file/-854508939.yaml
rename to src/mtc/mtc_templates/-854508939.yaml
diff --git a/src/mtc/mtc_task_file/dummy.yaml b/src/mtc/mtc_templates/dummy.yaml
similarity index 100%
rename from src/mtc/mtc_task_file/dummy.yaml
rename to src/mtc/mtc_templates/dummy.yaml
diff --git a/src/mtc/package.xml b/src/mtc/package.xml
index cd7cd58d6af1f909d2e16230737f9a0386f8c3b8..79cef2f50eeaaf2fe9620c2581950b1cfe090300 100644
--- a/src/mtc/package.xml
+++ b/src/mtc/package.xml
@@ -35,11 +35,14 @@
   <build_depend>moveit_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>YAML_CPP</build_depend> 
+  <build_depend>filesystem</build_depend> 
+
+
   <build_depend>pcl_ros</build_depend>
   <build_depend>yaml_to_mtc</build_depend>
 
   
-
+  <run_depend>filesystem</run_depend> 
   <run_depend>YAML_CPP</run_depend> 
   <run_depend>pcl_ros</run_depend>
   <run_depend>std_msgs</run_depend>
diff --git a/src/mtc/resources/dummy.yaml b/src/mtc/resources/dummy.yaml
index 0cfc174df5feb31252e335ae3167d2c08333979a..69292ef17ffe973211240b390bf2150bc314481d 100644
--- a/src/mtc/resources/dummy.yaml
+++ b/src/mtc/resources/dummy.yaml
@@ -22,6 +22,8 @@
   { '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 } },
 
@@ -32,8 +34,8 @@
   { 'id': 'table2_wheel_2',     'pos': { 'x': -0.28,'y': 1.03,'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.28,'y': 1.59,'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.28,'y': 1.03,'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': .345, y: 1.31, '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': 'table2_body_back',   'pos': { 'x': -.345, y: 1.31, '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': 'table2_body_front',  'pos': { 'x': .345, 'y': 1.31, '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': 'table2_body_back',   'pos': { 'x': -.345, 'y': 1.31, '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': 'table2_body_left',   'pos': { 'y': 1.655, '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': 'table2_body_right',  'pos': { 'y': 0.965, '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': 'table2_table_top',   'pos': { 'y': 1.31, '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 } },
diff --git a/src/mtc/results/dummy/-1692777619.yaml b/src/mtc/results/dummy/-1692777619.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6090c39a6e13bb198fbed95b86d73a7cd4f82114
--- /dev/null
+++ b/src/mtc/results/dummy/-1692777619.yaml
@@ -0,0 +1,27 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.405000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1705045806.yaml b/src/mtc/results/dummy/-1705045806.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..89536d2e44cbbd41422013ed2afb8c8c5d0a5819
--- /dev/null
+++ b/src/mtc/results/dummy/-1705045806.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1715933098.yaml b/src/mtc/results/dummy/-1715933098.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d67e3c4c71906fd9ba7df4c7aa6e074162e23078
--- /dev/null
+++ b/src/mtc/results/dummy/-1715933098.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1736477370.yaml b/src/mtc/results/dummy/-1736477370.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6090c39a6e13bb198fbed95b86d73a7cd4f82114
--- /dev/null
+++ b/src/mtc/results/dummy/-1736477370.yaml
@@ -0,0 +1,27 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.405000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1748758618.yaml b/src/mtc/results/dummy/-1748758618.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..89536d2e44cbbd41422013ed2afb8c8c5d0a5819
--- /dev/null
+++ b/src/mtc/results/dummy/-1748758618.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1759692146.yaml b/src/mtc/results/dummy/-1759692146.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d67e3c4c71906fd9ba7df4c7aa6e074162e23078
--- /dev/null
+++ b/src/mtc/results/dummy/-1759692146.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1803427587.yaml b/src/mtc/results/dummy/-1803427587.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..410f2ce1dc1ab8d80922cdf36f568b6eebfd1413
--- /dev/null
+++ b/src/mtc/results/dummy/-1803427587.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.013733, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': 0.070600, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.319165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.205430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1847007484.yaml b/src/mtc/results/dummy/-1847007484.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..410f2ce1dc1ab8d80922cdf36f568b6eebfd1413
--- /dev/null
+++ b/src/mtc/results/dummy/-1847007484.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.013733, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': 0.070600, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.319165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.205430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-1982088882.yaml b/src/mtc/results/dummy/-1982088882.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..83c4b90269a00fa5780b4b3c865eb0f9e6239308
--- /dev/null
+++ b/src/mtc/results/dummy/-1982088882.yaml
@@ -0,0 +1,25 @@
+{ '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': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , '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_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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/-2028402352.yaml b/src/mtc/results/dummy/-2028402352.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..83c4b90269a00fa5780b4b3c865eb0f9e6239308
--- /dev/null
+++ b/src/mtc/results/dummy/-2028402352.yaml
@@ -0,0 +1,25 @@
+{ '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': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , '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_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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1138642515.yaml b/src/mtc/results/dummy/1138642515.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a1457703e8c3b9112b631eb6f267828dfe529d94
--- /dev/null
+++ b/src/mtc/results/dummy/1138642515.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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.000006, 'y': 1.399998, '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_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_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_table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , '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.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.399997, '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/src/mtc/results/dummy/1150568114.yaml b/src/mtc/results/dummy/1150568114.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bba400b311ac9e525f99b6cefa5187af86e911ad
--- /dev/null
+++ b/src/mtc/results/dummy/1150568114.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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': 1.104998, '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_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_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_table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.757498 , '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.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.220005, 'y': 1.104997, '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/src/mtc/results/dummy/1191311555.yaml b/src/mtc/results/dummy/1191311555.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a1457703e8c3b9112b631eb6f267828dfe529d94
--- /dev/null
+++ b/src/mtc/results/dummy/1191311555.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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.000006, 'y': 1.399998, '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_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_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_table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , '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.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.399997, '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/src/mtc/results/dummy/1202468027.yaml b/src/mtc/results/dummy/1202468027.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bba400b311ac9e525f99b6cefa5187af86e911ad
--- /dev/null
+++ b/src/mtc/results/dummy/1202468027.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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': 1.104998, '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_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_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_table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.757498 , '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.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.220005, 'y': 1.104997, '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/src/mtc/results/dummy/1443430664.yaml b/src/mtc/results/dummy/1443430664.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..693ce63f5064d8a7dfefe20f25f9a7ddc6b59c41
--- /dev/null
+++ b/src/mtc/results/dummy/1443430664.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1454326127.yaml b/src/mtc/results/dummy/1454326127.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2c6ccac16c046684a5ba74e5ebf1363520d29116
--- /dev/null
+++ b/src/mtc/results/dummy/1454326127.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1461432933.yaml b/src/mtc/results/dummy/1461432933.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6efc5a58618be4ed6aa9205d9738928a4b61159e
--- /dev/null
+++ b/src/mtc/results/dummy/1461432933.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , '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': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , '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_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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1466555132.yaml b/src/mtc/results/dummy/1466555132.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d48db4d3ed1891ab4efa5a67b88c0bf54dff4184
--- /dev/null
+++ b/src/mtc/results/dummy/1466555132.yaml
@@ -0,0 +1,27 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.205000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1487024111.yaml b/src/mtc/results/dummy/1487024111.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..693ce63f5064d8a7dfefe20f25f9a7ddc6b59c41
--- /dev/null
+++ b/src/mtc/results/dummy/1487024111.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1497853990.yaml b/src/mtc/results/dummy/1497853990.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2c6ccac16c046684a5ba74e5ebf1363520d29116
--- /dev/null
+++ b/src/mtc/results/dummy/1497853990.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1510066581.yaml b/src/mtc/results/dummy/1510066581.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d48db4d3ed1891ab4efa5a67b88c0bf54dff4184
--- /dev/null
+++ b/src/mtc/results/dummy/1510066581.yaml
@@ -0,0 +1,27 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.205000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1524066269.yaml b/src/mtc/results/dummy/1524066269.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6efc5a58618be4ed6aa9205d9738928a4b61159e
--- /dev/null
+++ b/src/mtc/results/dummy/1524066269.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , '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': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , '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_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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1691132293.yaml b/src/mtc/results/dummy/1691132293.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c7d7c84e15828ae6e980e0de13c40f217629141d
--- /dev/null
+++ b/src/mtc/results/dummy/1691132293.yaml
@@ -0,0 +1,26 @@
+{ '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.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.087266 } },
+{ '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.113740, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087267}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019176, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332903, 'y': 1.280858, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1740918153.yaml b/src/mtc/results/dummy/1740918153.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c7d7c84e15828ae6e980e0de13c40f217629141d
--- /dev/null
+++ b/src/mtc/results/dummy/1740918153.yaml
@@ -0,0 +1,26 @@
+{ '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.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.087266 } },
+{ '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.113740, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087267}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019176, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332903, 'y': 1.280858, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1876946886.yaml b/src/mtc/results/dummy/1876946886.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..22bb2443cc1e2dc8b8b6f42907d596290de73412
--- /dev/null
+++ b/src/mtc/results/dummy/1876946886.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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': 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_table1_right_panel' , 'pos': { 'x': -0.000004 , '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_table1_left_panel' , 'pos': { 'x': -0.000000 , '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_table2_right_panel',  'pos': { 'x': -0.000007, '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.220002, '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.220005, '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/src/mtc/results/dummy/1926655009.yaml b/src/mtc/results/dummy/1926655009.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..22bb2443cc1e2dc8b8b6f42907d596290de73412
--- /dev/null
+++ b/src/mtc/results/dummy/1926655009.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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': 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_table1_right_panel' , 'pos': { 'x': -0.000004 , '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_table1_left_panel' , 'pos': { 'x': -0.000000 , '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_table2_right_panel',  'pos': { 'x': -0.000007, '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.220002, '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.220005, '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/src/mtc/results/dummy/1986169185.yaml b/src/mtc/results/dummy/1986169185.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..585676d10d50cee56471bff779451efa299c0055
--- /dev/null
+++ b/src/mtc/results/dummy/1986169185.yaml
@@ -0,0 +1,26 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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_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': '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/src/mtc/results/dummy/1997263146.yaml b/src/mtc/results/dummy/1997263146.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..742e17cf7c6f9deed89420c04893d190a8fe7f65
--- /dev/null
+++ b/src/mtc/results/dummy/1997263146.yaml
@@ -0,0 +1,26 @@
+{ '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.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 }}, 
+{ '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.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.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_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_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_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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ '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.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/1998466333.yaml b/src/mtc/results/dummy/1998466333.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a36b32e4230c1eb87db349e37ad89e610572ee66
--- /dev/null
+++ b/src/mtc/results/dummy/1998466333.yaml
@@ -0,0 +1,26 @@
+{ '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.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.087269 } },
+{ '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.113744, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087269}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.536273, 'y': 1.356903 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019177, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332906, 'y': 1.280857, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/2009643783.yaml b/src/mtc/results/dummy/2009643783.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7fd072365c2f94cc7ec468f5c5267d9c0f7469a4
--- /dev/null
+++ b/src/mtc/results/dummy/2009643783.yaml
@@ -0,0 +1,27 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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_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_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.305000 , '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': '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/src/mtc/results/dummy/2030127988.yaml b/src/mtc/results/dummy/2030127988.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..585676d10d50cee56471bff779451efa299c0055
--- /dev/null
+++ b/src/mtc/results/dummy/2030127988.yaml
@@ -0,0 +1,26 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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_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': '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/src/mtc/results/dummy/2040956261.yaml b/src/mtc/results/dummy/2040956261.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..742e17cf7c6f9deed89420c04893d190a8fe7f65
--- /dev/null
+++ b/src/mtc/results/dummy/2040956261.yaml
@@ -0,0 +1,26 @@
+{ '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.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 }}, 
+{ '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.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.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_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_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_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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ '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.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/2045084535.yaml b/src/mtc/results/dummy/2045084535.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a36b32e4230c1eb87db349e37ad89e610572ee66
--- /dev/null
+++ b/src/mtc/results/dummy/2045084535.yaml
@@ -0,0 +1,26 @@
+{ '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.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.087269 } },
+{ '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.113744, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087269}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.536273, 'y': 1.356903 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019177, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332906, 'y': 1.280857, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/2053173671.yaml b/src/mtc/results/dummy/2053173671.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7fd072365c2f94cc7ec468f5c5267d9c0f7469a4
--- /dev/null
+++ b/src/mtc/results/dummy/2053173671.yaml
@@ -0,0 +1,27 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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_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_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.305000 , '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': '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/src/mtc/results/dummy/313407848.yaml b/src/mtc/results/dummy/313407848.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0eb0fd862afa54f5ed36ccf401c587880599f3e2
--- /dev/null
+++ b/src/mtc/results/dummy/313407848.yaml
@@ -0,0 +1,26 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/324430568.yaml b/src/mtc/results/dummy/324430568.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..19a88f6f93eecf076cc5e71f77bddd3976a09798
--- /dev/null
+++ b/src/mtc/results/dummy/324430568.yaml
@@ -0,0 +1,26 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.675591 , 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.483532}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.105441, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/336691910.yaml b/src/mtc/results/dummy/336691910.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..37e2de46e32fd8084fd4ff590d39e970ad789a4d
--- /dev/null
+++ b/src/mtc/results/dummy/336691910.yaml
@@ -0,0 +1,27 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.563750, 'y': 1.243165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/357177233.yaml b/src/mtc/results/dummy/357177233.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0eb0fd862afa54f5ed36ccf401c587880599f3e2
--- /dev/null
+++ b/src/mtc/results/dummy/357177233.yaml
@@ -0,0 +1,26 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/368021780.yaml b/src/mtc/results/dummy/368021780.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..19a88f6f93eecf076cc5e71f77bddd3976a09798
--- /dev/null
+++ b/src/mtc/results/dummy/368021780.yaml
@@ -0,0 +1,26 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.675591 , 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.483532}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.105441, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/380238980.yaml b/src/mtc/results/dummy/380238980.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..37e2de46e32fd8084fd4ff590d39e970ad789a4d
--- /dev/null
+++ b/src/mtc/results/dummy/380238980.yaml
@@ -0,0 +1,27 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.563750, 'y': 1.243165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/390180518.yaml b/src/mtc/results/dummy/390180518.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b694cc6f6ed60cee97549e89a1cb978a88b72dff
--- /dev/null
+++ b/src/mtc/results/dummy/390180518.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/401939748.yaml b/src/mtc/results/dummy/401939748.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8492c8538d73b1d5b08e0fc056204c3e625ceb88
--- /dev/null
+++ b/src/mtc/results/dummy/401939748.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/415108929.yaml b/src/mtc/results/dummy/415108929.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7a58307491417a7ec4e0605b6116981035370a1d
--- /dev/null
+++ b/src/mtc/results/dummy/415108929.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.205000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/423704891.yaml b/src/mtc/results/dummy/423704891.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3d75735f83186b62466beec659e599b656b526c1
--- /dev/null
+++ b/src/mtc/results/dummy/423704891.yaml
@@ -0,0 +1,27 @@
+{ '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.200002 , '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 }}, 
+{ '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.200005, '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_table1_right_panel' , 'pos': { 'x': -0.200004 , '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_table1_front_panel' , 'pos': { 'x': 0.452498 , '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_table1_left_panel' , 'pos': { 'x': -0.200000 , '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_table2_right_panel',  'pos': { 'x': -0.200007, '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_table2_front_panel',  'pos': { 'x': 0.452495, 'y': 1.305000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.420002, '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.420005, '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/src/mtc/results/dummy/436801234.yaml b/src/mtc/results/dummy/436801234.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b694cc6f6ed60cee97549e89a1cb978a88b72dff
--- /dev/null
+++ b/src/mtc/results/dummy/436801234.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/448406387.yaml b/src/mtc/results/dummy/448406387.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8492c8538d73b1d5b08e0fc056204c3e625ceb88
--- /dev/null
+++ b/src/mtc/results/dummy/448406387.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/461556082.yaml b/src/mtc/results/dummy/461556082.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7a58307491417a7ec4e0605b6116981035370a1d
--- /dev/null
+++ b/src/mtc/results/dummy/461556082.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , 'y': -0.100002 , '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.100005, 'y': 1.204998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.205000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/466931166.yaml b/src/mtc/results/dummy/466931166.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3d75735f83186b62466beec659e599b656b526c1
--- /dev/null
+++ b/src/mtc/results/dummy/466931166.yaml
@@ -0,0 +1,27 @@
+{ '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.200002 , '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 }}, 
+{ '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.200005, '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_table1_right_panel' , 'pos': { 'x': -0.200004 , '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_table1_front_panel' , 'pos': { 'x': 0.452498 , '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_table1_left_panel' , 'pos': { 'x': -0.200000 , '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_table2_right_panel',  'pos': { 'x': -0.200007, '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_table2_front_panel',  'pos': { 'x': 0.452495, 'y': 1.305000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.420002, '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.420005, '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/src/mtc/results/dummy/497619452.yaml b/src/mtc/results/dummy/497619452.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b5c4d9a7538e6cdb036f1c272164389baf121b21
--- /dev/null
+++ b/src/mtc/results/dummy/497619452.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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': '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/src/mtc/results/dummy/509248143.yaml b/src/mtc/results/dummy/509248143.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a7d67f3832226a389c33ae37444a07e49b1f8ac2
--- /dev/null
+++ b/src/mtc/results/dummy/509248143.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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.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.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_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_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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ '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.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/522407406.yaml b/src/mtc/results/dummy/522407406.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5258e24fe52a438bc2f01f92e74c486475db060b
--- /dev/null
+++ b/src/mtc/results/dummy/522407406.yaml
@@ -0,0 +1,26 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.305000 , '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': '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/src/mtc/results/dummy/544096359.yaml b/src/mtc/results/dummy/544096359.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b5c4d9a7538e6cdb036f1c272164389baf121b21
--- /dev/null
+++ b/src/mtc/results/dummy/544096359.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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': '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/src/mtc/results/dummy/555705912.yaml b/src/mtc/results/dummy/555705912.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a7d67f3832226a389c33ae37444a07e49b1f8ac2
--- /dev/null
+++ b/src/mtc/results/dummy/555705912.yaml
@@ -0,0 +1,25 @@
+{ '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.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 }}, 
+{ '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.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.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_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_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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ '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.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/568954925.yaml b/src/mtc/results/dummy/568954925.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5258e24fe52a438bc2f01f92e74c486475db060b
--- /dev/null
+++ b/src/mtc/results/dummy/568954925.yaml
@@ -0,0 +1,26 @@
+{ '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.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 }}, 
+{ '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.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_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_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_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_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.305000 , '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': '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/src/mtc/results/dummy/605226781.yaml b/src/mtc/results/dummy/605226781.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5e36b135833721911b073309d37908a5036f6977
--- /dev/null
+++ b/src/mtc/results/dummy/605226781.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/613458718.yaml b/src/mtc/results/dummy/613458718.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..057ce407328910e1067b078c813b2913e521d31c
--- /dev/null
+++ b/src/mtc/results/dummy/613458718.yaml
@@ -0,0 +1,27 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/616917843.yaml b/src/mtc/results/dummy/616917843.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..df44868512301dd29b8b1be2a73f4bdb163eb3a0
--- /dev/null
+++ b/src/mtc/results/dummy/616917843.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/630149674.yaml b/src/mtc/results/dummy/630149674.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c47d0d5c9494aeb2219c20e8d86893def63bb210
--- /dev/null
+++ b/src/mtc/results/dummy/630149674.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.405000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/651836672.yaml b/src/mtc/results/dummy/651836672.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5e36b135833721911b073309d37908a5036f6977
--- /dev/null
+++ b/src/mtc/results/dummy/651836672.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/657141845.yaml b/src/mtc/results/dummy/657141845.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..057ce407328910e1067b078c813b2913e521d31c
--- /dev/null
+++ b/src/mtc/results/dummy/657141845.yaml
@@ -0,0 +1,27 @@
+{ '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.200002 , '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.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
+{ '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.086267, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
+{ 'id': 'table2_table2_right_panel',  'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'table2_table2_front_panel',  'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/663611616.yaml b/src/mtc/results/dummy/663611616.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..df44868512301dd29b8b1be2a73f4bdb163eb3a0
--- /dev/null
+++ b/src/mtc/results/dummy/663611616.yaml
@@ -0,0 +1,25 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
+{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/results/dummy/676899603.yaml b/src/mtc/results/dummy/676899603.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c47d0d5c9494aeb2219c20e8d86893def63bb210
--- /dev/null
+++ b/src/mtc/results/dummy/676899603.yaml
@@ -0,0 +1,26 @@
+{ '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.100002 , '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.100005, 'y': 1.404998, '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_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , '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_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , '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_table2_right_panel',  'pos': { 'x': -0.100007, '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': 'table2_table2_front_panel',  'pos': { 'x': 0.552495, 'y': 1.405000 , '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': 'arm1','type': 'ARM','pos': { 'x': -0.320002, '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.320005, '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 } } 
+]}
\ No newline at end of file
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index 5c02938fa082fcc034d75d226562a84639c43879..493dac15e36d2d70a87617c1b81ed2f0a52724d9 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -13,8 +13,30 @@
 #include "impl/map_loader.h"
 #include "impl/base_by_rotation.h"
 #include <xmlrpcpp/XmlRpc.h>
+#include <filesystem>
+
+float value_caster(XmlRpc::XmlRpcValue& val){
+    float t = 0;
+    try{
+        switch (val.getType()){
+            case XmlRpc::XmlRpcValue::TypeInt:
+                t = static_cast<int>(val);
+                break;
+            case XmlRpc::XmlRpcValue::TypeDouble:
+                t = static_cast<double>(val);
+                break;
+            case XmlRpc::XmlRpcValue::TypeString:
+                t = std::stof(val);
+                break;
+            default:
+                ROS_INFO("value type is some unknown type");
+            }
+    } catch (XmlRpc::XmlRpcException){
+        ROS_INFO("Something went wrong, returning 0");
+    }
+    return t;
+}
 
-// cpp inputs ,,,
 
 int main(int argc, char **argv){
     ros::init(argc, argv, "base_routine");
@@ -26,6 +48,11 @@ int main(int argc, char **argv){
     n.getParam("/objects",resources);
     n.getParam("/task/groups",task);
 
+    std::string filename;
+    n.getParam("/resource_name", filename);
+
+    if(std::filesystem::exists(ros::package::getPath("mtc") + "/results/" +  filename)) std::filesystem::remove_all(ros::package::getPath("mtc") + "/results/" +  filename);
+    std::filesystem::create_directory(ros::package::getPath("mtc") + "/results/" +  filename);
     
     Abstract_map_loader* map_loader = new Map_loader(map, task);
     Abstract_strategy* strategy = new Base_by_rotation();
@@ -35,696 +62,323 @@ int main(int argc, char **argv){
 
     Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)));
     mediator->set_result_vector(results);
+    mediator->set_dirname(filename);
 
     float a = 0; float b = 0;
     std::vector<std::vector<wing_BP>> blueprints_per_robot(2);
     std::vector<wing_BP> blueprints(3);
-    blueprints[0] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints[1] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
-    blueprints[2] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
+
+    blueprints[0] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
+    blueprints[1] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
+    blueprints[2] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
     blueprints_per_robot[0] = blueprints;
     blueprints_per_robot[1] = blueprints;
 
     for (int i = 0; i < resources.size(); i++){
-        XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = resources[i].begin();
-        for(; it != resources[i].end(); ++it){
-            if(it->first == "id"){
-                std::string top = it->second; 
-                if(top == "table1_table_top" ){
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-                    Abstract_robot* robo = new Robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(qx,qy,qz,qw), tf2::Vector3(0,0, z * 0.5f)), tf2::Vector3(l,w,h));
-                    Robot* ceti = dynamic_cast<Robot*>(robo);
-                    ceti->set_observer_mask(wing_config::RML);
-                    ceti->add_rviz_markers(new visualization_msgs::MarkerArray());
-                    mediator->connect_robots(robo);
-                }
-
-                if(top == "table2_table_top" ){
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-
-                    Abstract_robot* robo = new Robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(qx,qy,qz,qw), tf2::Vector3(0,0, z * 0.5f)), tf2::Vector3(l,w,h));
-                    Robot* ceti = dynamic_cast<Robot*>(robo);
-                    ceti->set_observer_mask(wing_config::RML);
-                    ceti->add_rviz_markers(new visualization_msgs::MarkerArray());
-                    mediator->connect_robots(robo);
-                } 
-
-                if(top == "table1_right_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-                                             
-                    blueprints_per_robot[0][0].name_ = "table1_right_panel";
-                    blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[0][0].size_ = tf2::Vector3(l,w,h);
-                    a += std::pow(2, 0);
-                }
-
-                if(top == "table1_front_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-                                             
-                    blueprints_per_robot[0][1].name_ = "table1_front_panel";
-                    blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[0][1].size_ = tf2::Vector3(l,w,h);
-                    a += std::pow(2, 1);
-                }
-
-                if(top == "table1_left_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-                                             
-                    blueprints_per_robot[0][2].name_ = "table1_left_panel";
-                    blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[0][2].size_ = tf2::Vector3(l,w,h);
-                    a += std::pow(2, 2);
-                }
-
-                if(top == "table2_right_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-                                             
-                    blueprints_per_robot[1][0].name_ = "table2_right_panel";
-                    blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[1][0].size_ = tf2::Vector3(l,w,h);
-                    b += std::pow(2, 0);
-                }
-
-                if(top == "table2_front_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-                                             
-                    blueprints_per_robot[1][1].name_ = "table2_front_panel";
-                    blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[1][1].size_ = tf2::Vector3(l,w,h);
-                    b += std::pow(2, 1);
-                }
-
-                if(top == "table2_left_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = t;
-                            if(ittt->first == "width") w = t;
-                            if(ittt->first == "height") h = t;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = t;
-                            if(ittt->first == "y") y = t;
-                            if(ittt->first == "z") z = t;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            float t = 0;
-                            switch (ittt->second.getType())
-                            {
-                            case XmlRpc::XmlRpcValue::TypeInt:
-                                t = static_cast<int>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeDouble:
-                                t = static_cast<double>(ittt->second);
-                                break;
-                            case XmlRpc::XmlRpcValue::TypeString:
-                                t = std::stof(ittt->second);
-                                break;
-                            default :
-                                break;
-                            }
-
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = t;
-                            if(ittt->first == "y") qy = t;
-                            if(ittt->first == "z") qz = t;
-                            if(ittt->first == "w") qw = t;
-                            }
-                        }
-                    }
-                                             
-                    blueprints_per_robot[1][2].name_ = "table2_left_panel";
-                    blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[1][2].size_ = tf2::Vector3(l,w,h);
-                    b += std::pow(2, 2);
-                }
-        
-            }
+        if(resources[i]["id"] == "table1_table_top"){
+            ROS_INFO("=> Robot: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            std::string name("panda_arm1");
+            mediator->connect_robots(new Robot(name, tf2::Transform(rot, pos), size));
+            ROS_INFO("=> Robot:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table2_table_top"){
+            ROS_INFO("=> Robot: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+            
+            std::string name("panda_arm2");
+            mediator->connect_robots(new Robot(name, tf2::Transform(rot, pos), size));
+            ROS_INFO("=> Robot:loading SUCCESS");
         }
     }
-    // mediator->set_wings(blueprints_per_robot);
+
+    
+    for (int i = 0; i < resources.size(); i++){
+        if(resources[i]["id"] == "table1_right_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            /*
+            Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[0][0]);
+            w->set_name("table1_right_panel");
+            w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
+            w->set_set(size);
+
+            blueprints_per_robot[0][0] = new Wing_rviz_decorator(w);
+            */
+
+            blueprints_per_robot[0][0].name_ = "table1_right_panel";
+            blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[0][0].size_ = size;
+
+
+            a += std::pow(2, 0);
+            ROS_INFO("=> WING:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table1_front_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            /*
+            Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
+            Wing* w = dynamic_cast<Wing*>(are);
+            w->set_name("table1_front_panel");
+            w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
+            w->set_set(size);
+
+            Abstract_robot_element* wrd = new Wing_rviz_decorator(w);
+            blueprints_per_robot[0][1] = wrd;
+
+            */
+            a += std::pow(2, 1);
+
+            blueprints_per_robot[0][1].name_ = std::string("table1_front_panel");
+                        ROS_INFO("uzguzg");
+            blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[0][1].size_ = size;
+            ROS_INFO("=> WING:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table1_left_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            /*
+            Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
+            Wing* w = dynamic_cast<Wing*>(are);
+            w->set_name("table1_left_panel");
+            w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
+            w->set_set(size);
+
+            Abstract_robot_element* wrd = new Wing_rviz_decorator(are);
+            blueprints_per_robot[0][2] = wrd;
+            */
+            blueprints_per_robot[0][2].name_ = "table1_left_panel";
+            blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[0][2].size_ = size;
+            a += std::pow(2, 2);
+            ROS_INFO("=> WING:loading SUCCESS");
+
+        }
+
+        if(resources[i]["id"] == "table2_right_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            /*
+            Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
+            Wing* w = dynamic_cast<Wing*>(are);
+            w->set_name("table2_right_panel");
+            w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
+            w->set_set(size);
+
+            blueprints_per_robot[1][0] = new Wing_rviz_decorator(w);
+            */
+            blueprints_per_robot[1][0].name_ = "table2_right_panel";
+            blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[1][0].size_ = size;
+            b += std::pow(2, 0);
+            ROS_INFO("=> WING:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table2_front_panel" ) {  
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            /*
+            Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[1][1]);                    
+            w->set_name("table2_front_panel");
+            w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
+            w->set_set(size);
+            blueprints_per_robot[1][1] = new Wing_rviz_decorator(w);
+            */
+            blueprints_per_robot[1][1].name_ = "table2_front_panel";
+            blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[1][1].size_ = size;
+            ROS_INFO("=> WING:loading SUCCESS");
+            b += std::pow(2, 1);
+
+        }
+
+        if(resources[i]["id"] == "table2_left_panel" ) {          
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+            /*
+            Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[1][2]);                    
+            w->set_name("table2_left_panel");
+            w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
+            w->set_set(size);
+            blueprints_per_robot[1][2] = new Wing_rviz_decorator(w);
+            */
+            
+            blueprints_per_robot[1][2].name_ = "table2_left_panel";
+            blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[1][2].size_ = size;
+            ROS_INFO("=> WING:loading SUCCESS");
+            b += std::pow(2, 2);
+        }
+    }
+    mediator->robots()[0]->set_observer_mask(static_cast<wing_config>(a));
+    mediator->robots()[1]->set_observer_mask(static_cast<wing_config>(b));
+
+
+    ROS_INFO("ist durch");
+    mediator->set_wings(blueprints_per_robot);
     mediator->mediate();
 
+
+
+    
     //map_loader->write_task(robo);
 
     //free(rviz_right);
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
index 162126265eb7fa49dc53befaa6b4d391dfc01108..c5697f3bdd4cd43fc8512e53debedfd15073e953 100644
--- a/src/mtc/src/cell_routine.cpp
+++ b/src/mtc/src/cell_routine.cpp
@@ -11,6 +11,28 @@
 #include "impl/collision_helper.h"
 #include <xmlrpcpp/XmlRpc.h>
 
+float value_caster(XmlRpc::XmlRpcValue& val){
+    float t = 0;
+    try{
+        switch (val.getType()){
+            case XmlRpc::XmlRpcValue::TypeInt:
+                t = static_cast<int>(val);
+                break;
+            case XmlRpc::XmlRpcValue::TypeDouble:
+                t = static_cast<double>(val);
+                break;
+            case XmlRpc::XmlRpcValue::TypeString:
+                t = std::stof(val);
+                break;
+            default:
+                ROS_INFO("value type is some unknown type");
+            }
+    } catch (XmlRpc::XmlRpcException){
+        ROS_INFO("Something went wrong, returning 0");
+    }
+    return t;
+}
+
 int main(int argc, char **argv){
     ros::init(argc, argv, "cell_routine");
     ros::NodeHandle nh;
@@ -43,352 +65,258 @@ int main(int argc, char **argv){
     blueprints_per_robot[1] = blueprints;
     int a = 0; int b = 0;
 
+for (int i = 0; i < resources.size(); i++){
+        if(resources[i]["id"] == "table1_table_top"){
+            ROS_INFO("=> Robot: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            std::string name("panda_arm1");
+            Abstract_robot* robot1 = new Moveit_robot(name, tf2::Transform(rot, pos), size);
+            Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(robot1);
+            ceti1->set_observer_mask(wing_config::RML);
+            mediator->connect_robots(robot1);
+            ROS_INFO("=> Robot:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table2_table_top"){
+            ROS_INFO("=> Robot: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            std::string name("panda_arm2");
+            Abstract_robot* robot2 = new Moveit_robot(name, tf2::Transform(rot, pos), size);
+            Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robot2);
+            ceti2->set_observer_mask(wing_config::RML);
+            mediator->connect_robots(robot2);
+
+            ROS_INFO("=> Robot:loading SUCCESS");
+        }
+    }
+
+    
     for (int i = 0; i < resources.size(); i++){
-        XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = resources[i].begin();
-        for(; it != resources[i].end(); ++it){
-            if(it->first == "id"){
-                std::string top = it->second; 
-                if(top == "table1_table_top" ){
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-
-                        }
-                    }
-                
-                    Abstract_robot* robo1 = new Moveit_robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(qx,qy ,qz, qw), tf2::Vector3(x,y, z *0.5f)),tf2::Vector3(l,w,h));
-                    robo1->set_observer_mask(wing_config::RML);
-                    mediator->connect_robots(robo1);
-                }
-
-                if(top == "table2_table_top" ){
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-                    }
-
-                    Abstract_robot* robo2 = new Moveit_robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(qx,qy ,qz, qw), tf2::Vector3(x,y, z *0.5f)),tf2::Vector3(l,w,h));
-                    robo2->set_observer_mask(wing_config::RML);
-                    mediator->connect_robots(robo2);
-                } 
-
-                if(top == "table1_right_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                         if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-
-                         }
-                    }
-                                             
-                    blueprints_per_robot[0][0].name_ = "table1_right_panel";
-                    blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[0][0].size_ = tf2::Vector3(l,w,h);
-                    a += std::pow(2, 0);
-                }
-
-                if(top == "table1_front_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                         if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-
-                         }
-                    }
-                                             
-                    blueprints_per_robot[0][1].name_ = "table1_front_panel";
-                    blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[0][1].size_ = tf2::Vector3(l,w,h);
-                    a += std::pow(2, 1);
-                }
-
-                if(top == "table1_left_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                         if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-
-                         }
-                    }
-                                             
-                    blueprints_per_robot[0][2].name_ = "table1_left_panel";
-                    blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[0][2].size_ = tf2::Vector3(l,w,h);
-                    a += std::pow(2, 2);
-                }
-
-                if(top == "table2_right_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                         if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-
-                         }
-                    }
-                                             
-                    blueprints_per_robot[1][0].name_ = "table2_right_panel";
-                    blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[1][0].size_ = tf2::Vector3(l,w,h);
-                    b += std::pow(2, 0);
-                }
-
-                if(top == "table2_front_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                         if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-
-                         }
-                    }
-                                             
-                    blueprints_per_robot[1][1].name_ = "table2_front_panel";
-                    blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[1][1].size_ = tf2::Vector3(l,w,h);
-                    b += std::pow(2, 1);
-                }
-
-                if(top == "table2_left_panel" ) { 
-                    XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-                    float h = 0; float w = 0; float l = 0;
-                    float x = 0; float y = 0; float z = 0;
-                    float qx = 0; float qy = 0; float qz = 0; float qw = 0;
-                    for(; itt != resources[i].end(); ++itt){
-                        if(itt->first == "size"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "length") l = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "width") w = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "height") h = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                        if(itt->first == "pos"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") x = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") y = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") z = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-                        }
-
-                         if(itt->first == "orientation"){
-                            XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                            for(; ittt != itt->second.end(); ++ittt){
-                            if(ittt->first == "x") qx = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "y") qy = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "z") qz = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            if(ittt->first == "w") qw = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                            }
-
-                         }
-                    }
-                                             
-                    blueprints_per_robot[1][2].name_ = "table2_left_panel";
-                    blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(tf2::Quaternion(qx, qy, qz, qw).normalized(), tf2::Vector3(x,y,z));
-                    blueprints_per_robot[1][2].size_ = tf2::Vector3(l,w,h);
-                    b += std::pow(2, 2);
-                }
-        
-            }
+        if(resources[i]["id"] == "table1_right_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            blueprints_per_robot[0][0].name_ = "table1_right_panel";
+            blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[0][0].size_ = size;
+
+
+            a += std::pow(2, 0);
+            ROS_INFO("=> WING:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table1_front_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            a += std::pow(2, 1);
+
+            blueprints_per_robot[0][1].name_ = std::string("table1_front_panel");
+                        ROS_INFO("uzguzg");
+            blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[0][1].size_ = size;
+            ROS_INFO("=> WING:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table1_left_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            blueprints_per_robot[0][2].name_ = "table1_left_panel";
+            blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[0][2].size_ = size;
+            a += std::pow(2, 2);
+            ROS_INFO("=> WING:loading SUCCESS");
+
+        }
+
+        if(resources[i]["id"] == "table2_right_panel" ) { 
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            blueprints_per_robot[1][0].name_ = "table2_right_panel";
+            blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[1][0].size_ = size;
+            b += std::pow(2, 0);
+            ROS_INFO("=> WING:loading SUCCESS");
+        }
+
+        if(resources[i]["id"] == "table2_front_panel" ) {  
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+            blueprints_per_robot[1][1].name_ = "table2_front_panel";
+            blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[1][1].size_ = size;
+            ROS_INFO("=> WING:loading SUCCESS");
+            b += std::pow(2, 1);
+
+        }
+
+        if(resources[i]["id"] == "table2_left_panel" ) {          
+            ROS_INFO("=> WING: description found, loading...");
+            tf2::Vector3 pos;
+            tf2::Vector3 size;
+            tf2::Quaternion rot;
+
+            (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
+            (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
+            (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+
+
+            (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
+            (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
+            (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0);
+            (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
+            (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
+            (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
+            (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
+            
+
+            ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+            ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+            ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+  
+            blueprints_per_robot[1][2].name_ = "table2_left_panel";
+            blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
+            blueprints_per_robot[1][2].size_ = size;
+            ROS_INFO("=> WING:loading SUCCESS");
+            b += std::pow(2, 2);
         }
     }
 
 
 
+    ROS_INFO("ndfasofn");
     moveit_mediator->set_wings(blueprints_per_robot);
     Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]);
     Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]);
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index 95aeeb10ec5f64c9e49ed8cb6251a17836954b71..807148f8e1bd13ceb287c0b52c1fc51df956876d 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -3,7 +3,52 @@
 #include "impl/wing_rviz_decorator.h"
 #include <tf2/LinearMath/Scalar.h>
 
+void Mediator::setup_rviz(){
+    visualization_msgs::MarkerArray ma;
+    for (int i = 0; i < wings_.size(); i++){
+        Robot* r = dynamic_cast<Robot*>(robots_[i]);
+        for (int j = 0; j < wings_[i].size(); j++){
+            Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(wings_[i][j]);
+            Wing* w = dynamic_cast<Wing*>(wrd->wing());
+            visualization_msgs::Marker marker;
+            marker.header.frame_id = "map";
+            marker.header.stamp = ros::Time();
+            marker.ns = w->name();
+            marker.id = 1;
+            marker.type = visualization_msgs::Marker::CUBE;
+            marker.action = visualization_msgs::Marker::ADD;
+            marker.pose.position.x = w->world_tf().getOrigin().getX();
+            marker.pose.position.y = w->world_tf().getOrigin().getY();
+            marker.pose.position.z = w->world_tf().getOrigin().getZ();
+            marker.pose.orientation.x = w->world_tf().getRotation().getX();
+            marker.pose.orientation.y = w->world_tf().getRotation().getY();
+            marker.pose.orientation.z = w->world_tf().getRotation().getZ();
+            marker.pose.orientation.w = w->world_tf().getRotation().getW();
+            marker.scale.x = w->size().getX();
+            marker.scale.y = w->size().getY();
+            marker.scale.z = w->size().getZ();
+            marker.color.r = 1.0;
+            marker.color.g = 1.0;
+            marker.color.b = 1.0;
+            marker.color.a = 1.0;
+            ma.markers.push_back(marker);
+        }
+    }
+    //pub_->publish(ma);
+}
 
+void Mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
+
+    for (int i =0; i < wbp.size(); i++){
+        std::vector<Abstract_robot_element*> v;
+        for (int j =0; j < wbp[i].size(); j++){
+            Abstract_robot_element* are = new Wing_rviz_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
+            v.push_back(are);
+        }
+        wings_.push_back(v);
+    }
+
+}
 
 bool Mediator::check_collision( const int& robot){
     bool succ = true;
@@ -12,6 +57,7 @@ bool Mediator::check_collision( const int& robot){
     count_v.resize(r->observers().size()+1);
     std::string str;
 
+    visualization_msgs::MarkerArray ma;
     for(long unsigned int j = 0; j < objects_[robot].size(); j++){
         visualization_msgs::Marker marker;
         marker.header.frame_id = "map";
@@ -42,10 +88,10 @@ bool Mediator::check_collision( const int& robot){
             marker.color.a = 1.0;
             succ = false;
         }
-
+        ma.markers.push_back(marker);
         robots_[robot]->workload_checker(count_v, objects_[robot][j]);
-        rviz_markers_->markers.push_back(marker);
     }
+    pub_->publish(ma);
 
     for (int& i : count_v){
         if(i == 0) {succ = false; break;}
@@ -73,78 +119,177 @@ void Mediator::mediate(){
         }
     }
 
-    for (long unsigned int i = objects_[0].size()-1; i > 0; i--){
-        if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back();
+    for(int i = 0; i < objects_.size(); i++){
+        if (i+1 < objects_.size()){
+            for (long unsigned int j = objects_[i].size()-1; j > 0; j--){
+                if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) objects_[i+1].pop_back();
+            }
+        }
     }
 
+    calculate(ground_per_robot);
+}
 
-    ros::Rate loop_rate(10);
+void Mediator::calculate(std::vector<tf2::Transform>& ground_per_robot){
+    visualization_msgs::MarkerArray ma;
     int r1 = 0; 
     Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
     for(int j = 0; j <= 7; j++){
         std::bitset<3> wing_config(j);
         build_wings(wing_config, r1);
+
+        visualization_msgs::Marker m;
+        m.header.frame_id = "map";
+        m.header.stamp = ros::Time();
+        m.ns = ceti1->name();
+        m.id = 1;
+        m.type = visualization_msgs::Marker::CUBE;
+        m.action = visualization_msgs::Marker::ADD;
+        m.pose.position.x = ceti1->tf().getOrigin().getX();
+        m.pose.position.y = ceti1->tf().getOrigin().getY();
+        m.pose.position.z = ceti1->tf().getOrigin().getZ();
+        m.pose.orientation.x = ceti1->tf().getRotation().getX();
+        m.pose.orientation.y = ceti1->tf().getRotation().getY();
+        m.pose.orientation.z = ceti1->tf().getRotation().getZ();
+        m.pose.orientation.w = ceti1->tf().getRotation().getW();
+        m.scale.x = ceti1->size().getX();
+        m.scale.y = ceti1->size().getY();
+        m.scale.z = ceti1->tf().getOrigin().getZ()*2;
+        m.color.r = 1.0;
+        m.color.g = 1.0;
+        m.color.b = 1.0;
+        m.color.a = 1.0;
+        ma.markers.push_back(m);
+        pub_->publish(ma);
+        ma.markers.clear();
+
+        ros::Duration timer(0.10);
+
         for (long unsigned int i = 0; i < ground_per_robot.size(); i++){
-            robots_[0]->set_tf(ground_per_robot[i]);
+            ceti1->set_tf(ground_per_robot[i]);
+            // vis. robot 1
+
             for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
-                for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear();
-                rviz_markers_->markers.clear();
                 ceti1->rotate(0.0872665f);
                 ceti1->notify();
-                
-                if (check_collision(0)) {
-                approximation(ceti1);
+
+                m.action = visualization_msgs::Marker::MODIFY;
+                m.pose.position.x = ceti1->tf().getOrigin().getX();
+                m.pose.position.y = ceti1->tf().getOrigin().getY();
+                m.pose.position.z = ceti1->tf().getOrigin().getZ();
+                m.pose.orientation.x = ceti1->tf().getRotation().getX();
+                m.pose.orientation.y = ceti1->tf().getRotation().getY();
+                m.pose.orientation.z = ceti1->tf().getRotation().getZ();
+                m.pose.orientation.w = ceti1->tf().getRotation().getW();
+                ma.markers.push_back(m);
+
+                for (Abstract_robot_element* are : ceti1->observers()){
+                    Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
+                    ma.markers.push_back(*wrd->markers());
+                }
+
+                pub_->publish(ma);
+                ma.markers.clear();
+
+                if (check_collision(r1)) {
+                    approximation(ceti1);
                 }   else {
                     continue;
                 }
-                for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) pub_->publish(*markers);
-                pub_->publish(*rviz_markers_);
+                
+               //timer.sleep();
             }
         }
         ceti1->reset();
+        m.action = visualization_msgs::Marker::DELETEALL;
+        ma.markers.push_back(m);
+        pub_->publish(ma);
+        ma.markers.clear();
     }
 }
 
 void Mediator::approximation(Robot* robot){
-    ROS_INFO("assigne result to first robot...");
-    Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
+    Robot* ceti1 = dynamic_cast<Robot*>(robots_[1]);
     int r1 = 1;
+
+    visualization_msgs::MarkerArray ma;
+
     for (int i = 0; i <= 7; i++){
         std::bitset<3> wing_config(i);
         build_wings(wing_config, r1);
+
+        visualization_msgs::Marker m;
+        m.header.frame_id = "map";
+        m.header.stamp = ros::Time();
+        m.ns = ceti1->name();
+        m.id = 1;
+        m.type = visualization_msgs::Marker::CUBE;
+        m.action = visualization_msgs::Marker::ADD;
+        m.pose.position.x = ceti1->tf().getOrigin().getX();
+        m.pose.position.y = ceti1->tf().getOrigin().getY();
+        m.pose.position.z = ceti1->tf().getOrigin().getZ();
+        m.pose.orientation.x = ceti1->tf().getRotation().getX();
+        m.pose.orientation.y = ceti1->tf().getRotation().getY();
+        m.pose.orientation.z = ceti1->tf().getRotation().getZ();
+        m.pose.orientation.w = ceti1->tf().getRotation().getW();
+        m.scale.x = ceti1->size().getX();
+        m.scale.y = ceti1->size().getY();
+        m.scale.z = ceti1->tf().getOrigin().getZ()*2;
+        m.color.r = 1.0;
+        m.color.g = 1.0;
+        m.color.b = 1.0;
+        m.color.a = 1.0;
+        ma.markers.push_back(m);
+        pub_->publish(ma);
+        ma.markers.clear();
+
         for (Abstract_robot_element* fields : robot->access_fields()) {
-            ceti->set_tf(fields->world_tf());
+            ceti1->set_tf(fields->world_tf());
+            //ROS_INFO("field size %i", robot->access_fields().size());
             for ( float p = 0; p < 2*M_PI; p += M_PI/2){
-                for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) markers->markers.clear();
-                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
-                rviz_markers_->markers.clear();
+                ceti1->rotate(M_PI/2);
+                ceti1->notify();
 
-                ceti->rotate(M_PI/2);
-                ceti->notify();
-                
-                if (robot->check_robot_collision(ceti)) continue;
+                m.action = visualization_msgs::Marker::MODIFY;
+                m.pose.position.x = ceti1->tf().getOrigin().getX();
+                m.pose.position.y = ceti1->tf().getOrigin().getY();
+                m.pose.position.z = ceti1->tf().getOrigin().getZ();
+                m.pose.orientation.x = ceti1->tf().getRotation().getX();
+                m.pose.orientation.y = ceti1->tf().getRotation().getY();
+                m.pose.orientation.z = ceti1->tf().getRotation().getZ();
+                m.pose.orientation.w = ceti1->tf().getRotation().getW();
+                ma.markers.push_back(m);
                 
-                if (check_collision(1)) {
-                    ROS_INFO("should work");
-                    write_file(robot, ceti);
-                    } else {
-                        continue;
-                    }
-                for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) pub_->publish(*markers);
-                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) pub_->publish(*markers);
-                pub_->publish(*rviz_markers_);
+                for (Abstract_robot_element* are : ceti1->observers()){
+                    Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
+                    ma.markers.push_back(*wrd->markers());
+                }
 
-            }
+                pub_->publish(ma);
+                ma.markers.clear();
+                if (robot->check_robot_collision(ceti1)) continue;
+
+                if (check_collision(r1)) {
+                    write_file(robot, ceti1);
+                } else {
+                    continue;
+                }
 
+            }
+            
             //relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ()));
         }
-        ceti->reset();
+        ceti1->reset();
+        m.action = visualization_msgs::Marker::DELETEALL;
+        ma.markers.push_back(m);
+        pub_->publish(ma);
+        ma.markers.clear();
     }
 }
 
 
 void Mediator::write_file(Robot* A, Robot* B){
-    std::ofstream o(ros::package::getPath("mtc") + "/results/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
+    std::ofstream o(ros::package::getPath("mtc") + "/results/" + dirname_ + "/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
     double r,p,y;
     tf2::Matrix3x3 m(A->tf().getRotation());
     m.getRPY(r,p,y);
@@ -215,7 +360,7 @@ void Mediator::write_file(Robot* A, Robot* B){
         float width = w->size().getY();
         float height = w->size().getZ();
 
-        ss << "{ 'id': 'table" << A->name().back() << "_" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
+        ss << "{ 'id': 'table" << A->name().back() << "_" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
     }
 
     
@@ -234,7 +379,7 @@ void Mediator::write_file(Robot* A, Robot* B){
         float width = w->size().getY();
         float height = w->size().getZ();
 
-        ss << "{ 'id': 'table" << B->name().back() << "_" << w->name() << "',  'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
+        ss << "{ 'id': 'table" << B->name().back() << "_" << w->name() << "',  'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
     }
     tf2::Transform tf_arm =  A->tf() * A->root_tf();
     float arm_x = tf_arm.getOrigin().getX();
@@ -268,10 +413,49 @@ void Mediator::write_file(Robot* A, Robot* B){
 void Mediator::build_wings(std::bitset<3>& wing, int& robot){
     std::bitset<3> result = robots_[robot]->observer_mask() & wing;
     Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
+    
     for (std::size_t i = 0; i < result.size(); i++){
         if (result[i]){
-            Abstract_robot_element* moveit_right = new Wing_rviz_decorator(new Wing(wings_[robot][i].name_, wings_[robot][i].pos_, wings_[robot][i].size_), ceti->rviz_markers()[0]);
-            ceti->register_observers(moveit_right);
+            ceti->register_observers(wings_[robot][i]);
         }
     }
+
+    visualization_msgs::MarkerArray ma;
+    for (Abstract_robot_element* are : ceti->observers()){
+        Wing_rviz_decorator* wad = dynamic_cast<Wing_rviz_decorator*>(are);
+        wad->markers()->action = visualization_msgs::Marker::ADD;
+        ma.markers.push_back(*wad->markers());
+    }
+    pub_->publish(ma);
+}
+
+void Mediator::publish(Robot* ar){
+    visualization_msgs::MarkerArray ma;
+    visualization_msgs::Marker marker;
+    marker.header.frame_id = "map";
+    marker.header.stamp = ros::Time();
+    marker.ns = ar->name();
+    marker.id = 1;
+    marker.type = visualization_msgs::Marker::CUBE;
+    marker.action = visualization_msgs::Marker::MODIFY;
+    marker.pose.position.x = ar->tf().getOrigin().getX();
+    marker.pose.position.y = ar->tf().getOrigin().getY();
+    marker.pose.position.z = ar->tf().getOrigin().getZ();
+    marker.pose.orientation.x = ar->tf().getRotation().getX();
+    marker.pose.orientation.y = ar->tf().getRotation().getY();
+    marker.pose.orientation.z = ar->tf().getRotation().getZ();
+    marker.pose.orientation.w = ar->tf().getRotation().getW();
+    marker.scale.x = ar->size().getX();
+    marker.scale.y = ar->size().getY();
+    marker.scale.z = ar->size().getZ();
+    marker.color.r = 1.0;
+    marker.color.g = 1.0;
+    marker.color.b = 1.0;
+    marker.color.a = 1.0;
+    for (auto w : ar->observers()){
+        Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(w);
+        ma.markers.push_back(*wrd->markers());
+    }
+    
+    //pub_->publish(ma);
 }
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index 680147d065667bf03ba0a0f9caedcdadf240f463..1d53c0e7b6c34c1d40820fdfbe34a4ad95728eb9 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -9,8 +9,9 @@ void Moveit_mediator::publish_tables(moveit::planning_interface::PlanningSceneIn
     ros::WallDuration sleep_time(1.0);
     for(long unsigned int i = 0; i < robots_.size();i++){
         Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
-        for (moveit_msgs::CollisionObject* markers : ceti->coll_markers()) {
-            psi.applyCollisionObject(*markers);
+        for (Abstract_robot_element* are : ceti->observers()) {
+            Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
+            psi.applyCollisionObject(*wmd->markers());
             sleep_time.sleep();
         }
     }
@@ -55,7 +56,9 @@ void Moveit_mediator::mediate(){
     bottle.operation = bottle.ADD;
 
     psi.applyCollisionObject(bottle);
-    rewrite_task_template(robots_[0], bottle);
+
+    tf2::Transform target(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.1f, 0.6f, 0.9305f));
+    rewrite_task_template(robots_[0], psi.getObjects().at("bottle"), target);
 
 
     Yaml_Mtc_Parser parser = Yaml_Mtc_Parser();
@@ -68,34 +71,10 @@ void Moveit_mediator::mediate(){
 
             execute_result = task.execute(*task.solutions().front());
             //task.introspection().publishSolution(*task.solutions().front());
-            /*
-            if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
-                ROS_INFO("irgendwas ging schief");
-	        }
-            */
         }
-        ROS_INFO("durch, print mich ");
 	} catch (const moveit::task_constructor::InitStageException& ex) {
 		std::cerr << "planning failed with exception" << std::endl << ex << task;
 	}
-
-    /*
-    ROS_INFO("durch, print mich daddy");
-    tf2::Vector3 source2(0.1, 1, 0.9305);
-    rewrite_task_template(robots_[1], source2);
-
-    task.clear();
-    task = parser.init_task(this->nh_);
-    this->nh_.getParam("max_planning_solutions", max_planning_solutions);
-    try {
-		if (task.plan(max_planning_solutions)) {
-			task.introspection().publishSolution(*task.solutions().front());
-		}
-	} catch (const moveit::task_constructor::InitStageException& ex) {
-		std::cerr << "planning failed with exception" << std::endl << ex << task;
-	}
-
-    */
 	ros::waitForShutdown();
 };
 
@@ -104,19 +83,28 @@ void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
     Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
     for (std::size_t i = 0; i < result.size(); i++){
         if (result[i]){
-            moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject();
-            ceti->add_coll_markers(marker);
-            Abstract_robot_element* moveit_right = new Wing_moveit_decorator(new Wing(wings_[robot][i].name_, wings_[robot][i].pos_, wings_[robot][i].size_), marker);
-            ceti->register_observers(moveit_right);
+            ceti->register_observers(wings_[robot][i]);
         }
     }
 }
 
-void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source){
-    tf2::Transform t(tf2::Quaternion(source.primitive_poses[0].orientation.x, source.primitive_poses[0].orientation.y, source.primitive_poses[0].orientation.z, source.primitive_poses[0].orientation.w), tf2::Vector3(source.primitive_poses[0].position.x, source.primitive_poses[0].position.y, source.primitive_poses[0].position.z));
-    tf2::Transform target(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.1f, 0.6f, 0.9305f));
+void Moveit_mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
+    for (int i =0; i < wbp.size(); i++){
+        std::vector<Abstract_robot_element*> v;
+        for (int j =0; j < wbp[i].size(); j++){
+            Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
+            v.push_back(are);
+        }
+        wings_.push_back(v);
+    }
+};
 
 
+void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){
+    tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
+    // unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up
+    setup_task();
+
     XmlRpc::XmlRpcValue task;
     nh_.getParam("task/stages", task);
 
@@ -168,10 +156,9 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
     c[4]["set"]["allow_collisions"]["first"] = source.id;
     c[4]["set"]["allow_collisions"]["second"] = e;
 
-    h["x"] = static_cast<double>(0);
-    h["y"] = static_cast<double>(-0.5f);
-    h["z"] = static_cast<double>(0.9305f);
-    c[2]["stage"]["set"]["pose"]["point"]= h;
+    c[2]["stage"]["set"]["pose"]["point"]["x"]= static_cast<double>(target.getOrigin().getX());
+    c[2]["stage"]["set"]["pose"]["point"]["y"]= static_cast<double>(target.getOrigin().getY());
+    c[2]["stage"]["set"]["pose"]["point"]["z"]= static_cast<double>(target.getOrigin().getZ());
 
 
     task[2]["properties"] = a;
@@ -214,7 +201,18 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
     task[6]["stages"] = c;
     task[4]["stages"] = b;
 
+    nh_.setParam("task/stages", task);
+}
 
 
-    nh_.setParam("task/stages", task);
+void Moveit_mediator::setup_task (){
+    for (Abstract_robot* robot : robots_){
+        Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
+
+        properties_[mr->map()["group"].first] = mr->map()["group"].second;
+        properties_[mr->map()["eef"].first] = mr->map()["eef"].second;
+        properties_[mr->map()["hand_grasping_frame"].first] = mr->map()["hand_grasping_frame"].second;
+        properties_[mr->map()["ik_frame"].first] = mr->map()["ik_frame"].second;
+        properties_[mr->map()["hand"].first] = mr->map()["hand"].second;
+    }
 }
diff --git a/src/mtc/src/impl/robot.cpp b/src/mtc/src/impl/robot.cpp
index d444e069bda77aae45b2826b1e18689c2d1ac037..66fcafd0a32f2ca25384ddc508dcbe970b28d136 100644
--- a/src/mtc/src/impl/robot.cpp
+++ b/src/mtc/src/impl/robot.cpp
@@ -104,7 +104,6 @@ bool Robot::check_single_object_collision(tf2::Transform& obj, std::string& str)
             
             full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
             sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
-
             if ((std::floor(sum*100)/100.f) <= full_area) {
                 str = ceti->name();
                 return true;
@@ -120,7 +119,6 @@ void Robot::reset(){
     observers_.clear();
     access_fields_.clear();
     generate_access_fields();
-
     tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0, tf().getOrigin().getZ()));
 }
 
diff --git a/src/mtc/src/impl/wing_rviz_decorator.cpp b/src/mtc/src/impl/wing_rviz_decorator.cpp
index 234530266a854a5574f2ebd385c3e18f3e89dd26..7dae8f505bfd2999d64cbd9712e93cd2362ef107 100644
--- a/src/mtc/src/impl/wing_rviz_decorator.cpp
+++ b/src/mtc/src/impl/wing_rviz_decorator.cpp
@@ -1,9 +1,7 @@
 #include "impl/wing_rviz_decorator.h"
 
 void Wing_rviz_decorator::update(tf2::Transform& tf) {
-    if(markers_->markers.empty()) {
-        input_filter(tf);
-    }
+    //input_filter(tf);
     Abstract_robot_element_decorator::update(tf);
     output_filter();
 }
@@ -15,8 +13,8 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) {
     visualization_msgs::Marker marker;
     marker.header.frame_id = "map";
     marker.header.stamp = ros::Time();
-    marker.ns = "ceti_table ";
-    marker.id = *((int*)(&markers_));
+    marker.ns = "";
+    marker.id = 1;
     marker.type = visualization_msgs::Marker::CUBE;
     marker.action = visualization_msgs::Marker::ADD;
     marker.pose.position.x = world_origin.getX();
@@ -33,7 +31,6 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) {
     marker.color.g = 1.0;
     marker.color.b = 1.0;
     marker.color.a = 1.0; // Don't forget to set the alpha
-    markers_->markers.push_back(marker);
 }
 
 void Wing_rviz_decorator::output_filter() {
@@ -41,37 +38,34 @@ void Wing_rviz_decorator::output_filter() {
     tf2::Vector3 world_origin = wing->world_tf().getOrigin();
     tf2::Quaternion world_quat = wing->world_tf().getRotation().normalized();
 
-    visualization_msgs::Marker marker;
-    marker.header.frame_id = "map";
-    marker.header.stamp = ros::Time();
-    marker.ns = "wings and robo ";
-    marker.id = *((int*)(&next_));
-    marker.type = visualization_msgs::Marker::CUBE;
-    marker.action = visualization_msgs::Marker::ADD;
-    marker.pose.position.x = world_origin.getX();
-    marker.pose.position.y = world_origin.getY();
-    marker.pose.position.z = world_origin.getZ();
-    marker.pose.orientation.x = world_quat.getX();
-    marker.pose.orientation.y = world_quat.getY();
-    marker.pose.orientation.z = world_quat.getZ();
-    marker.pose.orientation.w = world_quat.getW();
-    marker.scale.x = wing->size().getX();
-    marker.scale.y = wing->size().getY();
-    marker.scale.z = wing->size().getZ();
-    marker.color.r = 1.0;
-    marker.color.g = 1.0;
-    marker.color.b = 1.0;
-    marker.color.a = 1.0; // Don't forget to set the alpha!
-
-    markers_->markers.push_back(marker);
-
+    marker_->header.frame_id = "map";
+    marker_->header.stamp = ros::Time();
+    marker_->ns = wing->name();
+    marker_->id = 1;
+    marker_->type = visualization_msgs::Marker::CUBE;
+    marker_->action = visualization_msgs::Marker::MODIFY;
+    marker_->pose.position.x = world_origin.getX();
+    marker_->pose.position.y = world_origin.getY();
+    marker_->pose.position.z = world_origin.getZ();
+    marker_->pose.orientation.x = world_quat.getX();
+    marker_->pose.orientation.y = world_quat.getY();
+    marker_->pose.orientation.z = world_quat.getZ();
+    marker_->pose.orientation.w = world_quat.getW();
+    marker_->scale.x = wing->size().getX();
+    marker_->scale.y = wing->size().getY();
+    marker_->scale.z = wing->size().getZ();
+    marker_->color.r = 1.0;
+    marker_->color.g = 1.0;
+    marker_->color.b = 1.0;
+    marker_->color.a = 1.0; // Don't forget to set the alpha!
+    /*
     visualization_msgs::Marker bounds;
     bounds.header.frame_id = "map";
     bounds.header.stamp = ros::Time();
     bounds.ns = "bounds";
     bounds.id = *((int*)(&wing));
     bounds.type = visualization_msgs::Marker::POINTS;
-    bounds.action = visualization_msgs::Marker::ADD;
+    bounds.action = visualization_msgs::Marker::MODIFY;
     bounds.scale.x = 0.01f;
     bounds.scale.y = 0.01f;
     bounds.scale.z = 0.01f;
@@ -89,7 +83,6 @@ void Wing_rviz_decorator::output_filter() {
         point.z = point_posistion.getOrigin().getZ();
         bounds.points.push_back(point);
     }
-    markers_->markers.push_back(bounds);
-
+    */
 }
 
diff --git a/src/reachability/package.xml b/src/reachability/package.xml
index 9cae359fd37493df56f1910cfdf8b819b97f229d..865c04a1827cd5aaf20aace8eb14c15ebe657c08 100644
--- a/src/reachability/package.xml
+++ b/src/reachability/package.xml
@@ -35,6 +35,7 @@
   <exec_depend>interactive_markers</exec_depend>
   <exec_depend>eigen_conversions</exec_depend>
   <exec_depend>YAML_CPP</exec_depend>
+  
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>