h3n3/klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

kinematics: Generic Cartesian kinematics implementation (#6815)

Browse Source 
* 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>
This commit is contained in:
Dmitry Butyugin
2025-05-07 00:06:36 +02:00
committed by GitHub
parent 1cc6398074
commit cc6736c3e3
27 changed files with 1855 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

294
klippy/kinematics/idex_modes.py
View File

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