diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 057e740eb89066501f6dd52e8841d52ad3ae7dcc..641c8c6c7f74edba53652dc4a810dc9769a53211 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 14204
+/home/matteo/reachability/devel/./cmake.lock 16019
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/franka_description/robots/ceti_double.urdf.xacro b/src/franka_description/robots/ceti_double.urdf.xacro
index b68990c9a21cee1674ab36f681c9fc5bb48b6bd6..4a54ab2bbff093e919d46336b655af4f2e6acc92 100644
--- a/src/franka_description/robots/ceti_double.urdf.xacro
+++ b/src/franka_description/robots/ceti_double.urdf.xacro
@@ -4,13 +4,14 @@
   <xacro:arg name="arm_id_2" default="panda_2" />
 
 
+
   <xacro:include filename="panda_arm.xacro"/>
   <xacro:include filename="hand.xacro"/>
 
   <link name="world" />
 
 
-  <xacro:property name="yaml_file" value="$(find mtc)/results/-229104537.yaml" />
+  <xacro:property name="yaml_file" value="$(find mtc)/results/298901127.yaml" />
   <xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" />
 
   <xacro:property name="x1" value="${props['objects'][8]['pos']['x']}" />
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index 727fd05602c460624cdaa6b4f85795a8e84de809..7bee017cb617eadee9cd99c235a589ff13bac335 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -23,7 +23,6 @@ struct wing_BP {
     std::string name_;
     tf2::Transform pos_;
     tf2::Vector3 size_;
-    wing_BP(std::string name, tf2::Transform pos, tf2::Vector3 size) : name_ (name), pos_(pos), size_(size){};
 };
 
 
@@ -34,7 +33,7 @@ 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<wing_BP> wings_;
+    std::vector<std::vector<wing_BP>> wings_;
 
 
 
@@ -42,22 +41,27 @@ class Abstract_mediator {
 
     public:
     Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
+        /*
         wings_.push_back(wing_BP("right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size));
         wings_.push_back(wing_BP("front_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid_size));
         wings_.push_back(wing_BP("left_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left_size));
+        */
     }
 
     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<wing_BP> wings() {return wings_;}
+    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<pcl::PointXYZ>>& result_vector() {return result_vector_;}
+    inline std::vector<Abstract_robot*> robots(){return robots_;}
+
 
     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 bool check_collision( const int& robot)=0;
     virtual void mediate()=0;
-    virtual void build_wings(std::bitset<3>& wing,Robot* robot)=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 499ae7b9f16a09d38f1a0cb33ca5e4e5e7564cee..69928e0325bc0538e3aea2804af2fb47c23b9c7b 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/src/mtc/include/impl/abstract_robot.h
@@ -25,6 +25,7 @@ enum wing_config{
 class Abstract_robot {
     protected:
     std::string name_;
+    tf2::Vector3 size_;
     tf2::Transform tf_;
     tf2::Transform root_tf_;
     std::vector<tf2::Transform> bounds_;
@@ -34,7 +35,7 @@ class Abstract_robot {
 
 
     public:
-    Abstract_robot(std::string name, tf2::Transform tf) : name_(name), tf_(tf) {
+    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()))); // ++
@@ -53,8 +54,10 @@ class Abstract_robot {
     
     inline std::string& name() { return name_;}
     inline tf2::Transform& tf() { return tf_;}
+    inline tf2::Vector3& size() { return size_;}
     inline tf2::Transform& root_tf() { return root_tf_;}
     inline std::vector<tf2::Transform>& bounds() {return bounds_;}
+    inline void size(tf2::Vector3& s) { size_ = s;}
     inline void set_tf(tf2::Transform& t) { tf_ = t;}
     inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;}
     inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;}
@@ -63,8 +66,11 @@ class Abstract_robot {
     inline void set_observer_mask(int i) {observer_mask_ = std::bitset<3>(i);}
 
 
+
     virtual void notify()= 0;
     virtual bool check_single_object_collision(tf2::Transform& obj)= 0;
+    virtual void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) =0;
+
 };
 
 
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index 700e6bfbbf3f4c3e24f4ebb989134aca7f02c28e..74a2da1e362693c38e929c96e1574a4dee0b1b3f 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/src/mtc/include/impl/mediator.h
@@ -19,12 +19,12 @@ class Mediator : public Abstract_mediator{
     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);
     void approximation(Robot* robot, int& wings);
-    void write_file(Robot* A, int& a, Robot* B, int& b);
+    void write_file(Robot* A, Robot* B);
 
 
     bool check_collision(const int& robot) override;
     void mediate() override;
-    void build_wings(std::bitset<3>& wings, Robot* robot) 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 766473719a260f56b0143def624782762f42f35f..77c2c499450f815e7e05a651a4b3908582751cfc 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -32,11 +32,11 @@ class Moveit_mediator : public Abstract_mediator{
 
     bool check_collision(const int& robot) override;
     void mediate() override;
-    void build_wings(std::bitset<3>& wing,Robot* robot) override;
+    void build_wings(std::bitset<3>& wing,int& robot) override;
 
     void publish_tables();
     void load_robot_description();
-
+    void rewrite_task_template();
 
 };
 
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
index 15d12a1b46f48986bdc90eebe8f71548562b119c..4178d1a128653d972f23b6c8a024c8db6d0167fb 100644
--- a/src/mtc/include/impl/moveit_robot.h
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -10,7 +10,7 @@ class Moveit_robot : public Robot{
     moveit::planning_interface::MoveGroupInterface* mgi_;
 
     public:
-    Moveit_robot(std::string name, tf2::Transform tf) : Robot(name, tf){
+    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());
     }
diff --git a/src/mtc/include/impl/robot.h b/src/mtc/include/impl/robot.h
index a7cc77bd61411c8b0e840f96c08772e2bf95e564..6a0a891e1806838977f4872cacf6be29c5c27eb6 100644
--- a/src/mtc/include/impl/robot.h
+++ b/src/mtc/include/impl/robot.h
@@ -25,7 +25,7 @@ class Robot : public Abstract_robot{
 
 
     public:
-    Robot(std::string name, tf2::Transform tf) : Abstract_robot(name, tf){ 
+    Robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){ 
         generate_access_fields();
     }
     
@@ -44,6 +44,7 @@ class Robot : public Abstract_robot{
     bool check_robot_collision(Robot* R);
 
     bool check_single_object_collision(tf2::Transform& obj) override;
+    void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) override;
     void notify() override;
 
 
diff --git a/src/mtc/include/impl/wing.h b/src/mtc/include/impl/wing.h
index 0b8921c40bac761e57e068a98577e7700a04570e..f94a8040bfa6dad9bfd29e943464fd42da24635a 100644
--- a/src/mtc/include/impl/wing.h
+++ b/src/mtc/include/impl/wing.h
@@ -5,13 +5,13 @@
 #include "impl/abstract_robot_element.h"
 
 class Wing  : public Abstract_robot_element{
-    private:
+    protected:
     std::string name_;
     tf2::Vector3 size_;
     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))); //+-
diff --git a/src/mtc/launch/base_routine.launch b/src/mtc/launch/base_routine.launch
index a021de08a8fb9410e8753334f40ecf5e16779687..6c5facb67a03d5b4f1b72472e6fcdc4d0e7be131 100644
--- a/src/mtc/launch/base_routine.launch
+++ b/src/mtc/launch/base_routine.launch
@@ -1,6 +1,8 @@
 <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"/>
+
 
     <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 7b4e44d6e7a88af379bef9c0734835f0e5605b7c..80eff90b47f04faf82758e88e05c1931329258a9 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -1,6 +1,6 @@
 <launch>    
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
-    <rosparam command="load" file="$(find mtc)/results/-229104537.yaml"/>
+    <rosparam command="load" file="$(find mtc)/results/298901127.yaml"/>
     <rosparam command="load" file="$(find mtc)/mtc_task_file/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/dummy.yaml b/src/mtc/mtc_task_file/dummy.yaml
index aeac2aae10b2f7fd557a034e8b5e2104b96931dd..44f7293c41c824f89aa79f2bad719f2b4e752210 100644
--- a/src/mtc/mtc_task_file/dummy.yaml
+++ b/src/mtc/mtc_task_file/dummy.yaml
@@ -11,11 +11,11 @@ planners:
 task:
   name: Pick and Place test
   properties:
-    group: panda_arm1
-    eef: hand_1
-    hand_grasping_frame: panda_1_link8
-    ik_frame: panda_1_link8
-    hand: hand_1
+    group: ""
+    eef: ""
+    hand_grasping_frame: ""
+    ik_frame: ""
+    hand: ""
   stages:
     - name: current
       type: CurrentState
@@ -33,7 +33,7 @@ task:
       planner: sampling
       id: hand_open
       properties:
-        group: hand_1
+        group: ""
       set:
         goal: open
     - type: Connect
@@ -53,15 +53,15 @@ task:
         - type: MoveRelative
           planner: cartesian
           properties:
