From 15ab45aea97cb8051d50dfb6958db1d6b6b03749 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Mon, 14 Nov 2022 12:10:54 +0100
Subject: [PATCH] some fixes

---
 .../profiles/default/devel_collisions.txt     |  2 +-
 .../robots/ceti_double.urdf.xacro             |  2 +-
 src/mtc/README.md                             |  7 ------
 src/mtc/launch/base_routine.launch            |  8 +++---
 src/mtc/launch/cell_routine.launch            |  5 ++--
 src/mtc/launch/mtc2taskspace.launch           |  6 ++++-
 src/mtc/results/dummy.yaml                    | 25 -------------------
 .../{1002613186.yaml => -1113960400.yaml}     |  0
 .../{945520390.yaml => -1130060311.yaml}      |  0
 .../{934553240.yaml => -1141587806.yaml}      |  0
 .../{957818188.yaml => -1164134400.yaml}      |  0
 .../{990058227.yaml => -1177867363.yaml}      |  0
 .../{978772872.yaml => -1190597504.yaml}      |  0
 .../{841127331.yaml => -1241254685.yaml}      |  0
 .../{890568488.yaml => -1287858113.yaml}      |  0
 .../{409146028.yaml => -1725746253.yaml}      |  0
 .../{396809119.yaml => -1739448485.yaml}      |  0
 .../{385732106.yaml => -1751589910.yaml}      |  0
 .../{453123235.yaml => -1775227539.yaml}      |  0
 .../{440798129.yaml => -1789367903.yaml}      |  0
 .../{429872256.yaml => -1801493263.yaml}      |  0
 .../{512151887.yaml => -2019620283.yaml}      |  0
 .../{568064102.yaml => -2070819246.yaml}      |  0
 .../{-953422313.yaml => 1086409496.yaml}      |  0
 .../{-551819248.yaml => 1086505991.yaml}      |  0
 .../{-540664156.yaml => 1097760815.yaml}      |  0
 .../{-997664293.yaml => 1132989736.yaml}      |  0
 .../{-598701477.yaml => 1138917448.yaml}      |  0
 .../{-587407747.yaml => 1150205447.yaml}      |  0
 .../{-239267840.yaml => 1420108742.yaml}      |  0
 .../{-288597854.yaml => 1467312061.yaml}      |  0
 .../dummy/{-26465509.yaml => 1634357786.yaml} |  0
 .../dummy/{-73418491.yaml => 1681783699.yaml} |  0
 .../dummy/{106540234.yaml => 1830642082.yaml} |  0
 .../dummy/{161620969.yaml => 1878182050.yaml} |  0
 .../{-118381120.yaml => 1927303313.yaml}      |  0
 .../{-107328901.yaml => 1938979366.yaml}      |  0
 .../dummy/{241047948.yaml => 1951197037.yaml} |  0
 .../{-139322185.yaml => 1952346695.yaml}      |  0
 .../{-163087766.yaml => 1974030854.yaml}      |  0
 .../{-151902566.yaml => 1985613525.yaml}      |  0
 .../dummy/{-94891628.yaml => 1998093904.yaml} |  0
 .../dummy/{288860765.yaml => 1998619737.yaml} |  0
 .../{-1299660620.yaml => 287216712.yaml}      |  0
 .../{-1287907953.yaml => 299277607.yaml}      |  0
 .../{-1274604634.yaml => 312857611.yaml}      |  0
 .../{-1346543685.yaml => 335131185.yaml}      |  0
 .../{-1334648679.yaml => 347069224.yaml}      |  0
 .../{-1321395857.yaml => 360590070.yaml}      |  0
 .../{-1191626777.yaml => 397572609.yaml}      |  0
 .../{-1179960295.yaml => 409649008.yaml}      |  0
 .../{-1166700698.yaml => 423450698.yaml}      |  0
 .../{-1238289141.yaml => 445682784.yaml}      |  0
 .../{-1226601008.yaml => 461533561.yaml}      |  0
 .../{-1213390021.yaml => 477108873.yaml}      |  0
 .../{-1083227167.yaml => 518554172.yaml}      |  0
 .../{-1071524157.yaml => 530725211.yaml}      |  0
 .../{-1058266167.yaml => 544173349.yaml}      |  0
 .../{-1130182242.yaml => 571021364.yaml}      |  0
 .../{-1118397307.yaml => 582894029.yaml}      |  0
 .../{-1105096712.yaml => 597217384.yaml}      |  0
 .../{-1280463185.yaml => 759067436.yaml}      |  0
 .../{-1269289091.yaml => 770754525.yaml}      |  0
 .../{-1256829190.yaml => 784016529.yaml}      |  0
 .../{-1325476019.yaml => 807557424.yaml}      |  0
 .../{-1313991264.yaml => 827720657.yaml}      |  0
 .../{-1301462009.yaml => 840878174.yaml}      |  0
 .../{-1164235259.yaml => 886296696.yaml}      |  0
 .../{-1213000891.yaml => 931879073.yaml}      |  0
 src/mtc/src/impl/mediator.cpp                 |  8 +++---
 src/mtc/src/mtc2taskspace.cpp                 | 23 ++++++-----------
 71 files changed, 26 insertions(+), 60 deletions(-)
 delete mode 100644 src/mtc/results/dummy.yaml
 rename src/mtc/results/dummy/{1002613186.yaml => -1113960400.yaml} (100%)
 rename src/mtc/results/dummy/{945520390.yaml => -1130060311.yaml} (100%)
 rename src/mtc/results/dummy/{934553240.yaml => -1141587806.yaml} (100%)
 rename src/mtc/results/dummy/{957818188.yaml => -1164134400.yaml} (100%)
 rename src/mtc/results/dummy/{990058227.yaml => -1177867363.yaml} (100%)
 rename src/mtc/results/dummy/{978772872.yaml => -1190597504.yaml} (100%)
 rename src/mtc/results/dummy/{841127331.yaml => -1241254685.yaml} (100%)
 rename src/mtc/results/dummy/{890568488.yaml => -1287858113.yaml} (100%)
 rename src/mtc/results/dummy/{409146028.yaml => -1725746253.yaml} (100%)
 rename src/mtc/results/dummy/{396809119.yaml => -1739448485.yaml} (100%)
 rename src/mtc/results/dummy/{385732106.yaml => -1751589910.yaml} (100%)
 rename src/mtc/results/dummy/{453123235.yaml => -1775227539.yaml} (100%)
 rename src/mtc/results/dummy/{440798129.yaml => -1789367903.yaml} (100%)
 rename src/mtc/results/dummy/{429872256.yaml => -1801493263.yaml} (100%)
 rename src/mtc/results/dummy/{512151887.yaml => -2019620283.yaml} (100%)
 rename src/mtc/results/dummy/{568064102.yaml => -2070819246.yaml} (100%)
 rename src/mtc/results/dummy/{-953422313.yaml => 1086409496.yaml} (100%)
 rename src/mtc/results/dummy/{-551819248.yaml => 1086505991.yaml} (100%)
 rename src/mtc/results/dummy/{-540664156.yaml => 1097760815.yaml} (100%)
 rename src/mtc/results/dummy/{-997664293.yaml => 1132989736.yaml} (100%)
 rename src/mtc/results/dummy/{-598701477.yaml => 1138917448.yaml} (100%)
 rename src/mtc/results/dummy/{-587407747.yaml => 1150205447.yaml} (100%)
 rename src/mtc/results/dummy/{-239267840.yaml => 1420108742.yaml} (100%)
 rename src/mtc/results/dummy/{-288597854.yaml => 1467312061.yaml} (100%)
 rename src/mtc/results/dummy/{-26465509.yaml => 1634357786.yaml} (100%)
 rename src/mtc/results/dummy/{-73418491.yaml => 1681783699.yaml} (100%)
 rename src/mtc/results/dummy/{106540234.yaml => 1830642082.yaml} (100%)
 rename src/mtc/results/dummy/{161620969.yaml => 1878182050.yaml} (100%)
 rename src/mtc/results/dummy/{-118381120.yaml => 1927303313.yaml} (100%)
 rename src/mtc/results/dummy/{-107328901.yaml => 1938979366.yaml} (100%)
 rename src/mtc/results/dummy/{241047948.yaml => 1951197037.yaml} (100%)
 rename src/mtc/results/dummy/{-139322185.yaml => 1952346695.yaml} (100%)
 rename src/mtc/results/dummy/{-163087766.yaml => 1974030854.yaml} (100%)
 rename src/mtc/results/dummy/{-151902566.yaml => 1985613525.yaml} (100%)
 rename src/mtc/results/dummy/{-94891628.yaml => 1998093904.yaml} (100%)
 rename src/mtc/results/dummy/{288860765.yaml => 1998619737.yaml} (100%)
 rename src/mtc/results/dummy/{-1299660620.yaml => 287216712.yaml} (100%)
 rename src/mtc/results/dummy/{-1287907953.yaml => 299277607.yaml} (100%)
 rename src/mtc/results/dummy/{-1274604634.yaml => 312857611.yaml} (100%)
 rename src/mtc/results/dummy/{-1346543685.yaml => 335131185.yaml} (100%)
 rename src/mtc/results/dummy/{-1334648679.yaml => 347069224.yaml} (100%)
 rename src/mtc/results/dummy/{-1321395857.yaml => 360590070.yaml} (100%)
 rename src/mtc/results/dummy/{-1191626777.yaml => 397572609.yaml} (100%)
 rename src/mtc/results/dummy/{-1179960295.yaml => 409649008.yaml} (100%)
 rename src/mtc/results/dummy/{-1166700698.yaml => 423450698.yaml} (100%)
 rename src/mtc/results/dummy/{-1238289141.yaml => 445682784.yaml} (100%)
 rename src/mtc/results/dummy/{-1226601008.yaml => 461533561.yaml} (100%)
 rename src/mtc/results/dummy/{-1213390021.yaml => 477108873.yaml} (100%)
 rename src/mtc/results/dummy/{-1083227167.yaml => 518554172.yaml} (100%)
 rename src/mtc/results/dummy/{-1071524157.yaml => 530725211.yaml} (100%)
 rename src/mtc/results/dummy/{-1058266167.yaml => 544173349.yaml} (100%)
 rename src/mtc/results/dummy/{-1130182242.yaml => 571021364.yaml} (100%)
 rename src/mtc/results/dummy/{-1118397307.yaml => 582894029.yaml} (100%)
 rename src/mtc/results/dummy/{-1105096712.yaml => 597217384.yaml} (100%)
 rename src/mtc/results/dummy/{-1280463185.yaml => 759067436.yaml} (100%)
 rename src/mtc/results/dummy/{-1269289091.yaml => 770754525.yaml} (100%)
 rename src/mtc/results/dummy/{-1256829190.yaml => 784016529.yaml} (100%)
 rename src/mtc/results/dummy/{-1325476019.yaml => 807557424.yaml} (100%)
 rename src/mtc/results/dummy/{-1313991264.yaml => 827720657.yaml} (100%)
 rename src/mtc/results/dummy/{-1301462009.yaml => 840878174.yaml} (100%)
 rename src/mtc/results/dummy/{-1164235259.yaml => 886296696.yaml} (100%)
 rename src/mtc/results/dummy/{-1213000891.yaml => 931879073.yaml} (100%)

diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 461c7ae..6e11520 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 23953
+/home/matteo/reachability/devel/./cmake.lock 24055
 /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 50104db..9619220 100644
--- a/src/franka_description/robots/ceti_double.urdf.xacro
+++ b/src/franka_description/robots/ceti_double.urdf.xacro
@@ -13,7 +13,7 @@
   <link name="world" />
 
 
-  <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-151902566.yaml" />
+  <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-1801493263.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/README.md b/src/mtc/README.md
index 5b726e3..f2997d8 100644
--- a/src/mtc/README.md
+++ b/src/mtc/README.md
@@ -35,10 +35,3 @@ State: in progress. Every executable is working with some explained downfalls th
 :black_square_button: include ferdinants work (main focus)
 
 Im working non stop, so don't forget to pull.
-
-
-
-:ballot_box_with_check:
-
-
-:ballot_box_with_check:
diff --git a/src/mtc/launch/base_routine.launch b/src/mtc/launch/base_routine.launch
index f13e2e6..7522368 100644
--- a/src/mtc/launch/base_routine.launch
+++ b/src/mtc/launch/base_routine.launch
@@ -1,11 +1,11 @@
 <launch>
-    <arg name="map" default="dummy" />
-    <arg name="description" default="dummy" />
+    <arg name="map" default="dummy.yaml" />
+    <arg name="description" default="dummy.yaml" />
     <arg name="resource" default="dummy" />
 
 
