forked from Klipper3d/klipper
-
Notifications
You must be signed in to change notification settings - Fork 1
/
shaper_calibrate.py
408 lines (358 loc) · 15.8 KB
/
shaper_calibrate.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
# Automatic calibration of input shapers
#
# Copyright (C) 2020 Dmitry Butyugin <[email protected]>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import collections, 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
######################################################################
InputShaperCfg = collections.namedtuple(
'InputShaperCfg', ('name', 'init_func', '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)
def get_shaper_smoothing(shaper):
# Smoothing calculation params
HALF_ACCEL = 2500.
SCV = 5.
A, T = shaper
inv_D = 1. / 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.
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_180 *= inv_D
return max(offset_90, offset_180)
# min_freq for each shaper is chosen to have max projected smoothing ~= 0.33
INPUT_SHAPERS = [
InputShaperCfg('zv', get_zv_shaper, min_freq=22.),
InputShaperCfg('mzv', get_mzv_shaper, min_freq=25.),
InputShaperCfg('ei', get_ei_shaper, min_freq=31.),
InputShaperCfg('2hump_ei', get_2hump_ei_shaper, min_freq=40.),
InputShaperCfg('3hump_ei', get_3hump_ei_shaper, min_freq=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.
CalibrationResult = collections.namedtuple(
'CalibrationResult',
('name', 'freq', 'vals', 'vibrs', 'smoothing', 'score'))
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, max_smoothing):
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_res = None
results = []
for test_freq in test_freqs[::-1]:
shaper_vibrations = 0.
shaper_vals = np.zeros(shape=freq_bins.shape)
shaper = shaper_cfg.init_func(test_freq, SHAPER_DAMPING_RATIO)
shaper_smoothing = get_shaper_smoothing(shaper)
if max_smoothing and shaper_smoothing > max_smoothing and best_res:
return best_res
# 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 > shaper_vibrations:
shaper_vibrations = vibrations
# 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_vibrations**1.5 * shaper_smoothing
results.append(
CalibrationResult(
name=shaper_cfg.name, freq=test_freq, vals=shaper_vals,
vibrs=shaper_vibrations, smoothing=shaper_smoothing,
score=shaper_score))
if best_res is None or best_res.vibrs > results[-1].vibrs:
# The current frequency is better for the shaper.
best_res = results[-1]
# Try to find an 'optimal' shapper configuration: the one that is not
# much worse than the 'best' one, but gives much less smoothing
selected = best_res
for res in results[::-1]:
if res.vibrs < best_res.vibrs * 1.1 and res.score < selected.score:
selected = res
return selected
def find_best_shaper(self, calibration_data, max_smoothing, logger=None):
best_shaper = None
all_shapers = []
for shaper_cfg in INPUT_SHAPERS:
shaper = self.background_process_exec(self.fit_shaper, (
shaper_cfg, calibration_data, max_smoothing))
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))
all_shapers.append(shaper)
if (best_shaper is None or shaper.score * 1.2 < best_shaper.score or
(shaper.score * 1.1 < best_shaper.score and
shaper.smoothing * 1.1 < best_shaper.smoothing)):
# Either the shaper significantly improves the score (by 20%),
# or it improves both the score and smoothing (by 10%)
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)
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=None):
try:
with open(output, "w") as csvfile:
csvfile.write("freq,psd_x,psd_y,psd_z,psd_xyz")
if shapers:
for shaper in shapers:
csvfile.write(",%s(%.1f)" % (shaper.name, shaper.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:
for shaper in shapers:
csvfile.write(",%.3f" % (shaper.vals[i],))
csvfile.write("\n")
except IOError as e:
raise self.error("Error writing to file '%s': %s", output, str(e))