diff --git a/cobot1_v1.xml b/cobot1_v1.xml
index 2ecf333e5e22e658248cb33d64cbf3f63d7e22fa..351ff748caa9013cde738ccae394b530d4d4d5b7 100644
--- a/cobot1_v1.xml
+++ b/cobot1_v1.xml
@@ -16,7 +16,7 @@ chan move_to_start_pos;</declaration>
 	<template>
 		<name x="5" y="5">Cobot</name>
 		<declaration>// Place local declarations here.</declaration>
-		<location id="id0" x="-510" y="-365">
+		<location id="id0" x="-510" y="-280">
 		</location>
 		<location id="id1" x="-340" y="-187">
 		</location>
@@ -69,7 +69,14 @@ chan move_to_start_pos;</declaration>
 		<location id="id21" x="272" y="212">
 			<committed/>
 		</location>
-		<init ref="id0"/>
+		<location id="id22" x="-510" y="-408">
+		</location>
+		<init ref="id22"/>
+		<transition>
+			<source ref="id22"/>
+			<target ref="id0"/>
+			<label kind="synchronisation" x="-654" y="-374">move_to_start_pos?</label>
+		</transition>
 		<transition>
 			<source ref="id7"/>
 			<target ref="id15"/>
@@ -261,98 +268,98 @@ chan move_to_start_pos;</declaration>
 		<transition>
 			<source ref="id0"/>
 			<target ref="id9"/>
-			<label kind="select" x="-629" y="-340">i : int[5,10]</label>
-			<label kind="synchronisation" x="-637" y="-323">pressure_signal?</label>
-			<label kind="assignment" x="-731" y="-306">init_retry = i, retry = init_retry</label>
+			<label kind="select" x="-629" y="-289">i : int[5,10]</label>
+			<label kind="synchronisation" x="-646" y="-272">pressure_signal?</label>
+			<label kind="assignment" x="-731" y="-255">init_retry = i, retry = init_retry</label>
 		</transition>
 	</template>
 	<template>
 		<name>channels</name>
-		<location id="id22" x="-561" y="-144">
+		<location id="id23" x="-561" y="-144">
 			<committed/>
 		</location>
-		<init ref="id22"/>
+		<init ref="id23"/>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-697" y="-408">move_to_start_pos!</label>
 			<nail x="-680" y="-391"/>
 			<nail x="-595" y="-391"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-892" y="-382">glass_place_target!</label>
 			<nail x="-561" y="-153"/>
 			<nail x="-807" y="-340"/>
 			<nail x="-748" y="-365"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-977" y="-297">glass_place_start!</label>
 			<nail x="-867" y="-255"/>
 			<nail x="-841" y="-297"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-960" y="-204">glass_pickup!</label>
 			<nail x="-867" y="-187"/>
 			<nail x="-858" y="-221"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-943" y="-102">bottle_place!</label>
 			<nail x="-850" y="-93"/>
 			<nail x="-850" y="-119"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-884" y="0">bottle_pickup!</label>
 			<nail x="-782" y="-8"/>
 			<nail x="-807" y="-34"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-739" y="85">stop_pour!</label>
 			<nail x="-671" y="59"/>
 			<nail x="-722" y="25"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-578" y="76">start_pour!</label>
 			<nail x="-535" y="59"/>
 			<nail x="-595" y="68"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-425" y="-17">move_to_glass_end!</label>
 			<nail x="-433" y="0"/>
 			<nail x="-493" y="42"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-340" y="-93">move_to_glass_start!</label>
 			<nail x="-340" y="-110"/>
 			<nail x="-340" y="-51"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-340" y="-221">move_to_bottle_pos!</label>
 			<nail x="-340" y="-246"/>
 			<nail x="-331" y="-187"/>
 		</transition>
 		<transition>
-			<source ref="id22"/>
-			<target ref="id22"/>
+			<source ref="id23"/>
+			<target ref="id23"/>
 			<label kind="synchronisation" x="-425" y="-374">pressure_signal!</label>
 			<nail x="-459" y="-382"/>
 			<nail x="-391" y="-306"/>
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index dd67bae8c89b2bc3c76cc121c37abef23a9c02b5..fdedc99f1fe1ebefad52e8066849fbfcaba66d5f 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -445,7 +445,7 @@ bool compare_poses(geometry_msgs::Pose p1, geometry_msgs::Pose p2, double pos_to
     double y_d = p1.position.y - p2.position.y;
     double z_d = p1.position.z - p2.position.z;
     double pos_diff = std::sqrt(x_d*x_d + y_d*y_d + z_d*z_d);
-    if (angle_diff < angle_tol * M_PI && pos_diff < pos_tolerance) 
+    if (angle_diff < angle_tol && pos_diff < pos_tolerance) 
         return true;
     return false;
 }