-            link: panda_1_link8
-            min_distance: 0.07
-            max_distance: 0.2
+            link: ""
+            min_distance: 0
+            max_distance: 0
           propertiesConfigureInitFrom:
             source: PARENT
             values: [group]
           set:
             direction:
-              vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: panda_1_link8}}
+              vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: ""}}
         - type: ComputeIK
           properties: {max_ik_solutions: 5}
           set:
@@ -69,7 +69,7 @@ task:
               isometry:
                 translation: {x: 0.1, y: 0.0, z: 0.0}
                 quaternion: {r: 1.571, p: 0.785, y: 1.571}
-              link: panda_1_link8
+              link: ""
           propertiesConfigureInitFrom:
             - source: PARENT
               values: [eef, group]
@@ -90,19 +90,19 @@ task:
             allow_collisions:
               first: bottle
               second:
-                joint_model_group_name: hand_1
+                joint_model_group_name: ""
               allow: true
         - type: MoveTo
           planner: sampling
           properties:
-            group: hand_1
+            group: ""
           set:
             goal: close
         - type: ModifyPlanningScene
           set:
             attach_object:
-              object: bottle
-              link: panda_1_link8
+              object: ""
+              link: ""
         - type: MoveRelative
           planner: cartesian
           id: pick_up
@@ -114,7 +114,7 @@ task:
             max_distance: 0.2
           set:
             ik_frame:
-              link: panda_1_link8
+              link: ""
             direction:
               vector: {x: 0.0, y: 0.0, z: 1.0}
     - type: Connect
@@ -133,9 +133,9 @@ task:
         - type: MoveRelative
           planner: cartesian
           properties:
-            link: panda_1_link8
-            min_distance: 0.1
-            max_distance: 0.2
+            link: ""
+            min_distance: 0
+            max_distance: 0
           propertiesConfigureInitFrom:
             source: PARENT
             values: [group]
@@ -149,7 +149,7 @@ task:
               isometry:
                 translation: {x: 0.1, y: 0.0, z: 0.0}
                 quaternion: {r: 1.571, p: 0.785, y: 1.571}
-              link: panda_1_link8
+              link: ""
           propertiesConfigureInitFrom:
             - source: PARENT
               values: [eef, group]
@@ -160,40 +160,40 @@ task:
             set:
               monitored_stage: pick_up
               pose:
-                point: {x: 0.200000, y: 0.400000, z: 0.9305}
+                point: {x: 0.100000, y: 0.300000, z: 0.9305}
                 orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
         - type: MoveTo
           planner: sampling
           properties:
-            group: hand_1
+            group: ""
           set:
             goal: open
         - type: ModifyPlanningScene
           set:
             detach_object:
-              object: bottle
-              link: panda_1_link8
+              object: ""
+              link: ""
             allow_collisions:
               first: bottle
               second:
-                joint_model_group_name: hand_1
+                joint_model_group_name: ""
               allow: false
         - type: MoveRelative
           planner: cartesian
           properties:
-            link: panda_1_link8
-            min_distance: 0.07
-            max_distance: 0.2
+            link: ""
+            min_distance: 0
+            max_distance: 0
           propertiesConfigureInitFrom:
             source: PARENT
             values: [group]
           set:
             direction:
-              vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: panda_1_link8}}
+              vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: ""}}
         - type: MoveTo
           planner: sampling
           properties:
-            group: hand_1
+            group: ""
           set:
             goal: close
         - name: move to ready
