fix(trajectory_planner): resume from the applied acceleration after a stop; per-axis jerk; reverse speed limit (#850, #852)

components/trajectory_planner/README.md

@@ -20,11 +20,17 @@ constraints.
 
 - **Normalized inputs** - `set_target(linear, angular)` accepts `[-1, +1]`;
   `+/-1` maps to configured maximum speed. `get_target()` reads it back.
-- **Velocity limiting** - outputs bounded by `max_linear_velocity` / `max_angular_velocity`.
+- **Velocity limiting** - outputs bounded by `max_linear_velocity` / `max_angular_velocity`;
+  an optional `max_reverse_linear_velocity` makes full reverse stick slower than full
+  forward (powered wheelchairs typically reverse at ~70 % of forward speed).
 - **Dual motion profiles** - separate `driving_profile` and `stopping_profile`:
   - **Trapezoidal**: `max_linear_jerk = max_angular_jerk = 0` (default)
   - **S-curve**: non-zero jerk limits; discrete optimal-control landing
   - Recommended: S-curve driving + trapezoidal stopping (jerk = 0) -> smooth starts, overshoot-free stops.
+  - The choice is per axis (a profile may be S-curve on one axis and trapezoidal on
+    the other), and the hand-off between profiles is continuous: each step resumes
+    from the acceleration actually applied on the previous one, so after a
+    trapezoidal stop the next S-curve start begins from zero.
 - **Motion envelope enforcement** - optionally constrains `(v/v_max)^2 + (w/w_max)^2 <= 1`.
 - **Centripetal acceleration limiting** - constrains `|v * w| <= max_centripetal_acceleration`.
 - **Planning timer** - `planning_period` drives `update()` (default 20 ms / 50 Hz);
@@ -39,7 +45,8 @@ constraints.
 
 ```cpp
 espp::TrajectoryPlanner planner({
-    .max_linear_velocity  = 1.0f,       // m/s
+    .max_linear_velocity  = 1.0f,       // m/s at full forward stick
+    .max_reverse_linear_velocity = 0.7f, // m/s at full reverse stick (0 = same as forward)
     .max_angular_velocity = 3.14159f,   // rad/s
 
     // S-curve driving profile

components/trajectory_planner/include/detail/trajectory_planner_math.hpp

@@ -0,0 +1,144 @@
+#pragma once
+
+// The per-axis maths of espp::TrajectoryPlanner, standard-library only so it
+// can be unit-tested on the host (see test/trajectory_planner_host_test.cpp).
+// TrajectoryPlanner::update() steps its linear and angular axes with
+// step_axis(); nothing here knows about targets, profiles or threads.
+
+#include <algorithm>
+#include <cmath>
+#include <tuple>
+
+namespace espp {
+namespace detail {
+namespace trajectory_planner {
+
+/// Minimum velocity-change distance when decelerating |a| to 0 at max jerk
+/// (`step` = jerk * dt).
+inline float stopping_distance(float a_abs, float step, float dt) {
+  const int n = static_cast<int>(a_abs / step);
+  return n * a_abs * dt - n * (n + 1) * 0.5f * step * dt;
+}
+
+/// {d_now, d_maintain, d_increase}, signed like `a`: the velocity change still
+/// to come if the acceleration is wound down to zero now, kept for one more
+/// step, or increased by one more step. Matches the Python model's
+/// stopping_thresholds().
+inline std::tuple<float, float, float> stopping_thresholds(float a, float step, float dt) {
+  if (a == 0.0f)
+    return {0.0f, 0.0f, step * dt};
+  const float sign = a > 0.0f ? 1.0f : -1.0f;
+  const float a_abs = std::abs(a);
+  const float d_now = stopping_distance(a_abs, step, dt);
+  const float d_maintain = a_abs * dt + d_now;
+  const float d_increase = (a_abs + step) * dt + stopping_distance(a_abs + step, step, dt);
+  return {sign * d_now, sign * d_maintain, sign * d_increase};
+}
+
+/**
+ * @brief One jerk-limited acceleration update (discrete optimal control).
+ * @param a Acceleration in / out: the value applied on the previous step, and
+ *          the value to apply now.
+ * @param dist Remaining velocity change to the target (target - v).
+ * @param step Largest acceleration change per step (jerk * dt).
+ * @param a_max Acceleration limit of the profile in force.
+ * @param dt Step length.
+ * @note The change of `a` is limited to `step` in every case, including when
+ *       the incoming `a` is beyond `a_max` (the previous step belonged to a
+ *       profile with a higher limit, e.g. a firmer stopping profile): it is
+ *       then pulled back into range at the jerk limit, not clamped at once.
+ */
+inline void update_accel(float &a, float dist, float step, float a_max, float dt) {
+  const auto [d_now, d_maintain, d_increase] = stopping_thresholds(a, step, dt);
+  const float a_prev = a;
+
+  if (dist > 0.0f) {
+    if (dist > d_increase)
+      a += step;
+    else if (dist <= d_maintain) {
+      const float ideal_a = dist / dt; // positive
+      if (ideal_a <= a && ideal_a >= a - step && ideal_a < step)
+        a = ideal_a;
+      else
+        a -= step;
+    }
+    // else: maintain the current a
+  } else if (dist < 0.0f) {
+    if (dist < d_increase)
+      a -= step;
+    else if (dist >= d_maintain) {
+      const float ideal_a = dist / dt; // negative
+      if (ideal_a >= a && ideal_a <= a + step && ideal_a > -step)
+        a = ideal_a;
+      else
+        a += step;
+    }
+    // else: maintain the current a
+  } else {
+    a = 0.0f;
+  }
+
+  // Safety: limit the change to the jerk limit in case the decision block over-stepped.
+  const float delta_a = a - a_prev;
+  if (std::abs(delta_a) > step)
+    a = a_prev + std::copysign(step, delta_a);
+
+  // Clamp to the profile's acceleration limit; an acceleration inherited from
+  // a profile with a higher limit comes back into range one jerk step at a time.
+  const float lo = std::min(-a_max, a_prev + step);
+  const float hi = std::max(a_max, a_prev - step);
+  a = std::clamp(a, lo, hi);
+}
+
+/// Trapezoidal rate limiter: advance `current` toward `target` by at most max_accel * dt.
+inline float rate_limit(float current, float target, float max_accel, float dt) {
+  const float max_delta = max_accel * dt;
+  return current + std::clamp(target - current, -max_delta, max_delta);
+}
+
+/// The motion state of one axis between steps.
+struct AxisState {
+  float v = 0.0f; ///< velocity
+  float a = 0.0f; ///< acceleration applied on the last step
+};
+
+/**
+ * @brief Advance one axis by one planning step toward `target`.
+ *
+ * With `jerk` > 0 the acceleration is planned by update_accel() (S-curve);
+ * with `jerk` == 0 the velocity is rate-limited by `a_max` directly
+ * (trapezoidal). Either way `state.a` ends up as the acceleration that was
+ * actually applied, so that a later jerk-limited step resumes from the real
+ * value: after a trapezoidal stop has completed it is 0, not the last S-curve
+ * acceleration; if the stop is interrupted it is the deceleration in force;
+ * while the velocity sits on a bound it is 0. An S-curve step whose velocity
+ * was not clamped keeps the planned acceleration bit for bit (recomputing it
+ * from the velocity delta would feed rounding noise into update_accel()'s
+ * step-count truncation).
+ *
+ * @param state Axis state in / out.
+ * @param target Target velocity.
+ * @param v_min Lowest velocity allowed (<= 0; e.g. minus the reverse limit).
+ * @param v_max Highest velocity allowed (>= 0); the result is clamped to [v_min, v_max].
+ * @param a_max Acceleration limit.
+ * @param jerk Jerk limit; 0 selects the trapezoidal profile.
+ * @param dt Step length (> 0).
+ */
+inline void step_axis(AxisState &state, float target, float v_min, float v_max, float a_max,
+                      float jerk, float dt) {
+  const float v_prev = state.v;
+  if (jerk > 0.0f) {
+    update_accel(state.a, target - state.v, jerk * dt, a_max, dt);
+    const float v_free = state.v + state.a * dt;
+    state.v = std::clamp(v_free, v_min, v_max);
+    if (state.v != v_free) // clamped: the acceleration applied is what the bound allowed
+      state.a = (state.v - v_prev) / dt;
+  } else {
+    state.v = std::clamp(rate_limit(state.v, target, a_max, dt), v_min, v_max);
+    state.a = (state.v - v_prev) / dt;
+  }
+}
+
+} // namespace trajectory_planner
+} // namespace detail
+} // namespace espp

components/trajectory_planner/include/trajectory_planner.hpp

@@ -84,11 +84,16 @@ class TrajectoryPlanner : public BaseComponent {
    * @brief Acceleration and jerk limits for one phase of motion.
    *
    * Set max_linear_jerk / max_angular_jerk to 0 for a trapezoidal (ramp)
-   * profile, or to a positive value for an S-curve profile.
+   * profile, or to a positive value for an S-curve profile. The choice is per
+   * axis: a profile may be jerk-limited on one axis and trapezoidal on the
+   * other.
    *
    * @note Using a trapezoidal stopping profile (jerk = 0) is recommended to
    *       avoid S-curve overshoot past zero when the planner decelerates from
-   *       a jerk-limited driving phase.
+   *       a jerk-limited driving phase. The hand-off between the profiles is
+   *       continuous: the next step always resumes from the acceleration
+   *       actually applied on the previous one, and an acceleration beyond the
+   *       new profile's limit is wound back at that profile's jerk limit.
    */
   struct MotionProfile {
     float max_linear_acceleration = 0.0f;  /**< Linear acceleration limit (m/s^2). */
@@ -101,8 +106,12 @@ class TrajectoryPlanner : public BaseComponent {
    * @brief Configuration for the TrajectoryPlanner.
    */
   struct Config {
-    float max_linear_velocity = 1.0f;      /**< Maximum linear velocity magnitude (m/s). */
-    float max_angular_velocity = 3.14159f; /**< Maximum angular velocity magnitude (rad/s). */
+    float max_linear_velocity = 1.0f;         /**< Maximum forward linear velocity (m/s): the
+                                                   speed at full forward stick. */
+    float max_reverse_linear_velocity = 0.0f; /**< Maximum reverse linear velocity (m/s): the
+                                                   speed at full reverse stick. 0 = the same as
+                                                   max_linear_velocity (symmetric). */
+    float max_angular_velocity = 3.14159f;    /**< Maximum angular velocity magnitude (rad/s). */
     espp::TrajectoryPlanner::MotionProfile
         driving_profile; /**< Accel/jerk limits used when target != (0, 0). */
     espp::TrajectoryPlanner::MotionProfile stopping_profile; /**< Accel/jerk limits used when target
@@ -166,7 +175,8 @@ Leave as nullptr to disable. */
    *
    * Both inputs are in the range [-1, +1] and are scaled internally:
    * @code
-   *   v_target  = linear  * max_linear_velocity
+   *   v_target  = linear  * max_linear_velocity          (linear >= 0)
+   *   v_target  = linear  * max_reverse_linear_velocity  (linear < 0; = max_linear_velocity when 0)
    *   w_target  = angular * max_angular_velocity
    * @endcode
    * Values outside [-1, +1] are clamped before scaling.
@@ -182,6 +192,8 @@ Leave as nullptr to disable. */
    * @brief Get the current normalized velocity target.
    * @note  If enforce_motion_envelope is enabled the stored target may differ
    *        from the value passed to set_target() (it is projected onto the unit circle).
+   *        A negative linear target is normalized by the reverse limit, so -1 is
+   *        full reverse stick whatever the two limits are.
    * @return Pair of {linear, angular} in [-1, +1].
    */
   std::pair<float, float> get_target() const;
@@ -215,12 +227,18 @@ Leave as nullptr to disable. */
   bool is_running() const;
 
 protected:
+  /// The reverse linear velocity limit in force (max_reverse_linear_velocity, or
+  /// max_linear_velocity when it is 0). Call with mutex_ held.
+  float reverse_velocity_limit_locked() const;
+
   /// Internal motion state tracked between update() calls.
   struct State {
     float v = 0.0f;   /**< Current linear velocity (m/s). */
     float w = 0.0f;   /**< Current angular velocity (rad/s). */
-    float a_v = 0.0f; /**< Current linear acceleration (m/s^2) -- jerk-limited mode only. */
-    float a_w = 0.0f; /**< Current angular acceleration (rad/s^2) -- jerk-limited mode only. */
+    float a_v = 0.0f; /**< Linear acceleration (m/s^2) applied on the last update(), in either
+                           profile mode (0 once a stop has completed). */
+    float a_w = 0.0f; /**< Angular acceleration (rad/s^2) applied on the last update(), in either
+                           profile mode (0 once a stop has completed). */
   };
   friend struct fmt::formatter<State>;
 

components/trajectory_planner/include/trajectory_planner_formatters.hpp

@@ -54,14 +54,18 @@ template <> struct fmt::formatter<espp::TrajectoryPlanner::Config> {
     return fmt::format_to(ctx.out(),
                           "TrajectoryPlanner::Config{{\n"
                           "  max_linear_velocity:   {:.3f} m/s\n"
+                          "  max_reverse_velocity:  {:.3f} m/s{}\n"
                           "  max_angular_velocity:  {:.3f} rad/s\n"
                           "  driving_profile:       {}\n"
                           "  stopping_profile:      {}\n"
                           "  enforce_envelope:      {}\n"
                           "  max_centripetal_accel: {:.3f} m/s^2\n"
                           "}}",
-                          cfg.max_linear_velocity, cfg.max_angular_velocity, cfg.driving_profile,
-                          cfg.stopping_profile, cfg.enforce_motion_envelope,
-                          cfg.max_centripetal_acceleration);
+                          cfg.max_linear_velocity,
+                          cfg.max_reverse_linear_velocity > 0.0f ? cfg.max_reverse_linear_velocity
+                                                                 : cfg.max_linear_velocity,
+                          cfg.max_reverse_linear_velocity > 0.0f ? "" : " (= forward)",
+                          cfg.max_angular_velocity, cfg.driving_profile, cfg.stopping_profile,
+                          cfg.enforce_motion_envelope, cfg.max_centripetal_acceleration);
   }
 };