diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 461c7aeaf8fcd4f23aa7fef815a45f0807915613..6e11520998536a490c5e8ee512c497b4502c2550 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 50104dbbc69c278ad2bd91824190be7a469dcc3d..96192201a6b1a2b2ae377c7c7f4ca65a187955c8 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 5b726e303cfac763fcb19a00795fac5ffe696969..f2997d8486a06d7a8ba27dcc1a9c92410869873a 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 f13e2e65e2aee76f16eeed237a626bbec2bd319c..75223686d744b751f9a0adc3bc57604214d62abb 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 82f43ba274076109efa54c6cc90d5076d9a194b7..f5c4b997ded04d1527be5e281a4c92c8315e0b2f 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 c165f181ab10ac9af290c6f0d27a68518a921616..d9691353d28011487e375a1e4cd407569f3753ed 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 d93ffb798fc40b538a830d6d7664f60b0be5319c..0000000000000000000000000000000000000000
--- 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 0730159559b2642a1a3e4d92dcced78ce8b7669d..a530e8ac37415b6b4b0e3a67e478b5bf686c4cfa 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 db30d166f0f06c865db81ac8ed019ff27b4c34fb..312acab8fbaf1c6d1fac77ddf19f93075859bf39 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