Make spline functions return whether the robot is moving or not

urmahp · Sep 6, 2024 · a090e2e · a090e2e
1 parent 9ea9723
commit a090e2e
Showing 1 changed file with 10 additions and 10 deletions.
diff --git a/resources/external_control.urscript b/resources/external_control.urscript
@@ -207,6 +207,8 @@ thread speedThread():
   stopj(STOPJ_ACCELERATION)
 end
 
+# Function return value (bool) determines whether the robot is moving after this spline segment or
+# not.
 def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=False):
   local str = str_cat(end_q, str_cat(end_qd, time))
   textmsg("Cubic spline arg: ", str)
@@ -222,7 +224,7 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=Fals
         local splineTimerTraveled = 0.0
         # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled)
         textmsg("Ignoring first point with time 0")
-        return None
+        return False
       end
     end
     error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion."
@@ -240,13 +242,15 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=Fals
     local coefficients4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     local coefficients5 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, is_last_point)
-    return True
+    return True and (is_last_point == False)
   else:
     trajectory_result = TRAJECTORY_RESULT_CANCELED
     return False
   end
 end
 
+# Function return value (bool) determines whether the robot is moving after this spline segment or
+# not.
 def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False, is_first_point=False):
   local str = str_cat(end_q, str_cat(end_qd, str_cat(end_qdd, time)))
   textmsg("Quintic spline arg: ", str)
@@ -259,7 +263,7 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False, is_first
       # ignore that point
       local distance = norm(end_q - get_target_joint_positions())
       if distance < 0.01:
-        return None
+        return False
       end
     end
     error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion."
@@ -280,7 +284,7 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False, is_first
     local coefficients4 = (30.0 * start_q - 30.0 * end_q + 3.0 * start_qdd * TIME2 - 2.0 * end_qdd * TIME2 + 16.0 * start_qd * time + 14.0 * end_qd * time) / (2.0 * pow(time, 4))
     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)
-    return True
+    return True and (is_last_point == False)
   else:
     trajectory_result = TRAJECTORY_RESULT_CANCELED
     return False
@@ -487,19 +491,15 @@ thread trajectoryThread():
         # 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]
-          if cubicSplineRun(q, qd, tmptime, is_last_point, is_first_point):
-            is_robot_moving = True
-          end
+          is_robot_moving = cubicSplineRun(q, qd, tmptime, is_last_point, is_first_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]
-          if quinticSplineRun(q, qd, qdd, tmptime, is_last_point, is_first_point):
-            is_robot_moving = True
-          end
+          is_robot_moving = quinticSplineRun(q, qd, qdd, tmptime, is_last_point, is_first_point)
         else:
           textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])
           clear_remaining_trajectory_points()