feat: capsule collision checking and clearance (WP-12d)

CollisionModel reports the closest approach between an arm's links and its
surroundings, and between the arm's own links, at any configuration.

WP-12d bundled three things and this is one of them. The other two are open for
reasons worth recording. CUDA cannot be verified -- GitHub's runners have no
GPU, and a component whose correctness rested on a machine under a desk would
be the first thing here not gated by CI. Full TOPP has no jerk limit: it
optimises in the phase plane subject to velocity and acceleration, and this
library calls jerk the limit that decides whether the mechanics ring. Shipping
a planner that dropped it for a shorter cycle time would contradict ADR-0006,
and adding jerk to TOPP is a harder problem than TOPP rather than a refinement
of it.

Everything is a capsule -- a segment with a radius -- so the distance between
any two things is the distance between two segments minus their radii. One
function, no special cases, no orientation to track. The cost is fidelity: a
link that is not very capsule-shaped is covered generously and the model refuses
poses a real machine could hold. That is the right direction to be wrong in and
the fix is more capsules.

Skipping adjacent links is not enough, and believing it is breaks the model.
Everyone knows two links sharing a joint touch always. Less obvious is that a
spherical wrist turns its last three links about nearly one point, so links two
apart overlap there at every configuration as well -- and a model with only the
adjacent rule reports a collision while the arm is parked. Nobody debugs a
collision checker that fires on a stationary machine; they switch collision
checking off, and then the cell has none. build() therefore takes the pairs to
ignore, the example ships with the three wrist pairs already excluded, and the
dullest test in the file asserts the arm is clear standing still. An exclusion
naming a link that does not exist is refused rather than dropped: somebody wrote
it meaning something, and ignoring it leaves a model that believes it is safer
than it is.

Self clearance and obstacle clearance are reported apart, and the tests are what
forced it. Returning only the overall minimum is exactly what it says and
useless: a straight arm's own links sit 0.22 m apart, so the overall minimum
reports 0.22 wherever an obstacle is, until the obstacle is nearer than the arm
is to itself. An obstacle 30 cm away was invisible to somebody asking about
obstacles. Not a bug in the arithmetic -- a question answered so literally it
stopped being useful.

Distances are signed, so an overlap reports its depth rather than clamping at
zero; touching and driven 40 mm through want different responses. With nothing
to measure at all the answer is kNothingNear, a large finite number, because
infinity survives arithmetic that should have failed and returns as a NaN
somewhere else much later.

Two of my own errors the tests caught. An obstacle expected at 0.84 came back at
0.8372, because the upper arm's thicker 0.07 sleeve beats the forearm's 0.06
despite its end sitting slightly further off. And the configuration written to
demonstrate self-collision does not collide -- the elbow had to bend the other
way, which was found by sweeping rather than by picturing it.

A query costs 432 ns for six links, three obstacles and nine self pairs --
twenty-seven capsule distances, 0.043% of a millisecond cycle, allocating
nothing -- so it can gate every setpoint rather than only every plan. It is
deliberately not wired into CartesianPlan: the obstacle set changes far more
often than the arm does, and baking it in would mean replanning to answer a
question about a fence that moved. A test follows clearance along a planned
Cartesian move instead, which is the composition without the coupling.

Nothing checks the swept volume between two configurations, so a step large
enough to pass through a thin obstacle passes through it undetected. Stated
here rather than discovered later; following a plan means sampling it densely.

Also corrects a comment from the previous commit. It said
upload-pages-artifact@v3 is not part of the Node 20 deprecation. It does not
appear in the notice directly, being a composite action, and it still carries
one through the upload-artifact it calls -- which is the single warning the last
run reported, down from ten. Comment only, no behaviour.

15 new tests; 222 total, green under GCC and Clang in Debug and Release,
ASan/UBSan and clang-tidy, 205 under TSan, with the documentation gate and the
installed-package consumer both extended -- the consumer checking a parked arm
is clear of itself, which fails first if the allowed-collision set did not
survive the install.

Author: Onwcan

.github/workflows/ci.yml

@@ -168,6 +168,7 @@ jobs:
           #include <cstddef>
           #include "motionkit/core/calibration.hpp"
           #include "motionkit/core/cartesian.hpp"
