Trajectory Planner APIs

Trajectory Planner

The TrajectoryPlanner component converts normalized joystick velocity commands (v_cmd, w_cmd) into smooth, dynamically feasible chassis motion commands (v_ref, w_ref). It is drive-system independent – the downstream kinematics layer is responsible for converting chassis commands into individual motor commands.

The jerk-limited mode uses a discrete optimal-control approach: at each step the planner computes the minimum velocity-change distance needed to decelerate the current acceleration to zero, then decides whether to accelerate, maintain, or decelerate – landing exactly on the target without overshoot, equivalent to a time-optimal S-curve under joint jerk and acceleration constraints.

Code examples are provided in the trajectory_planner example folder.

API Reference

Header File

Classes

class TrajectoryPlanner : public espp::BaseComponent

Converts normalized joystick velocity commands into smooth, dynamically feasible chassis motion commands (v, w).

The planner is drive-system independent — it does not know about wheel geometry or kinematics. It only enforces velocity, acceleration, and jerk limits on chassis-level commands. The downstream kinematics layer converts (v_ref, w_ref) into individual motor commands.

### Algorithm The jerk-limited mode uses a discrete optimal-control approach: at each step the planner computes the minimum velocity-change distance needed to decelerate the current acceleration to zero, then decides whether to accelerate, maintain, or decelerate to land exactly on the target without overshoot — equivalent to a time-optimal S-curve under jerk and acceleration constraints.

### Profiles Motion limits are grouped into two MotionProfile objects inside Config:

  • **driving_profile** — used whenever the target is non-zero.

  • **stopping_profile** — used when the target is (0, 0). Setting jerk to 0 gives a trapezoidal stop; higher acceleration gives faster, firmer braking.

A MotionProfile selects its mode automatically:

  • **Trapezoidal**: `max_linear_jerk == 0 && max_angular_jerk == 0`

  • **S-curve**: either jerk field is non-zero

### Timing Two independent timers run internally:

  • **planning timer** — calls `update()` at `planning_period` (default 20 ms / 50 Hz). Recommended range: 5-200 ms on microcontrollers.

  • **callback task** — fires `output_callback` on every `update()` that produces a new output value (CV-notified by the planning timer); no separate period needed.

This class is thread-safe: set_target(), get_target(), output(), stop(), and reset() may be called from different threads concurrently.

Quick-Start: Full Public API

    // 1. Construct with a Config - task starts automatically.
    espp::TrajectoryPlanner planner({
        .max_linear_velocity = 1.0f,      // m/s
        .max_angular_velocity = 3.14159f, // rad/s
        .driving_profile = {.max_linear_acceleration = 2.0f,
                            .max_angular_acceleration = 6.28f,
                            .max_linear_jerk = 2.0f,
                            .max_angular_jerk = 20.0f},
        // Trapezoidal stop (no jerk) - fast, clean, no overshoot
        .stopping_profile = {.max_linear_acceleration = 5.0f, .max_angular_acceleration = 10.0f},
        .enforce_motion_envelope = true,      // keep (v/vmax)²+(ω/ωmax)²≤1
        .max_centripetal_acceleration = 0.4f, // m/s²
        .output_callback =
            [&logger](const espp::TrajectoryPlanner::MotionCommand &cmd) {
              logger.debug("callback: {}", cmd);
            },
        .planning_period = planning_period,
        .planning_task_config = {.name = "tp_qs", .stack_size_bytes = 4096},
    });

    // 2. is_running() - confirm the task started.
    logger.info("Task running: {}", planner.is_running());

    // 3. get_config() - inspect active configuration.
    auto cfg = planner.get_config();
    logger.info("Config: {}", cfg);

    // 4. set_target(linear, angular) - normalized [-1, +1] joystick inputs.
    //    +1.0 linear = max_linear_velocity forward.
    planner.set_target(1.0f, 0.0f);
    std::this_thread::sleep_for(600ms);

    // 5. output() - poll the latest smoothed command at any time.
    auto cmd = planner.output();
    logger.info("Polled output: {}", cmd);

    // 6. set_target with combined motion - forward + right turn.
    planner.set_target(0.6f, -0.5f);
    std::this_thread::sleep_for(600ms);

    // 7. set_config() - change parameters at runtime; resets state by default.
    espp::TrajectoryPlanner::Config new_cfg = planner.get_config();
    new_cfg.max_linear_velocity = 0.5f; // half speed cap
    planner.set_config(new_cfg, /*reset_state=*/false);
    logger.info("Updated max_linear_velocity to 0.5 m/s");
    planner.set_target(1.0f, 0.0f); // still clamped to new 0.5 m/s
    std::this_thread::sleep_for(600ms);

    // 8. stop() - ramp down to zero respecting deceleration limits.
    logger.info("Commanding stop (ramp-down)");
    planner.stop();
    std::this_thread::sleep_for(400ms);

    // 9. reset() - zero state immediately (e.g. after e-stop).
    logger.info("Emergency reset");
    planner.reset();
    logger.info("Output after reset: {}", planner.output());

    // 10. Destructor stops the task automatically when planner leaves scope.

