Move limit check to receiving thread instead of servo thread

The servo thread might run at a lower frequency than the receiving
thread, as servoj will be blocking for the scaled time. Thus, the new
command will be compared with one that is older. We want to make sure
that the commands that we receive are smooth.

Author: urfeex

resources/external_control.urscript

@@ -150,8 +150,10 @@ end
 
 def set_servo_setpoint(q):
   cmd_servo_state = SERVO_RUNNING
-  cmd_servo_q_last = cmd_servo_q
-  cmd_servo_q = q
+  if targetWithinLimits(cmd_servo_q, q, steptime):
+    cmd_servo_q_last = cmd_servo_q
+    cmd_servo_q = q
+  end
 end
 
 def extrapolate():
@@ -165,7 +167,6 @@ end
 thread servoThread():
   textmsg("ExternalControl: Starting servo thread")
   state = SERVO_IDLE
-  local q_last = get_joint_positions()
   while control_mode == MODE_SERVOJ:
     enter_critical
     q = cmd_servo_q
@@ -185,17 +186,11 @@ thread servoThread():
       end
 
       q = extrapolate()
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-        q_last = q
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
 
     elif state == SERVO_RUNNING:
       extrapolate_count = 0
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-        q_last = q
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
     else:
       extrapolate_count = 0
       sync()
@@ -564,7 +559,6 @@ end
 thread servoThreadP():
   textmsg("Starting pose servo thread")
   state = SERVO_IDLE
-  local q_last = get_joint_positions()
   while control_mode == MODE_POSE:
     enter_critical
     q = cmd_servo_q
@@ -584,15 +578,11 @@ thread servoThreadP():
       end
 
       q = extrapolate()
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
 
     elif state == SERVO_RUNNING:
       extrapolate_count = 0
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
     else:
       extrapolate_count = 0
       sync()
@@ -603,12 +593,6 @@ thread servoThreadP():
   stopj(STOPJ_ACCELERATION)
 end
 
-def set_servo_pose(pose):
-  cmd_servo_state = SERVO_RUNNING
-  cmd_servo_q_last = cmd_servo_q
-  cmd_servo_q = get_inverse_kin(pose, cmd_servo_q)
-end
-
 def tool_contact_detection():
   # Detect tool contact in the directions that the TCP is moving
   step_back = tool_contact(direction = get_actual_tcp_speed())
@@ -727,6 +711,7 @@ while control_mode > MODE_STOPPED:
         join thread_move
       end
       if control_mode == MODE_SERVOJ:
+        cmd_servo_q = get_joint_positions()
         thread_move = run servoThread()
       elif control_mode == MODE_SPEEDJ:
         thread_move = run speedThread()
@@ -736,6 +721,7 @@ while control_mode > MODE_STOPPED:
       elif control_mode == MODE_SPEEDL:
         thread_move = run speedlThread()
       elif control_mode == MODE_POSE:
+        cmd_servo_q = get_joint_positions()
         thread_move = run servoThreadP()
       end
     end
@@ -765,7 +751,7 @@ while control_mode > MODE_STOPPED:
       set_speedl(twist)
     elif control_mode == MODE_POSE:
       pose = p[params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
-      set_servo_pose(pose)
+      set_servo_setpoint(get_inverse_kin(pose, cmd_servo_q))
     elif control_mode == MODE_FREEDRIVE:
       if params_mult[2] == FREEDRIVE_MODE_START:
         textmsg("Entering freedrive mode")