tp: S-curve planner performance and control fixes (solve storm, planner pool, cornering scale, G64 R)

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
+  <<gcode:g64,G64 R word>>; 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 <<gcode:g64,G64 R>>
+  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 <<gcode:g64,G64 R word>>.
 * `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.

docs/src/gcode/g-code.adoc

@@ -1619,11 +1619,12 @@ overview of coordinate systems.
 
 [source,ngc]
 ----
-G64 <P- <Q->>
+G64 <P- <Q-> <R->>
 ----
 
 * '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 <<sub:ini:sec:rs274ngc,RS274NGC>>) 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 <P- <Q->>
   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
+(<<sub:ini:sec:traj,[TRAJ]SMOOTH_PLANNER>>), 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

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)

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) \

src/emc/ini/initraj.cc

@@ -166,6 +166,33 @@ static int loadTraj(const IniFile &ini)
     }
     old_inihal_data.traj_planner_type = planner_type;
 
+    // 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;
+    }
+    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);

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;

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,16 +1238,63 @@ 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;
+		{
+			/* 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;
+				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:
+		/* 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->planner_type = emcmotCommand->planner_type;
+			emcmotStatus->scurve_peak_scale = emcmotCommand->scurve_peak_scale;
 		}
 		break;
-				
+
 	case EMCMOT_PAUSE:
 	    /* pause the motion */
 	    /* can happen at any time */

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());

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)

src/emc/motion/motion.c

@@ -17,6 +17,7 @@
 #include <hal.h>		/* 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) {

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 */

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);
 

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

src/emc/nml_intf/emc.hh

@@ -387,6 +387,10 @@ 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);
+/* 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();