From 0895169e7ae83b30248941005ad47b144546f99f Mon Sep 17 00:00:00 2001 From: Rogerio Goncalves Date: Sun, 10 Sep 2023 16:00:24 +0100 Subject: [PATCH] sensille's z-tilt calibration (#54) --- docs/Config_Reference.md | 35 ++++- docs/G-Codes.md | 20 ++- klippy/extras/probe.py | 15 +- klippy/extras/z_tilt.py | 303 ++++++++++++++++++++++++++++++++++++++- 4 files changed, 365 insertions(+), 8 deletions(-) diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 9916e50ed..725a294c5 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -1126,7 +1126,15 @@ extended [G-Code command](G-Codes.md#z_tilt) becomes available. # stepper. It is described using nozzle coordinates (the X, Y position # of the nozzle if it could move directly above the point). The # first entry corresponds to stepper_z, the second to stepper_z1, -# the third to stepper_z2, etc. This parameter must be provided. +# the third to stepper_z2, etc. This parameter must be provided, +# unless the parameter "extra_points" is provided. In that case only +# the command Z_TILT_AUTODETECT can be run to automatically determine +# the z_positions. See 'extra_points' below. +#z_offsets: +# A list of Z offsets for each z_position. The z_offset is added to each +# probed value during Z_TILT_ADJUST to offset for unevenness of the bed. +# This values can also be automatically detected by running +# Z_TILT_CALIBRATE. See "extra_points" below. #points: # A list of X, Y coordinates (one per line; subsequent lines # indented) that should be probed during a Z_TILT_ADJUST command. @@ -1149,6 +1157,31 @@ extended [G-Code command](G-Codes.md#z_tilt) becomes available. # more points than steppers then you will likely have a fixed # minimum value for the range of probed points which you can learn # by observing command output. +#extra_points: +# A list in the same format as "points" above. This list contains +# additional points to be probed during the two calibration commands +# Z_TILT_CALIBRATE and Z_TILT_AUTODETECT. If the bed is not perfectly +# level, it is possible to specify more probing points with "points". +# In that Z_TILT_ADJUST will determine the best fit via a least squares +# algorithm. As this comes with additional overhead on each Z_TILT_ADJUST +# run, it is instead possible to move the additional probing points here, +# and use Z_TILT_CALIBRATE to find z_offsets to use for the probing points +# used in Z_TILT_ADJUST. +# The extra points are also used during T_ZILT_AUTODETECT. This command +# can determine the z_positions automatically by during several probings +# with intentionally tilted bed. It is currently only implemented for 3 +# z steppers. +# Note that for both commands to work numpy has to be installed. +#averaging_len: 3 +# Z_TILT_CALIBRATE and Z_TILT_AUTODETECT both run repeatedly until the +# result can no longer be improved. To determine this, the probed values +# are averaged. The number of runs to average over is configured with this +# parameter. +#autodetect_delta: 1.0 +# The amount by which Z_TILT_AUTODETECT intentionally tilts the bed. Higher +# values yield better results, but can also lead to situations where the +# bed is tilted in a way that the nozzle touched the bed before the probe. +# The default is conservative. ``` ### [quad_gantry_level] diff --git a/docs/G-Codes.md b/docs/G-Codes.md index 2416fb4ef..8fb7108c7 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -925,7 +925,7 @@ test. #### SET_HEATER_PID `SET_HEATER_PID HEATER= KP= KI= KD=`: Will -allow one to manually change PID parameters of heaters without a +allow one to manually change PID parameters of heaters without a reload of the firmware. ### [pause_resume] @@ -1408,3 +1408,21 @@ command will probe the points specified in the config and then make independent adjustments to each Z stepper to compensate for tilt. See the PROBE command for details on the optional probe parameters. The optional `HORIZONTAL_MOVE_Z` value overrides the `horizontal_move_z` option specified in the config file. +The follwing commands are availabe when the parameter "extra_points" is +configured in the z_tilt_section: +- `Z_TILT_CALIBRATE [AVGLEN=]`: This command does multiple probe + runs similar to Z_TILT_ADJUST, but with the additional points given in + "extra_points". This leads to a more balanced bed adjustment in case the + bed is not perfectly flat. The command averages the error over multiple + runs and continues until the error does not decrease any further. It + calculates values for the z_offsets config parameter, which will in turn + be used by T_TILT_ADJUST to achieve the same accuracy without the extra + points. +- `Z_TILT_AUTODETECT [AVGLEN=] [DELTA=]`: This command + determines the positions of the pivot points for each stepper motor. + It works silimar to Z_TILT_CALIBRATE, but it probes the bed with intential + small misalgnments of the steppers. The amount of misalignment can be + configured with the DELTA paramter. It iterates until the calculated + positions cannot be improved any further. This is can be lengthy procedure. +IMPORTANT: For the Z_TILT_CALIBRATE and Z_TILT_AUTODETECT commands to work +the numpy package has to be installed via ~/klippy-env/bin/pip install -v numpy. diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py index 83912c75d..1e581b291 100644 --- a/klippy/extras/probe.py +++ b/klippy/extras/probe.py @@ -462,16 +462,22 @@ def get_position_endstop(self): # Helper code that can probe a series of points and report the # position at each point. class ProbePointsHelper: - def __init__(self, config, finalize_callback, default_points=None): + def __init__( + self, + config, + finalize_callback, + default_points=None, + option_name="points", + ): self.printer = config.get_printer() self.finalize_callback = finalize_callback self.probe_points = default_points self.name = config.get_name() self.gcode = self.printer.lookup_object("gcode") # Read config settings - if default_points is None or config.get("points", None) is not None: + if default_points is None or config.get(option_name, None) is not None: self.probe_points = config.getlists( - "points", seps=(",", "\n"), parser=float, count=2 + option_name, seps=(",", "\n"), parser=float, count=2 ) def_move_z = config.getfloat("horizontal_move_z", 5.0) self.default_horizontal_move_z = def_move_z @@ -482,6 +488,9 @@ def __init__(self, config, finalize_callback, default_points=None): self.probe_offsets = (0.0, 0.0, 0.0) self.results = [] + def get_probe_points(self): + return self.probe_points + def minimum_points(self, n): if len(self.probe_points) < n: raise self.printer.config_error( diff --git a/klippy/extras/z_tilt.py b/klippy/extras/z_tilt.py index 948551f05..f632aa856 100644 --- a/klippy/extras/z_tilt.py +++ b/klippy/extras/z_tilt.py @@ -5,9 +5,25 @@ # This file may be distributed under the terms of the GNU GPLv3 license. import logging import mathutil +import importlib from . import probe +def params_to_normal_form(np, params, offsets): + v = np.array([offsets[0], offsets[1], params["z_adjust"]]) + r = np.array([1, 0, params["x_adjust"]]) + s = np.array([0, 1, params["y_adjust"]]) + cp = np.cross(r, s) + return np.append(cp, np.dot(cp, v)) + + +def intersect_3_planes(np, p1, p2, p3): + a = np.array([p1[0:3], p2[0:3], p3[0:3]]) + b = np.array([p1[3], p2[3], p3[3]]) + sol = np.linalg.solve(a, b) + return sol + + class ZAdjustHelper: def __init__(self, config, z_count): self.printer = config.get_printer() @@ -21,7 +37,13 @@ def __init__(self, config, z_count): def handle_connect(self): kin = self.printer.lookup_object("toolhead").get_kinematics() z_steppers = [s for s in kin.get_steppers() if s.is_active_axis("z")] - if len(z_steppers) != self.z_count: + if self.z_count is None: + if len(z_steppers) != 3: + raise self.printer.config_error( + "%s z_positions needs exactly 3 items for calibration" + % (self.name) + ) + elif len(z_steppers) != self.z_count: raise self.printer.config_error( "%s z_positions needs exactly %d items" % (self.name, len(z_steppers)) @@ -159,14 +181,53 @@ def check_retry(self, z_positions): class ZTilt: def __init__(self, config): self.printer = config.get_printer() + self.section = config.get_name() + + try: + self.numpy = importlib.import_module("numpy") + except ImportError: + logging.info( + "numpy not installed, Z_TILT_CALIBRATE will not be " "available" + ) + self.numpy = None + self.z_positions = config.getlists( "z_positions", seps=(",", "\n"), parser=float, count=2 ) + z_count = len(self.z_positions) + self.retry_helper = RetryHelper(config) self.probe_helper = probe.ProbePointsHelper(config, self.probe_finalize) self.probe_helper.minimum_points(2) + + self.z_offsets = config.getlists( + "z_offsets", parser=float, count=z_count, default=None + ) + self.z_status = ZAdjustStatus(self.printer) - self.z_helper = ZAdjustHelper(config, len(self.z_positions)) + self.z_helper = ZAdjustHelper(config, z_count) + # probe points for calibrate/autodetect + cal_probe_points = list(self.probe_helper.get_probe_points()) + self.num_probe_points = len(cal_probe_points) + self.cal_helper = None + if config.get("extra_points", None) is not None: + self.cal_helper = probe.ProbePointsHelper( + config, self.cal_finalize, option_name="extra_points" + ) + cal_probe_points.extend(self.cal_helper.get_probe_points()) + self.cal_helper.update_probe_points(cal_probe_points, 3) + self.ad_helper = probe.ProbePointsHelper(config, self.ad_finalize) + self.ad_helper.update_probe_points(cal_probe_points, 3) + self.cal_conf_avg_len = config.getint("averaging_len", 3, minval=1) + self.ad_conf_delta = config.getfloat( + "autodetect_delta", 1.0, minval=0.1 + ) + if ( + config.get("autodetect_delta", None) is not None + or self.z_positions is None + ) and self.numpy is None: + raise config.error(self.err_missing_numpy) + # Register Z_TILT_ADJUST command gcode = self.printer.lookup_object("gcode") gcode.register_command( @@ -174,19 +235,44 @@ def __init__(self, config): self.cmd_Z_TILT_ADJUST, desc=self.cmd_Z_TILT_ADJUST_help, ) + if self.cal_helper is not None: + gcode.register_command( + "Z_TILT_CALIBRATE", + self.cmd_Z_TILT_CALIBRATE, + desc=self.cmd_Z_TILT_CALIBRATE_help, + ) + gcode.register_command( + "Z_TILT_AUTODETECT", + self.cmd_Z_TILT_AUTODETECT, + desc=self.cmd_Z_TILT_AUTODETECT_help, + ) cmd_Z_TILT_ADJUST_help = "Adjust the Z tilt" + cmd_Z_TILT_CALIBRATE_help = ( + "Calibrate Z tilt with additional probing " "points" + ) + cmd_Z_TILT_AUTODETECT_help = "Autodetect pivot point of Z motors" + err_missing_numpy = ( + "Failed to import `numpy` module, make sure it was " + "installed via `~/klippy-env/bin/pip install`" + ) def cmd_Z_TILT_ADJUST(self, gcmd): + if self.z_positions is None: + gcmd.respond_info( + "No z_positions configured. Run Z_TILT_AUTODETECT first" + ) + return self.z_status.reset() self.retry_helper.start(gcmd) self.probe_helper.start_probe(gcmd) - def probe_finalize(self, offsets, positions): + def perform_coordinate_descent(self, offsets, positions): # Setup for coordinate descent analysis z_offset = offsets[2] logging.info("Calculating bed tilt with: %s", positions) params = {"x_adjust": 0.0, "y_adjust": 0.0, "z_adjust": z_offset} + # Perform coordinate descent def adjusted_height(pos, params): x, y, z = pos @@ -203,12 +289,21 @@ def errorfunc(params): total_error += adjusted_height(pos, params) ** 2 return total_error + new_params = mathutil.coordinate_descent( + params.keys(), params, errorfunc + ) + new_params = mathutil.coordinate_descent( params.keys(), params, errorfunc ) # Apply results speed = self.probe_helper.get_lift_speed() logging.info("Calculated bed tilt parameters: %s", new_params) + return new_params + + def apply_adjustments(self, offsets, new_params): + z_offset = offsets[2] + speed = self.probe_helper.get_lift_speed() x_adjust = new_params["x_adjust"] y_adjust = new_params["y_adjust"] z_adjust = ( @@ -221,10 +316,212 @@ def errorfunc(params): x * x_adjust + y * y_adjust + z_adjust for x, y in self.z_positions ] self.z_helper.adjust_steppers(adjustments, speed) + + def probe_finalize(self, offsets, positions): + if self.z_offsets is not None: + positions = [ + [p[0], p[1], p[2] - o] + for (p, o) in zip(positions, self.z_offsets) + ] + new_params = self.perform_coordinate_descent(offsets, positions) + self.apply_adjustments(offsets, new_params) return self.z_status.check_retry_result( self.retry_helper.check_retry([p[2] for p in positions]) ) + def cmd_Z_TILT_CALIBRATE(self, gcmd): + if self.numpy is None: + gcmd.respond_info(self.err_missing_numpy) + return + self.cal_avg_len = gcmd.get_int("AVGLEN", self.cal_conf_avg_len) + self.cal_gcmd = gcmd + self.cal_runs = [] + self.cal_helper.start_probe(gcmd) + + def cal_finalize(self, offsets, positions): + np = self.numpy + avlen = self.cal_avg_len + new_params = self.perform_coordinate_descent(offsets, positions) + self.apply_adjustments(offsets, new_params) + self.cal_runs.append([p[2] for p in positions]) + if len(self.cal_runs) < avlen + 1: + return "retry" + prev_error = np.std(self.cal_runs[-avlen - 1 : -1], axis=0) + prev_error = np.std(prev_error) + this_error = np.std(self.cal_runs[-avlen:], axis=0) + this_error = np.std(this_error) + self.cal_gcmd.respond_info( + "previous error: %.6f current error: %.6f" + % (prev_error, this_error) + ) + if this_error < prev_error: + return "retry" + z_offsets = np.mean(self.cal_runs[-avlen:], axis=0) + z_offsets = [z - offsets[2] for z in z_offsets] + self.z_offsets = z_offsets + s_zoff = "" + for off in z_offsets[0 : self.num_probe_points]: + s_zoff += "%.6f, " % off + s_zoff = s_zoff[:-2] + self.cal_gcmd.respond_info("final z_offsets are: %s" % (s_zoff)) + configfile = self.printer.lookup_object("configfile") + section = self.section + configfile.set(section, "z_offsets", s_zoff) + self.cal_gcmd.respond_info( + "The SAVE_CONFIG command will update the printer config\n" + "file with these parameters and restart the printer." + ) + + def ad_init(self): + self.ad_phase = 0 + self.ad_params = [] + + def cmd_Z_TILT_AUTODETECT(self, gcmd): + if self.numpy is None: + gcmd.respond_info(self.err_missing_numpy) + self.cal_avg_len = gcmd.get_int("AVGLEN", self.cal_conf_avg_len) + self.ad_delta = gcmd.get_float("DELTA", self.ad_conf_delta, minval=0.1) + self.ad_init() + self.ad_gcmd = gcmd + self.ad_runs = [] + self.ad_points = [] + self.ad_error = None + self.ad_helper.start_probe(gcmd) + + ad_adjustments = [ + [0.5, -0.5, -0.5], # p1 up + [-1, 1, 0], # p2 up + [0, -1, 1], # p3 up + [0, 1, 0], # p3 + p2 up + [1, -1, 0], # p3 + p1 up + [0, 1, -1], # p2 + p1 up + [-0.5, -0.5, 0.5], # back to level + ] + + def ad_finalize(self, offsets, positions): + np = self.numpy + avlen = self.cal_avg_len + delta = self.ad_delta + speed = self.probe_helper.get_lift_speed() + new_params = self.perform_coordinate_descent(offsets, positions) + if self.ad_phase in range(1, 4): + new_params["z_adjust"] -= delta / 2 + if self.ad_phase in range(4, 7): + new_params["z_adjust"] += delta / 2 + if self.ad_phase == 0: + self.ad_points.append( + [z for _, _, z in positions[: self.num_probe_points]] + ) + self.ad_params.append(new_params) + adjustments = [_a * delta for _a in self.ad_adjustments[self.ad_phase]] + self.z_helper.adjust_steppers(adjustments, speed) + if self.ad_phase < 6: + self.ad_phase += 1 + return "retry" + # calculcate results + p = [] + for i in range(7): + p.append(params_to_normal_form(np, self.ad_params[i], offsets)) + + # This is how it works. + # To find the pivot point, we take 3 planes: + # a) the original untilted plane + # b) the plane with one motor raised, on one corner opposite the + # one we want to determine the pivot point of + # c) the plane with the other motor opposite the one we want to + # determine the pivot point raised + # The intersection of all 3 planes is a point very near the pivot + # point we search for. If the pivot point would be a point on the + # bed surface, we would already be done. But as the actual pivot + # point is in most cases below the bed, the intersection of the 3 + # points is behind or in front of the actual point (in X/Y). To + # compensate for this error, we do the same calculation again, but + # with the planes b) and c) tilted in the opposite direction and + # take the average of the 2 points. + + z_p1 = ( + intersect_3_planes(np, p[0], p[2], p[3])[:2], + intersect_3_planes(np, p[0], p[1], p[3])[:2], + intersect_3_planes(np, p[0], p[1], p[2])[:2], + ) + + z_p2 = ( + intersect_3_planes(np, p[0], p[5], p[6])[:2], + intersect_3_planes(np, p[0], p[4], p[6])[:2], + intersect_3_planes(np, p[0], p[4], p[5])[:2], + ) + + # take the average of positive and negative measurement + z_pos = [] + for _zp1, _zp2 in zip(z_p1, z_p2): + _z = [] + for _z1, _z2 in zip(_zp1, _zp2): + _z.append((_z1 + _z2) / 2) + z_pos.append(_z) + s_zpos = "" + for zp in z_pos: + s_zpos += "%.6f, %.6f\n" % tuple(zp) + self.ad_gcmd.respond_info("current estimated z_positions %s" % (s_zpos)) + self.ad_runs.append(z_pos) + if len(self.ad_runs) >= avlen: + self.z_positions = np.mean(self.ad_runs[-avlen:], axis=0) + else: + self.z_positions = np.mean(self.ad_runs, axis=0) + + # We got a first estimate of the pivot points. Now apply the + # adjustemts to all motors and repeat the process until the result + # converges. We determine convergence by keeping track of the last + # + 1 runs and compare the standard deviation over that + # len between the last two runs. When the error stops to decrease, we + # are done. The final z_positions are determined by calculating the + # average over the last calculated positions. + + self.apply_adjustments(offsets, self.ad_params[0]) + if len(self.ad_runs) >= avlen: + errors = np.std(self.ad_runs[-avlen:], axis=0) + error = np.std(errors) + if self.ad_error is None: + self.ad_gcmd.respond_info("current error: %.6f" % (error)) + else: + self.ad_gcmd.respond_info( + "previous error: %.6f current error: %.6f" + % (self.ad_error, error) + ) + if self.ad_error is not None: + if error >= self.ad_error: + self.ad_finalize_done(offsets) + return + self.ad_error = error + # restart + self.ad_init() + return "retry" + + def ad_finalize_done(self, offsets): + np = self.numpy + avlen = self.cal_avg_len + # calculate probe point z offsets + z_offsets = np.mean(self.ad_points[-avlen:], axis=0) + z_offsets = [z - offsets[2] for z in z_offsets] + self.z_offsets = z_offsets + logging.info("final z_offsets %s", (z_offsets)) + configfile = self.printer.lookup_object("configfile") + section = self.section + s_zoff = "" + for off in z_offsets: + s_zoff += "%.6f, " % off + s_zoff = s_zoff[:-2] + configfile.set(section, "z_offsets", s_zoff) + s_zpos = "" + for zpos in self.z_positions: + s_zpos += "%.6f, %.6f\n" % tuple(zpos) + configfile.set(section, "z_positions", s_zpos) + self.ad_gcmd.respond_info("final z_positions are %s" % (s_zpos)) + self.ad_gcmd.respond_info("final z_offsets are: %s" % (s_zoff)) + self.ad_gcmd.respond_info( + "The SAVE_CONFIG command will update the printer config\n" + "file with these parameters and restart the printer." + ) + def get_status(self, eventtime): return self.z_status.get_status(eventtime)