Use diff between commands instead of distance from current joint pose…

… to target pose

resources/external_control.urscript

@@ -89,13 +89,12 @@ thread_script_commands = 0
 #
 # @returns bool true if the target is reachable within the robot's speed limits, false otherwise
 ###
-def targetWithinLimits(target, time):
+def targetWithinLimits(step_start, step_end, time):
   local idx = 0
-  local actual_q = get_actual_joint_positions()
   while idx < 6:
-    local velocity = norm(target[idx] - actual_q[idx]) / time
+    local velocity = norm(step_end[idx] - step_start[idx]) / time
     if velocity > MAX_JOINT_SPEED:
-      local str = str_cat(str_cat("Velocity required to reach the received target ", target), str_cat(str_cat(" within ", time), " seconds is exceeding the joint velocity limits. Aborting script."))
+      local str = str_cat(str_cat("Velocity ", velocity), str_cat(str_cat(" required to reach the received target ", step_end), str_cat(str_cat(" within ", time), " seconds is exceeding the joint velocity limits. Aborting script.")))
       textmsg(str)
       return False
     end
@@ -127,6 +126,7 @@ end
 thread servoThread():
   textmsg("ExternalControl: Starting servo thread")
   state = SERVO_IDLE
+  local q_last = get_target_joint_positions()
   while control_mode == MODE_SERVOJ:
     enter_critical
     q = cmd_servo_q
@@ -146,16 +146,18 @@ thread servoThread():
       end
 
       q = extrapolate()
-      if targetWithinLimits(q, steptime):
+      if targetWithinLimits(q_last, q, steptime):
         servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+        q_last = q
       else:
         terminateProgram()
       end
 
     elif state == SERVO_RUNNING:
       extrapolate_count = 0
-      if targetWithinLimits(q, steptime):
+      if targetWithinLimits(q_last, q, steptime):
         servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+        q_last = q
       else:
         terminateProgram()
       end
@@ -189,13 +191,13 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False):
   local str = str_cat(end_q, str_cat(end_qd, time))
   textmsg("Cubic spline arg: ", str)
 
+  local start_q = get_target_joint_positions()
   # Zero time means infinite velocity to reach the target and is therefore impossible
   if time <= 0.0:
     textmsg("Spline time shouldn't be zero as it would require infinite velocity to reach the target. Aborting script.")
     socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
     terminateProgram()
-  elif targetWithinLimits(end_q, time):
-    local start_q = get_target_joint_positions()
+  elif targetWithinLimits(start_q, end_q, time):
     local start_qd = spline_qd
 
     # Coefficients0 is not included, since we do not need to calculate the position
@@ -215,13 +217,13 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False):
   local str = str_cat(end_q, str_cat(end_qd, str_cat(end_qdd, time)))
   textmsg("Quintic spline arg: ", str)
 
+  local start_q = get_target_joint_positions()
   # Zero time means infinite velocity to reach the target and is therefore impossible
   if time <= 0.0:
     textmsg("Spline time shouldn't be zero as it would require infinite velocity to reach the target. Aborting script.")
     socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
     terminateProgram()
-  elif targetWithinLimits(end_q, time):
-    local start_q = get_target_joint_positions()
+  elif targetWithinLimits(start_q, end_q, time):
     local start_qd = spline_qd
     local start_qdd = spline_qdd