S-Curve Driving / Trapezoidal Stop

    std::atomic<int> tick{0};

    espp::TrajectoryPlanner planner({
        .max_linear_velocity = 1.0f,      // m/s
        .max_angular_velocity = 3.14159f, // rad/s
        // S-curve driving: smooth ramp with jerk limiting
        .driving_profile = {.max_linear_acceleration = 2.0f,
                            .max_angular_acceleration = 6.28f,
                            .max_linear_jerk = 2.0f,
                            .max_angular_jerk = 25.0f},
        // Trapezoidal stop: no jerk = immediate deceleration, no overshoot
        .stopping_profile = {.max_linear_acceleration = 6.0f, .max_angular_acceleration = 12.0f},
        .enforce_motion_envelope = true,
        .max_centripetal_acceleration = 0.5f, // m/s²
        .output_callback =
            [&logger, &tick](const espp::TrajectoryPlanner::MotionCommand &cmd) {
              logger.info("[{:3d}] {}", tick.load(), cmd);
              tick++;
            },
        .planning_period = planning_period,
        .planning_task_config =
            {.name = "tp_ex1", .stack_size_bytes = 10240, .priority = 5, .core_id = -1},
    });

    // Full forward (normalized: 1.0 = max_linear_velocity)
    logger.info("Commanding full forward");
    planner.set_target(1.0f, 0.0f);
    std::this_thread::sleep_for(3s);

    // Gentle right curve at half speed
    logger.info("Commanding half-speed right curve");
    planner.set_target(0.5f, -0.4f);
    std::this_thread::sleep_for(3s);

    // Controlled stop - uses braking deceleration limits
    logger.info("Stopping");
    planner.stop();
    std::this_thread::sleep_for(500ms);
    // task stops automatically when planner goes out of scope