+          #include "motionkit/core/collision.hpp"
           #include "motionkit/core/dynamics.hpp"
           #include "motionkit/core/frame_graph.hpp"
           #include "motionkit/core/kinematics.hpp"
@@ -316,6 +317,24 @@ jobs:
                 !arm.forward(ended).value.isApprox(route.back(), 1e-5, 1e-5)) {
               return 22;
             }
+            // The collision header. A parked arm must be clear of itself --
+            // the check that fails first if the exported allowed-collision set
+            // did not survive the install.
+            const auto guard = CollisionModel::sixAxisExample();
+            if (!guard) {
+              return 23;
+            }
+            const std::array<Scalar, 6> parked{};
+            const auto nearest = guard.value.clearance(parked, {});
+            if (!nearest || !(nearest.value.to_self > 0.0)) {
+              return 24;
+            }
+            const std::array<Capsule, 1> fence{
+                Capsule{Vec3{0.30, 0.0, 0.0}, Vec3{0.30, 0.0, 1.2}, 0.05}};
+            const auto against = guard.value.clearance(parked, fence);
+            if (!against || against.value.to_obstacles >= kNothingNear) {
+              return 25;
+            }
             return 0;
           }
           EOT
@@ -348,10 +367,15 @@ jobs:
           cmake --build build --target docs
 
       - name: Upload for publication
-        # Deliberately still v3. It is a composite action with no Node runtime
-        # of its own, so it is not part of the Node 20 deprecation, and v3 with
-        # deploy-pages@v5 is the pairing GitHub ships in its own Pages starter
-        # workflow. Newer majors exist; none of them buy anything here.
+        # Deliberately still v3, but not for the reason first written here.
+        #
+        # It is a composite action with no Node runtime of its own, so it does
+        # not appear in the deprecation notice directly -- and it still carries
+        # one, because the upload-artifact it calls internally is a Node 20
+        # action. That is the single warning the last run reported, down from
+        # ten. Clearing it means a newer major of this action, which is a
+        # different pairing with deploy-pages from the one GitHub ships in its
+        # own Pages starter workflow, and that trade has not been made yet.
         uses: actions/upload-pages-artifact@v3
         with:
           path: build/docs/html

CHANGELOG.md

@@ -31,6 +31,13 @@ bump as "something may have moved".
 
 ### Added
 
+- **Capsule collision checking** (WP-12d). `CollisionModel` reports the closest
+  approach between an arm's links and its surroundings, and between the arm's
+  own links, at any configuration. Distances are signed, so an overlap reports
+  its depth. Self and obstacle clearances are reported separately because one
+  routinely masks the other. Takes an allowed-collision set, without which a
+  spherical wrist reports a collision while parked.
+  ([ADR-0015](docs/adr/0015-collision-checking-in-capsules.md))
 - **Blended routes through waypoints** (WP-12c). `CartesianPlan::planThrough`
   plans a whole sequence as one path under one profile, so the tool does not
   stop at intermediate waypoints. Interior corners are rounded by a configurable

CMakeLists.txt

