diff --git a/bindings/generated_docstrings/BUILD.bazel b/bindings/generated_docstrings/BUILD.bazel index 6043fcf5245e..c64ec2b7d5f4 100644 --- a/bindings/generated_docstrings/BUILD.bazel +++ b/bindings/generated_docstrings/BUILD.bazel @@ -50,6 +50,7 @@ _SUBDIRS = [ "multibody/tree", "perception", "planning", + "planning/continuous_collision", "planning/experimental", "planning/graph_algorithms", "planning/iris", diff --git a/bindings/generated_docstrings/planning_continuous_collision.h b/bindings/generated_docstrings/planning_continuous_collision.h new file mode 100644 index 000000000000..c0c8c63f0c45 --- /dev/null +++ b/bindings/generated_docstrings/planning_continuous_collision.h @@ -0,0 +1,301 @@ +#pragma once + +// GENERATED FILE DO NOT EDIT +// This file contains docstrings for the Python bindings that were +// automatically extracted by mkdoc.py. + +#include +#include + +#if defined(__GNUG__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif + +// #include "drake/planning/continuous_collision/continuous_collision_checker.h" + +// Symbol: pydrake_doc_planning_continuous_collision +constexpr struct /* pydrake_doc_planning_continuous_collision */ { + // Symbol: drake + struct /* drake */ { + // Symbol: drake::planning + struct /* planning */ { + // Symbol: drake::planning::continuous_collision + struct /* continuous_collision */ { + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker + struct /* ContinuousCollisionChecker */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Certifies, rather than samples, that a trajectory is collision-free +over its entire continuous time domain. + +Guarantee: if a check returns Verdict∷kCertifiedFree, then for every +time t in the trajectory's domain and every unfiltered geometry pair +(A, B), the signed distance ϕ_AB(q(t)) exceeds Options∷margin. That +holds under three assumptions: exact real arithmetic up to an internal +numerical slack, a distance oracle accurate to its stated tolerance, +and Mesh ≡ convex hull. The proof is a property of the path, so +retiming the trajectory afterwards does not invalidate it. + +Resolution contract: write δ for Options∷margin, r for +Options∷distance_resolution, τ_p for the oracle tolerance of pair p +(at least 1 µm; Drake's documented signed-distance accuracy for that +shape combination), ε for the internal slack (1 nm), and σ_p for the +residual motion of coordinates the trajectory holds constant only to +within the continuity tolerance (exactly zero when they are exactly +constant, the common case). Then, for every pair, - if ϕ_p(q(t)) > δ + +r + σ_p + 2τ_p + ε for every t, the pair is certified, so a trajectory +that clears the margin by that much everywhere returns +Verdict∷kCertifiedFree; - if ϕ_p(q(t)) < δ − (r + σ_p + 2τ_p) for some +t, the check returns Verdict∷kViolationFound; - Verdict∷kInconclusive +is therefore possible only when some pair's clearance comes within +that band of the margin, and its Finding then names an on-trajectory +configuration whose reported distance lies in [δ − τ_p, δ + τ_p + ε + +σ_p + r]. Resolutions below what double precision can represent along +a segment are capped by a floating-point backstop. + +Thread safety: the Check* methods are const, own no mutable state +outside per-call scratch, and may be called concurrently on one +instance from arbitrary threads. This is stronger than +planning∷CollisionChecker, whose documentation requires a per-thread +clone for use from threads the checker does not itself own; no clone +is needed here. Construction and destruction are not thread-safe.)"""; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::CheckEdge + struct /* CheckEdge */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Certifies the straight configuration-space edge q1 → q2. + +Raises: + RuntimeError if q1 or q2 does not have one entry per generalized + position of the plant. + +Raises: + RuntimeError under every condition CheckTrajectory() lists.)"""; + } CheckEdge; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::CheckPath + struct /* CheckPath */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Certifies the piecewise-linear path through the given waypoint +columns. + +Raises: + RuntimeError if ``waypoints`` has fewer than two columns, or does + not have one row per generalized position of the plant. + +Raises: + RuntimeError under every condition CheckTrajectory() lists.)"""; + } CheckPath; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::CheckTrajectory + struct /* CheckTrajectory */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Certifies a trajectory (BezierCurve, BsplineTrajectory, +PiecewisePolynomial, or a CompositeTrajectory of those). + +Raises: + RuntimeError if Options∷margin is not a finite nonnegative + distance, or if Options∷distance_resolution is not a finite + positive distance. + +Raises: + RuntimeError if the trajectory's row count differs from the + plant's number of generalized positions. + +Raises: + RuntimeError if the trajectory is not one of the supported types, + has a segment of degree above 10, or is discontinuous at a + junction. + +Raises: + RuntimeError if Options∷continuous_revolute_indices names a + coordinate outside the plant's. + +Raises: + RuntimeError if the trajectory moves a coordinate of an + unsupported joint type (quaternion floating, ball), or moves a + HalfSpace across a rotational coordinate.)"""; + } CheckTrajectory; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::ContinuousCollisionChecker + struct /* ctor */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Builds contexts, bounding spheres and topology tables, and runs the +capability probe. + +Raises: + RuntimeError if ``model`` is null or its plant is not finalized. + +Raises: + RuntimeError if ``default_options`` is invalid; see + CheckTrajectory(). + +Raises: + RuntimeError if a pair's shape combination is unsupported, i.e. a + deformable geometry or halfspace against halfspace. + +Raises: + RuntimeError if the plant's topology or geometry defeats the + motion bound: a rotating HalfSpace, a reversed joint, a kinematic + loop, or a proximity shape with no bounding sphere.)"""; + } ctor; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::model + struct /* model */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } model; + } ContinuousCollisionChecker; + // Symbol: drake::planning::continuous_collision::Finding + struct /* Finding */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Where the plan fails, or where it could not be decided.)"""; + // Symbol: drake::planning::continuous_collision::Finding::body_a + struct /* body_a */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } body_a; + // Symbol: drake::planning::continuous_collision::Finding::body_b + struct /* body_b */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } body_b; + // Symbol: drake::planning::continuous_collision::Finding::distance + struct /* distance */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""(Signed distance of the pair at q.)"""; + } distance; + // Symbol: drake::planning::continuous_collision::Finding::geometry_a + struct /* geometry_a */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } geometry_a; + // Symbol: drake::planning::continuous_collision::Finding::geometry_b + struct /* geometry_b */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } geometry_b; + // Symbol: drake::planning::continuous_collision::Finding::nearest_a_W + struct /* nearest_a_W */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Closest points in the world frame at q; present for violations, so +that planners can push the trajectory out of collision.)"""; + } nearest_a_W; + // Symbol: drake::planning::continuous_collision::Finding::nearest_b_W + struct /* nearest_b_W */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } nearest_b_W; + // Symbol: drake::planning::continuous_collision::Finding::q + struct /* q */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(The witness configuration, exactly on the trajectory.)"""; + } q; + // Symbol: drake::planning::continuous_collision::Finding::time + struct /* time */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Trajectory time of the witness configuration.)"""; + } time; + } Finding; + // Symbol: drake::planning::continuous_collision::Options + struct /* Options */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""(Options controlling one check.)"""; + // Symbol: drake::planning::continuous_collision::Options::continuous_revolute_indices + struct /* continuous_revolute_indices */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Position coordinates whose junction continuity is checked modulo 2π +(the GcsTrajectoryOptimization continuous-revolute convention). + +See also: + planning∷trajectory_optimization∷GetContinuousRevoluteJointIndices)"""; + } continuous_revolute_indices; + // Symbol: drake::planning::continuous_collision::Options::distance_resolution + struct /* distance_resolution */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Resolution floor r in meters. A pair stops being refined on a node +once its bounded relative motion over that node is at most r; if it is +still undecided there, the check reports Verdict∷kInconclusive with +that node's midpoint as the witness. Definitive verdicts are +guaranteed for a trajectory whose clearance stays more than r (plus +the oracle tolerance, see the class documentation) away from the +margin everywhere; the cost of a grazing trajectory grows roughly +linearly in 1/r. Must be finite and positive.)"""; + } distance_resolution; + // Symbol: drake::planning::continuous_collision::Options::margin + struct /* margin */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Clearance margin δ in meters: the check certifies signed distance > +margin for every unfiltered pair at every time. Must be finite and +nonnegative.)"""; + } margin; + // Symbol: drake::planning::continuous_collision::Options::parallelism + struct /* parallelism */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } parallelism; + } Options; + // Symbol: drake::planning::continuous_collision::Result + struct /* Result */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""(Result of one check.)"""; + // Symbol: drake::planning::continuous_collision::Result::finding + struct /* finding */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(The earliest violation, or the inconclusive witness; empty iff the +verdict is Verdict∷kCertifiedFree.)"""; + } finding; + // Symbol: drake::planning::continuous_collision::Result::num_nodes + struct /* num_nodes */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Nodes visited by the adaptive subdivision; a cost measure.)"""; + } num_nodes; + // Symbol: drake::planning::continuous_collision::Result::verdict + struct /* verdict */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } verdict; + } Result; + // Symbol: drake::planning::continuous_collision::Verdict + struct /* Verdict */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""(Outcome of one check.)"""; + // Symbol: drake::planning::continuous_collision::Verdict::kCertifiedFree + struct /* kCertifiedFree */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Proof: every unfiltered pair keeps signed distance > margin over the +entire continuous time domain.)"""; + } kCertifiedFree; + // Symbol: drake::planning::continuous_collision::Verdict::kInconclusive + struct /* kInconclusive */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Some pair's clearance comes within Options∷distance_resolution (plus +the oracle tolerance) of the margin, so refining further cannot decide +it: the trajectory grazes the margin.)"""; + } kInconclusive; + // Symbol: drake::planning::continuous_collision::Verdict::kViolationFound + struct /* kViolationFound */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(An exactly-on-trajectory configuration violates the threshold.)"""; + } kViolationFound; + } Verdict; + } continuous_collision; + } planning; + } drake; +} pydrake_doc_planning_continuous_collision; + +#if defined(__GNUG__) +#pragma GCC diagnostic pop +#endif diff --git a/bindings/pydrake/BUILD.bazel b/bindings/pydrake/BUILD.bazel index 1a94f861234b..055f7128ac1f 100644 --- a/bindings/pydrake/BUILD.bazel +++ b/bindings/pydrake/BUILD.bazel @@ -449,6 +449,7 @@ PYI_FILES = [ "pydrake/multibody/tree.pyi", "pydrake/perception.pyi", "pydrake/planning/__init__.pyi", + "pydrake/planning/continuous_collision.pyi", "pydrake/planning/experimental.pyi", "pydrake/polynomial.pyi", "pydrake/solvers.pyi", diff --git a/bindings/pydrake/planning/BUILD.bazel b/bindings/pydrake/planning/BUILD.bazel index 6e1429689a03..df4c75885ecd 100644 --- a/bindings/pydrake/planning/BUILD.bazel +++ b/bindings/pydrake/planning/BUILD.bazel @@ -25,6 +25,7 @@ drake_pybind_library( name = "planning", cc_deps = [ "//bindings/generated_docstrings:planning", + "//bindings/generated_docstrings:planning_continuous_collision", "//bindings/generated_docstrings:planning_experimental", "//bindings/generated_docstrings:planning_graph_algorithms", "//bindings/generated_docstrings:planning_iris", @@ -43,6 +44,7 @@ drake_pybind_library( "planning_py.cc", "planning_py_collision_checker.cc", "planning_py_collision_checker_interface_types.cc", + "planning_py_continuous_collision.cc", "planning_py_dof_mask.cc", "planning_py_experimental_placeholder.cc", "planning_py_graph_algorithms.cc", @@ -102,6 +104,14 @@ drake_py_unittest( ], ) +drake_py_unittest( + name = "continuous_collision_test", + num_threads = 2, + deps = [ + ":planning", + ], +) + drake_py_unittest( name = "dof_mask_test", deps = [ diff --git a/bindings/pydrake/planning/planning_py.cc b/bindings/pydrake/planning/planning_py.cc index 481830bac498..651de0d31eee 100644 --- a/bindings/pydrake/planning/planning_py.cc +++ b/bindings/pydrake/planning/planning_py.cc @@ -35,6 +35,12 @@ and/or trajectories of dynamical systems. internal::DefinePlanningIrisZo(m); internal::DefinePlanningIrisFromCliqueCover(m); internal::DefinePlanningZmpPlanner(m); + // The continuous_collision C++ sub-namespace gets its own Python submodule + // (mirroring pydrake.geometry.optimization) because its type names -- + // Options, Statistics, Certificate, Finding, PairId -- are only meaningful + // when namespace-qualified, and would pollute pydrake.planning if flattened. + internal::DefinePlanningContinuousCollision( + m.def_submodule("continuous_collision")); // Experimental modules. auto experimental = m.def_submodule("experimental"); diff --git a/bindings/pydrake/planning/planning_py.h b/bindings/pydrake/planning/planning_py.h index 32ebef249f39..18129e867a11 100644 --- a/bindings/pydrake/planning/planning_py.h +++ b/bindings/pydrake/planning/planning_py.h @@ -20,6 +20,9 @@ void DefinePlanningCollisionChecker(py::module_ m); /* Defines bindings per planning_py_collision_checker_interface_types.cc. */ void DefinePlanningCollisionCheckerInterfaceTypes(py::module_ m); +/* Defines bindings per planning_py_continuous_collision.cc. */ +void DefinePlanningContinuousCollision(py::module_ m); + /* Defines bindings per planning_py_dof_mask.cc. */ void DefinePlanningDofMask(py::module_ m); diff --git a/bindings/pydrake/planning/planning_py_continuous_collision.cc b/bindings/pydrake/planning/planning_py_continuous_collision.cc new file mode 100644 index 000000000000..a27086470239 --- /dev/null +++ b/bindings/pydrake/planning/planning_py_continuous_collision.cc @@ -0,0 +1,121 @@ +#include + +#include "drake/bindings/generated_docstrings/planning_continuous_collision.h" +#include "drake/bindings/pydrake/planning/planning_py.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace pydrake { +namespace internal { + +void DefinePlanningContinuousCollision(py::module_ m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::planning::continuous_collision; + constexpr auto& doc = pydrake_doc_planning_continuous_collision.drake.planning + .continuous_collision; + + using drake::planning::RobotDiagram; + + m.doc() = R"""( +Certified continuous collision checking: proves that a trajectory is +collision-free over its entire continuous time domain, rather than sampling it. +)"""; + + { + using Class = Verdict; + constexpr auto& cls_doc = doc.Verdict; + py::enum_(m, "Verdict", cls_doc.doc) + .value( + "kCertifiedFree", Class::kCertifiedFree, cls_doc.kCertifiedFree.doc) + .value("kViolationFound", Class::kViolationFound, + cls_doc.kViolationFound.doc) + .value( + "kInconclusive", Class::kInconclusive, cls_doc.kInconclusive.doc); + } + + { + using Class = Finding; + constexpr auto& cls_doc = doc.Finding; + class_ cls(m, "Finding", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_rw("time", &Class::time, cls_doc.time.doc) + .def_rw("q", &Class::q, cls_doc.q.doc) + .def_rw("geometry_a", &Class::geometry_a, cls_doc.geometry_a.doc) + .def_rw("geometry_b", &Class::geometry_b, cls_doc.geometry_b.doc) + .def_rw("body_a", &Class::body_a, cls_doc.body_a.doc) + .def_rw("body_b", &Class::body_b, cls_doc.body_b.doc) + .def_rw("distance", &Class::distance, cls_doc.distance.doc) + .def_rw("nearest_a_W", &Class::nearest_a_W, cls_doc.nearest_a_W.doc) + .def_rw("nearest_b_W", &Class::nearest_b_W, cls_doc.nearest_b_W.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = Options; + constexpr auto& cls_doc = doc.Options; + class_ cls(m, "Options", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_rw("margin", &Class::margin, cls_doc.margin.doc) + .def_rw("distance_resolution", &Class::distance_resolution, + cls_doc.distance_resolution.doc) + .def_rw("continuous_revolute_indices", + &Class::continuous_revolute_indices, + cls_doc.continuous_revolute_indices.doc) + .def_rw("parallelism", &Class::parallelism, cls_doc.parallelism.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = Result; + constexpr auto& cls_doc = doc.Result; + class_ cls(m, "Result", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_rw("verdict", &Class::verdict, cls_doc.verdict.doc) + .def_rw("finding", &Class::finding, cls_doc.finding.doc) + .def_rw("num_nodes", &Class::num_nodes, cls_doc.num_nodes.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = ContinuousCollisionChecker; + constexpr auto& cls_doc = doc.ContinuousCollisionChecker; + class_ cls(m, "ContinuousCollisionChecker", cls_doc.doc); + cls // BR + .def( + "__init__", + [](Class* self, py::object model, const Options& default_options) { + // For lifetime management, add a python reference to model + // (owned by the shared pointer) and transfer that to the c++ + // checker. + new (self) Class( + make_shared_ptr_from_py_object>(model), + default_options); + }, + py::kw_only(), py::arg("model"), + py::arg("default_options") = Options{}, cls_doc.ctor.doc) + .def("CheckTrajectory", &Class::CheckTrajectory, py::arg("trajectory"), + py::arg("options") = std::nullopt, + py::call_guard(), + cls_doc.CheckTrajectory.doc) + .def("CheckPath", &Class::CheckPath, py::arg("waypoints"), + py::arg("options") = std::nullopt, + py::call_guard(), cls_doc.CheckPath.doc) + .def("CheckEdge", &Class::CheckEdge, py::arg("q1"), py::arg("q2"), + py::arg("options") = std::nullopt, + py::call_guard(), cls_doc.CheckEdge.doc) + .def("model", &Class::model, py_rvp::reference_internal, + cls_doc.model.doc); + } +} + +} // namespace internal +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/planning/test/continuous_collision_test.py b/bindings/pydrake/planning/test/continuous_collision_test.py new file mode 100644 index 000000000000..83778cdea5f0 --- /dev/null +++ b/bindings/pydrake/planning/test/continuous_collision_test.py @@ -0,0 +1,156 @@ +import pydrake.planning.continuous_collision as mut # ruff: isort: skip + +import unittest + +import numpy as np + +from pydrake.common import Parallelism +from pydrake.geometry import Box, Sphere +from pydrake.math import RigidTransform +from pydrake.multibody.plant import CoulombFriction +from pydrake.multibody.tree import ( + FixedOffsetFrame, + PrismaticJoint, + RevoluteJoint, + SpatialInertia, + UnitInertia, +) +from pydrake.planning import RobotDiagramBuilder +from pydrake.trajectories import PiecewisePolynomial + + +def _inertia(): + return SpatialInertia( + mass=1.0, + p_PScm_E=np.zeros(3), + G_SP_E=UnitInertia(Ixx=1.0, Iyy=1.0, Izz=1.0), + ) + + +def _friction(): + return CoulombFriction(1.0, 1.0) + + +def _make_model(): + """A planar 2-dof arm (revolute shoulder, then prismatic slide carrying a + tool sphere) with one anchored post at (0, 0.60, 0). q = (theta, slide); + the tool sits at the post's center at q = (pi/2, 0.30). + """ + builder = RobotDiagramBuilder() + plant = builder.plant() + link = plant.AddRigidBody(name="link", M_BBo_B=_inertia()) + tool = plant.AddRigidBody(name="tool", M_BBo_B=_inertia()) + plant.AddJoint( + RevoluteJoint( + name="shoulder", + frame_on_parent=plant.world_frame(), + frame_on_child=link.body_frame(), + axis=[0, 0, 1], + ) + ) + slide_frame = plant.AddFrame( + FixedOffsetFrame( + name="slide_offset", + P=link.body_frame(), + X_PF=RigidTransform([0.30, 0.0, 0.0]), + ) + ) + plant.AddJoint( + PrismaticJoint( + name="slide", + frame_on_parent=slide_frame, + frame_on_child=tool.body_frame(), + axis=[1, 0, 0], + ) + ) + plant.RegisterCollisionGeometry( + body=link, + X_BG=RigidTransform([0.15, 0.0, 0.0]), + shape=Box(0.30, 0.05, 0.05), + name="link_geom", + coulomb_friction=_friction(), + ) + plant.RegisterCollisionGeometry( + body=tool, + X_BG=RigidTransform(), + shape=Sphere(0.04), + name="tool_geom", + coulomb_friction=_friction(), + ) + post = plant.AddRigidBody(name="post", M_BBo_B=_inertia()) + plant.WeldFrames( + frame_on_parent_F=plant.world_frame(), + frame_on_child_M=post.body_frame(), + X_FM=RigidTransform([0.0, 0.60, 0.0]), + ) + plant.RegisterCollisionGeometry( + body=post, + X_BG=RigidTransform(), + shape=Sphere(0.08), + name="post_geom", + coulomb_friction=_friction(), + ) + return builder.Build() + + +class TestContinuousCollision(unittest.TestCase): + def setUp(self): + self.model = _make_model() + options = mut.Options() + options.parallelism = Parallelism(num_threads=1) + self.checker = mut.ContinuousCollisionChecker( + model=self.model, default_options=options + ) + + def test_options_round_trip(self): + dut = mut.Options() + self.assertEqual(dut.margin, 0.0) + self.assertEqual(dut.distance_resolution, 1e-6) + dut.margin = 0.01 + dut.distance_resolution = 1e-4 + dut.continuous_revolute_indices = [0] + dut.parallelism = Parallelism(num_threads=2) + self.assertEqual(dut.margin, 0.01) + self.assertEqual(dut.distance_resolution, 1e-4) + self.assertEqual(dut.continuous_revolute_indices, [0]) + self.assertEqual(dut.parallelism.num_threads(), 2) + self.assertIsInstance(mut.Options(margin=0.02), mut.Options) + + def test_model(self): + self.assertIs(self.checker.model(), self.model) + + def test_free_edge(self): + result = self.checker.CheckEdge(q1=[0.0, 0.0], q2=[0.0, 0.2]) + self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) + self.assertIsNone(result.finding) + self.assertGreater(result.num_nodes, 0) + + def test_colliding_edge(self): + result = self.checker.CheckEdge(q1=[0.0, 0.0], q2=[np.pi / 2, 0.30]) + self.assertEqual(result.verdict, mut.Verdict.kViolationFound) + finding = result.finding + self.assertIsInstance(finding, mut.Finding) + self.assertLess(finding.distance, 0.0) + self.assertEqual(finding.q.shape, (2,)) + self.assertIsNotNone(finding.geometry_a) + self.assertIsNotNone(finding.body_a) + self.assertEqual(finding.nearest_a_W.shape, (3,)) + self.assertEqual(finding.nearest_b_W.shape, (3,)) + + def test_trajectory_and_path(self): + trajectory = PiecewisePolynomial.FirstOrderHold( + breaks=[0.0, 1.0], samples=np.array([[0.0, 0.0], [0.0, 0.2]]) + ) + options = mut.Options(margin=0.01) + result = self.checker.CheckTrajectory( + trajectory=trajectory, options=options + ) + self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) + result = self.checker.CheckPath( + waypoints=np.array([[0.0, 0.0, 0.0], [0.0, 0.1, 0.2]]) + ) + self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) + + def test_throw(self): + with self.assertRaisesRegex(RuntimeError, "generalized positions"): + self.checker.CheckEdge(q1=[0.0], q2=[0.0]) diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel new file mode 100644 index 000000000000..f000a860659f --- /dev/null +++ b/planning/continuous_collision/BUILD.bazel @@ -0,0 +1,333 @@ +load("//tools/lint:lint.bzl", "add_lint_tests") +load( + "//tools/skylark:drake_cc.bzl", + "drake_cc_googletest", + "drake_cc_library", + "drake_cc_package_library", +) + +package(default_visibility = ["//visibility:public"]) + +drake_cc_package_library( + name = "continuous_collision", + visibility = ["//visibility:public"], + deps = [":continuous_collision_checker"], +) + +# Numerical policy, shape classification and the pair record, shared by every +# translation unit here. +drake_cc_library( + name = "internal", + hdrs = ["internal.h"], + internal = True, + visibility = ["//visibility:private"], + deps = [ + "//common:unused", + "//geometry:geometry_ids", + "//geometry:shape_specification", + "//multibody/tree:multibody_tree_indexes", + ], +) + +drake_cc_library( + name = "piecewise_bezier_path", + srcs = ["piecewise_bezier_path.cc"], + hdrs = ["piecewise_bezier_path.h"], + internal = True, + visibility = ["//visibility:private"], + deps = [ + "//common:essential", + "//common/trajectories:trajectory", + "@eigen", + ], + implementation_deps = [ + ":internal", + "//common:nice_type_name", + "//common/trajectories:bezier_curve", + "//common/trajectories:bspline_trajectory", + "//common/trajectories:composite_trajectory", + "//common/trajectories:piecewise_polynomial", + "//math:binomial_coefficient", + "@fmt", + ], +) + +# The kinematic analysis: bounding spheres, J(p) and the lambda table. +drake_cc_library( + name = "motion_bound_table", + srcs = ["motion_bound_table.cc"], + hdrs = ["motion_bound_table.h"], + internal = True, + visibility = ["//visibility:private"], + deps = [ + ":internal", + ":piecewise_bezier_path", + "//common:essential", + "//geometry:geometry_ids", + "//math:geometric_transform", + "//multibody/plant", + "//multibody/tree:multibody_tree_indexes", + "//planning:robot_diagram", + "@eigen", + ], + implementation_deps = [ + "//geometry:geometry_roles", + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//geometry/proximity:polygon_surface_mesh", + "//multibody/tree", + "@fmt", + ], +) + +drake_cc_library( + name = "distance_oracle", + srcs = ["distance_oracle.cc"], + hdrs = ["distance_oracle.h"], + internal = True, + visibility = ["//visibility:private"], + deps = [ + ":internal", + "//common:essential", + "//geometry:scene_graph", + "//planning:robot_diagram", + "@eigen", + ], + implementation_deps = [ + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//geometry/proximity:polygon_surface_mesh", + "//math:geometric_transform", + "//multibody/plant", + "@fmt", + ], +) + +# The public facade plus the node recursion it drives. certifier.{h,cc} are +# private to this target: certifier.h names the public Options/Result types, so +# it cannot live in a library the facade depends on. It is listed in hdrs +# rather than srcs so that install_hdrs_exclude can keep it out of the install +# tree; a private header left in srcs is installed by default, and the internal +# headers it includes are not, so mkdoc would fail to parse it there. +drake_cc_library( + name = "continuous_collision_checker", + srcs = [ + "certifier.cc", + "continuous_collision_checker.cc", + ], + hdrs = [ + "certifier.h", + "continuous_collision_checker.h", + ], + install_hdrs_exclude = ["certifier.h"], + deps = [ + "//common:essential", + "//common:parallelism", + "//common/trajectories:trajectory", + "//geometry:geometry_ids", + "//multibody/tree:multibody_tree_indexes", + "//planning:robot_diagram", + "@eigen", + ], + implementation_deps = [ + ":distance_oracle", + ":internal", + ":motion_bound_table", + ":piecewise_bezier_path", + "//geometry:scene_graph", + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/plant", + "//planning:collision_checker_context", + "@fmt", + ], +) + +# === test/ === + +# The helpers the tests share: seeded random primitives and surface samplers, +# the throw-message probe, the random world generator two corpora are built +# from, and the corpus plus deep workload concurrency_test.cc pins the driver's +# determinism against. +drake_cc_library( + name = "test_utilities", + testonly = 1, + hdrs = ["test/test_utilities.h"], + deps = [ + ":continuous_collision_checker", + ":distance_oracle", + "//common:parallelism", + "//common/trajectories:bezier_curve", + "//geometry:scene_graph", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/plant", + "//multibody/tree", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + "@googletest//:gtest", + ], +) + +# Curve module acceptance tests. +drake_cc_googletest( + name = "piecewise_bezier_path_test", + deps = [ + ":internal", + ":piecewise_bezier_path", + "//common:copyable_unique_ptr", + "//common/test_utilities:expect_throws_message", + "//common/test_utilities:limit_malloc", + "//common/trajectories:bezier_curve", + "//common/trajectories:bspline_trajectory", + "//common/trajectories:composite_trajectory", + "//common/trajectories:piecewise_polynomial", + "//math:bspline_basis", + ], +) + +# The displacement lemma, the lambda table and the J(p) subtree logic. +drake_cc_googletest( + name = "motion_bound_test", + timeout = "moderate", + deps = [ + ":motion_bound_table", + ":test_utilities", + "//common/test_utilities:expect_throws_message", + "//geometry:geometry_roles", + "//geometry:scene_graph_inspector", + "//multibody/tree", + "@fmt", + ], +) + +# The bounding-sphere containment property test. +drake_cc_googletest( + name = "bounding_sphere_test", + deps = [ + ":motion_bound_table", + ":test_utilities", + "//common:memory_file", + "//common/test_utilities:expect_throws_message", + "//geometry:in_memory_mesh", + "//geometry:shape_specification", + "//geometry/proximity:polygon_surface_mesh", + "//math:geometric_transform", + ], +) + +# Oracle accuracy, probe classification and the analytic half-space fallback. +drake_cc_googletest( + name = "distance_oracle_test", + data = ["//geometry:test_obj_files"], + deps = [ + ":distance_oracle", + "//common:find_resource", + "//common:memory_file", + "//common/test_utilities:expect_throws_message", + "//geometry:geometry_instance", + "//geometry:in_memory_mesh", + "//geometry:proximity_properties", + "//geometry:scene_graph", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/fem:deformable_body_config", + "//multibody/plant", + "//multibody/tree:spatial_inertia", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + ], +) + +# Certifier semantics, including retiming invariance and the standing soundness +# guard, on a focused, hand-built corpus. +drake_cc_googletest( + name = "certifier_test", + deps = [ + ":piecewise_bezier_path", + ":test_utilities", + ], +) + +# The randomized soundness fuzz: random worlds x random trajectories, +# cross-checked against dense sampling. The dense cross-check (~1e7 +# signed-distance queries) is what makes this test long rather than the +# certification itself. +# +# Under an instrumented build that cross-check is what blows the budget, so +# the corpus shrinks to a quarter of its size there (the assertions are +# fractions of kNumCases and hold either way; see soundness_fuzz_test.cc). +# asan and lsan are excluded outright: they slow the dense sweep by more than +# the quarter corpus recovers. +drake_cc_googletest( + name = "soundness_fuzz_test", + opt_out_conditions = [ + "//tools/asan:enabled", + "//tools/lsan:enabled", + ], + timeout = "long", + # The two settings are mutually exclusive (each dynamic-analysis config + # matches exactly one of them), so this select is unambiguous. + defines = select({ + "//tools/valgrind:enabled": ["DRAKE_CCD_FUZZ_SMALL_CORPUS"], + "//tools:using_sanitizer": ["DRAKE_CCD_FUZZ_SMALL_CORPUS"], + "//conditions:default": [], + }), + deps = [ + ":distance_oracle", + ":internal", + ":piecewise_bezier_path", + ":test_utilities", + "//common/trajectories:bspline_trajectory", + "//common/trajectories:piecewise_polynomial", + "//geometry:scene_graph_inspector", + "//math:bspline_basis", + ], +) + +# Regression on obstacles a sampled checker steps over: a plate thinner than +# SceneGraphCollisionChecker's default edge_step_size, and the millimetre-scale +# gap that must still certify free. +drake_cc_googletest( + name = "thin_obstacle_test", + deps = [ + ":test_utilities", + "//planning:collision_checker_params", + "//planning:scene_graph_collision_checker", + ], +) + +# Concurrency determinism. Running with many threads is the point of this +# test: it pins the answer at Parallelism {1, 2, 8, 16}. Every case is an +# equality, so this target runs under every build flavor, sanitizers included. +# +# "Every build flavor" is also why the timeout is moderate rather than the +# default short: --config=debug and --config=lsan scale the short budget to +# 120 s and 72 s respectively, and building the deep workload costs a fraction +# of a second optimized but seconds at -O0 (see test_utilities.h). +drake_cc_googletest( + name = "concurrency_test", + timeout = "moderate", + num_threads = 16, + deps = [ + ":test_utilities", + "//common:parallelism", + ], +) + +# API / UX clear-throw tests. +drake_cc_googletest( + name = "api_test", + deps = [ + ":test_utilities", + "//common/test_utilities:expect_throws_message", + "//geometry:geometry_instance", + "//geometry:proximity_properties", + "//multibody/fem:deformable_body_config", + "//multibody/plant", + "//multibody/tree", + ], +) + +add_lint_tests() diff --git a/planning/continuous_collision/certifier.cc b/planning/continuous_collision/certifier.cc new file mode 100644 index 000000000000..55abced6196d --- /dev/null +++ b/planning/continuous_collision/certifier.cc @@ -0,0 +1,865 @@ +#include "drake/planning/continuous_collision/certifier.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/parallelism.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::geometry::QueryObject; +using drake::math::RigidTransformd; +using drake::multibody::BodyIndex; + +constexpr double kInfinity = std::numeric_limits::infinity(); + +/* Global (trajectory) time of parameter s in `seg`. Segment times are pure + bookkeeping: the recursion runs in the segment parameter s ∈ [0, 1] and only + the *reported* times go through this map, which is why the proof is invariant + under time reparametrization. */ +double TimeOf(const BezierSegment& seg, double s) { + return seg.t_start + s * (seg.t_end - seg.t_start); +} + +Finding MakeFinding(double time, const Eigen::VectorXd& q, + const PairRecord& pair, double distance, + const Eigen::Vector3d& nearest_a_W, + const Eigen::Vector3d& nearest_b_W) { + Finding finding; + finding.time = time; + finding.q = q; + finding.geometry_a = pair.a; + finding.geometry_b = pair.b; + finding.body_a = pair.body_a; + finding.body_b = pair.body_b; + finding.distance = distance; + finding.nearest_a_W = nearest_a_W; + finding.nearest_b_W = nearest_b_W; + return finding; +} + +/* Caches one world-frame bounding-sphere center per geometry per node. The + poses behind them are pulled lazily from Drake's FK cache and only for + geometries of still-active pairs. Invalidation is a stamp bump, so switching + to a new configuration is O(1). */ +class GeometryCache { + public: + GeometryCache(const PrefilterTable& table, const ThreadContext& context) + : table_(&table), + context_(&context), + center_W_(table.geometries.size()), + stamp_of_(table.geometries.size(), 0) {} + + /* Invalidates every cached center; call once per configuration. */ + void NewConfiguration() { ++stamp_; } + + const Eigen::Vector3d& Center(int slot) { + if (stamp_of_[slot] != stamp_) { + const PrefilterTable::Geometry& g = table_->geometries[slot]; + center_W_[slot] = context_->EvalBodyPose(g.body) * g.center_L; + stamp_of_[slot] = stamp_; + } + return center_W_[slot]; + } + + /* The free-sphere lower bound on the pair's signed distance at the + configuration last set: phi >= ||c_A - c_B|| - rho_A - rho_B. */ + double LowerBound(int slot_a, int slot_b) { + return (Center(slot_a) - Center(slot_b)).norm() - + table_->geometries[slot_a].radius - + table_->geometries[slot_b].radius; + } + + private: + const PrefilterTable* table_{}; + const ThreadContext* context_{}; + std::vector center_W_; + std::vector stamp_of_; + std::uint64_t stamp_{1}; +}; + +/* Collects the earliest violation and the earliest inconclusive witness from + every worker. Cold path: guarded by one mutex. */ +class FindingSink { + public: + void AddDefinite(Finding finding) { + const double time = finding.time; + { + std::lock_guard guard(mutex_); + if (!definite_.has_value() || time < definite_->time) { + definite_ = std::move(finding); + } + } + // Branch-and-bound bound: workers skip nodes whose interval starts at or + // after the earliest witness known so far. The bound decreases + // monotonically, so a node that could hold an earlier witness is never + // pruned and the answer does not depend on timing. + double previous = best_violation_time_.load(std::memory_order_relaxed); + while (time < previous && !best_violation_time_.compare_exchange_weak( + previous, time, std::memory_order_relaxed)) { + } + } + + void AddInconclusive(Finding finding) { + std::lock_guard guard(mutex_); + if (!inconclusive_.has_value() || finding.time < inconclusive_->time) { + inconclusive_ = std::move(finding); + } + } + + double best_violation_time() const { + return best_violation_time_.load(std::memory_order_relaxed); + } + + std::optional& definite() { return definite_; } + std::optional& inconclusive() { return inconclusive_; } + + private: + std::mutex mutex_; + std::optional definite_; + std::optional inconclusive_; + std::atomic best_violation_time_{kInfinity}; +}; + +/* One unit of shared work: a node, self-contained so a worker can pick it up + without touching any other worker's arenas. + + Work items carry copies (control points and the active-pair span) rather than + pointing into the producing worker's arenas, because the producer walks on + immediately. The steady-state loop still allocates nothing, because the queue + recycles item *shells*: a popped shell goes back on a free list and is handed + to the next producer, whose `resize`/`assign` then reuse the buffers already + attached to it. Allocation happens while the free list is filling up and never + again. */ +struct WorkItem { + int segment{}; + double s_lo{0.0}; + double s_hi{1.0}; + Eigen::MatrixXd control_points; + std::vector active; +}; + +/* Mutex-guarded LIFO work source with quiescence detection, shell recycling + and the occupancy counter that drives the sharing policy. The *only* shared + mutable state of the parallel driver is this queue, the FindingSink, and the + atomic node counter, which is what makes the driver TSan-clean by + construction. */ +class WorkQueue { + public: + /* Moves `*item` into the queue and hands back a recycled shell (or an empty + one) so the producer can fill it again without allocating. */ + void Push(WorkItem* item) { + { + std::lock_guard guard(mutex_); + items_.push_back(std::move(*item)); + if (free_.empty()) { + *item = WorkItem{}; + } else { + *item = std::move(free_.back()); + free_.pop_back(); + } + size_.store(static_cast(items_.size()), std::memory_order_relaxed); + } + condition_.notify_one(); + } + + /* Blocks until an item is available, or until every worker is idle and the + queue is empty (returns false), or until Abort() (returns false). `*item`'s + previous contents are recycled into the free list. */ + bool Pop(WorkItem* item) { + std::unique_lock lock(mutex_); + while (true) { + if (aborted_ || done_) return false; + if (!items_.empty()) { + free_.push_back(std::move(*item)); + *item = std::move(items_.back()); + items_.pop_back(); + size_.store(static_cast(items_.size()), std::memory_order_relaxed); + ++busy_; + return true; + } + if (busy_ == 0) { + done_ = true; + condition_.notify_all(); + return false; + } + condition_.wait(lock); + } + } + + void FinishItem() { + { + std::lock_guard guard(mutex_); + --busy_; + if (busy_ > 0 && items_.empty()) return; + } + condition_.notify_all(); + } + + void Abort() { + { + std::lock_guard guard(mutex_); + aborted_ = true; + } + condition_.notify_all(); + } + + /* The sharing policy (see certifier.h): a worker gives one child away + whenever the queue holds fewer items than there are live workers. Reading + the length through a relaxed atomic keeps the *test* off the queue's mutex, + so only an actual share pays for the lock; a stale answer costs at most one + redundant or one skipped share. `num_workers` is 0 until helpers are hired, + which is exactly how lazy recruitment disables sharing. */ + bool ShouldShare() const { + return size_.load(std::memory_order_relaxed) < + num_workers_.load(std::memory_order_relaxed); + } + + void set_num_workers(int count) { + num_workers_.store(count, std::memory_order_relaxed); + } + + private: + std::mutex mutex_; + std::condition_variable condition_; + std::vector items_; + std::vector free_; + std::atomic size_{0}; + std::atomic num_workers_{0}; + int busy_{0}; + bool done_{false}; + bool aborted_{false}; +}; + +/* Lazy-recruitment state. Only the lead worker ever touches it, so it needs no + synchronization of its own: `hire` is called from inside the lead's node loop + the first time the run has visited enough nodes to be worth spreading. */ +struct Recruitment { + std::uint64_t nodes{0}; + std::function hire; +}; + +/* How many nodes a run must have visited before it hires helpers. + + Hiring costs one ContextPool lease, the construction of the helper Worker + objects, one thread creation per helper and, at the end of the run, one join + per helper. Thread creation dominates that list at tens of microseconds per + worker, while a node costs ~7-13 us on a modern desktop core, so 64 nodes of + work already done is roughly a 3x margin over the price of a full fifteen + helpers. It also bounds the one case lazy recruitment cannot avoid, a check + that ends immediately after hiring, to a few hundred microseconds. Below the + threshold a run is exactly serial at any Options::parallelism, which matters + because Parallelism::Max() is that field's default. */ +constexpr std::uint64_t kNodesBeforeHiringHelpers = 64; + +/* One frame of the explicit LIFO node stack. The frame at stack index k owns + control-point slab k of the worker's pool, and the pair indices it is still + active for live in arena[active_offset, active_offset + active_length). */ +struct NodeFrame { + double s_lo{0.0}; + double s_hi{1.0}; + int active_offset{0}; + int active_length{0}; +}; + +/* A worker owns all per-thread scratch of the node recursion; nothing in the + steady-state loop allocates: + + - `slabs_` is the node pool: slab k is the n × (m+1) control-point matrix of + the frame at stack index k. Splitting a node writes its left child into + slab k+1 and swaps its right child into slab k (an O(1) Eigen buffer + swap), so a split costs zero copies and zero allocations. Slabs are + (re)sized only when the worker moves to a segment of a different Bézier + order. + - `arena_` is the survivor arena: each frame's active pair list is an index + span into it. Both children of a node share one span (their active sets + are identical), and a popped frame writes its survivors immediately above + its own span, which is free because every frame still on the stack owns a + span at or below that point. + - `stack_` grows by one per level, so all three arrays are bounded by the + depth the resolution floor allows. */ +class Worker { + public: + Worker(const CertifierInput& input, ThreadContext* context, FindingSink* sink, + std::atomic* node_counter, WorkQueue* queue, + Recruitment* recruit) + : input_(input), + context_(context), + sink_(sink), + node_counter_(node_counter), + queue_(queue), + recruit_(recruit), + geometry_(*input.prefilter, *context) { + const int n = input_.path->num_positions(); + q_mid_.resize(n); + w_.resize(n); + } + + /* Parallel entry point: pulls items until the work source is quiescent. The + item buffer is reused across iterations and recycled through the queue's + free list, so the loop allocates nothing after the first few rounds. */ + void Run() { + while (queue_->Pop(&item_)) { + RunItem(&item_); + queue_->FinishItem(); + } + } + + /* Serial entry point (and the body of the parallel one). */ + void RunItem(WorkItem* item); + + private: + /* Ensures the pool holds `count` slabs of the given shape. Cold path: hit + once per worker and again whenever the Bézier order changes. */ + void EnsureSlabs(int count, int rows, int cols) { + if (rows != slab_rows_ || cols != slab_cols_) { + for (Eigen::MatrixXd& slab : slabs_) slab.resize(rows, cols); + split_scratch_.resize(rows, cols); + slab_rows_ = rows; + slab_cols_ = cols; + } + while (static_cast(slabs_.size()) < count) { + slabs_.emplace_back(rows, cols); + } + } + + void EnsureArena(int size) { + if (static_cast(arena_.size()) < size) { + arena_.resize(std::max(size, 2 * static_cast(arena_.size()) + 64)); + } + } + + const CertifierInput& input_; + ThreadContext* context_{}; + FindingSink* sink_{}; + std::atomic* node_counter_{}; + WorkQueue* queue_{}; + /* Non-null only for the lead worker, and only until it has hired. */ + Recruitment* recruit_{}; + GeometryCache geometry_; + + /* Reused buffers for the queue's two directions (see WorkItem). */ + WorkItem item_; + WorkItem share_; + + std::vector slabs_; + Eigen::MatrixXd split_scratch_; + int slab_rows_{-1}; + int slab_cols_{-1}; + std::vector stack_; + std::vector arena_; + Eigen::VectorXd q_mid_; + Eigen::VectorXd w_; + Eigen::Vector3d nearest_a_; + Eigen::Vector3d nearest_b_; +}; + +void Worker::RunItem(WorkItem* item) { + const BezierSegment& segment = input_.path->segments()[item->segment]; + const MotionBoundTable& table = *input_.table; + const DistanceOracle& oracle = *input_.oracle; + const std::vector& pairs = *input_.pairs; + const std::vector& tau = *input_.tau; + const PrefilterTable& prefilter = *input_.prefilter; + const double threshold = input_.options.margin; + const double resolution = input_.options.distance_resolution; + const int rows = static_cast(item->control_points.rows()); + const int cols = static_cast(item->control_points.cols()); + + // Seed the local stack with this work item. + EnsureSlabs(2, rows, cols); + slabs_[0] = item->control_points; + EnsureArena(static_cast(item->active.size()) + 1); + std::copy(item->active.begin(), item->active.end(), arena_.begin()); + stack_.clear(); + stack_.push_back(NodeFrame{item->s_lo, item->s_hi, 0, + static_cast(item->active.size())}); + + while (!stack_.empty()) { + const NodeFrame frame = stack_.back(); + stack_.pop_back(); + const int k = static_cast(stack_.size()); + + // Branch-and-bound on time: a node starting at or after the earliest + // witness known so far cannot contain an earlier one. + if (TimeOf(segment, frame.s_lo) >= sink_->best_violation_time()) continue; + node_counter_->fetch_add(1, std::memory_order_relaxed); + + // Lazy recruitment (see certifier.h): the lead worker runs alone until the + // run has visited enough nodes to pay for helpers, then hires them once + // and drops the hook. Every other worker carries a null `recruit_`. + if (recruit_ != nullptr && ++recruit_->nodes >= kNodesBeforeHiringHelpers) { + Recruitment* const recruitment = recruit_; + recruit_ = nullptr; + recruitment->hire(); + } + + EnsureSlabs(k + 2, rows, cols); + const Eigen::MatrixXd& control_points = slabs_[k]; + + // The split *is* the evaluation: the apex of the de Casteljau triangle at + // the midpoint is exactly q(s_mid), so the node's representative + // configuration comes for free. + DeCasteljauSplitAtHalf(control_points, &slabs_[k + 1], &split_scratch_, + &q_mid_); + + // w_i = max_j |P_{j,i} − qc_i|. By the convex-hull property of the + // Bernstein basis, |q_i(s) − qc_i| ≤ w_i for every s in this node. + w_.setZero(); + for (int j = 0; j < cols; ++j) { + for (int i = 0; i < rows; ++i) { + w_[i] = std::max(w_[i], std::abs(control_points(i, j) - q_mid_[i])); + } + } + + // One FK per node; body poses and the query object are pulled lazily + // below, and only for pairs that survive that far. + context_->SetPositions(q_mid_); + geometry_.NewConfiguration(); + const QueryObject& query_object = context_->query_object(); + + const double s_mid = 0.5 * (frame.s_lo + frame.s_hi); + const double t_mid = TimeOf(segment, s_mid); + // Hard floating-point backstop: once the midpoint no longer separates the + // endpoints in double arithmetic the node cannot be split any further, + // whatever the resolution says. Without it a pathologically small + // resolution would spin forever. + const bool fp_backstop = !(s_mid > frame.s_lo && s_mid < frame.s_hi); + + const int survivor_offset = frame.active_offset + frame.active_length; + int survivor_count = 0; + EnsureArena(survivor_offset + frame.active_length + 1); + + for (int e = frame.active_offset; + e < frame.active_offset + frame.active_length; ++e) { + const int p = arena_[e]; + const PairRecord& pair = pairs[p]; + const double tau_p = tau[p]; + // Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p)·w_j, a sparse dot + // product over this pair's CSR row. + const double travel = table.TravelBound(p, w_); + const double motion_bound = table.carveout_slack(p) + travel; + // The resolution floor is per pair and in meters: once this pair's + // relative motion over the node is bounded by the requested resolution, + // splitting further cannot decide it any better than the oracle + // tolerance already allows, so the pair is decided here or reported as + // inconclusive. Only the travel term is tested, because the carve-out + // residual does not shrink with splitting. + const bool at_floor = fp_backstop || travel <= resolution; + + // --- Early-out 1: the free-sphere prefilter. --- + // ϕ_p ≥ ‖c_A − c_B‖ − ρ_A − ρ_B with the bounding spheres posed at qc, + // so the lower bound stands in for ϕ̂ in the certificate test below and + // is sound by the same displacement-lemma argument. It needs no + // narrowphase and no allocation, only the lazily pulled body poses. It + // is charged the same τ_p as the oracle even though it is exact given + // the poses: that costs nothing (τ ~ 1e-6 m against centimetre-scale + // sphere gaps) and keeps the arithmetic uniform. + const int slot_a = prefilter.slot_a[p]; + const int slot_b = prefilter.slot_b[p]; + if (slot_a >= 0 && slot_b >= 0 && + IsCertified(geometry_.LowerBound(slot_a, slot_b), tau_p, motion_bound, + threshold)) { + continue; + } + + // --- Narrowphase. ---------------------------------------------------- + const double phi_hat = + oracle.SignedDistance(query_object, pair, &nearest_a_, &nearest_b_); + + if (IsDefiniteViolation(phi_hat, tau_p, threshold)) { + // qc is exactly on the trajectory (it is the de Casteljau apex), so + // ϕ_true(qc) ≤ ϕ̂ + τ_p < m is a definite violation of the continuum + // statement, not a sampling artifact. + sink_->AddDefinite( + MakeFinding(t_mid, q_mid_, pair, phi_hat, nearest_a_, nearest_b_)); + // A floor node has no children to refine into; otherwise keep p active + // so the branch-and-bound recursion can refine the witness toward the + // earliest violating time. + if (at_floor) continue; + } else if (IsCertified(phi_hat, tau_p, motion_bound, threshold)) { + // Displacement lemma: for every s in this node, + // ϕ_p(q(s)) ≥ ϕ_true(qc) − Σ_{j∈J(p)} λ(j,p)·|q_j(s) − qc_j| + // ≥ (ϕ̂ − τ_p) − Δ_p(ν) > m + ε, + // using |q_j(s) − qc_j| ≤ w_j from the convex-hull property. The whole + // closed parameter interval of the node is therefore certified and the + // pair drops out of the entire subtree, which is the dominant work + // saver. + continue; + } else if (at_floor) { + // --- Gray at the resolution floor. --- + sink_->AddInconclusive( + MakeFinding(t_mid, q_mid_, pair, phi_hat, nearest_a_, nearest_b_)); + continue; + } + + arena_[survivor_offset + survivor_count] = p; + ++survivor_count; + } + + if (survivor_count == 0) continue; + + // At this point the split has left the *left* child in slab k+1 and the + // *right* child in split_scratch_. + const NodeFrame right{s_mid, frame.s_hi, survivor_offset, survivor_count}; + const NodeFrame left{frame.s_lo, s_mid, survivor_offset, survivor_count}; + if (queue_ != nullptr && queue_->ShouldShare()) { + // Occupancy-driven sharing (see certifier.h): the shared queue is + // running dry, so hand the right child over and carry on down the left + // one. This is the only mechanism that spreads a deep tree, and because + // it is driven by how hungry the other workers are rather than by depth, + // it keeps spreading right down to the last subtree, which is exactly + // what a fixed seeding depth cannot do. + share_.segment = item->segment; + share_.s_lo = right.s_lo; + share_.s_hi = right.s_hi; + share_.control_points = split_scratch_; + share_.active.assign(arena_.begin() + survivor_offset, + arena_.begin() + survivor_offset + survivor_count); + queue_->Push(&share_); + // The frame at stack index k must own slab k, so move the left child + // down into it (an O(1) Eigen buffer swap, like the split itself). + slabs_[k].swap(slabs_[k + 1]); + stack_.push_back(left); // slab k holds the left child. + } else { + slabs_[k].swap(split_scratch_); // O(1): slab k = right child. + // LIFO with the left child on top => a left-to-right sweep in time, so + // the serial driver walks the trajectory in order. + stack_.push_back(right); // slab k holds the right child. + stack_.push_back(left); // slab k+1 holds the left child. + } + } +} + +/* Evaluates one breakpoint configuration against every pair. Breakpoints are + the finitely many configurations the midpoint recursion only approaches in the + limit (t0, every junction, tf), so checking them discretely is what gives + violations *at* interval endpoints clean semantics. + + When `resolve_static` is true (the t0 breakpoint) the pairs with J(p) = ∅ are + also resolved here, once and for all: no motion of the trajectory can change + their relative pose, so their status at q(t0) is their status everywhere. */ +void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, + GeometryCache* geometry, const Eigen::VectorXd& q, + double time, bool resolve_static, FindingSink* sink) { + const std::vector& pairs = *input.pairs; + const std::vector& tau = *input.tau; + const PrefilterTable& prefilter = *input.prefilter; + const MotionBoundTable& table = *input.table; + const double threshold = input.options.margin; + + context->SetPositions(q); + geometry->NewConfiguration(); + const QueryObject& query_object = context->query_object(); + + Eigen::Vector3d nearest_a; + Eigen::Vector3d nearest_b; + for (int p = 0; p < static_cast(pairs.size()); ++p) { + const PairRecord& pair = pairs[p]; + const double tau_p = tau[p]; + const bool is_static = table.pair_is_static(p); + // Δ_p for a static pair: J(p) is empty, so the sparse dot product is empty + // and MotionBound() collapses to the pair's carve-out slack whatever w is. + // That slack is normally exactly 0, and "static" then means genuinely + // immobile, but a pair whose whole J_topo(p) was carved out on a + // *tolerance* can still drift by that much, and the discrete test below + // has to charge it or the carved coordinates' residual would go + // unaccounted for on exactly the pairs made entirely of them. + const double static_bound = table.carveout_slack(p); + // A static pair's clearance is the same at every configuration of the + // trajectory, so the t0 pass settles it for good; re-testing it at every + // junction would only pay a narrowphase query per junction. + if (is_static && !resolve_static) continue; + + double lower_bound = -kInfinity; + const int slot_a = prefilter.slot_a[p]; + const int slot_b = prefilter.slot_b[p]; + if (slot_a >= 0 && slot_b >= 0) { + lower_bound = geometry->LowerBound(slot_a, slot_b); + } + + if (is_static) { + // Δ_p is the constant `static_bound` for a static pair, so the node + // certificate degenerates to a single discrete test that holds for the + // whole domain. + if (IsCertified(lower_bound, tau_p, static_bound, threshold)) continue; + } else if (lower_bound >= threshold) { + // A definite violation needs ϕ̂ + τ_p < m, and ϕ̂ ≥ ϕ_true − τ_p ≥ + // lower_bound − τ_p, so lower_bound ≥ m rules one out with no query. + continue; + } + + const double phi_hat = input.oracle->SignedDistance(query_object, pair, + &nearest_a, &nearest_b); + + if (IsDefiniteViolation(phi_hat, tau_p, threshold)) { + sink->AddDefinite( + MakeFinding(time, q, pair, phi_hat, nearest_a, nearest_b)); + continue; + } + if (!is_static) continue; + if (IsCertified(phi_hat, tau_p, static_bound, threshold)) continue; + // Neither certified nor violating, and no subdivision can help: this + // pair's clearance is constant along the trajectory (up to the carve-out + // residual) and sits within oracle tolerance of the threshold. + sink->AddInconclusive( + MakeFinding(time, q, pair, phi_hat, nearest_a, nearest_b)); + } +} + +} // namespace + +void ThreadContext::SetPositions(const Eigen::VectorXd& q) { + model_->plant().SetPositions(&context_.mutable_plant_context(), q); +} + +const RigidTransformd& ThreadContext::EvalBodyPose(BodyIndex body) const { + return model_->plant().EvalBodyPoseInWorld(context_.plant_context(), + model_->plant().get_body(body)); +} + +ContextPool::ContextPool(const drake::planning::RobotDiagram& model, + int initial_size) + : model_(&model) { + for (int i = 0; i < std::max(1, initial_size); ++i) { + slots_.push_back(std::make_unique(model)); + in_use_.push_back(false); + } +} + +ContextPool::Lease ContextPool::Acquire(int count) const { + DRAKE_DEMAND(count >= 1); + std::vector contexts; + std::vector slots; + contexts.reserve(count); + slots.reserve(count); + std::lock_guard guard(mutex_); + for (int i = 0; i < static_cast(slots_.size()) && + static_cast(slots.size()) < count; + ++i) { + if (!in_use_[i]) { + in_use_[i] = true; + slots.push_back(i); + contexts.push_back(slots_[i].get()); + } + } + while (static_cast(slots.size()) < count) { + slots_.push_back(std::make_unique(*model_)); + in_use_.push_back(true); + slots.push_back(static_cast(slots_.size()) - 1); + contexts.push_back(slots_.back().get()); + } + return Lease(this, std::move(contexts), std::move(slots)); +} + +void ContextPool::Release(const std::vector& slots) const { + std::lock_guard guard(mutex_); + for (const int slot : slots) in_use_[slot] = false; +} + +ContextPool::Lease& ContextPool::Lease::operator=(Lease&& other) noexcept { + if (this == &other) return *this; + if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); + pool_ = other.pool_; + contexts_ = std::move(other.contexts_); + slots_ = std::move(other.slots_); + other.pool_ = nullptr; + other.contexts_.clear(); + other.slots_.clear(); + return *this; +} + +ContextPool::Lease::~Lease() { + if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); +} + +Result RunCertifier(const CertifierInput& input, ContextPool* pool) { + DRAKE_DEMAND(input.oracle != nullptr); + DRAKE_DEMAND(input.table != nullptr); + DRAKE_DEMAND(input.path != nullptr); + DRAKE_DEMAND(input.pairs != nullptr); + DRAKE_DEMAND(input.tau != nullptr); + DRAKE_DEMAND(input.prefilter != nullptr); + DRAKE_DEMAND(pool != nullptr); + + const PiecewiseBezierPath& path = *input.path; + const int num_pairs = static_cast(input.pairs->size()); + const int num_segments = static_cast(path.segments().size()); + + FindingSink sink; + std::atomic node_counter{0}; + + // Bounding the width by the machine's keeps a program that runs many + // concurrent parallel checks from multiplying threads without limit; the + // bound comes from Parallelism::Max() rather than hardware_concurrency() + // directly, so it honours DRAKE_NUM_THREADS like the rest of Drake. + const int num_threads = + std::min(std::max(1, input.options.parallelism.num_threads()), + Parallelism::Max().num_threads()); + // Only the lead worker's context is leased up front. Helpers lease theirs + // when (if) they are hired, so a small check under the default + // Parallelism::Max() never pays for sixteen leases it will not use. + ContextPool::Lease lease = pool->Acquire(1); + + // --- Steps 1 and 2: breakpoints and static pairs (serial, O(#segments)). -- + if (num_segments > 0 && num_pairs > 0) { + GeometryCache geometry(*input.prefilter, lease[0]); + for (int k = 0; k <= num_segments; ++k) { + // Segment k's start, plus the last segment's end. At a junction the two + // sides are the same physical configuration (they may differ by 2πk in a + // continuous-revolute coordinate, which forward kinematics ignores), so + // one evaluation per breakpoint suffices. Endpoints are Bézier control + // points, so they are exact; no curve evaluation needed. + const Eigen::VectorXd q = + (k < num_segments) + ? Eigen::VectorXd(path.segments()[k].control_points.col(0)) + : Eigen::VectorXd( + path.segments()[k - 1].control_points.rightCols(1)); + const double time = (k < num_segments) ? path.segments()[k].t_start + : path.segments()[k - 1].t_end; + RunBreakpointPass(input, &lease[0], &geometry, q, time, + /* resolve_static = */ k == 0, &sink); + } + } + + // --- Step 3: the adaptive recursion over every segment. ------------------ + std::vector moving_pairs; + moving_pairs.reserve(num_pairs); + for (int p = 0; p < num_pairs; ++p) { + if (!input.table->pair_is_static(p)) moving_pairs.push_back(p); + } + + if (!moving_pairs.empty() && num_segments > 0 && num_threads <= 1) { + // Serial: one worker, one local stack, no shared queue and no thread + // interleaving => bit-deterministic results. + Worker worker(input, &lease[0], &sink, &node_counter, nullptr, nullptr); + for (int k = 0; k < num_segments; ++k) { + WorkItem item; + item.segment = k; + item.control_points = path.segments()[k].control_points; + item.active = moving_pairs; + worker.RunItem(&item); + } + } else if (!moving_pairs.empty() && num_segments > 0) { + // Parallel driver: lazy recruitment + occupancy-driven sharing. The full + // rationale, and why static seeding is not used, is documented on + // RunCertifier() in certifier.h. + WorkQueue queue; + { + // Seeded in reverse so the LIFO hands segment 0 out first. Before any + // helper exists that reproduces the serial left-to-right sweep exactly, + // and once helpers arrive it still lets the earliest-violation bound + // tighten from the front of the trajectory. + WorkItem seed; + for (int k = num_segments - 1; k >= 0; --k) { + seed.segment = k; + seed.s_lo = 0.0; + seed.s_hi = 1.0; + seed.control_points = path.segments()[k].control_points; + seed.active = moving_pairs; + queue.Push(&seed); + } + } + + // The oracle is documented to throw, and any allocation can. A worker that + // let an exception escape would terminate the process, and, because it + // would skip WorkQueue::FinishItem(), would also strand every other + // worker in Pop(). So every worker catches, aborts the work source, and + // the first exception is rethrown once all of them have finished. + std::exception_ptr first_error; + std::mutex error_mutex; + const auto record_error = [&]() { + std::lock_guard guard(error_mutex); + if (first_error == nullptr) first_error = std::current_exception(); + }; + + // The futures are declared last so that they are destroyed, and therefore + // waited on, before the workers, contexts and lease their tasks reference, + // on every path including the throwing one. + std::optional helper_lease; + std::vector> helpers; + std::vector> helper_futures; + + Recruitment recruitment; + // Hiring is a per-call cold path: it runs at most once per check, only + // after the run has proved itself worth spreading, and it is the only + // place in the driver that allocates or creates a thread once the node + // loop is turning; only the steady state is allocation-free. + recruitment.hire = [&]() { + const int hired = num_threads - 1; + if (hired <= 0) return; + helper_lease.emplace(pool->Acquire(hired)); + helpers.reserve(hired); + for (int i = 0; i < hired; ++i) { + helpers.push_back(std::make_unique( + input, &(*helper_lease)[i], &sink, &node_counter, &queue, nullptr)); + } + // Every consumer of the queue, the lead included: this count is the + // occupancy target of the sharing policy, and setting it from zero is + // what switches sharing on. + queue.set_num_workers(hired + 1); + helper_futures.reserve(hired); + for (int i = 0; i < hired; ++i) { + // std::async throws std::system_error when the system refuses a + // thread. The helpers that did start are still joined below, and the + // lead's catch turns the refusal into the same aborted run any other + // throw out of the node loop produces. + helper_futures.push_back(std::async(std::launch::async, [&, i]() { + try { + helpers[i]->Run(); + } catch (...) { + record_error(); + queue.Abort(); + } + })); + } + }; + + Worker lead(input, &lease[0], &sink, &node_counter, &queue, &recruitment); + try { + lead.Run(); + } catch (...) { + record_error(); + queue.Abort(); + } + // The helper tasks swallow their own exceptions into `first_error`, so + // get() here is a join and never throws. + for (std::future& helper : helper_futures) helper.get(); + if (first_error != nullptr) std::rethrow_exception(first_error); + } + + Result result; + result.num_nodes = node_counter.load(std::memory_order_relaxed); + if (sink.definite().has_value()) { + // The branch-and-bound recursion refines toward the earliest witness, so + // this *is* the earliest witness, identical serially and in parallel. + result.verdict = Verdict::kViolationFound; + result.finding = std::move(sink.definite()); + } else if (sink.inconclusive().has_value()) { + result.verdict = Verdict::kInconclusive; + result.finding = std::move(sink.inconclusive()); + } else { + result.verdict = Verdict::kCertifiedFree; + } + return result; +} + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/certifier.h b/planning/continuous_collision/certifier.h new file mode 100644 index 000000000000..3b601a0d161d --- /dev/null +++ b/planning/continuous_collision/certifier.h @@ -0,0 +1,205 @@ +#pragma once + +// Internal driver of the adaptive interval certifier. Nothing here is part of +// the public API; it exists so that continuous_collision_checker.cc and +// certifier.cc can share one set of per-call data structures. + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/geometry/query_object.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" +#include "drake/planning/collision_checker_context.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" +#include "drake/planning/continuous_collision/distance_oracle.h" +#include "drake/planning/continuous_collision/internal.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { + +/* One thread's view of the model: a CollisionCheckerContext (which owns the +root diagram context and the plant and scene-graph sub-contexts pulled out of +it once), plus the two model queries the node loop makes of it. */ +class ThreadContext { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ThreadContext); + + /* Allocates a root context of `model`. `model` is aliased and must outlive + this object. */ + explicit ThreadContext(const RobotDiagram& model) + : model_(&model), context_(&model) {} + + /* The one FK trigger per node: sets the plant's generalized positions. + Drake caches forward kinematics per context afterwards, so body poses and + the query object are pulled lazily and only for the bodies/pairs that are + still active. */ + void SetPositions(const Eigen::VectorXd& q); + + /* The scene graph's query object at the configuration last set. */ + const geometry::QueryObject& query_object() const { + return context_.GetQueryObject(); + } + + /* World pose of `body` at the configuration last set (Drake's cache + computes it on first use and reuses it afterwards). */ + const math::RigidTransform& EvalBodyPose( + multibody::BodyIndex body) const; + + private: + const RobotDiagram* model_{}; + CollisionCheckerContext context_; +}; + +/* A checkout pool of ThreadContexts; construction allocates +`parallelism.num_threads()` RobotDiagram contexts. + +The pool is a *checkout* pool rather than a thread-indexed array so that the +public Check* methods stay safe to call concurrently from several threads: +each call leases the contexts it needs for its duration and no two workers can +ever share one. A lease larger than the pre-warmed pool grows it (a cold-path +allocation); nothing shrinks it. */ +class ContextPool { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ContextPool); + + /* Pre-warms `initial_size` contexts of `model`, which is aliased and must + outlive this pool. */ + ContextPool(const RobotDiagram& model, int initial_size); + + /* RAII handle for a set of leased contexts. */ + class Lease { + public: + Lease(const Lease&) = delete; + Lease& operator=(const Lease&) = delete; + /* Hand-written rather than defaulted: the moved-from lease must stop + owning its slots, and move-assignment must return the slots it already + holds, or those pool entries stay marked in-use forever. */ + Lease(Lease&& other) noexcept { *this = std::move(other); } + Lease& operator=(Lease&& other) noexcept; + ~Lease(); + + ThreadContext& operator[](int i) const { return *contexts_[i]; } + + private: + friend class ContextPool; + Lease(const ContextPool* pool, std::vector contexts, + std::vector slots) + : pool_(pool), + contexts_(std::move(contexts)), + slots_(std::move(slots)) {} + + const ContextPool* pool_{}; + std::vector contexts_; + std::vector slots_; + }; + + /* Leases exactly `count` contexts, growing the pool if it is exhausted. */ + Lease Acquire(int count) const; + + private: + void Release(const std::vector& slots) const; + + const RobotDiagram* model_{}; + mutable std::mutex mutex_; + /* A deque so that growing never invalidates the ThreadContext addresses + already handed out. */ + mutable std::deque> slots_; + mutable std::vector in_use_; +}; + +/* Per-pair broadphase data for the free-sphere prefilter: geometry bounding +spheres in their body frames, indexed by dense slots so the node loop can cache +one world-frame center per geometry per node. */ +struct PrefilterTable { + struct Geometry { + multibody::BodyIndex body; + Eigen::Vector3d center_L{Eigen::Vector3d::Zero()}; + double radius{0.0}; + }; + /* Dense geometry slots; only geometries that *have* a bounding sphere + appear (HalfSpace has none). */ + std::vector geometries; + /* Per pair: slot of geometry a / b, or -1 when that geometry has no sphere + (a HalfSpace), in which case the pair skips the prefilter and goes straight + to the (cheap, analytic) oracle route. */ + std::vector slot_a; + std::vector slot_b; +}; + +/* Everything one run needs; assembled by the facade. All pointers are aliased +and must outlive the call. */ +struct CertifierInput { + const DistanceOracle* oracle{}; + const MotionBoundTable* table{}; + const PiecewiseBezierPath* path{}; + /* Indexed consistently with `table`, `tau` and `prefilter`. */ + const std::vector* pairs{}; + /* Per-pair oracle tolerance τ_p; see the accuracy table in + continuous_collision_checker.cc. */ + const std::vector* tau{}; + const PrefilterTable* prefilter{}; + Options options; +}; + +/* Runs the breakpoint pre-pass, the static-pair resolution and the adaptive +node recursion over every segment of `input.path`, serially or in parallel +according to `input.options.parallelism`. `pool` supplies the per-thread +contexts; helper threads, if any are hired, are created and joined within this +call. + +The search returns the earliest-in-time violation and stops as soon as that +violation is proven earliest. + +Parallel driver. Static seeding, i.e. cutting a fixed set of node roots up +front, does not work here: the trees are unbalanced, because a grazing +trajectory concentrates its subdivision in a band a few 10⁻³ wide in segment +parameter, so whatever fixed set of seeds is cut, one of them holds nearly the +whole tree. Three policies replace it. The only shared state is the per-thread +contexts, one atomic earliest-violation bound, an atomic node counter, and a +findings sink under a mutex. + +Sharing is occupancy-driven, not depth-driven. There is one shared LIFO work +source; a worker that has just split a node pushes its *right* child there when +the queue is shorter than the number of live workers, and otherwise keeps both +children. A saturated queue therefore costs nothing, and sharing does not stop +at any depth: a worker on the last deep subtree with every other worker idle +hands out a node per level until the tail is spread. Giving away the right +child keeps each worker's own descent left-first, which is what makes the +earliest-violation bound tighten early. + +Recruitment is lazy. The call starts as a serial descent on the calling thread +with sharing disabled and hires helpers only after visiting +`kNodesBeforeHiringHelpers` nodes, so a check whose whole workload is smaller +than that runs at exactly serial speed whatever `Options::parallelism` says. +That matters because `Parallelism::Max()` is the default. Helpers are +call-scoped threads; nothing owns a background thread between calls. + +Determinism survives sharing, because moving nodes between workers does not +change which nodes exist. Every node's decisions depend only on its own control +points and its inherited active set, so the *reported witness* and the verdict +are identical serially and at any thread count; Result::num_nodes is not, +because the bound prunes a timing-dependent set of nodes that start at or after +a witness already found. One exception: on a degenerate segment with t_start == +t_end every node maps to the same time, so the bound prunes on a tie and the +reported configuration (not its time) may differ. + +@throws std::exception if the oracle throws for any pair; a parallel run waits +for every worker first and rethrows the first failure. */ +Result RunCertifier(const CertifierInput& input, ContextPool* pool); + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc new file mode 100644 index 000000000000..33fa616b6d7f --- /dev/null +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -0,0 +1,318 @@ +#include "drake/planning/continuous_collision/continuous_collision_checker.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/geometry/scene_graph.h" +#include "drake/geometry/scene_graph_inspector.h" +#include "drake/geometry/shape_specification.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/planning/continuous_collision/certifier.h" +#include "drake/planning/continuous_collision/internal.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace { + +using drake::geometry::GeometryId; +using drake::multibody::BodyIndex; +using internal::Classify; +using internal::DistanceRoute; +using internal::kNumShapeClasses; +using internal::PairRecord; +using internal::ShapeClass; + +// --------------------------------------------------------------------------- +// Per-pair oracle tolerance τ_p. +// --------------------------------------------------------------------------- +// +// Drake documents ComputeSignedDistancePairClosestPoints() accuracy as bad as +// 5e-5 m for some shape pairs, well outside the 1e-6 m internal:: +// kQueryTolerance, and an oracle that over-reports a distance at or above the +// threshold can fake a certificate. Every use of τ (node certificate test, +// definite violation test, breakpoints) therefore takes τ_p = +// max(kQueryTolerance, documented_accuracy(shape_a, shape_b)); pairs routed +// through the analytic halfspace fallback are closed-form and keep the raw +// kQueryTolerance. +// +// The table below is Table 4 of drake/geometry/query_object.h. Mesh is +// certified as its convex hull, so its row and column duplicate Convex's, and +// a shape the checker cannot classify is charged the worst documented value. +// Never relax an entry ahead of Drake's own documentation. +// +// clang-format off +// | | Box | Caps | Conv | Cyl | Ellip | Mesh | Sph | +// | Box | 4e-15 | | | | | | | +// | Capsule | 3e-6 | 2e-5 | | | | | | +// | Convex | 3e-15 | 2e-5 | 3e-15 | | | | | +// | Cylinder | 6e-6 | 1e-5 | 6e-6 | 2e-5 | | | | +// | Ellipsoid | 9e-6 | 5e-6 | 9e-6 | 5e-5 | 2e-5 | | | +// | Mesh | (= the Convex row) | 3e-15 | | +// | Sphere | 4e-15 | 6e-15 | 3e-6 | 5e-15 | 4e-5 | 3e-6 | 6e-15 | +// clang-format on + +/* Worst documented error over the whole table, charged to any shape the + checker cannot classify. Such a shape never reaches the narrowphase, because + the capability probe refuses unknown shapes at construction, but the default + must still be the conservative one. */ +constexpr double kWorstDocumentedAccuracy = 5e-5; + +using AccuracyTable = + std::array, kNumShapeClasses>; + +const AccuracyTable& DocumentedAccuracyTable() { + static const AccuracyTable table = []() { + AccuracyTable t{}; + for (auto& row : t) row.fill(kWorstDocumentedAccuracy); + const auto set = [&t](ShapeClass a, ShapeClass b, double value) { + t[static_cast(a)][static_cast(b)] = value; + t[static_cast(b)][static_cast(a)] = value; + }; + using S = ShapeClass; + set(S::kSphere, S::kSphere, 6e-15); + set(S::kSphere, S::kBox, 4e-15); + set(S::kSphere, S::kCapsule, 6e-15); + set(S::kSphere, S::kCylinder, 5e-15); + set(S::kSphere, S::kEllipsoid, 4e-5); + set(S::kSphere, S::kConvex, 3e-6); + set(S::kSphere, S::kMesh, 3e-6); + set(S::kBox, S::kBox, 4e-15); + set(S::kBox, S::kCapsule, 3e-6); + set(S::kBox, S::kCylinder, 6e-6); + set(S::kBox, S::kEllipsoid, 9e-6); + set(S::kBox, S::kConvex, 3e-15); + set(S::kBox, S::kMesh, 3e-15); + set(S::kCapsule, S::kCapsule, 2e-5); + set(S::kCapsule, S::kCylinder, 1e-5); + set(S::kCapsule, S::kEllipsoid, 5e-6); + set(S::kCapsule, S::kConvex, 2e-5); + set(S::kCapsule, S::kMesh, 2e-5); + set(S::kCylinder, S::kCylinder, 2e-5); + set(S::kCylinder, S::kEllipsoid, 5e-5); + set(S::kCylinder, S::kConvex, 6e-6); + set(S::kCylinder, S::kMesh, 6e-6); + set(S::kEllipsoid, S::kEllipsoid, 2e-5); + set(S::kEllipsoid, S::kConvex, 9e-6); + set(S::kEllipsoid, S::kMesh, 9e-6); + set(S::kConvex, S::kConvex, 3e-15); + set(S::kConvex, S::kMesh, 3e-15); + set(S::kMesh, S::kMesh, 3e-15); + // Drake supports exactly one halfspace combination natively (Sphere, at + // 3e-15); the rest it refuses. Halfspace pairs never reach the + // narrowphase here, because the capability probe routes every one of them + // through the exact analytic support-function fallback and + // ComputeTauTable() below never consults this table for a non-native + // route. The entries are filled anyway, with the documented value where + // there is one and the worst documented value otherwise, so that a future + // routing change cannot inherit a τ of zero. + set(S::kSphere, S::kHalfSpace, 3e-15); + return t; + }(); + return table; +} + +/* τ_p for every pair of `pairs`. */ +std::vector ComputeTauTable(const RobotDiagram& model, + const std::vector& pairs) { + const drake::geometry::SceneGraphInspector& inspector = + model.scene_graph().model_inspector(); + const AccuracyTable& table = DocumentedAccuracyTable(); + std::vector tau(pairs.size(), internal::kQueryTolerance); + for (int p = 0; p < static_cast(pairs.size()); ++p) { + if (pairs[p].route != DistanceRoute::kNative) continue; // exact. + const int a = static_cast(Classify(inspector.GetShape(pairs[p].a))); + const int b = static_cast(Classify(inspector.GetShape(pairs[p].b))); + tau[p] = std::max(internal::kQueryTolerance, table[a][b]); + } + return tau; +} + +/* Per-pair bounding-sphere slots for the broadphase prefilter. */ +internal::PrefilterTable ComputePrefilterTable( + const internal::KinematicsEngine& engine, + const std::vector& pairs) { + internal::PrefilterTable table; + table.slot_a.resize(pairs.size(), -1); + table.slot_b.resize(pairs.size(), -1); + std::unordered_map slot_of; + + // HalfSpace has no bounding sphere, so such pairs skip the prefilter + // entirely and go straight to the analytic oracle route, which is cheap + // anyway. The oracle probe already found the halfspace: a pair is routed + // kHalfSpaceA/kHalfSpaceB exactly when geometry a/b is one. + const auto slot = [&](GeometryId id, BodyIndex body, bool is_half_space) { + if (is_half_space) return -1; + const auto it = slot_of.find(id); + if (it != slot_of.end()) return it->second; + const internal::BoundingSphere& sphere = engine.geometry_sphere(id); + const int index = static_cast(table.geometries.size()); + table.geometries.push_back(internal::PrefilterTable::Geometry{ + body, sphere.center_L, sphere.radius}); + slot_of.emplace(id, index); + return index; + }; + + for (int p = 0; p < static_cast(pairs.size()); ++p) { + table.slot_a[p] = slot(pairs[p].a, pairs[p].body_a, + pairs[p].route == DistanceRoute::kHalfSpaceA); + table.slot_b[p] = slot(pairs[p].b, pairs[p].body_b, + pairs[p].route == DistanceRoute::kHalfSpaceB); + } + return table; +} + +void ValidateOptions(const Options& options) { + // The displacement lemma argues entirely in the separated regime, so the + // proof is meaningless for a negative threshold: a pair meant to touch must + // be collision-filtered, not given a negative margin. + if (!(options.margin >= 0.0) || !std::isfinite(options.margin)) { + throw std::runtime_error(fmt::format( + "ContinuousCollisionChecker: Options::margin must be a finite " + "nonnegative distance; got {}. Filter a pair out instead of giving it " + "a negative margin.", + options.margin)); + } + if (!(options.distance_resolution > 0.0) || + !std::isfinite(options.distance_resolution)) { + throw std::runtime_error(fmt::format( + "ContinuousCollisionChecker: Options::distance_resolution must be a " + "finite positive distance in meters; got {}.", + options.distance_resolution)); + } +} + +} // namespace + +class ContinuousCollisionChecker::Impl { + public: + Impl(std::shared_ptr> model, + const Options& default_options) + : model_(std::move(model)), + default_options_(default_options), + engine_(*model_), + oracle_(*model_), + pairs_(oracle_.pairs()), + tau_(ComputeTauTable(*model_, pairs_)), + prefilter_(ComputePrefilterTable(engine_, pairs_)), + pool_(*model_, + std::max(1, default_options_.parallelism.num_threads())) {} + + const RobotDiagram& model() const { return *model_; } + + const Options& Resolve(const std::optional& options) const { + return options.has_value() ? *options : default_options_; + } + + Result Check(const internal::PiecewiseBezierPath& path, + const Options& options) const { + ValidateOptions(options); + const int expected = model_->plant().num_positions(); + if (path.num_positions() != expected) { + throw std::runtime_error(fmt::format( + "ContinuousCollisionChecker: the trajectory has {} rows but the " + "plant has {} generalized positions.", + path.num_positions(), expected)); + } + + // The λ table is per call: it depends on the trajectory's control box. + const internal::MotionBoundTable table = + engine_.ComputeMotionBoundTable(path, pairs_); + + internal::CertifierInput input; + input.oracle = &oracle_; + input.table = &table; + input.path = &path; + input.pairs = &pairs_; + input.tau = &tau_; + input.prefilter = &prefilter_; + input.options = options; + return internal::RunCertifier(input, &pool_); + } + + private: + std::shared_ptr> model_; + Options default_options_; + internal::KinematicsEngine engine_; + internal::DistanceOracle oracle_; + std::vector pairs_; + /* τ_p: max(kQueryTolerance, Drake's documented accuracy for the pair). */ + std::vector tau_; + internal::PrefilterTable prefilter_; + mutable internal::ContextPool pool_; +}; + +ContinuousCollisionChecker::ContinuousCollisionChecker( + std::shared_ptr> model, + const Options& default_options) { + if (model == nullptr) { + throw std::runtime_error( + "ContinuousCollisionChecker: the model is null; supply a RobotDiagram " + "whose plant is finalized."); + } + if (!model->plant().is_finalized()) { + throw std::runtime_error( + "ContinuousCollisionChecker: the plant is not finalized; call " + "MultibodyPlant::Finalize() (or RobotDiagramBuilder::Build()) first."); + } + ValidateOptions(default_options); + impl_ = std::make_unique(std::move(model), default_options); +} + +ContinuousCollisionChecker::~ContinuousCollisionChecker() = default; + +Result ContinuousCollisionChecker::CheckTrajectory( + const drake::trajectories::Trajectory& trajectory, + const std::optional& options) const { + const Options& resolved = impl_->Resolve(options); + return impl_->Check(internal::PiecewiseBezierPath::FromTrajectory( + trajectory, resolved.continuous_revolute_indices), + resolved); +} + +Result ContinuousCollisionChecker::CheckPath( + const Eigen::MatrixXd& waypoints, + const std::optional& options) const { + const Options& resolved = impl_->Resolve(options); + const int expected = impl_->model().plant().num_positions(); + if (waypoints.rows() != expected) { + throw std::runtime_error(fmt::format( + "ContinuousCollisionChecker::CheckPath: the waypoint matrix has {} " + "rows but the plant has {} generalized positions (waypoints are " + "columns).", + waypoints.rows(), expected)); + } + return impl_->Check(internal::PiecewiseBezierPath::FromWaypoints(waypoints), + resolved); +} + +Result ContinuousCollisionChecker::CheckEdge( + const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, + const std::optional& options) const { + const int expected = impl_->model().plant().num_positions(); + if (q1.size() != expected || q2.size() != expected) { + throw std::runtime_error(fmt::format( + "ContinuousCollisionChecker::CheckEdge: the endpoints have sizes {} " + "and {} but the plant has {} generalized positions.", + q1.size(), q2.size(), expected)); + } + Eigen::MatrixXd waypoints(expected, 2); + waypoints.col(0) = q1; + waypoints.col(1) = q2; + return CheckPath(waypoints, options); +} + +const RobotDiagram& ContinuousCollisionChecker::model() const { + return impl_->model(); +} + +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/continuous_collision_checker.h b/planning/continuous_collision/continuous_collision_checker.h new file mode 100644 index 000000000000..9bebc5fb0803 --- /dev/null +++ b/planning/continuous_collision/continuous_collision_checker.h @@ -0,0 +1,182 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/parallelism.h" +#include "drake/common/trajectories/trajectory.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace continuous_collision { + +/** Outcome of one check. +@ingroup planning_collision_checker */ +enum class Verdict { + /** Proof: every unfiltered pair keeps signed distance > margin over the + entire continuous time domain. */ + kCertifiedFree, + /** An exactly-on-trajectory configuration violates the threshold. */ + kViolationFound, + /** Some pair's clearance comes within Options::distance_resolution (plus + the oracle tolerance) of the margin, so refining further cannot decide it: + the trajectory grazes the margin. */ + kInconclusive, +}; + +/** Where the plan fails, or where it could not be decided. +@ingroup planning_collision_checker */ +struct Finding { + /** Trajectory time of the witness configuration. */ + double time{}; + /** The witness configuration, exactly on the trajectory. */ + Eigen::VectorXd q; + geometry::GeometryId geometry_a; + geometry::GeometryId geometry_b; + multibody::BodyIndex body_a; + multibody::BodyIndex body_b; + /** Signed distance of the pair at q. */ + double distance{}; + /** Closest points in the world frame at q; present for violations, so that + planners can push the trajectory out of collision. */ + std::optional nearest_a_W; + std::optional nearest_b_W; +}; + +/** Options controlling one check. +@ingroup planning_collision_checker */ +struct Options { + /** Clearance margin δ in meters: the check certifies signed distance + > margin for every unfiltered pair at every time. Must be finite and + nonnegative. */ + double margin{0.0}; + /** Resolution floor r in meters. A pair stops being refined on a node once + its bounded relative motion over that node is at most r; if it is still + undecided there, the check reports Verdict::kInconclusive with that node's + midpoint as the witness. Definitive verdicts are guaranteed for a trajectory + whose clearance stays more than r (plus the oracle tolerance, see the class + documentation) away from the margin everywhere; the cost of a grazing + trajectory grows roughly linearly in 1/r. Must be finite and positive. */ + double distance_resolution{1e-6}; + /** Position coordinates whose junction continuity is checked modulo 2π + (the GcsTrajectoryOptimization continuous-revolute convention). + @see planning::trajectory_optimization::GetContinuousRevoluteJointIndices */ + std::vector continuous_revolute_indices{}; + Parallelism parallelism{Parallelism::Max()}; +}; + +/** Result of one check. +@ingroup planning_collision_checker */ +struct Result { + Verdict verdict{}; + /** The earliest violation, or the inconclusive witness; empty iff the + verdict is Verdict::kCertifiedFree. */ + std::optional finding; + /** Nodes visited by the adaptive subdivision; a cost measure. */ + uint64_t num_nodes{0}; +}; + +/** Certifies, rather than samples, that a trajectory is collision-free over +its entire continuous time domain. + +Guarantee: if a check returns Verdict::kCertifiedFree, then for every time t in +the trajectory's domain and every unfiltered geometry pair (A, B), the signed +distance ϕ_AB(q(t)) exceeds Options::margin. That holds under three +assumptions: exact real arithmetic up to an internal numerical slack, a +distance oracle accurate to its stated tolerance, and Mesh ≡ convex hull. The +proof is a property of the path, so retiming the trajectory afterwards does not +invalidate it. + +Resolution contract: write δ for Options::margin, r for +Options::distance_resolution, τ_p for the oracle tolerance of pair p (at least +1 µm; Drake's documented signed-distance accuracy for that shape combination), +ε for the internal slack (1 nm), and σ_p for the residual motion of coordinates +the trajectory holds constant only to within the continuity tolerance (exactly +zero when they are exactly constant, the common case). Then, for every pair, + - if ϕ_p(q(t)) > δ + r + σ_p + 2τ_p + ε for every t, the pair is certified, + so a trajectory that clears the margin by that much everywhere returns + Verdict::kCertifiedFree; + - if ϕ_p(q(t)) < δ − (r + σ_p + 2τ_p) for some t, the check returns + Verdict::kViolationFound; + - Verdict::kInconclusive is therefore possible only when some pair's + clearance comes within that band of the margin, and its Finding then names + an on-trajectory configuration whose reported distance lies in + [δ − τ_p, δ + τ_p + ε + σ_p + r]. +Resolutions below what double precision can represent along a segment are +capped by a floating-point backstop. + +Thread safety: the Check* methods are const, own no mutable state outside +per-call scratch, and may be called concurrently on one instance from arbitrary +threads. This is stronger than planning::CollisionChecker, whose documentation +requires a per-thread clone for use from threads the checker does not itself +own; no clone is needed here. Construction and destruction are not +thread-safe. +@ingroup planning_collision_checker */ +class ContinuousCollisionChecker { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ContinuousCollisionChecker); + + /** Builds contexts, bounding spheres and topology tables, and runs the + capability probe. + @throws std::exception if `model` is null or its plant is not finalized. + @throws std::exception if `default_options` is invalid; see CheckTrajectory(). + @throws std::exception if a pair's shape combination is unsupported, i.e. a + deformable geometry or halfspace against halfspace. + @throws std::exception if the plant's topology or geometry defeats the motion + bound: a rotating HalfSpace, a reversed joint, a kinematic loop, or a + proximity shape with no bounding sphere. */ + explicit ContinuousCollisionChecker( + std::shared_ptr> model, + const Options& default_options = {}); + + ~ContinuousCollisionChecker(); + + /** Certifies a trajectory (BezierCurve, BsplineTrajectory, + PiecewisePolynomial, or a CompositeTrajectory of those). + @throws std::exception if Options::margin is not a finite nonnegative + distance, or if Options::distance_resolution is not a finite positive + distance. + @throws std::exception if the trajectory's row count differs from the + plant's number of generalized positions. + @throws std::exception if the trajectory is not one of the supported types, + has a segment of degree above 10, or is discontinuous at a junction. + @throws std::exception if Options::continuous_revolute_indices names a + coordinate outside the plant's. + @throws std::exception if the trajectory moves a coordinate of an + unsupported joint type (quaternion floating, ball), or moves a HalfSpace + across a rotational coordinate. */ + Result CheckTrajectory(const trajectories::Trajectory& trajectory, + const std::optional& options = {}) const; + + /** Certifies the piecewise-linear path through the given waypoint columns. + @throws std::exception if `waypoints` has fewer than two columns, or does not + have one row per generalized position of the plant. + @throws std::exception under every condition CheckTrajectory() lists. */ + Result CheckPath(const Eigen::MatrixXd& waypoints, + const std::optional& options = {}) const; + + /** Certifies the straight configuration-space edge q1 → q2. + @throws std::exception if q1 or q2 does not have one entry per generalized + position of the plant. + @throws std::exception under every condition CheckTrajectory() lists. */ + Result CheckEdge(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, + const std::optional& options = {}) const; + + const RobotDiagram& model() const; + + private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/distance_oracle.cc b/planning/continuous_collision/distance_oracle.cc new file mode 100644 index 000000000000..3b431fd3b729 --- /dev/null +++ b/planning/continuous_collision/distance_oracle.cc @@ -0,0 +1,428 @@ +#include "drake/planning/continuous_collision/distance_oracle.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/geometry/proximity/polygon_surface_mesh.h" +#include "drake/geometry/scene_graph.h" +#include "drake/geometry/scene_graph_inspector.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/planning/continuous_collision/internal.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::geometry::GeometryId; +using drake::geometry::QueryObject; +using drake::geometry::SceneGraphInspector; +using drake::math::RigidTransformd; + +/* Everything the analytic halfspace fallback needs about the *non*-halfspace +partner, extracted once by the probe. Only the fields relevant to `klass` are +populated. All quantities are in the geometry's canonical frame G. */ +struct SupportData { + ShapeClass klass{ShapeClass::kUnsupported}; + /* Sphere / Capsule / Cylinder radius. */ + double radius{0.0}; + /* Half the axial length of a Capsule / Cylinder. */ + double half_length{0.0}; + /* Box half-sizes, or Ellipsoid semi-axes (a, b, c). */ + Eigen::Vector3d extent{Eigen::Vector3d::Zero()}; + /* Convex / Mesh: the vertices of the very hull object the proximity engine + collides (`GetConvexHull()`), so scale and any degeneracy inflation Drake + applied are already baked in. */ + Eigen::Matrix3Xd hull_G; +}; + +Eigen::Matrix3Xd HullVertices( + const drake::geometry::PolygonSurfaceMesh& hull) { + Eigen::Matrix3Xd v(3, hull.num_vertices()); + for (int i = 0; i < hull.num_vertices(); ++i) { + v.col(i) = hull.vertex(i); + } + return v; +} + +SupportData MakeSupportData(const drake::geometry::Shape& shape) { + SupportData data; + data.klass = Classify(shape); + switch (data.klass) { + case ShapeClass::kSphere: + data.radius = static_cast(shape).radius(); + break; + case ShapeClass::kBox: + data.extent = + static_cast(shape).size() / 2.0; + break; + case ShapeClass::kCapsule: { + const auto& s = static_cast(shape); + data.radius = s.radius(); + data.half_length = s.length() / 2.0; + break; + } + case ShapeClass::kCylinder: { + const auto& s = static_cast(shape); + data.radius = s.radius(); + data.half_length = s.length() / 2.0; + break; + } + case ShapeClass::kEllipsoid: { + const auto& s = static_cast(shape); + data.extent = Eigen::Vector3d(s.a(), s.b(), s.c()); + break; + } + case ShapeClass::kConvex: + data.hull_G = HullVertices( + static_cast(shape).GetConvexHull()); + break; + case ShapeClass::kMesh: + data.hull_G = HullVertices( + static_cast(shape).GetConvexHull()); + break; + case ShapeClass::kHalfSpace: + case ShapeClass::kUnsupported: + break; + } + return data; +} + +/* Returns argmax over x ∈ C of d_W·x, with C the shape described by `data` +posed at `X_WC` and `d_W` a unit vector -- i.e. the point attaining the +support function h_C(d_W). Each branch is the standard closed form. + +Below R = X_WC.rotation(), c = X_WC.translation(), d_C = Rᵀ·d_W, and +â = R·ẑ is the shape's canonical axis expressed in world. */ +Eigen::Vector3d SupportPoint(const SupportData& data, + const RigidTransformd& X_WC, + const Eigen::Vector3d& d_W) { + const Eigen::Matrix3d& R = X_WC.rotation().matrix(); + const Eigen::Vector3d& c = X_WC.translation(); + switch (data.klass) { + case ShapeClass::kSphere: + return c + data.radius * d_W; + case ShapeClass::kBox: { + // The box is a product of intervals in frame C, so each coordinate + // maximizes independently at the half-size with the sign of d_C. + const Eigen::Vector3d d_C = R.transpose() * d_W; + Eigen::Vector3d corner_C; + for (int i = 0; i < 3; ++i) { + corner_C(i) = (d_C(i) >= 0.0 ? data.extent(i) : -data.extent(i)); + } + return c + R * corner_C; + } + case ShapeClass::kCapsule: { + // Minkowski sum of the axis segment and a ball: support functions add. + const Eigen::Vector3d axis = R.col(2); + const double s = (d_W.dot(axis) >= 0.0 ? 1.0 : -1.0); + return c + (s * data.half_length) * axis + data.radius * d_W; + } + case ShapeClass::kCylinder: { + // Product of the axis segment and a disk in the orthogonal plane, so + // the axial and radial maximizations are independent. + const Eigen::Vector3d axis = R.col(2); + const double s = (d_W.dot(axis) >= 0.0 ? 1.0 : -1.0); + Eigen::Vector3d p = c + (s * data.half_length) * axis; + const Eigen::Vector3d d_perp = d_W - d_W.dot(axis) * axis; + const double norm = d_perp.norm(); + // Near-axis-parallel direction: every rim point ties, so keep the cap + // center. It is still a supporting point and still on the surface (the + // caps are flat disks). + if (norm > 1e-14) { + p += (data.radius / norm) * d_perp; + } + return p; + } + case ShapeClass::kEllipsoid: { + // E = {c + M·u : ‖u‖ ≤ 1} with M = R·diag(a,b,c). Cauchy-Schwarz gives + // max_{‖u‖≤1} d·(c + M·u) = d·c + ‖Mᵀd‖, attained at u* = Mᵀd/‖Mᵀd‖, + // so x* = c + M·Mᵀd/‖Mᵀd‖. M is invertible (radii > 0) and ‖d‖ = 1, so + // ‖Mᵀd‖ ≥ min(a,b,c) > 0. + const Eigen::Matrix3d M = R * data.extent.asDiagonal(); + const Eigen::Vector3d Mt_d = M.transpose() * d_W; + const double norm = Mt_d.norm(); + if (norm <= 0.0) return c; + return c + (M * Mt_d) / norm; + } + case ShapeClass::kConvex: + case ShapeClass::kMesh: { + // The support of a polytope is attained at a vertex; maximize in the + // geometry frame so the rotation is applied only once, at the end. + const Eigen::Vector3d d_C = R.transpose() * d_W; + Eigen::Index best = 0; + (d_C.transpose() * data.hull_G).maxCoeff(&best); + return c + R * data.hull_G.col(best); + } + case ShapeClass::kHalfSpace: + case ShapeClass::kUnsupported: + break; + } + throw std::logic_error( + "DistanceOracle: internal error - no support function for this shape " + "class; the capability probe should have refused it."); +} + +std::string ClassName(ShapeClass klass) { + switch (klass) { + case ShapeClass::kSphere: + return "Sphere"; + case ShapeClass::kBox: + return "Box"; + case ShapeClass::kCapsule: + return "Capsule"; + case ShapeClass::kCylinder: + return "Cylinder"; + case ShapeClass::kEllipsoid: + return "Ellipsoid"; + case ShapeClass::kConvex: + return "Convex"; + case ShapeClass::kMesh: + return "Mesh"; + case ShapeClass::kHalfSpace: + return "HalfSpace"; + case ShapeClass::kUnsupported: + break; + } + return ""; +} + +/* "geometry_name (ShapeType)", for error messages. */ +std::string Describe(const SceneGraphInspector& inspector, + GeometryId id) { + return fmt::format("{} ({})", inspector.GetName(id), + inspector.GetShape(id).type_name()); +} + +/* A distinct unordered shape-type combination, the route it resolved to, and +a representative pair for the probe query and its error message. */ +struct ComboRow { + DistanceRoute route{DistanceRoute::kNative}; + GeometryId example_a; + GeometryId example_b; +}; + +} // namespace + +struct DistanceOracle::Impl { + /* Closed-form support data for every geometry that partners a halfspace. + Keyed by geometry id because the facade hands back its own PairRecord + copies, so SignedDistance() cannot index into pairs_. */ + std::unordered_map support; +}; + +DistanceOracle::DistanceOracle(const RobotDiagram& model) { + auto impl = std::make_shared(); + + const drake::geometry::SceneGraph& scene_graph = model.scene_graph(); + const SceneGraphInspector& inspector = scene_graph.model_inspector(); + const drake::multibody::MultibodyPlant& plant = model.plant(); + + // --- Deformables are out of scope: refuse, naming them. ----------------- + const std::vector deformables = + inspector.GetAllDeformableGeometryIds(); + if (!deformables.empty()) { + std::string names; + for (const GeometryId id : deformables) { + names += fmt::format("\n - {}", inspector.GetName(id)); + } + throw std::runtime_error(fmt::format( + "DistanceOracle: deformable geometries are not supported (certified " + "continuous collision checking assumes rigid bodies whose motion the " + "plant's kinematics describe). Offending geometries:{}", + names)); + } + + // --- Snapshot the unfiltered pairs and classify each one. ---------------- + // GetCollisionCandidates() returns a sorted std::set, so pairs_ is + // deterministic for a given model. + std::map, ComboRow> combos; + + for (const auto& [id_a, id_b] : inspector.GetCollisionCandidates()) { + const drake::multibody::RigidBody* body_a = + plant.GetBodyFromFrameId(inspector.GetFrameId(id_a)); + const drake::multibody::RigidBody* body_b = + plant.GetBodyFromFrameId(inspector.GetFrameId(id_b)); + if (body_a == nullptr || body_b == nullptr) { + throw std::runtime_error(fmt::format( + "DistanceOracle: collision geometry {} is not attached to a " + "MultibodyPlant body; the checker can only certify geometry whose " + "motion the plant describes.", + Describe(inspector, body_a == nullptr ? id_a : id_b))); + } + + const ShapeClass class_a = Classify(inspector.GetShape(id_a)); + const ShapeClass class_b = Classify(inspector.GetShape(id_b)); + + if (class_a == ShapeClass::kHalfSpace && + class_b == ShapeClass::kHalfSpace) { + throw std::runtime_error(fmt::format( + "DistanceOracle: signed distance between two HalfSpace geometries " + "is undefined, so the pair {} / {} cannot be certified. Remove one " + "halfspace, or filter the pair (CollisionFilterManager / a " + "collision filter group).", + Describe(inspector, id_a), Describe(inspector, id_b))); + } + + DistanceRoute route = DistanceRoute::kNative; + if (class_a == ShapeClass::kHalfSpace) { + route = DistanceRoute::kHalfSpaceA; + } else if (class_b == ShapeClass::kHalfSpace) { + route = DistanceRoute::kHalfSpaceB; + } + + if (route != DistanceRoute::kNative) { + // The analytic fallback needs a closed-form support function for the + // partner; anything outside the supported set is refused here rather + // than mid-certification. + const bool a_is_halfspace = (route == DistanceRoute::kHalfSpaceA); + const GeometryId partner = a_is_halfspace ? id_b : id_a; + const ShapeClass partner_class = a_is_halfspace ? class_b : class_a; + if (partner_class == ShapeClass::kUnsupported) { + throw std::runtime_error(fmt::format( + "DistanceOracle: no closed-form support function for shape type " + "'{}', so the halfspace pair {} / {} cannot be certified.", + inspector.GetShape(partner).type_name(), Describe(inspector, id_a), + Describe(inspector, id_b))); + } + if (impl->support.find(partner) == impl->support.end()) { + impl->support.emplace(partner, + MakeSupportData(inspector.GetShape(partner))); + } + } + + const auto key = std::minmax(class_a, class_b); + combos.emplace(std::pair{key.first, key.second}, + ComboRow{route, id_a, id_b}); + + pairs_.push_back( + PairRecord{id_a, id_b, body_a->index(), body_b->index(), route}); + } + + // --- One probe query per distinct native combination. -------------------- + // The whole point of the probe: an unsupported (type, type) combination is + // discovered here, at construction, and never mid-certification. + if (!combos.empty()) { + std::unique_ptr> root_context = + model.CreateDefaultContext(); + const drake::systems::Context& sg_context = + scene_graph.GetMyContextFromRoot(*root_context); + const auto& query_object = + scene_graph.get_query_output_port().Eval>( + sg_context); + + for (const auto& [combo, row] : combos) { + if (row.route != DistanceRoute::kNative) continue; + try { + query_object.ComputeSignedDistancePairClosestPoints(row.example_a, + row.example_b); + } catch (const std::exception& e) { + throw std::runtime_error(fmt::format( + "DistanceOracle: this Drake build cannot compute signed distance " + "for the shape combination ({}, {}); an offending pair is {} / " + "{}. Filter the pair, or replace the geometry with a supported " + "shape (Convex is always supported). Drake reported: {}", + ClassName(combo.first), ClassName(combo.second), + Describe(inspector, row.example_a), + Describe(inspector, row.example_b), e.what())); + } + } + } + + impl_ = std::move(impl); +} + +double DistanceOracle::SignedDistance(const QueryObject& query_object, + const PairRecord& pair, + Eigen::Vector3d* nearest_a_W, + Eigen::Vector3d* nearest_b_W) const { + if (pair.route == DistanceRoute::kNative) { + const drake::geometry::SignedDistancePair result = + query_object.ComputeSignedDistancePairClosestPoints(pair.a, pair.b); + // Drake reports the pair in its own fixed but undocumented order, which + // may be the reverse of this record's; the witness points come back in + // *its* A/B geometry frames, so undo any swap explicitly. + Eigen::Vector3d p_ACa; + Eigen::Vector3d p_BCb; + if (result.id_A == pair.a && result.id_B == pair.b) { + p_ACa = result.p_ACa; + p_BCb = result.p_BCb; + } else if (result.id_A == pair.b && result.id_B == pair.a) { + p_ACa = result.p_BCb; + p_BCb = result.p_ACa; + } else { + throw std::runtime_error( + "DistanceOracle: Drake returned a signed distance result for a " + "different geometry pair than the one queried."); + } + if (nearest_a_W != nullptr) { + *nearest_a_W = query_object.GetPoseInWorld(pair.a) * p_ACa; + } + if (nearest_b_W != nullptr) { + *nearest_b_W = query_object.GetPoseInWorld(pair.b) * p_BCb; + } + return result.distance; + } + + // --- Analytic halfspace fallback (exact). -------------------------------- + // Drake's HalfSpace is {x : n̂·(x - p0) ≤ 0}, with n̂ = R_WG·ẑ the outward + // normal and p0 = X_WG.translation() a point of the boundary plane. For a + // convex partner C, + // ϕ = min_{x ∈ C} n̂·(x - p0) = -h_C(-n̂) - n̂·p0. + // Proof that this is the signed distance on both branches: translating C by + // t·n̂ shifts the minimum by exactly t, and C is disjoint from the halfspace + // iff that minimum is ≥ 0. Hence for ϕ ≥ 0 the pair is separated and the + // minimizer together with its foot on the plane realizes the gap (any point + // of C is at least ϕ from the plane, and the minimizer is exactly ϕ), while + // for ϕ < 0 the smallest translation that separates them has length -ϕ, + // which is Drake's negative-penetration-depth definition. Exact, so this + // route contributes 0 to τ -- but τ accounting stays uniform (the numerical + // policy). + const bool a_is_halfspace = (pair.route == DistanceRoute::kHalfSpaceA); + const GeometryId halfspace_id = a_is_halfspace ? pair.a : pair.b; + const GeometryId partner_id = a_is_halfspace ? pair.b : pair.a; + + const auto it = impl_->support.find(partner_id); + if (it == impl_->support.end()) { + throw std::runtime_error( + "DistanceOracle::SignedDistance(): the pair's halfspace route names a " + "geometry the capability probe never classified. Pass PairRecords " + "obtained from pairs() (thresholds may be rewritten; ids and routes " + "may not)."); + } + + const RigidTransformd& X_WH = query_object.GetPoseInWorld(halfspace_id); + const Eigen::Vector3d n_W = X_WH.rotation().matrix().col(2); + const Eigen::Vector3d p0_W = X_WH.translation(); + const RigidTransformd& X_WC = query_object.GetPoseInWorld(partner_id); + + const Eigen::Vector3d x_W = SupportPoint(it->second, X_WC, -n_W); + const double phi = n_W.dot(x_W - p0_W); + // The halfspace witness is the minimizer's orthogonal projection onto the + // boundary plane; the witness displacement is then exactly ϕ·n̂. + const Eigen::Vector3d plane_W = x_W - phi * n_W; + + if (nearest_a_W != nullptr) { + *nearest_a_W = a_is_halfspace ? plane_W : x_W; + } + if (nearest_b_W != nullptr) { + *nearest_b_W = a_is_halfspace ? x_W : plane_W; + } + return phi; +} + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/distance_oracle.h b/planning/continuous_collision/distance_oracle.h new file mode 100644 index 000000000000..95e68fe82729 --- /dev/null +++ b/planning/continuous_collision/distance_oracle.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/geometry/query_object.h" +#include "drake/planning/continuous_collision/internal.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { + +/* Narrowphase distance abstraction. Stateless per query and +thread-compatible: configuration comes in via the caller's QueryObject. + +Contract: SignedDistance returns ϕ̂ with |ϕ̂ − ϕ_true| ≤ τ_p whenever ϕ_true is +at or above −τ_p, and returns a definitely negative value when the shapes +interpenetrate beyond τ_p. Only over-reporting a distance at or above the +threshold could fake a certificate, which is why the capability probe keeps any +not-a-true-distance backend out of the loop entirely. + +The collision filter state is snapshotted from the model inspector at +construction: pairs() is the set of pairs that were unfiltered *then*. Filter +changes applied to a Context afterwards are not observed, so a checker built +on this oracle keeps certifying the pair set it was constructed with. */ +class DistanceOracle { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(DistanceOracle); + + /* Runs the capability probe: enumerates the unfiltered proximity pairs + from the model's SceneGraph inspector (collision filter state snapshotted at + construction) and classifies every (shape, shape) combination as native, + halfspace-fallback or unsupported. An unsupported pair is reported here, so + one is never discovered mid-certification. + @throws std::exception naming the offending geometries if any pair is + unsupported, i.e. involves a deformable geometry or is halfspace against + halfspace, or if this Drake build cannot compute signed distance for one of + the shape combinations present. */ + explicit DistanceOracle(const RobotDiagram& model); + + /* The unfiltered pairs found by the probe. */ + const std::vector& pairs() const { return pairs_; } + + /* Signed distance for one pair at the configuration already set in the + context that produced `query_object`, optionally reporting the world-frame + closest points. Both routes always fill the optional out-params. + + @throws std::exception if `pair` carries a halfspace route but its + geometries were not classified by this oracle's capability probe. */ + double SignedDistance(const geometry::QueryObject& query_object, + const PairRecord& pair, + Eigen::Vector3d* nearest_a_W = nullptr, + Eigen::Vector3d* nearest_b_W = nullptr) const; + + private: + std::vector pairs_; + + /* Immutable capability-probe results: closed-form support data for every + halfspace partner. Held by shared_ptr so the oracle stays cheaply copyable + and thread-compatible; the probe output is never mutated after + construction. */ + struct Impl; + std::shared_ptr impl_; +}; + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/internal.h b/planning/continuous_collision/internal.h new file mode 100644 index 000000000000..96c45e63644e --- /dev/null +++ b/planning/continuous_collision/internal.h @@ -0,0 +1,145 @@ +#pragma once + +/* @file +Numerical policy, shape classification and the pair record shared by every +translation unit of this package. Nothing here is public API. + +Numerics. Let ϕ̂ be the oracle's reported signed distance at the node's +representative configuration, τ the oracle accuracy contract (|ϕ̂ − ϕ_true| ≤ τ +on the at-or-above-threshold branch), Δ the motion bound for the node, m the +threshold (Options::margin), and ε the numerical slack. + + - Certified: ϕ̂ − τ − Δ > m + ε (sound by the displacement lemma: + every configuration on the node keeps clearance > m). + - Definite violation: ϕ̂ + τ < m (the true clearance at an exactly + on-trajectory configuration is below threshold). + - Otherwise the pair is gray and drives subdivision, until the node's travel + bound Σ λ·w for that pair is at most r (Options::distance_resolution); + a pair still gray there is reported as inconclusive. + +The certificate is mathematical modulo τ and ε. ε is 1e-9 m, which dominates +the accumulated floating-point error of the w, λ and dot-product expression +depths involved. + +TODO(wernerpe): Harden the arithmetic with directed rounding, so that the +certificate holds without the ε slack. */ + +#include + +#include "drake/common/unused.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/geometry/shape_specification.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { + +/* Junction C0-continuity tolerance (per coordinate; modulo 2π for coordinates + listed in Options::continuous_revolute_indices). Doubles as the width below + which a coordinate's global control-point range counts as constant. */ +constexpr double kContinuityTolerance = 1e-7; + +/* τ: the distance oracle's baseline accuracy contract in meters. The per-pair + τ_p also charges Drake's documented per-shape-combination accuracy. */ +constexpr double kQueryTolerance = 1e-6; + +/* ε: swallows floating-point noise in the bound arithmetic. */ +constexpr double kNumericalSlack = 1e-9; + +/* Maximum polynomial degree accepted for monomial→Bernstein conversion. */ +constexpr int kMaxConversionDegree = 10; + +/* True iff the pair is certified on the whole node. */ +inline bool IsCertified(double phi_hat, double tau, double motion_bound, + double threshold, double slack = kNumericalSlack) { + return phi_hat - tau - motion_bound > threshold + slack; +} + +/* True iff the representative configuration is a definite violation. */ +inline bool IsDefiniteViolation(double phi_hat, double tau, double threshold) { + return phi_hat + tau < threshold; +} + +/* The closed set of shape classes this package recognizes. The enumerator + values are the row and column indices of the documented-accuracy table in + continuous_collision_checker.cc, so they must stay contiguous from zero. + Anything outside the set is `kUnsupported` and is refused by the oracle's + capability probe, mirroring the throw-on-unknown-shape rule + ComputeBoundingSphere() uses. */ +enum class ShapeClass { + kSphere = 0, + kBox = 1, + kCapsule = 2, + kCylinder = 3, + kEllipsoid = 4, + kConvex = 5, + kMesh = 6, + kHalfSpace = 7, + kUnsupported = 8, +}; + +constexpr int kNumShapeClasses = 9; + +/* Classifies `shape` into the set above. */ +inline ShapeClass Classify(const geometry::Shape& shape) { + return shape.Visit([](const auto& s) { + using S = std::decay_t; + unused(s); + if constexpr (std::is_same_v) { + return ShapeClass::kSphere; + } else if constexpr (std::is_same_v) { + return ShapeClass::kBox; + } else if constexpr (std::is_same_v) { + return ShapeClass::kCapsule; + } else if constexpr (std::is_same_v) { + return ShapeClass::kCylinder; + } else if constexpr (std::is_same_v) { + return ShapeClass::kEllipsoid; + } else if constexpr (std::is_same_v) { + return ShapeClass::kConvex; + } else if constexpr (std::is_same_v) { + return ShapeClass::kMesh; + } else if constexpr (std::is_same_v) { + return ShapeClass::kHalfSpace; + } else { + return ShapeClass::kUnsupported; + } + }); +} + +/* True iff `shape` is a HalfSpace. A halfspace is unbounded, so it has no + bounding sphere, and Drake computes signed distance against it only for a + Sphere partner. */ +inline bool IsHalfSpace(const geometry::Shape& shape) { + return Classify(shape) == ShapeClass::kHalfSpace; +} + +/* How the oracle computes signed distance for one pair, resolved once by the + capability probe: no per-query dispatch decisions. */ +enum class DistanceRoute { + /* QueryObject::ComputeSignedDistancePairClosestPoints. */ + kNative, + /* Analytic halfspace support-function fallback; geometry `a` is the + halfspace. */ + kHalfSpaceA, + /* Same, geometry `b` is the halfspace. */ + kHalfSpaceB, +}; + +/* One unfiltered proximity geometry pair with its pre-resolved distance + route. The threshold is not carried here: it is Options::margin, uniform over + the pairs. */ +struct PairRecord { + geometry::GeometryId a; + geometry::GeometryId b; + multibody::BodyIndex body_a; + multibody::BodyIndex body_b; + DistanceRoute route{DistanceRoute::kNative}; +}; + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/motion_bound_table.cc b/planning/continuous_collision/motion_bound_table.cc new file mode 100644 index 000000000000..990c301548d1 --- /dev/null +++ b/planning/continuous_collision/motion_bound_table.cc @@ -0,0 +1,1055 @@ +#include "drake/planning/continuous_collision/motion_bound_table.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_throw.h" +#include "drake/geometry/geometry_roles.h" +#include "drake/geometry/proximity/polygon_surface_mesh.h" +#include "drake/geometry/scene_graph_inspector.h" +#include "drake/geometry/shape_specification.h" +#include "drake/multibody/tree/joint.h" +#include "drake/multibody/tree/screw_joint.h" +#include "drake/multibody/tree/weld_joint.h" +#include "drake/planning/continuous_collision/internal.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { + +using drake::geometry::GeometryId; +using drake::geometry::Role; +using drake::geometry::Shape; +using drake::math::RigidTransform; +using drake::multibody::BodyIndex; +using drake::multibody::Joint; +using drake::multibody::JointIndex; +using drake::multibody::MultibodyPlant; +using drake::multibody::ScrewJoint; +using drake::multibody::WeldJoint; + +namespace { + +using drake::geometry::Box; +using drake::geometry::Capsule; +using drake::geometry::Convex; +using drake::geometry::Cylinder; +using drake::geometry::Ellipsoid; +using drake::geometry::Mesh; +using drake::geometry::PolygonSurfaceMesh; +using drake::geometry::ShapeReifier; +using drake::geometry::Sphere; + +/* Computes the bounding sphere of a supported shape posed at X_LG in a body + (link) frame L. + + Every formula below is an *exact containment* statement about the shape's + canonical frame G: `radius` is the circumradius of the shape about Go, and the + sphere is centred at Go's image in L, i.e. c_L = X_LG.translation(). Because + the rotation part of X_LG is an isometry, ‖X_LG·p − c_L‖ = ‖R_LG·p‖ = ‖p‖ for + every material point p of the shape, so containment in L follows from + containment in G with no dependence on the orientation. That is why the centre + never needs a search and the radius never needs inflating for rotation. + + The origin-centred radius the reach chain consumes is ‖c_L‖ + radius, sound by + the triangle inequality; the tighter centre is what the broadphase prefilter + wants. + + An under-bounding formula produces an unsound λ with no other symptom, so this + reifier enumerates the closed set of supported shapes and lets every other + shape fall through to ShapeReifier's default, which routes to + ThrowUnsupportedGeometry() below. */ +class BoundingSphereReifier final : public ShapeReifier { + public: + explicit BoundingSphereReifier(const RigidTransform& X_LG) + : X_LG_(X_LG) {} + + const BoundingSphere& sphere() const { return sphere_; } + + /* Pulls in ShapeReifier's throwing defaults for every shape this class does + not override below (HalfSpace, MeshcatCone, and any shape a future Drake + adds). The overrides declared after it hide the corresponding defaults. */ + using ShapeReifier::ImplementGeometry; + + void ImplementGeometry(const Sphere& sphere, void*) final { + SetCentered(sphere.radius()); + } + + void ImplementGeometry(const Box& box, void*) final { + // Drake's Box stores FULL side lengths, so the circumradius about the box + // centre is half the space diagonal: max over the 8 corners + // (±w/2, ±d/2, ±h/2) of ‖c‖ = ½·√(w² + d² + h²). + SetCentered(0.5 * box.size().norm()); + } + + void ImplementGeometry(const Capsule& capsule, void*) final { + // Spine segment [−L/2, L/2]·ẑ inflated by r; the farthest point is a pole. + SetCentered(0.5 * capsule.length() + capsule.radius()); + } + + void ImplementGeometry(const Cylinder& cylinder, void*) final { + // The farthest point from Go is always on a rim. For a point + // p = z·ẑ + r'·û with |z| ≤ L/2, r' ≤ r and û ⊥ ẑ, + // ‖p‖² = z² + r'², + // which is maximised at |z| = L/2 and r' = r, so R = √(r² + (L/2)²). + // Cap-disk interior points (r' < r) and lateral points with |z| < L/2 are + // both strictly dominated. An origin-centred form of the same argument + // would pick up the ‖t‖ cross terms; here the centre rides along with the + // geometry, so only the canonical-frame extent matters. + SetCentered(std::hypot(cylinder.radius(), 0.5 * cylinder.length())); + } + + void ImplementGeometry(const Ellipsoid& ellipsoid, void*) final { + // ‖diag(a,b,c)·u‖ ≤ max(a,b,c)·‖u‖ for every unit u, with equality along + // the largest semi-axis: exact for the axis-aligned ellipsoid in its own + // frame, which is all this centre-following sphere needs. + SetCentered(std::max({ellipsoid.a(), ellipsoid.b(), ellipsoid.c()})); + } + + void ImplementGeometry(const Convex& convex, void*) final { + SetFromHull(convex.GetConvexHull()); + } + + void ImplementGeometry(const Mesh& mesh, void*) final { + // Drake collides a Mesh as its convex hull in signed-distance queries, and + // the hull contains the mesh, so bounding the hull bounds the geometry + // actually checked. + SetFromHull(mesh.GetConvexHull()); + } + + private: + void ThrowUnsupportedGeometry(const std::string& shape_name) final { + throw std::runtime_error(fmt::format( + "ComputeBoundingSphere(): does not support the shape " + "type '{}'. Supported proximity shapes are Sphere, Box, Capsule, " + "Cylinder, Ellipsoid, Convex and Mesh. HalfSpace has no finite " + "bounding sphere and is governed by dedicated rules instead: it must " + "be anchored, or move only by translation relative to its partner. " + "Any other shape must be replaced by a Convex/Mesh approximation " + "before it can be certified.", + shape_name)); + } + + /* Sets the sphere centred on the geometry frame origin's image in L, with + the given circumradius about that origin. */ + void SetCentered(double radius_about_Go) { + DRAKE_DEMAND(std::isfinite(radius_about_Go)); + DRAKE_DEMAND(radius_about_Go >= 0.0); + sphere_.center_L = X_LG_.translation(); + sphere_.radius = radius_about_Go; + } + + /* Centroid-centred sphere over the hull vertices. Unlike the primitives this + sphere is NOT centred on Go: the centroid is a much better centre for the + broadphase prefilter, and ‖c_L‖ + radius still bounds the origin-centred + reach the λ chain needs. The hull is a convex polytope, so containing every + vertex contains the whole shape. */ + void SetFromHull(const PolygonSurfaceMesh& hull) { + const int num_vertices = hull.num_vertices(); + // Drake's hull computation refuses degenerate vertex sets, so a hull + // always has at least a tetrahedron's worth of vertices; assert the + // non-empty precondition the centroid needs regardless. + DRAKE_DEMAND(num_vertices > 0); + Eigen::Vector3d centroid_L = Eigen::Vector3d::Zero(); + for (int v = 0; v < num_vertices; ++v) { + centroid_L += X_LG_ * hull.vertex(v); + } + centroid_L /= static_cast(num_vertices); + double radius = 0.0; + for (int v = 0; v < num_vertices; ++v) { + radius = std::max(radius, (X_LG_ * hull.vertex(v) - centroid_L).norm()); + } + sphere_.center_L = centroid_L; + sphere_.radius = radius; + } + + const RigidTransform& X_LG_; + BoundingSphere sphere_; +}; + +} // namespace + +BoundingSphere ComputeBoundingSphere(const Shape& shape, + const RigidTransform& X_LG) { + BoundingSphereReifier reifier(X_LG); + shape.Reify(&reifier); + const BoundingSphere& result = reifier.sphere(); + // A zero or non-finite radius under-bounds every λ built on it, so + // re-assert the postcondition every caller relies on. + DRAKE_DEMAND(std::isfinite(result.radius) && result.radius >= 0.0); + DRAKE_DEMAND(result.center_L.allFinite()); + return result; +} + +std::vector> MotionBoundTable::GetEntries( + int pair_index) const { + DRAKE_THROW_UNLESS(pair_index >= 0 && pair_index < num_pairs()); + std::vector> out; + out.reserve(row_start_[pair_index + 1] - row_start_[pair_index]); + for (int e = row_start_[pair_index]; e < row_start_[pair_index + 1]; ++e) { + out.emplace_back(coord_[e], lambda_[e]); + } + return out; +} + +KinematicsEngine::KinematicsEngine(const RobotDiagram& model) + : model_(&model), plant_(&model.plant()) { + if (!plant_->is_finalized()) { + throw std::runtime_error( + "KinematicsEngine: requires a finalized " + "MultibodyPlant; call Finalize() before building the checker."); + } + BuildTopology(); + BuildGeometry(); + CheckHalfSpaceRule(); +} + +void KinematicsEngine::BuildTopology() { + const MultibodyPlant& plant = *plant_; + num_positions_ = plant.num_positions(); + num_bodies_ = plant.num_bodies(); + + // ------------------------------------------------------------------ + // 1. Classify every joint (welds included) and cache its per-hop fixed + // translation norms. + // ------------------------------------------------------------------ + const std::vector& joint_indices = plant.GetJointIndices(); + int max_joint_index = -1; + for (JointIndex ji : joint_indices) { + max_joint_index = std::max(max_joint_index, static_cast(ji)); + } + joint_ordinal_.assign(max_joint_index + 1, -1); + joints_.clear(); + joints_.reserve(joint_indices.size()); + + for (JointIndex ji : joint_indices) { + const Joint& joint = plant.get_joint(ji); + JointRecord rec; + rec.index = ji; + rec.name = joint.name(); + rec.type_name = joint.type_name(); + rec.num_positions = joint.num_positions(); + rec.position_start = rec.num_positions > 0 ? joint.position_start() : 0; + + // `coord_rules` classifies each coordinate for the carve-out residual + // (see ComputeMotionBoundTable). For the supported kinds it mirrors the + // λ switch in the CSR assembly, coordinate for coordinate; the two must + // agree, and the property test in test/motion_bound_test.cc pins that. + using R = CoordRule; + bool translation_known = false; + if (rec.type_name == WeldJoint::kTypeName) { + rec.kind = JointKind::kWeld; + translation_known = true; + } else if (rec.type_name == "revolute") { + rec.kind = JointKind::kRevolute; + rec.coord_rules = {R::kRotation}; + translation_known = true; + } else if (rec.type_name == "prismatic") { + rec.kind = JointKind::kPrismatic; + rec.coord_rules = {R::kTranslation}; + translation_known = true; + } else if (rec.type_name == "planar") { + rec.kind = JointKind::kPlanar; + // q = (x, y, θ); see PlanarJoint's class documentation. + rec.coord_rules = {R::kTranslation, R::kTranslation, R::kRotation}; + translation_known = true; + } else if (rec.type_name == ScrewJoint::kTypeName) { + rec.kind = JointKind::kScrew; + rec.screw_pitch = + dynamic_cast&>(joint).screw_pitch(); + rec.coord_rules = {R::kScrewCoord}; + translation_known = true; + } else if (rec.type_name == "quaternion_floating") { + // q = (q_FM wxyz, p_FM): the translation lives in coordinates 4..6. + rec.kind = JointKind::kUnsupported; + rec.translation_offsets = {4, 5, 6}; + rec.coord_rules = {R::kQuaternion, R::kQuaternion, R::kQuaternion, + R::kQuaternion, R::kTranslation, R::kTranslation, + R::kTranslation}; + translation_known = true; + } else if (rec.type_name == "rpy_floating") { + // q = (rpy, p_FM): the translation lives in coordinates 3..5. + rec.kind = JointKind::kUnsupported; + rec.translation_offsets = {3, 4, 5}; + rec.coord_rules = {R::kRotation, R::kRotation, R::kRotation, + R::kTranslation, R::kTranslation, R::kTranslation}; + translation_known = true; + } else if (rec.type_name == "ball_rpy" || rec.type_name == "universal") { + // Pure rotation about coincident origins: X_FM has zero translation. + rec.kind = JointKind::kUnsupported; + rec.coord_rules.assign(rec.num_positions, R::kRotation); + translation_known = true; + } else { + // A shape of joint this library has never been taught. It cannot even + // contribute a chain hop safely, so it is rejected unconditionally in + // ComputeMotionBoundTable(). It gets no coord_rules either: without + // knowing what its coordinates *are*, no λ̃ can be written down. + rec.kind = JointKind::kUnsupported; + translation_known = false; + } + rec.translation_offsets_known = translation_known; + if (translation_known && rec.num_positions > 0) { + // Every coordinate of a joint we admit must have a carve-out rule, or + // a carved coordinate could slip through uncharged. + DRAKE_DEMAND(static_cast(rec.coord_rules.size()) == + rec.num_positions); + } + + // Frame offsets: F = frame_on_parent (Jp), M = frame_on_child (Jc). + // ‖p_PF‖ and ‖p_CM‖ are the two configuration-independent legs of one hop + // across this joint; the middle leg is the translation of X_FM, which is + // zero for a revolute, fixed for a weld, and box-bounded otherwise. + RigidTransform X_PF; + RigidTransform X_CM; + try { + X_PF = joint.frame_on_parent().GetFixedPoseInBodyFrame(); + X_CM = joint.frame_on_child().GetFixedPoseInBodyFrame(); + } catch (const std::exception& e) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: joint '{}' ({}) is mounted on a frame whose pose " + "in its body is not fixed, so its chain contribution to the reach " + "bound cannot be computed at construction time. Mount joints on body " + "frames or FixedOffsetFrames. Underlying error: {}", + rec.name, rec.type_name, e.what())); + } + rec.p_CM_norm = X_CM.translation().norm(); + rec.fixed_hop = rec.p_CM_norm + X_PF.translation().norm(); + if (rec.kind == JointKind::kWeld) { + rec.fixed_hop += dynamic_cast&>(joint) + .X_FM() + .translation() + .norm(); + } + + joint_ordinal_[ji] = static_cast(joints_.size()); + joints_.push_back(std::move(rec)); + } + + // ------------------------------------------------------------------ + // 2. Orient the joint graph into the world-rooted multibody tree by a + // breadth-first walk from the world over the (body, joint) graph. Post + // Finalize() every non-world body has exactly one inboard joint + // (ephemeral floating joints included), so the walk is well defined. + // + // The walk is what supplies the inboard/outboard orientation and the + // per-hop reach data, neither of which the plant exposes. The descendant + // sets it implies are *not* what the λ table then uses: step 3 takes each + // joint's subtree from the plant's own GetBodiesKinematicallyAffectedBy() + // and throws if the two disagree. The walk is therefore an independent + // cross-check of Drake's answer rather than a substitute for it. + // ------------------------------------------------------------------ + std::vector> incident(num_bodies_); + for (int k = 0; k < static_cast(joints_.size()); ++k) { + const Joint& joint = plant.get_joint(joints_[k].index); + incident[joint.parent_body().index()].push_back(k); + incident[joint.child_body().index()].push_back(k); + } + + inboard_joint_.assign(num_bodies_, -1); + std::vector visited(num_bodies_, false); + const BodyIndex world = plant.world_body().index(); + visited[world] = true; + std::queue bfs; + bfs.push(world); + while (!bfs.empty()) { + const BodyIndex b = bfs.front(); + bfs.pop(); + for (int k : incident[b]) { + const Joint& joint = plant.get_joint(joints_[k].index); + const BodyIndex parent = joint.parent_body().index(); + const BodyIndex child = joint.child_body().index(); + const BodyIndex other = (parent == b) ? child : parent; + if (visited[other]) continue; + visited[other] = true; + inboard_joint_[other] = k; + joints_[k].inboard = b; + joints_[k].outboard = other; + bfs.push(other); + } + } + for (int b = 0; b < num_bodies_; ++b) { + if (!visited[b]) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: body '{}' is not connected to the world through " + "the plant's joints; the kinematics module requires the single " + "world-rooted tree a finalized MultibodyPlant provides.", + plant.get_body(BodyIndex(b)).name())); + } + } + for (const JointRecord& rec : joints_) { + if (!rec.outboard.is_valid()) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: joint '{}' ({}) closes a kinematic loop (both of " + "its bodies are already reachable from the world without it). The " + "motion bound is defined over a single world-rooted tree, so the " + "loop-closing joint must be removed; express the constraint it " + "carried with a MultibodyPlant constraint instead.", + rec.name, rec.type_name)); + } + } + + // Descendant sets implied by the tree we just built: walking each body up to + // the world marks it into every joint it hangs below. O(#bodies × depth). + std::vector> tree_subtree( + joints_.size(), std::vector(num_bodies_, false)); + for (int b = 0; b < num_bodies_; ++b) { + int k = inboard_joint_[b]; + int guard = 0; + while (k >= 0) { + tree_subtree[k][b] = true; + k = inboard_joint_[joints_[k].inboard]; + DRAKE_DEMAND(++guard <= num_bodies_ + 1); + } + } + + // ------------------------------------------------------------------ + // 3. Subtree membership S_j for the positioned joints, taken from Drake so + // J(p) matches the plant's own notion of "kinematically affected", and + // cross-checked against the tree walk above (they must agree; a + // disagreement would mean the chain walk and J(p) disagree about which + // side is distal, which is a soundness hazard). + // ------------------------------------------------------------------ + positioned_order_.clear(); + for (int k = 0; k < static_cast(joints_.size()); ++k) { + JointRecord& rec = joints_[k]; + const Joint& joint = plant.get_joint(rec.index); + if (joint.num_velocities() == 0) { + DRAKE_DEMAND(rec.num_positions == 0); + continue; + } + DRAKE_DEMAND(rec.num_positions > 0); + if (rec.outboard != joint.child_body().index()) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: joint '{}' ({}) is reversed: its declared parent " + "body '{}' is outboard of its declared child body '{}' in the " + "multibody tree. The frame that stays fixed under the joint's motion " + "is then on the outboard side, which the reach chain does not model. " + "Re-declare the joint with the inboard body as its parent.", + rec.name, rec.type_name, joint.parent_body().name(), + joint.child_body().name())); + } + rec.subtree.assign(num_bodies_, false); + for (BodyIndex b : plant.GetBodiesKinematicallyAffectedBy({rec.index})) { + rec.subtree[b] = true; + } + if (rec.subtree != tree_subtree[k]) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: the plant's kinematically-affected set for joint " + "'{}' ({}) disagrees with the world-rooted tree walk over the same " + "joints, so which side of a pair is distal to this joint is " + "ambiguous. This model's topology is not supported.", + rec.name, rec.type_name)); + } + positioned_order_.push_back(k); + } + std::sort(positioned_order_.begin(), positioned_order_.end(), + [this](int a, int b) { + return joints_[a].position_start < joints_[b].position_start; + }); + + // Position coordinate -> owning joint ordinal (every coordinate is owned). + coord_joint_.assign(num_positions_, -1); + for (int k : positioned_order_) { + const JointRecord& rec = joints_[k]; + for (int c = rec.position_start; c < rec.position_start + rec.num_positions; + ++c) { + DRAKE_DEMAND(c >= 0 && c < num_positions_); + DRAKE_DEMAND(coord_joint_[c] == -1); + coord_joint_[c] = k; + } + } + for (int c = 0; c < num_positions_; ++c) { + DRAKE_DEMAND(coord_joint_[c] >= 0); + } +} + +void KinematicsEngine::BuildGeometry() { + const MultibodyPlant& plant = *plant_; + const auto& inspector = model_->scene_graph().model_inspector(); + + body_radius_.assign(num_bodies_, 0.0); + body_has_halfspace_.assign(num_bodies_, false); + body_halfspace_name_.assign(num_bodies_, std::string{}); + + for (int b = 0; b < num_bodies_; ++b) { + const BodyIndex body(b); + DRAKE_DEMAND(plant.get_body(body).index() == body); + const std::optional frame_id = + plant.GetBodyFrameIdIfExists(body); + if (!frame_id.has_value()) continue; + for (GeometryId gid : + inspector.GetGeometries(*frame_id, Role::kProximity)) { + const Shape& shape = inspector.GetShape(gid); + if (IsHalfSpace(shape)) { + // Half spaces are unbounded: they get no bounding sphere, and + // CheckHalfSpaceRule() keeps them off the distal side of any + // rotational coordinate. + body_has_halfspace_[b] = true; + if (body_halfspace_name_[b].empty()) { + body_halfspace_name_[b] = inspector.GetName(gid); + } + continue; + } + BoundingSphere sphere; + try { + sphere = ComputeBoundingSphere(shape, inspector.GetPoseInFrame(gid)); + } catch (const std::exception& e) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: proximity geometry '{}' on body '{}' cannot be " + "bounded. {}", + inspector.GetName(gid), plant.get_body(body).name(), e.what())); + } + // Origin-centred radius for the reach chain: ‖c_L‖ + ρ bounds every + // point of the geometry's distance from the body frame origin, by the + // triangle inequality on the sphere that contains it. + body_radius_[b] = + std::max(body_radius_[b], sphere.center_L.norm() + sphere.radius); + geometry_spheres_.emplace(gid, sphere); + } + } +} + +void KinematicsEngine::CheckHalfSpaceRule() const { + const MultibodyPlant& plant = *plant_; + const auto& inspector = model_->scene_graph().model_inspector(); + + for (const auto& [ga, gb] : inspector.GetCollisionCandidates()) { + const bool a_is_half = IsHalfSpace(inspector.GetShape(ga)); + const bool b_is_half = IsHalfSpace(inspector.GetShape(gb)); + if (!a_is_half && !b_is_half) continue; + const drake::multibody::RigidBody* body_a = + plant.GetBodyFromFrameId(inspector.GetFrameId(ga)); + const drake::multibody::RigidBody* body_b = + plant.GetBodyFromFrameId(inspector.GetFrameId(gb)); + DRAKE_THROW_UNLESS(body_a != nullptr && body_b != nullptr); + const BodyIndex ia = body_a->index(); + const BodyIndex ib = body_b->index(); + + for (int k : positioned_order_) { + const JointRecord& rec = joints_[k]; + const bool in_a = rec.subtree[ia]; + const bool in_b = rec.subtree[ib]; + if (in_a == in_b) continue; + // The distal side is the one inside S_j; only *it* needs a finite reach. + // A half space that is merely the static partner of a rotating body is + // fine: λ then bounds the partner's points, and signed distance is + // symmetric, so the certificate still holds. + const bool distal_is_halfspace = in_a ? a_is_half : b_is_half; + if (!distal_is_halfspace) continue; + const bool rotational = rec.kind == JointKind::kRevolute || + rec.kind == JointKind::kScrew || + rec.kind == JointKind::kPlanar; + if (!rotational) continue; + const GeometryId offender = in_a ? ga : gb; + const GeometryId partner = in_a ? gb : ga; + throw std::runtime_error(fmt::format( + "KinematicsEngine: HalfSpace geometry '{}' (body '{}') rotates " + "relative to its unfiltered partner geometry '{}' (body '{}') " + "through joint '{}' ({}). A half space has unbounded reach, so no " + "finite motion bound λ exists for that pair. Fix the model by " + "anchoring the half space, filtering the pair, or replacing the " + "half space with a large Box.", + inspector.GetName(offender), plant.get_body(in_a ? ia : ib).name(), + inspector.GetName(partner), plant.get_body(in_a ? ib : ia).name(), + rec.name, rec.type_name)); + } + } +} + +std::vector KinematicsEngine::CoordinatesAffectingPair( + BodyIndex body_a, BodyIndex body_b) const { + DRAKE_THROW_UNLESS(body_a.is_valid() && body_a < num_bodies_); + DRAKE_THROW_UNLESS(body_b.is_valid() && body_b < num_bodies_); + std::vector out; + for (int k : positioned_order_) { + const JointRecord& rec = joints_[k]; + // Joint j ∈ J(p) iff exactly one of the pair's bodies is outboard of it: + // only then does moving j change the pair's relative pose. + if (rec.subtree[body_a] == rec.subtree[body_b]) continue; + for (int c = rec.position_start; c < rec.position_start + rec.num_positions; + ++c) { + out.push_back(c); + } + } + return out; +} + +double KinematicsEngine::Reach(int joint_ord, BodyIndex body, + const std::vector& box_hop) const { + // r(j, B): distance from joint j's outboard (M) frame origin to any point of + // B's proximity geometry, bounded uniformly over the control box. + // + // The walk accumulates translation norms only. Every hop composes rigid + // transforms, and a rotation preserves norms, so by the triangle inequality + // ‖X_PF · X_FM · X_MC · p_C‖ ≤ ‖p_PF‖ + ‖t_FM‖ + ‖p_MC‖ + ‖p_C‖, + // with ‖p_MC‖ = ‖p_CM‖. Only the middle term depends on the configuration, + // and box_hop[] holds a uniform bound on it over the control box. + double r = body_radius_[body]; + BodyIndex b = body; + for (int guard = 0; guard <= num_bodies_; ++guard) { + const int k = inboard_joint_[b]; + DRAKE_DEMAND(k >= 0); + if (k == joint_ord) { + // Top of the chain: measure from j's M-frame origin, the point that + // stays fixed when coordinate j moves (for a revolute, the axis passes + // through it). j's own X_FM and parent-side offset are excluded. + return r + joints_[k].p_CM_norm; + } + r += joints_[k].fixed_hop + box_hop[k]; + b = joints_[k].inboard; + } + throw std::runtime_error( + "KinematicsEngine: internal error: the reach chain walk did not reach " + "the requested joint. This indicates inconsistent topology tables."); +} + +MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( + const PiecewiseBezierPath& path, + const std::vector& pairs) const { + if (path.num_positions() != num_positions_) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: the path has {} positions but the plant has {}.", + path.num_positions(), num_positions_)); + } + return ComputeMotionBoundTable(path.global_lower_bound(), + path.global_upper_bound(), + path.constant_coordinates(), pairs); +} + +MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( + const Eigen::VectorXd& lower, const Eigen::VectorXd& upper, + const std::vector& constant_coordinates, + const std::vector& pairs) const { + if (lower.size() != num_positions_ || upper.size() != num_positions_ || + static_cast(constant_coordinates.size()) != num_positions_) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: control-box size mismatch: got lower={}, upper={}, " + "constant_coordinates={} for a plant with {} positions.", + lower.size(), upper.size(), constant_coordinates.size(), + num_positions_)); + } + for (int c = 0; c < num_positions_; ++c) { + if (!std::isfinite(lower[c]) || !std::isfinite(upper[c]) || + lower[c] > upper[c]) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: the trajectory's global control box is invalid at " + "coordinate {}: [{}, {}].", + c, lower[c], upper[c])); + } + } + + const auto abs_max = [&lower, &upper](int c) { + return std::max(std::abs(lower[c]), std::abs(upper[c])); + }; + // The carve-out flags a coordinate constant when its whole control-point + // range collapses to within kContinuityTolerance. That is a + // tolerance, not an identity, and `range` is what the residual is charged + // against. + const auto range = [&lower, &upper](int c) { + return upper[c] - lower[c]; + }; + + // ------------------------------------------------------------------ + // Per-joint, box-dependent bound on ‖translation(X_FM)‖. This is the only + // part of a chain hop that varies with the configuration; taking the max + // over the trajectory's *control box* (not the plant's joint limits) keeps + // unbounded prismatic joints usable and makes every reach trajectory + // adaptive. + // ------------------------------------------------------------------ + std::vector box_hop(joints_.size(), 0.0); + for (int k = 0; k < static_cast(joints_.size()); ++k) { + const JointRecord& rec = joints_[k]; + const int ps = rec.position_start; + switch (rec.kind) { + case JointKind::kWeld: + // Fixed X_FM; already folded into fixed_hop at construction. + break; + case JointKind::kRevolute: + // X_FM is a pure rotation about a point: zero translation. + break; + case JointKind::kPrismatic: + box_hop[k] = abs_max(ps); + break; + case JointKind::kPlanar: + // p_FoMo_F = (x, y, 0); ‖(x, y)‖ ≤ ‖(max|x|, max|y|)‖ over the box. + box_hop[k] = std::hypot(abs_max(ps), abs_max(ps + 1)); + break; + case JointKind::kScrew: + // Drake's screw pitch is meters of travel per full revolution, so the + // helix advances |θ|·|pitch| / 2π meters. + box_hop[k] = abs_max(ps) * std::abs(rec.screw_pitch) / (2 * M_PI); + break; + case JointKind::kUnsupported: { + for (int c = ps; c < ps + rec.num_positions; ++c) { + if (!constant_coordinates[c]) { + // TODO(wernerpe): Support quaternion coordinates via a + // manifold-curve bound. + throw std::runtime_error(fmt::format( + "KinematicsEngine: this trajectory moves coordinate {} of " + "joint '{}', whose type '{}' is not supported. Quaternion " + "coordinates are not a vector space, so Bézier interpolation " + "of their components has no rotation-space meaning and the " + "convex-hull motion bound does not apply. Supported joint " + "types are revolute, prismatic, planar, screw and weld; a " + "floating base whose pose is *constant* along the trajectory " + "is accepted via the constant-coordinate carve-out.", + c, rec.name, rec.type_name)); + } + } + if (!rec.translation_offsets_known) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: joint '{}' has type '{}', which this library " + "does not know how to bound even when held constant. Supported " + "joint types are revolute, prismatic, planar, screw and weld.", + rec.name, rec.type_name)); + } + double sum_sq = 0.0; + for (int off : rec.translation_offsets) { + const double m = abs_max(ps + off); + sum_sq += m * m; + } + box_hop[k] = std::sqrt(sum_sq); + break; + } + } + DRAKE_DEMAND(std::isfinite(box_hop[k]) && box_hop[k] >= 0.0); + } + + // ------------------------------------------------------------------ + // m_k: the minimum Euclidean norm of the quaternion 4-vector over the + // control box, for the quaternion-floating joints. ‖q‖² is separable over + // the coordinates, so the minimum is attained coordinate-wise at whichever + // of {lower, upper, 0} lies in the interval and is closest to zero. It is + // the only box-dependent quantity the quaternion λ̃ needs (see below); it is + // left at 0 for every other joint, where it is never read. + // ------------------------------------------------------------------ + std::vector quat_min_norm(joints_.size(), 0.0); + for (int k = 0; k < static_cast(joints_.size()); ++k) { + const JointRecord& rec = joints_[k]; + double sum_sq = 0.0; + bool any_quaternion = false; + for (int off = 0; off < static_cast(rec.coord_rules.size()); ++off) { + if (rec.coord_rules[off] != CoordRule::kQuaternion) continue; + any_quaternion = true; + const int c = rec.position_start + off; + const double closest = + (lower[c] <= 0.0 && upper[c] >= 0.0) + ? 0.0 + : std::min(std::abs(lower[c]), std::abs(upper[c])); + sum_sq += closest * closest; + } + if (any_quaternion) quat_min_norm[k] = std::sqrt(sum_sq); + } + + // ------------------------------------------------------------------ + // Assemble the CSR table. + // + // Displacement lemma (PWL ancestor: Schwarzer, Saha & Latombe, + // "Adaptive Dynamic Collision Checking for Single and Multiple Articulated + // Robots in Complex Environments", IEEE T-RO 21(3), 2005): + // + // For any q, q′ in the control box and any pair p = (A, B), the signed + // distance between the two geometries changes by at most + // Σ_{j ∈ J(p)} λ(j,p)·|q′_j − q_j|. + // + // Proof sketch. Walk from q to q′ one coordinate at a time along the + // axis-aligned path; every intermediate configuration stays inside the box + // (a box is closed under coordinate-wise interpolation), so every reach r, + // computed as a uniform bound over that box, is valid at each step. On the + // step that moves coordinate j alone, only the distal side D(j,p) (the body + // of the pair inside S_j) moves relative to the other body, and the relative + // transform factors as + // X_{O,D}(q) = X_{O,P}·X_{P,F}·X_FM(q_j)·X_{M,C}·X_{C,D}, + // in which every factor but X_FM(q_j) is constant. A material point of D is + // therefore displaced, in O's frame, by exactly + // ‖(X_FM(q′_j) − X_FM(q_j))·u‖ with ‖u‖ ≤ r(j, D), + // because the leading factors are isometries and u is the point measured + // from Mo. Bounding that per joint type gives the λ values below: + // revolute chord ≤ arc => λ = r; + // prismatic pure unit translation => λ = 1; + // planar λ = 1 for x and y, λ = r for θ; + // screw rotation + |pitch|/2π of axial travel => λ = r + |pitch|/2π. + // Since a rigid motion of one of two sets changes their separation distance + // by at most the supremum pointwise displacement (triangle inequality on the + // minimizing witness pair), each step changes the distance by at most + // λ(j,p)·|Δq_j|, and the telescoping sum over steps gives the lemma. Note + // that the *distal side varies per joint* on a self-collision pair; the sum + // is still valid because each step is bounded in the frame of that step's + // static side and distance is frame-invariant. + // + // Only the separated branch of the distance function is ever used, so no + // penetration-depth regularity is needed. + // + // ------------------------------------------------------------------ + // The carve-out residual (carveout_slack_p). + // + // The constant-coordinate carve-out drops coordinate j from J(p) when its + // *whole* control-point range fits inside kContinuityTolerance. + // That is a tolerance, not an identity: the curve may still move q_j + // anywhere inside [lower_j, upper_j], and the telescoping proof above + // therefore still owes one step for j. Dropping the step outright would + // understate Δ_p by up to λ̃_j·range_j, which is unaccounted for anywhere + // else and is orders of magnitude above kNumericalSlack, so the + // certificate inequality could pass with the true clearance below the + // threshold by that much. We charge the step at its worst case instead, + // once per pair, against the *global* range (the node's own excursion in a + // carved coordinate is contained in it): + // + // carveout_slack_p = Σ_{j ∈ J_topo(p), j carved} λ̃_j · range_j, + // range_j = upper_j − lower_j. + // + // J_topo(p) is the pre-carve-out coordinate set, so the sum runs over + // exactly the steps the CSR row no longer carries. MotionBound() adds it + // unconditionally, which restores the telescoping sum in full. It is + // bit-exactly zero whenever every carved coordinate is exactly constant, + // which is the case for every path whose control points repeat the + // coordinate's value verbatim. λ̃_j, per coordinate kind: + // + // * revolute / prismatic / planar / screw: the λ formulas above, + // unchanged. The step being bounded is the same step; the carve-out + // changed nothing about the geometry, only about what the table stores. + // + // * RpyFloating, BallRpy and Universal *rotation* coordinates: λ̃ = r. + // Each such angle enters X_FM as one factor of a product of elementary + // rotations about axes through Mo (Rz(y)·Ry(p)·Rx(r) for rpy, likewise + // for a universal joint's two angles), so changing angle j alone takes + // R to R′ with R′R⁻¹ conjugate to a rotation by |Δq_j|, i.e. a rotation + // by exactly |Δq_j| about *some* axis through Mo. A material point u of + // the distal side, measured from Mo, is then displaced by + // ‖(R′ − R)u‖ = ‖(R′R⁻¹ − I)(Ru)‖ ≤ |Δq_j|·‖u‖ ≤ r·|Δq_j|, which is the + // revolute bound with the same r from the same chain walk (the walk + // bounds the distance from Mo to the distal geometry and does not care + // what kind of joint sits at the top of the chain). X_FM's translation + // is untouched by these coordinates: zero for BallRpy/Universal, and + // p_FM for RpyFloating, which is carried by its own coordinates. + // + // * RpyFloating / QuaternionFloating *translation* coordinates: λ̃ = 1. + // They are p_FM's components; a unit change translates the whole distal + // side by one unit. + // + // * QuaternionFloating quaternion coefficients: λ̃ = 2r/m ≤ 4r, with + // m = min over the control box of ‖q‖ (computed above). Derivation. + // Drake normalizes internally, X_FM using R(q/‖q‖), so the map from + // coefficients to rotation is q ↦ R(π(q)) with π(q) = q/‖q‖. π has + // derivative Dπ(q) = (I − q̂q̂ᵀ)/‖q‖, an orthogonal projector scaled by + // 1/‖q‖, hence ‖Dπ(q)‖₂ = 1/‖q‖. The control box is convex and every + // point of it has ‖q‖ ≥ m, so for u, v in the box the straight segment + // between them stays in the box and the geodesic distance on S³ between + // π(u) and π(v) is at most the length of its image, + // ψ ≤ ∫₀¹ ‖Dπ(γ(t))·γ′(t)‖ dt ≤ ‖u − v‖ / m. + // The rotation-angle metric on SO(3) is at most twice the geodesic + // metric on S³ (the unit quaternions double-cover SO(3): a geodesic of + // length ψ maps to a rotation of angle 2ψ), so the rotation angle + // between R(π(u)) and R(π(v)) obeys θ ≤ 2‖u − v‖/m. A material point at + // distance ≤ r from Mo is displaced by at most the chord + // 2r·sin(θ/2) ≤ r·θ ≤ (2r/m)·‖u − v‖, and since + // ‖u − v‖₂ ≤ ‖u − v‖₁ ≤ Σ_j range_j over the four coefficients, charging + // λ̃ = 2r/m per coefficient covers every pair (u, v) in the box. + // In the regime the carve-out produces, a box of diameter + // ρ ≤ kContinuityTolerance around a unit quaternion, m ≥ 1 − ρ, so + // 2r/m ≤ 2r/(1 − ρ) ≤ 4r for any ρ ≤ 1/2: the coefficient is at worst + // the small-angle constant 2r with a factor-2 margin, and is computed + // rather than assumed. m = 0, a box containing the zero quaternion, + // admits no bound at all, because Drake's own normalization is undefined + // there, and throws. + // + // * Any rotational carved coordinate whose distal side carries a HalfSpace + // has no finite r and therefore no finite λ̃; its residual is genuinely + // unbounded. Such a coordinate must be *exactly* constant; anything else + // throws. + // ------------------------------------------------------------------ + std::vector row_start; + std::vector coord; + std::vector lambda; + std::vector carveout_slack; + row_start.reserve(pairs.size() + 1); + row_start.push_back(0); + carveout_slack.reserve(pairs.size()); + + // r(j, D) is shared by every pair with the same (joint, distal body), which + // is the common case for an environment-heavy scene. + std::unordered_map reach_cache; + const auto reach_of = [&](int k, BodyIndex distal) { + const std::int64_t key = + static_cast(k) * num_bodies_ + static_cast(distal); + auto it = reach_cache.find(key); + if (it != reach_cache.end()) return it->second; + const double r = Reach(k, distal, box_hop); + reach_cache.emplace(key, r); + return r; + }; + + for (const PairRecord& pair : pairs) { + const BodyIndex a = pair.body_a; + const BodyIndex b = pair.body_b; + if (!a.is_valid() || !b.is_valid() || a >= num_bodies_ || + b >= num_bodies_) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: pair references body indices ({}, {}) outside the " + "plant's {} bodies.", + static_cast(a), static_cast(b), num_bodies_)); + } + double slack = 0.0; + for (int k : positioned_order_) { + const JointRecord& rec = joints_[k]; + const bool in_a = rec.subtree[a]; + const bool in_b = rec.subtree[b]; + if (in_a == in_b) continue; // j ∉ J_topo(p). + const BodyIndex distal = in_a ? a : b; + const int ps = rec.position_start; + + double r = -1.0; // Computed lazily: only rotational λ needs it. + const auto reach = [&]() { + if (r < 0.0) { + if (body_has_halfspace_[distal]) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: HalfSpace geometry '{}' on body '{}' is the " + "distal side of joint '{}' ({}), which rotates it. A half " + "space has unbounded reach, so no finite λ exists.", + body_halfspace_name_[distal], plant_->get_body(distal).name(), + rec.name, rec.type_name)); + } + r = reach_of(k, distal); + } + return r; + }; + + for (int c = ps; c < ps + rec.num_positions; ++c) { + if (constant_coordinates[c]) { + // Joint-support carve-out: c leaves J(p), and its residual + // motion inside the control box is charged to the pair's slack + // instead. See the derivation above for every λ̃ used here. + const double span = range(c); + DRAKE_DEMAND(std::isfinite(span) && span >= 0.0); + if (span == 0.0) continue; // Exactly constant: nothing to charge. + if (rec.coord_rules.empty()) { + // Unreachable: a joint kind with no rules is rejected above, + // constant or not. Kept as a guard so a future joint kind cannot + // reach here uncharged. + throw std::runtime_error(fmt::format( + "KinematicsEngine: joint '{}' has type '{}', which this " + "library does not know how to bound even when held constant.", + rec.name, rec.type_name)); + } + const CoordRule rule = rec.coord_rules[c - ps]; + if (IsRotationalRule(rule) && body_has_halfspace_[distal]) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: HalfSpace geometry '{}' on body '{}' is the " + "distal side of coordinate {} of joint '{}' ({}), which " + "rotates it, and this trajectory holds that coordinate " + "constant only to within a tolerance: its control-point " + "range is {}, not 0. A half space has unbounded reach, so the " + "residual motion of a rotational coordinate across it cannot " + "be bounded by any finite λ. A half space may only sit across " + "a rotational coordinate that is EXACTLY constant. Fix the " + "trajectory so that coordinate's control points are " + "identical, anchor the half space, filter the pair, or " + "replace the half space with a large Box.", + body_halfspace_name_[distal], plant_->get_body(distal).name(), + c, rec.name, rec.type_name, span)); + } + double lam_tilde = 0.0; + switch (rule) { + case CoordRule::kTranslation: + lam_tilde = 1.0; + break; + case CoordRule::kRotation: + lam_tilde = reach(); + break; + case CoordRule::kScrewCoord: + lam_tilde = reach() + std::abs(rec.screw_pitch) / (2 * M_PI); + break; + case CoordRule::kQuaternion: { + const double m = quat_min_norm[k]; + if (!(m > 0.0)) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: the trajectory's control box for the " + "quaternion coordinates of joint '{}' ({}) contains the " + "zero quaternion, whose normalized rotation is undefined, " + "so the residual motion of its carved-out coordinates " + "cannot be bounded. Quaternion control points must be " + "unit quaternions.", + rec.name, rec.type_name)); + } + lam_tilde = 2.0 * reach() / m; + break; + } + } + DRAKE_DEMAND(std::isfinite(lam_tilde) && lam_tilde >= 0.0); + slack += lam_tilde * span; + continue; + } + double lam = 0.0; + switch (rec.kind) { + case JointKind::kRevolute: + lam = reach(); + break; + case JointKind::kPrismatic: + lam = 1.0; + break; + case JointKind::kPlanar: + // q = (x, y, θ); see PlanarJoint's class documentation. + lam = (c == ps + 2) ? reach() : 1.0; + break; + case JointKind::kScrew: + lam = reach() + std::abs(rec.screw_pitch) / (2 * M_PI); + break; + case JointKind::kWeld: + case JointKind::kUnsupported: + throw std::runtime_error(fmt::format( + "KinematicsEngine: internal error: joint '{}' ({}) reached " + "the λ assembly with an unsupported kind.", + rec.name, rec.type_name)); + } + DRAKE_DEMAND(std::isfinite(lam) && lam >= 0.0); + coord.push_back(c); + lambda.push_back(lam); + } + } + DRAKE_DEMAND(std::isfinite(slack) && slack >= 0.0); + carveout_slack.push_back(slack); + row_start.push_back(static_cast(coord.size())); + } + return MotionBoundTable(std::move(row_start), std::move(coord), + std::move(lambda), std::move(carveout_slack)); +} + +const BoundingSphere& KinematicsEngine::geometry_sphere(GeometryId id) const { + auto it = geometry_spheres_.find(id); + if (it == geometry_spheres_.end()) { + throw std::runtime_error(fmt::format( + "KinematicsEngine: geometry {} has no bounding sphere; it is either " + "not a proximity geometry of this model or it is a HalfSpace.", + id)); + } + return it->second; +} + +bool KinematicsEngine::body_has_halfspace(BodyIndex body) const { + DRAKE_THROW_UNLESS(body.is_valid() && body < num_bodies_); + return body_has_halfspace_[body]; +} + +double KinematicsEngine::body_radius(BodyIndex body) const { + DRAKE_THROW_UNLESS(body.is_valid() && body < num_bodies_); + return body_radius_[body]; +} + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/motion_bound_table.h b/planning/continuous_collision/motion_bound_table.h new file mode 100644 index 000000000000..26851c65c1af --- /dev/null +++ b/planning/continuous_collision/motion_bound_table.h @@ -0,0 +1,347 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" +#include "drake/planning/continuous_collision/internal.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { + +/* A sphere, expressed in the owning body (link) frame L, that contains a +proximity geometry at every configuration of the body. */ +struct BoundingSphere { + Eigen::Vector3d center_L{Eigen::Vector3d::Zero()}; + double radius{0.0}; +}; + +/* Computes a bounding sphere, in the body frame, of shape `shape` posed at +X_LG in the body frame. + +The sphere is centered at the shape's natural center, which is tighter for the +broadphase prefilter than an origin-centered radius. The origin-centered bound +the reach chain needs is ‖center_L‖ + radius, which is sound because the sphere +contains the geometry. Formulas are exact containment per shape: + + - Sphere(r): center X_LG·0, radius r. + - Box(w,d,h; Drake stores full sizes): box center, radius = half diagonal. + - Capsule(r, L): center, radius = L/2 + r. + - Cylinder(r, L): center, radius = √(r² + (L/2)²) (farthest point on a rim). + - Ellipsoid(a,b,c): center, radius = max(a,b,c). + - Convex / Mesh: centroid of the convex-hull vertices, radius = max vertex + distance. The vertices MUST come from the same hull object the proximity + engine collides (Shape::GetConvexHull()), never from the raw file: the + engine's hull bakes in scale and degeneracy inflation, and the radius must + bound the geometry actually checked. + +An under-bounding formula produces an unsound λ with no other symptom, so this +function switches on the closed set of supported shape types rather than +falling back to a generic bound. +@throws std::exception on any other shape type, HalfSpace included; half +spaces are handled by dedicated rules, never through a bounding sphere. */ +BoundingSphere ComputeBoundingSphere(const geometry::Shape& shape, + const math::RigidTransform& X_LG); + +/* Per-pair motion-bound coefficients in CSR layout: for pair index k, a +contiguous span of (position-coordinate index j, λ(j, p)) entries over J(p), +the coordinates that change the pair's relative pose. λ is meters of worst-case +displacement of the pair's distal side per unit change of coordinate j, valid +for every configuration in the trajectory's global control-point box. + +Each pair also carries a scalar carveout_slack(p), the residual motion of the +coordinates the constant-coordinate carve-out removed from J(p). MotionBound() +charges it unconditionally, which is what makes Δ_p an upper bound on the +pair's relative motion over the whole trajectory. */ +class MotionBoundTable { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(MotionBoundTable); + + /* Constructs an empty table (zero pairs). */ + MotionBoundTable() = default; + + int num_pairs() const { return static_cast(row_start_.size()) - 1; } + + /* True iff J(p) is empty after the constant-coordinate carve-out: no + coordinate the trajectory *moves* changes this pair's relative pose, so it + is checked once. Note that "static" does not mean "immobile": a static pair + can still drift by carveout_slack(p), which callers that shortcut + MotionBound() for such a pair must charge themselves. + @pre 0 <= pair_index < num_pairs(). */ + bool pair_is_static(int pair_index) const { + return row_start_[pair_index] == row_start_[pair_index + 1]; + } + + /* Σ_{j ∈ J(p)} λ(j,p) · w_j: a sparse dot product against the node's + per-coordinate deviations w. This is the part of Δ_p(ν) that shrinks as a + node is split, and therefore what Options::distance_resolution is tested + against. + @pre 0 <= pair_index < num_pairs(). + @pre w.size() equals the plant's number of position coordinates. */ + double TravelBound(int pair_index, const Eigen::VectorXd& w) const { + double travel = 0.0; + for (int e = row_start_[pair_index]; e < row_start_[pair_index + 1]; ++e) { + travel += lambda_[e] * w[coord_[e]]; + } + return travel; + } + + /* Δ_p(ν) = carveout_slack(p) + TravelBound(p, w): the travel bound plus the + carved coordinates' residual. + @pre 0 <= pair_index < num_pairs(). + @pre w.size() equals the plant's number of position coordinates. */ + double MotionBound(int pair_index, const Eigen::VectorXd& w) const { + return carveout_slack_[pair_index] + TravelBound(pair_index, w); + } + + /* Σ over the coordinates of J_topo(p) that the carve-out removed of + λ̃_j · (global_upper_j − global_lower_j): an upper bound on how far this + pair's two geometries can move relative to each other purely through the + coordinates the table no longer tracks. The carve-out's "constant" is a + tolerance, a coordinate whose global control-box range is at most + kContinuityTolerance, so this is zero exactly when every carved coordinate is + exactly constant. + @pre 0 <= pair_index < num_pairs(). */ + double carveout_slack(int pair_index) const { + return carveout_slack_[pair_index]; + } + + /* Introspection for tests: the (coordinate, λ) entries of one pair, + ordered by increasing coordinate index. + @throws std::exception if pair_index is outside [0, num_pairs()). */ + std::vector> GetEntries(int pair_index) const; + + private: + friend class KinematicsEngine; + + /* Takes the four CSR arrays as assembled by KinematicsEngine, which is the + only producer. */ + MotionBoundTable(std::vector row_start, std::vector coord, + std::vector lambda, + std::vector carveout_slack) + : row_start_(std::move(row_start)), + coord_(std::move(coord)), + lambda_(std::move(lambda)), + carveout_slack_(std::move(carveout_slack)) {} + + std::vector row_start_{0}; + std::vector coord_; + std::vector lambda_; + std::vector carveout_slack_; +}; + +/* Construction-time kinematic analysis of a plant: joint classification, +per-hop fixed-transform translations, per-body proximity geometry bounding +spheres, and subtree tables for J(p). Thread-compatible; all methods are const +after construction and hold no mutable state, so concurrent +ComputeMotionBoundTable() calls are safe. + +Typical use by the certifier: +- once, at checker construction: KinematicsEngine engine(model); + engine.geometry_sphere(id) for the + prefilter; +- once per Check* call: engine.ComputeMotionBoundTable(path, pairs); +- once per node, per pair: table.MotionBound(pair_index, w). */ +class KinematicsEngine { + public: + /* Copies alias the same model: the RobotDiagram passed to the constructor + must outlive every copy, not just the original. */ + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(KinematicsEngine); + + /* Builds topology tables and per-body geometry bounding spheres. + Classification only; unsupported joint types throw later, and only if a + given path actually moves them. + + `model` is aliased and must outlive this object. + + @throws std::exception if the plant is not finalized. + @throws std::exception if a HalfSpace geometry is on the *distal* side of a + rotational coordinate relative to an unfiltered partner, whose reach is then + unbounded. A HalfSpace that is merely the static partner of a rotating body, + such as the anchored ground plane under a robot arm, is accepted: λ then + bounds the partner's points, and signed distance is symmetric, so the + certificate still holds. + @throws std::exception if a joint is "reversed", i.e. its declared parent + body is outboard of its declared child body in the multibody tree. + @throws std::exception if a joint closes a kinematic loop, or if the plant's + kinematically-affected sets disagree with the world-rooted tree walk. + @throws std::exception if any proximity geometry has a shape + ComputeBoundingSphere() rejects. */ + explicit KinematicsEngine(const RobotDiagram& model); + + /* The position-coordinate indices whose motion changes the relative pose + of the two bodies (J(p) before any carve-out), from topology alone. Sorted + ascending. */ + std::vector CoordinatesAffectingPair(multibody::BodyIndex body_a, + multibody::BodyIndex body_b) const; + + /* Assembles the λ CSR table for `pairs` given the path's global + control-point box; prismatic chain contributions use the box, so the bound is + trajectory-adaptive. Coordinates flagged constant by the path are removed + from every J(p), and their residual motion inside the box is charged to + MotionBoundTable::carveout_slack() instead. + @throws std::exception if the path's number of positions differs from the + plant's. + @throws std::exception naming the joint if the path moves a coordinate of + an unsupported joint type (quaternion floating, ball). */ + MotionBoundTable ComputeMotionBoundTable( + const PiecewiseBezierPath& path, + const std::vector& pairs) const; + + /* Raw-data overload of the above, for callers (and tests) that already + hold the trajectory's global control-point box. `lower` and `upper` are the + per-coordinate box bounds and `constant_coordinates` flags the coordinates + the path cannot change; all three have size num_positions(). A coordinate + flagged constant still contributes (upper − lower) worth of residual motion + to the pair's carve-out slack, so the two arguments must describe the same + trajectory: flagging a coordinate constant does not license widening its + box. + @throws std::exception on a size mismatch, an empty box (lower > upper), a + non-finite bound, a moving coordinate of an unsupported joint type, a pair + whose distal side carries a HalfSpace across a rotational coordinate, or a + pair whose distal side carries a HalfSpace across a rotational coordinate + that is constant only to within a tolerance (a HalfSpace has no finite + reach, so such a coordinate must be *exactly* constant). */ + MotionBoundTable ComputeMotionBoundTable( + const Eigen::VectorXd& lower, const Eigen::VectorXd& upper, + const std::vector& constant_coordinates, + const std::vector& pairs) const; + + /* The bounding sphere (in its body's frame) of one proximity geometry. + @throws std::exception if `id` is not a proximity geometry of this model or + is a HalfSpace (which has none). */ + const BoundingSphere& geometry_sphere(geometry::GeometryId id) const; + + /* True iff `body` carries at least one HalfSpace proximity geometry. */ + bool body_has_halfspace(multibody::BodyIndex body) const; + + /* Radius, about the body frame origin, of a sphere containing every + proximity geometry of `body`; this is the start of the reach chain. Zero for + a body with no (non-HalfSpace) proximity geometry. */ + double body_radius(multibody::BodyIndex body) const; + + int num_positions() const { return num_positions_; } + + const multibody::MultibodyPlant& plant() const { return *plant_; } + + private: + /* The λ rule a joint's coordinates follow. */ + enum class JointKind { + kWeld, // 0 dof; contributes fixed translations to reach only. + kRevolute, // λ = r. + kPrismatic, // λ = 1. + kPlanar, // λ = 1 (x, y), λ = r (θ). + kScrew, // λ = r + |pitch| / 2π. + kUnsupported // Throws if the path moves any of its coordinates. + }; + + /* The λ̃ rule a single *coordinate* follows when the carve-out has removed + it from J(p) but it is not exactly constant. It is per coordinate, not per + joint, because the joint kinds the carve-out admits (floating bases in + particular) mix rotation and translation coordinates inside one joint. For + the four supported kinds this reproduces JointKind's λ exactly; it extends + to the kUnsupported kinds, which have no λ but do have a λ̃. */ + enum class CoordRule { + kTranslation, // λ̃ = 1: a unit translation of the outboard frame. + kRotation, // λ̃ = r: a unit-angle rotation about an axis through Mo. + kScrewCoord, // λ̃ = r + |pitch| / 2π. + kQuaternion, // λ̃ = 2r / m; see ComputeMotionBoundTable() for the proof. + }; + + static bool IsRotationalRule(CoordRule rule) { + return rule != CoordRule::kTranslation; + } + + /* One tree edge, oriented from its outboard body toward the world. */ + struct JointRecord { + multibody::JointIndex index; + std::string name; + std::string type_name; + JointKind kind{JointKind::kUnsupported}; + int position_start{0}; + int num_positions{0}; + /* Tree-inboard / tree-outboard bodies (from the world-rooted walk, which + is cross-checked against Drake's own subtree query). */ + multibody::BodyIndex inboard; + multibody::BodyIndex outboard; + /* ‖p_PF‖ + ‖p_CM‖ (+ ‖p_FM‖ for a weld): the configuration-independent + part of one hop from the outboard body frame to the inboard body frame. */ + double fixed_hop{0.0}; + /* ‖p_CM‖ alone: the top-of-chain term, from the outboard body's frame + origin to the joint's M-frame origin (the point that stays fixed when + this joint's coordinates move). */ + double p_CM_norm{0.0}; + /* Screw pitch (meters of translation per full revolution). */ + double screw_pitch{0.0}; + /* Position-coordinate offsets, relative to position_start, holding a + translation of X_FM for the unsupported-but-constant carve-out. */ + std::vector translation_offsets; + /* False for a joint type this library has never been taught, whose X_FM + translation cannot be bounded from the control box at all. */ + bool translation_offsets_known{false}; + /* One CoordRule per position coordinate of this joint (size + num_positions), used only for coordinates the carve-out removed. Empty + exactly when translation_offsets_known is false, i.e. for a joint type + this library cannot bound even when held constant. */ + std::vector coord_rules; + /* Subtree membership: bodies whose pose depends on this joint's + coordinates. Empty for welds. */ + std::vector subtree; + }; + + /* Returns the joint ordinal (index into joints_) of `body`'s inboard joint, + or -1 for the world body. */ + int inboard_joint_of(multibody::BodyIndex body) const { + return inboard_joint_[body]; + } + + /* r(joint_ord, body): an upper bound, valid over the whole control box, on + the distance from the joint's M-frame origin to any point of `body`'s + proximity geometry. `box_hop` holds the per-call, box-dependent part of + each joint's hop translation. Requires `body` to be in the joint's + subtree. */ + double Reach(int joint_ord, multibody::BodyIndex body, + const std::vector& box_hop) const; + + void BuildTopology(); + void BuildGeometry(); + void CheckHalfSpaceRule() const; + + const RobotDiagram* model_{}; + const multibody::MultibodyPlant* plant_{}; + int num_positions_{0}; + int num_bodies_{0}; + + std::vector joints_; + /* Ordinals of the joints with at least one position coordinate, sorted by + position_start so that every J(p) comes out in ascending coordinate order. */ + std::vector positioned_order_; + /* JointIndex value -> ordinal into joints_, or -1. */ + std::vector joint_ordinal_; + /* BodyIndex value -> ordinal of its inboard joint, or -1 for the world. */ + std::vector inboard_joint_; + /* Position coordinate -> ordinal of the owning joint. */ + std::vector coord_joint_; + + std::vector body_radius_; + std::vector body_has_halfspace_; + std::vector body_halfspace_name_; + std::unordered_map geometry_spheres_; +}; + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/piecewise_bezier_path.cc b/planning/continuous_collision/piecewise_bezier_path.cc new file mode 100644 index 000000000000..a3584f2693fa --- /dev/null +++ b/planning/continuous_collision/piecewise_bezier_path.cc @@ -0,0 +1,543 @@ +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_throw.h" +#include "drake/common/nice_type_name.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/common/trajectories/bspline_trajectory.h" +#include "drake/common/trajectories/composite_trajectory.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/math/binomial_coefficient.h" +#include "drake/planning/continuous_collision/internal.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::NiceTypeName; +using drake::math::BinomialCoefficient; +using drake::trajectories::BezierCurve; +using drake::trajectories::BsplineTrajectory; +using drake::trajectories::CompositeTrajectory; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; + +/* Relative slack when clamping an evaluation parameter back onto the closed +domain. Callers legitimately land a hair outside after their own arithmetic; +anything larger is a programming error and throws. */ +constexpr double kParameterSlack = 1e-12; + +/* Every conversion below inherits its breakpoints verbatim from the source +trajectory, so consecutive segments meet exactly in exact arithmetic; this +absorbs only round-off in the caller's own time bookkeeping. */ +constexpr double kTimeContiguitySlack = 1e-9; + +/* Converts one BsplineTrajectory into Bézier segments (trajectory +normalization, item 4). + +Knot insertion (Boehm, via BsplineTrajectory::InsertKnots) raises every +distinct knot value inside the closed domain to multiplicity >= degree p. All +copies of a value are contiguous in a sorted knot vector, so afterwards every +nonempty span [t_i, t_{i+1}) satisfies t_{i-p+1} = ... = t_i and t_{i+1} = ... += t_{i+p}; under exactly those conditions the p+1 basis functions active on the +span, N_{i-p}, ..., N_i, reduce to the Bernstein basis of degree p in +(t - t_i)/(t_{i+1} - t_i), so control points i-p ... i ARE that span's Bézier +control points. The conversion is exact in exact arithmetic; the acceptance +test in test/piecewise_bezier_path_test.cc (1e-10 over >= 1e4 dense samples) +guards the indexing. */ +void AppendBsplineSegments(const BsplineTrajectory& bspline, + int source_index, + std::vector* segments) { + if (bspline.cols() != 1) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: the BsplineTrajectory at segment index {} is " + "{}x{}-valued; only column-vector-valued trajectories (cols() == 1) " + "over the plant's generalized positions are supported.", + source_index, bspline.rows(), bspline.cols())); + } + // InsertKnots mutates in place, so work on a copy of the caller's object. + BsplineTrajectory traj = bspline; + const int order = traj.basis().order(); + const int degree = order - 1; + + if (degree > 0) { + const double t0 = traj.basis().initial_parameter_value(); + const double tf = traj.basis().final_parameter_value(); + std::vector additional_knots; + const std::vector& knots = traj.basis().knots(); + for (std::size_t i = 0; i < knots.size();) { + std::size_t j = i; + while (j < knots.size() && knots[j] == knots[i]) { + ++j; + } + const int multiplicity = static_cast(j - i); + // Knots outside the domain do not bound any span we extract. + if (knots[i] >= t0 && knots[i] <= tf) { + for (int c = multiplicity; c < degree; ++c) { + additional_knots.push_back(knots[i]); + } + } + i = j; + } + if (!additional_knots.empty()) { + traj.InsertKnots(additional_knots); + } + } + + const std::vector& knots = traj.basis().knots(); + const int num_control_points = traj.num_control_points(); + const int num_positions = static_cast(traj.rows()); + const std::size_t num_before = segments->size(); + for (int i = order - 1; i < num_control_points; ++i) { + if (!(knots[i] < knots[i + 1])) { + continue; // Empty span, contributes no segment. + } + BezierSegment segment; + segment.t_start = knots[i]; + segment.t_end = knots[i + 1]; + segment.control_points.resize(num_positions, order); + for (int j = 0; j < order; ++j) { + segment.control_points.col(j) = traj.control_points()[i - degree + j]; + } + segments->push_back(std::move(segment)); + } + if (segments->size() == num_before) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: the BsplineTrajectory at segment index {} has " + "an empty parameter domain; a trajectory must span a positive time " + "interval.", + source_index)); + } +} + +/* Converts one PiecewisePolynomial into Bézier segments (trajectory +normalization, item 5). + +Drake stores each segment's polynomial in the monomial basis of the segment's +*relative* time tau = t - t_start. With s = tau/(t_end - t_start) in [0, 1] the +coefficients become alpha_a = c_a * (t_end - t_start)^a, and the exact monomial +-> Bernstein change of basis for a degree-m representation is + + s^a = sum_{j=a}^{m} [C(j, a) / C(m, a)] B_{j,m}(s), hence + P_j = sum_{a=0}^{j} [C(j, a) / C(m, a)] alpha_a. + +The map is increasingly ill-conditioned in m, hence kMaxConversionDegree. */ +void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, + int source_index, + std::vector* segments) { + if (pp.cols() != 1) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: the PiecewisePolynomial at segment index {} is " + "{}x{}-valued; only column-vector-valued trajectories (cols() == 1) " + "over the plant's generalized positions are supported.", + source_index, pp.rows(), pp.cols())); + } + const int num_positions = static_cast(pp.rows()); + const int num_pp_segments = pp.get_number_of_segments(); + if (num_pp_segments < 1) { + throw std::runtime_error( + fmt::format("PiecewiseBezierPath: the PiecewisePolynomial at segment " + "index {} has no segments.", + source_index)); + } + for (int k = 0; k < num_pp_segments; ++k) { + int m = 0; + for (int r = 0; r < num_positions; ++r) { + m = std::max(m, pp.getSegmentPolynomialDegree(k, r, 0)); + } + if (m > kMaxConversionDegree) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: PiecewisePolynomial segment {} (source segment " + "index {}) has polynomial degree {}, above the supported maximum of " + "{}. The monomial-to-Bernstein change of basis is ill-conditioned at " + "high degree; re-express the trajectory with more, lower-degree " + "segments.", + k, source_index, m, kMaxConversionDegree)); + } + const double t_start = pp.start_time(k); + const double t_end = pp.end_time(k); + const double duration = t_end - t_start; + if (!(duration > 0.0)) { + throw std::runtime_error( + fmt::format("PiecewiseBezierPath: PiecewisePolynomial segment {} " + "(source segment index {}) has non-positive duration {}.", + k, source_index, duration)); + } + BezierSegment segment; + segment.t_start = t_start; + segment.t_end = t_end; + segment.control_points.setZero(num_positions, m + 1); + Eigen::VectorXd alpha(m + 1); + for (int r = 0; r < num_positions; ++r) { + const Eigen::VectorXd coefficients = + pp.getPolynomial(k, r, 0).GetCoefficients(); + const int degree = static_cast(coefficients.size()) - 1; + alpha.setZero(); + double scale = 1.0; + for (int a = 0; a <= std::min(degree, m); ++a) { + alpha[a] = coefficients[a] * scale; + scale *= duration; + } + for (int j = 0; j <= m; ++j) { + double sum = 0.0; + for (int a = 0; a <= j; ++a) { + sum += (static_cast(BinomialCoefficient(j, a)) / + BinomialCoefficient(m, a)) * + alpha[a]; + } + segment.control_points(r, j) = sum; + } + } + segments->push_back(std::move(segment)); + } +} + +/* Dispatches `trajectory` by dynamic type and appends its Bézier segments, +recursing through CompositeTrajectory. `source_index` counts source segments +visited so far and appears in error messages. +*/ +void AppendSegments(const Trajectory& trajectory, int* source_index, + std::vector* segments) { + if (const auto* bezier = + dynamic_cast*>(&trajectory)) { + if (bezier->control_points().cols() < 1) { + throw std::runtime_error( + fmt::format("PiecewiseBezierPath: the BezierCurve at segment index " + "{} has no control points.", + *source_index)); + } + BezierSegment segment; + segment.t_start = bezier->start_time(); + segment.t_end = bezier->end_time(); + segment.control_points = bezier->control_points(); + segments->push_back(std::move(segment)); + ++(*source_index); + return; + } + if (const auto* composite = + dynamic_cast*>(&trajectory)) { + const int num = composite->get_number_of_segments(); + if (num < 1) { + throw std::runtime_error( + fmt::format("PiecewiseBezierPath: the CompositeTrajectory at " + "segment index {} has no segments.", + *source_index)); + } + for (int i = 0; i < num; ++i) { + AppendSegments(composite->segment(i), source_index, segments); + } + return; + } + if (const auto* bspline = + dynamic_cast*>(&trajectory)) { + AppendBsplineSegments(*bspline, *source_index, segments); + ++(*source_index); + return; + } + if (const auto* pp = + dynamic_cast*>(&trajectory)) { + AppendPiecewisePolynomialSegments(*pp, *source_index, segments); + ++(*source_index); + return; + } + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: unsupported trajectory type '{}' at segment index " + "{}. Supported types are drake::trajectories::BezierCurve, " + "drake::trajectories::BsplineTrajectory, " + "drake::trajectories::PiecewisePolynomial, and " + "drake::trajectories::CompositeTrajectory whose segments are " + "themselves supported.", + NiceTypeName::Get(trajectory), *source_index)); +} + +/* Checks shape, time ordering/contiguity and C0 junctions (trajectory + * normalization). */ +void ValidateSegments(int num_positions, + const std::vector& continuous_revolute_indices, + const std::vector& segments) { + if (segments.empty()) { + throw std::runtime_error( + "PiecewiseBezierPath: the trajectory produced no Bézier segments."); + } + for (std::size_t i = 0; i < segments.size(); ++i) { + const BezierSegment& segment = segments[i]; + if (segment.control_points.rows() != num_positions) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: segment {} has {} rows but the trajectory " + "declares {} generalized positions; every segment must be valued in " + "the same position space.", + i, segment.control_points.rows(), num_positions)); + } + if (segment.control_points.cols() < 1) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: segment {} has no control points.", i)); + } + if (!(segment.t_end >= segment.t_start)) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: segment {} spans [{}, {}], which runs " + "backwards in time.", + i, segment.t_start, segment.t_end)); + } + if (i > 0) { + const double previous_end = segments[i - 1].t_end; + const double slack = + kTimeContiguitySlack * + std::max({1.0, std::abs(previous_end), std::abs(segment.t_start)}); + if (std::abs(segment.t_start - previous_end) > slack) { + throw std::runtime_error( + fmt::format("PiecewiseBezierPath: segments are not contiguous " + "in time: segment {} ends at {} but segment {} " + "starts at {}. Segments must be ordered and meet " + "end-to-start.", + i - 1, previous_end, i, segment.t_start)); + } + } + } + + std::vector is_continuous_revolute(num_positions, false); + for (int index : continuous_revolute_indices) { + if (index < 0 || index >= num_positions) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: Options::continuous_revolute_indices contains " + "{}, which is out of range for a trajectory with {} generalized " + "positions.", + index, num_positions)); + } + is_continuous_revolute[index] = true; + } + + // C0 junction check, per coordinate, modulo 2π for continuous-revolute + // coordinates. A legitimate 2πk offset (GcsTrajectoryOptimization emits + // these) is accepted and the control points are left exactly as they are: + // forward kinematics is 2π-periodic in a revolute coordinate, so the + // certificate is unaffected and re-aligning segments would be a no-op that + // only risks introducing error. + for (std::size_t i = 1; i < segments.size(); ++i) { + const Eigen::MatrixXd& previous = segments[i - 1].control_points; + const Eigen::MatrixXd& next = segments[i].control_points; + for (int c = 0; c < num_positions; ++c) { + const double raw_gap = next(c, 0) - previous(c, previous.cols() - 1); + double gap = raw_gap; + if (is_continuous_revolute[c]) { + gap -= 2 * M_PI * std::round(gap / (2 * M_PI)); + } + if (std::abs(gap) > kContinuityTolerance) { + const std::string modulo = is_continuous_revolute[c] + ? fmt::format(" ({} modulo 2π)", gap) + : ""; + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: C0 discontinuity at the junction between " + "segments {} and {} in coordinate {}: the gap is {}{}, which " + "exceeds the continuity tolerance {}. A discontinuous " + "trajectory teleports; per-segment certificates would not cover " + "the jump. If coordinate {} is a continuous revolute joint, list " + "it in Options::continuous_revolute_indices.", + i - 1, i, c, raw_gap, modulo, kContinuityTolerance, c)); + } + } + } +} + +} // namespace + +PiecewiseBezierPath PiecewiseBezierPath::FromTrajectory( + const Trajectory& trajectory, + const std::vector& continuous_revolute_indices) { + if (trajectory.cols() != 1) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath::FromTrajectory: the trajectory is {}x{}-valued; " + "only column-vector-valued trajectories (cols() == 1) over the plant's " + "generalized positions are supported.", + trajectory.rows(), trajectory.cols())); + } + const int num_positions = static_cast(trajectory.rows()); + if (num_positions < 1) { + throw std::runtime_error( + "PiecewiseBezierPath::FromTrajectory: the trajectory has zero rows; " + "expected one row per generalized position."); + } + + PiecewiseBezierPath path; + path.num_positions_ = num_positions; + int source_index = 0; + AppendSegments(trajectory, &source_index, &path.segments_); + ValidateSegments(num_positions, continuous_revolute_indices, path.segments_); + path.FinalizeMetadata(); + return path; +} + +PiecewiseBezierPath PiecewiseBezierPath::FromWaypoints( + const Eigen::MatrixXd& waypoints) { + if (waypoints.rows() < 1) { + throw std::runtime_error( + "PiecewiseBezierPath::FromWaypoints: the waypoint matrix has zero " + "rows; expected one row per generalized position."); + } + if (waypoints.cols() < 2) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath::FromWaypoints: at least 2 waypoints (columns) " + "are required to form a path; got {}.", + waypoints.cols())); + } + const int num_positions = static_cast(waypoints.rows()); + const int num_segments = static_cast(waypoints.cols()) - 1; + + PiecewiseBezierPath path; + path.num_positions_ = num_positions; + path.segments_.reserve(num_segments); + for (int k = 0; k < num_segments; ++k) { + // A straight waypoint-to-waypoint move is exactly the order-1 Bézier with + // control points {q_k, q_{k+1}}. Segment + // k spans the nominal time interval [k, k+1]; the certificate does not + // depend on the time parametrization. + BezierSegment segment; + segment.t_start = k; + segment.t_end = k + 1; + segment.control_points.resize(num_positions, 2); + segment.control_points.col(0) = waypoints.col(k); + segment.control_points.col(1) = waypoints.col(k + 1); + path.segments_.push_back(std::move(segment)); + } + ValidateSegments(num_positions, {}, path.segments_); + path.FinalizeMetadata(); + return path; +} + +void PiecewiseBezierPath::FinalizeMetadata() { + const int n = num_positions_; + global_lower_ = + Eigen::VectorXd::Constant(n, std::numeric_limits::infinity()); + global_upper_ = + Eigen::VectorXd::Constant(n, -std::numeric_limits::infinity()); + for (const BezierSegment& segment : segments_) { + global_lower_ = + global_lower_.cwiseMin(segment.control_points.rowwise().minCoeff()); + global_upper_ = + global_upper_.cwiseMax(segment.control_points.rowwise().maxCoeff()); + } + // By the convex-hull property the curve never leaves [global_lower_, + // global_upper_], so a coordinate whose whole control-point range collapses + // to within the continuity tolerance cannot move on this path and is + // treated as welded. + constant_coordinates_.assign(n, false); + for (int i = 0; i < n; ++i) { + constant_coordinates_[i] = + (global_upper_[i] - global_lower_[i]) <= kContinuityTolerance; + } +} + +Eigen::VectorXd PiecewiseBezierPath::Value(double t) const { + DRAKE_DEMAND(!segments_.empty()); + const double t0 = start_time(); + const double tf = end_time(); + const double slack = + kParameterSlack * std::max({1.0, std::abs(t0), std::abs(tf)}); + if (!(t >= t0 - slack) || !(t <= tf + slack)) { + throw std::runtime_error( + fmt::format("PiecewiseBezierPath::Value: time {} is outside the path's " + "domain [{}, {}].", + t, t0, tf)); + } + const double clamped = std::clamp(t, t0, tf); + // Last segment whose start time is at or before `clamped`; at an interior + // junction the later segment wins. + const auto next = + std::upper_bound(segments_.begin(), segments_.end(), clamped, + [](double time, const BezierSegment& seg) { + return time < seg.t_start; + }); + const int index = static_cast(next - segments_.begin()) - 1; + DRAKE_DEMAND(index >= 0); + const BezierSegment& segment = segments_[index]; + const double duration = segment.t_end - segment.t_start; + const double s = + (duration > 0.0) ? (clamped - segment.t_start) / duration : 0.0; + return EvaluateSegment(index, std::clamp(s, 0.0, 1.0)); +} + +Eigen::VectorXd PiecewiseBezierPath::EvaluateSegment(int segment_index, + double s) const { + if (segment_index < 0 || + segment_index >= static_cast(segments_.size())) { + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath::EvaluateSegment: segment index {} is out of " + "range; the path has {} segments.", + segment_index, segments_.size())); + } + if (!(s >= -kParameterSlack) || !(s <= 1.0 + kParameterSlack)) { + throw std::runtime_error( + fmt::format("PiecewiseBezierPath::EvaluateSegment: parameter s = {} " + "is outside the segment's domain [0, 1].", + s)); + } + const double u = std::clamp(s, 0.0, 1.0); + const Eigen::MatrixXd& control_points = + segments_[segment_index].control_points; + const int m = static_cast(control_points.cols()) - 1; + // de Casteljau: repeated convex combinations, so the evaluation never leaves + // the convex hull of the control points and is numerically stable. + Eigen::MatrixXd work = control_points; + for (int r = 1; r <= m; ++r) { + for (int j = 0; j <= m - r; ++j) { + work.col(j) = (1.0 - u) * work.col(j) + u * work.col(j + 1); + } + } + return work.col(0); +} + +void DeCasteljauSplitAtHalf(const Eigen::MatrixXd& cps, Eigen::MatrixXd* left, + Eigen::MatrixXd* right, Eigen::VectorXd* mid) { + DRAKE_THROW_UNLESS(left != nullptr); + DRAKE_THROW_UNLESS(right != nullptr); + DRAKE_THROW_UNLESS(mid != nullptr); + const int n = static_cast(cps.rows()); + const int m = static_cast(cps.cols()) - 1; + DRAKE_THROW_UNLESS(m >= 0); + // Eigen's resize() is a no-op when the size already matches, so a caller + // that pre-sizes the outputs pays no allocation here (the performance + // requirements, P1). + if (left->rows() != n || left->cols() != m + 1) { + left->resize(n, m + 1); + } + if (right->rows() != n || right->cols() != m + 1) { + right->resize(n, m + 1); + } + if (mid->size() != n) { + mid->resize(n); + } + + // de Casteljau at u = 1/2 with the triangle b_j^r built in place inside + // `right`: b_j^r = (b_j^{r-1} + b_{j+1}^{r-1})/2 for j = 0 ... m-r. Sweeping + // j upward is safe because entry j+1 is not written until the next j. The + // left child's control points are the first entries of each triangle row, + // b_0^r; the right child's are the last entries, b_{m-r}^r, which is exactly + // the entry the sweep leaves at column m-r; and the apex b_0^m = q(1/2) is + // the right child's first control point. + *right = cps; + left->col(0) = cps.col(0); + for (int r = 1; r <= m; ++r) { + for (int j = 0; j <= m - r; ++j) { + right->col(j) = 0.5 * (right->col(j) + right->col(j + 1)); + } + left->col(r) = right->col(0); + } + *mid = right->col(0); +} + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/piecewise_bezier_path.h b/planning/continuous_collision/piecewise_bezier_path.h new file mode 100644 index 000000000000..bd7fc1d2444a --- /dev/null +++ b/planning/continuous_collision/piecewise_bezier_path.h @@ -0,0 +1,105 @@ +#pragma once + +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/trajectories/trajectory.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { + +/* One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1]. */ +struct BezierSegment { + /* Original time interval (bookkeeping only; the proof is a property of the + path and is invariant under time reparametrization). */ + double t_start{}; + double t_end{}; + /* n × (m+1); column j is control point P_j. */ + Eigen::MatrixXd control_points; +}; + +/* Ordered, C0-validated piecewise-Bézier path over the plant's generalized +positions. Every accepted trajectory type is converted, exactly, into this +representation up front. + +Two Bézier facts the whole method rests on: (1) the curve lies in the convex +hull of its control points, so per coordinate i, q_i(s) ∈ [min_j P_{j,i}, +max_j P_{j,i}]; (2) de Casteljau subdivision at any parameter u yields two +child curves whose control points exactly represent the two sub-curves and +are convex combinations of the parent's, so every descendant node's control +box is contained in this path's global control box. The apex of the de +Casteljau triangle at u is exactly q(u). */ +class PiecewiseBezierPath { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewiseBezierPath); + + /* Normalizes any supported Drake trajectory (BezierCurve, + CompositeTrajectory, BsplineTrajectory via knot insertion, + PiecewisePolynomial via monomial→Bernstein change of basis). + @throws std::exception on unsupported segment types, degree above + kMaxConversionDegree, or junction discontinuity beyond kContinuityTolerance + (modulo 2π for coordinates in `continuous_revolute_indices`). */ + static PiecewiseBezierPath FromTrajectory( + const trajectories::Trajectory& trajectory, + const std::vector& continuous_revolute_indices); + + /* Normalizes an n × K waypoint matrix into K−1 order-1 segments (exact). + Segment k spans time [k, k+1]. + @throws std::exception if `waypoints` has fewer than two columns or zero + rows. */ + static PiecewiseBezierPath FromWaypoints(const Eigen::MatrixXd& waypoints); + + int num_positions() const { return num_positions_; } + const std::vector& segments() const { return segments_; } + double start_time() const { return segments_.front().t_start; } + double end_time() const { return segments_.back().t_end; } + + /* Per-coordinate global control-point box over all segments, used for + trajectory-adaptive prismatic reach bounds. */ + const Eigen::VectorXd& global_lower_bound() const { return global_lower_; } + const Eigen::VectorXd& global_upper_bound() const { return global_upper_; } + + /* True for coordinates whose value is identical (within + kContinuityTolerance) across all control points of all segments; such + coordinates are treated as welded for the check. */ + const std::vector& constant_coordinates() const { + return constant_coordinates_; + } + + /* Evaluates the path at time t, for tests and breakpoint checks; the hot + loop uses de Casteljau apexes instead. + @throws std::exception if t is outside [start_time(), end_time()], up to a + parameter slack. */ + Eigen::VectorXd Value(double t) const; + + /* Evaluates segment `segment_index` at local parameter s ∈ [0, 1]. + @throws std::exception if segment_index is out of range, or if s is outside + [0, 1] up to a parameter slack. */ + Eigen::VectorXd EvaluateSegment(int segment_index, double s) const; + + private: + PiecewiseBezierPath() = default; + void FinalizeMetadata(); + + int num_positions_{}; + std::vector segments_; + Eigen::VectorXd global_lower_; + Eigen::VectorXd global_upper_; + std::vector constant_coordinates_; +}; + +/* Splits the Bézier control matrix `cps` (n × (m+1)) at u = 1/2 by de +Casteljau, writing the two children into `left` and `right` (resized as +needed) and the curve value at the midpoint (the apex) into `mid`. +Allocation-free when the outputs are already correctly sized. */ +void DeCasteljauSplitAtHalf(const Eigen::MatrixXd& cps, Eigen::MatrixXd* left, + Eigen::MatrixXd* right, Eigen::VectorXd* mid); + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/api_test.cc b/planning/continuous_collision/test/api_test.cc new file mode 100644 index 000000000000..3291f6c17026 --- /dev/null +++ b/planning/continuous_collision/test/api_test.cc @@ -0,0 +1,374 @@ +// Which joints, geometries, dimensions and options the checker accepts, and +// what it says when it refuses. A refusal must name the joint, geometry, +// coordinate, index or size the caller has to go and fix, so these tests assert +// on message content: a bare EXPECT_THROW would pass for a message reading +// "error". +// +// The refusals that belong to trajectory normalization (junction +// discontinuity, degree cap, unsupported trajectory type, out-of-range +// continuous-revolute index) are asserted, with the same message identifiers, +// in test/piecewise_bezier_path_test.cc. The pydrake surface is covered in +// bindings/pydrake/planning/test/continuous_collision_test.py. + +#include +#include +#include +#include +#include + +#include +#include + +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/geometry/geometry_instance.h" +#include "drake/geometry/proximity_properties.h" +#include "drake/multibody/fem/deformable_body_config.h" +#include "drake/multibody/plant/deformable_model.h" +#include "drake/multibody/tree/joint.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace { + +using drake::geometry::GeometryInstance; +using drake::geometry::ProximityProperties; +using drake::multibody::Joint; +using Eigen::Vector3d; +using Eigen::VectorXd; +using test::BezierCurve; +using test::Box; +using test::Friction; +using test::HalfSpace; +using test::Inertia; +using test::MakeCheckerPtr; +using test::MultibodyPlant; +using test::Parallelism; +using test::PrismaticJoint; +using test::RevoluteJoint; +using test::RigidBody; +using test::RigidTransformd; +using test::RobotDiagram; +using test::RobotDiagramBuilder; +using test::Sphere; +using test::ThrowMessage; +using ::testing::AllOf; +using ::testing::HasSubstr; + +Options SerialOptions() { + Options options; + options.parallelism = Parallelism::None(); + return options; +} + +// A planar 2-dof arm (revolute, prismatic) with one anchored obstacle: the +// well-formed world the dimension / options / trajectory tests use. +std::unique_ptr> MakeArmWorld() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const RigidBody& link = plant.AddRigidBody("link", Inertia()); + const RigidBody& tool = plant.AddRigidBody("tool", Inertia()); + plant.AddJoint("shoulder", plant.world_body(), {}, link, {}, + Vector3d::UnitZ()); + plant.AddJoint("slide", link, + RigidTransformd(Vector3d(0.30, 0.0, 0.0)), + tool, {}, Vector3d::UnitX()); + plant.RegisterCollisionGeometry(link, RigidTransformd(Vector3d(0.15, 0, 0)), + Box(0.30, 0.05, 0.05), "link_geom", + Friction()); + plant.RegisterCollisionGeometry(tool, RigidTransformd(), Sphere(0.04), + "tool_geom", Friction()); + const RigidBody& post = plant.AddRigidBody("post", Inertia()); + plant.WeldFrames(plant.world_frame(), post.body_frame(), + RigidTransformd(Vector3d(0.0, 0.60, 0.0))); + plant.RegisterCollisionGeometry(post, RigidTransformd(), Sphere(0.08), + "post_geom", Friction()); + return builder.Build(); +} + +// A floating base body carrying a one-revolute arm, plus an anchored obstacle. +// MultibodyPlant::Finalize() gives the free base a QuaternionFloatingJoint, so +// q = [quaternion(4), position(3), elbow(1)]. +std::unique_ptr> MakeFloatingBaseWorld() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const RigidBody& base = plant.AddRigidBody("base", Inertia()); + const RigidBody& arm = plant.AddRigidBody("arm", Inertia()); + plant.AddJoint("elbow", base, + RigidTransformd(Vector3d(0.10, 0.0, 0.0)), arm, + {}, Vector3d::UnitZ()); + plant.RegisterCollisionGeometry(base, RigidTransformd(), Sphere(0.05), + "base_geom", Friction()); + plant.RegisterCollisionGeometry(arm, RigidTransformd(Vector3d(0.12, 0, 0)), + Box(0.24, 0.04, 0.04), "arm_geom", + Friction()); + const RigidBody& post = plant.AddRigidBody("post", Inertia()); + plant.WeldFrames(plant.world_frame(), post.body_frame(), + RigidTransformd(Vector3d(0.0, 0.90, 0.0))); + plant.RegisterCollisionGeometry(post, RigidTransformd(), Sphere(0.06), + "post_geom", Friction()); + return builder.Build(); +} + +// The name Drake gave the quaternion floating joint it added at Finalize(). +std::string FloatingJointName(const MultibodyPlant& plant) { + for (drake::multibody::JointIndex index : plant.GetJointIndices()) { + const Joint& joint = plant.get_joint(index); + if (joint.type_name() == "quaternion_floating") return joint.name(); + } + ADD_FAILURE() << "the plant has no quaternion floating joint"; + return {}; +} + +// q for MakeFloatingBaseWorld(): identity quaternion, `p` for the base +// position, `elbow` for the joint. +VectorXd FloatingQ(const Vector3d& p, double elbow) { + VectorXd q(8); + q << 1.0, 0.0, 0.0, 0.0, p.x(), p.y(), p.z(), elbow; + return q; +} + +// --------------------------------------------------------------------------- +// 1. Joint scope: quaternion bases, and the constant-coordinate carve-out that +// makes them usable anyway. +// --------------------------------------------------------------------------- + +GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { + std::shared_ptr> model = MakeFloatingBaseWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + const std::string joint_name = FloatingJointName(model->plant()); + ASSERT_FALSE(joint_name.empty()); + + // Move a quaternion coordinate: straight-line interpolation of quaternion + // components is not a rotation-space geodesic, so the convex-hull motion + // bound has no meaning and the library must refuse rather than guess. The + // message must also point at the way out. + Eigen::MatrixXd points(8, 2); + points.col(0) = FloatingQ(Vector3d::Zero(), 0.0); + points.col(1) = FloatingQ(Vector3d::Zero(), 0.0); + points(0, 1) = 0.7071067811865476; // w + points(3, 1) = 0.7071067811865476; // z + EXPECT_THAT(ThrowMessage([&]() { + checker->CheckTrajectory(BezierCurve(0.0, 1.0, points)); + }), + AllOf(HasSubstr(joint_name), HasSubstr("quaternion_floating"), + HasSubstr("constant-coordinate carve-out"))); + + // Translating the base is refused for the same reason (the coordinate belongs + // to an excluded joint), and the message names the coordinate index. + Eigen::MatrixXd translated(8, 2); + translated.col(0) = FloatingQ(Vector3d::Zero(), 0.0); + translated.col(1) = FloatingQ(Vector3d(0.2, 0.0, 0.0), 0.0); + EXPECT_THAT( + ThrowMessage([&]() { + checker->CheckTrajectory(BezierCurve(0.0, 1.0, translated)); + }), + AllOf(HasSubstr(joint_name), HasSubstr("coordinate 4"))); +} + +GTEST_TEST(ApiTest, ConstantQuaternionBaseIsAcceptedEndToEnd) { + // A floating base whose pose is constant along the trajectory is treated as + // welded, so a floating-base robot is usable as long as the trajectory does + // not move the base. `wobble` is the sub-tolerance drift of the base's y + // position: at exactly zero the carve-out is exact, while a base held only to + // within the continuity tolerance is still carved but owes its residual to + // the motion bound (motion_bound_test.cc proves that residual is charged). + for (const double wobble : {0.0, 6e-8}) { + SCOPED_TRACE("wobble = " + std::to_string(wobble)); + const auto checker = + MakeCheckerPtr(MakeFloatingBaseWorld(), SerialOptions()); + Eigen::MatrixXd points(8, 3); + for (int j = 0; j < 3; ++j) { + points.col(j) = FloatingQ(Vector3d(0.05, -0.10, 0.0), 0.0); + } + points(5, 1) += wobble; + points(7, 1) = 0.35; // Only the elbow moves. + points(7, 2) = 0.70; + EXPECT_EQ( + checker->CheckTrajectory(BezierCurve(0.0, 1.0, points)).verdict, + Verdict::kCertifiedFree); + } +} + +// --------------------------------------------------------------------------- +// 2. Geometry scope: rotating half spaces and deformables. +// --------------------------------------------------------------------------- + +GTEST_TEST(ApiTest, RotatingHalfSpaceThrowsAtConstruction) { + // A half space on a body that rotates relative to an unfiltered partner has + // unbounded reach, so no finite lambda exists for that pair. This must be + // refused when the checker is built, not discovered mid-certification, and + // the message must say what to do about it ("Box"). + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const RigidBody& blade = plant.AddRigidBody("blade", Inertia()); + plant.AddJoint("spin", plant.world_body(), {}, blade, {}, + Vector3d::UnitX()); + plant.RegisterCollisionGeometry(blade, RigidTransformd(), HalfSpace(), + "blade_halfspace", Friction()); + const RigidBody& post = plant.AddRigidBody("post", Inertia()); + plant.WeldFrames(plant.world_frame(), post.body_frame(), + RigidTransformd(Vector3d(0.0, 0.5, 0.0))); + plant.RegisterCollisionGeometry(post, RigidTransformd(), Sphere(0.05), + "post_geom", Friction()); + std::shared_ptr> model = builder.Build(); + + EXPECT_THAT(ThrowMessage([&]() { + MakeCheckerPtr(model, SerialOptions()); + }), + AllOf(HasSubstr("blade_halfspace"), HasSubstr("post_geom"), + HasSubstr("spin"), HasSubstr("Box"))); +} + +GTEST_TEST(ApiTest, AnchoredHalfSpaceUnderARotatingArmIsAccepted) { + // The complement, so the rule above is not read as "half spaces are + // unsupported". An anchored ground plane under a rotating arm is accepted, + // because lambda then bounds the arm's points and signed distance is + // symmetric. + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const RigidBody& link = plant.AddRigidBody("link", Inertia()); + plant.AddJoint("shoulder", plant.world_body(), {}, link, {}, + Vector3d::UnitZ()); + plant.RegisterCollisionGeometry(link, RigidTransformd(Vector3d(0.15, 0, 0)), + Box(0.30, 0.05, 0.05), "link_geom", + Friction()); + const RigidBody& ground = plant.AddRigidBody("ground", Inertia()); + plant.WeldFrames(plant.world_frame(), ground.body_frame(), + RigidTransformd(Vector3d(0.0, 0.0, -0.4))); + plant.RegisterCollisionGeometry(ground, RigidTransformd(), HalfSpace(), + "ground_halfspace", Friction()); + + const auto checker = MakeCheckerPtr(builder.Build(), SerialOptions()); + EXPECT_EQ( + checker->CheckEdge(VectorXd::Constant(1, 0.0), VectorXd::Constant(1, 1.5)) + .verdict, + Verdict::kCertifiedFree); +} + +GTEST_TEST(ApiTest, DeformableGeometryIsRefusedNamingIt) { + // Deformables are out of scope: their motion is not described by the plant's + // generalized positions, so no motion bound exists for them at all. + // Registering one needs a discrete plant, which RobotDiagramBuilder's default + // time step already gives, so the refusal is exercised on a real model. + RobotDiagramBuilder builder(0.01); + MultibodyPlant& plant = builder.plant(); + const RigidBody& post = plant.AddRigidBody("post", Inertia()); + plant.WeldFrames(plant.world_frame(), post.body_frame(), + RigidTransformd(Vector3d(0.4, 0.0, 0.0))); + plant.RegisterCollisionGeometry(post, RigidTransformd(), Sphere(0.05), + "post_geom", Friction()); + const RigidBody& link = plant.AddRigidBody("link", Inertia()); + plant.AddJoint("shoulder", plant.world_body(), {}, link, {}, + Vector3d::UnitZ()); + plant.RegisterCollisionGeometry(link, RigidTransformd(Vector3d(0.15, 0, 0)), + Box(0.30, 0.05, 0.05), "link_geom", + Friction()); + + auto instance = std::make_unique( + RigidTransformd(Vector3d(0.0, 0.5, 0.0)), std::make_unique(0.05), + "squishy_blob"); + ProximityProperties properties; + drake::geometry::AddContactMaterial(1e8, {}, Friction(), &properties); + instance->set_proximity_properties(properties); + drake::multibody::fem::DeformableBodyConfig config; + config.set_youngs_modulus(1e6); + plant.mutable_deformable_model().RegisterDeformableBody(std::move(instance), + config, 0.05); + std::shared_ptr> model = builder.Build(); + ASSERT_EQ(model->scene_graph() + .model_inspector() + .GetAllDeformableGeometryIds() + .size(), + 1u); + + EXPECT_THAT(ThrowMessage([&]() { + MakeCheckerPtr(model, SerialOptions()); + }), + AllOf(HasSubstr("deformable"), HasSubstr("squishy_blob"))); +} + +// --------------------------------------------------------------------------- +// 3. Dimensions, trajectory validation and options. +// --------------------------------------------------------------------------- + +GTEST_TEST(ApiTest, DimensionMismatchMessagesNameTheSizes) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + ASSERT_EQ(model->plant().num_positions(), 2); + + EXPECT_THAT(ThrowMessage([&]() { + checker->CheckPath(Eigen::MatrixXd::Zero(3, 4)); + }), + AllOf(HasSubstr("CheckPath"), HasSubstr("3 rows"), + HasSubstr("2 generalized positions"), + HasSubstr("waypoints are columns"))); + + EXPECT_THAT(ThrowMessage([&]() { + checker->CheckEdge(VectorXd::Zero(2), VectorXd::Zero(5)); + }), + AllOf(HasSubstr("CheckEdge"), HasSubstr("sizes 2 and 5"))); + + EXPECT_THAT(ThrowMessage([&]() { + checker->CheckTrajectory( + BezierCurve(0.0, 1.0, Eigen::MatrixXd::Zero(7, 3))); + }), + AllOf(HasSubstr("7 rows"), HasSubstr("2 generalized positions"))); + + // A single waypoint is not a path. + DRAKE_EXPECT_THROWS_MESSAGE(checker->CheckPath(Eigen::MatrixXd::Zero(2, 1)), + ".*at least 2 waypoints.*"); +} + +GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { + const auto checker = MakeCheckerPtr(MakeArmWorld(), SerialOptions()); + Eigen::MatrixXd points(2, 2); + points << 0.0, 0.2, 0.0, 0.05; + const BezierCurve trajectory(0.0, 1.0, points); + + // Each case names the option the caller has to fix. A negative margin is in + // the list because the displacement lemma is proved in the separated regime + // only: an unreachable pair must be collision-filtered, not given a negative + // threshold and silently "certified". + struct Case { + std::string needle; + double Options::* field; + double value; + }; + const double kNaN = std::numeric_limits::quiet_NaN(); + const double kInf = std::numeric_limits::infinity(); + const std::vector cases = { + {"distance_resolution", &Options::distance_resolution, 0.0}, + {"positive", &Options::distance_resolution, -1e-3}, + {"distance_resolution", &Options::distance_resolution, kNaN}, + {"finite", &Options::distance_resolution, kInf}, + {"nonnegative", &Options::margin, -0.01}, + {"margin", &Options::margin, kNaN}, + }; + for (const auto& [needle, field, value] : cases) { + SCOPED_TRACE(needle); + Options bad = SerialOptions(); + bad.*field = value; + EXPECT_THAT(ThrowMessage([&]() { + checker->CheckTrajectory(trajectory, bad); + }), + HasSubstr(needle)); + } +} + +GTEST_TEST(ApiTest, NullModelIsRefused) { + // The adjacent finalization guard has no reachable input: + // RobotDiagramBuilder::Build() finalizes unconditionally and RobotDiagram's + // constructor is private to the builder. The null-model message names both + // requirements, so this pins the wording for the pair. + EXPECT_THAT(ThrowMessage([&]() { + ContinuousCollisionChecker checker(nullptr); + }), + AllOf(HasSubstr("model is null"), HasSubstr("finalized"))); +} + +} // namespace +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/bounding_sphere_test.cc b/planning/continuous_collision/test/bounding_sphere_test.cc new file mode 100644 index 000000000000..cbcfa6f04429 --- /dev/null +++ b/planning/continuous_collision/test/bounding_sphere_test.cc @@ -0,0 +1,320 @@ +// The bounding-sphere radius property: for every supported shape class, at many +// random poses X_LG, every sampled surface point lies inside the reported +// sphere. A shape that picks up another shape's radius formula produces an +// unsound lambda with no other symptom, so the sweep covers the whole closed +// set of supported shapes and pins the throw-on-unsupported behaviour. Never +// loosen the tolerance to make a case pass. + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drake/common/fmt_eigen.h" +#include "drake/common/memory_file.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/geometry/in_memory_mesh.h" +#include "drake/geometry/proximity/polygon_surface_mesh.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::geometry::Box; +using drake::geometry::Capsule; +using drake::geometry::Convex; +using drake::geometry::Cylinder; +using drake::geometry::Ellipsoid; +using drake::geometry::HalfSpace; +using drake::geometry::Mesh; +using drake::geometry::MeshcatCone; +using drake::geometry::Shape; +using drake::geometry::Sphere; +using drake::math::RigidTransform; +using Eigen::Vector3d; +using test::Rng; +using test::Sampler; + +constexpr int kNumPoses = 100; +constexpr int kNumSurfaceSamples = 1000; +/* The containment claim is exact mathematics; this only absorbs the rounding of + re-evaluating it. The slack is taken relative to the origin-centred radius + R_g = ‖c_L‖ + ρ, which is the form the property is stated in: the test forms + ‖X_LG·p − c_L‖ by cancelling two quantities of magnitude ‖t‖, so its absolute + rounding error scales with ‖t‖ and not with ρ. Scaling the slack by ρ alone + would make the test's own arithmetic, rather than the formulas under test, + decide the outcome for a millimetre-scale shape parked a metre away. */ +constexpr double kRelativeSlack = 1e-12; + +/* For Convex and Mesh the "surface samples" are the convex-hull vertices + themselves: they are the extreme points of the very hull object the proximity + engine collides, so containing all of them is the whole claim. */ +Sampler HullVertexSampler( + const drake::geometry::PolygonSurfaceMesh& hull) { + return [&hull](Rng* rng) { + const int v = test::UniformInt(rng, 0, hull.num_vertices() - 1); + return Vector3d(hull.vertex(v)); + }; +} + +void CheckContainment(const Shape& shape, const Sampler& sampler, + const std::string& label, double translation_scale, + Rng* rng) { + SCOPED_TRACE(label); + for (int pose = 0; pose < kNumPoses; ++pose) { + const RigidTransform X_LG = + test::RandomTransform(rng, translation_scale); + const BoundingSphere sphere = ComputeBoundingSphere(shape, X_LG); + ASSERT_TRUE(std::isfinite(sphere.radius)) << label; + ASSERT_GE(sphere.radius, 0.0) << label; + ASSERT_TRUE(sphere.center_L.allFinite()) << label; + const double origin_radius = sphere.center_L.norm() + sphere.radius; + const double limit = sphere.radius + kRelativeSlack * origin_radius; + for (int i = 0; i < kNumSurfaceSamples; ++i) { + const Vector3d p_G = sampler(rng); + const double distance = (X_LG * p_G - sphere.center_L).norm(); + ASSERT_LE(distance, limit) + << label << ": pose " << pose << ", sample " << i << ", radius " + << sphere.radius << ", p_G " + << fmt::format("{}", fmt_eigen(p_G.transpose())); + } + // The reach chain consumes ‖c_L‖ + ρ as an origin-centred radius; check + // that relaxation too, since the displacement lemma depends on it directly. + for (int i = 0; i < 32; ++i) { + const Vector3d p_G = sampler(rng); + ASSERT_LE((X_LG * p_G).norm(), origin_radius * (1.0 + kRelativeSlack)) + << label << " (origin-centred R_g)"; + } + } +} + +struct PrimitiveCase { + std::string label; + std::unique_ptr shape; + Sampler sampler; +}; + +/* Every supported primitive class, each at four sizes spanning the extremes the + formulas have to survive: near-isotropic, needle-thin, plate-thin, and + millimetre-scale. */ +std::vector PrimitiveCases() { + std::vector cases; + const auto add = [&cases](std::string label, std::unique_ptr shape, + Sampler sampler) { + cases.push_back( + PrimitiveCase{std::move(label), std::move(shape), std::move(sampler)}); + }; + for (const double r : {1e-4, 0.05, 1.0, 7.5}) { + add(fmt::format("Sphere({})", r), std::make_unique(r), + [r](Rng* rng) { + return test::SampleSphere(rng, r); + }); + } + for (const Vector3d& s : + {Vector3d(1.0, 1.0, 1.0), Vector3d(0.01, 2.0, 0.3), + Vector3d(5.0, 0.002, 0.002), Vector3d(0.4, 0.7, 1.9)}) { + add(fmt::format("Box({}, {}, {})", s.x(), s.y(), s.z()), + std::make_unique(s.x(), s.y(), s.z()), [s](Rng* rng) { + return test::SampleBox(rng, s); + }); + } + for (const Vector3d& c : {Vector3d(0.1, 1.0, 0), Vector3d(1.0, 0.01, 0), + Vector3d(0.001, 3.0, 0), Vector3d(0.5, 0.5, 0)}) { + const double r = c.x(); + const double length = c.y(); + add(fmt::format("Capsule({}, {})", r, length), + std::make_unique(r, length), [r, length](Rng* rng) { + return test::SampleCapsule(rng, r, length); + }); + } + for (const Vector3d& c : {Vector3d(0.1, 1.0, 0), Vector3d(2.0, 0.01, 0), + Vector3d(0.002, 4.0, 0), Vector3d(0.5, 0.5, 0)}) { + const double r = c.x(); + const double length = c.y(); + add(fmt::format("Cylinder({}, {})", r, length), + std::make_unique(r, length), [r, length](Rng* rng) { + return test::SampleCylinder(rng, r, length); + }); + } + for (const Vector3d& e : + {Vector3d(1.0, 1.0, 1.0), Vector3d(0.01, 0.5, 2.0), + Vector3d(3.0, 0.001, 0.001), Vector3d(0.2, 0.9, 0.05)}) { + add(fmt::format("Ellipsoid({}, {}, {})", e.x(), e.y(), e.z()), + std::make_unique(e.x(), e.y(), e.z()), [e](Rng* rng) { + return test::SampleEllipsoid(rng, e); + }); + } + return cases; +} + +GTEST_TEST(BoundingSphereTest, PrimitivesContainTheirSurface) { + Rng rng(0x5eed0001); + for (const PrimitiveCase& entry : PrimitiveCases()) { + CheckContainment(*entry.shape, entry.sampler, entry.label, 2.0, &rng); + } +} + +/* Builds a variety of vertex sets: generic, redundant (interior points that do + not survive the hull), near-degenerate (a very thin slab and a near-sliver), + exactly planar, and tiny. */ +std::vector> MakeVertexSets(Rng* rng) { + std::vector> out; + + { // Generic cloud on a ball. + Eigen::Matrix3Xd v(3, 30); + for (int i = 0; i < v.cols(); ++i) { + v.col(i) = test::Uniform(rng, 0.2, 1.0) * test::RandomUnitVector(rng); + } + out.emplace_back("convex/generic", v); + } + { // Cube corners plus many redundant interior points. + Eigen::Matrix3Xd v(3, 8 + 40); + int col = 0; + for (int sx : {-1, 1}) { + for (int sy : {-1, 1}) { + for (int sz : {-1, 1}) { + v.col(col++) = Vector3d(0.5 * sx, 0.5 * sy, 0.5 * sz); + } + } + } + for (; col < v.cols(); ++col) { + v.col(col) = test::UniformVector(rng, -0.4, 0.4); + } + out.emplace_back("convex/redundant", v); + } + { // Exactly planar (Drake documents this as non-degenerate). + Eigen::Matrix3Xd v(3, 16); + for (int i = 0; i < v.cols(); ++i) { + v.col(i) = Vector3d(test::Uniform(rng, -1.0, 1.0), + test::Uniform(rng, -1.0, 1.0), 0.0); + } + out.emplace_back("convex/planar", v); + } + { // Near-degenerate slab: 1 µm thick, 1 m wide. + Eigen::Matrix3Xd v(3, 24); + for (int i = 0; i < v.cols(); ++i) { + v.col(i) = + Vector3d(test::Uniform(rng, -1.0, 1.0), test::Uniform(rng, -1.0, 1.0), + test::Uniform(rng, -5e-7, 5e-7)); + } + out.emplace_back("convex/thin-slab", v); + } + { // Near-sliver: nearly one-dimensional. + Eigen::Matrix3Xd v(3, 20); + for (int i = 0; i < v.cols(); ++i) { + v.col(i) = Vector3d(test::Uniform(rng, -2.0, 2.0), + test::Uniform(rng, -1e-5, 1e-5), + test::Uniform(rng, -1e-5, 1e-5)); + } + out.emplace_back("convex/sliver", v); + } + { // Tiny. + Eigen::Matrix3Xd v(3, 20); + for (int i = 0; i < v.cols(); ++i) { + v.col(i) = 1e-4 * test::RandomUnitVector(rng); + } + out.emplace_back("convex/tiny", v); + } + return out; +} + +GTEST_TEST(BoundingSphereTest, ConvexContainsHullVertices) { + Rng rng(0x5eed0006); + int checked = 0; + for (const auto& [name, vertices] : MakeVertexSets(&rng)) { + for (const Vector3d& scale3 : + {Vector3d(1.0, 1.0, 1.0), Vector3d(2.0, 0.5, 1.3)}) { + const Convex shape(vertices, name, scale3); + const drake::geometry::PolygonSurfaceMesh* hull = nullptr; + try { + hull = &shape.GetConvexHull(); + } catch (const std::exception& e) { + // Drake rejects hulls it considers degenerate; the checker inherits + // that decision, and the radius claims nothing about a shape the + // proximity engine cannot build either. + GTEST_LOG_(INFO) << name << ": Drake refused the hull: " << e.what(); + continue; + } + ASSERT_GT(hull->num_vertices(), 0) << name; + CheckContainment(shape, HullVertexSampler(*hull), + fmt::format("{} scale3=({}, {}, {})", name, scale3.x(), + scale3.y(), scale3.z()), + 1.0, &rng); + ++checked; + } + } + EXPECT_GE(checked, 4) << "too few Convex vertex sets survived hull " + "construction to make this test meaningful"; +} + +/* A small nonconvex OBJ (an L-shaped prism) so the Mesh path exercises + hull-vs-mesh semantics, not just a convex primitive in disguise. It is built in + memory: nothing here needs a file on disk, and a write that silently failed + would turn this case into a vacuous pass. */ +geometry::InMemoryMesh LShapedObj() { + std::ostringstream out; + // Six-vertex L profile in the z = ±0.25 planes. + const std::vector> profile{ + {0.0, 0.0}, {1.0, 0.0}, {1.0, 0.3}, {0.3, 0.3}, {0.3, 1.2}, {0.0, 1.2}}; + for (double z : {-0.25, 0.25}) { + for (const auto& [x, y] : profile) { + out << "v " << x << " " << y << " " << z << "\n"; + } + } + // Two end caps as fans plus the side quads (triangulated); winding does not + // matter for the convex hull. + for (int base : {1, 7}) { + for (int i = 1; i + 1 < 6; ++i) { + out << "f " << base << " " << base + i << " " << base + i + 1 << "\n"; + } + } + for (int i = 0; i < 6; ++i) { + const int a = 1 + i; + const int b = 1 + (i + 1) % 6; + out << "f " << a << " " << b << " " << b + 6 << "\n"; + out << "f " << a << " " << b + 6 << " " << a + 6 << "\n"; + } + return geometry::InMemoryMesh{ + MemoryFile(out.str(), ".obj", "ccd_l_prism.obj")}; +} + +GTEST_TEST(BoundingSphereTest, MeshContainsHullVertices) { + Rng rng(0x5eed0007); + for (const Vector3d& scale3 : + {Vector3d(1.0, 1.0, 1.0), Vector3d(0.4, 1.7, 1.0)}) { + const Mesh shape(LShapedObj(), scale3); + const auto& hull = shape.GetConvexHull(); + ASSERT_GT(hull.num_vertices(), 3); + CheckContainment(shape, HullVertexSampler(hull), + fmt::format("Mesh scale3=({}, {}, {})", scale3.x(), + scale3.y(), scale3.z()), + 1.0, &rng); + } +} + +/* A shape that is not on the supported list must throw, never silently inherit + some other shape's formula. */ +GTEST_TEST(BoundingSphereTest, ThrowsOnUnsupportedShapes) { + const RigidTransform X_LG = RigidTransform::Identity(); + DRAKE_EXPECT_THROWS_MESSAGE(ComputeBoundingSphere(HalfSpace(), X_LG), + ".*HalfSpace.*"); + EXPECT_THROW(ComputeBoundingSphere(MeshcatCone(1.0, 0.5, 0.25), X_LG), + std::exception); +} + +} // namespace +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/certifier_test.cc b/planning/continuous_collision/test/certifier_test.cc new file mode 100644 index 000000000000..1fbec4fc1357 --- /dev/null +++ b/planning/continuous_collision/test/certifier_test.cc @@ -0,0 +1,455 @@ +// End-to-end tests of the certifier core and the public facade on a focused, +// hand-built corpus. The large randomized corpus lives in +// test/soundness_fuzz_test.cc, the thread-count sweep in +// test/concurrency_test.cc and the API throw conditions in test/api_test.cc; +// none is duplicated here. +// +// Every world is built programmatically, every trajectory is fixed, and every +// cross-check is dense sampling of the *same* path the checker certified, so +// the suite is deterministic and fast. + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace { + +using Eigen::Vector3d; +using Eigen::VectorXd; +using internal::PiecewiseBezierPath; +using test::BezierCurve; +using test::Box; +using test::DistanceAtFinding; +using test::Friction; +using test::HalfSpace; +using test::Inertia; +using test::MakeCheckerPtr; +using test::MultibodyPlant; +using test::Parallelism; +using test::PrismaticJoint; +using test::QueryObject; +using test::RevoluteJoint; +using test::RigidBody; +using test::RigidTransformd; +using test::RobotDiagram; +using test::RobotDiagramBuilder; +using test::Sphere; + +constexpr double kMargin = 0.01; + +// A planar 3-dof arm (revolute, revolute, prismatic) in the z = 0 plane: +// clang-format off +// world --j1(Rz)--> link1 [box, x ∈ 0 .. 0.40] +// --j2(Rz @ x=0.40)--> link2 [box, x ∈ 0 .. 0.30] +// --j3(Px @ x=0.30)--> tool [sphere r = 0.05] +// clang-format on +// so q = (θ1, θ2, d) and the tool centre sits at radius ≈ 0.70 + d when the +// arm is straight. Obstacles are welded to the world. +void AddArm(MultibodyPlant* plant) { + const RigidBody& link1 = plant->AddRigidBody("link1", Inertia()); + const RigidBody& link2 = plant->AddRigidBody("link2", Inertia()); + const RigidBody& tool = plant->AddRigidBody("tool", Inertia()); + + plant->AddJoint("j1", plant->world_body(), {}, link1, {}, + Vector3d::UnitZ()); + plant->AddJoint("j2", link1, + RigidTransformd(Vector3d(0.40, 0.0, 0.0)), + link2, {}, Vector3d::UnitZ()); + plant->AddJoint("j3", link2, + RigidTransformd(Vector3d(0.30, 0.0, 0.0)), + tool, {}, Vector3d::UnitX()); + + plant->RegisterCollisionGeometry( + link1, RigidTransformd(Vector3d(0.20, 0.0, 0.0)), Box(0.40, 0.06, 0.06), + "link1_geom", Friction()); + plant->RegisterCollisionGeometry( + link2, RigidTransformd(Vector3d(0.15, 0.0, 0.0)), Box(0.30, 0.06, 0.06), + "link2_geom", Friction()); + plant->RegisterCollisionGeometry(tool, RigidTransformd(), Sphere(0.05), + "tool_geom", Friction()); +} + +void AddWeldedSphere(MultibodyPlant* plant, const std::string& name, + const Vector3d& p_W, double radius) { + const RigidBody& body = plant->AddRigidBody(name, Inertia()); + plant->WeldFrames(plant->world_frame(), body.body_frame(), + RigidTransformd(p_W)); + plant->RegisterCollisionGeometry(body, RigidTransformd(), Sphere(radius), + name + "_geom", Friction()); +} + +// The main world: the arm, two round obstacles at different sweep angles, a +// ground halfspace (which exercises the analytic distance route and the +// "skip the sphere prefilter" path) and a far ceiling box. +std::shared_ptr> MakeArmWorld() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + AddArm(&plant); + // Angle ≈ 1.571 rad from the arm's home direction, radius 0.75. + AddWeldedSphere(&plant, "post", Vector3d(0.0, 0.75, 0.0), 0.10); + // Angle ≈ 2.575 rad, radius 0.65. + AddWeldedSphere(&plant, "pillar", Vector3d(-0.55, 0.35, 0.0), 0.08); + + const RigidBody& ground = plant.AddRigidBody("ground", Inertia()); + plant.WeldFrames(plant.world_frame(), ground.body_frame(), + RigidTransformd(Vector3d(0.0, 0.0, -0.50))); + plant.RegisterCollisionGeometry(ground, RigidTransformd(), HalfSpace(), + "ground_geom", Friction()); + + const RigidBody& ceiling = plant.AddRigidBody("ceiling", Inertia()); + plant.WeldFrames(plant.world_frame(), ceiling.body_frame(), + RigidTransformd(Vector3d(0.0, 0.0, 0.90))); + plant.RegisterCollisionGeometry(ceiling, RigidTransformd(), + Box(2.0, 2.0, 0.20), "ceiling_geom", + Friction()); + return builder.Build(); +} + +Options SerialOptions() { + Options options; + options.margin = kMargin; + options.parallelism = Parallelism::None(); + return options; +} + +// A Bézier from `start` to `end` with linearly spaced control points (so the +// curve is the straight segment, traversed with a nontrivial parametrization) +// over [t0, t1]. +BezierCurve MakeBezier(const VectorXd& start, const VectorXd& end, + int order, double t0 = 0.0, double t1 = 1.0) { + Eigen::MatrixXd control_points(start.size(), order + 1); + for (int j = 0; j <= order; ++j) { + const double u = static_cast(j) / order; + control_points.col(j) = (1.0 - u) * start + u * end; + } + return BezierCurve(t0, t1, control_points); +} + +VectorXd MakeQ(double theta1, double theta2, double d) { + VectorXd q(3); + q << theta1, theta2, d; + return q; +} + +PiecewiseBezierPath Normalize(const BezierCurve& trajectory) { + return PiecewiseBezierPath::FromTrajectory(trajectory, {}); +} + +// Result of the dense-sampling cross-check. +struct SampledClearance { + double min_clearance{std::numeric_limits::infinity()}; + // Time of the first sample whose clearance drops below `threshold`, or NaN. + double first_crossing{std::numeric_limits::quiet_NaN()}; +}; + +// Densely samples `path` and evaluates every unfiltered pair discretely. This +// is the independent check the certifier's continuum claim is measured against; +// it reuses the distance oracle (tested on its own in +// test/distance_oracle_test.cc) so that halfspace pairs are handled the same +// way. +SampledClearance SampleClearance(const RobotDiagram& model, + const PiecewiseBezierPath& path, + int samples_per_segment, double threshold) { + auto root = model.CreateDefaultContext(); + auto& plant_context = model.plant().GetMyMutableContextFromRoot(root.get()); + const auto& scene_graph = model.scene_graph(); + const internal::DistanceOracle oracle(model); + SampledClearance result; + for (int k = 0; k < static_cast(path.segments().size()); ++k) { + const internal::BezierSegment& segment = path.segments()[k]; + for (int i = 0; i <= samples_per_segment; ++i) { + const double s = static_cast(i) / samples_per_segment; + model.plant().SetPositions(&plant_context, path.EvaluateSegment(k, s)); + const auto& query_object = + scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*root)); + for (const internal::PairRecord& pair : oracle.pairs()) { + const double phi = oracle.SignedDistance(query_object, pair); + result.min_clearance = std::min(result.min_clearance, phi); + if (phi < threshold && std::isnan(result.first_crossing)) { + result.first_crossing = + segment.t_start + s * (segment.t_end - segment.t_start); + } + } + } + } + return result; +} + +// --------------------------------------------------------------------------- +// 1. Free trajectories are certified, and dense sampling agrees. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, FreeTrajectoryCertified) { + const auto model = MakeArmWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3); + + const Result result = checker->CheckTrajectory(trajectory); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + EXPECT_FALSE(result.finding.has_value()); + EXPECT_GT(result.num_nodes, 0u); + + // Independent cross-check: 10^4 dense samples must all clear the margin. + const SampledClearance sampled = + SampleClearance(*model, Normalize(trajectory), 10000, kMargin); + EXPECT_GT(sampled.min_clearance, kMargin); + EXPECT_TRUE(std::isnan(sampled.first_crossing)); + + // The same statement through the other two entry points. + Eigen::MatrixXd waypoints(3, 3); + waypoints.col(0) = MakeQ(0.0, 0.0, 0.0); + waypoints.col(1) = MakeQ(0.4, -0.2, 0.05); + waypoints.col(2) = MakeQ(0.8, -0.4, 0.10); + EXPECT_EQ(checker->CheckPath(waypoints).verdict, Verdict::kCertifiedFree); + EXPECT_EQ( + checker->CheckEdge(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10)).verdict, + Verdict::kCertifiedFree); +} + +GTEST_TEST(CertifierTest, NarrowGapCertifiedBySubdivision) { + // A genuinely free squeeze: the tool slides between two spheres that leave + // only 5 mm of clearance over the margin (0.115 − 0.05 − 0.05 = 0.015 = + // kMargin + 0.005), so the certificate is real but has to be earned by + // subdividing. + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + AddArm(&plant); + AddWeldedSphere(&plant, "gap_left", Vector3d(0.80, 0.115, 0.0), 0.05); + AddWeldedSphere(&plant, "gap_right", Vector3d(0.80, -0.115, 0.0), 0.05); + const std::shared_ptr> model = builder.Build(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + // Only the prismatic coordinate moves: the tool slides through the gap. + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.0, 0.0, 0.20), 1); + + const Result result = checker->CheckTrajectory(trajectory); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + EXPECT_FALSE(result.finding.has_value()); + // A 5 mm gap over the margin against ~0.2 m of travel cannot be certified at + // the root: the motion bound has to be tightened by subdivision. + EXPECT_GE(result.num_nodes, 16u); + + const SampledClearance sampled = + SampleClearance(*model, Normalize(trajectory), 10000, kMargin); + EXPECT_GT(sampled.min_clearance, kMargin); + EXPECT_LT(sampled.min_clearance, kMargin + 0.01); +} + +// --------------------------------------------------------------------------- +// 2. A sweeping trajectory that hits an obstacle: the witness is exact, and it +// is the earliest one. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, ViolationWitnessIsExactAndEarliest) { + const auto model = MakeArmWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1); + + const Result result = checker->CheckTrajectory(trajectory); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_TRUE(result.finding.has_value()); + const Finding& finding = *result.finding; + EXPECT_TRUE(finding.nearest_a_W.has_value()); + EXPECT_TRUE(finding.nearest_b_W.has_value()); + + // The witness is exactly on the trajectory, so re-evaluating the path at the + // reported time must reproduce it; and re-querying the distance from a fresh + // context must confirm the violation. + const PiecewiseBezierPath path = Normalize(trajectory); + EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), 1e-9); + const double phi = DistanceAtFinding(*model, finding); + EXPECT_LT(phi, kMargin); + EXPECT_NEAR(phi, finding.distance, 1e-12); + + // The branch-and-bound recursion drives the reported witness to the earliest + // violating time, which dense sampling brackets from above. + const SampledClearance sampled = + SampleClearance(*model, path, 10000, kMargin); + ASSERT_FALSE(std::isnan(sampled.first_crossing)); + EXPECT_NEAR(finding.time, sampled.first_crossing, 5e-3); + EXPECT_LE(finding.time, sampled.first_crossing + 1e-9); +} + +GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { + // A world built for exact tangency: with θ1 = θ2 = 0 held constant the tool + // centre slides along +x through (0.80, 0, 0), where the "graze" sphere sits + // at distance 0.11, which is exactly r_tool + r_graze + kMargin. + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + AddArm(&plant); + AddWeldedSphere(&plant, "graze", Vector3d(0.80, 0.11, 0.0), 0.05); + const std::shared_ptr> model = builder.Build(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.0, 0.0, 0.20), 1); + + Options options = SerialOptions(); + // A coarse resolution keeps the cost of the tangency cascade bounded; the + // verdict is what matters here, not the depth. + options.distance_resolution = 1e-4; + const Result result = checker->CheckTrajectory(trajectory, options); + + EXPECT_EQ(result.verdict, Verdict::kInconclusive); + ASSERT_TRUE(result.finding.has_value()); + // The near-witness sits within a hair of the threshold. + EXPECT_NEAR(result.finding->distance, kMargin, 1e-3); + EXPECT_NEAR(DistanceAtFinding(*model, *result.finding), + result.finding->distance, 1e-12); + + // Dense sampling confirms the tangency: the minimum clearance touches the + // margin but (up to sampling) never dips meaningfully below it. + const SampledClearance sampled = + SampleClearance(*model, Normalize(trajectory), 10000, kMargin); + EXPECT_NEAR(sampled.min_clearance, kMargin, 1e-6); +} + +// --------------------------------------------------------------------------- +// 3. Breakpoints: violations exactly at t0 and at a junction. Node midpoints +// are strictly interior, so only the breakpoint pre-pass can find these. +// A static pair (one whose J(p) the constant-coordinate carve-out emptied) +// is likewise resolved there, once, at q(t0). +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, BreakpointWitnessesAreReported) { + const auto model = MakeArmWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + + // q(t0) puts the arm straight into the post. + const Result at_start = checker->CheckTrajectory( + MakeBezier(MakeQ(1.5708, 0.0, 0.0), MakeQ(0.5, 0.0, 0.0), 1)); + ASSERT_EQ(at_start.verdict, Verdict::kViolationFound); + ASSERT_TRUE(at_start.finding.has_value()); + EXPECT_EQ(at_start.finding->time, 0.0); + EXPECT_LT(DistanceAtFinding(*model, *at_start.finding), kMargin); + + // A 3-waypoint path whose middle waypoint (the junction between segments, at + // t = 1) is inside the post, and whose first segment is free: the earliest + // witness must be the junction configuration itself. + Eigen::MatrixXd waypoints(3, 3); + waypoints.col(0) = MakeQ(0.0, 0.0, 0.0); + waypoints.col(1) = MakeQ(1.5708, 0.0, 0.0); + waypoints.col(2) = MakeQ(3.0, 0.0, 0.0); + const Result at_junction = checker->CheckPath(waypoints); + ASSERT_EQ(at_junction.verdict, Verdict::kViolationFound); + ASSERT_TRUE(at_junction.finding.has_value()); + EXPECT_LE(at_junction.finding->time, 1.0); + EXPECT_LT(DistanceAtFinding(*model, *at_junction.finding), kMargin); +} + +GTEST_TEST(CertifierTest, StaticPairsAreResolvedAtTheStart) { + // MultibodyPlant::Finalize() already filters every pair *within* a welded + // subgraph, so two anchored obstacles never even reach the checker as a + // candidate pair. The reachable source of J(p) = ∅ is the + // constant-coordinate carve-out: with θ1 and θ2 constant, the link1/link2 + // pair and every link-vs-obstacle pair stop depending on any moving + // coordinate and are settled once, at q(t0). + const auto model = MakeArmWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + + // Free: only the prismatic coordinate moves, away from every obstacle. + EXPECT_EQ(checker + ->CheckTrajectory(MakeBezier(MakeQ(0.3, -0.2, 0.0), + MakeQ(0.3, -0.2, 0.15), 2)) + .verdict, + Verdict::kCertifiedFree); + + // Violating: link1 is parked inside the pillar for the whole trajectory, so + // the only witness available is the static-pair test at q(t0). + const Result result = checker->CheckTrajectory( + MakeBezier(MakeQ(2.575, 0.0, 0.0), MakeQ(2.575, 0.0, 0.15), 2)); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_TRUE(result.finding.has_value()); + EXPECT_EQ(result.finding->time, 0.0); + EXPECT_LT(DistanceAtFinding(*model, *result.finding), kMargin); +} + +// --------------------------------------------------------------------------- +// 4. Retiming invariance: the proof is a property of the path. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, RetimingInvariance) { + const auto checker = MakeCheckerPtr(MakeArmWorld(), SerialOptions()); + const VectorXd start = MakeQ(0.0, 0.0, 0.0); + const VectorXd end = MakeQ(0.8, -0.4, 0.10); + + const Result a = + checker->CheckTrajectory(MakeBezier(start, end, 3, 0.0, 1.0)); + const Result b = + checker->CheckTrajectory(MakeBezier(start, end, 3, -2.5, 4.2)); + + // The recursion runs in the segment parameter; only the reported *times* + // would ever differ, and on a certified run there are none. + EXPECT_EQ(a.verdict, b.verdict); + EXPECT_EQ(a.verdict, Verdict::kCertifiedFree); + EXPECT_EQ(a.num_nodes, b.num_nodes); +} + +// --------------------------------------------------------------------------- +// 5. A small seeded soundness sweep. The full corpus (random worlds, +// B-splines, 10^5 samples, hundreds of cases) lives in +// test/soundness_fuzz_test.cc, which is timeout=long and opts out of asan +// and lsan; this is the cheap standing guard that runs in every build +// flavor. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, RandomTrajectoriesAreSoundAgainstDenseSampling) { + const auto model = MakeArmWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); + std::mt19937 rng(1234); + std::uniform_real_distribution theta1(-3.0, 3.0); + std::uniform_real_distribution theta2(-2.0, 2.0); + std::uniform_real_distribution slide(0.0, 0.25); + + int certified = 0; + int violating = 0; + for (int trial = 0; trial < 15; ++trial) { + Eigen::MatrixXd control_points(3, 4); + for (int j = 0; j < 4; ++j) { + control_points.col(j) << theta1(rng), theta2(rng), slide(rng); + } + const BezierCurve trajectory(0.0, 1.0, control_points); + const Result result = checker->CheckTrajectory(trajectory); + const PiecewiseBezierPath path = Normalize(trajectory); + + if (result.verdict == Verdict::kCertifiedFree) { + ++certified; + const SampledClearance sampled = + SampleClearance(*model, path, 2000, kMargin); + EXPECT_GT(sampled.min_clearance, kMargin) + << "trial " << trial << " was certified but dense sampling found a " + << "configuration at clearance " << sampled.min_clearance; + } else if (result.verdict == Verdict::kViolationFound) { + ++violating; + // The witness must be exactly on the path and must really violate. + const Finding& finding = *result.finding; + EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), + 1e-9) + << "trial " << trial; + EXPECT_LT(DistanceAtFinding(*model, finding), kMargin) + << "trial " << trial; + } + } + // The corpus must actually exercise both outcomes. + EXPECT_GT(certified, 0); + EXPECT_GT(violating, 0); +} + +} // namespace +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc new file mode 100644 index 000000000000..a08a814dd0e9 --- /dev/null +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -0,0 +1,202 @@ +// Concurrency determinism, on the fixed corpus of ten random cases (a mix of +// free and violating) that test_utilities.h builds, plus the deep workload it +// derives from that corpus: +// +// 1. The answer does not depend on the thread count. Verdict and earliest +// witness are identical at Parallelism {1, 2, 8, 16}, and on a case that +// certifies free (where the branch-and-bound bound never tightens and the +// whole tree is explored) so is Result::num_nodes. +// 2. The public Check* methods are safe to call concurrently on one instance. +// 3. The deep workload, which unlike any corpus case is big enough that the +// driver actually hires helpers, explores the same tree and reports the +// same result at every thread count, concurrent callers included. +// +// Every case is an equality, not a wall-clock claim, so this target runs under +// every build flavor. This is the test to run under ThreadSanitizer: +// +// bazel test --config=tsan //planning/continuous_collision:concurrency_test +// +// On recent kernels the default `vm.mmap_rnd_bits` puts mappings outside the +// range TSan's shadow memory expects and the runtime aborts before main ever +// runs; run the binary under `setarch $(uname -m) -R`, or lower +// vm.mmap_rnd_bits to 28. A report rooted in a continuous_collision frame is a +// real bug; one rooted entirely in Drake belongs in a suppression file. + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace test { +namespace { + +// Verdict and witness always; the node count too whenever the run certified, +// because then nothing was pruned and both runs walked the identical tree. +::testing::AssertionResult SameResult(const Result& a, const Result& b) { + if (a.verdict != b.verdict) { + return ::testing::AssertionFailure() << "verdicts differ"; + } + if (a.verdict == Verdict::kCertifiedFree && a.num_nodes != b.num_nodes) { + return ::testing::AssertionFailure() + << "node counts differ: " << a.num_nodes << " vs " << b.num_nodes; + } + return FindingIdentical(a.finding, b.finding); +} + +GTEST_TEST(ConcurrencyTest, CorpusIsBalanced) { + const auto& corpus = Corpus(); + ASSERT_EQ(static_cast(corpus.size()), kNumCases); + int free_count = 0; + int violating_count = 0; + for (const auto& entry : corpus) { + (entry->serial_verdict == Verdict::kCertifiedFree ? free_count + : violating_count) += 1; + } + EXPECT_GE(free_count, kMinFreeCases); + EXPECT_GE(violating_count, kMinViolatingCases); +} + +GTEST_TEST(ConcurrencyTest, ThreadCountInvariantAndSeriallyDeterministic) { + for (const auto& entry : Corpus()) { + const BezierCurve trajectory = entry->trajectory(); + const Options serial_options = BaseOptions(Parallelism::None()); + const Result serial = + entry->checker->CheckTrajectory(trajectory, serial_options); + SCOPED_TRACE(entry->name); + // Serial runs are bit-deterministic. + EXPECT_TRUE(SameResult( + serial, entry->checker->CheckTrajectory(trajectory, serial_options))); + for (const int threads : {2, 8, 16}) { + SCOPED_TRACE("threads " + std::to_string(threads)); + EXPECT_TRUE(SameResult( + serial, entry->checker->CheckTrajectory( + trajectory, BaseOptions(Parallelism(threads))))); + } + } +} + +GTEST_TEST(ConcurrencyTest, ConcurrentCallsOnOneCheckerMatchSequential) { + // Every worker hits the *same* checker object through all three public entry + // points, so they contend for the construction-time context pool; the lease + // must hand each call its own contexts. Each worker also asks for internal + // parallelism, so the pool is under pressure from both directions at once. + const auto& corpus = Corpus(); + const Options options = BaseOptions(Parallelism(2)); + + std::vector trajectory_expected; + std::vector edge_expected; + std::vector path_expected; + std::vector waypoints; + for (const auto& entry : corpus) { + const VectorXd q1 = entry->control_points.col(0); + const VectorXd q2 = entry->control_points.rightCols(1); + Eigen::MatrixXd w(q1.size(), 3); + w.col(0) = q1; + w.col(1) = 0.5 * (q1 + q2); + w.col(2) = q2; + waypoints.push_back(w); + trajectory_expected.push_back( + entry->checker->CheckTrajectory(entry->trajectory(), options)); + edge_expected.push_back(entry->checker->CheckEdge(q1, q2, options)); + path_expected.push_back(entry->checker->CheckPath(w, options)); + } + + constexpr int kThreads = 8; + // gtest assertions are not safe off the main thread, so each worker counts + // its own mismatches into its own slot and the main thread asserts after the + // join. + std::vector mismatches(kThreads, 0); + std::vector threads; + for (int t = 0; t < kThreads; ++t) { + threads.emplace_back([&, t]() { + for (int r = 0; r < 3; ++r) { + for (std::size_t k = 0; k < corpus.size(); ++k) { + const Case& entry = *corpus[k]; + const VectorXd q1 = entry.control_points.col(0); + const VectorXd q2 = entry.control_points.rightCols(1); + if (!SameResult( + entry.checker->CheckTrajectory(entry.trajectory(), options), + trajectory_expected[k]) || + !SameResult(entry.checker->CheckEdge(q1, q2, options), + edge_expected[k]) || + !SameResult(entry.checker->CheckPath(waypoints[k], options), + path_expected[k])) { + ++mismatches[t]; + } + } + } + }); + } + for (std::thread& thread : threads) thread.join(); + for (int t = 0; t < kThreads; ++t) EXPECT_EQ(mismatches[t], 0); +} + +// The sharing path only ever runs on a workload big enough to hire a helper, +// which the corpus cases above never are. The two tests below are where it gets +// its coverage, TSan's included. + +GTEST_TEST(ConcurrencyTest, DeepWorkloadIsThreadCountInvariant) { + const DeepWorkload& deep = Deep(); + ASSERT_NE(deep.entry, nullptr); + // Without this floor the test could silently degenerate into measuring a + // handful of nodes if the corpus or the bisection ever drifted. + EXPECT_GE(deep.num_nodes, kMinDeepNodes) << "grazing margin " << deep.margin; + std::cout << "\n[ concurrency ] deep workload: " << deep.entry->name + << ", margin " << deep.margin << ", " << deep.num_nodes + << " nodes\n\n"; + + const BezierCurve trajectory = deep.entry->trajectory(); + const Result serial = deep.entry->checker->CheckTrajectory( + trajectory, deep.options(Parallelism::None())); + for (const int threads : {4, 16}) { + SCOPED_TRACE("threads " + std::to_string(threads)); + EXPECT_TRUE(SameResult( + serial, deep.entry->checker->CheckTrajectory( + trajectory, deep.options(Parallelism(threads))))); + } +} + +GTEST_TEST(ConcurrencyTest, DeepWorkloadSurvivesConcurrentParallelCalls) { + // Several caller threads each asking the *same* checker for internal + // parallelism on a workload big enough to hire: this is the only test that + // makes concurrent calls contend for the checker's worker threads as well as + // its context pool, and the case where a lease returning fewer contexts than + // the pool was warmed for is the normal outcome rather than an edge case. + const DeepWorkload& deep = Deep(); + ASSERT_NE(deep.entry, nullptr); + const Result expected = deep.entry->checker->CheckTrajectory( + deep.entry->trajectory(), deep.options(Parallelism::None())); + + constexpr int kThreads = 4; + std::vector mismatches(kThreads, 0); + std::vector threads; + for (int t = 0; t < kThreads; ++t) { + threads.emplace_back([&, t]() { + for (int r = 0; r < 2; ++r) { + if (!SameResult( + deep.entry->checker->CheckTrajectory( + deep.entry->trajectory(), deep.options(Parallelism(4))), + expected)) { + ++mismatches[t]; + } + } + }); + } + for (std::thread& thread : threads) thread.join(); + for (int t = 0; t < kThreads; ++t) EXPECT_EQ(mismatches[t], 0); +} + +} // namespace +} // namespace test +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/distance_oracle_test.cc b/planning/continuous_collision/test/distance_oracle_test.cc new file mode 100644 index 000000000000..7fd9f0caa3b6 --- /dev/null +++ b/planning/continuous_collision/test/distance_oracle_test.cc @@ -0,0 +1,855 @@ +// Distance oracle accuracy, capability-probe classification, the analytic +// halfspace fallback and Mesh-as-convex-hull semantics. Every world is built +// programmatically with RobotDiagramBuilder and every randomized case uses a +// fixed seed, so the suite is deterministic. + +#include "drake/planning/continuous_collision/distance_oracle.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drake/common/find_resource.h" +#include "drake/common/memory_file.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/geometry/geometry_instance.h" +#include "drake/geometry/in_memory_mesh.h" +#include "drake/geometry/proximity_properties.h" +#include "drake/geometry/query_object.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/rotation_matrix.h" +#include "drake/multibody/fem/deformable_body_config.h" +#include "drake/multibody/plant/coulomb_friction.h" +#include "drake/multibody/plant/deformable_model.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/spatial_inertia.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::geometry::Box; +using drake::geometry::Capsule; +using drake::geometry::Convex; +using drake::geometry::Cylinder; +using drake::geometry::Ellipsoid; +using drake::geometry::GeometryId; +using drake::geometry::HalfSpace; +using drake::geometry::InMemoryMesh; +using drake::geometry::Mesh; +using drake::geometry::QueryObject; +using drake::geometry::Shape; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +using drake::math::RotationMatrixd; +using drake::multibody::BodyIndex; +using drake::multibody::CoulombFriction; +using drake::multibody::MultibodyPlant; +using drake::multibody::RigidBody; +using drake::multibody::SpatialInertia; +using drake::planning::RobotDiagram; +using drake::planning::RobotDiagramBuilder; +using drake::systems::Context; +using Eigen::Matrix3Xd; +using Eigen::Vector3d; +using ::testing::HasSubstr; + +// Exactness bar for the analytic halfspace fallback and for round trips that +// must land on identical code paths. +constexpr double kExact = 1e-12; +// Accuracy bar for Drake's native (partly iterative GJK) narrowphase. +constexpr double kNative = 1e-6; + +// -------------------------------------------------------------------------- +// World construction helpers. +// -------------------------------------------------------------------------- + +// A built RobotDiagram plus a context, with convenience accessors. +class World { + public: + explicit World(std::unique_ptr> diagram) + : diagram_(std::move(diagram)), + context_(diagram_->CreateDefaultContext()) {} + + const RobotDiagram& diagram() const { return *diagram_; } + const MultibodyPlant& plant() const { return diagram_->plant(); } + + Context& plant_context() { + return diagram_->plant().GetMyMutableContextFromRoot(context_.get()); + } + + // Re-evaluates the query output port; call after every pose change. + const QueryObject& query() { + const auto& scene_graph = diagram_->scene_graph(); + return scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*context_)); + } + + void SetPose(const RigidBody& body, const RigidTransformd& X_WB) { + diagram_->plant().SetFreeBodyPose(&plant_context(), body, X_WB); + } + + // Randomizes every floating body's pose. + void RandomizeAll(std::mt19937* rng, double range); + + private: + std::unique_ptr> diagram_; + std::unique_ptr> context_; +}; + +CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +// Deterministic random pose: uniform translation in [-range, range]^3 and a +// uniformly distributed orientation. +RigidTransformd RandomPose(std::mt19937* rng, double range) { + std::uniform_real_distribution uniform(-range, range); + std::normal_distribution normal(0.0, 1.0); + Eigen::Quaterniond q(normal(*rng), normal(*rng), normal(*rng), normal(*rng)); + if (q.norm() < 1e-8) q = Eigen::Quaterniond::Identity(); + q.normalize(); + return RigidTransformd(RotationMatrixd(q), + Vector3d(uniform(*rng), uniform(*rng), uniform(*rng))); +} + +void World::RandomizeAll(std::mt19937* rng, double range) { + for (BodyIndex i(1); i < plant().num_bodies(); ++i) { + const RigidBody& body = plant().get_body(i); + if (body.is_floating_base_body()) SetPose(body, RandomPose(rng, range)); + } +} + +// Adds a floating body carrying `shape` as its only collision geometry. The +// default pose spreads bodies out so the capability probe's default-context +// queries do not run on a pile of coincident geometry. +const RigidBody& AddShapeBody(MultibodyPlant* plant, + const std::string& name, + const Shape& shape, + const Vector3d& default_p_WB) { + const RigidBody& body = plant->AddRigidBody( + name, SpatialInertia::SolidSphereWithMass(1.0, 0.1)); + plant->RegisterCollisionGeometry(body, RigidTransformd::Identity(), shape, + name + "_geometry", Friction()); + plant->SetDefaultFloatingBaseBodyPose(body, RigidTransformd(default_p_WB)); + return body; +} + +// The single collision geometry registered on `body_name`. +GeometryId GeometryOf(const MultibodyPlant& plant, + const std::string& body_name) { + const auto& ids = + plant.GetCollisionGeometriesForBody(plant.GetBodyByName(body_name)); + EXPECT_EQ(ids.size(), 1u); + return ids.front(); +} + +// Finds the probe record for the unordered pair {a, b}. +const PairRecord& FindPair(const DistanceOracle& oracle, GeometryId a, + GeometryId b) { + for (const PairRecord& p : oracle.pairs()) { + if ((p.a == a && p.b == b) || (p.a == b && p.b == a)) { + return p; + } + } + ADD_FAILURE() << "pair not found in the probe's snapshot"; + return oracle.pairs().front(); +} + +// -------------------------------------------------------------------------- +// Independently derived ground truth. +// -------------------------------------------------------------------------- + +// Distance from a point to a box, both in the box's frame; zero inside. +double PointBoxDistance(const Vector3d& p_B, const Vector3d& half) { + return (p_B.cwiseAbs() - half).cwiseMax(0.0).norm(); +} + +// Hand-derived halfspace distances: phi = min over the shape of n^T(x - p0), +// written here from the textbook support functions rather than from the +// oracle's support-point machinery, so the two derivations stay independent. +// n is the outward unit normal (R_WH's third column) and p0 a boundary point. + +double HalfSpaceSphere(const Vector3d& n, const Vector3d& p0, + const RigidTransformd& X_WC, double r) { + return n.dot(X_WC.translation() - p0) - r; +} + +double HalfSpaceBox(const Vector3d& n, const Vector3d& p0, + const RigidTransformd& X_WC, const Vector3d& half) { + const Vector3d n_C = X_WC.rotation().matrix().transpose() * n; + return n.dot(X_WC.translation() - p0) - half.dot(n_C.cwiseAbs()); +} + +double HalfSpaceCapsule(const Vector3d& n, const Vector3d& p0, + const RigidTransformd& X_WC, double r, double length) { + const Vector3d axis = X_WC.rotation().matrix().col(2); + return n.dot(X_WC.translation() - p0) - 0.5 * length * std::abs(n.dot(axis)) - + r; +} + +double HalfSpaceCylinder(const Vector3d& n, const Vector3d& p0, + const RigidTransformd& X_WC, double r, double length) { + const Vector3d axis = X_WC.rotation().matrix().col(2); + const double axial = n.dot(axis); + return n.dot(X_WC.translation() - p0) - 0.5 * length * std::abs(axial) - + r * (n - axial * axis).norm(); +} + +double HalfSpaceEllipsoid(const Vector3d& n, const Vector3d& p0, + const RigidTransformd& X_WC, const Vector3d& radii) { + const Vector3d n_C = X_WC.rotation().matrix().transpose() * n; + return n.dot(X_WC.translation() - p0) - + Vector3d(radii.asDiagonal() * n_C).norm(); +} + +double HalfSpaceVertices(const Vector3d& n, const Vector3d& p0, + const RigidTransformd& X_WC, const Matrix3Xd& v_C) { + const Vector3d n_C = X_WC.rotation().matrix().transpose() * n; + return n.dot(X_WC.translation() - p0) + (n_C.transpose() * v_C).minCoeff(); +} + +// -------------------------------------------------------------------------- +// Mesh fixtures for the hull-semantics cases. +// -------------------------------------------------------------------------- + +// The 8 corners of a box centered on its frame origin. +Matrix3Xd BoxCorners(const Vector3d& half) { + Matrix3Xd v(3, 8); + int col = 0; + for (const double sx : {-1.0, 1.0}) { + for (const double sy : {-1.0, 1.0}) { + for (const double sz : {-1.0, 1.0}) { + v.col(col++) = Vector3d(sx * half.x(), sy * half.y(), sz * half.z()); + } + } + } + return v; +} + +const Vector3d& CubeHalf() { + static const Vector3d half(0.15, 0.20, 0.25); + return half; +} + +// The cube of the mesh cases: Drake's shipped unit cube (vertices at ±1) scaled +// to CubeHalf() by the non-uniform Mesh/Convex scale argument, so its vertices +// coincide with BoxCorners(CubeHalf()) to the last bit. +const std::string& CubeObjPath() { + static const std::string path = + FindResourceOrThrow("drake/geometry/test/quad_cube.obj"); + return path; +} + +Mesh CubeMesh() { + return Mesh(CubeObjPath(), CubeHalf()); +} + +Convex CubeConvex() { + return Convex(CubeObjPath(), CubeHalf()); +} + +// The L-shaped prism's cross-section, counter-clockwise. The reflex vertex is +// (1, 1); the convex hull closes the notch with the edge x + y = 3. +const std::vector& LProfile() { + static const std::vector profile = { + {0.0, 0.0}, {2.0, 0.0}, {2.0, 1.0}, {1.0, 1.0}, {1.0, 2.0}, {0.0, 2.0}}; + return profile; +} + +constexpr double kLHalfHeight = 0.5; + +// A closed, genuinely non-convex L-prism. No shipped asset matches the analytic +// expectations of the mesh-vs-hull cases below, so this one is generated, into +// memory rather than into a file so that the test stays off the filesystem. +InMemoryMesh LPrismMesh() { + const std::string contents = [] { + std::ostringstream out; + const auto& profile = LProfile(); + const int n = static_cast(profile.size()); + for (const double z : {-kLHalfHeight, kLHalfHeight}) { + for (const auto& xy : profile) { + out << "v " << xy.x() << " " << xy.y() << " " << z << "\n"; + } + } + // Side quads, split into triangles. OBJ indices are 1-based. + for (int i = 0; i < n; ++i) { + const int j = (i + 1) % n; + out << "f " << i + 1 << " " << j + 1 << " " << j + 1 + n << "\n"; + out << "f " << i + 1 << " " << j + 1 + n << " " << i + 1 + n << "\n"; + } + // Caps: a fan from vertex 0, which is valid for this particular profile. + for (int i = 1; i + 1 < n; ++i) { + out << "f 1 " << i + 2 << " " << i + 1 << "\n"; + out << "f " << 1 + n << " " << i + 1 + n << " " << i + 2 + n << "\n"; + } + return out.str(); + }(); + return InMemoryMesh{MemoryFile(contents, ".obj", "ccd_l_prism.obj")}; +} + +// ========================================================================== +// Accuracy vs analytic ground truth (native route). +// ========================================================================== + +// Poses two floating bodies at random 250 times and compares the oracle +// against `expected`, which returns the reference distance, or nullopt for a +// pose the reference formula does not cover. Both the separated and the +// penetrating branch must be exercised, and where the pair is separated the +// returned witnesses must be exactly phi apart. +void CheckAgainstReference( + World* world, const DistanceOracle& oracle, const RigidBody& body_a, + const RigidBody& body_b, double range, + const std::function( + const RigidTransformd&, const RigidTransformd&)>& expected, + std::mt19937* rng) { + ASSERT_EQ(oracle.pairs().size(), 1u); + const PairRecord& pair = oracle.pairs().front(); + EXPECT_EQ(pair.route, DistanceRoute::kNative); + int separated = 0; + int penetrating = 0; + for (int trial = 0; trial < 250; ++trial) { + const RigidTransformd X_WA = RandomPose(rng, range); + const RigidTransformd X_WB = RandomPose(rng, range); + world->SetPose(body_a, X_WA); + world->SetPose(body_b, X_WB); + const std::optional reference = expected(X_WA, X_WB); + if (!reference.has_value()) continue; + + Vector3d p_a_W; + Vector3d p_b_W; + const double phi = + oracle.SignedDistance(world->query(), pair, &p_a_W, &p_b_W); + EXPECT_NEAR(phi, *reference, kNative) << "trial " << trial; + if (phi > 1e-9) { + ++separated; + EXPECT_NEAR((p_a_W - p_b_W).norm(), phi, kNative) << "trial " << trial; + } else { + ++penetrating; + } + } + EXPECT_GT(separated, 0); + EXPECT_GT(penetrating, 0) << "the sweep never exercised the penetrating " + "branch"; +} + +GTEST_TEST(DistanceOracleAccuracy, SphereSphereMatchesAnalyticDistance) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + const double r_a = 0.13; + const double r_b = 0.21; + const auto& body_a = + AddShapeBody(&plant, "sphere_a", Sphere(r_a), Vector3d(-1, 0, 0)); + const auto& body_b = + AddShapeBody(&plant, "sphere_b", Sphere(r_b), Vector3d(1, 0, 0)); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram()); + + std::mt19937 rng(20260826); + // Exact for two spheres on both branches. + CheckAgainstReference( + &world, oracle, body_a, body_b, 0.4, + [r_a, r_b](const RigidTransformd& X_WA, const RigidTransformd& X_WB) { + return (X_WB.translation() - X_WA.translation()).norm() - r_a - r_b; + }, + &rng); +} + +GTEST_TEST(DistanceOracleAccuracy, SphereBoxMatchesAnalyticDistance) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + const double radius = 0.1; + const Vector3d half(0.15, 0.2, 0.25); + const auto& sphere_body = + AddShapeBody(&plant, "sphere", Sphere(radius), Vector3d(-1, 0, 0)); + const auto& box_body = + AddShapeBody(&plant, "box", Box(2 * half.x(), 2 * half.y(), 2 * half.z()), + Vector3d(1, 0, 0)); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram()); + + std::mt19937 rng(881); + // For a sphere whose center lies outside a convex body, the signed distance + // is exactly dist(center, body) - radius on both branches: the sublevel sets + // of dist(., body) are the Minkowski sums body (+) ball. A center inside the + // box has no such reference, so those poses are skipped. + CheckAgainstReference( + &world, oracle, sphere_body, box_body, 0.5, + [radius, half](const RigidTransformd& X_WS, + const RigidTransformd& X_WB) -> std::optional { + const Vector3d p_B = X_WB.inverse() * X_WS.translation(); + const double center_distance = PointBoxDistance(p_B, half); + if (center_distance <= 0.0) return std::nullopt; + return center_distance - radius; + }, + &rng); +} + +// ========================================================================== +// Analytic halfspace fallback: exact against hand-derived formulas. +// ========================================================================== + +// One world holding a halfspace plus one geometry of every partner class, +// all on floating bodies so both sides can be posed arbitrarily. +class HalfSpaceFallbackTest : public ::testing::Test { + protected: + void SetUp() override { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + AddShapeBody(&plant, "halfspace", HalfSpace(), Vector3d(0, 0, -3)); + AddShapeBody(&plant, "sphere", Sphere(kRadius), Vector3d(-3, 0, 0)); + AddShapeBody(&plant, "box", + Box(2 * kHalf.x(), 2 * kHalf.y(), 2 * kHalf.z()), + Vector3d(-1, 0, 0)); + AddShapeBody(&plant, "capsule", Capsule(kRadius, kLength), + Vector3d(1, 0, 0)); + AddShapeBody(&plant, "cylinder", Cylinder(kRadius, kLength), + Vector3d(3, 0, 0)); + AddShapeBody(&plant, "ellipsoid", + Ellipsoid(kRadii.x(), kRadii.y(), kRadii.z()), + Vector3d(0, 3, 0)); + AddShapeBody(&plant, "tetra", Convex(TetraVertices(), "tetra"), + Vector3d(0, -3, 0)); + world_ = std::make_unique(builder.Build()); + oracle_ = std::make_unique(world_->diagram()); + halfspace_id_ = GeometryOf(world_->plant(), "halfspace"); + } + + // An asymmetric tetrahedron: its hull vertices are exactly the four input + // points, so the reference minimum can be written down. + static Matrix3Xd TetraVertices() { + Matrix3Xd v(3, 4); + v.col(0) = Vector3d(0.0, 0.0, 0.0); + v.col(1) = Vector3d(0.31, 0.02, -0.05); + v.col(2) = Vector3d(0.04, 0.27, 0.03); + v.col(3) = Vector3d(-0.06, 0.05, 0.23); + return v; + } + + const RigidBody& Body(const std::string& name) const { + return world_->plant().GetBodyByName(name); + } + + static constexpr double kRadius = 0.11; + static constexpr double kLength = 0.34; + static inline const Vector3d kHalf{0.15, 0.2, 0.25}; + static inline const Vector3d kRadii{0.12, 0.19, 0.07}; + + std::unique_ptr world_; + std::unique_ptr oracle_; + GeometryId halfspace_id_; +}; + +TEST_F(HalfSpaceFallbackTest, EveryPartnerMatchesHandDerivedFormulaExactly) { + using Reference = std::function; + const std::vector> partners = { + {"sphere", + [](const Vector3d& n, const Vector3d& p0, const RigidTransformd& X) { + return HalfSpaceSphere(n, p0, X, kRadius); + }}, + {"box", + [](const Vector3d& n, const Vector3d& p0, const RigidTransformd& X) { + return HalfSpaceBox(n, p0, X, kHalf); + }}, + {"capsule", + [](const Vector3d& n, const Vector3d& p0, const RigidTransformd& X) { + return HalfSpaceCapsule(n, p0, X, kRadius, kLength); + }}, + {"cylinder", + [](const Vector3d& n, const Vector3d& p0, const RigidTransformd& X) { + return HalfSpaceCylinder(n, p0, X, kRadius, kLength); + }}, + {"ellipsoid", + [](const Vector3d& n, const Vector3d& p0, const RigidTransformd& X) { + return HalfSpaceEllipsoid(n, p0, X, kRadii); + }}, + {"tetra", + [](const Vector3d& n, const Vector3d& p0, const RigidTransformd& X) { + return HalfSpaceVertices(n, p0, X, TetraVertices()); + }}, + }; + + std::mt19937 rng(4242); + int positive = 0; + int negative = 0; + for (int trial = 0; trial < 200; ++trial) { + const RigidTransformd X_WH = RandomPose(&rng, 0.3); + world_->SetPose(Body("halfspace"), X_WH); + std::vector poses; + for (const auto& [name, reference] : partners) { + poses.push_back(RandomPose(&rng, 0.4)); + world_->SetPose(Body(name), poses.back()); + } + const QueryObject& query = world_->query(); + const Vector3d n_W = X_WH.rotation().matrix().col(2); + const Vector3d p0_W = X_WH.translation(); + + for (size_t i = 0; i < partners.size(); ++i) { + const PairRecord& pair = + FindPair(*oracle_, halfspace_id_, + GeometryOf(world_->plant(), partners[i].first)); + ASSERT_NE(pair.route, DistanceRoute::kNative) << partners[i].first; + + Vector3d p_a_W; + Vector3d p_b_W; + const double phi = oracle_->SignedDistance(query, pair, &p_a_W, &p_b_W); + EXPECT_NEAR(phi, partners[i].second(n_W, p0_W, poses[i]), kExact) + << partners[i].first << ", trial " << trial; + // Witnesses: separated by exactly |phi|, with the halfspace-side witness + // on the boundary plane. + const Vector3d& on_plane = (pair.a == halfspace_id_) ? p_a_W : p_b_W; + EXPECT_NEAR(n_W.dot(on_plane - p0_W), 0.0, kExact); + EXPECT_NEAR((p_a_W - p_b_W).norm(), std::abs(phi), kExact); + (phi > 0.0 ? positive : negative) += 1; + } + } + EXPECT_GT(positive, 0); + EXPECT_GT(negative, 0) << "no penetrating halfspace cases were exercised"; +} + +TEST_F(HalfSpaceFallbackTest, AxisParallelCylinderDirectionIsHandled) { + // Degenerate support direction: the plane normal is parallel to the cylinder + // axis, so every rim point ties. phi must still be exact. + world_->SetPose(Body("halfspace"), RigidTransformd(Vector3d(0, 0, -0.5))); + world_->SetPose(Body("cylinder"), RigidTransformd(Vector3d(0.0, 0.0, 0.4))); + const PairRecord& pair = FindPair(*oracle_, halfspace_id_, + GeometryOf(world_->plant(), "cylinder")); + Vector3d p_a_W; + Vector3d p_b_W; + const double phi = + oracle_->SignedDistance(world_->query(), pair, &p_a_W, &p_b_W); + EXPECT_NEAR(phi, 0.4 + 0.5 - kLength / 2, kExact); + EXPECT_NEAR((p_a_W - p_b_W).norm(), std::abs(phi), kExact); +} + +// ========================================================================== +// Capability probe: classification snapshot and refusals. +// ========================================================================== + +// A world with one geometry of every supported shape class. +class AllShapesTest : public ::testing::Test { + protected: + void SetUp() override { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + AddShapeBody(&plant, "sphere", Sphere(0.1), Vector3d(-1.5, 0, 1)); + AddShapeBody(&plant, "box", Box(0.2, 0.3, 0.4), Vector3d(-0.5, 0, 1)); + AddShapeBody(&plant, "capsule", Capsule(0.08, 0.3), Vector3d(0.5, 0, 1)); + AddShapeBody(&plant, "cylinder", Cylinder(0.09, 0.25), Vector3d(1.5, 0, 1)); + AddShapeBody(&plant, "ellipsoid", Ellipsoid(0.12, 0.09, 0.07), + Vector3d(-1.5, 1.5, 1)); + AddShapeBody(&plant, "convex", Convex(BoxCorners(CubeHalf()), "convex"), + Vector3d(-0.5, 1.5, 1)); + AddShapeBody(&plant, "mesh", CubeMesh(), Vector3d(0.5, 1.5, 1)); + // The halfspace is anchored on the world body: z <= 0 is solid. + halfspace_id_ = plant.RegisterCollisionGeometry( + plant.world_body(), RigidTransformd::Identity(), HalfSpace(), + "ground_geometry", Friction()); + world_ = std::make_unique(builder.Build()); + oracle_ = std::make_unique(world_->diagram()); + } + + static constexpr int kDynamicBodies = 7; + static constexpr int kNativePairs = kDynamicBodies * (kDynamicBodies - 1) / 2; + + std::unique_ptr world_; + std::unique_ptr oracle_; + GeometryId halfspace_id_; +}; + +TEST_F(AllShapesTest, ProbeClassifiesEveryPairSnapshot) { + // 7 dynamic geometries pairwise, plus each against the anchored halfspace. + ASSERT_EQ(static_cast(oracle_->pairs().size()), + kNativePairs + kDynamicBodies); + + int halfspace_pairs = 0; + int native_pairs = 0; + for (const PairRecord& pair : oracle_->pairs()) { + if (pair.a == halfspace_id_) { + EXPECT_EQ(pair.route, DistanceRoute::kHalfSpaceA); + ++halfspace_pairs; + } else if (pair.b == halfspace_id_) { + EXPECT_EQ(pair.route, DistanceRoute::kHalfSpaceB); + ++halfspace_pairs; + } else { + EXPECT_EQ(pair.route, DistanceRoute::kNative); + ++native_pairs; + } + // Every record carries the bodies its geometries hang from. + EXPECT_NE(pair.body_a, pair.body_b); + } + EXPECT_EQ(halfspace_pairs, kDynamicBodies); + EXPECT_EQ(native_pairs, kNativePairs); +} + +TEST_F(AllShapesTest, WitnessPointsAreConsistentForSeparatedNativePairs) { + std::mt19937 rng(31337); + int checked = 0; + for (int trial = 0; trial < 25; ++trial) { + world_->RandomizeAll(&rng, 0.6); + const QueryObject& query = world_->query(); + for (const PairRecord& pair : oracle_->pairs()) { + if (pair.route != DistanceRoute::kNative) continue; + Vector3d p_a_W; + Vector3d p_b_W; + const double phi = oracle_->SignedDistance(query, pair, &p_a_W, &p_b_W); + if (phi <= 1e-6) continue; + // Drake's own worst-case table for these combinations tops out at 5e-5. + EXPECT_NEAR((p_a_W - p_b_W).norm(), phi, 1e-4) << "trial " << trial; + ++checked; + } + } + EXPECT_GT(checked, 100); +} + +TEST_F(AllShapesTest, IdOrderingIsSymmetric) { + // Swapping a record's two slots (and, on the analytic route, the halfspace + // side with it) must return the same distance and the mirrored witnesses, + // bit for bit. + std::mt19937 rng(777); + world_->RandomizeAll(&rng, 0.5); + const QueryObject& query = world_->query(); + + int native = 0; + int halfspace = 0; + for (const PairRecord& pair : oracle_->pairs()) { + PairRecord swapped = pair; + std::swap(swapped.a, swapped.b); + std::swap(swapped.body_a, swapped.body_b); + if (pair.route == DistanceRoute::kNative) { + ++native; + } else { + ++halfspace; + swapped.route = (pair.route == DistanceRoute::kHalfSpaceA) + ? DistanceRoute::kHalfSpaceB + : DistanceRoute::kHalfSpaceA; + } + + Vector3d a1; + Vector3d b1; + Vector3d a2; + Vector3d b2; + const double phi = oracle_->SignedDistance(query, pair, &a1, &b1); + const double phi_swapped = + oracle_->SignedDistance(query, swapped, &a2, &b2); + EXPECT_EQ(phi, phi_swapped); + EXPECT_EQ(a1, b2); + EXPECT_EQ(b1, a2); + } + EXPECT_EQ(native, kNativePairs); + EXPECT_EQ(halfspace, kDynamicBodies); +} + +GTEST_TEST(DistanceOracleProbe, HalfSpaceHalfSpacePairThrowsAtConstruction) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransformd::Identity(), HalfSpace(), + "ground_geometry", Friction()); + AddShapeBody(&plant, "ceiling", HalfSpace(), Vector3d(0, 0, 2)); + World world(builder.Build()); + + // Both geometries must be named, in whichever order the candidate set has + // them. + DRAKE_EXPECT_THROWS_MESSAGE( + DistanceOracle(world.diagram()), + "[\\s\\S]*two HalfSpace geometries[\\s\\S]*" + "(ground_geometry[\\s\\S]*ceiling_geometry" + "|ceiling_geometry[\\s\\S]*ground_geometry)[\\s\\S]*"); +} + +GTEST_TEST(DistanceOracleProbe, ProbeSnapshotIsStableAcrossConstructions) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + AddShapeBody(&plant, "sphere", Sphere(0.1), Vector3d(-1, 0, 0)); + AddShapeBody(&plant, "box", Box(0.2, 0.2, 0.2), Vector3d(1, 0, 0)); + World world(builder.Build()); + + const DistanceOracle first(world.diagram()); + const DistanceOracle second(world.diagram()); + ASSERT_EQ(first.pairs().size(), second.pairs().size()); + for (size_t i = 0; i < first.pairs().size(); ++i) { + EXPECT_EQ(first.pairs()[i].a, second.pairs()[i].a); + EXPECT_EQ(first.pairs()[i].b, second.pairs()[i].b); + } +} + +GTEST_TEST(DistanceOracleProbe, DeformableGeometryIsRefusedByName) { + // Deformables need a discrete plant. + RobotDiagramBuilder builder(0.01); + MultibodyPlant& plant = builder.plant(); + AddShapeBody(&plant, "sphere", Sphere(0.1), Vector3d(0, 0, 1)); + + auto instance = std::make_unique( + RigidTransformd(Vector3d(0, 0, 0.4)), std::make_unique(0.05), + "squishy"); + drake::geometry::ProximityProperties props; + drake::geometry::AddContactMaterial(std::nullopt, std::nullopt, Friction(), + &props); + instance->set_proximity_properties(std::move(props)); + plant.mutable_deformable_model().RegisterDeformableBody( + std::move(instance), + drake::multibody::fem::DeformableBodyConfig{}, 0.05); + World world(builder.Build()); + + DRAKE_EXPECT_THROWS_MESSAGE(DistanceOracle(world.diagram()), + "[\\s\\S]*deformable[\\s\\S]*squishy[\\s\\S]*"); +} + +GTEST_TEST(DistanceOracleProbe, EmptyWorldProbesCleanly) { + RobotDiagramBuilder builder(0.0); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram()); + EXPECT_TRUE(oracle.pairs().empty()); +} + +// ========================================================================== +// Mesh is certified as its convex hull. +// ========================================================================== + +GTEST_TEST(DistanceOracleMesh, MeshDistanceEqualsConvexHullDistance) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + // The same cube: once as a Mesh (Drake silently hulls it), once as a Convex + // built from that hull's vertices. + const auto& mesh_body = + AddShapeBody(&plant, "mesh", CubeMesh(), Vector3d(-1, 0, 0)); + const auto& convex_body = + AddShapeBody(&plant, "convex", Convex(BoxCorners(CubeHalf()), "cube"), + Vector3d(1, 0, 0)); + const auto& probe_body = + AddShapeBody(&plant, "probe", Sphere(0.07), Vector3d(0, 2, 0)); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram()); + + const GeometryId probe_id = GeometryOf(world.plant(), "probe"); + const PairRecord& mesh_pair = + FindPair(oracle, GeometryOf(world.plant(), "mesh"), probe_id); + const PairRecord& convex_pair = + FindPair(oracle, GeometryOf(world.plant(), "convex"), probe_id); + + std::mt19937 rng(9091); + int separated = 0; + int penetrating = 0; + for (int trial = 0; trial < 100; ++trial) { + // Pose both cubes identically, then probe with a sphere. + const RigidTransformd X_WCube = RandomPose(&rng, 0.3); + world.SetPose(mesh_body, X_WCube); + world.SetPose(convex_body, X_WCube); + world.SetPose(probe_body, RandomPose(&rng, 0.5)); + + const QueryObject& query = world.query(); + const double phi_mesh = oracle.SignedDistance(query, mesh_pair); + EXPECT_NEAR(phi_mesh, oracle.SignedDistance(query, convex_pair), kExact) + << "trial " << trial; + (phi_mesh > 0 ? separated : penetrating) += 1; + } + EXPECT_GT(separated, 0); + EXPECT_GT(penetrating, 0); +} + +// A non-convex Mesh is measured as its convex hull, so a probe sitting in the +// L's concave notch, genuinely 0.35 m clear of the solid, is reported as +// penetrating. +GTEST_TEST(DistanceOracleMesh, + NonconvexMeshIsMeasuredAsItsConvexHullNotItsSurface) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + const auto& l_mesh_body = + AddShapeBody(&plant, "l_mesh", Mesh(LPrismMesh()), Vector3d(0, 0, 0)); + const auto& l_convex_body = + AddShapeBody(&plant, "l_convex", Convex(LPrismMesh()), Vector3d(0, 0, 0)); + const double probe_radius = 0.05; + const auto& probe_body = + AddShapeBody(&plant, "probe", Sphere(probe_radius), Vector3d(5, 5, 5)); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram()); + + const GeometryId probe_id = GeometryOf(world.plant(), "probe"); + const PairRecord& mesh_pair = + FindPair(oracle, GeometryOf(world.plant(), "l_mesh"), probe_id); + const PairRecord& convex_pair = + FindPair(oracle, GeometryOf(world.plant(), "l_convex"), probe_id); + world.SetPose(l_mesh_body, RigidTransformd::Identity()); + world.SetPose(l_convex_body, RigidTransformd::Identity()); + + // (1.4, 1.4) sits in the notch: outside the L (the nearest solid points are + // (1.4, 1.0) and (1.0, 1.4), so the true clearance is 0.4 - r = 0.35), but + // inside the hull, whose closing edge is x + y = 3. (1.9, 1.9) is outside the + // hull too, so there both agree and both are positive. + ASSERT_GT(0.4 - probe_radius, 0.0); + for (const auto& [p_W, expect_negative] : + std::vector>{ + {Vector3d(1.4, 1.4, 0.0), true}, {Vector3d(1.9, 1.9, 0.0), false}}) { + SCOPED_TRACE("probe at " + std::to_string(p_W.x())); + world.SetPose(probe_body, RigidTransformd(p_W)); + const QueryObject& query = world.query(); + const double phi_mesh = oracle.SignedDistance(query, mesh_pair); + EXPECT_NEAR(phi_mesh, oracle.SignedDistance(query, convex_pair), kExact); + EXPECT_EQ(phi_mesh < 0.0, expect_negative) + << "the L-mesh must measure as its convex hull, which swallows the " + "notch; got phi = " + << phi_mesh; + } +} + +GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + const auto& mesh_body = + AddShapeBody(&plant, "mesh", CubeMesh(), Vector3d(0, 0, 1)); + const auto& convex_body = + AddShapeBody(&plant, "convex", CubeConvex(), Vector3d(0, 3, 1)); + const GeometryId ground = plant.RegisterCollisionGeometry( + plant.world_body(), RigidTransformd::Identity(), HalfSpace(), + "ground_geometry", Friction()); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram()); + + const PairRecord& mesh_pair = + FindPair(oracle, ground, GeometryOf(world.plant(), "mesh")); + const PairRecord& convex_pair = + FindPair(oracle, ground, GeometryOf(world.plant(), "convex")); + const Matrix3Xd corners = BoxCorners(CubeHalf()); + + std::mt19937 rng(5150); + for (int trial = 0; trial < 100; ++trial) { + const RigidTransformd X_W = RandomPose(&rng, 0.3); + world.SetPose(mesh_body, X_W); + world.SetPose(convex_body, X_W); + const QueryObject& query = world.query(); + const double phi_mesh = oracle.SignedDistance(query, mesh_pair); + EXPECT_NEAR(phi_mesh, oracle.SignedDistance(query, convex_pair), kExact) + << "trial " << trial; + // The ground plane is z = 0 with the solid below, so the reference is the + // lowest transformed cube corner. + double lowest = std::numeric_limits::infinity(); + for (int i = 0; i < corners.cols(); ++i) { + lowest = std::min(lowest, (X_W * Vector3d(corners.col(i))).z()); + } + EXPECT_NEAR(phi_mesh, lowest, kExact) << "trial " << trial; + } +} + +} // namespace +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/motion_bound_test.cc b/planning/continuous_collision/test/motion_bound_test.cc new file mode 100644 index 000000000000..6898bac72d5f --- /dev/null +++ b/planning/continuous_collision/test/motion_bound_test.cc @@ -0,0 +1,1534 @@ +/* The displacement lemma and the J(p) subtree logic. An under-bounding λ(j, p) + makes the certifier certify a colliding path, with no other symptom, so a + failure here is a soundness bug in the kinematics module, not a test to loosen. + + Three property tests share one random-plant corpus: (1) move a single + coordinate j ∈ J(p) and check that every sampled point of the distal side + D(j, p) displaces by at most λ(j,p)·|Δq_j| in the other body's frame; (2) move + all coordinates and check that every A-point-to-B-point distance changes by at + most Σ λ(j,p)·|Δq_j|; (3) for pairs whose whole J(p) shares one distal side, + check the stronger one-sided form. + + (2) is stated on distances rather than on B's points in A's frame because the + distal side changes from joint to joint along a self-collision pair's J(p). + Each telescoping step is bounded in the frame of that step's static side, and + point-to-point distance is frame invariant. */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "drake/geometry/geometry_roles.h" +#include "drake/geometry/scene_graph_inspector.h" +#include "drake/multibody/tree/ball_rpy_joint.h" +#include "drake/multibody/tree/planar_joint.h" +#include "drake/multibody/tree/quaternion_floating_joint.h" +#include "drake/multibody/tree/rpy_floating_joint.h" +#include "drake/multibody/tree/screw_joint.h" +#include "drake/multibody/tree/weld_joint.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::geometry::GeometryId; +using drake::multibody::BodyIndex; +using drake::multibody::JointIndex; +using drake::multibody::PlanarJoint; +using drake::multibody::QuaternionFloatingJoint; +using drake::multibody::RpyFloatingJoint; +using drake::multibody::ScrewJoint; +using drake::multibody::WeldJoint; +using Eigen::Matrix3Xd; +using Eigen::Vector3d; +using Eigen::VectorXd; +using test::Box; +using test::Capsule; +using test::Friction; +using test::HalfSpace; +using test::Inertia; +using test::MultibodyPlant; +using test::PrismaticJoint; +using test::RevoluteJoint; +using test::RigidBody; +using test::RigidTransformd; +using test::Rng; +using test::RobotDiagram; +using test::RobotDiagramBuilder; +using test::Sphere; +using test::ThrowMessage; +using test::Uniform; +using test::UniformInt; +using ::testing::AllOf; +using ::testing::HasSubstr; + +/* Absolute slack on every displacement assertion. The claims are exact + mathematics; this only absorbs floating-point noise in Drake's forward + kinematics and in this test's own accumulation (both ~1e-15 here). */ +constexpr double kSlack = 1e-9; + +// --------------------------------------------------------------------------- +// A random world: a random tree of bodies with random joints, random fixed +// frame offsets on both sides of every joint, and random primitive geometries +// at random body-frame poses. +// --------------------------------------------------------------------------- + +struct RandomWorld { + std::unique_ptr> diagram; + /* Surface samples of each proximity geometry, expressed in its BODY frame + (that is, X_BG already applied). */ + std::unordered_map points_B; + int num_screw_joints{0}; +}; + +/* Adds one random primitive geometry to `body`; records its surface samples in + the body frame. */ +void AddRandomGeometry(Rng* rng, MultibodyPlant* plant, + const RigidBody& body, const std::string& name, + int num_samples, RandomWorld* world) { + const RigidTransformd X_BG = test::RandomTransform(rng, 0.2); + GeometryId gid; + Matrix3Xd p_G; + switch (UniformInt(rng, 0, 2)) { + case 0: { + const double r = Uniform(rng, 0.02, 0.15); + gid = plant->RegisterCollisionGeometry(body, X_BG, Sphere(r), name, + Friction()); + p_G = test::SampleSurface(rng, num_samples, [r](Rng* g) { + return test::SampleSphere(g, r); + }); + break; + } + case 1: { + const Vector3d size = test::UniformVector(rng, 0.02, 0.3); + gid = plant->RegisterCollisionGeometry(body, X_BG, Box(size), name, + Friction()); + p_G = test::SampleSurface(rng, num_samples, [size](Rng* g) { + return test::SampleBox(g, size); + }); + break; + } + default: { + const double r = Uniform(rng, 0.02, 0.1); + const double length = Uniform(rng, 0.05, 0.4); + gid = plant->RegisterCollisionGeometry(body, X_BG, Capsule(r, length), + name, Friction()); + p_G = test::SampleSurface(rng, num_samples, [r, length](Rng* g) { + return test::SampleCapsule(g, r, length); + }); + break; + } + } + Matrix3Xd p_B(3, p_G.cols()); + for (int i = 0; i < p_G.cols(); ++i) p_B.col(i) = X_BG * p_G.col(i); + world->points_B.emplace(gid, std::move(p_B)); +} + +RandomWorld MakeRandomWorld(Rng* rng, bool allow_screw, int num_samples) { + RandomWorld world; + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + + const int num_bodies = UniformInt(rng, 3, 7); + std::vector*> bodies{&plant.world_body()}; + for (int i = 0; i < num_bodies; ++i) { + const RigidBody& body = + plant.AddRigidBody(fmt::format("b{}", i), Inertia()); + // Parent is any earlier body (including the world), so the corpus mixes + // serial chains with branching trees. + const RigidBody& parent = + *bodies[UniformInt(rng, 0, static_cast(bodies.size()) - 1)]; + const RigidTransformd X_PF = test::RandomTransform(rng, 0.25); + const RigidTransformd X_CM = test::RandomTransform(rng, 0.25); + const std::string jn = fmt::format("j{}", i); + switch (UniformInt(rng, 0, allow_screw ? 4 : 3)) { + case 0: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + test::RandomUnitVector(rng)); + break; + case 1: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + test::RandomUnitVector(rng)); + break; + case 2: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + Vector3d::Zero()); + break; + case 3: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + test::RandomTransform(rng, 0.2)); + break; + default: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + test::RandomUnitVector(rng), + Uniform(rng, 0.05, 0.6), 0.0); + ++world.num_screw_joints; + break; + } + bodies.push_back(&body); + } + + // Always give the world and the first body a geometry so every world has at + // least one pair; sprinkle the rest randomly (some bodies get none, which + // exercises geometry-free bodies contributing chain hops only). + AddRandomGeometry(rng, &plant, plant.world_body(), "g_world", num_samples, + &world); + for (size_t i = 1; i < bodies.size(); ++i) { + const int count = (i == 1) ? 1 : UniformInt(rng, 0, 2); + for (int g = 0; g < count; ++g) { + AddRandomGeometry(rng, &plant, *bodies[i], fmt::format("g{}_{}", i, g), + num_samples, &world); + } + } + + world.diagram = builder.Build(); + return world; +} + +// --------------------------------------------------------------------------- +// Plant introspection helpers used by the tests (independent of the module +// under test, so a bug in the module cannot hide behind them). +// --------------------------------------------------------------------------- + +/* Subtree membership S_j for every joint that has velocities, straight from + Drake. */ +std::map> SubtreeSets( + const MultibodyPlant& plant) { + std::map> out; + for (JointIndex ji : plant.GetJointIndices()) { + const auto& joint = plant.get_joint(ji); + if (joint.num_velocities() == 0) continue; + std::vector members(plant.num_bodies(), false); + for (BodyIndex b : plant.GetBodiesKinematicallyAffectedBy({ji})) { + members[b] = true; + } + out.emplace(ji, std::move(members)); + } + return out; +} + +/* Position coordinate -> owning JointIndex. */ +std::vector CoordinateOwners(const MultibodyPlant& plant) { + std::vector owner(plant.num_positions()); + for (JointIndex ji : plant.GetJointIndices()) { + const auto& joint = plant.get_joint(ji); + for (int c = 0; c < joint.num_positions(); ++c) { + owner[joint.position_start() + c] = ji; + } + } + return owner; +} + +/* True for coordinates that parameterize a rotation (used only to pick + sensible random control-box widths). */ +std::vector AngularCoordinates(const MultibodyPlant& plant) { + std::vector angular(plant.num_positions(), false); + for (JointIndex ji : plant.GetJointIndices()) { + const auto& joint = plant.get_joint(ji); + const int ps = joint.position_start(); + if (joint.type_name() == "revolute" || joint.type_name() == "screw") { + for (int c = 0; c < joint.num_positions(); ++c) angular[ps + c] = true; + } else if (joint.type_name() == "planar") { + angular[ps + 2] = true; + } + } + return angular; +} + +std::vector CollisionPairs(const RobotDiagram& diagram) { + const MultibodyPlant& plant = diagram.plant(); + const auto& inspector = diagram.scene_graph().model_inspector(); + std::vector pairs; + for (const auto& [ga, gb] : inspector.GetCollisionCandidates()) { + const BodyIndex ba = + plant.GetBodyFromFrameId(inspector.GetFrameId(ga))->index(); + const BodyIndex bb = + plant.GetBodyFromFrameId(inspector.GetFrameId(gb))->index(); + pairs.push_back(PairRecord{ga, gb, ba, bb}); + } + return pairs; +} + +/* The whole-plant λ table over the box [lower, upper] with `constant` carved + out, on a model with exactly one collision pair. */ +MotionBoundTable OnePairTable(const KinematicsEngine& engine, + const std::vector& pairs, + const VectorXd& lower, const VectorXd& upper, + const std::vector& constant) { + EXPECT_EQ(pairs.size(), 1u); + return engine.ComputeMotionBoundTable(lower, upper, constant, pairs); +} + +/* Max over `points_B` of how far the point moves in `frame_o` when the plant + goes from `q` to `qp`. */ +double Displacement(const MultibodyPlant& plant, + drake::systems::Context* ctx, + const Matrix3Xd& points_B, + const drake::multibody::Frame& frame_d, + const drake::multibody::Frame& frame_o, + const VectorXd& q, const VectorXd& qp) { + Matrix3Xd before(3, points_B.cols()); + Matrix3Xd after(3, points_B.cols()); + plant.SetPositions(ctx, q); + plant.CalcPointsPositions(*ctx, frame_d, points_B, frame_o, &before); + plant.SetPositions(ctx, qp); + plant.CalcPointsPositions(*ctx, frame_d, points_B, frame_o, &after); + return (after - before).colwise().norm().maxCoeff(); +} + +// --------------------------------------------------------------------------- +// Part 1. J(p) subtree logic on hand-built plants. +// --------------------------------------------------------------------------- + +/* The position coordinates of a named joint. */ +std::vector CoordsOf(const MultibodyPlant& plant, + const std::string& joint_name) { + const auto& joint = plant.GetJointByName(joint_name); + std::vector out; + for (int c = 0; c < joint.num_positions(); ++c) { + out.push_back(joint.position_start() + c); + } + return out; +} + +std::vector Merge(std::vector> groups) { + std::vector out; + for (const auto& g : groups) out.insert(out.end(), g.begin(), g.end()); + std::sort(out.begin(), out.end()); + return out; +} + +/* One plant carrying every topology J(p) has to get right at once: + + world --w_env(weld)--> env (anchored) + world --j0(Rz)------> b1 --jl(Ry)--> left --jt(Rx)--> tip + --jr(planar)--> right + tip --w1(weld)--> hand --w2(weld)--> finger (a welded cluster) */ +GTEST_TEST(JointSupportTest, TopologyDeterminesTheCoordinateSet) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& env = plant.AddRigidBody("env", Inertia()); + const auto& b1 = plant.AddRigidBody("b1", Inertia()); + const auto& left = plant.AddRigidBody("left", Inertia()); + const auto& right = plant.AddRigidBody("right", Inertia()); + const auto& tip = plant.AddRigidBody("tip", Inertia()); + const auto& hand = plant.AddRigidBody("hand", Inertia()); + const auto& finger = plant.AddRigidBody("finger", Inertia()); + const auto tx = [](double x) { + return RigidTransformd(Vector3d(x, 0.0, 0.0)); + }; + plant.AddJoint("w_env", plant.world_body(), {}, env, {}, tx(1.0)); + plant.AddJoint("j0", plant.world_body(), {}, b1, {}, + Vector3d::UnitZ()); + plant.AddJoint("jl", b1, {}, left, {}, Vector3d::UnitY()); + plant.AddJoint("jr", b1, {}, right, {}, Vector3d::Zero()); + plant.AddJoint("jt", left, {}, tip, {}, Vector3d::UnitX()); + plant.AddJoint("w1", tip, {}, hand, {}, tx(0.2)); + plant.AddJoint("w2", hand, {}, finger, {}, tx(0.05)); + auto diagram = builder.Build(); + const KinematicsEngine engine(*diagram); + const auto& p = diagram->plant(); + const auto affecting = [&engine](const RigidBody& a, + const RigidBody& b) { + return engine.CoordinatesAffectingPair(a.index(), b.index()); + }; + + const std::vector j0 = CoordsOf(p, "j0"); + const std::vector jl = CoordsOf(p, "jl"); + const std::vector jr = CoordsOf(p, "jr"); + const std::vector jt = CoordsOf(p, "jt"); + ASSERT_EQ(jr.size(), 3); // A planar joint contributes three coordinates. + + // A serial chain against the anchored environment, or against the world: the + // ancestors of the robot body. + EXPECT_EQ(affecting(env, tip), Merge({j0, jl, jt})); + EXPECT_EQ(affecting(p.world_body(), left), Merge({j0, jl})); + // A self pair through the common ancestor: the path between the two bodies. + EXPECT_EQ(affecting(b1, tip), Merge({jl, jt})); + EXPECT_EQ(affecting(left, tip), Merge({jt})); + // The symmetric difference across a branch point: j0 affects both sides and + // drops out. + EXPECT_EQ(affecting(tip, right), Merge({jl, jr, jt})); + // A body against itself, and two anchored bodies, are static ... + EXPECT_TRUE(affecting(tip, tip).empty()); + EXPECT_TRUE(affecting(p.world_body(), env).empty()); + // ... as is everything inside a welded cluster, which for every other pair + // moves as the single body it hangs off. + EXPECT_TRUE(affecting(tip, finger).empty()); + EXPECT_TRUE(affecting(hand, finger).empty()); + EXPECT_EQ(affecting(env, finger), Merge({j0, jl, jt})); +} + +GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& l1 = plant.AddRigidBody("l1", Inertia()); + const auto& l2 = plant.AddRigidBody("l2", Inertia()); + plant.AddJoint("j1", plant.world_body(), {}, l1, {}, + Vector3d::UnitZ()); + plant.AddJoint("j2", l1, {}, l2, {}, Vector3d::UnitY()); + plant.RegisterCollisionGeometry(plant.world_body(), RigidTransformd(), + Sphere(0.1), "g_world", Friction()); + plant.RegisterCollisionGeometry(l2, RigidTransformd(Vector3d(0.3, 0, 0)), + Sphere(0.05), "g_tip", Friction()); + auto diagram = builder.Build(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + const int nq = diagram->plant().num_positions(); + const VectorXd lower = VectorXd::Constant(nq, -0.5); + const VectorXd upper = VectorXd::Constant(nq, 0.5); + + { // Nothing constant: both coordinates appear. + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, std::vector(nq, false)); + ASSERT_EQ(table.num_pairs(), 1); + EXPECT_FALSE(table.pair_is_static(0)); + EXPECT_EQ(table.GetEntries(0).size(), 2); + } + { // One constant: only the other survives. + std::vector constant(nq, false); + constant[0] = true; + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, constant); + ASSERT_EQ(table.GetEntries(0).size(), 1); + EXPECT_EQ(table.GetEntries(0)[0].first, 1); + } + { // All constant, and *exactly* so (the box collapses with the flags, as it + // does for a real path): the pair becomes static and its motion bound is + // exactly zero. + const VectorXd pinned = VectorXd::Constant(nq, 0.25); + const MotionBoundTable table = OnePairTable(engine, pairs, pinned, pinned, + std::vector(nq, true)); + EXPECT_TRUE(table.pair_is_static(0)); + EXPECT_EQ(table.carveout_slack(0), 0.0); + EXPECT_EQ(table.MotionBound(0, VectorXd::Constant(nq, 1.0)), 0.0); + } +} + +GTEST_TEST(JointSupportTest, ReversedJointThrowsWithAnActionableMessage) { + // A joint whose declared parent ends up OUTBOARD of its declared child once + // the tree is rooted at the world. Drake reverses the mobilizer internally; + // the reach chain does not model that, so the library rejects it by name. + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& a = plant.AddRigidBody("body_a", Inertia()); + const auto& b = plant.AddRigidBody("body_b", Inertia()); + plant.AddJoint("w", plant.world_body(), {}, b, {}, + RigidTransformd(Vector3d(0.1, 0.0, 0.0))); + // Parent is `a` (which hangs off `b`), child is `b` (already anchored). + plant.AddJoint("reversed", a, {}, b, {}, Vector3d::UnitZ()); + std::unique_ptr> diagram; + try { + diagram = builder.Build(); + } catch (const std::exception& e) { + GTEST_SKIP() << "this Drake refuses the model outright: " << e.what(); + } + EXPECT_THAT(ThrowMessage([&]() { + KinematicsEngine engine(*diagram); + }), + HasSubstr("reversed")); +} + +// --------------------------------------------------------------------------- +// Part 1b. The half-space rule. +// --------------------------------------------------------------------------- + +/* world --(revolute or prismatic)--> link, with a half space on + `halfspace_on_link` and a sphere on the other body. */ +std::unique_ptr> MakeHalfSpaceModel(bool halfspace_on_link, + bool prismatic) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& link = plant.AddRigidBody("link", Inertia()); + if (prismatic) { + plant.AddJoint("j", plant.world_body(), {}, link, {}, + Vector3d::UnitZ()); + } else { + plant.AddJoint("j", plant.world_body(), {}, link, {}, + Vector3d::UnitY()); + } + const RigidTransformd I; + if (halfspace_on_link) { + plant.RegisterCollisionGeometry(link, I, HalfSpace(), "hs", Friction()); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransformd(Vector3d(0, 0, 1.0)), + Sphere(0.1), "ball", Friction()); + } else { + plant.RegisterCollisionGeometry(plant.world_body(), I, HalfSpace(), "hs", + Friction()); + plant.RegisterCollisionGeometry(link, RigidTransformd(Vector3d(0.3, 0, 0)), + Sphere(0.1), "ball", Friction()); + } + return builder.Build(); +} + +GTEST_TEST(HalfSpaceRuleTest, OnlyRotationRelativeToAHalfSpaceIsRefused) { + // An anchored ground plane under a rotating arm is the canonical accepted + // case: the half space is never the *distal* side, so λ bounds the arm's + // points and the pair is perfectly certifiable. A half space that itself + // rotates relative to its partner has no finite reach and must be refused at + // construction, by name. Pure translation of the half space keeps every one + // of its points moving by |Δq|, so λ = 1 is finite and correct even though + // the reach is not. + for (const bool prismatic : {false, true}) { + auto ground = + MakeHalfSpaceModel(/* halfspace_on_link = */ false, prismatic); + const KinematicsEngine engine(*ground); + const std::vector pairs = CollisionPairs(*ground); + const int nq = ground->plant().num_positions(); + const MotionBoundTable table = + OnePairTable(engine, pairs, VectorXd::Constant(nq, -1.0), + VectorXd::Constant(nq, 1.0), std::vector(nq, false)); + ASSERT_EQ(table.GetEntries(0).size(), 1); + EXPECT_GT(table.GetEntries(0)[0].second, 0.0); + EXPECT_TRUE(std::isfinite(table.GetEntries(0)[0].second)); + } + + auto translating = MakeHalfSpaceModel(/* halfspace_on_link = */ true, true); + const KinematicsEngine engine(*translating); + const std::vector pairs = CollisionPairs(*translating); + const int nq = translating->plant().num_positions(); + const MotionBoundTable table = + OnePairTable(engine, pairs, VectorXd::Constant(nq, -1.0), + VectorXd::Constant(nq, 1.0), std::vector(nq, false)); + ASSERT_EQ(table.GetEntries(0).size(), 1); + EXPECT_EQ(table.GetEntries(0)[0].second, 1.0); + + auto rotating = MakeHalfSpaceModel(/* halfspace_on_link = */ true, false); + EXPECT_THAT(ThrowMessage([&]() { + KinematicsEngine bad(*rotating); + }), + AllOf(HasSubstr("hs"), HasSubstr("HalfSpace"))); +} + +/* world --(revolute j0)--> b1 --(quaternion floating)--> b2 --(revolute j1)--> + b3, with geometry on the world and on b3. The floating joint sits mid-chain so + that the reach for j0 has to cross it, which is where its X_FM translation + must be picked up from the control box. */ +std::unique_ptr> MakeMidChainFloatingModel() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& b1 = plant.AddRigidBody("b1", Inertia()); + const auto& b2 = plant.AddRigidBody("b2", Inertia()); + const auto& b3 = plant.AddRigidBody("b3", Inertia()); + plant.AddJoint("j0", plant.world_body(), {}, b1, {}, + Vector3d::UnitZ()); + plant.AddJoint( + "jf", b1, RigidTransformd(Vector3d(0.1, 0.0, 0.0)), b2, + RigidTransformd(Vector3d(0.0, 0.05, 0.0))); + plant.AddJoint( + "j1", b2, RigidTransformd(Vector3d(0.0, 0.0, 0.15)), b3, + RigidTransformd(Vector3d(0.07, 0.0, 0.0)), Vector3d::UnitY()); + plant.RegisterCollisionGeometry(plant.world_body(), RigidTransformd(), + Sphere(0.1), "g_world", Friction()); + plant.RegisterCollisionGeometry(b3, RigidTransformd(Vector3d(0.2, 0, 0)), + Sphere(0.05), "g_tip", Friction()); + return builder.Build(); +} + +GTEST_TEST(JointSupportTest, MovingQuaternionFloatingJointThrows) { + auto diagram = MakeMidChainFloatingModel(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + const int nq = diagram->plant().num_positions(); + EXPECT_THAT(ThrowMessage([&]() { + engine.ComputeMotionBoundTable( + VectorXd::Constant(nq, -0.5), VectorXd::Constant(nq, 0.5), + std::vector(nq, false), pairs); + }), + AllOf(HasSubstr("jf"), HasSubstr("quaternion_floating"), + HasSubstr("constant"))); +} + +GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { + auto diagram = MakeMidChainFloatingModel(); + const MultibodyPlant& plant = diagram->plant(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + + const auto& jf = plant.GetJointByName("jf"); + const int nq = plant.num_positions(); + ASSERT_EQ(jf.num_positions(), 7); + + // The floating pose is pinned: identity orientation, a large offset. + const Vector3d p_FM(0.9, -0.7, 0.4); + VectorXd q0 = VectorXd::Zero(nq); + std::vector constant(nq, false); + const int fs = jf.position_start(); + q0[fs] = 1.0; // w of the wxyz quaternion. + q0.segment<3>(fs + 4) = p_FM; + for (int c = fs; c < fs + 7; ++c) constant[c] = true; + + VectorXd lower = q0; + VectorXd upper = q0; + const auto& j0 = plant.GetJointByName("j0"); + const auto& j1 = plant.GetJointByName("j1"); + for (int c : {j0.position_start(), j1.position_start()}) { + lower[c] = -1.0; + upper[c] = 1.0; + } + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, constant); + ASSERT_EQ(table.GetEntries(0).size(), 2); + + // The reach for j0 must include the floating joint's 1.22 m offset; a bound + // that silently dropped it would be far too small. + double lambda_j0 = 0.0; + for (const auto& [c, lam] : table.GetEntries(0)) { + if (c == j0.position_start()) lambda_j0 = lam; + } + EXPECT_GT(lambda_j0, p_FM.norm()); + + // And the displacement lemma must hold on this model. + auto root = diagram->CreateDefaultContext(); + auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); + Rng rng(0xF10A7); + const Matrix3Xd points_B = + test::SampleSurface(&rng, 128, + [](Rng* g) { + return test::SampleSphere(g, 0.05); + }) + .colwise() + + Vector3d(0.2, 0, 0); + const auto& frame_tip = plant.GetBodyByName("b3").body_frame(); + for (int trial = 0; trial < 200; ++trial) { + VectorXd q = q0; + VectorXd qp = q0; + for (int c : {j0.position_start(), j1.position_start()}) { + q[c] = Uniform(&rng, lower[c], upper[c]); + qp[c] = Uniform(&rng, lower[c], upper[c]); + } + const double displacement = Displacement(plant, &ctx, points_B, frame_tip, + plant.world_frame(), q, qp); + const double bound = table.MotionBound(0, (qp - q).cwiseAbs()); + ASSERT_LE(displacement, bound + kSlack) + << "trial " << trial << ": displacement " << displacement << " > bound " + << bound; + } +} + +// --------------------------------------------------------------------------- +// Part 1c. An exactly tight reach chain. +// +// The chain walk's triangle inequalities are slack at random poses, so a term +// that is merely too small hides inside that slack in the randomized corpus. +// Here every offset lies along +x with identity rotation, giving +// ‖a + b‖ = ‖a‖ + ‖b‖ at every hop, so each contribution to r shows up in λ. +// +// world --j_top(axis ẑ)--> b1 --j_slide(axis x̂)--> b2 --weld--> b3(sphere) +// +// with, all along x̂: ‖p_CM(j_top)‖ = L1, ‖p_PF(j_slide)‖ = d1, the slide's box +// maximum s, ‖p_CM(j_slide)‖ = d2, the weld's ‖p_PF‖ = e1, ‖X_FM‖ = e2, +// ‖p_CM‖ = e3, and the sphere reaching L3 + ρ from b3's origin. At the slide's +// box maximum the farthest sphere point sits at exactly +// r = (L3 + ρ) + (e3 + e2 + e1) + (d2 + s + d1) + L1 +// from j_top's M-frame origin, in the plane normal to the joint axis. +// --------------------------------------------------------------------------- + +struct TightChain { + std::unique_ptr> diagram; + double expected_reach{}; + double slide_max{}; + Vector3d far_point_b3; // The exactly-reaching material point, in b3's frame. +}; + +TightChain MakeTightChain(bool screw_top, double screw_pitch) { + constexpr double kL1 = 0.37, kD1 = 0.29, kS = 0.53, kD2 = 0.19; + constexpr double kE1 = 0.11, kE2 = 0.23, kE3 = 0.17; + constexpr double kL3 = 0.31, kRho = 0.13; + const auto tx = [](double x) { + return RigidTransformd(Vector3d(x, 0.0, 0.0)); + }; + + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& b1 = plant.AddRigidBody("b1", Inertia()); + const auto& b2 = plant.AddRigidBody("b2", Inertia()); + const auto& b3 = plant.AddRigidBody("b3", Inertia()); + if (screw_top) { + plant.AddJoint("j_top", plant.world_body(), tx(0.0), b1, + tx(-kL1), Vector3d::UnitZ(), screw_pitch, 0.0); + } else { + plant.AddJoint("j_top", plant.world_body(), tx(0.0), b1, + tx(-kL1), Vector3d::UnitZ()); + } + plant.AddJoint("j_slide", b1, tx(kD1), b2, tx(-kD2), + Vector3d::UnitX()); + plant.AddJoint("j_weld", b2, tx(kE1), b3, tx(-kE3), tx(kE2)); + plant.RegisterCollisionGeometry(plant.world_body(), RigidTransformd(), + Sphere(0.02), "g_world", Friction()); + plant.RegisterCollisionGeometry(b3, tx(kL3), Sphere(kRho), "g_tip", + Friction()); + + TightChain out; + out.diagram = builder.Build(); + out.expected_reach = + (kL3 + kRho) + (kE3 + kE2 + kE1) + (kD2 + kS + kD1) + kL1; + out.slide_max = kS; + out.far_point_b3 = Vector3d(kL3 + kRho, 0.0, 0.0); + return out; +} + +/* Reads λ(j_top) and λ(j_slide) off the tight chain and returns the true + displacement of the exactly-reaching material point under a small Δθ at the + slide's box maximum, where the chord 2r·sin(Δθ/2) recovers r·Δθ to eight + digits. */ +struct TightChainProbe { + double lambda_top{}; + double lambda_slide{}; + double displacement{}; + double dtheta{1e-4}; + double whole_box_bound{}; +}; + +TightChainProbe ProbeTightChain(const TightChain& chain) { + const MultibodyPlant& plant = chain.diagram->plant(); + const KinematicsEngine engine(*chain.diagram); + const std::vector pairs = CollisionPairs(*chain.diagram); + const int nq = plant.num_positions(); + const auto& j_top = plant.GetJointByName("j_top"); + const auto& j_slide = plant.GetJointByName("j_slide"); + VectorXd lower = VectorXd::Zero(nq); + VectorXd upper = VectorXd::Zero(nq); + lower[j_top.position_start()] = -1.0; + upper[j_top.position_start()] = 1.0; + upper[j_slide.position_start()] = chain.slide_max; + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, std::vector(nq, false)); + + TightChainProbe probe; + for (const auto& [c, lam] : table.GetEntries(0)) { + if (c == j_top.position_start()) probe.lambda_top = lam; + if (c == j_slide.position_start()) probe.lambda_slide = lam; + } + + auto root = chain.diagram->CreateDefaultContext(); + auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); + VectorXd q = VectorXd::Zero(nq); + q[j_slide.position_start()] = chain.slide_max; + VectorXd qp = q; + qp[j_top.position_start()] = probe.dtheta; + Matrix3Xd p_B3(3, 1); + p_B3.col(0) = chain.far_point_b3; + probe.displacement = + Displacement(plant, &ctx, p_B3, plant.GetBodyByName("b3").body_frame(), + plant.world_frame(), q, qp); + probe.whole_box_bound = table.MotionBound(0, (qp - q).cwiseAbs()); + return probe; +} + +GTEST_TEST(ReachTest, RevoluteChainIsExactAndTight) { + const TightChain chain = MakeTightChain(/* screw_top = */ false, 0.0); + const TightChainProbe probe = ProbeTightChain(chain); + // Every hop contributes digit for digit: p_CM at the top, both frame offsets + // and the box maximum of the slide, all three legs of the weld (including + // its X_FM translation), and the geometry's own reach past b3's origin. + EXPECT_NEAR(probe.lambda_top, chain.expected_reach, 1e-12); + EXPECT_EQ(probe.lambda_slide, 1.0); + + const double bound = probe.lambda_top * probe.dtheta; + EXPECT_LE(probe.displacement, bound + kSlack); + EXPECT_GT(probe.displacement / bound, 1.0 - 1e-8) + << "the reach must be exactly attained on this chain; a slack bound here " + "would mean a term is over-counted, and a violated bound would mean a " + "term is missing"; + // The whole-box motion bound must dominate the true displacement too. + EXPECT_LE(probe.displacement, probe.whole_box_bound + kSlack); +} + +GTEST_TEST(ReachTest, ScrewLambdaIncludesPitchAndIsNecessary) { + constexpr double kPitch = 8.0; // meters of travel per revolution. + const TightChain chain = MakeTightChain(/* screw_top = */ true, kPitch); + const TightChainProbe probe = ProbeTightChain(chain); + const double pitch_term = kPitch / (2.0 * M_PI); + EXPECT_NEAR(probe.lambda_top, chain.expected_reach + pitch_term, 1e-12); + EXPECT_LE(probe.displacement, probe.lambda_top * probe.dtheta + kSlack); + // The helix's axial travel is orthogonal to the chord it sweeps, so the true + // displacement is √(r² + (pitch/2π)²)·Δθ, strictly larger than r·Δθ, so + // dropping the pitch term would be unsound, not merely conservative. + EXPECT_GT(probe.displacement, + chain.expected_reach * probe.dtheta * (1.0 + 1e-6)) + << "a screw λ of r alone would under-bound this motion"; + EXPECT_NEAR(probe.displacement, + std::hypot(chain.expected_reach, pitch_term) * probe.dtheta, + 1e-11); +} + +// --------------------------------------------------------------------------- +// Part 2. The displacement lemma property test. +// --------------------------------------------------------------------------- + +struct LemmaStats { + int plants{0}; + int pairs{0}; + int atomic_checks{0}; + int aggregate_checks{0}; + int one_sided_checks{0}; + int screw_joints{0}; + /* Largest observed displacement / bound ratio. A corpus in which this stays + near zero would pass no matter how wrong λ is, so the tests assert it gets + close to 1: the bound must be *tight somewhere*, which is what makes the + property test sensitive to an under-bound. */ + double max_tightness{0.0}; + /* Pairs that were charged a nonzero carve-out slack, and the largest such + slack seen. Guards against the sub-tolerance corpus degenerating into the + exactly-constant one, which would test nothing new. */ + int slack_charged_pairs{0}; + double max_slack{0.0}; + + void Observe(double achieved, double bound) { + if (bound > 1e-12) { + max_tightness = std::max(max_tightness, achieved / bound); + } + } +}; + +/* How the random control box treats the coordinates it flags constant. */ +enum class CarveOut { + /* No coordinate is flagged constant. */ + kNone, + /* Flagged coordinates collapse to a single point: the carve-out is exact and + the slack must be bit-exactly zero. */ + kExact, + /* Flagged coordinates keep a random *sub-tolerance* width, which is what the + curve module's tolerance-based flag actually admits. The carve-out then owes + a residual, and MotionBoundTable::carveout_slack() must pay for it. */ + kSubTolerance, +}; + +/* Runs every displacement-lemma check on one random world. */ +void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, + LemmaStats* stats) { + const RobotDiagram& diagram = *world.diagram; + const MultibodyPlant& plant = diagram.plant(); + const KinematicsEngine engine(diagram); + const std::vector pairs = CollisionPairs(diagram); + if (pairs.empty()) return; + + const int nq = plant.num_positions(); + const std::vector angular = AngularCoordinates(plant); + const std::map> subtrees = SubtreeSets(plant); + const std::vector owner = CoordinateOwners(plant); + + // A random control box around a random nominal configuration. A + // tolerance-carved coordinate keeps a nonzero, sub-tolerance width: the box + // the curve module would hand us, not a collapsed point. The samples below + // draw from that width too, so the residual is genuinely exercised rather + // than assumed away. + VectorXd q0(nq); + VectorXd lower(nq); + VectorXd upper(nq); + std::vector constant(nq, false); + for (int c = 0; c < nq; ++c) { + q0[c] = angular[c] ? Uniform(rng, -M_PI, M_PI) : Uniform(rng, -0.5, 0.5); + const bool is_constant = + carve_out != CarveOut::kNone && Uniform(rng, 0.0, 1.0) < 0.3; + constant[c] = is_constant; + double half; + if (is_constant) { + half = (carve_out == CarveOut::kSubTolerance) + ? 0.5 * Uniform(rng, 0.02 * kContinuityTolerance, + kContinuityTolerance) + : 0.0; + } else { + half = angular[c] ? Uniform(rng, 0.05, 1.2) : Uniform(rng, 0.02, 0.4); + } + lower[c] = q0[c] - half; + upper[c] = q0[c] + half; + } + + const MotionBoundTable table = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + ASSERT_EQ(table.num_pairs(), static_cast(pairs.size())); + ++stats->plants; + + auto root = diagram.CreateDefaultContext(); + auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); + + // The λ table's coordinate sets must be exactly J(p) minus the constants. + for (int k = 0; k < table.num_pairs(); ++k) { + std::vector expected; + for (int c : + engine.CoordinatesAffectingPair(pairs[k].body_a, pairs[k].body_b)) { + if (!constant[c]) expected.push_back(c); + } + std::vector actual; + for (const auto& [c, lam] : table.GetEntries(k)) { + actual.push_back(c); + ASSERT_TRUE(std::isfinite(lam)); + ASSERT_GE(lam, 0.0); + } + ASSERT_EQ(actual, expected) << "pair " << k; + ASSERT_EQ(table.pair_is_static(k), expected.empty()); + + // The carve-out slack is a *residual*, so it is exactly zero unless some + // carved coordinate genuinely keeps a nonzero width. + const double pair_slack = table.carveout_slack(k); + ASSERT_TRUE(std::isfinite(pair_slack)); + ASSERT_GE(pair_slack, 0.0); + if (carve_out != CarveOut::kSubTolerance) { + ASSERT_EQ(pair_slack, 0.0) << "pair " << k; + } + if (pair_slack > 0.0) { + ++stats->slack_charged_pairs; + stats->max_slack = std::max(stats->max_slack, pair_slack); + } + } + + for (int sample = 0; sample < 4; ++sample) { + VectorXd q(nq); + VectorXd qp(nq); + for (int c = 0; c < nq; ++c) { + q[c] = Uniform(rng, lower[c], upper[c]); + qp[c] = Uniform(rng, lower[c], upper[c]); + } + const VectorXd dq = (qp - q).cwiseAbs(); + + for (int k = 0; k < table.num_pairs(); ++k) { + const PairRecord& pair = pairs[k]; + const Matrix3Xd& pts_a = world.points_B.at(pair.a); + const Matrix3Xd& pts_b = world.points_B.at(pair.b); + const auto& frame_a = plant.get_body(pair.body_a).body_frame(); + const auto& frame_b = plant.get_body(pair.body_b).body_frame(); + const double bound = table.MotionBound(k, dq); + ASSERT_TRUE(std::isfinite(bound)); + ++stats->pairs; + + // ---- (1) Atomic, one coordinate at a time. ----------------------- + bool single_distal_side = true; + BodyIndex common_distal; + for (const auto& [c, lam] : table.GetEntries(k)) { + const std::vector& S = subtrees.at(owner[c]); + ASSERT_NE(S[pair.body_a], S[pair.body_b]); + const BodyIndex distal = S[pair.body_a] ? pair.body_a : pair.body_b; + const BodyIndex other = S[pair.body_a] ? pair.body_b : pair.body_a; + if (!common_distal.is_valid()) { + common_distal = distal; + } else if (common_distal != distal) { + single_distal_side = false; + } + const Matrix3Xd& pts = (distal == pair.body_a) ? pts_a : pts_b; + const auto& frame_d = (distal == pair.body_a) ? frame_a : frame_b; + const auto& frame_o = (distal == pair.body_a) ? frame_b : frame_a; + + VectorXd q_step = q; + q_step[c] = qp[c]; + const double displacement = + Displacement(plant, &ctx, pts, frame_d, frame_o, q, q_step); + ASSERT_LE(displacement, lam * dq[c] + kSlack) + << "atomic step: pair " << k << ", coordinate " << c << ", λ " + << lam << ", |Δq| " << dq[c] << ", distal body " + << plant.get_body(distal).name() << ", other body " + << plant.get_body(other).name(); + stats->Observe(displacement, lam * dq[c]); + ++stats->atomic_checks; + } + + // ---- (2) Aggregate: material-point distances. --------------------- + // This is the claim signed distance actually needs, and it covers the + // static pairs too: a pair with empty J(p) may still drift by its + // carve-out residual, which is exactly zero when every carved coordinate + // is exactly constant. It is stated on distances rather than on B's + // points in A's frame because the (carved or not) coordinates of a + // self-collision pair need not share a distal side; the stronger + // one-sided form is checked in (3), where they do. + // + // Subsample: 28 × 28 point pairs is plenty to catch an under-bound and + // keeps the whole corpus inside the time budget. + const int na = std::min(28, pts_a.cols()); + const int nb = std::min(28, pts_b.cols()); + const Matrix3Xd a_sub = pts_a.leftCols(na); + const Matrix3Xd b_sub = pts_b.leftCols(nb); + Matrix3Xd b_in_a_q(3, nb); + Matrix3Xd b_in_a_qp(3, nb); + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_b, b_sub, frame_a, &b_in_a_q); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_b, b_sub, frame_a, &b_in_a_qp); + for (int i = 0; i < na; ++i) { + for (int j = 0; j < nb; ++j) { + const double d_q = (a_sub.col(i) - b_in_a_q.col(j)).norm(); + const double d_qp = (a_sub.col(i) - b_in_a_qp.col(j)).norm(); + ASSERT_LE(std::abs(d_qp - d_q), bound + kSlack) + << "aggregate: pair " << k << ", points (" << i << ", " << j + << "), bound " << bound; + } + } + ++stats->aggregate_checks; + + // ---- (3) One-sided aggregate when J(p) has a single distal side. --- + if (single_distal_side && common_distal.is_valid()) { + const bool a_is_distal = (common_distal == pair.body_a); + const double displacement = + Displacement(plant, &ctx, a_is_distal ? pts_a : pts_b, + a_is_distal ? frame_a : frame_b, + a_is_distal ? frame_b : frame_a, q, qp); + ASSERT_LE(displacement, bound + kSlack) + << "one-sided aggregate: pair " << k << ", bound " << bound; + stats->Observe(displacement, bound); + ++stats->one_sided_checks; + } + } + } +} + +GTEST_TEST(DisplacementLemmaTest, RandomPlants) { + Rng rng(0xD15B0); + LemmaStats stats; + constexpr int kNumPlants = 1500; + for (int trial = 0; trial < kNumPlants; ++trial) { + SCOPED_TRACE(fmt::format("random plant #{}", trial)); + // Screw joints in every third world. The carve-out cycles through its + // three regimes so the exactly-constant and sub-tolerance cases each get + // ~500 plants. + const RandomWorld world = + MakeRandomWorld(&rng, /* allow_screw = */ trial % 3 == 0, 128); + stats.screw_joints += world.num_screw_joints; + const CarveOut carve_out = (trial % 3 == 1) ? CarveOut::kExact + : (trial % 3 == 2) ? CarveOut::kSubTolerance + : CarveOut::kNone; + CheckWorld(&rng, world, carve_out, &stats); + if (HasFatalFailure()) return; + } + // Guard against the corpus silently degenerating into nothing. + EXPECT_GE(stats.plants, 1000); + EXPECT_GE(stats.pairs, 20000); + EXPECT_GE(stats.atomic_checks, 20000); + EXPECT_GE(stats.aggregate_checks, 10000); + EXPECT_GE(stats.one_sided_checks, 2000); + // Screw joints must actually appear, or the screw λ rule is never exercised. + EXPECT_GT(stats.screw_joints, 0); + // The bound must be near-tight somewhere, or this test would pass against an + // arbitrarily wrong λ. + EXPECT_GT(stats.max_tightness, 0.9); + EXPECT_LE(stats.max_tightness, 1.0 + 1e-9); + // The sub-tolerance third of the corpus must actually be charging residuals, + // or the assertions above would test the exactly-constant case twice. + EXPECT_GE(stats.slack_charged_pairs, 200); + EXPECT_GT(stats.max_slack, 0.0); + GTEST_LOG_(INFO) << fmt::format( + "plants={} pairs={} atomic={} aggregate={} one_sided={} screw_joints={} " + "max_tightness={:.6f} slack_pairs={} max_slack={:.3e}", + stats.plants, stats.pairs, stats.atomic_checks, stats.aggregate_checks, + stats.one_sided_checks, stats.screw_joints, stats.max_tightness, + stats.slack_charged_pairs, stats.max_slack); +} + +/* A dedicated screw-joint world, so the screw λ = r + |pitch|/2π rule is + exercised densely rather than incidentally. */ +GTEST_TEST(DisplacementLemmaTest, ScrewChain) { + Rng rng(0x5C2E7); + LemmaStats stats; + for (int trial = 0; trial < 80; ++trial) { + SCOPED_TRACE(fmt::format("screw world #{}", trial)); + RandomWorld world; + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + std::vector*> bodies{&plant.world_body()}; + for (int i = 0; i < 3; ++i) { + const auto& body = plant.AddRigidBody(fmt::format("b{}", i), Inertia()); + plant.AddJoint(fmt::format("j{}", i), *bodies.back(), + test::RandomTransform(&rng, 0.25), body, + test::RandomTransform(&rng, 0.25), + test::RandomUnitVector(&rng), + Uniform(&rng, -0.8, 0.8), 0.0); + bodies.push_back(&body); + } + AddRandomGeometry(&rng, &plant, plant.world_body(), "g_world", 128, &world); + for (size_t i = 1; i < bodies.size(); ++i) { + AddRandomGeometry(&rng, &plant, *bodies[i], fmt::format("g{}", i), 128, + &world); + } + world.diagram = builder.Build(); + CheckWorld(&rng, world, CarveOut::kNone, &stats); + if (HasFatalFailure()) return; + } + EXPECT_GE(stats.plants, 75); + EXPECT_GT(stats.atomic_checks, 500); + EXPECT_GT(stats.max_tightness, 0.5); +} + +// --------------------------------------------------------------------------- +// Part 3. The constant-coordinate carve-out's residual. +// +// The curve module flags a coordinate constant when its whole control-point +// range fits inside kContinuityTolerance. That is a tolerance, not an +// identity: such a coordinate is removed from every J(p) but may still move by +// up to its range, displacing the pair's distal side by λ̃·range. Uncharged, +// that residual would let the certificate inequality pass with the true +// clearance ~1e-7 m below threshold, two orders of magnitude above +// kNumericalSlack. MotionBoundTable::carveout_slack() pays for it. +// --------------------------------------------------------------------------- + +GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { + /* world --j_rot(revolute, ẑ)--> l1 --j_slide(prismatic, x̂)--> l2, with a + sphere on the world and one offset out along l2. */ + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& l1 = plant.AddRigidBody("l1", Inertia()); + const auto& l2 = plant.AddRigidBody("l2", Inertia()); + plant.AddJoint("j_rot", plant.world_body(), {}, l1, + RigidTransformd(Vector3d(-0.2, 0, 0)), + Vector3d::UnitZ()); + plant.AddJoint("j_slide", l1, + RigidTransformd(Vector3d(0.15, 0, 0)), l2, {}, + Vector3d::UnitX()); + plant.RegisterCollisionGeometry(plant.world_body(), RigidTransformd(), + Sphere(0.1), "g_world", Friction()); + plant.RegisterCollisionGeometry(l2, RigidTransformd(Vector3d(0.3, 0, 0)), + Sphere(0.05), "g_tip", Friction()); + auto diagram = builder.Build(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + const int nq = diagram->plant().num_positions(); + ASSERT_EQ(nq, 2); + const int rot = diagram->plant().GetJointByName("j_rot").position_start(); + const int slide = diagram->plant().GetJointByName("j_slide").position_start(); + + // The slide's box is the same in every build below, so the reach, and with it + // λ(j_rot), is identical throughout; a revolute λ does not depend on the + // revolute's own box, which is the only thing that changes. + VectorXd lower = VectorXd::Zero(nq); + VectorXd upper = VectorXd::Zero(nq); + lower[slide] = 0.1; + upper[slide] = 0.4; + + // (a) Nothing carved: no residual at all, and λ(j_rot) is read off here. + lower[rot] = -0.5; + upper[rot] = 0.5; + const MotionBoundTable moving = + OnePairTable(engine, pairs, lower, upper, std::vector(nq, false)); + ASSERT_EQ(moving.GetEntries(0).size(), 2); + EXPECT_EQ(moving.carveout_slack(0), 0.0); + double lambda_rot = 0.0; + for (const auto& [c, lam] : moving.GetEntries(0)) { + if (c == rot) lambda_rot = lam; + } + ASSERT_GT(lambda_rot, 0.0); + + // (b) j_rot carved on the tolerance: it leaves J(p), and exactly λ·range + // takes its place in the slack. + constexpr double kRange = 5e-8; + static_assert(kRange <= kContinuityTolerance); + std::vector constant(nq, false); + constant[rot] = true; + lower[rot] = 0.0; + upper[rot] = kRange; // upper − lower is exactly kRange in binary FP. + const MotionBoundTable carved = + OnePairTable(engine, pairs, lower, upper, constant); + ASSERT_EQ(carved.GetEntries(0).size(), 1); + EXPECT_EQ(carved.GetEntries(0)[0].first, slide); + const double expected = lambda_rot * kRange; + EXPECT_NEAR(carved.carveout_slack(0), expected, 1e-15 * expected); + // MotionBound() charges it unconditionally, on top of the CSR row. + VectorXd w = VectorXd::Zero(nq); + w[slide] = 0.02; + EXPECT_DOUBLE_EQ(carved.MotionBound(0, w), carved.carveout_slack(0) + 0.02); + + // (c) Exactly constant: nothing to charge, bit for bit. + lower[rot] = 0.0; + upper[rot] = 0.0; + const MotionBoundTable exact = + OnePairTable(engine, pairs, lower, upper, constant); + EXPECT_EQ(exact.carveout_slack(0), 0.0); + EXPECT_EQ(exact.MotionBound(0, w), 0.02); + + // (d) A *moving* coordinate never contributes to the slack, however wide. + const MotionBoundTable wide = + OnePairTable(engine, pairs, VectorXd::Constant(nq, -2.0), + VectorXd::Constant(nq, 2.0), std::vector(nq, false)); + EXPECT_EQ(wide.carveout_slack(0), 0.0); +} + +/* world --(base joint)--> link, with a HalfSpace on `link`, the distal side, + and a sphere on the world. The base joint's kind is excluded, so the + construction-time half-space rule, which knows only the supported rotational + kinds, lets this model through; the carve-out is then the only thing between it + and an unbounded λ̃. `rpy` picks a 6-dof rpy floating base over a 3-dof ball + joint. */ +std::unique_ptr> MakeCarvedHalfSpaceModel(bool rpy) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& link = plant.AddRigidBody("link", Inertia()); + if (rpy) { + plant.AddJoint("base", plant.world_body(), {}, link, {}); + } else { + plant.AddJoint("base", plant.world_body(), + {}, link, {}); + } + plant.RegisterCollisionGeometry(link, RigidTransformd(), HalfSpace(), "hs", + Friction()); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransformd(Vector3d(0, 0, 1.0)), + Sphere(0.1), "ball", Friction()); + return builder.Build(); +} + +GTEST_TEST(CarveOutSlackTest, HalfSpaceNeedsExactlyConstantRotation) { + // The unsound case the residual exposes: a half space has no finite reach, so + // a rotational coordinate carrying it has no finite λ̃ and its residual cannot + // be charged at all. Such a coordinate must be EXACTLY constant. + auto ball = MakeCarvedHalfSpaceModel(/* rpy = */ false); + // Constructing the engine must not throw: a ball joint is not a *supported* + // rotational kind, so the construction-time rule never sees it. + const KinematicsEngine engine(*ball); + const std::vector pairs = CollisionPairs(*ball); + const int nq = ball->plant().num_positions(); + ASSERT_EQ(nq, 3); + + EXPECT_THAT( + ThrowMessage([&]() { + engine.ComputeMotionBoundTable(VectorXd::Zero(nq), + VectorXd::Constant(nq, 5e-8), + std::vector(nq, true), pairs); + }), + AllOf(HasSubstr("hs"), HasSubstr("base"), HasSubstr("EXACTLY constant"))); + + // Exactly constant is accepted, and owes nothing. + const VectorXd pinned = VectorXd::Constant(nq, 0.3); + const MotionBoundTable table = + OnePairTable(engine, pairs, pinned, pinned, std::vector(nq, true)); + EXPECT_TRUE(table.pair_is_static(0)); + EXPECT_EQ(table.carveout_slack(0), 0.0); +} + +GTEST_TEST(CarveOutSlackTest, + HalfSpaceAcrossToleranceConstantTranslationIsAccepted) { + // λ̃ = 1 for a translation coordinate is finite and correct even for a half + // space (every point of it moves by |Δq|), so only the *rotational* + // coordinates have to be exactly constant. + auto diagram = MakeCarvedHalfSpaceModel(/* rpy = */ true); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + const int nq = diagram->plant().num_positions(); + ASSERT_EQ(nq, 6); // q = (rpy, p_FM). + + constexpr double kRange = 4e-8; + VectorXd lower = VectorXd::Zero(nq); + VectorXd upper = VectorXd::Zero(nq); + for (int c = 3; c < 6; ++c) upper[c] = kRange; // Only the translation. + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, std::vector(nq, true)); + EXPECT_TRUE(table.pair_is_static(0)); + EXPECT_DOUBLE_EQ(table.carveout_slack(0), 3.0 * kRange); +} + +// --------------------------------------------------------------------------- +// Part 3b. The residual of a floating base held constant on the tolerance. +// +// This is where λ̃ is not simply the λ the CSR row would have carried: these +// joint kinds are excluded and have no λ at all, only a carve-out λ̃. Each test +// drives Drake's own forward kinematics from configurations sampled inside the +// box, the carved base coordinates included, and checks the FK displacement +// against carveout_slack() directly. +// --------------------------------------------------------------------------- + +/* world --base(rpy or quaternion floating)--> b1 --jr(revolute)--> b2, with a + sphere on the world and one offset out along b2. */ +std::unique_ptr> MakeFloatingBaseChain(Rng* rng, + bool quaternion) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& b1 = plant.AddRigidBody("b1", Inertia()); + const auto& b2 = plant.AddRigidBody("b2", Inertia()); + const RigidTransformd X_PF = test::RandomTransform(rng, 0.2); + const RigidTransformd X_CM = test::RandomTransform(rng, 0.2); + if (quaternion) { + plant.AddJoint("base", plant.world_body(), X_PF, + b1, X_CM); + } else { + plant.AddJoint("base", plant.world_body(), X_PF, b1, + X_CM); + } + plant.AddJoint("jr", b1, test::RandomTransform(rng, 0.2), b2, + test::RandomTransform(rng, 0.2), + test::RandomUnitVector(rng)); + plant.RegisterCollisionGeometry(plant.world_body(), RigidTransformd(), + Sphere(0.1), "g_world", Friction()); + plant.RegisterCollisionGeometry(b2, RigidTransformd(Vector3d(0.25, 0, 0)), + Sphere(0.06), "g_tip", Friction()); + return builder.Build(); +} + +/* Shared body of the two floating-base property tests. `quaternion` picks the + base parameterization; `seed` keeps the two corpora independent. */ +void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { + Rng rng(seed); + constexpr int kTrials = 250; + int atomic_cases = 0; + int aggregate_cases = 0; + double max_ratio = 0.0; + + for (int trial = 0; trial < kTrials; ++trial) { + SCOPED_TRACE(fmt::format("floating-base carve-out #{}", trial)); + auto diagram = MakeFloatingBaseChain(&rng, quaternion); + const MultibodyPlant& plant = diagram->plant(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + const int nq = plant.num_positions(); + const int bs = plant.GetJointByName("base").position_start(); + const int nb = plant.GetJointByName("base").num_positions(); + const int jr = plant.GetJointByName("jr").position_start(); + ASSERT_EQ(nb, quaternion ? 7 : 6); + + // The base pose the trajectory holds. For the quaternion case it is a + // random unit quaternion perturbed by at most the continuity tolerance, + // exactly the box the curve module would flag constant. Drake normalizes + // the quaternion internally, so a raw sample from that box and its + // renormalization produce identical forward kinematics; sampling raw keeps + // the sample inside the box the bound is stated over. + VectorXd q0(nq); + if (quaternion) { + const Eigen::Quaterniond qb = test::RandomRotation(&rng).ToQuaternion(); + q0.segment<4>(bs) << qb.w(), qb.x(), qb.y(), qb.z(); + for (int i = 0; i < 3; ++i) q0[bs + 4 + i] = Uniform(&rng, -0.5, 0.5); + } else { + for (int i = 0; i < 3; ++i) q0[bs + i] = Uniform(&rng, -M_PI, M_PI); + for (int i = 0; i < 3; ++i) q0[bs + 3 + i] = Uniform(&rng, -0.5, 0.5); + } + q0[jr] = Uniform(&rng, -1.0, 1.0); + + auto root = diagram->CreateDefaultContext(); + auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); + const Matrix3Xd points_B = + test::SampleSurface(&rng, 64, + [](Rng* g) { + return test::SampleSphere(g, 0.06); + }) + .colwise() + + Vector3d(0.25, 0, 0); + const auto& frame_tip = plant.GetBodyByName("b2").body_frame(); + const auto displacement = [&](const VectorXd& q, const VectorXd& qp) { + return Displacement(plant, &ctx, points_B, frame_tip, plant.world_frame(), + q, qp); + }; + + std::vector constant(nq, false); + for (int c = bs; c < bs + nb; ++c) constant[c] = true; + + // ---- (A) Atomic: exactly one carved base coordinate has a width. ----- + // Every other base coordinate is exactly constant, so the pair's whole + // slack is λ̃_c · range_c. The probe below drives that one coordinate from + // one end of its interval to the other, pinning that single coefficient + // rather than a seven-term sum, which is what makes the corpus sensitive to + // an under-bound in any one λ̃. + { + const int c = bs + UniformInt(&rng, 0, nb - 1); + const double width = + Uniform(&rng, 0.2 * kContinuityTolerance, kContinuityTolerance); + VectorXd lower = q0; + VectorXd upper = q0; + lower[c] = q0[c] - 0.5 * width; + upper[c] = q0[c] + 0.5 * width; + lower[jr] = q0[jr] - 0.8; + upper[jr] = q0[jr] + 0.8; + + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, constant); + ASSERT_EQ(table.GetEntries(0).size(), 1); // Only the revolute survives. + const double slack = table.carveout_slack(0); + ASSERT_GT(slack, 0.0); + ASSERT_LT(slack, 1e-4) << "a metre-scale reach against a 1e-7 box cannot " + "produce a residual this large"; + ++atomic_cases; + + VectorXd q = q0; + q[jr] = Uniform(&rng, lower[jr], upper[jr]); + VectorXd qp = q; + q[c] = lower[c]; + qp[c] = upper[c]; + const double atomic = displacement(q, qp); + ASSERT_LE(atomic, slack + kSlack) + << "atomic carve-out residual: coordinate " << c << " moved by " + << (upper[c] - lower[c]) << ", displacement " << atomic << " > slack " + << slack; + max_ratio = std::max(max_ratio, atomic / slack); + } + + // ---- (B) Aggregate: every base coordinate carved, everything moving. -- + { + VectorXd lower = q0; + VectorXd upper = q0; + for (int c = bs; c < bs + nb; ++c) { + const double half = 0.5 * Uniform(&rng, 0.2 * kContinuityTolerance, + kContinuityTolerance); + lower[c] = q0[c] - half; + upper[c] = q0[c] + half; + } + lower[jr] = q0[jr] - 0.8; + upper[jr] = q0[jr] + 0.8; + + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, constant); + const double slack = table.carveout_slack(0); + ASSERT_GT(slack, 0.0); + ++aggregate_cases; + + VectorXd q(nq); + VectorXd qp(nq); + for (int c = 0; c < nq; ++c) { + q[c] = Uniform(&rng, lower[c], upper[c]); + qp[c] = Uniform(&rng, lower[c], upper[c]); + } + // Σ_{uncarved} λ|Δq| + carveout_slack, with q and q′ drawn from the whole + // box, the carved coordinates' tiny ranges included. + const double full = displacement(q, qp); + const double bound = table.MotionBound(0, (qp - q).cwiseAbs()); + ASSERT_LE(full, bound + kSlack) + << "full bound: displacement " << full << " > bound " << bound; + + // ... and again with the revolute pinned, so the slack alone carries it. + qp[jr] = q[jr]; + const double base_only = displacement(q, qp); + ASSERT_LE(base_only, slack + kSlack) + << "carved base residual: displacement " << base_only << " > slack " + << slack; + } + } + + EXPECT_GE(atomic_cases, 200); + EXPECT_GE(aggregate_cases, 200); + // λ̃ must be near-tight somewhere, or these assertions would hold against an + // arbitrarily inflated coefficient. (The dedicated tight model below pins + // every coefficient individually; here the chain walk's own triangle + // inequalities are slack at random poses, so only the translation rule + // reaches 1.) The tolerance on the upper check is relative to a bound of + // ~1e-7 m, where Drake's forward kinematics rounds at ~1e-15 m absolute. + EXPECT_GT(max_ratio, 0.9); + EXPECT_LE(max_ratio, 1.0 + 1e-6); +} + +GTEST_TEST(CarveOutSlackTest, ToleranceConstantRpyFloatingBase) { + RunFloatingBaseCarveOutCorpus(/* quaternion = */ false, 0x12F0BA5Eull); +} + +GTEST_TEST(CarveOutSlackTest, ToleranceConstantQuaternionFloatingBase) { + RunFloatingBaseCarveOutCorpus(/* quaternion = */ true, 0x9A7E48A5Eull); +} + +// --------------------------------------------------------------------------- +// Part 3c. An exactly tight floating-base λ̃. +// +// The random corpus above catches structural errors, but the chain walk's +// triangle inequalities are slack at random poses, so a λ̃ that is merely too +// small can hide there for the rotation rules. This model removes the slack the +// way MakeTightChain() does for the supported kinds: both joint frames are +// identity, so the joint's M-frame origin is the link's body origin, and the +// link's single sphere is centred on it. The reach is then exactly R in every +// direction, so whatever axis a carved rotation coordinate turns the link +// about, a material point sits at the full reach perpendicular to that axis and +// the chord 2R·sin(θ/2) recovers R·θ to fifteen digits at θ ~ 1e-7. +// --------------------------------------------------------------------------- + +void RunTightFloatingBaseLambda(bool quaternion) { + constexpr double kRadius = 0.4; + constexpr double kWidth = 8e-8; // ≤ kContinuityTolerance. + Rng rng(quaternion ? 0x7168A7ull : 0x51DE12ull); + + RobotDiagramBuilder builder; + MultibodyPlant& plant_in = builder.plant(); + const auto& link = plant_in.AddRigidBody("link", Inertia()); + if (quaternion) { + plant_in.AddJoint("base", plant_in.world_body(), + {}, link, {}); + } else { + plant_in.AddJoint("base", plant_in.world_body(), {}, link, + {}); + } + plant_in.RegisterCollisionGeometry(link, RigidTransformd(), Sphere(kRadius), + "g_link", Friction()); + plant_in.RegisterCollisionGeometry(plant_in.world_body(), + RigidTransformd(Vector3d(0, 0, 3.0)), + Sphere(0.05), "g_world", Friction()); + auto diagram = builder.Build(); + + const MultibodyPlant& plant = diagram->plant(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + const int nq = plant.num_positions(); + const auto& base = plant.GetJointByName("base"); + const int bs = base.position_start(); + const int nb = base.num_positions(); + ASSERT_EQ(nb, quaternion ? 7 : 6); + ASSERT_EQ(nq, nb); + + // Dense enough that some sample lands within ~1e-6 of the equator of any + // rotation axis, which is what makes the chord recover R·θ. + const Matrix3Xd points_B = test::SampleSurface(&rng, 4096, [](Rng* g) { + return test::SampleSphere(g, kRadius); + }); + auto root = diagram->CreateDefaultContext(); + auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); + const auto& frame_link = plant.GetBodyByName("link").body_frame(); + const std::vector constant(nq, true); + + for (int off = 0; off < nb; ++off) { + SCOPED_TRACE(fmt::format("base coordinate offset {}", off)); + VectorXd q0 = VectorXd::Zero(nq); + if (quaternion) { + // A unit quaternion with a *zero* in the coordinate being perturbed, so + // the perturbation is entirely orthogonal to it: normalization then + // absorbs none of it and the induced rotation is the full 2‖Δq‖ that + // λ̃ = 2r/m charges for. (A perturbation parallel to q induces no rotation + // at all, which is why the bound has to be stated for the worst case and + // cannot be tight in every direction at once.) + Eigen::Vector4d qb(0.31, 0.53, -0.62, 0.49); + if (off < 4) qb[off] = 0.0; + qb.normalize(); + q0.head<4>() = qb; + for (int i = 0; i < 3; ++i) q0[4 + i] = Uniform(&rng, -0.5, 0.5); + } else { + // rpy = 0: each angle then turns the link about a coordinate axis + // through Mo, and the sphere is centred there. + for (int i = 0; i < 3; ++i) q0[3 + i] = Uniform(&rng, -0.5, 0.5); + } + + VectorXd lower = q0; + VectorXd upper = q0; + lower[bs + off] = q0[bs + off] - 0.5 * kWidth; + upper[bs + off] = q0[bs + off] + 0.5 * kWidth; + const MotionBoundTable table = + OnePairTable(engine, pairs, lower, upper, constant); + ASSERT_TRUE(table.pair_is_static(0)); + const double slack = table.carveout_slack(0); + ASSERT_GT(slack, 0.0); + + const double displacement = Displacement(plant, &ctx, points_B, frame_link, + plant.world_frame(), lower, upper); + ASSERT_LE(displacement, slack + kSlack) + << "displacement " << displacement << " > slack " << slack; + EXPECT_GT(displacement / slack, 0.999) + << "the residual must be exactly attained on this model; a slack ratio " + "here would mean λ̃ is over-counted, and a violated bound would mean " + "it is under-counted. displacement " + << displacement << ", slack " << slack; + } +} + +GTEST_TEST(CarveOutSlackTest, RpyFloatingLambdaTildeIsExactAndTight) { + RunTightFloatingBaseLambda(/* quaternion = */ false); +} + +GTEST_TEST(CarveOutSlackTest, QuaternionFloatingLambdaTildeIsExactAndTight) { + RunTightFloatingBaseLambda(/* quaternion = */ true); +} + +} // namespace +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/piecewise_bezier_path_test.cc b/planning/continuous_collision/test/piecewise_bezier_path_test.cc new file mode 100644 index 000000000000..e34f538fedb6 --- /dev/null +++ b/planning/continuous_collision/test/piecewise_bezier_path_test.cc @@ -0,0 +1,972 @@ +/* Acceptance tests for the curve module. + +Every property test uses a fixed seed so the suite is reproducible and never +flaky. Reference values come from Drake's own trajectory classes, so these +tests check our conversions against an independent implementation rather than +against themselves. */ + +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/copyable_unique_ptr.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/common/test_utilities/limit_malloc.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/common/trajectories/bspline_trajectory.h" +#include "drake/common/trajectories/composite_trajectory.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/math/bspline_basis.h" +#include "drake/math/knot_vector_type.h" +#include "drake/planning/continuous_collision/internal.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::copyable_unique_ptr; +using drake::math::BsplineBasis; +using drake::math::KnotVectorType; +using drake::trajectories::BezierCurve; +using drake::trajectories::BsplineTrajectory; +using drake::trajectories::CompositeTrajectory; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; + +constexpr double kTwoPi = 6.2831853071795864769252867665590; + +/* A minimal Trajectory subclass that the curve module does not +support; used to exercise the unknown-segment-type error path. */ +class UnsupportedTrajectory final : public Trajectory { + public: + UnsupportedTrajectory(int rows, double t_start, double t_end) + : rows_(rows), t_start_(t_start), t_end_(t_end) {} + + private: + std::unique_ptr> DoClone() const final { + return std::make_unique(rows_, t_start_, t_end_); + } + Eigen::MatrixXd do_value(const double&) const final { + return Eigen::MatrixXd::Zero(rows_, 1); + } + Eigen::Index do_rows() const final { return rows_; } + Eigen::Index do_cols() const final { return 1; } + double do_start_time() const final { return t_start_; } + double do_end_time() const final { return t_end_; } + + int rows_{}; + double t_start_{}; + double t_end_{}; +}; + +Eigen::MatrixXd RandomMatrix(int rows, int cols, std::mt19937_64* generator) { + std::uniform_real_distribution distribution(-1.0, 1.0); + Eigen::MatrixXd result(rows, cols); + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + result(i, j) = distribution(*generator); + } + } + return result; +} + +/* Independent Bézier evaluation via Drake, for use as ground truth. */ +Eigen::VectorXd DrakeBezierValue(const Eigen::MatrixXd& control_points, + double s) { + return BezierCurve(0.0, 1.0, control_points).value(s); +} + +/* Builds a Bézier curve over [t_start, t_end] whose first control point is +`start` and whose remaining control points are random. */ +BezierCurve MakeBezierCurve(const Eigen::VectorXd& start, int order, + double t_start, double t_end, + std::mt19937_64* generator) { + Eigen::MatrixXd control_points = + RandomMatrix(static_cast(start.size()), order + 1, generator); + control_points.col(0) = start; + return BezierCurve(t_start, t_end, control_points); +} + +/* Wraps a vector of trajectories into a CompositeTrajectory. */ +CompositeTrajectory MakeComposite( + std::vector>> pieces) { + std::vector>> segments; + segments.reserve(pieces.size()); + for (auto& piece : pieces) { + segments.emplace_back(std::move(piece)); + } + return CompositeTrajectory(std::move(segments)); +} + +/* Maximum absolute deviation between the path and `trajectory` over +`num_samples` uniformly spaced times covering the whole domain. */ +double MaxSampledError(const PiecewiseBezierPath& path, + const Trajectory& trajectory, int num_samples) { + const double t0 = trajectory.start_time(); + const double tf = trajectory.end_time(); + double worst = 0.0; + for (int i = 0; i < num_samples; ++i) { + const double t = t0 + (tf - t0) * i / (num_samples - 1.0); + const Eigen::VectorXd expected = trajectory.value(t); + const Eigen::VectorXd actual = path.Value(t); + worst = std::max(worst, (expected - actual).cwiseAbs().maxCoeff()); + } + return worst; +} + +// -------------------------------------------------------------------------- +// Bézier evaluation and de Casteljau subdivision. +// -------------------------------------------------------------------------- + +/* Our de Casteljau evaluation must agree with BezierCurve::value to 1e-12 over +dense samples, for orders 1 through 5. */ +GTEST_TEST(BezierEvaluation, MatchesDrakeBezierCurve) { + std::mt19937_64 generator(1234); + for (int order = 1; order <= 5; ++order) { + for (int trial = 0; trial < 5; ++trial) { + const int num_positions = 1 + (trial % 6); + const Eigen::MatrixXd control_points = + RandomMatrix(num_positions, order + 1, &generator); + const double t_start = -0.75 + 0.4 * trial; + const double t_end = t_start + 1.0 + 0.3 * trial; + const BezierCurve curve(t_start, t_end, control_points); + + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(curve, {}); + ASSERT_EQ(path.num_positions(), num_positions); + ASSERT_EQ(path.segments().size(), 1u); + EXPECT_EQ(path.start_time(), t_start); + EXPECT_EQ(path.end_time(), t_end); + EXPECT_TRUE( + path.segments()[0].control_points.isApprox(control_points, 0.0)); + + constexpr int kNumSamples = 1001; + for (int i = 0; i < kNumSamples; ++i) { + const double s = static_cast(i) / (kNumSamples - 1); + const double t = t_start + s * (t_end - t_start); + const Eigen::VectorXd expected = curve.value(t); + EXPECT_LT((path.Value(t) - expected).cwiseAbs().maxCoeff(), 1e-12) + << "order " << order << " trial " << trial << " t " << t; + EXPECT_LT((path.EvaluateSegment(0, s) - expected).cwiseAbs().maxCoeff(), + 1e-12) + << "order " << order << " trial " << trial << " s " << s; + } + } + } +} + +/* Property test: for >= 1000 random curves the two children produced by +splitting at 1/2 reproduce the parent exactly (to 1e-12) on their halves, and +the apex is the parent's midpoint value. */ +GTEST_TEST(DeCasteljau, ChildrenReproduceParent) { + std::mt19937_64 generator(20260826); + std::uniform_int_distribution rows_distribution(1, 7); + std::uniform_int_distribution order_distribution(0, 6); + constexpr int kNumCases = 1000; + constexpr int kNumSamples = 21; + + Eigen::MatrixXd left; + Eigen::MatrixXd right; + Eigen::VectorXd mid; + double worst = 0.0; + for (int trial = 0; trial < kNumCases; ++trial) { + const int num_positions = rows_distribution(generator); + const int order = order_distribution(generator); + const Eigen::MatrixXd parent = + RandomMatrix(num_positions, order + 1, &generator); + + DeCasteljauSplitAtHalf(parent, &left, &right, &mid); + ASSERT_EQ(left.rows(), num_positions); + ASSERT_EQ(left.cols(), order + 1); + ASSERT_EQ(right.rows(), num_positions); + ASSERT_EQ(right.cols(), order + 1); + ASSERT_EQ(mid.size(), num_positions); + + // The apex is exactly q(1/2), and the children share the endpoints they + // must. + worst = std::max( + worst, (mid - DrakeBezierValue(parent, 0.5)).cwiseAbs().maxCoeff()); + worst = + std::max(worst, (left.col(0) - parent.col(0)).cwiseAbs().maxCoeff()); + worst = std::max( + worst, (right.col(order) - parent.col(order)).cwiseAbs().maxCoeff()); + worst = std::max(worst, (left.col(order) - mid).cwiseAbs().maxCoeff()); + worst = std::max(worst, (right.col(0) - mid).cwiseAbs().maxCoeff()); + + for (int i = 0; i < kNumSamples; ++i) { + const double s = static_cast(i) / (kNumSamples - 1); + const Eigen::VectorXd expected = DrakeBezierValue(parent, s); + const Eigen::VectorXd child_value = + (s <= 0.5) ? DrakeBezierValue(left, 2.0 * s) + : DrakeBezierValue(right, 2.0 * s - 1.0); + worst = std::max(worst, (child_value - expected).cwiseAbs().maxCoeff()); + } + } + EXPECT_LT(worst, 1e-12); +} + +/* The hot loop pre-sizes its outputs; re-splitting into already-correctly +sized buffers must not allocate at all, so the steady-state recursion the +certifier runs is allocation-free. */ +GTEST_TEST(DeCasteljau, PreSizedOutputsDoNotAllocate) { + std::mt19937_64 generator(7); + const Eigen::MatrixXd parent = RandomMatrix(6, 4, &generator); + Eigen::MatrixXd left(6, 4); + Eigen::MatrixXd right(6, 4); + Eigen::VectorXd mid(6); + DeCasteljauSplitAtHalf(parent, &left, &right, &mid); + // Splitting a child in place into the same buffers is the recursion the + // certifier runs; it must be allocation-free too. + const Eigen::MatrixXd child = left; + { + drake::test::LimitMalloc guard; + DeCasteljauSplitAtHalf(parent, &left, &right, &mid); + DeCasteljauSplitAtHalf(child, &left, &right, &mid); + } +} + +/* Property test: after a random sequence of subdivisions, the node's +control-point box contains every sample of the sub-curve it represents, and is +contained in its parent's box (the two Bézier facts the normalization relies +on). */ +GTEST_TEST(DeCasteljau, ControlBoxesContainCurveAfterRandomSubdivision) { + std::mt19937_64 generator(99991); + std::uniform_int_distribution rows_distribution(1, 5); + std::uniform_int_distribution order_distribution(1, 6); + std::uniform_int_distribution depth_distribution(1, 6); + std::uniform_int_distribution coin(0, 1); + constexpr int kNumCases = 1000; + constexpr int kNumSamples = 41; + constexpr double kTolerance = 1e-12; + + Eigen::MatrixXd left; + Eigen::MatrixXd right; + Eigen::VectorXd mid; + for (int trial = 0; trial < kNumCases; ++trial) { + const int num_positions = rows_distribution(generator); + const int order = order_distribution(generator); + const Eigen::MatrixXd root = + RandomMatrix(num_positions, order + 1, &generator); + const Eigen::VectorXd root_lower = root.rowwise().minCoeff(); + const Eigen::VectorXd root_upper = root.rowwise().maxCoeff(); + + Eigen::MatrixXd node = root; + double a = 0.0; + double b = 1.0; + const int depth = depth_distribution(generator); + for (int level = 0; level < depth; ++level) { + const Eigen::MatrixXd parent = node; + const Eigen::VectorXd parent_lower = parent.rowwise().minCoeff(); + const Eigen::VectorXd parent_upper = parent.rowwise().maxCoeff(); + DeCasteljauSplitAtHalf(parent, &left, &right, &mid); + const double midpoint = 0.5 * (a + b); + if (coin(generator) == 0) { + node = left; + b = midpoint; + } else { + node = right; + a = midpoint; + } + // Children are convex combinations of the parent's control points, so + // each child's box is inside the parent's. + const Eigen::VectorXd node_lower = node.rowwise().minCoeff(); + const Eigen::VectorXd node_upper = node.rowwise().maxCoeff(); + ASSERT_TRUE( + ((node_lower.array() >= parent_lower.array() - kTolerance).all())) + << "trial " << trial; + ASSERT_TRUE( + ((node_upper.array() <= parent_upper.array() + kTolerance).all())) + << "trial " << trial; + } + + const Eigen::VectorXd node_lower = node.rowwise().minCoeff(); + const Eigen::VectorXd node_upper = node.rowwise().maxCoeff(); + ASSERT_TRUE( + ((node_lower.array() >= root_lower.array() - kTolerance).all())); + ASSERT_TRUE( + ((node_upper.array() <= root_upper.array() + kTolerance).all())); + + for (int i = 0; i < kNumSamples; ++i) { + const double local = static_cast(i) / (kNumSamples - 1); + const double global = a + local * (b - a); + const Eigen::VectorXd from_node = DrakeBezierValue(node, local); + const Eigen::VectorXd from_root = DrakeBezierValue(root, global); + // The child exactly represents the sub-curve ... + ASSERT_LT((from_node - from_root).cwiseAbs().maxCoeff(), 1e-12) + << "trial " << trial << " local " << local; + // ... and the sub-curve lives in the child's control box. + ASSERT_TRUE( + ((from_node.array() >= node_lower.array() - kTolerance).all())) + << "trial " << trial; + ASSERT_TRUE( + ((from_node.array() <= node_upper.array() + kTolerance).all())) + << "trial " << trial; + } + } +} + +// -------------------------------------------------------------------------- +// B-spline → Bézier. +// -------------------------------------------------------------------------- + +BsplineTrajectory MakeBsplineFromBasis( + const BsplineBasis& basis, int num_positions, + std::mt19937_64* generator) { + std::vector control_points; + control_points.reserve(basis.num_basis_functions()); + for (int i = 0; i < basis.num_basis_functions(); ++i) { + control_points.push_back(RandomMatrix(num_positions, 1, generator)); + } + return BsplineTrajectory(basis, std::move(control_points)); +} + +/* Shared checker: the conversion must reproduce the B-spline to 1e-10 over +>= 1e4 dense samples, the segments must tile the domain, and there must be +`expected_segments` of them (or any number, when that is 0). */ +void CheckBsplineEquivalence(const BsplineTrajectory& bspline, + int expected_segments = 0) { + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(bspline, {}); + EXPECT_EQ(path.num_positions(), bspline.rows()); + EXPECT_NEAR(path.start_time(), bspline.start_time(), 1e-14); + EXPECT_NEAR(path.end_time(), bspline.end_time(), 1e-14); + for (const BezierSegment& segment : path.segments()) { + // Full interior multiplicity => every segment has exactly `order` control + // points, i.e. the degree of the source spline. + EXPECT_EQ(segment.control_points.cols(), bspline.basis().order()); + } + if (expected_segments > 0) { + EXPECT_EQ(static_cast(path.segments().size()), expected_segments); + } + EXPECT_LT(MaxSampledError(path, bspline, 10001), 1e-10); +} + +GTEST_TEST(BsplineConversion, ClampedUniformOrders2To6) { + std::mt19937_64 generator(4242); + for (int order = 2; order <= 6; ++order) { + SCOPED_TRACE("order " + std::to_string(order)); + const int num_basis_functions = order + 4; + // A clamped uniform basis has num_basis_functions - order + 1 spans. + CheckBsplineEquivalence( + MakeBsplineFromBasis( + BsplineBasis(order, num_basis_functions, + KnotVectorType::kClampedUniform, 0.0, 3.0), + 3, &generator), + num_basis_functions - order + 1); + } +} + +GTEST_TEST(BsplineConversion, NonUniformKnots) { + std::mt19937_64 generator(515151); + for (int order = 2; order <= 6; ++order) { + SCOPED_TRACE("order " + std::to_string(order)); + // Clamped, but with irregular interior spacing. + std::vector knots(order, 0.0); + for (double interior : {0.13, 0.29, 0.31, 1.70, 2.55}) { + knots.push_back(interior); + } + knots.insert(knots.end(), order, 3.0); + CheckBsplineEquivalence( + MakeBsplineFromBasis(BsplineBasis(order, knots), 4, &generator), + 6); + } +} + +GTEST_TEST(BsplineConversion, RepeatedInteriorKnots) { + std::mt19937_64 generator(606060); + // Order 4 (cubic): interior knot 1.0 with multiplicity 2 (C1 there) and + // interior knot 2.0 with multiplicity 3 (C0 there, the extreme case that + // still passes junction validation). Nonempty spans: [0,1], [1,2], [2,3], + // [3,3.5], [3.5,4]. + const std::vector knots{0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 2.0, 2.0, + 2.0, 3.0, 3.5, 4.0, 4.0, 4.0, 4.0}; + CheckBsplineEquivalence( + MakeBsplineFromBasis(BsplineBasis(4, knots), 2, &generator), 5); +} + +/* The representation KinematicTrajectoryOptimization emits: clamped uniform, +order 4, one control point per decision-variable column. */ +GTEST_TEST(BsplineConversion, KinematicTrajectoryOptimizationStyle) { + std::mt19937_64 generator(777); + CheckBsplineEquivalence( + MakeBsplineFromBasis( + BsplineBasis(4, 10, KnotVectorType::kClampedUniform, 0.0, + 5.0), + 7, &generator), + 7); +} + +/* General (unclamped) knot vectors are supported too: the domain endpoints are +raised to full multiplicity by the same insertion pass. */ +GTEST_TEST(BsplineConversion, UnclampedUniformKnots) { + std::mt19937_64 generator(31337); + for (int order = 2; order <= 5; ++order) { + SCOPED_TRACE("order " + std::to_string(order)); + CheckBsplineEquivalence(MakeBsplineFromBasis( + BsplineBasis(order, order + 5, KnotVectorType::kUniform, 0.0, + 2.0), + 3, &generator)); + } +} + +GTEST_TEST(BsplineConversion, SegmentTimesMatchKnotSpans) { + std::mt19937_64 generator(24680); + const std::vector knots{0.0, 0.0, 0.0, 0.5, 1.25, 2.0, 2.0, 2.0}; + const PiecewiseBezierPath path = PiecewiseBezierPath::FromTrajectory( + MakeBsplineFromBasis(BsplineBasis(3, knots), 2, &generator), {}); + ASSERT_EQ(path.segments().size(), 3u); + const std::vector expected{0.0, 0.5, 1.25, 2.0}; + for (int i = 0; i < 3; ++i) { + EXPECT_EQ(path.segments()[i].t_start, expected[i]); + EXPECT_EQ(path.segments()[i].t_end, expected[i + 1]); + } +} + +GTEST_TEST(BsplineConversion, MatrixValuedThrows) { + std::vector control_points(6, Eigen::MatrixXd::Zero(2, 2)); + const BsplineTrajectory bspline( + BsplineBasis(3, 6, KnotVectorType::kClampedUniform, 0.0, 1.0), + control_points); + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(bspline, {}), + "[\\s\\S]*column-vector-valued[\\s\\S]*"); +} + +// -------------------------------------------------------------------------- +// PiecewisePolynomial → Bernstein. +// -------------------------------------------------------------------------- + +GTEST_TEST(PiecewisePolynomialConversion, FirstOrderHold) { + std::mt19937_64 generator(11235); + const Eigen::VectorXd times = Eigen::VectorXd::LinSpaced(6, 0.0, 2.5); + const Eigen::MatrixXd samples = RandomMatrix(4, 6, &generator); + const PiecewisePolynomial pp = + PiecewisePolynomial::FirstOrderHold(times, samples); + + const PiecewiseBezierPath path = PiecewiseBezierPath::FromTrajectory(pp, {}); + ASSERT_EQ(path.segments().size(), 5u); + for (int k = 0; k < 5; ++k) { + // A first-order hold is exactly an order-1 Bézier per segment, whose + // control points are the waypoints themselves. + ASSERT_EQ(path.segments()[k].control_points.cols(), 2); + EXPECT_LT((path.segments()[k].control_points.col(0) - samples.col(k)) + .cwiseAbs() + .maxCoeff(), + 1e-14); + EXPECT_LT((path.segments()[k].control_points.col(1) - samples.col(k + 1)) + .cwiseAbs() + .maxCoeff(), + 1e-14); + } + EXPECT_LT(MaxSampledError(path, pp, 10001), 1e-10); +} + +GTEST_TEST(PiecewisePolynomialConversion, CubicSplines) { + std::mt19937_64 generator(626262); + const Eigen::VectorXd times = Eigen::VectorXd::LinSpaced(7, -1.0, 3.0); + const Eigen::MatrixXd samples = RandomMatrix(3, 7, &generator); + + const PiecewisePolynomial continuous_second = + PiecewisePolynomial::CubicWithContinuousSecondDerivatives( + times, samples); + const PiecewiseBezierPath path_a = + PiecewiseBezierPath::FromTrajectory(continuous_second, {}); + EXPECT_EQ(path_a.segments().size(), 6u); + for (const BezierSegment& segment : path_a.segments()) { + EXPECT_EQ(segment.control_points.cols(), 4); + } + EXPECT_LT(MaxSampledError(path_a, continuous_second, 10001), 1e-10); + + const PiecewisePolynomial shape_preserving = + PiecewisePolynomial::CubicShapePreserving(times, samples); + const PiecewiseBezierPath path_b = + PiecewiseBezierPath::FromTrajectory(shape_preserving, {}); + EXPECT_LT(MaxSampledError(path_b, shape_preserving, 10001), 1e-10); +} + +/* A single high-degree polynomial segment, from degree 1 up to the cap and one +past it. */ +GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCapAndBeyond) { + ASSERT_EQ(kMaxConversionDegree, 10); + for (int degree = 1; degree <= kMaxConversionDegree + 1; ++degree) { + SCOPED_TRACE("degree " + std::to_string(degree)); + const int num_points = degree + 1; + Eigen::VectorXd times(num_points); + Eigen::MatrixXd samples(2, num_points); + for (int i = 0; i < num_points; ++i) { + // A non-unit segment duration: the monomial coefficients must be + // rescaled by (t_end - t_start)^a before the change of basis. + times[i] = 0.3 + 1.7 * static_cast(i) / degree; + samples(0, i) = std::sin(3.0 * times[i]); + samples(1, i) = std::cos(2.0 * times[i]) - 0.25 * times[i]; + } + const PiecewisePolynomial pp = + PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, + samples); + if (degree > kMaxConversionDegree) { + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(pp, {}), + "[\\s\\S]*polynomial degree 11[\\s\\S]*"); + continue; + } + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(pp, {}); + ASSERT_EQ(path.segments().size(), 1u); + EXPECT_EQ(path.segments()[0].control_points.cols(), degree + 1); + EXPECT_LT(MaxSampledError(path, pp, 10001), 1e-10); + } +} + +GTEST_TEST(PiecewisePolynomialConversion, MatrixValuedThrows) { + std::vector samples; + samples.push_back(Eigen::MatrixXd::Zero(2, 2)); + samples.push_back(Eigen::MatrixXd::Ones(2, 2)); + const std::vector times{0.0, 1.0}; + const PiecewisePolynomial pp = + PiecewisePolynomial::FirstOrderHold(times, samples); + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(pp, {}), + "[\\s\\S]*column-vector-valued[\\s\\S]*"); +} + +// -------------------------------------------------------------------------- +// Junction (C0) validation. +// -------------------------------------------------------------------------- + +/* Builds a two-segment composite whose second segment starts at the first +segment's endpoint plus `offset`. */ +CompositeTrajectory MakeJunctionCase(const Eigen::VectorXd& offset, + std::mt19937_64* generator) { + const int num_positions = static_cast(offset.size()); + Eigen::MatrixXd first = RandomMatrix(num_positions, 4, generator); + Eigen::MatrixXd second = RandomMatrix(num_positions, 3, generator); + second.col(0) = first.col(3) + offset; + std::vector>> pieces; + pieces.push_back(std::make_unique>(0.0, 1.0, first)); + pieces.push_back(std::make_unique>(1.0, 2.5, second)); + return MakeComposite(std::move(pieces)); +} + +GTEST_TEST(JunctionValidation, InjectedDiscontinuityThrows) { + std::mt19937_64 generator(90210); + Eigen::VectorXd offset = Eigen::VectorXd::Zero(3); + offset[1] = 1e-3; + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(MakeJunctionCase(offset, &generator), + {}), + "[\\s\\S]*C0 discontinuity[\\s\\S]*coordinate 1[\\s\\S]*"); + + // A gap just under the tolerance is accepted. + Eigen::VectorXd tiny = Eigen::VectorXd::Zero(3); + tiny[2] = 9e-8; + EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory( + MakeJunctionCase(tiny, &generator), {})); +} + +GTEST_TEST(JunctionValidation, TwoPiOffsetAcceptedOnlyWhenDeclaredRevolute) { + std::mt19937_64 generator(1357); + Eigen::VectorXd offset = Eigen::VectorXd::Zero(3); + offset[1] = kTwoPi; + const CompositeTrajectory trajectory = + MakeJunctionCase(offset, &generator); + + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(trajectory, {}), + "[\\s\\S]*C0 discontinuity[\\s\\S]*"); + + // Declaring the *wrong* coordinate does not help. + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(trajectory, {0, 2}), + "[\\s\\S]*C0 discontinuity[\\s\\S]*"); + + const std::vector right{1}; + EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory(trajectory, right)); + + // Any integer multiple of 2π is fine ... + Eigen::VectorXd big_offset = Eigen::VectorXd::Zero(3); + big_offset[1] = -3.0 * kTwoPi; + EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory( + MakeJunctionCase(big_offset, &generator), right)); + + // ... but an offset that is not one is still a discontinuity. + Eigen::VectorXd off_by = Eigen::VectorXd::Zero(3); + off_by[1] = kTwoPi + 1e-3; + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory( + MakeJunctionCase(off_by, &generator), right), + "[\\s\\S]*C0 discontinuity[\\s\\S]*"); +} + +/* Forward kinematics is 2π-periodic, so a legitimate 2πk junction offset must +be left exactly as it is: the segments are NOT re-aligned. */ +GTEST_TEST(JunctionValidation, + ControlPointsAreNotRealignedAcrossTwoPiJunction) { + std::mt19937_64 generator(864213); + Eigen::VectorXd offset = Eigen::VectorXd::Zero(2); + offset[0] = kTwoPi; + const CompositeTrajectory trajectory = + MakeJunctionCase(offset, &generator); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, {0}); + ASSERT_EQ(path.segments().size(), 2u); + + const Eigen::MatrixXd& first = path.segments()[0].control_points; + const Eigen::MatrixXd& second = path.segments()[1].control_points; + EXPECT_NEAR(second(0, 0) - first(0, first.cols() - 1), kTwoPi, 1e-14); + // The path reproduces the source trajectory verbatim on both sides. + EXPECT_LT(MaxSampledError(path, trajectory, 501), 1e-12); + // The two sides of the junction are distinct representatives of the same + // configuration; Value() reports the later segment's, matching + // PiecewiseTrajectory::get_segment_index(). + EXPECT_NEAR(path.Value(1.0)[0], second(0, 0), 1e-14); + EXPECT_NEAR(path.EvaluateSegment(0, 1.0)[0], first(0, first.cols() - 1), + 1e-14); + // The global control box therefore spans the 2π jump, as intended. + EXPECT_GT(path.global_upper_bound()[0] - path.global_lower_bound()[0], 5.0); +} + +GTEST_TEST(JunctionValidation, OutOfRangeRevoluteIndexThrows) { + Eigen::MatrixXd control_points(2, 3); + control_points << 0.0, 1.0, 2.0, 0.0, 0.0, 0.0; + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory( + BezierCurve(0.0, 1.0, control_points), {2}), + "[\\s\\S]*continuous_revolute_indices[\\s\\S]*"); +} + +/* A zero-order hold genuinely teleports at every break; certifying it +per-segment would silently skip the jumps, so it is rejected. */ +GTEST_TEST(JunctionValidation, ZeroOrderHoldIsRejected) { + const Eigen::VectorXd times = Eigen::VectorXd::LinSpaced(4, 0.0, 3.0); + Eigen::MatrixXd samples(2, 4); + samples << 0.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 0.0; + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory( + PiecewisePolynomial::ZeroOrderHold(times, samples), {}), + "[\\s\\S]*C0 discontinuity[\\s\\S]*"); +} + +// -------------------------------------------------------------------------- +// Metadata: global control box and constant coordinates. +// -------------------------------------------------------------------------- + +GTEST_TEST(Metadata, GlobalControlBox) { + std::mt19937_64 generator(5150); + std::vector>> pieces; + Eigen::VectorXd start = RandomMatrix(3, 1, &generator).col(0); + double t = 0.0; + std::vector all_control_points; + for (int i = 0; i < 3; ++i) { + BezierCurve curve = + MakeBezierCurve(start, 3, t, t + 1.0, &generator); + all_control_points.push_back(curve.control_points()); + start = curve.control_points().col(3); + t += 1.0; + pieces.push_back(std::make_unique>(curve)); + } + const CompositeTrajectory trajectory = + MakeComposite(std::move(pieces)); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, {}); + + Eigen::VectorXd expected_lower = + Eigen::VectorXd::Constant(3, std::numeric_limits::infinity()); + Eigen::VectorXd expected_upper = + Eigen::VectorXd::Constant(3, -std::numeric_limits::infinity()); + for (const Eigen::MatrixXd& cps : all_control_points) { + expected_lower = expected_lower.cwiseMin(cps.rowwise().minCoeff()); + expected_upper = expected_upper.cwiseMax(cps.rowwise().maxCoeff()); + } + EXPECT_TRUE(path.global_lower_bound().isApprox(expected_lower, 0.0)); + EXPECT_TRUE(path.global_upper_bound().isApprox(expected_upper, 0.0)); + + // The convex-hull property: dense samples stay inside the global box. + for (int i = 0; i <= 1000; ++i) { + const Eigen::VectorXd q = path.Value(3.0 * i / 1000.0); + EXPECT_TRUE( + ((q.array() >= path.global_lower_bound().array() - 1e-12).all())); + EXPECT_TRUE( + ((q.array() <= path.global_upper_bound().array() + 1e-12).all())); + } +} + +GTEST_TEST(Metadata, ConstantCoordinateFlags) { + Eigen::MatrixXd waypoints(4, 4); + // Coordinate 0 moves; 1 is exactly constant; 2 wobbles below the tolerance; + // 3 moves by just above the tolerance. + waypoints.row(0) << 0.0, 0.5, -0.25, 1.0; + waypoints.row(1) << 2.0, 2.0, 2.0, 2.0; + waypoints.row(2) << 1.0, 1.0 + 5e-9, 1.0 - 2e-8, 1.0; + waypoints.row(3) << 0.0, 0.0, 2e-7, 0.0; + + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromWaypoints(waypoints); + ASSERT_EQ(path.constant_coordinates().size(), 4u); + EXPECT_FALSE(path.constant_coordinates()[0]); + EXPECT_TRUE(path.constant_coordinates()[1]); + EXPECT_TRUE(path.constant_coordinates()[2]); + EXPECT_FALSE(path.constant_coordinates()[3]); +} + +// -------------------------------------------------------------------------- +// CompositeTrajectory handling (the GcsTrajectoryOptimization output shape). +// -------------------------------------------------------------------------- + +GTEST_TEST(Composite, BezierSegmentsRoundTrip) { + std::mt19937_64 generator(31415); + const int num_positions = 5; + const std::vector orders{3, 5, 2, 1}; + const std::vector breaks{0.0, 0.4, 1.9, 2.0, 4.25}; + + Eigen::VectorXd start = RandomMatrix(num_positions, 1, &generator).col(0); + std::vector>> pieces; + for (std::size_t i = 0; i < orders.size(); ++i) { + BezierCurve curve = + MakeBezierCurve(start, orders[i], breaks[i], breaks[i + 1], &generator); + start = curve.control_points().col(orders[i]); + pieces.push_back(std::make_unique>(curve)); + } + const CompositeTrajectory trajectory = + MakeComposite(std::move(pieces)); + + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, {}); + ASSERT_EQ(path.segments().size(), orders.size()); + for (std::size_t i = 0; i < orders.size(); ++i) { + EXPECT_EQ(path.segments()[i].control_points.cols(), orders[i] + 1); + EXPECT_EQ(path.segments()[i].t_start, breaks[i]); + EXPECT_EQ(path.segments()[i].t_end, breaks[i + 1]); + } + EXPECT_EQ(path.start_time(), breaks.front()); + EXPECT_EQ(path.end_time(), breaks.back()); + EXPECT_LT(MaxSampledError(path, trajectory, 10001), 1e-12); +} + +GTEST_TEST(Composite, NestedCompositeRecursion) { + std::mt19937_64 generator(2718); + const int num_positions = 2; + Eigen::VectorXd start = RandomMatrix(num_positions, 1, &generator).col(0); + + BezierCurve a = MakeBezierCurve(start, 2, 0.0, 1.0, &generator); + start = a.control_points().col(2); + BezierCurve b = MakeBezierCurve(start, 3, 1.0, 2.0, &generator); + start = b.control_points().col(3); + BezierCurve c = MakeBezierCurve(start, 1, 2.0, 3.0, &generator); + + std::vector>> inner_pieces; + inner_pieces.push_back(std::make_unique>(b)); + inner_pieces.push_back(std::make_unique>(c)); + auto inner = std::make_unique>( + MakeComposite(std::move(inner_pieces))); + + std::vector>> outer_pieces; + outer_pieces.push_back(std::make_unique>(a)); + outer_pieces.push_back(std::move(inner)); + const CompositeTrajectory trajectory = + MakeComposite(std::move(outer_pieces)); + + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, {}); + ASSERT_EQ(path.segments().size(), 3u); + EXPECT_EQ(path.segments()[0].control_points.cols(), 3); + EXPECT_EQ(path.segments()[1].control_points.cols(), 4); + EXPECT_EQ(path.segments()[2].control_points.cols(), 2); + EXPECT_LT(MaxSampledError(path, trajectory, 5001), 1e-12); +} + +/* A CompositeTrajectory whose segments are B-splines and PiecewisePolynomials +recurses through the same rules. */ +GTEST_TEST(Composite, MixedSegmentTypes) { + std::mt19937_64 generator(11111); + const int num_positions = 2; + + const BsplineBasis basis(4, 8, KnotVectorType::kClampedUniform, 0.0, + 1.0); + BsplineTrajectory bspline = + MakeBsplineFromBasis(basis, num_positions, &generator); + + // Continue with a first-order hold that starts exactly where the B-spline + // ends, so the junction is C0. + const Eigen::VectorXd end_value = bspline.FinalValue(); + Eigen::MatrixXd samples(num_positions, 3); + samples.col(0) = end_value; + samples.col(1) = end_value + Eigen::VectorXd::Constant(num_positions, 0.3); + samples.col(2) = end_value - Eigen::VectorXd::Constant(num_positions, 0.1); + Eigen::VectorXd times(3); + times << 1.0, 1.5, 2.0; + const PiecewisePolynomial pp = + PiecewisePolynomial::FirstOrderHold(times, samples); + + std::vector>> pieces; + pieces.push_back(std::make_unique>(bspline)); + pieces.push_back(std::make_unique>(pp)); + const CompositeTrajectory trajectory = + MakeComposite(std::move(pieces)); + + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, {}); + // 5 Bézier segments from the clamped order-4 B-spline plus 2 from the FOH. + EXPECT_EQ(path.segments().size(), 7u); + EXPECT_LT(MaxSampledError(path, trajectory, 10001), 1e-10); +} + +GTEST_TEST(Composite, UnknownSegmentTypeThrowsWithIndexAndTypeName) { + std::mt19937_64 generator(4321); + const int num_positions = 3; + Eigen::VectorXd start = RandomMatrix(num_positions, 1, &generator).col(0); + BezierCurve first = MakeBezierCurve(start, 2, 0.0, 1.0, &generator); + + std::vector>> pieces; + pieces.push_back(std::make_unique>(first)); + pieces.push_back( + std::make_unique(num_positions, 1.0, 2.0)); + const CompositeTrajectory trajectory = + MakeComposite(std::move(pieces)); + + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(trajectory, {}), + "[\\s\\S]*UnsupportedTrajectory[\\s\\S]*segment index 1[\\s\\S]*"); + + // At the top level the offending segment index is 0. + const UnsupportedTrajectory bare(num_positions, 0.0, 1.0); + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(bare, {}), + "[\\s\\S]*segment index 0[\\s\\S]*"); +} + +// -------------------------------------------------------------------------- +// Waypoints and evaluation domain handling. +// -------------------------------------------------------------------------- + +GTEST_TEST(Waypoints, OrderOneSegmentsAreExact) { + std::mt19937_64 generator(19191); + const int num_positions = 6; + const int num_waypoints = 5; + const Eigen::MatrixXd waypoints = + RandomMatrix(num_positions, num_waypoints, &generator); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromWaypoints(waypoints); + + ASSERT_EQ(path.num_positions(), num_positions); + ASSERT_EQ(static_cast(path.segments().size()), num_waypoints - 1); + EXPECT_EQ(path.start_time(), 0.0); + EXPECT_EQ(path.end_time(), num_waypoints - 1); + for (int k = 0; k + 1 < num_waypoints; ++k) { + const BezierSegment& segment = path.segments()[k]; + EXPECT_EQ(segment.t_start, k); + EXPECT_EQ(segment.t_end, k + 1); + ASSERT_EQ(segment.control_points.cols(), 2); + EXPECT_TRUE(segment.control_points.col(0).isApprox(waypoints.col(k), 0.0)); + EXPECT_TRUE( + segment.control_points.col(1).isApprox(waypoints.col(k + 1), 0.0)); + // Straight-line interpolation is exact at every parameter. + for (int i = 0; i <= 100; ++i) { + const double s = i / 100.0; + const Eigen::VectorXd expected = + (1.0 - s) * waypoints.col(k) + s * waypoints.col(k + 1); + EXPECT_LT((path.Value(k + s) - expected).cwiseAbs().maxCoeff(), 1e-15); + EXPECT_LT((path.EvaluateSegment(k, s) - expected).cwiseAbs().maxCoeff(), + 1e-15); + } + } +} + +GTEST_TEST(Waypoints, TooFewWaypointsThrows) { + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd::Zero(3, 1)), + "[\\s\\S]*at least 2 waypoints[\\s\\S]*"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd(0, 4)), + "[\\s\\S]*zero rows[\\s\\S]*"); +} + +GTEST_TEST(Evaluation, DomainEdgesClampAndOutsideThrows) { + Eigen::MatrixXd waypoints(2, 3); + waypoints << 0.0, 1.0, 3.0, -1.0, 0.0, 1.0; + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromWaypoints(waypoints); + + EXPECT_TRUE(path.Value(0.0).isApprox(waypoints.col(0), 0.0)); + EXPECT_TRUE(path.Value(2.0).isApprox(waypoints.col(2), 0.0)); + // Within the clamping slack. + EXPECT_NO_THROW(path.Value(2.0 + 1e-13)); + EXPECT_TRUE(path.Value(-1e-13).isApprox(waypoints.col(0), 0.0)); + EXPECT_NO_THROW(path.EvaluateSegment(0, 1.0 + 1e-13)); + + for (const double t : {-1e-3, 2.5}) { + DRAKE_EXPECT_THROWS_MESSAGE(path.Value(t), + "[\\s\\S]*outside the path's domain[\\s\\S]*"); + } + for (const double bad_s : {-0.5, 1.5}) { + DRAKE_EXPECT_THROWS_MESSAGE( + path.EvaluateSegment(0, bad_s), + "[\\s\\S]*outside the segment's domain[\\s\\S]*"); + } + for (const int k : {-1, 2}) { + DRAKE_EXPECT_THROWS_MESSAGE(path.EvaluateSegment(k, 0.5), + "[\\s\\S]*out of range[\\s\\S]*"); + } +} + +/* Segment-time bookkeeping contract for downstream modules: at a junction time +shared by two segments, Value() evaluates the LATER segment, exactly as +drake::trajectories::PiecewiseTrajectory::get_segment_index() does; at the +domain end it evaluates the last segment. */ +GTEST_TEST(Evaluation, JunctionTimeSelectsTheLaterSegment) { + Eigen::MatrixXd waypoints(1, 4); + waypoints << 0.0, 1.0, 3.0, 6.0; + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromWaypoints(waypoints); + ASSERT_EQ(path.segments().size(), 3u); + // Segment k spans [k, k+1]; at t = 1 both segment 0's end and segment 1's + // start are the value 1.0, and the lookup lands on segment 1. + EXPECT_EQ(path.Value(0.0)[0], 0.0); + EXPECT_EQ(path.Value(1.0)[0], 1.0); + EXPECT_EQ(path.Value(2.0)[0], 3.0); + EXPECT_EQ(path.Value(3.0)[0], 6.0); + // Interior samples resolve to the expected segment. + EXPECT_NEAR(path.Value(1.5)[0], 2.0, 1e-15); + EXPECT_NEAR(path.Value(2.5)[0], 4.5, 1e-15); +} + +/* Junction times are shared by two segments; Value() must agree with the source +trajectory there regardless of which side the lookup lands on. */ +GTEST_TEST(Evaluation, JunctionTimesAreConsistent) { + std::mt19937_64 generator(606); + const int num_positions = 3; + Eigen::VectorXd start = RandomMatrix(num_positions, 1, &generator).col(0); + std::vector>> pieces; + double t = 0.0; + for (int i = 0; i < 4; ++i) { + BezierCurve curve = + MakeBezierCurve(start, 3, t, t + 0.75, &generator); + start = curve.control_points().col(3); + t += 0.75; + pieces.push_back(std::make_unique>(curve)); + } + const CompositeTrajectory trajectory = + MakeComposite(std::move(pieces)); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, {}); + for (int i = 0; i < 4; ++i) { + const double junction = 0.75 * i; + EXPECT_LT((path.Value(junction) - trajectory.value(junction)) + .cwiseAbs() + .maxCoeff(), + 1e-13) + << "junction " << junction; + } +} + +} // namespace +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/soundness_fuzz_test.cc b/planning/continuous_collision/test/soundness_fuzz_test.cc new file mode 100644 index 000000000000..6474bc5abbd9 --- /dev/null +++ b/planning/continuous_collision/test/soundness_fuzz_test.cc @@ -0,0 +1,1007 @@ +// End-to-end soundness fuzz: random worlds × random trajectories, cross-checked +// two ways. A sampled configuration whose clearance reaches the threshold +// refutes a `kCertifiedFree` verdict, so every certified case is searched for +// one (10⁴ configurations, 10⁵ on a subset); a `kViolationFound` witness is +// re-evaluated at its configuration, from a context this run never touched, and +// must really violate; and a `kInconclusive` witness must be backed by a +// clearance within 10·(τ_p + ε) of the threshold near the reported time. +// +// A failure here is a soundness bug, not a reason to loosen the test. Every +// message carries a complete repro: seed, world recipe, control points. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/common/trajectories/bspline_trajectory.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/trajectory.h" +#include "drake/geometry/query_object.h" +#include "drake/geometry/scene_graph_inspector.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/bspline_basis.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/roll_pitch_yaw.h" +#include "drake/multibody/plant/coulomb_friction.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/prismatic_joint.h" +#include "drake/multibody/tree/revolute_joint.h" +#include "drake/multibody/tree/spatial_inertia.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" +#include "drake/planning/continuous_collision/distance_oracle.h" +#include "drake/planning/continuous_collision/internal.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { +namespace { + +using drake::Parallelism; +using drake::geometry::Box; +using drake::geometry::Capsule; +using drake::geometry::Convex; +using drake::geometry::Cylinder; +using drake::geometry::Ellipsoid; +using drake::geometry::GeometryId; +using drake::geometry::HalfSpace; +using drake::geometry::QueryObject; +using drake::geometry::Shape; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +using drake::math::RollPitchYawd; +using drake::multibody::CoulombFriction; +using drake::multibody::MultibodyPlant; +using drake::multibody::PrismaticJoint; +using drake::multibody::RevoluteJoint; +using drake::multibody::RigidBody; +using drake::multibody::SpatialInertia; +using drake::planning::RobotDiagram; +using drake::planning::RobotDiagramBuilder; +using drake::trajectories::BezierCurve; +using drake::trajectories::BsplineTrajectory; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::Trajectory; +using Eigen::Vector3d; +using Eigen::VectorXd; + +#ifdef DRAKE_CCD_FUZZ_SMALL_CORPUS +// A quarter corpus for instrumented builds, where the dense cross-check runs +// one to two orders of magnitude slower than in Release. The case *recipes* do +// not change, so the shrunk corpus is a prefix of the full one and a failure it +// finds reproduces under the full run at the same case index. +constexpr int kNumCases = 50; +#else +// The gate is CI wall time, not case count: the dominant cost is the dense +// cross-check (~10⁷ signed-distance queries per run), not certification. Two +// hundred cases stay an order of magnitude inside the ~3 min budget in Release, +// which is the only flavor with that much room. +constexpr int kNumCases = 200; +#endif + +// Corpus-composition floors, expressed as fractions of kNumCases rather than +// as absolute counts so that the shrunk corpus is held to the same *shape* of +// corpus instead of to a floor it cannot reach. +constexpr int kMinCertified = kNumCases / 5; // 20% +constexpr int kMinViolation = kNumCases / 10; // 10% +constexpr int kMinInconclusive = kNumCases / 40; // 2.5% +constexpr int kMinPerTrajectoryFamily = kNumCases / 10; // 10% each +constexpr int kMinScanQueries = 500 * kNumCases; + +constexpr uint64_t kBaseSeed = 0x5eed'0000'0000'0000ull; +// kDenseSamples resolves any clearance dip wider than ~10⁻⁴ of the domain; +// every kDeepEvery-th certified case gets the 10⁵-sample sweep. Sample counts +// are approximate: they are split evenly across segments and each segment gets +// both endpoints. +constexpr int kDenseSamples = 10000; +constexpr int kDeepDenseSamples = 100000; +constexpr int kDeepEvery = 10; +// Samples used to locate a trajectory's minimum clearance when building a +// grazing case. +constexpr int kGrazeProbeSamples = 2000; + +// The worst signed-distance accuracy Drake documents for any supported shape +// combination (query_object.h Table 4, Cylinder–Ellipsoid): an upper bound on +// the per-pair τ_p, which is all the tests below need. +constexpr double kWorstTau = 5e-5; + +// The dense scan is an upper bound on the true minimum clearance (a dip can +// hide between two samples), so the certified-guaranteed regime is only +// claimed when the sampled minimum clears the contract band by this much. At +// kDenseSamples over the domain the clearance moves at most ~1 mm between +// adjacent samples on the fastest corpus worlds, so 2 mm is a safe guard. +constexpr double kScanGuard = 2e-3; + +// Recipes. Everything random about a case lives in these structs, and every one +// of them prints itself, so a failure message is a complete repro. + +enum class ShapeKind { + kSphere, + kBox, + kCapsule, + kCylinder, + kEllipsoid, + kConvex +}; + +struct ShapeSpec { + ShapeKind kind{ShapeKind::kSphere}; + // Sphere: (r, ·, ·). Box: full (w, d, h). Capsule/Cylinder: (r, length, ·). + // Ellipsoid: (a, b, c). Convex: (scale, ·, ·) of a regular tetrahedron. + Vector3d dims{Vector3d::Zero()}; +}; + +const char* Name(ShapeKind kind) { + static constexpr const char* kNames[] = { + "Sphere", "Box", "Capsule", "Cylinder", "Ellipsoid", "ConvexTetra"}; + return kNames[static_cast(kind)]; +} + +// A regular tetrahedron of circumradius √3·`scale`, as a vertex matrix; Drake +// takes the convex hull of these points. +Eigen::Matrix3Xd TetrahedronPoints(double scale) { + Eigen::Matrix3Xd points(3, 4); + points.col(0) = scale * Vector3d(1, 1, 1); + points.col(1) = scale * Vector3d(1, -1, -1); + points.col(2) = scale * Vector3d(-1, 1, -1); + points.col(3) = scale * Vector3d(-1, -1, 1); + return points; +} + +std::unique_ptr MakeShape(const ShapeSpec& spec) { + switch (spec.kind) { + case ShapeKind::kSphere: + return std::make_unique(spec.dims[0]); + case ShapeKind::kBox: + return std::make_unique(spec.dims[0], spec.dims[1], spec.dims[2]); + case ShapeKind::kCapsule: + return std::make_unique(spec.dims[0], spec.dims[1]); + case ShapeKind::kCylinder: + return std::make_unique(spec.dims[0], spec.dims[1]); + case ShapeKind::kEllipsoid: + return std::make_unique(spec.dims[0], spec.dims[1], + spec.dims[2]); + case ShapeKind::kConvex: + return std::make_unique(TetrahedronPoints(spec.dims[0]), + "fuzz_tetra"); + } + throw std::logic_error("unreachable"); +} + +enum class JointKind { kRevolute, kPrismatic }; + +struct LinkSpec { + // Index into WorldRecipe::links, or -1 for the world body. + int parent{-1}; + JointKind joint{JointKind::kRevolute}; + Vector3d axis{Vector3d::UnitZ()}; + // The joint's frame on the parent: rotation (rpy) and translation. + Vector3d rpy_PF{Vector3d::Zero()}; + Vector3d p_PF{Vector3d::Zero()}; + // The link geometry's pose in the link frame. + Vector3d p_LG{Vector3d::Zero()}; + ShapeSpec shape; +}; + +struct ObstacleSpec { + Vector3d p_W{Vector3d::Zero()}; + Vector3d rpy_W{Vector3d::Zero()}; + ShapeSpec shape; +}; + +struct WorldRecipe { + uint64_t seed{0}; + std::vector links; + std::vector obstacles; + // An anchored HalfSpace floor (exercises the analytic distance route). + bool floor{false}; + double floor_z{-0.45}; + + int num_positions() const { return static_cast(links.size()); } + std::string Describe() const; +}; + +enum class TrajectoryKind { kPwl, kBezier, kBspline }; + +struct TrajectoryRecipe { + TrajectoryKind kind{TrajectoryKind::kBezier}; + // Bézier order (1…5) or B-spline order (4). Unused for PWL. + int order{1}; + // n × K: waypoints (PWL) or control points (Bézier / B-spline). + Eigen::MatrixXd points; + std::string Describe() const; +}; + +std::string FormatVector(const Vector3d& v) { + std::ostringstream out; + out << "(" << v[0] << ", " << v[1] << ", " << v[2] << ")"; + return out.str(); +} + +std::string WorldRecipe::Describe() const { + std::ostringstream out; + out.precision(17); + out << "world seed=" << seed << " links=" << links.size() + << " obstacles=" << obstacles.size() + << " floor=" << (floor ? "yes" : "no") << "\n"; + for (std::size_t i = 0; i < links.size(); ++i) { + const LinkSpec& link = links[i]; + out << " link" << i << ": parent=" + << (link.parent < 0 ? std::string("world") + : "link" + std::to_string(link.parent)) + << " joint=" + << (link.joint == JointKind::kRevolute ? "revolute" : "prismatic") + << " axis=" << FormatVector(link.axis) + << " rpy_PF=" << FormatVector(link.rpy_PF) + << " p_PF=" << FormatVector(link.p_PF) + << " p_LG=" << FormatVector(link.p_LG) + << " shape=" << Name(link.shape.kind) << FormatVector(link.shape.dims) + << "\n"; + } + for (std::size_t i = 0; i < obstacles.size(); ++i) { + const ObstacleSpec& obstacle = obstacles[i]; + out << " obstacle" << i << ": p_W=" << FormatVector(obstacle.p_W) + << " rpy_W=" << FormatVector(obstacle.rpy_W) + << " shape=" << Name(obstacle.shape.kind) + << FormatVector(obstacle.shape.dims) << "\n"; + } + if (floor) out << " floor: HalfSpace at z = " << floor_z << "\n"; + return out.str(); +} + +std::string TrajectoryRecipe::Describe() const { + std::ostringstream out; + out.precision(17); + out << "trajectory kind=" + << (kind == TrajectoryKind::kPwl + ? "PWL" + : (kind == TrajectoryKind::kBezier ? "Bezier" : "Bspline")) + << " order=" << order << " points(" << points.rows() << "x" + << points.cols() << "):\n"; + for (int i = 0; i < points.rows(); ++i) { + out << " ["; + for (int j = 0; j < points.cols(); ++j) { + out << (j > 0 ? ", " : "") << points(i, j); + } + out << "]\n"; + } + return out.str(); +} + +// --------------------------------------------------------------------------- +// Random generation. +// --------------------------------------------------------------------------- + +class Rng { + public: + explicit Rng(uint64_t seed) : engine_(seed) {} + + double Uniform(double lo, double hi) { + return std::uniform_real_distribution(lo, hi)(engine_); + } + int Int(int lo, int hi) { + return std::uniform_int_distribution(lo, hi)(engine_); + } + bool Bernoulli(double p) { return std::bernoulli_distribution(p)(engine_); } + // Named locals throughout: the order in which a compiler evaluates sibling + // constructor arguments is unspecified, so drawing variates inline would make + // the corpus depend on the toolchain. + Vector3d UniformVector(double lo, double hi) { + const double x = Uniform(lo, hi); + const double y = Uniform(lo, hi); + const double z = Uniform(lo, hi); + return Vector3d(x, y, z); + } + // Rejection-sampled, so no pole bias. + Vector3d Direction() { + while (true) { + const Vector3d v = UniformVector(-1.0, 1.0); + const double n = v.norm(); + if (n > 1e-3 && n <= 1.0) return v / n; + } + } + Vector3d Offset(double lo, double hi) { + const Vector3d direction = Direction(); + const double length = Uniform(lo, hi); + return direction * length; + } + + private: + std::mt19937_64 engine_; +}; + +// Link geometries stay small (≤ 5 cm half-extent) and sit ~12–18 cm out along +// the link, while joints are ~25–35 cm apart, so adjacent links have real +// clearance in most configurations but can genuinely fold into each other. +// (MultibodyPlant::Finalize only filters *welded* subgraphs, so every +// parent/child pair here is live.) +ShapeSpec RandomLinkShape(Rng* rng) { + ShapeSpec spec; + const int roll = rng->Int(0, 11); + if (roll <= 2) { + spec.kind = ShapeKind::kSphere; + spec.dims[0] = rng->Uniform(0.02, 0.05); + } else if (roll <= 5) { + spec.kind = ShapeKind::kBox; + spec.dims = rng->UniformVector(0.04, 0.10); + } else if (roll <= 7) { + spec.kind = ShapeKind::kCapsule; + spec.dims[0] = rng->Uniform(0.02, 0.04); + spec.dims[1] = rng->Uniform(0.04, 0.12); + } else if (roll <= 9) { + spec.kind = ShapeKind::kCylinder; + spec.dims[0] = rng->Uniform(0.02, 0.04); + spec.dims[1] = rng->Uniform(0.04, 0.12); + } else if (roll == 10) { + spec.kind = ShapeKind::kEllipsoid; + spec.dims = rng->UniformVector(0.02, 0.06); + } else { + spec.kind = ShapeKind::kConvex; + spec.dims[0] = rng->Uniform(0.02, 0.04); + } + return spec; +} + +ShapeSpec RandomObstacleShape(Rng* rng) { + ShapeSpec spec; + const int roll = rng->Int(0, 9); + if (roll <= 3) { + spec.kind = ShapeKind::kBox; + spec.dims = rng->UniformVector(0.06, 0.22); + } else if (roll <= 6) { + spec.kind = ShapeKind::kSphere; + spec.dims[0] = rng->Uniform(0.04, 0.11); + } else if (roll <= 8) { + spec.kind = ShapeKind::kCapsule; + spec.dims[0] = rng->Uniform(0.03, 0.08); + spec.dims[1] = rng->Uniform(0.06, 0.20); + } else { + spec.kind = ShapeKind::kConvex; + spec.dims[0] = rng->Uniform(0.05, 0.10); + } + return spec; +} + +WorldRecipe RandomWorld(uint64_t seed) { + Rng rng(seed); + WorldRecipe recipe; + recipe.seed = seed; + const int num_links = rng.Int(2, 5); + for (int i = 0; i < num_links; ++i) { + LinkSpec link; + // A chain most of the time, a small tree otherwise: link i hangs off a + // uniformly chosen earlier link (or the world for link 0). + link.parent = + (i == 0) ? -1 : (rng.Bernoulli(0.72) ? i - 1 : rng.Int(0, i - 1)); + link.joint = + rng.Bernoulli(0.7) ? JointKind::kRevolute : JointKind::kPrismatic; + link.axis = rng.Direction(); + link.rpy_PF = rng.UniformVector(-0.6, 0.6); + link.p_PF = rng.Offset(0.25, 0.35); + link.p_LG = rng.Offset(0.12, 0.18); + link.shape = RandomLinkShape(&rng); + recipe.links.push_back(link); + } + const int num_obstacles = rng.Int(2, 6); + for (int i = 0; i < num_obstacles; ++i) { + ObstacleSpec obstacle; + obstacle.p_W = rng.Offset(0.25, 0.80); + obstacle.rpy_W = rng.UniformVector(-3.0, 3.0); + obstacle.shape = RandomObstacleShape(&rng); + recipe.obstacles.push_back(obstacle); + } + recipe.floor = rng.Bernoulli(0.3); + recipe.floor_z = rng.Uniform(-0.6, -0.35); + return recipe; +} + +std::unique_ptr> BuildWorld(const WorldRecipe& recipe) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto robot = plant.AddModelInstance("robot"); + const auto env = plant.AddModelInstance("env"); + const CoulombFriction friction(1.0, 1.0); + const SpatialInertia inertia = + SpatialInertia::SolidSphereWithMass(1.0, 0.05); + + std::vector*> bodies; + for (std::size_t i = 0; i < recipe.links.size(); ++i) { + const LinkSpec& link = recipe.links[i]; + const std::string name = "link" + std::to_string(i); + const RigidBody& body = plant.AddRigidBody(name, robot, inertia); + const RigidBody& parent = + link.parent < 0 ? plant.world_body() : *bodies[link.parent]; + const RigidTransformd X_PF(RollPitchYawd(link.rpy_PF), link.p_PF); + if (link.joint == JointKind::kRevolute) { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, body, + RigidTransformd(), link.axis); + } else { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, + body, RigidTransformd(), link.axis); + } + plant.RegisterCollisionGeometry(body, RigidTransformd(link.p_LG), + *MakeShape(link.shape), name + "_geom", + friction); + bodies.push_back(&body); + } + for (std::size_t i = 0; i < recipe.obstacles.size(); ++i) { + const ObstacleSpec& obstacle = recipe.obstacles[i]; + const std::string name = "obstacle" + std::to_string(i); + const RigidBody& body = plant.AddRigidBody(name, env, inertia); + plant.WeldFrames( + plant.world_frame(), body.body_frame(), + RigidTransformd(RollPitchYawd(obstacle.rpy_W), obstacle.p_W)); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + *MakeShape(obstacle.shape), name + "_geom", + friction); + } + if (recipe.floor) { + const RigidBody& body = plant.AddRigidBody("floor", env, inertia); + plant.WeldFrames(plant.world_frame(), body.body_frame(), + RigidTransformd(Vector3d(0.0, 0.0, recipe.floor_z))); + plant.RegisterCollisionGeometry(body, RigidTransformd(), HalfSpace(), + "floor_geom", friction); + } + return builder.Build(); +} + +// Random control/waypoint columns around a random centre. `excursion` scales +// the amplitude: small excursions mostly stay free, large ones sweep across +// the obstacle field, and the range is chosen so the corpus lands on a mix of +// certified / violating / grazing outcomes (asserted at the end of the run). +TrajectoryRecipe RandomTrajectory(const WorldRecipe& world, Rng* rng) { + const int n = world.num_positions(); + VectorXd centre(n); + VectorXd amplitude(n); + const double excursion = rng->Uniform(0.15, 1.6); + for (int i = 0; i < n; ++i) { + const bool prismatic = world.links[i].joint == JointKind::kPrismatic; + centre[i] = prismatic ? rng->Uniform(-0.10, 0.10) : rng->Uniform(-2.0, 2.0); + amplitude[i] = excursion * (prismatic ? 0.15 : 1.2); + } + + TrajectoryRecipe recipe; + const int kind_roll = rng->Int(0, 2); + int columns = 0; + if (kind_roll == 0) { + recipe.kind = TrajectoryKind::kPwl; + recipe.order = 1; + columns = rng->Int(2, 5); + } else if (kind_roll == 1) { + recipe.kind = TrajectoryKind::kBezier; + recipe.order = rng->Int(1, 5); + columns = recipe.order + 1; + } else { + recipe.kind = TrajectoryKind::kBspline; + recipe.order = 4; + columns = rng->Int(4, 7); + } + recipe.points.resize(n, columns); + for (int j = 0; j < columns; ++j) { + for (int i = 0; i < n; ++i) { + recipe.points(i, j) = centre[i] + amplitude[i] * rng->Uniform(-1.0, 1.0); + } + } + return recipe; +} + +std::unique_ptr> BuildTrajectory( + const TrajectoryRecipe& recipe) { + switch (recipe.kind) { + case TrajectoryKind::kPwl: { + // A first-order hold is a PiecewisePolynomial, so this also exercises + // trajectory normalization's monomial → Bernstein conversion route. + const int columns = static_cast(recipe.points.cols()); + VectorXd breaks(columns); + for (int j = 0; j < columns; ++j) breaks[j] = j; + return std::make_unique>( + PiecewisePolynomial::FirstOrderHold(breaks, recipe.points)); + } + case TrajectoryKind::kBezier: + return std::make_unique>(0.0, 1.0, recipe.points); + case TrajectoryKind::kBspline: { + std::vector control_points; + for (int j = 0; j < recipe.points.cols(); ++j) { + control_points.push_back(recipe.points.col(j)); + } + return std::make_unique>( + drake::math::BsplineBasis( + recipe.order, static_cast(control_points.size())), + control_points); + } + } + throw std::logic_error("unreachable"); +} + +// --------------------------------------------------------------------------- +// The independent dense cross-check. +// --------------------------------------------------------------------------- + +// Radius of the smallest sphere about the *geometry frame origin* containing +// the shape. Re-derived here rather than reused from the library, so the +// broadphase this cross-check uses to skip far pairs cannot inherit a bug from +// the code it audits. std::nullopt (HalfSpace, Convex, Mesh) means the pair +// always takes the narrowphase. +std::optional LocalRadius(const Shape& shape) { + return shape.Visit>( + [](const auto& s) -> std::optional { + using S = std::decay_t; + if constexpr (std::is_same_v) { + return s.radius(); + } else if constexpr (std::is_same_v) { + return 0.5 * s.size().norm(); + } else if constexpr (std::is_same_v) { + return 0.5 * s.length() + s.radius(); + } else if constexpr (std::is_same_v) { + return std::hypot(0.5 * s.length(), s.radius()); + } else if constexpr (std::is_same_v) { + return std::max({s.a(), s.b(), s.c()}); + } else { + return std::nullopt; + } + }); +} + +// Per-checker scaffolding for the dense scan: a dense list of the geometries +// that appear in some pair, their local radii, and each pair's two slots. +class DenseScanner { + public: + explicit DenseScanner(const RobotDiagram& model) + : model_(&model), + oracle_(model), + root_(model.CreateDefaultContext()), + plant_context_( + &model.plant().GetMyMutableContextFromRoot(root_.get())) { + const auto& inspector = model.scene_graph().model_inspector(); + const auto slot = [&](GeometryId id) { + for (std::size_t i = 0; i < geometries_.size(); ++i) { + if (geometries_[i] == id) return static_cast(i); + } + geometries_.push_back(id); + radius_.push_back(LocalRadius(inspector.GetShape(id))); + centre_.push_back(Vector3d::Zero()); + return static_cast(geometries_.size()) - 1; + }; + for (const PairRecord& pair : oracle_.pairs()) { + slot_a_.push_back(slot(pair.a)); + slot_b_.push_back(slot(pair.b)); + } + } + + // Worst (most negative) value of ϕ_p(q) − threshold over the dense samples, + // with the time and pair that attained it. + struct Worst { + double min_slack{std::numeric_limits::infinity()}; + double worst_time{std::numeric_limits::quiet_NaN()}; + int worst_pair{-1}; + }; + + // `threshold` is Options::margin, uniform over the pairs. + Worst Scan(const PiecewiseBezierPath& path, int total_samples, + double threshold) { + const int num_segments = static_cast(path.segments().size()); + const int per_segment = std::max(2, total_samples / num_segments); + Worst result; + for (int k = 0; k < num_segments; ++k) { + const BezierSegment& segment = path.segments()[k]; + for (int i = 0; i <= per_segment; ++i) { + const double s = static_cast(i) / per_segment; + const double time = + segment.t_start + s * (segment.t_end - segment.t_start); + Evaluate(path.EvaluateSegment(k, s), time, threshold, &result); + } + } + return result; + } + + // min over samples in [t − half_width, t + half_width] of |ϕ_p − m_p| for + // one pair: the "is this really grazing?" check for kInconclusive. + double MinAbsSlackNear(const PiecewiseBezierPath& path, int pair_index, + double threshold, double time, double half_width, + int samples) { + const double lo = std::max(path.start_time(), time - half_width); + const double hi = std::min(path.end_time(), time + half_width); + const PairRecord& pair = oracle_.pairs()[pair_index]; + double best = std::numeric_limits::infinity(); + for (int i = 0; i <= samples; ++i) { + const double u = (samples == 0) ? 0.0 : static_cast(i) / samples; + const double t = lo + u * (hi - lo); + SetPositions(path.Value(t)); + ++narrowphase_queries_; + best = std::min( + best, + std::abs(oracle_.SignedDistance(query_object(), pair) - threshold)); + } + return best; + } + + int64_t narrowphase_queries() const { return narrowphase_queries_; } + + // Signed distance of one pair at an arbitrary configuration, from this + // scanner's own fresh context. + double DistanceAt(const VectorXd& q, int pair_index) { + SetPositions(q); + ++narrowphase_queries_; + return oracle_.SignedDistance(query_object(), oracle_.pairs()[pair_index]); + } + + // Index of the pair with these two geometries, or -1. + int FindPair(GeometryId a, GeometryId b) const { + const auto& pairs = oracle_.pairs(); + for (int p = 0; p < static_cast(pairs.size()); ++p) { + if (pairs[p].a == a && pairs[p].b == b) return p; + } + return -1; + } + + private: + void SetPositions(const VectorXd& q) { + model_->plant().SetPositions(plant_context_, q); + } + + const QueryObject& query_object() const { + const auto& scene_graph = model_->scene_graph(); + return scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*root_)); + } + + void Evaluate(const VectorXd& q, double time, double threshold, + Worst* result) { + SetPositions(q); + const QueryObject& query = query_object(); + for (std::size_t i = 0; i < geometries_.size(); ++i) { + centre_[i] = query.GetPoseInWorld(geometries_[i]).translation(); + } + const auto& pairs = oracle_.pairs(); + for (int p = 0; p < static_cast(pairs.size()); ++p) { + const std::optional& ra = radius_[slot_a_[p]]; + const std::optional& rb = radius_[slot_b_[p]]; + if (ra.has_value() && rb.has_value()) { + // ϕ_p ≥ ‖c_a − c_b‖ − R_a − R_b: a pair whose *lower bound* already + // clears the threshold cannot be the worst one, so skip its + // narrowphase. This is what makes 10⁴ (and 10⁵) samples per case + // affordable; it can only ever cause the scan to miss a violation if + // one of the five radius formulas above under-bounds its shape, which + // is why they are exact circumradii and not estimates. + const double lower = + (centre_[slot_a_[p]] - centre_[slot_b_[p]]).norm() - *ra - *rb; + if (lower > threshold) continue; + } + ++narrowphase_queries_; + const double slack = oracle_.SignedDistance(query, pairs[p]) - threshold; + if (slack < result->min_slack) { + result->min_slack = slack; + result->worst_time = time; + result->worst_pair = p; + } + } + } + + const RobotDiagram* model_{}; + DistanceOracle oracle_; + std::unique_ptr> root_; + drake::systems::Context* plant_context_{}; + std::vector geometries_; + std::vector> radius_; + std::vector centre_; + std::vector slot_a_; + std::vector slot_b_; + int64_t narrowphase_queries_{0}; +}; + +// --------------------------------------------------------------------------- +// The fuzz itself. +// --------------------------------------------------------------------------- + +struct Tally { + int certified{0}; + int violation{0}; + int inconclusive{0}; + int deep_scans{0}; + int graze_cases{0}; + // Resolution-contract regimes, decided by the dense scan: cases whose + // sampled clearance puts them outside the band (and therefore owe a + // definitive verdict) and cases inside it (where any verdict is allowed). + int certified_guaranteed{0}; + int violation_guaranteed{0}; + int band{0}; + int pwl{0}; + int bezier{0}; + int bspline{0}; + int64_t scan_queries{0}; + int floors{0}; + // One counter per ShapeKind, over every geometry of every world built. + std::vector shapes = std::vector(6, 0); + // Smallest clearance-over-threshold the dense scan *measured* on a case the + // checker certified (pairs its broadphase skipped are provably clear but may + // be closer than this, so it is an upper bound on the true minimum). + // Reported, not asserted: it says how close the corpus gets to the + // certificate boundary, i.e. how much teeth the cross-check has. + double tightest_certified_slack{std::numeric_limits::infinity()}; +}; + +// Base options shared by every case. The resolution alternates between the +// default (1 µm) and a coarse 1 mm, so the completeness assertions below see +// both a band the corpus's clearances dwarf and one they fall inside. +Options FuzzOptions(double margin, double resolution) { + Options options; + options.margin = margin; + options.parallelism = Parallelism::None(); + options.distance_resolution = resolution; + return options; +} + +// The resolution contract (continuous_collision_checker.h): outside the band +// margin ± (r + σ + 2τ + ε) the verdict is definitive. The corpus never carves +// a coordinate (random control points are never constant to 1e-7), so σ = 0. +double ContractBand(double resolution) { + return resolution + 2.0 * kWorstTau + kNumericalSlack; +} + +GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { + Tally tally; + for (int case_index = 0; case_index < kNumCases; ++case_index) { + const uint64_t seed = kBaseSeed + case_index; + const WorldRecipe world = RandomWorld(seed); + for (const LinkSpec& link : world.links) { + ++tally.shapes[static_cast(link.shape.kind)]; + } + for (const ObstacleSpec& obstacle : world.obstacles) { + ++tally.shapes[static_cast(obstacle.shape.kind)]; + } + if (world.floor) ++tally.floors; + Rng rng(seed ^ 0x9e37'79b9'7f4a'7c15ull); + const TrajectoryRecipe trajectory_recipe = RandomTrajectory(world, &rng); + switch (trajectory_recipe.kind) { + case TrajectoryKind::kPwl: + ++tally.pwl; + break; + case TrajectoryKind::kBezier: + ++tally.bezier; + break; + case TrajectoryKind::kBspline: + ++tally.bspline; + break; + } + + std::shared_ptr> model = BuildWorld(world); + const std::unique_ptr> trajectory = + BuildTrajectory(trajectory_recipe); + + // Both halves of the margin sweep, a bare-contact threshold and a 1 cm + // clearance requirement, plus on every fifth case a *grazing* margin: the + // trajectory's own minimum clearance, located by a coarse pre-scan. Setting + // m_p exactly there makes the tangency unavoidable, which is the only + // reliable way to reach the kInconclusive branch on random geometry. + double margin = (case_index % 2 == 0) ? 0.0 : 0.01; + bool grazing = (case_index % 5) == 3; + if (grazing) { + DenseScanner probe_scanner(*model); + const DenseScanner::Worst probe_scan = probe_scanner.Scan( + PiecewiseBezierPath::FromTrajectory(*trajectory, {}), + kGrazeProbeSamples, 0.0); + tally.scan_queries += probe_scanner.narrowphase_queries(); + if (probe_scan.min_slack > 0.01 && probe_scan.min_slack < 0.5) { + margin = probe_scan.min_slack; + ++tally.graze_cases; + } else { + grazing = false; + } + } + + SCOPED_TRACE("REPRO: case " + std::to_string(case_index) + ", margin " + + std::to_string(margin) + (grazing ? " (grazing)" : "") + "\n" + + world.Describe() + trajectory_recipe.Describe()); + + const double resolution = (case_index % 4 == 1) ? 1e-3 : 1e-6; + const Options options = FuzzOptions(margin, resolution); + const ContinuousCollisionChecker checker(model, options); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(*trajectory, {}); + const Result result = checker.CheckTrajectory(*trajectory, options); + + DenseScanner scanner(*model); + + // The resolution contract, both directions, from the dense scan. The scan + // against the raised threshold skips only pairs whose *lower bound* clears + // it, so a positive result puts every pair's sampled clearance above + // margin + band + guard; the scan against the margin itself finds any + // on-trajectory sample below margin − band. + const double band = ContractBand(resolution); + const DenseScanner::Worst clear_scan = + scanner.Scan(path, kDenseSamples, margin + band + kScanGuard); + const DenseScanner::Worst margin_scan = + scanner.Scan(path, kDenseSamples, margin); + if (clear_scan.min_slack > 0.0) { + ++tally.certified_guaranteed; + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree) + << "every sampled clearance exceeds the margin by more than the " + "contract band (" + << band + << " m) plus the scan guard, so the " + "resolution contract promises kCertifiedFree"; + } else if (margin_scan.min_slack < -band) { + ++tally.violation_guaranteed; + EXPECT_EQ(result.verdict, Verdict::kViolationFound) + << "an on-trajectory sample at t = " << margin_scan.worst_time + << " sits " << -margin_scan.min_slack + << " m below the margin, more than the contract band (" << band + << " m), so the resolution contract promises kViolationFound"; + } else { + ++tally.band; + } + + switch (result.verdict) { + case Verdict::kCertifiedFree: { + ++tally.certified; + ASSERT_FALSE(result.finding.has_value()); + // Dense sampling must find no configuration at or below the threshold. + // A single one would be a false certificate. + const bool deep = (tally.certified % kDeepEvery) == 0; + if (deep) ++tally.deep_scans; + const DenseScanner::Worst scan = + deep ? scanner.Scan(path, kDeepDenseSamples, margin) : margin_scan; + tally.tightest_certified_slack = + std::min(tally.tightest_certified_slack, scan.min_slack); + EXPECT_GT(scan.min_slack, 0.0) + << "CERTIFIED FREE but dense sampling (" + << (deep ? kDeepDenseSamples : kDenseSamples) + << " configurations) found clearance " << scan.min_slack + << " m below the threshold at t = " << scan.worst_time + << " for pair " << scan.worst_pair; + break; + } + case Verdict::kViolationFound: + ++tally.violation; + ASSERT_TRUE(result.finding.has_value()); + break; + case Verdict::kInconclusive: + ++tally.inconclusive; + ASSERT_TRUE(result.finding.has_value()); + break; + } + + if (result.finding.has_value()) { + const Finding& finding = *result.finding; + const int pair_index = + scanner.FindPair(finding.geometry_a, finding.geometry_b); + ASSERT_GE(pair_index, 0) << "the finding names an unknown pair"; + ASSERT_EQ(finding.q.size(), world.num_positions()); + + if (result.verdict == Verdict::kViolationFound) { + // The witness is exactly on the trajectory ... + EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), + 1e-9) + << "a violation witness must be an on-trajectory configuration, " + "never an interpolation artifact"; + // ... and re-measuring its pair there, from a context this run never + // touched, must confirm the violation to within the oracle contract. + const double phi = scanner.DistanceAt(finding.q, pair_index); + EXPECT_LT(phi, margin + kWorstTau) + << "violation at t = " << finding.time + << " re-measures at phi = " << phi << " against threshold " + << margin; + EXPECT_NEAR(phi, finding.distance, 1e-9) + << "the reported distance is not reproducible at the witness"; + } else { + // An inconclusive witness is an on-trajectory configuration whose + // reported distance the contract places in + // [margin − τ_p, margin + τ_p + ε + r]; re-measured from a fresh + // context it must reproduce, and the true clearance there is within + // 2τ_p of the reported one. + EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), + 1e-9); + EXPECT_GE(finding.distance, margin - kWorstTau); + EXPECT_LE(finding.distance, + margin + kWorstTau + kNumericalSlack + resolution); + const double phi = scanner.DistanceAt(finding.q, pair_index); + EXPECT_NEAR(phi, finding.distance, 1e-9) + << "the reported distance is not reproducible at the witness"; + // And the trajectory really does graze: the closest sampled clearance + // near the reported time sits within the contract band of the margin. + const double tolerance = + resolution + 10.0 * (kWorstTau + kNumericalSlack); + const double window = + 0.01 * std::max(1e-12, path.end_time() - path.start_time()); + const double best = + scanner.MinAbsSlackNear(path, pair_index, margin, finding.time, + window, /* samples = */ 400); + EXPECT_LE(best, tolerance) + << "INCONCLUSIVE at t = " << finding.time + << " but the closest sampled clearance near it is " << best + << " m from the threshold, outside r + 10*(tau + eps) = " + << tolerance; + } + } + tally.scan_queries += scanner.narrowphase_queries(); + } + + std::cout << "\n[ fuzz summary ] cases = " << kNumCases + << " certified = " << tally.certified + << " violation = " << tally.violation + << " inconclusive = " << tally.inconclusive << "\n" + << " trajectories: PWL = " << tally.pwl + << ", Bezier = " << tally.bezier << ", B-spline = " << tally.bspline + << "; grazing-margin cases = " << tally.graze_cases << "\n" + << " resolution contract: certified-guaranteed" + << " = " << tally.certified_guaranteed + << ", violation-guaranteed = " << tally.violation_guaranteed + << ", in band = " << tally.band << "\n" + << " deep (1e5-sample) scans = " + << tally.deep_scans + << "\n cross-check narrowphase queries = " + << tally.scan_queries + << "; tightest measured clearance above a certified threshold = " + << tally.tightest_certified_slack << " m\n" + << " geometries:"; + for (int kind = 0; kind < 6; ++kind) { + std::cout << " " << Name(static_cast(kind)) << "=" + << tally.shapes[kind]; + } + std::cout << ", anchored HalfSpace floors = " << tally.floors << "\n\n"; + + // The corpus has to actually exercise the outcomes it claims to cross-check; + // a fuzz that certified everything (or violated everything) would pass every + // assertion above while testing nothing. +#ifdef DRAKE_CCD_FUZZ_SMALL_CORPUS + // The shrunk corpus is an instrumentation-only configuration; the full case + // count is satisfied by the uninstrumented run CI also performs. + static_assert(kNumCases >= 40, + "the shrunk corpus must stay large enough for the corpus " + "composition floors below to be nonzero"); +#else + static_assert(kNumCases >= 150, + "the soundness sweep needs at least 150 (world, trajectory) " + "cases per run"); +#endif + static_assert(kMinInconclusive >= 1, + "every composition floor must demand at least one case"); + EXPECT_GE(tally.certified, kMinCertified); + EXPECT_GE(tally.violation, kMinViolation); + EXPECT_GE(tally.inconclusive, kMinInconclusive) + << "the grazing-margin cases should have produced kInconclusive verdicts"; + // The resolution-contract assertions must have been exercised on both sides + // of the band, and the band itself must have been populated. + EXPECT_GE(tally.certified_guaranteed, kMinCertified); + EXPECT_GE(tally.violation_guaranteed, kMinViolation); + EXPECT_GE(tally.band, 1); + // All three trajectory families of trajectory normalization must be + // represented. + EXPECT_GE(tally.pwl, kMinPerTrajectoryFamily); + EXPECT_GE(tally.bezier, kMinPerTrajectoryFamily); + EXPECT_GE(tally.bspline, kMinPerTrajectoryFamily); + // The dense scan must really be measuring distances, not skipping everything + // through its broadphase. + EXPECT_GT(tally.scan_queries, kMinScanQueries); + // Every supported geometry class must have appeared somewhere: a fuzz that + // only ever built spheres and boxes would leave the τ_p table's expensive + // rows (capsule, cylinder, ellipsoid) and the Convex path untested. + for (int kind = 0; kind < 6; ++kind) { + EXPECT_GT(tally.shapes[kind], 0) + << "no " << Name(static_cast(kind)) + << " was generated anywhere in the corpus"; + } + EXPECT_GT(tally.floors, 0); +} + +} // namespace +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/test_utilities.h b/planning/continuous_collision/test/test_utilities.h new file mode 100644 index 000000000000..8de1c2b4a77e --- /dev/null +++ b/planning/continuous_collision/test/test_utilities.h @@ -0,0 +1,523 @@ +#pragma once + +// Helpers shared by this package's tests: seeded random primitives and surface +// samplers, the throw-message probe, the random world generator two corpora are +// built from, and the corpus plus deep workload that concurrency_test.cc pins +// the driver's determinism against. +// +// Nothing here asserts; the claims live in the test files. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/geometry/query_object.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/random_rotation.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/roll_pitch_yaw.h" +#include "drake/multibody/plant/coulomb_friction.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/prismatic_joint.h" +#include "drake/multibody/tree/revolute_joint.h" +#include "drake/multibody/tree/spatial_inertia.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" +#include "drake/planning/continuous_collision/distance_oracle.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace test { + +using drake::Parallelism; +using drake::geometry::Box; +using drake::geometry::Capsule; +using drake::geometry::Cylinder; +using drake::geometry::HalfSpace; +using drake::geometry::QueryObject; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +using drake::math::RollPitchYawd; +using drake::math::RotationMatrixd; +using drake::multibody::CoulombFriction; +using drake::multibody::MultibodyPlant; +using drake::multibody::PrismaticJoint; +using drake::multibody::RevoluteJoint; +using drake::multibody::RigidBody; +using drake::multibody::SpatialInertia; +using drake::planning::RobotDiagram; +using drake::planning::RobotDiagramBuilder; +using drake::trajectories::BezierCurve; +using Eigen::Matrix3Xd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using Rng = std::mt19937_64; + +inline CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +inline SpatialInertia Inertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +// --------------------------------------------------------------------------- +// Seeded random primitives. +// +// Every helper that draws more than one variate sequences them through named +// locals: the order in which a compiler evaluates sibling constructor or +// operator arguments is unspecified, so drawing inline would make a seeded +// corpus toolchain-dependent. +// --------------------------------------------------------------------------- + +inline double Uniform(Rng* rng, double lo, double hi) { + return std::uniform_real_distribution(lo, hi)(*rng); +} + +inline int UniformInt(Rng* rng, int lo, int hi) { + return std::uniform_int_distribution(lo, hi)(*rng); +} + +inline Vector3d UniformVector(Rng* rng, double lo, double hi) { + const double x = Uniform(rng, lo, hi); + const double y = Uniform(rng, lo, hi); + const double z = Uniform(rng, lo, hi); + return Vector3d(x, y, z); +} + +inline Vector3d RandomUnitVector(Rng* rng) { + std::normal_distribution normal(0.0, 1.0); + Vector3d v; + do { + const double x = normal(*rng); + const double y = normal(*rng); + const double z = normal(*rng); + v = Vector3d(x, y, z); + } while (v.norm() < 1e-6); + return v.normalized(); +} + +inline RotationMatrixd RandomRotation(Rng* rng) { + return math::UniformlyRandomRotationMatrix(rng); +} + +inline RigidTransformd RandomTransform(Rng* rng, double scale) { + const RotationMatrixd R = RandomRotation(rng); + return RigidTransformd(R, UniformVector(rng, -scale, scale)); +} + +// --------------------------------------------------------------------------- +// Surface samplers: one point on the surface of a primitive, in its canonical +// geometry frame G. Exact area weighting is irrelevant to the property tests +// that use these; hitting every region of the surface is not. +// --------------------------------------------------------------------------- + +using Sampler = std::function; + +inline Vector3d SampleSphere(Rng* rng, double radius) { + return radius * RandomUnitVector(rng); +} + +inline Vector3d SampleBox(Rng* rng, const Vector3d& size) { + const Vector3d half = 0.5 * size; + Vector3d p = UniformVector(rng, -1.0, 1.0).cwiseProduct(half); + const int axis = UniformInt(rng, 0, 2); + p(axis) = (UniformInt(rng, 0, 1) == 0 ? -1.0 : 1.0) * half(axis); + return p; +} + +inline Vector3d SampleCapsule(Rng* rng, double radius, double length) { + const double half = 0.5 * length; + if (UniformInt(rng, 0, 1) == 0) { // Barrel. + const double phi = Uniform(rng, 0.0, 2.0 * M_PI); + const double z = Uniform(rng, -half, half); + return Vector3d(radius * std::cos(phi), radius * std::sin(phi), z); + } + const Vector3d u = RandomUnitVector(rng); // Cap. + const double z0 = u.z() >= 0.0 ? half : -half; + return Vector3d(radius * u.x(), radius * u.y(), z0 + radius * u.z()); +} + +inline Vector3d SampleCylinder(Rng* rng, double radius, double length) { + const double half = 0.5 * length; + const double phi = Uniform(rng, 0.0, 2.0 * M_PI); + if (UniformInt(rng, 0, 1) == 0) { // Barrel. + const double z = Uniform(rng, -half, half); + return Vector3d(radius * std::cos(phi), radius * std::sin(phi), z); + } + // Cap disk: the sqrt keeps the sample uniform in area, and hits the rim. + const double rho = radius * std::sqrt(Uniform(rng, 0.0, 1.0)); + const double z = UniformInt(rng, 0, 1) == 0 ? -half : half; + return Vector3d(rho * std::cos(phi), rho * std::sin(phi), z); +} + +inline Vector3d SampleEllipsoid(Rng* rng, const Vector3d& radii) { + return radii.cwiseProduct(RandomUnitVector(rng)); +} + +// `count` columns drawn from `sampler`. +inline Matrix3Xd SampleSurface(Rng* rng, int count, const Sampler& sampler) { + Matrix3Xd p(3, count); + for (int i = 0; i < count; ++i) p.col(i) = sampler(rng); + return p; +} + +// --------------------------------------------------------------------------- +// Checker plumbing. +// --------------------------------------------------------------------------- + +// Runs `call`, requires it to throw, and returns the message, so the caller can +// assert on the several identifiers it must contain. (For a single identifier, +// prefer DRAKE_EXPECT_THROWS_MESSAGE.) +template +std::string ThrowMessage(Callable&& call) { + try { + call(); + } catch (const std::exception& error) { + return error.what(); + } + ADD_FAILURE() << "expected an exception, but the call returned normally"; + return {}; +} + +// The checker is neither copyable nor movable, so tests that need to own one +// inside a container take the pointer flavor. +inline std::unique_ptr MakeCheckerPtr( + std::shared_ptr> model, const Options& options) { + return std::make_unique(std::move(model), + options); +} + +// Signed distance of `finding`'s pair, re-measured at the witness +// configuration from a fresh context and a fresh oracle: an independent +// confirmation that the witness is a real contact and not an artifact of the +// search. +inline double DistanceAtFinding(const RobotDiagram& model, + const Finding& finding) { + auto root = model.CreateDefaultContext(); + auto& plant_context = model.plant().GetMyMutableContextFromRoot(root.get()); + model.plant().SetPositions(&plant_context, finding.q); + const auto& scene_graph = model.scene_graph(); + const auto& query_object = + scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*root)); + const internal::DistanceOracle oracle(model); + for (const internal::PairRecord& pair : oracle.pairs()) { + if (pair.a == finding.geometry_a && pair.b == finding.geometry_b) { + return oracle.SignedDistance(query_object, pair); + } + } + ADD_FAILURE() << "the finding names a pair the checker does not know."; + return std::numeric_limits::quiet_NaN(); +} + +// --------------------------------------------------------------------------- +// The random world generator behind two corpora. +// --------------------------------------------------------------------------- + +// A chain of revolute joints (every third one prismatic) carrying small +// primitive geometry, plus anchored obstacles and, on odd seeds, a HalfSpace +// floor, so a corpus exercises the native narrowphase route and the analytic +// one. Link geometries are small next to the joint spacing, so adjacent links +// have real clearance in most configurations but can genuinely fold into each +// other: MultibodyPlant::Finalize only filters *welded* subgraphs, so every +// parent/child pair here is live. +struct WorldSpec { + int num_links{4}; + int num_obstacles{4}; + bool floor{true}; +}; + +inline std::unique_ptr> MakeRandomWorld( + uint64_t seed, const WorldSpec& spec = {}) { + Rng rng(seed); + const auto offset = [&rng](double lo, double hi) { + const Vector3d unit = RandomUnitVector(&rng); + const double length = Uniform(&rng, lo, hi); + return Vector3d(unit * length); + }; + + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + std::vector*> links; + for (int i = 0; i < spec.num_links; ++i) { + const std::string name = "link" + std::to_string(i); + const RigidBody& body = plant.AddRigidBody(name, Inertia()); + const RigidBody& parent = + (i == 0) ? plant.world_body() : *links.back(); + const Vector3d rpy_PF = UniformVector(&rng, -0.5, 0.5); + const RigidTransformd X_PF(RollPitchYawd(rpy_PF), offset(0.22, 0.32)); + const Vector3d axis = RandomUnitVector(&rng); + const std::string joint = "j" + std::to_string(i); + if (i % 3 == 2) { + plant.AddJoint(joint, parent, X_PF, body, + RigidTransformd(), axis); + } else { + plant.AddJoint(joint, parent, X_PF, body, + RigidTransformd(), axis); + } + const RigidTransformd X_LG(offset(0.10, 0.16)); + if (i % 2 == 0) { + const double radius = Uniform(&rng, 0.02, 0.04); + const double length = Uniform(&rng, 0.05, 0.10); + plant.RegisterCollisionGeometry(body, X_LG, Capsule(radius, length), + name + "_geom", Friction()); + } else { + const Vector3d size = UniformVector(&rng, 0.04, 0.09); + plant.RegisterCollisionGeometry(body, X_LG, + Box(size.x(), size.y(), size.z()), + name + "_geom", Friction()); + } + links.push_back(&body); + } + for (int i = 0; i < spec.num_obstacles; ++i) { + const std::string name = "obstacle" + std::to_string(i); + const RigidBody& body = plant.AddRigidBody(name, Inertia()); + const Vector3d rpy_W = UniformVector(&rng, -3, 3); + plant.WeldFrames(plant.world_frame(), body.body_frame(), + RigidTransformd(RollPitchYawd(rpy_W), offset(0.30, 0.75))); + if (i % 3 == 0) { + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Sphere(Uniform(&rng, 0.05, 0.12)), + name + "_geom", Friction()); + } else if (i % 3 == 1) { + const Vector3d size = UniformVector(&rng, 0.08, 0.20); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Box(size.x(), size.y(), size.z()), + name + "_geom", Friction()); + } else { + const double radius = Uniform(&rng, 0.04, 0.09); + const double length = Uniform(&rng, 0.08, 0.18); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Cylinder(radius, length), name + "_geom", + Friction()); + } + } + if (spec.floor && seed % 2 == 1) { + const RigidBody& floor = plant.AddRigidBody("floor", Inertia()); + plant.WeldFrames(plant.world_frame(), floor.body_frame(), + RigidTransformd(Vector3d(0.0, 0.0, -0.5))); + plant.RegisterCollisionGeometry(floor, RigidTransformd(), HalfSpace(), + "floor_geom", Friction()); + } + return builder.Build(); +} + +// --------------------------------------------------------------------------- +// The concurrency corpus and the deep workload derived from it. +// --------------------------------------------------------------------------- + +constexpr double kMargin = 0.005; +// Ten cases keeps the full 4-thread-count sweep plus the concurrent-call test +// under a second in Release, which is what makes this affordable to run again +// under TSan (~100x slower). +constexpr int kNumCases = 10; +constexpr int kMinFreeCases = 3; +constexpr int kMinViolatingCases = 3; + +// A quintic Bezier with random control points, so the corpus has real curved +// trajectories rather than straight edges. +inline Eigen::MatrixXd MakeControlPoints(uint64_t seed, int num_positions) { + Rng rng(seed ^ 0xa5a5'5a5a'0f0f'f0f0ull); + Eigen::MatrixXd points(num_positions, 6); + for (int j = 0; j < 6; ++j) { + for (int i = 0; i < num_positions; ++i) { + points(i, j) = Uniform(&rng, -1.4, 1.4); + } + } + return points; +} + +inline Options BaseOptions(Parallelism parallelism) { + Options options; + options.margin = kMargin; + options.parallelism = parallelism; + // Bounded cost per run: the whole sweep is executed several times per case. + // See the note on DeepWorkload for why the resolution matters. + options.distance_resolution = 1e-6; + return options; +} + +struct Case { + std::string name; + std::shared_ptr> model; + std::unique_ptr checker; + Eigen::MatrixXd control_points; + Verdict serial_verdict{}; + + BezierCurve trajectory() const { + return BezierCurve(0.0, 1.0, control_points); + } +}; + +// Ten cases with at least three free and three violating, taken from the +// lowest seeds that supply them (deterministic, no hard-coded lucky numbers). +// +// The vector is allocated and never freed: it owns RobotDiagrams and checkers +// whose destruction would otherwise race Drake's own static teardown. Expect +// LSan to report it if an asan preset is ever added next to the tsan one. +inline const std::vector>& Corpus() { + static const std::vector>* corpus = [] { + auto* cases = new std::vector>(); + int free_count = 0; + int violating_count = 0; + for (uint64_t seed = 1; seed <= 200; ++seed) { + if (static_cast(cases->size()) >= kNumCases) break; + auto entry = std::make_unique(); + entry->name = "seed_" + std::to_string(seed); + entry->model = MakeRandomWorld(seed); + entry->checker = + MakeCheckerPtr(entry->model, BaseOptions(Parallelism::None())); + entry->control_points = + MakeControlPoints(seed, entry->model->plant().num_positions()); + entry->serial_verdict = + entry->checker->CheckTrajectory(entry->trajectory()).verdict; + // Keep the corpus balanced: stop taking more of whichever kind is + // already well represented. + const bool is_free = entry->serial_verdict == Verdict::kCertifiedFree; + const bool is_violating = + entry->serial_verdict == Verdict::kViolationFound; + if (!is_free && !is_violating) continue; + if (is_free && free_count >= kNumCases - kMinViolatingCases) continue; + if (is_violating && violating_count >= kNumCases - kMinFreeCases) { + continue; + } + (is_free ? free_count : violating_count) += 1; + cases->push_back(std::move(entry)); + } + return cases; + }(); + return *corpus; +} + +// Bit-for-bit equality of two reported witnesses. Nothing here is a tolerance: +// two runs of the same deterministic computation either agree exactly or the +// claim of determinism is false. +inline ::testing::AssertionResult FindingIdentical( + const std::optional& a, const std::optional& b) { + if (a.has_value() != b.has_value()) { + return ::testing::AssertionFailure() + << "one run reported a finding and the other did not"; + } + if (!a.has_value()) return ::testing::AssertionSuccess(); + if (a->time != b->time) { + return ::testing::AssertionFailure() + << "time " << a->time << " vs " << b->time; + } + if (a->q.size() != b->q.size() || !(a->q.array() == b->q.array()).all()) { + return ::testing::AssertionFailure() << "witness configuration differs"; + } + if (a->geometry_a != b->geometry_a || a->geometry_b != b->geometry_b) { + return ::testing::AssertionFailure() << "pair differs"; + } + if (a->distance != b->distance) { + return ::testing::AssertionFailure() << "distance differs"; + } + if (a->nearest_a_W != b->nearest_a_W || a->nearest_b_W != b->nearest_b_W) { + return ::testing::AssertionFailure() << "witness points differ"; + } + return ::testing::AssertionSuccess(); +} + +// A corpus case run at a margin just below its own swept clearance, which is +// what makes the subdivision tree deep and *narrow*: certifying a node needs +// phi - tau - Delta > m, so as the threshold approaches the trajectory's +// closest approach the motion bound has to be driven to nothing there and +// nowhere else. The result is thousands of nodes concentrated in a tiny +// sub-interval of one segment, which is the shape a depth-seeded work queue +// cannot split. That margin is found by bisection rather than hard-coded, so +// the workload survives any change to the random worlds, the bounds, or Drake. +// +// kProbeBudget is the node count the bisection converges against, and +// kMinDeepNodes is the floor concurrency_test.cc holds the result to, so the +// workload cannot silently degenerate if the corpus or the bisection drifts. +constexpr uint64_t kProbeBudget = 6000; +constexpr uint64_t kMinDeepNodes = 800; + +// The workload runs at its own resolution, finer than BaseOptions', because +// the resolution sets how deep a *certified* tree can get: a margin close +// enough to the tangency to need a deeper tree than the resolution allows +// ends kInconclusive instead, so the bisection converges on the largest +// margin whose tree certifies above the floor, and a finer resolution admits +// a deeper one. Measured on this corpus, the converged tree has 625 nodes at +// 1e-6, 847 at 1e-7, 1055 at 1e-8 and 1237 at 1e-9. +// +// The resolution is also what bounds the bisection's cost in an unoptimized +// build. Most probes land on a margin the search rejects, and a rejected +// probe keeps subdividing until every pair on every leaf is either certified +// or bounded to within the resolution, so its cost grows roughly linearly in +// 1 / resolution: the whole bisection visits ~6.5e4 nodes at 1e-8 but ~3.7e5 +// at 1e-9, which is the difference between fitting the dbg test budget and +// straining it. (Before Options::max_nodes was withdrawn from the public API +// the probe bounded itself directly and the resolution did not have to.) +constexpr double kDeepResolution = 1e-8; + +struct DeepWorkload { + const Case* entry{}; + double margin{0.0}; + uint64_t num_nodes{0}; + + Options options(Parallelism parallelism) const { + Options options = BaseOptions(parallelism); + options.margin = margin; + options.distance_resolution = kDeepResolution; + return options; + } +}; + +inline const DeepWorkload& Deep() { + static const DeepWorkload* workload = []() { + auto* deep = new DeepWorkload(); + for (const auto& entry : Corpus()) { + if (entry->serial_verdict != Verdict::kCertifiedFree) continue; + deep->entry = entry.get(); + break; + } + if (deep->entry == nullptr) return deep; + + const auto certifiable_within_budget = [&](double margin) { + Options options = deep->options(Parallelism::None()); + options.margin = margin; + const Result result = deep->entry->checker->CheckTrajectory( + deep->entry->trajectory(), options); + return result.verdict == Verdict::kCertifiedFree && + result.num_nodes <= kProbeBudget; + }; + double certifiable = 0.0; + double grazing = kMargin; + for (int i = 0; i < 12 && certifiable_within_budget(grazing); ++i) { + certifiable = grazing; + grazing *= 2.0; + } + for (int i = 0; i < 30; ++i) { + const double mid = 0.5 * (certifiable + grazing); + (certifiable_within_budget(mid) ? certifiable : grazing) = mid; + } + deep->margin = certifiable; + deep->num_nodes = deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), + deep->options(Parallelism::None())) + .num_nodes; + return deep; + }(); + return *workload; +} + +} // namespace test +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/thin_obstacle_test.cc b/planning/continuous_collision/test/thin_obstacle_test.cc new file mode 100644 index 000000000000..72acbf3f39bf --- /dev/null +++ b/planning/continuous_collision/test/thin_obstacle_test.cc @@ -0,0 +1,362 @@ +// A plate thin enough that Drake's SceneGraphCollisionChecker steps over it at +// its default edge_step_size; ContinuousCollisionChecker must reject the edge +// anyway. The mirror image is here too: a millimetre-scale gap that is +// genuinely free and certifies with a bounded node budget. Every world is built +// programmatically and no case uses an RNG or a model file. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/planning/collision_checker_params.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" +#include "drake/planning/scene_graph_collision_checker.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace { + +using drake::planning::CollisionCheckerParams; +using drake::planning::SceneGraphCollisionChecker; +using Eigen::Vector3d; +using Eigen::VectorXd; +using test::Box; +using test::DistanceAtFinding; +using test::Friction; +using test::Inertia; +using test::MakeCheckerPtr; +using test::MultibodyPlant; +using test::Parallelism; +using test::PrismaticJoint; +using test::RigidBody; +using test::RigidTransformd; +using test::RobotDiagram; +using test::RobotDiagramBuilder; +using test::Sphere; + +// --------------------------------------------------------------------------- +// The geometry, and the arithmetic that makes default sampling blind to it. +// --------------------------------------------------------------------------- +// +// The robot is a 2-dof Cartesian gantry (prismatic x, then prismatic y) +// carrying a sphere of radius kToolRadius = 5 mm. Its configuration *is* the +// tool centre, which turns every number below into an exact, checkable +// statement about the sampled check. +// +// The edge runs from q1 = (-0.5, 0) to q2 = (+0.5, 0). +// +// * Drake's default configuration distance (LinearDistanceAndInterpolation- +// Provider with unit weights) is the Euclidean norm, so d(q1, q2) = 1.0 m +// exactly. +// * CollisionCheckerParams has *no* default edge_step_size: the field is +// value-initialized to 0 and set_edge_step_size() rejects anything +// non-positive, so "the default" is whatever the planning stack picks. +// kDrakeEdgeStepSize = 0.05 is the value Drake's own planning tests and the +// IRIS/GCS examples use, and for a 1 m edge it is generous. +// * The checker therefore samples ⌈1.0 / 0.05⌉ = 20 uniform intervals, i.e. +// 21 configurations 0.05 m apart in x, at x = -0.50, -0.45, …, 0.00, 0.05, +// …, 0.50. (Reconstructed and measured below.) +// * The plate is kPlateThickness = 1 mm thick in x and welded at +// x = kPlateX = 0.025, exactly halfway between the samples at x = 0.00 and +// x = 0.05. +// * Tool and plate are in contact for +// |x − 0.025| ≤ kToolRadius + kPlateThickness/2 = 0.0055 m, +// an interval 11 mm wide. 11 mm ≪ the 50 mm sample spacing, and the plate's +// mid-plane sits 25 mm from the nearest sample, so that sample still +// measures 25 − 5 − 0.5 = 19.5 mm of clearance. + +constexpr double kToolRadius = 0.005; +constexpr double kPlateThickness = 0.001; +constexpr double kPlateX = 0.025; +constexpr double kDrakeEdgeStepSize = 0.05; +// Half-width, in x, of the set of configurations that touch the plate. +constexpr double kContactHalfWidth = kToolRadius + 0.5 * kPlateThickness; + +// The gantry: q = (x, y) is the tool-sphere centre in the z = 0 plane. The +// robot lives in its own model instance so Drake's collision checker can be +// told which bodies are "the robot". +void AddGantry(MultibodyPlant* plant) { + const auto robot = plant->AddModelInstance("robot"); + const RigidBody& carriage = + plant->AddRigidBody("carriage", robot, Inertia()); + const RigidBody& tool = plant->AddRigidBody("tool", robot, Inertia()); + plant->AddJoint("gantry_x", plant->world_body(), {}, carriage, + {}, Vector3d::UnitX()); + plant->AddJoint("gantry_y", carriage, {}, tool, {}, + Vector3d::UnitY()); + plant->RegisterCollisionGeometry(tool, RigidTransformd(), Sphere(kToolRadius), + "tool_geom", Friction()); +} + +void AddAnchoredBox(MultibodyPlant* plant, const std::string& name, + const Vector3d& p_W, const Vector3d& full_size) { + const auto env = plant->GetModelInstanceByName("env"); + const RigidBody& body = plant->AddRigidBody(name, env, Inertia()); + plant->WeldFrames(plant->world_frame(), body.body_frame(), + RigidTransformd(p_W)); + plant->RegisterCollisionGeometry( + body, RigidTransformd(), Box(full_size.x(), full_size.y(), full_size.z()), + name + "_geom", Friction()); +} + +// One plate of the given thickness welded at x = `plate_x`, spanning 0.6 m in +// y and z so the tool cannot go around it. +std::unique_ptr> MakePlateWorld(double plate_x, + double thickness) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + plant.AddModelInstance("env"); + AddGantry(&plant); + AddAnchoredBox(&plant, "plate", Vector3d(plate_x, 0.0, 0.0), + Vector3d(thickness, 0.6, 0.6)); + return builder.Build(); +} + +// The mirrored world: a slot 2·`half_gap` wide in y formed by two thin plates, +// running along the whole of the tool's x travel. +std::unique_ptr> MakeSlotWorld(double half_gap) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + plant.AddModelInstance("env"); + AddGantry(&plant); + AddAnchoredBox(&plant, "slot_left", Vector3d(0.0, half_gap, 0.0), + Vector3d(0.8, kPlateThickness, 0.6)); + AddAnchoredBox(&plant, "slot_right", Vector3d(0.0, -half_gap, 0.0), + Vector3d(0.8, kPlateThickness, 0.6)); + return builder.Build(); +} + +// Drake's sampled edge checker, always on its own freshly built RobotDiagram: +// SceneGraphCollisionChecker rewrites collision filters on the model it is +// handed, which would otherwise perturb the pair table +// ContinuousCollisionChecker snapshots at construction. +SceneGraphCollisionChecker MakeDrakeChecker( + std::unique_ptr> model, double edge_step_size) { + std::shared_ptr> shared(std::move(model)); + CollisionCheckerParams params; + params.robot_model_instances = { + shared->plant().GetModelInstanceByName("robot")}; + params.model = std::move(shared); + params.edge_step_size = edge_step_size; + // This test never calls the parallel entry points, and the default + // (Parallelism::Max()) would allocate one context per hardware thread for + // nothing. + params.implicit_context_parallelism = Parallelism::None(); + return SceneGraphCollisionChecker(std::move(params)); +} + +Options CertifiedOptions() { + Options options; + options.margin = 0.0; + options.parallelism = Parallelism::None(); + return options; +} + +VectorXd MakeQ(double x, double y) { + VectorXd q(2); + q << x, y; + return q; +} + +Eigen::MatrixXd Waypoints(const VectorXd& q1, const VectorXd& q2) { + Eigen::MatrixXd waypoints(q1.size(), 2); + waypoints.col(0) = q1; + waypoints.col(1) = q2; + return waypoints; +} + +// --------------------------------------------------------------------------- +// 1. Pin the failure mode: Drake's sampled checker reports the edge free. +// --------------------------------------------------------------------------- + +GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { + const VectorXd q1 = MakeQ(-0.5, 0.0); + const VectorXd q2 = MakeQ(0.5, 0.0); + const SceneGraphCollisionChecker drake_checker = MakeDrakeChecker( + MakePlateWorld(kPlateX, kPlateThickness), kDrakeEdgeStepSize); + + // The distance the sample count is derived from is exactly the edge length, + // and with 1 mm of plate between the waypoints the sampled check still calls + // the edge free. + EXPECT_NEAR(drake_checker.ComputeConfigurationDistance(q1, q2), 1.0, 1e-15); + EXPECT_TRUE(drake_checker.CheckEdgeCollisionFree(q1, q2)) + << "the premise of this test no longer holds on this Drake pin: " + "default-resolution sampling now catches the 1 mm plate"; + + // The sample grid is ⌈1.0/0.05⌉ = 20 uniform intervals, i.e. samples at + // x = -0.5 + k/20, which are exactly the multiples of 0.05. Sliding the plate + // across one sample period and comparing Drake's verdict against "caught iff + // the plate's mid-plane is within the contact half-width of some multiple of + // 0.05" measures that grid instead of assuming it. + const auto gap_to_nearest_sample = [](double x) { + return std::abs(x - + kDrakeEdgeStepSize * std::round(x / kDrakeEdgeStepSize)); + }; + for (int i = 0; i <= 20; ++i) { + const double offset = i * (kDrakeEdgeStepSize / 20.0); + SCOPED_TRACE("plate mid-plane at x = " + std::to_string(offset)); + const SceneGraphCollisionChecker probe = MakeDrakeChecker( + MakePlateWorld(offset, kPlateThickness), kDrakeEdgeStepSize); + EXPECT_EQ(probe.CheckEdgeCollisionFree(q1, q2), + gap_to_nearest_sample(offset) > kContactHalfWidth) + << "Drake's sample grid is not the one this test's arithmetic assumes"; + } + + // With the grid confirmed, walk it through Drake's own interpolation function + // and measure the clearance at every sample. The nearest sample is 25 mm from + // the plate's mid-plane, i.e. 19.5 mm of clearance. + const int num_intervals = + static_cast(std::ceil(1.0 / kDrakeEdgeStepSize)); + double nearest_sample_gap = std::numeric_limits::infinity(); + for (int k = 0; k <= num_intervals; ++k) { + const double ratio = static_cast(k) / num_intervals; + const VectorXd q = + drake_checker.InterpolateBetweenConfigurations(q1, q2, ratio); + nearest_sample_gap = std::min(nearest_sample_gap, std::abs(q[0] - kPlateX)); + EXPECT_TRUE(drake_checker.CheckConfigCollisionFree(q)) + << "sample " << k << " at x = " << q[0]; + } + EXPECT_NEAR(nearest_sample_gap, 0.025, 1e-12); + EXPECT_GT(nearest_sample_gap, kContactHalfWidth) + << "the plate must sit strictly between two samples"; + + // The miss is a resolution gap, not a modelling one: shrink the step size and + // the very same sampled checker finds the plate. + EXPECT_FALSE(MakeDrakeChecker(MakePlateWorld(kPlateX, kPlateThickness), 0.002) + .CheckEdgeCollisionFree(q1, q2)); +} + +// --------------------------------------------------------------------------- +// 2. The certified checker returns a definite violation with a real witness. +// --------------------------------------------------------------------------- + +GTEST_TEST(ThinObstacleTest, CertifiedCheckerCatchesTheThinPlate) { + const VectorXd q1 = MakeQ(-0.5, 0.0); + const VectorXd q2 = MakeQ(0.5, 0.0); + const std::shared_ptr> model = + MakePlateWorld(kPlateX, kPlateThickness); + const auto checker = MakeCheckerPtr(model, CertifiedOptions()); + + const Result result = checker->CheckEdge(q1, q2); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_TRUE(result.finding.has_value()); + + const Finding& finding = *result.finding; + ASSERT_EQ(finding.q.size(), 2); + + // The witness lies inside the plate-crossing parameter interval. CheckEdge + // normalizes to one order-1 segment over t ∈ [0, 1] with q(t) = q1 + t·(q2 − + // q1), so x(t) = -0.5 + t and the crossing interval is + // t ∈ (0.5 + kPlateX − h, 0.5 + kPlateX + h) with h = kContactHalfWidth. + EXPECT_GT(finding.time, 0.5 + kPlateX - kContactHalfWidth); + EXPECT_LT(finding.time, 0.5 + kPlateX + kContactHalfWidth); + EXPECT_LT(std::abs(finding.q[0] - kPlateX), kContactHalfWidth); + EXPECT_NEAR(finding.q[1], 0.0, 1e-15); + // The witness is exactly on the trajectory ... + EXPECT_LT((MakeQ(-0.5 + finding.time, 0.0) - finding.q).cwiseAbs().maxCoeff(), + 1e-12); + + // ... and a direct distance query at the witness, from a context this run + // never touched, confirms the contact. + const double phi = DistanceAtFinding(*model, finding); + EXPECT_LT(phi, 0.0) << "the witness must be a genuine interpenetration"; + EXPECT_NEAR(phi, finding.distance, 1e-12); + EXPECT_TRUE(finding.nearest_a_W.has_value()); + EXPECT_TRUE(finding.nearest_b_W.has_value()); + + // CheckPath over the same two waypoints makes the same statement. + EXPECT_EQ(checker->CheckPath(Waypoints(q1, q2)).verdict, + Verdict::kViolationFound); +} + +// --------------------------------------------------------------------------- +// 3. The mirror image: a genuinely free 3 mm squeeze is certified cheaply. +// --------------------------------------------------------------------------- + +GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { + // Slot half-width 8.5 mm against a 5 mm tool sphere and a 0.5 mm plate + // half-thickness leaves exactly 3 mm of clearance on each side, constant over + // the whole 0.6 m of travel. + constexpr double kHalfGap = 0.0085; + constexpr double kClearance = kHalfGap - 0.5 * kPlateThickness - kToolRadius; + static_assert(kClearance > 0.0); + + const auto checker = + MakeCheckerPtr(MakeSlotWorld(kHalfGap), CertifiedOptions()); + const VectorXd q1 = MakeQ(-0.3, 0.0); + const VectorXd q2 = MakeQ(0.3, 0.0); + + // Sampling passes here too, and this time it is right; the certified checker + // agrees without sampling. + EXPECT_TRUE(MakeDrakeChecker(MakeSlotWorld(kHalfGap), kDrakeEdgeStepSize) + .CheckEdgeCollisionFree(q1, q2)); + + const Result result = checker->CheckEdge(q1, q2); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + EXPECT_FALSE(result.finding.has_value()); + + // Node budget. Only the prismatic x coordinate moves, so λ = 1 for the two + // tool-vs-plate pairs and the motion bound at depth d is the node's half + // width, 0.6 / 2^(d+1). Certification needs ϕ − τ − Δ > ε, i.e. + // 0.6 / 2^(d+1) < 0.003 − 1e-6 => 2^(d+1) > 200.1 => d = 7, + // and a full binary tree to depth 7 has 2^8 − 1 = 255 nodes. Both slot pairs + // certify at the same depth, so the whole recursion is that one tree. The + // ceiling below is ~2.5× that: loose enough to survive a differently-tuned + // prefilter, tight enough to catch a regression that made the search blow up. + EXPECT_LT(result.num_nodes, uint64_t{640}) + << "certifying a 3 mm gap should cost O(log(travel / clearance)) depth, " + "not a blow-up"; + EXPECT_GE(result.num_nodes, uint64_t{64}) + << "a 3 mm gap over 0.6 m of travel cannot be certified shallowly; if it " + "could, the motion bound would be unsound"; +} + +// --------------------------------------------------------------------------- +// 4. Thickness sweep: where the resolution gap closes. +// --------------------------------------------------------------------------- + +GTEST_TEST(ThinObstacleTest, ThicknessSweepBracketsTheResolutionGap) { + // Held fixed: the plate's mid-plane at x = 0.025 (halfway between two Drake + // samples) and the tool radius. The sampled checker can only see the plate + // once the contact half-width reaches the 25 mm sample gap, i.e. once + // thickness/2 + kToolRadius ≥ 0.025 ⇔ thickness ≥ 0.040 m. + // At thickness = 0.040 the nearest sample's signed distance is exactly 0, so + // "collision" (ϕ < 0) there is decided by rounding; that is why the + // crossover assertion is a window rather than an equality. The certified + // verdict must be kViolationFound at every thickness in the sweep, since the + // plate is crossed in all of them. + const VectorXd q1 = MakeQ(-0.5, 0.0); + const VectorXd q2 = MakeQ(0.5, 0.0); + double first_caught = std::numeric_limits::quiet_NaN(); + for (const double thickness : {0.001, 0.002, 0.005, 0.010, 0.020, 0.030, + 0.038, 0.040, 0.042, 0.050, 0.080}) { + SCOPED_TRACE("thickness = " + std::to_string(thickness)); + const bool drake_free = + MakeDrakeChecker(MakePlateWorld(kPlateX, thickness), kDrakeEdgeStepSize) + .CheckEdgeCollisionFree(q1, q2); + if (!drake_free && std::isnan(first_caught)) first_caught = thickness; + EXPECT_EQ( + MakeCheckerPtr(MakePlateWorld(kPlateX, thickness), CertifiedOptions()) + ->CheckEdge(q1, q2) + .verdict, + Verdict::kViolationFound); + } + // The crossover must land where the arithmetic above predicts, 2*(0.025 - + // kToolRadius) = 0.04 m, otherwise the sweep is measuring something other + // than the resolution gap. + EXPECT_GT(first_caught, 0.03); + EXPECT_LT(first_caught, 0.06); +} + +} // namespace +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl index e4ba8f5ca3bf..b6c1fa8efca7 100644 --- a/tools/install/libdrake/build_components.bzl +++ b/tools/install/libdrake/build_components.bzl @@ -76,6 +76,7 @@ LIBDRAKE_COMPONENTS = [ "//multibody/triangle_quadrature", "//perception", "//planning", + "//planning/continuous_collision", "//planning/experimental", "//planning/graph_algorithms", "//planning/iris",