High-Speed S-Curve with Centripetal Limiting

    std::atomic<int> tick{0};

    espp::TrajectoryPlanner planner({
        .max_linear_velocity = 4.47f,            // 10 mph
        .max_angular_velocity = 3.14159f / 2.0f, // 90 deg/s
        // S-curve driving: smooth acceleration with jerk limits
        .driving_profile = {.max_linear_acceleration = 2.0f,
                            .max_angular_acceleration = 6.28f,
                            .max_linear_jerk = 10.0f,
                            .max_angular_jerk = 30.0f},
        // Trapezoidal stop: no jerk limit = clean stop, no overshoot
        .stopping_profile = {.max_linear_acceleration = 4.0f, .max_angular_acceleration = 8.0f},
        .enforce_motion_envelope = true,
        .max_centripetal_acceleration = 0.3f,
        .output_callback =
            [&logger, &tick](const espp::TrajectoryPlanner::MotionCommand &cmd) {
              logger.info("[{:3d}] {}", tick.load(), cmd);
              tick++;
            },
        .planning_period = planning_period,
        .planning_task_config =
            {.name = "tp_ex2", .stack_size_bytes = 10240, .priority = 5, .core_id = -1},
    });

    // Forward + left turn (0.8 = 80% max linear, 0.5 = 50% max angular)
    logger.info("Commanding forward + left turn");
    planner.set_target(0.8f, 0.5f);
    std::this_thread::sleep_for(1s);

    // Reverse direction
    logger.info("Reversing direction");
    planner.set_target(-0.5f, -0.5f);
    std::this_thread::sleep_for(1s);

    // Controlled stop
    logger.info("Stopping");
    planner.stop();
    std::this_thread::sleep_for(500ms);
    // task stops automatically when planner goes out of scope

