Feat/trajectory planner

[guo-max]

Description

Trajectory planner that take normalized joystick input and output a linear and angular speed with pre defined acceleration limit and jerk limit.

Motivation and Context

Need an acceleration and jerk limited trajectory planner between joystick and motor control, so that user won't feel sudden speed / acceleration / jerk change to achieve a comfort ride.

How has this been tested?

1. test on MCU, make sure no acc/jerk exceed the limit.
2. Tested with python, make sure it works at different platform.
3. Tested on PC, make sure it works at different platform

Screenshots (if appropriate, e.g. schematic, board, console logs, lab pictures):

1. MCU

2. Python

3. PC

Types of changes

• Bug fix (non-breaking change which fixes an issue)
• New feature (non-breaking change which adds functionality)
• Breaking change (fix or feature that would cause existing functionality to not work as expected)
• Documentation Update
• Hardware (schematic, board, system design) change
• Software change

Checklist:

• My change requires a change to the documentation.
• I have added / updated the documentation related to this change via either README or WIKI

Software

• I have added tests to cover my changes.
• I have updated the .github/workflows/build.yml file to add my new test to the automated cloud build github action.
• All new and existing tests passed.
• My code follows the code style of this project.

Hardware

• I have updated the design files (schematic, board, libraries).
• I have attached the PDFs of the SCH / BRD to this PR
• I have updated the design output (GERBER, BOM) files.

[github-actions]

✅Static analysis result - no issues found! ✅

[Copilot]

Pull request overview

Adds a new TrajectoryPlanner component to ESPP that converts normalized joystick inputs into smooth chassis motion commands with configurable velocity/acceleration/jerk limits, plus PC tests and Python bindings/demo to validate behavior across platforms.

Changes:

• Introduces components/trajectory_planner (C++ implementation, formatters, and an ESP-IDF example).
• Exposes espp::TrajectoryPlanner to Python via pybind and updates generated stubs/autogen inputs.
• Adds a PC-side test executable and a Python plotting demo (with matplotlib added to requirements).

Reviewed changes

Copilot reviewed 22 out of 23 changed files in this pull request and generated 12 comments.

Show a summary per file

File	Description
python/trajectory_planner.py	New Python demo script to drive the planner and plot velocity/accel/jerk results.
python/requirements.txt	Adds matplotlib dependency for the new Python demo plotting.
pc/tests/trajectory_planner.cpp	New PC test program exercising planner behaviors (trapezoidal, S-curve, reversal, reset).
pc/CMakeLists.txt	Links winmm on MSVC so the new test can use Windows timing APIs.
lib/python_bindings/pybind_espp.cpp	Adds pybind exposure for TrajectoryPlanner and augments Timer docs/return typing; removes some Socket IPv6 bindings.
lib/python_bindings/espp/init.pyi	Updates generated type stubs for Timer and adds TrajectoryPlanner API surface.
lib/include/espp.hpp	Exposes the new component header through the umbrella include.
lib/espp.cmake	Adds trajectory_planner include and source paths to the PC build aggregation.
lib/autogenerate_bindings.py	Includes trajectory_planner in the bindings autogen pipeline.
components/trajectory_planner/CMakeLists.txt	New ESP-IDF component registration for trajectory_planner.
components/trajectory_planner/README.md	Component documentation describing features, algorithm, and usage.
components/trajectory_planner/idf_component.yml	ESP Component Manager manifest for the new component.
components/trajectory_planner/include/trajectory_planner.hpp	Public C++ API (Config/MotionProfile/MotionCommand) and task/timer integration members.
components/trajectory_planner/include/trajectory_planner_formatters.hpp	fmt formatters for logging Config/MotionProfile/MotionCommand.
components/trajectory_planner/src/trajectory_planner.cpp	Core planner implementation (envelope/centripetal limiting + trapezoidal/S-curve logic) and internal tasks.
components/trajectory_planner/example/CMakeLists.txt	New ESP-IDF example project wiring in the component.
components/trajectory_planner/example/README.md	Example documentation describing scenarios and validation.
components/trajectory_planner/example/sdkconfig.defaults	Example defaults (notably CONFIG_FREERTOS_HZ=1000).
components/trajectory_planner/example/main/CMakeLists.txt	Registers the example’s main component and depends on trajectory_planner.
components/trajectory_planner/example/main/trajectory_planner_example.cpp	ESP-IDF example demonstrating public API and validation scenarios.
components/trajectory_planner/trajactory_planner.py	New standalone Python analysis script checked into component dir (currently misspelled filename).
.gitignore	Ignores .github/agents/ local-only agent customizations.

