Add jerk-limited motion planning

FluidNC/src/Kinematics/ParallelDelta.cpp

@@ -73,6 +73,7 @@ namespace Kinematics {
         auto steps0 = axis0->_stepsPerMm;
         auto accel0 = axis0->_acceleration;
         auto rate0  = axis0->_maxRate;
+        auto jerkFactor0 = axis0->_jerkFactor;
 
         for (axis_t axis = X_AXIS; axis < A_AXIS; axis++) {
             auto axisp = axes->_axis[axis];
@@ -81,6 +82,7 @@ namespace Kinematics {
             axisp->_stepsPerMm   = steps0;
             axisp->_maxRate      = rate0;
             axisp->_acceleration = accel0;
+            axisp->_jerkFactor   = jerkFactor0;
         }
 
         init_position();

FluidNC/src/Machine/Axis.cpp

@@ -9,6 +9,7 @@ namespace Machine {
         handler.item("steps_per_mm", _stepsPerMm, 0.001, 100000.0);
         handler.item("max_rate_mm_per_min", _maxRate, 0.001, 250000.0);
         handler.item("acceleration_mm_per_sec2", _acceleration, 0.001, 100000.0);
+        handler.item("jerk_factor", _jerkFactor, 0.0, 1000.0);
         handler.item("max_travel_mm", _maxTravel, 0.1, 10000000.0);
         handler.item("soft_limits", _softLimits);
         handler.item("idle_disable", _idleDisable);

FluidNC/src/Machine/Axis.h

@@ -33,6 +33,7 @@ namespace Machine {
         float _stepsPerMm   = 80.0f;
         float _maxRate      = 1000.0f;
         float _acceleration = 25.0f;
+        float _jerkFactor   = 0.0f;
         float _maxTravel    = 1000.0f;
         bool  _softLimits   = false;
         bool  _idleDisable  = true;

FluidNC/src/NutsBolts.cpp

@@ -77,36 +77,6 @@ float convert_delta_vector_to_unit_vector(float* v) {
     return magnitude;
 }
 
-const float secPerMinSq = 60.0 * 60.0;  // Seconds Per Minute Squared, for acceleration conversion
-
-float limit_acceleration_by_axis_maximum(float* unit_vec) {
-    float limit_value = SOME_LARGE_VALUE;
-    auto  n_axis      = Axes::_numberAxis;
-    for (axis_t axis = X_AXIS; axis < n_axis; axis++) {
-        auto axisSetting = Axes::_axis[axis];
-        if (unit_vec[axis] != 0) {  // Avoid divide by zero.
-            limit_value = MIN(limit_value, fabsf(axisSetting->_acceleration / unit_vec[axis]));
-        }
-    }
-    // The acceleration setting is stored and displayed in units of mm/sec^2,
-    // but used in units of mm/min^2.  It suffices to perform the conversion once on
-    // exit, since the limit computation above is independent of units - it simply
-    // finds the smallest value.
-    return limit_value * secPerMinSq;
-}
-
-float limit_rate_by_axis_maximum(float* unit_vec) {
-    float limit_value = SOME_LARGE_VALUE;
-    auto  n_axis      = Axes::_numberAxis;
-    for (axis_t axis = X_AXIS; axis < n_axis; axis++) {
-        auto axisSetting = Axes::_axis[axis];
-        if (unit_vec[axis] != 0) {  // Avoid divide by zero.
-            limit_value = MIN(limit_value, fabsf(axisSetting->_maxRate / unit_vec[axis]));
-        }
-    }
-    return limit_value;
-}
-
 bool char_is_numeric(char value) {
     return value >= '0' && value <= '9';
 }

FluidNC/src/NutsBolts.h

@@ -74,8 +74,6 @@ float vector_length(float* v, size_t n);
 void scale_vector(float* v, float scale, size_t n);
 
 float convert_delta_vector_to_unit_vector(float* vector);
-float limit_acceleration_by_axis_maximum(float* unit_vec);
-float limit_rate_by_axis_maximum(float* unit_vec);
 
 const char* to_hex(uint32_t n);
 

FluidNC/src/Planner.cpp

@@ -20,6 +20,52 @@ static uint8_t       block_buffer_head;       // Index of the next block to be p
 static uint8_t       next_buffer_head;        // Index of the next buffer head
 static uint8_t       block_buffer_planned;    // Index of the optimally planned block
 
+namespace {
+const float secPerMinSq = 60.0 * 60.0;         // Seconds Per Minute Squared, for acceleration conversion
+const float secPerMinCu = 60.0 * 60.0 * 60.0;  // Seconds Per Minute Cubed, for jerk conversion
+
+float limit_acceleration_by_axis_maximum(float* unit_vec) {
+    float limit_value = SOME_LARGE_VALUE;
+    auto  n_axis      = Axes::_numberAxis;
+    for (axis_t axis = X_AXIS; axis < n_axis; axis++) {
+        auto axisSetting = Axes::_axis[axis];
+        if (unit_vec[axis] != 0) {  // Avoid divide by zero.
+            limit_value = MIN(limit_value, fabsf(axisSetting->_acceleration / unit_vec[axis]));
+        }
+    }
+    return limit_value * secPerMinSq;
+}
+
+float limit_jerk_by_axis_maximum(float* unit_vec) {
+    float limit_value = SOME_LARGE_VALUE;
+    auto  n_axis      = Axes::_numberAxis;
+    for (axis_t axis = X_AXIS; axis < n_axis; axis++) {
+        auto axisSetting = Axes::_axis[axis];
+        if (unit_vec[axis] != 0) {  // Avoid divide by zero.
+            float axisJerk = 0.0f;
+            if (axisSetting->_jerkFactor > 0.0f) {
+                axisJerk = 60.0f * axisSetting->_jerkFactor * axisSetting->_acceleration * axisSetting->_acceleration
+                           / axisSetting->_maxRate;
+            }
+            limit_value = MIN(limit_value, fabsf(axisJerk / unit_vec[axis]));
+        }
+    }
+    return limit_value * secPerMinCu;
+}
+
+float limit_rate_by_axis_maximum(float* unit_vec) {
+    float limit_value = SOME_LARGE_VALUE;
+    auto  n_axis      = Axes::_numberAxis;
+    for (axis_t axis = X_AXIS; axis < n_axis; axis++) {
+        auto axisSetting = Axes::_axis[axis];
+        if (unit_vec[axis] != 0) {  // Avoid divide by zero.
+            limit_value = MIN(limit_value, fabsf(axisSetting->_maxRate / unit_vec[axis]));
+        }
+    }
+    return limit_value;
+}
+}  // namespace
+
 void plan_init() {
     if (block_buffer) {
         delete[] block_buffer;
@@ -244,6 +290,41 @@ uint8_t plan_check_full_buffer() {
     return block_buffer_tail == next_buffer_head;
 }
 
+static float plan_compute_effective_acceleration(plan_block_t* block, float target_speed) {
+    if (block->jerk <= 0.0f) {
+        return block->max_acceleration;
+    }
+
+    float time_to_max_accel     = block->max_acceleration / block->jerk;
+    float speed_after_jerk_ramp = 0.5f * block->jerk * time_to_max_accel * time_to_max_accel;
+    float half_target_speed     = 0.5f * target_speed;
+
+    if (half_target_speed > speed_after_jerk_ramp) {
+        return target_speed
+               / (2.0f * (time_to_max_accel + (half_target_speed - speed_after_jerk_ramp) / block->max_acceleration));
+    }
+
+    return target_speed / (2.0f * sqrtf(target_speed / block->jerk));
+}
+
+static void plan_refresh_block_directional_limits(plan_block_t* block) {
+    float unit_vec[MAX_N_AXIS];
+    auto  n_axis = Axes::_numberAxis;
+
+    for (axis_t axis = X_AXIS; axis < n_axis; axis++) {
+        steps_t signed_steps = block->steps[axis];
+        if (block->direction_bits & bitnum_to_mask(axis)) {
+            signed_steps = -signed_steps;
+        }
+        unit_vec[axis] = steps_to_motor_pos(signed_steps, axis);
+    }
+
+    convert_delta_vector_to_unit_vector(unit_vec);
+    block->max_acceleration = limit_acceleration_by_axis_maximum(unit_vec);
+    block->jerk             = (block->is_jog || block->motion.systemMotion) ? 0.0f : limit_jerk_by_axis_maximum(unit_vec);
+    block->rapid_rate       = limit_rate_by_axis_maximum(unit_vec);
+}
+
 // Computes and returns block nominal speed based on running condition and override values.
 // NOTE: All system motion commands, such as homing/parking, are not subject to overrides.
 float plan_compute_profile_nominal_speed(plan_block_t* block) {
@@ -287,7 +368,9 @@ void plan_update_velocity_profile_parameters() {
     float         prev_nominal_speed = SOME_LARGE_VALUE;  // Set high for first block nominal speed calculation.
     while (block_index != block_buffer_head) {
         block         = &block_buffer[block_index];
+        plan_refresh_block_directional_limits(block);
         nominal_speed = plan_compute_profile_nominal_speed(block);
+        block->acceleration = plan_compute_effective_acceleration(block, nominal_speed);
         plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed);
         prev_nominal_speed = nominal_speed;
         block_index        = plan_next_block_index(block_index);
@@ -348,9 +431,8 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) {
     // down such that no individual axes maximum values are exceeded with respect to the line direction.
     // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
     // if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
-    block->millimeters  = convert_delta_vector_to_unit_vector(unit_vec);
-    block->acceleration = limit_acceleration_by_axis_maximum(unit_vec);
-    block->rapid_rate   = limit_rate_by_axis_maximum(unit_vec);
+    block->millimeters = convert_delta_vector_to_unit_vector(unit_vec);
+    plan_refresh_block_directional_limits(block);
     // Store programmed rate.
     if (block->motion.rapidMotion) {
         block->programmed_rate = block->rapid_rate;
@@ -360,6 +442,11 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) {
             block->programmed_rate *= block->millimeters;
         }
     }
+
+    // Use an effective acceleration for lookahead while keeping the stepper-side
+    // segment generator free to apply a jerk-limited ramp shape later.
+    block->acceleration = plan_compute_effective_acceleration(block, plan_compute_profile_nominal_speed(block));
+
     // TODO: Need to check this method handling zero junction speeds when starting from rest.
     if ((block_buffer_head == block_buffer_tail) || (block->motion.systemMotion)) {
         // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later.

FluidNC/src/Planner.h

@@ -44,8 +44,10 @@ struct plan_block_t {
     float entry_speed_sqr;      // The current planned entry speed at block junction in (mm/min)^2
     float max_entry_speed_sqr;  // Maximum allowable entry speed based on the minimum of junction limit and
     //   neighboring nominal speeds with overrides in (mm/min)^2
-    float acceleration;  // Axis-limit adjusted line acceleration in (mm/min^2). Does not change.
-    float millimeters;   // The remaining distance for this block to be executed in (mm).
+    float acceleration;      // Effective line acceleration used by the planner in (mm/min^2).
+    float max_acceleration;  // Axis-limit adjusted line acceleration in (mm/min^2). Does not change.
+    float jerk;              // Axis-limit adjusted line jerk in (mm/min^3). Does not change.
+    float millimeters;       // The remaining distance for this block to be executed in (mm).
     // NOTE: This value may be altered by stepper algorithm during execution.
 
     // Stored rate limiting data used by planner when changes occur.

FluidNC/src/Protocol.cpp

@@ -698,6 +698,7 @@ static void protocol_do_initiate_cycle() {
     // log_debug("protocol_do_initiate_cycle " << state_name());
     // Start cycle only if queued motions exist in planner buffer and the motion is not canceled.
     sys.step_control = {};  // Restore step control to normal operation
+    plan_update_velocity_profile_parameters();
     plan_block_t* pb;
     if ((pb = plan_get_current_block()) && !sys.suspend().bit.motionCancel) {
         auto suspend  = sys.suspend();