Constraint Validation

    using Cfg = espp::TrajectoryPlanner::Config;
    using Cmd = espp::TrajectoryPlanner::MotionCommand;

    // ---- Validator struct ------------------------------------------------
    struct Validator {
      // Immutable limits
      float max_v, max_w;
      float a_drv_v, a_drv_w, j_drv_v, j_drv_w; // driving profile
      float a_stp_v, a_stp_w, j_stp_v, j_stp_w; // stopping profile
      float max_cp, dt;

      // Callback-thread history (only written from callback)
      float pv{0}, pw{0}, pa_v{0}, pa_w{0};
      bool inited{false};
      int skip{2}; // skip first N ticks for derivative settling
      std::chrono::steady_clock::time_point prev_time{};
      bool time_inited{false};

      // Shared results - protected by mtx
      mutable std::mutex mtx;
      espp::Logger *log{nullptr}; // set after construction
      bool stopping{false};
      int viol_drv{0}; // violations during driving phase
      int viol_stp{0}; // violations during stopping phase

      struct PhasePeaks {
        float v{0}, w{0}, a_v{0}, a_w{0}, j_v{0}, j_w{0}, cp{0};
        void reset() { v = w = a_v = a_w = j_v = j_w = cp = 0; }
      };
      PhasePeaks pk_drv, pk_stp;

      void mark_stopping() {
        std::lock_guard<std::mutex> lk(mtx);
        stopping = true;
        skip = 3;
      }

      // Call before resuming a non-zero target after a stop.
      void mark_driving() {
        std::lock_guard<std::mutex> lk(mtx);
        stopping = false;
        skip = 3;
      }

      // Call before any abrupt target change (direction reversal, etc.).
      void mark_transition() {
        std::lock_guard<std::mutex> lk(mtx);
        skip = 3;
      }

      void reset() {
        // Always called before a new planner starts - no concurrent access
        std::lock_guard<std::mutex> lk(mtx);
        stopping = false;
        viol_drv = 0;
        viol_stp = 0;
        pk_drv.reset();
        pk_stp.reset();
        pv = pw = pa_v = pa_w = 0;
        inited = false;
        skip = 2;
        time_inited = false;
      }

      void operator()(const Cmd &cmd) {
        std::lock_guard<std::mutex> lk(mtx);

        const float tol = 1.08f; // 8% margin for scheduler jitter
        const float ma_v = stopping ? a_stp_v : a_drv_v;
        const float ma_w = stopping ? a_stp_w : a_drv_w;
        const float mj_v = stopping ? j_stp_v : j_drv_v;
        const float mj_w = stopping ? j_stp_w : j_drv_w;

        // Speed limits
        PhasePeaks &pk = stopping ? pk_stp : pk_drv;
        pk.v = std::max(pk.v, std::abs(cmd.linear_velocity));
        pk.w = std::max(pk.w, std::abs(cmd.angular_velocity));
        if (std::abs(cmd.linear_velocity) > max_v * tol) {
          if (log)
            log->warn("VIOL speed_v: {:.3f} > {:.3f} ({})", cmd.linear_velocity, max_v * tol,
                      stopping ? "stop" : "drv");
          stopping ? viol_stp++ : viol_drv++;
        }
        if (std::abs(cmd.angular_velocity) > max_w * tol) {
          if (log)
            log->warn("VIOL speed_w: {:.3f} > {:.3f} ({})", cmd.angular_velocity, max_w * tol,
                      stopping ? "stop" : "drv");
          stopping ? viol_stp++ : viol_drv++;
        }

        // Centripetal limit
        float cp = std::abs(cmd.linear_velocity * cmd.angular_velocity);
        pk.cp = std::max(pk.cp, cp);

        if (inited) {
          // Use actual elapsed time to avoid systematic error from FreeRTOS
          // scheduling jitter (actual tick ~30 ms vs. nominal 20 ms).
          auto now = std::chrono::steady_clock::now();
          float adt = time_inited ? std::chrono::duration<float>(now - prev_time).count()
                                  : dt; // nominal for first step

          prev_time = now;
          time_inited = true;

          float a_v = (cmd.linear_velocity - pv) / adt;
          float a_w = (cmd.angular_velocity - pw) / adt;
          pk.a_v = std::max(pk.a_v, std::abs(a_v));
          pk.a_w = std::max(pk.a_w, std::abs(a_w));
          float j_v = (a_v - pa_v) / adt;
          float j_w = (a_w - pa_w) / adt;

          if (skip > 0) {
            --skip;
          } else {
            pk.j_v = std::max(pk.j_v, std::abs(j_v));
            pk.j_w = std::max(pk.j_w, std::abs(j_w));
            if (std::abs(a_v) > ma_v * tol) {
              if (log)
                log->warn("VIOL accel_v: {:.3f} > {:.3f} ({})", a_v, ma_v * tol,
                          stopping ? "stop" : "drv");
              stopping ? viol_stp++ : viol_drv++;
            }
            if (std::abs(a_w) > ma_w * tol) {
              if (log)
                log->warn("VIOL accel_w: {:.3f} > {:.3f} ({})", a_w, ma_w * tol,
                          stopping ? "stop" : "drv");
              stopping ? viol_stp++ : viol_drv++;
            }
            if (mj_v > 0.0f && std::abs(j_v) > mj_v * tol) {
              if (log)
                log->warn("VIOL jerk_v:  {:.3f} > {:.3f} ({})", j_v, mj_v * tol,
                          stopping ? "stop" : "drv");
              stopping ? viol_stp++ : viol_drv++;
            }
            if (mj_w > 0.0f && std::abs(j_w) > mj_w * tol) {
              if (log)
                log->warn("VIOL jerk_w:  {:.3f} > {:.3f} ({})", j_w, mj_w * tol,
                          stopping ? "stop" : "drv");
              stopping ? viol_stp++ : viol_drv++;
            }
          }
          pa_v = a_v;
          pa_w = a_w;
        } else {
          // First tick: start the clock
          prev_time = std::chrono::steady_clock::now();
          time_inited = true;
        }
        pv = cmd.linear_velocity;
        pw = cmd.angular_velocity;
        inited = true;
      }

      bool report(espp::Logger &log, std::string_view name) {
        std::lock_guard<std::mutex> lk(mtx);
        log.info("--- {} ---  drv_viol={} stp_viol={}", name, viol_drv, viol_stp);
        log.info("  [driving]");
        log.info("    v={:.3f}/{:.1f}   w={:.3f}/{:.1f}", pk_drv.v, max_v, pk_drv.w, max_w);
        log.info("    a_v={:.3f}/{:.1f}  a_w={:.3f}/{:.1f}", pk_drv.a_v, a_drv_v, pk_drv.a_w,
                 a_drv_w);
        log.info("    j_v={:.3f}/{:.1f}  j_w={:.3f}/{:.1f}", pk_drv.j_v, j_drv_v, pk_drv.j_w,
                 j_drv_w);
        log.info("    cp={:.3f}/{:.2f}", pk_drv.cp, max_cp);
        log.info("  [stopping]");
        log.info("    v={:.3f}/{:.1f}   w={:.3f}/{:.1f}", pk_stp.v, max_v, pk_stp.w, max_w);
        log.info("    a_v={:.3f}/{:.1f}  a_w={:.3f}/{:.1f}", pk_stp.a_v, a_stp_v, pk_stp.a_w,
                 a_stp_w);
        log.info("    j_v={:.3f}/{:.1f}  j_w={:.3f}/{:.1f}", pk_stp.j_v, j_stp_v, pk_stp.j_w,
                 j_stp_w);
        log.info("    cp={:.3f}/{:.2f}", pk_stp.cp, max_cp);
        log.info((viol_drv == 0 && viol_stp == 0) ? "  [PASS]" : "  [FAIL]");
        return viol_drv == 0 && viol_stp == 0;
      }
    };

    // ---- Shared limits ---------------------------------------------------
    constexpr float MAX_V = 1.0f, MAX_W = 3.14159f;
    constexpr float A_DRV_V = 2.0f, A_DRV_W = 6.28f;
    constexpr float J_DRV_V = 10.0f, J_DRV_W = 25.0f;
    constexpr float A_STP_V = 5.0f, A_STP_W = 10.0f;
    constexpr float MAX_CP = 0.4f, DT = 0.020f;

    Validator val;
    val.max_v = MAX_V;
    val.max_w = MAX_W;
    val.a_drv_v = A_DRV_V;
    val.a_drv_w = A_DRV_W;
    val.j_drv_v = J_DRV_V;
    val.j_drv_w = J_DRV_W;
    val.a_stp_v = A_STP_V;
    val.a_stp_w = A_STP_W;
    val.j_stp_v = 0.0f;
    val.j_stp_w = 0.0f; // tests use trapez stop; set non-zero if needed
    val.max_cp = MAX_CP;
    val.dt = DT;
    val.log = &logger;

    const Cfg cfg{
        .max_linear_velocity = MAX_V,
        .max_angular_velocity = MAX_W,
        .driving_profile = {.max_linear_acceleration = A_DRV_V,
                            .max_angular_acceleration = A_DRV_W,
                            .max_linear_jerk = J_DRV_V,
                            .max_angular_jerk = J_DRV_W},
        .stopping_profile = {.max_linear_acceleration = A_STP_V,
                             .max_angular_acceleration = A_STP_W},
        .enforce_motion_envelope = true,
        .max_centripetal_acceleration = MAX_CP,
        .output_callback = [&val](const Cmd &cmd) { val(cmd); },
        .planning_period = std::chrono::milliseconds(50),
        .planning_task_config = {.name = "tp_val", .stack_size_bytes = 1024 * 10, .priority = 5},
        .callback_task_config = {.name = "tp_val_cb", .stack_size_bytes = 8192, .priority = 5},

    };

    // ---- Test A: pure linear ramp ----------------------------------------
    {
      logger.info("[A] Pure linear ramp  target=(+1, 0)");
      val.reset();
      espp::TrajectoryPlanner planner(cfg);
      planner.set_target(1.0f, 0.0f);
      std::this_thread::sleep_for(800ms);
      val.report(logger, "A: Pure linear ramp");
    }

    // ---- Test B: pure angular ramp ----------------------------------------
    {
      logger.info("[B] Pure angular ramp  target=(0, +1)");
      val.reset();
      espp::TrajectoryPlanner planner(cfg);
      planner.set_target(0.0f, 1.0f);
      std::this_thread::sleep_for(800ms);
      val.report(logger, "B: Pure angular ramp");
    }

    // ---- Test C: combined motion - tests envelope + centripetal ----------
    {
      logger.info("[C] Combined  target=(0.9, 0.9)  envelope + centripetal");
      val.reset();
      espp::TrajectoryPlanner planner(cfg);
      planner.set_target(0.9f, 0.9f);
      std::this_thread::sleep_for(1s);
      val.report(logger, "C: Combined (envelope + centripetal)");
    }

    // ---- Test D: drive → controlled stop (stopping profile) ---------------
    {
      logger.info("[D] Drive then stop  - tests stopping profile switch");
      val.reset();
      espp::TrajectoryPlanner planner(cfg);
      planner.set_target(1.0f, 0.0f);
      std::this_thread::sleep_for(600ms);
      val.mark_stopping();
      planner.stop();
      std::this_thread::sleep_for(400ms);
      val.report(logger, "D: Drive then stop");
    }

    // ---- Test E: direction reversal ---------------------------------------
    {
      logger.info("[E] Direction reversal  (+1, 0) → (-1, 0)");
      val.reset();
      espp::TrajectoryPlanner planner(cfg);
      planner.set_target(1.0f, 0.0f);
      std::this_thread::sleep_for(1000ms);
      planner.set_target(-1.0f, 0.0f);
      std::this_thread::sleep_for(2000ms);
      val.report(logger, "E: Direction reversal");
    }

    // ---- Test F: multi-step sweep - various (linear, angular) pairs -------
    {
      logger.info("[F] Multi-step sweep  - varied inputs");
      val.reset();
      espp::TrajectoryPlanner planner(cfg);
      bool was_stopping = false;
      for (float lin : {0.3f, 0.6f, 1.0f, -0.5f, 0.0f}) {
        for (float ang : {0.0f, 0.5f, -0.5f}) {
          bool next_stopping = (lin == 0.0f && ang == 0.0f);
          if (next_stopping && !was_stopping)
            val.mark_stopping();
          else if (!next_stopping && was_stopping)
            val.mark_driving();
          was_stopping = next_stopping;
          planner.set_target(lin, ang);
          std::this_thread::sleep_for(250ms);
        }
      }
      val.mark_stopping();
      planner.stop();
      std::this_thread::sleep_for(300ms);
      val.report(logger, "F: Multi-step sweep");
    }

