diff --git a/components/trajectory_planner/README.md b/components/trajectory_planner/README.md index 7128f65116..5b667cce2d 100644 --- a/components/trajectory_planner/README.md +++ b/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 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..aa024d0794 --- /dev/null +++ b/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 +#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, 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 diff --git a/components/trajectory_planner/include/trajectory_planner.hpp b/components/trajectory_planner/include/trajectory_planner.hpp index 8f7a183f92..e2d7265fba 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). */ @@ -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 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; 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 824f5d8b1f..4c722a1a51 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 { @@ -93,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; @@ -106,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) { @@ -143,6 +89,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_; @@ -164,12 +115,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; } @@ -183,7 +136,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 @@ -210,7 +164,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) { @@ -225,29 +181,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_, -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; + 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..1de8b6404a --- /dev/null +++ b/components/trajectory_planner/test/trajectory_planner_host_test.cpp @@ -0,0 +1,195 @@ +// 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, 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, 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)); + } + + // ---- 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, + .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; +} 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 "