diff --git a/src/mtc/results/-1004344220.yaml b/src/mtc/results/-1004344220.yaml
deleted file mode 100644
index 5ae3821597c5c5a632758e86d2dc5984f66a3b05..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1004344220.yaml
+++ /dev/null
@@ -1,25 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , '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_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/-1005378619.yaml b/src/mtc/results/-1005378619.yaml
deleted file mode 100644
index 9c8d6120a789d7687c24f5e03faab174c5b94f32..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1005378619.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.405000 , 'z': 0.880000 },'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.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/-1189622466.yaml b/src/mtc/results/-1189622466.yaml
deleted file mode 100644
index fa25c702b3f95058795c7a3a870fac6d3563ac33..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1189622466.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.737278 , 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.658065}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.763760, 'y': 1.243161 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_front_panel',  'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.880000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_left_panel',  'pos': { 'x': 0.536273, 'y': 1.356902 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'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.094569, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
-]}
\ No newline at end of file
diff --git a/src/mtc/results/-1268940442.yaml b/src/mtc/results/-1268940442.yaml
deleted file mode 100644
index bde7e383a0a40ffc3be9989eb3eb4da4ff3e73c2..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1268940442.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.252498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.600000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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/-1273525956.yaml b/src/mtc/results/-1273525956.yaml
deleted file mode 100644
index f257457419f50aef806ba6bd0ab365613ce03664..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1273525956.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.304996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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/-1278222581.yaml b/src/mtc/results/-1278222581.yaml
deleted file mode 100644
index ee379d80084a82666a51a3731c4ed5f6c0bf793c..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1278222581.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , '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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.600000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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/-1287586577.yaml b/src/mtc/results/-1287586577.yaml
deleted file mode 100644
index 863de40067fec48b37e9743fe7a51b3256535417..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1287586577.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.252498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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/-1295989721.yaml b/src/mtc/results/-1295989721.yaml
deleted file mode 100644
index 11914268727eb5e961d294db76d8cc8eba4133d0..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1295989721.yaml
+++ /dev/null
@@ -1,25 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , '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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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/-1296841825.yaml b/src/mtc/results/-1296841825.yaml
deleted file mode 100644
index f05c1874459a7afdb96bbcef7ff1312433e20dad..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1296841825.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.957498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.305000 , 'z': 0.880000 },'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.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/-1307606004.yaml b/src/mtc/results/-1307606004.yaml
deleted file mode 100644
index 2ec6c7cf11f48da8ea00eea0752e24cce5eca48b..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1307606004.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.304996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.880000 },'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/-1356501630.yaml b/src/mtc/results/-1356501630.yaml
deleted file mode 100644
index 33a632fd6466608b73075823f69b4872f04b97d1..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1356501630.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.039443, 'y': 1.593910, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.550015 , 'y': -0.056870 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': 0.096311, 'y': 2.243927 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.689460, 'y': 1.537042 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.017424, 'y': 0.943893 , 'z': 0.880000 },'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.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.179720, 'y': 1.613083, '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/-1373237725.yaml b/src/mtc/results/-1373237725.yaml
deleted file mode 100644
index 689e1ba7d841e0640013d9a52c42589da814acf8..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1373237725.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.039443, 'y': 1.593910, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.550015 , 'y': -0.056870 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': 0.096311, 'y': 2.243927 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.017424, 'y': 0.943893 , 'z': 0.880000 },'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.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.179720, 'y': 1.613083, '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/-1438350693.yaml b/src/mtc/results/-1438350693.yaml
deleted file mode 100644
index 57811b67ede97e0d86c0d3651c62c461a479c266..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1438350693.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.737277 , 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.658063}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.763757, 'y': 1.243163 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_front_panel',  'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.880000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_left_panel',  'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'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.094566, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
-]}
\ No newline at end of file
diff --git a/src/mtc/results/-1023873889.yaml b/src/mtc/results/-1560146588.yaml
similarity index 100%
rename from src/mtc/results/-1023873889.yaml
rename to src/mtc/results/-1560146588.yaml
diff --git a/src/mtc/results/-1596565971.yaml b/src/mtc/results/-1596565971.yaml
deleted file mode 100644
index 0e98be64e571ef8dd18fa7337dd5dbdcafbdcf98..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1596565971.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.499998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.152498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.500000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.847498 , 'z': 0.880000 },'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.220006, 'y': 1.499997, '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/-1619491065.yaml b/src/mtc/results/-1619491065.yaml
deleted file mode 100644
index 0357a3b44162fe5aabe4db9e940f370a0b069998..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1619491065.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.499998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.152498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.847498 , 'z': 0.880000 },'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.220006, 'y': 1.499997, '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/-1628521134.yaml b/src/mtc/results/-1628521134.yaml
deleted file mode 100644
index 21518ec28215fbb5d3103b02fed88b7638ed8a22..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1628521134.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.857498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.205000 , 'z': 0.880000 },'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.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/-1676858731.yaml b/src/mtc/results/-1676858731.yaml
deleted file mode 100644
index d348ff1209bdbe96b3793a50cd63a0cd621f4f18..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1676858731.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.204996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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/-1713253988.yaml b/src/mtc/results/-1713253988.yaml
deleted file mode 100644
index af01fa96d0c9d1c26efdf471ff510cfae0211c4c..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1713253988.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.204996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.880000 },'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/-1199338409.yaml b/src/mtc/results/-1785667697.yaml
similarity index 100%
rename from src/mtc/results/-1199338409.yaml
rename to src/mtc/results/-1785667697.yaml
diff --git a/src/mtc/results/-1816178217.yaml b/src/mtc/results/-1816178217.yaml
deleted file mode 100644
index 849af50d74ec61b474db45891bd776debed82dfc..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1816178217.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.400000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.747498 , 'z': 0.880000 },'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/-1225592716.yaml b/src/mtc/results/-1821002738.yaml
similarity index 100%
rename from src/mtc/results/-1225592716.yaml
rename to src/mtc/results/-1821002738.yaml
diff --git a/src/mtc/results/-1835119305.yaml b/src/mtc/results/-1835119305.yaml
deleted file mode 100644
index 46eace67316e3c9ddbdf37a5cfbe2c4eed3ec220..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1835119305.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.747498 , 'z': 0.880000 },'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/-1844242686.yaml b/src/mtc/results/-1844242686.yaml
deleted file mode 100644
index 3772355cd58943882ee278b3cab6e214986be721..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1844242686.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.105000 , 'z': 0.880000 },'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.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/-1852888959.yaml b/src/mtc/results/-1852888959.yaml
deleted file mode 100644
index d84443ddd3f564c42292b437d96cded15f650fdf..0000000000000000000000000000000000000000
--- a/src/mtc/results/-1852888959.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.400000 , 'z': 0.880000 },'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.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/-1314536815.yaml b/src/mtc/results/-1934684646.yaml
similarity index 100%
rename from src/mtc/results/-1314536815.yaml
rename to src/mtc/results/-1934684646.yaml
diff --git a/src/mtc/results/-199414991.yaml b/src/mtc/results/-199414991.yaml
deleted file mode 100644
index d800cac7b24821723d6ed60950366955c629ac39..0000000000000000000000000000000000000000
--- a/src/mtc/results/-199414991.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.499998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.152498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.500000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.847498 , 'z': 0.880000 },'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.220006, 'y': 1.499997, '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/-867380608.yaml b/src/mtc/results/-2005299998.yaml
similarity index 100%
rename from src/mtc/results/-867380608.yaml
rename to src/mtc/results/-2005299998.yaml
diff --git a/src/mtc/results/-891801775.yaml b/src/mtc/results/-2042372792.yaml
similarity index 100%
rename from src/mtc/results/-891801775.yaml
rename to src/mtc/results/-2042372792.yaml
diff --git a/src/mtc/results/-901045779.yaml b/src/mtc/results/-2054540989.yaml
similarity index 100%
rename from src/mtc/results/-901045779.yaml
rename to src/mtc/results/-2054540989.yaml
diff --git a/src/mtc/results/-908818017.yaml b/src/mtc/results/-2065119826.yaml
similarity index 100%
rename from src/mtc/results/-908818017.yaml
rename to src/mtc/results/-2065119826.yaml
diff --git a/src/mtc/results/-1452613170.yaml b/src/mtc/results/-2070036432.yaml
similarity index 100%
rename from src/mtc/results/-1452613170.yaml
rename to src/mtc/results/-2070036432.yaml
diff --git a/src/mtc/results/-1483383559.yaml b/src/mtc/results/-2105624039.yaml
similarity index 100%
rename from src/mtc/results/-1483383559.yaml
rename to src/mtc/results/-2105624039.yaml
diff --git a/src/mtc/results/-220340715.yaml b/src/mtc/results/-220340715.yaml
deleted file mode 100644
index 9dbc3ef3949c4ed6c8128ad2946b860877b17d17..0000000000000000000000000000000000000000
--- a/src/mtc/results/-220340715.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.499998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.152498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.847498 , 'z': 0.880000 },'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.220006, 'y': 1.499997, '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/-229104537.yaml b/src/mtc/results/-229104537.yaml
deleted file mode 100644
index c4534754f8b379805ec652a821ef988359f8e61f..0000000000000000000000000000000000000000
--- a/src/mtc/results/-229104537.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.857498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.205000 , 'z': 0.880000 },'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.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/-247145609.yaml b/src/mtc/results/-247145609.yaml
deleted file mode 100644
index af053d955077d7446999f55b0388a24128b7d06d..0000000000000000000000000000000000000000
--- a/src/mtc/results/-247145609.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.857498 , 'z': 0.880000 },'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/-420832807.yaml b/src/mtc/results/-420832807.yaml
deleted file mode 100644
index ae54c2a54a5b6499d270772ce7ba08287aa6f529..0000000000000000000000000000000000000000
--- a/src/mtc/results/-420832807.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.400000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.747498 , 'z': 0.880000 },'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/-438677964.yaml b/src/mtc/results/-438677964.yaml
deleted file mode 100644
index d101578d02304a8c1584da60fc953c1c09821ef5..0000000000000000000000000000000000000000
--- a/src/mtc/results/-438677964.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.747498 , 'z': 0.880000 },'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/-447701259.yaml b/src/mtc/results/-447701259.yaml
deleted file mode 100644
index 259f28a14cfe74ea63dd5a6d0ee62888a7ced8c0..0000000000000000000000000000000000000000
--- a/src/mtc/results/-447701259.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.105000 , 'z': 0.880000 },'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.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/-456522214.yaml b/src/mtc/results/-456522214.yaml
deleted file mode 100644
index b0a8170ab6ddd0919a569e2892548e3b0d82d82b..0000000000000000000000000000000000000000
--- a/src/mtc/results/-456522214.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.400000 , 'z': 0.880000 },'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.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/-465284919.yaml b/src/mtc/results/-465284919.yaml
deleted file mode 100644
index 811be5d70db02dd8ef4b153e16df2c8f2a2b8a48..0000000000000000000000000000000000000000
--- a/src/mtc/results/-465284919.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.880000 },'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/-472754381.yaml b/src/mtc/results/-472754381.yaml
deleted file mode 100644
index a47668756701c18b1d016aab7b7596b31d64b247..0000000000000000000000000000000000000000
--- a/src/mtc/results/-472754381.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.200000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.880000 },'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/-47588666.yaml b/src/mtc/results/-47588666.yaml
deleted file mode 100644
index 6dd0489fe9b8abec2aa7a30468d04b9d805beef8..0000000000000000000000000000000000000000
--- a/src/mtc/results/-47588666.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.737277 , 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.658063}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.650015 , 'y': 0.056867 , 'z': 0.880000 } , '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': 'table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.763757, 'y': 1.243163 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_front_panel',  'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.880000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_left_panel',  'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'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.094566, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
-]}
\ No newline at end of file
diff --git a/src/mtc/results/-61102411.yaml b/src/mtc/results/-61102411.yaml
deleted file mode 100644
index a0c98d25c482baf785a5c4ae1c481c6ac143c9d7..0000000000000000000000000000000000000000
--- a/src/mtc/results/-61102411.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.737277 , 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.658063}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.650015 , 'y': 0.056867 , 'z': 0.880000 } , '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': 'table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.880000 } , '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_front_panel',  'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.880000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_left_panel',  'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'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.094566, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737277, 'w': 0.675590 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
-]}
\ No newline at end of file
diff --git a/src/mtc/results/-858151830.yaml b/src/mtc/results/-858151830.yaml
deleted file mode 100644
index 97a88f83c33884e43cdb26850e1e17295579b8a9..0000000000000000000000000000000000000000
--- a/src/mtc/results/-858151830.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.404996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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/-892179355.yaml b/src/mtc/results/-892179355.yaml
deleted file mode 100644
index 73186d34aa9164391d33406642fb552f012ee307..0000000000000000000000000000000000000000
--- a/src/mtc/results/-892179355.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.404996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.880000 },'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/-931589105.yaml b/src/mtc/results/-931589105.yaml
deleted file mode 100644
index ad1ff9390e8e5226e7cea7670c52c7daf3625f7a..0000000000000000000000000000000000000000
--- a/src/mtc/results/-931589105.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.039443, 'y': 1.693910, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': 0.096311, 'y': 2.343927 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.689460, 'y': 1.637042 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.017424, 'y': 1.043893 , 'z': 0.880000 },'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.179720, 'y': 1.713084, '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/-94055376.yaml b/src/mtc/results/-94055376.yaml
deleted file mode 100644
index 135dd7c788f206aae636c5251d6ce22e2b8bafaf..0000000000000000000000000000000000000000
--- a/src/mtc/results/-94055376.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.650015 , 'y': 0.056867 , 'z': 0.880000 } , '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': 'table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.880000 },'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/-941105643.yaml b/src/mtc/results/-941105643.yaml
deleted file mode 100644
index d74a367f3e3e4438538cdeeeb45c7f1e64a54b76..0000000000000000000000000000000000000000
--- a/src/mtc/results/-941105643.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.039443, 'y': 1.693910, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.880000 } , '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_front_panel',  'pos': { 'x': 0.689460, 'y': 1.637042 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.017424, 'y': 1.043893 , 'z': 0.880000 },'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.179720, 'y': 1.713084, '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/-949494958.yaml b/src/mtc/results/-949494958.yaml
deleted file mode 100644
index b491707cc97f5cc915aedaf315105eb383067eb3..0000000000000000000000000000000000000000
--- a/src/mtc/results/-949494958.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.039443, 'y': 1.693910, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': 0.096311, 'y': 2.343927 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.017424, 'y': 1.043893 , 'z': 0.880000 },'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.179720, 'y': 1.713084, '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/-957403952.yaml b/src/mtc/results/-957403952.yaml
deleted file mode 100644
index a3d6100da5ee76ba03639dac2f89b01d07086f63..0000000000000000000000000000000000000000
--- a/src/mtc/results/-957403952.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.039443, 'y': 1.693910, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.880000 } , '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_left_panel',  'pos': { 'x': -0.017424, 'y': 1.043893 , 'z': 0.880000 },'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.179720, 'y': 1.713084, '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/-958273308.yaml b/src/mtc/results/-958273308.yaml
deleted file mode 100644
index f405cc190fad8b533525cff70d3376b95750d9fa..0000000000000000000000000000000000000000
--- a/src/mtc/results/-958273308.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': 0.070600, 'y': 2.050050 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.663750, 'y': 1.343165 , 'z': 0.880000 },'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.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/-975176066.yaml b/src/mtc/results/-975176066.yaml
deleted file mode 100644
index b710146ca3ee92d00b612f9f08d38874a3d98ea3..0000000000000000000000000000000000000000
--- a/src/mtc/results/-975176066.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.352498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.700000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/-984742542.yaml b/src/mtc/results/-984742542.yaml
deleted file mode 100644
index b26110cd3234800e1925b846f6737751a37477fd..0000000000000000000000000000000000000000
--- a/src/mtc/results/-984742542.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , '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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.700000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/-995314920.yaml b/src/mtc/results/-995314920.yaml
deleted file mode 100644
index 15e47690e255bba191f525a19caded219c3393f2..0000000000000000000000000000000000000000
--- a/src/mtc/results/-995314920.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.352498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/1938381481.yaml b/src/mtc/results/1009749163.yaml
similarity index 100%
rename from src/mtc/results/1938381481.yaml
rename to src/mtc/results/1009749163.yaml
diff --git a/src/mtc/results/1974362417.yaml b/src/mtc/results/1055349651.yaml
similarity index 100%
rename from src/mtc/results/1974362417.yaml
rename to src/mtc/results/1055349651.yaml
diff --git a/src/mtc/results/1982506577.yaml b/src/mtc/results/1066094314.yaml
similarity index 100%
rename from src/mtc/results/1982506577.yaml
rename to src/mtc/results/1066094314.yaml
diff --git a/src/mtc/results/1992027095.yaml b/src/mtc/results/1078753196.yaml
similarity index 100%
rename from src/mtc/results/1992027095.yaml
rename to src/mtc/results/1078753196.yaml
diff --git a/src/mtc/results/2017996924.yaml b/src/mtc/results/1114475889.yaml
similarity index 100%
rename from src/mtc/results/2017996924.yaml
rename to src/mtc/results/1114475889.yaml
diff --git a/src/mtc/results/-1730204296.yaml b/src/mtc/results/1121318535.yaml
similarity index 100%
rename from src/mtc/results/-1730204296.yaml
rename to src/mtc/results/1121318535.yaml
diff --git a/src/mtc/results/-1722252488.yaml b/src/mtc/results/1132193382.yaml
similarity index 100%
rename from src/mtc/results/-1722252488.yaml
rename to src/mtc/results/1132193382.yaml
diff --git a/src/mtc/results/-1712921737.yaml b/src/mtc/results/1144412079.yaml
similarity index 100%
rename from src/mtc/results/-1712921737.yaml
rename to src/mtc/results/1144412079.yaml
diff --git a/src/mtc/results/1850540425.yaml b/src/mtc/results/114528184.yaml
similarity index 100%
rename from src/mtc/results/1850540425.yaml
rename to src/mtc/results/114528184.yaml
diff --git a/src/mtc/results/2052037927.yaml b/src/mtc/results/1160060243.yaml
similarity index 100%
rename from src/mtc/results/2052037927.yaml
rename to src/mtc/results/1160060243.yaml
diff --git a/src/mtc/results/2060212609.yaml b/src/mtc/results/1170927557.yaml
similarity index 100%
rename from src/mtc/results/2060212609.yaml
rename to src/mtc/results/1170927557.yaml
diff --git a/src/mtc/results/-1686073121.yaml b/src/mtc/results/1176800679.yaml
similarity index 100%
rename from src/mtc/results/-1686073121.yaml
rename to src/mtc/results/1176800679.yaml
diff --git a/src/mtc/results/2069892512.yaml b/src/mtc/results/1183463579.yaml
similarity index 100%
rename from src/mtc/results/2069892512.yaml
rename to src/mtc/results/1183463579.yaml
diff --git a/src/mtc/results/2095988690.yaml b/src/mtc/results/1218779973.yaml
similarity index 100%
rename from src/mtc/results/2095988690.yaml
rename to src/mtc/results/1218779973.yaml
diff --git a/src/mtc/results/143629389.yaml b/src/mtc/results/143629389.yaml
deleted file mode 100644
index 3c1f875fa39738ca5d65ca7f3bed327b208a4fe6..0000000000000000000000000000000000000000
--- a/src/mtc/results/143629389.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.650015 , 'y': 0.056869 , 'z': 0.880000 } , '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': 'table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.536273, 'y': 1.356903 , 'z': 0.880000 },'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/1875104773.yaml b/src/mtc/results/148591919.yaml
similarity index 100%
rename from src/mtc/results/1875104773.yaml
rename to src/mtc/results/148591919.yaml
diff --git a/src/mtc/results/-1874569492.yaml b/src/mtc/results/1664651135.yaml
similarity index 100%
rename from src/mtc/results/-1874569492.yaml
rename to src/mtc/results/1664651135.yaml
diff --git a/src/mtc/results/-1865328423.yaml b/src/mtc/results/1675054456.yaml
similarity index 100%
rename from src/mtc/results/-1865328423.yaml
rename to src/mtc/results/1675054456.yaml
diff --git a/src/mtc/results/-1333268938.yaml b/src/mtc/results/1681835555.yaml
similarity index 100%
rename from src/mtc/results/-1333268938.yaml
rename to src/mtc/results/1681835555.yaml
diff --git a/src/mtc/results/168513346.yaml b/src/mtc/results/168513346.yaml
deleted file mode 100644
index 487b104e9b459937fba70e4c2b6018b643b7532f..0000000000000000000000000000000000000000
--- a/src/mtc/results/168513346.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.737278 , 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.658065}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.650015 , 'y': 0.056869 , 'z': 0.880000 } , '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': 'table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.880000 } , '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_front_panel',  'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.880000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_left_panel',  'pos': { 'x': 0.536273, 'y': 1.356902 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'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.094569, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
-]}
\ No newline at end of file
diff --git a/src/mtc/results/-1316276304.yaml b/src/mtc/results/1692277982.yaml
similarity index 100%
rename from src/mtc/results/-1316276304.yaml
rename to src/mtc/results/1692277982.yaml
diff --git a/src/mtc/results/-1307278153.yaml b/src/mtc/results/1704405179.yaml
similarity index 100%
rename from src/mtc/results/-1307278153.yaml
rename to src/mtc/results/1704405179.yaml
diff --git a/src/mtc/results/-1282769960.yaml b/src/mtc/results/1737201961.yaml
similarity index 100%
rename from src/mtc/results/-1282769960.yaml
rename to src/mtc/results/1737201961.yaml
diff --git a/src/mtc/results/177732555.yaml b/src/mtc/results/177732555.yaml
deleted file mode 100644
index da1837c832a6ecac5b8db13782918c90331c0785..0000000000000000000000000000000000000000
--- a/src/mtc/results/177732555.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.737278 , 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.658065}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.650015 , 'y': 0.056869 , 'z': 0.880000 } , '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': 'table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.763760, 'y': 1.243161 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_front_panel',  'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.880000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'table2_left_panel',  'pos': { 'x': 0.536273, 'y': 1.356902 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'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.094569, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.737278, 'w': 0.675589 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
-]}
\ No newline at end of file
diff --git a/src/mtc/results/1782845983.yaml b/src/mtc/results/1782845983.yaml
deleted file mode 100644
index 0e79f406c304c63d203bd74bea3b8d85711eed9d..0000000000000000000000000000000000000000
--- a/src/mtc/results/1782845983.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.736284, 'y': 1.356900 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.880000 },'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/1817083531.yaml b/src/mtc/results/1817083531.yaml
deleted file mode 100644
index 016f3d14b9030bb8efdb210f24fa833e1b729d81..0000000000000000000000000000000000000000
--- a/src/mtc/results/1817083531.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.736284, 'y': 1.356900 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.563750, 'y': 1.243165 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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/1884541548.yaml b/src/mtc/results/1884541548.yaml
deleted file mode 100644
index 40973d11fc2f0bbf9a0e41e0194c7fdb7ac1ee6e..0000000000000000000000000000000000000000
--- a/src/mtc/results/1884541548.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.200004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.452498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.200000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.852505, 'y': 1.304996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.200007, 'y': 1.957498 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.452495, 'y': 1.305000 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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.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.200005, '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/1773423387.yaml b/src/mtc/results/18966477.yaml
similarity index 100%
rename from src/mtc/results/1773423387.yaml
rename to src/mtc/results/18966477.yaml
diff --git a/src/mtc/results/1909504090.yaml b/src/mtc/results/1909504090.yaml
deleted file mode 100644
index 26acf4beb91e0a263495a96a6dd0c8d4ca060770..0000000000000000000000000000000000000000
--- a/src/mtc/results/1909504090.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.204996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.880000 },'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/1948198908.yaml b/src/mtc/results/1948198908.yaml
deleted file mode 100644
index 5df99f22b68ef02a5042a35567f0931a8b281605..0000000000000000000000000000000000000000
--- a/src/mtc/results/1948198908.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.204996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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/-1645854958.yaml b/src/mtc/results/1985071259.yaml
similarity index 100%
rename from src/mtc/results/-1645854958.yaml
rename to src/mtc/results/1985071259.yaml
diff --git a/src/mtc/results/1991728090.yaml b/src/mtc/results/1991728090.yaml
deleted file mode 100644
index 5b526782260e5c37d86c6bd32379acadd68543ad..0000000000000000000000000000000000000000
--- a/src/mtc/results/1991728090.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.304996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.880000 },'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/2027358893.yaml b/src/mtc/results/2027358893.yaml
deleted file mode 100644
index fa627e486d3abadba9f199b93dc255f501c1657b..0000000000000000000000000000000000000000
--- a/src/mtc/results/2027358893.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.880000 } , '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_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.880000 } , '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_right_panel',  'pos': { 'x': -0.736284, 'y': 1.456900 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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.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.105441, 'y': 1.180869, '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/2028141574.yaml b/src/mtc/results/2028141574.yaml
deleted file mode 100644
index 6b661e84c2b4401ac5550766493d53947688b9fa..0000000000000000000000000000000000000000
--- a/src/mtc/results/2028141574.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.304996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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/2069479591.yaml b/src/mtc/results/2069479591.yaml
deleted file mode 100644
index dfbd9cf248a65248f7c770926302c4c433c49c53..0000000000000000000000000000000000000000
--- a/src/mtc/results/2069479591.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.404996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.880000 },'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/2105909767.yaml b/src/mtc/results/2105909767.yaml
deleted file mode 100644
index 4c7ad5100436e3258604808f8e75babeea630b51..0000000000000000000000000000000000000000
--- a/src/mtc/results/2105909767.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'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_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.880000 } , '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_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.752505, 'y': 1.404996 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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': 'table2_front_panel',  'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'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/-974999345.yaml b/src/mtc/results/2145537375.yaml
similarity index 100%
rename from src/mtc/results/-974999345.yaml
rename to src/mtc/results/2145537375.yaml
diff --git a/src/mtc/results/1991822656.yaml b/src/mtc/results/298901127.yaml
similarity index 100%
rename from src/mtc/results/1991822656.yaml
rename to src/mtc/results/298901127.yaml
diff --git a/src/mtc/results/1783220770.yaml b/src/mtc/results/30759948.yaml
similarity index 100%
rename from src/mtc/results/1783220770.yaml
rename to src/mtc/results/30759948.yaml
diff --git a/src/mtc/results/2017802615.yaml b/src/mtc/results/331487535.yaml
similarity index 100%
rename from src/mtc/results/2017802615.yaml
rename to src/mtc/results/331487535.yaml
diff --git a/src/mtc/results/342047305.yaml b/src/mtc/results/342047305.yaml
deleted file mode 100644
index da024ab54cf38ecfef67a446ce80eeb33bbde9f2..0000000000000000000000000000000000000000
--- a/src/mtc/results/342047305.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.880000 },'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/366115478.yaml b/src/mtc/results/366115478.yaml
deleted file mode 100644
index 7a60196cc73a003a1e23005b92fc23a25b1c3c6e..0000000000000000000000000000000000000000
--- a/src/mtc/results/366115478.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.405000 , 'z': 0.880000 },'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.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/368136013.yaml b/src/mtc/results/368136013.yaml
deleted file mode 100644
index 87d06b83b3bbab71ed71e86488a7b847a2292e8b..0000000000000000000000000000000000000000
--- a/src/mtc/results/368136013.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , '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_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/382291000.yaml b/src/mtc/results/382291000.yaml
deleted file mode 100644
index c53925b9d4305e7bef0f128da39c6a24fdca4284..0000000000000000000000000000000000000000
--- a/src/mtc/results/382291000.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.352498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/391137309.yaml b/src/mtc/results/391137309.yaml
deleted file mode 100644
index c88f5d15598d27852c648e03f147385859b3c15e..0000000000000000000000000000000000000000
--- a/src/mtc/results/391137309.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , '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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.700000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/399983059.yaml b/src/mtc/results/399983059.yaml
deleted file mode 100644
index 401b84729ea431ceedcba7e3211dd130c4b51a38..0000000000000000000000000000000000000000
--- a/src/mtc/results/399983059.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.699998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': 0.100000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.352498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.700000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 1.047498 , 'z': 0.880000 },'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.220006, 'y': 1.699997, '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/54073374.yaml b/src/mtc/results/54073374.yaml
deleted file mode 100644
index e3940be20871e58f76a428f5ad70ce424782e38e..0000000000000000000000000000000000000000
--- a/src/mtc/results/54073374.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.957498 , 'z': 0.880000 },'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/1807591579.yaml b/src/mtc/results/63055095.yaml
similarity index 100%
rename from src/mtc/results/1807591579.yaml
rename to src/mtc/results/63055095.yaml
diff --git a/src/mtc/results/70792428.yaml b/src/mtc/results/70792428.yaml
deleted file mode 100644
index a579190e1299f1fd2eb01911caafc69884fa8cb4..0000000000000000000000000000000000000000
--- a/src/mtc/results/70792428.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 1.957498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652495, 'y': 1.305000 , 'z': 0.880000 },'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.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/71971474.yaml b/src/mtc/results/71971474.yaml
deleted file mode 100644
index 75bd9cb0512ebfc926487f6f3bf0d8cb55df47d4..0000000000000000000000000000000000000000
--- a/src/mtc/results/71971474.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , '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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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/82360301.yaml b/src/mtc/results/82360301.yaml
deleted file mode 100644
index ce8cecb421999ff399528b310845130e2aee05e6..0000000000000000000000000000000000000000
--- a/src/mtc/results/82360301.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.252498 , 'z': 0.880000 },'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_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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/1765091973.yaml b/src/mtc/results/8759084.yaml
similarity index 100%
rename from src/mtc/results/1765091973.yaml
rename to src/mtc/results/8759084.yaml
diff --git a/src/mtc/results/90793898.yaml b/src/mtc/results/90793898.yaml
deleted file mode 100644
index 7aab5dab25a05718eb4528e7dbe10bf5ec56bfb9..0000000000000000000000000000000000000000
--- a/src/mtc/results/90793898.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , '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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.600000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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/1886450692.yaml b/src/mtc/results/950455502.yaml
similarity index 100%
rename from src/mtc/results/1886450692.yaml
rename to src/mtc/results/950455502.yaml
diff --git a/src/mtc/results/1894634384.yaml b/src/mtc/results/961502182.yaml
similarity index 100%
rename from src/mtc/results/1894634384.yaml
rename to src/mtc/results/961502182.yaml
diff --git a/src/mtc/results/1909836411.yaml b/src/mtc/results/974324017.yaml
similarity index 100%
rename from src/mtc/results/1909836411.yaml
rename to src/mtc/results/974324017.yaml
diff --git a/src/mtc/results/99762085.yaml b/src/mtc/results/99762085.yaml
deleted file mode 100644
index 31560d850f62df2c662d1ca1609e8355fac21d25..0000000000000000000000000000000000000000
--- a/src/mtc/results/99762085.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-{ '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.12,'width': 0.12,'height': 0.12 },'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.599998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.652498 , 'y': -0.000000 , 'z': 0.880000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000008, 'y': 2.252498 , 'z': 0.880000 },'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_front_panel',  'pos': { 'x': 0.652494, 'y': 1.600000 , 'z': 0.880000 },'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': 'table2_left_panel',  'pos': { 'x': -0.000004, 'y': 0.947498 , 'z': 0.880000 },'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.220006, 'y': 1.599997, '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 6495ca44bff50aee0a263e0253f9e3e8b04a8799..c222c7e92ea43e826cff10b6aebd6fb7d0500e68 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -20,10 +20,10 @@ int main(int argc, char **argv){
     ros::init(argc, argv, "base_routine");
     ros::NodeHandle n;
 
-    XmlRpc::XmlRpcValue map;
-    XmlRpc::XmlRpcValue task;
+    XmlRpc::XmlRpcValue map, task, resources;
 
     n.getParam("/data",map);
+    n.getParam("/objects",resources);
     n.getParam("/task/groups",task);
 
     
@@ -36,39 +36,373 @@ 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);
 