-    <rosparam command="load" file="$(find mtc)/maps/$(arg map).yaml"/>
-    <rosparam command="load" file="$(find mtc)/descriptions/$(arg description).yaml"/>
+    <rosparam command="load" file="$(find mtc)/maps/$(arg map)"/>
+    <rosparam command="load" file="$(find mtc)/descriptions/$(arg description)"/>
     <rosparam command="load" file="$(find mtc)/resources/$(arg resource).yaml"/>
 
     <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index 82f43ba..f5c4b99 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -1,10 +1,11 @@
 <launch>    
+    <arg name="result" default="dummy.yaml" />
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
 
     <!-- this is to change-->
-    <rosparam command="load" file="$(find mtc)/results/dummy/-151902566.yaml"/>
+    <rosparam command="load" file="$(find mtc)/results/dummy/-1801493263.yaml"/>
+
 
-    
     <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
     <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
diff --git a/src/mtc/launch/mtc2taskspace.launch b/src/mtc/launch/mtc2taskspace.launch
index c165f18..d969135 100644
--- a/src/mtc/launch/mtc2taskspace.launch
+++ b/src/mtc/launch/mtc2taskspace.launch
@@ -1,4 +1,8 @@
 <launch>
-    <rosparam command="load" file="$(find mtc)/resources/dummy.yaml"/>
+    <arg name="resource" default="dummy.yaml" />
+
+    <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
+    
+    <rosparam command="load" file="$(find mtc)/resources/$(arg resource)"/>
     <node pkg="mtc" type="mtc2taskspace" name="mtc2taskspace" output="screen" required="true"/>
 </launch>
