feat: add jerk-limited trajectory planning (WP-05)

ScurveProfile plans a time-optimal rest-to-rest move under velocity,
acceleration and jerk limits as seven constant-jerk segments. Short moves
lose the cruise, shorter ones lose the acceleration plateaus as well; all
three shapes come out of one construction with the unused segments at zero
duration, so sampling costs the same wherever in the profile it lands.
Sampling is O(1), noexcept and allocation-free, as is planning.

SynchronizedTrajectory drives every axis from a single path parameter
s: 0 -> 1. Planning each axis independently and stretching the faster ones
does synchronise the endpoints and keeps every axis inside its limits, but
it still bows the path because each axis retains its own profile shape and
the ratios between them drift through the move. Measured on a two-axis move:
13.6 mm off the straight line, against 1.1e-16 for the path-parameter form.

Two findings from the tests:

A short move produced a cruise segment of about 1e-17 seconds because
the cruise duration was computed from leftover distance in every case --
effectively asking for zero as the difference of two values agreeing to
fifteen digits. Numerically harmless, but hasCruisePhase() then reported
that the move cruised, and predicates are what callers branch on.
Cruise presence is now decided by a closed-form comparison.
Forward Euler at 1 kHz lags the commanded position by half a step of
velocity -- 1.0 mm on a 2 m/s move -- and the error cancels to exactly
zero by the end of a symmetric rest-to-rest profile. An acceptance test
checking only final position therefore passes even though the machine
was in the wrong place throughout the move. Both halves are measured;
the header explicitly directs callers to sample the profile rather than
integrate it.

MotionLimits defaults to zeros and validate() rejects them: an unconfigured
axis must be one that may not move, not one with no ceiling. Finiteness is
checked before positivity because a NaN limit passes <= 0 and then passes
every downstream bound check as well.

Expected moves into its own header and names its error type explicitly, so
the trajectory module can use it without depending on the frame graph. The
allocation probe is renamed to test_no_allocation.cpp now that its coverage
extends beyond the frame graph.

Also fixes MOTIONKIT_BUILD_BENCHMARKS, which previously failed at configure
time because add_subdirectory() pointed at an empty, untracked directory
that was absent from fresh checkouts. Benchmarks now exist, CI builds and
runs them, and the install-consumer job exercises the new headers so the
installed package contract covers the trajectory API.

As a supporting repository fix, line endings are pinned to LF so Windows
checkouts can execute the project scripts. With core.autocrlf=true and no
.gitattributes, scripts/format.sh acquired a /usr/bin/env bash\r shebang,
making it unrunnable under both WSL and Git Bash while Linux CI remained
unaffected.

31 new tests; 108 total, green under GCC and Clang in Debug and Release,
ASan/UBSan, TSan and clang-tidy.

Author: Onwcan

.gitattributes

@@ -0,0 +1,6 @@
+# Line endings are normalised to LF in the working tree, not only in the object
+# store. Without this, a Windows checkout with core.autocrlf=true gets CRLF
+# files, and `scripts/format.sh` -- whose shebang then reads `/usr/bin/env
+# bash\r` -- cannot be run at all from WSL or Git Bash. CI never sees it,
+# because CI checks out on Linux.
+* text=auto eol=lf

.github/workflows/ci.yml

@@ -78,6 +78,31 @@ jobs:
           UBSAN_OPTIONS: print_stacktrace=1
         run: ctest --preset ${{ matrix.preset }}
 
+  benchmarks:
+    name: benchmarks build and run
+    runs-on: ubuntu-24.04
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install toolchain
+        run: sudo apt-get update && sudo apt-get install -y ninja-build
+
+      # An option nobody exercises stops working quietly. MOTIONKIT_BUILD_BENCHMARKS
+      # spent its first months pointing add_subdirectory() at a directory that
+      # was empty -- and therefore untracked, and therefore absent on a fresh
+      # checkout -- so turning the option on failed at configure time.
+      #
+      # The numbers are not asserted on. A shared runner has no isolated cores
+      # and no real-time scheduling, so a threshold here would be a flake
+      # generator. This job proves the benchmark still builds and still runs to
+      # completion; the figures in the README come from a known machine.
+      - name: Configure with benchmarks on
+        run: |
+          cmake -S . -B build -DCMAKE_BUILD_TYPE=Release             -DMOTIONKIT_BUILD_BENCHMARKS=ON -DMOTIONKIT_BUILD_TESTS=OFF
+      - name: Build
+        run: cmake --build build -j "$(nproc)"
+      - name: Run
+        run: ./build/benchmarks/motionkit-bench
+
   static-analysis:
     name: clang-tidy + clang-format
     runs-on: ubuntu-24.04
