diff options
Diffstat (limited to 'klippy/mathutil.py')
| -rw-r--r-- | klippy/mathutil.py | 53 |
1 files changed, 35 insertions, 18 deletions
diff --git a/klippy/mathutil.py b/klippy/mathutil.py index c741d915..6de89c64 100644 --- a/klippy/mathutil.py +++ b/klippy/mathutil.py @@ -11,11 +11,12 @@ import queuelogger # Coordinate descent ###################################################################### + # Helper code that implements coordinate descent def coordinate_descent(adj_params, params, error_func): # Define potential changes params = dict(params) - dp = {param_name: 1. for param_name in adj_params} + dp = {param_name: 1.0 for param_name in adj_params} # Calculate the error best_err = error_func(params) logging.info("Coordinate descent initial error: %s", best_err) @@ -43,14 +44,15 @@ def coordinate_descent(adj_params, params, error_func): continue params[param_name] = orig dp[param_name] *= 0.9 - logging.info("Coordinate descent best_err: %s rounds: %d", - best_err, rounds) + logging.info("Coordinate descent best_err: %s rounds: %d", best_err, rounds) return params + # Helper to run the coordinate descent function in a background # process so that it does not block the main thread. def background_coordinate_descent(printer, adj_params, params, error_func): parent_conn, child_conn = multiprocessing.Pipe() + def wrapper(): queuelogger.clear_bg_logging() try: @@ -61,6 +63,7 @@ def background_coordinate_descent(printer, adj_params, params, error_func): return child_conn.send((False, res)) child_conn.close() + # Start a process to perform the calculation calc_proc = multiprocessing.Process(target=wrapper) calc_proc.daemon = True @@ -70,10 +73,10 @@ def background_coordinate_descent(printer, adj_params, params, error_func): gcode = printer.lookup_object("gcode") eventtime = last_report_time = reactor.monotonic() while calc_proc.is_alive(): - if eventtime > last_report_time + 5.: + if eventtime > last_report_time + 5.0: last_report_time = eventtime gcode.respond_info("Working on calibration...", log=False) - eventtime = reactor.pause(eventtime + .1) + eventtime = reactor.pause(eventtime + 0.1) # Return results is_err, res = parent_conn.recv() if is_err: @@ -87,6 +90,7 @@ def background_coordinate_descent(printer, adj_params, params, error_func): # Trilateration ###################################################################### + # Trilateration finds the intersection of three spheres. See the # wikipedia article for the details of the algorithm. def trilateration(sphere_coords, radius2): @@ -95,15 +99,15 @@ def trilateration(sphere_coords, radius2): s31 = matrix_sub(sphere_coord3, sphere_coord1) d = math.sqrt(matrix_magsq(s21)) - ex = matrix_mul(s21, 1. / d) + ex = matrix_mul(s21, 1.0 / d) i = matrix_dot(ex, s31) vect_ey = matrix_sub(s31, matrix_mul(ex, i)) - ey = matrix_mul(vect_ey, 1. / math.sqrt(matrix_magsq(vect_ey))) + ey = matrix_mul(vect_ey, 1.0 / math.sqrt(matrix_magsq(vect_ey))) ez = matrix_cross(ex, ey) j = matrix_dot(ey, s31) - x = (radius2[0] - radius2[1] + d**2) / (2. * d) - y = (radius2[0] - radius2[2] - x**2 + (x-i)**2 + j**2) / (2. * j) + x = (radius2[0] - radius2[1] + d**2) / (2.0 * d) + y = (radius2[0] - radius2[2] - x**2 + (x - i) ** 2 + j**2) / (2.0 * j) z = -math.sqrt(radius2[0] - x**2 - y**2) ex_x = matrix_mul(ex, x) @@ -116,37 +120,50 @@ def trilateration(sphere_coords, radius2): # Matrix helper functions for 3x1 matrices ###################################################################### + def matrix_cross(m1, m2): - return [m1[1] * m2[2] - m1[2] * m2[1], - m1[2] * m2[0] - m1[0] * m2[2], - m1[0] * m2[1] - m1[1] * m2[0]] + return [ + m1[1] * m2[2] - m1[2] * m2[1], + m1[2] * m2[0] - m1[0] * m2[2], + m1[0] * m2[1] - m1[1] * m2[0], + ] + def matrix_dot(m1, m2): return m1[0] * m2[0] + m1[1] * m2[1] + m1[2] * m2[2] + def matrix_magsq(m1): - return m1[0]**2 + m1[1]**2 + m1[2]**2 + return m1[0] ** 2 + m1[1] ** 2 + m1[2] ** 2 + def matrix_add(m1, m2): return [m1[0] + m2[0], m1[1] + m2[1], m1[2] + m2[2]] + def matrix_sub(m1, m2): return [m1[0] - m2[0], m1[1] - m2[1], m1[2] - m2[2]] + def matrix_mul(m1, s): - return [m1[0]*s, m1[1]*s, m1[2]*s] + return [m1[0] * s, m1[1] * s, m1[2] * s] + ###################################################################### # Matrix helper functions for 3x3 matrices ###################################################################### + def matrix_det(a): x0, x1, x2 = a return matrix_dot(x0, matrix_cross(x1, x2)) + def matrix_inv(a): x0, x1, x2 = a - inv_det = 1. / matrix_det(a) - return [matrix_mul(matrix_cross(x1, x2), inv_det), - matrix_mul(matrix_cross(x2, x0), inv_det), - matrix_mul(matrix_cross(x0, x1), inv_det)] + inv_det = 1.0 / matrix_det(a) + return [ + matrix_mul(matrix_cross(x1, x2), inv_det), + matrix_mul(matrix_cross(x2, x0), inv_det), + matrix_mul(matrix_cross(x0, x1), inv_det), + ] |