-    visualization_msgs::MarkerArray* wings1 = new visualization_msgs::MarkerArray();
-    visualization_msgs::MarkerArray* wings2 = new visualization_msgs::MarkerArray();
-    visualization_msgs::MarkerArray* fields1 = new visualization_msgs::MarkerArray();
-    visualization_msgs::MarkerArray* fields2 = new visualization_msgs::MarkerArray();
+    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_per_robot[0] = blueprints;
+    blueprints_per_robot[1] = blueprints;
 
-    Abstract_robot* robo = new Robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)));
-    Robot* ceti = dynamic_cast<Robot*>(robo);
-    ceti->set_observer_mask(wing_config::RML);
-    ceti->add_rviz_markers(wings1);
-    ceti->add_rviz_markers(fields1);
+    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" ){
+                    ROS_INFO("Bin in Robot 1");
+                    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;
 
-    map_loader->write_task(robo);
+                    for(; itt != resources[i].end(); ++itt){
+                        if(itt->first == "size"){
+                            ROS_INFO("Bin in size von robot 1");
+                            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;
+                            }
+                        }
 
-    
-    mediator->connect_robots(robo);
+                        if(itt->first == "pos"){
+                            ROS_INFO("Bin in pos von robot 1");
+                            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;
+                            }
+                        }
 