[Copilot]

Pull request overview

Copilot reviewed 21 out of 22 changed files in this pull request and generated no new comments.

Suppressed comments (10)

python/trajectory_planner.py:141

• The centripetal series is labeled as |v*w| but the plotted data uses v * w (can be negative), so the curve/limits are inconsistent. Plot the absolute value to match the label and the intended centripetal-acceleration magnitude.

    (axes[3], [([v * w for v, w in zip(v_hist, w_hist)], "|v*w| centripetal", "C6")],
     "Centripetal (m/s^2)", [cfg.max_centripetal_acceleration, -cfg.max_centripetal_acceleration]),

python/trajectory_planner.py:172

• There is unreachable code after sys.exit(0), and it references cmd which is not defined in this scope. This will never run as written, and it will raise a NameError if the sys.exit(0) line is removed later.

sys.exit(0)
print(f"After reset: v={cmd.linear_velocity:.3f}  w={cmd.angular_velocity:.3f}")

target = planner.get_target()
print(f"Target after reset: linear={target[0]:.3f}  angular={target[1]:.3f}")

# planner destructor stops both timers automatically
sys.exit(0)

components/trajectory_planner/src/trajectory_planner.cpp:64

• set_target() reads config_ (max velocities, envelope/centripetal settings) without holding mutex_, but set_config() mutates config_ under that mutex and the class documentation states these methods are thread-safe. This is a data race if set_config() can run concurrently with set_target().

