generic_cartesian: Added SET_STEPPER_KINEMATICS command

The new command allows to alter the behavior of the steppers at run
time. Option DISABLE_CHECKS=1 allows advanced and potentially unsafe
modifications of stepper kinematics, e.g. disabling the stepper, or
configuring incomplete motion kinematics, which can be useful for
advanced fully custom homing procedures. The latter required
implementing computation of the kinematics matrix inverse via SVD.

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

klippy/chelper/__init__.py

@@ -109,6 +109,8 @@
 defs_kin_generic_cartesian = """
     struct stepper_kinematics *generic_cartesian_stepper_alloc(double a_x
         , double a_y, double a_z);
+    void generic_cartesian_stepper_set_coeffs(struct stepper_kinematics *sk
+        , double a_x, double a_y, double a_z);
 """
 
 defs_kin_corexy = """

klippy/chelper/kin_generic.c

@@ -26,18 +26,27 @@ generic_cartesian_stepper_calc_position(struct stepper_kinematics *sk
     return cs->a.x * c.x + cs->a.y * c.y + cs->a.z * c.z;
 }
 
-struct stepper_kinematics * __visible
-generic_cartesian_stepper_alloc(double a_x, double a_y, double a_z)
+void __visible
+generic_cartesian_stepper_set_coeffs(struct stepper_kinematics *sk
+                                     , double a_x, double a_y, double a_z)
 {
-    struct generic_cartesian_stepper *cs = malloc(sizeof(*cs));
-    memset(cs, 0, sizeof(*cs));
+    struct generic_cartesian_stepper *cs = container_of(
+            sk, struct generic_cartesian_stepper, sk);
     cs->a.x = a_x;
     cs->a.y = a_y;
     cs->a.z = a_z;
-    cs->sk.calc_position_cb = generic_cartesian_stepper_calc_position;
     cs->sk.active_flags = 0;
     if (a_x) cs->sk.active_flags |= AF_X;
     if (a_y) cs->sk.active_flags |= AF_Y;
     if (a_z) cs->sk.active_flags |= AF_Z;
+}
+
+struct stepper_kinematics * __visible
+generic_cartesian_stepper_alloc(double a_x, double a_y, double a_z)
+{
+    struct generic_cartesian_stepper *cs = malloc(sizeof(*cs));
+    memset(cs, 0, sizeof(*cs));
+    cs->sk.calc_position_cb = generic_cartesian_stepper_calc_position;
+    generic_cartesian_stepper_set_coeffs(&cs->sk, a_x, a_y, a_z);
     return &cs->sk;
 }

klippy/extras/homing.py

@@ -64,7 +64,9 @@ def calc_toolhead_pos(self, kin_spos, offsets):
             sname = stepper.get_name()
             kin_spos[sname] += offsets.get(sname, 0) * stepper.get_step_dist()
         thpos = self.toolhead.get_position()
-        return list(kin.calc_position(kin_spos))[:3] + thpos[3:]
+        cpos = kin.calc_position(kin_spos)
+        return [cp if cp is not None else tp
+                for cp, tp in zip(cpos, thpos[:3])] + thpos[3:]
     def homing_move(self, movepos, speed, probe_pos=False,
                     triggered=True, check_triggered=True):
         # Notify start of homing/probing move
@@ -222,6 +224,10 @@ def home_rails(self, rails, forcepos, movepos):
                         for s in kin.get_steppers()}
             newpos = kin.calc_position(kin_spos)
             for axis in homing_axes:
+                if newpos[axis] is None:
+                    raise self.printer.command_error(
+                            "Cannot determine position of toolhead on "
+                            "axis %s after homing" % "xyz"[axis])
                 homepos[axis] = newpos[axis]
             self.toolhead.set_position(homepos)
 

klippy/extras/kinematic_stepper.py

@@ -7,8 +7,7 @@
 import logging, re
 import stepper, chelper
 
-def parse_kinematic_string(config, carriages):
-    kinematics_str = config.get('kinematics')
+def parse_kinematic_string(kinematics_str, carriages, parse_error):
     nxt = 0
     pat = re.compile('[+-]')
     coeffs = [0.] * 3
@@ -17,9 +16,14 @@ def parse_kinematic_string(config, carriages):
         match = pat.search(kinematics_str, nxt+1)
         end = len(kinematics_str) if match is None else match.start()
         term = kinematics_str[nxt:end].strip()
-        term_lst = term.split("*")
+        if '*' in term:
+            term_lst = term.split('*')
+        else:
+            term_lst = re.split(r"^([-+]?(?:\d*\.*\d+))", term)
+            if not term_lst[0]:
+                del term_lst[0]
         if len(term_lst) not in [1, 2]:
-            raise config.error(
+            raise parse_error(
                     "Invalid term '%s' in kinematics '%s'" % (term,
                                                               kinematics_str))
         if len(term_lst) == 2:
@@ -31,11 +35,12 @@ def parse_kinematic_string(config, carriages):
             coeff = -1. if term_lst[0].startswith('-') else 1.
             if term_lst[0].startswith('-') or term_lst[0].startswith('+'):
                 term_lst[0] = term_lst[0][1:]
-        if term_lst[-1] not in carriages:
-            raise config.error(
+        c = term_lst[-1]
+        if c not in carriages:
+            raise parse_error(
                     "Invalid '%s' carriage referenced in kinematics '%s'" % (
-                        term_lst[-1], kinematics_str))
-        carriage = carriages[term_lst[-1]]
+                        c, kinematics_str))
+        carriage = carriages[c]
         j = carriage.get_axis()
         if coeffs[j]:
             raise error("Axis '%s' was referenced multiple times in "
@@ -50,8 +55,8 @@ def __init__(self, config, carriages):
         self.printer = config.get_printer()
         self.config = config
         self.stepper = stepper.PrinterStepper(config)
-        self.kin_coeffs, self.carriages = parse_kinematic_string(config,
-                                                                 carriages)
+        self.kin_coeffs, self.carriages = parse_kinematic_string(
+                config.get('kinematics'), carriages, config.error)
         if not any(self.kin_coeffs):
             raise config.error(
                     "'%s' must provide a valid 'kinematics' configuration" %
@@ -60,6 +65,8 @@ def __init__(self, config, carriages):
         self.stepper.setup_itersolve(
                 'generic_cartesian_stepper_alloc',
                 self.kin_coeffs[0], self.kin_coeffs[1], self.kin_coeffs[2])
+        self.stepper_sk = self.stepper.get_stepper_kinematics()
+        # Add stepper methods
         self.get_name = self.stepper.get_name
         self.get_step_dist = self.stepper.get_step_dist
         self.units_in_radians = self.stepper.units_in_radians
@@ -94,3 +101,16 @@ def get_active_axes(self):
         return [i for i, c in enumerate(self.kin_coeffs) if c]
     def get_carriages(self):
         return self.carriages
+    def update_kinematics(self, kinematics_str, carriages, report_error=None):
+        kin_coeffs, carriages = parse_kinematic_string(
+                kinematics_str, carriages,
+                report_error or self.printer.command_error)
+        if report_error is not None and not any(kin_coeffs):
+            raise report_error(
+                    "A valid 'kinematics' that references at least one carriage"
+                    " must be provided for '%s'" % self.stepper.get_name())
+        self.kin_coeffs = kin_coeffs
+        self.carriages = carriages
+        ffi_main, ffi_lib = chelper.get_ffi()
+        ffi_lib.generic_cartesian_stepper_set_coeffs(
+                self.stepper_sk, kin_coeffs[0], kin_coeffs[1], kin_coeffs[2])

klippy/kinematics/generic_cartesian.py

@@ -4,7 +4,7 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 
-import copy, itertools, logging
+import copy, itertools, logging, math
 import gcode, mathutil, stepper
 import extras.kinematic_stepper as ks
 from . import idex_modes
@@ -25,6 +25,77 @@ def mat_transp(a):
         res.append([a[j][i] for j in range(len(a))])
     return res
 
+def find_eigenvectors(m, ek):
+    res = []
+    for i in range(3):
+        if abs(m[i][i]) < 1e-8:
+            for j in range(i+1, 3):
+                if abs(m[j][i]) > 1e-8:
+                    m[i], m[j] = m[j], m[i]
+                    break
+        if abs(m[i][i]) < 1e-8:
+            vr = [-m[k][i] for k in range(3)]
+            vr[i] = 1.
+            nrm_recipr = 1. / math.sqrt(mathutil.matrix_magsq(vr))
+            res.append([x * nrm_recipr for x in vr])
+            if len(res) == ek:
+                return res
+            continue
+        recipr = 1. / m[i][i]
+        for j in range(i+1, 3):
+            m[i][j] *= recipr
+        m[i][i] = 1.
+        for j in range(3):
+            if i != j:
+                c = m[j][i]
+                for k in range(i, 3):
+                    m[j][k] -= c * m[i][k]
+
+def mat_eigen(mtm):
+    a, d, f = mtm[0]
+    b, e = mtm[1][1:]
+    c = mtm[2][2]
+    x1 = a**2 + b**2 + c**2 - a*b - a*c - b*c + 3. * (d**2 + f**2 + e**2)
+    x2 = -(2. * a - b - c) * (2. * b - a - c) * (2. * c - a - b) \
+            + 9. * ((2. * c - a - b) * d**2 + (2. * b - a - c) * f**2 \
+            + (2. * a - b - c) * e**2) - 54. * d * e * f
+    phi = math.atan2(math.sqrt(4. * x1**3 - x2**2), x2)
+    sqrt_x1 = math.sqrt(x1)
+    # Closed-form expressions for eigenvalues of a symmetric matrix
+    l = [li / 3. if abs(li) > 1e-8 else 0. for li in
+         [a + b + c - 2. * sqrt_x1 * math.cos(phi / 3.),
+          a + b + c + 2. * sqrt_x1 * math.cos((phi - math.pi) / 3.),
+          a + b + c + 2. * sqrt_x1 * math.cos((phi + math.pi) / 3.)]]
+    l.sort(reverse=True)
+    # Count different eigenvalues
+    lc = {l[0]: 1}
+    j = 0
+    for i in range(1, 3):
+        if abs(l[i]-l[j]) < 1e-8:
+            lc[l[j]] += 1
+        else:
+            j = i
+            lc[l[j]] = 1
+    v = []
+    # Find eigenvector(s) for each eigenvalue and its multiplicity
+    for li in sorted(lc.keys(), reverse=True):
+        mc = copy.deepcopy(mtm)
+        for j in range(3):
+            mc[j][j] -= li
+        v.extend(find_eigenvectors(mc, lc[li]))
+    return l, mat_transp(v)
+
+def mat_pseudo_inverse(m):
+    mtm = mat_mul(mat_transp(m), m)
+    l, v = mat_eigen(mtm)
+    # Compute matrix SVD and S pseudo-inverse
+    s = [[0.]*3 for i in range(3)]
+    for i in range(3):
+        s[i][i] = 1. / math.sqrt(l[i]) if l[i] else 0.
+    u = mat_mul(m, mat_mul(v, s))
+    pinv = mat_mul(v, mat_mul(s, mat_transp(u)))
+    return pinv
+
 class MainCarriage(stepper.GenericPrinterCarriage):
     def __init__(self, config, axis):
         stepper.GenericPrinterCarriage.__init__(self, config)
@@ -55,6 +126,8 @@ def get_axis(self):
         return self.primary_carriage.get_axis()
     def add_stepper(self, stepper):
         self.primary_carriage.add_stepper(stepper, self.endstop_pin, self.name)
+    def del_stepper(self, stepper):
+        self.primary_carriage.del_stepper(stepper)
 
 class DualCarriage(stepper.GenericPrinterCarriage):
     def __init__(self, config, carriages):
@@ -102,13 +175,13 @@ def __init__(self, toolhead, config):
                     dcs = self.dc_module.get_dc()
                     dcs[active_dc].activate(axis, idex_modes.PRIMARY, zero_pos)
                     dcs[1-active_dc].inactivate(axis, zero_pos)
-                self._check_kinematics()
+                self._check_kinematics(config.error)
             for axis in axes:
                 self.dc_module.get_dc()[0].activate(axis, idex_modes.PRIMARY,
                                                     zero_pos)
                 self.dc_module.get_dc()[1].inactivate(axis, zero_pos)
         else:
-            self._check_kinematics()
+            self._check_kinematics(config.error)
         self.printer.register_event_handler("stepper_enable:motor_off",
                                             self._motor_off)
         # Setup boundary checks
@@ -118,6 +191,11 @@ def __init__(self, toolhead, config):
         self.max_z_accel = config.getfloat('max_z_accel', max_accel,
                                            above=0., maxval=max_accel)
         self.limits = [(1.0, -1.0)] * 3
+        # Register gcode commands
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_command("SET_STEPPER_KINEMATICS",
+                               self.cmd_SET_STEPPER_KINEMATICS,
+                               desc=self.cmd_SET_STEPPER_KINEMATICS_help)
     def _load_kinematics(self, config):
         carriages = {a : MainCarriage(config.getsection('carriage ' + a), a)
                      for a in 'xyz'}
@@ -142,11 +220,15 @@ def _load_kinematics(self, config):
                         "Redefinition of carriage %s" % ec.get_name())
             carriages[name] = ec
         self.steppers = self._load_steppers(config, carriages)
+        self.all_carriages = carriages
+        self._check_carriages_references(config.error)
+    def _check_carriages_references(self, report_error):
+        carriages = dict(self.all_carriages)
         for s in self.steppers:
             for c in s.get_carriages():
                 carriages.pop(c.get_name(), None)
         if carriages:
-            raise config.error(
+            raise report_error(
                     "Carriage(s) %s must be referenced by some "
                     "stepper(s) kinematics" % (", ".join(carriages),))
     def _load_steppers(self, config, carriages):
@@ -178,20 +260,22 @@ def _get_kinematics_coeffs(self):
         return [matr[s.get_name()] for s in self.steppers], \
                 [mathutil.matrix_dot(orig_matr[s.get_name()],
                                      offs[s.get_name()]) for s in self.steppers]
-    def _check_kinematics(self):
+    def _check_kinematics(self, report_error):
         matr, _ = self._get_kinematics_coeffs()
         det = mathutil.matrix_det(mat_mul(mat_transp(matr), matr))
         if abs(det) < 0.00001:
-            raise self.printer.config_error(
+            raise report_error(
                     "Verify configured stepper(s) and their 'kinematics' "
                     "specifications, the current configuration does not "
                     "allow independent movements of all printer axes.")
     def calc_position(self, stepper_positions):
         matr, offs = self._get_kinematics_coeffs()
-        inv = mathutil.matrix_inv(mat_mul(mat_transp(matr), matr))
-        proj = mat_mul(matr, inv)
         spos = [stepper_positions[s.get_name()] for s in self.steppers]
-        pos = mat_mul([[sp-o for sp, o in zip(spos, offs)]], proj)
+        pinv = mat_pseudo_inverse(matr)
+        pos = mat_mul([[sp-o for sp, o in zip(spos, offs)]], mat_transp(pinv))
+        for i in range(3):
+            if not any(pinv[i]):
+                pos[0][i] = None
         return pos[0]
     def update_limits(self, i, range):
         l, h = self.limits[i]
@@ -262,7 +346,7 @@ def get_status(self, eventtime):
         axes_min = gcode.Coord(*[r[0] for r in ranges], e=0.)
         axes_max = gcode.Coord(*[r[1] for r in ranges], e=0.)
         A = self._get_kinematics_coeffs()[0]
-        P = mat_mul(A, mathutil.matrix_inv(mat_mul(mat_transp(A), A)))
+        P = mat_transp(mat_pseudo_inverse(A))
         return {
             'homed_axes': "".join(axes),
             'axis_minimum': axes_min,
@@ -272,6 +356,51 @@ def get_status(self, eventtime):
             'toolhead_kinematics': {s.get_name() : P[i]
                                     for i, s in enumerate(self.steppers)},
         }
+    cmd_SET_STEPPER_KINEMATICS_help = "Set stepper kinematics"
+    def cmd_SET_STEPPER_KINEMATICS(self, gcmd):
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.flush_step_generation()
+        stepper_name = gcmd.get("STEPPER")
+        steppers = [stepper for stepper in self.steppers
+                    if stepper.get_name() == stepper_name]
+        if len(steppers) == 0:
+            raise gcmd.error("Invalid STEPPER '%s' specified" % stepper_name)
+        stepper = steppers[0]
+        kinematics_str = gcmd.get("KINEMATICS").lower()
+        validate = not gcmd.get_int("DISABLE_CHECKS", 0)
+        old_carriages = stepper.get_carriages()
+        stepper.update_kinematics(kinematics_str, self.all_carriages,
+                                  report_error=gcmd.error if validate else None)
+        new_carriages = stepper.get_carriages()
+        for c in old_carriages:
+            if c not in new_carriages:
+                c.del_stepper(stepper)
+        for c in new_carriages:
+            if c not in old_carriages:
+                c.add_stepper(stepper)
+        if not new_carriages:
+            stepper.set_trapq(None)
+        elif not old_carriages:
+            stepper.set_trapq(toolhead.get_trapq())
+        pos = toolhead.get_position()
+        stepper.set_position(pos)
+        if not validate:
+            return
+        self._check_carriages_references(gcmd.error)
+        if self.dc_module:
+            dc_state = self.dc_module.save_dual_carriage_state()
+            axes = [dc.get_axis() for dc in self.dc_carriages]
+            for active_dc_per_axis in itertools.product(*[(0, 1)]*len(axes)):
+                pos = toolhead.get_position()
+                for i, axis in enumerate(axes):
+                    active_dc = active_dc_per_axis[i]
+                    dcs = self.dc_module.get_dc()
+                    dcs[active_dc].activate(axis, idex_modes.PRIMARY, pos)
+                    dcs[1-active_dc].inactivate(axis, pos)
+                self._check_kinematics(gcmd.error)
+            self.dc_module.restore_dual_carriage_state(dc_state, move=0)
+        else:
+            self._check_kinematics(gcmd.error)
 
 def load_kinematics(toolhead, config):
     return GenericCartesianKinematics(toolhead, config)