-    Abstract_robot* robo2 = new Robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)));
-    Robot* ceti2 = dynamic_cast<Robot*>(robo2);
-    ceti2->set_observer_mask(wing_config::RML);
-    ceti2->add_rviz_markers(wings2);
-    ceti2->add_rviz_markers(fields2);
+                        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;
+                            }
+                            ROS_INFO("Bin in orientation von robot 1");
 
-    mediator->connect_robots(robo2);
-    mediator->mediate();
 
-    
+                        }
+                    }
+                
+                    Abstract_robot* robo = new 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));
+                    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();
+                            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 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));
+                    Robot* ceti2 = dynamic_cast<Robot*>(robo2);
+                    ceti2->set_observer_mask(wing_config::RML);
+                    ceti2->add_rviz_markers(new visualization_msgs::MarkerArray());
+                    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;
+                            }
+                        }
 
-    /*
-    for (int i = 0; i < ceti->access_fields1().size(); i++){
-        ceti->access_fields1()[i] = new Field_rviz_decorator(ceti->access_fields1()[i], fields1);
+                        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);
+                }
+            }
+        }
     }
-    */
-    
+    mediator->set_wings(blueprints_per_robot);
+    mediator->mediate();
+
+    //map_loader->write_task(robo);
+
     //free(rviz_right);
     //free(rviz_mid);
     //free(rviz_left);
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
index 5e0880ef6114757e481553f9279485818b0953e9..bf7c2b457e189dca8b17d44d99e212a03ed4ae72 100644
--- a/src/mtc/src/cell_routine.cpp
+++ b/src/mtc/src/cell_routine.cpp
@@ -26,115 +26,382 @@ int main(int argc, char **argv){
     
     Abstract_map_loader* map_loader = new Map_loader(map, task);
 
-    
-    float x1 = 0; float y1 = 0; float z1 = 0; float qx1 = 0; float qy1 = 0; float qz1 = 0; float qw1 = 0;
-    float x2 = 0; float y2 = 0; float z2 = 0; float qx2 = 0; float qy2 = 0; float qz2 = 0; float qw2 = 0;
+          
+    ros::Publisher* pub = new ros::Publisher(nh.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor
+
+    Abstract_mediator* mediator = new Moveit_mediator(map_loader->task_grasps(), pub, nh);
+    Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator);
+
+    moveit_mediator->load_robot_description();
+
+    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_per_robot[0] = blueprints;
+    blueprints_per_robot[1] = blueprints;
     int a = 0; int b = 0;
 
     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();
-            for(; itt != resources[i].end(); ++itt){
-              if(itt->first == "pos"){
-                XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                for(; ittt != itt->second.end(); ++ittt){
-                  float t = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                  ROS_INFO("%f", t);
-                  if(ittt->first == "x") x1 = t;
-                  if(ittt->first == "y") y1 = t;
-                  if(ittt->first == "z") z1 = t;
+        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(itt->first == "orientation"){
-                XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                for(; ittt != itt->second.end(); ++ittt){
-                  float t = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                  ROS_INFO("%f", t);
-                  if(ittt->first == "x") qx1 = t;
-                  if(ittt->first == "y") qy1 = t;
-                  if(ittt->first == "z") qz1 = t;
-                  if(ittt->first == "w") qw1 = t;
+
+                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 == "table2_table_top" ){
-            XmlRpc::XmlRpcValue::ValueStruct::const_iterator itt = resources[i].begin();
-            for(; itt != resources[i].end(); ++itt){
-              if(itt->first == "pos"){
-                XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                for(; ittt != itt->second.end(); ++ittt){
-                  float t = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                  ROS_INFO("%f", t);
-                  if(ittt->first == "x") x2 = t;
-                  if(ittt->first == "y") y2 = t;
-                  if(ittt->first == "z") z2 = t;
+
+                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(itt->first == "orientation"){
-                XmlRpc::XmlRpcValue::ValueStruct::const_iterator ittt = itt->second.begin();
-                for(; ittt != itt->second.end(); ++ittt){
-                  float t = (ittt->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(ittt->second) : (double) ittt->second;
-                  ROS_INFO("%f", t);
-                  if(ittt->first == "x") qx2 = t;
-                  if(ittt->first == "y") qy2 = t;
-                  if(ittt->first == "z") qz2 = t;
-                  if(ittt->first == "w") qw2 = t;
+
+                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 == "table1_right_panel" ) { a += std::pow(2, 0);}
-          if(top == "table1_front_panel" ) { a += std::pow(2, 1);}
-          if(top == "table1_left_panel" ) { 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(top == "table2_right_panel" ) { b += std::pow(2, 0);}
-          if(top == "table2_front_panel" ) { b += std::pow(2, 1);}
-          if(top == "table2_left_panel" ) { b += std::pow(2, 2);}
-        }
-      }
-    }
+                        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;
+                            }
+                        }
 
-      
-    ros::Publisher* pub = new ros::Publisher(nh.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor
+                         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_mediator* mediator = new Moveit_mediator(map_loader->task_grasps(), pub, nh);
-    Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator);
+                         }
+                    }
+                                             
+                    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);
+                }
 
-    moveit_mediator->load_robot_description();
+                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;
+                            }
+                        }
 
-    Abstract_robot* robo1 = new Moveit_robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(qx1,qy1,qz1, qw1), tf2::Vector3(x1,y1, z1/2)));
-    Abstract_robot* robo2 = new Moveit_robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(qx2,qy2,qz2, qw2), tf2::Vector3(x2,y2, z2/2)));
-    robo1->set_observer_mask(wing_config::RML);
-    robo2->set_observer_mask(wing_config::RML);
+                         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;
+                            }
+                        }
 
-    mediator->connect_robots(robo1);
-    mediator->connect_robots(robo2);
+                        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;
+                            }
 
-    
-    Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(robo1);
-    Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robo2);
-    
+                         }
+                    }
+                                             
+                    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);
+                }
+        
+            }
+        }
+    }
+
+    moveit_mediator->set_wings(blueprints_per_robot);
+    moveit_mediator->rewrite_task_template();
+    Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]);
+    Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]);
     
     std::bitset<3> ia = a;
     std::bitset<3> ib = b;
-    moveit_mediator->build_wings(ia, ceti1);
-    moveit_mediator->build_wings(ib, ceti2);
+    int r1 = 0; int r2 = 1;
+    moveit_mediator->build_wings(ia, r1);
+    moveit_mediator->build_wings(ib, r2);
 
     ceti1->notify();
     ceti2->notify();
     moveit_mediator->publish_tables();
     moveit_mediator->mediate();
-    //moveit_mediator->check_collision(1);
     
 
 
diff --git a/src/mtc/src/impl/collision_helper.cpp b/src/mtc/src/impl/collision_helper.cpp
index 66cc07240873fc3980e8c04d84af7d19e4333669..47703e3abbc76b962e92524e525b32fe2905bbc0 100644
--- a/src/mtc/src/impl/collision_helper.cpp
+++ b/src/mtc/src/impl/collision_helper.cpp
@@ -4,7 +4,8 @@
 
 bool Collision_helper::collision_detector(const std::string& object1_name, const std::string& object2_name){ 
     for (std::map<std::string, collision_detection::FCLObject>::const_iterator it = fcl_objs_.begin(); it != fcl_objs_.end(); ++it){
-        ROS_INFO("object %s", it->first);
+        std::string obj = it->first;
+        ROS_INFO("object %s", obj.c_str());
     }
     /*
     auto objA = fcl_objs_.at(object1_name);
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index c58b5d27b6fce53728231d50c549ee12ddb56aae..7e779988dd17e47deb4304bda608dfb0fed5f951 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -7,6 +7,10 @@
 
 bool Mediator::check_collision( const int& robot){
     bool succ = true;
+    std::vector<int> count_v;
+    Robot* r = dynamic_cast<Robot*>(robots_[robot]);
+    count_v.resize(r->observers().size()+1);
+
     for(long unsigned int j = 0; j < objects_[robot].size(); j++){
         visualization_msgs::Marker marker;
         marker.header.frame_id = "map";
@@ -37,8 +41,14 @@ bool Mediator::check_collision( const int& robot){
             marker.color.a = 1.0;
             succ = false;
         }
+
+        robots_[robot]->workload_checker(count_v, objects_[robot][j]);
         rviz_markers_->markers.push_back(marker);
     }
+
+    for (int& i : count_v){
+        if(i == 0) {succ = false; break;}
+    }
     return succ;
 }
 
@@ -68,11 +78,11 @@ void Mediator::mediate(){
 
 
     ros::Rate loop_rate(10);
+    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, ceti1);
-    
+        build_wings(wing_config, r1);
         for (long unsigned int i = 0; i < ground_per_robot.size(); i++){
             robots_[0]->set_tf(ground_per_robot[i]);
             for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
@@ -97,10 +107,10 @@ void Mediator::mediate(){
 void Mediator::approximation(Robot* robot, int& wings){
     ROS_INFO("assigne result to first robot...");
     Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
-
+    int r1 = 1;
     for (int i = 0; i <= 7; i++){
         std::bitset<3> wing_config(i);
-        build_wings(wing_config, ceti);
+        build_wings(wing_config, r1);
         for (Abstract_robot_element* fields : robot->access_fields()) {
             ceti->set_tf(fields->world_tf());
             for ( float p = 0; p < 2*M_PI; p += M_PI/2){
@@ -115,7 +125,7 @@ void Mediator::approximation(Robot* robot, int& wings){
                 
                 if (check_collision(1)) {
                     ROS_INFO("should work");
-                    write_file(robot, wings, ceti, i);
+                    write_file(robot, ceti);
                     } else {
                         continue;
                     }
@@ -132,7 +142,7 @@ void Mediator::approximation(Robot* robot, int& wings){
 }
 
 
-void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
+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");
     double r,p,y;
     tf2::Matrix3x3 m(A->tf().getRotation());
@@ -145,6 +155,10 @@ void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
     float rot_y = A->tf().getRotation().getY();
     float rot_z = A->tf().getRotation().getZ();
     float rot_w = A->tf().getRotation().getW();
+    float size_x = A->size().getX();
+    float size_y = A->size().getY();
+    float size_z = A->size().getZ();
+
 
 
 
@@ -158,7 +172,7 @@ void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
     ss << "{ 'id' : 'table" << A->name().back() << "_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 }}, \n";
     ss << "{ 'id' : 'table" << A->name().back() << "_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 }}, \n";
     ss << "{ 'id' : 'table" << A->name().back() << "_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
-    ss << "{ 'id' : 'table" << A->name().back() << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << " , 'y': "<< std::to_string(pos_y) << " , 'z': "<< std::to_string(pos_z) << " },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << " , 'p': " << std::to_string(p) << " , 'y': " << std::to_string(y) << " } },\n";
+    ss << "{ 'id' : 'table" << A->name().back() << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << " , 'y': "<< std::to_string(pos_y) << " , 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << " , 'p': " << std::to_string(p) << " , 'y': " << std::to_string(y) << " } },\n";
     
     m.setRotation(B->tf().getRotation());
     m.getRPY(r,p,y);
@@ -170,6 +184,9 @@ void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
     rot_y = B->tf().getRotation().getY();
     rot_z = B->tf().getRotation().getZ();
     rot_w = B->tf().getRotation().getW();
+    size_x = B->size().getX();
+    size_y = B->size().getY();
+    size_z = B->size().getZ();
 
     ss << "{ 'id' : 'table" << B->name().back() << "_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 }}, \n";
     ss << "{ 'id' : 'table" << B->name().back() << "_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 }},\n";
@@ -179,7 +196,7 @@ void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
     ss << "{ 'id' : 'table" << B->name().back() << "_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 }}, \n";
     ss << "{ 'id' : 'table" << B->name().back() << "_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 }}, \n";
     ss << "{ 'id' : 'table" << B->name().back() << "_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
-    ss << "{ 'id' : 'table" << B->name().back() << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << ", 'y': "<< std::to_string(pos_y) << ", 'z': "<< std::to_string(pos_z) << " },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << ", 'p': " << std::to_string(p) << ", 'y': " << std::to_string(y) << "}},\n";
+    ss << "{ 'id' : 'table" << B->name().back() << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << ", 'y': "<< std::to_string(pos_y) << ", 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << ", 'p': " << std::to_string(p) << ", 'y': " << std::to_string(y) << "}},\n";
 
 
     for (Abstract_robot_element* ae : A->observers()){
@@ -244,42 +261,16 @@ void Mediator::write_file(Robot* A, int& a, Robot* B, int& b){
 
     o << ss.str();
     o.close();
-    /*
-    node[A->name()]["pos_x"] = A->tf().getOrigin().getX();
-    node[A->name()]["pos_y"] = A->tf().getOrigin().getY();
-    node[A->name()]["pos_z"] = A->tf().getOrigin().getZ();
-    node[A->name()]["q_x"] = A->tf().getRotation().getX();
-    node[A->name()]["q_y"] = A->tf().getRotation().getY();
-    node[A->name()]["q_z"] = A->tf().getRotation().getZ();
-    node[A->name()]["q_w"] = A->tf().getRotation().getW();
-    node[A->name()]["rpy"] = std::to_string(r) + " " + std::to_string(p) + " " + std::to_string(y);
-    node[A->name()]["wings"] = a;
-
-
-
-
-    node[B->name()]["pos_x"] = B->tf().getOrigin().getX();
-    node[B->name()]["pos_y"] = B->tf().getOrigin().getY();
-    node[B->name()]["pos_z"] = B->tf().getOrigin().getZ();
-    node[B->name()]["q_x"] = B->tf().getRotation().getX();
-    node[B->name()]["q_y"] = B->tf().getRotation().getY();
-    node[B->name()]["q_z"] = B->tf().getRotation().getZ();
-    node[B->name()]["q_w"] = B->tf().getRotation().getW();
-    node[B->name()]["rpy"] = std::to_string(r) + " " + std::to_string(p) + " " + std::to_string(y);
-    node[B->name()]["wings"] = b;
-    o << node;
-    o.close();
-    */
     
 }
 
