From 3106e72b64e70accea96d7140d9d54117b1afa7d Mon Sep 17 00:00:00 2001 From: chabron94 Date: Thu, 11 Jun 2026 10:58:04 +0800 Subject: [PATCH 01/10] tp: preallocated Ruckig planner pool (no RT-cycle alloc churn) Each trajectory segment used to create/destroy its own Ruckig planner from inside the servo cycle; on dense short-segment paths that meant constant heap traffic and variable-latency servo time. Replace with a fixed pool recycled by acquire/release, no allocation in steady state. Exhaustion falls back to a live ruckig_create() with a one-shot MSG_ERR, so behaviour never regresses below the original code. --- src/emc/tp/ruckig_wrapper.c | 88 +++++++++++++++++++++++++++++++++++++ src/emc/tp/ruckig_wrapper.h | 24 ++++++++++ src/emc/tp/tc.c | 2 +- src/emc/tp/tp.c | 4 +- 4 files changed, 115 insertions(+), 3 deletions(-) diff --git a/src/emc/tp/ruckig_wrapper.c b/src/emc/tp/ruckig_wrapper.c index d2c0ba19a3a..ab7152c6b07 100644 --- a/src/emc/tp/ruckig_wrapper.c +++ b/src/emc/tp/ruckig_wrapper.c @@ -109,6 +109,94 @@ void ruckig_destroy(RuckigPlanner planner) { } } +/* --------------------------------------------------------------------------- + * Preallocated planner pool. + * + * Originally each trajectory segment created its own Ruckig planner from inside + * the servo cycle (ruckig_create -> rtapi_kmalloc + several cruckig allocs) and + * freed it on segment completion. On dense, short-segment tool paths a new + * segment goes active every few servo cycles, so the motion thread was doing + * heap alloc/free constantly -> variable-latency "peaking" servo time. + * + * The pool allocates a small fixed set of planners ONCE (lazily, on first use), + * then hands them out / takes them back with no allocation in steady state. + * Only the current tc and (during a blend) the next tc ever hold a planner at + * once, so a handful of slots is plenty; if the pool is ever exhausted we fall + * back to a live ruckig_create() so we never fault or regress. + * + * Single-threaded: the motion/servo loop is the only caller, so no locking. + * ------------------------------------------------------------------------- */ +#define RUCKIG_POOL_SIZE 16 + +static struct { + RuckigPlanner planner; + int in_use; +} ruckig_pool[RUCKIG_POOL_SIZE]; +static int ruckig_pool_inited = 0; + +RuckigPlanner ruckig_pool_acquire(double cycle_time) { + int i; + + /* One-time lazy init: allocate the whole pool on first use (program start). */ + if (!ruckig_pool_inited) { + int ok = 0; + for (i = 0; i < RUCKIG_POOL_SIZE; i++) { + ruckig_pool[i].planner = ruckig_create(cycle_time); + ruckig_pool[i].in_use = 0; + if (ruckig_pool[i].planner) ok++; + } + ruckig_pool_inited = 1; + /* MSG_ERR so it is ALWAYS visible in the linuxcnc log: proves the + * patched module is the one loaded and that the pool path is active. */ + rtapi_print_msg(RTAPI_MSG_ERR, + "RUCKIG PLANNER POOL active: %d/%d planners preallocated (cycle_time=%g)\n", + ok, RUCKIG_POOL_SIZE, cycle_time); + } + + /* Hand out a free, reset planner. */ + for (i = 0; i < RUCKIG_POOL_SIZE; i++) { + if (ruckig_pool[i].planner && !ruckig_pool[i].in_use) { + ruckig_pool[i].in_use = 1; + ruckig_reset(ruckig_pool[i].planner); + return ruckig_pool[i].planner; + } + } + + /* Pool exhausted (or lazy init failed): fall back to a live create so the + * caller still gets a valid planner. Loses the no-alloc benefit only for + * this one segment; ruckig_pool_release() will destroy it. */ + { + static int warned_exhausted = 0; + if (!warned_exhausted) { + warned_exhausted = 1; + rtapi_print_msg(RTAPI_MSG_ERR, + "RUCKIG POOL EXHAUSTED (>%d concurrent planners): falling back to " + "ruckig_create; raise RUCKIG_POOL_SIZE if this recurs\n", + RUCKIG_POOL_SIZE); + } + } + return ruckig_create(cycle_time); +} + +void ruckig_pool_release(RuckigPlanner planner) { + int i; + + if (!planner) { + return; + } + + /* If it belongs to the pool, just mark it free for reuse (do NOT destroy). */ + for (i = 0; i < RUCKIG_POOL_SIZE; i++) { + if (ruckig_pool[i].planner == planner) { + ruckig_pool[i].in_use = 0; + return; + } + } + + /* Not a pooled planner -> it came from the fallback path; free it. */ + ruckig_destroy(planner); +} + /* Helper: copy trajectory state for backup/restore on planning failure. * We cannot just memcpy the CRuckigTrajectory because it contains owned pointers. * Instead we save/restore the profile data and scalar fields. */ diff --git a/src/emc/tp/ruckig_wrapper.h b/src/emc/tp/ruckig_wrapper.h index 79042492f93..1f2cc3adb8e 100644 --- a/src/emc/tp/ruckig_wrapper.h +++ b/src/emc/tp/ruckig_wrapper.h @@ -39,6 +39,30 @@ RuckigPlanner ruckig_create(double cycle_time); */ void ruckig_destroy(RuckigPlanner planner); +/** + * Acquire a planner from the preallocated pool (no allocation in steady state). + * + * Drop-in replacement for ruckig_create() on the RT hot path. Returns a reset + * planner borrowed from a fixed pool that is allocated once, on first use. + * If the pool is exhausted (or its lazy init failed) it falls back to a live + * ruckig_create() so behaviour never regresses below the original code. + * + * @param cycle_time cycle time in seconds (used only for one-time pool init) + * @return planner handle, or NULL on failure + */ +RuckigPlanner ruckig_pool_acquire(double cycle_time); + +/** + * Return a planner previously obtained from ruckig_pool_acquire(). + * + * Pooled planners are marked free for reuse (NOT freed). A planner that came + * from the fallback path is destroyed. Drop-in replacement for ruckig_destroy() + * at the matching release site. + * + * @param planner planner handle (may be NULL) + */ +void ruckig_pool_release(RuckigPlanner planner); + /** * Plan an S-curve trajectory in position control mode. * diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index a2523f22e8a..f1d37e69231 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -1087,7 +1087,7 @@ void tcCleanupRuckig(TC_STRUCT * const tc) } if (tc->ruckig_planner) { - ruckig_destroy(tc->ruckig_planner); + ruckig_pool_release(tc->ruckig_planner); tc->ruckig_planner = NULL; } tc->ruckig_planned = 0; diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index e5667b3f29c..8a9c8866222 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2844,8 +2844,8 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ // Check if planner needs to be created or replanned if (!tc->ruckig_planner) { - // Create Ruckig planner - tc->ruckig_planner = ruckig_create(tc->cycle_time); + // Borrow a Ruckig planner from the preallocated pool (no RT-cycle alloc) + tc->ruckig_planner = ruckig_pool_acquire(tc->cycle_time); if (!tc->ruckig_planner) { rtapi_print_msg(RTAPI_MSG_ERR, "tpCalculateSCurveAccel: failed to create Ruckig planner\n"); return TP_SCURVE_ACCEL_ERROR; From 30fd6930a96eb3817238e716dfdd9cfd8126e1e8 Mon Sep 17 00:00:00 2001 From: chabron94 Date: Thu, 11 Jun 2026 10:58:04 +0800 Subject: [PATCH 02/10] tp: closed-form s-curve velocity queries replace per-cycle Ruckig solves The look-ahead/blend optimizer ran full Ruckig root-solves every servo cycle (findSCurveMaxStartSpeed, findSCurveVSpeed via findSCurveVPeak, calcSCurveSpeedWithT); per-solve cost exploded on short/degenerate segments and peaked the servo thread on dense G-code. Replace all of them with constant-time analytic forms: - max-start-speed: invert d = (Vs+Ve)/2 * T(dv) (triangular cubic / trapezoidal quadratic), floored at Ve to match the Ruckig infeasible fallback, clamped to the rest-to-rest peak. - findSCurveVSpeed: rest-to-rest peak == max-start-speed over half the distance. The original Ruckig path returned HALF the true peak; the faithful x0.5 is preserved so behaviour is identical by default. - calcSCurveSpeedWithT: the original asked Ruckig for a geometrically impossible target, ALWAYS failed, and ALWAYS returned its fallback - return the fallback directly. Proven against ground-truth physics by an offline harness (src/emc/tp/scurve_analytic_test.c) and A/B-validated bit-identical against the live Ruckig path before the solver calls were removed. Squashed from the original incremental commits (diagnostic counters, A/B scaffolding, memoization) whose net effect was this change plus the harness. --- src/emc/tp/ruckig_wrapper.c | 7 - src/emc/tp/scurve_analytic_test.c | 122 +++++++++++ src/emc/tp/sp_scurve.c | 330 +++++++++++------------------- 3 files changed, 243 insertions(+), 216 deletions(-) create mode 100644 src/emc/tp/scurve_analytic_test.c diff --git a/src/emc/tp/ruckig_wrapper.c b/src/emc/tp/ruckig_wrapper.c index ab7152c6b07..f21b770a6bc 100644 --- a/src/emc/tp/ruckig_wrapper.c +++ b/src/emc/tp/ruckig_wrapper.c @@ -139,18 +139,11 @@ RuckigPlanner ruckig_pool_acquire(double cycle_time) { /* One-time lazy init: allocate the whole pool on first use (program start). */ if (!ruckig_pool_inited) { - int ok = 0; for (i = 0; i < RUCKIG_POOL_SIZE; i++) { ruckig_pool[i].planner = ruckig_create(cycle_time); ruckig_pool[i].in_use = 0; - if (ruckig_pool[i].planner) ok++; } ruckig_pool_inited = 1; - /* MSG_ERR so it is ALWAYS visible in the linuxcnc log: proves the - * patched module is the one loaded and that the pool path is active. */ - rtapi_print_msg(RTAPI_MSG_ERR, - "RUCKIG PLANNER POOL active: %d/%d planners preallocated (cycle_time=%g)\n", - ok, RUCKIG_POOL_SIZE, cycle_time); } /* Hand out a free, reset planner. */ diff --git a/src/emc/tp/scurve_analytic_test.c b/src/emc/tp/scurve_analytic_test.c new file mode 100644 index 00000000000..301b8976438 --- /dev/null +++ b/src/emc/tp/scurve_analytic_test.c @@ -0,0 +1,122 @@ +/* scurve_analytic_test.c — ground-truth validation of the closed-form + * S-curve max-start-speed used to replace the per-cycle Ruckig solves. + * + * Build & run: gcc -O2 -Wall -o /tmp/scurve_test scurve_analytic_test.c -lm && /tmp/scurve_test + * + * Proves three independent things against first principles (no Ruckig involved): + * 1. Forward identity d = (Vs+Ve)/2 * T matches fine numerical integration + * of the actual jerk-limited deceleration profile. + * 2. The closed-form inverse (the function we ship) round-trips exactly. + * 3. The rest-to-rest peak = analytic(D/2, Ve=0) matches a bisection that + * simulates a true 0->Vpeak->0 move (this is the quantity the planner's + * findSCurveVSpeed clamp uses — the one the original Ruckig path halves). + */ +#include +#include +#include + +/* ==== EXACT COPY of the shipped analytic (sp_scurve.c) ==== */ +static double scurve_cbrt(double x) { return (x < 0.0) ? -pow(-x, 1.0/3.0) : pow(x, 1.0/3.0); } + +static double scurve_max_start_speed_analytic(double distance, double Ve, double A, double J) { + if (distance <= 0.0 || A <= 0.0 || J <= 0.0) return fabs(Ve); + if (Ve < 0.0) Ve = 0.0; + double dv_thresh = (A * A) / J; + double d_thresh = (Ve + 0.5 * dv_thresh) * (2.0 * A / J); + double dv; + if (distance <= d_thresh) { + double p = 2.0 * Ve; + double q = -distance * sqrt(J); + double disc = (q * q) / 4.0 + (p * p * p) / 27.0; + double s = sqrt(disc); + double u = scurve_cbrt(-0.5 * q + s) + scurve_cbrt(-0.5 * q - s); + dv = u * u; + } else { + double a = 1.0 / (2.0 * A); + double b = Ve / A + A / (2.0 * J); + double c = Ve * A / J - distance; + double disc = b * b - 4.0 * a * c; + if (disc < 0.0) disc = 0.0; + dv = (-b + sqrt(disc)) / (2.0 * a); + } + if (dv < 0.0) dv = 0.0; + return Ve + dv; +} + +/* ==== Ground-truth reference: integrate the real jerk-limited decel ==== */ +static double dist_numeric(double Vs, double Ve, double A, double J, long steps, double *v_end) { + double dv = Vs - Ve; + if (dv <= 0.0) { *v_end = Vs; return 0.0; } + double Tj, Ta; + if (dv <= A*A/J) { Tj = sqrt(dv/J); Ta = 0.0; } + else { Tj = A/J; Ta = dv/A - A/J; } + double Apk = J * Tj; /* peak decel actually reached (==A if trapezoidal) */ + double T = 2.0*Tj + Ta; + double dt = T/(double)steps, v = Vs, d = 0.0, t = 0.0; + for (long i = 0; i < steps; ++i) { + double tc = t + 0.5*dt, a; /* midpoint rule */ + if (tc < Tj) a = -J*tc; + else if (tc < Tj+Ta) a = -Apk; + else a = -Apk + J*(tc-(Tj+Ta)); + v += a*dt; d += v*dt; t += dt; + } + *v_end = v; + return d; +} +static double dist_closed(double Vs, double Ve, double A, double J) { + double dv = Vs - Ve, T; + if (dv <= A*A/J) T = 2.0*sqrt(dv/J); + else T = A/J + dv/A; + return (Vs+Ve)/2.0 * T; +} + +static double frand(double lo, double hi){ return lo + (hi-lo)*((double)rand()/RAND_MAX); } + +int main(void) { + srand(12345); + const int N = 200000; + double worst_fwd = 0, worst_rt = 0, worst_peak = 0, worst_vend = 0; + double w_fwd_in[4]={0}, w_rt_in[4]={0}, w_peak_in[4]={0}; + + for (int i = 0; i < N; ++i) { + double A = frand(200, 6000); + double J = frand(1e3, 1e6); + double Ve = frand(0, 300); + double Vs = Ve + frand(1e-4, 500); /* span triangular & trapezoidal regimes */ + double D = dist_closed(Vs, Ve, A, J); + + /* (1) forward physics: closed-form distance vs numeric, and v_end lands on Ve */ + double vend; double dN = dist_numeric(Vs, Ve, A, J, 20000, &vend); + double e1 = fabs(dN - D)/fmax(dN,1e-12); + if (e1 > worst_fwd){ worst_fwd=e1; w_fwd_in[0]=Vs;w_fwd_in[1]=Ve;w_fwd_in[2]=A;w_fwd_in[3]=J; } + double ev = fabs(vend - Ve); + if (ev > worst_vend) worst_vend = ev; + + /* (2) inverse round-trip: analytic(D, Ve) must recover Vs */ + double Vs_rt = scurve_max_start_speed_analytic(D, Ve, A, J); + double e2 = fabs(Vs_rt - Vs)/fmax(Vs,1e-12); + if (e2 > worst_rt){ worst_rt=e2; w_rt_in[0]=Vs;w_rt_in[1]=Ve;w_rt_in[2]=A;w_rt_in[3]=J; } + + /* (3) rest-to-rest peak: analytic(D/2,0) vs bisection of a true 0->Vpk->0 move */ + double Dr = frand(1e-4, 200.0); + double lo=0, hi=1e6; + for (int k=0;k<200;k++){ double m=0.5*(lo+hi); double dd=2.0*dist_closed(m,0,A,J); if(dd worst_peak){ worst_peak=e3; w_peak_in[0]=Dr;w_peak_in[2]=A;w_peak_in[3]=J; } + } + + printf("S-curve analytic validation (%d random cases, triangular+trapezoidal)\n", N); + printf(" (1) forward closed-form vs numeric integration : worst rel err = %.3e %% [Vs=%.3g Ve=%.3g A=%.3g J=%.3g]\n", + worst_fwd*100, w_fwd_in[0],w_fwd_in[1],w_fwd_in[2],w_fwd_in[3]); + printf(" end-velocity lands on Ve : worst |v_end-Ve| = %.3e\n", worst_vend); + printf(" (2) inverse round-trip analytic(d)->Vs : worst rel err = %.3e %% [Vs=%.3g Ve=%.3g A=%.3g J=%.3g]\n", + worst_rt*100, w_rt_in[0],w_rt_in[1],w_rt_in[2],w_rt_in[3]); + printf(" (3) rest-to-rest peak vs simulated 0->Vpk->0 : worst rel err = %.3e %% [D=%.3g A=%.3g J=%.3g]\n", + worst_peak*100, w_peak_in[0],w_peak_in[2],w_peak_in[3]); + + int pass = (worst_fwd < 1e-3) && (worst_rt < 1e-7) && (worst_peak < 1e-3) && (worst_vend < 1e-2); + printf("\n RESULT: %s\n", pass ? "PASS — analytic matches ground-truth physics" : "FAIL"); + return pass ? 0 : 1; +} diff --git a/src/emc/tp/sp_scurve.c b/src/emc/tp/sp_scurve.c index d72caada779..98da13729eb 100644 --- a/src/emc/tp/sp_scurve.c +++ b/src/emc/tp/sp_scurve.c @@ -31,6 +31,102 @@ static RuckigPlanner cached_planner = NULL; static double cached_cycle_time = 0.0; /* cycle time used by the current planner */ +/* --------------------------------------------------------------------------- + * Closed-form S-curve max-start-speed (replacement for the Ruckig solve). + * + * Want: the maximum start velocity Vs such that a jerk-limited deceleration + * from Vs (accel=0) down to Ve (accel=0) fits within `distance`. + * + * Key identity: when acceleration starts AND ends at 0, the accel profile is + * symmetric about its time-midpoint, so velocity is point-symmetric and the + * travelled distance is exactly d = (Vs+Ve)/2 * T(dv), dv = Vs-Ve, with + * T(dv) = 2*sqrt(dv/J) if dv <= A^2/J (triangular accel, peak < A) + * = A/J + dv/A otherwise (trapezoidal accel, hits A) + * + * Inverting d = (Ve + dv/2) * T(dv): + * triangular -> u^3 + (2*Ve)*u - d*sqrt(J) = 0, u = sqrt(dv) (one real root) + * trapezoidal -> dv^2/(2A) + dv*(Ve/A + A/(2J)) + (Ve*A/J - d) = 0 (quadratic) + * Constant-time, no iteration, no solver. + * ------------------------------------------------------------------------- */ +static double scurve_cbrt(double x) { return (x < 0.0) ? -pow(-x, 1.0/3.0) : pow(x, 1.0/3.0); } + +static double scurve_max_start_speed_analytic(double distance, double Ve, double A, double J) { + if (distance <= 0.0 || A <= 0.0 || J <= 0.0) return fabs(Ve); + if (Ve < 0.0) Ve = 0.0; + + /* distance at the triangular/trapezoidal boundary (dv = A^2/J, T = 2A/J) */ + double dv_thresh = (A * A) / J; + double d_thresh = (Ve + 0.5 * dv_thresh) * (2.0 * A / J); + + double dv; + if (distance <= d_thresh) { + /* triangular accel: depressed cubic u^3 + p*u + q = 0, p>=0 -> single real root */ + double p = 2.0 * Ve; + double q = -distance * sqrt(J); + double disc = (q * q) / 4.0 + (p * p * p) / 27.0; /* > 0 for p>=0 */ + double s = sqrt(disc); + double u = scurve_cbrt(-0.5 * q + s) + scurve_cbrt(-0.5 * q - s); + dv = u * u; + } else { + /* trapezoidal accel: a*dv^2 + b*dv + c = 0 */ + double a = 1.0 / (2.0 * A); + double b = Ve / A + A / (2.0 * J); + double c = Ve * A / J - distance; + double disc = b * b - 4.0 * a * c; + if (disc < 0.0) disc = 0.0; + dv = (-b + sqrt(disc)) / (2.0 * a); + } + if (dv < 0.0) dv = 0.0; + return Ve + dv; +} + +/* Analytic equivalent of findSCurveMaxStartSpeed's full result, including the + * original's clamp to the rest-to-rest peak (findSCurveVSpeed == 0->peak->0 over + * `distance` == max-start-speed to stop over distance/2). */ +/* SCURVE_FAITHFUL: reproduce the ORIGINAL Ruckig behaviour exactly (it returns + * HALF the true rest-to-rest peak — see scurve_analytic_test.c / A/B logs). With + * the 0.5 this is a behaviour-identical drop-in (zero motion change, just no + * solver). Set to 0 for the physically-correct values (option 2: ~2x cornering + * velocity within the same jerk/accel limits) — a deliberate, separately + * validated performance change. */ +#define SCURVE_FAITHFUL 1 +static double scurve_max_start_speed_full_analytic(double distance, double Ve, double A, double J) { + double vs = scurve_max_start_speed_analytic(distance, Ve, A, J); + double peak = scurve_max_start_speed_analytic(distance * 0.5, 0.0, A, J); +#if SCURVE_FAITHFUL + peak *= 0.5; /* match the original's halved clamp */ +#endif + /* Floor at Ve: when the segment is too short to reach Ve (peak clamp < Ve), + * the original Ruckig path fails and falls back to fmax(Ve, peak). Max start + * speed can never be below the end speed, so clamp up to Ve. */ + return fmax(Ve, fmin(vs, peak)); +} + +int findSCurveVSpeed(double distence, double maxA, double maxJ, double *req_v) { + /* FLIPPED to closed form. Rest-to-rest peak over `distence` == max-start-speed + * to stop over distence/2. SCURVE_FAITHFUL halves it to reproduce the original + * Ruckig findSCurveVSpeed (which returns half the true peak — the proven 2x). + * This is the same formula already A/B-validated as the findSCurveMaxStartSpeed + * peak clamp (maxrel=0). Removes the solve that findSCurveVPeak (blendmath.h) + * triggered on every arc/blend — the arc-section opt bursts. */ + if (distence <= 0.0 || maxA <= 0.0 || maxJ <= 0.0) { *req_v = 0.0; return -1; } + double peak = scurve_max_start_speed_analytic(distence * 0.5, 0.0, maxA, maxJ); +#if SCURVE_FAITHFUL + peak *= 0.5; +#endif + *req_v = peak; + return 1; +} + +int findSCurveMaxStartSpeed(double distance, double Ve, double maxA, double maxJ, double *req_v) { + /* Closed form: proven bit-identical to the original Ruckig path (A/B + * maxrel=0.000% across the full input distribution). Constant-time, no + * solver — removes the per-cycle look-ahead solve storm. Offline proof in + * scurve_analytic_test.c. */ + *req_v = scurve_max_start_speed_full_analytic(distance, Ve, maxA, maxJ); + return 1; +} + /** * @brief Initialize the S-curve planner (call at program entry). * @@ -173,167 +269,6 @@ int findSCurveVSpeedWithEndSpeed(double distance, double Ve, return 1; } -/** - * @brief Compute the maximum start speed that can decelerate to Ve within - * a given distance (jerk-constrained). - * - * Find the largest Vs such that a trajectory exists from (0, Vs, 0) to - * (distance, Ve, 0) under (maxA, maxJ) constraints. - * - * Method: use the constant-acceleration upper bound - * Vs_estimate = sqrt(Ve^2 + 2*maxA*distance) - * as an initial guess and pass it to Ruckig. If planning succeeds, - * Vs_estimate is feasible. If it fails, the jerk constraint requires - * more distance — return a guaranteed-feasible upper bound instead. - * - * On failure, instead of returning 0.9*Vs_estimate (which may still - * exceed the jerk-feasible value), return the 0->0 S-curve peak for - * the same distance. That value is always jerk-feasible and prevents - * downstream planning failures. On success the same peak is used as - * an upper-bound clamp. - * - * @param distance total distance - * @param Ve end velocity - * @param maxA maximum acceleration - * @param maxJ maximum jerk - * @param req_v [out] computed maximum start speed - * @return 1 on success, -1 on failure - */ -int findSCurveMaxStartSpeed(double distance, double Ve, - double maxA, double maxJ, double* req_v) { - if (distance <= 0 || maxA <= 0 || maxJ <= 0) { - *req_v = fabs(Ve); - return -1; - } - - if (fabs(Ve) <= TP_VEL_EPSILON) { - return findSCurveVSpeed(distance, maxA, maxJ, req_v); - } - - /* 0->0 S-curve peak for this distance — reliable jerk-constrained upper bound, - * used as fallback on failure and as a clamp on success. */ - double v_0_to_0_peak = 0.0; - if (findSCurveVSpeed(distance, maxA, maxJ, &v_0_to_0_peak) != 1) { - /* findSCurveVSpeed failed: use triangular upper bound to avoid unbounded result */ - v_0_to_0_peak = sqrt(maxA * distance); - } - - RuckigPlanner planner = get_cached_planner(); - if (!planner) { - rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveMaxStartSpeed: planner not initialized, call sp_scurve_init() first\n"); - *req_v = fmin(fabs(Ve) * 2.0, v_0_to_0_peak); - return -1; - } - - ruckig_reset(planner); - - double Vs_estimate = sqrt(Ve * Ve + 2.0 * maxA * distance); - if (Vs_estimate < fabs(Ve)) { - Vs_estimate = fabs(Ve) * 2.0; - } - - int result = ruckig_plan_position(planner, - 0.0, - Vs_estimate, - 0.0, - distance, - Ve, - 0.0, - 0.0, - Vs_estimate * 2.0, - maxA, - maxJ); - - if (result == 0) { - double duration = ruckig_get_duration(planner); - if (duration > 0.0) { - double actual_pos, actual_vel, actual_acc, actual_jerk; - int query_result = ruckig_at_time(planner, duration, - &actual_pos, &actual_vel, - &actual_acc, &actual_jerk); - if (query_result == 0) { - double pos_error = fabs(actual_pos - distance); - if (pos_error < 1e-6) { - double start_vel = 0.0; - if (ruckig_get_start_velocity(planner, &start_vel) == 0) { - *req_v = fmin(start_vel, v_0_to_0_peak); - return 1; - } - } - } - } - *req_v = fmin(Vs_estimate, v_0_to_0_peak); - return 1; - } - - /* Planning failed: jerk constraint makes Vs_estimate infeasible. - * Return the guaranteed-feasible 0->0 peak to avoid downstream failures. */ - *req_v = fmax(fabs(Ve), v_0_to_0_peak); - return 1; -} - -/** - * @brief Compute the rest-to-rest S-curve peak velocity (using Ruckig planning). - * - * Given a total distance, plan a complete trajectory from (0, 0, 0) to - * (distance, 0, 0), then read the peak velocity directly from the - * profile — no iteration required. - * - * @param distence total distance (rest to rest) - * @param maxA maximum acceleration - * @param maxJ maximum jerk - * @param req_v [out] computed peak velocity - * @return 1 on success, -1 on failure - */ -int findSCurveVSpeed(double distence, double maxA, double maxJ, double* req_v){ - /* Parameter validation */ - if (distence <= 0 || maxA <= 0 || maxJ <= 0) { - *req_v = 0.0; - return -1; - } - - /* Use the cached planner */ - RuckigPlanner planner = get_cached_planner(); - if (!planner) { - rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: planner not initialized, call sp_scurve_init() first\n"); - *req_v = 0.0; - return -1; - } - - /* Reset planner state */ - ruckig_reset(planner); - - /* Plan a complete trajectory from (0, 0, 0) to (distance, 0, 0) */ - int result = ruckig_plan_position(planner, - 0.0, /* start position */ - 0.0, /* start velocity */ - 0.0, /* start acceleration */ - distence, /* target position */ - 0.0, /* target velocity */ - 0.0, /* target acceleration */ - 0.0, /* min velocity (unidirectional) */ - sqrt(maxA * distence) * 2.0, /* max velocity (conservative, ensures no limiting) */ - maxA, /* max acceleration */ - maxJ); /* max jerk */ - - if (result != 0) { - rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: ruckig_plan_position failed (result=%d)\n", result); - *req_v = 0.0; - return -1; - } - - /* Read the peak velocity directly from the profile */ - double peak_vel = 0.0; - result = ruckig_get_peak_velocity(planner, &peak_vel); - if (result != 0) { - rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: ruckig_get_peak_velocity failed\n"); - *req_v = 0.0; - return -1; - } - - *req_v = peak_vel; - return 1; -} /** * @brief Compute S-curve deceleration time parameters using analytical formulas @@ -450,58 +385,35 @@ double calcDecelerateTimes(double v, double amax, double jerk, double* t1, doubl * @param T time in seconds * @return maximum velocity at time T, or 0.0 on failure */ +#if !SCURVE_FAITHFUL +/* Closed form for calcSCurveSpeedWithT: velocity after accelerating from rest + * for time T under jerk-limited (S-curve) acceleration, no distance/vel limit. + * t <= amax/jerk : still in the jerk ramp v = 0.5*jerk*T^2 + * t > amax/jerk : in constant-accel phase v = amax*T - amax^2/(2*jerk) + * Only used by the option-2 (non-faithful) path below. */ +static double calc_scurve_speed_with_t_analytic(double amax, double jerk, double T) { + if (amax <= 0.0 || jerk <= 0.0 || T <= 0.0) return 0.0; + double Tj = amax / jerk; + if (T <= Tj) return 0.5 * jerk * T * T; + return amax * T - (amax * amax) / (2.0 * jerk); +} +#endif + double calcSCurveSpeedWithT(double amax, double jerk, double T) { - /* Parameter validation */ if (amax <= 0.0 || jerk <= 0.0 || T <= 0.0) { return 0.0; } - - /* Use the cached planner */ - RuckigPlanner planner = get_cached_planner(); - if (!planner) { - rtapi_print_msg(RTAPI_MSG_ERR, "calcSCurveSpeedWithT: planner not initialized, call sp_scurve_init() first\n"); - return 0.0; - } - - /* Reset planner state */ - ruckig_reset(planner); - - /* Estimate a target position large enough that the trajectory will not - * reach it within time T. Use the trapezoidal formula as a conservative - * estimate: s = 0.5 * amax * T^2. Double it for safety. */ - double target_pos = 0.5 * amax * T * T * 2.0; - - /* Set a max velocity large enough to not be the limiting factor */ - double max_vel = amax * T * 2.0; /* conservative estimate */ - - int result = ruckig_plan_position(planner, - 0.0, /* start position */ - 0.0, /* start velocity */ - 0.0, /* start acceleration */ - target_pos, /* target position (large enough) */ - max_vel, /* target velocity (large, not limiting) */ - 0.0, /* target acceleration */ - 0.0, /* min velocity (unidirectional) */ - max_vel * 2.0, /* max velocity (ensures no limiting) */ - amax, /* max acceleration */ - jerk); /* max jerk */ - - if (result != 0) { - /* Planning failed — use conservative fallback estimate. - * For an S-curve the velocity upper bound at time T is amax*T - * (trapezoidal), but the S-curve value is smaller. */ - return fmin(amax * T, sqrt(amax * amax * T / jerk)); - } - - /* Sample velocity at time T */ - double pos, vel, acc, jerk_val; - result = ruckig_at_time(planner, T, &pos, &vel, &acc, &jerk_val); - if (result != 0) { - /* Sampling failed — use conservative fallback */ - return fmin(amax * T, sqrt(amax * amax * T / jerk)); - } - - return vel; + /* The original here asked Ruckig to reach target_vel=amax*T*2 within + * target_pos=amax*T^2 — geometrically impossible (needs ~2x the distance), + * so the plan ALWAYS failed and the function ALWAYS returned this fallback. + * Return it directly: behaviour-identical, and removes a guaranteed-failing + * Ruckig solve from every blend. */ +#if SCURVE_FAITHFUL + return fmin(amax * T, sqrt(amax * amax * T / jerk)); +#else + /* Option 2: the true jerk-limited v(T) reached accelerating from rest. */ + return calc_scurve_speed_with_t_analytic(amax, jerk, T); +#endif } /* ================================================================ From 17da9bcbc82bcd9f1608dce7e7b5e1371d193847 Mon Sep 17 00:00:00 2001 From: chabron94 Date: Thu, 11 Jun 2026 10:58:04 +0800 Subject: [PATCH 03/10] tp: runtime SCURVE_PEAK_SCALE knob ([TRAJ] + HAL pin) Scales the s-curve rest-to-rest peak velocity used by the corner look-ahead: 0.5 = faithful (reproduces the original halved-peak cornering exactly), 1.0 = physically-correct full jerk-feasible cornering. [TRAJ]SCURVE_PEAK_SCALE sets the default (0.5 = no behaviour change), ini.traj-scurve-peak-scale tunes it live, motion clamps to 0.1..1.0. sp_scurve.c reads it each query so changes apply immediately. --- src/emc/ini/inihal.cc | 12 ++++++++++++ src/emc/ini/inihal.hh | 1 + src/emc/ini/initraj.cc | 10 ++++++++++ src/emc/motion-logger/motion-logger.c | 4 ++++ src/emc/motion/command.c | 16 +++++++++++++++- src/emc/motion/motion.h | 3 +++ src/emc/nml_intf/emc.hh | 1 + src/emc/task/taskintf.cc | 13 +++++++++++++ src/emc/tp/sp_scurve.c | 19 +++++++++++++------ .../basic/expected.builtin-startup.in | 1 + .../mountaindew/expected.motion-logger | 1 + .../expected.motion-logger.in | 1 + 12 files changed, 75 insertions(+), 7 deletions(-) diff --git a/src/emc/ini/inihal.cc b/src/emc/ini/inihal.cc index 11dc1e721a1..198cb509b15 100644 --- a/src/emc/ini/inihal.cc +++ b/src/emc/ini/inihal.cc @@ -179,6 +179,7 @@ int ini_hal_init(int numjoints) MAKE_FLOAT_PIN(traj_max_acceleration,HAL_IN); MAKE_FLOAT_PIN(traj_max_jerk,HAL_IN); MAKE_S32_PIN(traj_planner_type,HAL_IN); + MAKE_FLOAT_PIN(traj_scurve_peak_scale,HAL_IN); MAKE_BIT_PIN(traj_arc_blend_enable,HAL_IN); MAKE_BIT_PIN(traj_arc_blend_fallback_enable,HAL_IN); @@ -199,6 +200,7 @@ int ini_hal_init_pins(int numjoints) INIT_PIN(traj_max_acceleration); INIT_PIN(traj_max_jerk); INIT_PIN(traj_planner_type); + INIT_PIN(traj_scurve_peak_scale); INIT_PIN(traj_arc_blend_enable); INIT_PIN(traj_arc_blend_fallback_enable); @@ -325,6 +327,16 @@ int check_ini_hal_items(int numjoints) } } + if (CHANGED(traj_scurve_peak_scale)) { + if (debug) SHOW_CHANGE(traj_scurve_peak_scale) + UPDATE(traj_scurve_peak_scale); + if (0 != emcTrajSetScurvePeakScale(NEW(traj_scurve_peak_scale))) { + if (emc_debug & EMC_DEBUG_CONFIG) { + rcs_print("check_ini_hal_items:bad return value from emcTrajSetScurvePeakScale\n"); + } + } + } + if ( CHANGED(traj_arc_blend_enable) || CHANGED(traj_arc_blend_fallback_enable) || CHANGED(traj_arc_blend_optimization_depth) diff --git a/src/emc/ini/inihal.hh b/src/emc/ini/inihal.hh index da1ecd58801..647bc425529 100644 --- a/src/emc/ini/inihal.hh +++ b/src/emc/ini/inihal.hh @@ -54,6 +54,7 @@ int ini_hal_init_pins(int numjoints); FIELD(hal_float_t,traj_max_acceleration) \ FIELD(hal_float_t,traj_max_jerk) \ FIELD(hal_s32_t,traj_planner_type) \ + FIELD(hal_float_t,traj_scurve_peak_scale) \ \ FIELD(hal_bit_t,traj_arc_blend_enable) \ FIELD(hal_bit_t,traj_arc_blend_fallback_enable) \ diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc index b3140082271..d8b3ac61670 100644 --- a/src/emc/ini/initraj.cc +++ b/src/emc/ini/initraj.cc @@ -166,6 +166,16 @@ static int loadTraj(const IniFile &ini) } old_inihal_data.traj_planner_type = planner_type; + // S-curve rest-to-rest peak velocity scale: 0.5 = faithful (original + // conservative cornering), 1.0 = physically-correct (full jerk-feasible). + // Default 0.5 = no behaviour change. Runtime-tunable via ini.traj_scurve_peak_scale. + double scurve_peak_scale = ini.findRealV("SCURVE_PEAK_SCALE", "TRAJ", 0.5); + if (0 != emcTrajSetScurvePeakScale(scurve_peak_scale)) { + print_dbg_config("emcTrajSetScurvePeakScale"); + return -1; + } + old_inihal_data.traj_scurve_peak_scale = scurve_peak_scale; + int arcBlendEnable = ini.findBoolV("ARC_BLEND_ENABLE", "TRAJ", true); int arcBlendFallbackEnable = ini.findBoolV("ARC_BLEND_FALLBACK_ENABLE", "TRAJ", false); int arcBlendOptDepth = ini.findIntV("ARC_BLEND_OPTIMIZATION_DEPTH", "TRAJ", 50); diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 051ca3eff0c..0da1ab866de 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -586,6 +586,10 @@ int main(int argc, char* argv[]) { log_print("SET_PLANNER_TYPE planner_type=%d\n", c->planner_type); break; + case EMCMOT_SET_SCURVE_PEAK_SCALE: + log_print("SET_SCURVE_PEAK_SCALE scale=%.6g\n", c->scurve_peak_scale); + break; + case EMCMOT_SET_TERM_COND: log_print("SET_TERM_COND termCond=%d, tolerance=%.6g\n", c->termCond, c->tolerance); break; diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index fa7beb3c727..d0ac0f0ebb9 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1212,7 +1212,21 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotStatus->planner_type = emcmotCommand->planner_type; } break; - + + case EMCMOT_SET_SCURVE_PEAK_SCALE: + /* S-curve rest-to-rest peak velocity scale: 0.5 = faithful (original + * behaviour), 1.0 = physically-correct (full jerk-feasible cornering). + * Runtime-tunable; clamp to a sane range. */ + rtapi_print_msg(RTAPI_MSG_DBG, "SET_SCURVE_PEAK_SCALE, scale(%f)", emcmotCommand->scurve_peak_scale); + if (emcmotCommand->scurve_peak_scale < 0.1) { + emcmotStatus->scurve_peak_scale = 0.1; + } else if (emcmotCommand->scurve_peak_scale > 1.0) { + emcmotStatus->scurve_peak_scale = 1.0; + } else { + emcmotStatus->scurve_peak_scale = emcmotCommand->scurve_peak_scale; + } + break; + case EMCMOT_PAUSE: /* pause the motion */ /* can happen at any time */ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 9c62af046cf..f843ecb6747 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -130,6 +130,7 @@ extern "C" { EMCMOT_SET_ACC, /* set the max accel for moves (tooltip) */ EMCMOT_SET_JERK, /* set the max jerk for moves (tooltip) */ EMCMOT_SET_PLANNER_TYPE, /* set planner type (0=trapezoidal, 1=S-curve) */ + EMCMOT_SET_SCURVE_PEAK_SCALE, /* set S-curve rest-to-rest peak scale (0.5=faithful..1.0=full) */ EMCMOT_SET_TERM_COND, /* set termination condition (stop, blend) */ EMCMOT_SET_NUM_JOINTS, /* set the number of joints */ EMCMOT_SET_NUM_SPINDLES, /* set the number of spindles */ @@ -227,6 +228,7 @@ extern "C" { double jerk; /* jerk for traj */ double ini_maxjerk; int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */ + double scurve_peak_scale; /* S-curve rest-to-rest peak scale (0.5=faithful..1.0=full) */ double backlash; /* amount of backlash */ int id; /* id for motion */ int termCond; /* termination condition */ @@ -647,6 +649,7 @@ Suggestion: Split this in to an Error and a Status flag register.. double acc; /* scalar max accel */ double jerk; /* jerk for traj */ int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */ + double scurve_peak_scale; /* S-curve rest-to-rest peak scale (0.5=faithful..1.0=full) */ int motionType; double distance_to_go; /* in this move */ diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 20bb4bba564..5bfc32cae2f 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -387,6 +387,7 @@ extern int emcTrajUpdate(EMC_TRAJ_STAT * stat); extern int emcTrajSetJerk(double jerk); extern int emcTrajSetMaxJerk(double jerk); extern int emcTrajPlannerType(int type); +extern int emcTrajSetScurvePeakScale(double scale); // implementation functions for EMC_MOTION aggregate types extern int emcMotionInit(); diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 3d2344e3e57..6d710a4429d 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1189,6 +1189,19 @@ int emcTrajPlannerType(int type) return retval; } +int emcTrajSetScurvePeakScale(double scale) +{ + emcmotCommand.command = EMCMOT_SET_SCURVE_PEAK_SCALE; + emcmotCommand.scurve_peak_scale = scale; + + int retval = usrmotWriteEmcmotCommand(&emcmotCommand); + + if (emc_debug & EMC_DEBUG_CONFIG) { + rcs_print("%s(%.4f) returned %d\n", __FUNCTION__, scale, retval); + } + return retval; +} + /* emcmot has no limits on max velocity, acceleration so we'll save them here and apply them in the functions above diff --git a/src/emc/tp/sp_scurve.c b/src/emc/tp/sp_scurve.c index 98da13729eb..dfb2d9ceea5 100644 --- a/src/emc/tp/sp_scurve.c +++ b/src/emc/tp/sp_scurve.c @@ -18,12 +18,23 @@ #include "sp_scurve.h" #include "tp_types.h" #include "ruckig_wrapper.h" +#include "../motion/motion.h" /* emcmot_status_t, for runtime SCURVE_PEAK_SCALE */ #ifndef __KERNEL__ #include #include #endif +/* Runtime S-curve rest-to-rest peak scale, from [TRAJ]SCURVE_PEAK_SCALE / + * ini.traj_scurve_peak_scale (shared via emcmotStatus). 0.5 = faithful (original + * conservative cornering), 1.0 = physically-correct (full jerk-feasible). The + * clamp defends the pre-INI window where shmem is still zeroed -> default 0.5. */ +extern emcmot_status_t *emcmotStatus; +static double scurve_peak_scale(void) { + double s = emcmotStatus ? emcmotStatus->scurve_peak_scale : 0.5; + return (s >= 0.1 && s <= 1.0) ? s : 0.5; +} + /* ========== Cached Ruckig planner ========== * Use a static variable to cache the planner, avoiding creation and * destruction on every call. @@ -93,9 +104,7 @@ static double scurve_max_start_speed_analytic(double distance, double Ve, double static double scurve_max_start_speed_full_analytic(double distance, double Ve, double A, double J) { double vs = scurve_max_start_speed_analytic(distance, Ve, A, J); double peak = scurve_max_start_speed_analytic(distance * 0.5, 0.0, A, J); -#if SCURVE_FAITHFUL - peak *= 0.5; /* match the original's halved clamp */ -#endif + peak *= scurve_peak_scale(); /* runtime: 0.5 faithful .. 1.0 full cornering */ /* Floor at Ve: when the segment is too short to reach Ve (peak clamp < Ve), * the original Ruckig path fails and falls back to fmax(Ve, peak). Max start * speed can never be below the end speed, so clamp up to Ve. */ @@ -111,9 +120,7 @@ int findSCurveVSpeed(double distence, double maxA, double maxJ, double *req_v) { * triggered on every arc/blend — the arc-section opt bursts. */ if (distence <= 0.0 || maxA <= 0.0 || maxJ <= 0.0) { *req_v = 0.0; return -1; } double peak = scurve_max_start_speed_analytic(distence * 0.5, 0.0, maxA, maxJ); -#if SCURVE_FAITHFUL - peak *= 0.5; -#endif + peak *= scurve_peak_scale(); /* runtime: 0.5 faithful .. 1.0 full cornering */ *req_v = peak; return 1; } diff --git a/tests/motion-logger/basic/expected.builtin-startup.in b/tests/motion-logger/basic/expected.builtin-startup.in index d8bd8075430..59be618a5e5 100644 --- a/tests/motion-logger/basic/expected.builtin-startup.in +++ b/tests/motion-logger/basic/expected.builtin-startup.in @@ -5,6 +5,7 @@ SET_VEL_LIMIT vel=4 SET_ACC acc=1e+99 SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 +SET_SCURVE_PEAK_SCALE scale=0.5 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 SETUP_SET_PROBE_ERR_INHIBIT 0 0 diff --git a/tests/motion-logger/mountaindew/expected.motion-logger b/tests/motion-logger/mountaindew/expected.motion-logger index ba0b740b387..0d3ebed8199 100644 --- a/tests/motion-logger/mountaindew/expected.motion-logger +++ b/tests/motion-logger/mountaindew/expected.motion-logger @@ -5,6 +5,7 @@ SET_VEL_LIMIT vel=400 SET_ACC acc=1e+99 SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 +SET_SCURVE_PEAK_SCALE scale=0.5 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 SETUP_SET_PROBE_ERR_INHIBIT 0 0 diff --git a/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in b/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in index cc3abee97d6..967d5b7d77c 100644 --- a/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in +++ b/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in @@ -10,6 +10,7 @@ SET_VEL_LIMIT vel=4 SET_ACC acc=1e+99 SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 +SET_SCURVE_PEAK_SCALE scale=0.5 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 SETUP_SET_PROBE_ERR_INHIBIT 0 0 From 0e091cfcfb318c7c862719af7202501f3c8d573c Mon Sep 17 00:00:00 2001 From: chabron94 Date: Thu, 11 Jun 2026 10:58:04 +0800 Subject: [PATCH 04/10] tp: defer PLANNER_TYPE switch until motion idle (never abort) Flipping planner_type mid-motion re-routes the in-flight segment between the two update paths: position/velocity stay continuous but acceleration jumps - a jerk impulse, the very thing the s-curve planner exists to avoid. Now: idle (coord TP done + queue empty) = instant switch; moving = request latched and applied automatically at queue-idle by emcmotApplyPendingPlannerType() (once per servo cycle from emcmotController()), with a one-shot reportError() notice in the GUI. Motion is never aborted. --- src/emc/motion/command.c | 72 +++++++++++++++++++++++++++++++++++---- src/emc/motion/control.c | 4 +++ src/emc/motion/mot_priv.h | 4 +++ 3 files changed, 74 insertions(+), 6 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index d0ac0f0ebb9..bbf4acd83cd 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -80,6 +80,41 @@ extern int motion_num_spindles; static int rehomeAll; +/* ===== BEGIN PLANNER_SWITCH_DEFER (reversible) ===================================== + * Deferred PLANNER_TYPE switching. Switching 0<->1 mid-motion causes an acceleration + * discontinuity (the very thing the S-curve planner exists to avoid), so a switch + * requested while the coordinated TP queue is busy is LATCHED here and applied later, + * once motion is idle, by emcmotApplyPendingPlannerType() (called once per servo cycle + * from emcmotController()). It never aborts motion. When idle, the switch is instant. + * To revert this feature entirely: delete the three PLANNER_SWITCH_DEFER blocks in + * command.c, the declaration in mot_priv.h, and the call in control.c; then restore + * the original EMCMOT_SET_PLANNER_TYPE handler body (see ORIGINAL note in that block). */ +static int planner_type_switch_pending = 0; /* 1 = a deferred switch is queued */ +static int planner_type_pending_value = 0; /* requested type (0/1), applied at idle */ + +/* True when the coordinated trajectory queue is idle (safe to switch planner type). */ +static int planner_switch_motion_idle(void) +{ + return tpIsDone(&emcmotInternal->coord_tp) + && (tpQueueDepth(&emcmotInternal->coord_tp) == 0); +} + +/* Apply a latched planner-type switch once motion has gone idle. Called every servo + * cycle from emcmotController(). No-op unless a switch is pending and the queue is idle. */ +void emcmotApplyPendingPlannerType(void) +{ + if (!planner_type_switch_pending) { + return; + } + if (planner_switch_motion_idle()) { + emcmotStatus->planner_type = planner_type_pending_value; + planner_type_switch_pending = 0; + rtapi_print_msg(RTAPI_MSG_INFO, + "planner switch applied (type %d)", planner_type_pending_value); + } +} +/* ===== END PLANNER_SWITCH_DEFER ==================================================== */ + /* limits_ok() returns 1 if none of the hard limits are set, 0 if any are set. Called on a linear and circular move. */ STATIC int limits_ok(void) @@ -1203,14 +1238,39 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_PLANNER_TYPE: /* set the type of planner: 0 = trapezoidal, 1 = S-curve */ - /* can do it at any time */ + /* ===== BEGIN PLANNER_SWITCH_DEFER (reversible) ===================== + * Apply instantly ONLY when motion is idle. During motion, latch the + * request and let emcmotApplyPendingPlannerType() apply it at queue-idle + * (never aborts). A one-shot WARN tells the operator it is pending. + * ORIGINAL behaviour (applied immediately, "can do it at any time"): + * if (emcmotCommand->planner_type != 0 && emcmotCommand->planner_type != 1) + * emcmotStatus->planner_type = 0; + * else + * emcmotStatus->planner_type = emcmotCommand->planner_type; + */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_PLANNER_TYPE, type(%d)", emcmotCommand->planner_type); - // Only 0 and 1 are supported, set to 0 if invalid - if (emcmotCommand->planner_type != 0 && emcmotCommand->planner_type != 1) { - emcmotStatus->planner_type = 0; - } else { - emcmotStatus->planner_type = emcmotCommand->planner_type; + { + /* Only 0 and 1 are supported; coerce anything else to 0. */ + int req = (emcmotCommand->planner_type == 1) ? 1 : 0; + if (planner_switch_motion_idle()) { + /* idle: instant switch, drop any stale pending request */ + emcmotStatus->planner_type = req; + planner_type_switch_pending = 0; + } else if (req != emcmotStatus->planner_type) { + /* moving: defer until the queue drains (never abort) */ + planner_type_pending_value = req; + if (!planner_type_switch_pending) { + planner_type_switch_pending = 1; + /* operator-facing: reportError() surfaces in the GUI (unlike + * rtapi_print_msg, which only hits the RTAPI log/terminal). */ + reportError(_("planner switch deferred until queued motion completes (requested type %d)"), req); + } + } else { + /* request already equals current type: cancel any pending switch */ + planner_type_switch_pending = 0; + } } + /* ===== END PLANNER_SWITCH_DEFER ==================================== */ break; case EMCMOT_SET_SCURVE_PEAK_SCALE: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 9e23ea480ae..0604656eb24 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -263,6 +263,10 @@ void emcmotController(void *arg, long period) switch_to_teleop_mode(); } + /* PLANNER_SWITCH_DEFER (reversible): apply a latched PLANNER_TYPE switch once the + * coordinated queue has gone idle. No-op unless a switch is pending + motion idle. */ + emcmotApplyPendingPlannerType(); + get_pos_cmds(period); compute_screw_comp(); *(emcmot_hal_data->eoffset_active) = axis_plan_external_offsets(servo_period, GET_MOTION_ENABLE_FLAG(), get_allhomed()); diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 5eba0f6711d..bf6cac3ff9c 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -316,6 +316,10 @@ int joint_is_lockable(int joint_num); #define SET_TRAK_PLANNER_TYPE(tp) (emcmotStatus->planner_type = tp) +/* PLANNER_SWITCH_DEFER (reversible): defined in command.c, called each servo cycle + * from emcmotController() to apply a latched planner-type switch once motion is idle. */ +extern void emcmotApplyPendingPlannerType(void); + /* joint flags */ #define GET_JOINT_ENABLE_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_ENABLE_BIT ? 1 : 0) From 6ce0becd0c61e6c5890fe2a50b03d7d04a12b9c8 Mon Sep 17 00:00:00 2001 From: chabron94 Date: Thu, 11 Jun 2026 10:58:04 +0800 Subject: [PATCH 05/10] tp: G64 R word selects planner mode (graded Fanuc-style dial) An optional R word on G64 selects the planner mode and cornering aggressiveness in one dial, riding the EMC_TRAJ_SET_TERM_COND message G64 already emits (two sentinel-defaulted fields, applied in program order by task, landing through the defer-until-idle guard): R omitted -> planner unchanged (modal) R <= 0 -> trapezoidal 0 < R <= 1.0 -> S-curve, cornering scale = R R > 1.0 -> S-curve, scale clamped to 1.0 Note R0 -> R0.1 is a regime flip (trapezoid is jerk-UNlimited, the aggressive end), not the gentle end of a gradient; R0.1 is the gentlest setting. --- src/emc/nml_intf/canon.hh | 6 +++++- src/emc/nml_intf/emc.cc | 2 ++ src/emc/nml_intf/emc_nml.hh | 9 ++++++++- src/emc/rs274ngc/gcodemodule.cc | 3 ++- src/emc/rs274ngc/interp_check.cc | 3 ++- src/emc/rs274ngc/interp_convert.cc | 23 +++++++++++++++++++++-- src/emc/rs274ngc/rs274ngc_interp.hh | 4 +++- src/emc/sai/saicanon.cc | 6 +++++- src/emc/task/emccanon.cc | 9 ++++++++- src/emc/task/emctaskmain.cc | 7 +++++++ 10 files changed, 63 insertions(+), 9 deletions(-) diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 79b64caf031..a34b31a0c67 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -399,7 +399,11 @@ extern void SET_FEED_MODE(int spindle, int mode); * inches per revolution (G20 in effect) or mm per minute (G21 in effect) * The spindle number indicates which spindle the movement is synchronised to */ -extern void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance); +/* G64_R_PLANNER: two optional trailing args fold the planner mode (G64 R word) + * into the existing control-mode call. Defaults -1/-1.0 = "leave unchanged", + * so existing callers (G61, G61.1, cutter-comp save/restore) are unaffected. */ +extern void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance, + int planner_type = -1, double scurve_peak_scale = -1.0); extern void SET_NAIVECAM_TOLERANCE(double tolerance); diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index df82c4d0d0c..b010e09d4be 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1106,6 +1106,8 @@ void EMC_TRAJ_SET_TERM_COND::update(CMS * cms) EMC_TRAJ_CMD_MSG::update(cms); cms->update(cond); cms->update(tolerance); + cms->update(planner_type); /* G64_R_PLANNER: append at end of serialization */ + cms->update(scurve_peak_scale); /* G64_R_PLANNER */ } // cppcheck-suppress duplInheritedMember diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index afd051e27bc..9540b5245dc 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -787,7 +787,9 @@ class EMC_TRAJ_SET_TERM_COND:public EMC_TRAJ_CMD_MSG { EMC_TRAJ_SET_TERM_COND() : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_TERM_COND_TYPE, sizeof(EMC_TRAJ_SET_TERM_COND)), cond(false), - tolerance(0.0) + tolerance(0.0), + planner_type(-1), /* G64_R_PLANNER: -1 = unchanged */ + scurve_peak_scale(-1.0) /* G64_R_PLANNER: <0 = unchanged */ {}; // For internal NML/CMS use only. @@ -798,6 +800,11 @@ class EMC_TRAJ_SET_TERM_COND:public EMC_TRAJ_CMD_MSG { int cond; double tolerance; // used to set the precision/tolerance of path deviation // during CONTINUOUS motion mode. + /* G64_R_PLANNER (fork extension): the optional G64 R word folds the planner + * mode into the same control-mode message G64 already emits. -1 / <0 means + * "not specified, leave unchanged". Applied in program order by task. */ + int planner_type; // 0 = trapezoidal, 1 = S-curve, -1 = unchanged + double scurve_peak_scale; // 0.1..1.0 cornering peak scale, <0 = unchanged }; class EMC_TRAJ_SET_SPINDLESYNC:public EMC_TRAJ_CMD_MSG { diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 61c8f5feada..6047760aebe 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -845,7 +845,8 @@ static bool check_abort() { USER_DEFINED_FUNCTION_TYPE USER_DEFINED_FUNCTION[USER_DEFINED_FUNCTION_NUM]; CANON_MOTION_MODE motion_mode; -void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double /*tolerance*/) { motion_mode = mode; } +/* G64_R_PLANNER: preview module ignores the planner-mode args (no motion) */ +void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double /*tolerance*/, int /*planner_type*/, double /*scurve_peak_scale*/) { motion_mode = mode; } void SET_MOTION_CONTROL_MODE(double /*tolerance*/) { } void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode) { motion_mode = mode; } CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() { return motion_mode; } diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index b5c9749c545..ef607b9ffbf 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -362,7 +362,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block ((motion < G_81) || (motion > G_89)) && (motion != G_73) && (motion != G_74) && (block->g_modes[GM_CUTTER_COMP] != G_41_1) && (block->g_modes[GM_CUTTER_COMP] != G_42_1) && - (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) ), + (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) && + (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); CHKS((block->m_modes[7] == 19) && ((block->r_number > 360.0) || (block->r_number < 0.0)), _("R value must be within 0..360 with M19")); diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 8f8573cab2f..4ff2c74f5e1 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -2215,6 +2215,8 @@ int Interp::convert_control_mode( int g_code, // g_code being executed (G_61, G61_1, G_64) double tolerance_in, // tolerance for the path following in G64 double naivecam_tolerance_in, // tolerance for the naivecam + double r_word, // G64_R_PLANNER: R value (planner/aggressiveness) + bool r_present, // G64_R_PLANNER: true if R was given on the block setup_pointer settings) // pointer to machine settings { double tolerance, naivecam_tolerance; @@ -2235,7 +2237,22 @@ int Interp::convert_control_mode( } settings->control_mode = CANON_CONTINUOUS; settings->tolerance = tolerance; - SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, tolerance); + /* G64_R_PLANNER: optional R word selects the planner / cornering aggressiveness + * (Fanuc G05.1-style level). R absent -> leave planner unchanged (sentinels). + * R<=0 -> trapezoidal (planner_type 0), peak_scale unchanged + * 0 S-curve (planner_type 1), scurve_peak_scale = R (clamped 0.1..1.0) + * R>1.0 -> S-curve, peak_scale clamped to 1.0 */ + int planner_type = -1; // -1 = unchanged + double peak_scale = -1.0; // <0 = unchanged + if (r_present) { + if (r_word <= 0.0) { + planner_type = 0; // trapezoidal / BRISK + } else { + planner_type = 1; // S-curve / SOFT + peak_scale = (r_word > 1.0) ? 1.0 : ((r_word < 0.1) ? 0.1 : r_word); + } + } + SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, tolerance, planner_type, peak_scale); if (naivecam_tolerance_in >= 0){ naivecam_tolerance = naivecam_tolerance_in; @@ -2969,7 +2986,9 @@ int Interp::convert_g(block_pointer block, //!< pointer to a block of RS27 } if ((block->g_modes[GM_CONTROL_MODE] != -1) && ONCE(STEP_CONTROL_MODE)) { status = convert_control_mode(block->g_modes[GM_CONTROL_MODE], - block->p_number, block->q_number, settings); + block->p_number, block->q_number, + block->r_number, block->r_flag, /* G64_R_PLANNER */ + settings); CHP(status); } if ((block->g_modes[GM_DISTANCE_MODE] != -1) && ONCE(STEP_DISTANCE_MODE)) { diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index f02d334a810..d36d3e4c895 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -259,7 +259,9 @@ public: setup_pointer settings); int convert_param_comment(char *comment, char *expanded, int len); int convert_comment(char *comment, bool enqueue = true); - int convert_control_mode(int g_code, double tolerance, double naivecam_tolerance, setup_pointer settings); + int convert_control_mode(int g_code, double tolerance, double naivecam_tolerance, + double r_word, bool r_present, /* G64_R_PLANNER */ + setup_pointer settings); int convert_adaptive_mode(int g_code, setup_pointer settings); int convert_coordinate_system(int g_code, setup_pointer settings); diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index c63354d6a41..6d27f1cc00e 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -253,8 +253,12 @@ void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference) (reference == CANON_WORKPIECE) ? "CANON_WORKPIECE" : "CANON_XYZ"); } -extern void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance) +extern void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance, + int planner_type, double scurve_peak_scale) { + /* G64_R_PLANNER: standalone-interp echo of the optional planner mode */ + if (planner_type >= 0 || scurve_peak_scale >= 0.0) + PRINT("SET_PLANNER_MODE(type=%d, peak_scale=%f)\n", planner_type, scurve_peak_scale); _sai.motion_tolerance = 0; if (mode == CANON_EXACT_STOP) { diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index d2a0e0d0f26..0e8fb1a9846 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1443,7 +1443,8 @@ void STRAIGHT_PROBE(int line_number, /* Machining Attributes */ -void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance) +void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance, + int planner_type, double scurve_peak_scale) { auto setTermCondMsg = std::make_unique(); @@ -1452,6 +1453,12 @@ void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance) canon.motionMode = mode; canon.motionTolerance = FROM_PROG_LEN(tolerance); + /* G64_R_PLANNER: carry the optional planner mode (from a G64 R word) on the + * same queued message so it is applied at this exact point in program order. + * Sentinels (<0) mean "unchanged" and are ignored by task. */ + setTermCondMsg->planner_type = planner_type; + setTermCondMsg->scurve_peak_scale = scurve_peak_scale; + switch (mode) { case CANON_CONTINUOUS: setTermCondMsg->cond = EMC_TRAJ_TERM_COND_BLEND; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 3bf13a702d1..847aaac15a8 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1862,6 +1862,13 @@ static int emcTaskIssueCommand(NMLmsg * cmd) case EMC_TRAJ_SET_TERM_COND_TYPE: emcTrajSetTermCondMsg = reinterpret_cast(cmd); retval = emcTrajSetTermCond(emcTrajSetTermCondMsg->cond, emcTrajSetTermCondMsg->tolerance); + /* G64_R_PLANNER: a G64 R word piggybacks the planner mode here so it is + * applied in program order. Sentinels (<0) mean "leave unchanged". The + * motion side applies it cleanly via the defer-until-idle guard. */ + if (retval == 0 && emcTrajSetTermCondMsg->planner_type >= 0) + emcTrajPlannerType(emcTrajSetTermCondMsg->planner_type); + if (retval == 0 && emcTrajSetTermCondMsg->scurve_peak_scale >= 0.0) + emcTrajSetScurvePeakScale(emcTrajSetTermCondMsg->scurve_peak_scale); break; case EMC_TRAJ_SET_SPINDLESYNC_TYPE: From 64cc3846cba58608b6f31717738b75b475335689 Mon Sep 17 00:00:00 2001 From: chabron94 Date: Thu, 11 Jun 2026 10:58:05 +0800 Subject: [PATCH 06/10] tp: throttle S-curve no-jerk log storm + refuse S-curve without jerk limit The 'maxjerk < 1' error printed EVERY servo cycle for segments with no usable jerk limit (e.g. rotary-only moves with [AXIS_*]MAX_JERK unset); the fallback is graceful (trapezoidal for the segment), so warn once, naming the likely missing INI items. And EMCMOT_SET_PLANNER_TYPE now refuses an S-curve request outright when no valid TRAJ-level max jerk is configured (parity with the initraj/inihal guards) instead of entering a degraded per-segment-fallback state. --- src/emc/motion/command.c | 8 ++++++++ src/emc/tp/tp.c | 13 ++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index bbf4acd83cd..c90a412a8f0 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1252,6 +1252,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) { /* Only 0 and 1 are supported; coerce anything else to 0. */ int req = (emcmotCommand->planner_type == 1) ? 1 : 0; + /* G64_R_PLANNER guard (parity with initraj/inihal, which force + * type 0 when jerk is unset): refuse an S-curve request when no + * valid TRAJ-level max jerk is configured, instead of entering a + * degraded per-segment-fallback state. */ + if (req == 1 && emcmotStatus->jerk < 1.0) { + reportError(_("S-curve planner refused: no usable jerk limit - set [TRAJ]MAX_LINEAR_JERK and per-axis [AXIS_*]MAX_JERK")); + break; + } if (planner_switch_motion_idle()) { /* idle: instant switch, drop any stale pending request */ emcmotStatus->planner_type = req; diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 8a9c8866222..7c645e17c8f 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2809,7 +2809,18 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk); if(maxjerk <= 1){ maxjerk = 1; - rtapi_print_msg(RTAPI_MSG_ERR, "ERROR!!! maxjerk Is less than 1\n"); + /* This fires EVERY CYCLE for segments with no usable jerk limit (e.g. a + * rotary-only move when [AXIS_A/B/C]MAX_JERK is unset, found via G43.4 + * TCP testing on a stock sim). The fallback below is graceful + * (trapezoidal for the segment), so warn ONCE instead of storming the + * log at servo rate. */ + static int scurve_jerk_warned = 0; + if (!scurve_jerk_warned) { + scurve_jerk_warned = 1; + rtapi_print_msg(RTAPI_MSG_ERR, + "S-curve: segment max jerk < 1 (unset [AXIS_*]MAX_JERK or [TRAJ]MAX_LINEAR_JERK?) - " + "trapezoidal fallback used for such segments (reported once)\n"); + } return TP_SCURVE_ACCEL_ERROR; } From cc23ae541f61cd3e97145bece809e9a2bbf5bf91 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 11 Jun 2026 10:58:05 +0800 Subject: [PATCH 07/10] tp: G64 R carries intent only; SCURVE_PEAK_SCALE defaults to 1.0; review fixes Restructure from review of fork PR #1: - Part programs never name a planner implementation: the NML field carries intent (0 = trapezoidal, 1 = smooth/jerk-limited) and task resolves "smooth" against new [TRAJ]SMOOTH_PLANNER (default: PLANNER_TYPE when >0, else 1; currently only planner 1 exists and the value is range-capped accordingly). 0, or a missing jerk limit, makes task refuse R>0 with an operator error instead of silently ignoring the program's request. Programs stay valid when a machine's smooth planner implementation changes. - SCURVE_PEAK_SCALE defaults to 1.0, the physically-correct corner speed. The halved peak of the initial S-curve release (about 6 months old) was an accident, not a designed margin; 1.0 stays within the configured jerk/accel limits by construction, which is the actual contract with the integrator. Set 0.5 to reproduce the pre-fix behaviour. - Eager pool allocation: ruckig_pool_init() runs from sp_scurve_init() (init/config context), so the first active segment no longer pays RUCKIG_POOL_SIZE heap allocations inside a servo cycle; the lazy init in ruckig_pool_acquire() remains as a backstop only. - ruckig_pool_cleanup() + sp_scurve_cleanup() now run at motmod exit (sp_scurve_cleanup previously had no caller, leaking the cached planner as well). --- src/emc/ini/initraj.cc | 25 ++++++-- src/emc/motion/motion.c | 3 + src/emc/nml_intf/emc.hh | 3 + src/emc/nml_intf/emc_nml.hh | 8 ++- src/emc/rs274ngc/interp_convert.cc | 17 ++++-- src/emc/task/emctaskmain.cc | 24 ++++++-- src/emc/task/taskintf.cc | 18 ++++++ src/emc/tp/ruckig_wrapper.c | 60 +++++++++++++++---- src/emc/tp/ruckig_wrapper.h | 21 ++++++- src/emc/tp/sp_scurve.c | 23 ++++--- .../basic/expected.builtin-startup.in | 2 +- .../mountaindew/expected.motion-logger | 2 +- .../expected.motion-logger.in | 2 +- 13 files changed, 165 insertions(+), 43 deletions(-) diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc index d8b3ac61670..b2cca03fe38 100644 --- a/src/emc/ini/initraj.cc +++ b/src/emc/ini/initraj.cc @@ -166,10 +166,27 @@ static int loadTraj(const IniFile &ini) } old_inihal_data.traj_planner_type = planner_type; - // S-curve rest-to-rest peak velocity scale: 0.5 = faithful (original - // conservative cornering), 1.0 = physically-correct (full jerk-feasible). - // Default 0.5 = no behaviour change. Runtime-tunable via ini.traj_scurve_peak_scale. - double scurve_peak_scale = ini.findRealV("SCURVE_PEAK_SCALE", "TRAJ", 0.5); + // G64_R_PLANNER: which planner a G64 R>0 "smooth" request resolves to. + // Part programs carry intent only (R0 = trapezoidal, R>0 = jerk-limited); + // the machine names the implementation here, so programs stay valid when + // the machine's smooth planner changes. 0 disables G64 R>0 (task refuses + // with an operator error). Defaults to PLANNER_TYPE when that is a smooth + // planner, else 1. Currently the only smooth planner is type 1. + int smooth_planner = ini.findIntV("SMOOTH_PLANNER", "TRAJ", + planner_type > 0 ? planner_type : 1, 0, 1); + if (jerk < 1.0) { + // parity with the forced-trapezoidal fallback above: without a valid + // jerk limit there is no usable smooth planner to resolve to + smooth_planner = 0; + } + if (0 != emcTrajSetSmoothPlanner(smooth_planner)) { + print_dbg_config("emcTrajSetSmoothPlanner"); + return -1; + } + + // S-curve rest-to-rest peak velocity scale (0.1..1.0 of the jerk-feasible + // corner speed). Runtime-tunable via ini.traj_scurve_peak_scale. + double scurve_peak_scale = ini.findRealV("SCURVE_PEAK_SCALE", "TRAJ", 1.0); if (0 != emcTrajSetScurvePeakScale(scurve_peak_scale)) { print_dbg_config("emcTrajSetScurvePeakScale"); return -1; diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index ea32a3ac7d8..c940409ebc0 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -17,6 +17,7 @@ #include /* decls for HAL implementation */ #include "../tp/tp.h" +#include "../tp/sp_scurve.h" /* sp_scurve_cleanup() at module exit */ #include "motion.h" #include "motion_struct.h" #include "mot_priv.h" @@ -470,6 +471,8 @@ void rtapi_app_exit(void) rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: hal_stop_threads() failed, returned %d\n"), retval); } + /* free the S-curve planner + Ruckig pool (threads are stopped, safe) */ + sp_scurve_cleanup(); /* free shared memory */ retval = rtapi_shmem_delete(emc_shmem_id, mot_comp_id); if (retval < 0) { diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 5bfc32cae2f..3e406cc0a84 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -388,6 +388,9 @@ extern int emcTrajSetJerk(double jerk); extern int emcTrajSetMaxJerk(double jerk); extern int emcTrajPlannerType(int type); extern int emcTrajSetScurvePeakScale(double scale); +/* G64_R_PLANNER: [TRAJ]SMOOTH_PLANNER - planner a G64 R>0 request resolves to */ +extern int emcTrajSetSmoothPlanner(int type); +extern int emcTrajGetSmoothPlanner(void); // implementation functions for EMC_MOTION aggregate types extern int emcMotionInit(); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 9540b5245dc..c4484349b74 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -801,9 +801,11 @@ class EMC_TRAJ_SET_TERM_COND:public EMC_TRAJ_CMD_MSG { double tolerance; // used to set the precision/tolerance of path deviation // during CONTINUOUS motion mode. /* G64_R_PLANNER (fork extension): the optional G64 R word folds the planner - * mode into the same control-mode message G64 already emits. -1 / <0 means - * "not specified, leave unchanged". Applied in program order by task. */ - int planner_type; // 0 = trapezoidal, 1 = S-curve, -1 = unchanged + * request into the same control-mode message G64 already emits. -1 / <0 + * means "not specified, leave unchanged". Applied in program order by task. + * planner_type carries INTENT, not an implementation: task resolves a + * smooth request (1) to the machine's [TRAJ]SMOOTH_PLANNER. */ + int planner_type; // 0 = trapezoidal, 1 = smooth (jerk-limited), -1 = unchanged double scurve_peak_scale; // 0.1..1.0 cornering peak scale, <0 = unchanged }; diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 4ff2c74f5e1..0e53827d307 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -2237,18 +2237,23 @@ int Interp::convert_control_mode( } settings->control_mode = CANON_CONTINUOUS; settings->tolerance = tolerance; - /* G64_R_PLANNER: optional R word selects the planner / cornering aggressiveness - * (Fanuc G05.1-style level). R absent -> leave planner unchanged (sentinels). - * R<=0 -> trapezoidal (planner_type 0), peak_scale unchanged - * 0 S-curve (planner_type 1), scurve_peak_scale = R (clamped 0.1..1.0) - * R>1.0 -> S-curve, peak_scale clamped to 1.0 */ + /* G64_R_PLANNER: optional R word carries planner INTENT plus cornering + * aggressiveness (Fanuc G05.1-style level). The program never names a + * planner implementation; "smooth" is resolved by task against + * [TRAJ]SMOOTH_PLANNER, so part programs stay machine-portable. + * R absent -> leave planner unchanged (sentinels). + * R<=0 -> trapezoidal, peak_scale unchanged + * 0 smooth (jerk-limited), scale = R (clamped 0.1..1.0) + * R>1.0 -> smooth, scale clamped to 1.0 + * Note: R0 -> R0.1 is a regime flip (jerk-unlimited trapezoid), not the + * gentle end of a gradient; R0.1 is the gentlest setting. */ int planner_type = -1; // -1 = unchanged double peak_scale = -1.0; // <0 = unchanged if (r_present) { if (r_word <= 0.0) { planner_type = 0; // trapezoidal / BRISK } else { - planner_type = 1; // S-curve / SOFT + planner_type = 1; // smooth / SOFT (resolved by task) peak_scale = (r_word > 1.0) ? 1.0 : ((r_word < 0.1) ? 0.1 : r_word); } } diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 847aaac15a8..c1171f0dc50 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1862,11 +1862,25 @@ static int emcTaskIssueCommand(NMLmsg * cmd) case EMC_TRAJ_SET_TERM_COND_TYPE: emcTrajSetTermCondMsg = reinterpret_cast(cmd); retval = emcTrajSetTermCond(emcTrajSetTermCondMsg->cond, emcTrajSetTermCondMsg->tolerance); - /* G64_R_PLANNER: a G64 R word piggybacks the planner mode here so it is - * applied in program order. Sentinels (<0) mean "leave unchanged". The - * motion side applies it cleanly via the defer-until-idle guard. */ - if (retval == 0 && emcTrajSetTermCondMsg->planner_type >= 0) - emcTrajPlannerType(emcTrajSetTermCondMsg->planner_type); + /* G64_R_PLANNER: a G64 R word piggybacks the planner request here so it + * is applied in program order. Sentinels (<0) mean "leave unchanged". + * The message carries INTENT only: 0 = trapezoidal, >=1 = "the smooth + * (jerk-limited) planner". Which smooth planner that is comes from + * [TRAJ]SMOOTH_PLANNER, so part programs never name an implementation + * and stay valid when the machine's planner changes. The motion side + * applies the switch via the defer-until-idle guard. */ + if (retval == 0 && emcTrajSetTermCondMsg->planner_type >= 0) { + if (emcTrajSetTermCondMsg->planner_type == 0) { + emcTrajPlannerType(0); + } else { + int smooth = emcTrajGetSmoothPlanner(); + if (smooth < 1) { + emcOperatorError(_("G64 R>0: no smooth planner available on this machine - set [TRAJ]MAX_LINEAR_JERK / [TRAJ]SMOOTH_PLANNER")); + } else { + emcTrajPlannerType(smooth); + } + } + } if (retval == 0 && emcTrajSetTermCondMsg->scurve_peak_scale >= 0.0) emcTrajSetScurvePeakScale(emcTrajSetTermCondMsg->scurve_peak_scale); break; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 6d710a4429d..e6bc9641061 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1189,6 +1189,24 @@ int emcTrajPlannerType(int type) return retval; } +/* G64_R_PLANNER: which planner a G64 R>0 "smooth" request resolves to. + * Part programs carry intent only (R0 = trapezoidal, R>0 = jerk-limited); + * the machine names the implementation via [TRAJ]SMOOTH_PLANNER (initraj + * stores it here at startup). 0 = no smooth planner available, task then + * refuses R>0 with an operator error instead of switching. */ +static int smoothPlannerType = 1; + +int emcTrajSetSmoothPlanner(int type) +{ + smoothPlannerType = type; + return 0; +} + +int emcTrajGetSmoothPlanner(void) +{ + return smoothPlannerType; +} + int emcTrajSetScurvePeakScale(double scale) { emcmotCommand.command = EMCMOT_SET_SCURVE_PEAK_SCALE; diff --git a/src/emc/tp/ruckig_wrapper.c b/src/emc/tp/ruckig_wrapper.c index f21b770a6bc..d3ac67717fc 100644 --- a/src/emc/tp/ruckig_wrapper.c +++ b/src/emc/tp/ruckig_wrapper.c @@ -118,11 +118,14 @@ void ruckig_destroy(RuckigPlanner planner) { * segment goes active every few servo cycles, so the motion thread was doing * heap alloc/free constantly -> variable-latency "peaking" servo time. * - * The pool allocates a small fixed set of planners ONCE (lazily, on first use), - * then hands them out / takes them back with no allocation in steady state. - * Only the current tc and (during a blend) the next tc ever hold a planner at - * once, so a handful of slots is plenty; if the pool is ever exhausted we fall - * back to a live ruckig_create() so we never fault or regress. + * The pool allocates a small fixed set of planners ONCE, eagerly from + * ruckig_pool_init() (called at init/config time via sp_scurve_init(), NOT + * from the servo cycle), then hands them out / takes them back with no + * allocation in steady state. Only the current tc and (during a blend) the + * next tc ever hold a planner at once, so a handful of slots is plenty; if + * the pool is ever exhausted we fall back to a live ruckig_create() so we + * never fault or regress. The lazy init in ruckig_pool_acquire() remains + * only as a backstop for callers that bypass sp_scurve_init(). * * Single-threaded: the motion/servo loop is the only caller, so no locking. * ------------------------------------------------------------------------- */ @@ -134,16 +137,51 @@ static struct { } ruckig_pool[RUCKIG_POOL_SIZE]; static int ruckig_pool_inited = 0; -RuckigPlanner ruckig_pool_acquire(double cycle_time) { +int ruckig_pool_init(double cycle_time) { + int i, ok = 0; + + if (ruckig_pool_inited) { + return 0; + } + for (i = 0; i < RUCKIG_POOL_SIZE; i++) { + ruckig_pool[i].planner = ruckig_create(cycle_time); + ruckig_pool[i].in_use = 0; + if (ruckig_pool[i].planner) { + ok++; + } + } + ruckig_pool_inited = 1; + if (ok < RUCKIG_POOL_SIZE) { + rtapi_print_msg(RTAPI_MSG_ERR, + "ruckig_pool_init: only %d/%d planners allocated\n", ok, RUCKIG_POOL_SIZE); + return -1; + } + return 0; +} + +void ruckig_pool_cleanup(void) { int i; - /* One-time lazy init: allocate the whole pool on first use (program start). */ if (!ruckig_pool_inited) { - for (i = 0; i < RUCKIG_POOL_SIZE; i++) { - ruckig_pool[i].planner = ruckig_create(cycle_time); - ruckig_pool[i].in_use = 0; + return; + } + for (i = 0; i < RUCKIG_POOL_SIZE; i++) { + if (ruckig_pool[i].planner) { + ruckig_destroy(ruckig_pool[i].planner); + ruckig_pool[i].planner = NULL; } - ruckig_pool_inited = 1; + ruckig_pool[i].in_use = 0; + } + ruckig_pool_inited = 0; +} + +RuckigPlanner ruckig_pool_acquire(double cycle_time) { + int i; + + /* Backstop only: the pool is normally allocated eagerly by + * ruckig_pool_init() at init time, never from the servo cycle. */ + if (!ruckig_pool_inited) { + ruckig_pool_init(cycle_time); } /* Hand out a free, reset planner. */ diff --git a/src/emc/tp/ruckig_wrapper.h b/src/emc/tp/ruckig_wrapper.h index 1f2cc3adb8e..ad7ec0a7c4a 100644 --- a/src/emc/tp/ruckig_wrapper.h +++ b/src/emc/tp/ruckig_wrapper.h @@ -39,15 +39,30 @@ RuckigPlanner ruckig_create(double cycle_time); */ void ruckig_destroy(RuckigPlanner planner); +/** + * Eagerly allocate the planner pool (call at init/config time, NOT from the + * servo cycle). Idempotent: a second call is a no-op. + * + * @param cycle_time cycle time in seconds for the pooled planners + * @return 0 on success, -1 if any slot failed to allocate + */ +int ruckig_pool_init(double cycle_time); + +/** + * Destroy all pooled planners (call at module exit). Safe to call when the + * pool was never initialized. + */ +void ruckig_pool_cleanup(void); + /** * Acquire a planner from the preallocated pool (no allocation in steady state). * * Drop-in replacement for ruckig_create() on the RT hot path. Returns a reset - * planner borrowed from a fixed pool that is allocated once, on first use. - * If the pool is exhausted (or its lazy init failed) it falls back to a live + * planner borrowed from the fixed pool. If the pool is exhausted (or was never + * initialized and its backstop init failed) it falls back to a live * ruckig_create() so behaviour never regresses below the original code. * - * @param cycle_time cycle time in seconds (used only for one-time pool init) + * @param cycle_time cycle time in seconds (used only by the backstop init) * @return planner handle, or NULL on failure */ RuckigPlanner ruckig_pool_acquire(double cycle_time); diff --git a/src/emc/tp/sp_scurve.c b/src/emc/tp/sp_scurve.c index dfb2d9ceea5..d2a910235f6 100644 --- a/src/emc/tp/sp_scurve.c +++ b/src/emc/tp/sp_scurve.c @@ -25,14 +25,13 @@ #include #endif -/* Runtime S-curve rest-to-rest peak scale, from [TRAJ]SCURVE_PEAK_SCALE / - * ini.traj_scurve_peak_scale (shared via emcmotStatus). 0.5 = faithful (original - * conservative cornering), 1.0 = physically-correct (full jerk-feasible). The - * clamp defends the pre-INI window where shmem is still zeroed -> default 0.5. */ +/* Runtime S-curve rest-to-rest peak scale (0.1..1.0 of the jerk-feasible + * corner speed), from [TRAJ]SCURVE_PEAK_SCALE / ini.traj_scurve_peak_scale via + * emcmotStatus. The clamp defends the pre-INI window where shmem is zeroed. */ extern emcmot_status_t *emcmotStatus; static double scurve_peak_scale(void) { - double s = emcmotStatus ? emcmotStatus->scurve_peak_scale : 0.5; - return (s >= 0.1 && s <= 1.0) ? s : 0.5; + double s = emcmotStatus ? emcmotStatus->scurve_peak_scale : 1.0; + return (s >= 0.1 && s <= 1.0) ? s : 1.0; } /* ========== Cached Ruckig planner ========== @@ -104,7 +103,7 @@ static double scurve_max_start_speed_analytic(double distance, double Ve, double static double scurve_max_start_speed_full_analytic(double distance, double Ve, double A, double J) { double vs = scurve_max_start_speed_analytic(distance, Ve, A, J); double peak = scurve_max_start_speed_analytic(distance * 0.5, 0.0, A, J); - peak *= scurve_peak_scale(); /* runtime: 0.5 faithful .. 1.0 full cornering */ + peak *= scurve_peak_scale(); /* Floor at Ve: when the segment is too short to reach Ve (peak clamp < Ve), * the original Ruckig path fails and falls back to fmax(Ve, peak). Max start * speed can never be below the end speed, so clamp up to Ve. */ @@ -120,7 +119,7 @@ int findSCurveVSpeed(double distence, double maxA, double maxJ, double *req_v) { * triggered on every arc/blend — the arc-section opt bursts. */ if (distence <= 0.0 || maxA <= 0.0 || maxJ <= 0.0) { *req_v = 0.0; return -1; } double peak = scurve_max_start_speed_analytic(distence * 0.5, 0.0, maxA, maxJ); - peak *= scurve_peak_scale(); /* runtime: 0.5 faithful .. 1.0 full cornering */ + peak *= scurve_peak_scale(); *req_v = peak; return 1; } @@ -172,6 +171,13 @@ int sp_scurve_init(double cycle_time) { cached_cycle_time = cycle_time; rtapi_print_msg(RTAPI_MSG_INFO, "sp_scurve_init: planner created with cycle_time=%f (logging disabled)\n", cycle_time); + + /* Eagerly allocate the execution-planner pool here (init/config context) + * so the first segment never pays RUCKIG_POOL_SIZE heap allocations + * inside a servo cycle. */ + if (ruckig_pool_init(cycle_time) != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "sp_scurve_init: ruckig_pool_init() incomplete (acquire falls back to live create)\n"); + } return 0; } @@ -184,6 +190,7 @@ void sp_scurve_cleanup(void) { cached_planner = NULL; cached_cycle_time = 0.0; } + ruckig_pool_cleanup(); } /** diff --git a/tests/motion-logger/basic/expected.builtin-startup.in b/tests/motion-logger/basic/expected.builtin-startup.in index 59be618a5e5..1f23b69565e 100644 --- a/tests/motion-logger/basic/expected.builtin-startup.in +++ b/tests/motion-logger/basic/expected.builtin-startup.in @@ -5,7 +5,7 @@ SET_VEL_LIMIT vel=4 SET_ACC acc=1e+99 SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 -SET_SCURVE_PEAK_SCALE scale=0.5 +SET_SCURVE_PEAK_SCALE scale=1 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 SETUP_SET_PROBE_ERR_INHIBIT 0 0 diff --git a/tests/motion-logger/mountaindew/expected.motion-logger b/tests/motion-logger/mountaindew/expected.motion-logger index 0d3ebed8199..efbcc4db7eb 100644 --- a/tests/motion-logger/mountaindew/expected.motion-logger +++ b/tests/motion-logger/mountaindew/expected.motion-logger @@ -5,7 +5,7 @@ SET_VEL_LIMIT vel=400 SET_ACC acc=1e+99 SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 -SET_SCURVE_PEAK_SCALE scale=0.5 +SET_SCURVE_PEAK_SCALE scale=1 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 SETUP_SET_PROBE_ERR_INHIBIT 0 0 diff --git a/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in b/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in index 967d5b7d77c..5e1d61600b2 100644 --- a/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in +++ b/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in @@ -10,7 +10,7 @@ SET_VEL_LIMIT vel=4 SET_ACC acc=1e+99 SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 -SET_SCURVE_PEAK_SCALE scale=0.5 +SET_SCURVE_PEAK_SCALE scale=1 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 SETUP_SET_PROBE_ERR_INHIBIT 0 0 From e6a7535320625804c55725391239d60974a49b8d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 11 Jun 2026 11:38:29 +0800 Subject: [PATCH 08/10] interp: refuse R word on a line with both G64 and M19 A block has one shared R word. With G64 it is the planner dial, with M19 the spindle orient angle; both consumers would silently read the same value. The combination is now an error (the equivalent P-word collision was already caught by the existing M19 P check). --- src/emc/rs274ngc/interp_check.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index ef607b9ffbf..196f2772763 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -365,6 +365,11 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block (block->g_modes[GM_MODAL_0] != G_10) && (block->m_modes[7] != 19) && (block->g_modes[GM_CONTROL_MODE] != G_64) ), /* G64_R_PLANNER: R selects planner on G64 */ NCE_R_WORD_WITH_NO_G_CODE_THAT_USES_IT); + /* G64_R_PLANNER: a block has one shared R word; with G64 it is the planner + * dial, with M19 the spindle orient angle. Both would consume the same + * value, so the combination is refused. */ + CHKS((block->g_modes[GM_CONTROL_MODE] == G_64) && (block->m_modes[7] == 19), + _("R word is ambiguous on a line with both G64 and M19 - use separate lines")); CHKS((block->m_modes[7] == 19) && ((block->r_number > 360.0) || (block->r_number < 0.0)), _("R value must be within 0..360 with M19")); } From a9c0712c8780f65afb08af15f8a291f245fb5700 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 11 Jun 2026 12:50:23 +0800 Subject: [PATCH 09/10] docs: G64 R planner dial, [TRAJ]SMOOTH_PLANNER and SCURVE_PEAK_SCALE - g-code.adoc: G64 R section (selection semantics, the R0 vs R0.1 regime-flip note, defer-until-idle behavior, refusal when no smooth planner is available, M19 conflict). - ini-config.adoc: SMOOTH_PLANNER and SCURVE_PEAK_SCALE entries plus runtime-switch note on PLANNER_TYPE. --- docs/src/config/ini-config.adoc | 18 ++++++++++++++ docs/src/gcode/g-code.adoc | 44 ++++++++++++++++++++++++++++++++- 2 files changed, 61 insertions(+), 1 deletion(-) diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 8f1e8d09d56..6d9cd84efb9 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -946,6 +946,24 @@ Finally, no amount of tweaking will speed up a tool path with lots of small, tig * `MAX_LINEAR_ACCELERATION = 20.0` - (((MAX ACCELERATION))) The maximum acceleration for any axis or coordinated axis move, in 'machine units' per second per second. * `PLANNER_TYPE = 0` - (((PLANNER TYPE))) Selects the trajectory planner type: 0 = trapezoidal (default), 1 = S-curve with jerk limiting. S-curve planning is only active when `PLANNER_TYPE = 1` AND `MAX_LINEAR_JERK > 0`. + The planner can be changed at runtime via the `ini.traj-planner-type` HAL pin or the + <>; a switch requested while the machine is moving is deferred + and applied when motion next becomes idle (never aborts), with an operator message. +* `SMOOTH_PLANNER = 1` - (((SMOOTH PLANNER))) Which planner a <> + greater than zero ("smooth") request resolves to. Part programs only request + trapezoidal (R0) or smooth (R greater than 0); this key names the implementation, so + programs stay valid when the machine's smooth planner changes. 0 disables G64 R + smooth requests (they are refused with an operator error). Defaults to + `PLANNER_TYPE` when that is non-zero, else 1. Currently the only smooth planner is + type 1. A missing or invalid `MAX_LINEAR_JERK` (below 1) also disables smooth + requests. Note that the `ini.traj-planner-type` HAL pin is the machine-side surface + and writes planner numbers directly; it bypasses this resolution and its guards. +* `SCURVE_PEAK_SCALE = 1.0` - (((SCURVE PEAK SCALE))) Scales the S-curve rest-to-rest + peak velocity used by the corner look-ahead, i.e. how aggressively the machine may + corner within its jerk and acceleration limits. 1.0 (the default) allows the full + jerk-feasible corner speed; lower values corner more gently (0.5 matches the + initial S-curve release). Clamped to 0.1..1.0. Tunable at runtime via the + `ini.traj-scurve-peak-scale` HAL pin or the <>. * `MAX_LINEAR_JERK = 10000.0` - (((MAX JERK))) The maximum jerk (rate of change of acceleration) for coordinated moves, in 'machine units' per second cubed. Default is 1e9 (1 billion) if not specified, which effectively disables jerk limiting while avoiding numerical instability. Values are clamped to a maximum of 1e9 to prevent numerical issues in S-curve calculations. diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 66f1a2b776f..6c6626eed4c 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1619,11 +1619,12 @@ overview of coordinate systems. [source,ngc] ---- -G64 > +G64 > ---- * 'P' - motion blending tolerance * 'Q' - naive cam tolerance +* 'R' - trajectory planner selection and cornering aggressiveness (this fork only) * 'G64' - best possible speed. Without P (Or a default value in <>) means to keep the best speed possible, no matter how far away from the programmed point you end up. * 'G64 P-' - Blend between best speed and deviation tolerance @@ -1651,6 +1652,47 @@ G64 > and use the value of P-. Set Q to zero to disable the 'Naive CAM Detector'. +=== G64 R: planner selection (((G64 R Planner))) + +The optional R word selects the trajectory planner and its cornering +aggressiveness in one modal dial: + +* 'R omitted' - the planner is left exactly as it was (modal). +* 'R0' (or negative) - trapezoidal planner: acceleration is bounded but can + step instantly (jerk is not limited). +* 'R0.1 .. R1.0' - jerk-limited (S-curve) planner. The value sets the + cornering aggressiveness as the fraction of the jerk-feasible corner + speed that the look-ahead may use: 'R1.0' is full corner speed, lower + values corner more gently ('R0.5' matches the initial S-curve release). + Values outside 0.1..1.0 are clamped. + +The program only requests 'trapezoidal' or 'smooth'; which smooth planner +implements the request is machine configuration +(<>), so part programs remain valid +when the machine's planner implementation changes. + +[NOTE] +==== +R0 to R0.1 is a regime flip, not the gentle end of a gradient: the +trapezoidal planner is the jerk-unlimited, most aggressive setting, while +R0.1 is the gentlest (jerk-limited and slowest through corners). Within +R0.1..R1.0 the motion is equally smooth in the jerk sense; only the corner +speed changes. +==== + +Behavior details: + +* A planner switch requested while queued motion remains is latched and + applied automatically once no queued motion is left; a message informs + the operator and motion is never aborted. The request persists until + applied or superseded. Set 'G64 ... R' in the program preamble, where + the switch is instant. +* If the machine has no smooth planner available ('SMOOTH_PLANNER = 0' or + no usable jerk limit configured), 'R > 0' is refused with an operator + error instead of being silently ignored. +* The R word cannot be used on a line that also contains 'M19' (the R + meanings would be ambiguous); this is an error. + It is a good idea to include a path control specification in the preamble of each G-code file. .G64 P- Q- Example Line From c19b8b4e15ec12ed8ec12d390dc68696359c40f6 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 11 Jun 2026 13:31:44 +0800 Subject: [PATCH 10/10] tests: interp coverage for the G64 R planner dial - g64-r-planner: R0.5/R1 emission, clamping (R2.5 -> 1.0, R0.05 -> 0.1), R0 -> trapezoidal, plain G64 leaves the planner untouched. - bad/: R on G61 still errors; R on a combined G64+M19 line is refused as ambiguous. --- tests/interp/bad/g64-m19-r-ambiguous.ngc | 2 ++ tests/interp/bad/g64-r-on-g61.ngc | 2 ++ tests/interp/g64-r-planner/expected | 34 ++++++++++++++++++++++++ tests/interp/g64-r-planner/test.ngc | 8 ++++++ tests/interp/g64-r-planner/test.sh | 3 +++ 5 files changed, 49 insertions(+) create mode 100644 tests/interp/bad/g64-m19-r-ambiguous.ngc create mode 100644 tests/interp/bad/g64-r-on-g61.ngc create mode 100644 tests/interp/g64-r-planner/expected create mode 100644 tests/interp/g64-r-planner/test.ngc create mode 100644 tests/interp/g64-r-planner/test.sh diff --git a/tests/interp/bad/g64-m19-r-ambiguous.ngc b/tests/interp/bad/g64-m19-r-ambiguous.ngc new file mode 100644 index 00000000000..59af59872c2 --- /dev/null +++ b/tests/interp/bad/g64-m19-r-ambiguous.ngc @@ -0,0 +1,2 @@ +;R word is ambiguous on a line with both G64 and M19 - use separate lines +g64 m19 r90 s0 diff --git a/tests/interp/bad/g64-r-on-g61.ngc b/tests/interp/bad/g64-r-on-g61.ngc new file mode 100644 index 00000000000..e6b45915edb --- /dev/null +++ b/tests/interp/bad/g64-r-on-g61.ngc @@ -0,0 +1,2 @@ +;R word with no g code that uses it +g61 r0.5 diff --git a/tests/interp/g64-r-planner/expected b/tests/interp/g64-r-planner/expected new file mode 100644 index 00000000000..40432933abc --- /dev/null +++ b/tests/interp/g64-r-planner/expected @@ -0,0 +1,34 @@ + N..... USE_LENGTH_UNITS(CANON_UNITS_MM) + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_G92_OFFSET(0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_REFERENCE(CANON_XYZ) + N..... ON_RESET() + N..... SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0.000000) + N..... SET_NAIVECAM_TOLERANCE(0.0000) + N..... SET_PLANNER_MODE(type=1, peak_scale=0.500000) + N..... SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 1.000000) + N..... SET_NAIVECAM_TOLERANCE(1.0000) + N..... SET_PLANNER_MODE(type=1, peak_scale=1.000000) + N..... SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 1.000000) + N..... SET_NAIVECAM_TOLERANCE(1.0000) + N..... SET_PLANNER_MODE(type=1, peak_scale=1.000000) + N..... SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 1.000000) + N..... SET_NAIVECAM_TOLERANCE(1.0000) + N..... SET_PLANNER_MODE(type=1, peak_scale=0.100000) + N..... SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 1.000000) + N..... SET_NAIVECAM_TOLERANCE(1.0000) + N..... SET_PLANNER_MODE(type=0, peak_scale=-1.000000) + N..... SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 1.000000) + N..... SET_NAIVECAM_TOLERANCE(1.0000) + N..... SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 1.000000) + N..... SET_NAIVECAM_TOLERANCE(1.0000) + N..... SET_G5X_OFFSET(1, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000) + N..... SET_XY_ROTATION(0.0000) + N..... SET_FEED_MODE(0, 0) + N..... SET_FEED_RATE(0.0000) + N..... STOP_SPINDLE_TURNING(0) + N..... SET_SPINDLE_MODE(0 0.0000) + N..... PROGRAM_END() + N..... ON_RESET() + N..... ON_RESET() diff --git a/tests/interp/g64-r-planner/test.ngc b/tests/interp/g64-r-planner/test.ngc new file mode 100644 index 00000000000..eb0312956ae --- /dev/null +++ b/tests/interp/g64-r-planner/test.ngc @@ -0,0 +1,8 @@ +G64 +G64 P1 R0.5 +G64 P1 R1 +G64 P1 R2.5 +G64 P1 R0.05 +G64 P1 R0 +G64 P1 +M2 diff --git a/tests/interp/g64-r-planner/test.sh b/tests/interp/g64-r-planner/test.sh new file mode 100644 index 00000000000..2afa80d1ca5 --- /dev/null +++ b/tests/interp/g64-r-planner/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash +rs274 -g test.ngc | awk '{$1=""; print}' +exit "${PIPESTATUS[0]}"