Public Types

typedef std::function<void(const MotionCommand&)> output_callback_t

Callback invoked at the end of every update() call with the latest smoothed MotionCommand. Use to feed the output directly into a kinematics layer without polling output().

Note

The callback is called without holding the internal mutex, so it is safe to call set_target(), stop(), or output() from within it.

Public Functions

explicit TrajectoryPlanner(const Config &config)

Construct the planner with the given configuration. The periodic update task is started automatically.

Parameters:

config – Configuration parameters.

~TrajectoryPlanner()

Destructor. Stops the periodic update task before destruction.

bool set_config(const Config &config, bool reset_state = true)

Update the planner configuration.

Parameters:
  • config – New configuration parameters.

  • reset_state – If true (default), resets velocity/acceleration state to zero.

Returns:

true if the configuration was successfully applied.

const Config &get_config() const

Get the current configuration.

Returns:

Const reference to the active Config.

void set_target(float linear, float angular)

Set the desired chassis velocity target using normalized joystick inputs.

Both inputs are in the range [-1, +1] and are scaled internally:

v_target  = linear  * max_linear_velocity
w_target  = angular * max_angular_velocity
Values outside [-1, +1] are clamped before scaling.

Parameters:
  • linear – Normalized linear velocity command [-1, +1]. +1 = full forward, -1 = full reverse.

  • angular – Normalized angular velocity command [-1, +1]. +1 = full left turn, -1 = full right turn.