@@ -138,7 +163,10 @@ jobs:
           target_link_libraries(consumer PRIVATE motionkit::core)
           EOT
           cat > /tmp/consumer/main.cpp <<'EOT'
+          #include <array>
+          #include <cmath>
           #include "motionkit/core/frame_graph.hpp"
+          #include "motionkit/core/trajectory.hpp"
           int main() {
             using namespace motionkit;
             const SE3 pose(SO3::rotZ(1.0), Vec3{1.0, 2.0, 3.0});
@@ -158,6 +186,23 @@ jobs:
             if (!resolved || !resolved.value.isApprox(pose, 1e-12, 1e-12)) {
               return 4;
             }
+            // Every public header has to be reachable from the installed tree,
+            // not just the one the first consumer happened to use.
+            const std::array<Scalar, 2> here{0.0, 0.0};
+            const std::array<Scalar, 2> there{1.0, 0.25};
+            const std::array<MotionLimits, 2> limits{MotionLimits{2.0, 8.0, 40.0},
+                                                     MotionLimits{2.0, 8.0, 40.0}};
+            const auto move = SynchronizedTrajectory::plan(here, there, limits);
+            if (!move || move.value.duration() <= 0.0) {
+              return 5;
+            }
+            std::array<MotionSample, 2> setpoints{};
+            if (!move.value.sample(move.value.duration(), setpoints)) {
+              return 6;
+            }
+            if (std::abs(setpoints[1].position - 0.25) > 1e-12) {
+              return 7;
+            }
             return 0;
           }
           EOT

CMakeLists.txt