@@ -47,6 +47,7 @@ add_library(motionkit_core
   src/core/se3.cpp
   src/core/frame_graph.cpp
   src/core/cartesian.cpp
+  src/core/collision.cpp
   src/core/calibration.cpp
   src/core/dynamics.cpp
   src/core/kinematics.cpp

CONTRIBUTING.md

@@ -20,7 +20,7 @@ Two caveats worth knowing before you spend an afternoon on them:
 
 - **TSan excludes the allocation tests.** They replace global `operator new`,
   which is exactly what TSan's runtime also does. `MOTIONKIT_BUILD_ALLOCATION_TESTS`
-  is off under that preset, so the count is 191 rather than 207.
+  is off under that preset, so the count is 205 rather than 222.
 - **`ScurveProfile` is rest-to-rest; `ReachProfile` is not.** Use the latter
   when the axis is already moving. `StopProfile` starts from an arbitrary state
   and has no position target, which is a third problem again.

README.md

@@ -26,11 +26,12 @@ Eigen, no KDL, no Pinocchio — the algorithms are the point.
 | WP-12a | Straight-line Cartesian moves, paced by joint limits | **Done** |
 | WP-12b | Moves from a non-zero state (the piece blending needed) | **Done** |
 | WP-12c | Blended routes through waypoints, without stopping | **Done** |
-| WP-12d | Full TOPP, CUDA batch IK and collision checking | Planned |
+| WP-12d | Capsule collision checking and clearance | **Done** |
+| WP-12e | Full TOPP, CUDA batch IK | Planned |
 | WP-15 | API reference, architecture docs, contribution process | **Done** |
 
-207 tests, all passing under GCC and Clang in Debug and Release. ASan and UBSan
-exercise the full suite. TSan exercises the 191 ordinary tests; the sixteen
+222 tests, all passing under GCC and Clang in Debug and Release. ASan and UBSan
+exercise the full suite. TSan exercises the 205 ordinary tests; the seventeen
 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.
@@ -49,7 +50,7 @@ ctest --preset debug
 ```
 
 Other presets: `release`, `asan`, `tsan`, `tidy`. The `tsan` preset intentionally
-runs 191 tests: the sixteen tests that instrument global allocation are a
+runs 205 tests: the seventeen 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:
@@ -238,6 +239,23 @@ allocates nothing, which is 0.23 % of a 1 kHz cycle, so it can run in the loop
 that needs the answer rather than on a thread with a queue in front of it. See
 [ADR-0008](docs/adr/0008-kinematics-by-screws-and-a-damped-inverse.md).
 
+**Skipping adjacent links is not enough for self-collision, and believing it is
+breaks the model.** Two links sharing a joint touch always, so everyone skips
+adjacent pairs. But a spherical wrist turns its last three links about nearly one
+point, so links *two* apart overlap at every configuration too — and a model with
+only the adjacent rule reports a collision while the arm is parked. Nobody debugs
+that; they switch collision checking off, and then the cell has none. `build`
+takes an allowed-collision set, and the dullest test in the repo asserts the
+example arm is clear while standing still.
+
+**Self-clearance and obstacle clearance are reported apart.** Returning only the
+overall minimum was the first design and the tests killed it: a straight arm's own
+links sit **0.22 m** apart, so the overall minimum reports 0.22 regardless of where
+an obstacle is, until the obstacle is nearer than the arm is to itself. An obstacle
+30 cm away became invisible. Not a bug in the arithmetic — a question answered so
+literally it stopped being useful. See
+[ADR-0015](docs/adr/0015-collision-checking-in-capsules.md).
+
 **Blending cuts the corner, and the library says how much.** A tool cannot turn
 a sharp corner at speed — its velocity would have to change direction
 instantaneously — so the only choice is between stopping at the waypoint and not
@@ -422,8 +440,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 207 tests, `-fno-sanitize-recover=all` | A UBSan finding fails the build rather than printing a note |
-| TSan on the 191 ordinary tests | Ahead of the threaded executor in WP-08; the sixteen allocator-interposition tests are excluded because TSan defines the same global allocation hooks |
+| ASan + UBSan on all 222 tests, `-fno-sanitize-recover=all` | A UBSan finding fails the build rather than printing a note |
+| TSan on the 205 ordinary tests | Ahead of the threaded executor in WP-08; the seventeen 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` |
@@ -436,7 +454,7 @@ test wrong.
 | Document | What it answers |
 |---|---|
 | [docs/architecture.md](docs/architecture.md) | C4 context, container and component views, and the rules that decide where new code goes |
-| [docs/adr/](docs/adr/) | Fourteen decisions, each with the alternatives that lost and why |
+| [docs/adr/](docs/adr/) | Fifteen decisions, each with the alternatives that lost and why |
 | [CONTRIBUTING.md](CONTRIBUTING.md) | How to build, what the gates are, and the conventions clang-format cannot express |
 | [docs/review-checklist.md](docs/review-checklist.md) | The questions that have actually caught something here |
 | [CHANGELOG.md](CHANGELOG.md) | What changed, and what `0.x` promises |
@@ -473,7 +491,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.
 
-Sixteen allocation tests are instrumentation rather than ordinary unit tests. They
+Seventeen 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
@@ -524,6 +542,7 @@ no real-time scheduling:
 | `DynamicChain::massMatrix`, 6R CRBA | 399.8 | 427.0 | 1050.9 | 0.043 % |
 | `CartesianPlan::sample`, 6R 33 knots | 17.9 | 18.4 | 28.5 | 0.002 % |
 | `CartesianPlan::plan`, 6R 33 knots | 76109 | 77373 | 87793 | 7.74 % |
+| `CollisionModel::clearance`, 6R + 3 obstacles | 416.1 | 431.6 | 1013.2 | 0.043 % |
 
 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.

benchmarks/bench_core.cpp

@@ -18,6 +18,7 @@
 #include <vector>
 
 #include "motionkit/core/cartesian.hpp"
+#include "motionkit/core/collision.hpp"
 #include "motionkit/core/dynamics.hpp"
 #include "motionkit/core/frame_graph.hpp"
 #include "motionkit/core/kinematics.hpp"
@@ -243,6 +244,16 @@ int main() {
            g_sink = replanned.value.duration();
          }));
 