std::pair<float, float> get_target() const

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).

Returns:

Pair of {linear, angular} in [-1, +1].

espp::TrajectoryPlanner::MotionCommand output() const

Get the current smoothed motion command.

Returns:

MotionCommand containing the trajectory-limited (v_ref, w_ref).

void stop()

Command the planner to decelerate to a full stop.

Equivalent to set_target(0, 0). The planner ramps down respecting all configured limits rather than cutting output immediately.

void reset()

Reset velocity, acceleration state, and target to zero immediately.

The next call to output() will return (0, 0). Use after an emergency stop or before re-initialising with a new configuration.

bool is_running() const

Check whether the periodic update task is currently running.

Returns:

true if the task is running.

inline const std::string &get_name() const

Get the name of the component

Note

This is the tag of the logger

Returns:

A const reference to the name of the component

inline void set_log_tag(const std::string_view &tag)

Set the tag for the logger

Parameters:

tag – The tag to use for the logger

inline espp::Logger::Verbosity get_log_level() const

Get the log level for the logger

Returns:

The verbosity level of the logger

inline void set_log_level(espp::Logger::Verbosity level)

Set the log level for the logger

Parameters:

level – The verbosity level to use for the logger

inline void set_log_verbosity(espp::Logger::Verbosity level)

Set the log verbosity for the logger