@@ -44,7 +44,8 @@ target_compile_options(motionkit_warnings INTERFACE
 add_library(motionkit_core
   src/core/so3.cpp
   src/core/se3.cpp
-  src/core/frame_graph.cpp)
+  src/core/frame_graph.cpp
+  src/core/trajectory.cpp)
 add_library(motionkit::core ALIAS motionkit_core)
 
 target_include_directories(motionkit_core

README.md

@@ -20,12 +20,13 @@ Eigen, no KDL, no Pinocchio — the algorithms are the point.
 | WP-02 | SE(3) transforms, frame graph, tool modeling | **Done** |
 | WP-03 | Forward and inverse kinematics (6R) | Planned |
 | WP-04 | Rigid-body dynamics (RNEA, CRBA) | Planned |
-| WP-05 | Trajectory planning (S-curve, TOPP, blending) | Planned |
+| WP-05 | Trajectory planning (jerk-limited S-curve, multi-axis synchronisation) | **Done** |
 | WP-06 | Hand-eye, TCP and base-frame calibration | Planned |
+| WP-11 | Blending, TOPP, and planning from a non-zero initial state | Planned |
 | WP-12 | CUDA batch IK and collision checking | Planned |
 
-77 tests, all passing under GCC and Clang in Debug and Release. ASan and UBSan
-exercise the full suite. TSan exercises the 73 ordinary tests; the four
+108 tests, all passing under GCC and Clang in Debug and Release. ASan and UBSan
+exercise the full suite. TSan exercises the 101 ordinary tests; the seven
 allocator-interposition tests run in a dedicated executable and are excluded
 from TSan because both the tests and the sanitizer runtime replace the global
 allocation functions.
@@ -44,8 +45,8 @@ ctest --preset debug
 ```
 
 Other presets: `release`, `asan`, `tsan`, `tidy`. The `tsan` preset intentionally
-runs 73 tests: the four tests that instrument global allocation are a test-harness
-incompatibility with TSan, not an exemption for production code.
+runs 101 tests: the seven tests that instrument global allocation are a
+test-harness incompatibility with TSan, not an exemption for production code.
 
 Before pushing, run the formatter -- CI enforces it:
 
@@ -97,6 +98,26 @@ if (const auto camera_T_tcp = frames.lookup(camera, tcp)) {
 }
 ```
 
+Point-to-point motion is planned once and sampled every cycle. Limits are
+per-axis; the axes stay synchronised and travel a straight line in joint space:
+
+```cpp
+#include "motionkit/core/trajectory.hpp"
+
+const std::array<Scalar, 6> here{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+const std::array<Scalar, 6> there{1.0, 0.2, -0.8, 0.4, 1.6, -0.3};
+const std::array<MotionLimits, 6> limits{/* v, a, j per axis */};
+
+const auto move = SynchronizedTrajectory::plan(here, there, limits);
+if (!move) {
+  return log(toString(move.error));
+}
+
+// In the 1 kHz task: sample, never integrate.
+std::array<MotionSample, 6> setpoints{};
+move.value.sample(elapsed_seconds, setpoints);
+```
+
 ---
 
 ## Design decisions worth arguing about
@@ -148,6 +169,33 @@ counts allocations proves nothing if the counter is inert. Failures come back as
 yet calibrated — and `Disconnected` is deliberately a different answer from
 `UnknownFrame`.
 
+**Multi-axis moves are driven by one path parameter, not one profile per axis.**
+Planning each axis separately and stretching the quick ones does synchronise the
+endpoints, and no axis exceeds a limit — and the path is still bent, because
+each axis keeps its own profile shape and the ratios between them drift through
+the move. Measured on a two-axis move: **13.6 mm** off the straight line, from a
+plan in which nothing was ever violated. Driving every axis from a single
+`s: 0 → 1` makes the ratios constant by construction; the same measurement comes
+back 1.1e-16. Whichever axis binds each limit runs exactly at it, which is what
+time-optimal means once the path is fixed. See
+[ADR-0006](docs/adr/0006-jerk-limited-profiles-and-a-single-path-parameter.md).
+
+**Sample a trajectory; do not integrate it.** Forward Euler at 1 kHz lags the
+commanded position by half a step of velocity — 1.0 mm on a 2 m/s move. Then,
+because a rest-to-rest profile accelerates and decelerates by equal amounts, the
+error cancels to **exactly zero** by the end. An acceptance test that checks the
+final position passes, while the machine was in the wrong place for the entire
+move. `TrajectorySampling.EulerIntegrationLagsMidMoveThenLandsOnTargetAnyway`
+measures both halves.
+
+**Unset limits mean the axis may not move.** `MotionLimits` defaults to zeros
+and `validate()` rejects them. Reading an unset limit as "no limit" makes
+forgetting to configure an axis indistinguishable from configuring it for full
+speed, and the difference is only observable on the machine. Finiteness is
+checked before positivity, because a NaN limit passes `<= 0` and then passes
+every bound check downstream too — comparisons against NaN are false however
+they are written.
+
 **Euler angles are an export format, never a representation.**
 `toRPY` recovers pitch via `atan2(-m₂₀, hypot(m₀₀, m₁₀))` rather than
 `asin(-m₂₀)`, for the same conditioning reason — and tool-down poses sit exactly
@@ -167,8 +215,8 @@ test wrong.
 |---|---|
 | GCC + Clang × Debug + Release | `-Wconversion` and `-Wold-style-cast` fire on different constructs per compiler |
 | `-Werror` with `-Wconversion -Wsign-conversion -Wold-style-cast -Wshadow` | Silent narrowing in a pose pipeline is a field failure, not a warning |
-| ASan + UBSan on all 77 tests, `-fno-sanitize-recover=all` | A UBSan finding fails the build rather than printing a note |
-| TSan on the 73 ordinary tests | Ahead of the threaded executor in WP-08; the four allocator-interposition tests are excluded because TSan defines the same global allocation hooks |
+| ASan + UBSan on all 108 tests, `-fno-sanitize-recover=all` | A UBSan finding fails the build rather than printing a note |
+| TSan on the 101 ordinary tests | Ahead of the threaded executor in WP-08; the seven allocator-interposition tests are excluded because TSan defines the same global allocation hooks |
 | clang-tidy, `--warnings-as-errors=*` | Rule set and exclusions justified in ADR-0002 |
 | `scripts/format.sh --check` with clang-format 18 | Formatting is not a review topic, and CI runs the same check developers run |
 | **install with repository tests off + downstream consumer compile and run** | Exercises only the installed package contract; it caught a real bug on first run when the exported target was `motionkit::motionkit_core` but consumers used `motionkit::core` |
@@ -181,7 +229,7 @@ Unit tests assert known values; the interesting ones assert **properties** over
 thousands of uniformly sampled rotations from a fixed seed — a property test you
 cannot replay is a flake, not a test.
 
-Four allocation tests are instrumentation rather than ordinary unit tests. They
+Seven allocation tests are instrumentation rather than ordinary unit tests. They
 run in their own executable because their global `operator new`/`operator delete`
 replacements affect an entire process. That target alone suppresses GNU's
 `-Wmismatched-new-delete` diagnostic: the `malloc`/`free` pairing is deliberate
@@ -197,6 +245,38 @@ and is the mechanism being tested. The warning remains enabled everywhere else.
 
 ---
 
+## Benchmarks
+
+The claim that these operations are callable from a cyclic task is only worth as
+much as the number, so there is a number. No google-benchmark: the library takes
+no third-party dependencies, and a benchmark you cannot build straight after
+cloning is a benchmark nobody runs.
+
+```bash
+cmake -S . -B build/bench -DCMAKE_BUILD_TYPE=Release -DMOTIONKIT_BUILD_BENCHMARKS=ON
+cmake --build build/bench -j
+./build/bench/benchmarks/motionkit-bench
+```
+
+Nanoseconds per call, GCC 15 `-O2`, ordinary desktop with no core isolation and
+no real-time scheduling:
+
+| Operation | min | median | max | of a 1 kHz cycle |
+|---|---|---|---|---|
+| `SO3` composition | 46.1 | 46.3 | 95.3 | 0.005 % |
+| `FrameGraph::lookup`, tool to camera | 250.2 | 251.8 | 311.5 | 0.025 % |
+| `ScurveProfile::sample` | 4.0 | 4.4 | 11.7 | 0.0004 % |
+| `SynchronizedTrajectory::sample`, 6 axes | 8.2 | 8.6 | 15.1 | 0.001 % |
+| `SynchronizedTrajectory::plan`, 6 axes | 82.8 | 86.2 | 119.0 | 0.009 % |
+
+The maximum column is dominated by whatever else the machine was doing, and is
+reported anyway: a control loop is sized by its worst cycle, not its median.
+The last row is the interesting one — planning a six-axis move costs less than a
+`FrameGraph` lookup, so a mid-move re-plan on a feed-rate override is something
+the cyclic task can do itself rather than hand to another thread.
+
+---
+
 ## Licence
 
 Apache-2.0.

benchmarks/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Deliberately no google-benchmark. The library takes no third-party
+# dependencies, and a benchmark that pulls one in cannot be built by someone who
+# just cloned the repo -- which is when the numbers are wanted.
+add_executable(motionkit_bench bench_core.cpp)
+target_link_libraries(motionkit_bench PRIVATE motionkit::core motionkit::warnings)
+set_target_properties(motionkit_bench PROPERTIES OUTPUT_NAME motionkit-bench)

benchmarks/bench_core.cpp

@@ -0,0 +1,157 @@
+// SPDX-License-Identifier: Apache-2.0
+//
+// Per-call cost of the operations this library claims are callable from a
+// cyclic control task. The claim is only worth as much as the number.
+//
+// Method: each operation runs in batches, and the batch is timed rather than
+// the call, because a steady_clock read costs more than most of the calls
+// measured here. Batches are repeated and the distribution reported, since the
+// median says what a cycle normally costs and the maximum says whether a cycle
+// can be missed.
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstddef>
+#include <cstdio>
+#include <string>
+#include <vector>
+
+#include "motionkit/core/frame_graph.hpp"
+#include "motionkit/core/trajectory.hpp"
+
+namespace motionkit {
+namespace {
+
+constexpr std::size_t kBatch = 1000;
+constexpr std::size_t kRepeats = 500;
+/// One cycle of a 1 kHz control task, for scale.
+constexpr double kCycleNanos = 1'000'000.0;
+
+/// Somewhere for results to go that the optimiser cannot reason about.
+volatile Scalar g_sink = 0.0;
+
+void barrier() {
+#if defined(__GNUC__) || defined(__clang__)
+  asm volatile("" ::: "memory");
+#endif
+}
+
+struct Result {
+  double min_ns{0.0};
+  double median_ns{0.0};
+  double max_ns{0.0};
+};
+
+template <typename F>
+Result measure(F&& body) {
+  std::vector<double> per_call;
+  per_call.reserve(kRepeats);
+
+  // Warm the caches and let the branch predictors settle; the first batch is
+  // not what a control loop experiences.
+  for (std::size_t i = 0; i < kBatch; ++i) {
+    body(i);
+  }
+
+  for (std::size_t r = 0; r < kRepeats; ++r) {
+    barrier();
+    const auto start = std::chrono::steady_clock::now();
+    for (std::size_t i = 0; i < kBatch; ++i) {
+      body(i);
+    }
+    const auto finish = std::chrono::steady_clock::now();
+    barrier();
+    const auto elapsed =
+        std::chrono::duration_cast<std::chrono::nanoseconds>(finish - start).count();
+    per_call.push_back(static_cast<double>(elapsed) / static_cast<double>(kBatch));
+  }
+
+  std::sort(per_call.begin(), per_call.end());
+  return Result{per_call.front(), per_call[per_call.size() / 2], per_call.back()};
+}
+
+void report(const char* name, const Result& r) {
+  std::printf("  %-38s %8.1f %9.1f %9.1f   %7.4f%%\n", name, r.min_ns, r.median_ns,
+              r.max_ns, 100.0 * r.median_ns / kCycleNanos);
+}
+
+FrameGraph buildArm(std::array<FrameId, 3>& of_interest) {
+  FrameGraph graph;
+  graph.reserve(24);
+  FrameId current = graph.declareRoot("base").value;
+  for (int i = 0; i < 6; ++i) {
+    const SE3 link(SO3::fromRPY(0.11, -0.07, 0.23), Vec3{0.13, -0.05, 0.21});
+    current = graph.declareFrame("joint" + std::to_string(i), current, link).value;
+  }
+  const SE3 offset(SO3::fromRPY(0.0, 0.0, 0.4), Vec3{0.0, 0.0, 0.125});
+  of_interest[0] = graph.declareFrame("tcp", current, offset).value;
+  of_interest[1] = graph.declareFrame("camera", current, offset).value;
+  of_interest[2] = current;
+  return graph;
+}
+
+}  // namespace
+}  // namespace motionkit
+
+int main() {
+  using namespace motionkit;
+
+  std::array<FrameId, 3> frames{};
+  FrameGraph graph = buildArm(frames);
+
+  constexpr MotionLimits axis{2.0, 8.0, 40.0};
+  const ScurveProfile profile = ScurveProfile::plan(0.0, 2.0, axis).value;
+
+  const std::array<Scalar, 6> start{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  const std::array<Scalar, 6> goal{1.0, 0.2, -0.8, 0.4, 1.6, -0.3};
+  const std::array<MotionLimits, 6> limits{axis, axis, axis, axis, axis, axis};
+  const SynchronizedTrajectory arm =
+      SynchronizedTrajectory::plan(start, goal, limits).value;
+  std::array<MotionSample, 6> samples{};
+
+  std::printf("motionkit micro-benchmarks (nanoseconds per call)\n");
+  std::printf("  %-38s %8s %9s %9s   %8s\n", "operation", "min", "median", "max",
+              "of 1 kHz");
+  std::printf("  %s\n", std::string(78, '-').c_str());
+
+  report("SO3 composition", measure([&](std::size_t) {
+           const SO3 r = SO3::fromRPY(0.1, 0.2, 0.3) * SO3::fromRPY(0.3, 0.2, 0.1);
+           g_sink = r.w();
+         }));
+
+  report("FrameGraph::lookup (tcp <- camera)", measure([&](std::size_t) {
+           g_sink = graph.lookup(frames[0], frames[1]).value.translation().x;
+         }));
+
+  report("FrameGraph::lookup (tcp <- base)", measure([&](std::size_t) {
+           g_sink = graph.lookup(frames[0], frames[2]).value.translation().z;
+         }));
+
+  report("ScurveProfile::sample", measure([&](std::size_t i) {
+           const Scalar t = profile.duration() * static_cast<Scalar>(i % 1000) / 1000.0;
+           g_sink = profile.sample(t).position;
+         }));
+
+  report("SynchronizedTrajectory::sample (6 axes)", measure([&](std::size_t i) {
+           const Scalar t = arm.duration() * static_cast<Scalar>(i % 1000) / 1000.0;
+           (void)arm.sample(t, samples);
+           g_sink = samples[0].position;
+         }));
+
+  report("SynchronizedTrajectory::plan (6 axes)", measure([&](std::size_t i) {
+           const Scalar factor = 0.5 + 0.0005 * static_cast<Scalar>(i % 1000);
+           const std::array<MotionLimits, 6> scaled{
+               limits[0].scaled(factor), limits[1].scaled(factor),
+               limits[2].scaled(factor), limits[3].scaled(factor),
+               limits[4].scaled(factor), limits[5].scaled(factor)};
+           g_sink = SynchronizedTrajectory::plan(start, goal, scaled).value.duration();
+         }));
+
+  std::printf(
+      "\n  Timings come from an ordinary desktop with no isolated cores and no\n"
+      "  real-time scheduling, so the maximum column is dominated by whatever\n"
+      "  else the machine was doing. It is reported anyway: a control loop is\n"
+      "  sized by its worst cycle, not its median.\n");
+  return 0;
+}