From 39d6f928015b65742bef2039c8c381eb7c9096f4 Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Tue, 31 Mar 2026 09:09:59 +0800 Subject: [PATCH] motion/control: guard jerk decomposition for identity kinematics only The S-curve jerk decomposition assumed joint[0,1,2] = X,Y,Z, which is only true for identity kinematics. For KINEMATICS_BOTH (e.g. XXYYYZ or rotatekins) joint indices don't map to axis letters, making the decomposition incorrect. Guard with KINEMATICS_IDENTITY check so non-trivial kinematics fall back to cubic interpolator jerk values. --- src/emc/motion/control.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index d139def4fb3..fd42ac027fe 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1398,21 +1398,17 @@ static void get_pos_cmds(long period) joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, &(joint->vel_cmd), &(joint->acc_cmd), &(joint->jerk_cmd)); } - /* Use accurate jerk values from TP output (for Cartesian machines only) - * For standard XYZ machines, joint[0-2] correspond to X, Y, Z axes - * TP outputs: current_jerk (path jerk) and current_dir (direction unit vector) - * Per-axis jerk = path_jerk * direction_component + /* Use accurate jerk values from TP output for identity kinematics only. + * For KINEMATICS_BOTH (non-trivial joint mapping), joint indices don't + * necessarily correspond to XYZ axes, so keep cubic interpolator values. */ - if (emcmotStatus->planner_type == 1) { - // S-curve mode: use accurate jerk values + if (emcmotStatus->planner_type == 1 + && emcmotConfig->kinType == KINEMATICS_IDENTITY) { double path_jerk = emcmotStatus->current_jerk; PmCartesian dir = emcmotStatus->current_dir; - - // For the first 3 joints (assuming X, Y, Z), use accurate jerk if (NO_OF_KINS_JOINTS >= 1) joints[0].jerk_cmd = path_jerk * dir.x; if (NO_OF_KINS_JOINTS >= 2) joints[1].jerk_cmd = path_jerk * dir.y; if (NO_OF_KINS_JOINTS >= 3) joints[2].jerk_cmd = path_jerk * dir.z; - // Rotary axes (A, B, C) keep the cubic interpolator values for now } /* report motion status */