More fixes

urmahp · Jul 16, 2024 · 533e9b2 · 533e9b2
1 parent 8367cef
commit 533e9b2
Showing 1 changed file with 57 additions and 52 deletions.
diff --git a/resources/external_control.urscript b/resources/external_control.urscript
@@ -78,6 +78,7 @@ global trajectory_points_left = 0
 global spline_qdd = [0, 0, 0, 0, 0, 0]
 global spline_qd = [0, 0, 0, 0, 0, 0]
 global tool_contact_running = False
+global trajectory_result = 0
 
 # Global thread variables
 thread_move = 0
@@ -210,7 +211,7 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False):
   if time <= 0.0:
     error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion."
     textmsg(error_str)
-    socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
+    trajectory_result = TRAJECTORY_RESULT_CANCELED
     popup(error_str, title="External Control error", blocking=False, error=True)
   elif targetWithinLimits(start_q, end_q, time):
     local start_qd = spline_qd
@@ -223,7 +224,7 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False):
     local coefficients5 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, is_last_point)
   else:
-    socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
+    trajectory_result = TRAJECTORY_RESULT_CANCELED
   end
 end
 
@@ -236,7 +237,7 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False):
   if time <= 0.0:
     error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion."
     textmsg(error_str)
-    socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
+    trajectory_result = TRAJECTORY_RESULT_CANCELED
     popup(error_str, title="External Control error", blocking=False, error=True)
   elif targetWithinLimits(start_q, end_q, time):
     local start_qd = spline_qd
@@ -252,7 +253,7 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False):
     local coefficients5 = (-12.0 * start_q + 12.0 * end_q - start_qdd * TIME2 + end_qdd * TIME2 - 6.0 * start_qd * time - 6.0 * end_qd * time) / (2.0 * pow(time, 5))
     jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, is_last_point)
   else:
-    socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
+    trajectory_result = TRAJECTORY_RESULT_CANCELED
   end
 end
 
@@ -412,63 +413,66 @@ thread trajectoryThread():
   spline_qdd = [0, 0, 0, 0, 0, 0]
   spline_qd = [0, 0, 0, 0, 0, 0]
   enter_critical
-  while trajectory_points_left > 0:
-    #reading trajectory point + blend radius + type of point (cartesian/joint based)
-    local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime())
-    trajectory_points_left = trajectory_points_left - 1
-
-    if raw_point[0] > 0:
-      local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate]
-      local tmptime = raw_point[INDEX_TIME] / MULT_time
-      local blend_radius = raw_point[INDEX_BLEND] / MULT_time
-      local is_last_point = False
-      if trajectory_points_left == 0:
-        blend_radius = 0.0
-        is_last_point = True
-      end
-      # MoveJ point
-      if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT:
-        movej(q, t=tmptime, r=blend_radius)
-
-        # reset old acceleration
-        spline_qdd = [0, 0, 0, 0, 0, 0]
-        spline_qd = [0, 0, 0, 0, 0, 0]
-
-      # Movel point
-      elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN:
-        movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius)
+  trajectory_result = TRAJECTORY_RESULT_SUCCESS
+  while trajectory_result == TRAJECTORY_RESULT_SUCCESS:
+    while trajectory_points_left > 0:
+      #reading trajectory point + blend radius + type of point (cartesian/joint based)
+      local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime())
+      trajectory_points_left = trajectory_points_left - 1
+
+      if raw_point[0] > 0:
+        local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate]
+        local tmptime = raw_point[INDEX_TIME] / MULT_time
+        local blend_radius = raw_point[INDEX_BLEND] / MULT_time
+        local is_last_point = False
+        if trajectory_points_left == 0:
+          blend_radius = 0.0
+          is_last_point = True
+        end
+        # MoveJ point
+        if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT:
+          movej(q, t=tmptime, r=blend_radius)
 
-        # reset old acceleration
-        spline_qdd = [0, 0, 0, 0, 0, 0]
-        spline_qd = [0, 0, 0, 0, 0, 0]
+          # reset old acceleration
+          spline_qdd = [0, 0, 0, 0, 0, 0]
+          spline_qd = [0, 0, 0, 0, 0, 0]
 
-      # Joint spline point
-      elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
+        # Movel point
+        elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN:
+          movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius)
 
-        # Cubic spline
-        if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
-          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
-          cubicSplineRun(q, qd, tmptime, is_last_point)
           # reset old acceleration
           spline_qdd = [0, 0, 0, 0, 0, 0]
-
-        # Quintic spline
-        elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
-          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
-          qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate]
-          quinticSplineRun(q, qd, qdd, tmptime, is_last_point)
-        else:
-          textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])
-          clear_remaining_trajectory_points()
-          socket_send_int(TRAJECTORY_RESULT_FAILURE, "trajectory_socket")
+          spline_qd = [0, 0, 0, 0, 0, 0]
+
+        # Joint spline point
+        elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE:
+
+          # Cubic spline
+          if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
+            qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
+            cubicSplineRun(q, qd, tmptime, is_last_point)
+            # reset old acceleration
+            spline_qdd = [0, 0, 0, 0, 0, 0]
+
+          # Quintic spline
+          elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
+            qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
+            qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate]
+            quinticSplineRun(q, qd, qdd, tmptime, is_last_point)
+          else:
+            textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])
+            clear_remaining_trajectory_points()
+            trajectory_result = TRAJECTORY_RESULT_FAILURE
+          end
         end
       end
     end
   end
   exit_critical
   stopj(STOPJ_ACCELERATION)
-  socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket")
-  textmsg("Trajectory finished")
+  socket_send_int(trajectory_result, "trajectory_socket")
+  textmsg("Trajectory finished with result ", trajectory_result)
 end
 
 def clear_remaining_trajectory_points():
@@ -497,6 +501,7 @@ end
 thread servoThreadP():
   textmsg("Starting pose servo thread")
   state = SERVO_IDLE
+  local q_last = get_target_joint_positions()
   while control_mode == MODE_POSE:
     enter_critical
     q = cmd_servo_q
@@ -516,13 +521,13 @@ thread servoThreadP():
       end
 
       q = extrapolate()
-      if targetWithinLimits(q, get_steptime()):
+      if targetWithinLimits(q_last, q, steptime):
         servoj(q, t=steptime, {{SERVO_J_REPLACE}})
       end
 
     elif state == SERVO_RUNNING:
       extrapolate_count = 0
-      if targetWithinLimits(q, get_steptime()):
+      if targetWithinLimits(q_last, q, steptime):
         servoj(q, t=steptime, {{SERVO_J_REPLACE}})
       end
     else: