diff --git a/urx/robot.py b/urx/robot.py
index c8d36c0..cc45fa9 100644
--- a/urx/robot.py
+++ b/urx/robot.py
@@ -40,7 +40,7 @@ class Robot(URRobot):
         """
         self.csys = transform
 
-    def set_orientation(self, orient, acc=0.01, vel=0.01, wait=True):
+    def set_orientation(self, orient, acc=0.01, vel=0.01, wait=True, threshold=0.001):
         """
         set tool orientation using a orientation matric from math3d
         or a orientation vector
@@ -49,9 +49,9 @@ class Robot(URRobot):
             orient = m3d.Orientation(orient)
         trans = self.get_pose()
         trans.orient = orient
-        self.set_pose(trans, acc, vel, wait=wait)
+        self.set_pose(trans, acc, vel, wait=wait, threshold=threshold)
 
-    def translate_tool(self, vect, acc=0.01, vel=0.01, wait=True):
+    def translate_tool(self, vect, acc=0.01, vel=0.01, wait=True, threshold=0.001):
         """
         move tool in tool coordinate, keeping orientation
         """
@@ -59,7 +59,7 @@ class Robot(URRobot):
         if not isinstance(vect, m3d.Vector):
             vect = m3d.Vector(vect)
         t.pos += vect
-        return self.add_pose_tool(t, acc, vel, wait=wait)
+        return self.add_pose_tool(t, acc, vel, wait=wait, threshold=threshold)
 
     def back(self, z=0.05, acc=0.01, vel=0.01):
         """
@@ -67,27 +67,27 @@ class Robot(URRobot):
         """
         self.translate_tool((0, 0, -z), acc=acc, vel=vel)
 
-    def set_pos(self, vect, acc=0.01, vel=0.01, wait=True):
+    def set_pos(self, vect, acc=0.01, vel=0.01, wait=True, threshold=0.001):
         """
         set tool to given pos, keeping constant orientation
         """
         if not isinstance(vect, m3d.Vector):
             vect = m3d.Vector(vect)
         trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
-        return self.set_pose(trans, acc, vel, wait=wait)
+        return self.set_pose(trans, acc, vel, wait=wait, threshold=threshold)
 
-    def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True):
+    def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=0.001):
         """
         Move Circular: Move to position (circular in tool-space)
         see UR documentation
         """
         pose_via = self.csys * m3d.Transform(pose_via)
         pose_to = self.csys * m3d.Transform(pose_to)
-        pose = URRobot.movec(self, pose_via.pose_vector, pose_to.pose_vector, acc=acc, vel=vel, wait=wait)
+        pose = URRobot.movec(self, pose_via.pose_vector, pose_to.pose_vector, acc=acc, vel=vel, wait=wait, threshold=threshold)
         if pose is not None:
             return self.csys.inverse * m3d.Transform(pose)
 