\ No newline at end of file
diff --git a/src/mtc/results/dummy.yaml b/src/mtc/results/dummy.yaml
deleted file mode 100644
index d93ffb7..0000000
--- a/src/mtc/results/dummy.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.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
-{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
-{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50  },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, 
-{ 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
-{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'table2_right_panel',  'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, 
-{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
-{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } 
-]}
diff --git a/src/mtc/results/dummy/1002613186.yaml b/src/mtc/results/dummy/-1113960400.yaml
similarity index 100%
rename from src/mtc/results/dummy/1002613186.yaml
rename to src/mtc/results/dummy/-1113960400.yaml
diff --git a/src/mtc/results/dummy/945520390.yaml b/src/mtc/results/dummy/-1130060311.yaml
similarity index 100%
rename from src/mtc/results/dummy/945520390.yaml
rename to src/mtc/results/dummy/-1130060311.yaml
diff --git a/src/mtc/results/dummy/934553240.yaml b/src/mtc/results/dummy/-1141587806.yaml
similarity index 100%
rename from src/mtc/results/dummy/934553240.yaml
rename to src/mtc/results/dummy/-1141587806.yaml
diff --git a/src/mtc/results/dummy/957818188.yaml b/src/mtc/results/dummy/-1164134400.yaml
similarity index 100%
rename from src/mtc/results/dummy/957818188.yaml
rename to src/mtc/results/dummy/-1164134400.yaml
diff --git a/src/mtc/results/dummy/990058227.yaml b/src/mtc/results/dummy/-1177867363.yaml
similarity index 100%
rename from src/mtc/results/dummy/990058227.yaml
rename to src/mtc/results/dummy/-1177867363.yaml
diff --git a/src/mtc/results/dummy/978772872.yaml b/src/mtc/results/dummy/-1190597504.yaml
similarity index 100%
rename from src/mtc/results/dummy/978772872.yaml
rename to src/mtc/results/dummy/-1190597504.yaml
diff --git a/src/mtc/results/dummy/841127331.yaml b/src/mtc/results/dummy/-1241254685.yaml
similarity index 100%
rename from src/mtc/results/dummy/841127331.yaml
rename to src/mtc/results/dummy/-1241254685.yaml
diff --git a/src/mtc/results/dummy/890568488.yaml b/src/mtc/results/dummy/-1287858113.yaml
similarity index 100%
rename from src/mtc/results/dummy/890568488.yaml
rename to src/mtc/results/dummy/-1287858113.yaml
diff --git a/src/mtc/results/dummy/409146028.yaml b/src/mtc/results/dummy/-1725746253.yaml
similarity index 100%
rename from src/mtc/results/dummy/409146028.yaml
rename to src/mtc/results/dummy/-1725746253.yaml
diff --git a/src/mtc/results/dummy/396809119.yaml b/src/mtc/results/dummy/-1739448485.yaml
similarity index 100%
rename from src/mtc/results/dummy/396809119.yaml
rename to src/mtc/results/dummy/-1739448485.yaml
diff --git a/src/mtc/results/dummy/385732106.yaml b/src/mtc/results/dummy/-1751589910.yaml
similarity index 100%
rename from src/mtc/results/dummy/385732106.yaml
rename to src/mtc/results/dummy/-1751589910.yaml
diff --git a/src/mtc/results/dummy/453123235.yaml b/src/mtc/results/dummy/-1775227539.yaml
similarity index 100%
rename from src/mtc/results/dummy/453123235.yaml
rename to src/mtc/results/dummy/-1775227539.yaml
diff --git a/src/mtc/results/dummy/440798129.yaml b/src/mtc/results/dummy/-1789367903.yaml
similarity index 100%
rename from src/mtc/results/dummy/440798129.yaml
rename to src/mtc/results/dummy/-1789367903.yaml
diff --git a/src/mtc/results/dummy/429872256.yaml b/src/mtc/results/dummy/-1801493263.yaml
similarity index 100%
rename from src/mtc/results/dummy/429872256.yaml
rename to src/mtc/results/dummy/-1801493263.yaml
diff --git a/src/mtc/results/dummy/512151887.yaml b/src/mtc/results/dummy/-2019620283.yaml
similarity index 100%
rename from src/mtc/results/dummy/512151887.yaml
rename to src/mtc/results/dummy/-2019620283.yaml
diff --git a/src/mtc/results/dummy/568064102.yaml b/src/mtc/results/dummy/-2070819246.yaml
similarity index 100%
rename from src/mtc/results/dummy/568064102.yaml
rename to src/mtc/results/dummy/-2070819246.yaml
diff --git a/src/mtc/results/dummy/-953422313.yaml b/src/mtc/results/dummy/1086409496.yaml
similarity index 100%
rename from src/mtc/results/dummy/-953422313.yaml
rename to src/mtc/results/dummy/1086409496.yaml
diff --git a/src/mtc/results/dummy/-551819248.yaml b/src/mtc/results/dummy/1086505991.yaml
similarity index 100%
rename from src/mtc/results/dummy/-551819248.yaml
rename to src/mtc/results/dummy/1086505991.yaml
diff --git a/src/mtc/results/dummy/-540664156.yaml b/src/mtc/results/dummy/1097760815.yaml
similarity index 100%
rename from src/mtc/results/dummy/-540664156.yaml
rename to src/mtc/results/dummy/1097760815.yaml
diff --git a/src/mtc/results/dummy/-997664293.yaml b/src/mtc/results/dummy/1132989736.yaml
similarity index 100%
rename from src/mtc/results/dummy/-997664293.yaml
rename to src/mtc/results/dummy/1132989736.yaml
diff --git a/src/mtc/results/dummy/-598701477.yaml b/src/mtc/results/dummy/1138917448.yaml
similarity index 100%
rename from src/mtc/results/dummy/-598701477.yaml
rename to src/mtc/results/dummy/1138917448.yaml
diff --git a/src/mtc/results/dummy/-587407747.yaml b/src/mtc/results/dummy/1150205447.yaml
similarity index 100%
rename from src/mtc/results/dummy/-587407747.yaml
rename to src/mtc/results/dummy/1150205447.yaml
diff --git a/src/mtc/results/dummy/-239267840.yaml b/src/mtc/results/dummy/1420108742.yaml
similarity index 100%
rename from src/mtc/results/dummy/-239267840.yaml
rename to src/mtc/results/dummy/1420108742.yaml
diff --git a/src/mtc/results/dummy/-288597854.yaml b/src/mtc/results/dummy/1467312061.yaml
similarity index 100%
rename from src/mtc/results/dummy/-288597854.yaml
rename to src/mtc/results/dummy/1467312061.yaml
diff --git a/src/mtc/results/dummy/-26465509.yaml b/src/mtc/results/dummy/1634357786.yaml
similarity index 100%
rename from src/mtc/results/dummy/-26465509.yaml
rename to src/mtc/results/dummy/1634357786.yaml
diff --git a/src/mtc/results/dummy/-73418491.yaml b/src/mtc/results/dummy/1681783699.yaml
similarity index 100%
rename from src/mtc/results/dummy/-73418491.yaml
rename to src/mtc/results/dummy/1681783699.yaml
diff --git a/src/mtc/results/dummy/106540234.yaml b/src/mtc/results/dummy/1830642082.yaml
similarity index 100%
rename from src/mtc/results/dummy/106540234.yaml
rename to src/mtc/results/dummy/1830642082.yaml
diff --git a/src/mtc/results/dummy/161620969.yaml b/src/mtc/results/dummy/1878182050.yaml
similarity index 100%
rename from src/mtc/results/dummy/161620969.yaml
rename to src/mtc/results/dummy/1878182050.yaml
diff --git a/src/mtc/results/dummy/-118381120.yaml b/src/mtc/results/dummy/1927303313.yaml
similarity index 100%
rename from src/mtc/results/dummy/-118381120.yaml
rename to src/mtc/results/dummy/1927303313.yaml
diff --git a/src/mtc/results/dummy/-107328901.yaml b/src/mtc/results/dummy/1938979366.yaml
similarity index 100%
rename from src/mtc/results/dummy/-107328901.yaml
rename to src/mtc/results/dummy/1938979366.yaml
diff --git a/src/mtc/results/dummy/241047948.yaml b/src/mtc/results/dummy/1951197037.yaml
similarity index 100%
rename from src/mtc/results/dummy/241047948.yaml
rename to src/mtc/results/dummy/1951197037.yaml
diff --git a/src/mtc/results/dummy/-139322185.yaml b/src/mtc/results/dummy/1952346695.yaml
similarity index 100%
rename from src/mtc/results/dummy/-139322185.yaml
rename to src/mtc/results/dummy/1952346695.yaml
diff --git a/src/mtc/results/dummy/-163087766.yaml b/src/mtc/results/dummy/1974030854.yaml
similarity index 100%
rename from src/mtc/results/dummy/-163087766.yaml
rename to src/mtc/results/dummy/1974030854.yaml
diff --git a/src/mtc/results/dummy/-151902566.yaml b/src/mtc/results/dummy/1985613525.yaml
similarity index 100%
rename from src/mtc/results/dummy/-151902566.yaml
rename to src/mtc/results/dummy/1985613525.yaml
diff --git a/src/mtc/results/dummy/-94891628.yaml b/src/mtc/results/dummy/1998093904.yaml
similarity index 100%
rename from src/mtc/results/dummy/-94891628.yaml
rename to src/mtc/results/dummy/1998093904.yaml
diff --git a/src/mtc/results/dummy/288860765.yaml b/src/mtc/results/dummy/1998619737.yaml
similarity index 100%
rename from src/mtc/results/dummy/288860765.yaml
rename to src/mtc/results/dummy/1998619737.yaml
diff --git a/src/mtc/results/dummy/-1299660620.yaml b/src/mtc/results/dummy/287216712.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1299660620.yaml
rename to src/mtc/results/dummy/287216712.yaml
diff --git a/src/mtc/results/dummy/-1287907953.yaml b/src/mtc/results/dummy/299277607.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1287907953.yaml
rename to src/mtc/results/dummy/299277607.yaml
diff --git a/src/mtc/results/dummy/-1274604634.yaml b/src/mtc/results/dummy/312857611.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1274604634.yaml
rename to src/mtc/results/dummy/312857611.yaml
diff --git a/src/mtc/results/dummy/-1346543685.yaml b/src/mtc/results/dummy/335131185.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1346543685.yaml
rename to src/mtc/results/dummy/335131185.yaml
diff --git a/src/mtc/results/dummy/-1334648679.yaml b/src/mtc/results/dummy/347069224.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1334648679.yaml
rename to src/mtc/results/dummy/347069224.yaml
diff --git a/src/mtc/results/dummy/-1321395857.yaml b/src/mtc/results/dummy/360590070.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1321395857.yaml
rename to src/mtc/results/dummy/360590070.yaml
diff --git a/src/mtc/results/dummy/-1191626777.yaml b/src/mtc/results/dummy/397572609.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1191626777.yaml
rename to src/mtc/results/dummy/397572609.yaml
diff --git a/src/mtc/results/dummy/-1179960295.yaml b/src/mtc/results/dummy/409649008.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1179960295.yaml
rename to src/mtc/results/dummy/409649008.yaml
diff --git a/src/mtc/results/dummy/-1166700698.yaml b/src/mtc/results/dummy/423450698.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1166700698.yaml
rename to src/mtc/results/dummy/423450698.yaml
diff --git a/src/mtc/results/dummy/-1238289141.yaml b/src/mtc/results/dummy/445682784.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1238289141.yaml
rename to src/mtc/results/dummy/445682784.yaml
diff --git a/src/mtc/results/dummy/-1226601008.yaml b/src/mtc/results/dummy/461533561.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1226601008.yaml
rename to src/mtc/results/dummy/461533561.yaml
diff --git a/src/mtc/results/dummy/-1213390021.yaml b/src/mtc/results/dummy/477108873.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1213390021.yaml
rename to src/mtc/results/dummy/477108873.yaml
diff --git a/src/mtc/results/dummy/-1083227167.yaml b/src/mtc/results/dummy/518554172.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1083227167.yaml
rename to src/mtc/results/dummy/518554172.yaml
diff --git a/src/mtc/results/dummy/-1071524157.yaml b/src/mtc/results/dummy/530725211.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1071524157.yaml
rename to src/mtc/results/dummy/530725211.yaml
diff --git a/src/mtc/results/dummy/-1058266167.yaml b/src/mtc/results/dummy/544173349.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1058266167.yaml
rename to src/mtc/results/dummy/544173349.yaml
diff --git a/src/mtc/results/dummy/-1130182242.yaml b/src/mtc/results/dummy/571021364.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1130182242.yaml
rename to src/mtc/results/dummy/571021364.yaml
diff --git a/src/mtc/results/dummy/-1118397307.yaml b/src/mtc/results/dummy/582894029.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1118397307.yaml
rename to src/mtc/results/dummy/582894029.yaml
diff --git a/src/mtc/results/dummy/-1105096712.yaml b/src/mtc/results/dummy/597217384.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1105096712.yaml
rename to src/mtc/results/dummy/597217384.yaml
diff --git a/src/mtc/results/dummy/-1280463185.yaml b/src/mtc/results/dummy/759067436.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1280463185.yaml
rename to src/mtc/results/dummy/759067436.yaml
diff --git a/src/mtc/results/dummy/-1269289091.yaml b/src/mtc/results/dummy/770754525.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1269289091.yaml
rename to src/mtc/results/dummy/770754525.yaml
diff --git a/src/mtc/results/dummy/-1256829190.yaml b/src/mtc/results/dummy/784016529.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1256829190.yaml
rename to src/mtc/results/dummy/784016529.yaml
diff --git a/src/mtc/results/dummy/-1325476019.yaml b/src/mtc/results/dummy/807557424.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1325476019.yaml
rename to src/mtc/results/dummy/807557424.yaml
diff --git a/src/mtc/results/dummy/-1313991264.yaml b/src/mtc/results/dummy/827720657.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1313991264.yaml
rename to src/mtc/results/dummy/827720657.yaml
diff --git a/src/mtc/results/dummy/-1301462009.yaml b/src/mtc/results/dummy/840878174.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1301462009.yaml
rename to src/mtc/results/dummy/840878174.yaml
diff --git a/src/mtc/results/dummy/-1164235259.yaml b/src/mtc/results/dummy/886296696.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1164235259.yaml
rename to src/mtc/results/dummy/886296696.yaml
diff --git a/src/mtc/results/dummy/-1213000891.yaml b/src/mtc/results/dummy/931879073.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1213000891.yaml
rename to src/mtc/results/dummy/931879073.yaml
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index 0730159..a530e8a 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -301,7 +301,7 @@ void Mediator::write_file(Robot* A, Robot* B){
 
     float pos_x = A->tf().getOrigin().getX();
     float pos_y = A->tf().getOrigin().getY();
-    float pos_z = A->tf().getOrigin().getZ() *2 - size_z/2;
+    float pos_z = A->tf().getOrigin().getZ() *2 ;
     float rot_x = A->tf().getRotation().getX();
     float rot_y = A->tf().getRotation().getY();
     float rot_z = A->tf().getRotation().getZ();
@@ -332,7 +332,7 @@ void Mediator::write_file(Robot* A, Robot* B){
 
     pos_x = B->tf().getOrigin().getX();
     pos_y = B->tf().getOrigin().getY();
-    pos_z = B->tf().getOrigin().getZ() *2 - size_z/2;
+    pos_z = B->tf().getOrigin().getZ() *2 ;
     rot_x = B->tf().getRotation().getX();
     rot_y = B->tf().getRotation().getY();
     rot_z = B->tf().getRotation().getZ();
@@ -364,7 +364,7 @@ void Mediator::write_file(Robot* A, Robot* B){
         float width = w->size().getY();
         float height = w->size().getZ();
 
-        ss << "{ 'id': '" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
+        ss << "{ 'id': '" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
     }
 
     
@@ -383,7 +383,7 @@ void Mediator::write_file(Robot* A, Robot* B){
         float width = w->size().getY();
         float height = w->size().getZ();
 
-        ss << "{ 'id': '" << w->name() << "',  'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
+        ss << "{ 'id': '" << w->name() << "',  'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
     }
     tf2::Transform tf_arm =  A->tf() * A->root_tf();
     float arm_x = tf_arm.getOrigin().getX();
diff --git a/src/mtc/src/mtc2taskspace.cpp b/src/mtc/src/mtc2taskspace.cpp
index db30d16..312acab 100644
--- a/src/mtc/src/mtc2taskspace.cpp
+++ b/src/mtc/src/mtc2taskspace.cpp
@@ -6,7 +6,9 @@ int main(int argc, char **argv){
     ros::init(argc, argv, "mtc2taskspace");
     ros::NodeHandle n;
 
-    std::string path;
+    std::string filename;
+    n.getParam("/resource_name", filename);
+
     XmlRpc::XmlRpcValue resources;
     n.getParam("/objects", resources);
 
@@ -60,34 +62,25 @@ int main(int argc, char **argv){
 
     // lets make some result data
     ROS_INFO("writing file...");
-    std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy2.yaml");
+    std::ofstream o(ros::package::getPath("mtc") + "/descriptions/" + filename);
     YAML::Node node;
 
     ROS_INFO("writing items for robot1...");
     for(tf2::Vector3& vec : robot_1){
-      node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
+      node["task"]["groups"]["panda_arm1"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
     }
 
     ROS_INFO("writing items for robot2...");
     for(tf2::Vector3& vec : robot_2){
-      node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
+      node["task"]["groups"]["panda_arm2"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
     }
 
     ROS_INFO("writing intersection...");
     for(tf2::Vector3& vec : robot_middle){
-      node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
-      node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+      node["task"]["groups"]["panda_arm1"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+      node["task"]["groups"]["panda_arm2"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
     }
 
-    ROS_INFO("writing intersection...");
-
-    for(tf2::Vector3& vec : robot_middle){
-      node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
-      node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
-    }
-
-    node.SetStyle(YAML::EmitterStyle::Flow);
-
     o << node;
     return 0;
 }
\ No newline at end of file
-- 
GitLab