See also

set_log_level

Note

This is a convenience method that calls set_log_level

Parameters:

level – The verbosity level to use for the logger

inline espp::Logger::Verbosity get_log_verbosity() const

Get the log verbosity for the logger

See also

get_log_level

Note

This is a convenience method that calls get_log_level

Returns:

The verbosity level of the logger

inline void set_log_rate_limit(std::chrono::duration<float> rate_limit)

Set the rate limit for the logger

Note

Only calls to the logger that have _rate_limit suffix will be rate limited

Parameters:

rate_limit – The rate limit to use for the logger

struct Config

Configuration for the TrajectoryPlanner.

Public Members

float max_linear_velocity = 1.0f

Maximum linear velocity magnitude (m/s).

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 == (0, 0). Set jerk to 0 here for a clean trapezoidal stop with no overshoot. Higher acceleration than the driving profile gives faster, firmer braking.

bool enforce_motion_envelope = false

When true, enforces (v/vmax)^2+(w/wmax)^2<=1 on output to prevent infeasible combined commands.

float max_centripetal_acceleration = 0.1f

Maximum centripetal acceleration |v*w| (m/s^2). 0 disables the limit. Both v and w are scaled proportionally when the limit is exceeded.

espp::TrajectoryPlanner::output_callback_t output_callback = nullptr

Optional callback invoked after each update() with the latest MotionCommand output. Leave as nullptr to disable.

std::chrono::duration<float> planning_period = std::chrono::milliseconds(20)

Planner update rate (default 50 Hz). Recommended: 5-200 ms on microcontrollers.

espp::Task::BaseConfig planning_task_config  = {.name = "TP_planning",.stack_size_bytes = 4096,.priority = 0,.core_id = -1}

Underlying task config for the timer.

espp::Task::BaseConfig callback_task_config  = {.name = "TP_cb",.stack_size_bytes = 8192,.priority = 0,.core_id = -1}

Underlying task config for the callback timer.

espp::Logger::Verbosity log_level = espp::Logger::Verbosity::WARN

Logger verbosity.

struct MotionCommand

Chassis motion command produced by the planner.

Public Members

float linear_velocity = 0.0f

Linear velocity reference (m/s).

float angular_velocity = 0.0f

Angular velocity reference (rad/s).

struct MotionProfile

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.

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.

Public Members

float max_linear_acceleration = 0.0f

Linear acceleration limit (m/s^2).

float max_angular_acceleration = 0.0f

Angular acceleration limit (rad/s^2).

float max_linear_jerk = 0.0f

Linear jerk limit (m/s^3). 0 = trapezoidal.

float max_angular_jerk = 0.0f

Angular jerk limit (rad/s^3). 0 = trapezoidal.