+  const auto guard = CollisionModel::sixAxisExample();
+  const std::array<Capsule, 3> obstacles{
+      Capsule{Vec3{0.6, 0.0, 0.0}, Vec3{0.6, 0.0, 1.2}, 0.05},
+      Capsule{Vec3{-0.5, 0.4, 0.3}, Vec3{-0.5, 0.4, 0.3}, 0.10},
+      Capsule{Vec3{0.0, -0.7, 0.0}, Vec3{0.4, -0.7, 0.9}, 0.03}};
+
+  report("CollisionModel::clearance (6R, 3 obstacles)", measure([&](std::size_t) {
+           g_sink = guard.value.clearance(pose, obstacles).value.distance;
+         }));
+
   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"

docs/adr/0015-collision-checking-in-capsules.md

@@ -0,0 +1,144 @@
+# ADR-0015: Collision checking in capsules, with an allowed-collision set that is not optional
+
+- **Status**: Accepted
+- **Date**: 2026-09-10
+- **Deciders**: Onur Can Urhan
+
+## Context
+
+WP-12d bundled three things: full time-optimal path parameterisation, CUDA
+batch inverse kinematics, and collision checking. Only the third is here, and
+the other two are open for reasons worth writing down rather than leaving to be
+inferred from an empty directory.
+
+**CUDA cannot be verified.** GitHub's runners have no GPU, and every claim this
+library makes is currently gated by something that runs on every pull request.
+A component whose correctness rested on a machine under a desk would be the
+first exception, and the first exception is how a project stops being able to
+say its tests mean anything.
+
+**Full TOPP conflicts with a decision already made.** Classical TOPP optimises
+in the phase plane of the path parameter and its rate, subject to velocity and
+acceleration. It has **no jerk limit** -- and this library's README calls jerk
+"the limit that distinguishes an S-curve from a trapezoid, and the one that
+decides whether the mechanics ring". Shipping a planner that quietly dropped it
+in exchange for a shorter cycle time would contradict
+[ADR-0006](0006-jerk-limited-profiles-and-a-single-path-parameter.md). Adding
+jerk to TOPP is a substantially harder problem than TOPP, not a refinement of
+it, and pretending otherwise in a commit message would be the worst of the
+options.
+
+Collision checking has none of those problems. It is exactly testable, needs no
+hardware, and answers a question the Cartesian planner already half-answers:
+`least_manipulability` reports how close a move came to a singularity, and
+nothing reported how close it came to the fence.
+
+## Decision 1: everything is a capsule
+
+A link, a fence post, a fixture, a cable: all of them a segment with a radius.
+
+The distance between two capsules is the distance between two segments minus
+their radii. That is **one function with no special cases and no orientation to
+track**, and it is the entire geometry in the file. Boxes need separating-axis
+tests and an orientation each; meshes need a broad phase before they are
+affordable at all.
+
+The cost is fidelity, and it is a real cost. A link that is not very
+capsule-shaped is covered generously, so the model refuses moves that would
+have been fine. That is the right direction to be wrong in, and it is cheaper to
+fix by adding a second capsule than by adopting a mesh.
+
+Capsules are given **in the base frame with every joint at zero**, the same
+frame joints and inertias use, so all three can be read off one CAD model in one
+pose. [ADR-0008](0008-kinematics-by-screws-and-a-damped-inverse.md) explains why
+this library keeps asking for that frame and no other.
+
+## Decision 2: skipping adjacent links is not enough, and believing it is breaks the model
+
+Two links sharing a joint touch at every configuration, so a self-collision
+check that includes them reports a collision always. Skipping adjacent pairs is
+obvious and universal.
+
+It is also insufficient, and the insufficiency is not obvious. A spherical wrist
+turns its last three links about nearly one point, so links **two** apart
+overlap there at every configuration as well. A model with only the adjacent
+rule reports a collision while the arm is parked.
+
+That failure has a predictable ending. Nobody debugs a collision checker that
+fires on a stationary machine; they switch collision checking off, and then the
+cell has none. So `build` takes the pairs to ignore -- the same
+allowed-collision set a real cell keeps beside its geometry -- and the example
+model ships with the three wrist pairs already excluded. The test that matters
+most in the file is the dull one asserting the example arm reports positive
+clearance while standing still.
+
+An exclusion naming a link that does not exist is **refused**, not dropped.
+Somebody wrote it meaning something, and a model that silently ignores it
+believes it is safer than it is.
+
+## Decision 3: self-clearance and obstacle clearance are reported apart
+
+`clearance()` returns the closest pair overall, and beside it `to_self` and
+`to_obstacles`.
+
+Returning only the overall minimum was the first design, and the tests killed
+it. A straight arm's own links sit a fixed distance apart -- **0.22 m** on the
+example, between the upper arm and the wrist -- so the overall minimum reports
+0.22 no matter where an obstacle is, until the obstacle comes nearer than the
+arm is to itself. An obstacle 30 cm away is then completely invisible to
+somebody asking about obstacles.
+
+That is not a bug in the arithmetic; the overall minimum was exactly the overall
+minimum. It is a question answered so literally that it stopped being useful.
+The two numbers cost one extra comparison each and answer the two questions
+people actually ask.
+
+## Decision 4: distances are signed
+
+Negative means overlapping, by the depth of the overlap, rather than clamped at
+zero. "Touching" and "driven 40 mm through" call for different responses, and a
+caller who only wants a yes or no compares against zero.
+
+When there is genuinely nothing to measure -- no obstacles, every link pair
+adjacent or excluded -- the answer is `kNothingNear`, a large finite number.
+Infinity is the honest answer and a terrible one to hand back, because it
+survives arithmetic that should have failed and reappears as a NaN much later,
+somewhere else.
+
+## Consequences
+
+- A clearance query costs **432 ns** for six links, three obstacles and nine
+  self pairs -- twenty-seven capsule distances, 0.043% of a millisecond cycle,
+  allocating nothing. It can therefore run as a gate on every setpoint rather
+  than as a planning-time check, which is the difference between catching a bad
+  move and catching a bad *command*.
+- The model is per-configuration. Nothing here checks the swept volume between
+  two configurations, so a step large enough to pass through a thin obstacle
+  passes through it undetected. Following a plan means sampling it densely
+  enough, and a test does exactly that along a Cartesian move.
+- Six links of capsules is a coarse robot. It will refuse poses a real machine
+  could hold. That is stated rather than hidden, and the fix is more capsules.
+- Nothing is wired into `CartesianPlan` automatically. Planning does not take
+  obstacles and does not report clearance, because the obstacle set changes far
+  more often than the arm does and baking it into the planner would mean
+  replanning to answer a question about a fence that moved.
+
+## Alternatives considered
+
+**Spheres only.** Simpler still -- distance between two spheres is one
+subtraction -- and a link needs five or six of them to be covered as well as one
+capsule covers it. The segment-segment distance is thirty lines and pays for
+itself immediately.
+
+**A full mesh with a broad phase.** What a production cell uses, and correct in
+a way capsules never are. It is also a bounding-volume hierarchy, a mesh loader
+and a format decision, none of which this library has any business owning.
+
+**Reporting every colliding pair rather than the closest.** A caller deciding
+whether to move needs one number. A caller diagnosing why gets the pair that
+produced it, which in practice is the one being argued about anyway.
+
+**Checking swept volumes between configurations.** The honest fix for the gap
+named above, and it needs a continuous-collision formulation -- conservative
+advancement, or a bound on how far the geometry moves per unit of path. Both are
+larger than this whole file.