3D-printing/klipper
1
0
Fork 0
mirror of https://github.com/Klipper3d/klipper.git synced 2026-01-18 05:45:44 -07:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Removes some buggy cache stuff

Browse source
This commit is contained in:
Torbjørn Ludvigsen 2025-10-11 02:09:43 +02:00
parent bc9e5b2a0c
commit c819db6be2
5 changed files with 277 additions and 74 deletions
Download patch file Download diff file Expand all files Collapse all files

98
klippy/chelper/kin_winch.c
View file

@ -8,6 +8,7 @@
#include <math.h> // sqrt, fabs, fmax, fmin
#include <stddef.h> // offsetof
#include <stdlib.h> // malloc
#include <stdio.h> // malloc
#include <string.h> // 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;

79
klippy/kinematics/winch.py
View file

@ -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())

1
test/__init__.py Normal file
View file

@ -0,0 +1 @@
# Enable Python package imports for test modules

1
test/klippy/__init__.py Normal file
View file

@ -0,0 +1 @@
# Package marker for Klippy unit tests

172
test/klippy/test_winch.py Normal file
View file

@ -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()