From 97669df88e380cca69259862a05c260183116fa1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 9 Nov 2026 23:51:31 +1100 Subject: [PATCH] motion: a kinematics switch reads the joints where the planner put them A kinematics switch re-reads the pose from the joints, and it read them at pos_cmd, the cubic interpolation behind the planner. Task issues the switch once motion reports the moves before it done, which is when the planner is done; pos_cmd is a few servo cycles behind it then. With task on a 1 ms cycle the switch reaches motion inside that window, the pose is read off the joints short of the end of the move, the planner is put there, and the joints stay there. On the kins-switch sim with task on 1 ms, a G12.1 right after a G0 W0 left W short of 0: by 0.000025 mm with the sim's former limits, in 18 runs of 20, and by 0.0083 mm with the faster ones of #4807, in every run. With task on its usual 10 ms cycle the window has passed before the switch arrives. The switch now reads coarse_pos, where the planner has put the joints, as the re-read after a tool offset change already does. The interpolation then finishes the move under the new kinematics. In free mode coarse_pos is pos_cmd, so a switch there is unchanged. tests/kins-switch runs task on a 1 ms cycle and checks that W is back on 0 after the program's G0 W0 and the switch after it: 3 failures in 3 runs before, passing after, also with six copies beside four busy loops and pinned to one CPU. --- src/emc/motion/control.c | 9 +++++---- tests/kins-switch/test-ui.py | 9 +++++++++ tests/kins-switch/test.ini | 4 +++- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 22cbfdaf5a4..1a95e90fb6c 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -347,12 +347,13 @@ static void handle_kinematicsSwitch(void) { emcmot_joint_t *jointKinsSwitch; double joint_posKinsSwitch[EMCMOT_MAX_JOINTS] = {0,}; - /* copy joint position feedback to local array */ + /* the joints where the planner has put them: pos_cmd is the cubic + interpolation behind it, still a few servo cycles short of the end + of a move motion reports done, and a pose read off it would hold + the joints there */ for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { - /* point to joint struct */ jointKinsSwitch = &joints[joint_num]; - /* copy feedback */ - joint_posKinsSwitch[joint_num] = jointKinsSwitch->pos_cmd; + joint_posKinsSwitch[joint_num] = jointKinsSwitch->coarse_pos; } /* a module refuses a type it does not provide and goes on running the diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index d245113f5ab..44b73115b3c 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -164,6 +164,15 @@ def mdi(cmd): print("W ran to %.4f, the other joints held to %.2e" % (w_reached, max(strayed[j] for j in CARRIED))) +# the G0 W0 ends on W0 although the switch after it comes as soon as motion +# reports the move done, while the joints are still a few servo cycles +# short of its end: a switch that read the pose there held W short +w_home = settled()[5] +if abs(w_home) > 1e-9: + error("W stopped at %.9f, not on the 0 of the G0 W0 the switch came after" % w_home) +else: + print("W came back to 0 before the switch") + # the program walks 0, 1, 0, 1 and the line after each selection reports # what the interpreter holds and what motion runs; the pin read there is # the check that the selection went through before the next line, which diff --git a/tests/kins-switch/test.ini b/tests/kins-switch/test.ini index 59e2c6d151a..cc47ac69889 100644 --- a/tests/kins-switch/test.ini +++ b/tests/kins-switch/test.ini @@ -38,7 +38,9 @@ COMM_TIMEOUT = 4 [TASK] TASK = milltask -CYCLE_TIME = 0.010 +# a short cycle: the switch after the program's G0 W0 reaches motion within +# a few servo cycles of the move's end +CYCLE_TIME = 0.001 [EMCIO] TOOL_TABLE = tool.tbl