kinematics: Generic Cartesian kinematics implementation (#6815)

* tests: Added a regression test for generic_cartesian kinematics

* kinematics: An intial implementation of generic_cartesian kinematics

* generic_cartesian: Refactored kinematics configuration API

* generic_cartesian: Use stepper instead of kinematic_stepper in configs

* generic_cartesian: Added SET_STEPPER_KINEMATICS command

* generic_cartesian: Fixed parsing of section names

* docs: Generic Caretsian kinematics documentation and config samples

* generic_cartesian: Implemented multi-mcu homing validation

* generic_cartesian: Fixed typos in docs, minor fixes

* generic_cartesian: Renamed `kinematics` option to `carriages`

* generic_cartesian: Moved kinematic_stepper.py file

* idex_modes: Internal refactoring of handling dual carriages

* stepper: Refactored the code to not store a reference to config object

* config: Updated example-generic-cartesian config

* generic_cartesian: Restricted SET_STEPPER_CARRIAGES and exported status

* idex_modes: Fixed handling stepper kinematics with input shaper enabled

* config: Updated configs and tests for SET_DUAL_CARRIAGE new params

* generic_cartesian: Avoid inheritance in the added classes

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

klippy/chelper/__init__.py

@@ -21,7 +21,7 @@ SOURCE_FILES = [
     'pollreactor.c', 'msgblock.c', 'trdispatch.c',
     'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c',
     'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c',
-    'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c',
+    'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', 'kin_generic.c'
 ]
 DEST_LIB = "c_helper.so"
 OTHER_FILES = [
@@ -106,6 +106,12 @@ defs_trapq = """
 defs_kin_cartesian = """
     struct stepper_kinematics *cartesian_stepper_alloc(char axis);
 """
+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 = """
     struct stepper_kinematics *corexy_stepper_alloc(char type);
@@ -224,6 +230,7 @@ defs_all = [
     defs_kin_cartesian, defs_kin_corexy, defs_kin_corexz, defs_kin_delta,
     defs_kin_deltesian, defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch,
     defs_kin_extruder, defs_kin_shaper, defs_kin_idex,
+    defs_kin_generic_cartesian,
 ]
 
 # Update filenames to an absolute path

klippy/chelper/kin_generic.c

@@ -0,0 +1,52 @@
+// Generic cartesian kinematics stepper position calculation
+//
+// Copyright (C) 2024  Dmitry Butyugin <dmbutyugin@google.com>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // move_get_coord
+
+struct generic_cartesian_stepper {
+    struct stepper_kinematics sk;
+    struct coord a;
+};
+
+static double
+generic_cartesian_stepper_calc_position(struct stepper_kinematics *sk
+                                        , struct move *m, double move_time)
+{
+    struct generic_cartesian_stepper *cs = container_of(
+            sk, struct generic_cartesian_stepper, sk);
+    struct coord c = move_get_coord(m, move_time);
+    return cs->a.x * c.x + cs->a.y * c.y + cs->a.z * c.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 = 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.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/chelper/kin_idex.c

@@ -77,5 +77,6 @@ dual_carriage_alloc(void)
     struct dual_carriage_stepper *dc = malloc(sizeof(*dc));
     memset(dc, 0, sizeof(*dc));
     dc->m.move_t = 2. * DUMMY_T;
+    dc->x_scale = dc->y_scale = 1.0;
     return &dc->sk;
 }

klippy/extras/endstop_phase.py

@@ -52,7 +52,7 @@ class PhaseCalc:
 class EndstopPhase:
     def __init__(self, config):
         self.printer = config.get_printer()
-        self.name = config.get_name().split()[1]
+        self.name = " ".join(config.get_name().split()[1:])
         # Obtain step_distance and microsteps from stepper config section
         sconfig = config.getsection(self.name)
         rotation_dist, steps_per_rotation = stepper.parse_step_distance(sconfig)
@@ -118,7 +118,7 @@ class EndstopPhase:
         return delta * self.step_dist
     def handle_home_rails_end(self, homing_state, rails):
         for rail in rails:
-            stepper = rail.get_steppers()[0]
+            stepper = rail.get_endstops()[0][0].get_steppers()[0]
             if stepper.get_name() == self.name:
                 trig_mcu_pos = homing_state.get_trigger_position(self.name)
                 align = self.align_endstop(rail)

klippy/extras/homing.py

@@ -45,7 +45,7 @@ class StepperPosition:
 class HomingMove:
     def __init__(self, printer, endstops, toolhead=None):
         self.printer = printer
-        self.endstops = endstops
+        self.endstops = [es for es in endstops if es[0].get_steppers()]
         if toolhead is None:
             toolhead = printer.lookup_object('toolhead')
         self.toolhead = toolhead
@@ -71,7 +71,9 @@ class HomingMove:
             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
@@ -233,6 +235,10 @@ class Homing:
                         for s in kin.get_steppers()}
             newpos = kin.calc_position(kin_spos)
             for axis in force_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/manual_stepper.py

@@ -11,7 +11,7 @@ class ManualStepper:
         self.printer = config.get_printer()
         if config.get('endstop_pin', None) is not None:
             self.can_home = True
-            self.rail = stepper.PrinterRail(
+            self.rail = stepper.LookupRail(
                 config, need_position_minmax=False, default_position_endstop=0.)
             self.steppers = self.rail.get_steppers()
         else:

klippy/extras/probe.py

@@ -177,6 +177,9 @@ def lookup_minimum_z(config):
     if config.has_section('stepper_z'):
         zconfig = config.getsection('stepper_z')
         return zconfig.getfloat('position_min', 0., note_valid=False)
+    elif config.has_section('carriage z'):
+        zconfig = config.getsection('carriage z')
+        return zconfig.getfloat('position_min', 0., note_valid=False)
     pconfig = config.getsection('printer')
     return pconfig.getfloat('minimum_z_position', 0., note_valid=False)
 

klippy/kinematics/cartesian.py

@@ -29,14 +29,10 @@ class CartKinematics:
             self.rails.append(stepper.LookupMultiRail(dc_config))
             self.rails[3].setup_itersolve('cartesian_stepper_alloc',
                                           dc_axis.encode())
-            dc_rail_0 = idex_modes.DualCarriagesRail(
-                    self.rails[self.dual_carriage_axis],
-                    axis=self.dual_carriage_axis, active=True)
-            dc_rail_1 = idex_modes.DualCarriagesRail(
-                    self.rails[3], axis=self.dual_carriage_axis, active=False)
             self.dc_module = idex_modes.DualCarriages(
-                    dc_config, dc_rail_0, dc_rail_1,
-                    axis=self.dual_carriage_axis)
+                    self.printer, [self.rails[self.dual_carriage_axis]],
+                    [self.rails[3]], axes=[self.dual_carriage_axis],
+                    safe_dist=config.getfloat('safe_distance', None, minval=0.))
         for s in self.get_steppers():
             s.set_trapq(toolhead.get_trapq())
             toolhead.register_step_generator(s.generate_steps)
@@ -52,9 +48,10 @@ class CartKinematics:
     def calc_position(self, stepper_positions):
         rails = self.rails
         if self.dc_module:
-            primary_rail = self.dc_module.get_primary_rail().get_rail()
-            rails = (rails[:self.dc_module.axis] +
-                     [primary_rail] + rails[self.dc_module.axis+1:])
+            primary_rail = self.dc_module.get_primary_rail(
+                    self.dual_carriage_axis)
+            rails = (rails[:self.dual_carriage_axis] +
+                     [primary_rail] + rails[self.dual_carriage_axis+1:])
         return [stepper_positions[rail.get_name()] for rail in rails]
     def update_limits(self, i, range):
         l, h = self.limits[i]
@@ -67,8 +64,8 @@ class CartKinematics:
             rail.set_position(newpos)
         for axis_name in homing_axes:
             axis = "xyz".index(axis_name)
-            if self.dc_module and axis == self.dc_module.axis:
-                rail = self.dc_module.get_primary_rail().get_rail()
+            if self.dc_module and axis == self.dual_carriage_axis:
+                rail = self.dc_module.get_primary_rail(self.dual_carriage_axis)
             else:
                 rail = self.rails[axis]
             self.limits[axis] = rail.get_range()
@@ -93,7 +90,7 @@ class CartKinematics:
         # Each axis is homed independently and in order
         for axis in homing_state.get_axes():
             if self.dc_module is not None and axis == self.dual_carriage_axis:
-                self.dc_module.home(homing_state)
+                self.dc_module.home(homing_state, self.dual_carriage_axis)
             else:
                 self.home_axis(homing_state, axis, self.rails[axis])
     def _check_endstops(self, move):

klippy/kinematics/deltesian.py

@@ -17,10 +17,10 @@ class DeltesianKinematics:
         self.rails = [None] * 3
         stepper_configs = [config.getsection('stepper_' + s)
                                     for s in ['left', 'right', 'y']]
-        self.rails[0] = stepper.PrinterRail(
+        self.rails[0] = stepper.LookupRail(
             stepper_configs[0], need_position_minmax = False)
         def_pos_es = self.rails[0].get_homing_info().position_endstop
-        self.rails[1] = stepper.PrinterRail(
+        self.rails[1] = stepper.LookupRail(
             stepper_configs[1], need_position_minmax = False,
             default_position_endstop = def_pos_es)
         self.rails[2] = stepper.LookupMultiRail(stepper_configs[2])

klippy/kinematics/generic_cartesian.py

@@ -0,0 +1,358 @@
+# Code for generic handling the kinematics of cartesian-like printers
+#
+# Copyright (C) 2024-2025  Dmitry Butyugin <dmbutyugin@google.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import copy, itertools, logging, math
+import gcode, mathutil, stepper
+from . import idex_modes
+from . import kinematic_stepper as ks
+
+def mat_mul(a, b):
+    if len(a[0]) != len(b):
+        return None
+    res = []
+    for i in range(len(a)):
+        res.append([])
+        for j in range(len(b[0])):
+            res[i].append(sum(a[i][k] * b[k][j] for k in range(len(b))))
+    return res
+
+def mat_transp(a):
+    res = []
+    for i in range(len(a[0])):
+        res.append([a[j][i] for j in range(len(a))])
+    return res
+
+def mat_pseudo_inverse(m):
+    mt = mat_transp(m)
+    mtm = mat_mul(mt, m)
+    pinv = mat_mul(mathutil.matrix_inv(mtm), mt)
+    return pinv
+
+class MainCarriage:
+    def __init__(self, config, axis):
+        self.rail = stepper.GenericPrinterRail(config)
+        self.axis = ord(axis) - ord('x')
+        self.axis_name = axis
+        self.dual_carriage = None
+    def get_name(self):
+        return self.rail.get_name()
+    def get_axis(self):
+        return self.axis
+    def get_rail(self):
+        return self.rail
+    def add_stepper(self, kin_stepper):
+        self.rail.add_stepper(kin_stepper.get_stepper())
+    def is_active(self):
+        if self.dual_carriage is None:
+            return True
+        return self.dual_carriage.get_dc_module().is_active(self.rail)
+    def set_dual_carriage(self, carriage):
+        old_dc = self.dual_carriage
+        self.dual_carriage = carriage
+        return old_dc
+    def get_dual_carriage(self):
+        return self.dual_carriage
+
+class ExtraCarriage:
+    def __init__(self, config, carriages):
+        self.name = config.get_name().split()[-1]
+        self.primary_carriage = config.getchoice('primary_carriage', carriages)
+        self.endstop_pin = config.get('endstop_pin')
+    def get_name(self):
+        return self.name
+    def get_axis(self):
+        return self.primary_carriage.get_axis()
+    def get_rail(self):
+        return self.primary_carriage.get_rail()
+    def add_stepper(self, kin_stepper):
+        self.get_rail().add_stepper(kin_stepper.get_stepper(),
+                                    self.endstop_pin, self.name)
+
+class DualCarriage:
+    def __init__(self, config, carriages):
+        self.printer = config.get_printer()
+        self.rail = stepper.GenericPrinterRail(config)
+        self.primary_carriage = config.getchoice('primary_carriage', carriages)
+        if self.primary_carriage.set_dual_carriage(self) is not None:
+            raise config.error(
+                    "Redefinition of dual_carriage for carriage '%s'" %
+                    self.primary_carriage.get_name())
+        self.axis = self.primary_carriage.get_axis()
+        if self.axis > 1:
+            raise config.error("Invalid axis '%s' for dual_carriage" %
+                               self.primary_carriage.get_axis_name())
+        self.safe_dist = config.getfloat('safe_distance', None, minval=0.)
+    def get_name(self):
+        return self.rail.get_name()
+    def get_axis(self):
+        return self.primary_carriage.get_axis()
+    def get_rail(self):
+        return self.rail
+    def get_safe_dist(self):
+        return self.safe_dist
+    def get_dc_module(self):
+        return self.printer.lookup_object('dual_carriage')
+    def is_active(self):
+        return self.get_dc_module().is_active(self.rail)
+    def get_dual_carriage(self):
+        return self.primary_carriage
+    def add_stepper(self, kin_stepper):
+        self.rail.add_stepper(kin_stepper.get_stepper())
+
+class GenericCartesianKinematics:
+    def __init__(self, toolhead, config):
+        self.printer = config.get_printer()
+        self._load_kinematics(config)
+        for s in self.get_steppers():
+            s.set_trapq(toolhead.get_trapq())
+            toolhead.register_step_generator(s.generate_steps)
+        self.dc_module = None
+        if self.dc_carriages:
+            pcs = [dc.get_dual_carriage() for dc in self.dc_carriages]
+            primary_rails = [pc.get_rail() for pc in pcs]
+            dual_rails = [dc.get_rail() for dc in self.dc_carriages]
+            axes = [dc.get_axis() for dc in self.dc_carriages]
+            safe_dist = {dc.get_axis() : dc.get_safe_dist()
+                         for dc in self.dc_carriages}
+            self.dc_module = dc_module = idex_modes.DualCarriages(
+                    self.printer, primary_rails, dual_rails, axes, safe_dist)
+            zero_pos = (0., 0.)
+            for acs in itertools.product(*zip(pcs, self.dc_carriages)):
+                for c in acs:
+                    dc_module.get_dc_rail_wrapper(c.get_rail()).activate(
+                            idex_modes.PRIMARY, zero_pos)
+                    dc_rail = c.get_dual_carriage().get_rail()
+                    dc_module.get_dc_rail_wrapper(dc_rail).inactivate(zero_pos)
+                self._check_kinematics(config.error)
+            for c in pcs:
+                dc_module.get_dc_rail_wrapper(c.get_rail()).activate(
+                        idex_modes.PRIMARY, zero_pos)
+                dc_rail = c.get_dual_carriage().get_rail()
+                dc_module.get_dc_rail_wrapper(dc_rail).inactivate(zero_pos)
+        else:
+            self._check_kinematics(config.error)
+        # Setup boundary checks
+        max_velocity, max_accel = toolhead.get_max_velocity()
+        self.max_z_velocity = config.getfloat('max_z_velocity', max_velocity,
+                                              above=0., maxval=max_velocity)
+        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_CARRIAGES",
+                               self.cmd_SET_STEPPER_CARRIAGES,
+                               desc=self.cmd_SET_STEPPER_CARRIAGES_help)
+    def _load_kinematics(self, config):
+        carriages = {a : MainCarriage(config.getsection('carriage ' + a), a)
+                     for a in 'xyz'}
+        dc_carriages = []
+        for c in config.get_prefix_sections('dual_carriage '):
+            dc_carriages.append(DualCarriage(c, carriages))
+        for dc in dc_carriages:
+            name = dc.get_name()
+            if name in carriages:
+                raise config.error("Redefinition of carriage %s" % name)
+            carriages[name] = dc
+        self.carriages = dict(carriages)
+        self.dc_carriages = dc_carriages
+        ec_carriages = []
+        for c in config.get_prefix_sections('extra_carriage '):
+            ec_carriages.append(ExtraCarriage(c, carriages))
+        for ec in ec_carriages:
+            name = ec.get_name()
+            if name in carriages:
+                raise config.error("Redefinition of carriage %s" % name)
+            carriages[name] = ec
+        self.kin_steppers = self._load_steppers(config, carriages)
+        self.all_carriages = carriages
+        self._check_carriages_references(config.error)
+        self._check_multi_mcu_homing(config.error)
+    def _check_carriages_references(self, report_error):
+        carriages = dict(self.all_carriages)
+        for s in self.kin_steppers:
+            for c in s.get_carriages():
+                carriages.pop(c.get_name(), None)
+        if carriages:
+            raise report_error(
+                    "Carriage(s) %s must be referenced by some "
+                    "stepper(s)" % (", ".join(carriages),))
+    def _check_multi_mcu_homing(self, report_error):
+        for carriage in self.carriages.values():
+            for es in carriage.get_rail().get_endstops():
+                stepper_mcus = set([s.get_mcu() for s in es[0].get_steppers()
+                                    if s in carriage.get_rail().get_steppers()])
+                if len(stepper_mcus) > 1:
+                    raise report_error("Multi-mcu homing not supported on"
+                                       " multi-mcu shared carriage %s" % es[1])
+    def _load_steppers(self, config, carriages):
+        return [ks.KinematicStepper(c, carriages)
+                for c in config.get_prefix_sections('stepper ')]
+    def get_steppers(self):
+        return [s.get_stepper() for s in self.kin_steppers]
+    def get_primary_carriages(self):
+        carriages = [self.carriages[a] for a in "xyz"]
+        if self.dc_module:
+            for a in self.dc_module.get_axes():
+                primary_rail = self.dc_module.get_primary_rail(a)
+                for c in self.carriages.values():
+                    if c.get_rail() == primary_rail:
+                        carriages[a] = c
+        return carriages
+    def _get_kinematics_coeffs(self):
+        matr = {s.get_name() : list(s.get_kin_coeffs())
+                for s in self.kin_steppers}
+        offs = {s.get_name() : [0.] * 3 for s in self.kin_steppers}
+        if self.dc_module is None:
+            return ([matr[s.get_name()] for s in self.kin_steppers],
+                    [0. for s in self.kin_steppers])
+        axes = [dc.get_axis() for dc in self.dc_carriages]
+        orig_matr = copy.deepcopy(matr)
+        for dc in self.dc_carriages:
+            axis = dc.get_axis()
+            for c in [dc.get_dual_carriage(), dc]:
+                m, o = self.dc_module.get_transform(c.get_rail())
+                for s in c.get_rail().get_steppers():
+                    matr[s.get_name()][axis] *= m
+                    offs[s.get_name()][axis] += o
+        return ([matr[s.get_name()] for s in self.kin_steppers],
+                [mathutil.matrix_dot(orig_matr[s.get_name()],
+                                     offs[s.get_name()])
+                 for s in self.kin_steppers])
+    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 report_error(
+                    "Verify configured stepper(s) and their 'carriages' "
+                    "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()
+        spos = [stepper_positions[s.get_name()] for s in self.kin_steppers]
+        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]
+        # Only update limits if this axis was already homed,
+        # otherwise leave in un-homed state.
+        if l <= h:
+            self.limits[i] = range
+    def set_position(self, newpos, homing_axes):
+        for s in self.kin_steppers:
+            s.set_position(newpos)
+        for axis_name in homing_axes:
+            axis = "xyz".index(axis_name)
+            for c in self.carriages.values():
+                if c.get_axis() == axis and c.is_active():
+                    self.limits[axis] = c.get_rail().get_range()
+                    break
+    def clear_homing_state(self, clear_axes):
+        for axis, axis_name in enumerate("xyz"):
+            if axis_name in clear_axes:
+                self.limits[axis] = (1.0, -1.0)
+    def home_axis(self, homing_state, axis, rail):
+        # Determine movement
+        position_min, position_max = rail.get_range()
+        hi = rail.get_homing_info()
+        homepos = [None, None, None, None]
+        homepos[axis] = hi.position_endstop
+        forcepos = list(homepos)
+        if hi.positive_dir:
+            forcepos[axis] -= 1.5 * (hi.position_endstop - position_min)
+        else:
+            forcepos[axis] += 1.5 * (position_max - hi.position_endstop)
+        # Perform homing
+        homing_state.home_rails([rail], forcepos, homepos)
+    def home(self, homing_state):
+        self._check_kinematics(self.printer.command_error)
+        # Each axis is homed independently and in order
+        for axis in homing_state.get_axes():
+            carriage = self.carriages["xyz"[axis]]
+            if carriage.get_dual_carriage() != None:
+                self.dc_module.home(homing_state, axis)
+            else:
+                self.home_axis(homing_state, axis, carriage.get_rail())
+    def _check_endstops(self, move):
+        end_pos = move.end_pos
+        for i in (0, 1, 2):
+            if (move.axes_d[i]
+                and (end_pos[i] < self.limits[i][0]
+                     or end_pos[i] > self.limits[i][1])):
+                if self.limits[i][0] > self.limits[i][1]:
+                    raise move.move_error("Must home axis first")
+                raise move.move_error()
+    def check_move(self, move):
+        limits = self.limits
+        xpos, ypos = move.end_pos[:2]
+        if (xpos < limits[0][0] or xpos > limits[0][1]
+            or ypos < limits[1][0] or ypos > limits[1][1]):
+            self._check_endstops(move)
+        if not move.axes_d[2]:
+            # Normal XY move - use defaults
+            return
+        # Move with Z - update velocity and accel for slower Z axis
+        self._check_endstops(move)
+        z_ratio = move.move_d / abs(move.axes_d[2])
+        move.limit_speed(
+            self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
+    def get_status(self, eventtime):
+        axes = [a for a, (l, h) in zip("xyz", self.limits) if l <= h]
+        ranges = [c.get_rail().get_range()
+                  for c in self.get_primary_carriages()]
+        axes_min = gcode.Coord(*[r[0] for r in ranges], e=0.)
+        axes_max = gcode.Coord(*[r[1] for r in ranges], e=0.)
+        return {
+            'homed_axes': "".join(axes),
+            'axis_minimum': axes_min,
+            'axis_maximum': axes_max,
+        }
+    cmd_SET_STEPPER_CARRIAGES_help = "Set stepper carriages"
+    def cmd_SET_STEPPER_CARRIAGES(self, gcmd):
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.flush_step_generation()
+        stepper_name = gcmd.get("STEPPER")
+        steppers = [stepper for stepper in self.kin_steppers
+                    if stepper.get_name() == stepper_name
+                    or stepper.get_name(short=True) == stepper_name]
+        if len(steppers) != 1:
+            raise gcmd.error("Invalid STEPPER '%s' specified" % stepper_name)
+        stepper = steppers[0]
+        carriages_str = gcmd.get("CARRIAGES").lower()
+        validate = not gcmd.get_int("DISABLE_CHECKS", 0)
+        old_carriages = stepper.get_carriages()
+        old_kin_coeffs = stepper.get_kin_coeffs()
+        stepper.update_carriages(carriages_str, self.all_carriages, gcmd.error)
+        new_carriages = stepper.get_carriages()
+        if new_carriages != old_carriages:
+            stepper.update_kin_coeffs(old_kin_coeffs)
+            raise gcmd.error("SET_STEPPER_CARRIAGES cannot add or remove "
+                             "carriages that the stepper controls")
+        pos = toolhead.get_position()
+        stepper.set_position(pos)
+        if not validate:
+            return
+        if self.dc_module:
+            dc_state = self.dc_module.save_dual_carriage_state()
+            pcs = [dc.get_dual_carriage() for dc in self.dc_carriages]
+            axes = [dc.get_axis() for dc in self.dc_carriages]
+            for acs in itertools.product(*zip(pcs, self.dc_carriages)):
+                for c in acs:
+                    self.dc_module.get_dc_rail_wrapper(c.get_rail()).activate(
+                            idex_modes.PRIMARY, pos)
+                    self.dc_module.get_dc_rail_wrapper(
+                            c.get_dual_carriage().get_rail()).inactivate(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)

klippy/kinematics/hybrid_corexy.py

@@ -12,7 +12,7 @@ class HybridCoreXYKinematics:
     def __init__(self, toolhead, config):
         self.printer = config.get_printer()
         # itersolve parameters
-        self.rails = [ stepper.PrinterRail(config.getsection('stepper_x')),
+        self.rails = [ stepper.LookupRail(config.getsection('stepper_x')),
                        stepper.LookupMultiRail(config.getsection('stepper_y')),
                        stepper.LookupMultiRail(config.getsection('stepper_z'))]
         self.rails[1].get_endstops()[0][0].add_stepper(
@@ -29,16 +29,13 @@ class HybridCoreXYKinematics:
             # dummy for cartesian config users
             dc_config.getchoice('axis', ['x'], default='x')
             # setup second dual carriage rail
-            self.rails.append(stepper.PrinterRail(dc_config))
+            self.rails.append(stepper.LookupRail(dc_config))
             self.rails[1].get_endstops()[0][0].add_stepper(
                 self.rails[3].get_steppers()[0])
             self.rails[3].setup_itersolve('corexy_stepper_alloc', b'+')
-            dc_rail_0 = idex_modes.DualCarriagesRail(
-                    self.rails[0], axis=0, active=True)
-            dc_rail_1 = idex_modes.DualCarriagesRail(
-                    self.rails[3], axis=0, active=False)
             self.dc_module = idex_modes.DualCarriages(
-                    dc_config, dc_rail_0, dc_rail_1, axis=0)
+                    self.printer, [self.rails[0]], [self.rails[3]], axes=[0],
+                    safe_dist=config.getfloat('safe_distance', None, minval=0.))
         for s in self.get_steppers():
             s.set_trapq(toolhead.get_trapq())
             toolhead.register_step_generator(s.generate_steps)
@@ -69,8 +66,8 @@ class HybridCoreXYKinematics:
             rail.set_position(newpos)
         for axis_name in homing_axes:
             axis = "xyz".index(axis_name)
-            if self.dc_module and axis == self.dc_module.axis:
-                rail = self.dc_module.get_primary_rail().get_rail()
+            if self.dc_module and axis == 0:
+                rail = self.dc_module.get_primary_rail(axis)
             else:
                 rail = self.rails[axis]
             self.limits[axis] = rail.get_range()
@@ -93,7 +90,7 @@ class HybridCoreXYKinematics:
     def home(self, homing_state):
         for axis in homing_state.get_axes():
             if self.dc_module is not None and axis == 0:
-                self.dc_module.home(homing_state)
+                self.dc_module.home(homing_state, axis)
             else:
                 self.home_axis(homing_state, axis, self.rails[axis])
     def _check_endstops(self, move):

klippy/kinematics/hybrid_corexz.py

@@ -12,7 +12,7 @@ class HybridCoreXZKinematics:
     def __init__(self, toolhead, config):
         self.printer = config.get_printer()
         # itersolve parameters
-        self.rails = [ stepper.PrinterRail(config.getsection('stepper_x')),
+        self.rails = [ stepper.LookupRail(config.getsection('stepper_x')),
                        stepper.LookupMultiRail(config.getsection('stepper_y')),
                        stepper.LookupMultiRail(config.getsection('stepper_z'))]
         self.rails[2].get_endstops()[0][0].add_stepper(
@@ -29,16 +29,13 @@ class HybridCoreXZKinematics:
             # dummy for cartesian config users
             dc_config.getchoice('axis', ['x'], default='x')
             # setup second dual carriage rail
-            self.rails.append(stepper.PrinterRail(dc_config))
+            self.rails.append(stepper.LookupRail(dc_config))
             self.rails[2].get_endstops()[0][0].add_stepper(
                 self.rails[3].get_steppers()[0])
             self.rails[3].setup_itersolve('corexz_stepper_alloc', b'+')
-            dc_rail_0 = idex_modes.DualCarriagesRail(
-                self.rails[0], axis=0, active=True)
-            dc_rail_1 = idex_modes.DualCarriagesRail(
-                self.rails[3], axis=0, active=False)
             self.dc_module = idex_modes.DualCarriages(
-                    dc_config, dc_rail_0, dc_rail_1, axis=0)
+                    self.printer, [self.rails[0]], [self.rails[3]], axes=[0],
+                    safe_dist=config.getfloat('safe_distance', None, minval=0.))
         for s in self.get_steppers():
             s.set_trapq(toolhead.get_trapq())
             toolhead.register_step_generator(s.generate_steps)
@@ -69,8 +66,8 @@ class HybridCoreXZKinematics:
             rail.set_position(newpos)
         for axis_name in homing_axes:
             axis = "xyz".index(axis_name)
-            if self.dc_module and axis == self.dc_module.axis:
-                rail = self.dc_module.get_primary_rail().get_rail()
+            if self.dc_module and axis == 0:
+                rail = self.dc_module.get_primary_rail(axis)
             else:
                 rail = self.rails[axis]
             self.limits[axis] = rail.get_range()
@@ -93,7 +90,7 @@ class HybridCoreXZKinematics:
     def home(self, homing_state):
         for axis in homing_state.get_axes():
             if self.dc_module is not None and axis == 0:
-                self.dc_module.home(homing_state)
+                self.dc_module.home(homing_state, axis)
             else:
                 self.home_axis(homing_state, axis, self.rails[axis])
     def _check_endstops(self, move):

klippy/kinematics/idex_modes.py

@@ -1,10 +1,10 @@
 # Support for duplication and mirroring modes for IDEX printers
 #
 # Copyright (C) 2021  Fabrice Gallet <tircown@gmail.com>
-# Copyright (C) 2023  Dmitry Butyugin <dmbutyugin@google.com>
+# Copyright (C) 2023-2025  Dmitry Butyugin <dmbutyugin@google.com>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
-import logging, math
+import collections, logging, math
 import chelper
 
 INACTIVE = 'INACTIVE'
@@ -14,18 +14,34 @@ MIRROR = 'MIRROR'
 
 class DualCarriages:
     VALID_MODES = [PRIMARY, COPY, MIRROR]
-    def __init__(self, dc_config, rail_0, rail_1, axis):
-        self.printer = dc_config.get_printer()
-        self.axis = axis
-        self.dc = (rail_0, rail_1)
+    def __init__(self, printer, primary_rails, dual_rails, axes,
+                 safe_dist={}):
+        self.printer = printer
+        self.axes = axes
+        self._init_steppers(primary_rails + dual_rails)
+        self.primary_rails = [
+                DualCarriagesRail(c, dual_rails[i], axes[i], active=True)
+                for i, c in enumerate(primary_rails)]
+        self.dual_rails = [
+                DualCarriagesRail(c, primary_rails[i], axes[i], active=False)
+                for i, c in enumerate(dual_rails)]
+        self.dc_rails = collections.OrderedDict(
+                [(c.rail.get_name(), c)
+                 for c in self.primary_rails + self.dual_rails])
         self.saved_states = {}
-        safe_dist = dc_config.getfloat('safe_distance', None, minval=0.)
-        if safe_dist is None:
-            dc0_rail = rail_0.get_rail()
-            dc1_rail = rail_1.get_rail()
-            safe_dist = min(abs(dc0_rail.position_min - dc1_rail.position_min),
-                            abs(dc0_rail.position_max - dc1_rail.position_max))
-        self.safe_dist = safe_dist
+        self.safe_dist = {}
+        for i, dc in enumerate(dual_rails):
+            axis = axes[i]
+            if isinstance(safe_dist, dict):
+                if axis in safe_dist:
+                    self.safe_dist[axis] = safe_dist[axis]
+                    continue
+            elif safe_dist is not None:
+                self.safe_dist[axis] = safe_dist
+                continue
+            pc = primary_rails[i]
+            self.safe_dist[axis] = min(abs(pc.position_min - dc.position_min),
+                                       abs(pc.position_max - dc.position_max))
         self.printer.add_object('dual_carriage', self)
         self.printer.register_event_handler("klippy:ready", self._handle_ready)
         gcode = self.printer.lookup_object('gcode')
@@ -40,58 +56,93 @@ class DualCarriages:
                    'RESTORE_DUAL_CARRIAGE_STATE',
                    self.cmd_RESTORE_DUAL_CARRIAGE_STATE,
                    desc=self.cmd_RESTORE_DUAL_CARRIAGE_STATE_help)
-    def get_rails(self):
-        return self.dc
-    def get_primary_rail(self):
-        for rail in self.dc:
-            if rail.mode == PRIMARY:
-                return rail
+    def _init_steppers(self, rails):
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self.dc_stepper_kinematics = []
+        self.orig_stepper_kinematics = []
+        steppers = set()
+        for rail in rails:
+            c_steppers = rail.get_steppers()
+            if not c_steppers:
+                raise self.printer.config_error(
+                        "At least one stepper must be "
+                        "associated with carriage: %s" % rail.get_name())
+            steppers.update(c_steppers)
+        for s in steppers:
+            sk = ffi_main.gc(ffi_lib.dual_carriage_alloc(), ffi_lib.free)
+            orig_sk = s.get_stepper_kinematics()
+            ffi_lib.dual_carriage_set_sk(sk, orig_sk)
+            self.dc_stepper_kinematics.append(sk)
+            self.orig_stepper_kinematics.append(orig_sk)
+            s.set_stepper_kinematics(sk)
+    def get_axes(self):
+        return self.axes
+    def get_primary_rail(self, axis):
+        for dc_rail in self.dc_rails.values():
+            if dc_rail.mode == PRIMARY and dc_rail.axis == axis:
+                return dc_rail.rail
         return None
-    def toggle_active_dc_rail(self, index):
+    def get_dc_rail_wrapper(self, rail):
+        for dc_rail in self.dc_rails.values():
+            if dc_rail.rail == rail:
+                return dc_rail
+        return None
+    def get_transform(self, rail):
+        dc_rail = self.get_dc_rail_wrapper(rail)
+        if dc_rail is not None:
+            return (dc_rail.scale, dc_rail.offset)
+        return (0., 0.)
+    def is_active(self, rail):
+        dc_rail = self.get_dc_rail_wrapper(rail)
+        return dc_rail.is_active() if dc_rail is not None else False
+    def toggle_active_dc_rail(self, target_dc):
         toolhead = self.printer.lookup_object('toolhead')
         toolhead.flush_step_generation()
         pos = toolhead.get_position()
         kin = toolhead.get_kinematics()
-        for i, dc in enumerate(self.dc):
-            dc_rail = dc.get_rail()
-            if i != index:
-                if dc.is_active():
-                    dc.inactivate(pos)
-        target_dc = self.dc[index]
+        axis = target_dc.axis
+        for dc in self.dc_rails.values():
+            if dc != target_dc and dc.axis == axis and dc.is_active():
+                dc.inactivate(pos)
         if target_dc.mode != PRIMARY:
-            newpos = pos[:self.axis] + [target_dc.get_axis_position(pos)] \
-                        + pos[self.axis+1:]
+            newpos = pos[:axis] + [target_dc.get_axis_position(pos)] \
+                        + pos[axis+1:]
             target_dc.activate(PRIMARY, newpos, old_position=pos)
             toolhead.set_position(newpos)
-        kin.update_limits(self.axis, target_dc.get_rail().get_range())
-    def home(self, homing_state):
+        kin.update_limits(axis, target_dc.rail.get_range())
+    def home(self, homing_state, axis):
         kin = self.printer.lookup_object('toolhead').get_kinematics()
-        enumerated_dcs = list(enumerate(self.dc))
-        if (self.get_dc_order(0, 1) > 0) != \
-                self.dc[0].get_rail().get_homing_info().positive_dir:
+        dcs = [dc for dc in self.dc_rails.values() if dc.axis == axis]
+        if (self.get_dc_order(dcs[0], dcs[1]) > 0) != \
+                dcs[0].rail.get_homing_info().positive_dir:
             # The second carriage must home first, because the carriages home in
             # the same direction and the first carriage homes on the second one
-            enumerated_dcs.reverse()
-        for i, dc_rail in enumerated_dcs:
-            self.toggle_active_dc_rail(i)
-            kin.home_axis(homing_state, self.axis, dc_rail.get_rail())
+            dcs.reverse()
+        for dc in dcs:
+            self.toggle_active_dc_rail(dc)
+            kin.home_axis(homing_state, axis, dc.rail)
         # Restore the original rails ordering
-        self.toggle_active_dc_rail(0)
+        self.toggle_active_dc_rail(dcs[0])
     def get_status(self, eventtime=None):
-        return {('carriage_%d' % (i,)) : dc.mode
-                for (i, dc) in enumerate(self.dc)}
-    def get_kin_range(self, toolhead, mode):
+        status = {'carriages' : {dc.get_name() : dc.mode
+                                 for dc in self.dc_rails.values()}}
+        if len(self.dc_rails) == 2:
+            status.update({('carriage_%d' % (i,)) : dc.mode
+                           for i, dc in enumerate(self.dc_rails.values())})
+        return status
+    def get_kin_range(self, toolhead, mode, axis):
         pos = toolhead.get_position()
-        axes_pos = [dc.get_axis_position(pos) for dc in self.dc]
-        dc0_rail = self.dc[0].get_rail()
-        dc1_rail = self.dc[1].get_rail()
-        if mode != PRIMARY or self.dc[0].is_active():
+        dcs = [dc for dc in self.dc_rails.values() if dc.axis == axis]
+        axes_pos = [dc.get_axis_position(pos) for dc in dcs]
+        dc0_rail = dcs[0].rail
+        dc1_rail = dcs[1].rail
+        if mode != PRIMARY or dcs[0].is_active():
             range_min = dc0_rail.position_min
             range_max = dc0_rail.position_max
         else:
             range_min = dc1_rail.position_min
             range_max = dc1_rail.position_max
-        safe_dist = self.safe_dist
+        safe_dist = self.safe_dist[axis]
         if not safe_dist:
             return (range_min, range_max)
 
@@ -101,7 +152,7 @@ class DualCarriages:
             range_max = min(range_max,
                             axes_pos[0] - axes_pos[1] + dc1_rail.position_max)
         elif mode == MIRROR:
-            if self.get_dc_order(0, 1) > 0:
+            if self.get_dc_order(dcs[0], dcs[1]) > 0:
                 range_min = max(range_min,
                                 0.5 * (sum(axes_pos) + safe_dist))
                 range_max = min(range_max,
@@ -113,9 +164,9 @@ class DualCarriages:
                                 sum(axes_pos) - dc1_rail.position_max)
         else:
             # mode == PRIMARY
-            active_idx = 1 if self.dc[1].is_active() else 0
+            active_idx = 1 if dcs[1].is_active() else 0
             inactive_idx = 1 - active_idx
-            if self.get_dc_order(active_idx, inactive_idx) > 0:
+            if self.get_dc_order(dcs[active_idx], dcs[inactive_idx]) > 0:
                 range_min = max(range_min, axes_pos[inactive_idx] + safe_dist)
             else:
                 range_max = min(range_max, axes_pos[inactive_idx] - safe_dist)
@@ -131,14 +182,14 @@ class DualCarriages:
             # which actually permits carriage motion.
             return (range_min, range_min)
         return (range_min, range_max)
-    def get_dc_order(self, first, second):
-        if first == second:
+    def get_dc_order(self, first_dc, second_dc):
+        if first_dc == second_dc:
             return 0
         # Check the relative order of the first and second carriages and
         # return -1 if the first carriage position is always smaller
         # than the second one and 1 otherwise
-        first_rail = self.dc[first].get_rail()
-        second_rail = self.dc[second].get_rail()
+        first_rail = first_dc.rail
+        second_rail = second_dc.rail
         first_homing_info = first_rail.get_homing_info()
         second_homing_info = second_rail.get_homing_info()
         if first_homing_info.positive_dir != second_homing_info.positive_dir:
@@ -148,50 +199,71 @@ class DualCarriages:
         if first_rail.position_endstop > second_rail.position_endstop:
             return 1
         return -1
-    def activate_dc_mode(self, index, mode):
+    def activate_dc_mode(self, dc, mode):
         toolhead = self.printer.lookup_object('toolhead')
         toolhead.flush_step_generation()
         kin = toolhead.get_kinematics()
+        axis = dc.axis
         if mode == INACTIVE:
-            self.dc[index].inactivate(toolhead.get_position())
+            dc.inactivate(toolhead.get_position())
         elif mode == PRIMARY:
-            self.toggle_active_dc_rail(index)
+            self.toggle_active_dc_rail(dc)
         else:
-            self.toggle_active_dc_rail(0)
-            self.dc[index].activate(mode, toolhead.get_position())
-        kin.update_limits(self.axis, self.get_kin_range(toolhead, mode))
+            self.toggle_active_dc_rail(self.get_dc_rail_wrapper(dc.dual_rail))
+            dc.activate(mode, toolhead.get_position())
+        kin.update_limits(axis, self.get_kin_range(toolhead, mode, axis))
     def _handle_ready(self):
         # Apply the transform later during Klipper initialization to make sure
         # that input shaping can pick up the correct stepper kinematic flags.
-        for dc in self.dc:
-            dc.apply_transform()
+        for dc_rail in self.dc_rails.values():
+            dc_rail.apply_transform()
     cmd_SET_DUAL_CARRIAGE_help = "Configure the dual carriages mode"
     def cmd_SET_DUAL_CARRIAGE(self, gcmd):
-        index = gcmd.get_int('CARRIAGE', minval=0, maxval=1)
+        carriage_str = gcmd.get('CARRIAGE', None)
+        if carriage_str is None:
+            raise gcmd.error('CARRIAGE must be specified')
+        if carriage_str in self.dc_rails:
+            dc_rail = self.dc_rails[carriage_str]
+        else:
+            dc_rail = None
+            if len(self.dc_rails) == 2:
+                try:
+                    index = int(carriage_str.strip())
+                    if index < 0 or index > 1:
+                        raise gcmd.error('Invalid CARRIAGE=%d index' % index)
+                    dc_rail = (self.dual_rails if index
+                               else self.primary_rails)[0]
+                except ValueError:
+                    pass
+            if dc_rail is None:
+                raise gcmd.error('Invalid CARRIAGE=%s specified' % carriage_str)
         mode = gcmd.get('MODE', PRIMARY).upper()
         if mode not in self.VALID_MODES:
             raise gcmd.error("Invalid mode=%s specified" % (mode,))
         if mode in [COPY, MIRROR]:
-            if index == 0:
+            if dc_rail in self.primary_rails:
                 raise gcmd.error(
-                        "Mode=%s is not supported for carriage=0" % (mode,))
+                        "Mode=%s is not supported for carriage=%s" % (
+                            mode, dc_rail.get_name()))
             curtime = self.printer.get_reactor().monotonic()
             kin = self.printer.lookup_object('toolhead').get_kinematics()
-            axis = 'xyz'[self.axis]
+            axis = 'xyz'[dc_rail.axis]
             if axis not in kin.get_status(curtime)['homed_axes']:
                 raise gcmd.error(
                         "Axis %s must be homed prior to enabling mode=%s" %
-                        (axis, mode))
-        self.activate_dc_mode(index, mode)
+                        (axis.upper(), mode))
+        self.activate_dc_mode(dc_rail, mode)
     cmd_SAVE_DUAL_CARRIAGE_STATE_help = \
             "Save dual carriages modes and positions"
     def cmd_SAVE_DUAL_CARRIAGE_STATE(self, gcmd):
         state_name = gcmd.get('NAME', 'default')
+        self.saved_states[state_name] = self.save_dual_carriage_state()
+    def save_dual_carriage_state(self):
         pos = self.printer.lookup_object('toolhead').get_position()
-        self.saved_states[state_name] = {
-            'carriage_modes': [dc.mode for dc in self.dc],
-            'axes_positions': [dc.get_axis_position(pos) for dc in self.dc],
-        }
+        return {'carriage_modes': {dc.get_name() : dc.mode
+                                   for dc in self.dc_rails.values()},
+                'carriage_positions': {dc.get_name() : dc.get_axis_position(pos)
+                                       for dc in self.dc_rails.values()}}
     cmd_RESTORE_DUAL_CARRIAGE_STATE_help = \
             "Restore dual carriages modes and positions"
     def cmd_RESTORE_DUAL_CARRIAGE_STATE(self, gcmd):
@@ -200,67 +272,69 @@ class DualCarriages:
         if saved_state is None:
             raise gcmd.error("Unknown DUAL_CARRIAGE state: %s" % (state_name,))
         move_speed = gcmd.get_float('MOVE_SPEED', 0., above=0.)
+        move = gcmd.get_int('MOVE', 1)
+        self.restore_dual_carriage_state(saved_state, move, move_speed)
+    def restore_dual_carriage_state(self, saved_state, move, move_speed=0.):
         toolhead = self.printer.lookup_object('toolhead')
         toolhead.flush_step_generation()
-        if gcmd.get_int('MOVE', 1):
+        if move:
             homing_speed = 99999999.
+            move_pos = list(toolhead.get_position())
             cur_pos = []
-            for i, dc in enumerate(self.dc):
-                self.toggle_active_dc_rail(i)
-                homing_speed = min(homing_speed, dc.get_rail().homing_speed)
+            carriage_positions = saved_state['carriage_positions']
+            dcs = list(self.dc_rails.values())
+            for dc in dcs:
+                self.toggle_active_dc_rail(dc)
+                homing_speed = min(homing_speed, dc.rail.homing_speed)
                 cur_pos.append(toolhead.get_position())
-            move_pos = list(cur_pos[0])
-            dl = [saved_state['axes_positions'][i] - cur_pos[i][self.axis]
-                  for i in range(2)]
-            primary_ind = 0 if abs(dl[0]) >= abs(dl[1]) else 1
-            self.toggle_active_dc_rail(primary_ind)
-            move_pos[self.axis] = saved_state['axes_positions'][primary_ind]
-            dc_mode = INACTIVE if min(abs(dl[0]), abs(dl[1])) < 0.000000001 \
-                    else COPY if dl[0] * dl[1] > 0 else MIRROR
-            if dc_mode != INACTIVE:
-                self.dc[1-primary_ind].activate(dc_mode, cur_pos[primary_ind])
-                self.dc[1-primary_ind].override_axis_scaling(
-                        abs(dl[1-primary_ind] / dl[primary_ind]),
-                        cur_pos[primary_ind])
+            dl = [carriage_positions[dc.get_name()] - cur_pos[i][dc.axis]
+                  for i, dc in enumerate(dcs)]
+            for axis in self.axes:
+                dc_ind = [i for i, dc in enumerate(dcs) if dc.axis == axis]
+                if abs(dl[dc_ind[0]]) >= abs(dl[dc_ind[1]]):
+                    primary_ind, secondary_ind = dc_ind[0], dc_ind[1]
+                else:
+                    primary_ind, secondary_ind = dc_ind[1], dc_ind[0]
+                primary_dc = dcs[primary_ind]
+                self.toggle_active_dc_rail(primary_dc)
+                move_pos[axis] = carriage_positions[primary_dc.get_name()]
+                dc_mode = INACTIVE if min(abs(dl[primary_ind]),
+                                          abs(dl[secondary_ind])) < .000000001 \
+                        else COPY if dl[primary_ind] * dl[secondary_ind] > 0 \
+                        else MIRROR
+                if dc_mode != INACTIVE:
+                    dcs[secondary_ind].activate(dc_mode, cur_pos[primary_ind])
+                    dcs[secondary_ind].override_axis_scaling(
+                            abs(dl[secondary_ind] / dl[primary_ind]),
+                            cur_pos[primary_ind])
             toolhead.manual_move(move_pos, move_speed or homing_speed)
             toolhead.flush_step_generation()
             # Make sure the scaling coefficients are restored with the mode
-            self.dc[0].inactivate(move_pos)
-            self.dc[1].inactivate(move_pos)
-        for i, dc in enumerate(self.dc):
-            saved_mode = saved_state['carriage_modes'][i]
-            self.activate_dc_mode(i, saved_mode)
+            for dc in dcs:
+                dc.inactivate(move_pos)
+        for dc in self.dc_rails.values():
+            saved_mode = saved_state['carriage_modes'][dc.get_name()]
+            self.activate_dc_mode(dc, saved_mode)
 
 class DualCarriagesRail:
     ENC_AXES = [b'x', b'y']
-    def __init__(self, rail, axis, active):
+    def __init__(self, rail, dual_rail, axis, active):
         self.rail = rail
+        self.dual_rail = dual_rail
+        self.sks = [s.get_stepper_kinematics() for s in rail.get_steppers()]
         self.axis = axis
         self.mode = (INACTIVE, PRIMARY)[active]
         self.offset = 0.
         self.scale = 1. if active else 0.
-        ffi_main, ffi_lib = chelper.get_ffi()
-        self.dc_stepper_kinematics = []
-        self.orig_stepper_kinematics = []
-        for s in rail.get_steppers():
-            sk = ffi_main.gc(ffi_lib.dual_carriage_alloc(), ffi_lib.free)
-            orig_sk = s.get_stepper_kinematics()
-            ffi_lib.dual_carriage_set_sk(sk, orig_sk)
-            # Set the default transform for the other axis
-            ffi_lib.dual_carriage_set_transform(
-                    sk, self.ENC_AXES[1 - axis], 1., 0.)
-            self.dc_stepper_kinematics.append(sk)
-            self.orig_stepper_kinematics.append(orig_sk)
-            s.set_stepper_kinematics(sk)
-    def get_rail(self):
-        return self.rail
+    def get_name(self):
+        return self.rail.get_name()
     def is_active(self):
         return self.mode != INACTIVE
     def get_axis_position(self, position):
         return position[self.axis] * self.scale + self.offset
     def apply_transform(self):
         ffi_main, ffi_lib = chelper.get_ffi()
-        for sk in self.dc_stepper_kinematics:
+        for sk in self.sks:
             ffi_lib.dual_carriage_set_transform(
                     sk, self.ENC_AXES[self.axis], self.scale, self.offset)
     def activate(self, mode, position, old_position=None):

klippy/kinematics/kinematic_stepper.py

@@ -0,0 +1,92 @@
+# Kinematic stepper class for generic cartesian kinematics
+#
+# Copyright (C) 2024  Dmitry Butyugin <dmbutyugin@google.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import logging, re
+import stepper, chelper
+
+def parse_carriages_string(carriages_str, printer_carriages, parse_error):
+    nxt = 0
+    pat = re.compile('[+-]')
+    coeffs = [0.] * 3
+    ref_carriages = []
+    while nxt < len(carriages_str):
+        match = pat.search(carriages_str, nxt+1)
+        end = len(carriages_str) if match is None else match.start()
+        term = carriages_str[nxt:end].strip()
+        term_lst = term.split('*')
+        if len(term_lst) not in [1, 2]:
+            raise parse_error(
+                    "Invalid term '%s' in '%s'" % (term, carriages_str))
+        if len(term_lst) == 2:
+            try:
+                coeff = float(term_lst[0])
+            except ValueError:
+                raise error("Invalid float '%s'" % term_lst[0])
+        else:
+            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:]
+        c = term_lst[-1]
+        if c not in printer_carriages:
+            raise parse_error("Invalid '%s' carriage referenced in '%s'" %
+                              (c, carriages_str))
+        carriage = printer_carriages[c]
+        j = carriage.get_axis()
+        if coeffs[j]:
+            raise error("Carriage '%s' was referenced multiple times in '%s'" %
+                        (c, carriages_str))
+        coeffs[j] = coeff
+        ref_carriages.append(carriage)
+        nxt = end
+    return coeffs, ref_carriages
+
+class KinematicStepper:
+    def __init__(self, config, printer_carriages):
+        self.printer = config.get_printer()
+        self.stepper = stepper.PrinterStepper(config)
+        self.kin_coeffs, self.carriages = parse_carriages_string(
+                config.get('carriages'), printer_carriages, config.error)
+        if not any(self.kin_coeffs):
+            raise config.error(
+                    "'%s' must provide a valid 'carriages' configuration" %
+                    self.stepper.get_name())
+        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 to the carriages it references
+        for sc in self.carriages:
+            sc.add_stepper(self)
+    def get_name(self, short=False):
+        name = self.stepper.get_name(short)
+        if short and name.startswith('stepper '):
+            return name[8:]
+        return name
+    def get_stepper(self):
+        return self.stepper
+    def get_kin_coeffs(self):
+        return tuple(self.kin_coeffs)
+    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_kin_coeffs(self, kin_coeffs):
+        self.kin_coeffs = kin_coeffs
+        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])
+    def update_carriages(self, carriages_str, printer_carriages, report_error):
+        kin_coeffs, carriages = parse_carriages_string(
+                carriages_str, printer_carriages,
+                report_error or self.printer.command_error)
+        if report_error is not None and not any(kin_coeffs):
+            raise report_error(
+                    "A valid string that references at least one carriage"
+                    " must be provided for '%s'" % self.get_name())
+        self.carriages = carriages
+        self.update_kin_coeffs(kin_coeffs)
+    def set_position(self, coord):
+        self.stepper.set_position(coord)