-void Mediator::build_wings(std::bitset<3>& wing, Robot* robot){
-    
-    std::bitset<3> result = robot->observer_mask() & wing;
+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_[i].name_, wings_[i].pos_, wings_[i].size_), robot->rviz_markers()[0]);
-            robot->register_observers(moveit_right);
+            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);
         }
     }
 }
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index c0c6d9b98485571a1cac8b017f924768f061e8b4..786f5a28ec524f4da6f00f594872b4cc56d6dcd8 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -31,8 +31,8 @@ void Moveit_mediator::load_robot_description(){
 }
 
 bool Moveit_mediator::check_collision(const int& robot){
-    auto env = ps_->getCollisionEnv();
-    Collision_helper* ch = static_cast<Collision_helper*>(const_cast<collision_detection::CollisionEnv*>(env.get()));
+    auto env = ps_->getCollisionEnvNonConst();
+    Collision_helper* ch = static_cast<Collision_helper*>(env.get());
     return ch->collision_detector("object_A", "object_B"); //
 }
 
@@ -84,17 +84,96 @@ void Moveit_mediator::mediate(){
 	}
 
 	ros::waitForShutdown();
-    
 };
 
-void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){
-    std::bitset<3> result = robot->observer_mask() & wing;
+void Moveit_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]){
             moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject();
-            robot->add_coll_markers(marker);
-            Abstract_robot_element* moveit_right = new Wing_moveit_decorator(new Wing(wings_[i].name_, wings_[i].pos_, wings_[i].size_), marker);
-            robot->register_observers(moveit_right);
+            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);
+        }
+    }
+}
+
+void Moveit_mediator::rewrite_task_template(){
+    XmlRpc::XmlRpcValue task;
+    nh_.getParam("task/stages", task);
+    /*
+    task:
+  name: Pick and Place test
+  properties:
+    group: panda_arm1
+    eef: hand_1
+    hand_grasping_frame: panda_1_link8
+    ik_frame: panda_1_link8
+    hand: hand_1
+    */
+
+    nh_.setParam("/task/properties/group", "panda_arm1");
+    nh_.setParam("/task/properties/eef", "hand_1");
+    nh_.setParam("/task/properties/hand_grasping_frame", "panda_1_link8");
+    nh_.setParam("/task/properties/ik_frame", "panda_1_link8");
+    nh_.setParam("/task/properties/hand", "hand_1");
+
+    for(int i = 0; i < task.size(); i++){
+        XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = task[i].begin();
+        for(; it != task[i].end(); ++it){
         }
     }
-}
\ No newline at end of file
+
+    XmlRpc::XmlRpcValue a, b, c, d, e;
+    a["group"] = "hand_1";
+    e["joint_model_group_name"] = "hand_1";
+
+    b = task[4]["stages"];
+    b[3]["properties"] = a;
+    b[2]["set"]["allow_collisions"]["second"] = e;
+
+    c = task[6]["stages"];
+    c[2]["properties"] = a;
+    c[5]["properties"] = a;
+    c[3]["set"]["allow_collisions"]["second"] = e;
+    
+
+    task[2]["properties"] = a;
+
+    XmlRpc::XmlRpcValue connect, f, g;
+    f["panda_arm1"] = "sampling";
+    g["source"] = "PARENT";
+    connect["type"] = "Connect";
+    connect["group_planner_vector"] = f;
+    connect["propertiesConfigureInitFrom"] = g;
+    task[5] = connect;
+    task[3] = connect;
+
+    a.clear();
+    e.clear();
+    a["link"] = "panda_1_link8";
+    a["min_distance"] = 0.07;
+    a["max_distance"] = 0.2;
+    c[4]["properties"] = a;
+    b[0]["properties"] = a;
+    a["min_distance"] = 0.1;
+    a["max_distance"] = 0.2;
+    c[0]["properties"] = a;
+
+    e["object"] = "bottle";
+    e["link"] = "panda_1_link8";
+    b[4]["set"]["attach_object"] = e;
+    c[3]["set"]["detach_object"] = e;
+    c[1]["set"]["ik_frame"]["link"] = "panda_1_link8";
+    c[4]["set"]["direction"]["vector"]["header"]["frame_id"]  = "panda_1_link8";
+    b[1]["set"]["ik_frame"]["link"] = "panda_1_link8";
+    b[5]["set"]["ik_frame"]["link"] = "panda_1_link8";
+    b[0]["set"]["direction"]["vector"]["header"]["frame_id"] = "panda_1_link8";
+    task[6]["stages"] = c;
+    task[4]["stages"] = b;
+
+
+
+    nh_.setParam("task/stages", task);
+}
diff --git a/src/mtc/src/impl/robot.cpp b/src/mtc/src/impl/robot.cpp
index cb7c9db5119f541b8914f66ebb075834ffeb58d7..292517c3d229616ea0e60f922edd5b0ec000282c 100644
--- a/src/mtc/src/impl/robot.cpp
+++ b/src/mtc/src/impl/robot.cpp
@@ -35,6 +35,40 @@ float Robot::area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transfo
                     (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;
 }
 
