tp: S-curve planner performance and control fixes (solve storm, planner pool, cornering scale, G64 R)#4154
Open
grandixximo wants to merge 10 commits into
Open
tp: S-curve planner performance and control fixes (solve storm, planner pool, cornering scale, G64 R)#4154grandixximo wants to merge 10 commits into
grandixximo wants to merge 10 commits into
Conversation
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.
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.
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.
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.
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.
…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.
…iew 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).
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).
- 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.
- 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.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This series fixes a real-time performance problem in the S-curve planner and cleans up its control surface. The original investigation and most of the implementation are by @greatEndian; I reviewed, restructured and extended the series. 10 commits, each one concern.
The core fix: per-cycle Ruckig solve storm
On dense G-code (150k+ short segments) the look-ahead ran full Ruckig root solves every servo cycle through findSCurveMaxStartSpeed, findSCurveVSpeed and calcSCurveSpeedWithT. Per-solve cost explodes on short and degenerate segments; the result is peaking servo-thread time and following-error trips. The series replaces these queries with constant-time closed forms (triangular cubic and trapezoidal quadratic regimes), validated two ways: an offline harness against numerically integrated ground truth (src/emc/tp/scurve_analytic_test.c, worst relative error 8e-14 percent), and A/B against the live Ruckig path before the solver calls were removed. Two side findings are preserved in the commit messages: the original findSCurveVSpeed returned half the true rest-to-rest peak, and calcSCurveSpeedWithT asked Ruckig for a geometrically impossible target, always failed, and always returned its fallback.
Supporting RT fix: trajectory segments no longer create and destroy a Ruckig planner each on the servo thread; a small pool is allocated at init and recycled, with a live-create fallback if exhausted, and freed at module exit.
Behavior change: SCURVE_PEAK_SCALE
The halved corner peak above means the planner corners at half its jerk-feasible speed. A new [TRAJ]SCURVE_PEAK_SCALE (plus ini.traj-scurve-peak-scale HAL pin) scales the rest-to-rest peak used by the look-ahead, clamped 0.1..1.0. The default is 1.0, the physically correct value: it stays within the configured jerk and acceleration limits by construction, which is the actual contract with the integrator. SCURVE_PEAK_SCALE = 0.5 reproduces the previous behavior exactly; given the planner shipped recently I think fixing the default is right, but I am happy to flip the default to 0.5 if preferred.
Control surface
Compatibility and validation