void TrajectoryPlanner::set_target(float linear, float angular) {

  // clamp the input to [-1, +1] first
  linear = std::clamp(linear, -1.0f, 1.0f);
  angular = std::clamp(angular, -1.0f, 1.0f);
  // the target should be clamped to a vector of a unit circle

components/trajectory_planner/src/trajectory_planner.cpp:106

• get_target() divides by config_.max_linear_velocity / config_.max_angular_velocity without guarding against 0. This can produce inf/NaN (or a fault on some platforms) if a default/invalid config is used (e.g. Python bindings default the fields to 0).

std::pair<float, float> TrajectoryPlanner::get_target() const {
  std::lock_guard<std::recursive_mutex> lk(mutex_);
  return {target_v_ / config_.max_linear_velocity, target_w_ / config_.max_angular_velocity};
}

components/trajectory_planner/src/trajectory_planner.cpp:191

• In jerk-limited mode, when one axis has max_*_jerk == 0 but the other axis has jerk enabled, step_* becomes 1e9f * dt. For that axis, stopping_thresh() returns an enormous d_increase when a == 0, so the controller can get stuck at zero acceleration and never move toward the target. Using the acceleration limit as the effective step when jerk==0 avoids the stall while still allowing “infinite jerk” behavior (trapezoidal) on that axis.

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

components/trajectory_planner/src/trajectory_planner.cpp:256

• The callback task ignores the std::mutex, std::condition_variable, and notified flag that espp::Task provides for interruptible waits. Instead it waits on a separate callback_wake_cv_. This works only as long as TrajectoryPlanner::stop_task() is the only way the task is stopped (because Task::stop() only notifies its own CV, not callback_wake_cv_), otherwise Task::stop()/join can deadlock with the callback thread stuck waiting on callback_wake_cv_.

Consider either (1) using the Task-provided cv/notified for the wait, or (2) switching callback_task_ to a callback signature that doesn't imply use of the Task CV and documenting the invariant that stop_task() must wake the callback wait.

  callback_task_ = espp::Task::make_unique({
      .callback = [this](std::mutex &, std::condition_variable &, bool &) -> bool {
        // Wait indefinitely for a notification from update() (no polling timeout needed).
        {
          std::unique_lock<std::mutex> lk(callback_wake_m_);
          callback_wake_cv_.wait(lk, [this] { return callback_wake_flag_ || callback_stop_flag_; });
          if (callback_stop_flag_)

components/trajectory_planner/include/trajectory_planner.hpp:105

• Config::max_linear_velocity and Config::max_angular_velocity are not default-initialized. Default-constructing Config in C++ leaves these fields indeterminate, which can later lead to UB (including division-by-zero/NaNs in get_target()). Prefer giving them safe defaults (e.g. 0.0f) like the other fields in Config.

  struct Config {
    float max_linear_velocity;  /**< Maximum linear velocity magnitude (m/s). */
    float max_angular_velocity; /**< Maximum angular velocity magnitude (rad/s). */
    espp::TrajectoryPlanner::MotionProfile

components/trajectory_planner/trajactory_planner.py:1

• This file name appears to be misspelled (trajactory_planner.py vs trajectory_planner.py). If this script is meant to be kept, consider renaming it so it’s discoverable and consistent with the component name; otherwise, consider removing it if it was only a local development aid.

import numpy as np

lib/python_bindings/pybind_espp.cpp:1657

• The Python bindings removed Socket.Info.ipv6_ptr() and the from_sockaddr(sockaddr_in6) overload, even though the underlying C++ API still provides them behind an IPv6 guard (socket.hpp has #if !defined(ESP_PLATFORM) || LWIP_IPV6). This is a breaking change for Python users and isn’t mentioned in the PR description.

If the removal is intentional, it should be called out in release notes / docs. If it’s accidental, consider restoring these bindings (also updating the .pyi stub accordingly) and guarding them the same way the C++ header does.

            .def("init_ipv4", &espp::Socket::Info::init_ipv4, py::arg("addr"), py::arg("prt"),
                 "*\n     * @brief Initialize the struct as an ipv4 address/port combo.\n     * "
                 "@param addr IPv4 address string\n     * @param prt port number\n")
            .def("ipv4_ptr", &espp::Socket::Info::ipv4_ptr,
                 "*\n     * @brief Gives access to IPv4 sockaddr structure (sockaddr_in) for use\n "
                 "    *        with low level socket calls like sendto / recvfrom.\n     * @return "
                 "*sockaddr_in pointer to ipv4 data structure\n")
            .def("update", &espp::Socket::Info::update,
                 "*\n     * @brief Will update address and port based on the curent data in raw.\n")
            .def("from_sockaddr",
                 py::overload_cast<const struct sockaddr_storage &>(
                     &espp::Socket::Info::from_sockaddr),
                 py::arg("source_address"),
                 "*\n     * @brief Fill this Info from the provided sockaddr struct.\n     * "
                 "@param &source_address sockaddr info filled out by recvfrom.\n")
            .def("from_sockaddr",
                 py::overload_cast<const struct sockaddr_in &>(&espp::Socket::Info::from_sockaddr),
                 py::arg("source_address"),
                 "*\n     * @brief Fill this Info from the provided sockaddr struct.\n     * "
                 "@param &source_address sockaddr info filled out by recvfrom.\n");

lib/python_bindings/espp/init.pyi:2802

• The .pyi stub drops Socket.Info.ipv6_ptr() and the from_sockaddr(sockaddr_in6) overload, matching the removal in pybind_espp.cpp. Since the C++ API still includes these under an IPv6 guard, please confirm whether removing these Python APIs is intentional and (if so) document it; otherwise, consider restoring both the binding and this stub entry with the same conditional support expectations as the C++ layer.

        def ipv4_ptr(self) -> struct sockaddr_in:
            """*
                 * @brief Gives access to IPv4 sockaddr structure (sockaddr_in) for use
                 *        with low level socket calls like sendto / recvfrom.
                 * @return *sockaddr_in pointer to ipv4 data structure

            """
            pass


        def update(self) -> None:
            """*
                 * @brief Will update address and port based on the curent data in raw.

            """
            pass

        @overload
        def from_sockaddr(self, source_address: struct sockaddr_storage) -> None:
            """*
                 * @brief Fill this Info from the provided sockaddr struct.
                 * @param &source_address sockaddr info filled out by recvfrom.

            """
            pass
        @overload
        def from_sockaddr(self, source_address: struct sockaddr_in) -> None:
            """*
                 * @brief Fill this Info from the provided sockaddr struct.
                 * @param &source_address sockaddr info filled out by recvfrom.

            """
            pass

[Copilot]

Pull request overview

Copilot reviewed 25 out of 26 changed files in this pull request and generated no new comments.

Suppressed comments (9)

components/trajectory_planner/src/trajectory_planner.cpp:71

• set_config() updates config_.planning_period, but if the planner task is already running the underlying Timer keeps its old period. That means changing planning_period via set_config() has no effect (despite being part of Config and validated here).

  logger_.info("Updated config: {}", config);
  config_ = config;
  output_callback_ = config.output_callback;
  if (reset_state) {
    reset();

components/trajectory_planner/src/trajectory_planner.cpp:103

• set_target() reads config_ fields (max velocities and centripetal limit) outside the mutex_ lock, while set_config() can update config_ concurrently. This is a data race and can also apply mixed config values within a single target update.

  auto target_v = linear * config_.max_linear_velocity;
  auto target_w = angular * config_.max_angular_velocity;

  // Centripetal acceleration limit: |v · w| ≤ a_c_max
  // Scale both v and w proportionally to preserve the turning radius.

pc/tests/trajectory_planner.cpp:125

• After expanding Stats::check() to validate angular limits too, this call site needs to pass the angular acceleration/velocity limits from the planner config so the test actually enforces them.

  return stats.check(log, 2.0f, 5.0f, 1.0f);

.github/workflows/build.yml:258

• The build matrix list appears to be kept sorted by component path, but this new entry is inserted between timer and touch/tla2528, which breaks the alphabetical ordering and makes future maintenance/error-prone merges more likely.

        - path: 'components/timer/example'
          target: esp32
        - path: 'components/trajectory_planner/example'
          target: esp32
        - path: 'components/touch/example'

.github/workflows/build.yml:257

• The new trajectory_planner component is added to the build example matrix, but it is not present in .github/workflows/upload_components.yml's components: list, so registry uploads/dry-runs will skip it.

        - path: 'components/trajectory_planner/example'
          target: esp32

components/trajectory_planner/src/trajectory_planner.cpp:1

• update() uses std::tuple (and structured bindings) but this translation unit doesn't include <tuple>. Relying on transitive includes can break builds on some toolchains / standard library versions.

#include "trajectory_planner.hpp"

pc/tests/trajectory_planner.cpp:63

• Stats::check() tracks angular velocity/acceleration (max_w, max_a_w_*) but doesn't validate them, so these PC tests can pass even if the planner violates angular constraints.

  bool check(espp::Logger &log, float drv_a_lim, float stp_a_lim, float v_lim, float tol = 1.10f) {
    bool ok = (max_v <= v_lim * 1.05f) && (max_a_v_drv <= drv_a_lim * tol) &&
              (max_a_v_stp <= stp_a_lim * tol);
    log.info("  max|v|={:.3f}/{:.1f}  |a_v_drv|={:.3f}/{:.1f}  |a_v_stp|={:.3f}/{:.1f}  -> {}",
             max_v, v_lim, max_a_v_drv, drv_a_lim, max_a_v_stp, stp_a_lim, ok ? "PASS" : "FAIL");

pc/tests/trajectory_planner.cpp:91

• After expanding Stats::check() to validate angular limits too, this call site needs to pass the angular acceleration/velocity limits from the planner config so the test actually enforces them.

This issue also appears on line 125 of the same file.

  return stats.check(log, 2.0f, 4.0f, 1.0f);

pc/tests/trajectory_planner.cpp:10

• On MSVC, timeBeginPeriod / TIMERR_NOERROR are declared in the Windows multimedia headers (timeapi.h/mmsystem.h) and typically require linking against winmm. Including only <windows.h> can result in missing declarations on some setups.

#ifdef _MSC_VER
#include <windows.h>
#endif

[finger563]

Might be good to add links to the plot screenshots you uploaded to the PR text to this readme and the example README (and in the example readme add a link to the micro log output screenshot as well)

[finger563]

Since you've regenerated, it seems you should merge main into this branch and then regenerate, since your current generated file removes some socket methods from the generated api

[guo-max]

I did merge main, but the socket function is still not there. Is this because the Macro?

#if !defined(ESP_PLATFORM) || LWIP_IPV6
    /**
     * @brief Gives access to IPv6 sockaddr structure (sockaddr_in6) for use
     *        with low level socket calls like sendto / recvfrom.
     * @return *sockaddr_in6 pointer to ipv6 data structure
     */
    struct sockaddr_in6 *ipv6_ptr();
#endif // !defined(ESP_PLATFORM) || LWIP_IPV6

[finger563]

Looks good! i've gone ahead and approved it with some comments as well for some suggested changes prior to the merge.

[Copilot]

Pull request overview

Copilot reviewed 25 out of 26 changed files in this pull request and generated 4 comments.

Suppressed comments (11)

components/trajectory_planner/src/trajectory_planner.cpp:166

• These warn logs can spam in normal operation (e.g., full-stick diagonal inputs frequently exceed the unit circle; centripetal scaling can occur continuously). Consider lowering to debug/info, adding rate-limiting, or only warning when a configuration is inconsistent rather than when routine clamping occurs.

        logger_.warn("Motion envelope enforced: linear={}, angular={}, magnitude={}", linear,
                     angular, magnitude);
      }

components/trajectory_planner/src/trajectory_planner.cpp:183

• These warn logs can spam in normal operation (e.g., full-stick diagonal inputs frequently exceed the unit circle; centripetal scaling can occur continuously). Consider lowering to debug/info, adding rate-limiting, or only warning when a configuration is inconsistent rather than when routine clamping occurs.

      logger_.warn(
          "Centripetal acceleration limit enforced: target_v={}, target_w={}, scale={}, a_c={}",
          target_v, target_w, scale, a_c);

components/trajectory_planner/src/trajectory_planner.cpp:243

• update() wakes the callback task every tick even when output_callback_ is null (and start_task() always starts the callback task). This adds avoidable wakeups/locking overhead. Consider only starting the callback task when a callback is configured, and/or only notifying when output_callback_ is non-null (handling enable/disable via set_config() accordingly).

  {
    std::lock_guard<std::mutex> lk(callback_wake_m_);
    callback_wake_flag_ = true;
  }
  callback_wake_cv_.notify_one();

components/trajectory_planner/include/trajectory_planner.hpp:261

• The comment says the callback task 'falls back to timeout', but in the current implementation it waits indefinitely on a condition variable (no polling timeout). Update the comment to match the actual behavior to avoid misleading future changes.

  std::unique_ptr<espp::Task> callback_task_ =
      nullptr; // woken by CV from update(), falls back to timeout

pc/tests/trajectory_planner.cpp:17

• high_resolution_clock may not be monotonic on some platforms (it can alias system_clock). For elapsed-time measurements in tests, prefer std::chrono::steady_clock to avoid time adjustments affecting dt and potentially test outcomes.

static auto prog_start = std::chrono::high_resolution_clock::now();
static float elapsed() {
  return std::chrono::duration<float>(std::chrono::high_resolution_clock::now() - prog_start)
      .count();
}

pc/tests/trajectory_planner.cpp:63

• Stats tracks max_w, max_a_w_drv, and max_a_w_stp but check() ignores them, so tests can pass while violating angular velocity/acceleration limits. Update check() (and its logging) to validate angular limits alongside linear limits.

  bool check(espp::Logger &log, float drv_a_lim, float stp_a_lim, float v_lim, float tol = 1.10f) {
    bool ok = (max_v <= v_lim * 1.05f) && (max_a_v_drv <= drv_a_lim * tol) &&
              (max_a_v_stp <= stp_a_lim * tol);
    log.info("  max|v|={:.3f}/{:.1f}  |a_v_drv|={:.3f}/{:.1f}  |a_v_stp|={:.3f}/{:.1f}  -> {}",
             max_v, v_lim, max_a_v_drv, drv_a_lim, max_a_v_stp, stp_a_lim, ok ? "PASS" : "FAIL");
    return ok;
  }

pc/tests/trajectory_planner.cpp:198

• On Windows, if timeBeginPeriod(1) succeeds it should typically be paired with timeEndPeriod(1) before exit to restore the system timer resolution. Consider calling timeEndPeriod(1) once tests complete (ideally guarded so it only runs when timeBeginPeriod succeeded).

#ifdef _MSC_VER
  log.info("On Windows, setting timeBeginPeriod(1)");
  if (timeBeginPeriod(1) == TIMERR_NOERROR) {
    log.info("Success");
  } else {
    log.error("Failed to set timeBeginPeriod(1)");
  }
#endif

python/trajectory_planner.py:134

• The plotted centripetal term is v*w, but the label indicates |v*w|. If you want centripetal acceleration magnitude, plot abs(v*w) and consider drawing only the positive limit line (centripetal magnitude is non-negative).

    (axes[3], [([v * w for v, w in zip(v_hist, w_hist)], "|v*w| centripetal", "C6")],
     "Centripetal (m/s^2)", [cfg.max_centripetal_acceleration, -cfg.max_centripetal_acceleration]),

lib/python_bindings/espp/init.pyi:4054

• The stub advertises a no-arg TrajectoryPlanner() constructor, but the binding in pybind_espp.cpp registers __init__(const TrajectoryPlanner::Config&). This mismatch will cause runtime TypeError for users following the stub. Update the stub generation to only emit a default __init__ when the binding actually exposes one, or add a real default constructor in the binding if intended.

    def __init__(self) -> None:
        """Auto-generated default constructor"""
        pass

lib/python_bindings/pybind_espp.cpp:1644

• The PR description focuses on TrajectoryPlanner, but this PR also removes IPv6 Socket::Info bindings and adds new socket APIs (Socket.native_handle, UdpSocket.bind) plus changes Timer start() return docs/signatures. Please either update the PR description to mention these additional API changes or split them into a separate PR to keep scope/review focused.

                 "*\n     * @brief Gives access to IPv4 sockaddr structure (sockaddr_in) for use\n "
                 "    *        with low level socket calls like sendto / recvfrom.\n     * @return "
                 "*sockaddr_in pointer to ipv4 data structure\n")

lib/python_bindings/pybind_espp.cpp:1669

• The PR description focuses on TrajectoryPlanner, but this PR also removes IPv6 Socket::Info bindings and adds new socket APIs (Socket.native_handle, UdpSocket.bind) plus changes Timer start() return docs/signatures. Please either update the PR description to mention these additional API changes or split them into a separate PR to keep scope/review focused.

      .def("native_handle", &espp::Socket::native_handle,
           "*\n   * @brief Get the underlying native socket file descriptor / handle.\n   * @note "
           "Provided so an external event loop (e.g. SocketReactor) can add this\n   *       "