+void Robot::workload_checker(std::vector<int>& count_vector, tf2::Transform& obj){
+    for (int i = 0; i < observers_.size(); i++){
+        Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(observers_[i]);
+        Wing* ceti = dynamic_cast<Wing*>(deco->wing());
+        tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
+        tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
+        tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
+        tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
+            
+        float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+        float 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) { 
+            count_vector[i]++; return;
+        } else {
+            continue;
+        }
+    }
+
+    
+    tf2::Transform A = tf_ * bounds_[0];
+    tf2::Transform B = tf_ * bounds_[1];
+    tf2::Transform C = tf_ * bounds_[2];
+    tf2::Transform D = tf_ * bounds_[3];
+
+    float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+    float 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) count_vector.back()++; return;
+
+    
+}
+
+
+
 bool Robot::check_single_object_collision(tf2::Transform& obj){
 
     tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
@@ -102,55 +136,44 @@ bool Robot::check_robot_collision( Robot* rob){
     float sum = area_calculation(A,rob->tf(),D) + area_calculation(D,rob->tf(),C) + area_calculation(C,rob->tf(),B) + area_calculation(rob->tf(), B, A);
     if ((std::floor(sum*100)/100.f) <= full_area) return true;
 
-    if (!rob->observers().empty()){
-        std::vector<Abstract_robot_element*>::const_iterator it = rob->observers().begin();
-        while (it != rob->observers().end()) {            
-            Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
-            sum = area_calculation(A,deco->wing()->world_tf(),D) + area_calculation(D,deco->wing()->world_tf(),C) + area_calculation(C,deco->wing()->world_tf(),B) + area_calculation(deco->wing()->world_tf(), B, A);
-            if ((std::floor(sum*100)/100.f) <= full_area) {
-                return true;
-            } else {
-                ++it;
-            }
-        }
+    for(Abstract_robot_element* are : rob->observers()) {
+        Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
+        Wing* w = dynamic_cast<Wing*>(wrd->wing());
+
+        tf2::Transform WA = w->world_tf() * w->bounds()[0];
+        tf2::Transform WB = w->world_tf() * w->bounds()[1];
+        tf2::Transform WC = w->world_tf() * w->bounds()[2];
+        tf2::Transform WD = w->world_tf() * w->bounds()[3];
+        sum = area_calculation(A,WA,D) + area_calculation(D,WA,C) + area_calculation(C,WA,B) + area_calculation(WA, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
+        sum = area_calculation(A,WB,D) + area_calculation(D,WB,C) + area_calculation(C,WB,B) + area_calculation(WB, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
+        sum = area_calculation(A,WC,D) + area_calculation(D,WC,C) + area_calculation(C,WC,B) + area_calculation(WC, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
+        sum = area_calculation(A,WD,D) + area_calculation(D,WD,C) + area_calculation(C,WD,B) + area_calculation(WD, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
     }
 
-    if (!observers_.empty()){
-        std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();            
-        while (it != observers_.end()) {
-            Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
-            Wing* ceti = dynamic_cast<Wing*>(deco->wing());
-            tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
-            tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
-            tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
-            tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
-            
-            full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
-            sum = area_calculation(A,rob->tf(),D) + area_calculation(D,rob->tf(),C) + area_calculation(C,rob->tf(),B) + area_calculation(rob->tf(), B, A);
-            if ((std::floor(sum*100)/100.f) <= full_area) return true;
-
-            if (!rob->observers().empty()){
-                std::vector<Abstract_robot_element*>::const_iterator itt = rob->observers().begin();
-                while (itt != rob->observers().end()) {            
-                    Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*itt);
-                    sum = area_calculation(A,deco->wing()->world_tf(),D) + area_calculation(D,deco->wing()->world_tf(),C) + area_calculation(C,deco->wing()->world_tf(),B) + area_calculation(deco->wing()->world_tf(), B, A);
-                    if ((std::floor(sum*100)/100.f) <= full_area) {
-                        return true;
-                    } else {
-                    ++itt;
-                    }
-                }
-            }
-        
-            if ((std::floor(sum*100)/100.f) <= full_area) {
-                return true;
-            } else {
-                ++it;
-            }
+    for (Abstract_robot_element* ar : observers_){
+        Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(ar);
+        Wing* w = dynamic_cast<Wing*>(wrd->wing());
+
+        tf2::Transform WA = w->world_tf() * w->bounds()[0];
+        tf2::Transform WB = w->world_tf() * w->bounds()[1];
+        tf2::Transform WC = w->world_tf() * w->bounds()[2];
+        tf2::Transform WD = w->world_tf() * w->bounds()[3];
+
+        full_area = area_calculation(WA,WB,WC) + area_calculation(WA,WD,WC);
+        for(Abstract_robot_element* are : rob->observers()) {
+            Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
+            Wing* w = dynamic_cast<Wing*>(wrd->wing());
+
+            tf2::Transform WA2 = w->world_tf() * w->bounds()[0];
+            tf2::Transform WB2 = w->world_tf() * w->bounds()[1];
+            tf2::Transform WC2 = w->world_tf() * w->bounds()[2];
+            tf2::Transform WD2 = w->world_tf() * w->bounds()[3];
+            sum = area_calculation(WA,WA2,WD) + area_calculation(WD,WA2,WC) + area_calculation(WC,WA2,WB) + area_calculation(WA2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
+            sum = area_calculation(WA,WB2,WD) + area_calculation(WD,WB2,WC) + area_calculation(WC,WB2,WB) + area_calculation(WB2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
+            sum = area_calculation(WA,WC2,WD) + area_calculation(WD,WC2,WC) + area_calculation(WC,WC2,WB) + area_calculation(WC2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
+            sum = area_calculation(WA,WD2,WD) + area_calculation(WD,WD2,WC) + area_calculation(WC,WD2,WB) + area_calculation(WD2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
         }
     }
-    return false;
-
 }
 
 
diff --git a/src/mtc/src/impl/wing_moveit_decorator.cpp b/src/mtc/src/impl/wing_moveit_decorator.cpp
index cb6d96d47b777b6abcd1d136a7665b223856eeaa..6943b43df32ac3ff4ac9053742f4ed3edac9df0a 100644
--- a/src/mtc/src/impl/wing_moveit_decorator.cpp
+++ b/src/mtc/src/impl/wing_moveit_decorator.cpp
@@ -34,7 +34,4 @@ void Wing_moveit_decorator::output_filter() {
     markers_->primitive_poses[0].orientation.w = world_quat.getW();
 
     markers_->operation = markers_->ADD;
-        
-    ROS_INFO("%f %f %f %f", world_quat.getX(), world_quat.getY(), world_quat.getZ(), world_quat.getW());
-
 }
\ No newline at end of file