klippy/kinematics/polar.py

@@ -11,7 +11,7 @@ class PolarKinematics:
         # Setup axis steppers
         stepper_bed = stepper.PrinterStepper(config.getsection('stepper_bed'),
                                              units_in_radians=True)
-        rail_arm = stepper.PrinterRail(config.getsection('stepper_arm'))
+        rail_arm = stepper.LookupRail(config.getsection('stepper_arm'))
         rail_z = stepper.LookupMultiRail(config.getsection('stepper_z'))
         stepper_bed.setup_itersolve('polar_stepper_alloc', b'a')
         rail_arm.setup_itersolve('polar_stepper_alloc', b'r')

klippy/kinematics/rotary_delta.py

@@ -10,14 +10,14 @@ class RotaryDeltaKinematics:
     def __init__(self, toolhead, config):
         # Setup tower rails
         stepper_configs = [config.getsection('stepper_' + a) for a in 'abc']
-        rail_a = stepper.PrinterRail(
+        rail_a = stepper.LookupRail(
             stepper_configs[0], need_position_minmax=False,
             units_in_radians=True)
         a_endstop = rail_a.get_homing_info().position_endstop
-        rail_b = stepper.PrinterRail(
+        rail_b = stepper.LookupRail(
             stepper_configs[1], need_position_minmax=False,
             default_position_endstop=a_endstop, units_in_radians=True)
-        rail_c = stepper.PrinterRail(
+        rail_c = stepper.LookupRail(
             stepper_configs[2], need_position_minmax=False,
             default_position_endstop=a_endstop, units_in_radians=True)
         self.rails = [rail_a, rail_b, rail_c]

klippy/mathutil.py

@@ -135,3 +135,18 @@ def matrix_sub(m1, m2):
 
 def matrix_mul(m1, s):
     return [m1[0]*s, m1[1]*s, m1[2]*s]
+
+######################################################################
+# Matrix helper functions for 3x3 matrices
+######################################################################
+
+def matrix_det(a):
+    x0, x1, x2 = a
+    return matrix_dot(x0, matrix_cross(x1, x2))
+
+def matrix_inv(a):
+    x0, x1, x2 = a
+    inv_det = 1. / matrix_det(a)
+    return [matrix_mul(matrix_cross(x1, x2), inv_det),
+            matrix_mul(matrix_cross(x2, x0), inv_det),
+            matrix_mul(matrix_cross(x0, x1), inv_det)]

klippy/stepper.py

@@ -56,7 +56,8 @@ class MCU_stepper:
     def get_mcu(self):
         return self._mcu
     def get_name(self, short=False):
-        if short and self._name.startswith('stepper_'):
+        if short and self._name.startswith('stepper'):
+            # Skip an extra symbol after 'stepper'
             return self._name[8:]
         return self._name
     def units_in_radians(self):
@@ -315,23 +316,21 @@ def parse_step_distance(config, units_in_radians=None, note_valid=False):
 # Stepper controlled rails
 ######################################################################
 
-# A motor control "rail" with one (or more) steppers and one (or more)
+# A motor control carriage with one (or more) steppers and one (or more)
 # endstops.
-class PrinterRail:
+class GenericPrinterRail:
     def __init__(self, config, need_position_minmax=True,
                  default_position_endstop=None, units_in_radians=False):
-        # Primary stepper and endstop
         self.stepper_units_in_radians = units_in_radians
+        self.printer = config.get_printer()
+        self.name = config.get_name().split()[-1]
         self.steppers = []
         self.endstops = []
         self.endstop_map = {}
-        self.add_extra_stepper(config)
-        mcu_stepper = self.steppers[0]
-        self.get_name = mcu_stepper.get_name
-        self.get_commanded_position = mcu_stepper.get_commanded_position
-        self.calc_position_from_coord = mcu_stepper.calc_position_from_coord
+        self.endstop_pin = config.get('endstop_pin')
         # Primary endstop position
-        mcu_endstop = self.endstops[0][0]
+        self.query_endstops = self.printer.load_object(config, 'query_endstops')
+        mcu_endstop = self.lookup_endstop(self.endstop_pin, self.name)
         if hasattr(mcu_endstop, "get_position_endstop"):
             self.position_endstop = mcu_endstop.get_position_endstop()
         elif default_position_endstop is None:
@@ -380,6 +379,11 @@ class PrinterRail:
             raise config.error(
                 "Invalid homing_positive_dir / position_endstop in '%s'"
                 % (config.get_name(),))
+    def get_name(self, short=False):
+        if short and self.name.startswith('stepper'):
+            # Skip an extra symbol after 'stepper'
+            return self.name[8:]
+        return self.name
     def get_range(self):
         return self.position_min, self.position_max
     def get_homing_info(self):
@@ -394,16 +398,8 @@ class PrinterRail:
         return list(self.steppers)
     def get_endstops(self):
         return list(self.endstops)
-    def add_extra_stepper(self, config):
-        stepper = PrinterStepper(config, self.stepper_units_in_radians)
-        self.steppers.append(stepper)
-        if self.endstops and config.get('endstop_pin', None) is None:
-            # No endstop defined - use primary endstop
-            self.endstops[0][0].add_stepper(stepper)
-            return
-        endstop_pin = config.get('endstop_pin')
-        printer = config.get_printer()
-        ppins = printer.lookup_object('pins')
+    def lookup_endstop(self, endstop_pin, name):
+        ppins = self.printer.lookup_object('pins')
         pin_params = ppins.parse_pin(endstop_pin, True, True)
         # Normalize pin name
         pin_name = "%s:%s" % (pin_params['chip_name'], pin_params['pin'])
@@ -415,19 +411,32 @@ class PrinterRail:
             self.endstop_map[pin_name] = {'endstop': mcu_endstop,
                                           'invert': pin_params['invert'],
                                           'pullup': pin_params['pullup']}
-            name = stepper.get_name(short=True)
             self.endstops.append((mcu_endstop, name))
-            query_endstops = printer.load_object(config, 'query_endstops')
-            query_endstops.register_endstop(mcu_endstop, name)
+            self.query_endstops.register_endstop(mcu_endstop, name)
         else:
             mcu_endstop = endstop['endstop']
             changed_invert = pin_params['invert'] != endstop['invert']
             changed_pullup = pin_params['pullup'] != endstop['pullup']
             if changed_invert or changed_pullup:
-                raise error("Printer rail %s shared endstop pin %s "
-                            "must specify the same pullup/invert settings" % (
-                                self.get_name(), pin_name))
+                raise self.printer.config_error(
+                        "Printer rail %s shared endstop pin %s "
+                        "must specify the same pullup/invert settings" % (
+                            self.get_name(), pin_name))
+        return mcu_endstop
+    def add_stepper(self, stepper, endstop_pin=None, endstop_name=None):
+        if not self.steppers:
+            self.get_commanded_position = stepper.get_commanded_position
+            self.calc_position_from_coord = stepper.calc_position_from_coord
+        self.steppers.append(stepper)
+        if endstop_pin is not None:
+            mcu_endstop = self.lookup_endstop(
+                    endstop_pin, endstop_name or stepper.get_name(short=True))
+        else:
+            mcu_endstop = self.lookup_endstop(self.endstop_pin, self.name)
         mcu_endstop.add_stepper(stepper)
+    def add_stepper_from_config(self, config):
+        stepper = PrinterStepper(config, self.stepper_units_in_radians)
+        self.add_stepper(stepper, config.get('endstop_pin', None))
     def setup_itersolve(self, alloc_func, *params):
         for stepper in self.steppers:
             stepper.setup_itersolve(alloc_func, *params)
@@ -441,13 +450,21 @@ class PrinterRail:
         for stepper in self.steppers:
             stepper.set_position(coord)
 
+def LookupRail(config, need_position_minmax=True,
+               default_position_endstop=None, units_in_radians=False):
+    rail = GenericPrinterRail(config, need_position_minmax,
+                              default_position_endstop, units_in_radians)
+    rail.add_stepper_from_config(config)
+    return rail
+
 # Wrapper for dual stepper motor support
 def LookupMultiRail(config, need_position_minmax=True,
-                 default_position_endstop=None, units_in_radians=False):
-    rail = PrinterRail(config, need_position_minmax,
-                       default_position_endstop, units_in_radians)
+                    default_position_endstop=None, units_in_radians=False):
+    rail = LookupRail(config, need_position_minmax,
+                      default_position_endstop, units_in_radians)
     for i in range(1, 99):
         if not config.has_section(config.get_name() + str(i)):
             break
-        rail.add_extra_stepper(config.getsection(config.get_name() + str(i)))
+        rail.add_stepper_from_config(
+                config.getsection(config.get_name() + str(i)))
     return rail