diff options
Diffstat (limited to 'klippy/extras/shaper_calibrate.py')
| -rw-r--r-- | klippy/extras/shaper_calibrate.py | 288 |
1 files changed, 182 insertions, 106 deletions
diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index f497171f..117409aa 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -4,21 +4,23 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import collections, importlib, logging, math, multiprocessing, traceback -shaper_defs = importlib.import_module('.shaper_defs', 'extras') -MIN_FREQ = 5. -MAX_FREQ = 200. +shaper_defs = importlib.import_module(".shaper_defs", "extras") + +MIN_FREQ = 5.0 +MAX_FREQ = 200.0 WINDOW_T_SEC = 0.5 -MAX_SHAPER_FREQ = 150. +MAX_SHAPER_FREQ = 150.0 -TEST_DAMPING_RATIOS=[0.075, 0.1, 0.15] +TEST_DAMPING_RATIOS = [0.075, 0.1, 0.15] -AUTOTUNE_SHAPERS = ['zv', 'mzv', 'ei', '2hump_ei', '3hump_ei'] +AUTOTUNE_SHAPERS = ["zv", "mzv", "ei", "2hump_ei", "3hump_ei"] ###################################################################### # Frequency response calculation and shaper auto-tuning ###################################################################### + class CalibrationData: def __init__(self, freq_bins, psd_sum, psd_x, psd_y, psd_z): self.freq_bins = freq_bins @@ -27,9 +29,14 @@ class CalibrationData: self.psd_y = psd_y self.psd_z = psd_z self._psd_list = [self.psd_sum, self.psd_x, self.psd_y, self.psd_z] - self._psd_map = {'x': self.psd_x, 'y': self.psd_y, 'z': self.psd_z, - 'all': self.psd_sum} + self._psd_map = { + "x": self.psd_x, + "y": self.psd_y, + "z": self.psd_z, + "all": self.psd_sum, + } self.data_sets = 1 + def add_data(self, other): np = self.numpy joined_data_sets = self.data_sets + other.data_sets @@ -37,45 +44,55 @@ class CalibrationData: # `other` data may be defined at different frequency bins, # interpolating to fix that. other_normalized = other.data_sets * np.interp( - self.freq_bins, other.freq_bins, other_psd) + self.freq_bins, other.freq_bins, other_psd + ) psd *= self.data_sets - psd[:] = (psd + other_normalized) * (1. / joined_data_sets) + psd[:] = (psd + other_normalized) * (1.0 / joined_data_sets) self.data_sets = joined_data_sets + def set_numpy(self, numpy): self.numpy = numpy + def normalize_to_frequencies(self): for psd in self._psd_list: # Avoid division by zero errors - psd /= self.freq_bins + .1 + psd /= self.freq_bins + 0.1 # Remove low-frequency noise - low_freqs = self.freq_bins < 2. * MIN_FREQ + low_freqs = self.freq_bins < 2.0 * MIN_FREQ psd[low_freqs] *= self.numpy.exp( - -(2. * MIN_FREQ / (self.freq_bins[low_freqs] + .1))**2 + 1.) - def get_psd(self, axis='all'): + -((2.0 * MIN_FREQ / (self.freq_bins[low_freqs] + 0.1)) ** 2) + 1.0 + ) + + def get_psd(self, axis="all"): return self._psd_map[axis] CalibrationResult = collections.namedtuple( - 'CalibrationResult', - ('name', 'freq', 'vals', 'vibrs', 'smoothing', 'score', 'max_accel')) + "CalibrationResult", + ("name", "freq", "vals", "vibrs", "smoothing", "score", "max_accel"), +) + class ShaperCalibrate: def __init__(self, printer): self.printer = printer self.error = printer.command_error if printer else Exception try: - self.numpy = importlib.import_module('numpy') + self.numpy = importlib.import_module("numpy") except ImportError: raise self.error( - "Failed to import `numpy` module, make sure it was " - "installed via `~/klippy-env/bin/pip install` (refer to " - "docs/Measuring_Resonances.md for more details).") + "Failed to import `numpy` module, make sure it was " + "installed via `~/klippy-env/bin/pip install` (refer to " + "docs/Measuring_Resonances.md for more details)." + ) def background_process_exec(self, method, args): if self.printer is None: return method(*args) import queuelogger + parent_conn, child_conn = multiprocessing.Pipe() + def wrapper(): queuelogger.clear_bg_logging() try: @@ -86,6 +103,7 @@ class ShaperCalibrate: 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 @@ -95,10 +113,10 @@ class ShaperCalibrate: gcode = self.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("Wait for calculations..", log=False) - eventtime = reactor.pause(eventtime + .1) + eventtime = reactor.pause(eventtime + 0.1) # Return results is_err, res = parent_conn.recv() if is_err: @@ -115,12 +133,13 @@ class ShaperCalibrate: shape = (window_size, n_windows) strides = (x.strides[-1], step_between_windows * x.strides[-1]) return self.numpy.lib.stride_tricks.as_strided( - x, shape=shape, strides=strides, writeable=False) + x, shape=shape, strides=strides, writeable=False + ) def _psd(self, x, fs, nfft): # Calculate power spectral density (PSD) using Welch's algorithm np = self.numpy - window = np.kaiser(nfft, 6.) + window = np.kaiser(nfft, 6.0) # Compensation for windowing loss scale = 1.0 / (window**2).sum() @@ -138,13 +157,13 @@ class ShaperCalibrate: # For one-sided FFT output the response must be doubled, except # the last point for unpaired Nyquist frequency (assuming even nfft) # and the 'DC' term (0 Hz) - result[1:-1,:] *= 2. + result[1:-1, :] *= 2.0 # Welch's algorithm: average response over windows psd = result.real.mean(axis=-1) # Calculate the frequency bins - freqs = np.fft.rfftfreq(nfft, 1. / fs) + freqs = np.fft.rfftfreq(nfft, 1.0 / fs) return freqs, psd def calc_freq_response(self, raw_values): @@ -160,7 +179,7 @@ class ShaperCalibrate: data = np.array(samples) N = data.shape[0] - T = data[-1,0] - data[0,0] + T = data[-1, 0] - data[0, 0] SAMPLING_FREQ = N / T # Round up to the nearest power of 2 for faster FFT M = 1 << int(SAMPLING_FREQ * WINDOW_T_SEC - 1).bit_length() @@ -169,17 +188,19 @@ class ShaperCalibrate: # Calculate PSD (power spectral density) of vibrations per # frequency bins (the same bins for X, Y, and Z) - fx, px = self._psd(data[:,1], SAMPLING_FREQ, M) - fy, py = self._psd(data[:,2], SAMPLING_FREQ, M) - fz, pz = self._psd(data[:,3], SAMPLING_FREQ, M) - return CalibrationData(fx, px+py+pz, px, py, pz) + fx, px = self._psd(data[:, 1], SAMPLING_FREQ, M) + fy, py = self._psd(data[:, 2], SAMPLING_FREQ, M) + fz, pz = self._psd(data[:, 3], SAMPLING_FREQ, M) + return CalibrationData(fx, px + py + pz, px, py, pz) def process_accelerometer_data(self, data): calibration_data = self.background_process_exec( - self.calc_freq_response, (data,)) + self.calc_freq_response, (data,) + ) if calibration_data is None: raise self.error( - "Internal error processing accelerometer data %s" % (data,)) + "Internal error processing accelerometer data %s" % (data,) + ) calibration_data.set_numpy(self.numpy) return calibration_data @@ -187,51 +208,59 @@ class ShaperCalibrate: np = self.numpy A, T = np.array(shaper[0]), np.array(shaper[1]) - inv_D = 1. / A.sum() + inv_D = 1.0 / A.sum() - omega = 2. * math.pi * test_freqs + omega = 2.0 * math.pi * test_freqs damping = test_damping_ratio * omega - omega_d = omega * math.sqrt(1. - test_damping_ratio**2) + omega_d = omega * math.sqrt(1.0 - test_damping_ratio**2) W = A * np.exp(np.outer(-damping, (T[-1] - T))) S = W * np.sin(np.outer(omega_d, T)) C = W * np.cos(np.outer(omega_d, T)) - return np.sqrt(S.sum(axis=1)**2 + C.sum(axis=1)**2) * inv_D + return np.sqrt(S.sum(axis=1) ** 2 + C.sum(axis=1) ** 2) * inv_D - def _estimate_remaining_vibrations(self, shaper, test_damping_ratio, - freq_bins, psd): + def _estimate_remaining_vibrations( + self, shaper, test_damping_ratio, freq_bins, psd + ): vals = self._estimate_shaper(shaper, test_damping_ratio, freq_bins) # The input shaper can only reduce the amplitude of vibrations by # SHAPER_VIBRATION_REDUCTION times, so all vibrations below that # threshold can be igonred vibr_threshold = psd.max() / shaper_defs.SHAPER_VIBRATION_REDUCTION - remaining_vibrations = self.numpy.maximum( - vals * psd - vibr_threshold, 0).sum() + remaining_vibrations = self.numpy.maximum(vals * psd - vibr_threshold, 0).sum() all_vibrations = self.numpy.maximum(psd - vibr_threshold, 0).sum() return (remaining_vibrations / all_vibrations, vals) - def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.): - half_accel = accel * .5 + def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.0): + half_accel = accel * 0.5 A, T = shaper - inv_D = 1. / sum(A) + inv_D = 1.0 / sum(A) n = len(T) # Calculate input shaper shift ts = sum([A[i] * T[i] for i in range(n)]) * inv_D # Calculate offset for 90 and 180 degrees turn - offset_90 = offset_180 = 0. + offset_90 = offset_180 = 0.0 for i in range(n): if T[i] >= ts: # Calculate offset for one of the axes - offset_90 += A[i] * (scv + half_accel * (T[i]-ts)) * (T[i]-ts) - offset_180 += A[i] * half_accel * (T[i]-ts)**2 - offset_90 *= inv_D * math.sqrt(2.) + offset_90 += A[i] * (scv + half_accel * (T[i] - ts)) * (T[i] - ts) + offset_180 += A[i] * half_accel * (T[i] - ts) ** 2 + offset_90 *= inv_D * math.sqrt(2.0) offset_180 *= inv_D return max(offset_90, offset_180) - def fit_shaper(self, shaper_cfg, calibration_data, shaper_freqs, - damping_ratio, scv, max_smoothing, test_damping_ratios, - max_freq): + def fit_shaper( + self, + shaper_cfg, + calibration_data, + shaper_freqs, + damping_ratio, + scv, + max_smoothing, + test_damping_ratios, + max_freq, + ): np = self.numpy damping_ratio = damping_ratio or shaper_defs.DEFAULT_DAMPING_RATIO @@ -241,9 +270,8 @@ class ShaperCalibrate: shaper_freqs = (None, None, None) if isinstance(shaper_freqs, tuple): freq_end = shaper_freqs[1] or MAX_SHAPER_FREQ - freq_start = min(shaper_freqs[0] or shaper_cfg.min_freq, - freq_end - 1e-7) - freq_step = shaper_freqs[2] or .2 + freq_start = min(shaper_freqs[0] or shaper_cfg.min_freq, freq_end - 1e-7) + freq_step = shaper_freqs[2] or 0.2 test_freqs = np.arange(freq_start, freq_end, freq_step) else: test_freqs = np.array(shaper_freqs) @@ -257,7 +285,7 @@ class ShaperCalibrate: best_res = None results = [] for test_freq in test_freqs[::-1]: - shaper_vibrations = 0. + shaper_vibrations = 0.0 shaper_vals = np.zeros(shape=freq_bins.shape) shaper = shaper_cfg.init_func(test_freq, damping_ratio) shaper_smoothing = self._get_shaper_smoothing(shaper, scv=scv) @@ -267,7 +295,8 @@ class ShaperCalibrate: # remaining vibrations over possible damping values for dr in test_damping_ratios: vibrations, vals = self._estimate_remaining_vibrations( - shaper, dr, freq_bins, psd) + shaper, dr, freq_bins, psd + ) shaper_vals = np.maximum(shaper_vals, vals) if vibrations > shaper_vibrations: shaper_vibrations = vibrations @@ -275,13 +304,20 @@ class ShaperCalibrate: # The score trying to minimize vibrations, but also accounting # the growth of smoothing. The formula itself does not have any # special meaning, it simply shows good results on real user data - shaper_score = shaper_smoothing * (shaper_vibrations**1.5 + - shaper_vibrations * .2 + .01) + shaper_score = shaper_smoothing * ( + shaper_vibrations**1.5 + shaper_vibrations * 0.2 + 0.01 + ) results.append( - CalibrationResult( - name=shaper_cfg.name, freq=test_freq, vals=shaper_vals, - vibrs=shaper_vibrations, smoothing=shaper_smoothing, - score=shaper_score, max_accel=max_accel)) + CalibrationResult( + name=shaper_cfg.name, + freq=test_freq, + vals=shaper_vals, + vibrs=shaper_vibrations, + smoothing=shaper_smoothing, + score=shaper_score, + max_accel=max_accel, + ) + ) if best_res is None or best_res.vibrs > results[-1].vibrs: # The current frequency is better for the shaper. best_res = results[-1] @@ -294,17 +330,17 @@ class ShaperCalibrate: return selected def _bisect(self, func): - left = right = 1. + left = right = 1.0 if not func(1e-9): - return 0. + return 0.0 while not func(left): right = left - left *= .5 + left *= 0.5 if right == left: while func(right): - right *= 2. + right *= 2.0 while right - left > 1e-8: - middle = (left + right) * .5 + middle = (left + right) * 0.5 if func(middle): left = middle else: @@ -315,63 +351,99 @@ class ShaperCalibrate: # Just some empirically chosen value which produces good projections # for max_accel without much smoothing TARGET_SMOOTHING = 0.12 - max_accel = self._bisect(lambda test_accel: self._get_shaper_smoothing( - shaper, test_accel, scv) <= TARGET_SMOOTHING) + max_accel = self._bisect( + lambda test_accel: self._get_shaper_smoothing(shaper, test_accel, scv) + <= TARGET_SMOOTHING + ) return max_accel - def find_best_shaper(self, calibration_data, shapers=None, - damping_ratio=None, scv=None, shaper_freqs=None, - max_smoothing=None, test_damping_ratios=None, - max_freq=None, logger=None): + def find_best_shaper( + self, + calibration_data, + shapers=None, + damping_ratio=None, + scv=None, + shaper_freqs=None, + max_smoothing=None, + test_damping_ratios=None, + max_freq=None, + logger=None, + ): best_shaper = None all_shapers = [] shapers = shapers or AUTOTUNE_SHAPERS for shaper_cfg in shaper_defs.INPUT_SHAPERS: if shaper_cfg.name not in shapers: continue - shaper = self.background_process_exec(self.fit_shaper, ( - shaper_cfg, calibration_data, shaper_freqs, damping_ratio, - scv, max_smoothing, test_damping_ratios, max_freq)) + shaper = self.background_process_exec( + self.fit_shaper, + ( + shaper_cfg, + calibration_data, + shaper_freqs, + damping_ratio, + scv, + max_smoothing, + test_damping_ratios, + max_freq, + ), + ) if logger is not None: - logger("Fitted shaper '%s' frequency = %.1f Hz " - "(vibrations = %.1f%%, smoothing ~= %.3f)" % ( - shaper.name, shaper.freq, shaper.vibrs * 100., - shaper.smoothing)) - logger("To avoid too much smoothing with '%s', suggested " - "max_accel <= %.0f mm/sec^2" % ( - shaper.name, round(shaper.max_accel / 100.) * 100.)) + logger( + "Fitted shaper '%s' frequency = %.1f Hz " + "(vibrations = %.1f%%, smoothing ~= %.3f)" + % (shaper.name, shaper.freq, shaper.vibrs * 100.0, shaper.smoothing) + ) + logger( + "To avoid too much smoothing with '%s', suggested " + "max_accel <= %.0f mm/sec^2" + % (shaper.name, round(shaper.max_accel / 100.0) * 100.0) + ) all_shapers.append(shaper) - if (best_shaper is None or shaper.score * 1.2 < best_shaper.score or - (shaper.score * 1.05 < best_shaper.score and - shaper.smoothing * 1.1 < best_shaper.smoothing)): + if ( + best_shaper is None + or shaper.score * 1.2 < best_shaper.score + or ( + shaper.score * 1.05 < best_shaper.score + and shaper.smoothing * 1.1 < best_shaper.smoothing + ) + ): # Either the shaper significantly improves the score (by 20%), # or it improves the score and smoothing (by 5% and 10% resp.) best_shaper = shaper return best_shaper, all_shapers def save_params(self, configfile, axis, shaper_name, shaper_freq): - if axis == 'xy': - self.save_params(configfile, 'x', shaper_name, shaper_freq) - self.save_params(configfile, 'y', shaper_name, shaper_freq) + if axis == "xy": + self.save_params(configfile, "x", shaper_name, shaper_freq) + self.save_params(configfile, "y", shaper_name, shaper_freq) else: - configfile.set('input_shaper', 'shaper_type_'+axis, shaper_name) - configfile.set('input_shaper', 'shaper_freq_'+axis, - '%.1f' % (shaper_freq,)) + configfile.set("input_shaper", "shaper_type_" + axis, shaper_name) + configfile.set( + "input_shaper", "shaper_freq_" + axis, "%.1f" % (shaper_freq,) + ) def apply_params(self, input_shaper, axis, shaper_name, shaper_freq): - if axis == 'xy': - self.apply_params(input_shaper, 'x', shaper_name, shaper_freq) - self.apply_params(input_shaper, 'y', shaper_name, shaper_freq) + if axis == "xy": + self.apply_params(input_shaper, "x", shaper_name, shaper_freq) + self.apply_params(input_shaper, "y", shaper_name, shaper_freq) return gcode = self.printer.lookup_object("gcode") axis = axis.upper() - input_shaper.cmd_SET_INPUT_SHAPER(gcode.create_gcode_command( - "SET_INPUT_SHAPER", "SET_INPUT_SHAPER", { + input_shaper.cmd_SET_INPUT_SHAPER( + gcode.create_gcode_command( + "SET_INPUT_SHAPER", + "SET_INPUT_SHAPER", + { "SHAPER_TYPE_" + axis: shaper_name, - "SHAPER_FREQ_" + axis: shaper_freq})) + "SHAPER_FREQ_" + axis: shaper_freq, + }, + ) + ) - def save_calibration_data(self, output, calibration_data, shapers=None, - max_freq=None): + def save_calibration_data( + self, output, calibration_data, shapers=None, max_freq=None + ): try: max_freq = max_freq or MAX_FREQ with open(output, "w") as csvfile: @@ -384,12 +456,16 @@ class ShaperCalibrate: for i in range(num_freqs): if calibration_data.freq_bins[i] >= max_freq: break - csvfile.write("%.1f,%.3e,%.3e,%.3e,%.3e" % ( - calibration_data.freq_bins[i], - calibration_data.psd_x[i], - calibration_data.psd_y[i], - calibration_data.psd_z[i], - calibration_data.psd_sum[i])) + csvfile.write( + "%.1f,%.3e,%.3e,%.3e,%.3e" + % ( + calibration_data.freq_bins[i], + calibration_data.psd_x[i], + calibration_data.psd_y[i], + calibration_data.psd_z[i], + calibration_data.psd_sum[i], + ) + ) if shapers: for shaper in shapers: csvfile.write(",%.3f" % (shaper.vals[i],)) |