-    def set_pose(self, trans, acc=0.01, vel=0.01, wait=True, command="movel"):
+    def set_pose(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
         """
         move tcp to point and orientation defined by a transformation
         UR robots have several move commands, by default movel is used but it can be changed
@@ -95,23 +95,23 @@ class Robot(URRobot):
         """
         self.logger.debug("Setting pose to %s", trans.pose_vector)
         t = self.csys * trans
-        pose = URRobot.movex(self, command, t.pose_vector, acc=acc, vel=vel, wait=wait)
+        pose = URRobot.movex(self, command, t.pose_vector, acc=acc, vel=vel, wait=wait, threshold=threshold)
         if pose is not None:
             return self.csys.inverse * m3d.Transform(pose)
 
-    def add_pose_base(self, trans, acc=0.01, vel=0.01, wait=True, command="movel"):
+    def add_pose_base(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
         """
         Add transform expressed in base coordinate
         """
         pose = self.get_pose()
-        return self.set_pose(trans * pose, acc, vel, wait=wait, command=command)
+        return self.set_pose(trans * pose, acc, vel, wait=wait, command=command, threshold=threshold)
 
-    def add_pose_tool(self, trans, acc=0.01, vel=0.01, wait=True, command="movel"):
+    def add_pose_tool(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
         """
         Add transform expressed in tool coordinate
         """
         pose = self.get_pose()
-        return self.set_pose(pose * trans, acc, vel, wait=wait, command=command)
+        return self.set_pose(pose * trans, acc, vel, wait=wait, command=command, threshold=threshold)
 
     def get_pose(self, wait=False, _log=True):
         """
@@ -155,18 +155,18 @@ class Robot(URRobot):
         w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:])
         URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time)
 
-    def movex(self, command, pose, acc=0.01, vel=0.01, wait=True, relative=False):
+    def movex(self, command, pose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
         """
         Send a move command to the robot. since UR robotene have several methods this one
         sends whatever is defined in 'command' string
         """
         t = m3d.Transform(pose)
         if relative:
-            return self.add_pose_base(t, acc, vel, wait=wait, command=command)
+            return self.add_pose_base(t, acc, vel, wait=wait, command=command, threshold=threshold)
         else:
-            return self.set_pose(t, acc, vel, wait=wait, command=command)
+            return self.set_pose(t, acc, vel, wait=wait, command=command, threshold=threshold)
 
-    def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True):
+    def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
         """
         Concatenate several movex commands and applies a blending radius
         pose_list is a list of pose.
@@ -178,17 +178,17 @@ class Robot(URRobot):
             t = self.csys * m3d.Transform(pose)
             pose = t.pose_vector
             new_poses.append(pose)
-        return URRobot.movexs(self, command, new_poses, acc, vel, radius, wait=wait)
+        return URRobot.movexs(self, command, new_poses, acc, vel, radius, wait=wait, threshold=threshold)
 
-    def movel_tool(self, pose, acc=0.01, vel=0.01, wait=True):
+    def movel_tool(self, pose, acc=0.01, vel=0.01, wait=True, threshold=0.001):
         """
         move linear to given pose in tool coordinate
         """
-        return self.movex_tool("movel", pose, acc=acc, vel=vel, wait=wait)
+        return self.movex_tool("movel", pose, acc=acc, vel=vel, wait=wait, threshold=threshold)
 
-    def movex_tool(self, command, pose, acc=0.01, vel=0.01, wait=True):
+    def movex_tool(self, command, pose, acc=0.01, vel=0.01, wait=True, threshold=0.001):
         t = m3d.Transform(pose)
-        self.add_pose_tool(t, acc, vel, wait=wait, command=command)
+        self.add_pose_tool(t, acc, vel, wait=wait, command=command, threshold=threshold)
 
     def getl(self, wait=False, _log=True):
         """
diff --git a/urx/urrobot.py b/urx/urrobot.py
index c4051e8..1e837c0 100644
--- a/urx/urrobot.py
+++ b/urx/urrobot.py
@@ -185,28 +185,29 @@ class URRobot(object):
         prog = "set_tool_voltage(%s)" % (val)
         self.send_program(prog)
 
-    def _wait_for_move(self, target, radius=0.001, timeout=2):
+    def _wait_for_move(self, target, threshold=0.001, timeout=2):
         """
         wait for a move to complete. Unfortunately there is no good way to know when a move has finished
         so for every received data from robot we compute a dist equivalent and when it is lower than
-        'radius' we return.
-        if radius is not reached within timeout, and exceptino is raised
+        'threshold' we return.
+        if threshold is not reached within timeout, and exceptino is raised
         """
-        self.logger.info("Waiting for move completion using radius %s and target %s", radius, target)
+        self.logger.debug("Waiting for move completion using threshold %s and target %s", threshold, target)
         count = 0
         while True:
             if not self.is_running():
                 raise RobotException("Robot stopped")
             dist = self._get_dist(target)
-            self.logger.debug("distance to target is: %s, target dist is %s", dist, radius)
-            if dist < radius:
+            self.logger.debug("distance to target is: %s, target dist is %s", dist, threshold)
+            if self.secmon.is_program_running():
+                count = 0
+                continue
+            if dist < threshold:
                 self.logger.debug("move has ended")
                 return
             count += 1
             if count > timeout * 10:
                 raise RobotException("Goal not reached but no program has been running for {} seconds. dist is {}, target is {}, current pose is {}".format(timeout, dist, target, URRobot.getl(self)))
-            if self.secmon.is_program_running():
-                count = 0
 
     def _get_dist(self, target):
         pose = URRobot.getl(self, wait=True)
@@ -216,22 +217,31 @@ class URRobot(object):
         for i in range(3, 6):
             dist += ((target[i] - pose[i]) / 5) ** 2 # arbitraty length like
         return dist ** 0.5 
+    
+    def _get_joints_dist(self, target):
+        joints = self.getj(wait=True)
+        dist = 0
+        for i in range(6):
+            dist += (target[i] - joints[i]) ** 2
+        return dist ** 0.5
 
-    def _wait_for_move_joints(self, radius, target):
-        #FIXME raise exception if target not reached after x seconds
-        self.logger.debug("Waiting for joint move completion with radius %s and target %s", radius, target)
+    def _wait_for_move_joints(self, target, threshold, timeout=2):
+        self.logger.debug("Waiting for joint move completion with threshold %s and target %s", threshold, target)
+        count = 0
         while True:
             if not self.is_running():
                 raise RobotException("Robot stopped")
-            joints = self.getj(wait=True)
-            dist = 0
-            for i in range(6):
-                dist += (target[i] - joints[i]) ** 2
-            dist = dist ** 0.5
-            self.logger.debug("distance to target is: %s, target dist is %s", dist, radius)
-            if dist < radius:
+            dist = self._get_joints_dist(target)
+            self.logger.debug("distance to target is: %s, target dist is %s", dist, threshold)
+            if self.secmon.is_program_running():
+                count = 0
+                continue
+            if dist < threshold:
                 self.logger.debug("move has ended")
                 return
+            count += 1
+            if count > timeout * 10:
+                raise RobotException("Goal not reached but no program has been running for {} seconds. dist is {}, target is {}, current pose is {}".format(timeout, dist, target, URRobot.getl(self)))
 
     def getj(self, wait=False):
         """
@@ -259,7 +269,7 @@ class URRobot(object):
         """
         return self.speedx("speedj", velocities, acc, min_time)
 
-    def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False):
+    def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=0.001):
         """
         move in joint space
         """
@@ -269,26 +279,26 @@ class URRobot(object):
         prog = self._format_move("movej", joints, acc, vel)
         self.send_program(prog)
         if wait:
-            self._wait_for_move_joints(0.001, joints[:6])
+            self._wait_for_move_joints(joints[:6], threshold=threshold)
             return self.getj()
 
-    def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False):
+    def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
         """
         Send a movel command to the robot. See URScript documentation.
         """
-        return self.movex("movel", tpose, acc=acc, vel=vel, wait=wait, relative=relative)
+        return self.movex("movel", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold)
 
-    def movep(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False):
+    def movep(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
         """
         Send a movep command to the robot. See URScript documentation.
         """
-        return self.movex("movep", tpose, acc=acc, vel=vel, wait=wait, relative=relative)
+        return self.movex("movep", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold)
 
-    def servoc(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False):
+    def servoc(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
         """
         Send a servoc command to the robot. See URScript documentation.
         """
-        return self.movex("servoc", tpose, acc=acc, vel=vel, wait=wait, relative=relative)
+        return self.movex("servoc", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold)
 
     def _format_move(self, command, tpose, acc, vel, radius=0, prefix=""):
         tpose = [round(i, self.max_float_length) for i in tpose]
@@ -297,7 +307,7 @@ class URRobot(object):
         tpose.append(radius)
         return "{}({}[{},{},{},{},{},{}], a={}, v={}, r={})".format(command, prefix, *tpose)
 
-    def movex(self, command, tpose, acc=0.01, vel=0.01, wait=True, relative=False):
+    def movex(self, command, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
         """
         Send a move command to the robot. since UR robotene have several methods this one
         sends whatever is defined in 'command' string
@@ -308,7 +318,7 @@ class URRobot(object):
         prog = self._format_move(command, tpose, acc, vel, prefix="p")
         self.send_program(prog)
         if wait:
-            self._wait_for_move(tpose[:6])
+            self._wait_for_move(tpose[:6], threshold=threshold)
             return self.getl()
 
     def getl(self, wait=False, _log=True):
@@ -322,7 +332,7 @@ class URRobot(object):
             self.logger.debug("Received pose from robot: %s", pose)
         return pose
 
-    def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True):
+    def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=0.001):
         """
         Move Circular: Move to position (circular in tool-space)
         see UR documentation
@@ -332,19 +342,19 @@ class URRobot(object):
         prog = "movec(p%s, p%s, a=%s, v=%s, r=%s)" % (pose_via, pose_to, acc, vel, "0")
         self.send_program(prog)
         if wait:
-            self._wait_for_move(pose_to)
+            self._wait_for_move(pose_to, threshold=threshold)
             return self.getl()
 
-    def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True):
+    def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
         """
         Concatenate several movel commands and applies a blending radius
         pose_list is a list of pose.
         This method is usefull since any new command from python
         to robot make the robot stop
         """
-        return self.movexs("movel", pose_list, acc, vel, radius, wait)
+        return self.movexs("movel", pose_list, acc, vel, radius, wait, threshold=threshold)
 
-    def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True):
+    def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
         """
         Concatenate several movex commands and applies a blending radius
         pose_list is a list of pose.
@@ -361,7 +371,7 @@ class URRobot(object):
         prog += end
         self.send_program(prog)
         if wait:
-            self._wait_for_move(target=pose_list[-1])
+            self._wait_for_move(target=pose_list[-1], threshold=threshold)
             return self.getl()
 
     def stopl(self, acc=0.5):