resonance_tester: Resonance testing and input shaper auto-calibration (#3381)

Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>

1 files changed, 382 insertions, 0 deletions

diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py
new file mode 100644
index 00000000..67160b4f
--- /dev/null
+++ b/klippy/extras/shaper_calibrate.py
@@ -0,0 +1,382 @@
+# Automatic calibration of input shapers
+#
+# Copyright (C) 2020  Dmitry Butyugin <dmbutyugin@google.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import importlib, logging, math, multiprocessing
+
+MIN_FREQ = 5.
+MAX_FREQ = 200.
+WINDOW_T_SEC = 0.5
+MAX_SHAPER_FREQ = 150.
+
+TEST_DAMPING_RATIOS=[0.075, 0.1, 0.15]
+SHAPER_DAMPING_RATIO = 0.1
+
+######################################################################
+# Input shapers
+######################################################################
+
+class InputShaperCfg:
+    def __init__(self, name, init_func, min_freq):
+        self.name = name
+        self.init_func = init_func
+        self.min_freq = min_freq
+
+def get_zv_shaper(shaper_freq, damping_ratio):
+    df = math.sqrt(1. - damping_ratio**2)
+    K = math.exp(-damping_ratio * math.pi / df)
+    t_d = 1. / (shaper_freq * df)
+    A = [1., K]
+    T = [0., .5*t_d]
+    return (A, T)
+
+def get_zvd_shaper(shaper_freq, damping_ratio):
+    df = math.sqrt(1. - damping_ratio**2)
+    K = math.exp(-damping_ratio * math.pi / df)
+    t_d = 1. / (shaper_freq * df)
+    A = [1., 2.*K, K**2]
+    T = [0., .5*t_d, t_d]
+    return (A, T)
+
+def get_mzv_shaper(shaper_freq, damping_ratio):
+    df = math.sqrt(1. - damping_ratio**2)
+    K = math.exp(-.75 * damping_ratio * math.pi / df)
+    t_d = 1. / (shaper_freq * df)
+
+    a1 = 1. - 1. / math.sqrt(2.)
+    a2 = (math.sqrt(2.) - 1.) * K
+    a3 = a1 * K * K
+
+    A = [a1, a2, a3]
+    T = [0., .375*t_d, .75*t_d]
+    return (A, T)
+
+def get_ei_shaper(shaper_freq, damping_ratio):
+    v_tol = 0.05 # vibration tolerance
+    df = math.sqrt(1. - damping_ratio**2)
+    K = math.exp(-damping_ratio * math.pi / df)
+    t_d = 1. / (shaper_freq * df)
+
+    a1 = .25 * (1. + v_tol)
+    a2 = .5 * (1. - v_tol) * K
+    a3 = a1 * K * K
+
+    A = [a1, a2, a3]
+    T = [0., .5*t_d, t_d]
+    return (A, T)
+
+def get_2hump_ei_shaper(shaper_freq, damping_ratio):
+    v_tol = 0.05 # vibration tolerance
+    df = math.sqrt(1. - damping_ratio**2)
+    K = math.exp(-damping_ratio * math.pi / df)
+    t_d = 1. / (shaper_freq * df)
+
+    V2 = v_tol**2
+    X = pow(V2 * (math.sqrt(1. - V2) + 1.), 1./3.)
+    a1 = (3.*X*X + 2.*X + 3.*V2) / (16.*X)
+    a2 = (.5 - a1) * K
+    a3 = a2 * K
+    a4 = a1 * K * K * K
+
+    A = [a1, a2, a3, a4]
+    T = [0., .5*t_d, t_d, 1.5*t_d]
+    return (A, T)
+
+def get_3hump_ei_shaper(shaper_freq, damping_ratio):
+    v_tol = 0.05 # vibration tolerance
+    df = math.sqrt(1. - damping_ratio**2)
+    K = math.exp(-damping_ratio * math.pi / df)
+    t_d = 1. / (shaper_freq * df)
+
+    K2 = K*K
+    a1 = 0.0625 * (1. + 3. * v_tol + 2. * math.sqrt(2. * (v_tol + 1.) * v_tol))
+    a2 = 0.25 * (1. - v_tol) * K
+    a3 = (0.5 * (1. + v_tol) - 2. * a1) * K2
+    a4 = a2 * K2
+    a5 = a1 * K2 * K2
+
+    A = [a1, a2, a3, a4, a5]
+    T = [0., .5*t_d, t_d, 1.5*t_d, 2.*t_d]
+    return (A, T)
+
+INPUT_SHAPERS = [
+    InputShaperCfg('zv', get_zv_shaper, 15.),
+    InputShaperCfg('mzv', get_mzv_shaper, 25.),
+    InputShaperCfg('ei', get_ei_shaper, 30.),
+    InputShaperCfg('2hump_ei', get_2hump_ei_shaper, 37.5),
+    InputShaperCfg('3hump_ei', get_3hump_ei_shaper, 50.),
+]
+
+######################################################################
+# 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
+        self.psd_sum = psd_sum
+        self.psd_x = psd_x
+        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.data_sets = 1
+    def join(self, other):
+        np = self.numpy
+        joined_data_sets = self.data_sets + other.data_sets
+        for psd, other_psd in zip(self._psd_list, other._psd_list):
+            # `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)
+            psd *= self.data_sets
+            psd[:] = (psd + other_normalized) * (1. / 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
+            # Remove low-frequency noise
+            psd[self.freq_bins < MIN_FREQ] = 0.
+
+
+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')
+        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).")
+
+    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:
+                res = method(*args)
+            except:
+                child_conn.send((True, traceback.format_exc()))
+                child_conn.close()
+                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
+        calc_proc.start()
+        # Wait for the process to finish
+        reactor = self.printer.get_reactor()
+        gcode = self.printer.lookup_object("gcode")
+        eventtime = last_report_time = reactor.monotonic()
+        while calc_proc.is_alive():
+            if eventtime > last_report_time + 5.:
+                last_report_time = eventtime
+                gcode.respond_info("Wait for calculations..", log=False)
+            eventtime = reactor.pause(eventtime + .1)
+        # Return results
+        is_err, res = parent_conn.recv()
+        if is_err:
+            raise self.error("Error in remote calculation: %s" % (res,))
+        calc_proc.join()
+        parent_conn.close()
+        return res
+
+    def _split_into_windows(self, x, window_size, overlap):
+        # Memory-efficient algorithm to split an input 'x' into a series
+        # of overlapping windows
+        step_between_windows = window_size - overlap
+        n_windows = (x.shape[-1] - overlap) // step_between_windows
+        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)
+
+    def _psd(self, x, fs, nfft):
+        # Calculate power spectral density (PSD) using Welch's algorithm
+        np = self.numpy
+        window = np.kaiser(nfft, 6.)
+        # Compensation for windowing loss
+        scale = 1.0 / (window**2).sum()
+
+        # Split into overlapping windows of size nfft
+        overlap = nfft // 2
+        x = self._split_into_windows(x, nfft, overlap)
+
+        # First detrend, then apply windowing function
+        x = window[:, None] * (x - np.mean(x, axis=0))
+
+        # Calculate frequency response for each window using FFT
+        result = np.fft.rfft(x, n=nfft, axis=0)
+        result = np.conjugate(result) * result
+        result *= scale / fs
+        # 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.
+
+        # Welch's algorithm: average response over windows
+        psd = result.real.mean(axis=-1)
+
+        # Calculate the frequency bins
+        freqs = np.fft.rfftfreq(nfft, 1. / fs)
+        return freqs, psd
+
+    def calc_freq_response(self, raw_values):
+        np = self.numpy
+        if raw_values is None:
+            return None
+        if isinstance(raw_values, np.ndarray):
+            data = raw_values
+        else:
+            data = np.array(raw_values.decode_samples())
+
+        N = data.shape[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()
+        if N <= M:
+            return None
+
+        # 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)
+
+    def process_accelerometer_data(self, data):
+        calibration_data = self.background_process_exec(
+                self.calc_freq_response, (data,))
+        if calibration_data is None:
+            raise self.error(
+                    "Internal error processing accelerometer data %s" % (data,))
+        calibration_data.set_numpy(self.numpy)
+        return calibration_data
+
+    def _estimate_shaper(self, shaper, test_damping_ratio, test_freqs):
+        np = self.numpy
+
+        A, T = np.array(shaper[0]), np.array(shaper[1])
+        inv_D = 1. / A.sum()
+
+        omega = 2. * math.pi * test_freqs
+        damping = test_damping_ratio * omega
+        omega_d = omega * math.sqrt(1. - 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
+
+    def _estimate_remaining_vibrations(self, shaper, test_damping_ratio,
+                                       freq_bins, psd):
+        vals = self._estimate_shaper(shaper, test_damping_ratio, freq_bins)
+        remaining_vibrations = (vals * psd).sum() / psd.sum()
+        return (remaining_vibrations, vals)
+
+    def fit_shaper(self, shaper_cfg, calibration_data):
+        np = self.numpy
+
+        test_freqs = np.arange(shaper_cfg.min_freq, MAX_SHAPER_FREQ, .2)
+
+        freq_bins = calibration_data.freq_bins
+        psd = calibration_data.psd_sum[freq_bins <= MAX_FREQ]
+        freq_bins = freq_bins[freq_bins <= MAX_FREQ]
+
+        best_freq = None
+        best_remaining_vibrations = 0
+        best_shaper_vals = []
+
+        for test_freq in test_freqs[::-1]:
+            cur_remaining_vibrations = 0.
+            shaper_vals = np.zeros(shape=freq_bins.shape)
+            shaper = shaper_cfg.init_func(test_freq, SHAPER_DAMPING_RATIO)
+            # Exact damping ratio of the printer is unknown, pessimizing
+            # remaining vibrations over possible damping values.
+            for dr in TEST_DAMPING_RATIOS:
+                vibrations, vals = self._estimate_remaining_vibrations(
+                        shaper, dr, freq_bins, psd)
+                shaper_vals = np.maximum(shaper_vals, vals)
+                if vibrations > cur_remaining_vibrations:
+                    cur_remaining_vibrations = vibrations
+            if (best_freq is None or
+                    best_remaining_vibrations > cur_remaining_vibrations):
+                # The current frequency is better for the shaper.
+                best_freq = test_freq
+                best_remaining_vibrations = cur_remaining_vibrations
+                best_shaper_vals = shaper_vals
+        return (best_freq, best_remaining_vibrations, best_shaper_vals)
+
+    def find_best_shaper(self, calibration_data, logger=None):
+        best_shaper = prev_shaper = None
+        best_freq = prev_freq = 0.
+        best_vibrations = prev_vibrations = 0.
+        all_shaper_vals = []
+        for shaper in INPUT_SHAPERS:
+            shaper_freq, vibrations, shaper_vals = self.background_process_exec(
+                    self.fit_shaper, (shaper, calibration_data))
+            if logger is not None:
+                logger("Fitted shaper '%s' frequency = %.1f Hz "
+                       "(vibrations = %.1f%%)" % (
+                           shaper.name, shaper_freq, vibrations * 100.))
+            if best_shaper is None or 1.75 * vibrations < best_vibrations:
+                if 1.25 * vibrations < prev_vibrations:
+                    best_shaper = shaper.name
+                    best_freq = shaper_freq
+                    best_vibrations = vibrations
+                else:
+                    # The current shaper is good, but not sufficiently better
+                    # than the previous one, using previous shaper instead.
+                    best_shaper = prev_shaper
+                    best_freq = prev_freq
+                    best_vibrations = prev_vibrations
+            prev_shaper = shaper.name
+            prev_shaper_vals = shaper_vals
+            prev_freq = shaper_freq
+            prev_vibrations = vibrations
+            all_shaper_vals.append((shaper.name, shaper_freq, shaper_vals))
+        return (best_shaper, best_freq, all_shaper_vals)
+
+    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)
+        else:
+            configfile.set('input_shaper', 'shaper_type_'+axis, shaper_name)
+            configfile.set('input_shaper', 'shaper_freq_'+axis,
+                           '%.1f' % (shaper_freq,))
+
+    def save_calibration_data(self, output, calibration_data,
+                              shapers_vals=None):
+        try:
+            with open(output, "w") as csvfile:
+                csvfile.write("freq,psd_x,psd_y,psd_z,psd_xyz")
+                if shapers_vals:
+                    for name, freq, _ in shapers_vals:
+                        csvfile.write(",%s(%.1f)" % (name, freq))
+                csvfile.write("\n")
+                num_freqs = calibration_data.freq_bins.shape[0]
+                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]))
+                    if shapers_vals:
+                        for _, _, vals in shapers_vals:
+                            csvfile.write(",%.3f" % (vals[i],))
+                    csvfile.write("\n")
+        except IOError as e:
+            raise self.error("Error writing to file '%s': %s", output, str(e))