Merge pull request #3744 from grandixximo/master

S-Curve refinements

Author: andypugh

configs/sim/axis/axis_9axis_scurve.ini

@@ -54,7 +54,6 @@ HALFILE = axis_9axis_scurve.hal
 # PLANNER_TYPE: Select the acceleration profile shape
 #   0 = Trapezoidal (traditional - constant acceleration)
 #   1 = S-curve (smoother - limited jerk/derivative of acceleration)
-#   2 = S-curve (smoother - but sharp in corners - to be deprecated)
 # Default: 0 (trapezoidal)
 #
 # NOTE: This parameter can be changed at RUNTIME via HAL pin:

configs/sim/axis/axis_mm_scurve.ini

@@ -54,7 +54,6 @@ HALFILE = axis_mm_scurve.hal
 # PLANNER_TYPE: Select the acceleration profile shape
 #   0 = Trapezoidal (traditional - constant acceleration)
 #   1 = S-curve (smoother - limited jerk/derivative of acceleration)
-#   2 = S-curve (smoother - but sharp in corners - to be deprecated)
 # Default: 0 (trapezoidal)
 #
 # NOTE: This parameter can be changed at RUNTIME via HAL pin:

src/emc/ini/inihal.cc

@@ -297,7 +297,12 @@ int check_ini_hal_items(int numjoints)
     if (CHANGED(traj_planner_type)) {
         if (debug) SHOW_CHANGE_INT(traj_planner_type)
         UPDATE(traj_planner_type);
-        if (0 != emcTrajPlannerType(NEW(traj_planner_type))) {
+        // Only 0 and 1 are supported, set to 0 if invalid
+        int planner_type = NEW(traj_planner_type);
+        if (planner_type != 0 && planner_type != 1) {
+            planner_type = 0;
+        }
+        if (0 != emcTrajPlannerType(planner_type)) {
             if (emc_debug & EMC_DEBUG_CONFIG) {
                 rcs_print("check_ini_hal_items:bad return value from emcTrajPlannerType\n");
             }

src/emc/ini/initraj.cc

@@ -225,6 +225,10 @@ static int loadTraj(EmcIniFile *trajInifile)
         old_inihal_data.traj_default_jerk = jerk;
         planner_type = 0;  // Default: 0 = trapezoidal, 1 = S-curve
         trajInifile->Find(&planner_type, "PLANNER_TYPE", "TRAJ");
+        // Only 0 and 1 are supported, set to 0 if invalid
+        if (planner_type != 0 && planner_type != 1) {
+            planner_type = 0;
+        }
         if (0 != emcTrajPlannerType(planner_type)) {
             if (emc_debug & EMC_DEBUG_CONFIG) {
                 rcs_print("bad return value from emcTrajPlannerType\n");

src/emc/motion/command.c

@@ -1194,7 +1194,12 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 		/* set the type of planner: 0 = trapezoidal, 1 = S-curve */
 		/* can do it at any time */
 		rtapi_print_msg(RTAPI_MSG_DBG, "SET_PLANNER_TYPE, type(%d)", emcmotCommand->planner_type);
-		emcmotStatus->planner_type = 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;
+		}
 		break;
 
 	case EMCMOT_PAUSE:

src/emc/task/emccanon.cc

@@ -704,13 +704,16 @@ static double getStraightJerk(double x, double y, double z,
     }
 
     // Pure linear move:
+    // For jerk-limited motion: d = (1/6)*j*t³, so t = cbrt(6*d/j)
+    // We use t = cbrt(d/j) as a characteristic time (omitting the constant factor,
+    // which cancels out when we compute path jerk = dtot / tmax³)
     if (canon.cartesian_move && !canon.angular_move) {
-        tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
-        ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
-        tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
-        tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
-        tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
-        tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
+        tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
+        ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
+        tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
+        tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
+        tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
+        tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
             out.tmax = MAX3(tx, ty ,tz);
             out.tmax = MAX4(tu, tv, tw, out.tmax);
 
@@ -720,38 +723,38 @@ static double getStraightJerk(double x, double y, double z,
                 out.dtot = sqrt(du * du + dv * dv + dw * dw);
 
         if (out.tmax > 0.0) {
-            out.jerk = out.dtot / (out.tmax * out.tmax);
+            out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax);
         }
     }
     // Pure angular move:
     else if (!canon.cartesian_move && canon.angular_move) {
-        ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
-        tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
-        tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
+        ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
+        tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
+        tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
             out.tmax = MAX3(ta, tb, tc);
 
         out.dtot = sqrt(da * da + db * db + dc * dc);
         if (out.tmax > 0.0) {
-            out.jerk = out.dtot / (out.tmax * out.tmax);
+            out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax);
         }
     }
     // Combination angular and linear move:
     else if (canon.cartesian_move && canon.angular_move) {
-        tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
-        ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
-        tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
-        ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
-        tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
-        tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
-        tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
-        tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
-        tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
+        tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0;
+        ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0;
+        tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0;
+        ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0;
+        tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0;
+        tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0;
+        tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0;
+        tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0;
+        tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0;
             out.tmax = MAX9(tx, ty, tz,
                         ta, tb, tc,
                         tu, tv, tw);
 
         if(debug_velacc)
-            printf("getStraightJerk t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n",
+            printf("getStraightJerk t tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n",
                 tx, ty, tz, ta, tb, tc, tu, tv, tw);
 
         if(dx || dy || dz)
@@ -760,7 +763,7 @@ static double getStraightJerk(double x, double y, double z,
             out.dtot = sqrt(du * du + dv * dv + dw * dw);
 
         if (out.tmax > 0.0) {
-            out.jerk = out.dtot / (out.tmax * out.tmax);
+            out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax);
         }
     }
     //if(debug_velacc)

src/emc/tp/blendmath.c

@@ -1156,7 +1156,7 @@ int blendComputeParameters(BlendParameters * const param)
 
     // Find maximum velocity allowed by accel and radius
     double v_normal;
-    
+
     if(GET_TRAJ_PLANNER_TYPE() == 1){
         v_normal = findSCurveVPeak(param->a_n_max, emcmotStatus->jerk, R_geom);
     }else{
@@ -1179,7 +1179,38 @@ int blendComputeParameters(BlendParameters * const param)
     double s_blend = t_blend * param->v_plan;
     double R_blend = fmin(s_blend / param->phi, R_geom);   //Clamp by limiting radius
 
-    param->R_plan = fmax(pmSq(param->v_plan) / param->a_n_max, R_blend);
+    // Calculate minimum radius needed to keep jerk within limits at v_plan
+    // For circular motion: j = v³/R² (worst case at corner transitions)
+    // Therefore: R_min = v^(3/2) / sqrt(j_max)
+    double R_jerk_min = 0.0;
+    if (GET_TRAJ_PLANNER_TYPE() == 1 && emcmotStatus->jerk > TP_POS_EPSILON) {
+        // R_min = v^(3/2) / sqrt(j)
+        double v_32 = pmSqrt(param->v_plan) * param->v_plan;  // v^(3/2)
+        double j_sqrt = pmSqrt(emcmotStatus->jerk);
+        R_jerk_min = v_32 / j_sqrt;
+
+        tp_debug_print("R_jerk_min = %f (for v=%f, j=%f)\n",
+                       R_jerk_min, param->v_plan, emcmotStatus->jerk);
+    }
+
+    // Calculate radius from acceleration constraint
+    double R_accel = pmSq(param->v_plan) / param->a_n_max;
+
+    // Apply jerk constraint - use the larger of R_blend and R_jerk_min
+    double R_min = fmax(R_blend, R_jerk_min);
+
+    // Final radius must satisfy both acceleration and minimum (blend/jerk) constraints
+    param->R_plan = fmax(R_accel, R_min);
+
+    // Calculate arc length
+    param->s_arc = param->R_plan * param->phi;
+
+    tp_debug_print("R_accel=%f, R_blend=%f, R_jerk_min=%f, R_plan=%f\n",
+                   R_accel, R_blend, R_jerk_min, param->R_plan);
+
+    // Note: Velocity jerk limiting for blend arcs is now handled uniformly
+    // in tcUpdateArcLimits() during segment finalization, along with TC_CIRCULAR arcs.
+
     param->d_plan = param->R_plan / tan(param->theta);
 
     tp_debug_print("v_plan = %f\n", param->v_plan);
@@ -1662,43 +1693,6 @@ double pmCartAbsMax(PmCartesian const * const v)
 }
 
 
-PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle,
-        double v_max,
-        double a_max)
-{
-    double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max;
-
-    double eff_radius = pmCircleEffectiveMinRadius(circle);
-    // Find the acceleration necessary to reach the maximum velocity
-    double a_n_vmax = pmSq(v_max) / fmax(eff_radius, DOUBLE_FUZZ);
-    // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio
-    double v_max_cutoff = pmSqrt(a_n_max_cutoff * eff_radius);
-
-    double v_max_actual = v_max;
-    double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL;
-
-    if (a_n_vmax > a_n_max_cutoff) {
-        v_max_actual = v_max_cutoff;
-    } else {
-        acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max));
-    }
-
-    tp_debug_json_start(pmCircleActualMaxVel);
-    tp_debug_json_double(eff_radius);
-    tp_debug_json_double(v_max);
-    tp_debug_json_double(v_max_cutoff);
-    tp_debug_json_double(a_n_max_cutoff);
-    tp_debug_json_double(a_n_vmax);
-    tp_debug_json_double(acc_ratio_tan);
-    tp_debug_json_end();
-
-    PmCircleLimits limits = {
-        v_max_actual,
-        acc_ratio_tan
-    };
-
-    return limits;
-}
 
 
 /** @section spiralfuncs Functions to approximate spiral arc length */

src/emc/tp/blendmath.h

@@ -243,15 +243,6 @@ int blendPoints3Print(BlendPoints3 const * const points);
 
 double pmCartAbsMax(PmCartesian const * const v);
 
-typedef struct {
-    double v_max;
-    double acc_ratio;
-} PmCircleLimits;
-
-PmCircleLimits pmCircleActualMaxVel(const PmCircle *circle,
-        double v_max_nominal,
-        double a_max_nominal);
-
 int findSpiralArcLengthFit(PmCircle const * const circle,
         SpiralArcLengthFit * const fit);
 int pmCircleAngleFromProgress(PmCircle const * const circle,