[planning] Fast continuous collision checking for polynomial cspace trajectories - #24924
Open
wernerpe wants to merge 22 commits into
Open
[planning] Fast continuous collision checking for polynomial cspace trajectories#24924wernerpe wants to merge 22 commits into
wernerpe wants to merge 22 commits into
Conversation
Adds the shared option/verdict types, the numerical accounting used by the certificate arithmetic, and PiecewiseBezierPath: the exact normalization of BezierCurve, CompositeTrajectory, BsplineTrajectory (via knot insertion) and PiecewisePolynomial (via a monomial to Bernstein change of basis) into one piecewise-Bezier representation, plus the de Casteljau split the node recursion runs on. Ported from a standalone CMake package; this is a mechanical port (namespace, include paths, flattened directory layout) with no behavioral changes.
Adds ComputeBoundingSphere (a body-frame sphere containing a proximity geometry, over the closed set of supported shapes) and KinematicsEngine, the construction-time analysis that decides which joints can move a geometry pair and derives the per-pair motion-bound coefficients the displacement lemma consumes. Unsupported joint types, rotating half spaces and unbounded-reach models are refused by name at construction time rather than silently mis-bounded.
Adds DistanceOracle, the narrowphase abstraction the certifier queries, with a construction-time capability probe that classifies every unfiltered pair once (so no per-query dispatch decisions remain) and refuses the pairs whose signed distance Drake cannot report to the documented accuracy. Half-space pairs take a dedicated exact fallback. Also adds AddVPolytopeGeometry, the V-polytope ingestion helper the oracle tests use to build convex geometry from vertex data.
Adds the internal driver: the breakpoint pre-pass, the static-pair resolution, the adaptive de Casteljau node recursion that proves clearance over whole parameter intervals, and the parallel driver (a shared LIFO work source with occupancy-driven sharing and lazy recruitment) whose answers are identical to the serial path. Also adds the certificate audit trail and VerifyCertificate, an independent replay that re-derives every recorded certification event from the path and the recorded distances alone. The two live in one library because the replay and the recursion share the per-call data structures. The end-to-end tests for this code arrive with the public facade in the following commit.
Adds the public facade. It owns the construction-time analysis (pair enumeration, per-pair tolerances and padding, the kinematic tables) and offers CheckEdge/CheckTrajectory, which return a proof that the signed distance of every unfiltered pair stays above margin + padding over the entire continuous time domain, a witness configuration exactly on the trajectory, or an inconclusive verdict. Brings the remaining six suites with it: the certifier corpus, the certificate audit and mutation tests, the randomized soundness fuzz, the thin-obstacle regression that motivates the library, the concurrency determinism tests, and the API/UX clear-throw tests.
Adds the manual-only performance benchmark: iiwa14 in a bookcase at three swept-clearance tiers, a PWL edge, a dual-arm handover, the grazing pathological case, and thread scaling, each compared against Drake's own sampled SceneGraphCollisionChecker on the same RobotDiagram and each verified by dense sampling before it is measured. Results are written as JSON. The binary is tagged manual: a full run takes minutes and reports measurements rather than assertions.
The displacement lemma argues entirely in the separated regime, so the certificate is meaningless when margin + padding is negative for a pair. Rather than returning a verdict outside the proven regime, CheckEdge and CheckTrajectory now throw a message naming both bodies and pointing the caller at collision filtering, which is the sound way to exempt a pair that is meant to touch.
The constant-coordinate carve-out drops a coordinate from J(p) when its whole control-point range fits inside Options::continuity_tolerance. That is a tolerance, not an identity, so the dropped coordinate could still displace the pair by up to lambda-tilde * range -- small, but two orders of magnitude above Options::certificate_slack and charged nowhere, which let the certificate inequality pass with the true clearance below the threshold by that much. MotionBoundTable now carries a per-pair carveout_slack() that MotionBound() adds unconditionally, restoring the telescoping sum in full. It is bit- exactly zero whenever every carved coordinate is exactly constant, which is every path whose control points repeat the coordinate's value verbatim. The per-coordinate lambda-tilde reproduces the existing lambda for the supported joint kinds and extends to the kinds the carve-out admits but the table cannot otherwise bound: r for rpy/ball/universal rotations, 1 for floating-base translations, and a proved 2r/m for quaternion coefficients, with the Lipschitz and double-cover derivation recorded in the source. A HalfSpace across a rotational carved coordinate has no finite reach and so must now be exactly constant; anything else throws. The breakpoint/static-pair path in certifier.cc and the certificate replay in certificate.cc charge the same slack instead of a literal zero.
Renames the package directory planning/certified_ccd to
planning/continuous_collision, the namespace drake::planning::certified_ccd
to drake::planning::continuous_collision, and the class
CertifiedContinuousCollisionChecker to ContinuousCollisionChecker (with its
files and BUILD target renamed to match).
The "certified_ccd: " error-message prefix, which named the package rather
than the code that throws, is replaced by the owning class or function
("KinematicsEngine: ", "ComputeBoundingSphere(): ").
Placement and API: - Renames certifier.h to certifier_internal.h and excludes it from the installed headers. - Caps the worker pool at Parallelism::Max().num_threads() rather than std::thread::hardware_concurrency(), so it honours DRAKE_NUM_THREADS. - Adds @InGroup planning_collision_checker to every public type and free function. - Gives every class an explicit copy/move declaration: the value types are DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN, ContinuousCollisionChecker and the thread/context pools are DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN. - Replaces the local StrCat helper and the throwing paths' ostringstream and string concatenation with fmt::format. - Renames the benchmark's `benchmark` namespace to `internal`. - Narrows <Eigen/Dense> to <Eigen/Core> in the headers. Documentation: - PaddingSpec now documents the rule the code implements (env vs self is decided by anchoring; NaN matrix entries fall back to the scalars) instead of claiming to mirror CollisionChecker. - DistanceOracle documents that collision filters are snapshotted at construction. - ContinuousCollisionChecker documents that concurrent Check* calls from arbitrary threads are supported. - The world-rooted tree walk is described as an independent cross-check of GetBodiesKinematicallyAffectedBy(), which is what the code does. Style: - Converts internal-invariant DRAKE_THROW_UNLESS to DRAKE_DEMAND, leaving the user-facing checks as throws. - Replaces MotionBoundTable's four mutable_*() builder accessors with a validating constructor, and renames entries() to GetEntries(). - Makes DistanceOracle's members private and support_report() return a reference. - Drops redundant drake:: qualification inside namespace drake, removes unused includes and adds missing direct ones.
No Drake compute class owns persistent background threads; the upstream pattern is call-scoped fork-join. Replace internal::WorkerPool (parked threads owned by the checker, reserved through a Batch handle) with std::async(std::launch::async) futures collected in a local vector, the shape planning/iris/iris_from_clique_cover.cc uses. The lazy recruitment policy is structurally unchanged: the call still starts as a serial descent on the calling thread with sharing disabled and still hires N-1 helpers exactly once, when the run crosses the node threshold. Only the price of hiring changes, so the threshold moves with it: spawning a thread measures 34 us on the reference machine against ~6-7 us to notify a parked one, which pushes the break-even up about 4x, from 16 nodes to 64. A check that ends right after hiring loses at most a few hundred microseconds, and a check under 64 nodes never hires at all. The exception path is preserved exactly: every worker catches, aborts the work source, and the first exception is rethrown once all of them have finished. The futures are declared after everything their tasks reference so that ~future joins before those objects are destroyed, which is what the Batch destructor used to guarantee. The worker-count cap stays Parallelism::Max().num_threads(); with no pool left to bound, RunCertifier applies it to the requested width directly. Also rename certifier.cc to certifier_internal.cc so clang-format and cpplint treat certifier_internal.h as the related header again. Measured on the in-tree benchmark (--only profile, AMD 7950X3D): deep grazing workload at min_interval 1e-6 (12570 nodes) 5.9-6.1x at 16 threads against 6.2-6.4x pooled; the 1.6 ms difference is 0.51 ms of thread creation plus 0.64 ms of the 48 extra nodes the higher threshold runs serially. The 15-node PWL edge stays flat at 1.00-1.03x from Parallelism(1) to Parallelism(16), and the 146-node shelf check keeps a speedup above 1 at every width.
… flavors The suite was written against Release timings and a Release corpus, so several targets either failed or overran under the instrumented build flavors. Each fix is local to the flavor that needed it. Timing claims out of concurrency_test. DeepWorkloadIsFasterInParallel and SmallCheckIsNotSlowerInParallel are wall-clock claims in a file that is otherwise all equalities. Valgrind serializes threads, which inverts "parallel is faster than serial" and fails them for a reason unrelated to the driver. They move to concurrency_timing_test, which carries disable_in_compilation_mode_dbg and no_valgrind_tools; the determinism and invariance cases stay in concurrency_test and keep running everywhere, sanitizers included. The corpus, the deep workload and the option defaults they share now live in test/concurrency_test_utilities.h. The skip predicate also consults VALGRIND_OPTS, ASAN_OPTIONS, LSAN_OPTIONS, TSAN_OPTIONS and UBSAN_OPTIONS, since a tool that instruments at runtime is invisible to the compile-time tests it used to rely on (the same check limit_malloc.cc and gcs_trajectory_optimization_test.cc make). soundness_fuzz_test. Corpus composition was asserted as absolute counts, which pinned the test to a 200-case corpus. The floors become fractions of kNumCases, and kNumCases drops to 50 under sanitizers or memcheck, where the dense cross-check is one to two orders of magnitude slower. The floors still demand at least one case of every kind at that size, which a static_assert enforces. asan and lsan are excluded outright (the corpus is deliberately leaked), the timeout goes to "long", and the file's budget note now says which margins are Release-only. Smaller items: motion_bound_test gains a moderate timeout; certifier_test declares num_threads = 8 for the eight caller threads it spawns; the two temp_directory() calls and all three unchecked std::ofstream writes are gone, since the cube mesh is now Drake's shipped geometry/test/ quad_cube.obj scaled to the same half-extents through the non-uniform Mesh/Convex scale argument, and both L-prisms -- which encode analytic expectations no shipped asset matches -- are built as InMemoryMesh. The benchmark loses its manual tag and becomes testonly so CI compiles it, defaults --out to TEST_TMPDIR, and gains a two-second smoke test on the cheapest scenario. Verified: full suite green; concurrency_test clean under --config=tsan (concurrency_timing_test is filtered out there by its own no_tsan tag, as intended); soundness_fuzz_test passes its fractional assertions with the shrunk corpus forced on under both defines.
Binds the public surface of planning/continuous_collision into a pydrake.planning.continuous_collision submodule: the Options/Verdict/ SearchMode configuration surface, ContinuousCollisionChecker (+ Params) and VerifyCertificate, Certificate records, PiecewiseBezierPath, MotionBoundTable / KinematicsEngine, DistanceOracle, ComputeBoundingSphere, and AddVPolytopeObstacle. Unlike graph_algorithms and trajectory_optimization, which flatten into pydrake.planning because their type names are self-qualifying, this sub-namespace exports names -- Options, Statistics, Certificate, Finding, PairId -- that are only meaningful when namespace-qualified, so it gets its own submodule following the pydrake.geometry.optimization precedent. The two out-param signatures (DeCasteljauSplitAtHalf and DistanceOracle::SignedDistance) are bound as lambdas returning tuples, with the deviation from the C++ signature noted in their docstrings.
Rebased onto upstream/master 23e8561 (2026-08-27), ~1072 commits and ten months past the previous base. Purely mechanical adaptation to upstream API/tooling changes; no behavior was redesigned. Build rules (RobotLocomotion#24413, "Use opt_out_condition for excluding dynamic_analysis configurations"): - concurrency_timing_test: `disable_in_compilation_mode_dbg = True` plus `tags = ["no_valgrind_tools"]` are both gone; replaced by the single `opt_out_conditions = ["//tools:unoptimized"]`, which now expands to the sanitizers, dbg, kcov and every Valgrind tool -- the same set the two old spellings covered together. - soundness_fuzz_test: `tags = ["no_asan", "no_lsan"]` -> `opt_out_conditions = ["//tools/asan:enabled", "//tools/lsan:enabled"]`, and the small-corpus select key `//tools:using_memcheck` (deleted) -> `//tools/valgrind:enabled`. pydrake, aligning with the pybind11 -> nanobind port (RobotLocomotion#24840 and the "align with nanobind ... API" series). The module still builds under pybind11 by default; these spellings are the ones valid under both: - `py::module` -> `py::module_` in the definition and in planning_py.h. - `.def_readwrite` -> `.def_rw` (56 sites); `.def_property` -> `.def_prop_rw` (RobotLocomotion#24562, RobotLocomotion#24567). - `py::class_<T>` -> pydrake's own `class_<T>` alias (RobotLocomotion#24669), which also registers weak-referenceability and trampoline type aliases. - ContinuousCollisionChecker's kwargs constructor: `py::init(lambda)` -> a placement-new `.def("__init__", ...)`, since nanobind's `nb::init` takes types only (RobotLocomotion#24742, RobotLocomotion#24600); member `params_py.cast<T>()` -> free `py::cast<T>(params_py)` (RobotLocomotion#24583); and `py::arg("kwargs")` added under `#ifdef PYDRAKE_USE_NANOBIND`, which nanobind requires once any argument is named. - test/continuous_collision_test.py: `# ruff: isort: skip` on the `mut` import (RobotLocomotion#23672) and reformatted with the pinned ruff. Vendored docstrings: regenerated bindings/generated_docstrings/ planning_continuous_collision.h on the new base. The only change is upstream RobotLocomotion#24127, which rewrites `::` as U+2237 inside docstring prose; :diff_test passes. Regenerating the other 64 headers reproduced them byte-for-byte, confirming the toolchain matches. Reformatted planning_py_continuous_collision.cc with the repo's pinned clang-format (now 22.1.8) and planning/continuous_collision/BUILD.bazel with the pinned buildifier. Also declares pydrake/planning/continuous_collision.pyi in PYI_FILES. That entry was missing from the original bindings commit rather than being rebase drift, but stubgen fails without it for any module that calls def_submodule, exactly as upstream's own planning/experimental submodule shows.
wernerpe
commented
Aug 28, 2026
wernerpe
left a comment
wernerpe
left a comment
Contributor
Author
There was a problem hiding this comment.
+a:@TobiaMarcucci for feature review. thanks! I will post a clean writeup of the approach shortly.
@wernerpe made 1 comment.
Reviewable status: LGTM missing from assignee TobiaMarcucci, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on TobiaMarcucci).
…nings, dbg timeout) Installed headers. certifier.h sat in the srcs of :continuous_collision_checker, and drake_cc_library installs private headers found in srcs by default. It therefore landed in libdrake's drake_headers tree while the internal headers it includes (distance_oracle.h and friends) correctly did not, so mkdoc's parse of that tree died on a missing include and every platform failed to build //bindings/generated_docstrings:gen_planning_continuous_collision. It moves to hdrs with install_hdrs_exclude, the same shape multibody/parsing uses for detail_composite_parse.h; the package's only installed header is now continuous_collision_checker.h. The vendored docstrings are regenerated: they had drifted from the header, still carrying U+03C6 where the guarantee now reads U+03D5 and a two-line form for a doc comment that is one line today. Clang warnings. A lambda in motion_bound_test.cc captured a constexpr local it did not need, which -Wunused-lambda-capture rejects under the clang jobs' -Werror=all. Re-running every translation unit in the package, plus the pydrake bindings, through clang with the linux-clang job's exact flag set turned up no others. Dbg timeout. Deep() bisects for the margin whose certifying tree is largest within kProbeBudget nodes, and it used to bound each probe with Options::max_nodes. That option was withdrawn when the public API was minimized, so the budget became a post-hoc check on a run that had already finished, and the probes the search rejects -- roughly half of them, and the expensive half -- began exploring to exhaustion: up to 479k nodes each against the 1.5k the workload itself needs. Options::min_interval is the cost bound that remains, and the workload now simply inherits BaseOptions' 1e-6 rather than overriding it to 1e-8. The certifying side is unaffected (its tree is 19 levels deep, so it never approaches either floor) and the workload is unchanged at margin 0.101269 and 1479 nodes, while the rejected probes fall to about 5e3 nodes. DeepWorkloadIsThreadCountInvariant goes from 280.4 s to 7.1 s in a dbg build. The target also declares timeout = "moderate", since --config=debug and --config=lsan scale the short budget down, to 120 s and 72 s, rather than up.
…-resolution floor
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
I have been working on a fast collision checker that does continuous collision checks along polynomial trajectories. It uses certified collision-free c-space spheres (Quinlan 1994; Schwarzer, Saha & Latombe, IEEE T-RO 2005) and subdivision of the trajectory to rapidly certify entire trajectories. Let me know if there is any interest in this! I would be happy to type up more details if there are any takers for the feature review.
This change is