From c819db6be2d1e271741dc15d162880de14bad424 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Sat, 11 Oct 2025 02:09:43 +0200 Subject: [PATCH] Removes some buggy cache stuff --- klippy/chelper/kin_winch.c | 98 +++++++++++---------- klippy/kinematics/winch.py | 79 +++++++++++------ test/__init__.py | 1 + test/klippy/__init__.py | 1 + test/klippy/test_winch.py | 172 +++++++++++++++++++++++++++++++++++++ 5 files changed, 277 insertions(+), 74 deletions(-) create mode 100644 test/__init__.py create mode 100644 test/klippy/__init__.py create mode 100644 test/klippy/test_winch.py diff --git a/klippy/chelper/kin_winch.c b/klippy/chelper/kin_winch.c index ad09b6d83..83a77eb49 100644 --- a/klippy/chelper/kin_winch.c +++ b/klippy/chelper/kin_winch.c @@ -8,6 +8,7 @@ #include // sqrt, fabs, fmax, fmin #include // offsetof #include // malloc +#include // malloc #include // memset #include "compiler.h" // __visible #include "itersolve.h" // struct stepper_kinematics @@ -30,17 +31,13 @@ struct winch_flex { double guy_wires[WINCH_MAX_ANCHORS]; double distances_origin[WINCH_MAX_ANCHORS]; double relaxed_origin[WINCH_MAX_ANCHORS]; - double cache_distances[WINCH_MAX_ANCHORS]; - double cache_flex[WINCH_MAX_ANCHORS]; - struct move *cache_move; - double cache_time; - int cache_valid; }; struct winch_stepper { struct stepper_kinematics sk; struct winch_flex *wf; int index; + struct coord anchor; }; static inline double @@ -83,8 +80,9 @@ gauss_jordan_3x5(double M[3][5]) pivot = row; } } - if (max_val < EPSILON) + if (max_val < EPSILON) { return 0; + } if (pivot != col) { for (int c = col; c < 5; ++c) { double tmp = M[col][c]; @@ -158,8 +156,9 @@ static_forces_tetrahedron(struct winch_flex *wf, const struct coord *pos, } M[2][3] += mg; - if (!gauss_jordan_3x5(M)) + if (!gauss_jordan_3x5(M)) { return; + } double A_mg = M[0][3], B_mg = M[1][3], C_mg = M[2][3]; double A_pre = M[0][4], B_pre = M[1][4], C_pre = M[2][4]; @@ -277,8 +276,13 @@ static_forces_quadrilateral(struct winch_flex *wf, const struct coord *pos, lower = fmax(lower, fabs(safe_ratio(wf->target_force - m_vec[i], p[i]))); double upper = fabs(safe_ratio(wf->max_force[4] - top_mg, top_pre)); - for (int i = 0; i < 4; ++i) - upper = fmin(upper, fabs(safe_ratio(wf->max_force[i] - m_vec[i], p[i]))); + for (int i = 0; i < 4; ++i) { + double denom = fabs(p[i]); + double term = denom < EPSILON ? 1e9 + : fabs((wf->max_force[i] - m_vec[i]) / p[i]); + if (term < upper) + upper = term; + } double preFac = fmin(lower, upper); if (!isfinite(preFac)) @@ -293,29 +297,21 @@ static_forces_quadrilateral(struct winch_flex *wf, const struct coord *pos, F[i] = clampd(total[i], wf->min_force[i], wf->max_force[i]); } -static void -static_forces(struct winch_flex *wf, const struct coord *pos, double *F) -{ - memset(F, 0, sizeof(double) * wf->num_anchors); - if (!wf->flex_enabled) - return; - if (wf->num_anchors == 4) - static_forces_tetrahedron(wf, pos, F); - else if (wf->num_anchors == 5) - static_forces_quadrilateral(wf, pos, F); -} - static void compute_flex(struct winch_flex *wf, double x, double y, double z, double *distances, double *flex) { int num = wf->num_anchors; - struct coord pos = { x, y, z }; + struct coord pos; + pos.x = x; + pos.y = y; + pos.z = z; for (int i = 0; i < num; ++i) { double dx = wf->anchors[i].x - x; double dy = wf->anchors[i].y - y; double dz = wf->anchors[i].z - z; - distances[i] = hypot3(dx, dy, dz); + double dist = hypot3(dx, dy, dz); + distances[i] = dist; } if (!wf->flex_enabled) { for (int i = 0; i < num; ++i) @@ -323,7 +319,14 @@ compute_flex(struct winch_flex *wf, double x, double y, double z, return; } double forces[WINCH_MAX_ANCHORS]; - static_forces(wf, &pos, forces); + memset(forces, 0, sizeof(forces)); + if (wf->num_anchors == 4) { + static_forces_tetrahedron(wf, &pos, forces); + } + else if (wf->num_anchors == 5) + static_forces_quadrilateral(wf, &pos, forces); + else + return; for (int i = 0; i < num; ++i) { double spring_length = distances[i] + wf->guy_wires[i]; if (spring_length < EPSILON) @@ -335,29 +338,21 @@ compute_flex(struct winch_flex *wf, double x, double y, double z, flex[i] = line_pos - delta; } } - -static void -update_cache(struct winch_flex *wf, struct move *m, double move_time) -{ - if (!wf || wf->num_anchors <= 0) - return; - if (wf->cache_valid && wf->cache_move == m && wf->cache_time == move_time) - return; - struct coord pos = move_get_coord(m, move_time); - compute_flex(wf, pos.x, pos.y, pos.z, wf->cache_distances, wf->cache_flex); - wf->cache_move = m; - wf->cache_time = move_time; - wf->cache_valid = 1; -} - static double calc_position_common(struct winch_stepper *ws, struct move *m, double move_time) { + struct coord pos = move_get_coord(m, move_time); + double dx = ws->anchor.x - pos.x; + double dy = ws->anchor.y - pos.y; + double dz = ws->anchor.z - pos.z; + double dist = hypot3(dx, dy, dz); struct winch_flex *wf = ws->wf; - if (!wf || ws->index >= wf->num_anchors) - return 0.; - update_cache(wf, m, move_time); - return wf->cache_distances[ws->index] - wf->cache_flex[ws->index]; + if (!wf || ws->index >= wf->num_anchors || !wf->flex_enabled) + return dist; + double distances[WINCH_MAX_ANCHORS]; + double flex[WINCH_MAX_ANCHORS]; + compute_flex(wf, pos.x, pos.y, pos.z, distances, flex); + return distances[ws->index] - flex[ws->index]; } static double @@ -418,9 +413,17 @@ recalc_origin(struct winch_flex *wf) return; } - struct coord origin = {0., 0., 0.}; - double forces[WINCH_MAX_ANCHORS]; - static_forces(wf, &origin, forces); + struct coord origin; + origin.x = 0.; + origin.y = 0.; + origin.z = 0.; + double forces[WINCH_MAX_ANCHORS] = {0.}; + if (num == 4) + static_forces_tetrahedron(wf, &origin, forces); + else if (num == 5) + static_forces_quadrilateral(wf, &origin, forces); + else + return; for (int i = 0; i < num; ++i) { double spring_length = wf->distances_origin[i] + wf->guy_wires[i]; @@ -472,7 +475,6 @@ winch_flex_configure(struct winch_flex *wf, int num_anchors, wf->flex_enabled = (num_anchors >= 4 && mover_weight > 0. && spring_constant > 0.); - wf->cache_valid = 0; recalc_origin(wf); } @@ -494,6 +496,8 @@ winch_stepper_alloc(struct winch_flex *wf, int index) memset(ws, 0, sizeof(*ws)); ws->wf = wf; ws->index = index; + if (wf && index >= 0 && index < wf->num_anchors) + ws->anchor = wf->anchors[index]; ws->sk.calc_position_cb = winch_stepper_calc_position; ws->sk.active_flags = AF_X | AF_Y | AF_Z; return &ws->sk; diff --git a/klippy/kinematics/winch.py b/klippy/kinematics/winch.py index 8bb236e36..96d8ce7a8 100644 --- a/klippy/kinematics/winch.py +++ b/klippy/kinematics/winch.py @@ -7,43 +7,66 @@ import stepper, mathutil, chelper class WinchFlexHelper: def __init__(self, anchors, config): - self.ffi_main, self.ffi_lib = chelper.get_ffi() - self.num = len(anchors) - self.ptr = self.ffi_main.NULL + self._anchors = tuple(anchors) + self.num = len(self._anchors) + self.ptr = None self.enabled = False + self.ffi_main = self.ffi_lib = None + self._read_config(config) if not self.num: return + self.ffi_main, self.ffi_lib = chelper.get_ffi() self.ptr = self.ffi_main.gc( self.ffi_lib.winch_flex_alloc(), self.ffi_lib.winch_flex_free) - self._configure(anchors, config) + self._configure() - def _configure(self, anchors, config): - mover_weight = config.getfloat('winch_mover_weight', 0., minval=0.) - spring_constant = config.getfloat('winch_spring_constant', 0., minval=0.) - target_force = config.getfloat('winch_target_force', 0., minval=0.) - min_force = list(config.getfloatlist( - 'winch_min_force', [0.] * self.num, count=self.num)) - max_force = list(config.getfloatlist( - 'winch_max_force', [120.] * self.num, count=self.num)) - guy_raw = config.get('winch_guy_wire_lengths', None, note_valid=False) - guy_valid = 0 - guy_ptr = self.ffi_main.NULL - if guy_raw is not None: - guy_vals = list(config.getfloatlist( - 'winch_guy_wire_lengths', count=self.num)) - guy_ptr = self.ffi_main.new("double[]", guy_vals) - guy_valid = 1 - anchors_flat = [float(coord) for anchor in anchors for coord in anchor] + def _read_config(self, config): + self.mover_weight = config.getfloat('winch_mover_weight', 0., minval=0.) + self.spring_constant = config.getfloat('winch_spring_constant', 0., minval=0.) + self.target_force = config.getfloat('winch_target_force', 0., minval=0.) + if self.num: + default_min = tuple(0. for _ in range(self.num)) + default_max = tuple(120. for _ in range(self.num)) + self.min_force = list(config.getfloatlist( + 'winch_min_force', default_min, count=self.num)) + self.max_force = list(config.getfloatlist( + 'winch_max_force', default_max, count=self.num)) + guy_raw = config.get('winch_guy_wire_lengths', None, note_valid=False) + self.guy_wires = [] + self.guy_wires_valid = 0 + if guy_raw is not None: + self.guy_wires = list(config.getfloatlist( + 'winch_guy_wire_lengths', count=self.num)) + self.guy_wires_valid = 1 + else: + self.min_force = [] + self.max_force = [] + self.guy_wires = [] + self.guy_wires_valid = 0 + self.enabled = bool(self.num >= 4 + and self.mover_weight > 0. + and self.spring_constant > 0.) + + def _configure(self): + anchors_flat = [float(coord) for anchor in self._anchors for coord in anchor] anchors_c = self.ffi_main.new("double[]", anchors_flat) - min_c = self.ffi_main.new("double[]", min_force) - max_c = self.ffi_main.new("double[]", max_force) + min_c = self.ffi_main.new("double[]", self.min_force) + max_c = self.ffi_main.new("double[]", self.max_force) + if self.guy_wires_valid: + guy_ptr = self.ffi_main.new("double[]", self.guy_wires) + else: + guy_ptr = self.ffi_main.NULL self.ffi_lib.winch_flex_configure( - self.ptr, self.num, anchors_c, mover_weight, spring_constant, - target_force, min_c, max_c, guy_ptr, guy_valid) - self.enabled = bool(self.num >= 4 and mover_weight > 0. and spring_constant > 0.) + self.ptr, self.num, anchors_c, self.mover_weight, + self.spring_constant, self.target_force, min_c, max_c, + guy_ptr, self.guy_wires_valid) def get_ptr(self): - return self.ptr + if self.ptr is not None: + return self.ptr + if self.ffi_main is None: + self.ffi_main, _ = chelper.get_ffi() + return self.ffi_main.NULL def calc_arrays(self, pos): if not self.ptr or not self.num: @@ -71,6 +94,8 @@ class WinchKinematics: self.anchors.append(a) self.flex_helper = WinchFlexHelper(self.anchors, config) flex_ptr = self.flex_helper.get_ptr() + if not flex_ptr: + raise config.error("Failed to initialise winch flex helper") for idx, s in enumerate(self.steppers): s.setup_itersolve('winch_stepper_alloc', flex_ptr, idx) s.set_trapq(toolhead.get_trapq()) diff --git a/test/__init__.py b/test/__init__.py new file mode 100644 index 000000000..9ffab18f3 --- /dev/null +++ b/test/__init__.py @@ -0,0 +1 @@ +# Enable Python package imports for test modules diff --git a/test/klippy/__init__.py b/test/klippy/__init__.py new file mode 100644 index 000000000..30367eb9d --- /dev/null +++ b/test/klippy/__init__.py @@ -0,0 +1 @@ +# Package marker for Klippy unit tests diff --git a/test/klippy/test_winch.py b/test/klippy/test_winch.py new file mode 100644 index 000000000..ef7b7207b --- /dev/null +++ b/test/klippy/test_winch.py @@ -0,0 +1,172 @@ +import os +import sys +import math +import unittest +import configparser + +HERE = os.path.abspath(__file__) +TEST_DIR = os.path.dirname(HERE) +ROOT = os.path.dirname(TEST_DIR) +REPO_ROOT = os.path.dirname(ROOT) +KLIPPY_ROOT = os.path.join(REPO_ROOT, 'klippy') +if KLIPPY_ROOT not in sys.path: + sys.path.insert(0, KLIPPY_ROOT) + +import configfile # noqa: E402 +import chelper # noqa: E402 +from kinematics.winch import WinchFlexHelper # noqa: E402 + + +ffi, lib = chelper.get_ffi() + + +def list_str(values): + return ', '.join(f"{v:.6f}" for v in values) + + +class DummyPrinter: + def lookup_object(self, name, default=None): + if default is not None: + return default + raise configfile.error("Unknown object '%s'" % (name,)) + + +FOUR_ANCHORS_DEFAULT = ( + (16.4, -1610.98, -131.53), + (1314.22, 1268.14, -121.28), + (-1415.73, 707.61, -121.82), + (0.0, 0.0, 2299.83), +) + +FOUR_ANCHORS_PROBLEM = ( + (-1000.0, 1000.0, -121.28), + (500.0, -500.0, -131.53), + (-1415.73, 707.61, -121.82), + (0.0, 0.0, 2299.83), +) + +FIVE_ANCHORS_DEFAULT = ( + (16.4, -1610.98, -131.53), + (1314.22, 128.14, -121.28), + (-15.73, 1415.61, -121.82), + (-1211.62, 18.14, -111.18), + (10.0, -10.0, 2299.83), +) + + +class WinchFlexHelperTests(unittest.TestCase): + def build_helper(self, anchors, mover_weight=2.0, spring=20000.0, + target=20.0, min_force=None, max_force=None, + guy_wires=None): + num = len(anchors) + if min_force is None: + min_force = [0.0] * num + if max_force is None: + max_force = [120.0] * num + cfg = configparser.ConfigParser() + options = { + 'winch_mover_weight': f"{mover_weight}", + 'winch_spring_constant': f"{spring}", + 'winch_target_force': f"{target}", + 'winch_min_force': list_str(min_force), + 'winch_max_force': list_str(max_force), + } + if guy_wires is not None: + options['winch_guy_wire_lengths'] = list_str(guy_wires) + cfg['printer'] = options + wrapper = configfile.ConfigWrapper(DummyPrinter(), cfg, {}, 'printer') + return WinchFlexHelper(tuple(anchors), wrapper) + + def spool_lengths(self, helper, pos): + distances, flex = helper.calc_arrays(pos) + lengths = [d + f for d, f in zip(distances, flex)] + return lengths, distances, flex + + def test_disabled_flex_matches_geometry(self): + helper = self.build_helper(FOUR_ANCHORS_DEFAULT, + mover_weight=0.0, + spring=0.0, + target=0.0) + self.assertFalse(helper.enabled) + ptr = helper.get_ptr() + self.assertTrue(ptr) + stepper = ffi.gc(lib.winch_stepper_alloc(ptr, 0), lib.free) + dist = lib.itersolve_calc_position_from_coord(stepper, 0.0, 0.0, 0.0) + expected = math.sqrt(sum(v * v for v in FOUR_ANCHORS_DEFAULT[0])) + self.assertAlmostEqual(dist, expected, places=6) + for pos in ((0.0, 0.0, 0.0), (100.0, -50.0, 10.0), (-200.0, 150.0, 75.0)): + lengths, distances, flex = self.spool_lengths(helper, pos) + for d, f, l in zip(distances, flex, lengths): + self.assertAlmostEqual(f, 0.0, places=9) + self.assertAlmostEqual(l, d, places=9) + + def test_origin_with_flex_applies_no_offset(self): + helper = self.build_helper(FOUR_ANCHORS_DEFAULT) + self.assertTrue(helper.enabled) + lengths, distances, flex = self.spool_lengths(helper, (0.0, 0.0, 0.0)) + for d, f, l in zip(distances, flex, lengths): + self.assertAlmostEqual(f, 0.0, places=9) + self.assertAlmostEqual(l, d, places=9) + + def test_four_anchor_small_perturbation(self): + helper = self.build_helper(FOUR_ANCHORS_DEFAULT) + origin_lengths, _, _ = self.spool_lengths(helper, (0.0, 0.0, 0.0)) + + def rel(lengths): + return [l - o for l, o in zip(lengths, origin_lengths)] + + left = rel(self.spool_lengths(helper, (16.3, 0.0, 0.0))[0]) + nominal = rel(self.spool_lengths(helper, (16.4, 0.0, 0.0))[0]) + right = rel(self.spool_lengths(helper, (16.5, 0.0, 0.0))[0]) + + for i in range(4): + self.assertLess(abs(left[i] - nominal[i]), 0.2) + self.assertLess(abs(right[i] - nominal[i]), 0.2) + self.assertLess(abs(left[i] - right[i]), 0.2) + for i in range(3): + self.assertGreater(abs(left[i] - nominal[i]), 0.0001) + self.assertGreater(abs(right[i] - nominal[i]), 0.0001) + self.assertGreater(abs(left[i] - right[i]), 0.0001) + + def test_four_anchor_near_singularity(self): + helper = self.build_helper(FOUR_ANCHORS_PROBLEM) + origin_lengths, _, _ = self.spool_lengths(helper, (0.0, 0.0, 0.0)) + + def rel(lengths): + return [l - o for l, o in zip(lengths, origin_lengths)] + + left = rel(self.spool_lengths(helper, (-0.1, 0.0, 0.0))[0]) + nominal = [0.0] * len(origin_lengths) + right = rel(self.spool_lengths(helper, (0.1, 0.0, 0.0))[0]) + + for i in range(4): + self.assertLess(abs(left[i] - nominal[i]), 10.0) + self.assertLess(abs(right[i] - nominal[i]), 10.0) + self.assertLess(abs(left[i] - right[i]), 10.0) + for i in range(3): + self.assertGreater(abs(left[i] - nominal[i]), 0.0001) + self.assertGreater(abs(right[i] - nominal[i]), 0.0001) + self.assertGreater(abs(left[i] - right[i]), 0.0001) + + def test_five_anchor_mass_zero(self): + helper = self.build_helper(FIVE_ANCHORS_DEFAULT, + mover_weight=0.0, + spring=0.0, + target=0.0) + self.assertFalse(helper.enabled) + for pos in ((0.0, 0.0, 0.0), (100.0, -50.0, 10.0)): + lengths, distances, flex = self.spool_lengths(helper, pos) + for d, f, l in zip(distances, flex, lengths): + self.assertAlmostEqual(f, 0.0, places=9) + self.assertAlmostEqual(l, d, places=9) + + def test_five_anchor_origin(self): + helper = self.build_helper(FIVE_ANCHORS_DEFAULT) + lengths, distances, flex = self.spool_lengths(helper, (0.0, 0.0, 0.0)) + for d, f, l in zip(distances, flex, lengths): + self.assertAlmostEqual(f, 0.0, places=9) + self.assertAlmostEqual(l, d, places=9) + + +if __name__ == '__main__': + unittest.main()