From a63ad9ecb54a0a46be7b081902d234f801d41b26 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 20 Oct 2026 15:30:25 -0500 Subject: [PATCH 1/3] fix(trajectory_planner): keep the applied acceleration across profile hand-offs; per-axis jerk update() only wrote state_.a_v / a_w in the jerk-limited branch, so after a trapezoidal stop the S-curve resumed from the last driving acceleration: the first step after a stop jumped straight to it, and a reversal from standstill first accelerated the wrong way (issue #850). A profile jerk-limited on one axis only also fed the other axis through the S-curve logic with a huge fake jerk, whose decision block then never left zero acceleration. The per-axis maths now lives in detail/trajectory_planner_math.hpp (standard-library only) and update() steps each axis with step_axis(): the S-curve where that axis' jerk is > 0, the trapezoidal rate limiter where it is 0, and in both cases the acceleration actually applied, (v_new - v_old) / dt after the velocity clamp, is kept in the state. An acceleration inherited from a profile with a higher limit (a firm stop interrupted by the stick) is wound back into the new profile's range at its jerk limit instead of being clamped in one step. Host test (components/trajectory_planner/test) runs the issue's scenario -- 0.5 s driving, 0.5 s trapezoidal stop, reverse -- and checks the stop leaves a = 0, the first reverse step is at the jerk limit and backwards, the jerk limit holds over whole stop-and-go cycles including an interrupted stop, the all-trapezoidal case is unchanged, and the mixed-axis profile moves. Fixes #850 Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01DFjjnq3XCTRAXENSxsCxJU --- components/trajectory_planner/README.md | 4 + .../detail/trajectory_planner_math.hpp | 137 +++++++++++++++ .../include/trajectory_planner.hpp | 15 +- .../src/trajectory_planner.cpp | 108 ++---------- .../test/trajectory_planner_host_test.cpp | 160 ++++++++++++++++++ 5 files changed, 329 insertions(+), 95 deletions(-) create mode 100644 components/trajectory_planner/include/detail/trajectory_planner_math.hpp create mode 100644 components/trajectory_planner/test/trajectory_planner_host_test.cpp diff --git a/components/trajectory_planner/README.md b/components/trajectory_planner/README.md index 7128f65116..402dabc0c9 100644 --- a/components/trajectory_planner/README.md +++ b/components/trajectory_planner/README.md @@ -25,6 +25,10 @@ constraints. - **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); diff --git a/components/trajectory_planner/include/detail/trajectory_planner_math.hpp b/components/trajectory_planner/include/detail/trajectory_planner_math.hpp new file mode 100644 index 0000000000..6bb04ff60b --- /dev/null +++ b/components/trajectory_planner/include/detail/trajectory_planner_math.hpp @@ -0,0 +1,137 @@ +#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 +#include +#include + +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(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 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 -- (v_new - v_old) / dt, after the velocity clamp -- 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, + * and if the stop is interrupted it is the deceleration in force. + * + * @param state Axis state in / out. + * @param target Target velocity. + * @param v_max Velocity limit (the result is clamped to +-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_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); + state.v = std::clamp(state.v + state.a * dt, -v_max, v_max); + } else { + state.v = std::clamp(rate_limit(state.v, target, a_max, dt), -v_max, v_max); + } + state.a = (state.v - v_prev) / dt; +} + +} // namespace trajectory_planner +} // namespace detail +} // namespace espp diff --git a/components/trajectory_planner/include/trajectory_planner.hpp b/components/trajectory_planner/include/trajectory_planner.hpp index 8f7a183f92..7de406bf03 100644 --- a/components/trajectory_planner/include/trajectory_planner.hpp +++ b/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). */ @@ -219,8 +224,10 @@ Leave as nullptr to disable. */ 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; diff --git a/components/trajectory_planner/src/trajectory_planner.cpp b/components/trajectory_planner/src/trajectory_planner.cpp index 824f5d8b1f..ad2797f349 100644 --- a/components/trajectory_planner/src/trajectory_planner.cpp +++ b/components/trajectory_planner/src/trajectory_planner.cpp @@ -1,73 +1,6 @@ #include "trajectory_planner.hpp" -#include - -namespace { - -// Minimum velocity-change distance when decelerating |a| to 0 at max jerk. -static float sd(float a_abs, float step, float dt_) { - int n = static_cast(a_abs / step); - return n * a_abs * dt_ - n * (n + 1) * 0.5f * step * dt_; -} - -// Returns {d_now, d_maintain, d_increase} signed by sign(a). Matches Python stopping_thresholds(). -static std::tuple stopping_thresh(float a, float step, float dt_) { - if (a == 0.0f) - return {0.0f, 0.0f, step * dt_}; - float sign = a > 0.0f ? 1.0f : -1.0f; - float a_abs = std::abs(a); - float d_now = sd(a_abs, step, dt_); - float d_maintain = a_abs * dt_ + d_now; - float d_increase = (a_abs + step) * dt_ + sd(a_abs + step, step, dt_); - return {sign * d_now, sign * d_maintain, sign * d_increase}; -} - -// Per-axis discrete optimal-control acceleration update. Matches the Python decision block. -static void update_accel(float &a, float dist, float step, float a_max, float dt_) { - auto [d_now, d_maintain, d_increase] = stopping_thresh(a, step, dt_); - float a_prev = a; - - if (dist > 0.0f) { - if (dist > d_increase) - a += step; - else if (dist <= d_maintain) { - auto ideal_a = dist / dt_; // positive - if (ideal_a <= a && ideal_a >= a - step && ideal_a < step) - a = ideal_a; - else - a -= step; - } - // else: maintain current a - } else if (dist < 0.0f) { - if (dist < d_increase) - a -= step; - else if (dist >= d_maintain) { - auto ideal_a = dist / dt_; // negative - if (ideal_a >= a && ideal_a <= a + step && ideal_a > -step) - a = ideal_a; - else - a += step; - } - // else: maintain current a - } else { - a = 0.0f; - } - - // Safety: clamp delta_a to the jerk limit in case the decision block over-stepped. - float delta_a = a - a_prev; - if (std::abs(delta_a) > step) - a = a_prev + std::copysign(step, delta_a); - - a = std::clamp(a, -a_max, a_max); -} - -// Trapezoidal rate-limiter: advance current toward target by at most max_accel*dt. -static float rate_limit(float current, float target, float max_accel, float dt_) { - float max_delta = max_accel * dt_; - return current + std::clamp(target - current, -max_delta, max_delta); -} - -} // namespace +#include "detail/trajectory_planner_math.hpp" namespace espp { @@ -225,29 +158,22 @@ void TrajectoryPlanner::update(float dt) { const bool is_stopping = (target_v_ == 0.0f && target_w_ == 0.0f); const MotionProfile &profile = is_stopping ? config_.stopping_profile : config_.driving_profile; - const bool jerk_limited = (profile.max_linear_jerk > 0.0f) || (profile.max_angular_jerk > 0.0f); - - if (jerk_limited) { - const float step_lj = - profile.max_linear_jerk > 0.0f ? profile.max_linear_jerk * dt : 1e9f * dt; - const float step_aj = - profile.max_angular_jerk > 0.0f ? profile.max_angular_jerk * dt : 1e9f * dt; - - update_accel(state_.a_v, target_v_ - state_.v, step_lj, profile.max_linear_acceleration, dt); - update_accel(state_.a_w, target_w_ - state_.w, step_aj, profile.max_angular_acceleration, dt); - - state_.v = std::clamp(state_.v + state_.a_v * dt, -config_.max_linear_velocity, - config_.max_linear_velocity); - state_.w = std::clamp(state_.w + state_.a_w * dt, -config_.max_angular_velocity, - config_.max_angular_velocity); - - } else { - // Trapezoidal mode: rate-limit velocity directly by acceleration limit. - state_.v = std::clamp(rate_limit(state_.v, target_v_, profile.max_linear_acceleration, dt), - -config_.max_linear_velocity, config_.max_linear_velocity); - state_.w = std::clamp(rate_limit(state_.w, target_w_, profile.max_angular_acceleration, dt), - -config_.max_angular_velocity, config_.max_angular_velocity); - } + // Each axis is stepped on its own: S-curve where its jerk limit is > 0, + // trapezoidal where it is 0. The acceleration actually applied is kept + // in the state either way, so a jerk-limited step after a trapezoidal + // one (a stop, then driving again) resumes from the real value. + using detail::trajectory_planner::AxisState; + using detail::trajectory_planner::step_axis; + AxisState lin{.v = state_.v, .a = state_.a_v}; + AxisState ang{.v = state_.w, .a = state_.a_w}; + step_axis(lin, target_v_, config_.max_linear_velocity, profile.max_linear_acceleration, + profile.max_linear_jerk, dt); + step_axis(ang, target_w_, config_.max_angular_velocity, profile.max_angular_acceleration, + profile.max_angular_jerk, dt); + state_.v = lin.v; + state_.a_v = lin.a; + state_.w = ang.v; + state_.a_w = ang.a; logger_.debug("State: {}, target_v={:.3f}, target_w={:.3f}", state_, target_v_, target_w_); } // mutex_ released here diff --git a/components/trajectory_planner/test/trajectory_planner_host_test.cpp b/components/trajectory_planner/test/trajectory_planner_host_test.cpp new file mode 100644 index 0000000000..cb020e3a77 --- /dev/null +++ b/components/trajectory_planner/test/trajectory_planner_host_test.cpp @@ -0,0 +1,160 @@ +// Host-buildable test for the TrajectoryPlanner's per-axis maths +// (include/detail/trajectory_planner_math.hpp -- standard library only). +// Steps one axis through the planner's profile hand-offs exactly as +// TrajectoryPlanner::update() does. Build & run: +// c++ -std=c++23 -Wall -Wextra -Werror -I components/trajectory_planner/include \ +// components/trajectory_planner/test/trajectory_planner_host_test.cpp -o test && ./test + +#include +#include + +#include "detail/trajectory_planner_math.hpp" + +namespace tp = espp::detail::trajectory_planner; + +static int g_failures = 0; +#define CHECK(cond) \ + do { \ + if (!(cond)) { \ + std::printf(" FAIL: %s (line %d)\n", #cond, __LINE__); \ + ++g_failures; \ + } \ + } while (0) + +static bool near(float a, float b, float tol = 1e-4f) { return std::abs(a - b) <= tol; } + +// One axis with the planner's profile selection: a driving profile while the +// target is non-zero, a stopping profile while it is zero (as update() does). +struct Axis { + float v_max = 1.0f, a_drive = 1.0f, j_drive = 0.0f, a_stop = 1.0f, j_stop = 0.0f, dt = 0.02f; + tp::AxisState s{}; + float max_jerk_seen = 0.0f; // |delta a| / dt over every step + void step(float target) { + const bool stopping = target == 0.0f; + const float a_prev = s.a; + tp::step_axis(s, target, v_max, stopping ? a_stop : a_drive, stopping ? j_stop : j_drive, dt); + max_jerk_seen = std::max(max_jerk_seen, std::abs(s.a - a_prev) / dt); + } + void run(float target, int steps) { + for (int i = 0; i < steps; i++) + step(target); + } +}; + +int main() { + // ---- the scenario of issue #850: S-curve driving, trapezoidal stopping ---- + // driving a 1.0 m/s^2 / 5 m/s^3, stopping 1.6 m/s^2 trapezoidal, 50 Hz + { + Axis ax{.v_max = 1.8f, + .a_drive = 1.0f, + .j_drive = 5.0f, + .a_stop = 1.6f, + .j_stop = 0.0f, + .dt = 0.02f}; + ax.run(1.8f, 25); // drive 0.5 s: a ramps 0.1 .. 1.0 over 10 steps, then holds 1.0 + CHECK(near(ax.s.v, 0.41f) && near(ax.s.a, 1.0f)); + ax.run(0.0f, 25); // stop 0.5 s (needs 13 steps at 1.6 m/s^2) + CHECK(near(ax.s.v, 0.0f)); + CHECK(near(ax.s.a, 0.0f)); // the stale 1.0 of the report is gone + // reverse: the first step must already go backwards, at the jerk limit (-0.1 m/s^2) + ax.step(-1.8f); + CHECK(near(ax.s.a, -0.1f) && ax.s.v < 0.0f); + float v_peak = 0.0f; + for (int i = 0; i < 24; i++) { + ax.step(-1.8f); + v_peak = std::max(v_peak, ax.s.v); + } + CHECK(v_peak <= 0.0f); // never moved forward first (the report saw +0.090 m/s) + // forward again from standstill: the first step is jerk-limited, not a jump to 1.0 + ax.run(0.0f, 50); + CHECK(near(ax.s.v, 0.0f) && near(ax.s.a, 0.0f)); + ax.step(1.8f); + CHECK(near(ax.s.a, 0.1f)); + } + + // ---- the jerk limit holds across the whole stop-and-go cycle in driving mode ---- + // (the trapezoidal stop itself is allowed its own acceleration step) + { + Axis ax{.v_max = 1.8f, + .a_drive = 1.0f, + .j_drive = 5.0f, + .a_stop = 1.6f, + .j_stop = 0.0f, + .dt = 0.02f}; + ax.run(1.8f, 25); + ax.run(0.0f, 25); + ax.max_jerk_seen = 0.0f; + ax.run(-1.8f, 100); + ax.run(1.8f, 100); + CHECK(ax.max_jerk_seen <= 5.0f + 1e-3f); + } + + // ---- a stop interrupted mid-way: the S-curve resumes from the real deceleration ---- + // and winds it back into its own limit at the jerk limit (no 1.6 -> 1.0 jump) + { + Axis ax{.v_max = 1.8f, + .a_drive = 1.0f, + .j_drive = 5.0f, + .a_stop = 1.6f, + .j_stop = 0.0f, + .dt = 0.02f}; + ax.run(1.8f, 40); // v = 0.71 + ax.run(0.0f, 5); // braking at -1.6 m/s^2 for 0.1 s + CHECK(near(ax.s.a, -1.6f)); + ax.max_jerk_seen = 0.0f; + ax.step(1.8f); // push the stick again: a moves from -1.6 by at most 0.1 + CHECK(near(ax.s.a, -1.5f)); + ax.run(1.8f, 60); + CHECK(ax.max_jerk_seen <= 5.0f + 1e-3f); + CHECK(ax.s.a <= 1.0f + 1e-6f); + } + + // ---- both profiles trapezoidal: unchanged behaviour, acceleration recorded ---- + { + Axis ax{.v_max = 1.8f, + .a_drive = 1.0f, + .j_drive = 0.0f, + .a_stop = 1.6f, + .j_stop = 0.0f, + .dt = 0.02f}; + ax.step(1.8f); + CHECK(near(ax.s.v, 0.02f) && near(ax.s.a, 1.0f)); + ax.run(1.8f, 200); + CHECK(near(ax.s.v, 1.8f) && near(ax.s.a, 0.0f)); // at the velocity limit: no acceleration + ax.run(0.0f, 100); + CHECK(near(ax.s.v, 0.0f) && near(ax.s.a, 0.0f)); + } + + // ---- a profile jerk-limited on one axis only: the other axis is trapezoidal ---- + // (it used to be fed through the S-curve logic with a huge "jerk" and never moved) + { + tp::AxisState s{}; + tp::step_axis(s, 3.0f, 3.0f, 6.0f, 0.0f, 0.02f); + CHECK(near(s.v, 0.12f) && near(s.a, 6.0f)); + } + + // ---- the S-curve alone: ramps at the jerk limit, caps at a_max, lands without overshoot ---- + { + Axis ax{.v_max = 10.0f, + .a_drive = 2.0f, + .j_drive = 2.0f, + .a_stop = 2.0f, + .j_stop = 2.0f, + .dt = 0.05f}; + float v_peak = 0.0f, a_peak = 0.0f; + for (int i = 0; i < 200; i++) { + ax.step(3.0f); + v_peak = std::max(v_peak, ax.s.v); + a_peak = std::max(a_peak, ax.s.a); + } + CHECK(near(ax.s.v, 3.0f, 1e-3f) && near(ax.s.a, 0.0f, 1e-3f)); + CHECK(v_peak <= 3.0f + 1e-3f && a_peak <= 2.0f + 1e-6f); + CHECK(ax.max_jerk_seen <= 2.0f + 1e-3f); + } + + if (g_failures == 0) + std::printf("trajectory_planner_host_test: all checks passed\n"); + else + std::printf("trajectory_planner_host_test: %d check(s) FAILED\n", g_failures); + return g_failures == 0 ? 0 : 1; +} From 9065b5c11e61e3d223c3cef8cb92586a2e3719e9 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 20 Oct 2026 15:56:03 -0500 Subject: [PATCH 2/3] feat(trajectory_planner): separate maximum reverse linear velocity Powered wheelchair controllers run reverse slower than forward (the R-Net recording in #852: 71 %), and scaling the stick outside the planner puts chassis behaviour where it does not belong. Config gains max_reverse_linear_velocity (m/s at full reverse stick; 0 = the same as max_linear_velocity, so nothing changes by default): set_target() scales a negative linear input by it, get_target() normalizes by it, and the linear axis is clamped to [-reverse limit, +max_linear_velocity] through the per-axis step's new asymmetric bounds. Validated (>= 0), shown by the Config formatter, documented in the README; the python bindings and stub carry the new field and constructor argument (mirrored by hand, litgen is not installed here; same order as the struct). Verified through the python package against the planner's own timer: full reverse settles at the reverse limit and reads back as -1, full forward at the forward limit, the default stays symmetric. Fixes #852 Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01DFjjnq3XCTRAXENSxsCxJU --- components/trajectory_planner/README.md | 7 +++-- .../detail/trajectory_planner_math.hpp | 11 ++++---- .../include/trajectory_planner.hpp | 17 +++++++++--- .../include/trajectory_planner_formatters.hpp | 10 ++++--- .../src/trajectory_planner.cpp | 26 ++++++++++++++----- .../test/trajectory_planner_host_test.cpp | 16 ++++++++++-- lib/python_bindings/espp/__init__.pyi | 7 +++-- lib/python_bindings/pybind_espp.cpp | 24 ++++++++++++----- 8 files changed, 88 insertions(+), 30 deletions(-) diff --git a/components/trajectory_planner/README.md b/components/trajectory_planner/README.md index 402dabc0c9..5b667cce2d 100644 --- a/components/trajectory_planner/README.md +++ b/components/trajectory_planner/README.md @@ -20,7 +20,9 @@ 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 @@ -43,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 diff --git a/components/trajectory_planner/include/detail/trajectory_planner_math.hpp b/components/trajectory_planner/include/detail/trajectory_planner_math.hpp index 6bb04ff60b..9648a03bb5 100644 --- a/components/trajectory_planner/include/detail/trajectory_planner_math.hpp +++ b/components/trajectory_planner/include/detail/trajectory_planner_math.hpp @@ -115,19 +115,20 @@ struct AxisState { * * @param state Axis state in / out. * @param target Target velocity. - * @param v_max Velocity limit (the result is clamped to +-v_max). + * @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_max, float a_max, float jerk, - float dt) { +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); - state.v = std::clamp(state.v + state.a * dt, -v_max, v_max); + state.v = std::clamp(state.v + state.a * dt, v_min, v_max); } else { - state.v = std::clamp(rate_limit(state.v, target, a_max, dt), -v_max, v_max); + state.v = std::clamp(rate_limit(state.v, target, a_max, dt), v_min, v_max); } state.a = (state.v - v_prev) / dt; } diff --git a/components/trajectory_planner/include/trajectory_planner.hpp b/components/trajectory_planner/include/trajectory_planner.hpp index 7de406bf03..e2d7265fba 100644 --- a/components/trajectory_planner/include/trajectory_planner.hpp +++ b/components/trajectory_planner/include/trajectory_planner.hpp @@ -106,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 @@ -171,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. @@ -187,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 get_target() const; @@ -220,6 +227,10 @@ 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). */ diff --git a/components/trajectory_planner/include/trajectory_planner_formatters.hpp b/components/trajectory_planner/include/trajectory_planner_formatters.hpp index 2712838a2c..ee32033f11 100644 --- a/components/trajectory_planner/include/trajectory_planner_formatters.hpp +++ b/components/trajectory_planner/include/trajectory_planner_formatters.hpp @@ -54,14 +54,18 @@ template <> struct fmt::formatter { 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); } }; diff --git a/components/trajectory_planner/src/trajectory_planner.cpp b/components/trajectory_planner/src/trajectory_planner.cpp index ad2797f349..77f0143f35 100644 --- a/components/trajectory_planner/src/trajectory_planner.cpp +++ b/components/trajectory_planner/src/trajectory_planner.cpp @@ -26,6 +26,10 @@ bool TrajectoryPlanner::set_config(const Config &config, bool reset_state) { logger_.error("max_linear_velocity must be > 0"); return false; } + if (config.max_reverse_linear_velocity < 0.0f) { + logger_.error("max_reverse_linear_velocity must be >= 0 (0 = same as max_linear_velocity)"); + return false; + } if (config.max_angular_velocity <= 0.0f) { logger_.error("max_angular_velocity must be > 0"); return false; @@ -76,6 +80,11 @@ bool TrajectoryPlanner::set_config(const Config &config, bool reset_state) { return true; } +float TrajectoryPlanner::reverse_velocity_limit_locked() const { + return config_.max_reverse_linear_velocity > 0.0f ? config_.max_reverse_linear_velocity + : config_.max_linear_velocity; +} + const TrajectoryPlanner::Config &TrajectoryPlanner::get_config() const { std::lock_guard lk(mutex_); return config_; @@ -97,12 +106,14 @@ void TrajectoryPlanner::set_target(float linear, float angular) { bool enforce_motion_envelope = false; float max_centripetal_acceleration = 0.0f; float max_linear_velocity = 0.0f; + float max_reverse_velocity = 0.0f; float max_angular_velocity = 0.0f; { std::lock_guard lk(mutex_); enforce_motion_envelope = config_.enforce_motion_envelope; max_centripetal_acceleration = config_.max_centripetal_acceleration; max_linear_velocity = config_.max_linear_velocity; + max_reverse_velocity = reverse_velocity_limit_locked(); max_angular_velocity = config_.max_angular_velocity; } @@ -116,7 +127,8 @@ void TrajectoryPlanner::set_target(float linear, float angular) { } } - auto target_v = linear * max_linear_velocity; + // full forward stick = max_linear_velocity, full reverse stick = the reverse limit + auto target_v = linear * (linear < 0.0f ? max_reverse_velocity : max_linear_velocity); auto target_w = angular * max_angular_velocity; // Centripetal acceleration limit: |v · w| ≤ a_c_max @@ -143,7 +155,9 @@ void TrajectoryPlanner::set_target(float linear, float angular) { std::pair TrajectoryPlanner::get_target() const { std::lock_guard lk(mutex_); - return {target_v_ / config_.max_linear_velocity, target_w_ / config_.max_angular_velocity}; + const float v_limit = + target_v_ < 0.0f ? reverse_velocity_limit_locked() : config_.max_linear_velocity; + return {target_v_ / v_limit, target_w_ / config_.max_angular_velocity}; } void TrajectoryPlanner::update(float dt) { @@ -166,10 +180,10 @@ void TrajectoryPlanner::update(float dt) { using detail::trajectory_planner::step_axis; AxisState lin{.v = state_.v, .a = state_.a_v}; AxisState ang{.v = state_.w, .a = state_.a_w}; - step_axis(lin, target_v_, config_.max_linear_velocity, profile.max_linear_acceleration, - profile.max_linear_jerk, dt); - step_axis(ang, target_w_, config_.max_angular_velocity, profile.max_angular_acceleration, - profile.max_angular_jerk, dt); + step_axis(lin, target_v_, -reverse_velocity_limit_locked(), config_.max_linear_velocity, + profile.max_linear_acceleration, profile.max_linear_jerk, dt); + step_axis(ang, target_w_, -config_.max_angular_velocity, config_.max_angular_velocity, + profile.max_angular_acceleration, profile.max_angular_jerk, dt); state_.v = lin.v; state_.a_v = lin.a; state_.w = ang.v; diff --git a/components/trajectory_planner/test/trajectory_planner_host_test.cpp b/components/trajectory_planner/test/trajectory_planner_host_test.cpp index cb020e3a77..dea33e7b46 100644 --- a/components/trajectory_planner/test/trajectory_planner_host_test.cpp +++ b/components/trajectory_planner/test/trajectory_planner_host_test.cpp @@ -32,7 +32,8 @@ struct Axis { void step(float target) { const bool stopping = target == 0.0f; const float a_prev = s.a; - tp::step_axis(s, target, v_max, stopping ? a_stop : a_drive, stopping ? j_stop : j_drive, dt); + tp::step_axis(s, target, -v_max, v_max, stopping ? a_stop : a_drive, + stopping ? j_stop : j_drive, dt); max_jerk_seen = std::max(max_jerk_seen, std::abs(s.a - a_prev) / dt); } void run(float target, int steps) { @@ -129,10 +130,21 @@ int main() { // (it used to be fed through the S-curve logic with a huge "jerk" and never moved) { tp::AxisState s{}; - tp::step_axis(s, 3.0f, 3.0f, 6.0f, 0.0f, 0.02f); + tp::step_axis(s, 3.0f, -3.0f, 3.0f, 6.0f, 0.0f, 0.02f); CHECK(near(s.v, 0.12f) && near(s.a, 6.0f)); } + // ---- asymmetric velocity bounds (issue #852): full reverse settles at the reverse limit ---- + { + tp::AxisState s{}; + for (int i = 0; i < 100; i++) + tp::step_axis(s, -1.8f, -1.0f, 1.8f, 1.6f, 0.0f, 0.02f); // target beyond the bound + CHECK(near(s.v, -1.0f) && near(s.a, 0.0f)); + for (int i = 0; i < 100; i++) + tp::step_axis(s, 1.8f, -1.0f, 1.8f, 1.6f, 0.0f, 0.02f); + CHECK(near(s.v, 1.8f)); + } + // ---- the S-curve alone: ramps at the jerk limit, caps at a_max, lands without overshoot ---- { Axis ax{.v_max = 10.0f, diff --git a/lib/python_bindings/espp/__init__.pyi b/lib/python_bindings/espp/__init__.pyi index d6a780f919..1c3d289a0c 100644 --- a/lib/python_bindings/espp/__init__.pyi +++ b/lib/python_bindings/espp/__init__.pyi @@ -3997,7 +3997,8 @@ class TrajectoryPlanner: * @brief Configuration for the TrajectoryPlanner. """ - max_linear_velocity: float = 1.0 #*< Maximum linear velocity magnitude (m/s). + max_linear_velocity: float = 1.0 #*< Maximum forward linear velocity (m/s): the speed at full forward stick. + max_reverse_linear_velocity: float = 0.0 #*< Maximum reverse linear velocity (m/s): the speed at full reverse stick. 0 = the same as max_linear_velocity (symmetric). max_angular_velocity: float = 3.14159 #*< Maximum angular velocity magnitude (rad/s). driving_profile: TrajectoryPlanner.MotionProfile #*< Accel/jerk limits used when target != (0, 0). stopping_profile: TrajectoryPlanner.MotionProfile #*< Accel/jerk limits used when target @@ -4030,6 +4031,7 @@ class TrajectoryPlanner: def __init__( self, max_linear_velocity: float = 1.0, + max_reverse_linear_velocity: float = 0.0, max_angular_velocity: float = 3.14159, driving_profile: TrajectoryPlanner.MotionProfile = TrajectoryPlanner.MotionProfile(), stopping_profile: TrajectoryPlanner.MotionProfile = TrajectoryPlanner.MotionProfile(), @@ -4080,7 +4082,8 @@ class TrajectoryPlanner: * * 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. diff --git a/lib/python_bindings/pybind_espp.cpp b/lib/python_bindings/pybind_espp.cpp index 38c1ab81e4..6ba72fe1cd 100644 --- a/lib/python_bindings/pybind_espp.cpp +++ b/lib/python_bindings/pybind_espp.cpp @@ -2547,7 +2547,8 @@ void py_init_module_espp(py::module &m) { "*\n * @brief Configuration for the TrajectoryPlanner.\n") .def( py::init<>( - [](float max_linear_velocity = 1.0f, float max_angular_velocity = 3.14159f, + [](float max_linear_velocity = 1.0f, float max_reverse_linear_velocity = 0.0f, + float max_angular_velocity = 3.14159f, espp::TrajectoryPlanner::MotionProfile driving_profile = espp::TrajectoryPlanner::MotionProfile(), espp::TrajectoryPlanner::MotionProfile stopping_profile = @@ -2567,6 +2568,7 @@ void py_init_module_espp(py::module &m) { espp::Logger::Verbosity log_level = espp::Logger::Verbosity::WARN) { auto r_ctor_ = std::make_unique(); r_ctor_->max_linear_velocity = max_linear_velocity; + r_ctor_->max_reverse_linear_velocity = max_reverse_linear_velocity; r_ctor_->max_angular_velocity = max_angular_velocity; r_ctor_->driving_profile = driving_profile; r_ctor_->stopping_profile = stopping_profile; @@ -2579,7 +2581,9 @@ void py_init_module_espp(py::module &m) { r_ctor_->log_level = log_level; return r_ctor_; }), - py::arg("max_linear_velocity") = 1.0f, py::arg("max_angular_velocity") = 3.14159f, + py::arg("max_linear_velocity") = 1.0f, + py::arg("max_reverse_linear_velocity") = 0.0f, + py::arg("max_angular_velocity") = 3.14159f, py::arg("driving_profile") = espp::TrajectoryPlanner::MotionProfile(), py::arg("stopping_profile") = espp::TrajectoryPlanner::MotionProfile(), py::arg("enforce_motion_envelope") = false, @@ -2594,9 +2598,13 @@ void py_init_module_espp(py::module &m) { espp::Task::BaseConfig{ .name = "TP_cb", .stack_size_bytes = 8192, .priority = 0, .core_id = -1}, py::arg("log_level") = espp::Logger::Verbosity::WARN) - .def_readwrite("max_linear_velocity", - &espp::TrajectoryPlanner::Config::max_linear_velocity, - "*< Maximum linear velocity magnitude (m/s).") + .def_readwrite( + "max_linear_velocity", &espp::TrajectoryPlanner::Config::max_linear_velocity, + "*< Maximum forward linear velocity (m/s): the speed at full forward stick.") + .def_readwrite("max_reverse_linear_velocity", + &espp::TrajectoryPlanner::Config::max_reverse_linear_velocity, + "*< Maximum reverse linear velocity (m/s): the speed at full reverse " + "stick. 0 = the same as max_linear_velocity (symmetric).") .def_readwrite("max_angular_velocity", &espp::TrajectoryPlanner::Config::max_angular_velocity, "*< Maximum angular velocity magnitude (rad/s).") @@ -2650,8 +2658,10 @@ void py_init_module_espp(py::module &m) { py::arg("angular"), "*\n * @brief Set the desired chassis velocity target using normalized joystick " "inputs.\n *\n * Both inputs are in the range [-1, +1] and are scaled internally:\n " - " * @code\n * v_target = linear * max_linear_velocity\n * w_target = " - "angular * max_angular_velocity\n * @endcode\n * Values outside [-1, +1] are " + " * @code\n * v_target = linear * max_linear_velocity (linear >= 0)\n" + " * v_target = linear * max_reverse_linear_velocity (linear < 0; = " + "max_linear_velocity when 0)\n * w_target = angular * max_angular_velocity\n" + " * @endcode\n * Values outside [-1, +1] are " "clamped before scaling.\n *\n * @param linear Normalized linear velocity command " "[-1, +1].\n * +1 = full forward, -1 = full reverse.\n * @param " "angular Normalized angular velocity command [-1, +1].\n * +1 = full " From 88aef2994410494eff203ca62ff3257165d31491 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Tue, 20 Oct 2026 16:05:18 -0500 Subject: [PATCH 3/3] fix(trajectory_planner): keep an unclamped S-curve step's acceleration bit-exact; reject a stopping profile with no acceleration Self-review of the per-axis step: recomputing the S-curve acceleration from the velocity delta even when the velocity clamp did not bite fed float rounding noise into update_accel()'s step-count truncation, which can flip a boundary case. The planned value is now kept bit for bit unless the clamp engaged (the velocity sits on a bound: the acceleration applied is what the bound allowed, 0 once pinned), with a host test for both. set_config() validated only the driving profile's accelerations; a stopping profile with 0 acceleration could never bring the chassis to rest, so it is rejected the same way. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01DFjjnq3XCTRAXENSxsCxJU --- .../detail/trajectory_planner_math.hpp | 18 ++++++++++----- .../src/trajectory_planner.cpp | 9 ++++++++ .../test/trajectory_planner_host_test.cpp | 23 +++++++++++++++++++ 3 files changed, 44 insertions(+), 6 deletions(-) diff --git a/components/trajectory_planner/include/detail/trajectory_planner_math.hpp b/components/trajectory_planner/include/detail/trajectory_planner_math.hpp index 9648a03bb5..aa024d0794 100644 --- a/components/trajectory_planner/include/detail/trajectory_planner_math.hpp +++ b/components/trajectory_planner/include/detail/trajectory_planner_math.hpp @@ -108,10 +108,13 @@ struct AxisState { * 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 -- (v_new - v_old) / dt, after the velocity clamp -- 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, - * and if the stop is interrupted it is the deceleration in force. + * 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. @@ -126,11 +129,14 @@ inline void step_axis(AxisState &state, float target, float v_min, float v_max, const float v_prev = state.v; if (jerk > 0.0f) { update_accel(state.a, target - state.v, jerk * dt, a_max, dt); - state.v = std::clamp(state.v + state.a * dt, v_min, v_max); + 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; } - state.a = (state.v - v_prev) / dt; } } // namespace trajectory_planner diff --git a/components/trajectory_planner/src/trajectory_planner.cpp b/components/trajectory_planner/src/trajectory_planner.cpp index 77f0143f35..4c722a1a51 100644 --- a/components/trajectory_planner/src/trajectory_planner.cpp +++ b/components/trajectory_planner/src/trajectory_planner.cpp @@ -43,6 +43,15 @@ bool TrajectoryPlanner::set_config(const Config &config, bool reset_state) { logger_.error("driving_profile.max_angular_acceleration must be > 0"); return false; } + // a stopping profile with no acceleration could never bring the chassis to rest + if (config.stopping_profile.max_linear_acceleration <= 0.0f) { + logger_.error("stopping_profile.max_linear_acceleration must be > 0"); + return false; + } + if (config.stopping_profile.max_angular_acceleration <= 0.0f) { + logger_.error("stopping_profile.max_angular_acceleration must be > 0"); + return false; + } if (config.driving_profile.max_linear_jerk * config.planning_period.count() > config.driving_profile.max_linear_acceleration) { diff --git a/components/trajectory_planner/test/trajectory_planner_host_test.cpp b/components/trajectory_planner/test/trajectory_planner_host_test.cpp index dea33e7b46..1de8b6404a 100644 --- a/components/trajectory_planner/test/trajectory_planner_host_test.cpp +++ b/components/trajectory_planner/test/trajectory_planner_host_test.cpp @@ -145,6 +145,29 @@ int main() { CHECK(near(s.v, 1.8f)); } + // ---- an unclamped S-curve step keeps the planned acceleration bit for bit ---- + { + tp::AxisState s{.v = 0.3f, .a = 0.7f}; + float planned = s.a; + tp::update_accel(planned, 1.8f - s.v, 5.0f * 0.02f, 1.0f, 0.02f); + tp::step_axis(s, 1.8f, -1.8f, 1.8f, 1.0f, 5.0f, 0.02f); + CHECK(s.a == planned); + } + + // ---- an S-curve pinned at the velocity bound records no acceleration, and leaves it from 0 ---- + { + Axis ax{.v_max = 0.5f, + .a_drive = 1.0f, + .j_drive = 5.0f, + .a_stop = 1.6f, + .j_stop = 5.0f, + .dt = 0.02f}; + ax.run(0.5f, 100); + CHECK(near(ax.s.v, 0.5f) && near(ax.s.a, 0.0f)); + ax.step(-0.5f); + CHECK(near(ax.s.a, -0.1f)); // one jerk step from 0, not from a phantom +1.0 + } + // ---- the S-curve alone: ramps at the jerk limit, caps at a_max, lands without overshoot ---- { Axis ax{.v_max = 10.0f,