From fdd416b2e51bda9e875e64bf7559ef68879bfef2 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 14:39:55 -0400 Subject: [PATCH 01/22] [planning] Add certified_ccd: curve normalization Adds the shared option/verdict types, the numerical accounting used by the certificate arithmetic, and PiecewiseBezierPath: the exact normalization of BezierCurve, CompositeTrajectory, BsplineTrajectory (via knot insertion) and PiecewisePolynomial (via a monomial to Bernstein change of basis) into one piecewise-Bezier representation, plus the de Casteljau split the node recursion runs on. Ported from a standalone CMake package; this is a mechanical port (namespace, include paths, flattened directory layout) with no behavioral changes. --- planning/certified_ccd/BUILD.bazel | 74 ++ planning/certified_ccd/numerics.h | 39 + planning/certified_ccd/options.h | 124 ++ .../certified_ccd/piecewise_bezier_path.cc | 566 ++++++++ .../certified_ccd/piecewise_bezier_path.h | 99 ++ .../test/piecewise_bezier_path_test.cc | 1135 +++++++++++++++++ tools/install/libdrake/build_components.bzl | 1 + 7 files changed, 2038 insertions(+) create mode 100644 planning/certified_ccd/BUILD.bazel create mode 100644 planning/certified_ccd/numerics.h create mode 100644 planning/certified_ccd/options.h create mode 100644 planning/certified_ccd/piecewise_bezier_path.cc create mode 100644 planning/certified_ccd/piecewise_bezier_path.h create mode 100644 planning/certified_ccd/test/piecewise_bezier_path_test.cc diff --git a/planning/certified_ccd/BUILD.bazel b/planning/certified_ccd/BUILD.bazel new file mode 100644 index 000000000000..58d99f9e01d5 --- /dev/null +++ b/planning/certified_ccd/BUILD.bazel @@ -0,0 +1,74 @@ +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 = "certified_ccd", + visibility = ["//visibility:public"], + deps = [ + ":numerics", + ":options", + ":piecewise_bezier_path", + ], +) + +drake_cc_library( + name = "numerics", + hdrs = ["numerics.h"], +) + +drake_cc_library( + name = "options", + hdrs = ["options.h"], + deps = [ + "//common:parallelism", + "//geometry:geometry_ids", + "//multibody/tree:multibody_tree_indexes", + "@eigen", + ], +) + +drake_cc_library( + name = "piecewise_bezier_path", + srcs = ["piecewise_bezier_path.cc"], + hdrs = ["piecewise_bezier_path.h"], + deps = [ + ":options", + "//common/trajectories:trajectory", + "@eigen", + ], + implementation_deps = [ + "//common:essential", + "//common:nice_type_name", + "//common/trajectories:bezier_curve", + "//common/trajectories:bspline_trajectory", + "//common/trajectories:composite_trajectory", + "//common/trajectories:piecewise_polynomial", + "@fmt", + ], +) + +# === test/ === + +# T1 — curve module acceptance tests. +drake_cc_googletest( + name = "piecewise_bezier_path_test", + deps = [ + ":piecewise_bezier_path", + "//common:copyable_unique_ptr", + "//common:polynomial", + "//common/trajectories:bezier_curve", + "//common/trajectories:bspline_trajectory", + "//common/trajectories:composite_trajectory", + "//common/trajectories:piecewise_polynomial", + "//math:bspline_basis", + ], +) + +add_lint_tests() diff --git a/planning/certified_ccd/numerics.h b/planning/certified_ccd/numerics.h new file mode 100644 index 000000000000..e75d0248aa38 --- /dev/null +++ b/planning/certified_ccd/numerics.h @@ -0,0 +1,39 @@ +#pragma once + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** @file +Single home of the numerical accounting used everywhere (the numerical policy). + +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 +effective threshold (margin + padding), and ε the certificate 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. + +The certificate is mathematical modulo τ and ε: the library does not use +directed rounding (that hardening is a future extension); ε defaults +to 1e-9 m which dominates the accumulated FP error of the w/λ/dot-product +expression depths involved. */ + +/** 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) { + 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; +} + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/options.h b/planning/certified_ccd/options.h new file mode 100644 index 000000000000..174c0bce7a20 --- /dev/null +++ b/planning/certified_ccd/options.h @@ -0,0 +1,124 @@ +#pragma once + +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** Search modes for certification (the search algorithm). */ +enum class SearchMode { + /** Return on the first definite violation; serial execution returns the + earliest one in time. */ + kFindFirstViolation, + /** Certify the full domain and return every violation / inconclusive + region found (bounded by Options::max_reported_findings). */ + kCertifyAll, +}; + +/** Outcome of a certification run (the problem statement). */ +enum class Verdict { + /** Proof: every unfiltered pair keeps signed distance > margin + padding + over the entire continuous time domain. */ + kCertifiedFree, + /** An exactly-on-trajectory configuration violates the threshold. */ + kViolationFound, + /** Subdivision hit the resolution floor with some pair's clearance within + oracle tolerance of the threshold (grazing trajectory). */ + kInconclusive, + /** The optional node budget was exhausted first. */ + kBudgetExhausted, +}; + +/** Options controlling one certification call (the architecture; the numerical + * policy). */ +struct Options { + /** Global clearance margin δ in meters. The certificate proves signed + distance > margin + padding for every pair at every time. */ + double margin{0.0}; + /** Junction C0-continuity tolerance (per coordinate; modulo 2π for + coordinates listed in continuous_revolute_indices). */ + double continuity_tolerance{1e-7}; + /** τ: the distance oracle's accuracy contract in meters (the distance-oracle + * contract; the numerical policy). */ + double query_tolerance{1e-6}; + /** ε_slack: swallows floating-point noise in the bound arithmetic. */ + double certificate_slack{1e-9}; + /** Resolution floor as a fraction of a segment's parameter width; nodes + narrower than this become kInconclusive findings instead of splitting. */ + double min_interval{1e-9}; + /** Position coordinates whose junction continuity is checked modulo 2π + (GcsTrajectoryOptimization continuous-revolute convention). */ + std::vector continuous_revolute_indices{}; + /** Maximum polynomial degree accepted for monomial→Bernstein conversion. */ + int max_conversion_degree{10}; + SearchMode mode{SearchMode::kCertifyAll}; + int max_reported_findings{32}; + /** Optional node budget; exceeded ⇒ Verdict::kBudgetExhausted. */ + std::optional max_nodes{}; + /** If true, every certification event is recorded into a Certificate that + VerifyCertificate() can independently replay (the search algorithm). */ + bool emit_certificate{false}; + drake::Parallelism parallelism{drake::Parallelism::Max()}; +}; + +/** Per-body-pair padding, mirroring drake::planning::CollisionChecker +semantics: the effective threshold for pair p is margin + padding(p). */ +struct PaddingSpec { + /** Padding for robot-vs-environment pairs. */ + double env_padding{0.0}; + /** Padding for robot-vs-robot (self-collision) pairs. */ + double self_padding{0.0}; + /** Optional dense symmetric matrix indexed by BodyIndex; when set it + overrides the two scalars for the pairs it covers. */ + std::optional per_body_pair{}; +}; + +/** Identifies an unfiltered proximity geometry pair. */ +struct PairId { + drake::geometry::GeometryId a; + drake::geometry::GeometryId b; + drake::multibody::BodyIndex body_a; + drake::multibody::BodyIndex body_b; +}; + +/** One violation or inconclusive record (the architecture). */ +struct Finding { + /** Trajectory time of the witness configuration. */ + double time{}; + /** The witness configuration, exactly on the trajectory. */ + Eigen::VectorXd q; + PairId pair; + /** Oracle signed distance at q for this pair. */ + double distance{}; + /** Motion bound Δ_p at the terminal node (0 for breakpoint findings). */ + double motion_bound{}; + /** true ⇒ definite violation; false ⇒ grazing / inconclusive. */ + bool definite{}; + /** Closest points in world frame at q, when the narrowphase provides + them (violation findings; planners use these to push trajectories out + of collision). */ + std::optional nearest_a_W{}; + std::optional nearest_b_W{}; +}; + +/** Cost accounting for one certification call. */ +struct Statistics { + uint64_t nodes{0}; + uint64_t narrowphase_queries{0}; + uint64_t sphere_certifications{0}; + int max_depth{0}; + double wall_time_s{0.0}; +}; + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/piecewise_bezier_path.cc b/planning/certified_ccd/piecewise_bezier_path.cc new file mode 100644 index 000000000000..cb1ddb37a279 --- /dev/null +++ b/planning/certified_ccd/piecewise_bezier_path.cc @@ -0,0 +1,566 @@ +#include "drake/planning/certified_ccd/piecewise_bezier_path.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::NiceTypeName; +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; + +/* 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; + +/* Streams `args` into one string. Used only on the throwing paths. */ +template +std::string StrCat(Args&&... args) { + std::ostringstream stream; + (stream << ... << args); + return stream.str(); +} + +/* Pascal's triangle up to row `m`; table(j, a) = C(j, a) for a <= j, 0 +otherwise. Exact in double for the degrees this file accepts (the default cap +is 10; C(10, 5) = 252). */ +Eigen::MatrixXd BinomialTable(int m) { + Eigen::MatrixXd table = Eigen::MatrixXd::Zero(m + 1, m + 1); + for (int j = 0; j <= m; ++j) { + table(j, 0) = 1.0; + for (int a = 1; a <= j; ++a) { + table(j, a) = table(j - 1, a - 1) + (a <= j - 1 ? table(j - 1, a) : 0.0); + } + } + return table; +} + +/* 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 the test plan's T1 (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(StrCat( + "PiecewiseBezierPath: the BsplineTrajectory at segment index ", + source_index, " is ", bspline.rows(), "x", bspline.cols(), + "-valued; only column-vector-valued trajectories (cols() == 1) over " + "the plant's generalized positions are supported.")); + } + // 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(StrCat( + "PiecewiseBezierPath: the BsplineTrajectory at segment index ", + source_index, + " has an empty parameter domain; a trajectory must span a positive " + "time interval.")); + } +} + +/* 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 options.max_conversion_ +degree. */ +void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, + const Options& options, int source_index, + std::vector* segments) { + if (pp.cols() != 1) { + throw std::runtime_error(StrCat( + "PiecewiseBezierPath: the PiecewisePolynomial at segment index ", + source_index, " is ", pp.rows(), "x", pp.cols(), + "-valued; only column-vector-valued trajectories (cols() == 1) over " + "the plant's generalized positions are supported.")); + } + 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( + StrCat("PiecewiseBezierPath: the PiecewisePolynomial at segment index ", + source_index, " has no segments.")); + } + 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 > options.max_conversion_degree) { + throw std::runtime_error(StrCat( + "PiecewiseBezierPath: PiecewisePolynomial segment ", k, + " (source segment index ", source_index, ") has polynomial degree ", + m, ", above options.max_conversion_degree = ", + options.max_conversion_degree, + ". The monomial-to-Bernstein change of basis is ill-conditioned at " + "high degree; either raise Options::max_conversion_degree " + "deliberately or re-express the trajectory with more, lower-degree " + "segments.")); + } + 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( + StrCat("PiecewiseBezierPath: PiecewisePolynomial segment ", k, + " (source segment index ", source_index, + ") has non-positive duration ", duration, ".")); + } + const Eigen::MatrixXd binomial = BinomialTable(m); + 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 += (binomial(j, a) / binomial(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 (trajectory normalization, item 3). +*/ +void AppendSegments(const Trajectory& trajectory, + const Options& options, int* source_index, + std::vector* segments) { + if (const auto* bezier = + dynamic_cast*>(&trajectory)) { + if (bezier->control_points().cols() < 1) { + throw std::runtime_error( + StrCat("PiecewiseBezierPath: the BezierCurve at segment index ", + *source_index, " has no control points.")); + } + 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( + StrCat("PiecewiseBezierPath: the CompositeTrajectory at segment " + "index ", + *source_index, " has no segments.")); + } + for (int i = 0; i < num; ++i) { + AppendSegments(composite->segment(i), options, 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, options, *source_index, segments); + ++(*source_index); + return; + } + throw std::runtime_error(StrCat( + "PiecewiseBezierPath: unsupported trajectory type '", + NiceTypeName::Get(trajectory), "' at segment index ", *source_index, + ". Supported types are drake::trajectories::BezierCurve, " + "drake::trajectories::BsplineTrajectory, " + "drake::trajectories::PiecewisePolynomial, and " + "drake::trajectories::CompositeTrajectory whose segments are " + "themselves supported.")); +} + +/* Checks shape, time ordering/contiguity and C0 junctions (trajectory + * normalization). */ +void ValidateSegments(int num_positions, const Options& options, + 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(StrCat( + "PiecewiseBezierPath: segment ", i, " has ", + segment.control_points.rows(), " rows but the trajectory declares ", + num_positions, + " generalized positions; every segment must be valued in the same " + "position space.")); + } + if (segment.control_points.cols() < 1) { + throw std::runtime_error(StrCat("PiecewiseBezierPath: segment ", i, + " has no control points.")); + } + if (!(segment.t_end >= segment.t_start)) { + throw std::runtime_error(StrCat( + "PiecewiseBezierPath: segment ", i, " spans [", segment.t_start, ", ", + segment.t_end, "], which runs backwards in time.")); + } + 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( + StrCat("PiecewiseBezierPath: segments are not contiguous in time — " + "segment ", + i - 1, " ends at ", previous_end, " but segment ", i, + " starts at ", segment.t_start, + ". Segments must be ordered and meet end-to-start.")); + } + } + } + + std::vector is_continuous_revolute(num_positions, false); + for (int index : options.continuous_revolute_indices) { + if (index < 0 || index >= num_positions) { + throw std::runtime_error(StrCat( + "PiecewiseBezierPath: Options::continuous_revolute_indices contains ", + index, ", which is out of range for a trajectory with ", + num_positions, " generalized 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 (trajectory normalization). + 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 -= kTwoPi * std::round(gap / kTwoPi); + } + if (std::abs(gap) > options.continuity_tolerance) { + throw std::runtime_error(StrCat( + "PiecewiseBezierPath: C0 discontinuity at the junction between " + "segments ", + i - 1, " and ", i, " in coordinate ", c, ": the gap is ", raw_gap, + (is_continuous_revolute[c] ? " (" : ""), + (is_continuous_revolute[c] ? StrCat(gap, " modulo 2π)") + : std::string()), + ", which exceeds Options::continuity_tolerance = ", + options.continuity_tolerance, + ". A discontinuous trajectory teleports; per-segment certificates " + "would not cover the jump. If coordinate ", + c, + " is a continuous revolute joint, list it in " + "Options::continuous_revolute_indices.")); + } + } + } +} + +} // namespace + +PiecewiseBezierPath PiecewiseBezierPath::FromTrajectory( + const Trajectory& trajectory, const Options& options) { + if (trajectory.cols() != 1) { + throw std::runtime_error(StrCat( + "PiecewiseBezierPath::FromTrajectory: the trajectory is ", + trajectory.rows(), "x", trajectory.cols(), + "-valued; only column-vector-valued trajectories (cols() == 1) over " + "the plant's generalized positions are supported.")); + } + 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, options, &source_index, &path.segments_); + ValidateSegments(num_positions, options, path.segments_); + path.FinalizeMetadata(options.continuity_tolerance); + return path; +} + +PiecewiseBezierPath PiecewiseBezierPath::FromWaypoints( + const Eigen::MatrixXd& waypoints, const Options& options) { + 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(StrCat( + "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}} (trajectory normalization, item 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, options, path.segments_); + path.FinalizeMetadata(options.continuity_tolerance); + return path; +} + +void PiecewiseBezierPath::FinalizeMetadata(double continuity_tolerance) { + 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 (trajectory normalization; the joint-support scope). + constant_coordinates_.assign(n, false); + for (int i = 0; i < n; ++i) { + constant_coordinates_[i] = + (global_upper_[i] - global_lower_[i]) <= continuity_tolerance; + } +} + +Eigen::VectorXd PiecewiseBezierPath::Value(double t) const { + DRAKE_THROW_UNLESS(!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(StrCat("PiecewiseBezierPath::Value: time ", t, + " is outside the path's domain [", 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, matching + // drake::trajectories::PiecewiseTrajectory::get_segment_index(). The choice + // is observable only when a junction carries a legitimate 2πk offset in a + // continuous-revolute coordinate, where the two sides are different + // representatives of the same configuration (trajectory normalization). + int low = 0; + int high = static_cast(segments_.size()) - 1; + while (low < high) { + const int mid = low + (high - low + 1) / 2; + if (segments_[mid].t_start <= clamped) { + low = mid; + } else { + high = mid - 1; + } + } + const BezierSegment& segment = segments_[low]; + const double duration = segment.t_end - segment.t_start; + const double s = + (duration > 0.0) ? (clamped - segment.t_start) / duration : 0.0; + return EvaluateSegment(low, 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(StrCat( + "PiecewiseBezierPath::EvaluateSegment: segment index ", segment_index, + " is out of range; the path has ", segments_.size(), " segments.")); + } + if (!(s >= -kParameterSlack) || !(s <= 1.0 + kParameterSlack)) { + throw std::runtime_error( + StrCat("PiecewiseBezierPath::EvaluateSegment: parameter s = ", s, + " is outside the segment's domain [0, 1].")); + } + 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 (trajectory normalization; the search + // algorithm). + *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 certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/piecewise_bezier_path.h b/planning/certified_ccd/piecewise_bezier_path.h new file mode 100644 index 000000000000..aabdb7cfe920 --- /dev/null +++ b/planning/certified_ccd/piecewise_bezier_path.h @@ -0,0 +1,99 @@ +#pragma once + +#include + +#include + +#include "drake/common/trajectories/trajectory.h" +#include "drake/planning/certified_ccd/options.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1] (trajectory + * normalization). */ +struct BezierSegment { + /** Original time interval (bookkeeping only; the certificate 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 (trajectory normalization). + +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: + /** 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 + options.max_conversion_degree, or junction discontinuity beyond + options.continuity_tolerance (modulo 2π for coordinates in + options.continuous_revolute_indices). */ + static PiecewiseBezierPath FromTrajectory( + const drake::trajectories::Trajectory& trajectory, + const Options& options); + + /** Normalizes an n × K waypoint matrix into K−1 order-1 segments (exact). + Segment k spans time [k, k+1]. @throws std::exception if K < 2. */ + static PiecewiseBezierPath FromWaypoints(const Eigen::MatrixXd& waypoints, + const Options& options); + + 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 (trajectory + normalization); 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 the continuity + tolerance) across all control points of all segments; such coordinates are + treated as welded for the check (trajectory normalization; the joint-support + scope). */ + const std::vector& constant_coordinates() const { + return constant_coordinates_; + } + + /** Evaluates the path at time t (for tests and breakpoint checks; the hot + loop never calls this — it uses de Casteljau apexes). */ + Eigen::VectorXd Value(double t) const; + + /** Evaluates segment `segment_index` at local parameter s ∈ [0, 1]. */ + Eigen::VectorXd EvaluateSegment(int segment_index, double s) const; + + private: + PiecewiseBezierPath() = default; + void FinalizeMetadata(double continuity_tolerance); + + 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 certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/piecewise_bezier_path_test.cc b/planning/certified_ccd/test/piecewise_bezier_path_test.cc new file mode 100644 index 000000000000..53db2150756b --- /dev/null +++ b/planning/certified_ccd/test/piecewise_bezier_path_test.cc @@ -0,0 +1,1135 @@ +/* T1 — curve module acceptance tests (the test plan, T1). + +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/certified_ccd/piecewise_bezier_path.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drake/common/copyable_unique_ptr.h" +#include "drake/common/polynomial.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" + +namespace drake { +namespace planning { +namespace certified_ccd { +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; +using ::testing::HasSubstr; + +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); +} + +void ExpectThrowsWith(const std::function& statement, + const std::string& substring) { + try { + statement(); + ADD_FAILURE() << "Expected an exception whose message contains \"" + << substring << "\", but nothing was thrown."; + } catch (const std::exception& e) { + EXPECT_THAT(std::string(e.what()), HasSubstr(substring)); + } +} + +/* 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. +// -------------------------------------------------------------------------- + +/* 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, Options{}); + 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; + } + } + } +} + +// -------------------------------------------------------------------------- +// de Casteljau subdivision. +// -------------------------------------------------------------------------- + +/* 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 (trajectory normalization). */ +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). + worst = std::max( + worst, (mid - DrakeBezierValue(parent, 0.5)).cwiseAbs().maxCoeff()); + // Children share the endpoints they must. + 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 reallocate them (the performance requirements, P1). */ +GTEST_TEST(DeCasteljau, PreSizedOutputsAreNotReallocated) { + 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); + const double* left_data = left.data(); + const double* right_data = right.data(); + const double* mid_data = mid.data(); + + DeCasteljauSplitAtHalf(parent, &left, &right, &mid); + EXPECT_EQ(left.data(), left_data); + EXPECT_EQ(right.data(), right_data); + EXPECT_EQ(mid.data(), mid_data); + + // Splitting a child in place into the same buffers is the recursion the + // certifier runs; it must also be stable. + const Eigen::MatrixXd child = left; + DeCasteljauSplitAtHalf(child, &left, &right, &mid); + EXPECT_EQ(left.data(), left_data); + EXPECT_EQ(right.data(), right_data); + EXPECT_EQ(mid.data(), mid_data); +} + +/* 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, and the segments must tile the domain (trajectory +normalization; the test plan). */ +void CheckBsplineEquivalence(const BsplineTrajectory& bspline) { + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(bspline, Options{}); + 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()); + } + constexpr int kNumSamples = 10001; + EXPECT_LT(MaxSampledError(path, bspline, kNumSamples), 1e-10); +} + +GTEST_TEST(BsplineConversion, ClampedUniformOrders2To6) { + std::mt19937_64 generator(4242); + for (int order = 2; order <= 6; ++order) { + const int num_basis_functions = order + 4; + const BsplineBasis basis(order, num_basis_functions, + KnotVectorType::kClampedUniform, 0.0, 3.0); + const BsplineTrajectory bspline = + MakeBsplineFromBasis(basis, 3, &generator); + SCOPED_TRACE("order " + std::to_string(order)); + CheckBsplineEquivalence(bspline); + + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(bspline, Options{}); + // A clamped uniform basis has num_basis_functions - order + 1 nonempty + // spans. + EXPECT_EQ(static_cast(path.segments().size()), + num_basis_functions - order + 1); + } +} + +GTEST_TEST(BsplineConversion, NonUniformKnots) { + std::mt19937_64 generator(515151); + for (int order = 2; order <= 6; ++order) { + // Clamped, but with irregular interior spacing. + std::vector knots; + for (int i = 0; i < order; ++i) { + knots.push_back(0.0); + } + for (double interior : {0.13, 0.29, 0.31, 1.70, 2.55}) { + knots.push_back(interior); + } + for (int i = 0; i < order; ++i) { + knots.push_back(3.0); + } + const BsplineBasis basis(order, knots); + const BsplineTrajectory bspline = + MakeBsplineFromBasis(basis, 4, &generator); + SCOPED_TRACE("order " + std::to_string(order)); + CheckBsplineEquivalence(bspline); + EXPECT_EQ( + static_cast(PiecewiseBezierPath::FromTrajectory(bspline, Options{}) + .segments() + .size()), + 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). + 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}; + const BsplineBasis basis(4, knots); + const BsplineTrajectory bspline = + MakeBsplineFromBasis(basis, 2, &generator); + CheckBsplineEquivalence(bspline); + // Nonempty spans: [0,1], [1,2], [2,3], [3,3.5], [3.5,4]. + EXPECT_EQ( + static_cast(PiecewiseBezierPath::FromTrajectory(bspline, Options{}) + .segments() + .size()), + 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); + const BsplineBasis basis(4, 10, KnotVectorType::kClampedUniform, 0.0, + 5.0); + const BsplineTrajectory bspline = + MakeBsplineFromBasis(basis, 7, &generator); + CheckBsplineEquivalence(bspline); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(bspline, Options{}); + EXPECT_EQ(static_cast(path.segments().size()), 7); + EXPECT_EQ(path.num_positions(), 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) { + const BsplineBasis basis(order, order + 5, KnotVectorType::kUniform, + 0.0, 2.0); + const BsplineTrajectory bspline = + MakeBsplineFromBasis(basis, 3, &generator); + SCOPED_TRACE("order " + std::to_string(order)); + CheckBsplineEquivalence(bspline); + } +} + +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 BsplineBasis basis(3, knots); + const BsplineTrajectory bspline = + MakeBsplineFromBasis(basis, 2, &generator); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(bspline, Options{}); + 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); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(bspline, Options{}); + }, + "column-vector-valued"); +} + +// -------------------------------------------------------------------------- +// 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, Options{}); + ASSERT_EQ(path.segments().size(), 5u); + for (const BezierSegment& segment : path.segments()) { + // A first-order hold is exactly an order-1 Bézier per segment. + EXPECT_EQ(segment.control_points.cols(), 2); + } + // Order-1 Bézier control points are the waypoints themselves. + for (int k = 0; k < 5; ++k) { + 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, Options{}); + 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, Options{}); + EXPECT_LT(MaxSampledError(path_b, shape_preserving, 10001), 1e-10); +} + +/* A single high-degree polynomial segment, up to the default degree cap. */ +GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCap) { + const Options options; + ASSERT_EQ(options.max_conversion_degree, 10); + for (int degree = 1; degree <= options.max_conversion_degree; ++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 deliberately 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); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(pp, options); + ASSERT_EQ(path.segments().size(), 1u); + EXPECT_EQ(path.segments()[0].control_points.cols(), degree + 1); + EXPECT_LT(MaxSampledError(path, pp, 10001), 1e-10) << "degree " << degree; + } +} + +GTEST_TEST(PiecewisePolynomialConversion, DegreeAboveCapThrows) { + const int degree = 11; + const int num_points = degree + 1; + Eigen::VectorXd times(num_points); + Eigen::MatrixXd samples(1, num_points); + for (int i = 0; i < num_points; ++i) { + times[i] = 0.3 + 1.7 * static_cast(i) / degree; + samples(0, i) = std::sin(2.0 * times[i]); + } + const PiecewisePolynomial pp = + PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, + samples); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(pp, Options{}); + }, + "max_conversion_degree"); + + // Raising the cap deliberately makes it work. + Options relaxed; + relaxed.max_conversion_degree = degree; + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(pp, relaxed); + 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); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(pp, Options{}); + }, + "column-vector-valued"); +} + +// -------------------------------------------------------------------------- +// 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; + const CompositeTrajectory trajectory = + MakeJunctionCase(offset, &generator); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + }, + "C0 discontinuity"); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + }, + "coordinate 1"); + + // A gap just under the tolerance is accepted. + Eigen::VectorXd tiny = Eigen::VectorXd::Zero(3); + tiny[2] = 9e-8; + const CompositeTrajectory ok = MakeJunctionCase(tiny, &generator); + EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory(ok, Options{})); +} + +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); + + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + }, + "C0 discontinuity"); + + // Declaring the *wrong* coordinate does not help. + Options wrong; + wrong.continuous_revolute_indices = {0, 2}; + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(trajectory, wrong); + }, + "C0 discontinuity"); + + Options right; + right.continuous_revolute_indices = {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; + const CompositeTrajectory big = + MakeJunctionCase(big_offset, &generator); + EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory(big, right)); +} + +GTEST_TEST(JunctionValidation, NonMultipleOfTwoPiOffsetThrowsEvenWhenRevolute) { + std::mt19937_64 generator(2468); + Eigen::VectorXd offset = Eigen::VectorXd::Zero(2); + offset[0] = kTwoPi + 1e-3; + const CompositeTrajectory trajectory = + MakeJunctionCase(offset, &generator); + Options options; + options.continuous_revolute_indices = {0, 1}; + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(trajectory, options); + }, + "C0 discontinuity"); +} + +/* 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 (trajectory +normalization). */ +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); + Options options; + options.continuous_revolute_indices = {0}; + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, options); + 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 waypoints(2, 3); + waypoints << 0.0, 1.0, 2.0, 0.0, 0.0, 0.0; + Options options; + options.continuous_revolute_indices = {2}; + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromWaypoints(waypoints, options); + }, + "continuous_revolute_indices"); +} + +/* 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; + const PiecewisePolynomial pp = + PiecewisePolynomial::ZeroOrderHold(times, samples); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(pp, Options{}); + }, + "C0 discontinuity"); +} + +// -------------------------------------------------------------------------- +// 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, Options{}); + + 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, Options{}); + 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]); + + // A looser tolerance sweeps coordinate 3 in as well. + Options loose; + loose.continuity_tolerance = 1e-5; + const PiecewiseBezierPath loose_path = + PiecewiseBezierPath::FromWaypoints(waypoints, loose); + EXPECT_TRUE(loose_path.constant_coordinates()[3]); + EXPECT_FALSE(loose_path.constant_coordinates()[0]); +} + +// -------------------------------------------------------------------------- +// 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, Options{}); + 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, Options{}); + 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 (trajectory normalization, item 3). */ +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, Options{}); + // 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)); + + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + }, + "segment index 1"); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + }, + "UnsupportedTrajectory"); + + // At the top level the offending segment index is 0. + const UnsupportedTrajectory bare(num_positions, 0.0, 1.0); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromTrajectory(bare, Options{}); + }, + "segment index 0"); +} + +// -------------------------------------------------------------------------- +// Waypoints. +// -------------------------------------------------------------------------- + +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, Options{}); + + 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 k = 0; k + 1 < num_waypoints; ++k) { + 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) { + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd::Zero(3, 1), + Options{}); + }, + "at least 2 waypoints"); + ExpectThrowsWith( + [&]() { + PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd(0, 4), Options{}); + }, + "zero rows"); +} + +// -------------------------------------------------------------------------- +// Evaluation domain handling. +// -------------------------------------------------------------------------- + +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, Options{}); + + 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(-1e-13)); + EXPECT_NO_THROW(path.Value(2.0 + 1e-13)); + EXPECT_TRUE(path.Value(-1e-13).isApprox(waypoints.col(0), 0.0)); + + ExpectThrowsWith( + [&]() { + path.Value(-1e-3); + }, + "outside the path's domain"); + ExpectThrowsWith( + [&]() { + path.Value(2.5); + }, + "outside the path's domain"); + ExpectThrowsWith( + [&]() { + path.EvaluateSegment(0, 1.5); + }, + "outside the segment's domain"); + ExpectThrowsWith( + [&]() { + path.EvaluateSegment(0, -0.5); + }, + "outside the segment's domain"); + EXPECT_NO_THROW(path.EvaluateSegment(0, 1.0 + 1e-13)); +} + +GTEST_TEST(Evaluation, SegmentIndexOutOfRangeThrows) { + Eigen::MatrixXd waypoints(2, 3); + waypoints << 0.0, 1.0, 3.0, -1.0, 0.0, 1.0; + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromWaypoints(waypoints, Options{}); + ExpectThrowsWith( + [&]() { + path.EvaluateSegment(2, 0.5); + }, + "out of range"); + ExpectThrowsWith( + [&]() { + path.EvaluateSegment(-1, 0.5); + }, + "out of range"); +} + +/* 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, Options{}); + 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(1.0)[0], 1.0); + EXPECT_EQ(path.Value(2.0)[0], 3.0); + EXPECT_EQ(path.Value(3.0)[0], 6.0); + EXPECT_EQ(path.Value(0.0)[0], 0.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 be consistent 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, Options{}); + 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 certified_ccd +} // namespace planning +} // namespace drake diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl index e4ba8f5ca3bf..6acfabea0880 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/certified_ccd", "//planning/experimental", "//planning/graph_algorithms", "//planning/iris", From 0268bbbe9fd242aafe6fc995879aa9fed9c4e433 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 14:39:55 -0400 Subject: [PATCH 02/22] [planning] Add certified_ccd: kinematic motion bounds Adds ComputeBoundingSphere (a body-frame sphere containing a proximity geometry, over the closed set of supported shapes) and KinematicsEngine, the construction-time analysis that decides which joints can move a geometry pair and derives the per-pair motion-bound coefficients the displacement lemma consumes. Unsupported joint types, rotating half spaces and unbounded-reach models are refused by name at construction time rather than silently mis-bounded. --- planning/certified_ccd/BUILD.bazel | 67 + planning/certified_ccd/bounding_sphere.cc | 177 +++ planning/certified_ccd/bounding_sphere.h | 50 + planning/certified_ccd/motion_bound_table.cc | 719 ++++++++++ planning/certified_ccd/motion_bound_table.h | 246 ++++ .../test/bounding_sphere_test.cc | 425 ++++++ .../certified_ccd/test/motion_bound_test.cc | 1228 +++++++++++++++++ 7 files changed, 2912 insertions(+) create mode 100644 planning/certified_ccd/bounding_sphere.cc create mode 100644 planning/certified_ccd/bounding_sphere.h create mode 100644 planning/certified_ccd/motion_bound_table.cc create mode 100644 planning/certified_ccd/motion_bound_table.h create mode 100644 planning/certified_ccd/test/bounding_sphere_test.cc create mode 100644 planning/certified_ccd/test/motion_bound_test.cc diff --git a/planning/certified_ccd/BUILD.bazel b/planning/certified_ccd/BUILD.bazel index 58d99f9e01d5..d504286f0ee0 100644 --- a/planning/certified_ccd/BUILD.bazel +++ b/planning/certified_ccd/BUILD.bazel @@ -12,6 +12,8 @@ drake_cc_package_library( name = "certified_ccd", visibility = ["//visibility:public"], deps = [ + ":bounding_sphere", + ":motion_bound_table", ":numerics", ":options", ":piecewise_bezier_path", @@ -54,6 +56,43 @@ drake_cc_library( ], ) +drake_cc_library( + name = "bounding_sphere", + srcs = ["bounding_sphere.cc"], + hdrs = ["bounding_sphere.h"], + deps = [ + "//geometry:shape_specification", + "//math:geometric_transform", + "@eigen", + ], + implementation_deps = [ + "//common:essential", + "//geometry/proximity:polygon_surface_mesh", + "@fmt", + ], +) + +drake_cc_library( + name = "motion_bound_table", + srcs = ["motion_bound_table.cc"], + hdrs = ["motion_bound_table.h"], + deps = [ + ":bounding_sphere", + ":options", + ":piecewise_bezier_path", + "//planning:robot_diagram", + "@eigen", + ], + implementation_deps = [ + "//common:essential", + "//geometry:geometry_roles", + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//multibody/tree", + "@fmt", + ], +) + # === test/ === # T1 — curve module acceptance tests. @@ -71,4 +110,32 @@ drake_cc_googletest( ], ) +# T2 — the displacement lemma, the lambda table and the J(p) subtree logic. +drake_cc_googletest( + name = "motion_bound_test", + deps = [ + ":motion_bound_table", + "//geometry:geometry_roles", + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/plant", + "//multibody/tree", + "//planning:robot_diagram_builder", + ], +) + +# T2 — the bounding-sphere radius property test. +drake_cc_googletest( + name = "bounding_sphere_test", + deps = [ + ":bounding_sphere", + "//common:essential", + "//common:temp_directory", + "//geometry:shape_specification", + "//geometry/proximity:polygon_surface_mesh", + "//math:geometric_transform", + ], +) + add_lint_tests() diff --git a/planning/certified_ccd/bounding_sphere.cc b/planning/certified_ccd/bounding_sphere.cc new file mode 100644 index 000000000000..a831250361ea --- /dev/null +++ b/planning/certified_ccd/bounding_sphere.cc @@ -0,0 +1,177 @@ +#include "drake/planning/certified_ccd/bounding_sphere.h" + +#include +#include +#include +#include + +#include + +#include "drake/common/drake_throw.h" +#include "drake/geometry/proximity/polygon_surface_mesh.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +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::Shape; +using drake::geometry::ShapeReifier; +using drake::geometry::Sphere; +using drake::math::RigidTransform; + +/* 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 (a + sound relaxation of the geometry-support scope's exact per-shape R_g, by the + triangle inequality); the tighter centre is what the broadphase prefilter + wants. + + λ soundness dies quietly if any formula under-bounds, 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 (the geometry-support scope). */ +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 (the geometry-support + // scope). 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. (The same rim argument in the geometry-support + // scope's origin-centred form picks 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 (the geometry-support scope). + SetFromHull(mesh.GetConvexHull()); + } + + private: + void ThrowUnsupportedGeometry(const std::string& shape_name) final { + throw std::runtime_error(fmt::format( + "certified_ccd: 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 the dedicated rules in the " + "geometry-support scope " + "(anchored, or translation-only relative motion 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_THROW_UNLESS(std::isfinite(radius_about_Go)); + DRAKE_THROW_UNLESS(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_THROW_UNLESS(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 silently-zero or non-finite radius is the exact failure mode the + // geometry-support scope warns about, so re-assert the postcondition every + // caller relies on. + DRAKE_THROW_UNLESS(std::isfinite(result.radius) && result.radius >= 0.0); + DRAKE_THROW_UNLESS(result.center_L.allFinite()); + return result; +} + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/bounding_sphere.h b/planning/certified_ccd/bounding_sphere.h new file mode 100644 index 000000000000..a38f5a26394a --- /dev/null +++ b/planning/certified_ccd/bounding_sphere.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** A sphere, expressed in the owning body (link) frame L, that contains a +proximity geometry at every configuration of the body. */ +struct BoundingSphere { + /** Sphere center in the body frame. */ + 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 geometry-support scope). + +The sphere is centered at the shape's natural center (tighter for the +broadphase prefilter than the white paper's origin-centered radius R_g; 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. + +λ soundness dies quietly if any formula under-bounds, so this function +switches on the closed set of supported shape types and +@throws std::exception on anything else (HalfSpace included — halfspaces are +handled by dedicated rules, never through a bounding sphere). */ +BoundingSphere ComputeBoundingSphere( + const drake::geometry::Shape& shape, + const drake::math::RigidTransform& X_LG); + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/motion_bound_table.cc b/planning/certified_ccd/motion_bound_table.cc new file mode 100644 index 000000000000..7e60a5a10c0e --- /dev/null +++ b/planning/certified_ccd/motion_bound_table.cc @@ -0,0 +1,719 @@ +#include "drake/planning/certified_ccd/motion_bound_table.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_throw.h" +#include "drake/geometry/geometry_roles.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" + +namespace drake { +namespace planning { +namespace certified_ccd { + +using drake::geometry::GeometryId; +using drake::geometry::HalfSpace; +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 { + +constexpr double kTwoPi = 6.283185307179586476925286766559; + +bool IsHalfSpace(const Shape& shape) { + return dynamic_cast(&shape) != nullptr; +} + +} // namespace + +std::vector> MotionBoundTable::entries( + 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 drake::planning::RobotDiagram& model) + : model_(&model), plant_(&model.plant()) { + if (!plant_->is_finalized()) { + throw std::runtime_error( + "certified_ccd: 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; + + 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; + translation_known = true; + } else if (rec.type_name == "prismatic") { + rec.kind = JointKind::kPrismatic; + translation_known = true; + } else if (rec.type_name == "planar") { + rec.kind = JointKind::kPlanar; + translation_known = true; + } else if (rec.type_name == ScrewJoint::kTypeName) { + rec.kind = JointKind::kScrew; + rec.screw_pitch = + dynamic_cast&>(joint).screw_pitch(); + 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}; + 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}; + 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; + 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(). + rec.kind = JointKind::kUnsupported; + translation_known = false; + } + rec.translation_offsets_known = translation_known; + + // 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( + "certified_ccd: 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. Post + // Finalize() every non-world body has exactly one inboard joint + // (ephemeral floating joints included), so a breadth-first walk from the + // world over the (body, joint) graph recovers the tree exactly. + // ------------------------------------------------------------------ + 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( + "certified_ccd: 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( + "certified_ccd: joint '{}' ({}) closes a kinematic loop (both of its " + "bodies are already reachable from the world without it). Loop " + "topologies are not supported in v1.", + 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_THROW_UNLESS(++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_THROW_UNLESS(rec.num_positions == 0); + continue; + } + DRAKE_THROW_UNLESS(rec.num_positions > 0); + if (rec.outboard != joint.child_body().index()) { + throw std::runtime_error(fmt::format( + "certified_ccd: joint '{}' ({}) is reversed — its declared parent " + "body '{}' is outboard of its declared child body '{}' in the " + "multibody tree. Reversed mobilizers are a documented v1 exclusion " + "because 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( + "certified_ccd: the plant's kinematically-affected set for joint " + "'{}' ({}) disagrees with the world-rooted tree walk. This model's " + "topology is not supported in v1.", + 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_THROW_UNLESS(c >= 0 && c < num_positions_); + DRAKE_THROW_UNLESS(coord_joint_[c] == -1); + coord_joint_[c] = k; + } + } + for (int c = 0; c < num_positions_; ++c) { + DRAKE_THROW_UNLESS(coord_joint_[c] >= 0); + } +} + +void KinematicsEngine::BuildGeometry() { + const MultibodyPlant& plant = *plant_; + const auto& inspector = model_->scene_graph().model_inspector(); + + body_spheres_.assign(num_bodies_, {}); + body_sphere_geoms_.assign(num_bodies_, {}); + 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_THROW_UNLESS(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 the + // dedicated rule in CheckHalfSpaceRule() (the geometry-support scope) + // 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( + "certified_ccd: 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); + body_sphere_geoms_[b].push_back(gid); + body_spheres_[b].push_back(sphere); + 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( + "certified_ccd: 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 (the geometry-support scope). 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 (the displacement + // lemma). + 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_THROW_UNLESS(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 deliberately NOT + // included. + return r + joints_[k].p_CM_norm; + } + r += joints_[k].fixed_hop + box_hop[k]; + b = joints_[k].inboard; + } + throw std::runtime_error( + "certified_ccd: internal error — 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( + "certified_ccd: 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( + "certified_ccd: 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( + "certified_ccd: 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])); + }; + + // ------------------------------------------------------------------ + // 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 (the displacement lemma). + // ------------------------------------------------------------------ + 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) / kTwoPi; + break; + case JointKind::kUnsupported: { + for (int c = ps; c < ps + rec.num_positions; ++c) { + if (!constant_coordinates[c]) { + throw std::runtime_error(fmt::format( + "certified_ccd: this trajectory moves coordinate {} of joint " + "'{}', whose type '{}' is excluded in v1 (the joint-support " + "scope). 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. See " + "the white paper's future extensions for the " + "manifold-curve extension.", + c, rec.name, rec.type_name)); + } + } + if (!rec.translation_offsets_known) { + throw std::runtime_error(fmt::format( + "certified_ccd: 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_THROW_UNLESS(std::isfinite(box_hop[k]) && box_hop[k] >= 0.0); + } + + // ------------------------------------------------------------------ + // 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 (the + // soundness argument), so no penetration-depth regularity is needed. + // ------------------------------------------------------------------ + MotionBoundTable table; + std::vector& row_start = table.mutable_row_start(); + std::vector& coord = table.mutable_coord(); + std::vector& lambda = table.mutable_lambda(); + row_start.clear(); + row_start.reserve(pairs.size() + 1); + row_start.push_back(0); + + // 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 PairId& 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( + "certified_ccd: pair references body indices ({}, {}) outside the " + "plant's {} bodies.", + static_cast(a), static_cast(b), num_bodies_)); + } + 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(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( + "certified_ccd: HalfSpace geometry '{}' on body '{}' is the " + "distal side of joint '{}' ({}), which rotates it. A half " + "space has unbounded reach, so no finite λ exists (the " + "geometry-support scope).", + 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]) continue; // Joint-support carve-out. + 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) / kTwoPi; + break; + case JointKind::kWeld: + case JointKind::kUnsupported: + throw std::runtime_error(fmt::format( + "certified_ccd: internal error — joint '{}' ({}) reached the " + "λ assembly with an unsupported kind.", + rec.name, rec.type_name)); + } + DRAKE_THROW_UNLESS(std::isfinite(lam) && lam >= 0.0); + coord.push_back(c); + lambda.push_back(lam); + } + } + row_start.push_back(static_cast(coord.size())); + } + return table; +} + +const std::vector& KinematicsEngine::body_spheres( + BodyIndex body) const { + DRAKE_THROW_UNLESS(body.is_valid() && body < num_bodies_); + return body_spheres_[body]; +} + +const std::vector& KinematicsEngine::body_sphere_geometries( + BodyIndex body) const { + DRAKE_THROW_UNLESS(body.is_valid() && body < num_bodies_); + return body_sphere_geoms_[body]; +} + +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( + "certified_ccd: 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 certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/motion_bound_table.h b/planning/certified_ccd/motion_bound_table.h new file mode 100644 index 000000000000..675d86e253d0 --- /dev/null +++ b/planning/certified_ccd/motion_bound_table.h @@ -0,0 +1,246 @@ +#pragma once + +// NOTE(interface): This header is owned by the kinematics module. The class +// and file names and the documented semantics are fixed; internal details +// (private members, helper structs) may be refined by the implementation. + +#include +#include +#include +#include + +#include + +#include "drake/planning/certified_ccd/bounding_sphere.h" +#include "drake/planning/certified_ccd/options.h" +#include "drake/planning/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** Per-pair motion-bound coefficients in CSR layout (the displacement lemma): +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. λ has +units of meters of worst-case point 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. */ +class MotionBoundTable { + public: + int num_pairs() const { return static_cast(row_start_.size()) - 1; } + + /** True iff J(p) is empty after the constant-coordinate carve-out: the + trajectory cannot change this pair's status, so it is checked once. */ + bool pair_is_static(int pair_index) const { + return row_start_[pair_index] == row_start_[pair_index + 1]; + } + + /** Δ_p(ν) = Σ_{j ∈ J(p)} λ(j,p) · w_j — a sparse dot product against the + node's per-coordinate deviations w (the interval certificate, requirement P3). +*/ + double MotionBound(int pair_index, const Eigen::VectorXd& w) const { + double delta = 0.0; + for (int e = row_start_[pair_index]; e < row_start_[pair_index + 1]; ++e) { + delta += lambda_[e] * w[coord_[e]]; + } + return delta; + } + + /** Introspection for tests: the (coordinate, λ) entries of one pair, + ordered by increasing coordinate index. */ + std::vector> entries(int pair_index) const; + + /** Total number of (coordinate, λ) entries over all pairs. */ + int num_entries() const { return static_cast(coord_.size()); } + + /** Builder access (kinematics module internals only). */ + std::vector& mutable_row_start() { return row_start_; } + std::vector& mutable_coord() { return coord_; } + std::vector& mutable_lambda() { return lambda_; } + + private: + std::vector row_start_{0}; + std::vector coord_; + std::vector lambda_; +}; + +/** Construction-time kinematic analysis of a plant (the displacement lemma): +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.body_spheres(b) for the prefilter; +- once per Check* call: engine.ComputeMotionBoundTable(path, pairs); +- once per node, per pair: table.MotionBound(pair_index, w). */ +class KinematicsEngine { + public: + /** 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 (constant-coordinate carve-out, the + joint-support scope). + + `model` is aliased and must outlive this object. + + @throws std::exception if a HalfSpace geometry is on the *distal* side of a + rotational coordinate relative to an unfiltered partner (unbounded reach). + A HalfSpace that is merely the static partner of a rotating body — the + anchored ground plane under a robot arm, the overwhelmingly common case — is + accepted: λ then bounds the partner's points, and signed distance is + symmetric, so the certificate still holds. + @throws std::exception if the plant is not finalized, if a joint is + "reversed" (its declared parent body is outboard of its declared child body + in the multibody tree — a documented v1 exclusion), or if any proximity + geometry has a shape ComputeBoundingSphere() rejects. */ + explicit KinematicsEngine(const drake::planning::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( + drake::multibody::BodyIndex body_a, + drake::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; the displacement lemma). Coordinates flagged constant + by the path are removed from every J(p). + @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(). + @throws std::exception on a size mismatch, an empty box (lower > upper), a + non-finite bound, a moving coordinate of an unsupported joint type, or a + pair whose distal side carries a HalfSpace across a rotational coordinate. */ + MotionBoundTable ComputeMotionBoundTable( + const Eigen::VectorXd& lower, const Eigen::VectorXd& upper, + const std::vector& constant_coordinates, + const std::vector& pairs) const; + + /** Bounding spheres (body frame) of every proximity geometry of `body`, + used by the reach chain start and by the certifier's sphere prefilter. + HalfSpace geometries have no bounding sphere and are omitted. */ + const std::vector& body_spheres( + drake::multibody::BodyIndex body) const; + + /** The geometry ids matching body_spheres(body), element for element. */ + const std::vector& body_sphere_geometries( + drake::multibody::BodyIndex body) 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(drake::geometry::GeometryId id) const; + + /** True iff `body` carries at least one HalfSpace proximity geometry. */ + bool body_has_halfspace(drake::multibody::BodyIndex body) const; + + /** Radius, about the body frame origin, of a sphere containing every + proximity geometry of `body` — the start of the reach chain. Zero for a + body with no (non-HalfSpace) proximity geometry. */ + double body_radius(drake::multibody::BodyIndex body) const; + + int num_positions() const { return num_positions_; } + + const drake::multibody::MultibodyPlant& plant() const { + return *plant_; + } + + private: + /* The λ rule a joint's coordinates follow (the displacement lemma; the + * joint-support scope). */ + 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. + }; + + /* One tree edge, oriented from its outboard body toward the world. */ + struct JointRecord { + drake::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). */ + drake::multibody::BodyIndex inboard; + drake::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}; + /* 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(drake::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, drake::multibody::BodyIndex body, + const std::vector& box_hop) const; + + void BuildTopology(); + void BuildGeometry(); + void CheckHalfSpaceRule() const; + + const drake::planning::RobotDiagram* model_{}; + const drake::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_spheres_; + std::vector> body_sphere_geoms_; + std::vector body_radius_; + std::vector body_has_halfspace_; + std::vector body_halfspace_name_; + std::unordered_map + geometry_spheres_; +}; + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/bounding_sphere_test.cc b/planning/certified_ccd/test/bounding_sphere_test.cc new file mode 100644 index 000000000000..e7f4559d8b54 --- /dev/null +++ b/planning/certified_ccd/test/bounding_sphere_test.cc @@ -0,0 +1,425 @@ +/* T2 (the test plan) — the bounding-sphere radius property test. + * + * For every supported shape class, at many random poses X_LG, every sampled + * surface point must lie inside the reported sphere. A shape that silently + * picks up another shape's radius formula is a *silent* λ soundness bug, so + * this test is deliberately exhaustive over the closed set of supported shapes + * and also pins the throw-on-unsupported behaviour. Never loosen the tolerance + * to make a case pass (the implementation notes, item 2). */ + +#include "drake/planning/certified_ccd/bounding_sphere.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drake/common/fmt_eigen.h" +#include "drake/common/temp_directory.h" +#include "drake/geometry/proximity/polygon_surface_mesh.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/rotation_matrix.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +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 drake::math::RotationMatrix; +using Eigen::Vector3d; + +constexpr int kNumPoses = 100; +constexpr int kNumSurfaceSamples = 1000; +/* The containment claim is exact mathematics; this only absorbs the rounding + of re-evaluating it. Note the slack is taken relative to the *origin-centred* + radius R_g = ‖c_L‖ + ρ, exactly as the test plan's T2 states the property: 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; + +using Rng = std::mt19937_64; + +double Uniform(Rng* rng, double lo, double hi) { + return std::uniform_real_distribution(lo, hi)(*rng); +} + +Vector3d RandomUnitVector(Rng* rng) { + std::normal_distribution normal(0.0, 1.0); + Vector3d v; + do { + v = Vector3d(normal(*rng), normal(*rng), normal(*rng)); + } while (v.norm() < 1e-9); + return v.normalized(); +} + +RotationMatrix RandomRotation(Rng* rng) { + std::normal_distribution normal(0.0, 1.0); + Eigen::Quaterniond q; + do { + q = Eigen::Quaterniond(normal(*rng), normal(*rng), normal(*rng), + normal(*rng)); + } while (q.norm() < 1e-9); + q.normalize(); + return RotationMatrix(q); +} + +RigidTransform RandomTransform(Rng* rng, double translation_scale) { + return RigidTransform( + RandomRotation(rng), + Vector3d(Uniform(rng, -translation_scale, translation_scale), + Uniform(rng, -translation_scale, translation_scale), + Uniform(rng, -translation_scale, translation_scale))); +} + +/* Samples a point on the surface of the shape, expressed in its canonical + geometry frame G. */ +using Sampler = std::function; + +Sampler SphereSampler(double r) { + return [r](Rng* rng) -> Vector3d { + // The explicit return type materializes the Eigen product before the + // lambda returns; without it the deduced type is an expression template + // referencing the RandomUnitVector temporary, which dangles once the + // std::function wrapper converts the result. + return r * RandomUnitVector(rng); + }; +} + +Sampler BoxSampler(double w, double d, double h) { + const Vector3d half(0.5 * w, 0.5 * d, 0.5 * h); + return [half](Rng* rng) { + const int axis = std::uniform_int_distribution(0, 2)(*rng); + const double sign = + std::uniform_int_distribution(0, 1)(*rng) == 0 ? -1.0 : 1.0; + Vector3d p(Uniform(rng, -half.x(), half.x()), + Uniform(rng, -half.y(), half.y()), + Uniform(rng, -half.z(), half.z())); + p(axis) = sign * half(axis); + return p; + }; +} + +Sampler CapsuleSampler(double r, double length) { + const double half = 0.5 * length; + return [r, half](Rng* rng) { + // Total area is split between the cylindrical barrel and the two caps; + // exact area weighting is irrelevant here — every region must be sampled. + if (std::uniform_int_distribution(0, 1)(*rng) == 0) { + const double phi = Uniform(rng, 0.0, 2.0 * M_PI); + return Vector3d(r * std::cos(phi), r * std::sin(phi), + Uniform(rng, -half, half)); + } + const Vector3d u = RandomUnitVector(rng); + const double z_center = u.z() >= 0.0 ? half : -half; + return Vector3d(r * u.x(), r * u.y(), z_center + r * u.z()); + }; +} + +Sampler CylinderSampler(double r, double length) { + const double half = 0.5 * length; + return [r, half](Rng* rng) { + const double phi = Uniform(rng, 0.0, 2.0 * M_PI); + if (std::uniform_int_distribution(0, 1)(*rng) == 0) { + return Vector3d(r * std::cos(phi), r * std::sin(phi), + Uniform(rng, -half, half)); + } + // Cap disk: sqrt keeps the sample uniform in area, and hits the rim. + const double rho = r * std::sqrt(Uniform(rng, 0.0, 1.0)); + const double z = + std::uniform_int_distribution(0, 1)(*rng) == 0 ? -half : half; + return Vector3d(rho * std::cos(phi), rho * std::sin(phi), z); + }; +} + +Sampler EllipsoidSampler(double a, double b, double c) { + return [a, b, c](Rng* rng) { + const Vector3d u = RandomUnitVector(rng); + return Vector3d(a * u.x(), b * u.y(), c * u.z()); + }; +} + +/* 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 exactly the claim the + geometry-support scope makes. */ +Sampler HullVertexSampler( + const drake::geometry::PolygonSurfaceMesh& hull) { + return [&hull](Rng* rng) { + const int v = + std::uniform_int_distribution(0, hull.num_vertices() - 1)(*rng); + 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 = 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)"; + } + } +} + +GTEST_TEST(BoundingSphereTest, SphereContainsSurface) { + Rng rng(0x5eed0001); + for (double r : {1e-4, 0.05, 1.0, 7.5}) { + const Sphere shape(r); + CheckContainment(shape, SphereSampler(r), fmt::format("Sphere({})", r), 2.0, + &rng); + } +} + +GTEST_TEST(BoundingSphereTest, BoxContainsSurface) { + Rng rng(0x5eed0002); + const std::vector sizes{ + {1.0, 1.0, 1.0}, {0.01, 2.0, 0.3}, {5.0, 0.002, 0.002}, {0.4, 0.7, 1.9}}; + for (const Vector3d& s : sizes) { + const Box shape(s.x(), s.y(), s.z()); + CheckContainment(shape, BoxSampler(s.x(), s.y(), s.z()), + fmt::format("Box({}, {}, {})", s.x(), s.y(), s.z()), 2.0, + &rng); + } +} + +GTEST_TEST(BoundingSphereTest, CapsuleContainsSurface) { + Rng rng(0x5eed0003); + const std::vector> params{ + {0.1, 1.0}, {1.0, 0.01}, {0.001, 3.0}, {0.5, 0.5}}; + for (const auto& [r, length] : params) { + const Capsule shape(r, length); + CheckContainment(shape, CapsuleSampler(r, length), + fmt::format("Capsule({}, {})", r, length), 2.0, &rng); + } +} + +GTEST_TEST(BoundingSphereTest, CylinderContainsSurface) { + Rng rng(0x5eed0004); + const std::vector> params{ + {0.1, 1.0}, {2.0, 0.01}, {0.002, 4.0}, {0.5, 0.5}}; + for (const auto& [r, length] : params) { + const Cylinder shape(r, length); + CheckContainment(shape, CylinderSampler(r, length), + fmt::format("Cylinder({}, {})", r, length), 2.0, &rng); + } +} + +GTEST_TEST(BoundingSphereTest, EllipsoidContainsSurface) { + Rng rng(0x5eed0005); + const std::vector radii{ + {1.0, 1.0, 1.0}, {0.01, 0.5, 2.0}, {3.0, 0.001, 0.001}, {0.2, 0.9, 0.05}}; + for (const Vector3d& e : radii) { + const Ellipsoid shape(e.x(), e.y(), e.z()); + CheckContainment(shape, EllipsoidSampler(e.x(), e.y(), e.z()), + fmt::format("Ellipsoid({}, {}, {})", e.x(), e.y(), e.z()), + 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) = Uniform(rng, 0.2, 1.0) * 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) = Vector3d(Uniform(rng, -0.4, 0.4), Uniform(rng, -0.4, 0.4), + Uniform(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(Uniform(rng, -1.0, 1.0), 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(Uniform(rng, -1.0, 1.0), Uniform(rng, -1.0, 1.0), + 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(Uniform(rng, -2.0, 2.0), Uniform(rng, -1e-5, 1e-5), + 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 * 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 nothing about our radius is claimed for 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"; +} + +/* Writes a small nonconvex OBJ (an L-shaped prism) so the Mesh path exercises + hull-vs-mesh semantics, not just a convex primitive in disguise. */ +std::string WriteLShapedObj() { + const std::filesystem::path dir = + std::filesystem::path(drake::temp_directory()) / "ccd_bounding_sphere"; + std::filesystem::create_directories(dir); + const std::filesystem::path path = dir / "l_prism.obj"; + std::ofstream out(path); + // 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"; + } + out.close(); + return path.string(); +} + +GTEST_TEST(BoundingSphereTest, MeshContainsHullVertices) { + Rng rng(0x5eed0007); + const std::string obj = WriteLShapedObj(); + for (const Vector3d& scale3 : + {Vector3d(1.0, 1.0, 1.0), Vector3d(0.4, 1.7, 1.0)}) { + const Mesh shape(obj, 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); + } +} + +/* The closed-set requirement of the geometry-support scope: a shape that is not + on the supported list must throw, never silently inherit some other shape's + formula. */ +GTEST_TEST(BoundingSphereTest, ThrowsOnHalfSpace) { + const HalfSpace shape; + const RigidTransform X_LG = RigidTransform::Identity(); + EXPECT_THROW(ComputeBoundingSphere(shape, X_LG), std::exception); + try { + ComputeBoundingSphere(shape, X_LG); + GTEST_FAIL() << "expected a throw"; + } catch (const std::exception& e) { + const std::string what = e.what(); + EXPECT_NE(what.find("HalfSpace"), std::string::npos) << what; + } +} + +GTEST_TEST(BoundingSphereTest, ThrowsOnUnsupportedShape) { + const MeshcatCone shape(1.0, 0.5, 0.25); + const RigidTransform X_LG = RigidTransform::Identity(); + EXPECT_THROW(ComputeBoundingSphere(shape, X_LG), std::exception); +} + +} // namespace +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/motion_bound_test.cc b/planning/certified_ccd/test/motion_bound_test.cc new file mode 100644 index 000000000000..1c9fb2bf1966 --- /dev/null +++ b/planning/certified_ccd/test/motion_bound_test.cc @@ -0,0 +1,1228 @@ +/* T2 (the test plan) — the displacement lemma and the J(p) subtree logic. This + * is the load-bearing test of the whole library: if any λ(j, p) under-bounds + * the true motion of a pair's distal side, the certifier will happily certify a + * colliding trajectory. Any failure here is a soundness bug in the kinematics + * module and must be fixed there, never by loosening this test (the + * implementation notes, item 2). + * + * Three complementary property tests run over the same random-plant corpus: + * + * (1) Atomic, per coordinate. Move exactly one coordinate j ∈ J(p) and check + * that every sampled material point of the pair's distal side D(j, p) — + * the body inside S_j — displaces, measured in the *other* body's frame, + * by at most λ(j,p)·|Δq_j|. This is the elementary step the lemma's proof + * telescopes over, and it pins λ directly. + * + * (2) Aggregate, multi-coordinate. Move all coordinates at once and check + * that the distance between any material point of A's geometry and any + * material point of B's geometry changes by at most Σ λ(j,p)·|Δq_j|. This + * is the pairwise-distance form the certifier actually consumes. Note + * that a one-sided statement ("points of B in A's frame") is NOT valid in + * general for a self-collision pair, because the distal side changes from + * joint to joint along J(p); the sum survives only because each + * telescoping step is bounded in the frame of *that step's* static side, + * and point-to-point distance is frame invariant. + * + * (3) One-sided aggregate, for pairs whose whole J(p) shares a single distal + * side (every robot-vs-environment pair): then the stronger statement + * does hold and is checked. + */ + +#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/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/rotation_matrix.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/planar_joint.h" +#include "drake/multibody/tree/prismatic_joint.h" +#include "drake/multibody/tree/quaternion_floating_joint.h" +#include "drake/multibody/tree/revolute_joint.h" +#include "drake/multibody/tree/screw_joint.h" +#include "drake/multibody/tree/weld_joint.h" +#include "drake/planning/certified_ccd/motion_bound_table.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::geometry::Box; +using drake::geometry::Capsule; +using drake::geometry::GeometryId; +using drake::geometry::HalfSpace; +using drake::geometry::Sphere; +using drake::math::RigidTransform; +using drake::math::RotationMatrix; +using drake::multibody::BodyIndex; +using drake::multibody::CoulombFriction; +using drake::multibody::JointIndex; +using drake::multibody::MultibodyPlant; +using drake::multibody::PlanarJoint; +using drake::multibody::PrismaticJoint; +using drake::multibody::QuaternionFloatingJoint; +using drake::multibody::RevoluteJoint; +using drake::multibody::RigidBody; +using drake::multibody::ScrewJoint; +using drake::multibody::SpatialInertia; +using drake::multibody::WeldJoint; +using drake::planning::RobotDiagram; +using drake::planning::RobotDiagramBuilder; +using Eigen::Matrix3Xd; +using Eigen::Vector3d; +using Eigen::VectorXd; + +using Rng = std::mt19937_64; + +/* Absolute slack on every displacement assertion. The claims are exact + mathematics; this only absorbs floating-point noise in Drake's forward + kinematics and in our own accumulation (both ~1e-15 at these magnitudes). */ +constexpr double kSlack = 1e-9; + +// --------------------------------------------------------------------------- +// Small random utilities (seeded, deterministic). +// --------------------------------------------------------------------------- + +double Uniform(Rng* rng, double lo, double hi) { + return std::uniform_real_distribution(lo, hi)(*rng); +} + +int UniformInt(Rng* rng, int lo, int hi) { + return std::uniform_int_distribution(lo, hi)(*rng); +} + +Vector3d RandomUnitVector(Rng* rng) { + std::normal_distribution normal(0.0, 1.0); + Vector3d v; + do { + v = Vector3d(normal(*rng), normal(*rng), normal(*rng)); + } while (v.norm() < 1e-6); + return v.normalized(); +} + +RotationMatrix RandomRotation(Rng* rng) { + std::normal_distribution normal(0.0, 1.0); + Eigen::Quaterniond q; + do { + q = Eigen::Quaterniond(normal(*rng), normal(*rng), normal(*rng), + normal(*rng)); + } while (q.norm() < 1e-6); + q.normalize(); + return RotationMatrix(q); +} + +RigidTransform RandomTransform(Rng* rng, double scale) { + return RigidTransform( + RandomRotation(rng), + Vector3d(Uniform(rng, -scale, scale), Uniform(rng, -scale, scale), + Uniform(rng, -scale, scale))); +} + +SpatialInertia UnitInertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +// --------------------------------------------------------------------------- +// Surface sampling for the primitives the random worlds use. +// --------------------------------------------------------------------------- + +Matrix3Xd SampleSphereSurface(Rng* rng, double r, int n) { + Matrix3Xd p(3, n); + for (int i = 0; i < n; ++i) p.col(i) = r * RandomUnitVector(rng); + return p; +} + +Matrix3Xd SampleBoxSurface(Rng* rng, const Vector3d& size, int n) { + const Vector3d half = 0.5 * size; + Matrix3Xd p(3, n); + for (int i = 0; i < n; ++i) { + Vector3d v(Uniform(rng, -half.x(), half.x()), + Uniform(rng, -half.y(), half.y()), + Uniform(rng, -half.z(), half.z())); + const int axis = UniformInt(rng, 0, 2); + v(axis) = (UniformInt(rng, 0, 1) == 0 ? -1.0 : 1.0) * half(axis); + p.col(i) = v; + } + return p; +} + +Matrix3Xd SampleCapsuleSurface(Rng* rng, double r, double length, int n) { + const double half = 0.5 * length; + Matrix3Xd p(3, n); + for (int i = 0; i < n; ++i) { + if (UniformInt(rng, 0, 1) == 0) { + const double phi = Uniform(rng, 0.0, 2.0 * M_PI); + p.col(i) = Vector3d(r * std::cos(phi), r * std::sin(phi), + Uniform(rng, -half, half)); + } else { + const Vector3d u = RandomUnitVector(rng); + const double z0 = u.z() >= 0.0 ? half : -half; + p.col(i) = Vector3d(r * u.x(), r * u.y(), z0 + r * u.z()); + } + } + return p; +} + +// --------------------------------------------------------------------------- +// 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 RigidTransform X_BG = 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, + CoulombFriction(1.0, 1.0)); + p_G = SampleSphereSurface(rng, r, num_samples); + break; + } + case 1: { + const Vector3d size(Uniform(rng, 0.02, 0.3), Uniform(rng, 0.02, 0.3), + Uniform(rng, 0.02, 0.3)); + gid = plant->RegisterCollisionGeometry(body, X_BG, Box(size), name, + CoulombFriction(1.0, 1.0)); + p_G = SampleBoxSurface(rng, size, num_samples); + 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, + CoulombFriction(1.0, 1.0)); + p_G = SampleCapsuleSurface(rng, r, length, num_samples); + 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), UnitInertia()); + // 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 RigidTransform X_PF = RandomTransform(rng, 0.25); + const RigidTransform X_CM = RandomTransform(rng, 0.25); + const std::string jn = fmt::format("j{}", i); + const int kind = UniformInt(rng, 0, allow_screw ? 4 : 3); + switch (kind) { + case 0: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + RandomUnitVector(rng)); + break; + case 1: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + 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, + RandomTransform(rng, 0.2)); + break; + default: + plant.AddJoint(jn, parent, X_PF, body, X_CM, + 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(PairId{ga, gb, ba, bb}); + } + return pairs; +} + +// --------------------------------------------------------------------------- +// Part 1 — J(p) subtree logic on hand-built plants. +// --------------------------------------------------------------------------- + +/* Convenience: 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; +} + +GTEST_TEST(JointSupportTest, SerialChain) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& env = plant.AddRigidBody("env", UnitInertia()); + const auto& l1 = plant.AddRigidBody("l1", UnitInertia()); + const auto& l2 = plant.AddRigidBody("l2", UnitInertia()); + const auto& l3 = plant.AddRigidBody("l3", UnitInertia()); + plant.AddJoint("w_env", plant.world_body(), {}, env, {}, + RigidTransform(Vector3d(1.0, 0.0, 0.0))); + plant.AddJoint("j1", plant.world_body(), {}, l1, {}, + Vector3d::UnitZ()); + plant.AddJoint("j2", l1, {}, l2, {}, Vector3d::UnitY()); + plant.AddJoint("j3", l2, {}, l3, {}, Vector3d::UnitX()); + auto diagram = builder.Build(); + const KinematicsEngine engine(*diagram); + const auto& p = diagram->plant(); + + const std::vector j1 = CoordsOf(p, "j1"); + const std::vector j2 = CoordsOf(p, "j2"); + const std::vector j3 = CoordsOf(p, "j3"); + + // Robot vs. anchored environment: the ancestors of the robot body. + EXPECT_EQ(engine.CoordinatesAffectingPair(env.index(), l3.index()), + Merge({j1, j2, j3})); + EXPECT_EQ(engine.CoordinatesAffectingPair(p.world_body().index(), l2.index()), + Merge({j1, j2})); + // Self pair through the common ancestor: the path between the two bodies. + EXPECT_EQ(engine.CoordinatesAffectingPair(l1.index(), l3.index()), + Merge({j2, j3})); + // A body against itself, and two anchored bodies, are static. + EXPECT_TRUE(engine.CoordinatesAffectingPair(l3.index(), l3.index()).empty()); + EXPECT_TRUE( + engine.CoordinatesAffectingPair(p.world_body().index(), env.index()) + .empty()); +} + +GTEST_TEST(JointSupportTest, BranchingTree) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& b1 = plant.AddRigidBody("b1", UnitInertia()); + const auto& left = plant.AddRigidBody("left", UnitInertia()); + const auto& right = plant.AddRigidBody("right", UnitInertia()); + const auto& left_tip = plant.AddRigidBody("left_tip", UnitInertia()); + 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, {}, left_tip, {}, + Vector3d::UnitX()); + auto diagram = builder.Build(); + const KinematicsEngine engine(*diagram); + const auto& p = diagram->plant(); + + 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. + + // Symmetric difference across the common ancestor b1: j0 affects both sides + // and drops out. + EXPECT_EQ(engine.CoordinatesAffectingPair(left_tip.index(), right.index()), + Merge({jl, jr, jt})); + EXPECT_EQ( + engine.CoordinatesAffectingPair(p.world_body().index(), left_tip.index()), + Merge({j0, jl, jt})); + EXPECT_EQ(engine.CoordinatesAffectingPair(left.index(), left_tip.index()), + Merge({jt})); +} + +GTEST_TEST(JointSupportTest, WeldedClusterMovesAsOneBody) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& arm = plant.AddRigidBody("arm", UnitInertia()); + const auto& hand = plant.AddRigidBody("hand", UnitInertia()); + const auto& finger = plant.AddRigidBody("finger", UnitInertia()); + const auto& anchored = plant.AddRigidBody("anchored", UnitInertia()); + plant.AddJoint("j0", plant.world_body(), {}, arm, {}, + Vector3d::UnitZ()); + plant.AddJoint("w1", arm, {}, hand, {}, + RigidTransform(Vector3d(0.2, 0.0, 0.0))); + plant.AddJoint("w2", hand, {}, finger, {}, + RigidTransform(Vector3d(0.05, 0.0, 0.0))); + plant.AddJoint("w3", plant.world_body(), {}, anchored, {}, + RigidTransform(Vector3d(0.0, 1.0, 0.0))); + auto diagram = builder.Build(); + const KinematicsEngine engine(*diagram); + const auto& p = diagram->plant(); + + // Everything inside a welded cluster is mutually static ... + EXPECT_TRUE( + engine.CoordinatesAffectingPair(arm.index(), finger.index()).empty()); + EXPECT_TRUE( + engine.CoordinatesAffectingPair(hand.index(), finger.index()).empty()); + EXPECT_TRUE( + engine.CoordinatesAffectingPair(p.world_body().index(), anchored.index()) + .empty()); + // ... and the whole cluster inherits the revolute coordinate of its chain. + EXPECT_EQ(engine.CoordinatesAffectingPair(anchored.index(), finger.index()), + CoordsOf(p, "j0")); +} + +GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& l1 = plant.AddRigidBody("l1", UnitInertia()); + const auto& l2 = plant.AddRigidBody("l2", UnitInertia()); + plant.AddJoint("j1", plant.world_body(), {}, l1, {}, + Vector3d::UnitZ()); + plant.AddJoint("j2", l1, {}, l2, {}, Vector3d::UnitY()); + plant.RegisterCollisionGeometry( + plant.world_body(), RigidTransform::Identity(), Sphere(0.1), + "g_world", CoulombFriction(1.0, 1.0)); + plant.RegisterCollisionGeometry( + l2, RigidTransform(Vector3d(0.3, 0, 0)), Sphere(0.05), "g_tip", + CoulombFriction(1.0, 1.0)); + auto diagram = builder.Build(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + + 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 = engine.ComputeMotionBoundTable( + lower, upper, std::vector(nq, false), pairs); + ASSERT_EQ(table.num_pairs(), 1); + EXPECT_FALSE(table.pair_is_static(0)); + EXPECT_EQ(table.entries(0).size(), 2); + } + { // One constant: only the other survives. + std::vector constant(nq, false); + constant[0] = true; + const MotionBoundTable table = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + ASSERT_EQ(table.entries(0).size(), 1); + EXPECT_EQ(table.entries(0)[0].first, 1); + } + { // All constant: the pair becomes static and its motion bound is zero. + const MotionBoundTable table = engine.ComputeMotionBoundTable( + lower, upper, std::vector(nq, true), pairs); + EXPECT_TRUE(table.pair_is_static(0)); + EXPECT_EQ(table.MotionBound(0, VectorXd::Constant(nq, 1.0)), 0.0); + } +} + +// --------------------------------------------------------------------------- +// Part 1b — the joint-type and half-space carve-outs (the joint-support scope; +// the geometry-support scope). +// --------------------------------------------------------------------------- + +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 v1 rejects it by name (the + // displacement lemma). + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& a = plant.AddRigidBody("body_a", UnitInertia()); + const auto& b = plant.AddRigidBody("body_b", UnitInertia()); + plant.AddJoint("w", plant.world_body(), {}, b, {}, + RigidTransform(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(); + } + try { + const KinematicsEngine engine(*diagram); + GTEST_FAIL() << "expected a throw for a reversed joint"; + } catch (const std::exception& e) { + const std::string what = e.what(); + EXPECT_NE(what.find("reversed"), std::string::npos) << what; + } +} + +/* world --(revolute)--> 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", UnitInertia()); + if (prismatic) { + plant.AddJoint("j", plant.world_body(), {}, link, {}, + Vector3d::UnitZ()); + } else { + plant.AddJoint("j", plant.world_body(), {}, link, {}, + Vector3d::UnitY()); + } + const CoulombFriction mu(1.0, 1.0); + const RigidTransform I = RigidTransform::Identity(); + if (halfspace_on_link) { + plant.RegisterCollisionGeometry(link, I, HalfSpace(), "hs", mu); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransform(Vector3d(0, 0, 1.0)), + Sphere(0.1), "ball", mu); + } else { + plant.RegisterCollisionGeometry(plant.world_body(), I, HalfSpace(), "hs", + mu); + plant.RegisterCollisionGeometry(link, + RigidTransform(Vector3d(0.3, 0, 0)), + Sphere(0.1), "ball", mu); + } + return builder.Build(); +} + +GTEST_TEST(HalfSpaceRuleTest, AnchoredGroundPlaneIsAccepted) { + // The canonical case: a ground plane on the world with a rotating arm above + // it. The half space is never the *distal* side, so λ bounds the arm's + // points and the pair is perfectly certifiable. + auto diagram = MakeHalfSpaceModel(/* halfspace_on_link = */ false, false); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + const int nq = diagram->plant().num_positions(); + const MotionBoundTable table = engine.ComputeMotionBoundTable( + VectorXd::Constant(nq, -1.0), VectorXd::Constant(nq, 1.0), + std::vector(nq, false), pairs); + ASSERT_EQ(table.entries(0).size(), 1); + EXPECT_GT(table.entries(0)[0].second, 0.0); + EXPECT_TRUE(std::isfinite(table.entries(0)[0].second)); +} + +GTEST_TEST(HalfSpaceRuleTest, RotatingHalfSpaceThrowsAtConstruction) { + auto diagram = MakeHalfSpaceModel(/* halfspace_on_link = */ true, false); + try { + const KinematicsEngine engine(*diagram); + GTEST_FAIL() + << "expected a throw for a half space with revolute relative motion"; + } catch (const std::exception& e) { + const std::string what = e.what(); + EXPECT_NE(what.find("hs"), std::string::npos) << what; + EXPECT_NE(what.find("HalfSpace"), std::string::npos) << what; + } +} + +GTEST_TEST(HalfSpaceRuleTest, TranslatingHalfSpaceIsAccepted) { + // Pure translation keeps every point of the half space moving by |Δq|, so + // λ = 1 is finite and correct even though the reach is not (the + // geometry-support scope). + auto diagram = MakeHalfSpaceModel(/* halfspace_on_link = */ true, true); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + const int nq = diagram->plant().num_positions(); + const MotionBoundTable table = engine.ComputeMotionBoundTable( + VectorXd::Constant(nq, -1.0), VectorXd::Constant(nq, 1.0), + std::vector(nq, false), pairs); + ASSERT_EQ(table.entries(0).size(), 1); + EXPECT_EQ(table.entries(0)[0].second, 1.0); +} + +/* 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 exactly 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", UnitInertia()); + const auto& b2 = plant.AddRigidBody("b2", UnitInertia()); + const auto& b3 = plant.AddRigidBody("b3", UnitInertia()); + plant.AddJoint("j0", plant.world_body(), {}, b1, {}, + Vector3d::UnitZ()); + plant.AddJoint( + "jf", b1, RigidTransform(Vector3d(0.1, 0.0, 0.0)), b2, + RigidTransform(Vector3d(0.0, 0.05, 0.0))); + plant.AddJoint( + "j1", b2, RigidTransform(Vector3d(0.0, 0.0, 0.15)), b3, + RigidTransform(Vector3d(0.07, 0.0, 0.0)), Vector3d::UnitY()); + const CoulombFriction mu(1.0, 1.0); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransform::Identity(), + Sphere(0.1), "g_world", mu); + plant.RegisterCollisionGeometry(b3, + RigidTransform(Vector3d(0.2, 0, 0)), + Sphere(0.05), "g_tip", mu); + 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(); + try { + engine.ComputeMotionBoundTable(VectorXd::Constant(nq, -0.5), + VectorXd::Constant(nq, 0.5), + std::vector(nq, false), pairs); + GTEST_FAIL() << "expected a throw for a moving quaternion floating joint"; + } catch (const std::exception& e) { + const std::string what = e.what(); + EXPECT_NE(what.find("jf"), std::string::npos) << what; + EXPECT_NE(what.find("quaternion_floating"), std::string::npos) << what; + EXPECT_NE(what.find("constant"), std::string::npos) << what; + } +} + +GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { + auto diagram = MakeMidChainFloatingModel(); + const MultibodyPlant& plant = diagram->plant(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + + 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 = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + ASSERT_EQ(table.entries(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.entries(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 = + SampleSphereSurface(&rng, 0.05, 128).colwise() + Vector3d(0.2, 0, 0); + const auto& frame_tip = plant.GetBodyByName("b3").body_frame(); + const auto& frame_world = plant.world_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]); + } + Matrix3Xd out_q(3, points_B.cols()); + Matrix3Xd out_qp(3, points_B.cols()); + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_q); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_qp); + const double displacement = (out_qp - out_q).colwise().norm().maxCoeff(); + 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 randomized corpus below is excellent at catching *structural* errors +// (a dropped term, a wrong distal side), but the chain walk's triangle +// inequalities are strictly slack at random poses, so a term that is merely +// too small can hide inside that slack. This model removes the slack: every +// offset lies along +x with identity rotation, so ‖a + b‖ = ‖a‖ + ‖b‖ at every +// hop and the reach is *exactly attained* by a specific material point. Each +// contribution to r therefore shows up in λ digit for digit. +// +// 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 RigidTransform(Vector3d(x, 0.0, 0.0)); + }; + + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& b1 = plant.AddRigidBody("b1", UnitInertia()); + const auto& b2 = plant.AddRigidBody("b2", UnitInertia()); + const auto& b3 = plant.AddRigidBody("b3", UnitInertia()); + 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)); + const CoulombFriction mu(1.0, 1.0); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransform::Identity(), + Sphere(0.02), "g_world", mu); + plant.RegisterCollisionGeometry(b3, tx(kL3), Sphere(kRho), "g_tip", mu); + + 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; +} + +/* Locates the (world geometry, tip geometry) pair. */ +int FindPairIndex(const RobotDiagram& diagram, + const std::vector& pairs, const std::string& name_a, + const std::string& name_b) { + const auto& inspector = diagram.scene_graph().model_inspector(); + for (int k = 0; k < static_cast(pairs.size()); ++k) { + const std::string a = inspector.GetName(pairs[k].a); + const std::string b = inspector.GetName(pairs[k].b); + if ((a.find(name_a) != std::string::npos && + b.find(name_b) != std::string::npos) || + (a.find(name_b) != std::string::npos && + b.find(name_a) != std::string::npos)) { + return k; + } + } + return -1; +} + +GTEST_TEST(ReachTest, RevoluteChainIsExactAndTight) { + const TightChain chain = MakeTightChain(/* screw_top = */ false, 0.0); + const MultibodyPlant& plant = chain.diagram->plant(); + const KinematicsEngine engine(*chain.diagram); + const std::vector pairs = CollisionPairs(*chain.diagram); + const int k = FindPairIndex(*chain.diagram, pairs, "g_world", "g_tip"); + ASSERT_GE(k, 0); + + 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 = engine.ComputeMotionBoundTable( + lower, upper, std::vector(nq, false), pairs); + + double lambda_top = 0.0; + double lambda_slide = 0.0; + for (const auto& [c, lam] : table.entries(k)) { + if (c == j_top.position_start()) lambda_top = lam; + if (c == j_slide.position_start()) lambda_slide = lam; + } + // 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(lambda_top, chain.expected_reach, 1e-12); + EXPECT_EQ(lambda_slide, 1.0); + + // And the bound is attained: with the slide at its box maximum and a small + // Δθ, the chord 2r·sin(Δθ/2) recovers r·Δθ to eight digits. + 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; + const double dtheta = 1e-4; + qp[j_top.position_start()] = dtheta; + Matrix3Xd p_B3(3, 1); + p_B3.col(0) = chain.far_point_b3; + Matrix3Xd before(3, 1); + Matrix3Xd after(3, 1); + const auto& frame_tip = plant.GetBodyByName("b3").body_frame(); + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &before); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &after); + const double displacement = (after - before).norm(); + const double bound = lambda_top * dtheta; + EXPECT_LE(displacement, bound + kSlack); + EXPECT_GT(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. + const VectorXd dq = (qp - q).cwiseAbs(); + EXPECT_LE(displacement, table.MotionBound(k, dq) + kSlack); +} + +GTEST_TEST(ReachTest, ScrewLambdaIncludesPitchAndIsNecessary) { + constexpr double kPitch = 8.0; // meters of travel per revolution. + const TightChain chain = MakeTightChain(/* screw_top = */ true, kPitch); + const MultibodyPlant& plant = chain.diagram->plant(); + const KinematicsEngine engine(*chain.diagram); + const std::vector pairs = CollisionPairs(*chain.diagram); + const int k = FindPairIndex(*chain.diagram, pairs, "g_world", "g_tip"); + ASSERT_GE(k, 0); + + 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 = engine.ComputeMotionBoundTable( + lower, upper, std::vector(nq, false), pairs); + + double lambda_top = 0.0; + for (const auto& [c, lam] : table.entries(k)) { + if (c == j_top.position_start()) lambda_top = lam; + } + const double pitch_term = kPitch / (2.0 * M_PI); + EXPECT_NEAR(lambda_top, chain.expected_reach + pitch_term, 1e-12); + + 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; + const double dtheta = 1e-4; + qp[j_top.position_start()] = dtheta; + Matrix3Xd p_B3(3, 1); + p_B3.col(0) = chain.far_point_b3; + Matrix3Xd before(3, 1); + Matrix3Xd after(3, 1); + const auto& frame_tip = plant.GetBodyByName("b3").body_frame(); + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &before); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &after); + const double displacement = (after - before).norm(); + + EXPECT_LE(displacement, lambda_top * 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·Δθ. This + // is the direct evidence that dropping the pitch term would be UNSOUND, not + // merely conservative. + EXPECT_GT(displacement, chain.expected_reach * dtheta * (1.0 + 1e-6)) + << "a screw λ of r alone would under-bound this motion"; + const double exact = std::hypot(chain.expected_reach, pitch_term) * dtheta; + EXPECT_NEAR(displacement, exact, 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}; + + void Observe(double achieved, double bound) { + if (bound > 1e-12) { + max_tightness = std::max(max_tightness, achieved / bound); + } + } +}; + +/* Runs every displacement-lemma check on one random world. */ +void CheckWorld(Rng* rng, const RandomWorld& world, bool use_constant_coords, + 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. + 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 = + use_constant_coords && Uniform(rng, 0.0, 1.0) < 0.3; + constant[c] = is_constant; + const double half = is_constant ? 0.0 + : (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.entries(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()); + } + + 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 PairId& 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.entries(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]; + Matrix3Xd before(3, pts.cols()); + Matrix3Xd after(3, pts.cols()); + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &before); + plant.SetPositions(&ctx, q_step); + plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &after); + const double displacement = + (after - before).colwise().norm().maxCoeff(); + 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; + } + + if (table.pair_is_static(k)) { + // A static pair must not move at all under any q, q' in the box. + Matrix3Xd before(3, pts_b.cols()); + Matrix3Xd after(3, pts_b.cols()); + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_b, pts_b, frame_a, &before); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_b, pts_b, frame_a, &after); + ASSERT_LE((after - before).colwise().norm().maxCoeff(), kSlack) + << "pair " << k << " has empty J(p) but its relative pose moved"; + continue; + } + + // ---- (2) Aggregate: material-point distances. --------------------- + // Subsample: 24 × 24 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 Matrix3Xd& pts = (common_distal == pair.body_a) ? pts_a : pts_b; + const auto& frame_d = + (common_distal == pair.body_a) ? frame_a : frame_b; + const auto& frame_o = + (common_distal == pair.body_a) ? frame_b : frame_a; + Matrix3Xd before(3, pts.cols()); + Matrix3Xd after(3, pts.cols()); + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &before); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &after); + const double displacement = + (after - before).colwise().norm().maxCoeff(); + 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; constant-coordinate carve-outs in + // every other world. + const RandomWorld world = + MakeRandomWorld(&rng, /* allow_screw = */ trial % 3 == 0, 128); + stats.screw_joints += world.num_screw_joints; + CheckWorld(&rng, world, /* use_constant_coords = */ trial % 2 == 1, &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: the joint-support scope lists them as + // should-have, and this test is what decides whether they are supported or + // excluded. + 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); + GTEST_LOG_(INFO) << fmt::format( + "plants={} pairs={} atomic={} aggregate={} one_sided={} screw_joints={} " + "max_tightness={:.6f}", + stats.plants, stats.pairs, stats.atomic_checks, stats.aggregate_checks, + stats.one_sided_checks, stats.screw_joints, stats.max_tightness); +} + +/* 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), UnitInertia()); + plant.AddJoint( + fmt::format("j{}", i), *bodies.back(), RandomTransform(&rng, 0.25), + body, RandomTransform(&rng, 0.25), 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, /* use_constant_coords = */ false, &stats); + if (HasFatalFailure()) return; + } + EXPECT_GE(stats.plants, 75); + EXPECT_GT(stats.atomic_checks, 500); + EXPECT_GT(stats.max_tightness, 0.5); + GTEST_LOG_(INFO) << fmt::format("screw: plants={} atomic={} tightness={:.6f}", + stats.plants, stats.atomic_checks, + stats.max_tightness); +} + +} // namespace +} // namespace certified_ccd +} // namespace planning +} // namespace drake From 78fffff9eb80f41ba7f12f0b7ccadd8f7d1fea8f Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 14:39:55 -0400 Subject: [PATCH 03/22] [planning] Add certified_ccd: the distance oracle Adds DistanceOracle, the narrowphase abstraction the certifier queries, with a construction-time capability probe that classifies every unfiltered pair once (so no per-query dispatch decisions remain) and refuses the pairs whose signed distance Drake cannot report to the documented accuracy. Half-space pairs take a dedicated exact fallback. Also adds AddVPolytopeGeometry, the V-polytope ingestion helper the oracle tests use to build convex geometry from vertex data. --- planning/certified_ccd/BUILD.bazel | 60 + planning/certified_ccd/distance_oracle.cc | 525 ++++++++ planning/certified_ccd/distance_oracle.h | 110 ++ .../test/distance_oracle_test.cc | 1134 +++++++++++++++++ planning/certified_ccd/vpolytope_ingestion.cc | 62 + planning/certified_ccd/vpolytope_ingestion.h | 49 + 6 files changed, 1940 insertions(+) create mode 100644 planning/certified_ccd/distance_oracle.cc create mode 100644 planning/certified_ccd/distance_oracle.h create mode 100644 planning/certified_ccd/test/distance_oracle_test.cc create mode 100644 planning/certified_ccd/vpolytope_ingestion.cc create mode 100644 planning/certified_ccd/vpolytope_ingestion.h diff --git a/planning/certified_ccd/BUILD.bazel b/planning/certified_ccd/BUILD.bazel index d504286f0ee0..1af1ee1e9add 100644 --- a/planning/certified_ccd/BUILD.bazel +++ b/planning/certified_ccd/BUILD.bazel @@ -13,10 +13,12 @@ drake_cc_package_library( visibility = ["//visibility:public"], deps = [ ":bounding_sphere", + ":distance_oracle", ":motion_bound_table", ":numerics", ":options", ":piecewise_bezier_path", + ":vpolytope_ingestion", ], ) @@ -93,6 +95,43 @@ drake_cc_library( ], ) +drake_cc_library( + name = "distance_oracle", + srcs = ["distance_oracle.cc"], + hdrs = ["distance_oracle.h"], + deps = [ + ":options", + "//geometry:scene_graph", + "//planning:robot_diagram", + "@eigen", + ], + implementation_deps = [ + "//common:essential", + "//common:unused", + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//geometry/proximity:polygon_surface_mesh", + "//math:geometric_transform", + "//multibody/plant", + ], +) + +drake_cc_library( + name = "vpolytope_ingestion", + srcs = ["vpolytope_ingestion.cc"], + hdrs = ["vpolytope_ingestion.h"], + deps = [ + "//geometry:geometry_ids", + "//geometry/optimization:convex_set", + "//math:geometric_transform", + "//multibody/plant", + ], + implementation_deps = [ + "//common:essential", + "//geometry:shape_specification", + ], +) + # === test/ === # T1 — curve module acceptance tests. @@ -138,4 +177,25 @@ drake_cc_googletest( ], ) +# T3 — oracle accuracy, probe classification, half-space fallback, V-polytope. +drake_cc_googletest( + name = "distance_oracle_test", + deps = [ + ":distance_oracle", + ":vpolytope_ingestion", + "//common:temp_directory", + "//geometry:geometry_instance", + "//geometry:proximity_properties", + "//geometry:scene_graph", + "//geometry:shape_specification", + "//geometry/optimization:convex_set", + "//math:geometric_transform", + "//multibody/fem:deformable_body_config", + "//multibody/plant", + "//multibody/tree:spatial_inertia", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + ], +) + add_lint_tests() diff --git a/planning/certified_ccd/distance_oracle.cc b/planning/certified_ccd/distance_oracle.cc new file mode 100644 index 000000000000..4e180466c7c2 --- /dev/null +++ b/planning/certified_ccd/distance_oracle.cc @@ -0,0 +1,525 @@ +#include "drake/planning/certified_ccd/distance_oracle.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_throw.h" +#include "drake/common/unused.h" +#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" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::unused; +using drake::geometry::GeometryId; +using drake::geometry::QueryObject; +using drake::geometry::SceneGraphInspector; +using drake::math::RigidTransformd; + +/** The closed set of shape classes the oracle recognizes. Anything outside it +is `kUnsupported` and is refused by the capability probe (mirroring the +throw-on-unknown-shape rule the radius table uses; the geometry-support scope). +*/ +enum class ShapeClass { + kSphere, + kBox, + kCapsule, + kCylinder, + kEllipsoid, + kConvex, + kMesh, + kHalfSpace, + kUnsupported, +}; + +ShapeClass Classify(const drake::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; + } + }); +} + +/** 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 and the report. */ +std::string Describe(const SceneGraphInspector& inspector, + GeometryId id) { + std::ostringstream out; + out << inspector.GetName(id) << " (" << inspector.GetShape(id).type_name() + << ")"; + return out.str(); +} + +/** One row of the probe report: a distinct unordered shape-type combination +and the route it resolved to. */ +struct ComboRow { + DistanceRoute route{DistanceRoute::kNative}; + int pair_count{0}; + /** A representative pair, used for the probe query and error messages. */ + 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; + std::string report; +}; + +DistanceOracle::DistanceOracle( + const drake::planning::RobotDiagram& model, + double query_tolerance) { + DRAKE_THROW_UNLESS(query_tolerance >= 0.0); + tolerance_ = query_tolerance; + 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::ostringstream msg; + msg << "DistanceOracle: deformable geometries are not supported " + "(certified continuous collision checking assumes rigid bodies " + "whose motion the plant's kinematics describe). Offending " + "geometries:"; + for (const GeometryId id : deformables) { + msg << "\n - " << inspector.GetName(id); + } + throw std::runtime_error(msg.str()); + } + + // --- Snapshot the unfiltered pairs and classify each one. ---------------- + // GetCollisionCandidates() returns a sorted std::set and std::map keeps the + // report ordering fixed, so both pairs_ and support_report() are + // deterministic for a given model. + std::map, ComboRow> combos; + std::set mesh_names; + + 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( + "DistanceOracle: collision geometry " + + Describe(inspector, body_a == nullptr ? id_a : id_b) + + " is not attached to a MultibodyPlant body; the checker can only " + "certify geometry whose motion the plant describes."); + } + + 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( + "DistanceOracle: signed distance between two HalfSpace geometries " + "is undefined, so the pair " + + Describe(inspector, id_a) + " / " + Describe(inspector, id_b) + + " cannot be certified. Remove one halfspace, or filter the pair " + "(CollisionFilterManager / a collision filter group)."); + } + + 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( + "DistanceOracle: no closed-form support function for shape type '" + + std::string(inspector.GetShape(partner).type_name()) + + "', so the halfspace pair " + Describe(inspector, id_a) + " / " + + Describe(inspector, id_b) + " cannot be certified."); + } + if (impl->support.find(partner) == impl->support.end()) { + impl->support.emplace(partner, + MakeSupportData(inspector.GetShape(partner))); + } + } + + // Meshes are certified as their convex hulls; say so, loudly (the risk + // register). + if (class_a == ShapeClass::kMesh) + mesh_names.insert(inspector.GetName(id_a)); + if (class_b == ShapeClass::kMesh) + mesh_names.insert(inspector.GetName(id_b)); + + const auto key = std::minmax(class_a, class_b); + const std::pair combo{key.first, key.second}; + auto it = combos.find(combo); + if (it == combos.end()) { + combos.emplace(combo, ComboRow{route, 1, id_a, id_b}); + } else { + ++it->second.pair_count; + } + + pairs_.push_back(PairRecord{ + PairId{id_a, id_b, body_a->index(), body_b->index()}, route, 0.0}); + } + + // --- 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( + "DistanceOracle: this Drake build cannot compute signed distance " + "for the shape combination (" + + ClassName(combo.first) + ", " + ClassName(combo.second) + + "); an offending pair is " + Describe(inspector, row.example_a) + + " / " + Describe(inspector, row.example_b) + + ". Filter the pair, or replace the geometry with a supported " + "shape (Convex is always supported). Drake reported: " + + e.what()); + } + } + } + + // --- Render the report. -------------------------------------------------- + std::ostringstream report; + report << "DistanceOracle capability probe: " << pairs_.size() + << " unfiltered pair(s), " << combos.size() + << " distinct shape-type combination(s), tolerance tau = " + << tolerance_ << " m.\n"; + for (const auto& [combo, row] : combos) { + report << " " << ClassName(combo.first) << "-" << ClassName(combo.second) + << ": "; + switch (row.route) { + case DistanceRoute::kNative: + report << "native (ComputeSignedDistancePairClosestPoints, probed ok)"; + break; + case DistanceRoute::kHalfSpaceA: + case DistanceRoute::kHalfSpaceB: + report << "halfspace analytic support-function fallback (exact)"; + break; + } + report << "; " << row.pair_count << " pair(s)\n"; + } + for (const std::string& name : mesh_names) { + report << " Mesh " << name << ": certified as its convex hull\n"; + } + impl->report = report.str(); + + 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.id.a, + pair.id.b); + // Drake reports the pair in its own fixed (deliberately undocumented) + // order, which may be the reverse of ours; 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.id.a && result.id_B == pair.id.b) { + p_ACa = result.p_ACa; + p_BCb = result.p_BCb; + } else if (result.id_A == pair.id.b && result.id_B == pair.id.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.id.a) * p_ACa; + } + if (nearest_b_W != nullptr) { + *nearest_b_W = query_object.GetPoseInWorld(pair.id.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.id.a : pair.id.b; + const GeometryId partner_id = a_is_halfspace ? pair.id.b : pair.id.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; +} + +std::string DistanceOracle::support_report() const { + return impl_->report; +} + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/distance_oracle.h b/planning/certified_ccd/distance_oracle.h new file mode 100644 index 000000000000..7085d0122205 --- /dev/null +++ b/planning/certified_ccd/distance_oracle.h @@ -0,0 +1,110 @@ +#pragma once + +// NOTE(interface): This header is owned by the distance module. The class +// and file names and the documented semantics are fixed; internal details +// may be refined by the implementation. + +#include +#include +#include +#include + +#include + +#include "drake/geometry/query_object.h" +#include "drake/planning/certified_ccd/options.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** How the oracle computes signed distance for one pair, resolved once by +the capability probe (the geometry-support scope; the distance-oracle contract): +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 pair with its pre-resolved distance route and +effective threshold m_p = margin + padding(p). */ +struct PairRecord { + PairId id; + DistanceRoute route{DistanceRoute::kNative}; + /** Filled by the facade from margin + PaddingSpec. */ + double threshold{0.0}; +}; + +/** Narrowphase distance abstraction (the distance-oracle contract). Stateless +per query and thread-compatible: configuration comes in via the caller's +QueryObject. + +Contract: SignedDistance returns φ̂ with |φ̂ − φ_true| ≤ tolerance() +whenever φ_true is at or above −tolerance(), and returns a definitely +negative value when the shapes interpenetrate beyond tolerance. Only +over-reporting a distance at or above threshold could fake a certificate +(the soundness argument), which is why the capability probe keeps any +not-a-true-distance backend out of the loop entirely. */ +class DistanceOracle { + public: + /** Runs the capability probe: enumerates the unfiltered proximity pairs + from the model's SceneGraph inspector (collision filter state snapshotted + at construction), classifies every (shape, shape) combination as + {native, halfspace-fallback, unsupported}, and + @throws std::exception immediately naming the offending geometries if any + pair is unsupported (deformables; halfspace–halfspace). Never discovers an + unsupported pair mid-certification. */ + DistanceOracle(const drake::planning::RobotDiagram& model, + double query_tolerance); + + /** The unfiltered pairs found by the probe (thresholds default 0; the + facade rewrites them from margin + padding). */ + 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 reports world-frame + closest points when the route provides them. + + `pair` need not be an element of pairs(): the facade copies the probe's + records and rewrites their thresholds, so only `pair.id` and `pair.route` + are read here. 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 (i.e. the + record did not come from pairs()). */ + double SignedDistance( + const drake::geometry::QueryObject& query_object, + const PairRecord& pair, Eigen::Vector3d* nearest_a_W = nullptr, + Eigen::Vector3d* nearest_b_W = nullptr) const; + + /** τ used in the certificate arithmetic (the numerical policy). */ + double tolerance() const { return tolerance_; } + + /** Human-readable probe report: one line per distinct shape-type + combination and its route (includes the "Mesh certified as convex hull" + notices; the risk register). */ + std::string support_report() const; + + protected: + std::vector pairs_; + double tolerance_{1e-6}; + + private: + /** Immutable capability-probe results: closed-form support data for every + halfspace partner, the resolved per-shape-combination routes, and the + rendered report. 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 certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/distance_oracle_test.cc b/planning/certified_ccd/test/distance_oracle_test.cc new file mode 100644 index 000000000000..cb48ed69683d --- /dev/null +++ b/planning/certified_ccd/test/distance_oracle_test.cc @@ -0,0 +1,1134 @@ +/// @file +/// T3 (the test plan): distance oracle accuracy, capability-probe +/// classification, the analytic halfspace fallback, Mesh-as-convex-hull +/// semantics, and the V-polytope ingestion round trip. +/// +/// Every world is built programmatically with RobotDiagramBuilder and every +/// randomized case uses a fixed seed, so the suite is deterministic. + +#include "drake/planning/certified_ccd/distance_oracle.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/temp_directory.h" +#include "drake/geometry/geometry_instance.h" +#include "drake/geometry/optimization/vpolytope.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/certified_ccd/vpolytope_ingestion.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +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::Mesh; +using drake::geometry::QueryObject; +using drake::geometry::Shape; +using drake::geometry::Sphere; +using drake::geometry::optimization::VPolytope; +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; + +constexpr double kTau = 1e-6; +/// 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.id.a == a && p.id.b == b) || (p.id.a == b && p.id.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(); +} + +// -------------------------------------------------------------------------- +// On-disk meshes (written once per process into Drake's temp directory). +// -------------------------------------------------------------------------- + +/// 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; +} + +/// Writes a closed triangulated box OBJ; returns its path. +const std::string& CubeObjPath() { + static const std::string path = [] { + const std::filesystem::path p = + std::filesystem::path(drake::temp_directory()) / "ccd_cube.obj"; + std::ofstream out(p); + const Vector3d& h = CubeHalf(); + const double xs[8] = {-1, 1, 1, -1, -1, 1, 1, -1}; + const double ys[8] = {-1, -1, 1, 1, -1, -1, 1, 1}; + const double zs[8] = {-1, -1, -1, -1, 1, 1, 1, 1}; + for (int i = 0; i < 8; ++i) { + out << "v " << xs[i] * h.x() << " " << ys[i] * h.y() << " " + << zs[i] * h.z() << "\n"; + } + const int faces[12][3] = {{1, 4, 3}, {1, 3, 2}, {5, 6, 7}, {5, 7, 8}, + {1, 2, 6}, {1, 6, 5}, {3, 4, 8}, {3, 8, 7}, + {4, 1, 5}, {4, 5, 8}, {2, 3, 7}, {2, 7, 6}}; + for (const auto& f : faces) { + out << "f " << f[0] << " " << f[1] << " " << f[2] << "\n"; + } + return p.string(); + }(); + return path; +} + +/// 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; + +/// Writes a closed, genuinely non-convex L-prism OBJ; returns its path. +const std::string& LPrismObjPath() { + static const std::string path = [] { + const std::filesystem::path p = + std::filesystem::path(drake::temp_directory()) / "ccd_l_prism.obj"; + std::ofstream out(p); + 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 p.string(); + }(); + return path; +} + +// ========================================================================== +// Accuracy vs analytic ground truth (native route). +// ========================================================================== + +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(), kTau); + ASSERT_EQ(oracle.pairs().size(), 1u); + const PairRecord& pair = oracle.pairs().front(); + EXPECT_EQ(pair.route, DistanceRoute::kNative); + + std::mt19937 rng(20260826); + int penetrating = 0; + for (int trial = 0; trial < 250; ++trial) { + const RigidTransformd X_WA = RandomPose(&rng, 0.4); + const RigidTransformd X_WB = RandomPose(&rng, 0.4); + world.SetPose(body_a, X_WA); + world.SetPose(body_b, X_WB); + + Vector3d p_a_W; + Vector3d p_b_W; + const double phi = + oracle.SignedDistance(world.query(), pair, &p_a_W, &p_b_W); + // Exact for two spheres on both branches. + const double expected = + (X_WB.translation() - X_WA.translation()).norm() - r_a - r_b; + EXPECT_NEAR(phi, expected, kNative) << "trial " << trial; + if (phi < 0.0) ++penetrating; + if (phi > 1e-9) { + EXPECT_NEAR((p_a_W - p_b_W).norm(), phi, kNative) << "trial " << trial; + } + } + EXPECT_GT(penetrating, 0) << "the sweep never exercised the penetrating " + "branch"; +} + +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(), kTau); + ASSERT_EQ(oracle.pairs().size(), 1u); + const PairRecord& pair = oracle.pairs().front(); + + std::mt19937 rng(881); + int separated = 0; + int penetrating = 0; + for (int trial = 0; trial < 250; ++trial) { + const RigidTransformd X_WS = RandomPose(&rng, 0.5); + const RigidTransformd X_WB = RandomPose(&rng, 0.5); + world.SetPose(sphere_body, X_WS); + world.SetPose(box_body, X_WB); + + const Vector3d p_B = X_WB.inverse() * X_WS.translation(); + const double center_distance = PointBoxDistance(p_B, half); + if (center_distance <= 0.0) continue; // center inside the box + // 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. + const double expected = center_distance - radius; + + 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, expected, 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); +} + +// ========================================================================== +// 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(), kTau); + halfspace_id_ = GeometryOf(world_->plant(), "halfspace"); + } + + /// A deliberately 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; + struct Partner { + std::string name; + Reference reference; + }; + 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 Partner& p : partners) { + poses.push_back(RandomPose(&rng, 0.4)); + world_->SetPose(Body(p.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].name)); + ASSERT_NE(pair.route, DistanceRoute::kNative) << partners[i].name; + + Vector3d p_a_W; + Vector3d p_b_W; + const double phi = oracle_->SignedDistance(query, pair, &p_a_W, &p_b_W); + const double expected = partners[i].reference(n_W, p0_W, poses[i]); + EXPECT_NEAR(phi, expected, kExact) + << partners[i].name << ", trial " << trial; + // Witnesses: separated by exactly |phi|, with the halfspace-side witness + // on the boundary plane. + const Vector3d& on_plane = (pair.id.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); + if (phi > 0.0) { + ++positive; + } else { + ++negative; + } + } + } + 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); +} + +TEST_F(HalfSpaceFallbackTest, ReportNamesEveryCombinationAndRoute) { + const std::string report = oracle_->support_report(); + SCOPED_TRACE(report); + // Rows are ordered by shape class, so HalfSpace is always the second name. + EXPECT_NE(report.find("Sphere-HalfSpace"), std::string::npos); + EXPECT_NE(report.find("Box-HalfSpace"), std::string::npos); + EXPECT_NE(report.find("Capsule-HalfSpace"), std::string::npos); + EXPECT_NE(report.find("Cylinder-HalfSpace"), std::string::npos); + EXPECT_NE(report.find("Ellipsoid-HalfSpace"), std::string::npos); + EXPECT_NE(report.find("Convex-HalfSpace"), std::string::npos); + EXPECT_NE(report.find("halfspace analytic support-function fallback"), + std::string::npos); + EXPECT_NE(report.find("native (ComputeSignedDistancePairClosestPoints"), + std::string::npos); +} + +// ========================================================================== +// 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", Mesh(CubeObjPath()), 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(), kTau); + } + + 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.id.a == halfspace_id_) { + EXPECT_EQ(pair.route, DistanceRoute::kHalfSpaceA); + ++halfspace_pairs; + } else if (pair.id.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.id.body_a, pair.id.body_b); + EXPECT_EQ(pair.threshold, 0.0) << "the facade owns thresholds"; + } + EXPECT_EQ(halfspace_pairs, kDynamicBodies); + EXPECT_EQ(native_pairs, kNativePairs); +} + +TEST_F(AllShapesTest, ReportAnnouncesMeshAsConvexHull) { + const std::string report = oracle_->support_report(); + SCOPED_TRACE(report); + EXPECT_NE(report.find("Mesh mesh_geometry: certified as its convex hull"), + std::string::npos); + EXPECT_NE(report.find("distinct shape-type combination(s)"), + std::string::npos); + // 8 distinct classes, each present once: 8*7/2 = 28 combinations. + EXPECT_NE(report.find("28 distinct shape-type combination(s)"), + std::string::npos); +} + +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, IdOrderingIsSymmetricForNativePairs) { + std::mt19937 rng(777); + world_->RandomizeAll(&rng, 0.5); + const QueryObject& query = world_->query(); + + int checked = 0; + for (const PairRecord& pair : oracle_->pairs()) { + if (pair.route != DistanceRoute::kNative) continue; + PairRecord swapped = pair; + std::swap(swapped.id.a, swapped.id.b); + std::swap(swapped.id.body_a, swapped.id.body_b); + + 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); + ++checked; + } + EXPECT_EQ(checked, kNativePairs); +} + +TEST_F(AllShapesTest, IdOrderingIsSymmetricForHalfSpacePairs) { + std::mt19937 rng(778); + world_->RandomizeAll(&rng, 0.5); + const QueryObject& query = world_->query(); + + int checked = 0; + for (const PairRecord& pair : oracle_->pairs()) { + if (pair.route == DistanceRoute::kNative) continue; + PairRecord swapped = pair; + std::swap(swapped.id.a, swapped.id.b); + std::swap(swapped.id.body_a, swapped.id.body_b); + 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); + ++checked; + } + EXPECT_EQ(checked, kDynamicBodies); +} + +/// Records, for the certifier and benchmark authors, which (shape, shape) +/// combinations Drake's native narrowphase actually supports on the pinned +/// build. The oracle routes every halfspace pair through the analytic fallback +/// precisely because of the rows this test prints. +TEST_F(AllShapesTest, NativeSupportTableSnapshot) { + const auto& inspector = world_->diagram().scene_graph().model_inspector(); + const QueryObject& query = world_->query(); + std::string table = + "Native ComputeSignedDistancePairClosestPoints support:\n"; + int supported = 0; + int threw = 0; + for (const PairRecord& pair : oracle_->pairs()) { + const std::string combo = + std::string(inspector.GetShape(pair.id.a).type_name()) + "-" + + std::string(inspector.GetShape(pair.id.b).type_name()); + try { + query.ComputeSignedDistancePairClosestPoints(pair.id.a, pair.id.b); + table += " " + combo + ": supported\n"; + ++supported; + } catch (const std::exception&) { + table += " " + combo + ": THROWS\n"; + ++threw; + } + } + std::cout << table << std::flush; + EXPECT_EQ(supported + threw, static_cast(oracle_->pairs().size())); + // All non-halfspace combinations must work natively -- exactly what the + // capability probe asserted at construction. + EXPECT_GE(supported, kNativePairs); +} + +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()); + + try { + const DistanceOracle oracle(world.diagram(), kTau); + ADD_FAILURE() << "expected the capability probe to refuse the pair"; + } catch (const std::exception& e) { + const std::string what = e.what(); + SCOPED_TRACE(what); + EXPECT_NE(what.find("HalfSpace"), std::string::npos); + EXPECT_NE(what.find("ground_geometry"), std::string::npos); + EXPECT_NE(what.find("ceiling_geometry"), std::string::npos); + } +} + +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(), kTau); + const DistanceOracle second(world.diagram(), kTau); + EXPECT_EQ(first.support_report(), second.support_report()); + EXPECT_EQ(first.tolerance(), kTau); + ASSERT_EQ(first.pairs().size(), second.pairs().size()); + for (size_t i = 0; i < first.pairs().size(); ++i) { + EXPECT_EQ(first.pairs()[i].id.a, second.pairs()[i].id.a); + EXPECT_EQ(first.pairs()[i].id.b, second.pairs()[i].id.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()); + + try { + const DistanceOracle oracle(world.diagram(), kTau); + ADD_FAILURE() << "expected the probe to refuse the deformable geometry"; + } catch (const std::exception& e) { + const std::string what = e.what(); + SCOPED_TRACE(what); + EXPECT_NE(what.find("deformable"), std::string::npos); + EXPECT_NE(what.find("squishy"), std::string::npos); + } +} + +GTEST_TEST(DistanceOracleProbe, EmptyWorldProbesCleanly) { + RobotDiagramBuilder builder(0.0); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram(), kTau); + EXPECT_TRUE(oracle.pairs().empty()); + EXPECT_NE(oracle.support_report().find("0 unfiltered pair(s)"), + std::string::npos); +} + +// ========================================================================== +// 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", Mesh(CubeObjPath()), 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(), kTau); + + const GeometryId mesh_id = GeometryOf(world.plant(), "mesh"); + const GeometryId convex_id = GeometryOf(world.plant(), "convex"); + const GeometryId probe_id = GeometryOf(world.plant(), "probe"); + const PairRecord& mesh_pair = FindPair(oracle, mesh_id, probe_id); + const PairRecord& convex_pair = FindPair(oracle, convex_id, 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); + const double phi_convex = oracle.SignedDistance(query, convex_pair); + EXPECT_NEAR(phi_mesh, phi_convex, kExact) << "trial " << trial; + if (phi_mesh > 0) { + ++separated; + } else { + ++penetrating; + } + } + EXPECT_GT(separated, 0); + EXPECT_GT(penetrating, 0); +} + +/// Documents the semantics loudly (the geometry-support scope; the risk +/// register): 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(LPrismObjPath()), Vector3d(0, 0, 0)); + const auto& l_convex_body = AddShapeBody( + &plant, "l_convex", Convex(LPrismObjPath()), 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(), kTau); + + const GeometryId mesh_id = GeometryOf(world.plant(), "l_mesh"); + const GeometryId convex_id = GeometryOf(world.plant(), "l_convex"); + const GeometryId probe_id = GeometryOf(world.plant(), "probe"); + + // (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. + const double true_surface_clearance = 0.4 - probe_radius; + ASSERT_GT(true_surface_clearance, 0.0); + + world.SetPose(l_mesh_body, RigidTransformd::Identity()); + world.SetPose(l_convex_body, RigidTransformd::Identity()); + world.SetPose(probe_body, RigidTransformd(Vector3d(1.4, 1.4, 0.0))); + + { + const QueryObject& query = world.query(); + const double phi_mesh = + oracle.SignedDistance(query, FindPair(oracle, mesh_id, probe_id)); + const double phi_convex = + oracle.SignedDistance(query, FindPair(oracle, convex_id, probe_id)); + EXPECT_NEAR(phi_mesh, phi_convex, kExact); + EXPECT_LT(phi_mesh, 0.0) + << "the L-mesh must measure as its convex hull, which swallows the " + "notch; got phi = " + << phi_mesh; + } + + // Outside the hull too => both agree and both are positive. + world.SetPose(probe_body, RigidTransformd(Vector3d(1.9, 1.9, 0.0))); + { + const QueryObject& query = world.query(); + const double phi_mesh = + oracle.SignedDistance(query, FindPair(oracle, mesh_id, probe_id)); + const double phi_convex = + oracle.SignedDistance(query, FindPair(oracle, convex_id, probe_id)); + EXPECT_GT(phi_mesh, 0.0); + EXPECT_NEAR(phi_mesh, phi_convex, kExact); + } +} + +GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + const auto& mesh_body = + AddShapeBody(&plant, "mesh", Mesh(CubeObjPath()), Vector3d(0, 0, 1)); + const auto& convex_body = + AddShapeBody(&plant, "convex", Convex(CubeObjPath()), 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(), kTau); + + const GeometryId mesh_id = GeometryOf(world.plant(), "mesh"); + const GeometryId convex_id = 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, FindPair(oracle, ground, mesh_id)); + const double phi_convex = + oracle.SignedDistance(query, FindPair(oracle, ground, convex_id)); + EXPECT_NEAR(phi_mesh, phi_convex, 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; + } +} + +// ========================================================================== +// V-polytope ingestion round trip. +// ========================================================================== + +/// A deliberately lopsided polytope. +Matrix3Xd PolytopeVertices() { + Matrix3Xd v(3, 6); + v.col(0) = Vector3d(0.00, 0.00, 0.00); + v.col(1) = Vector3d(0.28, 0.03, 0.01); + v.col(2) = Vector3d(0.05, 0.31, -0.02); + v.col(3) = Vector3d(-0.04, 0.06, 0.24); + v.col(4) = Vector3d(0.22, 0.25, 0.19); + v.col(5) = Vector3d(0.10, -0.18, 0.11); + return v; +} + +/// The same polytope with interior points that add nothing to the hull. +Matrix3Xd RedundantPolytopeVertices() { + const Matrix3Xd v = PolytopeVertices(); + Matrix3Xd r(3, v.cols() + 3); + r.leftCols(v.cols()) = v; + r.col(v.cols() + 0) = v.rowwise().mean(); + r.col(v.cols() + 1) = 0.5 * (v.col(0) + v.col(4)); + r.col(v.cols() + 2) = 0.25 * (v.col(1) + v.col(2) + v.col(3) + v.col(5)); + return r; +} + +GTEST_TEST(VPolytopeIngestion, RoundTripMatchesDirectConvexRegistration) { + const Matrix3Xd vertices = PolytopeVertices(); + const VPolytope vpoly(vertices); + const VPolytope redundant_vpoly{RedundantPolytopeVertices()}; + + const RigidTransformd X_WG(RotationMatrixd(Eigen::AngleAxisd( + 0.7, Vector3d(0.3, -0.5, 0.8).normalized())), + Vector3d(0.2, -0.1, 0.35)); + + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + const GeometryId ingested = + AddVPolytopeObstacle(&plant, vpoly, X_WG, "ingested_vpolytope"); + const GeometryId ingested_redundant = + AddVPolytopeObstacle(&plant, redundant_vpoly, X_WG, "ingested_redundant"); + const GeometryId direct = plant.RegisterCollisionGeometry( + plant.world_body(), X_WG, Convex(vertices, "direct_convex"), + "direct_convex", Friction()); + const auto& probe_body = + AddShapeBody(&plant, "probe", Sphere(0.06), Vector3d(2, 2, 2)); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram(), kTau); + const GeometryId probe_id = GeometryOf(world.plant(), "probe"); + + // Three anchored obstacles against one dynamic probe. Anchored-anchored + // pairs are filtered by SceneGraph, so exactly three pairs survive. + ASSERT_EQ(oracle.pairs().size(), 3u); + + const PairRecord& p_ingested = FindPair(oracle, ingested, probe_id); + const PairRecord& p_redundant = + FindPair(oracle, ingested_redundant, probe_id); + const PairRecord& p_direct = FindPair(oracle, direct, probe_id); + + std::mt19937 rng(2718); + int separated = 0; + int penetrating = 0; + for (int trial = 0; trial < 120; ++trial) { + world.SetPose(probe_body, RandomPose(&rng, 0.5)); + const QueryObject& query = world.query(); + + Vector3d a_i; + Vector3d b_i; + Vector3d a_d; + Vector3d b_d; + const double phi_ingested = + oracle.SignedDistance(query, p_ingested, &a_i, &b_i); + const double phi_direct = + oracle.SignedDistance(query, p_direct, &a_d, &b_d); + const double phi_redundant = oracle.SignedDistance(query, p_redundant); + + EXPECT_NEAR(phi_ingested, phi_direct, kExact) << "trial " << trial; + EXPECT_NEAR(phi_redundant, phi_direct, kExact) << "trial " << trial; + if (phi_direct > 1e-9) { + ++separated; + EXPECT_LT((a_i - a_d).norm(), kExact) << "trial " << trial; + EXPECT_LT((b_i - b_d).norm(), kExact) << "trial " << trial; + } else { + ++penetrating; + } + } + EXPECT_GT(separated, 0); + EXPECT_GT(penetrating, 0); +} + +GTEST_TEST(VPolytopeIngestion, RoundTripAlsoHoldsOnTheHalfSpaceFallbackRoute) { + const Matrix3Xd vertices = PolytopeVertices(); + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + // Both copies ride floating bodies so the anchored halfspace can see them. + const VPolytope vpoly(vertices); + const auto& ingested_body = AddShapeBody( + &plant, "ingested", vpoly.ToShapeConvex("ingested"), Vector3d(0, 0, 1)); + const auto& direct_body = AddShapeBody( + &plant, "direct", Convex(vertices, "direct"), Vector3d(0, 2, 1)); + const GeometryId ground = plant.RegisterCollisionGeometry( + plant.world_body(), RigidTransformd::Identity(), HalfSpace(), + "ground_geometry", Friction()); + World world(builder.Build()); + const DistanceOracle oracle(world.diagram(), kTau); + + const GeometryId ingested_id = GeometryOf(world.plant(), "ingested"); + const GeometryId direct_id = GeometryOf(world.plant(), "direct"); + + std::mt19937 rng(1618); + for (int trial = 0; trial < 100; ++trial) { + const RigidTransformd X_W = RandomPose(&rng, 0.3); + world.SetPose(ingested_body, X_W); + world.SetPose(direct_body, X_W); + const QueryObject& query = world.query(); + const double phi_ingested = + oracle.SignedDistance(query, FindPair(oracle, ground, ingested_id)); + const double phi_direct = + oracle.SignedDistance(query, FindPair(oracle, ground, direct_id)); + EXPECT_NEAR(phi_ingested, phi_direct, kExact) << "trial " << trial; + // Reference: the lowest transformed vertex, since the ground is z = 0. + double lowest = std::numeric_limits::infinity(); + for (int i = 0; i < vertices.cols(); ++i) { + lowest = std::min(lowest, (X_W * Vector3d(vertices.col(i))).z()); + } + EXPECT_NEAR(phi_direct, lowest, kExact) << "trial " << trial; + } +} + +GTEST_TEST(VPolytopeIngestion, RejectsBadArguments) { + const VPolytope vpoly(PolytopeVertices()); + EXPECT_THROW( + AddVPolytopeObstacle(nullptr, vpoly, RigidTransformd::Identity(), "x"), + std::exception); + + { // 2-D polytope. + RobotDiagramBuilder builder(0.0); + Eigen::MatrixXd square(2, 4); + square << 0, 1, 1, 0, 0, 0, 1, 1; + const VPolytope flat(square); + EXPECT_THROW(AddVPolytopeObstacle(&builder.plant(), flat, + RigidTransformd::Identity(), "flat"), + std::exception); + } + { // Post-finalize. + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + plant.Finalize(); + EXPECT_THROW(AddVPolytopeObstacle(&plant, vpoly, + RigidTransformd::Identity(), "late"), + std::exception); + } +} + +} // namespace +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/vpolytope_ingestion.cc b/planning/certified_ccd/vpolytope_ingestion.cc new file mode 100644 index 000000000000..e3414aeb12bb --- /dev/null +++ b/planning/certified_ccd/vpolytope_ingestion.cc @@ -0,0 +1,62 @@ +#include "drake/planning/certified_ccd/vpolytope_ingestion.h" + +#include + +#include "drake/common/drake_throw.h" +#include "drake/geometry/shape_specification.h" +#include "drake/multibody/plant/coulomb_friction.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +using drake::geometry::GeometryId; +using drake::math::RigidTransformd; +using drake::multibody::CoulombFriction; +using drake::multibody::MultibodyPlant; + +namespace { +// Signed distance never reads friction, but RegisterCollisionGeometry demands +// some proximity properties. Unit friction is Drake's own conventional +// placeholder for "the caller does not care". +constexpr double kDefaultFriction = 1.0; +} // namespace + +GeometryId AddVPolytopeObstacle( + MultibodyPlant* plant, + const drake::geometry::optimization::VPolytope& vpoly, + const RigidTransformd& X_WG, const std::string& name) { + DRAKE_THROW_UNLESS(plant != nullptr); + if (plant->is_finalized()) { + throw std::runtime_error( + "AddVPolytopeObstacle(): cannot add obstacle '" + name + + "' because the plant is already finalized; register V-polytope " + "obstacles before calling MultibodyPlant::Finalize()."); + } + if (vpoly.ambient_dimension() != 3) { + throw std::runtime_error( + "AddVPolytopeObstacle(): obstacle '" + name + + "' has ambient " + "dimension " + + std::to_string(vpoly.ambient_dimension()) + + "; only 3-dimensional V-polytopes can be registered as geometry."); + } + if (vpoly.vertices().cols() == 0) { + throw std::runtime_error("AddVPolytopeObstacle(): obstacle '" + name + + "' has an empty vertex set."); + } + + // Drake's pinned VPolytope -> Convex entry point; it forwards the vertex + // matrix to Convex(Matrix3X, label, scale=1). The hull is computed lazily + // by Convex::GetConvexHull() and is the object the proximity engine + // actually collides. + const drake::geometry::Convex convex = vpoly.ToShapeConvex(name); + + return plant->RegisterCollisionGeometry( + plant->world_body(), X_WG, convex, name, + CoulombFriction(kDefaultFriction, kDefaultFriction)); +} + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/vpolytope_ingestion.h b/planning/certified_ccd/vpolytope_ingestion.h new file mode 100644 index 000000000000..4bdca888df00 --- /dev/null +++ b/planning/certified_ccd/vpolytope_ingestion.h @@ -0,0 +1,49 @@ +#pragma once + +#include + +#include "drake/geometry/geometry_ids.h" +#include "drake/geometry/optimization/vpolytope.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** Registers a V-polytope as an anchored obstacle with a collision role +(the geometry-support scope, "V-polytopes as first-class geometry", ingestion +route (b)). + +The polytope is converted to `drake::geometry::Convex` through Drake's own +`VPolytope::ToShapeConvex()` entry point (a thin wrapper over the +`Convex(Eigen::Matrix3X points, std::string label, double scale)` +constructor pinned at M0), then registered on the plant's world body. The +result therefore rides the ordinary native narrowphase path end to end: the +proximity engine and the certifier's radius/support code all read the same +`Convex::GetConvexHull()` object, so the certificate stays sound even for +redundant or degenerate vertex sets. + +@param plant The plant to register on. Must be non-null, must already be a + registered SceneGraph source, and must NOT be finalized. +@param vpoly The polytope. Its vertices are interpreted in the geometry + frame G, i.e. the world-frame obstacle is + `X_WG * conv(vpoly.vertices())`. Must be 3-dimensional with at + least one vertex. +@param X_WG Pose of the geometry frame in the world frame. +@param name Geometry name; also used as the `Convex` shape's label (which + Drake only uses in its own warning/error messages). Must not + contain a newline. +@returns The id of the newly registered collision geometry. +@throws std::exception if `plant` is null or already finalized, if + `vpoly.ambient_dimension() != 3`, if the vertex set is empty, or if + Drake rejects the resulting hull (e.g. a degenerate vertex set that + its hull computation cannot inflate). */ +drake::geometry::GeometryId AddVPolytopeObstacle( + drake::multibody::MultibodyPlant* plant, + const drake::geometry::optimization::VPolytope& vpoly, + const drake::math::RigidTransform& X_WG, const std::string& name); + +} // namespace certified_ccd +} // namespace planning +} // namespace drake From 4f76169713dbe509c6462489c8e23c3a7b663f33 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 14:39:55 -0400 Subject: [PATCH 04/22] [planning] Add certified_ccd: the interval certifier Adds the internal driver: the breakpoint pre-pass, the static-pair resolution, the adaptive de Casteljau node recursion that proves clearance over whole parameter intervals, and the parallel driver (a shared LIFO work source with occupancy-driven sharing and lazy recruitment) whose answers are identical to the serial path. Also adds the certificate audit trail and VerifyCertificate, an independent replay that re-derives every recorded certification event from the path and the recorded distances alone. The two live in one library because the replay and the recursion share the per-call data structures. The end-to-end tests for this code arrive with the public facade in the following commit. --- planning/certified_ccd/BUILD.bazel | 34 + planning/certified_ccd/certificate.cc | 353 +++++++ planning/certified_ccd/certificate.h | 38 + planning/certified_ccd/certifier.cc | 1324 +++++++++++++++++++++++++ planning/certified_ccd/certifier.h | 375 +++++++ 5 files changed, 2124 insertions(+) create mode 100644 planning/certified_ccd/certificate.cc create mode 100644 planning/certified_ccd/certificate.h create mode 100644 planning/certified_ccd/certifier.cc create mode 100644 planning/certified_ccd/certifier.h diff --git a/planning/certified_ccd/BUILD.bazel b/planning/certified_ccd/BUILD.bazel index 1af1ee1e9add..55afdfd4a2c3 100644 --- a/planning/certified_ccd/BUILD.bazel +++ b/planning/certified_ccd/BUILD.bazel @@ -13,6 +13,7 @@ drake_cc_package_library( visibility = ["//visibility:public"], deps = [ ":bounding_sphere", + ":certifier", ":distance_oracle", ":motion_bound_table", ":numerics", @@ -132,6 +133,39 @@ drake_cc_library( ], ) +# The certificate and the node recursion are mutually recursive translation +# units (certificate.cc replays the events that certifier.cc emits), so they +# form one library, exactly as they formed one module in the standalone +# package. +drake_cc_library( + name = "certifier", + srcs = [ + "certificate.cc", + "certifier.cc", + ], + hdrs = [ + "certificate.h", + "certifier.h", + ], + deps = [ + ":distance_oracle", + ":motion_bound_table", + ":numerics", + ":options", + ":piecewise_bezier_path", + "//geometry:scene_graph", + "//multibody/tree:multibody_tree_indexes", + "//planning:robot_diagram", + "//systems/framework:context", + "@eigen", + ], + implementation_deps = [ + "//common:essential", + "//multibody/plant", + "@fmt", + ], +) + # === test/ === # T1 — curve module acceptance tests. diff --git a/planning/certified_ccd/certificate.cc b/planning/certified_ccd/certificate.cc new file mode 100644 index 000000000000..78f6725ea1f7 --- /dev/null +++ b/planning/certified_ccd/certificate.cc @@ -0,0 +1,353 @@ +#include "drake/planning/certified_ccd/certificate.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_throw.h" +#include "drake/planning/certified_ccd/certifier.h" +#include "drake/planning/certified_ccd/numerics.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace internal { +namespace { + +/* Slop allowed between the certifier's arithmetic and the replay's. The two + compute the same quantities by *different* routes — repeated halving versus a + pair of arbitrary-u de Casteljau subdivisions — so they agree only to rounding + (both routes are sequences of convex combinations, hence numerically benign, + and in practice differ by ~1e-15·‖q‖). This tolerance sits far below anything + a tamperer could hide in and far above the true rounding gap. */ +constexpr double kReplayTolerance = 1e-9; + +/* One de Casteljau subdivision at u ∈ [0, 1]: `left` receives the control + points of the restriction to [0, u] and `right` those of the restriction to + [u, 1] (both n × (m+1)). The triangle b_j^r = (1−u)·b_j^{r−1} + u·b_{j+1}^{r−1} + is built in place inside `right`; its first column after round r is the left + child's r-th control point and the column it leaves at m−r is the right + child's. Written out here, rather than reused from the curve module, so that + the replay is genuinely independent of the code path it audits. */ +void SplitAt(const Eigen::MatrixXd& cps, double u, Eigen::MatrixXd* left, + Eigen::MatrixXd* right) { + const int m = static_cast(cps.cols()) - 1; + left->resize(cps.rows(), cps.cols()); + *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) = (1.0 - u) * right->col(j) + u * right->col(j + 1); + } + left->col(r) = right->col(0); + } +} + +} // namespace + +void RestrictBezier(const Eigen::MatrixXd& cps, double a, double b, + Eigen::MatrixXd* out) { + DRAKE_THROW_UNLESS(out != nullptr); + const double lo = std::clamp(a, 0.0, 1.0); + const double hi = std::clamp(b, 0.0, 1.0); + if (lo <= 0.0 && hi >= 1.0) { + *out = cps; + return; + } + Eigen::MatrixXd scratch; + Eigen::MatrixXd tail; + if (lo <= 0.0) { + tail = cps; + } else { + SplitAt(cps, lo, &scratch, &tail); + } + // `tail` is the curve on [lo, 1] in its own parameter v ∈ [0, 1]; the + // original parameter hi lands at v = (hi − lo)/(1 − lo). + const double span = 1.0 - lo; + const double v = (span > 0.0) ? std::clamp((hi - lo) / span, 0.0, 1.0) : 1.0; + if (v >= 1.0) { + *out = std::move(tail); + return; + } + SplitAt(tail, v, out, &scratch); +} + +Eigen::VectorXd EvaluateBezier(const Eigen::MatrixXd& cps, double u) { + const int m = static_cast(cps.cols()) - 1; + Eigen::MatrixXd work = cps; + 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); +} + +bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, + std::string* message) { + DRAKE_THROW_UNLESS(input.model != nullptr); + DRAKE_THROW_UNLESS(input.oracle != nullptr); + DRAKE_THROW_UNLESS(input.table != nullptr); + DRAKE_THROW_UNLESS(input.path != nullptr); + DRAKE_THROW_UNLESS(input.pairs != nullptr); + DRAKE_THROW_UNLESS(input.tau != nullptr); + + const auto fail = [message](std::string reason) { + if (message != nullptr) *message = std::move(reason); + return false; + }; + + const PiecewiseBezierPath& path = *input.path; + const std::vector& pairs = *input.pairs; + const std::vector& tau = *input.tau; + const MotionBoundTable& table = *input.table; + const int num_pairs = static_cast(pairs.size()); + const int num_segments = static_cast(path.segments().size()); + const int num_positions = path.num_positions(); + + // --- 1. The pair snapshot must be the checker's own table. --------------- + // Without this the record indices mean nothing, and every later check could + // be aimed at the wrong geometries. + if (static_cast(certificate.pairs.size()) != num_pairs) { + return fail( + fmt::format("certificate covers {} pair(s) but the checker has {}.", + certificate.pairs.size(), num_pairs)); + } + for (int p = 0; p < num_pairs; ++p) { + if (certificate.pairs[p].a != pairs[p].id.a || + certificate.pairs[p].b != pairs[p].id.b) { + return fail(fmt::format( + "certificate pair {} does not match the checker's pair table.", p)); + } + } + if (table.num_pairs() != num_pairs) { + return fail("the motion-bound table does not match the pair table."); + } + + // --- 2. Replay every record. --------------------------------------------- + ThreadContext context(*input.model); + Eigen::MatrixXd restricted; + Eigen::VectorXd w(num_positions); + Eigen::VectorXd last_qc; + std::vector claimed_threshold( + num_pairs, std::numeric_limits::quiet_NaN()); + + for (int r = 0; r < static_cast(certificate.records.size()); ++r) { + const CertificateRecord& record = certificate.records[r]; + const int p = record.pair_index; + if (p < 0 || p >= num_pairs) { + return fail( + fmt::format("record {} names pair index {}, out of range.", r, p)); + } + if (record.segment < 0 || record.segment >= num_segments) { + return fail(fmt::format("record {} names segment {}, out of range.", r, + record.segment)); + } + if (!(record.s_start >= 0.0) || !(record.s_end <= 1.0) || + !(record.s_start < record.s_end)) { + return fail(fmt::format( + "record {} has a degenerate or out-of-range interval [{}, {}].", r, + record.s_start, record.s_end)); + } + if (record.qc.size() != num_positions) { + return fail(fmt::format( + "record {} carries a representative configuration of size {}; the " + "plant has {} positions.", + r, record.qc.size(), num_positions)); + } + if (!std::isfinite(record.phi_hat) || !std::isfinite(record.motion_bound) || + !std::isfinite(record.threshold) || record.motion_bound < 0.0) { + return fail( + fmt::format("record {} carries non-finite or negative data.", r)); + } + // Every record of a pair must claim the same threshold: a certificate that + // silently lowers m_p on some intervals proves nothing coherent. + if (std::isnan(claimed_threshold[p])) { + claimed_threshold[p] = record.threshold; + } else if (claimed_threshold[p] != record.threshold) { + return fail(fmt::format( + "pair {} is certified against two different thresholds ({} and {}).", + p, claimed_threshold[p], record.threshold)); + } + // ... and the threshold it claims must be at least the one the caller + // expects. Checking only self-consistency would let a certificate whose + // records all say "threshold = -1e9" verify: it would be a true statement + // about a claim nobody asked for. + const double expected = pairs[p].threshold; + if (!(record.threshold >= + expected - kReplayTolerance * std::max(1.0, std::abs(expected)))) { + return fail(fmt::format( + "record {}: pair {} is certified only against threshold {}, which is " + "below the {} the checker's options call for.", + r, p, record.threshold, expected)); + } + + const bool is_static = table.pair_is_static(p); + double motion_bound = 0.0; + if (!is_static) { + // Re-restrict the segment's control points to the record's interval and + // recompute w about the record's qc from scratch. This is the half of + // the certificate the checker must not be believed on. + RestrictBezier(path.segments()[record.segment].control_points, + record.s_start, record.s_end, &restricted); + + // qc must be the node's own midpoint apex, i.e. a configuration exactly + // on the trajectory. (A qc merely inside the control box would still be + // sound by the displacement lemma, but pinning it to the apex is what + // the certifier emits, and it makes a tampered qc detectable.) + const Eigen::VectorXd apex = EvaluateBezier(restricted, 0.5); + const double qc_error = (apex - record.qc).cwiseAbs().maxCoeff(); + // Relative: a plant with large coordinate values (an unbounded prismatic + // joint, say) rounds proportionally, and the check must not turn into a + // scale-dependent false alarm. + const double qc_limit = + kReplayTolerance * std::max(1.0, record.qc.cwiseAbs().maxCoeff()); + if (!(qc_error <= qc_limit)) { + return fail(fmt::format( + "record {}: the stored representative configuration is not the " + "midpoint of the interval it claims (off by {}).", + r, qc_error)); + } + + w.setZero(); + for (int j = 0; j < restricted.cols(); ++j) { + for (int i = 0; i < num_positions; ++i) { + w[i] = std::max(w[i], std::abs(restricted(i, j) - record.qc[i])); + } + } + motion_bound = table.MotionBound(p, w); + if (!(record.motion_bound >= + motion_bound - kReplayTolerance * std::max(1.0, motion_bound))) { + return fail(fmt::format( + "record {}: the stored motion bound {} understates the recomputed " + "bound {}.", + r, record.motion_bound, motion_bound)); + } + } + // For a static pair J(p) = ∅: no coordinate the trajectory *moves* changes + // the pair's relative pose, so Δ_p ≡ 0 and one measurement certifies the + // whole domain. A *non*-static pair cannot smuggle in such a record: the + // recomputed Δ above would be the full node's bound and the test below + // would reject it. + // + // "Static" is relative to the constant-coordinate carve-out (trajectory + // normalization; the displacement lemma), so coordinates this path happens + // to hold fixed still move the pair in general — which means the + // representative configuration has to be pinned to the path, exactly as the + // certifier pins it (q(t0)), or a record could be re-based onto an off-path + // configuration that measures more clearance. + if (is_static) { + const Eigen::VectorXd q0 = path.segments()[0].control_points.col(0); + const double qc_error = (q0 - record.qc).cwiseAbs().maxCoeff(); + if (!(qc_error <= kReplayTolerance * + std::max(1.0, record.qc.cwiseAbs().maxCoeff()))) { + return fail(fmt::format( + "record {}: pair {} is certified statically from a configuration " + "that is not the path's start (off by {}).", + r, p, qc_error)); + } + } + + // Re-measure the distance ourselves. Records are emitted sorted, so the + // several pairs certified at one node arrive adjacently and share a qc; + // skipping the redundant SetPositions saves that many forward-kinematics + // evaluations on what is otherwise a linear scan of the whole audit trail. + if (last_qc.size() != record.qc.size() || + !(last_qc.array() == record.qc.array()).all()) { + context.SetPositions(record.qc); + last_qc = record.qc; + } + const double phi_replay = + input.oracle->SignedDistance(context.query_object(), pairs[p]); + const double tau_p = tau[p]; + // Coherence: a record may legitimately store *less* than the narrowphase + // reports (the sphere-prefilter branch stores a lower bound on φ), but it + // may never claim more than the oracle's own contract allows. + if (!(record.phi_hat <= phi_replay + tau_p + kReplayTolerance)) { + return fail(fmt::format( + "record {}: the stored clearance {} over-reports the independently " + "measured {} (pair {}).", + r, record.phi_hat, phi_replay, p)); + } + // The certificate test runs on min(stored, re-measured), so an inflated + // φ̂ can never buy a record anything: only the value this replay measured + // for itself can carry the inequality. Both are lower bounds we are + // entitled to charge τ_p against, and for an honest record the stored + // value is the smaller one (identical for a narrowphase record, the + // sphere bound for a prefilter record), so nothing legitimate is lost. + const double effective_phi = std::min(record.phi_hat, phi_replay); + if (!IsCertified(effective_phi, tau_p, motion_bound, record.threshold, + input.slack)) { + return fail(fmt::format( + "record {}: phi_hat {} - tau {} - Delta {} does not exceed threshold " + "{} + slack {} (pair {}, segment {}, [{}, {}]).", + r, effective_phi, tau_p, motion_bound, record.threshold, input.slack, + p, record.segment, record.s_start, record.s_end)); + } + } + + // --- 3. Coverage. -------------------------------------------------------- + // The certified intervals must tile [0, 1] of every segment for every pair. + // Without this a certificate could consist of a handful of perfectly valid + // records and still prove nothing about the parts of the path they miss. + struct Interval { + int pair; + int segment; + double lo; + double hi; + }; + std::vector intervals; + intervals.reserve(certificate.records.size()); + for (const CertificateRecord& record : certificate.records) { + intervals.push_back(Interval{record.pair_index, record.segment, + record.s_start, record.s_end}); + } + std::sort(intervals.begin(), intervals.end(), + [](const Interval& a, const Interval& b) { + if (a.pair != b.pair) return a.pair < b.pair; + if (a.segment != b.segment) return a.segment < b.segment; + return a.lo < b.lo; + }); + + std::size_t cursor = 0; + for (int p = 0; p < num_pairs; ++p) { + for (int k = 0; k < num_segments; ++k) { + const std::size_t begin = cursor; + while (cursor < intervals.size() && intervals[cursor].pair == p && + intervals[cursor].segment == k) { + ++cursor; + } + double covered_to = 0.0; + for (std::size_t i = begin; i < cursor; ++i) { + // Sorted by lo, so a start beyond the covered prefix is a real gap. + // Compared exactly and deliberately: the certifier's intervals are + // dyadic and abut bit-for-bit (a child's endpoint *is* the parent's + // computed midpoint), so any slack here would only buy a forged + // certificate the right to excise a sliver at every one of its + // thousands of record boundaries. + if (intervals[i].lo > covered_to) break; + covered_to = std::max(covered_to, intervals[i].hi); + } + if (!(covered_to >= 1.0)) { + return fail(fmt::format( + "pair {} is certified only up to s = {} of segment {}; the " + "certificate does not cover the whole path.", + p, covered_to, k)); + } + } + } + + if (message != nullptr) message->clear(); + return true; +} + +} // namespace internal +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/certificate.h b/planning/certified_ccd/certificate.h new file mode 100644 index 000000000000..621ae5b89c21 --- /dev/null +++ b/planning/certified_ccd/certificate.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include + +#include "drake/planning/certified_ccd/options.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** One certification event: pair `pair_index` was certified over the +parameter interval [s_start, s_end] of segment `segment` from representative +configuration qc (the search algorithm). */ +struct CertificateRecord { + int segment{}; + double s_start{}; + double s_end{}; + int pair_index{}; + Eigen::VectorXd qc; + double phi_hat{}; + double motion_bound{}; + double threshold{}; +}; + +/** Audit trail of every certification event of a run; an independent +replay (VerifyCertificate, declared in the api header) re-evaluates every +record and checks interval coverage of the full domain per pair. */ +struct Certificate { + std::vector records; + /** Pair table snapshot the indices refer to. */ + std::vector pairs; +}; + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/certifier.cc b/planning/certified_ccd/certifier.cc new file mode 100644 index 000000000000..ff042cd55773 --- /dev/null +++ b/planning/certified_ccd/certifier.cc @@ -0,0 +1,1324 @@ +#include "drake/planning/certified_ccd/certifier.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_throw.h" +#include "drake/geometry/scene_graph.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/planning/certified_ccd/numerics.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +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 certificate is + invariant under time reparametrization (the soundness argument, T6). */ +double TimeOf(const BezierSegment& seg, double s) { + return seg.t_start + s * (seg.t_end - seg.t_start); +} + +// --------------------------------------------------------------------------- +// Per-node world-frame geometry sphere centers. +// --------------------------------------------------------------------------- + +/* Caches one world-frame bounding-sphere center per geometry per node + (requirement P2: 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]; + } + + double radius(int slot) const { return table_->geometries[slot].radius; } + + private: + const PrefilterTable* table_{}; + const ThreadContext* context_{}; + std::vector center_W_; + std::vector stamp_of_; + std::uint64_t stamp_{1}; +}; + +// --------------------------------------------------------------------------- +// Findings sink. +// --------------------------------------------------------------------------- + +/* Collects findings from every worker. Cold path: guarded by one mutex. + Each list keeps only the `cap` earliest entries, so memory stays bounded no + matter how many violating nodes a pathological trajectory produces, while the + "earliest-first" contract of CertificationResult::findings is preserved + exactly (dropping the *latest* entry can never remove an earlier one). */ +class FindingSink { + public: + explicit FindingSink(int cap) : cap_(std::max(1, cap)) {} + + void AddDefinite(Finding finding) { + const double time = finding.time; + { + std::lock_guard guard(mutex_); + Insert(&definite_, std::move(finding)); + } + // Branch-and-bound bound for kFindFirstViolation (the search algorithm; + // parallelism and determinism): 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_); + Insert(&inconclusive_, std::move(finding)); + } + + double best_violation_time() const { + return best_violation_time_.load(std::memory_order_relaxed); + } + + /* Reports a node that was left unexplored when the node budget ran out; the + earliest such node over all workers is what the run reports (the search + algorithm: the budget "truncates in parameter order and reports the + remainder"). */ + void ReportPending(double time, const Eigen::VectorXd& q, int pair_index) { + std::lock_guard guard(mutex_); + if (!pending_valid_ || time < pending_time_) { + pending_valid_ = true; + pending_time_ = time; + pending_q_ = q; + pending_pair_ = pair_index; + } + } + + bool pending_valid() const { return pending_valid_; } + double pending_time() const { return pending_time_; } + const Eigen::VectorXd& pending_q() const { return pending_q_; } + int pending_pair() const { return pending_pair_; } + + const std::vector& definite() const { return definite_; } + const std::vector& inconclusive() const { return inconclusive_; } + + private: + void Insert(std::vector* list, Finding&& finding) { + if (static_cast(list->size()) >= cap_ && + finding.time >= list->back().time) { + return; + } + const auto position = + std::upper_bound(list->begin(), list->end(), finding.time, + [](double time, const Finding& other) { + return time < other.time; + }); + list->insert(position, std::move(finding)); + if (static_cast(list->size()) > cap_) list->pop_back(); + } + + const int cap_; + std::mutex mutex_; + std::vector definite_; + std::vector inconclusive_; + std::atomic best_violation_time_{kInfinity}; + bool pending_valid_{false}; + double pending_time_{kInfinity}; + Eigen::VectorXd pending_q_; + int pending_pair_{0}; +}; + +// --------------------------------------------------------------------------- +// Shared work source for the parallel driver (parallelism and determinism). +// --------------------------------------------------------------------------- + +/* 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. Requirement P1 (no per-node heap allocation) survives that + 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}; + int depth{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 / violation bound (parallelism and determinism), 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); + } + + /* Items never picked up; used to report what the node budget left + uncovered. Call only after every worker has finished. */ + std::vector& remaining() { return items_; } + + 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_before_hire{0}; + std::uint64_t nodes{0}; + std::function hire; +}; + +/* How many nodes a run must have visited before it hires helpers. + + This is a measured break-even, not a taste knob. Hiring costs one WorkerPool + reservation, one ContextPool lease, the construction of the helper Worker + objects, one notification per helper and — at the end of the run — one wakeup + per helper before the lead can collect their statistics: about 6-7 us per + helper, ~65 us for a full fifteen, on the machine the benchmark suite + was measured on. A node on that machine costs ~7-13 us. Sixteen nodes of work + already done is therefore roughly a 3x margin over the price of the helpers, + and it bounds the damage in the one case lazy recruitment cannot avoid — a + check that ends immediately after hiring — to that same ~65 us (~15% of such a + check). + + Everything smaller than this runs at exactly serial speed at any + Options::parallelism, which is the property that matters most in practice + because Parallelism::Max() is the default value of that field. */ +constexpr std::uint64_t kNodesBeforeHiringHelpers = 16; + +// --------------------------------------------------------------------------- +// The node loop. +// --------------------------------------------------------------------------- + +/* 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 depth{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 (requirement P1): + + - `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), + find_first_(input.options.mode == SearchMode::kFindFirstViolation), + emit_certificate_(input.options.emit_certificate) { + 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); + + const Statistics& stats() const { return stats_; } + std::vector& records() { return records_; } + + 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)); + } + } + + /* Appends one certification event to the audit trail (the search algorithm). + */ + void RecordCertification(int segment, double s_lo, double s_hi, int pair, + double phi_hat, double motion_bound, + double threshold) { + records_.push_back(CertificateRecord{segment, s_lo, s_hi, pair, q_mid_, + phi_hat, motion_bound, threshold}); + } + + 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_; + const bool find_first_{false}; + const bool emit_certificate_{false}; + + /* 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_; + + Statistics stats_; + std::vector records_; +}; + +void Worker::RunItem(WorkItem* item) { + const PiecewiseBezierPath& path = *input_.path; + const BezierSegment& segment = 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 Options& options = input_.options; + const double slack = options.certificate_slack; + const int rows = static_cast(item->control_points.rows()); + const int cols = static_cast(item->control_points.cols()); + const std::uint64_t max_nodes = + options.max_nodes.value_or(std::numeric_limits::max()); + + // 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, item->depth, 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 (the search algorithm; parallelism and + // determinism): a node starting at or after the earliest witness known so + // far cannot contain an earlier one. + if (find_first_ && + TimeOf(segment, frame.s_lo) >= sink_->best_violation_time()) { + continue; + } + if (node_counter_->fetch_add(1, std::memory_order_relaxed) >= max_nodes) { + // Budget exhausted: stop here and report the earliest node this worker + // leaves uncovered — which is exactly this one, because a left-first DFS + // pops in increasing parameter order and every frame still on the stack + // starts at or after this node's end. + sink_->ReportPending( + TimeOf(segment, frame.s_lo), slabs_[k].col(0), + frame.active_length > 0 ? arena_[frame.active_offset] : 0); + if (queue_ != nullptr) queue_->Abort(); + stack_.clear(); + return; + } + ++stats_.nodes; + stats_.max_depth = std::max(stats_.max_depth, frame.depth); + + // 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 >= recruit_->nodes_before_hire) { + 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 (trajectory normalization; the search + // algorithm). + 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 + // (the interval certificate). + 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 (requirement P2); 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); + // The resolution floor, plus a hard floating-point backstop: once the + // midpoint no longer separates the endpoints in double arithmetic the node + // cannot be split any further, whatever min_interval says. Without it a + // pathologically small min_interval would spin forever. + const bool at_floor = (frame.s_hi - frame.s_lo) <= options.min_interval || + !(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 threshold = pair.threshold; + const double tau_p = tau[p]; + // Δ_p(ν) = Σ_{j ∈ J(p)} λ(j,p)·w_j — a sparse dot product over this + // pair's CSR row (requirement P3). + const double motion_bound = table.MotionBound(p, w_); + + // --- Early-out 1: the free-sphere prefilter (requirements P4, P5). --- + // φ_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 certificate replay's 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) { + const double lower_bound = + (geometry_.Center(slot_a) - geometry_.Center(slot_b)).norm() - + geometry_.radius(slot_a) - geometry_.radius(slot_b); + if (IsCertified(lower_bound, tau_p, motion_bound, threshold, slack)) { + ++stats_.sphere_certifications; + if (emit_certificate_) { + RecordCertification(item->segment, frame.s_lo, frame.s_hi, p, + lower_bound, motion_bound, threshold); + } + continue; + } + } + + // --- Narrowphase. ---------------------------------------------------- + ++stats_.narrowphase_queries; + 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_p is a definite violation of the + // continuum statement, not a sampling artifact (the problem statement; + // the interval certificate). + Finding finding; + finding.time = t_mid; + finding.q = q_mid_; + finding.pair = pair.id; + finding.distance = phi_hat; + finding.motion_bound = motion_bound; + finding.definite = true; + finding.nearest_a_W = nearest_a_; + finding.nearest_b_W = nearest_b_; + sink_->AddDefinite(std::move(finding)); + if (!find_first_ || at_floor) { + // kCertifyAll (or a floor node, which has no children to refine + // into): drop p from this subtree. Without this a single + // violating pair would report one finding per node all the way down + // to the resolution floor; the earliest-first ordering and the + // max_reported_findings cap still apply, and every *disjoint* + // violating region of p is still reported because sibling subtrees + // carry their own copy of the active set. + continue; + } + // kFindFirstViolation: keep p active so the branch-and-bound recursion + // can refine the witness toward the earliest violating time. + } else if (IsCertified(phi_hat, tau_p, motion_bound, threshold, slack)) { + // 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_p + ε, + // 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 — the dominant work saver. + if (emit_certificate_) { + RecordCertification(item->segment, frame.s_lo, frame.s_hi, p, phi_hat, + motion_bound, threshold); + } + continue; + } + + // --- Gray: subdivide, unless we are already at the resolution floor. - + if (at_floor) { + Finding finding; + finding.time = t_mid; + finding.q = q_mid_; + finding.pair = pair.id; + finding.distance = phi_hat; + finding.motion_bound = motion_bound; + finding.definite = false; + finding.nearest_a_W = nearest_a_; + finding.nearest_b_W = nearest_b_; + sink_->AddInconclusive(std::move(finding)); + } else { + 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, frame.depth + 1, survivor_offset, + survivor_count}; + const NodeFrame left{frame.s_lo, s_mid, frame.depth + 1, 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_.depth = right.depth; + 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 (the search algorithm). + stack_.push_back(right); // slab k holds the right child. + stack_.push_back(left); // slab k+1 holds the left child. + } + } +} + +// --------------------------------------------------------------------------- +// Breakpoint pre-pass and static-pair resolution (the search algorithm, steps 1 +// and 2). +// --------------------------------------------------------------------------- + +/* 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, + Statistics* stats, + std::vector* records) { + const std::vector& pairs = *input.pairs; + const std::vector& tau = *input.tau; + const PrefilterTable& prefilter = *input.prefilter; + const MotionBoundTable& table = *input.table; + const double slack = input.options.certificate_slack; + const int num_segments = static_cast(input.path->segments().size()); + + 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 threshold = pair.threshold; + const double tau_p = tau[p]; + const bool is_static = table.pair_is_static(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 duplicate its finding (crowding out genuine ones + // under max_reported_findings) and 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->Center(slot_a) - geometry->Center(slot_b)).norm() - + geometry->radius(slot_a) - geometry->radius(slot_b); + } + + if (is_static) { + // Δ_p ≡ 0 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, 0.0, threshold, slack)) { + ++stats->sphere_certifications; + if (records != nullptr) { + for (int k = 0; k < num_segments; ++k) { + records->push_back(CertificateRecord{k, 0.0, 1.0, p, q, lower_bound, + 0.0, threshold}); + } + } + continue; + } + } else if (lower_bound >= threshold) { + // A definite violation needs φ̂ + τ_p < m_p, and φ̂ ≥ φ_true − τ_p ≥ + // lower_bound − τ_p, so lower_bound ≥ m_p rules one out with no query. + continue; + } + + ++stats->narrowphase_queries; + const double phi_hat = input.oracle->SignedDistance(query_object, pair, + &nearest_a, &nearest_b); + + if (IsDefiniteViolation(phi_hat, tau_p, threshold)) { + Finding finding; + finding.time = time; + finding.q = q; + finding.pair = pair.id; + finding.distance = phi_hat; + finding.motion_bound = 0.0; + finding.definite = true; + finding.nearest_a_W = nearest_a; + finding.nearest_b_W = nearest_b; + sink->AddDefinite(std::move(finding)); + continue; + } + if (!is_static) continue; + + if (IsCertified(phi_hat, tau_p, 0.0, threshold, slack)) { + if (records != nullptr) { + for (int k = 0; k < num_segments; ++k) { + records->push_back( + CertificateRecord{k, 0.0, 1.0, p, q, phi_hat, 0.0, threshold}); + } + } + continue; + } + // Neither certified nor violating, and no subdivision can help: this + // pair's clearance is constant along the trajectory and sits within oracle + // tolerance of the threshold. + Finding finding; + finding.time = time; + finding.q = q; + finding.pair = pair.id; + finding.distance = phi_hat; + finding.motion_bound = 0.0; + finding.definite = false; + finding.nearest_a_W = nearest_a; + finding.nearest_b_W = nearest_b; + sink->AddInconclusive(std::move(finding)); + } +} + +/* Orders the audit trail so that a run is comparable across thread counts and + across time reparametrizations (T6). */ +void SortRecords(std::vector* records) { + std::sort(records->begin(), records->end(), + [](const CertificateRecord& a, const CertificateRecord& b) { + if (a.segment != b.segment) return a.segment < b.segment; + if (a.s_start != b.s_start) return a.s_start < b.s_start; + if (a.s_end != b.s_end) return a.s_end < b.s_end; + return a.pair_index < b.pair_index; + }); +} + +} // namespace + +// --------------------------------------------------------------------------- +// ThreadContext / ContextPool. +// --------------------------------------------------------------------------- + +ThreadContext::ThreadContext(const drake::planning::RobotDiagram& model) + : model_(&model), root_(model.CreateDefaultContext()) { + plant_context_ = &model.plant().GetMyMutableContextFromRoot(root_.get()); + scene_graph_context_ = &model.scene_graph().GetMyContextFromRoot(*root_); +} + +void ThreadContext::SetPositions(const Eigen::VectorXd& q) { + model_->plant().SetPositions(plant_context_, q); +} + +const QueryObject& ThreadContext::query_object() const { + return model_->scene_graph() + .get_query_output_port() + .Eval>(*scene_graph_context_); +} + +const RigidTransformd& ThreadContext::EvalBodyPose(BodyIndex body) const { + return model_->plant().EvalBodyPoseInWorld(*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_THROW_UNLESS(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)); +} + +int ContextPool::size() const { + std::lock_guard guard(mutex_); + return static_cast(slots_.size()); +} + +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_); +} + +// --------------------------------------------------------------------------- +// WorkerPool. +// --------------------------------------------------------------------------- + +/* One parked thread. Each slot has its own mutex/condition variable so that + dispatching to n slots is n independent handoffs rather than a broadcast every + waiter has to filter. */ +struct WorkerPool::Slot { + std::mutex mutex; + std::condition_variable condition; + bool shutdown{false}; + bool has_task{false}; + const std::function* task{nullptr}; + int index{0}; + std::shared_ptr state; + std::thread thread; +}; + +/* Completion counter of one dispatch. Held by shared_ptr — the batch and every + slot that ran one of its tasks own a reference — because the slot signals + completion *through* this object and the waiter would otherwise be free to + destroy it while the signalling thread is still inside notify_all(). */ +struct WorkerPool::BatchState { + std::mutex mutex; + std::condition_variable condition; + int remaining{0}; +}; + +WorkerPool::WorkerPool() = default; + +WorkerPool::~WorkerPool() { + std::deque> slots; + { + std::lock_guard guard(mutex_); + shutdown_ = true; + slots.swap(slots_); + idle_.clear(); + } + // Outside the pool mutex: a slot thread never takes it, but keeping the + // teardown lock-free makes that independent of future edits. + for (const std::unique_ptr& slot : slots) { + { + std::lock_guard guard(slot->mutex); + slot->shutdown = true; + } + slot->condition.notify_one(); + } + for (const std::unique_ptr& slot : slots) { + if (slot->thread.joinable()) slot->thread.join(); + } +} + +WorkerPool::Batch WorkerPool::Reserve(int count) { + Batch batch; + batch.pool_ = this; + if (count <= 0) return batch; + // Bounding the pool by the machine's width keeps a program that runs many + // concurrent parallel checks from multiplying threads without limit; a call + // that finds nothing free simply runs with fewer workers, which is only a + // performance difference. + const int cap = + std::max(1, static_cast(std::thread::hardware_concurrency())); + std::lock_guard guard(mutex_); + if (shutdown_) return batch; + while (static_cast(batch.slots_.size()) < count && !idle_.empty()) { + const int index = idle_.back(); + idle_.pop_back(); + batch.slots_.push_back(index); + batch.handles_.push_back(slots_[index].get()); + } + while (static_cast(batch.slots_.size()) < count && + static_cast(slots_.size()) < cap) { + slots_.push_back(std::make_unique()); + Slot* const slot = slots_.back().get(); + const int index = static_cast(slots_.size()) - 1; + try { + slot->thread = std::thread([slot]() { + while (true) { + const std::function* task = nullptr; + int task_index = 0; + std::shared_ptr state; + { + std::unique_lock lock(slot->mutex); + slot->condition.wait(lock, [slot]() { + return slot->shutdown || slot->has_task; + }); + if (!slot->has_task) return; // shutdown + task = slot->task; + task_index = slot->index; + state = std::move(slot->state); + slot->task = nullptr; + slot->has_task = false; + } + // Tasks are documented not to throw (the certifier's worker lambda + // catches everything); swallowing here is the last line of defence + // against terminating the process and stranding the waiter. + try { + (*task)(task_index); + } catch (...) { // NOLINT(bugprone-empty-catch) + } + { + std::lock_guard state_guard(state->mutex); + --state->remaining; + state->condition.notify_all(); + } + } + }); + } catch (const std::system_error&) { + // Reserve() promises never to fail: running with fewer workers is only a + // performance difference. So when the system refuses a thread, drop the + // half-built slot and hand back what was already reserved. + slots_.pop_back(); + break; + } + batch.slots_.push_back(index); + batch.handles_.push_back(slot); + } + return batch; +} + +int WorkerPool::size() const { + std::lock_guard guard(mutex_); + return static_cast(slots_.size()); +} + +void WorkerPool::Release(const std::vector& slots) { + std::lock_guard guard(mutex_); + if (shutdown_) return; + for (const int slot : slots) idle_.push_back(slot); +} + +void WorkerPool::Batch::Dispatch(const std::function& task) { + if (handles_.empty()) return; + DRAKE_THROW_UNLESS(state_ == nullptr); + state_ = std::make_shared(); + state_->remaining = static_cast(handles_.size()); + for (int i = 0; i < static_cast(handles_.size()); ++i) { + Slot* const slot = handles_[i]; + { + std::lock_guard guard(slot->mutex); + slot->task = &task; + slot->index = i; + slot->state = state_; + slot->has_task = true; + } + slot->condition.notify_one(); + } +} + +void WorkerPool::Batch::Wait() { + if (state_ == nullptr) return; + { + std::unique_lock lock(state_->mutex); + state_->condition.wait(lock, [this]() { + return state_->remaining == 0; + }); + } + state_.reset(); +} + +WorkerPool::Batch& WorkerPool::Batch::operator=(Batch&& other) noexcept { + if (this == &other) return *this; + Wait(); + if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); + pool_ = other.pool_; + slots_ = std::move(other.slots_); + handles_ = std::move(other.handles_); + state_ = std::move(other.state_); + other.pool_ = nullptr; + other.slots_.clear(); + other.handles_.clear(); + other.state_.reset(); + return *this; +} + +WorkerPool::Batch::~Batch() { + Wait(); + if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); +} + +// --------------------------------------------------------------------------- +// RunCertifier. +// --------------------------------------------------------------------------- + +CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, + WorkerPool* workers) { + DRAKE_THROW_UNLESS(input.model != nullptr); + DRAKE_THROW_UNLESS(input.oracle != nullptr); + DRAKE_THROW_UNLESS(input.table != nullptr); + DRAKE_THROW_UNLESS(input.path != nullptr); + DRAKE_THROW_UNLESS(input.pairs != nullptr); + DRAKE_THROW_UNLESS(input.tau != nullptr); + DRAKE_THROW_UNLESS(input.prefilter != nullptr); + DRAKE_THROW_UNLESS(pool != nullptr); + + const Options& options = input.options; + const PiecewiseBezierPath& path = *input.path; + const std::vector& pairs = *input.pairs; + const int num_pairs = static_cast(pairs.size()); + const int num_segments = static_cast(path.segments().size()); + const bool emit = options.emit_certificate; + const std::uint64_t max_nodes = + options.max_nodes.value_or(std::numeric_limits::max()); + + CertifierOutput output; + if (emit) { + output.certificate.pairs.reserve(num_pairs); + for (const PairRecord& pair : pairs) { + output.certificate.pairs.push_back(pair.id); + } + } + + FindingSink sink(options.max_reported_findings); + std::atomic node_counter{0}; + Statistics stats; + std::vector records; + + const int requested_threads = std::max(1, options.parallelism.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, &stats, + emit ? &records : nullptr); + } + } + + // --- 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); + } + + const bool have_work = !moving_pairs.empty() && num_segments > 0; + const int num_threads = + (workers == nullptr) ? 1 : std::max(1, requested_threads); + const auto accumulate = [&](Worker* worker) { + stats.nodes += worker->stats().nodes; + stats.narrowphase_queries += worker->stats().narrowphase_queries; + stats.sphere_certifications += worker->stats().sphere_certifications; + stats.max_depth = std::max(stats.max_depth, worker->stats().max_depth); + if (emit) { + records.insert(records.end(), + std::make_move_iterator(worker->records().begin()), + std::make_move_iterator(worker->records().end())); + } + }; + + if (have_work && num_threads <= 1) { + // Serial: one worker, one local stack, no shared queue and no thread + // interleaving ⇒ bit-deterministic results and stats (requirement P7). + 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); + if (node_counter.load(std::memory_order_relaxed) > max_nodes) break; + } + accumulate(&worker); + } else if (have_work) { + // Parallel driver: lazy recruitment + occupancy-driven sharing. The full + // rationale — and the deviation from parallelism and determinism's static + // seeding — 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 kFindFirstViolation's 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.depth = 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(); + }; + + // Declared before the batch so that the batch — whose destructor waits for + // the helpers — is torn down first on every path, including the throwing + // one. + std::optional helper_lease; + std::vector> helpers; + std::function helper_task; + WorkerPool::Batch batch; + + Recruitment recruitment; + recruitment.nodes_before_hire = kNodesBeforeHiringHelpers; + recruitment.hire = [&]() { + batch = workers->Reserve(num_threads - 1); + const int hired = batch.size(); + if (hired == 0) return; // Pool exhausted: stay serial, still correct. + 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)); + } + helper_task = [&](int index) { + try { + helpers[index]->Run(); + } catch (...) { + record_error(); + queue.Abort(); + } + }; + // 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); + batch.Dispatch(helper_task); + }; + + Worker lead(input, &lease[0], &sink, &node_counter, &queue, &recruitment); + try { + lead.Run(); + } catch (...) { + record_error(); + queue.Abort(); + } + batch.Wait(); + if (first_error != nullptr) std::rethrow_exception(first_error); + accumulate(&lead); + for (const std::unique_ptr& helper : helpers) { + accumulate(helper.get()); + } + // Anything the budget left in the queue is uncovered too. + for (const WorkItem& item : queue.remaining()) { + sink.ReportPending(TimeOf(path.segments()[item.segment], item.s_lo), + item.control_points.col(0), + item.active.empty() ? 0 : item.active.front()); + } + } + + // --- Step 4: reduce per the search mode. --------------------------------- + const bool budget_exhausted = + options.max_nodes.has_value() && + node_counter.load(std::memory_order_relaxed) > *options.max_nodes; + + std::vector findings; + if (!sink.definite().empty() && + options.mode == SearchMode::kFindFirstViolation) { + // The branch-and-bound recursion refines toward the earliest witness and + // the sink keeps entries earliest-first, so this *is* the earliest witness + // the run found — identical serially and in parallel. + findings.push_back(sink.definite().front()); + } else { + findings = sink.definite(); + findings.insert(findings.end(), sink.inconclusive().begin(), + sink.inconclusive().end()); + } + + if (budget_exhausted && sink.pending_valid()) { + // Report what the budget left uncovered as a non-definite finding at the + // earliest uncovered time (the search algorithm: truncate in parameter + // order, report the remainder). + Finding finding; + finding.time = sink.pending_time(); + finding.q = sink.pending_q(); + finding.pair = pairs[sink.pending_pair()].id; + finding.motion_bound = 0.0; + finding.definite = false; + Eigen::Vector3d nearest_a; + Eigen::Vector3d nearest_b; + lease[0].SetPositions(finding.q); + finding.distance = input.oracle->SignedDistance(lease[0].query_object(), + pairs[sink.pending_pair()], + &nearest_a, &nearest_b); + finding.nearest_a_W = nearest_a; + finding.nearest_b_W = nearest_b; + ++stats.narrowphase_queries; + findings.push_back(std::move(finding)); + } + + std::stable_sort(findings.begin(), findings.end(), + [](const Finding& a, const Finding& b) { + return a.time < b.time; + }); + const int cap = std::max(1, options.max_reported_findings); + if (static_cast(findings.size()) > cap) findings.resize(cap); + + if (!sink.definite().empty()) { + output.verdict = Verdict::kViolationFound; + } else if (budget_exhausted) { + output.verdict = Verdict::kBudgetExhausted; + } else if (!sink.inconclusive().empty()) { + output.verdict = Verdict::kInconclusive; + } else { + output.verdict = Verdict::kCertifiedFree; + } + + output.findings = std::move(findings); + output.stats = stats; + if (emit) { + SortRecords(&records); + output.certificate.records = std::move(records); + } + return output; +} + +} // namespace internal +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/certifier.h b/planning/certified_ccd/certifier.h new file mode 100644 index 000000000000..22023b8cd62e --- /dev/null +++ b/planning/certified_ccd/certifier.h @@ -0,0 +1,375 @@ +#pragma once + +/// @file +/// Internal driver of the adaptive interval certifier (the search algorithm) +/// and of the independent certificate replay (the search algorithm, +/// "certificate audit trail"). +/// +/// Nothing in this header is part of the public API; it exists so the facade +/// (`certified_continuous_collision_checker.cc`), the certificate replay +/// (`certificate.cc`) and the node loop (`certifier.cc`) can share +/// one set of per-call data structures without the core module depending on +/// the api layer. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/geometry/query_object.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" +#include "drake/planning/certified_ccd/certificate.h" +#include "drake/planning/certified_ccd/distance_oracle.h" +#include "drake/planning/certified_ccd/motion_bound_table.h" +#include "drake/planning/certified_ccd/options.h" +#include "drake/planning/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/robot_diagram.h" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace internal { + +/** One thread's view of the model: a root diagram context plus the plant and +scene-graph sub-contexts pulled out of it once, so the hot loop pays a single +`SetPositions` per node (the performance requirements, P2) and no context +bookkeeping. */ +class ThreadContext { + public: + ThreadContext(const ThreadContext&) = delete; + ThreadContext& operator=(const ThreadContext&) = delete; + + /** Allocates a root context of `model`. `model` is aliased and must outlive + this object. */ + explicit ThreadContext(const drake::planning::RobotDiagram& 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 drake::geometry::QueryObject& query_object() const; + + /** World pose of `body` at the configuration last set (Drake's cache + computes it on first use and reuses it afterwards). */ + const drake::math::RigidTransform& EvalBodyPose( + drake::multibody::BodyIndex body) const; + + private: + const drake::planning::RobotDiagram* model_{}; + std::unique_ptr> root_; + drake::systems::Context* plant_context_{}; + const drake::systems::Context* scene_graph_context_{}; +}; + +/** A checkout pool of ThreadContexts (parallelism and determinism: +"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: + ContextPool(const ContextPool&) = delete; + ContextPool& operator=(const ContextPool&) = delete; + + /** Pre-warms `initial_size` contexts of `model`, which is aliased and must + outlive this pool. */ + ContextPool(const drake::planning::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(); + + int size() const { return static_cast(contexts_.size()); } + 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; + + /** Number of contexts currently held by the pool (for tests/diagnostics). */ + int size() const; + + private: + void Release(const std::vector& slots) const; + + const drake::planning::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_; +}; + +/** A pool of parked worker threads, reused across `Check*` calls. + +Why this exists: the driver used to spawn and join one `std::thread` per worker +per call, which the benchmark measured at ~23 µs per worker — 0.37 ms of pure +overhead on every 16-thread call, enough to make a sub-millisecond check +*slower* in parallel than in serial. Parked threads turn "hire 15 helpers" into +15 condition-variable notifications (a few µs), which is what makes the lazy +recruitment policy of RunCertifier() affordable: the driver can afford to start +serial and hire only once a run has proved itself big enough. + +Threads are created on demand (never at construction), capped at +`std::thread::hardware_concurrency()` per pool, parked on their own condition +variable when idle, and joined by the destructor. A `Batch` is a reservation of +some of them for the duration of one call; because reservations never block, +several concurrent `Check*` calls simply share out whatever threads exist and a +call that gets none just runs with fewer workers. */ +class WorkerPool { + private: + struct Slot; + struct BatchState; + + public: + /* Out of line (like the destructor) because Slot is incomplete here. */ + WorkerPool(); + WorkerPool(const WorkerPool&) = delete; + WorkerPool& operator=(const WorkerPool&) = delete; + ~WorkerPool(); + + /** Reserved threads for one call. Destruction waits for every dispatched + task to return and then releases the threads back to the pool. */ + class Batch { + public: + Batch() = default; + Batch(const Batch&) = delete; + Batch& operator=(const Batch&) = delete; + Batch(Batch&& other) noexcept { *this = std::move(other); } + Batch& operator=(Batch&& other) noexcept; + ~Batch(); + + /** How many threads were actually reserved (≤ the requested count). */ + int size() const { return static_cast(handles_.size()); } + + /** Runs `task(i)` for every i in [0, size()) on the reserved threads. The + referenced callable must outlive Wait(). Call at most once. */ + void Dispatch(const std::function& task); + + /** Blocks until every dispatched task has returned. Idempotent. */ + void Wait(); + + private: + friend class WorkerPool; + WorkerPool* pool_{}; + std::vector slots_; + std::vector handles_; + std::shared_ptr state_; + }; + + /** Reserves at most `count` currently idle threads, creating new ones (a + cold path) while the pool is below its cap. Never blocks. */ + Batch Reserve(int count); + + /** Number of threads the pool has created (for tests/diagnostics). */ + int size() const; + + private: + void Release(const std::vector& slots); + + mutable std::mutex mutex_; + /* A deque so that growing never invalidates the Slot addresses already + handed out to live batches. */ + std::deque> slots_; + std::vector idle_; + bool shutdown_{false}; +}; + +/** Per-pair broadphase data for the free-sphere prefilter (the interval +certificate): 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 { + drake::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 certification run needs; assembled by the facade. All +pointers are aliased and must outlive the call. */ +struct CertifierInput { + const drake::planning::RobotDiagram* model{}; + const DistanceOracle* oracle{}; + const MotionBoundTable* table{}; + const PiecewiseBezierPath* path{}; + /** Pair records with `threshold` = margin + padding resolved for this call. + Indexed consistently with `table`, `tau` and `prefilter`. */ + const std::vector* pairs{}; + /** Per-pair oracle tolerance τ_p (a refinement of the numerical policy; see + the table in the facade). */ + const std::vector* tau{}; + const PrefilterTable* prefilter{}; + Options options; +}; + +/** Result of one run, converted to a CertificationResult by the facade. */ +struct CertifierOutput { + Verdict verdict{Verdict::kCertifiedFree}; + /** Earliest-first, capped at Options::max_reported_findings. */ + std::vector findings; + Statistics stats; + /** Filled iff Options::emit_certificate; records are sorted by + (segment, s_start, pair_index) so a run is comparable across thread counts + and across time reparametrizations. + + A *complete* audit trail — one whose certified intervals cover the whole + domain for every pair, which is what ReplayCertificate() demands — is + produced only by a run that ends Verdict::kCertifiedFree. A run that found a + violation, hit the resolution floor, exhausted its budget, or pruned the + search (kFindFirstViolation) leaves the uncertified parts uncovered by + construction; its records are still individually valid, but they do not + amount to a proof and ReplayCertificate() will say so. */ + Certificate certificate; +}; + +/** Runs the breakpoint pre-pass, the static-pair resolution and the adaptive +node recursion of the search algorithm over every segment of `input.path`, +serially or in parallel according to `input.options.parallelism`. + +

Parallel driver (supersedes the white paper's seeding sketch)

+ +The white paper sketches "a work-stealing deque of nodes (seeded with all +segments' roots, or the top few bisection levels for small segment counts)". +That *static* seeding is what the first implementation did, and it does not +work: the certifier's trees are wildly unbalanced (a grazing trajectory +concentrates all of its subdivision in a band a few 10⁻³ wide in segment +parameter), so whatever fixed set of seeds is cut, one of them holds essentially +the whole tree. Measured: 0.98× at 16 threads on a 12.5k-node workload. Three +policies replace it; those *contracts* (per-thread contexts, one atomic +earliest- violation bound, findings sink under a mutex, only those three shared) +are unchanged. + +- **Sharing policy — occupancy-driven, not depth-driven.** There is one shared + LIFO work source. A worker that has just split a node consults the queue's + length: if it is below the number of live workers, the worker pushes its + *right* child there and keeps the left one on its local stack; otherwise it + keeps both. Sharing is therefore self-throttling (a saturated queue costs + nothing) and, crucially, does not stop at any depth — a worker sitting 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 kFindFirstViolation's bound tighten + early. +- **Recruitment policy — lazy.** The call starts as a plain serial descent on + the calling thread with sharing disabled, and hires helpers only after it has + visited `kNodesBeforeHiringHelpers` nodes. A check whose whole workload is + smaller than that (a PWL edge, a shallow shelf check) therefore runs at + exactly serial speed no matter what `Options::parallelism` says — which + matters because `Parallelism::Max()` is the default. Helpers come from a + `WorkerPool` that outlives the call, so hiring costs notifications rather + than thread creation. +- **Determinism policy — unchanged, because sharing moves nodes between + workers without changing which nodes exist.** Every node's decisions depend + only on its own control points and its inherited active set, so the tree, the + statistics summed over workers, and the findings are identical serially and + at any thread count in kCertifyAll. The two documented order-dependent + features are untouched: kFindFirstViolation's branch-and-bound (which prunes + only nodes starting at or after a witness already found, so the *reported* + witness is invariant while the statistics are not) and the `max_nodes` budget + (which truncates at a thread-count dependent place). + +Determinism (performance requirement P7; parallelism and +determinism): the serial path is bit-deterministic. In +kFindFirstViolation the *reported witness* is identical serially and at any +thread count — the branch-and-bound bound only ever prunes nodes that start at +or after a witness already found, so no node that could hold an earlier one is +ever skipped — while the statistics are not. Two documented exceptions to the +witness claim: a run that exhausts `max_nodes` truncates at a thread-count +dependent place, and 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. + +`pool` supplies the per-thread contexts; `workers` supplies the helper threads +and may be null, in which case every call runs serially on the calling thread. + +@throws std::exception if the oracle throws for any pair; a parallel run waits +for every worker first and rethrows the first failure. */ +CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, + WorkerPool* workers); + +// --------------------------------------------------------------------------- +// Certificate assembly + independent replay (implemented in certificate.cc). +// --------------------------------------------------------------------------- + +/** Restricts the Bézier control points `cps` (n × (m+1)) of a segment to the +sub-interval [a, b] ⊆ [0, 1] by two de Casteljau subdivisions, writing the +n × (m+1) control points of the restricted curve into `out`. + +This is a local, cold-path implementation used only by the certificate replay: +the replay must not reuse the certifier's own subdivision code path if it is to +be an independent check. */ +void RestrictBezier(const Eigen::MatrixXd& cps, double a, double b, + Eigen::MatrixXd* out); + +/** Evaluates the Bézier curve with control points `cps` at u ∈ [0, 1] by de +Casteljau (the apex of the triangle). Cold path. */ +Eigen::VectorXd EvaluateBezier(const Eigen::MatrixXd& cps, double u); + +/** Inputs of the independent certificate replay. All pointers are aliased. */ +struct ReplayInput { + const drake::planning::RobotDiagram* model{}; + const DistanceOracle* oracle{}; + const MotionBoundTable* table{}; + const PiecewiseBezierPath* path{}; + const std::vector* pairs{}; + const std::vector* tau{}; + double slack{1e-9}; +}; + +/** Independently re-evaluates every record of `certificate` and checks that +the certified intervals cover the whole domain for every pair (the search +algorithm). Returns true iff the certificate is a complete, self-consistent +proof that every pair stays above its recorded threshold everywhere on the path. +When it returns false and `message` is non-null, `*message` explains why. */ +bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, + std::string* message); + +} // namespace internal +} // namespace certified_ccd +} // namespace planning +} // namespace drake From 85ba3db49bd58e1e3107aefbd759f2fd63f1f17b Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 14:39:55 -0400 Subject: [PATCH 05/22] [planning] Add certified_ccd: CertifiedContinuousCollisionChecker Adds the public facade. It owns the construction-time analysis (pair enumeration, per-pair tolerances and padding, the kinematic tables) and offers CheckEdge/CheckTrajectory, which return a proof that the signed distance of every unfiltered pair stays above margin + padding over the entire continuous time domain, a witness configuration exactly on the trajectory, or an inconclusive verdict. Brings the remaining six suites with it: the certifier corpus, the certificate audit and mutation tests, the randomized soundness fuzz, the thin-obstacle regression that motivates the library, the concurrency determinism tests, and the API/UX clear-throw tests. --- planning/certified_ccd/BUILD.bazel | 146 +++ .../certified_continuous_collision_checker.cc | 622 ++++++++++ .../certified_continuous_collision_checker.h | 103 ++ planning/certified_ccd/test/api_test.cc | 605 ++++++++++ .../certified_ccd/test/certificate_test.cc | 706 +++++++++++ planning/certified_ccd/test/certifier_test.cc | 1032 +++++++++++++++++ .../certified_ccd/test/concurrency_test.cc | 860 ++++++++++++++ .../certified_ccd/test/soundness_fuzz_test.cc | 1004 ++++++++++++++++ .../certified_ccd/test/thin_obstacle_test.cc | 468 ++++++++ 9 files changed, 5546 insertions(+) create mode 100644 planning/certified_ccd/certified_continuous_collision_checker.cc create mode 100644 planning/certified_ccd/certified_continuous_collision_checker.h create mode 100644 planning/certified_ccd/test/api_test.cc create mode 100644 planning/certified_ccd/test/certificate_test.cc create mode 100644 planning/certified_ccd/test/certifier_test.cc create mode 100644 planning/certified_ccd/test/concurrency_test.cc create mode 100644 planning/certified_ccd/test/soundness_fuzz_test.cc create mode 100644 planning/certified_ccd/test/thin_obstacle_test.cc diff --git a/planning/certified_ccd/BUILD.bazel b/planning/certified_ccd/BUILD.bazel index 55afdfd4a2c3..42dd54b9c08d 100644 --- a/planning/certified_ccd/BUILD.bazel +++ b/planning/certified_ccd/BUILD.bazel @@ -13,6 +13,7 @@ drake_cc_package_library( visibility = ["//visibility:public"], deps = [ ":bounding_sphere", + ":certified_continuous_collision_checker", ":certifier", ":distance_oracle", ":motion_bound_table", @@ -166,6 +167,31 @@ drake_cc_library( ], ) +drake_cc_library( + name = "certified_continuous_collision_checker", + srcs = ["certified_continuous_collision_checker.cc"], + hdrs = ["certified_continuous_collision_checker.h"], + deps = [ + ":certifier", + ":distance_oracle", + ":motion_bound_table", + ":options", + ":piecewise_bezier_path", + "//common/trajectories:trajectory", + "//planning:robot_diagram", + "@eigen", + ], + implementation_deps = [ + "//common:essential", + "//common:unused", + "//geometry:scene_graph", + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//multibody/plant", + "@fmt", + ], +) + # === test/ === # T1 — curve module acceptance tests. @@ -232,4 +258,124 @@ drake_cc_googletest( ], ) +# T4/T6 — certifier semantics on a focused, hand-built corpus. +drake_cc_googletest( + name = "certifier_test", + deps = [ + ":certified_continuous_collision_checker", + "//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", + ], +) + +# T4 — the randomized soundness fuzz: random worlds x random trajectories, +# cross-checked against dense sampling and against the certificate replay. +# The dense cross-check (~1e7 signed-distance queries) is what makes this +# test long rather than the certification itself. +drake_cc_googletest( + name = "soundness_fuzz_test", + timeout = "moderate", + deps = [ + ":certified_continuous_collision_checker", + "//common:parallelism", + "//common/trajectories:bezier_curve", + "//common/trajectories:bspline_trajectory", + "//common/trajectories:piecewise_polynomial", + "//common/trajectories:trajectory", + "//geometry:scene_graph", + "//geometry:scene_graph_inspector", + "//geometry:shape_specification", + "//math:bspline_basis", + "//math:geometric_transform", + "//multibody/plant", + "//multibody/tree", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + ], +) + +# T5 — thin-obstacle regression (the reason this library exists). +drake_cc_googletest( + name = "thin_obstacle_test", + deps = [ + ":certified_continuous_collision_checker", + "//common:parallelism", + "//common/trajectories:bezier_curve", + "//geometry:scene_graph", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/plant", + "//multibody/tree", + "//planning:collision_checker_params", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + "//planning:scene_graph_collision_checker", + ], +) + +# T7 — certificate audit trail + mutation test. +drake_cc_googletest( + name = "certificate_test", + deps = [ + ":certified_continuous_collision_checker", + "//common:parallelism", + "//common/trajectories:bezier_curve", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/plant", + "//multibody/tree", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + ], +) + +# T8 — concurrency determinism. Running with many threads is the point of +# this test: it pins the answer at Parallelism {1, 2, 8, 16}. +drake_cc_googletest( + name = "concurrency_test", + num_threads = 16, + deps = [ + ":certified_continuous_collision_checker", + "//common:parallelism", + "//common/trajectories:bezier_curve", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/plant", + "//multibody/tree", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + ], +) + +# T9 — API / UX clear-throw tests. +drake_cc_googletest( + name = "api_test", + deps = [ + ":certified_continuous_collision_checker", + "//common:copyable_unique_ptr", + "//common:parallelism", + "//common/trajectories:bezier_curve", + "//common/trajectories:composite_trajectory", + "//common/trajectories:piecewise_polynomial", + "//common/trajectories:piecewise_quaternion", + "//common/trajectories:trajectory", + "//geometry:geometry_instance", + "//geometry:proximity_properties", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/fem:deformable_body_config", + "//multibody/plant", + "//multibody/tree", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + ], +) + add_lint_tests() diff --git a/planning/certified_ccd/certified_continuous_collision_checker.cc b/planning/certified_ccd/certified_continuous_collision_checker.cc new file mode 100644 index 000000000000..3cd5e9293643 --- /dev/null +++ b/planning/certified_ccd/certified_continuous_collision_checker.cc @@ -0,0 +1,622 @@ +/// @file +/// The public facade (the architecture). It owns the construction-time analysis +/// — kinematics engine, distance oracle and capability probe, per-pair padding, +/// per-pair oracle tolerances, the broadphase sphere table and the per-thread +/// context pool — assembles the per-call inputs of the node-loop driver in +/// certifier.{h,cc}, and hosts the independent certificate replay entry +/// point. +/// +/// The guarantee this file implements, stated verbatim as in the header: +/// +/// 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 margin + padding(A, B) — +/// under the stated assumptions: exact real arithmetic up to the configured +/// numerical slack, a distance oracle accurate to its stated tolerance, and +/// the geometry semantics of the geometry-support scope (Mesh ≡ convex hull). +/// This is a statement about the continuum of configurations, not about +/// samples. The certificate is a property of the path, so retiming the +/// trajectory afterwards does not invalidate it. + +#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/drake_throw.h" +#include "drake/common/unused.h" +#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/certified_ccd/certifier.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::geometry::GeometryId; +using drake::multibody::BodyIndex; +using drake::planning::RobotDiagram; + +// --------------------------------------------------------------------------- +// Per-pair oracle tolerance τ_p (a deliberate refinement of the numerical +// policy's uniform τ policy). +// --------------------------------------------------------------------------- +// +// The numerical policy charges every pair one global τ = +// Options::query_tolerance (default 1e-6 m). Drake's *documented* accuracy for +// QueryObject::ComputeSignedDistancePairClosestPoints() is worse than that for +// several shape combinations — up to 5e-5 m for Cylinder-Ellipsoid — because +// those combinations run an iterative GJK/EPA-style solver with a hard-coded +// iteration limit. +// +// Trusting the oracle to 1e-6 m where Drake only promises 5e-5 m is exactly +// the one failure mode that can fake a certificate: the soundness argument +// shows that oracle misbehaviour *below* the threshold cannot produce a false +// "free", but over-reporting a distance at or above the threshold can. So the +// checker uses +// +// τ_p = max(Options::query_tolerance, documented_accuracy(shape_a, +// shape_b)) +// +// everywhere the white paper says τ — in the node certificate test, in the +// definite violation test, at breakpoints and in the certificate replay. Pairs +// routed through the analytic halfspace fallback are exact (closed-form support +// functions, the geometry-support scope), so they carry τ_p = +// Options::query_tolerance and nothing more. +// +// The table below is transcribed from Table 4 of +// drake/geometry/query_object.h ("Worst observed error (in m) for 2mm +// penetration/separation between geometries approximately 20cm in size" for +// T = double) in the pinned Drake (~v1.45). Mesh is certified as its convex +// hull, so its row/column duplicates Convex's, exactly as the Drake table's +// footnote states. Anything the checker cannot classify is charged the worst +// documented value. +// +// | | Box | Capsule | Convex | Cylinder | Ellipsoid | Mesh | +// Sphere | | 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 | (= Convex row) | +// 3e-15 | | | Sphere | 3e-15 | 6e-15 | 3e-6 | 5e-15 | 4e-5 +// | 3e-6 | 6e-15 | +// +// If the pinned Drake ever tightens these numbers the table may be relaxed; +// it must never be relaxed ahead of Drake's own documentation. + +/** The closed set of shape classes the τ_p table knows. */ +enum class ShapeClass { + kSphere = 0, + kBox = 1, + kCapsule = 2, + kCylinder = 3, + kEllipsoid = 4, + kConvex = 5, + kMesh = 6, + kHalfSpace = 7, + kOther = 8, +}; +constexpr int kNumShapeClasses = 9; + +/** Worst documented error over the whole table; charged to any shape the +checker cannot classify (it never reaches the narrowphase anyway — the +capability probe refuses unknown shapes at construction — but the default must +be the conservative one). */ +constexpr double kWorstDocumentedAccuracy = 5e-5; + +ShapeClass Classify(const drake::geometry::Shape& shape) { + return shape.Visit([](const auto& s) { + using S = std::decay_t; + drake::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::kOther; + } + }); +} + +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, 3e-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 + // in this library anyway — the capability probe routes every one of them + // through the analytic support-function fallback, which is exact, and + // ComputeTauTable() below never consults this table for a non-native route + // — so these entries are belt-and-braces. They are filled with the + // documented value where there is one and with the worst documented value + // otherwise, so that a future routing change cannot silently inherit a + // τ of zero. + set(S::kSphere, S::kHalfSpace, 3e-15); + return t; + }(); + return table; +} + +/** τ_p for every pair of `pairs`, given the call's query tolerance. */ +std::vector ComputeTauTable(const RobotDiagram& model, + const std::vector& pairs, + double query_tolerance) { + const drake::geometry::SceneGraphInspector& inspector = + model.scene_graph().model_inspector(); + const AccuracyTable& table = DocumentedAccuracyTable(); + std::vector tau(pairs.size(), query_tolerance); + 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].id.a))); + const int b = static_cast(Classify(inspector.GetShape(pairs[p].id.b))); + tau[p] = std::max(query_tolerance, table[a][b]); + } + return tau; +} + +// --------------------------------------------------------------------------- +// Padding. +// --------------------------------------------------------------------------- +// +// PaddingSpec mirrors drake::planning::CollisionChecker: a pair's effective +// threshold is m_p = margin + padding(p), where padding comes from the dense +// per-body-pair matrix when one is supplied and otherwise from the {env, self} +// scalars. +// +// Environment-vs-self rule used here (documented as required): a body is +// *anchored* iff no position coordinate of the plant changes its pose relative +// to the world — that is, iff KinematicsEngine::CoordinatesAffectingPair(world, +// body) is empty, which covers the world body itself and everything welded +// (directly or transitively) to it. A pair is then +// +// - self iff BOTH bodies are non-anchored (a robot-vs-robot pair), and +// - env otherwise (at least one side is the world or rigidly attached to +// it). +// +// The rule is pure topology, so a pair's padding never depends on which +// trajectory is being checked; in particular the constant-coordinate carve-out +// of trajectory normalization (which can make a *moving* body behave as if +// welded for one trajectory) deliberately does not enter here. + +std::vector ComputePaddingTable(const KinematicsEngine& engine, + const std::vector& pairs, + const PaddingSpec& padding) { + const drake::multibody::MultibodyPlant& plant = engine.plant(); + const int num_bodies = plant.num_bodies(); + if (padding.per_body_pair.has_value()) { + const Eigen::MatrixXd& matrix = *padding.per_body_pair; + if (matrix.rows() != num_bodies || matrix.cols() != num_bodies) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: PaddingSpec::per_body_pair is " + "{}x{} but must be {}x{} (one row and column per BodyIndex of the " + "plant).", + matrix.rows(), matrix.cols(), num_bodies, num_bodies)); + } + } + + std::vector anchored(num_bodies, false); + for (int b = 0; b < num_bodies; ++b) { + anchored[b] = + engine + .CoordinatesAffectingPair(plant.world_body().index(), BodyIndex(b)) + .empty(); + } + + std::vector result(pairs.size(), 0.0); + for (int p = 0; p < static_cast(pairs.size()); ++p) { + const int a = static_cast(pairs[p].id.body_a); + const int b = static_cast(pairs[p].id.body_b); + double value = (!anchored[a] && !anchored[b]) ? padding.self_padding + : padding.env_padding; + if (padding.per_body_pair.has_value()) { + const double entry = (*padding.per_body_pair)(a, b); + // A NaN entry means "not covered by the matrix"; fall back to the + // scalars for that pair. + if (!std::isnan(entry)) value = entry; + } + if (!std::isfinite(value)) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: padding for the body pair " + "({}, {}) is not finite.", + plant.get_body(BodyIndex(a)).name(), + plant.get_body(BodyIndex(b)).name())); + } + result[p] = value; + } + return result; +} + +// --------------------------------------------------------------------------- +// Prefilter table. +// --------------------------------------------------------------------------- + +internal::PrefilterTable ComputePrefilterTable( + const RobotDiagram& model, const KinematicsEngine& engine, + const std::vector& pairs) { + const drake::geometry::SceneGraphInspector& inspector = + model.scene_graph().model_inspector(); + internal::PrefilterTable table; + table.slot_a.resize(pairs.size(), -1); + table.slot_b.resize(pairs.size(), -1); + std::unordered_map slot_of; + + const auto slot = [&](GeometryId id, BodyIndex body) { + // HalfSpace has no bounding sphere (the geometry-support scope): such pairs + // skip the prefilter entirely and go straight to the analytic oracle route, + // which is cheap anyway. + if (Classify(inspector.GetShape(id)) == ShapeClass::kHalfSpace) return -1; + const auto it = slot_of.find(id); + if (it != slot_of.end()) return it->second; + const 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].id.a, pairs[p].id.body_a); + table.slot_b[p] = slot(pairs[p].id.b, pairs[p].id.body_b); + } + return table; +} + +void ValidateOptions(const Options& options) { + if (!std::isfinite(options.margin)) { + throw std::runtime_error( + "CertifiedContinuousCollisionChecker: Options::margin must be finite."); + } + if (!(options.query_tolerance >= 0.0) || + !std::isfinite(options.query_tolerance)) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: Options::query_tolerance must be " + "a finite non-negative distance; got {}.", + options.query_tolerance)); + } + if (!(options.certificate_slack >= 0.0) || + !std::isfinite(options.certificate_slack)) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: Options::certificate_slack must " + "be a finite non-negative distance; got {}.", + options.certificate_slack)); + } + if (!(options.min_interval > 0.0) || !(options.min_interval <= 1.0)) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: Options::min_interval is a " + "fraction of a segment's parameter width and must lie in (0, 1]; got " + "{}.", + options.min_interval)); + } + if (options.max_reported_findings < 1) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: Options::max_reported_findings " + "must be at least 1; got {}.", + options.max_reported_findings)); + } + if (options.max_nodes.has_value() && *options.max_nodes == 0) { + throw std::runtime_error( + "CertifiedContinuousCollisionChecker: Options::max_nodes must be at " + "least 1 when set."); + } +} + +} // namespace + +// --------------------------------------------------------------------------- +// Impl. +// --------------------------------------------------------------------------- + +class CertifiedContinuousCollisionChecker::Impl { + public: + explicit Impl(Params params) + : model_(std::move(params.model)), + default_options_(std::move(params.default_options)), + engine_(*model_), + oracle_(*model_, default_options_.query_tolerance), + pairs_(oracle_.pairs()), + padding_(ComputePaddingTable(engine_, pairs_, params.padding)), + tau_base_(ComputeTauTable(*model_, pairs_, + /* query_tolerance = */ 0.0)), + prefilter_(ComputePrefilterTable(*model_, engine_, pairs_)), + pool_(*model_, + std::max(1, default_options_.parallelism.num_threads())) { + pair_ids_.reserve(pairs_.size()); + for (int p = 0; p < static_cast(pairs_.size()); ++p) { + pair_ids_.push_back(pairs_[p].id); + // pairs() reports the checker's default thresholds; every call rewrites + // its own copy from that call's margin. + pairs_[p].threshold = default_options_.margin + padding_[p]; + } + } + + const Options& default_options() const { return default_options_; } + const RobotDiagram& model() const { return *model_; } + const KinematicsEngine& engine() const { return engine_; } + const DistanceOracle& oracle() const { return oracle_; } + const std::vector& pairs() const { return pairs_; } + const std::vector& pair_ids() const { return pair_ids_; } + + const Options& Resolve(const std::optional& options) const { + return options.has_value() ? *options : default_options_; + } + + void ValidatePath(const PiecewiseBezierPath& path) const { + const int expected = model_->plant().num_positions(); + if (path.num_positions() != expected) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: the trajectory has {} rows but " + "the plant has {} generalized positions.", + path.num_positions(), expected)); + } + } + + CertificationResult Check(const PiecewiseBezierPath& path, + const Options& options) const { + ValidateOptions(options); + ValidatePath(path); + const auto start = std::chrono::steady_clock::now(); + + // Per-call: the λ table (it depends on the trajectory's control box), the + // effective thresholds and the per-pair oracle tolerances. + const MotionBoundTable table = + engine_.ComputeMotionBoundTable(path, pair_ids_); + std::vector pairs = pairs_; + std::vector tau(pairs.size()); + for (int p = 0; p < static_cast(pairs.size()); ++p) { + pairs[p].threshold = options.margin + padding_[p]; + tau[p] = std::max(options.query_tolerance, tau_base_[p]); + } + + internal::CertifierInput input; + input.model = model_.get(); + input.oracle = &oracle_; + input.table = &table; + input.path = &path; + input.pairs = &pairs; + input.tau = τ + input.prefilter = &prefilter_; + input.options = options; + + internal::CertifierOutput output = + internal::RunCertifier(input, &pool_, &worker_pool_); + + CertificationResult result; + result.verdict = output.verdict; + result.findings = std::move(output.findings); + result.stats = output.stats; + result.stats.wall_time_s = + std::chrono::duration(std::chrono::steady_clock::now() - start) + .count(); + if (options.emit_certificate) { + result.certificate = std::move(output.certificate); + } + return result; + } + + private: + std::shared_ptr> model_; + Options default_options_; + KinematicsEngine engine_; + DistanceOracle oracle_; + std::vector pairs_; + std::vector pair_ids_; + /** padding(p) alone; the margin is added per call. */ + std::vector padding_; + /** Drake's documented accuracy per pair; τ_p = max(query_tolerance, this). */ + std::vector tau_base_; + internal::PrefilterTable prefilter_; + mutable internal::ContextPool pool_; + /** Parked helper threads, created on demand by the first call that hires + any and reused by every later call (see internal::WorkerPool). Declared last + so that its destructor — which joins every parked thread — runs before the + contexts and tables those threads worked on are torn down. */ + mutable internal::WorkerPool worker_pool_; +}; + +// --------------------------------------------------------------------------- +// CertifiedContinuousCollisionChecker. +// --------------------------------------------------------------------------- + +CertifiedContinuousCollisionChecker::CertifiedContinuousCollisionChecker( + Params params) { + if (params.model == nullptr) { + throw std::runtime_error( + "CertifiedContinuousCollisionChecker: Params::model is null; supply a " + "RobotDiagram whose plant is finalized."); + } + if (!params.model->plant().is_finalized()) { + throw std::runtime_error( + "CertifiedContinuousCollisionChecker: the plant is not finalized; call " + "MultibodyPlant::Finalize() (or RobotDiagramBuilder::Build()) first."); + } + ValidateOptions(params.default_options); + impl_ = std::make_unique(std::move(params)); +} + +CertifiedContinuousCollisionChecker::~CertifiedContinuousCollisionChecker() = + default; + +CertificationResult CertifiedContinuousCollisionChecker::CheckTrajectory( + const drake::trajectories::Trajectory& trajectory, + const std::optional& options) const { + const Options& resolved = impl_->Resolve(options); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, resolved); + return impl_->Check(path, resolved); +} + +CertificationResult CertifiedContinuousCollisionChecker::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( + "CertifiedContinuousCollisionChecker::CheckPath: the waypoint matrix " + "has {} rows but the plant has {} generalized positions (waypoints are " + "columns).", + waypoints.rows(), expected)); + } + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromWaypoints(waypoints, resolved); + return impl_->Check(path, resolved); +} + +CertificationResult CertifiedContinuousCollisionChecker::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( + "CertifiedContinuousCollisionChecker::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); +} + +PiecewiseBezierPath CertifiedContinuousCollisionChecker::Normalize( + const drake::trajectories::Trajectory& trajectory, + const std::optional& options) const { + const Options& resolved = impl_->Resolve(options); + PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(trajectory, resolved); + impl_->ValidatePath(path); + return path; +} + +MotionBoundTable CertifiedContinuousCollisionChecker::ComputeMotionBounds( + const PiecewiseBezierPath& path) const { + impl_->ValidatePath(path); + return impl_->engine().ComputeMotionBoundTable(path, impl_->pair_ids()); +} + +const DistanceOracle& CertifiedContinuousCollisionChecker::distance_oracle() + const { + return impl_->oracle(); +} + +const KinematicsEngine& CertifiedContinuousCollisionChecker::kinematics_engine() + const { + return impl_->engine(); +} + +const std::vector& CertifiedContinuousCollisionChecker::pairs() + const { + return impl_->pairs(); +} + +const RobotDiagram& CertifiedContinuousCollisionChecker::model() const { + return impl_->model(); +} + +// --------------------------------------------------------------------------- +// VerifyCertificate. +// --------------------------------------------------------------------------- +// +// Deliberately written against the checker's *public* introspection seams +// only: it re-derives the λ table from the path, re-restricts every record's +// control points with its own de Casteljau code, recomputes w about the +// record's qc, re-queries the oracle at qc from a fresh context, and re-checks +// the interval-certificate inequality with τ_p. It then verifies that the +// certified intervals cover [0, 1] of every segment for every pair. Nothing of +// the certifier's own bookkeeping is trusted. +// +// The replay charges the checker's construction-time query tolerance and the +// documented Options::certificate_slack default; a run made with a *larger* +// slack (a stricter certificate) therefore still verifies. +// +// It returns true only for a *complete* proof. A certificate from a run that +// found a violation, ended inconclusive, exhausted its node budget, or pruned +// the search (kFindFirstViolation) necessarily leaves part of the domain +// uncovered, and the coverage check reports that as a failure — which is the +// correct answer to "does this certificate prove the path is free?". + +bool VerifyCertificate(const CertifiedContinuousCollisionChecker& checker, + const PiecewiseBezierPath& path, + const Certificate& certificate) { + const MotionBoundTable table = checker.ComputeMotionBounds(path); + const std::vector& pairs = checker.pairs(); + const std::vector tau = ComputeTauTable( + checker.model(), pairs, checker.distance_oracle().tolerance()); + + internal::ReplayInput input; + input.model = &checker.model(); + input.oracle = &checker.distance_oracle(); + input.table = &table; + input.path = &path; + input.pairs = &pairs; + input.tau = τ + input.slack = Options{}.certificate_slack; + return internal::ReplayCertificate(input, certificate, nullptr); +} + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/certified_continuous_collision_checker.h b/planning/certified_ccd/certified_continuous_collision_checker.h new file mode 100644 index 000000000000..69ee40e5e156 --- /dev/null +++ b/planning/certified_ccd/certified_continuous_collision_checker.h @@ -0,0 +1,103 @@ +#pragma once + +#include +#include +#include + +#include + +#include "drake/common/trajectories/trajectory.h" +#include "drake/planning/certified_ccd/certificate.h" +#include "drake/planning/certified_ccd/distance_oracle.h" +#include "drake/planning/certified_ccd/motion_bound_table.h" +#include "drake/planning/certified_ccd/options.h" +#include "drake/planning/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace certified_ccd { + +/** Result of one certification call (the architecture). */ +struct CertificationResult { + Verdict verdict{}; + /** Earliest-first. */ + std::vector findings; + Statistics stats; + /** Present iff Options::emit_certificate. */ + std::optional certificate; +}; + +/** Certifies — not samples — that a trajectory is collision-free over its +entire continuous time domain (the problem statement). + +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 margin + padding(A, B) — under the stated +assumptions: exact real arithmetic up to the configured numerical slack, a +distance oracle accurate to its stated tolerance, and the geometry semantics +of the geometry-support scope (Mesh ≡ convex hull). This is a statement about +the continuum of configurations, not about samples. The certificate is a +property of the path, so retiming the trajectory afterwards does not invalidate +it. + +Thread-compatible: the Check* methods are const, own no mutable state +outside per-call scratch, and are safe to call concurrently. */ +class CertifiedContinuousCollisionChecker { + public: + struct Params { + /** Plant + scene graph; the plant must be finalized. */ + std::shared_ptr> model; + /** Per-body-pair padding, drake::planning::CollisionChecker semantics. */ + PaddingSpec padding{}; + Options default_options{}; + }; + + /** Builds contexts, bounding spheres, topology tables, and runs the + capability probe (throws on unsupported geometry pairs; the geometry-support + scope). */ + explicit CertifiedContinuousCollisionChecker(Params params); + + ~CertifiedContinuousCollisionChecker(); + + /** Certifies a trajectory (any supported Drake trajectory type). */ + CertificationResult CheckTrajectory( + const drake::trajectories::Trajectory& trajectory, + const std::optional& options = {}) const; + + /** Certifies a piecewise-linear path through the given waypoint columns. */ + CertificationResult CheckPath( + const Eigen::MatrixXd& waypoints, + const std::optional& options = {}) const; + + /** Certifies the straight configuration-space edge q1 → q2. */ + CertificationResult CheckEdge( + const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, + const std::optional& options = {}) const; + + /** Introspection / testing seams (all const, thread-safe). */ + PiecewiseBezierPath Normalize( + const drake::trajectories::Trajectory& trajectory, + const std::optional& options = {}) const; + MotionBoundTable ComputeMotionBounds(const PiecewiseBezierPath& path) const; + const DistanceOracle& distance_oracle() const; + const KinematicsEngine& kinematics_engine() const; + const std::vector& pairs() const; + const drake::planning::RobotDiagram& model() const; + + private: + class Impl; + std::unique_ptr impl_; +}; + +/** Independently replays every record of `certificate` (recomputing node +control boxes from freshly restricted control points and re-querying +distances) and checks interval coverage of the full domain for every pair. +Returns true iff the certificate holds (the search algorithm). */ +bool VerifyCertificate(const CertifiedContinuousCollisionChecker& checker, + const PiecewiseBezierPath& path, + const Certificate& certificate); + +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/api_test.cc b/planning/certified_ccd/test/api_test.cc new file mode 100644 index 000000000000..01973aede772 --- /dev/null +++ b/planning/certified_ccd/test/api_test.cc @@ -0,0 +1,605 @@ +/// @file +/// T9 — API / UX (test plan T9: the joint-support and geometry-support +/// scopes, and the architecture). +/// +/// Every refusal this library makes has to be *actionable*: the message must +/// name the joint, geometry, coordinate, index or size the caller has to go and +/// fix. These tests therefore assert on message content, not just that +/// something was thrown — a bare EXPECT_THROW would pass for a message reading +/// "error" and leave a user with nothing to act on. +/// +/// Coverage notes for two items of test-plan T9: +/// * Python bindings do not exist yet, so the pydrake-style smoke +/// tests are out of scope here. +/// * An *unfinalized* plant cannot reach the checker through Drake's public +/// API on this pin: RobotDiagramBuilder::Build() finalizes the plant +/// unconditionally and RobotDiagram's constructor is private to the +/// builder, so there is no way to construct the input that guard rejects. +/// The guard is therefore defensive; the adjacent, reachable guards (null +/// model) are pinned instead. See NullModelIsRefused below. + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/copyable_unique_ptr.h" +#include "drake/common/parallelism.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/common/trajectories/composite_trajectory.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" +#include "drake/common/trajectories/trajectory.h" +#include "drake/geometry/geometry_instance.h" +#include "drake/geometry/proximity_properties.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.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/joint.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/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::Parallelism; +using drake::geometry::Box; +using drake::geometry::GeometryInstance; +using drake::geometry::HalfSpace; +using drake::geometry::ProximityProperties; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +using drake::multibody::CoulombFriction; +using drake::multibody::Joint; +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::CompositeTrajectory; +using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; +using drake::trajectories::Trajectory; +using Eigen::Vector3d; +using Eigen::VectorXd; + +CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +SpatialInertia Inertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +/// Runs `call`, requires it to throw, and returns the message so the caller can +/// assert on the identifiers it must contain. Reports the actual message on +/// every failure path, so a message regression is diagnosable from the log. +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 {}; +} + +void ExpectContains(const std::string& haystack, const std::string& needle) { + EXPECT_NE(haystack.find(needle), std::string::npos) + << "the message did not mention '" << needle << "'.\nMessage was:\n" + << haystack; +} + +std::unique_ptr MakeChecker( + std::shared_ptr> model) { + CertifiedContinuousCollisionChecker::Params params; + params.model = std::move(model); + params.default_options.parallelism = Parallelism::None(); + return std::make_unique(params); +} + +/// 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 (the joint-support 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 = MakeChecker(model); + 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. + 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 + const std::string message = ThrowMessage([&]() { + checker->CheckTrajectory(BezierCurve(0.0, 1.0, points)); + }); + ExpectContains(message, joint_name); + ExpectContains(message, "quaternion_floating"); + // The message must also point at the way out. + ExpectContains(message, "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); + const std::string translate_message = ThrowMessage([&]() { + checker->CheckTrajectory(BezierCurve(0.0, 1.0, translated)); + }); + ExpectContains(translate_message, joint_name); + ExpectContains(translate_message, "coordinate 4"); +} + +GTEST_TEST(ApiTest, ConstantQuaternionBaseIsAcceptedEndToEnd) { + // The joint-support carve-out: a floating base whose pose is *constant* along + // the trajectory is treated as welded, so a floating-base robot is fully + // usable as long as the given trajectory does not move the base. This is the + // end-to-end version of that promise — not just "does not throw", but a real + // verdict with a real certificate. + std::shared_ptr> model = MakeFloatingBaseWorld(); + const auto checker = MakeChecker(model); + + Options options; + options.parallelism = Parallelism::None(); + options.emit_certificate = true; + + 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(7, 1) = 0.35; // Only the elbow moves. + points(7, 2) = 0.70; + const BezierCurve trajectory(0.0, 1.0, points); + + const PiecewiseBezierPath path = checker->Normalize(trajectory, options); + const std::vector& constant = path.constant_coordinates(); + ASSERT_EQ(constant.size(), 8u); + for (int i = 0; i < 7; ++i) { + EXPECT_TRUE(constant[i]) + << "base coordinate " << i << " should have been flagged constant"; + } + EXPECT_FALSE(constant[7]); + + const CertificationResult result = + checker->CheckTrajectory(trajectory, options); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + ASSERT_TRUE(result.certificate.has_value()); + EXPECT_TRUE(VerifyCertificate(*checker, path, *result.certificate)); +} + +// --------------------------------------------------------------------------- +// 2. Geometry scope (the geometry-support 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 λ exists for that pair. This must be refused + // when the checker is built, not discovered mid-certification. + 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(); + + const std::string message = ThrowMessage([&]() { + MakeChecker(model); + }); + ExpectContains(message, "blade_halfspace"); + ExpectContains(message, "post_geom"); + ExpectContains(message, "spin"); + // ... and it must say what to do about it. + ExpectContains(message, "Box"); +} + +GTEST_TEST(ApiTest, AnchoredHalfSpaceUnderARotatingArmIsAccepted) { + // The complement, so the rule above is not read as "half spaces are + // unsupported": the overwhelmingly common case — an anchored ground plane + // under a rotating arm — is accepted, because λ 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()); + std::shared_ptr> model = builder.Build(); + + const auto checker = MakeChecker(model); + // The probe report is part of the UX: it must say how each pair is routed. + const std::string report = checker->distance_oracle().support_report(); + ExpectContains(report, "HalfSpace"); + 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 (the geometry-support scope): their motion is + // not described by the plant's generalized positions, so no motion bound + // exists for them at all. Registering one is possible on this Drake pin (the + // plant must be discrete, which RobotDiagramBuilder's default time step + // already is), so the refusal is exercised on a real model rather than argued + // about. + 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); + + const std::string message = ThrowMessage([&]() { + MakeChecker(model); + }); + ExpectContains(message, "deformable"); + ExpectContains(message, "squishy_blob"); +} + +// --------------------------------------------------------------------------- +// 3. Dimensions (trajectory normalization; the architecture). +// --------------------------------------------------------------------------- + +GTEST_TEST(ApiTest, DimensionMismatchMessagesNameTheSizes) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeChecker(model); + ASSERT_EQ(model->plant().num_positions(), 2); + + const std::string path_message = ThrowMessage([&]() { + checker->CheckPath(Eigen::MatrixXd::Zero(3, 4)); + }); + ExpectContains(path_message, "CheckPath"); + ExpectContains(path_message, "3 rows"); + ExpectContains(path_message, "2 generalized positions"); + ExpectContains(path_message, "waypoints are columns"); + + const std::string edge_message = ThrowMessage([&]() { + checker->CheckEdge(VectorXd::Zero(2), VectorXd::Zero(5)); + }); + ExpectContains(edge_message, "CheckEdge"); + ExpectContains(edge_message, "sizes 2 and 5"); + + const std::string trajectory_message = ThrowMessage([&]() { + checker->CheckTrajectory( + BezierCurve(0.0, 1.0, Eigen::MatrixXd::Zero(7, 3))); + }); + ExpectContains(trajectory_message, "7 rows"); + ExpectContains(trajectory_message, "2 generalized positions"); + + // A single waypoint is not a path. + const std::string single_message = ThrowMessage([&]() { + checker->CheckPath(Eigen::MatrixXd::Zero(2, 1)); + }); + ExpectContains(single_message, "at least 2 waypoints"); +} + +// --------------------------------------------------------------------------- +// 4. Trajectory validation (trajectory normalization). +// --------------------------------------------------------------------------- + +GTEST_TEST(ApiTest, DiscontinuousTrajectoryThrowsNamingTheJunction) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeChecker(model); + + Eigen::MatrixXd first(2, 2); + first << 0.0, 0.3, 0.0, 0.05; + Eigen::MatrixXd second(2, 2); + // Coordinate 1 teleports by 0.4 m at the junction. + second << 0.3, 0.6, 0.45, 0.50; + std::vector>> segments; + segments.emplace_back(std::make_unique>(0.0, 1.0, first)); + segments.emplace_back( + std::make_unique>(1.0, 2.0, second)); + const CompositeTrajectory trajectory(std::move(segments)); + + const std::string message = ThrowMessage([&]() { + checker->CheckTrajectory(trajectory); + }); + ExpectContains(message, "C0 discontinuity"); + ExpectContains(message, "segments 0 and 1"); + ExpectContains(message, "coordinate 1"); + ExpectContains(message, "continuity_tolerance"); +} + +GTEST_TEST(ApiTest, DegreeAboveConversionCapThrows) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeChecker(model); + + // 13 interpolation nodes ⇒ one polynomial segment of degree 12, above the + // default max_conversion_degree of 10. + const int kNodes = 13; + VectorXd times(kNodes); + Eigen::MatrixXd samples(2, kNodes); + for (int i = 0; i < kNodes; ++i) { + times[i] = i; + samples(0, i) = 0.1 * ((i % 3) - 1); + samples(1, i) = 0.02 * ((i % 5) - 2); + } + const PiecewisePolynomial trajectory = + PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, + samples); + const std::string message = ThrowMessage([&]() { + checker->CheckTrajectory(trajectory); + }); + ExpectContains(message, "polynomial degree 12"); + ExpectContains(message, "max_conversion_degree"); + + // Raising the cap deliberately is the documented escape hatch, and it works. + Options options; + options.parallelism = Parallelism::None(); + options.max_conversion_degree = 12; + EXPECT_NO_THROW(checker->Normalize(trajectory, options)); +} + +GTEST_TEST(ApiTest, UnsupportedTrajectoryTypeThrowsNamingTheType) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeChecker(model); + const PiecewiseQuaternionSlerp trajectory( + std::vector{0.0, 1.0}, + std::vector>{ + Eigen::Quaternion::Identity(), + Eigen::Quaternion(0.7071067811865476, 0.0, 0.0, + 0.7071067811865476)}); + const std::string message = ThrowMessage([&]() { + checker->CheckTrajectory(trajectory); + }); + ExpectContains(message, "unsupported trajectory type"); + ExpectContains(message, "PiecewiseQuaternionSlerp"); + // The message must list what *is* accepted. + ExpectContains(message, "BezierCurve"); + ExpectContains(message, "BsplineTrajectory"); +} + +GTEST_TEST(ApiTest, ContinuousRevoluteIndexOutOfRangeThrows) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeChecker(model); + Options options; + options.parallelism = Parallelism::None(); + options.continuous_revolute_indices = {0, 5}; + + Eigen::MatrixXd points(2, 2); + points << 0.0, 0.2, 0.0, 0.05; + const std::string message = ThrowMessage([&]() { + checker->CheckTrajectory(BezierCurve(0.0, 1.0, points), options); + }); + ExpectContains(message, "continuous_revolute_indices contains 5,"); + ExpectContains(message, "2 generalized positions"); + + // A negative index is out of range too. + options.continuous_revolute_indices = {-1}; + const std::string negative_message = ThrowMessage([&]() { + checker->CheckTrajectory(BezierCurve(0.0, 1.0, points), options); + }); + ExpectContains(negative_message, "continuous_revolute_indices contains -1,"); +} + +// --------------------------------------------------------------------------- +// 5. Options and construction. +// --------------------------------------------------------------------------- + +GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeChecker(model); + Eigen::MatrixXd points(2, 2); + points << 0.0, 0.2, 0.0, 0.05; + const BezierCurve trajectory(0.0, 1.0, points); + const auto check_with = [&](const Options& options) { + return ThrowMessage([&]() { + checker->CheckTrajectory(trajectory, options); + }); + }; + + Options options; + options.parallelism = Parallelism::None(); + + Options bad = options; + bad.min_interval = 0.0; + ExpectContains(check_with(bad), "min_interval"); + bad.min_interval = 2.0; + ExpectContains(check_with(bad), "(0, 1]"); + + bad = options; + bad.max_reported_findings = 0; + ExpectContains(check_with(bad), "max_reported_findings"); + + bad = options; + bad.query_tolerance = -1.0; + ExpectContains(check_with(bad), "query_tolerance"); + + bad = options; + bad.certificate_slack = -1e-9; + ExpectContains(check_with(bad), "certificate_slack"); + + bad = options; + bad.max_nodes = 0; + ExpectContains(check_with(bad), "max_nodes"); + + bad = options; + bad.margin = std::numeric_limits::quiet_NaN(); + ExpectContains(check_with(bad), "margin"); +} + +GTEST_TEST(ApiTest, NullModelIsRefused) { + CertifiedContinuousCollisionChecker::Params params; + const std::string message = ThrowMessage([&]() { + CertifiedContinuousCollisionChecker checker(params); + }); + ExpectContains(message, "Params::model is null"); + // The message points at the requirement the (unreachable-through-Drake's + // public API) finalization guard also enforces. + ExpectContains(message, "finalized"); +} + +GTEST_TEST(ApiTest, MaxReportedFindingsIsRespected) { + std::shared_ptr> model = MakeArmWorld(); + const auto checker = MakeChecker(model); + Options options; + options.parallelism = Parallelism::None(); + + // Sweep the arm out past the post at θ ≈ π/2 with the tool extended and back + // again: two segments, each with its own violating region, so kCertifyAll + // (which drops a violating pair once per subtree) has more than one finding + // to cap. + Eigen::MatrixXd waypoints(2, 3); + waypoints << 0.0, 2.4, 0.0, 0.25, 0.25, 0.25; + + const CertificationResult uncapped = checker->CheckPath(waypoints, options); + ASSERT_EQ(uncapped.verdict, Verdict::kViolationFound); + ASSERT_GE(uncapped.findings.size(), 2u); + EXPECT_LE(static_cast(uncapped.findings.size()), + options.max_reported_findings); + + for (const int cap : {1, 2}) { + SCOPED_TRACE("cap = " + std::to_string(cap)); + Options capped = options; + capped.max_reported_findings = cap; + const CertificationResult result = checker->CheckPath(waypoints, capped); + EXPECT_EQ(result.verdict, Verdict::kViolationFound); + // Exactly `cap`, not merely at most: the sink keeps the cap earliest + // entries, and this run has more than `cap` of them. An "at most" assertion + // would be satisfied by a regression that returned nothing, which would + // also make the prefix check below vacuous. + ASSERT_EQ(static_cast(result.findings.size()), cap); + // The cap keeps the *earliest* findings, so a capped run is a prefix of the + // uncapped one — dropping the latest entry can never remove an earlier one. + for (std::size_t i = 0; i < result.findings.size(); ++i) { + EXPECT_EQ(result.findings[i].time, uncapped.findings[i].time); + EXPECT_EQ(result.findings[i].definite, uncapped.findings[i].definite); + } + } +} + +} // namespace +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/certificate_test.cc b/planning/certified_ccd/test/certificate_test.cc new file mode 100644 index 000000000000..a1b8cc75afe9 --- /dev/null +++ b/planning/certified_ccd/test/certificate_test.cc @@ -0,0 +1,706 @@ +/// @file +/// T7 — certificate audit (test plan T7; the search algorithm's +/// "certificate audit trail"). +/// +/// `VerifyCertificate` is the library's second, independent line of defence: it +/// replays every certification event from the checker's *public* seams, +/// re-restricting control points, recomputing motion bounds and re-querying +/// distances, and then checks that the certified intervals tile the whole +/// domain for every pair. This file audits the auditor. +/// +/// Structure: a small corpus of certified runs — two random worlds plus one +/// hand-built world whose pair structure is designed (one pair that only +/// certifies after deep subdivision, one that certifies at the root) — and a +/// table of adversarial mutations, each applied to *every* corpus case. A +/// mutation that any case accepts is a hole in the audit. +/// +/// certifier_test.cc already covers a handful of single-case mutations on its +/// own world; this file is the sweep, plus the mutation classes that need a +/// designed pair structure (record relabelling) or a second run +/// (kFindFirstViolation and non-free verdicts). + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/geometry/shape_specification.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/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::Parallelism; +using drake::geometry::Box; +using drake::geometry::Capsule; +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 Eigen::Vector3d; +using Eigen::VectorXd; + +/// A non-zero margin *and* a non-zero environment padding, so that +/// m_p = margin + padding is a number a tamperer could plausibly try to lower +/// and the "threshold below what the options call for" branch has something to +/// bite on. +constexpr double kMargin = 0.005; +constexpr double kEnvPadding = 0.002; + +CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +SpatialInertia Inertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +Options AuditOptions() { + Options options; + options.margin = kMargin; + options.parallelism = Parallelism::None(); + options.emit_certificate = true; + return options; +} + +std::unique_ptr MakeChecker( + std::shared_ptr> model) { + CertifiedContinuousCollisionChecker::Params params; + params.model = std::move(model); + params.default_options = AuditOptions(); + params.padding.env_padding = kEnvPadding; + params.padding.self_padding = kEnvPadding; + return std::make_unique(params); +} + +// --------------------------------------------------------------------------- +// World 1 (hand-built): a designed pair structure. +// --------------------------------------------------------------------------- +// +// A 2-dof Cartesian gantry (prismatic x, prismatic y) carrying a 5 mm sphere, +// with exactly two unfiltered pairs: +// +// * tool vs. "near_plate" — a 1 mm plate parallel to the travel, offset in y +// so the clearance is a constant 12 mm. With m_p = 0.007 and λ = 1 for the +// moving x coordinate, certification needs Δ = w_x < 0.012 − 0.007 − τ ≈ +// 0.005, i.e. a node no wider than ~1/64 of the 0.6 m travel: this pair +// only certifies at depth 6, producing dozens of records. +// * tool vs. "far_ball" — 3 m away, certified by the sphere prefilter at the +// root: exactly one record per segment. +// +// Two pairs that could not be more different in how hard they are to certify is +// exactly what the record-relabelling mutation needs. +std::unique_ptr> MakeDesignedWorld() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const RigidBody& carriage = plant.AddRigidBody("carriage", Inertia()); + const RigidBody& tool = plant.AddRigidBody("tool", Inertia()); + plant.AddJoint("gantry_x", plant.world_body(), {}, carriage, + {}, Vector3d::UnitX()); + plant.AddJoint("gantry_y", carriage, {}, tool, {}, + Vector3d::UnitY()); + plant.RegisterCollisionGeometry(tool, RigidTransformd(), Sphere(0.005), + "tool_geom", Friction()); + + const RigidBody& plate = plant.AddRigidBody("near_plate", Inertia()); + plant.WeldFrames(plant.world_frame(), plate.body_frame(), + RigidTransformd(Vector3d(0.0, 0.0175, 0.0))); + plant.RegisterCollisionGeometry(plate, RigidTransformd(), + Box(0.9, 0.001, 0.6), "near_plate_geom", + Friction()); + + const RigidBody& ball = plant.AddRigidBody("far_ball", Inertia()); + plant.WeldFrames(plant.world_frame(), ball.body_frame(), + RigidTransformd(Vector3d(0.0, 3.0, 0.0))); + plant.RegisterCollisionGeometry(ball, RigidTransformd(), Sphere(0.05), + "far_ball_geom", Friction()); + return builder.Build(); +} + +// --------------------------------------------------------------------------- +// Worlds 2, 3 (small random): a trimmed copy of the T4 generator. +// --------------------------------------------------------------------------- + +std::unique_ptr> MakeRandomWorld(uint64_t seed) { + std::mt19937_64 rng(seed); + const auto uniform = [&rng](double lo, double hi) { + return std::uniform_real_distribution(lo, hi)(rng); + }; + // Named locals throughout: sibling constructor arguments are evaluated in an + // unspecified order, so drawing variates inline would make these worlds — and + // therefore which seeds land in the corpus — depend on the toolchain. + const auto vector3 = [&uniform](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); + }; + const auto direction = [&vector3]() { + Vector3d v; + do { + v = vector3(-1, 1); + } while (v.norm() < 1e-3 || v.norm() > 1.0); + return v.normalized(); + }; + const auto offset = [&direction, &uniform](double lo, double hi) { + const Vector3d unit = direction(); + const double length = uniform(lo, hi); + return Vector3d(unit * length); + }; + + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + std::vector*> links; + for (int i = 0; i < 3; ++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 = vector3(-0.5, 0.5); + const RigidTransformd X_PF(RollPitchYawd(rpy_PF), offset(0.25, 0.35)); + const Vector3d axis = direction(); + if (i == 1) { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, + body, RigidTransformd(), axis); + } else { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, body, + RigidTransformd(), axis); + } + const RigidTransformd X_LG(offset(0.12, 0.18)); + const double radius = uniform(0.02, 0.04); + const double length = uniform(0.05, 0.10); + plant.RegisterCollisionGeometry(body, X_LG, Capsule(radius, length), + name + "_geom", Friction()); + links.push_back(&body); + } + for (int i = 0; i < 3; ++i) { + const std::string name = "obstacle" + std::to_string(i); + const RigidBody& body = plant.AddRigidBody(name, Inertia()); + const Vector3d rpy_W = vector3(-3, 3); + plant.WeldFrames(plant.world_frame(), body.body_frame(), + RigidTransformd(RollPitchYawd(rpy_W), offset(0.3, 0.8))); + if (i % 2 == 0) { + const Vector3d size = vector3(0.08, 0.2); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Box(size.x(), size.y(), size.z()), + name + "_geom", Friction()); + } else { + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Sphere(uniform(0.05, 0.11)), + name + "_geom", Friction()); + } + } + return builder.Build(); +} + +/// A cubic Bézier whose control points are equally spaced from `start` to +/// `end` — the straight segment, but with four control points, so a mutation +/// can perturb an *interior* one without moving either endpoint (which would +/// change the path's start configuration and short-circuit the check we mean to +/// exercise). +Eigen::MatrixXd CubicControlPoints(const VectorXd& start, const VectorXd& end) { + Eigen::MatrixXd points(start.size(), 4); + for (int j = 0; j < 4; ++j) { + const double u = j / 3.0; + points.col(j) = (1.0 - u) * start + u * end; + } + return points; +} + +// --------------------------------------------------------------------------- +// The corpus. +// --------------------------------------------------------------------------- + +struct AuditCase { + std::string name; + std::shared_ptr> model; + std::unique_ptr checker; + Eigen::MatrixXd control_points; + std::optional path; + Certificate certificate; + /// true when this case's two pairs were designed to have wildly different + /// certification depths (only the hand-built world). + bool designed{false}; + + /// The path a verifier would be handed if one control point were nudged. + PiecewiseBezierPath PerturbedPath(double delta) const { + Eigen::MatrixXd points = control_points; + points(0, 1) += delta; + return checker->Normalize(BezierCurve(0.0, 1.0, points), + AuditOptions()); + } + + bool Verify(const Certificate& certificate_in) const { + return VerifyCertificate(*checker, *path, certificate_in); + } +}; + +/// Builds the corpus once. Everything in it is a run that ended +/// Verdict::kCertifiedFree with an emitted certificate; a case that failed to +/// certify is *not* added, so CorpusIsBuiltAndVerifies (which requires three +/// cases, the designed one first) is the single place that reports the problem. +/// No gtest assertion is used in here: this initializer runs inside whichever +/// test happens to touch Corpus() first, which changes under --gtest_filter or +/// --gtest_shuffle, and a failure charged to an arbitrary test is a failure +/// nobody can read. +/// +/// The vector is deliberately 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.) +const std::vector>& Corpus() { + static const std::vector>* corpus = [] { + auto* cases = new std::vector>(); + const Options options = AuditOptions(); + + // 1. The designed world. + { + auto entry = std::make_unique(); + entry->name = "designed_gantry"; + entry->designed = true; + entry->model = MakeDesignedWorld(); + entry->checker = MakeChecker(entry->model); + VectorXd start(2), end(2); + start << -0.3, 0.0; + end << 0.3, 0.0; + entry->control_points = CubicControlPoints(start, end); + const BezierCurve trajectory(0.0, 1.0, entry->control_points); + const CertificationResult result = + entry->checker->CheckTrajectory(trajectory, options); + if (result.verdict == Verdict::kCertifiedFree && + result.certificate.has_value()) { + entry->path = entry->checker->Normalize(trajectory, options); + entry->certificate = *result.certificate; + cases->push_back(std::move(entry)); + } + } + + // 2. Small random worlds — the first two seeds whose trajectory certifies. + // Sweeping deterministically (rather than hard-coding lucky seeds) keeps + // the corpus honest if the geometry ever shifts underneath it. + for (uint64_t seed = 1; seed <= 40 && cases->size() < 3; ++seed) { + auto entry = std::make_unique(); + entry->name = "random_world_seed_" + std::to_string(seed); + entry->model = MakeRandomWorld(seed); + entry->checker = MakeChecker(entry->model); + const int n = entry->model->plant().num_positions(); + VectorXd start = VectorXd::Zero(n); + VectorXd end = VectorXd::Zero(n); + for (int i = 0; i < n; ++i) { + start[i] = 0.15 * ((i % 2 == 0) ? 1.0 : -1.0); + end[i] = start[i] + 0.25; + } + entry->control_points = CubicControlPoints(start, end); + const BezierCurve trajectory(0.0, 1.0, entry->control_points); + const CertificationResult result = + entry->checker->CheckTrajectory(trajectory, options); + if (result.verdict != Verdict::kCertifiedFree) continue; + entry->path = entry->checker->Normalize(trajectory, options); + entry->certificate = *result.certificate; + cases->push_back(std::move(entry)); + } + return cases; + }(); + return *corpus; +} + +/// Record counts per pair, for picking "the hardest" and "the easiest" pair. +std::vector RecordsPerPair(const AuditCase& entry) { + std::vector counts(entry.certificate.pairs.size(), 0); + for (const CertificateRecord& record : entry.certificate.records) { + ++counts[record.pair_index]; + } + return counts; +} + +/// True iff `pair`'s records cover [0, 1] of every segment — the same coverage +/// property VerifyCertificate checks, re-derived here so a test can assert that +/// a mutation left coverage *intact* and therefore had to be caught by the +/// per-record arithmetic instead. +bool TilesEverySegment(const Certificate& certificate, int pair, + std::size_t num_segments) { + for (std::size_t segment = 0; segment < num_segments; ++segment) { + std::vector> intervals; + for (const CertificateRecord& record : certificate.records) { + if (record.pair_index == pair && + record.segment == static_cast(segment)) { + intervals.emplace_back(record.s_start, record.s_end); + } + } + std::sort(intervals.begin(), intervals.end()); + double covered_to = 0.0; + for (const auto& [lo, hi] : intervals) { + if (lo > covered_to) break; + covered_to = std::max(covered_to, hi); + } + if (!(covered_to >= 1.0)) return false; + } + return true; +} + +/// Index of a record whose pair the trajectory actually moves and whose +/// interval is a proper sub-interval — the kind a tamperer would target. +int MovingRecordIndex(const AuditCase& entry) { + const MotionBoundTable table = + entry.checker->ComputeMotionBounds(*entry.path); + for (int i = 0; i < static_cast(entry.certificate.records.size()); ++i) { + const CertificateRecord& record = entry.certificate.records[i]; + if (!table.pair_is_static(record.pair_index) && record.s_end < 1.0) { + return i; + } + } + return -1; +} + +// --------------------------------------------------------------------------- +// 1. Baseline. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertificateAuditTest, CorpusIsBuiltAndVerifies) { + const auto& corpus = Corpus(); + ASSERT_GE(corpus.size(), 3u) + << "the corpus needs the designed world plus at least two random ones; a " + "case that failed to certify is dropped rather than added empty, so a " + "short corpus means a run stopped certifying"; + ASSERT_TRUE(corpus.front()->designed) + << "the designed world must be first: the mutations that need its pair " + "structure index Corpus().front()"; + for (const auto& entry : corpus) { + SCOPED_TRACE(entry->name); + EXPECT_FALSE(entry->certificate.records.empty()); + EXPECT_EQ(entry->certificate.pairs.size(), entry->checker->pairs().size()); + EXPECT_TRUE(entry->Verify(entry->certificate)); + } +} + +GTEST_TEST(CertificateAuditTest, DesignedWorldHasTheIntendedPairStructure) { + ASSERT_FALSE(Corpus().empty()); + const AuditCase& entry = *Corpus().front(); + ASSERT_TRUE(entry.designed); + ASSERT_EQ(entry.certificate.pairs.size(), 2u) + << "the designed world should present exactly the tool/plate and " + "tool/ball pairs"; + const std::vector counts = RecordsPerPair(entry); + const int hardest = *std::max_element(counts.begin(), counts.end()); + const int easiest = *std::min_element(counts.begin(), counts.end()); + // The far pair certifies at the root: exactly one record, for the path's one + // segment. The 12 mm pair needs Δ = w_x < 0.012 − 0.007 − τ ≈ 0.005 against + // 0.6 m of travel, i.e. a node half-width of 0.3/2^d < 0.005 ⇒ d = 6, and a + // constant clearance means *every* depth-6 node certifies it: 2^6 = 64 + // records. Pinned exactly, so a regression that loosened (or tightened) the + // motion bound by even one bisection level shows up here rather than hiding + // behind an inequality. + EXPECT_EQ(easiest, 1); + EXPECT_EQ(hardest, 64); + // Every pair's records must claim the same, correct threshold. + for (const CertificateRecord& record : entry.certificate.records) { + EXPECT_DOUBLE_EQ(record.threshold, kMargin + kEnvPadding); + } +} + +// --------------------------------------------------------------------------- +// 2. Adversarial mutations, applied to every corpus case. +// --------------------------------------------------------------------------- + +using Mutation = std::function; + +/// Applies `mutate` to every corpus case and requires the result to be +/// rejected. `mutate` returns false when the case cannot host the mutation. +void ExpectRejectedEverywhere(const std::string& what, const Mutation& mutate) { + int applied = 0; + for (const auto& entry : Corpus()) { + SCOPED_TRACE(what + " on " + entry->name); + Certificate certificate = entry->certificate; + if (!mutate(*entry, &certificate)) continue; + ++applied; + EXPECT_FALSE(entry->Verify(certificate)) + << "VerifyCertificate accepted a certificate mutated by: " << what; + } + EXPECT_GT(applied, 0) << "the mutation '" << what + << "' was never applicable to any corpus case"; +} + +GTEST_TEST(CertificateAuditTest, RejectsInflatedClearance) { + ExpectRejectedEverywhere("inflate phi_hat", + [](const AuditCase&, Certificate* certificate) { + if (certificate->records.empty()) return false; + certificate->records.front().phi_hat += 1.0; + return true; + }); +} + +GTEST_TEST(CertificateAuditTest, RejectsWidenedInterval) { + ExpectRejectedEverywhere( + "widen a certified interval", + [](const AuditCase& entry, Certificate* certificate) { + const int index = MovingRecordIndex(entry); + if (index < 0) return false; + CertificateRecord& record = certificate->records[index]; + const double width = record.s_end - record.s_start; + record.s_end = std::min(1.0, record.s_end + width); + return record.s_end > entry.certificate.records[index].s_end; + }); +} + +GTEST_TEST(CertificateAuditTest, RejectsShiftedRepresentativeConfiguration) { + ExpectRejectedEverywhere( + "shift qc off the trajectory", + [](const AuditCase& entry, Certificate* certificate) { + const int index = MovingRecordIndex(entry); + if (index < 0) return false; + certificate->records[index].qc[0] += 0.05; + return true; + }); +} + +GTEST_TEST(CertificateAuditTest, RejectsDeletedRecord) { + // The certifier's intervals tile the domain disjointly, so deleting any + // record punches a coverage hole — even one whose own arithmetic was sound. + ExpectRejectedEverywhere( + "delete a record", [](const AuditCase&, Certificate* certificate) { + if (certificate->records.size() < 2) return false; + certificate->records.erase(certificate->records.begin()); + return true; + }); +} + +GTEST_TEST(CertificateAuditTest, RejectsTruncatedRecords) { + ExpectRejectedEverywhere( + "truncate the record list", + [](const AuditCase&, Certificate* certificate) { + if (certificate->records.size() < 4) return false; + certificate->records.resize(certificate->records.size() * 3 / 4); + return true; + }); +} + +GTEST_TEST(CertificateAuditTest, RejectsLoweredThreshold) { + // Lower *every* record of one pair, so the replay's self-consistency check + // ("all records of a pair claim the same threshold") passes and the mutation + // has to be caught by the check that actually matters: the claimed threshold + // must be at least the margin + padding the options call for. + ExpectRejectedEverywhere( + "lower one pair's threshold below margin + padding", + [](const AuditCase&, Certificate* certificate) { + if (certificate->records.empty()) return false; + const int pair = certificate->records.front().pair_index; + for (CertificateRecord& record : certificate->records) { + if (record.pair_index == pair) record.threshold -= 0.003; + } + return true; + }); +} + +GTEST_TEST(CertificateAuditTest, RejectsPairTableMismatch) { + ExpectRejectedEverywhere("drop a pair from the snapshot", + [](const AuditCase&, Certificate* certificate) { + if (certificate->pairs.size() < 2) return false; + certificate->pairs.pop_back(); + return true; + }); + ExpectRejectedEverywhere("swap two entries of the pair snapshot", + [](const AuditCase&, Certificate* certificate) { + if (certificate->pairs.size() < 2) return false; + std::swap(certificate->pairs.front(), + certificate->pairs.back()); + return true; + }); +} + +GTEST_TEST(CertificateAuditTest, RejectsRelabelledPairRecords) { + // Relabelling records between two pairs of *similar* difficulty can be a true + // statement about a claim nobody made, so this mutation is only meaningful + // where the pair structure is designed: give the 12 mm pair the far ball's + // single root-wide record and its motion bound (half the 0.6 m travel) + // swamps its 5 mm of slack. + ASSERT_FALSE(Corpus().empty()); + const AuditCase& entry = *Corpus().front(); + ASSERT_TRUE(entry.designed); + const std::vector counts = RecordsPerPair(entry); + const int hardest = static_cast( + std::max_element(counts.begin(), counts.end()) - counts.begin()); + const int easiest = static_cast( + std::min_element(counts.begin(), counts.end()) - counts.begin()); + ASSERT_NE(hardest, easiest); + + Certificate certificate = entry.certificate; + for (CertificateRecord& record : certificate.records) { + if (record.pair_index == hardest) { + record.pair_index = easiest; + } else if (record.pair_index == easiest) { + record.pair_index = hardest; + } + } + // Relabelling permutes two complete tilings, so coverage is *not* what + // catches this — verified rather than asserted, because a mutation that + // happened to break coverage would make the test pass for the wrong reason + // and leave the arithmetic untested. + for (int pair = 0; pair < static_cast(certificate.pairs.size()); + ++pair) { + EXPECT_TRUE( + TilesEverySegment(certificate, pair, entry.path->segments().size())) + << "pair " << pair + << " lost its full tiling, so this mutation would " + "have been caught by the coverage check instead of the arithmetic"; + } + EXPECT_FALSE(entry.Verify(certificate)); +} + +GTEST_TEST(CertificateAuditTest, RejectsPerturbedPath) { + // The certificate is a statement about one specific path. Handing the + // verifier a path with a nudged interior control point must not verify: every + // record's qc stops being the midpoint apex of the interval it names. + for (const auto& entry : Corpus()) { + SCOPED_TRACE(entry->name); + const PiecewiseBezierPath perturbed = entry->PerturbedPath(0.05); + EXPECT_FALSE( + VerifyCertificate(*entry->checker, perturbed, entry->certificate)); + } +} + +// --------------------------------------------------------------------------- +// 3. What a *valid* transformation looks like — pinned deliberately. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertificateAuditTest, AcceptsReorderedRecords) { + // Re-ordering is the one item on the classic mutation list that must NOT be + // rejected: a permutation of a valid proof is still a valid proof. The replay + // sorts the intervals itself before checking coverage and every record is + // checked independently, so order carries no information. Pinning this keeps + // a future "records must arrive sorted" shortcut from being mistaken for a + // security property — and keeps the mutations above honest, since a verifier + // that rejected everything would pass all of them. + std::mt19937 rng(20260826); + int shuffled = 0; + for (const auto& entry : Corpus()) { + SCOPED_TRACE(entry->name); + Certificate certificate = entry->certificate; + if (certificate.records.size() < 2) continue; + ++shuffled; + std::shuffle(certificate.records.begin(), certificate.records.end(), rng); + EXPECT_TRUE(entry->Verify(certificate)) + << "a permutation of a valid certificate is still a valid certificate"; + } + EXPECT_GE(shuffled, 3) << "every corpus case should have had a record list " + "long enough to permute"; +} + +// --------------------------------------------------------------------------- +// 4. Certificates a run cannot honestly produce. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertificateAuditTest, NoCertificateUnlessRequested) { + ASSERT_FALSE(Corpus().empty()); + const AuditCase& entry = *Corpus().front(); + Options options = AuditOptions(); + options.emit_certificate = false; + const BezierCurve trajectory(0.0, 1.0, entry.control_points); + const CertificationResult result = + entry.checker->CheckTrajectory(trajectory, options); + ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); + EXPECT_FALSE(result.certificate.has_value()); +} + +/// The designed world again, but driven straight through the 1 mm plate at +/// y = 0.0175: q(t) sweeps y from 0 to 0.05 while x crosses the plate's span. +Eigen::MatrixXd ViolatingControlPoints() { + VectorXd start(2), end(2); + start << -0.3, 0.0; + end << 0.3, 0.05; + return CubicControlPoints(start, end); +} + +GTEST_TEST(CertificateAuditTest, NonFreeVerdictCertificateIsNotAProof) { + // Pinned behaviour for "the certificate of a non-free run is absent or + // unusable": the field is *present* whenever emit_certificate was asked for, + // and the records the run did make are individually valid — but a run that + // found a violation dropped that pair from the subtree instead of certifying + // it, so the trail cannot cover the domain and the replay refuses it. The + // resolution is "usable as an audit trail, unusable as a proof". + ASSERT_FALSE(Corpus().empty()); + const AuditCase& entry = *Corpus().front(); + const Options options = AuditOptions(); + const BezierCurve trajectory(0.0, 1.0, ViolatingControlPoints()); + const PiecewiseBezierPath path = + entry.checker->Normalize(trajectory, options); + + const CertificationResult violating = + entry.checker->CheckTrajectory(trajectory, options); + ASSERT_EQ(violating.verdict, Verdict::kViolationFound); + ASSERT_TRUE(violating.certificate.has_value()); + EXPECT_FALSE(VerifyCertificate(*entry.checker, path, *violating.certificate)) + << "a certificate from a violating run must not read as a proof"; + + // Same for a run stopped by the node budget. That needs the *free* + // trajectory: a definite violation outranks budget exhaustion in the verdict + // reduction, so the budget branch is only reachable when nothing violates. + Options budgeted = options; + budgeted.max_nodes = 3; + const BezierCurve free_trajectory(0.0, 1.0, entry.control_points); + const CertificationResult truncated = + entry.checker->CheckTrajectory(free_trajectory, budgeted); + ASSERT_EQ(truncated.verdict, Verdict::kBudgetExhausted); + ASSERT_TRUE(truncated.certificate.has_value()); + EXPECT_FALSE(entry.Verify(*truncated.certificate)); + + // ... and for kFindFirstViolation, which additionally *prunes* the search: + // every node starting after the witness is skipped, so whole stretches of the + // domain are never visited at all. + Options find_first = options; + find_first.mode = SearchMode::kFindFirstViolation; + const CertificationResult pruned = + entry.checker->CheckTrajectory(trajectory, find_first); + ASSERT_EQ(pruned.verdict, Verdict::kViolationFound); + ASSERT_TRUE(pruned.certificate.has_value()); + EXPECT_FALSE(VerifyCertificate(*entry.checker, path, *pruned.certificate)); +} + +GTEST_TEST(CertificateAuditTest, + FindFirstViolationOnAFreeTrajectoryStillCovers) { + // The complement, pinned so the rule above is not mistaken for "the mode + // invalidates certificates": with nothing to find, kFindFirstViolation has + // nothing to prune against, explores the same tree as kCertifyAll, and its + // trail is a complete proof. + ASSERT_FALSE(Corpus().empty()); + const AuditCase& entry = *Corpus().front(); + Options options = AuditOptions(); + options.mode = SearchMode::kFindFirstViolation; + const BezierCurve trajectory(0.0, 1.0, entry.control_points); + const CertificationResult result = + entry.checker->CheckTrajectory(trajectory, options); + ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); + ASSERT_TRUE(result.certificate.has_value()); + EXPECT_TRUE(entry.Verify(*result.certificate)); +} + +} // namespace +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/certifier_test.cc b/planning/certified_ccd/test/certifier_test.cc new file mode 100644 index 000000000000..4114ce6a5bb1 --- /dev/null +++ b/planning/certified_ccd/test/certifier_test.cc @@ -0,0 +1,1032 @@ +/// @file +/// End-to-end tests of the certifier core and the public facade (the test plan, +/// T4/T6/T7 restricted to a focused corpus; the large randomized T4 fuzz +/// corpus is a separate milestone and deliberately not 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 + +#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/rigid_transform.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/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::Parallelism; +using drake::geometry::Box; +using drake::geometry::HalfSpace; +using drake::geometry::QueryObject; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +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::Vector3d; +using Eigen::VectorXd; + +constexpr double kMargin = 0.01; + +CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +SpatialInertia UnitInertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +/// A planar 3-dof arm (revolute, revolute, prismatic) in the z = 0 plane: +/// +/// 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] +/// +/// 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", UnitInertia()); + const RigidBody& link2 = plant->AddRigidBody("link2", UnitInertia()); + const RigidBody& tool = plant->AddRigidBody("tool", UnitInertia()); + + 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, UnitInertia()); + 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", UnitInertia()); + 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", UnitInertia()); + 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 std::shared_ptr>(builder.Build()); +} + +/// 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 — exactly r_tool + r_graze + kMargin. +std::shared_ptr> MakeGrazeWorld() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + AddArm(&plant); + AddWeldedSphere(&plant, "graze", Vector3d(0.80, 0.11, 0.0), 0.05); + return std::shared_ptr>(builder.Build()); +} + +/// A genuinely free squeeze: the tool slides between two spheres that leave +/// only 5 mm of clearance over the margin, so the certificate is real but has +/// to be earned by subdividing (the mirror image of the tangency world). +std::shared_ptr> MakeGapWorld() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + AddArm(&plant); + // Sphere surface to tool surface at the closest approach: + // 0.115 − 0.05 − 0.05 = 0.015 = kMargin + 0.005. + 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); + return std::shared_ptr>(builder.Build()); +} + +CertifiedContinuousCollisionChecker MakeChecker( + std::shared_ptr> model, Options options) { + CertifiedContinuousCollisionChecker::Params params; + params.model = std::move(model); + params.default_options = std::move(options); + return CertifiedContinuousCollisionChecker(params); +} + +Options SerialOptions() { + Options options; + options.margin = kMargin; + options.parallelism = Parallelism::None(); + return options; +} + +/// A cubic Bézier from `start` to `end` with linearly spaced control points +/// (so the curve is the straight segment, traversed with a nontrivial +/// parametrization) over the time interval [t0, t1]. +BezierCurve MakeBezier(const VectorXd& start, const VectorXd& end, + int order, double t0, double t1) { + 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); +} + +/// 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 (separately tested, T3) distance oracle so that +/// halfspace pairs are handled the same way. +SampledClearance SampleClearance( + const CertifiedContinuousCollisionChecker& checker, + const PiecewiseBezierPath& path, int samples_per_segment, + double threshold) { + const RobotDiagram& model = checker.model(); + auto root = model.CreateDefaultContext(); + auto& plant_context = model.plant().GetMyMutableContextFromRoot(root.get()); + const auto& scene_graph = model.scene_graph(); + SampledClearance result; + for (int k = 0; k < static_cast(path.segments().size()); ++k) { + const BezierSegment& segment = path.segments()[k]; + for (int i = 0; i <= samples_per_segment; ++i) { + const double s = static_cast(i) / samples_per_segment; + const VectorXd q = path.EvaluateSegment(k, s); + model.plant().SetPositions(&plant_context, q); + const auto& query_object = + scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*root)); + for (const PairRecord& pair : checker.pairs()) { + const double phi = + checker.distance_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; +} + +/// Re-evaluates one finding's configuration from scratch and returns the +/// oracle distance of its pair there. +double DistanceAtFinding(const CertifiedContinuousCollisionChecker& checker, + const Finding& finding) { + const RobotDiagram& model = checker.model(); + 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)); + for (const PairRecord& pair : checker.pairs()) { + if (pair.id.a == finding.pair.a && pair.id.b == finding.pair.b) { + return checker.distance_oracle().SignedDistance(query_object, pair); + } + } + ADD_FAILURE() << "the finding names a pair the checker does not know."; + return 0.0; +} + +VectorXd MakeQ(double theta1, double theta2, double d) { + VectorXd q(3); + q << theta1, theta2, d; + return q; +} + +// --------------------------------------------------------------------------- +// 1. A free trajectory is certified, and dense sampling agrees. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, FreeTrajectoryCertified) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); + + const CertificationResult result = checker.CheckTrajectory(trajectory); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + EXPECT_TRUE(result.findings.empty()); + EXPECT_GT(result.stats.nodes, 0u); + EXPECT_GT(result.stats.sphere_certifications, 0u); + EXPECT_FALSE(result.certificate.has_value()); + + // Independent cross-check: 10^4 dense samples must all clear the margin. + const PiecewiseBezierPath path = checker.Normalize(trajectory); + const SampledClearance sampled = + SampleClearance(checker, path, 10000, kMargin); + EXPECT_GT(sampled.min_clearance, kMargin); + EXPECT_TRUE(std::isnan(sampled.first_crossing)); +} + +GTEST_TEST(CertifierTest, NarrowGapCertifiedBySubdivision) { + const auto model = MakeGapWorld(); + const auto checker = MakeChecker(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, 0.0, 1.0); + + Options options = SerialOptions(); + options.emit_certificate = true; + const CertificationResult result = + checker.CheckTrajectory(trajectory, options); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + EXPECT_TRUE(result.findings.empty()); + // 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.stats.max_depth, 4); + + const PiecewiseBezierPath path = checker.Normalize(trajectory); + const SampledClearance sampled = + SampleClearance(checker, path, 10000, kMargin); + EXPECT_GT(sampled.min_clearance, kMargin); + EXPECT_LT(sampled.min_clearance, kMargin + 0.01); + ASSERT_TRUE(result.certificate.has_value()); + EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); +} + +GTEST_TEST(CertifierTest, FreePathAndEdgeCertified) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + + 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); + const CertificationResult path_result = checker.CheckPath(waypoints); + EXPECT_EQ(path_result.verdict, Verdict::kCertifiedFree); + + const CertificationResult edge_result = + checker.CheckEdge(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10)); + EXPECT_EQ(edge_result.verdict, Verdict::kCertifiedFree); +} + +// --------------------------------------------------------------------------- +// 2. A sweeping trajectory that hits an obstacle. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, ViolationFoundWithExactWitness) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); + + const CertificationResult result = checker.CheckTrajectory(trajectory); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_FALSE(result.findings.empty()); + const Finding& finding = result.findings.front(); + EXPECT_TRUE(finding.definite); + 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. + const PiecewiseBezierPath path = checker.Normalize(trajectory); + EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), 1e-9); + + // ... and re-querying the distance from a fresh context must confirm the + // violation. + const double phi = DistanceAtFinding(checker, finding); + EXPECT_LT(phi, kMargin); + EXPECT_NEAR(phi, finding.distance, 1e-12); +} + +GTEST_TEST(CertifierTest, FindFirstReturnsEarliestWitness) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); + + Options options = SerialOptions(); + options.mode = SearchMode::kFindFirstViolation; + const CertificationResult result = + checker.CheckTrajectory(trajectory, options); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_EQ(result.findings.size(), 1u); + + const PiecewiseBezierPath path = checker.Normalize(trajectory); + const SampledClearance sampled = + SampleClearance(checker, path, 10000, kMargin); + ASSERT_FALSE(std::isnan(sampled.first_crossing)); + // The branch-and-bound recursion drives the reported witness to the earliest + // violating time, which dense sampling brackets from above. + EXPECT_NEAR(result.findings.front().time, sampled.first_crossing, 5e-3); + EXPECT_LE(result.findings.front().time, sampled.first_crossing + 1e-9); +} + +// --------------------------------------------------------------------------- +// 3. Grazing tangency is inconclusive — never certified free. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { + const auto model = MakeGrazeWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + // θ1 = θ2 = 0 throughout; only the prismatic coordinate moves, sliding the + // tool sphere past the obstacle at exactly margin distance. + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.0, 0.0, 0.20), 1, 0.0, 1.0); + + Options options = SerialOptions(); + // A coarser floor keeps the cost of the tangency cascade bounded; the + // verdict is what matters here, not the depth. + options.min_interval = 1e-4; + const CertificationResult result = + checker.CheckTrajectory(trajectory, options); + + EXPECT_EQ(result.verdict, Verdict::kInconclusive); + EXPECT_NE(result.verdict, Verdict::kCertifiedFree); + ASSERT_FALSE(result.findings.empty()); + const Finding& finding = result.findings.front(); + EXPECT_FALSE(finding.definite); + // The near-witness sits within a hair of the threshold. + EXPECT_NEAR(finding.distance, kMargin, 1e-3); + EXPECT_NEAR(DistanceAtFinding(checker, finding), 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 PiecewiseBezierPath path = checker.Normalize(trajectory); + const SampledClearance sampled = + SampleClearance(checker, path, 10000, kMargin); + EXPECT_NEAR(sampled.min_clearance, kMargin, 1e-6); +} + +// --------------------------------------------------------------------------- +// 4. Static pairs (J(p) = ∅) are resolved once and certified globally. +// --------------------------------------------------------------------------- + +// Note on where static pairs come from: MultibodyPlant::Finalize() already +// filters every pair *within* a welded subgraph, so two anchored obstacles (or +// two members of a welded cluster on the robot) never even reach the checker +// as a candidate pair. The reachable source of J(p) = ∅ is therefore the +// constant-coordinate carve-out of trajectory normalization; the joint-support +// scope: a coordinate that no control point of the trajectory moves is removed +// from every J(p), and pairs left with an empty set are resolved once at q(t0). +GTEST_TEST(CertifierTest, StaticPairsResolvedOnce) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + // Only the prismatic coordinate moves: θ1 and θ2 are constant, so every pair + // whose relative pose depends only on them becomes static. + const BezierCurve trajectory = + MakeBezier(MakeQ(0.3, -0.2, 0.0), MakeQ(0.3, -0.2, 0.15), 2, 0.0, 1.0); + + const PiecewiseBezierPath path = checker.Normalize(trajectory); + const MotionBoundTable table = checker.ComputeMotionBounds(path); + + int num_static = 0; + int num_moving = 0; + for (int p = 0; p < table.num_pairs(); ++p) { + (table.pair_is_static(p) ? num_static : num_moving) += 1; + } + EXPECT_GT(num_static, 0) << "the constant-coordinate carve-out should have " + "made the link1/link2 pairs static"; + EXPECT_GT(num_moving, 0); + + Options options = SerialOptions(); + options.emit_certificate = true; + const CertificationResult result = + checker.CheckTrajectory(trajectory, options); + ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); + ASSERT_TRUE(result.certificate.has_value()); + + // A static pair is certified exactly once — one full-segment record per + // segment, all sharing the single representative configuration q(t0) — and + // never appears in a node record. + const int num_segments = static_cast(path.segments().size()); + std::vector records_per_pair(table.num_pairs(), 0); + for (const CertificateRecord& record : result.certificate->records) { + ++records_per_pair[record.pair_index]; + if (table.pair_is_static(record.pair_index)) { + EXPECT_EQ(record.s_start, 0.0); + EXPECT_EQ(record.s_end, 1.0); + EXPECT_EQ(record.motion_bound, 0.0); + EXPECT_LT( + (record.qc - path.EvaluateSegment(0, 0.0)).cwiseAbs().maxCoeff(), + 1e-15); + } + } + for (int p = 0; p < table.num_pairs(); ++p) { + if (table.pair_is_static(p)) { + EXPECT_EQ(records_per_pair[p], num_segments) + << "static pair " << p << " was resolved more than once"; + } + } + EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); +} + +// --------------------------------------------------------------------------- +// 4b. Padding reaches the effective threshold, and the env/self split is the +// documented one (self = both bodies move relative to the world). +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, PaddingSemantics) { + const auto model = MakeArmWorld(); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); + const auto is_arm_self_pair = [&model](const Finding& finding) { + const auto& plant = model->plant(); + const std::string a = plant.get_body(finding.pair.body_a).name(); + const std::string b = plant.get_body(finding.pair.body_b).name(); + return (a == "link1" || a == "link2" || a == "tool") && + (b == "link1" || b == "link2" || b == "tool"); + }; + + // The one robot-vs-robot pair (link1, tool) keeps ≈ 0.27 m of clearance on + // this trajectory, so 0.4 m of *self* padding must break it — and nothing + // else, because every other pair has an anchored side and takes the (zero) + // environment padding. + { + CertifiedContinuousCollisionChecker::Params params; + params.model = model; + params.default_options = SerialOptions(); + params.padding.self_padding = 0.40; + const CertifiedContinuousCollisionChecker checker(params); + const CertificationResult result = checker.CheckTrajectory(trajectory); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + for (const Finding& finding : result.findings) { + EXPECT_TRUE(is_arm_self_pair(finding)) + << "self padding must not apply to environment pairs"; + } + } + + // Mirrored: environment padding reaches the arm-vs-obstacle pairs (the + // ground halfspace is 0.45 m away) and leaves the self pair alone. + { + CertifiedContinuousCollisionChecker::Params params; + params.model = model; + params.default_options = SerialOptions(); + params.padding.env_padding = 0.50; + const CertifiedContinuousCollisionChecker checker(params); + const CertificationResult result = checker.CheckTrajectory(trajectory); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + for (const Finding& finding : result.findings) { + EXPECT_FALSE(is_arm_self_pair(finding)) + << "environment padding must not apply to robot self pairs"; + } + } + + // A per-body-pair matrix overrides the scalars. + { + CertifiedContinuousCollisionChecker::Params params; + params.model = model; + params.default_options = SerialOptions(); + params.padding.env_padding = 0.50; + params.padding.per_body_pair = Eigen::MatrixXd::Zero( + model->plant().num_bodies(), model->plant().num_bodies()); + const CertifiedContinuousCollisionChecker checker(params); + EXPECT_EQ(checker.CheckTrajectory(trajectory).verdict, + Verdict::kCertifiedFree); + } + + // A mis-sized matrix is a clear throw. + { + CertifiedContinuousCollisionChecker::Params params; + params.model = model; + params.default_options = SerialOptions(); + params.padding.per_body_pair = Eigen::MatrixXd::Zero(2, 2); + EXPECT_THROW(CertifiedContinuousCollisionChecker{params}, std::exception); + } +} + +// --------------------------------------------------------------------------- +// 5. Retiming invariance (T6): the certificate is a property of the path. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, RetimingInvariance) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const VectorXd start = MakeQ(0.0, 0.0, 0.0); + const VectorXd end = MakeQ(0.8, -0.4, 0.10); + const BezierCurve fast = MakeBezier(start, end, 3, 0.0, 1.0); + const BezierCurve slow = MakeBezier(start, end, 3, -2.5, 4.2); + + Options options = SerialOptions(); + options.emit_certificate = true; + const CertificationResult a = checker.CheckTrajectory(fast, options); + const CertificationResult b = checker.CheckTrajectory(slow, options); + + EXPECT_EQ(a.verdict, b.verdict); + EXPECT_EQ(a.stats.nodes, b.stats.nodes); + EXPECT_EQ(a.stats.narrowphase_queries, b.stats.narrowphase_queries); + EXPECT_EQ(a.stats.sphere_certifications, b.stats.sphere_certifications); + EXPECT_EQ(a.stats.max_depth, b.stats.max_depth); + + ASSERT_TRUE(a.certificate.has_value()); + ASSERT_TRUE(b.certificate.has_value()); + ASSERT_EQ(a.certificate->records.size(), b.certificate->records.size()); + for (size_t i = 0; i < a.certificate->records.size(); ++i) { + const CertificateRecord& ra = a.certificate->records[i]; + const CertificateRecord& rb = b.certificate->records[i]; + // The certified interval structure lives in parameter space, so it is + // bit-identical; only the reported *times* would differ. + EXPECT_EQ(ra.segment, rb.segment); + EXPECT_EQ(ra.s_start, rb.s_start); + EXPECT_EQ(ra.s_end, rb.s_end); + EXPECT_EQ(ra.pair_index, rb.pair_index); + EXPECT_EQ(ra.phi_hat, rb.phi_hat); + EXPECT_EQ(ra.motion_bound, rb.motion_bound); + EXPECT_EQ(ra.threshold, rb.threshold); + EXPECT_TRUE(ra.qc == rb.qc); + } +} + +// --------------------------------------------------------------------------- +// 6. Certificate emission, replay and mutation (T7). +// --------------------------------------------------------------------------- + +class CertificateFixture : public ::testing::Test { + protected: + CertificateFixture() + : model_(MakeArmWorld()), + checker_(MakeChecker(model_, SerialOptions())), + trajectory_(MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, + 0.0, 1.0)), + path_(checker_.Normalize(trajectory_)) { + Options options = SerialOptions(); + options.emit_certificate = true; + result_ = checker_.CheckTrajectory(trajectory_, options); + } + + /// Index of a record belonging to a pair the trajectory actually moves (so + /// the record carries a real node interval, not the global static one). + int MovingRecordIndex() const { + const MotionBoundTable table = checker_.ComputeMotionBounds(path_); + for (int i = 0; i < static_cast(result_.certificate->records.size()); + ++i) { + const CertificateRecord& record = result_.certificate->records[i]; + if (!table.pair_is_static(record.pair_index) && record.s_end < 1.0) { + return i; + } + } + return -1; + } + + std::shared_ptr> model_; + CertifiedContinuousCollisionChecker checker_; + BezierCurve trajectory_; + PiecewiseBezierPath path_; + CertificationResult result_; +}; + +TEST_F(CertificateFixture, VerifiesOnACertifiedRun) { + ASSERT_EQ(result_.verdict, Verdict::kCertifiedFree); + ASSERT_TRUE(result_.certificate.has_value()); + EXPECT_FALSE(result_.certificate->records.empty()); + EXPECT_TRUE(VerifyCertificate(checker_, path_, *result_.certificate)); +} + +TEST_F(CertificateFixture, RejectsShrunkClearance) { + Certificate certificate = *result_.certificate; + ASSERT_FALSE(certificate.records.empty()); + certificate.records[0].phi_hat = certificate.records[0].threshold; + EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); +} + +TEST_F(CertificateFixture, RejectsInflatedClearance) { + Certificate certificate = *result_.certificate; + ASSERT_FALSE(certificate.records.empty()); + certificate.records[0].phi_hat += 1.0; + EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); +} + +TEST_F(CertificateFixture, RejectsWidenedInterval) { + Certificate certificate = *result_.certificate; + const int index = MovingRecordIndex(); + ASSERT_GE(index, 0); + CertificateRecord& record = certificate.records[index]; + record.s_end = std::min(1.0, record.s_end + (record.s_end - record.s_start)); + EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); +} + +TEST_F(CertificateFixture, RejectsTamperedRepresentativeConfiguration) { + Certificate certificate = *result_.certificate; + const int index = MovingRecordIndex(); + ASSERT_GE(index, 0); + certificate.records[index].qc[0] += 0.1; + EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); +} + +TEST_F(CertificateFixture, RejectsDroppedCoverage) { + Certificate certificate = *result_.certificate; + const int index = MovingRecordIndex(); + ASSERT_GE(index, 0); + certificate.records.erase(certificate.records.begin() + index); + EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); +} + +TEST_F(CertificateFixture, RejectsLoweredThreshold) { + Certificate certificate = *result_.certificate; + ASSERT_GE(certificate.records.size(), 2u); + certificate.records[0].threshold -= 0.005; + EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); +} + +TEST_F(CertificateFixture, RejectsUniformlyLoweredThresholds) { + // Self-consistency is not enough: a certificate whose records *all* agree on + // a threshold nobody asked for proves a claim nobody asked for. + Certificate certificate = *result_.certificate; + ASSERT_FALSE(certificate.records.empty()); + for (CertificateRecord& record : certificate.records) { + record.threshold = -1e9; + } + EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); +} + +GTEST_TEST(CertifierTest, CertificateRejectsRebasedStaticRecord) { + // A static record must be measured at the path's own start configuration: + // "static" is relative to the constant-coordinate carve-out, so a record + // re-based onto an off-path configuration would measure a different pair + // pose entirely. + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.3, -0.2, 0.0), MakeQ(0.3, -0.2, 0.15), 2, 0.0, 1.0); + Options options = SerialOptions(); + options.emit_certificate = true; + const CertificationResult result = + checker.CheckTrajectory(trajectory, options); + ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); + ASSERT_TRUE(result.certificate.has_value()); + + const PiecewiseBezierPath path = checker.Normalize(trajectory); + const MotionBoundTable table = checker.ComputeMotionBounds(path); + EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); + + // Both directions: rotating θ1 toward the obstacles reduces the clearance + // the replay measures, while rotating away *increases* it — the case only + // the "static records are pinned to q(t0)" check can catch. + for (const double delta : {1.5, -1.5}) { + Certificate certificate = *result.certificate; + int tampered = 0; + for (CertificateRecord& record : certificate.records) { + if (table.pair_is_static(record.pair_index)) { + // θ1 is a coordinate this path holds constant, hence one the carve-out + // removed from J(p), but one that certainly moves the pair. + record.qc[0] += delta; + ++tampered; + break; + } + } + ASSERT_EQ(tampered, 1); + EXPECT_FALSE(VerifyCertificate(checker, path, certificate)) + << "delta = " << delta; + } +} + +// --------------------------------------------------------------------------- +// 7. Search modes, finding caps and the node budget. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, CertifyAllReportsEveryViolation) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + // Sweeping θ1 from 0 to 3 rad passes the post (≈1.57 rad) and then the + // pillar (≈2.58 rad): two disjoint violating regions, different pairs. + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(3.0, 0.0, 0.0), 1, 0.0, 1.0); + + const CertificationResult result = checker.CheckTrajectory(trajectory); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_GE(result.findings.size(), 2u); + for (size_t i = 1; i < result.findings.size(); ++i) { + EXPECT_LE(result.findings[i - 1].time, result.findings[i].time) + << "findings must be earliest-first"; + } + int definite = 0; + for (const Finding& finding : result.findings) { + if (finding.definite) { + ++definite; + EXPECT_LT(DistanceAtFinding(checker, finding), kMargin); + } + } + EXPECT_GE(definite, 2); + + Options capped = SerialOptions(); + capped.max_reported_findings = 1; + const CertificationResult capped_result = + checker.CheckTrajectory(trajectory, capped); + EXPECT_EQ(capped_result.verdict, Verdict::kViolationFound); + EXPECT_EQ(capped_result.findings.size(), 1u); + EXPECT_NEAR(capped_result.findings.front().time, result.findings.front().time, + 1e-12); +} + +GTEST_TEST(CertifierTest, NodeBudgetExhausted) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + Eigen::MatrixXd waypoints(3, 4); + waypoints.col(0) = MakeQ(0.0, 0.0, 0.0); + waypoints.col(1) = MakeQ(0.3, -0.1, 0.03); + waypoints.col(2) = MakeQ(0.6, -0.3, 0.07); + waypoints.col(3) = MakeQ(0.8, -0.4, 0.10); + + Options options = SerialOptions(); + options.max_nodes = 1; + const CertificationResult result = checker.CheckPath(waypoints, options); + EXPECT_EQ(result.verdict, Verdict::kBudgetExhausted); + ASSERT_FALSE(result.findings.empty()); + // The remainder is reported as a non-definite finding at the earliest + // uncovered time. + EXPECT_FALSE(result.findings.front().definite); + EXPECT_GE(result.findings.front().time, 0.0); +} + +// --------------------------------------------------------------------------- +// 8. Parallel smoke: same verdict and same witness as serial. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, ParallelMatchesSerialOnFreeTrajectory) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); + + const CertificationResult serial = checker.CheckTrajectory(trajectory); + Options parallel_options = SerialOptions(); + parallel_options.parallelism = Parallelism(4); + const CertificationResult parallel = + checker.CheckTrajectory(trajectory, parallel_options); + + EXPECT_EQ(serial.verdict, parallel.verdict); + EXPECT_EQ(parallel.verdict, Verdict::kCertifiedFree); + EXPECT_TRUE(parallel.findings.empty()); + // The same tree is explored either way; only the order differs. + EXPECT_EQ(serial.stats.nodes, parallel.stats.nodes); + EXPECT_EQ(serial.stats.narrowphase_queries, + parallel.stats.narrowphase_queries); +} + +GTEST_TEST(CertifierTest, ParallelMatchesSerialOnViolation) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); + + Options serial_options = SerialOptions(); + serial_options.mode = SearchMode::kFindFirstViolation; + Options parallel_options = serial_options; + parallel_options.parallelism = Parallelism(4); + + const CertificationResult serial = + checker.CheckTrajectory(trajectory, serial_options); + const CertificationResult parallel = + checker.CheckTrajectory(trajectory, parallel_options); + + ASSERT_EQ(serial.verdict, Verdict::kViolationFound); + ASSERT_EQ(parallel.verdict, Verdict::kViolationFound); + ASSERT_EQ(serial.findings.size(), 1u); + ASSERT_EQ(parallel.findings.size(), 1u); + // The earliest witness is deterministic across thread counts (the stats are + // not). + EXPECT_NEAR(serial.findings.front().time, parallel.findings.front().time, + 1e-12); + EXPECT_LT((serial.findings.front().q - parallel.findings.front().q) + .cwiseAbs() + .maxCoeff(), + 1e-12); +} + +GTEST_TEST(CertifierTest, ConcurrentChecksAreIndependent) { + // The Check* methods are const and documented thread-safe: concurrent calls + // must lease disjoint contexts from the pool (the full T8 sweep is a later + // milestone; this is the smoke test for the lease). + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve free_trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); + const BezierCurve bad_trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); + + Options options = SerialOptions(); + options.parallelism = Parallelism(2); + std::vector verdicts(8); + std::vector threads; + for (int i = 0; i < 8; ++i) { + threads.emplace_back([&, i]() { + verdicts[i] = + (i % 2 == 0) + ? checker.CheckTrajectory(free_trajectory, options).verdict + : checker.CheckTrajectory(bad_trajectory, options).verdict; + }); + } + for (std::thread& thread : threads) thread.join(); + for (int i = 0; i < 8; ++i) { + EXPECT_EQ(verdicts[i], (i % 2 == 0) ? Verdict::kCertifiedFree + : Verdict::kViolationFound); + } +} + +// --------------------------------------------------------------------------- +// 9. Breakpoint semantics. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, ViolationExactlyAtStartTime) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + // q(t0) puts the arm straight into the post. + const BezierCurve trajectory = + MakeBezier(MakeQ(1.5708, 0.0, 0.0), MakeQ(0.5, 0.0, 0.0), 1, 0.0, 1.0); + + const CertificationResult result = checker.CheckTrajectory(trajectory); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_FALSE(result.findings.empty()); + const Finding& finding = result.findings.front(); + // Only the breakpoint pre-pass can produce a witness *exactly* at t0; node + // midpoints are strictly interior. + EXPECT_EQ(finding.time, 0.0); + EXPECT_TRUE(finding.definite); + EXPECT_EQ(finding.motion_bound, 0.0); + EXPECT_LT(DistanceAtFinding(checker, finding), kMargin); +} + +GTEST_TEST(CertifierTest, ViolationAtAJunctionIsReported) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + // A 3-waypoint path whose middle waypoint — the junction between segments, + // at t = 1 — is inside the post. + 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 CertificationResult result = checker.CheckPath(waypoints); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + bool found_junction_witness = false; + for (const Finding& finding : result.findings) { + if (finding.time == 1.0 && finding.definite && + finding.motion_bound == 0.0) { + found_junction_witness = true; + EXPECT_LT(DistanceAtFinding(checker, finding), kMargin); + } + } + EXPECT_TRUE(found_junction_witness) + << "the breakpoint pre-pass must report the junction configuration " + "itself, not only interior node midpoints"; +} + +// --------------------------------------------------------------------------- +// 9b. A small seeded soundness sweep. The full T4 corpus (random worlds, +// B-splines, 10^5 samples, hundreds of cases) is a separate milestone; +// this is the cheap standing guard that no kCertifiedFree of *this* driver +// survives dense sampling, and that every definite witness really violates. +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, RandomTrajectoriesAreSoundAgainstDenseSampling) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(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 CertificationResult result = checker.CheckTrajectory(trajectory); + const PiecewiseBezierPath path = checker.Normalize(trajectory); + + if (result.verdict == Verdict::kCertifiedFree) { + ++certified; + const SampledClearance sampled = + SampleClearance(checker, path, 2000, kMargin); + EXPECT_GT(sampled.min_clearance, kMargin) + << "trial " << trial << " was certified but dense sampling found a " + << "configuration at clearance " << sampled.min_clearance; + } + for (const Finding& finding : result.findings) { + if (!finding.definite) continue; + ++violating; + // The witness must be exactly on the path and must really violate. + EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), + 1e-9) + << "trial " << trial; + EXPECT_LT(DistanceAtFinding(checker, finding), kMargin) + << "trial " << trial; + } + } + // The corpus must actually exercise both outcomes. + EXPECT_GT(certified, 0); + EXPECT_GT(violating, 0); +} + +// --------------------------------------------------------------------------- +// 10. API guardrails (the full T9 suite lives in api_test). +// --------------------------------------------------------------------------- + +GTEST_TEST(CertifierTest, ApiThrowsOnDimensionMismatch) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + + Eigen::MatrixXd wrong_rows(2, 3); + wrong_rows.setZero(); + EXPECT_THROW(checker.CheckPath(wrong_rows), std::exception); + + EXPECT_THROW(checker.CheckEdge(VectorXd::Zero(2), VectorXd::Zero(3)), + std::exception); + + Eigen::MatrixXd control_points(5, 2); + control_points.setZero(); + const BezierCurve wrong_trajectory(0.0, 1.0, control_points); + EXPECT_THROW(checker.CheckTrajectory(wrong_trajectory), std::exception); + + // A single waypoint is not a path. + Eigen::MatrixXd single(3, 1); + single.setZero(); + EXPECT_THROW(checker.CheckPath(single), std::exception); +} + +GTEST_TEST(CertifierTest, ApiThrowsOnBadOptions) { + const auto model = MakeArmWorld(); + const auto checker = MakeChecker(model, SerialOptions()); + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.1, 0.0, 0.0), 1, 0.0, 1.0); + + Options bad = SerialOptions(); + bad.min_interval = 0.0; + EXPECT_THROW(checker.CheckTrajectory(trajectory, bad), std::exception); + + bad = SerialOptions(); + bad.max_reported_findings = 0; + EXPECT_THROW(checker.CheckTrajectory(trajectory, bad), std::exception); + + bad = SerialOptions(); + bad.query_tolerance = -1.0; + EXPECT_THROW(checker.CheckTrajectory(trajectory, bad), std::exception); +} + +GTEST_TEST(CertifierTest, ConstructorRejectsNullModel) { + CertifiedContinuousCollisionChecker::Params params; + EXPECT_THROW(CertifiedContinuousCollisionChecker{params}, std::exception); +} + +} // namespace +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/concurrency_test.cc b/planning/certified_ccd/test/concurrency_test.cc new file mode 100644 index 000000000000..aa616a717784 --- /dev/null +++ b/planning/certified_ccd/test/concurrency_test.cc @@ -0,0 +1,860 @@ +/// @file +/// T8 — concurrency determinism (test plan T8; performance requirement +/// P7; parallelism and determinism). +/// +/// Four claims are pinned here. The first three run on a fixed corpus of ten +/// T4-style random cases (a mix of free and violating); the fourth builds one +/// deliberately deep workload out of that corpus: +/// +/// 1. The *answer* does not depend on the thread count. Verdict and earliest +/// witness are identical at Parallelism {1, 2, 8, 16} in both search +/// modes, and in kCertifyAll so are `nodes` and `narrowphase_queries` — +/// the parallel driver explores the same tree, only in a different order. +/// (In kFindFirstViolation the branch-and-bound bound arrives at different +/// times, so the *statistics* are explicitly not deterministic; the +/// reported witness still is.) +/// 2. Serial mode is bit-deterministic: two runs produce byte-identical +/// findings and statistics. +/// 3. The public Check* methods are safe to call concurrently on one checker +/// instance: eight threads hammering one checker get the same answers as +/// running the same calls one after another. +/// 4. Per-call parallelism actually distributes work, and never costs +/// anything when there is not enough of it to distribute: a deep tree +/// inside one segment gets measurably faster with threads, and a check +/// too small to pay for workers is no slower at Parallelism::Max() than +/// serially. These are the two regressions the driver rework of +/// certifier.cc fixed, as the benchmark suite's thread-scaling +/// results measured; the deep +/// workload is also where the sharing path gets its TSan coverage, since +/// the corpus cases of claims 1-3 are far too small to hire a helper. +/// +/// TSan. This file is the test to run under ThreadSanitizer. Drake's +/// build carries a `tsan` config, so the invocation is: +/// +/// bazel test --config=tsan //planning/certified_ccd: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 with +/// "unexpected memory mapping" before main ever runs; running the test +/// binary under `setarch $(uname -m) -R` (or lowering vm.mmap_rnd_bits to +/// 28) is the standard workaround. +/// +/// Result on Drake ~v1.45 at the time of writing: clean — no data races +/// reported over repeated runs, so no suppression file is shipped. That was +/// measured against a prebuilt (uninstrumented) Drake, so TSan saw only +/// certified_ccd frames. It sees all of the +/// driver's shared mutable state, though — the work queue, the findings sink, +/// the atomic node counter and bound, and the context pool are all ours — which +/// is exactly the surface the design claims is the only one there is. If a +/// future pin +/// does produce reports rooted entirely in Drake, triage them and park +/// them in a suppression file (TSAN_OPTIONS=suppressions=...); anything +/// rooted in a +/// certified_ccd frame is a real bug. + +#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/geometry/shape_specification.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/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::Parallelism; +using drake::geometry::Box; +using drake::geometry::Capsule; +using drake::geometry::Cylinder; +using drake::geometry::HalfSpace; +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 Eigen::Vector3d; +using Eigen::VectorXd; + +constexpr double kMargin = 0.005; +/// Ten cases keeps the full 4-thread-count × 2-mode sweep (80 certification +/// runs) plus the concurrent-call test under a second in Release, which is what +/// makes this affordable to run again under TSan (~100× slower). +constexpr int kNumCases = 10; +constexpr int kMinFreeCases = 3; +constexpr int kMinViolatingCases = 3; + +CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +SpatialInertia Inertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +/// A four-link chain of revolute and prismatic joints with primitive geometry, +/// four anchored obstacles and (on odd seeds) a HalfSpace floor, so the corpus +/// exercises the native narrowphase route and the analytic one. +std::unique_ptr> MakeWorld(uint64_t seed) { + std::mt19937_64 rng(seed); + const auto uniform = [&rng](double lo, double hi) { + return std::uniform_real_distribution(lo, hi)(rng); + }; + // Every helper below sequences its draws through named locals: the order in + // which a compiler evaluates sibling constructor or operator arguments is + // unspecified, so drawing inline would make the corpus toolchain-dependent + // and could silently shift the free/violating balance this file relies on. + const auto vector3 = [&uniform](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); + }; + const auto direction = [&vector3]() { + Vector3d v; + do { + v = vector3(-1, 1); + } while (v.norm() < 1e-3 || v.norm() > 1.0); + return v.normalized(); + }; + const auto offset = [&direction, &uniform](double lo, double hi) { + const Vector3d unit = direction(); + const double length = uniform(lo, hi); + return Vector3d(unit * length); + }; + const auto pose = [&vector3, &offset](double lo, double hi) { + const Vector3d rpy = vector3(-3, 3); + const Vector3d p = offset(lo, hi); + return RigidTransformd(RollPitchYawd(rpy), p); + }; + + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + std::vector*> links; + for (int i = 0; i < 4; ++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 = vector3(-0.5, 0.5); + const RigidTransformd X_PF(RollPitchYawd(rpy_PF), offset(0.22, 0.32)); + const Vector3d axis = direction(); + if (i == 2) { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, + body, RigidTransformd(), axis); + } else { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, body, + RigidTransformd(), axis); + } + const RigidTransformd X_LG(offset(0.10, 0.16)); + if (i % 2 == 0) { + const double radius = uniform(0.02, 0.04); + const double length = uniform(0.05, 0.10); + plant.RegisterCollisionGeometry(body, X_LG, Capsule(radius, length), + name + "_geom", Friction()); + } else { + const Vector3d size = vector3(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 < 4; ++i) { + const std::string name = "obstacle" + std::to_string(i); + const RigidBody& body = plant.AddRigidBody(name, Inertia()); + plant.WeldFrames(plant.world_frame(), body.body_frame(), pose(0.30, 0.75)); + if (i % 3 == 0) { + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Sphere(uniform(0.05, 0.12)), + name + "_geom", Friction()); + } else if (i % 3 == 1) { + const Vector3d size = vector3(0.08, 0.20); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Box(size.x(), size.y(), size.z()), + name + "_geom", Friction()); + } else { + const double radius = uniform(0.04, 0.09); + const double length = uniform(0.08, 0.18); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Cylinder(radius, length), name + "_geom", + Friction()); + } + } + if (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(); +} + +/// A quintic Bézier with random control points, so the corpus has real curved +/// trajectories rather than straight edges. +Eigen::MatrixXd MakeControlPoints(uint64_t seed, int num_positions) { + std::mt19937_64 rng(seed ^ 0xa5a5'5a5a'0f0f'f0f0ull); + std::uniform_real_distribution value(-1.4, 1.4); + Eigen::MatrixXd points(num_positions, 6); + for (int j = 0; j < 6; ++j) { + for (int i = 0; i < num_positions; ++i) points(i, j) = value(rng); + } + return points; +} + +Options BaseOptions(Parallelism parallelism, SearchMode mode) { + Options options; + options.margin = kMargin; + options.parallelism = parallelism; + options.mode = mode; + // Bounded cost per run: the whole sweep is executed 8 times per case. + options.min_interval = 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 deliberately 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.) +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 = MakeWorld(seed); + CertifiedContinuousCollisionChecker::Params params; + params.model = entry->model; + params.default_options = + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); + entry->checker = + std::make_unique(params); + entry->control_points = + MakeControlPoints(seed, entry->model->plant().num_positions()); + const CertificationResult result = entry->checker->CheckTrajectory( + entry->trajectory(), + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); + entry->serial_verdict = result.verdict; + // Keep the corpus balanced: stop taking more of whichever kind is + // already well represented. + const bool is_free = result.verdict == Verdict::kCertifiedFree; + const bool is_violating = result.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 findings. Nothing here is a tolerance: two runs +/// of the same deterministic computation either agree exactly or the claim of +/// determinism is false. +::testing::AssertionResult FindingsIdentical(const std::vector& a, + const std::vector& b) { + if (a.size() != b.size()) { + return ::testing::AssertionFailure() + << "finding counts differ: " << a.size() << " vs " << b.size(); + } + for (std::size_t i = 0; i < a.size(); ++i) { + if (a[i].time != b[i].time) { + return ::testing::AssertionFailure() + << "finding " << i << " time " << a[i].time << " vs " << b[i].time; + } + if (a[i].q.size() != b[i].q.size() || + !(a[i].q.array() == b[i].q.array()).all()) { + return ::testing::AssertionFailure() + << "finding " << i << " witness configuration differs"; + } + if (a[i].pair.a != b[i].pair.a || a[i].pair.b != b[i].pair.b) { + return ::testing::AssertionFailure() + << "finding " << i << " pair differs"; + } + if (a[i].distance != b[i].distance || + a[i].motion_bound != b[i].motion_bound || + a[i].definite != b[i].definite) { + return ::testing::AssertionFailure() + << "finding " << i << " payload differs"; + } + if (a[i].nearest_a_W.has_value() != b[i].nearest_a_W.has_value() || + (a[i].nearest_a_W.has_value() && + *a[i].nearest_a_W != *b[i].nearest_a_W)) { + return ::testing::AssertionFailure() + << "finding " << i << " witness point A differs"; + } + if (a[i].nearest_b_W.has_value() != b[i].nearest_b_W.has_value() || + (a[i].nearest_b_W.has_value() && + *a[i].nearest_b_W != *b[i].nearest_b_W)) { + return ::testing::AssertionFailure() + << "finding " << i << " witness point B differs"; + } + } + return ::testing::AssertionSuccess(); +} + +::testing::AssertionResult EarliestWitnessIdentical( + const CertificationResult& a, const CertificationResult& b) { + if (a.findings.empty() != b.findings.empty()) { + return ::testing::AssertionFailure() + << "one run reported findings and the other did not"; + } + if (a.findings.empty()) return ::testing::AssertionSuccess(); + return FindingsIdentical({a.findings.front()}, {b.findings.front()}); +} + +// --------------------------------------------------------------------------- +// 1. The answer does not depend on the thread count. +// --------------------------------------------------------------------------- + +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, VerdictAndEarliestWitnessAreThreadCountInvariant) { + for (const SearchMode mode : + {SearchMode::kCertifyAll, SearchMode::kFindFirstViolation}) { + for (const auto& entry : Corpus()) { + const BezierCurve trajectory = entry->trajectory(); + const CertificationResult serial = entry->checker->CheckTrajectory( + trajectory, BaseOptions(Parallelism::None(), mode)); + for (const int threads : {2, 8, 16}) { + SCOPED_TRACE(entry->name + ", mode " + + (mode == SearchMode::kCertifyAll ? "kCertifyAll" + : "kFindFirstViolation") + + ", threads " + std::to_string(threads)); + const CertificationResult parallel = entry->checker->CheckTrajectory( + trajectory, BaseOptions(Parallelism(threads), mode)); + EXPECT_EQ(serial.verdict, parallel.verdict); + EXPECT_TRUE(EarliestWitnessIdentical(serial, parallel)); + } + } + } +} + +GTEST_TEST(ConcurrencyTest, CertifyAllIsFullyThreadCountInvariant) { + // In kCertifyAll every node's decision depends only on its own control points + // and inherited active set, so the *whole* tree — and therefore every + // statistic and every finding — is thread-count independent, not just the + // earliest witness. + for (const auto& entry : Corpus()) { + const BezierCurve trajectory = entry->trajectory(); + const CertificationResult serial = entry->checker->CheckTrajectory( + trajectory, BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); + for (const int threads : {2, 8, 16}) { + SCOPED_TRACE(entry->name + ", threads " + std::to_string(threads)); + const CertificationResult parallel = entry->checker->CheckTrajectory( + trajectory, + BaseOptions(Parallelism(threads), SearchMode::kCertifyAll)); + EXPECT_EQ(serial.stats.nodes, parallel.stats.nodes); + EXPECT_EQ(serial.stats.narrowphase_queries, + parallel.stats.narrowphase_queries); + EXPECT_EQ(serial.stats.sphere_certifications, + parallel.stats.sphere_certifications); + EXPECT_EQ(serial.stats.max_depth, parallel.stats.max_depth); + EXPECT_TRUE(FindingsIdentical(serial.findings, parallel.findings)); + } + } +} + +GTEST_TEST(ConcurrencyTest, FindFirstViolationStatisticsAreAllowedToDiffer) { + // The complement of the test above, pinned so that a future reader does not + // "fix" a statistics mismatch that the design explicitly permits: under + // branch-and-bound the number of nodes a run visits depends on when the + // atomic bound tightens, which depends on timing. Only the answer is + // deterministic. (The assertion is therefore on the *witness*, and the + // statistics are merely reported.) + int cases_with_differing_stats = 0; + int examined = 0; + for (const auto& entry : Corpus()) { + if (entry->serial_verdict != Verdict::kViolationFound) continue; + ++examined; + const BezierCurve trajectory = entry->trajectory(); + const CertificationResult serial = entry->checker->CheckTrajectory( + trajectory, + BaseOptions(Parallelism::None(), SearchMode::kFindFirstViolation)); + const CertificationResult parallel = entry->checker->CheckTrajectory( + trajectory, + BaseOptions(Parallelism(16), SearchMode::kFindFirstViolation)); + ASSERT_EQ(serial.verdict, parallel.verdict); + ASSERT_EQ(serial.findings.size(), 1u); + ASSERT_EQ(parallel.findings.size(), 1u); + EXPECT_TRUE(EarliestWitnessIdentical(serial, parallel)); + if (serial.stats.nodes != parallel.stats.nodes) { + ++cases_with_differing_stats; + } + } + // Without this the `continue` above could silently empty the test. + EXPECT_GE(examined, kMinViolatingCases); + std::cout << "\n[ T8 ] kFindFirstViolation: node counts differed between 1 " + "and 16 threads on " + << cases_with_differing_stats << " of the " << examined + << " violating cases; the reported witness was identical on all of " + "them.\n\n"; +} + +// --------------------------------------------------------------------------- +// 2. Serial mode is bit-deterministic (the performance requirements, P7). +// --------------------------------------------------------------------------- + +GTEST_TEST(ConcurrencyTest, SerialModeIsBitDeterministic) { + for (const auto& entry : Corpus()) { + for (const SearchMode mode : + {SearchMode::kCertifyAll, SearchMode::kFindFirstViolation}) { + SCOPED_TRACE(entry->name); + const Options options = BaseOptions(Parallelism::None(), mode); + const BezierCurve trajectory = entry->trajectory(); + const CertificationResult first = + entry->checker->CheckTrajectory(trajectory, options); + const CertificationResult second = + entry->checker->CheckTrajectory(trajectory, options); + EXPECT_EQ(first.verdict, second.verdict); + EXPECT_TRUE(FindingsIdentical(first.findings, second.findings)); + EXPECT_EQ(first.stats.nodes, second.stats.nodes); + EXPECT_EQ(first.stats.narrowphase_queries, + second.stats.narrowphase_queries); + EXPECT_EQ(first.stats.sphere_certifications, + second.stats.sphere_certifications); + EXPECT_EQ(first.stats.max_depth, second.stats.max_depth); + } + } +} + +// --------------------------------------------------------------------------- +// 3. Concurrent Check* calls on one checker instance. +// --------------------------------------------------------------------------- + +GTEST_TEST(ConcurrencyTest, ConcurrentCallsOnOneCheckerMatchSequential) { + // Every worker hits the *same* checker object, 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), SearchMode::kCertifyAll); + + std::vector sequential; + for (const auto& entry : corpus) { + sequential.push_back( + entry->checker->CheckTrajectory(entry->trajectory(), options)); + } + + constexpr int kThreads = 8; + constexpr int kRepeats = 3; + std::vector> concurrent(kThreads * kRepeats); + std::vector threads; + for (int t = 0; t < kThreads; ++t) { + threads.emplace_back([&, t]() { + for (int r = 0; r < kRepeats; ++r) { + std::vector& slot = concurrent[t * kRepeats + r]; + for (const auto& entry : corpus) { + slot.push_back( + entry->checker->CheckTrajectory(entry->trajectory(), options)); + } + } + }); + } + for (std::thread& thread : threads) thread.join(); + + for (int i = 0; i < kThreads * kRepeats; ++i) { + ASSERT_EQ(concurrent[i].size(), sequential.size()); + for (std::size_t k = 0; k < sequential.size(); ++k) { + SCOPED_TRACE("worker " + std::to_string(i) + ", case " + corpus[k]->name); + EXPECT_EQ(concurrent[i][k].verdict, sequential[k].verdict); + EXPECT_TRUE( + FindingsIdentical(concurrent[i][k].findings, sequential[k].findings)); + EXPECT_EQ(concurrent[i][k].stats.nodes, sequential[k].stats.nodes); + EXPECT_EQ(concurrent[i][k].stats.narrowphase_queries, + sequential[k].stats.narrowphase_queries); + } + } +} + +GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { + // The same, through the other two public entry points and the const + // introspection seams, so that a mutable-state regression in any of them + // shows up here rather than in a user's planner. + const Case& entry = *Corpus().front(); + const Options options = BaseOptions(Parallelism(2), SearchMode::kCertifyAll); + const int n = entry.model->plant().num_positions(); + const VectorXd q1 = entry.control_points.col(0); + const VectorXd q2 = entry.control_points.rightCols(1); + Eigen::MatrixXd waypoints(n, 3); + waypoints.col(0) = q1; + waypoints.col(1) = 0.5 * (q1 + q2); + waypoints.col(2) = q2; + + const CertificationResult edge_expected = + entry.checker->CheckEdge(q1, q2, options); + const CertificationResult path_expected = + entry.checker->CheckPath(waypoints, options); + const MotionBoundTable table_expected = entry.checker->ComputeMotionBounds( + entry.checker->Normalize(entry.trajectory(), options)); + // Snapshot every λ entry, not just the CSR's size: the row layout is fixed by + // topology and would survive any amount of corruption in the coefficients. + std::vector>> lambda_expected; + for (int p = 0; p < table_expected.num_pairs(); ++p) { + lambda_expected.push_back(table_expected.entries(p)); + } + + const auto same_result = [](const CertificationResult& a, + const CertificationResult& b) { + return a.verdict == b.verdict && a.stats.nodes == b.stats.nodes && + a.stats.narrowphase_queries == b.stats.narrowphase_queries && + a.stats.sphere_certifications == b.stats.sphere_certifications && + FindingsIdentical(a.findings, b.findings); + }; + + 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 does the asserting + // 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 < 4; ++r) { + if (!same_result(entry.checker->CheckEdge(q1, q2, options), + edge_expected)) { + ++mismatches[t]; + } + if (!same_result(entry.checker->CheckPath(waypoints, options), + path_expected)) { + ++mismatches[t]; + } + const MotionBoundTable table = entry.checker->ComputeMotionBounds( + entry.checker->Normalize(entry.trajectory(), options)); + if (table.num_pairs() != table_expected.num_pairs()) { + ++mismatches[t]; + continue; + } + for (int p = 0; p < table.num_pairs(); ++p) { + if (table.entries(p) != lambda_expected[p]) ++mismatches[t]; + } + } + }); + } + for (std::thread& thread : threads) thread.join(); + for (int t = 0; t < kThreads; ++t) EXPECT_EQ(mismatches[t], 0); +} + +// --------------------------------------------------------------------------- +// 4. Per-call parallel scaling. +// --------------------------------------------------------------------------- +// +// These pin the two properties the driver rework of certifier.cc exists +// for, and that the benchmark suite's thread-scaling results measured +// the old driver failing: +// +// a) a deep tree inside a single segment actually spreads over the workers +// (the old depth-seeded driver got 0.98× at 16 threads on 12 570 nodes, +// because one fixed seed held essentially the whole tree); +// b) a check too small to pay for workers never loses by being asked for +// them — which matters because Parallelism::Max() is the *default* value +// of Options::parallelism. +// +// Both are timing claims, so both are written to survive a loaded machine: a +// ratio with a wide margin, best-of-three, and a skip when the hardware or the +// build cannot support the claim at all. They are not benchmarks — the numbers +// live in benchmark/results/ — they are regression detectors, and they should +// only ever fire on a driver that has stopped distributing work. + +/// True when the build cannot support a meaningful wall-clock claim: a +/// sanitizer build serializes and inflates everything, an unoptimized build +/// changes the ratios, and fewer than eight hardware threads means there is no +/// parallelism to measure. +bool TimingClaimsAreMeaningless() { +#if defined(__SANITIZE_THREAD__) || defined(__SANITIZE_ADDRESS__) + return true; +#elif defined(__has_feature) +#if __has_feature(thread_sanitizer) || __has_feature(address_sanitizer) + return true; +#endif +#endif +#ifndef NDEBUG + return true; +#else + return std::thread::hardware_concurrency() < 8; +#endif +} + +template +double BestOfThreeSeconds(F&& body) { + body(); // Warm up: first-touch page faults, the worker pool's threads. + double best = std::numeric_limits::infinity(); + for (int i = 0; i < 3; ++i) { + const auto start = std::chrono::steady_clock::now(); + body(); + best = std::min(best, std::chrono::duration( + std::chrono::steady_clock::now() - start) + .count()); + } + return best; +} + +/// The bisection's node budget below doubles as the deep workload's size: the +/// margin it converges to is the largest one still certifiable inside this +/// budget, so the tree it produces has just under this many nodes. Large +/// enough that a run takes tens of milliseconds (a wall-clock ratio then means +/// something) and that no fixed seeding depth could ever have covered it; +/// small enough that the ~40 probes that find it, and the timed repetitions +/// that use it, stay cheap — under a sanitizer too. +constexpr uint64_t kProbeBudget = 6000; +constexpr uint64_t kMinDeepNodes = 3000; + +/// A corpus case run at a margin just below its own swept clearance, which is +/// what makes the subdivision tree deep and *narrow* (the soundness argument): +/// certifying a node needs φ̂ − τ − Δ > m, so as the threshold m 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 — exactly the shape a depth-seeded work +/// queue cannot split, and the shape the benchmark suite's thread-scaling +/// results measured the old driver getting 0.98× on. +/// +/// That margin is found by bisection rather than hard-coded, so the workload +/// survives any change to the random worlds, the bounds, or Drake: the largest +/// margin still certifiable within kProbeBudget nodes is by construction the +/// one that costs about kProbeBudget nodes. +struct DeepWorkload { + const Case* entry{}; + double margin{0.0}; + double min_interval{1e-8}; + uint64_t nodes{0}; + + Options options(Parallelism parallelism) const { + Options options = BaseOptions(parallelism, SearchMode::kCertifyAll); + options.margin = margin; + options.min_interval = min_interval; + return options; + } +}; + +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; + options.max_nodes = kProbeBudget; + return deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), options) + .verdict == Verdict::kCertifiedFree; + }; + 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->nodes = deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), + deep->options(Parallelism::None())) + .stats.nodes; + return deep; + }(); + return *workload; +} + +GTEST_TEST(ConcurrencyTest, DeepWorkloadIsBigEnoughToBeWorthSpreading) { + // Without this the two tests below could silently degenerate into measuring + // a handful of nodes if the corpus or the bisection ever drifted. + const DeepWorkload& deep = Deep(); + ASSERT_NE(deep.entry, nullptr); + EXPECT_GE(deep.nodes, kMinDeepNodes) << "grazing margin " << deep.margin; + std::cout << "\n[ T8 ] deep workload: " << deep.entry->name << ", margin " + << deep.margin << ", " << deep.nodes << " nodes at min_interval " + << deep.min_interval << "\n\n"; +} + +GTEST_TEST(ConcurrencyTest, DeepWorkloadIsThreadCountInvariant) { + // The scaling test below only proves work moved between threads; this proves + // the *same* work moved. It runs in every build, sanitizers included, and is + // where the sharing path gets its TSan coverage — the corpus cases of the + // tests above are too small to ever hire a helper. + const DeepWorkload& deep = Deep(); + ASSERT_NE(deep.entry, nullptr); + const BezierCurve trajectory = deep.entry->trajectory(); + const CertificationResult serial = deep.entry->checker->CheckTrajectory( + trajectory, deep.options(Parallelism::None())); + for (const int threads : {4, 16}) { + SCOPED_TRACE("threads " + std::to_string(threads)); + const CertificationResult parallel = deep.entry->checker->CheckTrajectory( + trajectory, deep.options(Parallelism(threads))); + EXPECT_EQ(serial.verdict, parallel.verdict); + EXPECT_EQ(serial.stats.nodes, parallel.stats.nodes); + EXPECT_EQ(serial.stats.narrowphase_queries, + parallel.stats.narrowphase_queries); + EXPECT_EQ(serial.stats.sphere_certifications, + parallel.stats.sphere_certifications); + EXPECT_EQ(serial.stats.max_depth, parallel.stats.max_depth); + EXPECT_TRUE(FindingsIdentical(serial.findings, parallel.findings)); + } +} + +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 pool as well as + // its context pool, and the case where a reservation returning fewer threads + // than asked for is the normal outcome rather than an edge case. + const DeepWorkload& deep = Deep(); + ASSERT_NE(deep.entry, nullptr); + const CertificationResult expected = deep.entry->checker->CheckTrajectory( + deep.entry->trajectory(), deep.options(Parallelism::None())); + + constexpr int kThreads = 4; + // gtest assertions are not safe off the main thread, so each worker counts + // its own mismatches 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 < 2; ++r) { + const CertificationResult result = deep.entry->checker->CheckTrajectory( + deep.entry->trajectory(), deep.options(Parallelism(4))); + if (result.verdict != expected.verdict || + result.stats.nodes != expected.stats.nodes || + result.stats.narrowphase_queries != + expected.stats.narrowphase_queries || + result.stats.sphere_certifications != + expected.stats.sphere_certifications || + !FindingsIdentical(result.findings, expected.findings)) { + ++mismatches[t]; + } + } + }); + } + for (std::thread& thread : threads) thread.join(); + for (int t = 0; t < kThreads; ++t) EXPECT_EQ(mismatches[t], 0); +} + +GTEST_TEST(ConcurrencyTest, DeepWorkloadIsFasterInParallel) { + if (TimingClaimsAreMeaningless()) GTEST_SKIP(); + const DeepWorkload& deep = Deep(); + ASSERT_NE(deep.entry, nullptr); + const BezierCurve trajectory = deep.entry->trajectory(); + const Options serial_options = deep.options(Parallelism::None()); + const Options parallel_options = deep.options(Parallelism(8)); + + const double serial = BestOfThreeSeconds([&]() { + deep.entry->checker->CheckTrajectory(trajectory, serial_options); + }); + const double parallel = BestOfThreeSeconds([&]() { + deep.entry->checker->CheckTrajectory(trajectory, parallel_options); + }); + std::cout << "\n[ T8 ] deep workload: serial " << 1e3 * serial + << " ms, Parallelism(8) " << 1e3 * parallel << " ms (" + << serial / parallel << "x)\n\n"; + // Eight threads measure ~6x on the benchmark machine; 1.43x is the bound + // that separates "the driver distributes deep work" from the old driver's + // 0.98x without being a performance assertion in disguise. + EXPECT_LT(parallel, 0.7 * serial); +} + +GTEST_TEST(ConcurrencyTest, SmallCheckIsNotSlowerInParallel) { + if (TimingClaimsAreMeaningless()) GTEST_SKIP(); + // A two-waypoint edge in one of the corpus worlds is the small check: a + // handful of nodes, dominated by the serial breakpoint pass. Asked for the + // default Parallelism::Max(), the driver must decline to hire anyone rather + // than pay a worker-startup bill several times the size of the work. + const Case& entry = *Corpus().front(); + const VectorXd q1 = entry.control_points.col(0); + const VectorXd q2 = entry.control_points.rightCols(1); + const Options serial_options = + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); + const Options parallel_options = + BaseOptions(Parallelism::Max(), SearchMode::kCertifyAll); + ASSERT_LT(entry.checker->CheckEdge(q1, q2, serial_options).stats.nodes, 100u); + + const double serial = BestOfThreeSeconds([&]() { + entry.checker->CheckEdge(q1, q2, serial_options); + }); + const double parallel = BestOfThreeSeconds([&]() { + entry.checker->CheckEdge(q1, q2, parallel_options); + }); + std::cout << "\n[ T8 ] small check: serial " << 1e3 * serial + << " ms, Parallelism::Max() " << 1e3 * parallel << " ms (" + << serial / parallel << "x)\n\n"; + // Parity is what the driver actually delivers (it never hires for a check + // this small, so the two paths run the same code); the 1.5x bound leaves + // room for scheduler noise on a loaded machine without letting a return of + // the old 2.6x slowdown through. + EXPECT_LT(parallel, 1.5 * serial); +} + +} // namespace +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/soundness_fuzz_test.cc b/planning/certified_ccd/test/soundness_fuzz_test.cc new file mode 100644 index 000000000000..58422ead225b --- /dev/null +++ b/planning/certified_ccd/test/soundness_fuzz_test.cc @@ -0,0 +1,1004 @@ +/// @file +/// T4 — end-to-end soundness fuzz (test plan T4; implementation note 2). +/// +/// Random worlds × random trajectories, cross-checked three ways: +/// +/// * a single sampled configuration whose clearance reaches the threshold +/// would refute a `kCertifiedFree` verdict outright, so every certified +/// case is searched for one — hard (10⁴ configurations, 10⁵ on a subset) — +/// and its emitted certificate is independently replayed; +/// * every definite `Finding` is re-evaluated exactly at its witness +/// configuration, from a context this run never touched, and must really +/// violate; +/// * every non-definite `Finding` that claims to be a resolution-floor +/// grazing record must be backed by a clearance that really sits within +/// 10·(τ_p + ε) of the threshold near the reported time. +/// +/// Any cross-check failure here is a soundness bug in the library, never a +/// reason to loosen the test (the implementation notes, item 2). Failure +/// messages carry the complete repro — seed, world recipe, trajectory control +/// points — so a failing case can be reconstructed from the CI log alone. +/// +/// Budget. 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. +/// kNumCases = 200 clears test-plan T4's ≥ 150 (world, trajectory) pairs per CI +/// run by a third and measures ~14 s in Release here — a 10× margin against the +/// ~3 min budget, so the suite still fits on a CI machine an order of magnitude +/// slower. The spare budget is spent on resolution rather than on more +/// shallowly-checked cases: kDenseSamples = 10⁴ resolves any clearance dip +/// wider than ~10⁻⁴ of the domain, and every 10th certified case gets the 10⁵ +/// sweep, which resolves 10× finer at 10× the cost. (Sample counts are per +/// case and approximate: they are split evenly across segments and each segment +/// gets both endpoints, so the true count is total + #segments.) + +#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/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +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; + +constexpr int kNumCases = 200; +constexpr uint64_t kBaseSeed = 0x5eed'0000'0000'0000ull; +constexpr int kDenseSamples = 10000; +constexpr int kDeepDenseSamples = 100000; +/// Every kDeepEvery-th certified case gets the 10⁵-sample sweep. +constexpr int kDeepEvery = 10; +/// Samples used to locate a trajectory's minimum clearance when building a +/// deliberately 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). The checker +/// charges each pair its own τ_p ≥ Options::query_tolerance; the tests below +/// only ever need an upper bound on it, and this is it. +constexpr double kWorstTau = 5e-5; + +// --------------------------------------------------------------------------- +// 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()}; +}; + +std::string Name(ShapeKind kind) { + switch (kind) { + case ShapeKind::kSphere: + return "Sphere"; + case ShapeKind::kBox: + return "Box"; + case ShapeKind::kCapsule: + return "Capsule"; + case ShapeKind::kCylinder: + return "Cylinder"; + case ShapeKind::kEllipsoid: + return "Ellipsoid"; + case ShapeKind::kConvex: + return "ConvexTetra"; + } + return "?"; +} + +/// 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_); } + /// Note the named locals: the order in which a compiler evaluates sibling + /// constructor arguments is unspecified, so drawing three variates inline + /// would make the corpus depend on the toolchain. Every draw in this file is + /// sequenced explicitly for that reason. + 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); + } + /// A uniformly distributed direction (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; + } + } + /// A direction scaled by a length drawn *after* it. + 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. Adjacent links therefore have +/// real clearance in most configurations but can genuinely fold into each +/// other, which is what makes the self-collision half of the corpus nontrivial. +/// (MultibodyPlant::Finalize only filters *welded* subgraphs, so every +/// parent/child pair here is a live, unfiltered pair.) +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. Deliberately re-derived here — six exact one-liners, each +/// obviously correct — 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 is auditing. std::nullopt means "no finite radius available" (HalfSpace) +/// or "not worth deriving here" (Convex / Mesh); such pairs always take 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 CertifiedContinuousCollisionChecker& checker) + : checker_(&checker), + root_(checker.model().CreateDefaultContext()), + plant_context_( + &checker.model().plant().GetMyMutableContextFromRoot(root_.get())) { + const auto& inspector = checker.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 : checker.pairs()) { + slot_a_.push_back(slot(pair.id.a)); + slot_b_.push_back(slot(pair.id.b)); + } + } + + /// Worst (most negative) value of φ_p(q) − threshold over the dense samples, + /// with the time and pair that attained it. + struct Result { + double min_slack{std::numeric_limits::infinity()}; + double worst_time{std::numeric_limits::quiet_NaN()}; + int worst_pair{-1}; + }; + + /// `threshold` is m_p, which this fuzz keeps uniform across pairs because it + /// never sets a PaddingSpec (the case loop asserts that). + Result 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); + Result 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 = checker_->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(checker_->distance_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 checker_->distance_oracle().SignedDistance( + query_object(), checker_->pairs()[pair_index]); + } + + /// Index of the checker's pair matching `id`, or -1. + int FindPair(const PairId& id) const { + const auto& pairs = checker_->pairs(); + for (int p = 0; p < static_cast(pairs.size()); ++p) { + if (pairs[p].id.a == id.a && pairs[p].id.b == id.b) return p; + } + return -1; + } + + private: + void SetPositions(const VectorXd& q) { + checker_->model().plant().SetPositions(plant_context_, q); + } + + const QueryObject& query_object() const { + const auto& scene_graph = checker_->model().scene_graph(); + return scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*root_)); + } + + void Evaluate(const VectorXd& q, double time, double threshold, + Result* 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 = checker_->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 = + checker_->distance_oracle().SignedDistance(query, pairs[p]) - + threshold; + if (slack < result->min_slack) { + result->min_slack = slack; + result->worst_time = time; + result->worst_pair = p; + } + } + } + + const CertifiedContinuousCollisionChecker* checker_{}; + 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 budget{0}; + int deep_scans{0}; + int definite_findings{0}; + int inconclusive_findings{0}; + int graze_cases{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. +Options FuzzOptions(double margin) { + Options options; + options.margin = margin; + options.mode = SearchMode::kCertifyAll; + options.emit_certificate = true; + options.parallelism = Parallelism::None(); + // A coarser resolution floor than the 1e-9 default: a grazing pair still ends + // kInconclusive, but after ~20 bisections rather than ~30, which keeps the + // pathological cases of a 200-case corpus affordable. The node budget is the + // second guard; a case that hits it is counted and skipped, never silently + // accepted. + options.min_interval = 1e-6; + options.max_nodes = 300000; + return options; +} + +CertifiedContinuousCollisionChecker MakeChecker( + std::shared_ptr> model, const Options& options) { + CertifiedContinuousCollisionChecker::Params params; + params.model = std::move(model); + params.default_options = options; + return CertifiedContinuousCollisionChecker(params); +} + +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 test-plan T4's 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 exercise the kInconclusive branch (and its + // cross-check) on random geometry. Without it the corpus would never + // produce a grazing case, because a random trajectory is tangent to a + // random obstacle with probability zero. + double margin = (case_index % 2 == 0) ? 0.0 : 0.01; + bool grazing = (case_index % 5) == 3; + if (grazing) { + const Options probe_options = FuzzOptions(0.0); + const CertifiedContinuousCollisionChecker probe = + MakeChecker(model, probe_options); + DenseScanner probe_scanner(probe); + const DenseScanner::Result probe_scan = probe_scanner.Scan( + probe.Normalize(*trajectory, probe_options), 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 Options options = FuzzOptions(margin); + const CertifiedContinuousCollisionChecker checker = + MakeChecker(model, options); + // This fuzz never sets a PaddingSpec, so m_p = margin for every pair; the + // dense scan relies on that to compare against one number. + for (const PairRecord& pair : checker.pairs()) { + ASSERT_EQ(pair.threshold, margin); + } + + const PiecewiseBezierPath path = checker.Normalize(*trajectory, options); + const CertificationResult result = + checker.CheckTrajectory(*trajectory, options); + + DenseScanner scanner(checker); + + switch (result.verdict) { + case Verdict::kCertifiedFree: { + ++tally.certified; + ASSERT_TRUE(result.findings.empty()); + // (a) 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::Result scan = scanner.Scan( + path, deep ? kDeepDenseSamples : kDenseSamples, margin); + 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; + // (b) The audit trail must replay independently. + ASSERT_TRUE(result.certificate.has_value()); + EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)) + << "CERTIFIED FREE but the emitted certificate does not verify"; + break; + } + case Verdict::kViolationFound: + ++tally.violation; + EXPECT_FALSE(result.findings.empty()); + break; + case Verdict::kInconclusive: + ++tally.inconclusive; + EXPECT_FALSE(result.findings.empty()); + break; + case Verdict::kBudgetExhausted: + // The node budget is a safety valve, not an expected outcome; a run + // that hits it reports the earliest node it left uncovered, and there + // is nothing to cross-check because nothing was proved. The corpus-wide + // bound on how often this may happen is asserted after the loop. + ++tally.budget; + EXPECT_FALSE(result.findings.empty()) + << "budget exhaustion must report the uncovered remainder"; + break; + } + + // Findings are earliest-first, always. + for (std::size_t i = 1; i < result.findings.size(); ++i) { + EXPECT_LE(result.findings[i - 1].time, result.findings[i].time); + } + + for (const Finding& finding : result.findings) { + const int pair_index = scanner.FindPair(finding.pair); + ASSERT_GE(pair_index, 0) << "finding names an unknown pair"; + const double threshold = checker.pairs()[pair_index].threshold; + ASSERT_EQ(finding.q.size(), world.num_positions()); + + if (finding.definite) { + ++tally.definite_findings; + // The witness is exactly on the trajectory ... + EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), + 1e-9) + << "a definite 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, threshold + kWorstTau) + << "definite violation at t = " << finding.time + << " re-measures at phi = " << phi << " against threshold " + << threshold; + EXPECT_NEAR(phi, finding.distance, 1e-9) + << "the reported distance is not reproducible at the witness"; + } else if (result.verdict != Verdict::kBudgetExhausted) { + // Every non-definite finding that is *not* a budget remainder is a + // resolution-floor grazing record, whether the run as a whole ended + // kInconclusive or kViolationFound (in kCertifyAll the sink's + // inconclusive list is appended to the definite one, so a violating run + // can carry grazing records too). All of them get the same audit; only + // the synthesized "here is where the budget stopped us" finding is + // exempt, because its clearance carries no claim. + ++tally.inconclusive_findings; + // Test plan T4: a grazing record must be backed by a clearance that + // sits within 10·(τ_p + ε) of the threshold somewhere near the + // reported time. + const double tolerance = 10.0 * (kWorstTau + options.certificate_slack); + const double window = + 0.01 * std::max(1e-12, path.end_time() - path.start_time()); + const double best = + scanner.MinAbsSlackNear(path, pair_index, threshold, 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, far outside 10*(tau + eps) = " + << tolerance; + } + } + tally.scan_queries += scanner.narrowphase_queries(); + } + + std::cout << "\n[ T4 FUZZ SUMMARY ] cases = " << kNumCases + << " certified = " << tally.certified + << " violation = " << tally.violation + << " inconclusive = " << tally.inconclusive + << " budget = " << tally.budget << "\n" + << " trajectories: PWL = " << tally.pwl + << ", Bezier = " << tally.bezier << ", B-spline = " << tally.bspline + << "; grazing-margin cases = " << tally.graze_cases << "\n" + << " deep (1e5-sample) scans = " + << tally.deep_scans + << " definite findings = " << tally.definite_findings + << " inconclusive findings = " << tally.inconclusive_findings + << "\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. + static_assert( + kNumCases >= 150, + "test plan T4 asks for >= 150 (world, trajectory) cases per CI run"); + EXPECT_GE(tally.certified, 40); + EXPECT_GE(tally.violation, 20); + EXPECT_GE(tally.inconclusive, 5) + << "the grazing-margin cases should have produced kInconclusive verdicts"; + EXPECT_GE(tally.definite_findings, 20); + EXPECT_GE(tally.inconclusive_findings, 5); + // The node budget exists to bound a pathological case, not to be the usual + // answer: if it starts firing often, the corpus has stopped cross-checking + // anything and the numbers above would quietly stop meaning what they say. + EXPECT_LE(tally.budget, kNumCases / 20); + // All three trajectory families of trajectory normalization must be + // represented. + EXPECT_GE(tally.pwl, 20); + EXPECT_GE(tally.bezier, 20); + EXPECT_GE(tally.bspline, 20); + // The dense scan must really be measuring distances, not skipping everything + // through its broadphase. + EXPECT_GT(tally.scan_queries, 100000); + // Every supported geometry class must have appeared somewhere in the corpus, + // including the analytic HalfSpace route: 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 end to end. + 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 certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/test/thin_obstacle_test.cc b/planning/certified_ccd/test/thin_obstacle_test.cc new file mode 100644 index 000000000000..f4d504e3a740 --- /dev/null +++ b/planning/certified_ccd/test/thin_obstacle_test.cc @@ -0,0 +1,468 @@ +/// @file +/// T5 — the reason this library exists (test plan T5; the motivation for +/// the library). +/// +/// Drake's `drake::planning::SceneGraphCollisionChecker` checks an edge by +/// interpolating it at `edge_step_size` increments and running a *discrete* +/// check at each sample. A thin obstacle that sits between two samples is +/// invisible to it, however carefully the planner was written. This file builds +/// exactly that situation, pins Drake's miss, and shows that our continuum +/// certificate catches it — then shows the mirror image: a genuinely free +/// squeeze through a millimetre-scale gap that we certify with a bounded node +/// budget, so the gain is not bought with useless conservatism. +/// +/// Everything here is programmatic and deterministic: no RNG, no model files. + +#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/rigid_transform.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/spatial_inertia.h" +#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/collision_checker_params.h" +#include "drake/planning/robot_diagram.h" +#include "drake/planning/robot_diagram_builder.h" +#include "drake/planning/scene_graph_collision_checker.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace { + +using drake::Parallelism; +using drake::geometry::Box; +using drake::geometry::QueryObject; +using drake::geometry::Sphere; +using drake::math::RigidTransformd; +using drake::multibody::CoulombFriction; +using drake::multibody::MultibodyPlant; +using drake::multibody::PrismaticJoint; +using drake::multibody::RigidBody; +using drake::multibody::SpatialInertia; +using drake::planning::CollisionCheckerParams; +using drake::planning::RobotDiagram; +using drake::planning::RobotDiagramBuilder; +using drake::planning::SceneGraphCollisionChecker; +using drake::trajectories::BezierCurve; +using Eigen::Vector3d; +using Eigen::VectorXd; + +// --------------------------------------------------------------------------- +// 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 rather than a plausible story. +// +// 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 — 21 +// configurations 0.05 m apart in x, at x = -0.50, -0.45, …, 0.00, 0.05, …, +// 0.50. (Reconstructed and measured below rather than trusted.) +// * 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. +// +// A sampled checker at this resolution cannot tell this edge from an empty +// world. A certificate over the continuum can. + +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; + +CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +SpatialInertia Inertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +/// 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()); +} + +/// The thin-plate world: 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 +/// (the "clone of the same model" of test-plan T5): SceneGraphCollisionChecker +/// rewrites collision filters on the model it is handed, which would otherwise +/// perturb the pair table our checker 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)); +} + +CertifiedContinuousCollisionChecker MakeCertifiedChecker( + std::shared_ptr> model) { + CertifiedContinuousCollisionChecker::Params params; + params.model = std::move(model); + params.default_options.margin = 0.0; + params.default_options.parallelism = Parallelism::None(); + return CertifiedContinuousCollisionChecker(params); +} + +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; +} + +/// Signed distance of `finding`'s pair, re-measured from a fresh context at the +/// witness configuration: the independent confirmation that the witness is a +/// real contact and not an artifact of the search. +double DistanceAtFinding(const CertifiedContinuousCollisionChecker& checker, + const Finding& finding) { + const RobotDiagram& model = checker.model(); + 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)); + for (const PairRecord& pair : checker.pairs()) { + if (pair.id.a == finding.pair.a && pair.id.b == finding.pair.b) { + return checker.distance_oracle().SignedDistance(query_object, pair); + } + } + ADD_FAILURE() << "the finding names a pair the checker does not know."; + return std::numeric_limits::quiet_NaN(); +} + +// --------------------------------------------------------------------------- +// 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. + EXPECT_NEAR(drake_checker.ComputeConfigurationDistance(q1, q2), 1.0, 1e-15); + + // The headline: 1 mm of plate between the waypoints, and the sampled check + // calls the edge free. + EXPECT_TRUE(drake_checker.CheckEdgeCollisionFree(q1, q2)) + << "the premise of this test — that default-resolution sampling misses a " + "1 mm plate — no longer holds on this Drake pin"; + + // Show *why*. The model of Drake's behaviour 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. That model is *measured*, not assumed, by sliding the plate across + // one sample period and comparing Drake's verdict against the prediction + // "caught iff the plate's mid-plane is within the contact half-width of some + // multiple of 0.05". Every mismatch would mean the sample grid is not what + // the arithmetic above claims. + 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); + const bool predicted_free = + gap_to_nearest_sample(offset) > kContactHalfWidth; + EXPECT_EQ(probe.CheckEdgeCollisionFree(q1, q2), predicted_free) + << "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 one is 25 mm from + // the plate's mid-plane, i.e. 19.5 mm of clearance: the sampled check is not + // remotely close to seeing it. + 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. Replacing "shrink it and + // hope" with a proof is what this library is for. + const SceneGraphCollisionChecker fine_checker = + MakeDrakeChecker(MakePlateWorld(kPlateX, kPlateThickness), 0.002); + EXPECT_FALSE(fine_checker.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); + std::shared_ptr> model = + MakePlateWorld(kPlateX, kPlateThickness); + const CertifiedContinuousCollisionChecker checker = + MakeCertifiedChecker(model); + + const CertificationResult result = checker.CheckEdge(q1, q2); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_FALSE(result.findings.empty()); + + const Finding& finding = result.findings.front(); + EXPECT_TRUE(finding.definite); + 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(checker, 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); + + std::shared_ptr> model = MakeSlotWorld(kHalfGap); + const CertifiedContinuousCollisionChecker checker = + MakeCertifiedChecker(model); + + const VectorXd q1 = MakeQ(-0.3, 0.0); + const VectorXd q2 = MakeQ(0.3, 0.0); + + // Sampling passes here too — but this time it is *right*, and the point is + // that we agree without having to sample. + const SceneGraphCollisionChecker drake_checker = + MakeDrakeChecker(MakeSlotWorld(kHalfGap), kDrakeEdgeStepSize); + EXPECT_TRUE(drake_checker.CheckEdgeCollisionFree(q1, q2)); + + const CertificationResult result = checker.CheckEdge(q1, q2); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + EXPECT_TRUE(result.findings.empty()); + + // 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. + constexpr std::uint64_t kNodeCeiling = 640; + EXPECT_LT(result.stats.nodes, kNodeCeiling) + << "certifying a 3 mm gap should cost O(log(travel / clearance)) depth, " + "not a blow-up"; + EXPECT_GE(result.stats.max_depth, 6) + << "a 3 mm gap over 0.6 m of travel cannot be certified shallowly; if it " + "could, the motion bound would be unsound"; + EXPECT_LE(result.stats.max_depth, 12); + + // ... and the certificate for this run replays independently. + Options options; + options.margin = 0.0; + options.parallelism = Parallelism::None(); + options.emit_certificate = true; + const CertificationResult with_certificate = + checker.CheckPath(Waypoints(q1, q2), options); + ASSERT_EQ(with_certificate.verdict, Verdict::kCertifiedFree); + ASSERT_TRUE(with_certificate.certificate.has_value()); + const BezierCurve edge(0.0, 1.0, Waypoints(q1, q2)); + EXPECT_TRUE(VerifyCertificate(checker, checker.Normalize(edge, options), + *with_certificate.certificate)); +} + +// --------------------------------------------------------------------------- +// 4. Thickness sweep — reported, not asserted (test plan T5's diagnostic half). +// --------------------------------------------------------------------------- + +GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { + // 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. + // (thickness = 0.040 is the exact tangency, where the nearest sample's signed + // distance is 0 and "collision" — φ < 0 — is a coin toss decided by rounding; + // it is in the sweep because it is the interesting number, and the assertion + // at the end is a window, not an equality, for exactly that reason.) + // Our verdict must be kViolationFound at *every* thickness in the sweep: the + // plate is genuinely crossed in all of them. + const VectorXd q1 = MakeQ(-0.5, 0.0); + const VectorXd q2 = MakeQ(0.5, 0.0); + const std::vector thicknesses = {0.001, 0.002, 0.005, 0.010, + 0.020, 0.030, 0.038, 0.040, + 0.042, 0.050, 0.080}; + double first_caught = std::numeric_limits::quiet_NaN(); + std::cout << "\n[ THIN-PLATE SWEEP ] plate mid-plane x = " << kPlateX + << " m, tool radius = " << kToolRadius + << " m, Drake edge_step_size = " << kDrakeEdgeStepSize + << " m (samples 0.05 m apart in x)\n" + << " thickness[m] drake_sampled certified_ccd " + "contact_half_width[m]\n"; + for (const double thickness : thicknesses) { + SCOPED_TRACE("thickness = " + std::to_string(thickness)); + const SceneGraphCollisionChecker drake_checker = MakeDrakeChecker( + MakePlateWorld(kPlateX, thickness), kDrakeEdgeStepSize); + const bool drake_free = drake_checker.CheckEdgeCollisionFree(q1, q2); + + std::shared_ptr> model = + MakePlateWorld(kPlateX, thickness); + const CertifiedContinuousCollisionChecker checker = + MakeCertifiedChecker(model); + const CertificationResult result = checker.CheckEdge(q1, q2); + + if (!drake_free && std::isnan(first_caught)) first_caught = thickness; + std::cout << " " << thickness << "\t\t" + << (drake_free ? "free " : "IN COLLISION") << "\t" + << (result.verdict == Verdict::kViolationFound ? "violation" + : "OTHER ") + << "\t" << (0.5 * thickness + kToolRadius) << "\n"; + + // The assertion half of the sweep: our verdict is stable throughout. + EXPECT_EQ(result.verdict, Verdict::kViolationFound); + } + std::cout << " --> Drake's sampled checker first sees the plate at " + "thickness = " + << first_caught << " m; predicted crossover 2*(0.025 - " + << kToolRadius << ") = " << 2.0 * (0.025 - kToolRadius) << " m\n\n"; + // A report, not a gate — but the crossover must land in the right decade, + // 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 certified_ccd +} // namespace planning +} // namespace drake From c1b3f2a125019143ca5a4006bfbdf63fc4ad700e Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 14:39:55 -0400 Subject: [PATCH 06/22] [planning] Add certified_ccd: benchmark suite Adds the manual-only performance benchmark: iiwa14 in a bookcase at three swept-clearance tiers, a PWL edge, a dual-arm handover, the grazing pathological case, and thread scaling, each compared against Drake's own sampled SceneGraphCollisionChecker on the same RobotDiagram and each verified by dense sampling before it is measured. Results are written as JSON. The binary is tagged manual: a full run takes minutes and reports measurements rather than assertions. --- planning/certified_ccd/BUILD.bazel | 42 + .../certified_ccd/benchmark/benchmark_util.cc | 481 +++++++ .../certified_ccd/benchmark/benchmark_util.h | 206 +++ .../certified_ccd/benchmark/iiwa_benchmark.cc | 1164 +++++++++++++++++ .../benchmark/scenario_worlds.cc | 182 +++ .../certified_ccd/benchmark/scenario_worlds.h | 74 ++ 6 files changed, 2149 insertions(+) create mode 100644 planning/certified_ccd/benchmark/benchmark_util.cc create mode 100644 planning/certified_ccd/benchmark/benchmark_util.h create mode 100644 planning/certified_ccd/benchmark/iiwa_benchmark.cc create mode 100644 planning/certified_ccd/benchmark/scenario_worlds.cc create mode 100644 planning/certified_ccd/benchmark/scenario_worlds.h diff --git a/planning/certified_ccd/BUILD.bazel b/planning/certified_ccd/BUILD.bazel index 42dd54b9c08d..d71ddc322461 100644 --- a/planning/certified_ccd/BUILD.bazel +++ b/planning/certified_ccd/BUILD.bazel @@ -1,6 +1,7 @@ load("//tools/lint:lint.bzl", "add_lint_tests") load( "//tools/skylark:drake_cc.bzl", + "drake_cc_binary", "drake_cc_googletest", "drake_cc_library", "drake_cc_package_library", @@ -378,4 +379,45 @@ drake_cc_googletest( ], ) +# === benchmark/ === + +# The performance benchmark suite. Not part of the test suite: a full run +# takes minutes and reports measurements rather than assertions. Run it with +# bazel run //planning/certified_ccd:iiwa_benchmark -- \ +# --out /tmp/ccd --drake_commit $(git rev-parse HEAD) +drake_cc_binary( + name = "iiwa_benchmark", + srcs = [ + "benchmark/benchmark_util.cc", + "benchmark/benchmark_util.h", + "benchmark/iiwa_benchmark.cc", + "benchmark/scenario_worlds.cc", + "benchmark/scenario_worlds.h", + ], + data = [ + "@drake_models//:iiwa_description", + ], + tags = ["manual"], + deps = [ + ":certified_continuous_collision_checker", + "//common:copyable_unique_ptr", + "//common:parallelism", + "//common/trajectories:bezier_curve", + "//common/trajectories:composite_trajectory", + "//common/trajectories:trajectory", + "//geometry:geometry_ids", + "//geometry:scene_graph", + "//geometry:shape_specification", + "//math:geometric_transform", + "//multibody/parsing:parser", + "//multibody/plant", + "//multibody/tree", + "//planning:collision_checker_params", + "//planning:robot_diagram", + "//planning:robot_diagram_builder", + "//planning:scene_graph_collision_checker", + "@eigen", + ], +) + add_lint_tests() diff --git a/planning/certified_ccd/benchmark/benchmark_util.cc b/planning/certified_ccd/benchmark/benchmark_util.cc new file mode 100644 index 000000000000..7f0146507f4f --- /dev/null +++ b/planning/certified_ccd/benchmark/benchmark_util.cc @@ -0,0 +1,481 @@ +#include "drake/planning/certified_ccd/benchmark/benchmark_util.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drake/common/copyable_unique_ptr.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/geometry/query_object.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace benchmark { +namespace { + +using drake::geometry::GeometryId; +using drake::geometry::QueryObject; +using drake::geometry::SignedDistancePair; +using drake::planning::RobotDiagram; +using drake::systems::Context; +using drake::trajectories::BezierCurve; +using drake::trajectories::CompositeTrajectory; +using drake::trajectories::Trajectory; +using Eigen::MatrixXd; +using Eigen::VectorXd; + +/// Formats a double with enough digits to round-trip through the JSON. +std::string FormatDouble(double v) { + if (std::isnan(v)) return "null"; + if (std::isinf(v)) return v > 0 ? "1e999" : "-1e999"; + char buf[64]; + std::snprintf(buf, sizeof(buf), "%.10g", v); + return buf; +} + +std::string Escape(const std::string& s) { + std::string out; + for (const char c : s) { + switch (c) { + case '"': + out += "\\\""; + break; + case '\\': + out += "\\\\"; + break; + case '\n': + out += "\\n"; + break; + case '\t': + out += "\\t"; + break; + default: + out += c; + } + } + return out; +} + +/// One (t, min-distance-over-all-pairs, min-distance-over-env-pairs) probe. +struct Probe { + double all{0.0}; + double env{0.0}; +}; + +Probe ProbeAt(const RobotDiagram& diagram, Context* root, + const VectorXd& q, const std::unordered_set& env_ids, + const drake::geometry::SceneGraphInspector& inspector, + double max_distance) { + diagram.plant().SetPositions(&diagram.mutable_plant_context(root), q); + const auto& query_object = diagram.scene_graph() + .get_query_output_port() + .template Eval>( + diagram.scene_graph_context(*root)); + const std::vector> pairs = + query_object.ComputeSignedDistancePairwiseClosestPoints(max_distance); + Probe p{max_distance, max_distance}; + for (const auto& pair : pairs) { + if (pair.distance < p.all) p.all = pair.distance; + const bool a_env = env_ids.count(pair.id_A) > 0; + const bool b_env = env_ids.count(pair.id_B) > 0; + if (a_env != b_env && pair.distance < p.env) p.env = pair.distance; + (void)inspector; + } + return p; +} + +/// Golden-section minimization of `f` on [lo, hi]; the sampled bracket around +/// a dense-sample argmin is unimodal in practice for these smooth curves. +std::pair GoldenSectionMin( + const std::function& f, double lo, double hi, + int iterations) { + constexpr double kInvPhi = 0.6180339887498949; + double a = lo; + double b = hi; + double c = b - kInvPhi * (b - a); + double d = a + kInvPhi * (b - a); + double fc = f(c); + double fd = f(d); + for (int i = 0; i < iterations; ++i) { + if (fc < fd) { + b = d; + d = c; + fd = fc; + c = b - kInvPhi * (b - a); + fc = f(c); + } else { + a = c; + c = d; + fc = fd; + d = a + kInvPhi * (b - a); + fd = f(d); + } + } + return (fc < fd) ? std::make_pair(fc, c) : std::make_pair(fd, d); +} + +} // namespace + +// --------------------------------------------------------------------------- +// JsonWriter +// --------------------------------------------------------------------------- + +void JsonWriter::Indent() { + out_.append(static_cast(2 * depth_), ' '); +} + +void JsonWriter::Separator() { + if (!first_.empty()) { + if (first_.back()) { + first_.back() = false; + } else { + out_ += ","; + } + out_ += "\n"; + Indent(); + } +} + +void JsonWriter::BeginObject() { + Separator(); + out_ += "{"; + first_.push_back(true); + ++depth_; +} + +void JsonWriter::BeginObject(const std::string& key) { + Separator(); + out_ += "\"" + Escape(key) + "\": {"; + first_.push_back(true); + ++depth_; +} + +void JsonWriter::EndObject() { + const bool empty = first_.back(); + first_.pop_back(); + --depth_; + if (!empty) { + out_ += "\n"; + Indent(); + } + out_ += "}"; +} + +void JsonWriter::BeginArray(const std::string& key) { + Separator(); + out_ += "\"" + Escape(key) + "\": ["; + first_.push_back(true); + ++depth_; +} + +void JsonWriter::EndArray() { + const bool empty = first_.back(); + first_.pop_back(); + --depth_; + if (!empty) { + out_ += "\n"; + Indent(); + } + out_ += "]"; +} + +void JsonWriter::Write(const std::string& key, double value) { + Separator(); + out_ += "\"" + Escape(key) + "\": " + FormatDouble(value); +} + +void JsonWriter::Write(const std::string& key, int value) { + Separator(); + out_ += "\"" + Escape(key) + "\": " + std::to_string(value); +} + +void JsonWriter::Write(const std::string& key, long value) { // NOLINT + Separator(); + out_ += "\"" + Escape(key) + "\": " + std::to_string(value); +} + +void JsonWriter::Write(const std::string& key, + unsigned long value) { // NOLINT + Separator(); + out_ += "\"" + Escape(key) + "\": " + std::to_string(value); +} + +void JsonWriter::Write(const std::string& key, bool value) { + Separator(); + out_ += "\"" + Escape(key) + "\": " + (value ? "true" : "false"); +} + +void JsonWriter::Write(const std::string& key, const char* value) { + Write(key, std::string(value)); +} + +void JsonWriter::Write(const std::string& key, const std::string& value) { + Separator(); + out_ += "\"" + Escape(key) + "\": \"" + Escape(value) + "\""; +} + +void JsonWriter::WriteArrayValue(double value) { + Separator(); + out_ += FormatDouble(value); +} + +void JsonWriter::WriteArrayValue(const std::string& value) { + Separator(); + out_ += "\"" + Escape(value) + "\""; +} + +void WriteTextFile(const std::string& path, const std::string& text) { + const std::filesystem::path p(path); + if (p.has_parent_path()) { + std::filesystem::create_directories(p.parent_path()); + } + std::ofstream file(path); + if (!file) throw std::runtime_error("cannot open for writing: " + path); + file << text; +} + +void WriteTiming(JsonWriter* json, const std::string& key, + const TimingSummary& t) { + json->BeginObject(key); + json->Write("median", t.median_ms); + json->Write("min", t.min_ms); + json->Write("max", t.max_ms); + json->Write("reps", t.reps); + json->EndObject(); +} + +// --------------------------------------------------------------------------- +// Machine +// --------------------------------------------------------------------------- + +MachineInfo GetMachineInfo(const std::string& drake_commit) { + MachineInfo info; + info.core_count = static_cast(std::thread::hardware_concurrency()); + { + std::ifstream cpuinfo("/proc/cpuinfo"); + std::string line; + while (std::getline(cpuinfo, line)) { + const size_t colon = line.find(':'); + if (colon == std::string::npos) continue; + if (line.compare(0, 10, "model name") != 0) continue; + info.cpu_model = line.substr(colon + 1); + const size_t start = info.cpu_model.find_first_not_of(" \t"); + if (start != std::string::npos) + info.cpu_model = info.cpu_model.substr(start); + break; + } + } + // The Drake revision is not discoverable from inside the binary, so the + // caller passes it in (--drake_commit) and it is recorded verbatim. + info.drake_commit = drake_commit; + info.drake_version_note = "built from the Drake source tree"; + return info; +} + +void WriteMachine(JsonWriter* json, const MachineInfo& machine) { + json->BeginObject("machine"); + json->Write("cpu_model", machine.cpu_model); + json->Write("core_count", machine.core_count); + json->Write("drake_commit", machine.drake_commit); + json->Write("drake_version", machine.drake_version_note); + json->EndObject(); +} + +// --------------------------------------------------------------------------- +// Trajectories +// --------------------------------------------------------------------------- + +std::shared_ptr> MakeQuinticCompositeBezier( + const MatrixXd& waypoints, const std::vector& times) { + const int n = static_cast(waypoints.rows()); + const int k = static_cast(waypoints.cols()); + if (k < 2 || static_cast(times.size()) != k) { + throw std::runtime_error("MakeQuinticCompositeBezier: bad sizes"); + } + MatrixXd velocity = MatrixXd::Zero(n, k); + for (int i = 1; i + 1 < k; ++i) { + velocity.col(i) = (waypoints.col(i + 1) - waypoints.col(i - 1)) / + (times[i + 1] - times[i - 1]); + } + std::vector>> segments; + for (int i = 0; i + 1 < k; ++i) { + const double h = times[i + 1] - times[i]; + MatrixXd cps(n, 6); + const VectorXd p0 = waypoints.col(i); + const VectorXd p5 = waypoints.col(i + 1); + const VectorXd v0 = velocity.col(i); + const VectorXd v1 = velocity.col(i + 1); + cps.col(0) = p0; + cps.col(1) = p0 + h * v0 / 5.0; + cps.col(2) = p0 + 2.0 * h * v0 / 5.0; + cps.col(3) = p5 - 2.0 * h * v1 / 5.0; + cps.col(4) = p5 - h * v1 / 5.0; + cps.col(5) = p5; + segments.emplace_back( + std::make_unique>(times[i], times[i + 1], cps)); + } + return std::make_shared>(std::move(segments)); +} + +std::vector SampleTrajectory(const Trajectory& trajectory, + int count) { + const double t0 = trajectory.start_time(); + const double t1 = trajectory.end_time(); + std::vector out; + out.reserve(count); + for (int i = 0; i < count; ++i) { + const double t = (count == 1) ? t0 + : t0 + (t1 - t0) * static_cast(i) / + static_cast(count - 1); + out.push_back(trajectory.value(t).col(0)); + } + return out; +} + +double PathLengthInEdgeMetric(const Trajectory& trajectory, + int num_samples) { + const std::vector qs = SampleTrajectory(trajectory, num_samples); + double length = 0.0; + for (size_t i = 1; i < qs.size(); ++i) { + length += (qs[i] - qs[i - 1]).norm(); + } + return length; +} + +// --------------------------------------------------------------------------- +// Ground-truth swept clearance +// --------------------------------------------------------------------------- + +std::unordered_set CollectGeometryIds( + const RobotDiagram& diagram, + const std::vector& model_instance_names) { + const auto& plant = diagram.plant(); + const auto& inspector = diagram.scene_graph().model_inspector(); + std::unordered_set ids; + for (const std::string& name : model_instance_names) { + if (!plant.HasModelInstanceNamed(name)) continue; + const auto instance = plant.GetModelInstanceByName(name); + for (const auto& body_index : plant.GetBodyIndices(instance)) { + const auto frame_id = plant.GetBodyFrameIdOrThrow(body_index); + for (const auto& id : inspector.GetGeometries( + frame_id, drake::geometry::Role::kProximity)) { + ids.insert(id); + } + } + } + return ids; +} + +ClearanceReport MeasureSweptClearance( + const RobotDiagram& diagram, const Trajectory& trajectory, + const std::unordered_set& env_ids, int num_samples, + int num_threads, double max_distance) { + const double t0 = trajectory.start_time(); + const double t1 = trajectory.end_time(); + const auto& inspector = diagram.scene_graph().model_inspector(); + + const int threads = std::max(1, num_threads); + std::vector best_all(threads, max_distance); + std::vector best_env(threads, max_distance); + std::vector arg_all(threads, 0); + std::vector arg_env(threads, 0); + + const auto worker = [&](int tid) { + auto root = diagram.CreateDefaultContext(); + for (int i = tid; i < num_samples; i += threads) { + const double t = t0 + (t1 - t0) * static_cast(i) / + static_cast(num_samples - 1); + const Probe p = ProbeAt(diagram, root.get(), trajectory.value(t).col(0), + env_ids, inspector, max_distance); + if (p.all < best_all[tid]) { + best_all[tid] = p.all; + arg_all[tid] = i; + } + if (p.env < best_env[tid]) { + best_env[tid] = p.env; + arg_env[tid] = i; + } + } + }; + + if (threads == 1) { + worker(0); + } else { + std::vector pool; + pool.reserve(threads); + for (int i = 0; i < threads; ++i) pool.emplace_back(worker, i); + for (auto& th : pool) th.join(); + } + + ClearanceReport report; + report.samples = num_samples; + report.min_all = max_distance; + report.min_env = max_distance; + int i_all = 0; + int i_env = 0; + for (int i = 0; i < threads; ++i) { + if (best_all[i] < report.min_all) { + report.min_all = best_all[i]; + i_all = arg_all[i]; + } + if (best_env[i] < report.min_env) { + report.min_env = best_env[i]; + i_env = arg_env[i]; + } + } + + // Refine each sampled argmin by golden section on the neighbouring bracket. + auto root = diagram.CreateDefaultContext(); + const double dt = (t1 - t0) / static_cast(num_samples - 1); + const auto time_of = [&](int i) { + return std::min(t1, std::max(t0, t0 + dt * static_cast(i))); + }; + const auto refine = [&](int index, bool env_only, double* value, + double* argt) { + const double lo = time_of(index - 1); + const double hi = time_of(index + 1); + if (hi <= lo) { + *argt = time_of(index); + return; + } + const auto f = [&](double t) { + const Probe p = ProbeAt(diagram, root.get(), trajectory.value(t).col(0), + env_ids, inspector, max_distance); + return env_only ? p.env : p.all; + }; + const auto [best, at] = GoldenSectionMin(f, lo, hi, 60); + if (best < *value) *value = best; + *argt = at; + }; + refine(i_all, false, &report.min_all, &report.t_all); + refine(i_env, true, &report.min_env, &report.t_env); + return report; +} + +double BisectMonotone(const std::function& f, double lo, + double hi, double target, int iterations) { + double a = lo; + double b = hi; + for (int i = 0; i < iterations; ++i) { + const double mid = 0.5 * (a + b); + if (f(mid) < target) { + a = mid; + } else { + b = mid; + } + } + return 0.5 * (a + b); +} + +} // namespace benchmark +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/benchmark/benchmark_util.h b/planning/certified_ccd/benchmark/benchmark_util.h new file mode 100644 index 000000000000..26af9d15c06c --- /dev/null +++ b/planning/certified_ccd/benchmark/benchmark_util.h @@ -0,0 +1,206 @@ +#pragma once + +/// @file +/// Small, deliberately boring helpers shared by the benchmark scenarios +/// (the benchmark suite): a hand-rolled JSON writer, steady_clock timing with +/// medians, machine identification, quintic composite-Bézier construction, and +/// a dense ground-truth swept-clearance sampler used to *verify* — never to +/// certify — the clearance of every scenario trajectory. +/// +/// No third-party benchmark framework is used on purpose: the measurements +/// here are milliseconds-scale wall clock repeated by hand, and the JSON is +/// consumed by the white-paper author and by CI tracking. + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/trajectories/composite_trajectory.h" +#include "drake/common/trajectories/trajectory.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace benchmark { + +// --------------------------------------------------------------------------- +// JSON +// --------------------------------------------------------------------------- + +/// Minimal streaming JSON writer: enough for the fixed result schema, with no +/// dependency and no cleverness. Callers must balance Begin*/End* calls. +class JsonWriter { + public: + JsonWriter() = default; + + void BeginObject(); + void BeginObject(const std::string& key); + void EndObject(); + void BeginArray(const std::string& key); + void EndArray(); + + void Write(const std::string& key, double value); + void Write(const std::string& key, int value); + void Write(const std::string& key, long value); // NOLINT + void Write(const std::string& key, unsigned long value); // NOLINT + void Write(const std::string& key, bool value); + void Write(const std::string& key, const char* value); + void Write(const std::string& key, const std::string& value); + /// Appends a bare double to the innermost array. + void WriteArrayValue(double value); + void WriteArrayValue(const std::string& value); + + std::string str() const { return out_ + "\n"; } + + private: + void Separator(); + void Indent(); + + std::string out_; + std::vector first_; // per open container: "nothing written yet" + int depth_{0}; +}; + +/// Writes `text` to `path`, creating parent directories as needed. +void WriteTextFile(const std::string& path, const std::string& text); + +// --------------------------------------------------------------------------- +// Timing +// --------------------------------------------------------------------------- + +struct TimingSummary { + double median_ms{0.0}; + double min_ms{0.0}; + double max_ms{0.0}; + int reps{0}; +}; + +/// Runs `body` `warmup` times untimed, then `reps` times timed, and reduces +/// the sample to median/min/max. No pinning, no frequency control: the numbers +/// are what a user on this machine would see (the benchmark suite). +template +TimingSummary TimeRepeatedly(int warmup, int reps, F&& body) { + for (int i = 0; i < warmup; ++i) { + body(); + } + std::vector ms; + ms.reserve(reps); + for (int i = 0; i < reps; ++i) { + const auto t0 = std::chrono::steady_clock::now(); + body(); + const auto t1 = std::chrono::steady_clock::now(); + ms.push_back(std::chrono::duration(t1 - t0).count()); + } + std::sort(ms.begin(), ms.end()); + TimingSummary s; + s.reps = reps; + s.min_ms = ms.front(); + s.max_ms = ms.back(); + s.median_ms = + (reps % 2 == 1) ? ms[reps / 2] : 0.5 * (ms[reps / 2 - 1] + ms[reps / 2]); + return s; +} + +void WriteTiming(JsonWriter* json, const std::string& key, + const TimingSummary& t); + +// --------------------------------------------------------------------------- +// Machine identification +// --------------------------------------------------------------------------- + +struct MachineInfo { + std::string cpu_model; + int core_count{0}; + std::string drake_commit; + std::string drake_version_note; +}; + +/// Reads the CPU model from /proc/cpuinfo and records `drake_commit` (the +/// Drake revision the caller was built from, passed through verbatim) so +/// every result file self-identifies. +MachineInfo GetMachineInfo(const std::string& drake_commit); + +void WriteMachine(JsonWriter* json, const MachineInfo& machine); + +// --------------------------------------------------------------------------- +// Trajectories +// --------------------------------------------------------------------------- + +/// Builds a C2 composite quintic Bézier through the columns of `waypoints` +/// (n × K) at the given `times` (K values, strictly increasing). Waypoint +/// velocities come from centred finite differences (zero at both ends) and +/// waypoint accelerations are zero, which is exactly the smooth composite +/// Bézier a GCS/B-spline planner would hand us — degree 5, K−1 segments. +/// +/// Control points per segment (duration h, endpoint velocities v0, v1): +/// P0 = q0, P5 = q1, +/// P1 = P0 + h v0/5, P4 = P5 − h v1/5, +/// P2 = P0 + 2 h v0/5, P3 = P5 − 2 h v1/5, +/// which reproduces q(t0)=q0, q̇(t0)=v0, q̈(t0)=0 and likewise at t1. +std::shared_ptr> +MakeQuinticCompositeBezier(const Eigen::MatrixXd& waypoints, + const std::vector& times); + +/// Path length in the plant's default edge metric: the unweighted Euclidean +/// configuration distance (LinearDistanceAndInterpolationProvider's default +/// weights are 1 for every non-quaternion coordinate), integrated along the +/// trajectory with `num_samples` chords. Used to derive the number of samples +/// a sampled checker would take at a given edge_step_size. +double PathLengthInEdgeMetric(const drake::trajectories::Trajectory& t, + int num_samples); + +/// Samples `count` configurations uniformly in trajectory time (inclusive of +/// both endpoints). +std::vector SampleTrajectory( + const drake::trajectories::Trajectory& trajectory, int count); + +// --------------------------------------------------------------------------- +// Ground-truth swept clearance +// --------------------------------------------------------------------------- + +/// True minimum signed distance along a trajectory, obtained by dense +/// sampling plus a golden-section refinement of the sampled argmin. This is +/// the benchmark's independent oracle: it is what "achieved clearance" means +/// in the result files. `min_env` restricts the minimum to robot-vs- +/// environment pairs (the quantity a shelf shift/scale actually controls); +/// `min_all` also includes robot-vs-robot pairs. +struct ClearanceReport { + double min_all{0.0}; + double t_all{0.0}; + double min_env{0.0}; + double t_env{0.0}; + int samples{0}; +}; + +/// Geometry ids belonging to bodies of the named model instances. +std::unordered_set CollectGeometryIds( + const drake::planning::RobotDiagram& diagram, + const std::vector& model_instance_names); + +/// Dense-samples `trajectory` (`num_samples` configurations, split over +/// `num_threads` cloned contexts) and refines the minimum by golden section. +/// Distances beyond `max_distance` are not resolved; if no pair comes within +/// it the reported minimum saturates at `max_distance`. +ClearanceReport MeasureSweptClearance( + const drake::planning::RobotDiagram& diagram, + const drake::trajectories::Trajectory& trajectory, + const std::unordered_set& env_ids, + int num_samples, int num_threads, double max_distance); + +/// Bisects `f` (assumed non-decreasing) on [lo, hi] for f(x) = target. +/// Returns x. Used to place the shelf at a requested swept clearance. +double BisectMonotone(const std::function& f, double lo, + double hi, double target, int iterations); + +} // namespace benchmark +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/benchmark/iiwa_benchmark.cc b/planning/certified_ccd/benchmark/iiwa_benchmark.cc new file mode 100644 index 000000000000..37316b29fa95 --- /dev/null +++ b/planning/certified_ccd/benchmark/iiwa_benchmark.cc @@ -0,0 +1,1164 @@ +/// @file +/// The `certified_ccd` performance benchmark suite (the performance +/// targets and the benchmark deliverable of the white paper), adapted to what +/// exists on this machine: no trajectory optimizer is invoked, the smooth +/// composite Bézier trajectories are hand-constructed in +/// benchmark/scenario_worlds.cc, and every scenario's *true* swept clearance is +/// verified by dense sampling before it is benchmarked. +/// +/// Scenarios +/// a) iiwa14 + bookcase, three tiers at ~2 mm / 1 cm / 5 cm swept clearance +/// b) a two-waypoint PWL edge in the same world +/// c) dual-arm iiwa handover (self-collision heavy) +/// d) the grazing pathological case (kInconclusive cost at the floor) +/// e) thread scaling over a 1000-check batch, two ways +/// +/// Scenarios (a, 1 cm tier) and (b) are additionally compared against Drake's +/// own sampled `SceneGraphCollisionChecker` on the *same* RobotDiagram. +/// +/// Usage: iiwa_benchmark [--out DIR] [--reps N] [--warmup N] +/// [--dense-samples N] [--batch N] [--only NAME] +/// [--drake_commit SHA] +/// +/// `--out` defaults to the current directory, and `--drake_commit` (the +/// Drake revision this binary was built from, "unknown" by default) is +/// recorded verbatim in every result file so a JSON result identifies the +/// code it measured. + +#include +#include +#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/geometry/query_object.h" +#include "drake/planning/certified_ccd/benchmark/benchmark_util.h" +#include "drake/planning/certified_ccd/benchmark/scenario_worlds.h" +#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/collision_checker_params.h" +#include "drake/planning/scene_graph_collision_checker.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace benchmark { +namespace { + +using drake::Parallelism; +using drake::planning::CollisionCheckerParams; +using drake::planning::RobotDiagram; +using drake::planning::SceneGraphCollisionChecker; +using drake::trajectories::CompositeTrajectory; +using drake::trajectories::Trajectory; +using Eigen::MatrixXd; +using Eigen::VectorXd; + +/// drake::planning::CollisionCheckerParams::edge_step_size has NO library +/// default: the field is value-initialized to 0 and the CollisionChecker +/// constructor rejects any non-positive value, so every caller must choose +/// one. 0.05 rad is the value that appears most often in Drake's own tests +/// and examples; the other common choices (0.125, 0.1, 0.01) are measured and +/// reported too, so the comparison cannot be accused of picking a flattering +/// resolution. +constexpr double kEdgeStepSize = 0.05; +constexpr double kReportedEdgeStepSizes[] = {0.125, 0.1, 0.05, 0.01}; + +/// Distances beyond this are irrelevant to every scenario here; the ground +/// truth sampler saturates at it. +constexpr double kMaxProbeDistance = 0.30; + +struct Config { + std::string out_dir = "."; + std::string drake_commit = "unknown"; + int reps = 20; + int warmup = 3; + int dense_samples = 100000; + int tune_samples = 3000; + int tune_iterations = 24; + int batch = 1000; + int max_threads = 16; + std::string only; +}; + +std::string VerdictName(Verdict v) { + switch (v) { + case Verdict::kCertifiedFree: + return "kCertifiedFree"; + case Verdict::kViolationFound: + return "kViolationFound"; + case Verdict::kInconclusive: + return "kInconclusive"; + case Verdict::kBudgetExhausted: + return "kBudgetExhausted"; + } + return "unknown"; +} + +std::string ModeName(SearchMode m) { + return m == SearchMode::kCertifyAll ? "kCertifyAll" : "kFindFirstViolation"; +} + +/// A world plus both checkers built on the *same* RobotDiagram. The sampled +/// checker is constructed first on purpose: its constructor pushes its +/// nominal filtered-collision matrix into the SceneGraph, so building our +/// checker afterwards guarantees the two see a bit-identical unfiltered pair +/// set. Anything else would make the comparison unfair in our favour. +struct World { + std::shared_ptr> diagram; + std::unique_ptr sampled; + std::unique_ptr certified; + std::unordered_set env_ids; + int pair_count{0}; + /// SceneGraph's own unfiltered-candidate count *after* the sampled checker + /// pushed its filters in. Equality with pair_count is the evidence that + /// both checkers are looking at exactly the same pairs. + int scene_graph_candidates{0}; +}; + +World MakeWorld(std::shared_ptr> diagram, + const std::vector& robot_instance_names) { + World world; + world.diagram = std::move(diagram); + const auto& plant = world.diagram->plant(); + + CollisionCheckerParams params; + params.model = world.diagram; + for (const std::string& name : robot_instance_names) { + params.robot_model_instances.push_back(plant.GetModelInstanceByName(name)); + } + params.edge_step_size = kEdgeStepSize; + params.env_collision_padding = 0.0; + params.self_collision_padding = 0.0; + params.implicit_context_parallelism = Parallelism::None(); + world.sampled = + std::make_unique(std::move(params)); + + CertifiedContinuousCollisionChecker::Params cparams; + cparams.model = world.diagram; + world.certified = + std::make_unique(cparams); + + world.env_ids = CollectGeometryIds(*world.diagram, {"environment"}); + world.pair_count = static_cast(world.certified->pairs().size()); + world.scene_graph_candidates = static_cast(world.diagram->scene_graph() + .model_inspector() + .GetCollisionCandidates() + .size()); + return world; +} + +Options MakeOptions(SearchMode mode, Parallelism parallelism, + double min_interval = 1e-9) { + Options options; + options.margin = 0.0; + options.mode = mode; + options.parallelism = parallelism; + options.min_interval = min_interval; + return options; +} + +void WriteOptions(JsonWriter* json, const Options& options) { + json->BeginObject("options"); + json->Write("margin", options.margin); + json->Write("query_tolerance", options.query_tolerance); + json->Write("certificate_slack", options.certificate_slack); + json->Write("min_interval", options.min_interval); + json->Write("mode", ModeName(options.mode)); + json->Write("max_reported_findings", options.max_reported_findings); + json->Write("emit_certificate", options.emit_certificate); + json->Write("parallelism", options.parallelism.num_threads()); + json->EndObject(); +} + +void WriteStats(JsonWriter* json, const Statistics& stats) { + json->BeginObject("stats"); + json->Write("nodes", stats.nodes); + json->Write("narrowphase_queries", stats.narrowphase_queries); + json->Write("sphere_certifications", stats.sphere_certifications); + json->Write("max_depth", stats.max_depth); + json->EndObject(); +} + +void WriteClearance(JsonWriter* json, const ClearanceReport& clearance) { + json->BeginObject("achieved_clearance"); + json->Write("min_all_pairs_m", clearance.min_all); + json->Write("t_at_min_all", clearance.t_all); + json->Write("min_robot_env_pairs_m", clearance.min_env); + json->Write("t_at_min_env", clearance.t_env); + json->Write("dense_samples", clearance.samples); + json->Write("note", + "ground truth from dense sampling plus golden-section " + "refinement; the iiwa14 dense-sphere model has an intrinsic " + "~25.2 mm self-clearance floor (link_0 vs link_2 spheres) that " + "no shelf placement can raise, so min_all_pairs saturates " + "there once the environment clearance exceeds it"); + json->EndObject(); +} + +/// One certification measurement. +struct CertRun { + Verdict verdict{}; + Statistics stats; + TimingSummary timing; + int num_findings{0}; +}; + +CertRun MeasureCertify(const CertifiedContinuousCollisionChecker& checker, + const Trajectory& trajectory, + const Options& options, int warmup, int reps) { + CertRun run; + run.timing = TimeRepeatedly(warmup, reps, [&]() { + const CertificationResult result = + checker.CheckTrajectory(trajectory, options); + run.verdict = result.verdict; + run.stats = result.stats; + run.num_findings = static_cast(result.findings.size()); + }); + return run; +} + +CertRun MeasureCertifyEdge(const CertifiedContinuousCollisionChecker& checker, + const VectorXd& q1, const VectorXd& q2, + const Options& options, int warmup, int reps) { + CertRun run; + run.timing = TimeRepeatedly(warmup, reps, [&]() { + const CertificationResult result = checker.CheckEdge(q1, q2, options); + run.verdict = result.verdict; + run.stats = result.stats; + run.num_findings = static_cast(result.findings.size()); + }); + return run; +} + +void WriteCertRun(JsonWriter* json, const std::string& key, + const CertRun& run) { + json->BeginObject(key); + json->Write("verdict", VerdictName(run.verdict)); + json->Write("num_findings", run.num_findings); + WriteStats(json, run.stats); + WriteTiming(json, "wall_ms", run.timing); + json->EndObject(); +} + +// --------------------------------------------------------------------------- +// The sampled-checker comparison (the performance requirements, the headline +// number) +// --------------------------------------------------------------------------- + +/// For a curved trajectory a practitioner checks it the only way a sampled +/// checker allows: walk the path and call CheckConfigCollisionFree at the +/// same resolution the checker would use for an edge, i.e. one sample per +/// `edge_step_size` of path length in the plant's edge metric. We report the +/// implied sample count and the wall time of exactly that sweep. +void MeasureSampledPathSweep(JsonWriter* json, + const SceneGraphCollisionChecker& sampled, + const Trajectory& trajectory, int warmup, + int reps) { + const double length = PathLengthInEdgeMetric(trajectory, 20001); + json->BeginObject("sampled_comparison"); + json->Write("checker", "drake::planning::SceneGraphCollisionChecker"); + json->Write("edge_step_size_default_in_drake", + "none - CollisionCheckerParams::edge_step_size is a required " + "positive parameter with no library default"); + json->Write("edge_step_size", sampled.edge_step_size()); + json->Write("edge_metric", + "unweighted Euclidean (LinearDistanceAndInterpolationProvider " + "default weights = 1)"); + json->Write("path_length_edge_metric_rad", length); + json->BeginArray("sweeps"); + for (const double step : kReportedEdgeStepSizes) { + const int implied = static_cast(std::ceil(length / step)) + 1; + const std::vector configs = SampleTrajectory(trajectory, implied); + bool free = true; + const TimingSummary timing = TimeRepeatedly(warmup, reps, [&]() { + bool ok = true; + for (const VectorXd& q : configs) { + ok = sampled.CheckConfigCollisionFree(q) && ok; + } + free = ok; + }); + json->BeginObject(); + json->Write("edge_step_size", step); + json->Write("implied_samples", implied); + json->Write("collision_free", free); + WriteTiming(json, "sampled_wall_ms", timing); + json->EndObject(); + } + json->EndArray(); + json->EndObject(); +} + +void MeasureSampledEdge(JsonWriter* json, + const SceneGraphCollisionChecker& sampled, + const VectorXd& q1, const VectorXd& q2, int warmup, + int reps) { + const double length = sampled.ComputeConfigurationDistance(q1, q2); + json->BeginObject("sampled_comparison"); + json->Write("checker", "drake::planning::SceneGraphCollisionChecker"); + json->Write("edge_step_size_default_in_drake", + "none - CollisionCheckerParams::edge_step_size is a required " + "positive parameter with no library default"); + json->Write("edge_metric", + "unweighted Euclidean (LinearDistanceAndInterpolationProvider " + "default weights = 1)"); + json->Write("path_length_edge_metric_rad", length); + json->BeginArray("sweeps"); + // A SceneGraphCollisionChecker is not copy-assignable, so vary the step + // size on a mutable clone rather than rebuilding the model. + std::unique_ptr clone = sampled.Clone(); + for (const double step : kReportedEdgeStepSizes) { + clone->set_edge_step_size(step); + const int implied = static_cast(std::ceil(length / step)) + 1; + bool free = true; + const TimingSummary timing = TimeRepeatedly(warmup, reps, [&]() { + free = clone->CheckEdgeCollisionFree(q1, q2); + }); + json->BeginObject(); + json->Write("edge_step_size", step); + json->Write("implied_samples", implied); + json->Write("collision_free", free); + WriteTiming(json, "sampled_wall_ms", timing); + json->EndObject(); + } + json->EndArray(); + json->EndObject(); +} + +// --------------------------------------------------------------------------- +// Shared plumbing +// --------------------------------------------------------------------------- + +/// Places the bookcase so the fixed trajectory's robot-vs-environment swept +/// clearance equals `target` (bisection on the shelf scale, which is monotone +/// non-decreasing over [0.010, 0.090]). +double TuneShelfScale(const Config& config, double target) { + const MatrixXd waypoints = ShelfTrajectoryWaypoints(); + const auto trajectory = + MakeQuinticCompositeBezier(waypoints, ShelfTrajectoryTimes()); + const auto clearance_of = [&](double scale) { + const auto diagram = MakeShelfWorld(scale); + const auto env_ids = CollectGeometryIds(*diagram, {"environment"}); + return MeasureSweptClearance(*diagram, *trajectory, env_ids, + config.tune_samples, config.max_threads, + kMaxProbeDistance) + .min_env; + }; + return BisectMonotone(clearance_of, 0.010, 0.090, target, + config.tune_iterations); +} + +/// Repetition policy. Every millisecond-scale measurement gets the full +/// `--reps` after `--warmup` untimed runs. The grazing scenario at the +/// default 1e-9 resolution floor costs tens of seconds per call, where 20 +/// repetitions would blow the suite's time budget for no statistical gain +/// (the relative spread of a 40 s measurement is far below that of a 2 ms +/// one), so expensive cases fall back to a small fixed count. The chosen +/// count is recorded in every timing block, so no result is silently +/// under-sampled. +void PlanReps(const Config& config, double single_run_ms, int* warmup, + int* reps) { + if (single_run_ms > 1000.0) { + *warmup = 0; + *reps = std::min(config.reps, 3); + } else { + *warmup = config.warmup; + *reps = config.reps; + } +} + +/// Times one certification once, untimed, to price the case for PlanReps. +double ProbeCost(const CertifiedContinuousCollisionChecker& checker, + const Trajectory& trajectory, const Options& options) { + const auto t0 = std::chrono::steady_clock::now(); + checker.CheckTrajectory(trajectory, options); + const auto t1 = std::chrono::steady_clock::now(); + return std::chrono::duration(t1 - t0).count(); +} + +void PrintHeader() { + std::printf(" %-26s %-18s %8s %8s %10s %10s %7s\n", "case", "verdict", + "med_ms", "min_ms", "nodes", "np_query", "depth"); +} + +void PrintRow(const std::string& label, const CertRun& run) { + std::printf( + " %-26s %-18s %8.3f %8.3f %10llu %10llu %7d\n", label.c_str(), + VerdictName(run.verdict).c_str(), run.timing.median_ms, run.timing.min_ms, + static_cast(run.stats.nodes), // NOLINT(runtime/int) + static_cast( // NOLINT(runtime/int) + run.stats.narrowphase_queries), + run.stats.max_depth); +} + +// --------------------------------------------------------------------------- +// (b) the PWL edge, run inside the 1 cm shelf world. +// --------------------------------------------------------------------------- + +void RunPwlEdge(const Config& config, const MachineInfo& machine, + const World& world, const MatrixXd& shelf_waypoints, + double scale) { + const VectorXd q1 = shelf_waypoints.col(0); + const VectorXd q2 = shelf_waypoints.col(1); + MatrixXd edge(q1.size(), 2); + edge.col(0) = q1; + edge.col(1) = q2; + const auto edge_trajectory = MakeQuinticCompositeBezier(edge, {0.0, 1.0}); + const ClearanceReport clearance = MeasureSweptClearance( + *world.diagram, *edge_trajectory, world.env_ids, config.dense_samples, + config.max_threads, kMaxProbeDistance); + const CertRun certify_all = MeasureCertifyEdge( + *world.certified, q1, q2, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), config.warmup, + config.reps); + const CertRun find_first = MeasureCertifyEdge( + *world.certified, q1, q2, + MakeOptions(SearchMode::kFindFirstViolation, Parallelism::None()), + config.warmup, config.reps); + + std::printf( + "[b pwl edge] length=%.4f rad clearance all=%.6f m " + "env=%.6f m\n", + (q2 - q1).norm(), clearance.min_all, clearance.min_env); + PrintHeader(); + PrintRow("certify_all serial", certify_all); + PrintRow("find_first serial", find_first); + std::printf("\n"); + + JsonWriter json; + json.BeginObject(); + json.Write("scenario", "b_pwl_edge"); + json.Write("description", + "two-waypoint PWL edge (a single order-1 Bezier segment) in the " + "1 cm shelf world, healthy clearance"); + json.Write("model", kIiwaUrl); + json.Write("shelf_scale", scale); + json.Write("pair_count", world.pair_count); + json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); + json.Write("num_positions", world.diagram->plant().num_positions()); + json.Write("edge_length_rad", (q2 - q1).norm()); + WriteClearance(&json, clearance); + WriteOptions(&json, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); + json.Write("verdict", VerdictName(certify_all.verdict)); + WriteStats(&json, certify_all.stats); + WriteTiming(&json, "wall_ms", certify_all.timing); + WriteCertRun(&json, "certify_all_serial", certify_all); + WriteCertRun(&json, "find_first_serial", find_first); + MeasureSampledEdge(&json, *world.sampled, q1, q2, config.warmup, config.reps); + WriteMachine(&json, machine); + json.EndObject(); + WriteTextFile(config.out_dir + "/pwl_edge.json", json.str()); +} + +// --------------------------------------------------------------------------- +// (e) thread scaling. +// --------------------------------------------------------------------------- + +void RunThreadScaling(const Config& config, const MachineInfo& machine, + const World& world, const MatrixXd& shelf_waypoints, + double scale) { + std::printf( + "[e threads] building a batch of %d certified-free " + "trajectories ...\n", + config.batch); + std::mt19937 rng(20260826); + std::uniform_real_distribution jitter(-0.02, 0.02); + std::vector candidates; + const int max_candidates = 8 * config.batch; + candidates.reserve(max_candidates); + for (int i = 0; i < max_candidates; ++i) { + MatrixXd w = shelf_waypoints; + if (i > 0) { + for (int c = 0; c < w.cols(); ++c) { + for (int r = 0; r < w.rows(); ++r) w(r, c) += jitter(rng); + } + } + candidates.push_back(w); + } + + // Screen in parallel: only trajectories the checker *proves* free join the + // batch, so the throughput numbers are all full certifications. + std::vector ok(candidates.size(), 0); + { + std::atomic cursor{0}; + const auto screen = [&]() { + const Options options = + MakeOptions(SearchMode::kCertifyAll, Parallelism::None()); + for (;;) { + const size_t i = cursor.fetch_add(1); + if (i >= candidates.size()) return; + const auto traj = + MakeQuinticCompositeBezier(candidates[i], ShelfTrajectoryTimes()); + ok[i] = world.certified->CheckTrajectory(*traj, options).verdict == + Verdict::kCertifiedFree; + } + }; + std::vector pool; + pool.reserve(config.max_threads); + for (int t = 0; t < config.max_threads; ++t) pool.emplace_back(screen); + for (auto& th : pool) th.join(); + } + + std::vector>> accepted; + int screened = 0; + for (size_t i = 0; i < candidates.size() && + static_cast(accepted.size()) < config.batch; + ++i) { + ++screened; + if (!ok[i]) continue; + accepted.push_back( + MakeQuinticCompositeBezier(candidates[i], ShelfTrajectoryTimes())); + } + const double acceptance = + screened > 0 + ? static_cast(accepted.size()) / static_cast(screened) + : 0.0; + std::printf("[e threads] accepted %zu of %d screened (%.1f%%)\n", + accepted.size(), screened, 100.0 * acceptance); + + // Independent re-verification of a sample of the accepted batch: the + // certificate says free, dense sampling must agree. + double verify_min = kMaxProbeDistance; + const int verify_count = std::min(16, static_cast(accepted.size())); + for (int i = 0; i < verify_count; ++i) { + const size_t index = static_cast(i) * accepted.size() / + static_cast(verify_count); + const ClearanceReport r = + MeasureSweptClearance(*world.diagram, *accepted[index], world.env_ids, + 20000, config.max_threads, kMaxProbeDistance); + verify_min = std::min(verify_min, r.min_all); + } + std::printf( + "[e threads] re-verified %d sampled members; worst dense " + "clearance %.6f m\n", + verify_count, verify_min); + + JsonWriter json; + json.BeginObject(); + json.Write("scenario", "e_thread_scaling"); + json.Write("description", + "batch of certification calls on the 1 cm tier trajectory and " + "jittered variants (+/-0.02 rad on every waypoint coordinate) " + "that remain certified free"); + json.Write("model", kIiwaUrl); + json.Write("shelf_scale", scale); + json.Write("pair_count", world.pair_count); + json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); + json.Write("batch_size", static_cast(accepted.size())); + json.Write("candidates_screened", screened); + json.Write("acceptance_rate", acceptance); + json.Write("reverified_members", verify_count); + json.Write("reverified_worst_clearance_m", verify_min); + WriteOptions(&json, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); + WriteMachine(&json, machine); + + const int thread_counts[] = {1, 8, 16}; + json.BeginArray("thread_scaling"); + double baseline_per_call = 0.0; + for (const int p : thread_counts) { + const Options options = + MakeOptions(SearchMode::kCertifyAll, Parallelism(p)); + const auto t0 = std::chrono::steady_clock::now(); + for (const auto& traj : accepted) { + world.certified->CheckTrajectory(*traj, options); + } + const auto t1 = std::chrono::steady_clock::now(); + const double seconds = std::chrono::duration(t1 - t0).count(); + const double throughput = static_cast(accepted.size()) / seconds; + if (p == 1) baseline_per_call = throughput; + json.BeginObject(); + json.Write("mode", "per_call_parallelism"); + json.Write("threads", p); + json.Write("wall_s", seconds); + json.Write("checks_per_s", throughput); + json.Write("speedup", throughput / baseline_per_call); + json.EndObject(); + std::printf( + "[e threads] per-call p=%2d %8.3f s %9.1f checks/s " + "%5.2fx\n", + p, seconds, throughput, throughput / baseline_per_call); + } + double baseline_caller = 0.0; + for (const int t : thread_counts) { + const Options options = + MakeOptions(SearchMode::kCertifyAll, Parallelism::None()); + std::atomic cursor{0}; + const auto worker = [&]() { + for (;;) { + const size_t i = cursor.fetch_add(1); + if (i >= accepted.size()) return; + world.certified->CheckTrajectory(*accepted[i], options); + } + }; + const auto s0 = std::chrono::steady_clock::now(); + std::vector pool; + pool.reserve(t); + for (int k = 0; k < t; ++k) pool.emplace_back(worker); + for (auto& th : pool) th.join(); + const auto s1 = std::chrono::steady_clock::now(); + const double seconds = std::chrono::duration(s1 - s0).count(); + const double throughput = static_cast(accepted.size()) / seconds; + if (t == 1) baseline_caller = throughput; + json.BeginObject(); + json.Write("mode", "caller_threads_serial_checks"); + json.Write("threads", t); + json.Write("wall_s", seconds); + json.Write("checks_per_s", throughput); + json.Write("speedup", throughput / baseline_caller); + json.EndObject(); + std::printf( + "[e threads] caller t=%2d %8.3f s %9.1f checks/s " + "%5.2fx\n", + t, seconds, throughput, throughput / baseline_caller); + } + json.EndArray(); + json.EndObject(); + WriteTextFile(config.out_dir + "/thread_scaling.json", json.str()); + std::printf("\n"); +} + +// --------------------------------------------------------------------------- +// (c) dual-arm handover. +// --------------------------------------------------------------------------- + +void RunDualArm(const Config& config, const MachineInfo& machine) { + constexpr double kBaseSeparation = 1.00; + const MatrixXd waypoints = DualArmTrajectoryWaypoints(); + const auto trajectory = + MakeQuinticCompositeBezier(waypoints, DualArmTrajectoryTimes()); + World world = + MakeWorld(MakeDualArmWorld(kBaseSeparation), {"iiwa14", "iiwa14_1"}); + const ClearanceReport clearance = MeasureSweptClearance( + *world.diagram, *trajectory, world.env_ids, config.dense_samples, + config.max_threads, kMaxProbeDistance); + std::printf( + "[c dual arm] separation=%.3f m clearance all=%.6f m " + "env=%.6f m pairs=%d\n", + kBaseSeparation, clearance.min_all, clearance.min_env, world.pair_count); + + const CertRun serial_all = + MeasureCertify(*world.certified, *trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), + config.warmup, config.reps); + const CertRun par16 = + MeasureCertify(*world.certified, *trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism(16)), + config.warmup, config.reps); + PrintHeader(); + PrintRow("certify_all serial", serial_all); + PrintRow("certify_all 16 threads", par16); + std::printf("\n"); + + JsonWriter json; + json.BeginObject(); + json.Write("scenario", "c_dual_arm_handover"); + json.Write("description", + "two iiwa14 dense-sphere arms welded 1.00 m apart facing each " + "other; 4-segment quintic composite Bezier bringing the " + "end-effectors past each other and back"); + json.Write("model", kIiwaUrl); + json.Write("base_separation_m", kBaseSeparation); + json.Write("pair_count", world.pair_count); + json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); + json.Write("num_positions", world.diagram->plant().num_positions()); + json.Write("trajectory_segments", static_cast(waypoints.cols()) - 1); + json.Write("trajectory_degree", 5); + WriteClearance(&json, clearance); + WriteOptions(&json, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); + json.Write("verdict", VerdictName(serial_all.verdict)); + WriteStats(&json, serial_all.stats); + WriteTiming(&json, "wall_ms", serial_all.timing); + WriteCertRun(&json, "certify_all_serial", serial_all); + WriteCertRun(&json, "certify_all_16_threads", par16); + WriteMachine(&json, machine); + json.EndObject(); + WriteTextFile(config.out_dir + "/dual_arm.json", json.str()); +} + +// --------------------------------------------------------------------------- +// (d) grazing. +// --------------------------------------------------------------------------- + +void RunGrazing(const Config& config, const MachineInfo& machine, + const Trajectory& shelf_trajectory) { + std::printf("[d grazing] tuning shelf placement for zero clearance ...\n"); + const double scale = TuneShelfScale(config, 0.0); + World world = MakeWorld(MakeShelfWorld(scale), {"iiwa14"}); + const ClearanceReport clearance = MeasureSweptClearance( + *world.diagram, shelf_trajectory, world.env_ids, config.dense_samples, + config.max_threads, kMaxProbeDistance); + std::printf( + "[d grazing] shelf_scale=%.6f clearance env=%.9f m " + "all=%.9f m\n", + scale, clearance.min_env, clearance.min_all); + + JsonWriter json; + json.BeginObject(); + json.Write("scenario", "d_grazing"); + json.Write("description", + "the same shelf world placed so the trajectory's swept " + "clearance sits within the oracle tolerance of zero: the " + "conservative certifier must refine to the resolution floor and " + "report kInconclusive rather than a certificate"); + json.Write("model", kIiwaUrl); + json.Write("shelf_scale", scale); + json.Write("pair_count", world.pair_count); + json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); + WriteClearance(&json, clearance); + + // The resolution floor is the knob that prices the pathological case: cost + // at the floor grows like log2(1 / min_interval) (the soundness argument's + // termination proof). + constexpr double kFloors[] = {1e-9, 1e-6, 1e-4, 1e-2}; + CertRun default_run; + json.BeginArray("min_interval_sweep"); + PrintHeader(); + for (const double floor : kFloors) { + const Options options = + MakeOptions(SearchMode::kCertifyAll, Parallelism::None(), floor); + int warmup = 0; + int reps = 0; + PlanReps(config, ProbeCost(*world.certified, shelf_trajectory, options), + &warmup, &reps); + const CertRun run = MeasureCertify(*world.certified, shelf_trajectory, + options, warmup, reps); + if (floor == 1e-9) default_run = run; + json.BeginObject(); + json.Write("min_interval", floor); + json.Write("verdict", VerdictName(run.verdict)); + json.Write("num_findings", run.num_findings); + WriteStats(&json, run.stats); + WriteTiming(&json, "wall_ms", run.timing); + json.EndObject(); + char label[64]; + std::snprintf(label, sizeof(label), "min_interval=%g", floor); + PrintRow(label, run); + } + json.EndArray(); + // kFindFirstViolation at the same floor: with no definite violation + // anywhere on the trajectory the earliest-witness bound never prunes, so + // this is expected to cost the same as kCertifyAll — measured, not assumed. + const Options first_options = + MakeOptions(SearchMode::kFindFirstViolation, Parallelism::None()); + int first_warmup = 0; + int first_reps = 0; + PlanReps(config, ProbeCost(*world.certified, shelf_trajectory, first_options), + &first_warmup, &first_reps); + const CertRun find_first = + MeasureCertify(*world.certified, shelf_trajectory, first_options, + first_warmup, first_reps); + PrintRow("find_first serial", find_first); + std::printf("\n"); + + WriteOptions(&json, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); + json.Write("verdict", VerdictName(default_run.verdict)); + WriteStats(&json, default_run.stats); + WriteTiming(&json, "wall_ms", default_run.timing); + WriteCertRun(&json, "certify_all_serial", default_run); + WriteCertRun(&json, "find_first_serial", find_first); + WriteMachine(&json, machine); + json.EndObject(); + WriteTextFile(config.out_dir + "/grazing.json", json.str()); +} + +// --------------------------------------------------------------------------- +// (f) performance review: where the time goes, and why per-call parallelism +// saturates. Not a standard scenario — this exists to back the gap analysis +// in the benchmark write-up with measurements rather than assertions. +// --------------------------------------------------------------------------- + +void RunProfile(const Config& config, const MachineInfo& machine, + const Trajectory& shelf_trajectory) { + std::printf("[f profile] rebuilding the 1 cm world ...\n"); + const double scale = TuneShelfScale(config, 0.010); + World world = MakeWorld(MakeShelfWorld(scale), {"iiwa14"}); + + JsonWriter json; + json.BeginObject(); + json.Write("scenario", "f_profile"); + json.Write("description", + "cost attribution and parallel-granularity probe backing the " + "gap analysis in the benchmark write-up; not one of the standard " + "scenarios"); + json.Write("model", kIiwaUrl); + json.Write("shelf_scale", scale); + json.Write("pair_count", world.pair_count); + json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); + + // --- Cost attribution ----------------------------------------------------- + // Two microbenchmarks over the same inner loop isolate the marginal cost of + // a narrowphase query from the fixed per-configuration cost (SetPositions + // plus the pose/broadphase update the first query forces). + constexpr int kInner = 2000; + constexpr int kManyQueries = 27; // ~ the observed queries per node + const auto& oracle = world.certified->distance_oracle(); + const auto& pairs = world.certified->pairs(); + const auto& plant = world.diagram->plant(); + const auto& scene_graph = world.diagram->scene_graph(); + auto root = world.diagram->CreateDefaultContext(); + const std::vector configs = + SampleTrajectory(shelf_trajectory, kInner); + // Accumulator so the optimizer cannot discard the timed queries. + double sink = 0.0; + const auto sweep = [&](int queries_per_config) { + return TimeRepeatedly(config.warmup, config.reps, [&]() { + for (int i = 0; i < kInner; ++i) { + plant.SetPositions(&world.diagram->mutable_plant_context(root.get()), + configs[i]); + const auto& query_object = + scene_graph.get_query_output_port() + .Eval>( + world.diagram->scene_graph_context(*root)); + for (int k = 0; k < queries_per_config; ++k) { + sink += oracle.SignedDistance( + query_object, + pairs[static_cast(i * kManyQueries + k) % pairs.size()]); + } + } + }); + }; + const TimingSummary one = sweep(1); + const TimingSummary many = sweep(kManyQueries); + const double us_per_query = + 1000.0 * (many.median_ms - one.median_ms) / (kInner * (kManyQueries - 1)); + const double us_per_config = 1000.0 * one.median_ms / kInner - us_per_query; + + const CertRun reference = + MeasureCertify(*world.certified, shelf_trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), + config.warmup, config.reps); + const double predicted_ms = + (static_cast(reference.stats.nodes) * us_per_config + + static_cast(reference.stats.narrowphase_queries) * + us_per_query) / + 1000.0; + + json.BeginObject("cost_attribution"); + json.Write("us_per_configuration_fk_and_pose_update", us_per_config); + json.Write("us_per_narrowphase_query", us_per_query); + json.Write("nodes", reference.stats.nodes); + json.Write("narrowphase_queries", reference.stats.narrowphase_queries); + json.Write("sphere_certifications", reference.stats.sphere_certifications); + json.Write("predicted_ms", predicted_ms); + json.Write("measured_ms", reference.timing.median_ms); + json.Write("residual_ms", reference.timing.median_ms - predicted_ms); + json.Write("residual_note", + "residual covers the sphere prefilter, the lambda/Delta sparse " + "dot products, de Casteljau splitting and driver bookkeeping"); + json.Write("summed_distances_m", sink); + json.EndObject(); + std::printf( + "[f profile] %.3f us / configuration, %.3f us / narrowphase " + "query\n", + us_per_config, us_per_query); + std::printf( + "[f profile] predicted %.3f ms vs measured %.3f ms " + "(residual %.3f ms)\n", + predicted_ms, reference.timing.median_ms, + reference.timing.median_ms - predicted_ms); + + // --- Per-segment work distribution --------------------------------------- + // This measures the ceiling that *segment-root seeding* imposes: if the + // parallel driver's only work units are whole segments, the best per-call + // speedup a 6-segment trajectory can reach is total work / heaviest segment. + // Certifying each segment on its own measures it directly. The driver no + // longer works that way — it shares sub-segment nodes on demand (see + // certifier.h) — so this row is now a *reference* bound + // that the measured per-call speedup is allowed to exceed, and the record of + // why the old driver could not. + { + const PiecewiseBezierPath path = world.certified->Normalize( + shelf_trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); + uint64_t total = 0; + uint64_t heaviest = 0; + double heaviest_ms = 0.0; + double serial_sum_ms = 0.0; + json.BeginArray("per_segment_work"); + for (size_t i = 0; i < path.segments().size(); ++i) { + const auto& segment = path.segments()[i]; + const drake::trajectories::BezierCurve curve( + segment.t_start, segment.t_end, segment.control_points); + const CertRun run = MeasureCertify( + *world.certified, curve, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), + config.warmup, config.reps); + total += run.stats.nodes; + heaviest = std::max(heaviest, run.stats.nodes); + heaviest_ms = std::max(heaviest_ms, run.timing.median_ms); + serial_sum_ms += run.timing.median_ms; + json.BeginObject(); + json.Write("segment", static_cast(i)); + json.Write("nodes", run.stats.nodes); + json.Write("narrowphase_queries", run.stats.narrowphase_queries); + WriteTiming(&json, "wall_ms", run.timing); + json.EndObject(); + } + json.EndArray(); + json.BeginObject("per_segment_summary"); + json.Write("total_nodes_over_segments", total); + json.Write("heaviest_segment_nodes", heaviest); + json.Write("segment_seeding_bound_on_per_call_speedup", + serial_sum_ms / std::max(heaviest_ms, 1e-9)); + json.Write("note", + "each segment certified on its own; the sum exceeds the " + "whole-trajectory node count only by the per-segment " + "breakpoint work. segment_seeding_bound is the ceiling a " + "driver seeded with whole segments would hit; the current " + "driver shares sub-segment nodes on demand and is not bound " + "by it"); + json.EndObject(); + std::printf( + "[f profile] heaviest segment = %llu of %llu nodes; " + "segment-seeding bound on per-call speedup = %.2fx\n", + static_cast(heaviest), // NOLINT(runtime/int) + static_cast(total), // NOLINT(runtime/int) + serial_sum_ms / std::max(heaviest_ms, 1e-9)); + } + + // --- Parallel granularity ------------------------------------------------- + // Per-call parallelism is measured on three workloads spanning three orders + // of magnitude in node count, holding everything else fixed. The three sit + // on either side of the driver's lazy-recruitment threshold on purpose: the + // 15-node edge is below it (and must therefore be exactly serial at every p) + // while the other two are above it. + std::printf( + "[f profile] tuning the grazing world for the long " + "workload ...\n"); + const double graze_scale = TuneShelfScale(config, 0.0); + World graze = MakeWorld(MakeShelfWorld(graze_scale), {"iiwa14"}); + const MatrixXd shelf_waypoints = ShelfTrajectoryWaypoints(); + const VectorXd q1 = shelf_waypoints.col(0); + const VectorXd q2 = shelf_waypoints.col(1); + + constexpr int kParallelism[] = {1, 2, 4, 8, 16}; + json.BeginArray("parallel_granularity"); + std::printf(" %-18s %5s %10s %10s %8s\n", "workload", "p", "nodes", "med_ms", + "speedup"); + for (const char* which : {"pwl_edge_15_nodes", "shelf_1cm_146_nodes", + "grazing_min_interval_1e-6"}) { + double baseline = 0.0; + for (const int p : kParallelism) { + CertRun run; + if (std::strcmp(which, "pwl_edge_15_nodes") == 0) { + run = MeasureCertifyEdge( + *world.certified, q1, q2, + MakeOptions(SearchMode::kCertifyAll, Parallelism(p)), config.warmup, + config.reps); + } else if (std::strcmp(which, "shelf_1cm_146_nodes") == 0) { + run = + MeasureCertify(*world.certified, shelf_trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism(p)), + config.warmup, config.reps); + } else { + run = MeasureCertify( + *graze.certified, shelf_trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism(p), 1e-6), + config.warmup, config.reps); + } + if (p == 1) baseline = run.timing.median_ms; + json.BeginObject(); + json.Write("workload", which); + json.Write("threads", p); + json.Write("nodes", run.stats.nodes); + json.Write("verdict", VerdictName(run.verdict)); + WriteTiming(&json, "wall_ms", run.timing); + json.Write("speedup", baseline / run.timing.median_ms); + json.EndObject(); + std::printf(" %-18s %5d %10llu %10.3f %8.2f\n", which, p, + static_cast( // NOLINT(runtime/int) + run.stats.nodes), + run.timing.median_ms, baseline / run.timing.median_ms); + } + } + json.EndArray(); + WriteMachine(&json, machine); + json.EndObject(); + WriteTextFile(config.out_dir + "/profile.json", json.str()); + std::printf("\n"); +} + +int Main(int argc, char** argv) { + Config config; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + const auto next = [&]() -> std::string { + if (i + 1 >= argc) throw std::runtime_error("missing value for " + arg); + return argv[++i]; + }; + if (arg == "--out") { + config.out_dir = next(); + } else if (arg == "--reps") { + config.reps = std::stoi(next()); + } else if (arg == "--warmup") { + config.warmup = std::stoi(next()); + } else if (arg == "--dense-samples") { + config.dense_samples = std::stoi(next()); + } else if (arg == "--tune-samples") { + config.tune_samples = std::stoi(next()); + } else if (arg == "--batch") { + config.batch = std::stoi(next()); + } else if (arg == "--only") { + config.only = next(); + } else if (arg == "--drake_commit") { + config.drake_commit = next(); + } else { + std::fprintf(stderr, "unknown argument: %s\n", arg.c_str()); + return 1; + } + } + const auto wanted = [&](const std::string& name) { + return config.only.empty() || config.only == name; + }; + + const MachineInfo machine = GetMachineInfo(config.drake_commit); + std::printf("certified_ccd benchmark suite\n"); + std::printf(" cpu : %s (%d logical cores)\n", + machine.cpu_model.c_str(), machine.core_count); + std::printf(" drake pin : %s (%s)\n", machine.drake_commit.c_str(), + machine.drake_version_note.c_str()); + std::printf(" model : %s\n", kIiwaUrl); + std::printf(" reps : %d timed after %d warmup\n", config.reps, + config.warmup); + std::printf(" output : %s\n\n", config.out_dir.c_str()); + + const MatrixXd shelf_waypoints = ShelfTrajectoryWaypoints(); + const auto shelf_trajectory = + MakeQuinticCompositeBezier(shelf_waypoints, ShelfTrajectoryTimes()); + + struct Tier { + const char* name; + const char* file; + double target; + bool headline; + }; + constexpr Tier kTiers[] = { + {"a shelf 2mm", "shelf_2mm", 0.002, false}, + {"a shelf 1cm", "shelf_1cm", 0.010, true}, + {"a shelf 5cm", "shelf_5cm", 0.050, false}, + }; + + for (const Tier& tier : kTiers) { + const bool need_headline_world = + tier.headline && (wanted("pwl") || wanted("threads")); + if (!wanted("shelf") && !need_headline_world) continue; + + std::printf("[%s] tuning shelf placement for %.0f mm ...\n", tier.name, + 1000.0 * tier.target); + const double scale = TuneShelfScale(config, tier.target); + World world = MakeWorld(MakeShelfWorld(scale), {"iiwa14"}); + const ClearanceReport clearance = MeasureSweptClearance( + *world.diagram, *shelf_trajectory, world.env_ids, config.dense_samples, + config.max_threads, kMaxProbeDistance); + std::printf( + "[%s] shelf_scale=%.6f clearance env=%.6f m all=%.6f m " + "pairs=%d\n", + tier.name, scale, clearance.min_env, clearance.min_all, + world.pair_count); + + if (wanted("shelf")) { + const CertRun serial_all = MeasureCertify( + *world.certified, *shelf_trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), + config.warmup, config.reps); + const CertRun serial_first = MeasureCertify( + *world.certified, *shelf_trajectory, + MakeOptions(SearchMode::kFindFirstViolation, Parallelism::None()), + config.warmup, config.reps); + const CertRun par8 = + MeasureCertify(*world.certified, *shelf_trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism(8)), + config.warmup, config.reps); + const CertRun par16 = + MeasureCertify(*world.certified, *shelf_trajectory, + MakeOptions(SearchMode::kCertifyAll, Parallelism(16)), + config.warmup, config.reps); + + PrintHeader(); + PrintRow("certify_all serial", serial_all); + PrintRow("find_first serial", serial_first); + PrintRow("certify_all 8 threads", par8); + PrintRow("certify_all 16 threads", par16); + std::printf("\n"); + + JsonWriter json; + json.BeginObject(); + json.Write("scenario", std::string("a_") + tier.file); + json.Write("description", + "iiwa14 (dense-sphere collision model) welded to the world, " + "seven-box bookcase plus a table slab; 6-segment quintic " + "composite Bezier reaching into the shelf bay and back"); + json.Write("model", kIiwaUrl); + json.Write("shelf_scale", scale); + json.Write("target_clearance_m", tier.target); + json.Write("pair_count", world.pair_count); + json.Write("scene_graph_collision_candidates", + world.scene_graph_candidates); + json.Write("num_positions", world.diagram->plant().num_positions()); + json.Write("trajectory_segments", + static_cast(shelf_waypoints.cols()) - 1); + json.Write("trajectory_degree", 5); + WriteClearance(&json, clearance); + WriteOptions(&json, + MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); + json.Write("verdict", VerdictName(serial_all.verdict)); + WriteStats(&json, serial_all.stats); + WriteTiming(&json, "wall_ms", serial_all.timing); + WriteCertRun(&json, "certify_all_serial", serial_all); + WriteCertRun(&json, "find_first_serial", serial_first); + WriteCertRun(&json, "certify_all_8_threads", par8); + WriteCertRun(&json, "certify_all_16_threads", par16); + if (tier.headline) { + MeasureSampledPathSweep(&json, *world.sampled, *shelf_trajectory, + config.warmup, config.reps); + } + WriteMachine(&json, machine); + json.EndObject(); + WriteTextFile(config.out_dir + "/" + tier.file + ".json", json.str()); + } + + if (tier.headline && wanted("pwl")) { + RunPwlEdge(config, machine, world, shelf_waypoints, scale); + } + if (tier.headline && wanted("threads")) { + RunThreadScaling(config, machine, world, shelf_waypoints, scale); + } + } + + if (wanted("dual")) RunDualArm(config, machine); + if (wanted("grazing")) RunGrazing(config, machine, *shelf_trajectory); + if (wanted("profile")) RunProfile(config, machine, *shelf_trajectory); + + std::printf("done; results in %s\n", config.out_dir.c_str()); + return 0; +} + +} // namespace +} // namespace benchmark +} // namespace certified_ccd +} // namespace planning +} // namespace drake + +int main(int argc, char** argv) { + try { + return drake::planning::certified_ccd::benchmark::Main(argc, argv); + } catch (const std::exception& e) { + std::fprintf(stderr, "benchmark failed: %s\n", e.what()); + return 1; + } +} diff --git a/planning/certified_ccd/benchmark/scenario_worlds.cc b/planning/certified_ccd/benchmark/scenario_worlds.cc new file mode 100644 index 000000000000..99669cffa3f0 --- /dev/null +++ b/planning/certified_ccd/benchmark/scenario_worlds.cc @@ -0,0 +1,182 @@ +#include "drake/planning/certified_ccd/benchmark/scenario_worlds.h" + +#include + +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/rotation_matrix.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/coulomb_friction.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/spatial_inertia.h" +#include "drake/planning/robot_diagram_builder.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace benchmark { +namespace { + +using drake::geometry::Box; +using drake::math::RigidTransformd; +using drake::math::RotationMatrixd; +using drake::multibody::CoulombFriction; +using drake::multibody::ModelInstanceIndex; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::multibody::RigidBody; +using drake::multibody::SpatialInertia; +using drake::planning::RobotDiagram; +using drake::planning::RobotDiagramBuilder; +using Eigen::MatrixXd; +using Eigen::Vector3d; + +CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +/// Adds one anchored box to the "environment" model instance. +void AddAnchoredBox(MultibodyPlant* plant, const std::string& name, + const Vector3d& size, const RigidTransformd& X_WB) { + if (!plant->HasModelInstanceNamed("environment")) { + plant->AddModelInstance("environment"); + } + const ModelInstanceIndex instance = + plant->GetModelInstanceByName("environment"); + const RigidBody& body = + plant->AddRigidBody(name, instance, + SpatialInertia::SolidBoxWithMass( + 1.0, size.x(), size.y(), size.z())); + plant->WeldFrames(plant->world_frame(), body.body_frame(), X_WB); + plant->RegisterCollisionGeometry(body, RigidTransformd(), + Box(size.x(), size.y(), size.z()), + name + "_geom", Friction()); +} + +void AddTable(MultibodyPlant* plant) { + AddAnchoredBox(plant, "table", Vector3d(3.0, 3.0, 0.10), + RigidTransformd(Vector3d(0.0, 0.0, -0.05))); +} + +/// The bookcase: two side panels, four horizontal boards (bottom, the two +/// bounding the reached-into bay, and top) and a back panel — seven anchored +/// boxes, all in the "environment" instance so they form one welded subgraph +/// with the world and with each other. +void AddShelf(MultibodyPlant* plant, double s) { + using S = ShelfGeometry; + const double x_front = S::kFrontX + s; + const double x_back = x_front + S::kDepth; + const double bay_h = S::kBayHalfHeight + s; + const double x_mid = 0.5 * (x_front + x_back); + const double z_mid = 0.5 * (S::kBottomZ + S::kTopZ); + const double height = S::kTopZ - S::kBottomZ; + const double width = 2.0 * S::kHalfWidth; + + AddAnchoredBox( + plant, "shelf_side_l", Vector3d(S::kDepth, S::kPanel, height), + RigidTransformd(Vector3d(x_mid, S::kHalfWidth + 0.5 * S::kPanel, z_mid))); + AddAnchoredBox(plant, "shelf_side_r", Vector3d(S::kDepth, S::kPanel, height), + RigidTransformd( + Vector3d(x_mid, -S::kHalfWidth - 0.5 * S::kPanel, z_mid))); + AddAnchoredBox(plant, "shelf_board_bottom", + Vector3d(S::kDepth, width, S::kPanel), + RigidTransformd(Vector3d(x_mid, 0.0, S::kBottomZ))); + AddAnchoredBox(plant, "shelf_board_low", + Vector3d(S::kDepth, width, S::kPanel), + RigidTransformd(Vector3d( + x_mid, 0.0, S::kBayCentreZ - bay_h - 0.5 * S::kPanel))); + AddAnchoredBox(plant, "shelf_board_high", + Vector3d(S::kDepth, width, S::kPanel), + RigidTransformd(Vector3d( + x_mid, 0.0, S::kBayCentreZ + bay_h + 0.5 * S::kPanel))); + AddAnchoredBox(plant, "shelf_board_top", + Vector3d(S::kDepth, width, S::kPanel), + RigidTransformd(Vector3d(x_mid, 0.0, S::kTopZ))); + AddAnchoredBox( + plant, "shelf_back", Vector3d(S::kPanel, width + 2.0 * S::kPanel, height), + RigidTransformd(Vector3d(x_back + 0.5 * S::kPanel, 0.0, z_mid))); +} + +} // namespace + +const char* const kIiwaUrl = + "package://drake_models/iiwa_description/urdf/" + "iiwa14_spheres_dense_collision.urdf"; + +std::shared_ptr> MakeShelfWorld(double shelf_scale) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + builder.parser().AddModelsFromUrl(kIiwaUrl); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), + RigidTransformd()); + AddTable(&plant); + AddShelf(&plant, shelf_scale); + plant.Finalize(); + return std::shared_ptr>(builder.Build()); +} + +std::shared_ptr> MakeDualArmWorld(double base_separation) { + RobotDiagramBuilder builder(0.0); + MultibodyPlant& plant = builder.plant(); + builder.parser().SetAutoRenaming(true); + const ModelInstanceIndex arm_a = + builder.parser().AddModelsFromUrl(kIiwaUrl).at(0); + const ModelInstanceIndex arm_b = + builder.parser().AddModelsFromUrl(kIiwaUrl).at(0); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base", arm_a), + RigidTransformd()); + plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base", arm_b), + RigidTransformd(RotationMatrixd::MakeZRotation(M_PI), + Vector3d(base_separation, 0.0, 0.0))); + AddTable(&plant); + plant.Finalize(); + return std::shared_ptr>(builder.Build()); +} + +MatrixXd ShelfTrajectoryWaypoints() { + MatrixXd w(7, 7); + // clang-format off + w << 0.0, 0.196552, 0.096805, -0.016522, -0.138992, -0.254084, 0.0, + 0.0, -0.219376, 0.213452, 0.715028, 0.250007, -0.270004, 0.0, + 0.0, 0.220295, 0.105212, 0.034537, -0.061201, -0.171787, 0.0, + 0.0, -1.546472, -1.471150, -0.931648, -1.577882, -1.679656, 0.0, + 0.0, 0.035924, -0.011122, -0.017085, -0.005269, -0.019359, 0.0, + 0.0, 0.740798, 0.465992, 0.214658, 0.321424, 0.426071, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + // clang-format on + return w; +} + +std::vector ShelfTrajectoryTimes() { + return {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0}; +} + +MatrixXd DualArmTrajectoryWaypoints() { + Eigen::VectorXd reach(14); + // Arm A and arm B reach poses: both extend forward at ~0.49 m with a small + // base yaw so the wrists pass each other offset in y and z. + reach << 0.12, 0.0, 0.0, -1.40, 0.0, 1.10, 0.0, -0.12, 0.0, 0.0, -1.70, 0.0, + 0.80, 0.0; + Eigen::VectorXd offset = Eigen::VectorXd::Zero(14); + offset(0) = 0.10; + offset(3) = 0.08; + offset(7) = -0.10; + offset(10) = 0.08; + + MatrixXd w(14, 5); + w.col(0) = Eigen::VectorXd::Zero(14); + w.col(1) = 0.5 * reach; + w.col(2) = reach; + w.col(3) = 0.5 * reach + offset; + w.col(4) = Eigen::VectorXd::Zero(14); + return w; +} + +std::vector DualArmTrajectoryTimes() { + return {0.0, 1.0, 2.0, 3.0, 4.0}; +} + +} // namespace benchmark +} // namespace certified_ccd +} // namespace planning +} // namespace drake diff --git a/planning/certified_ccd/benchmark/scenario_worlds.h b/planning/certified_ccd/benchmark/scenario_worlds.h new file mode 100644 index 000000000000..fac65febe811 --- /dev/null +++ b/planning/certified_ccd/benchmark/scenario_worlds.h @@ -0,0 +1,74 @@ +#pragma once + +/// @file +/// The fixed, versioned benchmark worlds and trajectories (the benchmark +/// suite). Every world is built from the cached `drake_models` iiwa14 +/// dense-sphere collision model plus programmatic anchored boxes, so a run is +/// reproducible from this file alone. + +#include +#include +#include + +#include + +#include "drake/planning/robot_diagram.h" + +namespace drake { +namespace planning { +namespace certified_ccd { +namespace benchmark { + +/// The dense-sphere iiwa14 collision variant: 46 collision spheres over +/// links 0-7, i.e. realistic proximity-pair counts (the benchmark suite asks +/// for the realistic model, not the 4-primitive one). +extern const char* const kIiwaUrl; + +/// Nominal shelf geometry. `shelf_scale` s translates the whole bookcase by +/// +s in x *and* opens the reached-into bay by s on each side, so the swept +/// clearance of the fixed benchmark trajectory is monotone non-decreasing in +/// s over the useful range. This one scalar is what the tier bisection turns. +struct ShelfGeometry { + static constexpr double kBayCentreZ = 0.60; + static constexpr double kFrontX = 0.62; + static constexpr double kDepth = 0.32; + static constexpr double kBayHalfHeight = 0.13; + static constexpr double kPanel = 0.03; + static constexpr double kHalfWidth = 0.45; + static constexpr double kBottomZ = 0.05; + static constexpr double kTopZ = 1.25; +}; + +/// iiwa14 welded to the world origin, a 3 m table slab, and a seven-box +/// bookcase in reach. Model instances are named "iiwa14" and "environment". +std::shared_ptr> MakeShelfWorld( + double shelf_scale); + +/// Two iiwa14s welded to the world `base_separation` apart along +x, the +/// second rotated 180 degrees about z so the arms face each other, over the +/// same table slab. Model instances: "iiwa14", "iiwa14_1", "environment". +std::shared_ptr> MakeDualArmWorld( + double base_separation); + +/// The 7 x 7 joint-space waypoint matrix of the shelf-reaching trajectory: +/// home, up-and-over on the +y side, into the bay mouth, deep inside the bay, +/// out on the -y side, and home. Solved once offline with +/// drake::multibody::InverseKinematics (position + tool-axis + minimum- +/// distance constraints) against the shelf-free world, then frozen here so +/// the benchmark has no solver dependency and no run-to-run drift. +Eigen::MatrixXd ShelfTrajectoryWaypoints(); + +/// Times of the shelf waypoints (0, 1, ..., 6): 6 quintic Bézier segments. +std::vector ShelfTrajectoryTimes(); + +/// The 14 x 5 waypoint matrix of the dual-arm handover: both arms home, half +/// way, at the handover poses (end-effectors passing within a few cm), a +/// slightly different half-way pose on the way back, and home. +Eigen::MatrixXd DualArmTrajectoryWaypoints(); + +std::vector DualArmTrajectoryTimes(); + +} // namespace benchmark +} // namespace certified_ccd +} // namespace planning +} // namespace drake From efe7077b5630ab3b23bb5acae5fc195e8b70d13e Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 14:45:16 -0400 Subject: [PATCH 07/22] [planning] certified_ccd: reject negative effective thresholds The displacement lemma argues entirely in the separated regime, so the certificate is meaningless when margin + padding is negative for a pair. Rather than returning a verdict outside the proven regime, CheckEdge and CheckTrajectory now throw a message naming both bodies and pointing the caller at collision filtering, which is the sound way to exempt a pair that is meant to touch. --- .../certified_continuous_collision_checker.cc | 14 ++++++++++++++ planning/certified_ccd/test/api_test.cc | 17 +++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/planning/certified_ccd/certified_continuous_collision_checker.cc b/planning/certified_ccd/certified_continuous_collision_checker.cc index 3cd5e9293643..97e062e9f2a0 100644 --- a/planning/certified_ccd/certified_continuous_collision_checker.cc +++ b/planning/certified_ccd/certified_continuous_collision_checker.cc @@ -428,6 +428,20 @@ class CertifiedContinuousCollisionChecker::Impl { for (int p = 0; p < static_cast(pairs.size()); ++p) { pairs[p].threshold = options.margin + padding_[p]; tau[p] = std::max(options.query_tolerance, tau_base_[p]); + // The displacement lemma argues entirely in the separated regime, so + // the certificate is meaningless for a negative effective threshold: a + // pair meant to touch must be collision-filtered, not padded below + // zero. Negative padding is therefore rejected rather than certified. + if (pairs[p].threshold < 0.0) { + throw std::runtime_error(fmt::format( + "CertifiedContinuousCollisionChecker: margin ({}) + padding ({}) " + "is negative for the pair on bodies {} and {}. The certificate " + "is only proven for nonnegative thresholds; filter the pair out " + "instead of using negative padding.", + options.margin, padding_[p], + model_->plant().get_body(pairs[p].id.body_a).name(), + model_->plant().get_body(pairs[p].id.body_b).name())); + } } internal::CertifierInput input; diff --git a/planning/certified_ccd/test/api_test.cc b/planning/certified_ccd/test/api_test.cc index 01973aede772..59cfe53ff52c 100644 --- a/planning/certified_ccd/test/api_test.cc +++ b/planning/certified_ccd/test/api_test.cc @@ -370,6 +370,23 @@ GTEST_TEST(ApiTest, DeformableGeometryIsRefusedNamingIt) { // 3. Dimensions (trajectory normalization; the architecture). // --------------------------------------------------------------------------- +// The displacement lemma is proved in the separated regime only, so a +// negative effective threshold (margin + padding < 0) is outside what the +// checker can certify and must be rejected, not silently "certified". +GTEST_TEST(ApiTest, NegativeEffectiveThresholdIsRejected) { + const auto checker = MakeChecker(MakeArmWorld()); + Options options; + options.parallelism = Parallelism::None(); + options.margin = -0.01; + const VectorXd q0 = VectorXd::Zero(2); + const VectorXd q1 = VectorXd::Constant(2, 0.1); + const std::string message = ThrowMessage([&]() { + checker->CheckEdge(q0, q1, options); + }); + ExpectContains(message, "negative"); + ExpectContains(message, "filter the pair"); +} + GTEST_TEST(ApiTest, DimensionMismatchMessagesNameTheSizes) { std::shared_ptr> model = MakeArmWorld(); const auto checker = MakeChecker(model); From 39801a136955bf7607ec30486072f19c00243555 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Wed, 26 Aug 2026 15:41:58 -0400 Subject: [PATCH 08/22] [planning] certified_ccd: charge carved-coordinate residual motion The constant-coordinate carve-out drops a coordinate from J(p) when its whole control-point range fits inside Options::continuity_tolerance. That is a tolerance, not an identity, so the dropped coordinate could still displace the pair by up to lambda-tilde * range -- small, but two orders of magnitude above Options::certificate_slack and charged nowhere, which let the certificate inequality pass with the true clearance below the threshold by that much. MotionBoundTable now carries a per-pair carveout_slack() that MotionBound() adds unconditionally, restoring the telescoping sum in full. It is bit- exactly zero whenever every carved coordinate is exactly constant, which is every path whose control points repeat the coordinate's value verbatim. The per-coordinate lambda-tilde reproduces the existing lambda for the supported joint kinds and extends to the kinds the carve-out admits but the table cannot otherwise bound: r for rpy/ball/universal rotations, 1 for floating-base translations, and a proved 2r/m for quaternion coefficients, with the Lipschitz and double-cover derivation recorded in the source. A HalfSpace across a rotational carved coordinate has no finite reach and so must now be exactly constant; anything else throws. The breakpoint/static-pair path in certifier.cc and the certificate replay in certificate.cc charge the same slack instead of a literal zero. --- .../certified_ccd/benchmark/iiwa_benchmark.cc | 14 +- planning/certified_ccd/certificate.cc | 17 +- planning/certified_ccd/certifier.cc | 29 +- planning/certified_ccd/motion_bound_table.cc | 214 +++++- planning/certified_ccd/motion_bound_table.h | 78 ++- planning/certified_ccd/test/api_test.cc | 80 +++ .../certified_ccd/test/concurrency_test.cc | 5 + .../certified_ccd/test/motion_bound_test.cc | 620 +++++++++++++++++- 8 files changed, 1010 insertions(+), 47 deletions(-) diff --git a/planning/certified_ccd/benchmark/iiwa_benchmark.cc b/planning/certified_ccd/benchmark/iiwa_benchmark.cc index 37316b29fa95..92be93d2893a 100644 --- a/planning/certified_ccd/benchmark/iiwa_benchmark.cc +++ b/planning/certified_ccd/benchmark/iiwa_benchmark.cc @@ -439,9 +439,19 @@ void RunPwlEdge(const Config& config, const MachineInfo& machine, JsonWriter json; json.BeginObject(); json.Write("scenario", "b_pwl_edge"); + // The certified object and the ground-truth object are not the same + // trajectory, only the same point set: CheckEdge certifies a single order-1 + // Bezier segment, while the clearance written below is measured on the + // quintic composite Bezier through the same two waypoints. With two + // waypoints that quintic's endpoint velocities are zero, so its control + // points collapse to {q1, q1, q1, q2, q2, q2} and it traces exactly the same + // straight joint-space segment under a different time parametrization — + // which is why the clearance it measures is the certified edge's clearance. json.Write("description", - "two-waypoint PWL edge (a single order-1 Bezier segment) in the " - "1 cm shelf world, healthy clearance"); + "two-waypoint PWL edge in the 1 cm shelf world, healthy " + "clearance; certified as a single order-1 Bezier segment, with " + "the clearance ground truth measured on a quintic composite " + "Bezier tracing the same joint-space point set"); json.Write("model", kIiwaUrl); json.Write("shelf_scale", scale); json.Write("pair_count", world.pair_count); diff --git a/planning/certified_ccd/certificate.cc b/planning/certified_ccd/certificate.cc index 78f6725ea1f7..136853f3a8f2 100644 --- a/planning/certified_ccd/certificate.cc +++ b/planning/certified_ccd/certificate.cc @@ -189,7 +189,13 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, } const bool is_static = table.pair_is_static(p); - double motion_bound = 0.0; + // A static pair's J(p) is empty, so MotionBound() would return exactly the + // carve-out slack for any w: the residual of the coordinates the carve-out + // removed, which is nonzero only when some of them are constant merely to + // within Options::continuity_tolerance. Charging it here keeps the replay's + // Δ at least as large as the certifier's — a certificate emitted against a + // slack-inflated bound must not verify against a smaller one. + double motion_bound = table.carveout_slack(p); if (!is_static) { // Re-restrict the segment's control points to the record's interval and // recompute w about the record's qc from scratch. This is the half of @@ -231,10 +237,11 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, } } // For a static pair J(p) = ∅: no coordinate the trajectory *moves* changes - // the pair's relative pose, so Δ_p ≡ 0 and one measurement certifies the - // whole domain. A *non*-static pair cannot smuggle in such a record: the - // recomputed Δ above would be the full node's bound and the test below - // would reject it. + // the pair's relative pose, so Δ_p is the constant carve-out slack (0 in + // every case but a tolerance-constant coordinate) and one measurement + // certifies the whole domain. A *non*-static pair cannot smuggle in such a + // record: the recomputed Δ above would be the full node's bound and the + // test below would reject it. // // "Static" is relative to the constant-coordinate carve-out (trajectory // normalization; the displacement lemma), so coordinates this path happens diff --git a/planning/certified_ccd/certifier.cc b/planning/certified_ccd/certifier.cc index ff042cd55773..b51508d20e61 100644 --- a/planning/certified_ccd/certifier.cc +++ b/planning/certified_ccd/certifier.cc @@ -705,6 +705,14 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, const double threshold = pair.threshold; 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 — "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 duplicate its finding (crowding out genuine ones @@ -721,14 +729,15 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, } if (is_static) { - // Δ_p ≡ 0 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, 0.0, threshold, slack)) { + // Δ_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, slack)) { ++stats->sphere_certifications; if (records != nullptr) { for (int k = 0; k < num_segments; ++k) { records->push_back(CertificateRecord{k, 0.0, 1.0, p, q, lower_bound, - 0.0, threshold}); + static_bound, threshold}); } } continue; @@ -758,24 +767,24 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, } if (!is_static) continue; - if (IsCertified(phi_hat, tau_p, 0.0, threshold, slack)) { + if (IsCertified(phi_hat, tau_p, static_bound, threshold, slack)) { if (records != nullptr) { for (int k = 0; k < num_segments; ++k) { - records->push_back( - CertificateRecord{k, 0.0, 1.0, p, q, phi_hat, 0.0, threshold}); + records->push_back(CertificateRecord{k, 0.0, 1.0, p, q, phi_hat, + static_bound, threshold}); } } continue; } // Neither certified nor violating, and no subdivision can help: this - // pair's clearance is constant along the trajectory and sits within oracle - // tolerance of the threshold. + // pair's clearance is constant along the trajectory (up to the carve-out + // residual) and sits within oracle tolerance of the threshold. Finding finding; finding.time = time; finding.q = q; finding.pair = pair.id; finding.distance = phi_hat; - finding.motion_bound = 0.0; + finding.motion_bound = static_bound; finding.definite = false; finding.nearest_a_W = nearest_a; finding.nearest_b_W = nearest_b; diff --git a/planning/certified_ccd/motion_bound_table.cc b/planning/certified_ccd/motion_bound_table.cc index 7e60a5a10c0e..814b37dfb04f 100644 --- a/planning/certified_ccd/motion_bound_table.cc +++ b/planning/certified_ccd/motion_bound_table.cc @@ -100,46 +100,69 @@ void KinematicsEngine::BuildTopology() { 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(). + // 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_THROW_UNLESS(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 @@ -484,6 +507,12 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( 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 Options::continuity_tolerance — a tolerance, + // not an identity. `range` is exactly 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 @@ -552,6 +581,32 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( DRAKE_THROW_UNLESS(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. // @@ -590,14 +645,99 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // // Only the separated branch of the distance function is ever used (the // soundness argument), so no penetration-depth regularity is needed. + // + // ------------------------------------------------------------------ + // The carve-out residual (carveout_slack_p). + // + // The constant-coordinate carve-out (trajectory normalization; the + // joint-support scope) drops coordinate j from J(p) when its *whole* + // control-point range fits inside Options::continuity_tolerance. 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 — small (≈ 1e-7 m for a metre-scale reach), but two orders of + // magnitude above Options::certificate_slack and unaccounted anywhere, so the + // certificate inequality could pass with the true clearance below threshold + // by that much. Instead of ignoring the step we charge it at its worst case, + // 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, + // and that is the case for every path whose control points repeat the + // coordinate's value verbatim — the overwhelmingly common way a coordinate + // becomes constant. λ̃_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| — 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 uses 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 actually produces — a box of diameter + // ρ ≤ continuity_tolerance around a unit quaternion — m ≥ 1 − ρ, so + // 2r/m ≤ 2r/(1 − ρ) ≤ 4r for any ρ ≤ 1/2: the shipped 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 — 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 λ̃. Such a coordinate must be + // *exactly* constant; anything else throws (the geometry-support scope). + // Accepting a merely tolerance-constant one, as the code did before the + // residual was charged, is the one case where the residual is genuinely + // unbounded. // ------------------------------------------------------------------ MotionBoundTable table; std::vector& row_start = table.mutable_row_start(); std::vector& coord = table.mutable_coord(); std::vector& lambda = table.mutable_lambda(); + std::vector& carveout_slack = table.mutable_carveout_slack(); row_start.clear(); row_start.reserve(pairs.size() + 1); row_start.push_back(0); + carveout_slack.clear(); + 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. @@ -622,11 +762,12 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( "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(p). + if (in_a == in_b) continue; // j ∉ J_topo(p). const BodyIndex distal = in_a ? a : b; const int ps = rec.position_start; @@ -648,7 +789,71 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( }; for (int c = ps; c < ps + rec.num_positions; ++c) { - if (constant_coordinates[c]) continue; // Joint-support carve-out. + 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_THROW_UNLESS(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( + "certified_ccd: 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( + "certified_ccd: 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 λ (the geometry-support scope): 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) / kTwoPi; + break; + case CoordRule::kQuaternion: { + const double m = quat_min_norm[k]; + if (!(m > 0.0)) { + throw std::runtime_error(fmt::format( + "certified_ccd: 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_THROW_UNLESS(std::isfinite(lam_tilde) && lam_tilde >= 0.0); + slack += lam_tilde * span; + continue; + } double lam = 0.0; switch (rec.kind) { case JointKind::kRevolute: @@ -676,8 +881,11 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( lambda.push_back(lam); } } + DRAKE_THROW_UNLESS(std::isfinite(slack) && slack >= 0.0); + carveout_slack.push_back(slack); row_start.push_back(static_cast(coord.size())); } + DRAKE_THROW_UNLESS(carveout_slack.size() + 1 == row_start.size()); return table; } diff --git a/planning/certified_ccd/motion_bound_table.h b/planning/certified_ccd/motion_bound_table.h index 675d86e253d0..d461a4da5c03 100644 --- a/planning/certified_ccd/motion_bound_table.h +++ b/planning/certified_ccd/motion_bound_table.h @@ -25,28 +25,52 @@ 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. λ has units of meters of worst-case point 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. */ +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 (trajectory normalization; the +joint-support scope) removed from J(p). "Constant" there is a *tolerance* — a +coordinate whose global control-box range is at most +Options::continuity_tolerance — not an identity, so a carved coordinate may +still displace the pair's distal side by up to λ̃_j · range_j. That residual is +charged unconditionally inside MotionBound(), which is what makes Δ_p a true +upper bound on the pair's relative motion over the whole trajectory rather than +one that ignores the carved coordinates. It is exactly zero — bit for bit — +whenever every carved coordinate is *exactly* constant, which is the case for +every path whose control points repeat a coordinate's value verbatim. */ class MotionBoundTable { public: int num_pairs() const { return static_cast(row_start_.size()) - 1; } - /** True iff J(p) is empty after the constant-coordinate carve-out: the - trajectory cannot change this pair's status, so it is checked once. */ + /** 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. */ bool pair_is_static(int pair_index) const { return row_start_[pair_index] == row_start_[pair_index + 1]; } - /** Δ_p(ν) = Σ_{j ∈ J(p)} λ(j,p) · w_j — a sparse dot product against the - node's per-coordinate deviations w (the interval certificate, requirement P3). -*/ + /** Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j — a sparse dot + product against the node's per-coordinate deviations w, plus the carved + coordinates' residual (the interval certificate, requirement P3). */ double MotionBound(int pair_index, const Eigen::VectorXd& w) const { - double delta = 0.0; + double delta = carveout_slack_[pair_index]; for (int e = row_start_[pair_index]; e < row_start_[pair_index + 1]; ++e) { delta += lambda_[e] * w[coord_[e]]; } return delta; } + /** Σ 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. Zero when every carved coordinate is + exactly constant. */ + 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. */ std::vector> entries(int pair_index) const; @@ -58,11 +82,13 @@ class MotionBoundTable { std::vector& mutable_row_start() { return row_start_; } std::vector& mutable_coord() { return coord_; } std::vector& mutable_lambda() { return lambda_; } + std::vector& mutable_carveout_slack() { return carveout_slack_; } private: std::vector row_start_{0}; std::vector coord_; std::vector lambda_; + std::vector carveout_slack_; }; /** Construction-time kinematic analysis of a plant (the displacement lemma): @@ -107,7 +133,8 @@ class KinematicsEngine { /** 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; the displacement lemma). Coordinates flagged constant - by the path are removed from every J(p). + 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 naming the joint if the path moves a coordinate of an unsupported joint type (quaternion floating, ball). */ MotionBoundTable ComputeMotionBoundTable( @@ -116,10 +143,17 @@ class KinematicsEngine { /** 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(). + 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, or a - pair whose distal side carries a HalfSpace across a rotational coordinate. */ + 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, @@ -166,6 +200,23 @@ class KinematicsEngine { 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 { drake::multibody::JointIndex index; @@ -193,6 +244,11 @@ class KinematicsEngine { /* 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; diff --git a/planning/certified_ccd/test/api_test.cc b/planning/certified_ccd/test/api_test.cc index 59cfe53ff52c..a4cba4c6ba5f 100644 --- a/planning/certified_ccd/test/api_test.cc +++ b/planning/certified_ccd/test/api_test.cc @@ -255,9 +255,89 @@ GTEST_TEST(ApiTest, ConstantQuaternionBaseIsAcceptedEndToEnd) { checker->CheckTrajectory(trajectory, options); EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); ASSERT_TRUE(result.certificate.has_value()); + // Every base control point here is bit-identical, so the carve-out is exact + // and owes no residual at all. + const MotionBoundTable table = checker->ComputeMotionBounds(path); + for (int p = 0; p < table.num_pairs(); ++p) { + EXPECT_EQ(table.carveout_slack(p), 0.0) << "pair " << p; + } EXPECT_TRUE(VerifyCertificate(*checker, path, *result.certificate)); } +GTEST_TEST(ApiTest, ToleranceConstantQuaternionBaseChargesItsResidualEndToEnd) { + // The carve-out flags a coordinate constant on a *tolerance*, so a base held + // only to within continuity_tolerance is carved even though it still moves. + // Its residual is charged to MotionBoundTable::carveout_slack(), and that has + // to survive all the way through the certifier — including the static-pair + // shortcut, which never evaluates a per-node Δ — and the certificate replay, + // which recomputes Δ from scratch and would reject a record whose bound came + // out smaller than the one the certifier used. + std::shared_ptr> model = MakeFloatingBaseWorld(); + const auto checker = MakeChecker(model); + + Options options; + options.parallelism = Parallelism::None(); + options.emit_certificate = true; + + 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); + } + // A sub-tolerance wobble in the base's y position: still "constant" to the + // curve module, but no longer exactly so. + constexpr double kWobble = 6e-8; + ASSERT_LE(kWobble, options.continuity_tolerance); + points(5, 1) += kWobble; + points(7, 1) = 0.35; // ... and the elbow still moves. + points(7, 2) = 0.70; + const BezierCurve trajectory(0.0, 1.0, points); + + const PiecewiseBezierPath path = checker->Normalize(trajectory, options); + const std::vector& constant = path.constant_coordinates(); + ASSERT_EQ(constant.size(), 8u); + for (int i = 0; i < 7; ++i) { + EXPECT_TRUE(constant[i]) << "base coordinate " << i; + } + EXPECT_FALSE(constant[7]); + // The control-point range of the wobbled coordinate: `kWobble` up to the + // rounding of adding it to -0.10 and subtracting again. + const double range = + path.global_upper_bound()[5] - path.global_lower_bound()[5]; + EXPECT_GT(range, 0.0); + EXPECT_NEAR(range, kWobble, 1e-9 * kWobble); + + const MotionBoundTable table = checker->ComputeMotionBounds(path); + bool any_slack = false; + bool any_static_with_slack = false; + for (int p = 0; p < table.num_pairs(); ++p) { + if (table.carveout_slack(p) > 0.0) { + any_slack = true; + if (table.pair_is_static(p)) any_static_with_slack = true; + // λ̃ = 1 for a floating base's translation coordinates, and only that one + // coordinate has a width, so the residual is exactly that width. + EXPECT_DOUBLE_EQ(table.carveout_slack(p), range) << "pair " << p; + } + } + EXPECT_TRUE(any_slack); + EXPECT_TRUE(any_static_with_slack) + << "the base-vs-post pair depends only on the carved base coordinates, " + "so it is static and must still owe the residual"; + + const CertificationResult result = + checker->CheckTrajectory(trajectory, options); + EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); + ASSERT_TRUE(result.certificate.has_value()); + // The replay recomputes Δ through MotionBound(), so it charges the residual + // too: a certificate emitted against the inflated bound verifies, and one + // emitted against a smaller bound would not. + EXPECT_TRUE(VerifyCertificate(*checker, path, *result.certificate)); + for (const CertificateRecord& record : result.certificate->records) { + if (table.pair_is_static(record.pair_index)) { + EXPECT_GE(record.motion_bound, table.carveout_slack(record.pair_index)); + } + } +} + // --------------------------------------------------------------------------- // 2. Geometry scope (the geometry-support scope): rotating half spaces and // deformables. diff --git a/planning/certified_ccd/test/concurrency_test.cc b/planning/certified_ccd/test/concurrency_test.cc index aa616a717784..4cde52a7280e 100644 --- a/planning/certified_ccd/test/concurrency_test.cc +++ b/planning/certified_ccd/test/concurrency_test.cc @@ -553,8 +553,10 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { // Snapshot every λ entry, not just the CSR's size: the row layout is fixed by // topology and would survive any amount of corruption in the coefficients. std::vector>> lambda_expected; + std::vector slack_expected; for (int p = 0; p < table_expected.num_pairs(); ++p) { lambda_expected.push_back(table_expected.entries(p)); + slack_expected.push_back(table_expected.carveout_slack(p)); } const auto same_result = [](const CertificationResult& a, @@ -590,6 +592,9 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { } for (int p = 0; p < table.num_pairs(); ++p) { if (table.entries(p) != lambda_expected[p]) ++mismatches[t]; + // The carve-out residual is part of Δ_p, so it has to be + // bit-identical across threads too. + if (table.carveout_slack(p) != slack_expected[p]) ++mismatches[t]; } } }); diff --git a/planning/certified_ccd/test/motion_bound_test.cc b/planning/certified_ccd/test/motion_bound_test.cc index 1c9fb2bf1966..6ddf5bfbd526 100644 --- a/planning/certified_ccd/test/motion_bound_test.cc +++ b/planning/certified_ccd/test/motion_bound_test.cc @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -48,10 +49,12 @@ #include "drake/math/rigid_transform.h" #include "drake/math/rotation_matrix.h" #include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/ball_rpy_joint.h" #include "drake/multibody/tree/planar_joint.h" #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/quaternion_floating_joint.h" #include "drake/multibody/tree/revolute_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/certified_ccd/motion_bound_table.h" @@ -94,6 +97,12 @@ using Rng = std::mt19937_64; kinematics and in our own accumulation (both ~1e-15 at these magnitudes). */ constexpr double kSlack = 1e-9; +/* Options::continuity_tolerance's default — the width below which the curve + module flags a coordinate constant and the carve-out removes it from every + J(p). A coordinate carved on that *tolerance* can still move by up to this + much, which is what MotionBoundTable::carveout_slack() charges for. */ +constexpr double kContinuityTolerance = 1e-7; + // --------------------------------------------------------------------------- // Small random utilities (seeded, deterministic). // --------------------------------------------------------------------------- @@ -519,10 +528,14 @@ GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { ASSERT_EQ(table.entries(0).size(), 1); EXPECT_EQ(table.entries(0)[0].first, 1); } - { // All constant: the pair becomes static and its motion bound is zero. + { // 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 = engine.ComputeMotionBoundTable( - lower, upper, std::vector(nq, true), pairs); + pinned, pinned, std::vector(nq, true), pairs); 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); } } @@ -970,6 +983,11 @@ struct LemmaStats { 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) { @@ -978,8 +996,21 @@ struct LemmaStats { } }; +/* 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, bool use_constant_coords, +void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, LemmaStats* stats) { const RobotDiagram& diagram = *world.diagram; const MultibodyPlant& plant = diagram.plant(); @@ -1000,11 +1031,21 @@ void CheckWorld(Rng* rng, const RandomWorld& world, bool use_constant_coords, 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 = - use_constant_coords && Uniform(rng, 0.0, 1.0) < 0.3; + carve_out != CarveOut::kNone && Uniform(rng, 0.0, 1.0) < 0.3; constant[c] = is_constant; - const double half = is_constant ? 0.0 - : (angular[c] ? Uniform(rng, 0.05, 1.2) - : Uniform(rng, 0.02, 0.4)); + double half; + if (is_constant) { + // 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. + 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; } @@ -1032,6 +1073,19 @@ void CheckWorld(Rng* rng, const RandomWorld& world, bool use_constant_coords, } 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) { @@ -1090,15 +1144,17 @@ void CheckWorld(Rng* rng, const RandomWorld& world, bool use_constant_coords, } if (table.pair_is_static(k)) { - // A static pair must not move at all under any q, q' in the box. + // A static pair may move only by the carve-out residual — exactly zero + // when every carved coordinate is exactly constant. Matrix3Xd before(3, pts_b.cols()); Matrix3Xd after(3, pts_b.cols()); plant.SetPositions(&ctx, q); plant.CalcPointsPositions(ctx, frame_b, pts_b, frame_a, &before); plant.SetPositions(&ctx, qp); plant.CalcPointsPositions(ctx, frame_b, pts_b, frame_a, &after); - ASSERT_LE((after - before).colwise().norm().maxCoeff(), kSlack) - << "pair " << k << " has empty J(p) but its relative pose moved"; + ASSERT_LE((after - before).colwise().norm().maxCoeff(), bound + kSlack) + << "pair " << k << " has empty J(p) but its relative pose moved by " + << "more than its carve-out slack " << bound; continue; } @@ -1156,12 +1212,17 @@ GTEST_TEST(DisplacementLemmaTest, RandomPlants) { constexpr int kNumPlants = 1500; for (int trial = 0; trial < kNumPlants; ++trial) { SCOPED_TRACE(fmt::format("random plant #{}", trial)); - // Screw joints in every third world; constant-coordinate carve-outs in - // every other world. + // Screw joints in every third world. The carve-out cycles through its + // three regimes so that the exactly-constant case and the (load-bearing) + // sub-tolerance case each get ~500 plants: the latter is the one where the + // carved coordinates still move and carveout_slack() has to pay for them. const RandomWorld world = MakeRandomWorld(&rng, /* allow_screw = */ trial % 3 == 0, 128); stats.screw_joints += world.num_screw_joints; - CheckWorld(&rng, world, /* use_constant_coords = */ trial % 2 == 1, &stats); + 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. @@ -1178,11 +1239,16 @@ GTEST_TEST(DisplacementLemmaTest, RandomPlants) { // 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 be testing 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}", + "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.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 @@ -1211,7 +1277,7 @@ GTEST_TEST(DisplacementLemmaTest, ScrewChain) { &world); } world.diagram = builder.Build(); - CheckWorld(&rng, world, /* use_constant_coords = */ false, &stats); + CheckWorld(&rng, world, CarveOut::kNone, &stats); if (HasFatalFailure()) return; } EXPECT_GE(stats.plants, 75); @@ -1222,6 +1288,528 @@ GTEST_TEST(DisplacementLemmaTest, ScrewChain) { stats.max_tightness); } +// --------------------------------------------------------------------------- +// Part 3 — the constant-coordinate carve-out's residual. +// +// The curve module flags a coordinate constant when its whole control-point +// range fits inside Options::continuity_tolerance. 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. If that +// residual went uncharged the certificate inequality could pass with the true +// clearance below threshold by ~1e-7 m — two orders of magnitude above +// Options::certificate_slack. MotionBoundTable::carveout_slack() is what pays +// for it, and these tests are what pin it. +// --------------------------------------------------------------------------- + +/* world --j_rot(revolute, ẑ)--> l1 --j_slide(prismatic, x̂)--> l2, with a + sphere on the world and one offset out along l2. */ +std::unique_ptr> MakeCarveOutChain() { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& l1 = plant.AddRigidBody("l1", UnitInertia()); + const auto& l2 = plant.AddRigidBody("l2", UnitInertia()); + plant.AddJoint("j_rot", plant.world_body(), {}, l1, + RigidTransform(Vector3d(-0.2, 0, 0)), + Vector3d::UnitZ()); + plant.AddJoint("j_slide", l1, + RigidTransform(Vector3d(0.15, 0, 0)), + l2, {}, Vector3d::UnitX()); + const CoulombFriction mu(1.0, 1.0); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransform::Identity(), + Sphere(0.1), "g_world", mu); + plant.RegisterCollisionGeometry(l2, + RigidTransform(Vector3d(0.3, 0, 0)), + Sphere(0.05), "g_tip", mu); + return builder.Build(); +} + +GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { + auto diagram = MakeCarveOutChain(); + const MultibodyPlant& plant = diagram->plant(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + const int nq = plant.num_positions(); + ASSERT_EQ(nq, 2); + const int rot = plant.GetJointByName("j_rot").position_start(); + const int slide = 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 = engine.ComputeMotionBoundTable( + lower, upper, std::vector(nq, false), pairs); + ASSERT_EQ(moving.entries(0).size(), 2); + EXPECT_EQ(moving.carveout_slack(0), 0.0); + double lambda_rot = 0.0; + for (const auto& [c, lam] : moving.entries(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 = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + ASSERT_EQ(carved.entries(0).size(), 1); + EXPECT_EQ(carved.entries(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 = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + 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 = engine.ComputeMotionBoundTable( + VectorXd::Constant(nq, -2.0), VectorXd::Constant(nq, 2.0), + std::vector(nq, false), pairs); + 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 in v1, 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", UnitInertia()); + if (rpy) { + plant.AddJoint( + "base", plant.world_body(), {}, link, {}); + } else { + plant.AddJoint("base", plant.world_body(), + {}, link, {}); + } + const CoulombFriction mu(1.0, 1.0); + plant.RegisterCollisionGeometry(link, RigidTransform::Identity(), + HalfSpace(), "hs", mu); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransform(Vector3d(0, 0, 1.0)), + Sphere(0.1), "ball", mu); + return builder.Build(); +} + +GTEST_TEST(CarveOutSlackTest, HalfSpaceAcrossAToleranceConstantRotationThrows) { + // 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 diagram = MakeCarvedHalfSpaceModel(/* rpy = */ false); + const KinematicsEngine engine(*diagram); // Must not throw: it is not a + // *supported* rotational kind. + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + const int nq = diagram->plant().num_positions(); + ASSERT_EQ(nq, 3); + + VectorXd lower = VectorXd::Zero(nq); + VectorXd upper = VectorXd::Constant(nq, 5e-8); + try { + engine.ComputeMotionBoundTable(lower, upper, std::vector(nq, true), + pairs); + GTEST_FAIL() + << "expected a throw for a half space across a tolerance-constant " + "rotational coordinate"; + } catch (const std::exception& e) { + const std::string what = e.what(); + EXPECT_NE(what.find("hs"), std::string::npos) << what; + EXPECT_NE(what.find("base"), std::string::npos) << what; + EXPECT_NE(what.find("EXACTLY constant"), std::string::npos) << what; + } +} + +GTEST_TEST(CarveOutSlackTest, + HalfSpaceAcrossAnExactlyConstantRotationIsAccepted) { + auto diagram = MakeCarvedHalfSpaceModel(/* rpy = */ false); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + const int nq = diagram->plant().num_positions(); + const VectorXd pinned = VectorXd::Constant(nq, 0.3); + const MotionBoundTable table = engine.ComputeMotionBoundTable( + pinned, pinned, std::vector(nq, true), pairs); + 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 MultibodyPlant& plant = diagram->plant(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + const int nq = 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 = engine.ComputeMotionBoundTable( + lower, upper, std::vector(nq, true), pairs); + 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: the +// joint kinds are excluded in v1 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, with the arm coordinate +// pinned so that the slack is the *whole* bound and the check is sensitive to +// it, and then again with everything moving. +// --------------------------------------------------------------------------- + +/* 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", UnitInertia()); + const auto& b2 = plant.AddRigidBody("b2", UnitInertia()); + const RigidTransform X_PF = RandomTransform(rng, 0.2); + const RigidTransform X_CM = 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, RandomTransform(rng, 0.2), b2, + RandomTransform(rng, 0.2), + RandomUnitVector(rng)); + const CoulombFriction mu(1.0, 1.0); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransform::Identity(), + Sphere(0.1), "g_world", mu); + plant.RegisterCollisionGeometry(b2, + RigidTransform(Vector3d(0.25, 0, 0)), + Sphere(0.06), "g_tip", mu); + 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; + double max_slack = 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); + ASSERT_EQ(pairs.size(), 1); + 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 is + // therefore both the honest test (the sample is in the box the bound is + // stated over) and the same thing Drake would compute. + VectorXd q0(nq); + if (quaternion) { + const Eigen::Quaterniond qb = 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 = + SampleSphereSurface(&rng, 0.06, 64).colwise() + Vector3d(0.25, 0, 0); + const auto& frame_tip = plant.GetBodyByName("b2").body_frame(); + const auto& frame_world = plant.world_frame(); + Matrix3Xd out_q(3, points_B.cols()); + Matrix3Xd out_qp(3, points_B.cols()); + const auto displacement = [&](const VectorXd& q, const VectorXd& qp) { + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_q); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_qp); + return (out_qp - out_q).colwise().norm().maxCoeff(); + }; + + 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 and the probe below — which drives that one + // coordinate from one end of its interval to the other — pins that single + // coefficient rather than a seven-term sum. This 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 = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + ASSERT_EQ(table.entries(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; + max_slack = std::max(max_slack, slack); + + 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 = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + const double slack = table.carveout_slack(0); + ASSERT_GT(slack, 0.0); + max_slack = std::max(max_slack, slack); + ++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_LOG_(INFO) << fmt::format( + "{} base: atomic={} aggregate={} max_slack={:.3e} m max_ratio={:.6f}", + quaternion ? "quaternion floating" : "rpy floating", atomic_cases, + aggregate_cases, max_slack, max_ratio); +} + +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 inside that slack 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. Every λ̃ shows up digit for digit. +// --------------------------------------------------------------------------- + +std::unique_ptr> MakeTightFloatingChain(bool quaternion, + double radius) { + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + const auto& link = plant.AddRigidBody("link", UnitInertia()); + if (quaternion) { + plant.AddJoint("base", plant.world_body(), {}, + link, {}); + } else { + plant.AddJoint( + "base", plant.world_body(), {}, link, {}); + } + const CoulombFriction mu(1.0, 1.0); + plant.RegisterCollisionGeometry(link, RigidTransform::Identity(), + Sphere(radius), "g_link", mu); + plant.RegisterCollisionGeometry(plant.world_body(), + RigidTransform(Vector3d(0, 0, 3.0)), + Sphere(0.05), "g_world", mu); + return builder.Build(); +} + +void RunTightFloatingBaseLambda(bool quaternion) { + constexpr double kRadius = 0.4; + Rng rng(quaternion ? 0x7168A7ull : 0x51DE12ull); + auto diagram = MakeTightFloatingChain(quaternion, kRadius); + const MultibodyPlant& plant = diagram->plant(); + const KinematicsEngine engine(*diagram); + const std::vector pairs = CollisionPairs(*diagram); + ASSERT_EQ(pairs.size(), 1); + 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 = SampleSphereSurface(&rng, kRadius, 4096); + auto root = diagram->CreateDefaultContext(); + auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); + Matrix3Xd out_q(3, points_B.cols()); + Matrix3Xd out_qp(3, points_B.cols()); + const auto& frame_link = plant.GetBodyByName("link").body_frame(); + const auto& frame_world = plant.world_frame(); + + constexpr double kWidth = 8e-8; // ≤ Options::continuity_tolerance. + 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 = + engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + ASSERT_TRUE(table.pair_is_static(0)); + const double slack = table.carveout_slack(0); + ASSERT_GT(slack, 0.0); + + VectorXd q = lower; + VectorXd qp = upper; + plant.SetPositions(&ctx, q); + plant.CalcPointsPositions(ctx, frame_link, points_B, frame_world, &out_q); + plant.SetPositions(&ctx, qp); + plant.CalcPointsPositions(ctx, frame_link, points_B, frame_world, &out_qp); + const double displacement = (out_qp - out_q).colwise().norm().maxCoeff(); + + 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 certified_ccd } // namespace planning From 556b74a3c9514434b4cf2dc079dc0fcc078293f7 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 27 Aug 2026 14:12:54 -0400 Subject: [PATCH 09/22] [planning] Rename certified_ccd to continuous_collision Renames the package directory planning/certified_ccd to planning/continuous_collision, the namespace drake::planning::certified_ccd to drake::planning::continuous_collision, and the class CertifiedContinuousCollisionChecker to ContinuousCollisionChecker (with its files and BUILD target renamed to match). The "certified_ccd: " error-message prefix, which named the package rather than the code that throws, is replaced by the owning class or function ("KinematicsEngine: ", "ComputeBoundingSphere(): "). --- .../BUILD.bazel | 26 ++--- .../benchmark/benchmark_util.cc | 6 +- .../benchmark/benchmark_util.h | 4 +- .../benchmark/iiwa_benchmark.cc | 29 +++--- .../benchmark/scenario_worlds.cc | 6 +- .../benchmark/scenario_worlds.h | 4 +- .../bounding_sphere.cc | 8 +- .../bounding_sphere.h | 4 +- .../certificate.cc | 10 +- .../certificate.h | 6 +- .../certifier.cc | 8 +- .../certifier.h | 16 ++-- .../continuous_collision_checker.cc} | 77 +++++++-------- .../continuous_collision_checker.h} | 22 ++--- .../distance_oracle.cc | 6 +- .../distance_oracle.h | 6 +- .../motion_bound_table.cc | 94 +++++++++---------- .../motion_bound_table.h | 10 +- .../numerics.h | 4 +- .../options.h | 4 +- .../piecewise_bezier_path.cc | 6 +- .../piecewise_bezier_path.h | 6 +- .../test/api_test.cc | 16 ++-- .../test/bounding_sphere_test.cc | 6 +- .../test/certificate_test.cc | 14 +-- .../test/certifier_test.cc | 43 +++++---- .../test/concurrency_test.cc | 19 ++-- .../test/distance_oracle_test.cc | 8 +- .../test/motion_bound_test.cc | 6 +- .../test/piecewise_bezier_path_test.cc | 6 +- .../test/soundness_fuzz_test.cc | 21 ++--- .../test/thin_obstacle_test.cc | 25 +++-- .../vpolytope_ingestion.cc | 6 +- .../vpolytope_ingestion.h | 4 +- tools/install/libdrake/build_components.bzl | 2 +- 35 files changed, 262 insertions(+), 276 deletions(-) rename planning/{certified_ccd => continuous_collision}/BUILD.bazel (94%) rename planning/{certified_ccd => continuous_collision}/benchmark/benchmark_util.cc (99%) rename planning/{certified_ccd => continuous_collision}/benchmark/benchmark_util.h (99%) rename planning/{certified_ccd => continuous_collision}/benchmark/iiwa_benchmark.cc (98%) rename planning/{certified_ccd => continuous_collision}/benchmark/scenario_worlds.cc (97%) rename planning/{certified_ccd => continuous_collision}/benchmark/scenario_worlds.h (97%) rename planning/{certified_ccd => continuous_collision}/bounding_sphere.cc (97%) rename planning/{certified_ccd => continuous_collision}/bounding_sphere.h (96%) rename planning/{certified_ccd => continuous_collision}/certificate.cc (98%) rename planning/{certified_ccd => continuous_collision}/certificate.h (87%) rename planning/{certified_ccd => continuous_collision}/certifier.cc (99%) rename planning/{certified_ccd => continuous_collision}/certifier.h (97%) rename planning/{certified_ccd/certified_continuous_collision_checker.cc => continuous_collision/continuous_collision_checker.cc} (90%) rename planning/{certified_ccd/certified_continuous_collision_checker.h => continuous_collision/continuous_collision_checker.h} (85%) rename planning/{certified_ccd => continuous_collision}/distance_oracle.cc (99%) rename planning/{certified_ccd => continuous_collision}/distance_oracle.h (97%) rename planning/{certified_ccd => continuous_collision}/motion_bound_table.cc (92%) rename planning/{certified_ccd => continuous_collision}/motion_bound_table.h (98%) rename planning/{certified_ccd => continuous_collision}/numerics.h (95%) rename planning/{certified_ccd => continuous_collision}/options.h (98%) rename planning/{certified_ccd => continuous_collision}/piecewise_bezier_path.cc (99%) rename planning/{certified_ccd => continuous_collision}/piecewise_bezier_path.h (97%) rename planning/{certified_ccd => continuous_collision}/test/api_test.cc (98%) rename planning/{certified_ccd => continuous_collision}/test/bounding_sphere_test.cc (99%) rename planning/{certified_ccd => continuous_collision}/test/certificate_test.cc (98%) rename planning/{certified_ccd => continuous_collision}/test/certifier_test.cc (97%) rename planning/{certified_ccd => continuous_collision}/test/concurrency_test.cc (98%) rename planning/{certified_ccd => continuous_collision}/test/distance_oracle_test.cc (99%) rename planning/{certified_ccd => continuous_collision}/test/motion_bound_test.cc (99%) rename planning/{certified_ccd => continuous_collision}/test/piecewise_bezier_path_test.cc (99%) rename planning/{certified_ccd => continuous_collision}/test/soundness_fuzz_test.cc (98%) rename planning/{certified_ccd => continuous_collision}/test/thin_obstacle_test.cc (96%) rename planning/{certified_ccd => continuous_collision}/vpolytope_ingestion.cc (93%) rename planning/{certified_ccd => continuous_collision}/vpolytope_ingestion.h (96%) diff --git a/planning/certified_ccd/BUILD.bazel b/planning/continuous_collision/BUILD.bazel similarity index 94% rename from planning/certified_ccd/BUILD.bazel rename to planning/continuous_collision/BUILD.bazel index d71ddc322461..10cc639902ae 100644 --- a/planning/certified_ccd/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -10,12 +10,12 @@ load( package(default_visibility = ["//visibility:public"]) drake_cc_package_library( - name = "certified_ccd", + name = "continuous_collision", visibility = ["//visibility:public"], deps = [ ":bounding_sphere", - ":certified_continuous_collision_checker", ":certifier", + ":continuous_collision_checker", ":distance_oracle", ":motion_bound_table", ":numerics", @@ -169,9 +169,9 @@ drake_cc_library( ) drake_cc_library( - name = "certified_continuous_collision_checker", - srcs = ["certified_continuous_collision_checker.cc"], - hdrs = ["certified_continuous_collision_checker.h"], + name = "continuous_collision_checker", + srcs = ["continuous_collision_checker.cc"], + hdrs = ["continuous_collision_checker.h"], deps = [ ":certifier", ":distance_oracle", @@ -263,7 +263,7 @@ drake_cc_googletest( drake_cc_googletest( name = "certifier_test", deps = [ - ":certified_continuous_collision_checker", + ":continuous_collision_checker", "//common:parallelism", "//common/trajectories:bezier_curve", "//geometry:scene_graph", @@ -284,7 +284,7 @@ drake_cc_googletest( name = "soundness_fuzz_test", timeout = "moderate", deps = [ - ":certified_continuous_collision_checker", + ":continuous_collision_checker", "//common:parallelism", "//common/trajectories:bezier_curve", "//common/trajectories:bspline_trajectory", @@ -306,7 +306,7 @@ drake_cc_googletest( drake_cc_googletest( name = "thin_obstacle_test", deps = [ - ":certified_continuous_collision_checker", + ":continuous_collision_checker", "//common:parallelism", "//common/trajectories:bezier_curve", "//geometry:scene_graph", @@ -325,7 +325,7 @@ drake_cc_googletest( drake_cc_googletest( name = "certificate_test", deps = [ - ":certified_continuous_collision_checker", + ":continuous_collision_checker", "//common:parallelism", "//common/trajectories:bezier_curve", "//geometry:shape_specification", @@ -343,7 +343,7 @@ drake_cc_googletest( name = "concurrency_test", num_threads = 16, deps = [ - ":certified_continuous_collision_checker", + ":continuous_collision_checker", "//common:parallelism", "//common/trajectories:bezier_curve", "//geometry:shape_specification", @@ -359,7 +359,7 @@ drake_cc_googletest( drake_cc_googletest( name = "api_test", deps = [ - ":certified_continuous_collision_checker", + ":continuous_collision_checker", "//common:copyable_unique_ptr", "//common:parallelism", "//common/trajectories:bezier_curve", @@ -383,7 +383,7 @@ drake_cc_googletest( # The performance benchmark suite. Not part of the test suite: a full run # takes minutes and reports measurements rather than assertions. Run it with -# bazel run //planning/certified_ccd:iiwa_benchmark -- \ +# bazel run //planning/continuous_collision:iiwa_benchmark -- \ # --out /tmp/ccd --drake_commit $(git rev-parse HEAD) drake_cc_binary( name = "iiwa_benchmark", @@ -399,7 +399,7 @@ drake_cc_binary( ], tags = ["manual"], deps = [ - ":certified_continuous_collision_checker", + ":continuous_collision_checker", "//common:copyable_unique_ptr", "//common:parallelism", "//common/trajectories:bezier_curve", diff --git a/planning/certified_ccd/benchmark/benchmark_util.cc b/planning/continuous_collision/benchmark/benchmark_util.cc similarity index 99% rename from planning/certified_ccd/benchmark/benchmark_util.cc rename to planning/continuous_collision/benchmark/benchmark_util.cc index 7f0146507f4f..06831f718c73 100644 --- a/planning/certified_ccd/benchmark/benchmark_util.cc +++ b/planning/continuous_collision/benchmark/benchmark_util.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/benchmark/benchmark_util.h" +#include "drake/planning/continuous_collision/benchmark/benchmark_util.h" #include #include @@ -17,7 +17,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace benchmark { namespace { @@ -476,6 +476,6 @@ double BisectMonotone(const std::function& f, double lo, } } // namespace benchmark -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/benchmark/benchmark_util.h b/planning/continuous_collision/benchmark/benchmark_util.h similarity index 99% rename from planning/certified_ccd/benchmark/benchmark_util.h rename to planning/continuous_collision/benchmark/benchmark_util.h index 26af9d15c06c..2abe42a08d9d 100644 --- a/planning/certified_ccd/benchmark/benchmark_util.h +++ b/planning/continuous_collision/benchmark/benchmark_util.h @@ -28,7 +28,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace benchmark { // --------------------------------------------------------------------------- @@ -201,6 +201,6 @@ double BisectMonotone(const std::function& f, double lo, double hi, double target, int iterations); } // namespace benchmark -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/benchmark/iiwa_benchmark.cc b/planning/continuous_collision/benchmark/iiwa_benchmark.cc similarity index 98% rename from planning/certified_ccd/benchmark/iiwa_benchmark.cc rename to planning/continuous_collision/benchmark/iiwa_benchmark.cc index 92be93d2893a..fafd970b251b 100644 --- a/planning/certified_ccd/benchmark/iiwa_benchmark.cc +++ b/planning/continuous_collision/benchmark/iiwa_benchmark.cc @@ -1,5 +1,5 @@ /// @file -/// The `certified_ccd` performance benchmark suite (the performance +/// The `continuous_collision` performance benchmark suite (the performance /// targets and the benchmark deliverable of the white paper), adapted to what /// exists on this machine: no trajectory optimizer is invoked, the smooth /// composite Bézier trajectories are hand-constructed in @@ -44,15 +44,15 @@ #include "drake/common/parallelism.h" #include "drake/common/trajectories/bezier_curve.h" #include "drake/geometry/query_object.h" -#include "drake/planning/certified_ccd/benchmark/benchmark_util.h" -#include "drake/planning/certified_ccd/benchmark/scenario_worlds.h" -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" #include "drake/planning/collision_checker_params.h" +#include "drake/planning/continuous_collision/benchmark/benchmark_util.h" +#include "drake/planning/continuous_collision/benchmark/scenario_worlds.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include "drake/planning/scene_graph_collision_checker.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace benchmark { namespace { @@ -118,7 +118,7 @@ std::string ModeName(SearchMode m) { struct World { std::shared_ptr> diagram; std::unique_ptr sampled; - std::unique_ptr certified; + std::unique_ptr certified; std::unordered_set env_ids; int pair_count{0}; /// SceneGraph's own unfiltered-candidate count *after* the sampled checker @@ -145,10 +145,9 @@ World MakeWorld(std::shared_ptr> diagram, world.sampled = std::make_unique(std::move(params)); - CertifiedContinuousCollisionChecker::Params cparams; + ContinuousCollisionChecker::Params cparams; cparams.model = world.diagram; - world.certified = - std::make_unique(cparams); + world.certified = std::make_unique(cparams); world.env_ids = CollectGeometryIds(*world.diagram, {"environment"}); world.pair_count = static_cast(world.certified->pairs().size()); @@ -215,7 +214,7 @@ struct CertRun { int num_findings{0}; }; -CertRun MeasureCertify(const CertifiedContinuousCollisionChecker& checker, +CertRun MeasureCertify(const ContinuousCollisionChecker& checker, const Trajectory& trajectory, const Options& options, int warmup, int reps) { CertRun run; @@ -229,7 +228,7 @@ CertRun MeasureCertify(const CertifiedContinuousCollisionChecker& checker, return run; } -CertRun MeasureCertifyEdge(const CertifiedContinuousCollisionChecker& checker, +CertRun MeasureCertifyEdge(const ContinuousCollisionChecker& checker, const VectorXd& q1, const VectorXd& q2, const Options& options, int warmup, int reps) { CertRun run; @@ -379,7 +378,7 @@ void PlanReps(const Config& config, double single_run_ms, int* warmup, } /// Times one certification once, untimed, to price the case for PlanReps. -double ProbeCost(const CertifiedContinuousCollisionChecker& checker, +double ProbeCost(const ContinuousCollisionChecker& checker, const Trajectory& trajectory, const Options& options) { const auto t0 = std::chrono::steady_clock::now(); checker.CheckTrajectory(trajectory, options); @@ -1037,7 +1036,7 @@ int Main(int argc, char** argv) { }; const MachineInfo machine = GetMachineInfo(config.drake_commit); - std::printf("certified_ccd benchmark suite\n"); + std::printf("continuous_collision benchmark suite\n"); std::printf(" cpu : %s (%d logical cores)\n", machine.cpu_model.c_str(), machine.core_count); std::printf(" drake pin : %s (%s)\n", machine.drake_commit.c_str(), @@ -1160,13 +1159,13 @@ int Main(int argc, char** argv) { } // namespace } // namespace benchmark -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake int main(int argc, char** argv) { try { - return drake::planning::certified_ccd::benchmark::Main(argc, argv); + return drake::planning::continuous_collision::benchmark::Main(argc, argv); } catch (const std::exception& e) { std::fprintf(stderr, "benchmark failed: %s\n", e.what()); return 1; diff --git a/planning/certified_ccd/benchmark/scenario_worlds.cc b/planning/continuous_collision/benchmark/scenario_worlds.cc similarity index 97% rename from planning/certified_ccd/benchmark/scenario_worlds.cc rename to planning/continuous_collision/benchmark/scenario_worlds.cc index 99669cffa3f0..3438a4e0670a 100644 --- a/planning/certified_ccd/benchmark/scenario_worlds.cc +++ b/planning/continuous_collision/benchmark/scenario_worlds.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/benchmark/scenario_worlds.h" +#include "drake/planning/continuous_collision/benchmark/scenario_worlds.h" #include @@ -13,7 +13,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace benchmark { namespace { @@ -177,6 +177,6 @@ std::vector DualArmTrajectoryTimes() { } } // namespace benchmark -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/benchmark/scenario_worlds.h b/planning/continuous_collision/benchmark/scenario_worlds.h similarity index 97% rename from planning/certified_ccd/benchmark/scenario_worlds.h rename to planning/continuous_collision/benchmark/scenario_worlds.h index fac65febe811..37abb71143c5 100644 --- a/planning/certified_ccd/benchmark/scenario_worlds.h +++ b/planning/continuous_collision/benchmark/scenario_worlds.h @@ -16,7 +16,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace benchmark { /// The dense-sphere iiwa14 collision variant: 46 collision spheres over @@ -69,6 +69,6 @@ Eigen::MatrixXd DualArmTrajectoryWaypoints(); std::vector DualArmTrajectoryTimes(); } // namespace benchmark -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/bounding_sphere.cc b/planning/continuous_collision/bounding_sphere.cc similarity index 97% rename from planning/certified_ccd/bounding_sphere.cc rename to planning/continuous_collision/bounding_sphere.cc index a831250361ea..d8caef5c39c6 100644 --- a/planning/certified_ccd/bounding_sphere.cc +++ b/planning/continuous_collision/bounding_sphere.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/bounding_sphere.h" +#include "drake/planning/continuous_collision/bounding_sphere.h" #include #include @@ -12,7 +12,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::geometry::Box; @@ -109,7 +109,7 @@ class BoundingSphereReifier final : public ShapeReifier { private: void ThrowUnsupportedGeometry(const std::string& shape_name) final { throw std::runtime_error(fmt::format( - "certified_ccd: ComputeBoundingSphere() does not support the shape " + "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 the dedicated rules in the " @@ -172,6 +172,6 @@ BoundingSphere ComputeBoundingSphere(const Shape& shape, return result; } -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/bounding_sphere.h b/planning/continuous_collision/bounding_sphere.h similarity index 96% rename from planning/certified_ccd/bounding_sphere.h rename to planning/continuous_collision/bounding_sphere.h index a38f5a26394a..4dcdbdb7e35e 100644 --- a/planning/certified_ccd/bounding_sphere.h +++ b/planning/continuous_collision/bounding_sphere.h @@ -7,7 +7,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** A sphere, expressed in the owning body (link) frame L, that contains a proximity geometry at every configuration of the body. */ @@ -45,6 +45,6 @@ BoundingSphere ComputeBoundingSphere( const drake::geometry::Shape& shape, const drake::math::RigidTransform& X_LG); -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/certificate.cc b/planning/continuous_collision/certificate.cc similarity index 98% rename from planning/certified_ccd/certificate.cc rename to planning/continuous_collision/certificate.cc index 136853f3a8f2..42eaddc241f0 100644 --- a/planning/certified_ccd/certificate.cc +++ b/planning/continuous_collision/certificate.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/certificate.h" +#include "drake/planning/continuous_collision/certificate.h" #include #include @@ -11,12 +11,12 @@ #include #include "drake/common/drake_throw.h" -#include "drake/planning/certified_ccd/certifier.h" -#include "drake/planning/certified_ccd/numerics.h" +#include "drake/planning/continuous_collision/certifier.h" +#include "drake/planning/continuous_collision/numerics.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace internal { namespace { @@ -355,6 +355,6 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, } } // namespace internal -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/certificate.h b/planning/continuous_collision/certificate.h similarity index 87% rename from planning/certified_ccd/certificate.h rename to planning/continuous_collision/certificate.h index 621ae5b89c21..343bded9f0f4 100644 --- a/planning/certified_ccd/certificate.h +++ b/planning/continuous_collision/certificate.h @@ -4,11 +4,11 @@ #include -#include "drake/planning/certified_ccd/options.h" +#include "drake/planning/continuous_collision/options.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** One certification event: pair `pair_index` was certified over the parameter interval [s_start, s_end] of segment `segment` from representative @@ -33,6 +33,6 @@ struct Certificate { std::vector pairs; }; -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/certifier.cc b/planning/continuous_collision/certifier.cc similarity index 99% rename from planning/certified_ccd/certifier.cc rename to planning/continuous_collision/certifier.cc index b51508d20e61..57555bf5058a 100644 --- a/planning/certified_ccd/certifier.cc +++ b/planning/continuous_collision/certifier.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/certifier.h" +#include "drake/planning/continuous_collision/certifier.h" #include #include @@ -15,11 +15,11 @@ #include "drake/common/drake_throw.h" #include "drake/geometry/scene_graph.h" #include "drake/multibody/plant/multibody_plant.h" -#include "drake/planning/certified_ccd/numerics.h" +#include "drake/planning/continuous_collision/numerics.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace internal { namespace { @@ -1328,6 +1328,6 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, } } // namespace internal -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/certifier.h b/planning/continuous_collision/certifier.h similarity index 97% rename from planning/certified_ccd/certifier.h rename to planning/continuous_collision/certifier.h index 22023b8cd62e..516e4ee6df03 100644 --- a/planning/certified_ccd/certifier.h +++ b/planning/continuous_collision/certifier.h @@ -6,7 +6,7 @@ /// "certificate audit trail"). /// /// Nothing in this header is part of the public API; it exists so the facade -/// (`certified_continuous_collision_checker.cc`), the certificate replay +/// (`continuous_collision_checker.cc`), the certificate replay /// (`certificate.cc`) and the node loop (`certifier.cc`) can share /// one set of per-call data structures without the core module depending on /// the api layer. @@ -26,17 +26,17 @@ #include "drake/geometry/query_object.h" #include "drake/multibody/tree/multibody_tree_indexes.h" -#include "drake/planning/certified_ccd/certificate.h" -#include "drake/planning/certified_ccd/distance_oracle.h" -#include "drake/planning/certified_ccd/motion_bound_table.h" -#include "drake/planning/certified_ccd/options.h" -#include "drake/planning/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/continuous_collision/certificate.h" +#include "drake/planning/continuous_collision/distance_oracle.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" +#include "drake/planning/continuous_collision/options.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include "drake/planning/robot_diagram.h" #include "drake/systems/framework/context.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace internal { /** One thread's view of the model: a root diagram context plus the plant and @@ -370,6 +370,6 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, std::string* message); } // namespace internal -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/certified_continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc similarity index 90% rename from planning/certified_ccd/certified_continuous_collision_checker.cc rename to planning/continuous_collision/continuous_collision_checker.cc index 97e062e9f2a0..45d2c05406d1 100644 --- a/planning/certified_ccd/certified_continuous_collision_checker.cc +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -18,7 +18,7 @@ /// samples. The certificate is a property of the path, so retiming the /// trajectory afterwards does not invalidate it. -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include #include @@ -39,11 +39,11 @@ #include "drake/geometry/scene_graph_inspector.h" #include "drake/geometry/shape_specification.h" #include "drake/multibody/plant/multibody_plant.h" -#include "drake/planning/certified_ccd/certifier.h" +#include "drake/planning/continuous_collision/certifier.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::geometry::GeometryId; @@ -249,7 +249,7 @@ std::vector ComputePaddingTable(const KinematicsEngine& engine, const Eigen::MatrixXd& matrix = *padding.per_body_pair; if (matrix.rows() != num_bodies || matrix.cols() != num_bodies) { throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: PaddingSpec::per_body_pair is " + "ContinuousCollisionChecker: PaddingSpec::per_body_pair is " "{}x{} but must be {}x{} (one row and column per BodyIndex of the " "plant).", matrix.rows(), matrix.cols(), num_bodies, num_bodies)); @@ -277,11 +277,11 @@ std::vector ComputePaddingTable(const KinematicsEngine& engine, if (!std::isnan(entry)) value = entry; } if (!std::isfinite(value)) { - throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: padding for the body pair " - "({}, {}) is not finite.", - plant.get_body(BodyIndex(a)).name(), - plant.get_body(BodyIndex(b)).name())); + throw std::runtime_error( + fmt::format("ContinuousCollisionChecker: padding for the body pair " + "({}, {}) is not finite.", + plant.get_body(BodyIndex(a)).name(), + plant.get_body(BodyIndex(b)).name())); } result[p] = value; } @@ -327,38 +327,38 @@ internal::PrefilterTable ComputePrefilterTable( void ValidateOptions(const Options& options) { if (!std::isfinite(options.margin)) { throw std::runtime_error( - "CertifiedContinuousCollisionChecker: Options::margin must be finite."); + "ContinuousCollisionChecker: Options::margin must be finite."); } if (!(options.query_tolerance >= 0.0) || !std::isfinite(options.query_tolerance)) { throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: Options::query_tolerance must be " + "ContinuousCollisionChecker: Options::query_tolerance must be " "a finite non-negative distance; got {}.", options.query_tolerance)); } if (!(options.certificate_slack >= 0.0) || !std::isfinite(options.certificate_slack)) { throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: Options::certificate_slack must " + "ContinuousCollisionChecker: Options::certificate_slack must " "be a finite non-negative distance; got {}.", options.certificate_slack)); } if (!(options.min_interval > 0.0) || !(options.min_interval <= 1.0)) { throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: Options::min_interval is a " + "ContinuousCollisionChecker: Options::min_interval is a " "fraction of a segment's parameter width and must lie in (0, 1]; got " "{}.", options.min_interval)); } if (options.max_reported_findings < 1) { throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: Options::max_reported_findings " + "ContinuousCollisionChecker: Options::max_reported_findings " "must be at least 1; got {}.", options.max_reported_findings)); } if (options.max_nodes.has_value() && *options.max_nodes == 0) { throw std::runtime_error( - "CertifiedContinuousCollisionChecker: Options::max_nodes must be at " + "ContinuousCollisionChecker: Options::max_nodes must be at " "least 1 when set."); } } @@ -369,7 +369,7 @@ void ValidateOptions(const Options& options) { // Impl. // --------------------------------------------------------------------------- -class CertifiedContinuousCollisionChecker::Impl { +class ContinuousCollisionChecker::Impl { public: explicit Impl(Params params) : model_(std::move(params.model)), @@ -407,7 +407,7 @@ class CertifiedContinuousCollisionChecker::Impl { const int expected = model_->plant().num_positions(); if (path.num_positions() != expected) { throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: the trajectory has {} rows but " + "ContinuousCollisionChecker: the trajectory has {} rows but " "the plant has {} generalized positions.", path.num_positions(), expected)); } @@ -434,7 +434,7 @@ class CertifiedContinuousCollisionChecker::Impl { // zero. Negative padding is therefore rejected rather than certified. if (pairs[p].threshold < 0.0) { throw std::runtime_error(fmt::format( - "CertifiedContinuousCollisionChecker: margin ({}) + padding ({}) " + "ContinuousCollisionChecker: margin ({}) + padding ({}) " "is negative for the pair on bodies {} and {}. The certificate " "is only proven for nonnegative thresholds; filter the pair out " "instead of using negative padding.", @@ -491,29 +491,27 @@ class CertifiedContinuousCollisionChecker::Impl { }; // --------------------------------------------------------------------------- -// CertifiedContinuousCollisionChecker. +// ContinuousCollisionChecker. // --------------------------------------------------------------------------- -CertifiedContinuousCollisionChecker::CertifiedContinuousCollisionChecker( - Params params) { +ContinuousCollisionChecker::ContinuousCollisionChecker(Params params) { if (params.model == nullptr) { throw std::runtime_error( - "CertifiedContinuousCollisionChecker: Params::model is null; supply a " + "ContinuousCollisionChecker: Params::model is null; supply a " "RobotDiagram whose plant is finalized."); } if (!params.model->plant().is_finalized()) { throw std::runtime_error( - "CertifiedContinuousCollisionChecker: the plant is not finalized; call " + "ContinuousCollisionChecker: the plant is not finalized; call " "MultibodyPlant::Finalize() (or RobotDiagramBuilder::Build()) first."); } ValidateOptions(params.default_options); impl_ = std::make_unique(std::move(params)); } -CertifiedContinuousCollisionChecker::~CertifiedContinuousCollisionChecker() = - default; +ContinuousCollisionChecker::~ContinuousCollisionChecker() = default; -CertificationResult CertifiedContinuousCollisionChecker::CheckTrajectory( +CertificationResult ContinuousCollisionChecker::CheckTrajectory( const drake::trajectories::Trajectory& trajectory, const std::optional& options) const { const Options& resolved = impl_->Resolve(options); @@ -522,14 +520,14 @@ CertificationResult CertifiedContinuousCollisionChecker::CheckTrajectory( return impl_->Check(path, resolved); } -CertificationResult CertifiedContinuousCollisionChecker::CheckPath( +CertificationResult 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( - "CertifiedContinuousCollisionChecker::CheckPath: the waypoint matrix " + "ContinuousCollisionChecker::CheckPath: the waypoint matrix " "has {} rows but the plant has {} generalized positions (waypoints are " "columns).", waypoints.rows(), expected)); @@ -539,13 +537,13 @@ CertificationResult CertifiedContinuousCollisionChecker::CheckPath( return impl_->Check(path, resolved); } -CertificationResult CertifiedContinuousCollisionChecker::CheckEdge( +CertificationResult 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( - "CertifiedContinuousCollisionChecker::CheckEdge: the endpoints have " + "ContinuousCollisionChecker::CheckEdge: the endpoints have " "sizes {} and {} but the plant has {} generalized positions.", q1.size(), q2.size(), expected)); } @@ -555,7 +553,7 @@ CertificationResult CertifiedContinuousCollisionChecker::CheckEdge( return CheckPath(waypoints, options); } -PiecewiseBezierPath CertifiedContinuousCollisionChecker::Normalize( +PiecewiseBezierPath ContinuousCollisionChecker::Normalize( const drake::trajectories::Trajectory& trajectory, const std::optional& options) const { const Options& resolved = impl_->Resolve(options); @@ -565,28 +563,25 @@ PiecewiseBezierPath CertifiedContinuousCollisionChecker::Normalize( return path; } -MotionBoundTable CertifiedContinuousCollisionChecker::ComputeMotionBounds( +MotionBoundTable ContinuousCollisionChecker::ComputeMotionBounds( const PiecewiseBezierPath& path) const { impl_->ValidatePath(path); return impl_->engine().ComputeMotionBoundTable(path, impl_->pair_ids()); } -const DistanceOracle& CertifiedContinuousCollisionChecker::distance_oracle() - const { +const DistanceOracle& ContinuousCollisionChecker::distance_oracle() const { return impl_->oracle(); } -const KinematicsEngine& CertifiedContinuousCollisionChecker::kinematics_engine() - const { +const KinematicsEngine& ContinuousCollisionChecker::kinematics_engine() const { return impl_->engine(); } -const std::vector& CertifiedContinuousCollisionChecker::pairs() - const { +const std::vector& ContinuousCollisionChecker::pairs() const { return impl_->pairs(); } -const RobotDiagram& CertifiedContinuousCollisionChecker::model() const { +const RobotDiagram& ContinuousCollisionChecker::model() const { return impl_->model(); } @@ -612,7 +607,7 @@ const RobotDiagram& CertifiedContinuousCollisionChecker::model() const { // uncovered, and the coverage check reports that as a failure — which is the // correct answer to "does this certificate prove the path is free?". -bool VerifyCertificate(const CertifiedContinuousCollisionChecker& checker, +bool VerifyCertificate(const ContinuousCollisionChecker& checker, const PiecewiseBezierPath& path, const Certificate& certificate) { const MotionBoundTable table = checker.ComputeMotionBounds(path); @@ -631,6 +626,6 @@ bool VerifyCertificate(const CertifiedContinuousCollisionChecker& checker, return internal::ReplayCertificate(input, certificate, nullptr); } -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/certified_continuous_collision_checker.h b/planning/continuous_collision/continuous_collision_checker.h similarity index 85% rename from planning/certified_ccd/certified_continuous_collision_checker.h rename to planning/continuous_collision/continuous_collision_checker.h index 69ee40e5e156..777756603dce 100644 --- a/planning/certified_ccd/certified_continuous_collision_checker.h +++ b/planning/continuous_collision/continuous_collision_checker.h @@ -7,16 +7,16 @@ #include #include "drake/common/trajectories/trajectory.h" -#include "drake/planning/certified_ccd/certificate.h" -#include "drake/planning/certified_ccd/distance_oracle.h" -#include "drake/planning/certified_ccd/motion_bound_table.h" -#include "drake/planning/certified_ccd/options.h" -#include "drake/planning/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/continuous_collision/certificate.h" +#include "drake/planning/continuous_collision/distance_oracle.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" +#include "drake/planning/continuous_collision/options.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include "drake/planning/robot_diagram.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** Result of one certification call (the architecture). */ struct CertificationResult { @@ -43,7 +43,7 @@ it. Thread-compatible: the Check* methods are const, own no mutable state outside per-call scratch, and are safe to call concurrently. */ -class CertifiedContinuousCollisionChecker { +class ContinuousCollisionChecker { public: struct Params { /** Plant + scene graph; the plant must be finalized. */ @@ -56,9 +56,9 @@ class CertifiedContinuousCollisionChecker { /** Builds contexts, bounding spheres, topology tables, and runs the capability probe (throws on unsupported geometry pairs; the geometry-support scope). */ - explicit CertifiedContinuousCollisionChecker(Params params); + explicit ContinuousCollisionChecker(Params params); - ~CertifiedContinuousCollisionChecker(); + ~ContinuousCollisionChecker(); /** Certifies a trajectory (any supported Drake trajectory type). */ CertificationResult CheckTrajectory( @@ -94,10 +94,10 @@ class CertifiedContinuousCollisionChecker { control boxes from freshly restricted control points and re-querying distances) and checks interval coverage of the full domain for every pair. Returns true iff the certificate holds (the search algorithm). */ -bool VerifyCertificate(const CertifiedContinuousCollisionChecker& checker, +bool VerifyCertificate(const ContinuousCollisionChecker& checker, const PiecewiseBezierPath& path, const Certificate& certificate); -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/distance_oracle.cc b/planning/continuous_collision/distance_oracle.cc similarity index 99% rename from planning/certified_ccd/distance_oracle.cc rename to planning/continuous_collision/distance_oracle.cc index 4e180466c7c2..77f8e12114bd 100644 --- a/planning/certified_ccd/distance_oracle.cc +++ b/planning/continuous_collision/distance_oracle.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/distance_oracle.h" +#include "drake/planning/continuous_collision/distance_oracle.h" #include #include @@ -23,7 +23,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::unused; @@ -520,6 +520,6 @@ std::string DistanceOracle::support_report() const { return impl_->report; } -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/distance_oracle.h b/planning/continuous_collision/distance_oracle.h similarity index 97% rename from planning/certified_ccd/distance_oracle.h rename to planning/continuous_collision/distance_oracle.h index 7085d0122205..d335dfecf746 100644 --- a/planning/certified_ccd/distance_oracle.h +++ b/planning/continuous_collision/distance_oracle.h @@ -12,12 +12,12 @@ #include #include "drake/geometry/query_object.h" -#include "drake/planning/certified_ccd/options.h" +#include "drake/planning/continuous_collision/options.h" #include "drake/planning/robot_diagram.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** How the oracle computes signed distance for one pair, resolved once by the capability probe (the geometry-support scope; the distance-oracle contract): @@ -105,6 +105,6 @@ class DistanceOracle { std::shared_ptr impl_; }; -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/motion_bound_table.cc b/planning/continuous_collision/motion_bound_table.cc similarity index 92% rename from planning/certified_ccd/motion_bound_table.cc rename to planning/continuous_collision/motion_bound_table.cc index 814b37dfb04f..6db4ac686fae 100644 --- a/planning/certified_ccd/motion_bound_table.cc +++ b/planning/continuous_collision/motion_bound_table.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/motion_bound_table.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" #include #include @@ -25,7 +25,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { using drake::geometry::GeometryId; using drake::geometry::HalfSpace; @@ -65,7 +65,7 @@ KinematicsEngine::KinematicsEngine( : model_(&model), plant_(&model.plant()) { if (!plant_->is_finalized()) { throw std::runtime_error( - "certified_ccd: KinematicsEngine requires a finalized " + "KinematicsEngine: requires a finalized " "MultibodyPlant; call Finalize() before building the checker."); } BuildTopology(); @@ -175,10 +175,10 @@ void KinematicsEngine::BuildTopology() { X_CM = joint.frame_on_child().GetFixedPoseInBodyFrame(); } catch (const std::exception& e) { throw std::runtime_error(fmt::format( - "certified_ccd: 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: {}", + "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(); @@ -232,8 +232,8 @@ void KinematicsEngine::BuildTopology() { for (int b = 0; b < num_bodies_; ++b) { if (!visited[b]) { throw std::runtime_error(fmt::format( - "certified_ccd: body '{}' is not connected to the world through the " - "plant's joints; the kinematics module requires the single " + "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())); } @@ -241,8 +241,8 @@ void KinematicsEngine::BuildTopology() { for (const JointRecord& rec : joints_) { if (!rec.outboard.is_valid()) { throw std::runtime_error(fmt::format( - "certified_ccd: joint '{}' ({}) closes a kinematic loop (both of its " - "bodies are already reachable from the world without it). Loop " + "KinematicsEngine: joint '{}' ({}) closes a kinematic loop (both of " + "its bodies are already reachable from the world without it). Loop " "topologies are not supported in v1.", rec.name, rec.type_name)); } @@ -280,7 +280,7 @@ void KinematicsEngine::BuildTopology() { DRAKE_THROW_UNLESS(rec.num_positions > 0); if (rec.outboard != joint.child_body().index()) { throw std::runtime_error(fmt::format( - "certified_ccd: joint '{}' ({}) is reversed — its declared parent " + "KinematicsEngine: joint '{}' ({}) is reversed — its declared parent " "body '{}' is outboard of its declared child body '{}' in the " "multibody tree. Reversed mobilizers are a documented v1 exclusion " "because the frame that stays fixed under the joint's motion is then " @@ -295,7 +295,7 @@ void KinematicsEngine::BuildTopology() { } if (rec.subtree != tree_subtree[k]) { throw std::runtime_error(fmt::format( - "certified_ccd: the plant's kinematically-affected set for joint " + "KinematicsEngine: the plant's kinematically-affected set for joint " "'{}' ({}) disagrees with the world-rooted tree walk. This model's " "topology is not supported in v1.", rec.name, rec.type_name)); @@ -357,7 +357,7 @@ void KinematicsEngine::BuildGeometry() { sphere = ComputeBoundingSphere(shape, inspector.GetPoseInFrame(gid)); } catch (const std::exception& e) { throw std::runtime_error(fmt::format( - "certified_ccd: proximity geometry '{}' on body '{}' cannot be " + "KinematicsEngine: proximity geometry '{}' on body '{}' cannot be " "bounded. {}", inspector.GetName(gid), plant.get_body(body).name(), e.what())); } @@ -407,13 +407,12 @@ void KinematicsEngine::CheckHalfSpaceRule() const { const GeometryId offender = in_a ? ga : gb; const GeometryId partner = in_a ? gb : ga; throw std::runtime_error(fmt::format( - "certified_ccd: 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 (the geometry-support scope). Fix the " - "model by anchoring " - "the half space, filtering the pair, or replacing the half space " - "with a large Box.", + "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 (the geometry-support " + "scope). 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)); @@ -466,7 +465,7 @@ double KinematicsEngine::Reach(int joint_ord, BodyIndex body, b = joints_[k].inboard; } throw std::runtime_error( - "certified_ccd: internal error — reach chain walk did not reach the " + "KinematicsEngine: internal error — reach chain walk did not reach the " "requested joint. This indicates inconsistent topology tables."); } @@ -474,7 +473,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( const PiecewiseBezierPath& path, const std::vector& pairs) const { if (path.num_positions() != num_positions_) { throw std::runtime_error(fmt::format( - "certified_ccd: the path has {} positions but the plant has {}.", + "KinematicsEngine: the path has {} positions but the plant has {}.", path.num_positions(), num_positions_)); } return ComputeMotionBoundTable(path.global_lower_bound(), @@ -489,7 +488,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( if (lower.size() != num_positions_ || upper.size() != num_positions_ || static_cast(constant_coordinates.size()) != num_positions_) { throw std::runtime_error(fmt::format( - "certified_ccd: control-box size mismatch — got lower={}, upper={}, " + "KinematicsEngine: control-box size mismatch — got lower={}, upper={}, " "constant_coordinates={} for a plant with {} positions.", lower.size(), upper.size(), constant_coordinates.size(), num_positions_)); @@ -498,7 +497,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( if (!std::isfinite(lower[c]) || !std::isfinite(upper[c]) || lower[c] > upper[c]) { throw std::runtime_error(fmt::format( - "certified_ccd: the trajectory's global control box is invalid at " + "KinematicsEngine: the trajectory's global control box is invalid at " "coordinate {}: [{}, {}].", c, lower[c], upper[c])); } @@ -548,23 +547,22 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( for (int c = ps; c < ps + rec.num_positions; ++c) { if (!constant_coordinates[c]) { throw std::runtime_error(fmt::format( - "certified_ccd: this trajectory moves coordinate {} of joint " - "'{}', whose type '{}' is excluded in v1 (the joint-support " - "scope). 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. See " - "the white paper's future extensions for the " - "manifold-curve extension.", + "KinematicsEngine: this trajectory moves coordinate {} of " + "joint '{}', whose type '{}' is excluded in v1 (the " + "joint-support scope). 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. See the white paper's future " + "extensions for the manifold-curve extension.", c, rec.name, rec.type_name)); } } if (!rec.translation_offsets_known) { throw std::runtime_error(fmt::format( - "certified_ccd: joint '{}' has type '{}', which this library " + "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)); @@ -758,7 +756,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( if (!a.is_valid() || !b.is_valid() || a >= num_bodies_ || b >= num_bodies_) { throw std::runtime_error(fmt::format( - "certified_ccd: pair references body indices ({}, {}) outside the " + "KinematicsEngine: pair references body indices ({}, {}) outside the " "plant's {} bodies.", static_cast(a), static_cast(b), num_bodies_)); } @@ -776,7 +774,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( if (r < 0.0) { if (body_has_halfspace_[distal]) { throw std::runtime_error(fmt::format( - "certified_ccd: HalfSpace geometry '{}' on body '{}' is the " + "KinematicsEngine: HalfSpace geometry '{}' on body '{}' is the " "distal side of joint '{}' ({}), which rotates it. A half " "space has unbounded reach, so no finite λ exists (the " "geometry-support scope).", @@ -801,14 +799,14 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // constant or not. Kept as a guard so a future joint kind cannot // reach here uncharged. throw std::runtime_error(fmt::format( - "certified_ccd: joint '{}' has type '{}', which this library " - "does not know how to bound even when held constant.", + "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( - "certified_ccd: HalfSpace geometry '{}' on body '{}' is the " + "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 " @@ -838,7 +836,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( const double m = quat_min_norm[k]; if (!(m > 0.0)) { throw std::runtime_error(fmt::format( - "certified_ccd: the trajectory's control box for the " + "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 " @@ -872,8 +870,8 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( case JointKind::kWeld: case JointKind::kUnsupported: throw std::runtime_error(fmt::format( - "certified_ccd: internal error — joint '{}' ({}) reached the " - "λ assembly with an unsupported kind.", + "KinematicsEngine: internal error — joint '{}' ({}) reached " + "the λ assembly with an unsupported kind.", rec.name, rec.type_name)); } DRAKE_THROW_UNLESS(std::isfinite(lam) && lam >= 0.0); @@ -905,8 +903,8 @@ 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( - "certified_ccd: geometry {} has no bounding sphere; it is either not a " - "proximity geometry of this model or it is a HalfSpace.", + "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; @@ -922,6 +920,6 @@ double KinematicsEngine::body_radius(BodyIndex body) const { return body_radius_[body]; } -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/motion_bound_table.h b/planning/continuous_collision/motion_bound_table.h similarity index 98% rename from planning/certified_ccd/motion_bound_table.h rename to planning/continuous_collision/motion_bound_table.h index d461a4da5c03..6d5a944af69e 100644 --- a/planning/certified_ccd/motion_bound_table.h +++ b/planning/continuous_collision/motion_bound_table.h @@ -11,14 +11,14 @@ #include -#include "drake/planning/certified_ccd/bounding_sphere.h" -#include "drake/planning/certified_ccd/options.h" -#include "drake/planning/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/continuous_collision/bounding_sphere.h" +#include "drake/planning/continuous_collision/options.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include "drake/planning/robot_diagram.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** Per-pair motion-bound coefficients in CSR layout (the displacement lemma): for pair index k, a contiguous span of (position-coordinate index j, λ(j, p)) @@ -297,6 +297,6 @@ class KinematicsEngine { geometry_spheres_; }; -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/numerics.h b/planning/continuous_collision/numerics.h similarity index 95% rename from planning/certified_ccd/numerics.h rename to planning/continuous_collision/numerics.h index e75d0248aa38..63fda74a5481 100644 --- a/planning/certified_ccd/numerics.h +++ b/planning/continuous_collision/numerics.h @@ -2,7 +2,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** @file Single home of the numerical accounting used everywhere (the numerical policy). @@ -34,6 +34,6 @@ inline bool IsDefiniteViolation(double phi_hat, double tau, double threshold) { return phi_hat + tau < threshold; } -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/options.h b/planning/continuous_collision/options.h similarity index 98% rename from planning/certified_ccd/options.h rename to planning/continuous_collision/options.h index 174c0bce7a20..e27784465163 100644 --- a/planning/certified_ccd/options.h +++ b/planning/continuous_collision/options.h @@ -12,7 +12,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** Search modes for certification (the search algorithm). */ enum class SearchMode { @@ -119,6 +119,6 @@ struct Statistics { double wall_time_s{0.0}; }; -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/piecewise_bezier_path.cc b/planning/continuous_collision/piecewise_bezier_path.cc similarity index 99% rename from planning/certified_ccd/piecewise_bezier_path.cc rename to planning/continuous_collision/piecewise_bezier_path.cc index cb1ddb37a279..51b29ffabc7b 100644 --- a/planning/certified_ccd/piecewise_bezier_path.cc +++ b/planning/continuous_collision/piecewise_bezier_path.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include #include @@ -19,7 +19,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::NiceTypeName; @@ -561,6 +561,6 @@ void DeCasteljauSplitAtHalf(const Eigen::MatrixXd& cps, Eigen::MatrixXd* left, *mid = right->col(0); } -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/piecewise_bezier_path.h b/planning/continuous_collision/piecewise_bezier_path.h similarity index 97% rename from planning/certified_ccd/piecewise_bezier_path.h rename to planning/continuous_collision/piecewise_bezier_path.h index aabdb7cfe920..21eb672f082d 100644 --- a/planning/certified_ccd/piecewise_bezier_path.h +++ b/planning/continuous_collision/piecewise_bezier_path.h @@ -5,11 +5,11 @@ #include #include "drake/common/trajectories/trajectory.h" -#include "drake/planning/certified_ccd/options.h" +#include "drake/planning/continuous_collision/options.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1] (trajectory * normalization). */ @@ -94,6 +94,6 @@ 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 certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/api_test.cc b/planning/continuous_collision/test/api_test.cc similarity index 98% rename from planning/certified_ccd/test/api_test.cc rename to planning/continuous_collision/test/api_test.cc index a4cba4c6ba5f..fef80a28553f 100644 --- a/planning/certified_ccd/test/api_test.cc +++ b/planning/continuous_collision/test/api_test.cc @@ -45,13 +45,13 @@ #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include "drake/planning/robot_diagram.h" #include "drake/planning/robot_diagram_builder.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::Parallelism; @@ -106,12 +106,12 @@ void ExpectContains(const std::string& haystack, const std::string& needle) { << haystack; } -std::unique_ptr MakeChecker( +std::unique_ptr MakeChecker( std::shared_ptr> model) { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = std::move(model); params.default_options.parallelism = Parallelism::None(); - return std::make_unique(params); + return std::make_unique(params); } /// A planar 2-dof arm (revolute, prismatic) with one anchored obstacle: the @@ -647,9 +647,9 @@ GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { } GTEST_TEST(ApiTest, NullModelIsRefused) { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; const std::string message = ThrowMessage([&]() { - CertifiedContinuousCollisionChecker checker(params); + ContinuousCollisionChecker checker(params); }); ExpectContains(message, "Params::model is null"); // The message points at the requirement the (unreachable-through-Drake's @@ -697,6 +697,6 @@ GTEST_TEST(ApiTest, MaxReportedFindingsIsRespected) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/bounding_sphere_test.cc b/planning/continuous_collision/test/bounding_sphere_test.cc similarity index 99% rename from planning/certified_ccd/test/bounding_sphere_test.cc rename to planning/continuous_collision/test/bounding_sphere_test.cc index e7f4559d8b54..e82e3d9c4993 100644 --- a/planning/certified_ccd/test/bounding_sphere_test.cc +++ b/planning/continuous_collision/test/bounding_sphere_test.cc @@ -7,7 +7,7 @@ * and also pins the throw-on-unsupported behaviour. Never loosen the tolerance * to make a case pass (the implementation notes, item 2). */ -#include "drake/planning/certified_ccd/bounding_sphere.h" +#include "drake/planning/continuous_collision/bounding_sphere.h" #include #include @@ -30,7 +30,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::geometry::Box; @@ -420,6 +420,6 @@ GTEST_TEST(BoundingSphereTest, ThrowsOnUnsupportedShape) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/certificate_test.cc b/planning/continuous_collision/test/certificate_test.cc similarity index 98% rename from planning/certified_ccd/test/certificate_test.cc rename to planning/continuous_collision/test/certificate_test.cc index a1b8cc75afe9..caf51dd54440 100644 --- a/planning/certified_ccd/test/certificate_test.cc +++ b/planning/continuous_collision/test/certificate_test.cc @@ -40,13 +40,13 @@ #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include "drake/planning/robot_diagram.h" #include "drake/planning/robot_diagram_builder.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::Parallelism; @@ -91,14 +91,14 @@ Options AuditOptions() { return options; } -std::unique_ptr MakeChecker( +std::unique_ptr MakeChecker( std::shared_ptr> model) { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = std::move(model); params.default_options = AuditOptions(); params.padding.env_padding = kEnvPadding; params.padding.self_padding = kEnvPadding; - return std::make_unique(params); + return std::make_unique(params); } // --------------------------------------------------------------------------- @@ -242,7 +242,7 @@ Eigen::MatrixXd CubicControlPoints(const VectorXd& start, const VectorXd& end) { struct AuditCase { std::string name; std::shared_ptr> model; - std::unique_ptr checker; + std::unique_ptr checker; Eigen::MatrixXd control_points; std::optional path; Certificate certificate; @@ -701,6 +701,6 @@ GTEST_TEST(CertificateAuditTest, } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/certifier_test.cc b/planning/continuous_collision/test/certifier_test.cc similarity index 97% rename from planning/certified_ccd/test/certifier_test.cc rename to planning/continuous_collision/test/certifier_test.cc index 4114ce6a5bb1..d6e52836f50e 100644 --- a/planning/certified_ccd/test/certifier_test.cc +++ b/planning/continuous_collision/test/certifier_test.cc @@ -28,13 +28,13 @@ #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include "drake/planning/robot_diagram.h" #include "drake/planning/robot_diagram_builder.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::Parallelism; @@ -159,12 +159,12 @@ std::shared_ptr> MakeGapWorld() { return std::shared_ptr>(builder.Build()); } -CertifiedContinuousCollisionChecker MakeChecker( +ContinuousCollisionChecker MakeChecker( std::shared_ptr> model, Options options) { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = std::move(model); params.default_options = std::move(options); - return CertifiedContinuousCollisionChecker(params); + return ContinuousCollisionChecker(params); } Options SerialOptions() { @@ -198,10 +198,9 @@ struct SampledClearance { /// is the independent check the certifier's continuum claim is measured /// against; it reuses the (separately tested, T3) distance oracle so that /// halfspace pairs are handled the same way. -SampledClearance SampleClearance( - const CertifiedContinuousCollisionChecker& checker, - const PiecewiseBezierPath& path, int samples_per_segment, - double threshold) { +SampledClearance SampleClearance(const ContinuousCollisionChecker& checker, + const PiecewiseBezierPath& path, + int samples_per_segment, double threshold) { const RobotDiagram& model = checker.model(); auto root = model.CreateDefaultContext(); auto& plant_context = model.plant().GetMyMutableContextFromRoot(root.get()); @@ -232,7 +231,7 @@ SampledClearance SampleClearance( /// Re-evaluates one finding's configuration from scratch and returns the /// oracle distance of its pair there. -double DistanceAtFinding(const CertifiedContinuousCollisionChecker& checker, +double DistanceAtFinding(const ContinuousCollisionChecker& checker, const Finding& finding) { const RobotDiagram& model = checker.model(); auto root = model.CreateDefaultContext(); @@ -498,11 +497,11 @@ GTEST_TEST(CertifierTest, PaddingSemantics) { // else, because every other pair has an anchored side and takes the (zero) // environment padding. { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = model; params.default_options = SerialOptions(); params.padding.self_padding = 0.40; - const CertifiedContinuousCollisionChecker checker(params); + const ContinuousCollisionChecker checker(params); const CertificationResult result = checker.CheckTrajectory(trajectory); ASSERT_EQ(result.verdict, Verdict::kViolationFound); for (const Finding& finding : result.findings) { @@ -514,11 +513,11 @@ GTEST_TEST(CertifierTest, PaddingSemantics) { // Mirrored: environment padding reaches the arm-vs-obstacle pairs (the // ground halfspace is 0.45 m away) and leaves the self pair alone. { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = model; params.default_options = SerialOptions(); params.padding.env_padding = 0.50; - const CertifiedContinuousCollisionChecker checker(params); + const ContinuousCollisionChecker checker(params); const CertificationResult result = checker.CheckTrajectory(trajectory); ASSERT_EQ(result.verdict, Verdict::kViolationFound); for (const Finding& finding : result.findings) { @@ -529,24 +528,24 @@ GTEST_TEST(CertifierTest, PaddingSemantics) { // A per-body-pair matrix overrides the scalars. { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = model; params.default_options = SerialOptions(); params.padding.env_padding = 0.50; params.padding.per_body_pair = Eigen::MatrixXd::Zero( model->plant().num_bodies(), model->plant().num_bodies()); - const CertifiedContinuousCollisionChecker checker(params); + const ContinuousCollisionChecker checker(params); EXPECT_EQ(checker.CheckTrajectory(trajectory).verdict, Verdict::kCertifiedFree); } // A mis-sized matrix is a clear throw. { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = model; params.default_options = SerialOptions(); params.padding.per_body_pair = Eigen::MatrixXd::Zero(2, 2); - EXPECT_THROW(CertifiedContinuousCollisionChecker{params}, std::exception); + EXPECT_THROW(ContinuousCollisionChecker{params}, std::exception); } } @@ -624,7 +623,7 @@ class CertificateFixture : public ::testing::Test { } std::shared_ptr> model_; - CertifiedContinuousCollisionChecker checker_; + ContinuousCollisionChecker checker_; BezierCurve trajectory_; PiecewiseBezierPath path_; CertificationResult result_; @@ -1022,11 +1021,11 @@ GTEST_TEST(CertifierTest, ApiThrowsOnBadOptions) { } GTEST_TEST(CertifierTest, ConstructorRejectsNullModel) { - CertifiedContinuousCollisionChecker::Params params; - EXPECT_THROW(CertifiedContinuousCollisionChecker{params}, std::exception); + ContinuousCollisionChecker::Params params; + EXPECT_THROW(ContinuousCollisionChecker{params}, std::exception); } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc similarity index 98% rename from planning/certified_ccd/test/concurrency_test.cc rename to planning/continuous_collision/test/concurrency_test.cc index 4cde52a7280e..d8eb4802aa4d 100644 --- a/planning/certified_ccd/test/concurrency_test.cc +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -31,7 +31,7 @@ /// TSan. This file is the test to run under ThreadSanitizer. Drake's /// build carries a `tsan` config, so the invocation is: /// -/// bazel test --config=tsan //planning/certified_ccd:concurrency_test +/// 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 with @@ -42,7 +42,7 @@ /// Result on Drake ~v1.45 at the time of writing: clean — no data races /// reported over repeated runs, so no suppression file is shipped. That was /// measured against a prebuilt (uninstrumented) Drake, so TSan saw only -/// certified_ccd frames. It sees all of the +/// continuous_collision frames. It sees all of the /// driver's shared mutable state, though — the work queue, the findings sink, /// the atomic node counter and bound, and the context pool are all ours — which /// is exactly the surface the design claims is the only one there is. If a @@ -50,7 +50,7 @@ /// does produce reports rooted entirely in Drake, triage them and park /// them in a suppression file (TSAN_OPTIONS=suppressions=...); anything /// rooted in a -/// certified_ccd frame is a real bug. +/// continuous_collision frame is a real bug. #include #include @@ -76,13 +76,13 @@ #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include "drake/planning/robot_diagram.h" #include "drake/planning/robot_diagram_builder.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::Parallelism; @@ -245,7 +245,7 @@ Options BaseOptions(Parallelism parallelism, SearchMode mode) { struct Case { std::string name; std::shared_ptr> model; - std::unique_ptr checker; + std::unique_ptr checker; Eigen::MatrixXd control_points; Verdict serial_verdict{}; @@ -271,12 +271,11 @@ const std::vector>& Corpus() { auto entry = std::make_unique(); entry->name = "seed_" + std::to_string(seed); entry->model = MakeWorld(seed); - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = entry->model; params.default_options = BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); - entry->checker = - std::make_unique(params); + entry->checker = std::make_unique(params); entry->control_points = MakeControlPoints(seed, entry->model->plant().num_positions()); const CertificationResult result = entry->checker->CheckTrajectory( @@ -860,6 +859,6 @@ GTEST_TEST(ConcurrencyTest, SmallCheckIsNotSlowerInParallel) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/distance_oracle_test.cc b/planning/continuous_collision/test/distance_oracle_test.cc similarity index 99% rename from planning/certified_ccd/test/distance_oracle_test.cc rename to planning/continuous_collision/test/distance_oracle_test.cc index cb48ed69683d..be5fb470a4df 100644 --- a/planning/certified_ccd/test/distance_oracle_test.cc +++ b/planning/continuous_collision/test/distance_oracle_test.cc @@ -6,7 +6,7 @@ /// Every world is built programmatically with RobotDiagramBuilder and every /// randomized case uses a fixed seed, so the suite is deterministic. -#include "drake/planning/certified_ccd/distance_oracle.h" +#include "drake/planning/continuous_collision/distance_oracle.h" #include #include @@ -36,13 +36,13 @@ #include "drake/multibody/plant/deformable_model.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/certified_ccd/vpolytope_ingestion.h" +#include "drake/planning/continuous_collision/vpolytope_ingestion.h" #include "drake/planning/robot_diagram.h" #include "drake/planning/robot_diagram_builder.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::geometry::Box; @@ -1129,6 +1129,6 @@ GTEST_TEST(VPolytopeIngestion, RejectsBadArguments) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/motion_bound_test.cc b/planning/continuous_collision/test/motion_bound_test.cc similarity index 99% rename from planning/certified_ccd/test/motion_bound_test.cc rename to planning/continuous_collision/test/motion_bound_test.cc index 6ddf5bfbd526..0ebb2cb43253 100644 --- a/planning/certified_ccd/test/motion_bound_test.cc +++ b/planning/continuous_collision/test/motion_bound_test.cc @@ -57,12 +57,12 @@ #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/certified_ccd/motion_bound_table.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" #include "drake/planning/robot_diagram_builder.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::geometry::Box; @@ -1811,6 +1811,6 @@ GTEST_TEST(CarveOutSlackTest, QuaternionFloatingLambdaTildeIsExactAndTight) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/piecewise_bezier_path_test.cc b/planning/continuous_collision/test/piecewise_bezier_path_test.cc similarity index 99% rename from planning/certified_ccd/test/piecewise_bezier_path_test.cc rename to planning/continuous_collision/test/piecewise_bezier_path_test.cc index 53db2150756b..0dbe0d5aac8b 100644 --- a/planning/certified_ccd/test/piecewise_bezier_path_test.cc +++ b/planning/continuous_collision/test/piecewise_bezier_path_test.cc @@ -5,7 +5,7 @@ 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/certified_ccd/piecewise_bezier_path.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include #include @@ -31,7 +31,7 @@ against themselves. */ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::copyable_unique_ptr; @@ -1130,6 +1130,6 @@ GTEST_TEST(Evaluation, JunctionTimesAreConsistent) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/soundness_fuzz_test.cc b/planning/continuous_collision/test/soundness_fuzz_test.cc similarity index 98% rename from planning/certified_ccd/test/soundness_fuzz_test.cc rename to planning/continuous_collision/test/soundness_fuzz_test.cc index 58422ead225b..7ac640893efc 100644 --- a/planning/certified_ccd/test/soundness_fuzz_test.cc +++ b/planning/continuous_collision/test/soundness_fuzz_test.cc @@ -62,13 +62,13 @@ #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include "drake/planning/robot_diagram.h" #include "drake/planning/robot_diagram_builder.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::Parallelism; @@ -568,7 +568,7 @@ std::optional LocalRadius(const Shape& shape) { /// that appear in some pair, their local radii, and each pair's two slots. class DenseScanner { public: - explicit DenseScanner(const CertifiedContinuousCollisionChecker& checker) + explicit DenseScanner(const ContinuousCollisionChecker& checker) : checker_(&checker), root_(checker.model().CreateDefaultContext()), plant_context_( @@ -702,7 +702,7 @@ class DenseScanner { } } - const CertifiedContinuousCollisionChecker* checker_{}; + const ContinuousCollisionChecker* checker_{}; std::unique_ptr> root_; drake::systems::Context* plant_context_{}; std::vector geometries_; @@ -758,12 +758,12 @@ Options FuzzOptions(double margin) { return options; } -CertifiedContinuousCollisionChecker MakeChecker( +ContinuousCollisionChecker MakeChecker( std::shared_ptr> model, const Options& options) { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = std::move(model); params.default_options = options; - return CertifiedContinuousCollisionChecker(params); + return ContinuousCollisionChecker(params); } GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { @@ -808,7 +808,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { bool grazing = (case_index % 5) == 3; if (grazing) { const Options probe_options = FuzzOptions(0.0); - const CertifiedContinuousCollisionChecker probe = + const ContinuousCollisionChecker probe = MakeChecker(model, probe_options); DenseScanner probe_scanner(probe); const DenseScanner::Result probe_scan = probe_scanner.Scan( @@ -827,8 +827,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { world.Describe() + trajectory_recipe.Describe()); const Options options = FuzzOptions(margin); - const CertifiedContinuousCollisionChecker checker = - MakeChecker(model, options); + const ContinuousCollisionChecker checker = MakeChecker(model, options); // This fuzz never sets a PaddingSpec, so m_p = margin for every pair; the // dense scan relies on that to compare against one number. for (const PairRecord& pair : checker.pairs()) { @@ -999,6 +998,6 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/test/thin_obstacle_test.cc b/planning/continuous_collision/test/thin_obstacle_test.cc similarity index 96% rename from planning/certified_ccd/test/thin_obstacle_test.cc rename to planning/continuous_collision/test/thin_obstacle_test.cc index f4d504e3a740..59e8c193cdf8 100644 --- a/planning/certified_ccd/test/thin_obstacle_test.cc +++ b/planning/continuous_collision/test/thin_obstacle_test.cc @@ -34,15 +34,15 @@ #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/certified_ccd/certified_continuous_collision_checker.h" #include "drake/planning/collision_checker_params.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" #include "drake/planning/robot_diagram.h" #include "drake/planning/robot_diagram_builder.h" #include "drake/planning/scene_graph_collision_checker.h" namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { namespace { using drake::Parallelism; @@ -185,13 +185,13 @@ SceneGraphCollisionChecker MakeDrakeChecker( return SceneGraphCollisionChecker(std::move(params)); } -CertifiedContinuousCollisionChecker MakeCertifiedChecker( +ContinuousCollisionChecker MakeCertifiedChecker( std::shared_ptr> model) { - CertifiedContinuousCollisionChecker::Params params; + ContinuousCollisionChecker::Params params; params.model = std::move(model); params.default_options.margin = 0.0; params.default_options.parallelism = Parallelism::None(); - return CertifiedContinuousCollisionChecker(params); + return ContinuousCollisionChecker(params); } VectorXd MakeQ(double x, double y) { @@ -210,7 +210,7 @@ Eigen::MatrixXd Waypoints(const VectorXd& q1, const VectorXd& q2) { /// Signed distance of `finding`'s pair, re-measured from a fresh context at the /// witness configuration: the independent confirmation that the witness is a /// real contact and not an artifact of the search. -double DistanceAtFinding(const CertifiedContinuousCollisionChecker& checker, +double DistanceAtFinding(const ContinuousCollisionChecker& checker, const Finding& finding) { const RobotDiagram& model = checker.model(); auto root = model.CreateDefaultContext(); @@ -306,8 +306,7 @@ GTEST_TEST(ThinObstacleTest, CertifiedCheckerCatchesTheThinPlate) { const VectorXd q2 = MakeQ(0.5, 0.0); std::shared_ptr> model = MakePlateWorld(kPlateX, kPlateThickness); - const CertifiedContinuousCollisionChecker checker = - MakeCertifiedChecker(model); + const ContinuousCollisionChecker checker = MakeCertifiedChecker(model); const CertificationResult result = checker.CheckEdge(q1, q2); ASSERT_EQ(result.verdict, Verdict::kViolationFound); @@ -356,8 +355,7 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { static_assert(kClearance > 0.0); std::shared_ptr> model = MakeSlotWorld(kHalfGap); - const CertifiedContinuousCollisionChecker checker = - MakeCertifiedChecker(model); + const ContinuousCollisionChecker checker = MakeCertifiedChecker(model); const VectorXd q1 = MakeQ(-0.3, 0.0); const VectorXd q2 = MakeQ(0.3, 0.0); @@ -428,7 +426,7 @@ GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { << " m, tool radius = " << kToolRadius << " m, Drake edge_step_size = " << kDrakeEdgeStepSize << " m (samples 0.05 m apart in x)\n" - << " thickness[m] drake_sampled certified_ccd " + << " thickness[m] drake_sampled continuous_collision " "contact_half_width[m]\n"; for (const double thickness : thicknesses) { SCOPED_TRACE("thickness = " + std::to_string(thickness)); @@ -438,8 +436,7 @@ GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { std::shared_ptr> model = MakePlateWorld(kPlateX, thickness); - const CertifiedContinuousCollisionChecker checker = - MakeCertifiedChecker(model); + const ContinuousCollisionChecker checker = MakeCertifiedChecker(model); const CertificationResult result = checker.CheckEdge(q1, q2); if (!drake_free && std::isnan(first_caught)) first_caught = thickness; @@ -463,6 +460,6 @@ GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { } } // namespace -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/vpolytope_ingestion.cc b/planning/continuous_collision/vpolytope_ingestion.cc similarity index 93% rename from planning/certified_ccd/vpolytope_ingestion.cc rename to planning/continuous_collision/vpolytope_ingestion.cc index e3414aeb12bb..77653c3a65f6 100644 --- a/planning/certified_ccd/vpolytope_ingestion.cc +++ b/planning/continuous_collision/vpolytope_ingestion.cc @@ -1,4 +1,4 @@ -#include "drake/planning/certified_ccd/vpolytope_ingestion.h" +#include "drake/planning/continuous_collision/vpolytope_ingestion.h" #include @@ -8,7 +8,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { using drake::geometry::GeometryId; using drake::math::RigidTransformd; @@ -57,6 +57,6 @@ GeometryId AddVPolytopeObstacle( CoulombFriction(kDefaultFriction, kDefaultFriction)); } -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/certified_ccd/vpolytope_ingestion.h b/planning/continuous_collision/vpolytope_ingestion.h similarity index 96% rename from planning/certified_ccd/vpolytope_ingestion.h rename to planning/continuous_collision/vpolytope_ingestion.h index 4bdca888df00..dec84477c447 100644 --- a/planning/certified_ccd/vpolytope_ingestion.h +++ b/planning/continuous_collision/vpolytope_ingestion.h @@ -9,7 +9,7 @@ namespace drake { namespace planning { -namespace certified_ccd { +namespace continuous_collision { /** Registers a V-polytope as an anchored obstacle with a collision role (the geometry-support scope, "V-polytopes as first-class geometry", ingestion @@ -44,6 +44,6 @@ drake::geometry::GeometryId AddVPolytopeObstacle( const drake::geometry::optimization::VPolytope& vpoly, const drake::math::RigidTransform& X_WG, const std::string& name); -} // namespace certified_ccd +} // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/tools/install/libdrake/build_components.bzl b/tools/install/libdrake/build_components.bzl index 6acfabea0880..b6c1fa8efca7 100644 --- a/tools/install/libdrake/build_components.bzl +++ b/tools/install/libdrake/build_components.bzl @@ -76,7 +76,7 @@ LIBDRAKE_COMPONENTS = [ "//multibody/triangle_quadrature", "//perception", "//planning", - "//planning/certified_ccd", + "//planning/continuous_collision", "//planning/experimental", "//planning/graph_algorithms", "//planning/iris", From 7dec93a9a765b83e583ba067cc860519ab232c05 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 27 Aug 2026 14:30:58 -0400 Subject: [PATCH 10/22] [planning] continuous_collision: apply style and API review fixes Placement and API: - Renames certifier.h to certifier_internal.h and excludes it from the installed headers. - Caps the worker pool at Parallelism::Max().num_threads() rather than std::thread::hardware_concurrency(), so it honours DRAKE_NUM_THREADS. - Adds @ingroup planning_collision_checker to every public type and free function. - Gives every class an explicit copy/move declaration: the value types are DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN, ContinuousCollisionChecker and the thread/context pools are DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN. - Replaces the local StrCat helper and the throwing paths' ostringstream and string concatenation with fmt::format. - Renames the benchmark's `benchmark` namespace to `internal`. - Narrows to in the headers. Documentation: - PaddingSpec now documents the rule the code implements (env vs self is decided by anchoring; NaN matrix entries fall back to the scalars) instead of claiming to mirror CollisionChecker. - DistanceOracle documents that collision filters are snapshotted at construction. - ContinuousCollisionChecker documents that concurrent Check* calls from arbitrary threads are supported. - The world-rooted tree walk is described as an independent cross-check of GetBodiesKinematicallyAffectedBy(), which is what the code does. Style: - Converts internal-invariant DRAKE_THROW_UNLESS to DRAKE_DEMAND, leaving the user-facing checks as throws. - Replaces MotionBoundTable's four mutable_*() builder accessors with a validating constructor, and renames entries() to GetEntries(). - Makes DistanceOracle's members private and support_report() return a reference. - Drops redundant drake:: qualification inside namespace drake, removes unused includes and adds missing direct ones. --- planning/continuous_collision/BUILD.bazel | 20 +- .../benchmark/benchmark_util.cc | 4 +- .../benchmark/benchmark_util.h | 24 +-- .../benchmark/iiwa_benchmark.cc | 8 +- .../benchmark/scenario_worlds.cc | 4 +- .../benchmark/scenario_worlds.h | 12 +- .../continuous_collision/bounding_sphere.cc | 12 +- .../continuous_collision/bounding_sphere.h | 13 +- planning/continuous_collision/certificate.cc | 18 +- planning/continuous_collision/certificate.h | 8 +- planning/continuous_collision/certifier.cc | 68 ++++--- .../{certifier.h => certifier_internal.h} | 46 ++--- .../continuous_collision_checker.cc | 2 +- .../continuous_collision_checker.h | 30 ++- .../continuous_collision/distance_oracle.cc | 68 +++---- .../continuous_collision/distance_oracle.h | 36 ++-- .../motion_bound_table.cc | 82 +++++--- .../continuous_collision/motion_bound_table.h | 82 +++++--- planning/continuous_collision/numerics.h | 14 +- planning/continuous_collision/options.h | 57 ++++-- .../piecewise_bezier_path.cc | 192 +++++++++--------- .../piecewise_bezier_path.h | 16 +- .../test/concurrency_test.cc | 4 +- .../test/motion_bound_test.cc | 38 ++-- .../vpolytope_ingestion.cc | 34 ++-- .../vpolytope_ingestion.h | 11 +- 26 files changed, 497 insertions(+), 406 deletions(-) rename planning/continuous_collision/{certifier.h => certifier_internal.h} (92%) diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index 10cc639902ae..22eaf4de0ed9 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -47,11 +47,11 @@ drake_cc_library( hdrs = ["piecewise_bezier_path.h"], deps = [ ":options", + "//common:essential", "//common/trajectories:trajectory", "@eigen", ], implementation_deps = [ - "//common:essential", "//common:nice_type_name", "//common/trajectories:bezier_curve", "//common/trajectories:bspline_trajectory", @@ -85,11 +85,14 @@ drake_cc_library( ":bounding_sphere", ":options", ":piecewise_bezier_path", + "//common:essential", + "//geometry:geometry_ids", + "//multibody/plant", + "//multibody/tree:multibody_tree_indexes", "//planning:robot_diagram", "@eigen", ], implementation_deps = [ - "//common:essential", "//geometry:geometry_roles", "//geometry:scene_graph_inspector", "//geometry:shape_specification", @@ -104,18 +107,19 @@ drake_cc_library( hdrs = ["distance_oracle.h"], deps = [ ":options", + "//common:essential", "//geometry:scene_graph", "//planning:robot_diagram", "@eigen", ], implementation_deps = [ - "//common:essential", "//common:unused", "//geometry:scene_graph_inspector", "//geometry:shape_specification", "//geometry/proximity:polygon_surface_mesh", "//math:geometric_transform", "//multibody/plant", + "@fmt", ], ) @@ -132,6 +136,7 @@ drake_cc_library( implementation_deps = [ "//common:essential", "//geometry:shape_specification", + "@fmt", ], ) @@ -147,22 +152,25 @@ drake_cc_library( ], hdrs = [ "certificate.h", - "certifier.h", + "certifier_internal.h", ], + install_hdrs_exclude = ["certifier_internal.h"], deps = [ ":distance_oracle", ":motion_bound_table", ":numerics", ":options", ":piecewise_bezier_path", + "//common:essential", + "//common:parallelism", "//geometry:scene_graph", + "//math:geometric_transform", "//multibody/tree:multibody_tree_indexes", "//planning:robot_diagram", "//systems/framework:context", "@eigen", ], implementation_deps = [ - "//common:essential", "//multibody/plant", "@fmt", ], @@ -178,12 +186,12 @@ drake_cc_library( ":motion_bound_table", ":options", ":piecewise_bezier_path", + "//common:essential", "//common/trajectories:trajectory", "//planning:robot_diagram", "@eigen", ], implementation_deps = [ - "//common:essential", "//common:unused", "//geometry:scene_graph", "//geometry:scene_graph_inspector", diff --git a/planning/continuous_collision/benchmark/benchmark_util.cc b/planning/continuous_collision/benchmark/benchmark_util.cc index 06831f718c73..bef4f05a1af1 100644 --- a/planning/continuous_collision/benchmark/benchmark_util.cc +++ b/planning/continuous_collision/benchmark/benchmark_util.cc @@ -18,7 +18,7 @@ namespace drake { namespace planning { namespace continuous_collision { -namespace benchmark { +namespace internal { namespace { using drake::geometry::GeometryId; @@ -475,7 +475,7 @@ double BisectMonotone(const std::function& f, double lo, return 0.5 * (a + b); } -} // namespace benchmark +} // namespace internal } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/benchmark/benchmark_util.h b/planning/continuous_collision/benchmark/benchmark_util.h index 2abe42a08d9d..a919046b351b 100644 --- a/planning/continuous_collision/benchmark/benchmark_util.h +++ b/planning/continuous_collision/benchmark/benchmark_util.h @@ -19,7 +19,7 @@ #include #include -#include +#include #include "drake/common/trajectories/composite_trajectory.h" #include "drake/common/trajectories/trajectory.h" @@ -29,7 +29,7 @@ namespace drake { namespace planning { namespace continuous_collision { -namespace benchmark { +namespace internal { // --------------------------------------------------------------------------- // JSON @@ -145,7 +145,7 @@ void WriteMachine(JsonWriter* json, const MachineInfo& machine); /// P1 = P0 + h v0/5, P4 = P5 − h v1/5, /// P2 = P0 + 2 h v0/5, P3 = P5 − 2 h v1/5, /// which reproduces q(t0)=q0, q̇(t0)=v0, q̈(t0)=0 and likewise at t1. -std::shared_ptr> +std::shared_ptr> MakeQuinticCompositeBezier(const Eigen::MatrixXd& waypoints, const std::vector& times); @@ -154,13 +154,13 @@ MakeQuinticCompositeBezier(const Eigen::MatrixXd& waypoints, /// weights are 1 for every non-quaternion coordinate), integrated along the /// trajectory with `num_samples` chords. Used to derive the number of samples /// a sampled checker would take at a given edge_step_size. -double PathLengthInEdgeMetric(const drake::trajectories::Trajectory& t, +double PathLengthInEdgeMetric(const trajectories::Trajectory& t, int num_samples); /// Samples `count` configurations uniformly in trajectory time (inclusive of /// both endpoints). std::vector SampleTrajectory( - const drake::trajectories::Trajectory& trajectory, int count); + const trajectories::Trajectory& trajectory, int count); // --------------------------------------------------------------------------- // Ground-truth swept clearance @@ -181,8 +181,8 @@ struct ClearanceReport { }; /// Geometry ids belonging to bodies of the named model instances. -std::unordered_set CollectGeometryIds( - const drake::planning::RobotDiagram& diagram, +std::unordered_set CollectGeometryIds( + const RobotDiagram& diagram, const std::vector& model_instance_names); /// Dense-samples `trajectory` (`num_samples` configurations, split over @@ -190,17 +190,17 @@ std::unordered_set CollectGeometryIds( /// Distances beyond `max_distance` are not resolved; if no pair comes within /// it the reported minimum saturates at `max_distance`. ClearanceReport MeasureSweptClearance( - const drake::planning::RobotDiagram& diagram, - const drake::trajectories::Trajectory& trajectory, - const std::unordered_set& env_ids, - int num_samples, int num_threads, double max_distance); + const RobotDiagram& diagram, + const trajectories::Trajectory& trajectory, + const std::unordered_set& env_ids, int num_samples, + int num_threads, double max_distance); /// Bisects `f` (assumed non-decreasing) on [lo, hi] for f(x) = target. /// Returns x. Used to place the shelf at a requested swept clearance. double BisectMonotone(const std::function& f, double lo, double hi, double target, int iterations); -} // namespace benchmark +} // namespace internal } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/benchmark/iiwa_benchmark.cc b/planning/continuous_collision/benchmark/iiwa_benchmark.cc index fafd970b251b..bc2983fe9b5d 100644 --- a/planning/continuous_collision/benchmark/iiwa_benchmark.cc +++ b/planning/continuous_collision/benchmark/iiwa_benchmark.cc @@ -53,7 +53,7 @@ namespace drake { namespace planning { namespace continuous_collision { -namespace benchmark { +namespace internal { namespace { using drake::Parallelism; @@ -887,7 +887,7 @@ void RunProfile(const Config& config, const MachineInfo& machine, // speedup a 6-segment trajectory can reach is total work / heaviest segment. // Certifying each segment on its own measures it directly. The driver no // longer works that way — it shares sub-segment nodes on demand (see - // certifier.h) — so this row is now a *reference* bound + // certifier_internal.h) — so this row is now a *reference* bound // that the measured per-call speedup is allowed to exceed, and the record of // why the old driver could not. { @@ -1158,14 +1158,14 @@ int Main(int argc, char** argv) { } } // namespace -} // namespace benchmark +} // namespace internal } // namespace continuous_collision } // namespace planning } // namespace drake int main(int argc, char** argv) { try { - return drake::planning::continuous_collision::benchmark::Main(argc, argv); + return drake::planning::continuous_collision::internal::Main(argc, argv); } catch (const std::exception& e) { std::fprintf(stderr, "benchmark failed: %s\n", e.what()); return 1; diff --git a/planning/continuous_collision/benchmark/scenario_worlds.cc b/planning/continuous_collision/benchmark/scenario_worlds.cc index 3438a4e0670a..20d7ce8e8dc6 100644 --- a/planning/continuous_collision/benchmark/scenario_worlds.cc +++ b/planning/continuous_collision/benchmark/scenario_worlds.cc @@ -14,7 +14,7 @@ namespace drake { namespace planning { namespace continuous_collision { -namespace benchmark { +namespace internal { namespace { using drake::geometry::Box; @@ -176,7 +176,7 @@ std::vector DualArmTrajectoryTimes() { return {0.0, 1.0, 2.0, 3.0, 4.0}; } -} // namespace benchmark +} // namespace internal } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/benchmark/scenario_worlds.h b/planning/continuous_collision/benchmark/scenario_worlds.h index 37abb71143c5..f3b7ea8cf162 100644 --- a/planning/continuous_collision/benchmark/scenario_worlds.h +++ b/planning/continuous_collision/benchmark/scenario_worlds.h @@ -10,14 +10,14 @@ #include #include -#include +#include #include "drake/planning/robot_diagram.h" namespace drake { namespace planning { namespace continuous_collision { -namespace benchmark { +namespace internal { /// The dense-sphere iiwa14 collision variant: 46 collision spheres over /// links 0-7, i.e. realistic proximity-pair counts (the benchmark suite asks @@ -41,14 +41,12 @@ struct ShelfGeometry { /// iiwa14 welded to the world origin, a 3 m table slab, and a seven-box /// bookcase in reach. Model instances are named "iiwa14" and "environment". -std::shared_ptr> MakeShelfWorld( - double shelf_scale); +std::shared_ptr> MakeShelfWorld(double shelf_scale); /// Two iiwa14s welded to the world `base_separation` apart along +x, the /// second rotated 180 degrees about z so the arms face each other, over the /// same table slab. Model instances: "iiwa14", "iiwa14_1", "environment". -std::shared_ptr> MakeDualArmWorld( - double base_separation); +std::shared_ptr> MakeDualArmWorld(double base_separation); /// The 7 x 7 joint-space waypoint matrix of the shelf-reaching trajectory: /// home, up-and-over on the +y side, into the bay mouth, deep inside the bay, @@ -68,7 +66,7 @@ Eigen::MatrixXd DualArmTrajectoryWaypoints(); std::vector DualArmTrajectoryTimes(); -} // namespace benchmark +} // namespace internal } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/bounding_sphere.cc b/planning/continuous_collision/bounding_sphere.cc index d8caef5c39c6..bdd13cc354d5 100644 --- a/planning/continuous_collision/bounding_sphere.cc +++ b/planning/continuous_collision/bounding_sphere.cc @@ -7,7 +7,7 @@ #include -#include "drake/common/drake_throw.h" +#include "drake/common/drake_assert.h" #include "drake/geometry/proximity/polygon_surface_mesh.h" namespace drake { @@ -123,8 +123,8 @@ class BoundingSphereReifier final : public ShapeReifier { /* 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_THROW_UNLESS(std::isfinite(radius_about_Go)); - DRAKE_THROW_UNLESS(radius_about_Go >= 0.0); + 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; } @@ -139,7 +139,7 @@ class BoundingSphereReifier final : public ShapeReifier { // 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_THROW_UNLESS(num_vertices > 0); + 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); @@ -167,8 +167,8 @@ BoundingSphere ComputeBoundingSphere(const Shape& shape, // A silently-zero or non-finite radius is the exact failure mode the // geometry-support scope warns about, so re-assert the postcondition every // caller relies on. - DRAKE_THROW_UNLESS(std::isfinite(result.radius) && result.radius >= 0.0); - DRAKE_THROW_UNLESS(result.center_L.allFinite()); + DRAKE_DEMAND(std::isfinite(result.radius) && result.radius >= 0.0); + DRAKE_DEMAND(result.center_L.allFinite()); return result; } diff --git a/planning/continuous_collision/bounding_sphere.h b/planning/continuous_collision/bounding_sphere.h index 4dcdbdb7e35e..c2cd4007c37c 100644 --- a/planning/continuous_collision/bounding_sphere.h +++ b/planning/continuous_collision/bounding_sphere.h @@ -1,6 +1,6 @@ #pragma once -#include +#include #include "drake/geometry/shape_specification.h" #include "drake/math/rigid_transform.h" @@ -10,7 +10,8 @@ namespace planning { namespace continuous_collision { /** A sphere, expressed in the owning body (link) frame L, that contains a -proximity geometry at every configuration of the body. */ +proximity geometry at every configuration of the body. +@ingroup planning_collision_checker */ struct BoundingSphere { /** Sphere center in the body frame. */ Eigen::Vector3d center_L{Eigen::Vector3d::Zero()}; @@ -40,10 +41,10 @@ containment per shape: λ soundness dies quietly if any formula under-bounds, so this function switches on the closed set of supported shape types and @throws std::exception on anything else (HalfSpace included — halfspaces are -handled by dedicated rules, never through a bounding sphere). */ -BoundingSphere ComputeBoundingSphere( - const drake::geometry::Shape& shape, - const drake::math::RigidTransform& X_LG); +handled by dedicated rules, never through a bounding sphere). +@ingroup planning_collision_checker */ +BoundingSphere ComputeBoundingSphere(const geometry::Shape& shape, + const math::RigidTransform& X_LG); } // namespace continuous_collision } // namespace planning diff --git a/planning/continuous_collision/certificate.cc b/planning/continuous_collision/certificate.cc index 42eaddc241f0..bd1253dad60f 100644 --- a/planning/continuous_collision/certificate.cc +++ b/planning/continuous_collision/certificate.cc @@ -10,8 +10,8 @@ #include -#include "drake/common/drake_throw.h" -#include "drake/planning/continuous_collision/certifier.h" +#include "drake/common/drake_assert.h" +#include "drake/planning/continuous_collision/certifier_internal.h" #include "drake/planning/continuous_collision/numerics.h" namespace drake { @@ -53,7 +53,7 @@ void SplitAt(const Eigen::MatrixXd& cps, double u, Eigen::MatrixXd* left, void RestrictBezier(const Eigen::MatrixXd& cps, double a, double b, Eigen::MatrixXd* out) { - DRAKE_THROW_UNLESS(out != nullptr); + DRAKE_DEMAND(out != nullptr); const double lo = std::clamp(a, 0.0, 1.0); const double hi = std::clamp(b, 0.0, 1.0); if (lo <= 0.0 && hi >= 1.0) { @@ -91,12 +91,12 @@ Eigen::VectorXd EvaluateBezier(const Eigen::MatrixXd& cps, double u) { bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, std::string* message) { - DRAKE_THROW_UNLESS(input.model != nullptr); - DRAKE_THROW_UNLESS(input.oracle != nullptr); - DRAKE_THROW_UNLESS(input.table != nullptr); - DRAKE_THROW_UNLESS(input.path != nullptr); - DRAKE_THROW_UNLESS(input.pairs != nullptr); - DRAKE_THROW_UNLESS(input.tau != nullptr); + DRAKE_DEMAND(input.model != nullptr); + 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); const auto fail = [message](std::string reason) { if (message != nullptr) *message = std::move(reason); diff --git a/planning/continuous_collision/certificate.h b/planning/continuous_collision/certificate.h index 343bded9f0f4..248aee26b05c 100644 --- a/planning/continuous_collision/certificate.h +++ b/planning/continuous_collision/certificate.h @@ -2,7 +2,7 @@ #include -#include +#include #include "drake/planning/continuous_collision/options.h" @@ -12,7 +12,8 @@ namespace continuous_collision { /** One certification event: pair `pair_index` was certified over the parameter interval [s_start, s_end] of segment `segment` from representative -configuration qc (the search algorithm). */ +configuration qc (the search algorithm). +@ingroup planning_collision_checker */ struct CertificateRecord { int segment{}; double s_start{}; @@ -26,7 +27,8 @@ struct CertificateRecord { /** Audit trail of every certification event of a run; an independent replay (VerifyCertificate, declared in the api header) re-evaluates every -record and checks interval coverage of the full domain per pair. */ +record and checks interval coverage of the full domain per pair. +@ingroup planning_collision_checker */ struct Certificate { std::vector records; /** Pair table snapshot the indices refer to. */ diff --git a/planning/continuous_collision/certifier.cc b/planning/continuous_collision/certifier.cc index 57555bf5058a..1f27ee36af84 100644 --- a/planning/continuous_collision/certifier.cc +++ b/planning/continuous_collision/certifier.cc @@ -1,9 +1,8 @@ -#include "drake/planning/continuous_collision/certifier.h" - #include #include #include #include +#include #include #include #include @@ -11,10 +10,13 @@ #include #include #include +#include -#include "drake/common/drake_throw.h" +#include "drake/common/drake_assert.h" +#include "drake/common/parallelism.h" #include "drake/geometry/scene_graph.h" #include "drake/multibody/plant/multibody_plant.h" +#include "drake/planning/continuous_collision/certifier_internal.h" #include "drake/planning/continuous_collision/numerics.h" namespace drake { @@ -250,12 +252,12 @@ class WorkQueue { 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. */ + /* The sharing policy (see certifier_internal.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); @@ -485,9 +487,10 @@ void Worker::RunItem(WorkItem* item) { ++stats_.nodes; stats_.max_depth = std::max(stats_.max_depth, frame.depth); - // 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_`. + // Lazy recruitment (see certifier_internal.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 >= recruit_->nodes_before_hire) { Recruitment* const recruitment = recruit_; @@ -641,12 +644,12 @@ void Worker::RunItem(WorkItem* item) { const NodeFrame left{frame.s_lo, s_mid, frame.depth + 1, 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. + // Occupancy-driven sharing (see certifier_internal.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; @@ -841,7 +844,7 @@ ContextPool::ContextPool(const drake::planning::RobotDiagram& model, } ContextPool::Lease ContextPool::Acquire(int count) const { - DRAKE_THROW_UNLESS(count >= 1); + DRAKE_DEMAND(count >= 1); std::vector contexts; std::vector slots; contexts.reserve(count); @@ -950,9 +953,10 @@ WorkerPool::Batch WorkerPool::Reserve(int count) { // Bounding the pool by the machine's width keeps a program that runs many // concurrent parallel checks from multiplying threads without limit; a call // that finds nothing free simply runs with fewer workers, which is only a - // performance difference. - const int cap = - std::max(1, static_cast(std::thread::hardware_concurrency())); + // performance difference. The width comes from Parallelism::Max() rather + // than hardware_concurrency() directly, so the cap honours DRAKE_NUM_THREADS + // like the rest of Drake. + const int cap = Parallelism::Max().num_threads(); std::lock_guard guard(mutex_); if (shutdown_) return batch; while (static_cast(batch.slots_.size()) < count && !idle_.empty()) { @@ -1024,7 +1028,7 @@ void WorkerPool::Release(const std::vector& slots) { void WorkerPool::Batch::Dispatch(const std::function& task) { if (handles_.empty()) return; - DRAKE_THROW_UNLESS(state_ == nullptr); + DRAKE_DEMAND(state_ == nullptr); state_ = std::make_shared(); state_->remaining = static_cast(handles_.size()); for (int i = 0; i < static_cast(handles_.size()); ++i) { @@ -1077,14 +1081,14 @@ WorkerPool::Batch::~Batch() { CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, WorkerPool* workers) { - DRAKE_THROW_UNLESS(input.model != nullptr); - DRAKE_THROW_UNLESS(input.oracle != nullptr); - DRAKE_THROW_UNLESS(input.table != nullptr); - DRAKE_THROW_UNLESS(input.path != nullptr); - DRAKE_THROW_UNLESS(input.pairs != nullptr); - DRAKE_THROW_UNLESS(input.tau != nullptr); - DRAKE_THROW_UNLESS(input.prefilter != nullptr); - DRAKE_THROW_UNLESS(pool != nullptr); + DRAKE_DEMAND(input.model != nullptr); + 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 Options& options = input.options; const PiecewiseBezierPath& path = *input.path; @@ -1174,7 +1178,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, } else if (have_work) { // Parallel driver: lazy recruitment + occupancy-driven sharing. The full // rationale — and the deviation from parallelism and determinism's static - // seeding — is documented on RunCertifier() in certifier.h. + // seeding — is documented on RunCertifier() in certifier_internal.h. WorkQueue queue; { // Seeded in reverse so the LIFO hands segment 0 out first. Before any diff --git a/planning/continuous_collision/certifier.h b/planning/continuous_collision/certifier_internal.h similarity index 92% rename from planning/continuous_collision/certifier.h rename to planning/continuous_collision/certifier_internal.h index 516e4ee6df03..c077efdad65c 100644 --- a/planning/continuous_collision/certifier.h +++ b/planning/continuous_collision/certifier_internal.h @@ -11,20 +11,19 @@ /// one set of per-call data structures without the core module depending on /// the api layer. -#include -#include #include #include #include #include #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/continuous_collision/certificate.h" #include "drake/planning/continuous_collision/distance_oracle.h" @@ -45,12 +44,11 @@ scene-graph sub-contexts pulled out of it once, so the hot loop pays a single bookkeeping. */ class ThreadContext { public: - ThreadContext(const ThreadContext&) = delete; - ThreadContext& operator=(const ThreadContext&) = delete; + 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 drake::planning::RobotDiagram& model); + explicit ThreadContext(const RobotDiagram& model); /** The one FK trigger per node: sets the plant's generalized positions. Drake caches forward kinematics per context afterwards, so body poses and @@ -59,18 +57,18 @@ class ThreadContext { void SetPositions(const Eigen::VectorXd& q); /** The scene graph's query object at the configuration last set. */ - const drake::geometry::QueryObject& query_object() const; + const geometry::QueryObject& query_object() const; /** World pose of `body` at the configuration last set (Drake's cache computes it on first use and reuses it afterwards). */ - const drake::math::RigidTransform& EvalBodyPose( - drake::multibody::BodyIndex body) const; + const math::RigidTransform& EvalBodyPose( + multibody::BodyIndex body) const; private: - const drake::planning::RobotDiagram* model_{}; - std::unique_ptr> root_; - drake::systems::Context* plant_context_{}; - const drake::systems::Context* scene_graph_context_{}; + const RobotDiagram* model_{}; + std::unique_ptr> root_; + systems::Context* plant_context_{}; + const systems::Context* scene_graph_context_{}; }; /** A checkout pool of ThreadContexts (parallelism and determinism: @@ -83,13 +81,11 @@ ever share one. A lease larger than the pre-warmed pool grows it (a cold-path allocation); nothing shrinks it. */ class ContextPool { public: - ContextPool(const ContextPool&) = delete; - ContextPool& operator=(const ContextPool&) = delete; + 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 drake::planning::RobotDiagram& model, - int initial_size); + ContextPool(const RobotDiagram& model, int initial_size); /** RAII handle for a set of leased contexts. */ class Lease { @@ -128,7 +124,7 @@ class ContextPool { private: void Release(const std::vector& slots) const; - const drake::planning::RobotDiagram* model_{}; + const RobotDiagram* model_{}; mutable std::mutex mutex_; /* A deque so that growing never invalidates the ThreadContext addresses already handed out. */ @@ -147,7 +143,7 @@ recruitment policy of RunCertifier() affordable: the driver can afford to start serial and hire only once a run has proved itself big enough. Threads are created on demand (never at construction), capped at -`std::thread::hardware_concurrency()` per pool, parked on their own condition +`Parallelism::Max().num_threads()` per pool, parked on their own condition variable when idle, and joined by the destructor. A `Batch` is a reservation of some of them for the duration of one call; because reservations never block, several concurrent `Check*` calls simply share out whatever threads exist and a @@ -158,10 +154,10 @@ class WorkerPool { struct BatchState; public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(WorkerPool); + /* Out of line (like the destructor) because Slot is incomplete here. */ WorkerPool(); - WorkerPool(const WorkerPool&) = delete; - WorkerPool& operator=(const WorkerPool&) = delete; ~WorkerPool(); /** Reserved threads for one call. Destruction waits for every dispatched @@ -217,7 +213,7 @@ slots so the node loop can cache one world-frame center per geometry per node. */ struct PrefilterTable { struct Geometry { - drake::multibody::BodyIndex body; + multibody::BodyIndex body; Eigen::Vector3d center_L{Eigen::Vector3d::Zero()}; double radius{0.0}; }; @@ -234,7 +230,7 @@ struct PrefilterTable { /** Everything one certification run needs; assembled by the facade. All pointers are aliased and must outlive the call. */ struct CertifierInput { - const drake::planning::RobotDiagram* model{}; + const RobotDiagram* model{}; const DistanceOracle* oracle{}; const MotionBoundTable* table{}; const PiecewiseBezierPath* path{}; @@ -352,7 +348,7 @@ Eigen::VectorXd EvaluateBezier(const Eigen::MatrixXd& cps, double u); /** Inputs of the independent certificate replay. All pointers are aliased. */ struct ReplayInput { - const drake::planning::RobotDiagram* model{}; + const RobotDiagram* model{}; const DistanceOracle* oracle{}; const MotionBoundTable* table{}; const PiecewiseBezierPath* path{}; diff --git a/planning/continuous_collision/continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc index 45d2c05406d1..4d2d6626290d 100644 --- a/planning/continuous_collision/continuous_collision_checker.cc +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -39,7 +39,7 @@ #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/certifier_internal.h" namespace drake { namespace planning { diff --git a/planning/continuous_collision/continuous_collision_checker.h b/planning/continuous_collision/continuous_collision_checker.h index 777756603dce..20bf9073fa4b 100644 --- a/planning/continuous_collision/continuous_collision_checker.h +++ b/planning/continuous_collision/continuous_collision_checker.h @@ -4,8 +4,9 @@ #include #include -#include +#include +#include "drake/common/drake_copyable.h" #include "drake/common/trajectories/trajectory.h" #include "drake/planning/continuous_collision/certificate.h" #include "drake/planning/continuous_collision/distance_oracle.h" @@ -18,7 +19,8 @@ namespace drake { namespace planning { namespace continuous_collision { -/** Result of one certification call (the architecture). */ +/** Result of one certification call (the architecture). +@ingroup planning_collision_checker */ struct CertificationResult { Verdict verdict{}; /** Earliest-first. */ @@ -41,14 +43,21 @@ the continuum of configurations, not about samples. The certificate is a property of the path, so retiming the trajectory afterwards does not invalidate it. -Thread-compatible: the Check* methods are const, own no mutable state -outside per-call scratch, and are safe to call concurrently. */ +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 deliberately 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); + struct Params { /** Plant + scene graph; the plant must be finalized. */ - std::shared_ptr> model; - /** Per-body-pair padding, drake::planning::CollisionChecker semantics. */ + std::shared_ptr> model; + /** Per-body-pair padding; see PaddingSpec for the env/self rule. */ PaddingSpec padding{}; Options default_options{}; }; @@ -62,7 +71,7 @@ class ContinuousCollisionChecker { /** Certifies a trajectory (any supported Drake trajectory type). */ CertificationResult CheckTrajectory( - const drake::trajectories::Trajectory& trajectory, + const trajectories::Trajectory& trajectory, const std::optional& options = {}) const; /** Certifies a piecewise-linear path through the given waypoint columns. */ @@ -77,13 +86,13 @@ class ContinuousCollisionChecker { /** Introspection / testing seams (all const, thread-safe). */ PiecewiseBezierPath Normalize( - const drake::trajectories::Trajectory& trajectory, + const trajectories::Trajectory& trajectory, const std::optional& options = {}) const; MotionBoundTable ComputeMotionBounds(const PiecewiseBezierPath& path) const; const DistanceOracle& distance_oracle() const; const KinematicsEngine& kinematics_engine() const; const std::vector& pairs() const; - const drake::planning::RobotDiagram& model() const; + const RobotDiagram& model() const; private: class Impl; @@ -93,7 +102,8 @@ class ContinuousCollisionChecker { /** Independently replays every record of `certificate` (recomputing node control boxes from freshly restricted control points and re-querying distances) and checks interval coverage of the full domain for every pair. -Returns true iff the certificate holds (the search algorithm). */ +Returns true iff the certificate holds (the search algorithm). +@ingroup planning_collision_checker */ bool VerifyCertificate(const ContinuousCollisionChecker& checker, const PiecewiseBezierPath& path, const Certificate& certificate); diff --git a/planning/continuous_collision/distance_oracle.cc b/planning/continuous_collision/distance_oracle.cc index 77f8e12114bd..3577def5a156 100644 --- a/planning/continuous_collision/distance_oracle.cc +++ b/planning/continuous_collision/distance_oracle.cc @@ -12,6 +12,8 @@ #include #include +#include + #include "drake/common/drake_throw.h" #include "drake/common/unused.h" #include "drake/geometry/proximity/polygon_surface_mesh.h" @@ -271,9 +273,8 @@ struct DistanceOracle::Impl { std::string report; }; -DistanceOracle::DistanceOracle( - const drake::planning::RobotDiagram& model, - double query_tolerance) { +DistanceOracle::DistanceOracle(const RobotDiagram& model, + double query_tolerance) { DRAKE_THROW_UNLESS(query_tolerance >= 0.0); tolerance_ = query_tolerance; auto impl = std::make_shared(); @@ -286,15 +287,15 @@ DistanceOracle::DistanceOracle( const std::vector deformables = inspector.GetAllDeformableGeometryIds(); if (!deformables.empty()) { - std::ostringstream msg; - msg << "DistanceOracle: deformable geometries are not supported " - "(certified continuous collision checking assumes rigid bodies " - "whose motion the plant's kinematics describe). Offending " - "geometries:"; + std::string names; for (const GeometryId id : deformables) { - msg << "\n - " << inspector.GetName(id); + names += fmt::format("\n - {}", inspector.GetName(id)); } - throw std::runtime_error(msg.str()); + 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. ---------------- @@ -310,11 +311,11 @@ DistanceOracle::DistanceOracle( const drake::multibody::RigidBody* body_b = plant.GetBodyFromFrameId(inspector.GetFrameId(id_b)); if (body_a == nullptr || body_b == nullptr) { - throw std::runtime_error( - "DistanceOracle: collision geometry " + - Describe(inspector, body_a == nullptr ? id_a : id_b) + - " is not attached to a MultibodyPlant body; the checker can only " - "certify geometry whose motion the plant describes."); + 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)); @@ -322,12 +323,12 @@ DistanceOracle::DistanceOracle( if (class_a == ShapeClass::kHalfSpace && class_b == ShapeClass::kHalfSpace) { - throw std::runtime_error( + throw std::runtime_error(fmt::format( "DistanceOracle: signed distance between two HalfSpace geometries " - "is undefined, so the pair " + - Describe(inspector, id_a) + " / " + Describe(inspector, id_b) + - " cannot be certified. Remove one halfspace, or filter the pair " - "(CollisionFilterManager / a collision filter group)."); + "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; @@ -345,11 +346,11 @@ DistanceOracle::DistanceOracle( 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( - "DistanceOracle: no closed-form support function for shape type '" + - std::string(inspector.GetShape(partner).type_name()) + - "', so the halfspace pair " + Describe(inspector, id_a) + " / " + - Describe(inspector, id_b) + " cannot be certified."); + 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, @@ -395,15 +396,14 @@ DistanceOracle::DistanceOracle( query_object.ComputeSignedDistancePairClosestPoints(row.example_a, row.example_b); } catch (const std::exception& e) { - throw std::runtime_error( + throw std::runtime_error(fmt::format( "DistanceOracle: this Drake build cannot compute signed distance " - "for the shape combination (" + - ClassName(combo.first) + ", " + ClassName(combo.second) + - "); an offending pair is " + Describe(inspector, row.example_a) + - " / " + Describe(inspector, row.example_b) + - ". Filter the pair, or replace the geometry with a supported " - "shape (Convex is always supported). Drake reported: " + - e.what()); + "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())); } } } @@ -516,7 +516,7 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, return phi; } -std::string DistanceOracle::support_report() const { +const std::string& DistanceOracle::support_report() const { return impl_->report; } diff --git a/planning/continuous_collision/distance_oracle.h b/planning/continuous_collision/distance_oracle.h index d335dfecf746..7199692f6490 100644 --- a/planning/continuous_collision/distance_oracle.h +++ b/planning/continuous_collision/distance_oracle.h @@ -5,12 +5,12 @@ // may be refined by the implementation. #include -#include #include #include -#include +#include +#include "drake/common/drake_copyable.h" #include "drake/geometry/query_object.h" #include "drake/planning/continuous_collision/options.h" #include "drake/planning/robot_diagram.h" @@ -21,7 +21,8 @@ namespace continuous_collision { /** How the oracle computes signed distance for one pair, resolved once by the capability probe (the geometry-support scope; the distance-oracle contract): -no per-query dispatch decisions. */ +no per-query dispatch decisions. +@ingroup planning_collision_checker */ enum class DistanceRoute { /** QueryObject::ComputeSignedDistancePairClosestPoints. */ kNative, @@ -33,7 +34,8 @@ enum class DistanceRoute { }; /** One unfiltered proximity pair with its pre-resolved distance route and -effective threshold m_p = margin + padding(p). */ +effective threshold m_p = margin + padding(p). +@ingroup planning_collision_checker */ struct PairRecord { PairId id; DistanceRoute route{DistanceRoute::kNative}; @@ -50,9 +52,17 @@ whenever φ_true is at or above −tolerance(), and returns a definitely negative value when the shapes interpenetrate beyond tolerance. Only over-reporting a distance at or above threshold could fake a certificate (the soundness argument), which is why the capability probe keeps any -not-a-true-distance backend out of the loop entirely. */ +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. +@ingroup planning_collision_checker */ 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), classifies every (shape, shape) combination as @@ -60,8 +70,7 @@ class DistanceOracle { @throws std::exception immediately naming the offending geometries if any pair is unsupported (deformables; halfspace–halfspace). Never discovers an unsupported pair mid-certification. */ - DistanceOracle(const drake::planning::RobotDiagram& model, - double query_tolerance); + DistanceOracle(const RobotDiagram& model, double query_tolerance); /** The unfiltered pairs found by the probe (thresholds default 0; the facade rewrites them from margin + padding). */ @@ -78,10 +87,10 @@ class DistanceOracle { @throws std::exception if `pair` carries a halfspace route but its geometries were not classified by this oracle's capability probe (i.e. the record did not come from pairs()). */ - double SignedDistance( - const drake::geometry::QueryObject& query_object, - const PairRecord& pair, Eigen::Vector3d* nearest_a_W = nullptr, - Eigen::Vector3d* nearest_b_W = nullptr) const; + double SignedDistance(const geometry::QueryObject& query_object, + const PairRecord& pair, + Eigen::Vector3d* nearest_a_W = nullptr, + Eigen::Vector3d* nearest_b_W = nullptr) const; /** τ used in the certificate arithmetic (the numerical policy). */ double tolerance() const { return tolerance_; } @@ -89,13 +98,12 @@ class DistanceOracle { /** Human-readable probe report: one line per distinct shape-type combination and its route (includes the "Mesh certified as convex hull" notices; the risk register). */ - std::string support_report() const; + const std::string& support_report() const; - protected: + private: std::vector pairs_; double tolerance_{1e-6}; - private: /** Immutable capability-probe results: closed-form support data for every halfspace partner, the resolved per-shape-combination routes, and the rendered report. Held by shared_ptr so the oracle stays cheaply copyable diff --git a/planning/continuous_collision/motion_bound_table.cc b/planning/continuous_collision/motion_bound_table.cc index 6db4ac686fae..ca6c64164485 100644 --- a/planning/continuous_collision/motion_bound_table.cc +++ b/planning/continuous_collision/motion_bound_table.cc @@ -15,6 +15,7 @@ #include +#include "drake/common/drake_assert.h" #include "drake/common/drake_throw.h" #include "drake/geometry/geometry_roles.h" #include "drake/geometry/scene_graph_inspector.h" @@ -49,7 +50,25 @@ bool IsHalfSpace(const Shape& shape) { } // namespace -std::vector> MotionBoundTable::entries( +MotionBoundTable::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)) { + DRAKE_THROW_UNLESS(!row_start_.empty()); + DRAKE_THROW_UNLESS(row_start_.front() == 0); + for (int i = 1; i < static_cast(row_start_.size()); ++i) { + DRAKE_THROW_UNLESS(row_start_[i] >= row_start_[i - 1]); + } + DRAKE_THROW_UNLESS(coord_.size() == lambda_.size()); + DRAKE_THROW_UNLESS(static_cast(coord_.size()) == row_start_.back()); + DRAKE_THROW_UNLESS(carveout_slack_.size() + 1 == row_start_.size()); +} + +std::vector> MotionBoundTable::GetEntries( int pair_index) const { DRAKE_THROW_UNLESS(pair_index >= 0 && pair_index < num_pairs()); std::vector> out; @@ -60,8 +79,7 @@ std::vector> MotionBoundTable::entries( return out; } -KinematicsEngine::KinematicsEngine( - const drake::planning::RobotDiagram& model) +KinematicsEngine::KinematicsEngine(const RobotDiagram& model) : model_(&model), plant_(&model.plant()) { if (!plant_->is_finalized()) { throw std::runtime_error( @@ -160,8 +178,8 @@ void KinematicsEngine::BuildTopology() { 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_THROW_UNLESS(static_cast(rec.coord_rules.size()) == - rec.num_positions); + DRAKE_DEMAND(static_cast(rec.coord_rules.size()) == + rec.num_positions); } // Frame offsets: F = frame_on_parent (Jp), M = frame_on_child (Jc). @@ -195,10 +213,17 @@ void KinematicsEngine::BuildTopology() { } // ------------------------------------------------------------------ - // 2. Orient the joint graph into the world-rooted multibody tree. Post + // 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 a breadth-first walk from the - // world over the (body, joint) graph recovers the tree exactly. + // (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) { @@ -258,7 +283,7 @@ void KinematicsEngine::BuildTopology() { while (k >= 0) { tree_subtree[k][b] = true; k = inboard_joint_[joints_[k].inboard]; - DRAKE_THROW_UNLESS(++guard <= num_bodies_ + 1); + DRAKE_DEMAND(++guard <= num_bodies_ + 1); } } @@ -274,10 +299,10 @@ void KinematicsEngine::BuildTopology() { JointRecord& rec = joints_[k]; const Joint& joint = plant.get_joint(rec.index); if (joint.num_velocities() == 0) { - DRAKE_THROW_UNLESS(rec.num_positions == 0); + DRAKE_DEMAND(rec.num_positions == 0); continue; } - DRAKE_THROW_UNLESS(rec.num_positions > 0); + 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 " @@ -313,13 +338,13 @@ void KinematicsEngine::BuildTopology() { const JointRecord& rec = joints_[k]; for (int c = rec.position_start; c < rec.position_start + rec.num_positions; ++c) { - DRAKE_THROW_UNLESS(c >= 0 && c < num_positions_); - DRAKE_THROW_UNLESS(coord_joint_[c] == -1); + 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_THROW_UNLESS(coord_joint_[c] >= 0); + DRAKE_DEMAND(coord_joint_[c] >= 0); } } @@ -335,7 +360,7 @@ void KinematicsEngine::BuildGeometry() { for (int b = 0; b < num_bodies_; ++b) { const BodyIndex body(b); - DRAKE_THROW_UNLESS(plant.get_body(body).index() == body); + DRAKE_DEMAND(plant.get_body(body).index() == body); const std::optional frame_id = plant.GetBodyFrameIdIfExists(body); if (!frame_id.has_value()) continue; @@ -453,7 +478,7 @@ double KinematicsEngine::Reach(int joint_ord, BodyIndex body, BodyIndex b = body; for (int guard = 0; guard <= num_bodies_; ++guard) { const int k = inboard_joint_[b]; - DRAKE_THROW_UNLESS(k >= 0); + 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 @@ -576,7 +601,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( break; } } - DRAKE_THROW_UNLESS(std::isfinite(box_hop[k]) && box_hop[k] >= 0.0); + DRAKE_DEMAND(std::isfinite(box_hop[k]) && box_hop[k] >= 0.0); } // ------------------------------------------------------------------ @@ -726,15 +751,12 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // residual was charged, is the one case where the residual is genuinely // unbounded. // ------------------------------------------------------------------ - MotionBoundTable table; - std::vector& row_start = table.mutable_row_start(); - std::vector& coord = table.mutable_coord(); - std::vector& lambda = table.mutable_lambda(); - std::vector& carveout_slack = table.mutable_carveout_slack(); - row_start.clear(); + 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.clear(); carveout_slack.reserve(pairs.size()); // r(j, D) is shared by every pair with the same (joint, distal body), which @@ -792,7 +814,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // 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_THROW_UNLESS(std::isfinite(span) && span >= 0.0); + 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, @@ -848,7 +870,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( break; } } - DRAKE_THROW_UNLESS(std::isfinite(lam_tilde) && lam_tilde >= 0.0); + DRAKE_DEMAND(std::isfinite(lam_tilde) && lam_tilde >= 0.0); slack += lam_tilde * span; continue; } @@ -874,17 +896,17 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( "the λ assembly with an unsupported kind.", rec.name, rec.type_name)); } - DRAKE_THROW_UNLESS(std::isfinite(lam) && lam >= 0.0); + DRAKE_DEMAND(std::isfinite(lam) && lam >= 0.0); coord.push_back(c); lambda.push_back(lam); } } - DRAKE_THROW_UNLESS(std::isfinite(slack) && slack >= 0.0); + DRAKE_DEMAND(std::isfinite(slack) && slack >= 0.0); carveout_slack.push_back(slack); row_start.push_back(static_cast(coord.size())); } - DRAKE_THROW_UNLESS(carveout_slack.size() + 1 == row_start.size()); - return table; + return MotionBoundTable(std::move(row_start), std::move(coord), + std::move(lambda), std::move(carveout_slack)); } const std::vector& KinematicsEngine::body_spheres( diff --git a/planning/continuous_collision/motion_bound_table.h b/planning/continuous_collision/motion_bound_table.h index 6d5a944af69e..da48cf6a3498 100644 --- a/planning/continuous_collision/motion_bound_table.h +++ b/planning/continuous_collision/motion_bound_table.h @@ -9,8 +9,12 @@ #include #include -#include +#include +#include "drake/common/drake_copyable.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" #include "drake/planning/continuous_collision/bounding_sphere.h" #include "drake/planning/continuous_collision/options.h" #include "drake/planning/continuous_collision/piecewise_bezier_path.h" @@ -37,9 +41,26 @@ charged unconditionally inside MotionBound(), which is what makes Δ_p a true upper bound on the pair's relative motion over the whole trajectory rather than one that ignores the carved coordinates. It is exactly zero — bit for bit — whenever every carved coordinate is *exactly* constant, which is the case for -every path whose control points repeat a coordinate's value verbatim. */ +every path whose control points repeat a coordinate's value verbatim. +@ingroup planning_collision_checker */ class MotionBoundTable { public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(MotionBoundTable); + + /** Constructs an empty table (zero pairs). */ + MotionBoundTable() = default; + + /** Constructs the CSR table directly from its four arrays. + @param row_start Size num_pairs + 1, starting at 0 and non-decreasing; + row_start.back() is the total entry count. + @param coord Position-coordinate index of every entry. + @param lambda λ of every entry, element for element with `coord`. + @param carveout_slack One residual per pair. + @throws std::exception if the arrays do not satisfy those invariants. */ + MotionBoundTable(std::vector row_start, std::vector coord, + std::vector lambda, + std::vector carveout_slack); + int num_pairs() const { return static_cast(row_start_.size()) - 1; } /** True iff J(p) is empty after the constant-coordinate carve-out: no @@ -73,17 +94,11 @@ class MotionBoundTable { /** Introspection for tests: the (coordinate, λ) entries of one pair, ordered by increasing coordinate index. */ - std::vector> entries(int pair_index) const; + std::vector> GetEntries(int pair_index) const; /** Total number of (coordinate, λ) entries over all pairs. */ int num_entries() const { return static_cast(coord_.size()); } - /** Builder access (kinematics module internals only). */ - std::vector& mutable_row_start() { return row_start_; } - std::vector& mutable_coord() { return coord_; } - std::vector& mutable_lambda() { return lambda_; } - std::vector& mutable_carveout_slack() { return carveout_slack_; } - private: std::vector row_start_{0}; std::vector coord_; @@ -101,9 +116,14 @@ Typical use by the certifier: - once, at checker construction: KinematicsEngine engine(model); engine.body_spheres(b) for the prefilter; - once per Check* call: engine.ComputeMotionBoundTable(path, pairs); -- once per node, per pair: table.MotionBound(pair_index, w). */ +- once per node, per pair: table.MotionBound(pair_index, w). +@ingroup planning_collision_checker */ 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 (constant-coordinate carve-out, the @@ -121,14 +141,13 @@ class KinematicsEngine { "reversed" (its declared parent body is outboard of its declared child body in the multibody tree — a documented v1 exclusion), or if any proximity geometry has a shape ComputeBoundingSphere() rejects. */ - explicit KinematicsEngine(const drake::planning::RobotDiagram& model); + 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( - drake::multibody::BodyIndex body_a, - drake::multibody::BodyIndex body_b) const; + 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 @@ -163,30 +182,28 @@ class KinematicsEngine { used by the reach chain start and by the certifier's sphere prefilter. HalfSpace geometries have no bounding sphere and are omitted. */ const std::vector& body_spheres( - drake::multibody::BodyIndex body) const; + multibody::BodyIndex body) const; /** The geometry ids matching body_spheres(body), element for element. */ - const std::vector& body_sphere_geometries( - drake::multibody::BodyIndex body) const; + const std::vector& body_sphere_geometries( + multibody::BodyIndex body) 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(drake::geometry::GeometryId id) const; + const BoundingSphere& geometry_sphere(geometry::GeometryId id) const; /** True iff `body` carries at least one HalfSpace proximity geometry. */ - bool body_has_halfspace(drake::multibody::BodyIndex body) const; + bool body_has_halfspace(multibody::BodyIndex body) const; /** Radius, about the body frame origin, of a sphere containing every proximity geometry of `body` — the start of the reach chain. Zero for a body with no (non-HalfSpace) proximity geometry. */ - double body_radius(drake::multibody::BodyIndex body) const; + double body_radius(multibody::BodyIndex body) const; int num_positions() const { return num_positions_; } - const drake::multibody::MultibodyPlant& plant() const { - return *plant_; - } + const multibody::MultibodyPlant& plant() const { return *plant_; } private: /* The λ rule a joint's coordinates follow (the displacement lemma; the @@ -219,7 +236,7 @@ class KinematicsEngine { /* One tree edge, oriented from its outboard body toward the world. */ struct JointRecord { - drake::multibody::JointIndex index; + multibody::JointIndex index; std::string name; std::string type_name; JointKind kind{JointKind::kUnsupported}; @@ -227,8 +244,8 @@ class KinematicsEngine { int num_positions{0}; /* Tree-inboard / tree-outboard bodies (from the world-rooted walk, which is cross-checked against Drake's own subtree query). */ - drake::multibody::BodyIndex inboard; - drake::multibody::BodyIndex outboard; + 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}; @@ -256,7 +273,7 @@ class KinematicsEngine { /* Returns the joint ordinal (index into joints_) of `body`'s inboard joint, or -1 for the world body. */ - int inboard_joint_of(drake::multibody::BodyIndex body) const { + int inboard_joint_of(multibody::BodyIndex body) const { return inboard_joint_[body]; } @@ -265,15 +282,15 @@ class KinematicsEngine { 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, drake::multibody::BodyIndex body, + double Reach(int joint_ord, multibody::BodyIndex body, const std::vector& box_hop) const; void BuildTopology(); void BuildGeometry(); void CheckHalfSpaceRule() const; - const drake::planning::RobotDiagram* model_{}; - const drake::multibody::MultibodyPlant* plant_{}; + const RobotDiagram* model_{}; + const multibody::MultibodyPlant* plant_{}; int num_positions_{0}; int num_bodies_{0}; @@ -289,12 +306,11 @@ class KinematicsEngine { std::vector coord_joint_; std::vector> body_spheres_; - std::vector> body_sphere_geoms_; + std::vector> body_sphere_geoms_; std::vector body_radius_; std::vector body_has_halfspace_; std::vector body_halfspace_name_; - std::unordered_map - geometry_spheres_; + std::unordered_map geometry_spheres_; }; } // namespace continuous_collision diff --git a/planning/continuous_collision/numerics.h b/planning/continuous_collision/numerics.h index 63fda74a5481..e9e74248ec7d 100644 --- a/planning/continuous_collision/numerics.h +++ b/planning/continuous_collision/numerics.h @@ -1,9 +1,5 @@ #pragma once -namespace drake { -namespace planning { -namespace continuous_collision { - /** @file Single home of the numerical accounting used everywhere (the numerical policy). @@ -23,13 +19,19 @@ directed rounding (that hardening is a future extension); ε defaults to 1e-9 m which dominates the accumulated FP error of the w/λ/dot-product expression depths involved. */ -/** True iff the pair is certified on the whole node. */ +namespace drake { +namespace planning { +namespace continuous_collision { + +/** True iff the pair is certified on the whole node. +@ingroup planning_collision_checker */ inline bool IsCertified(double phi_hat, double tau, double motion_bound, double threshold, double slack) { return phi_hat - tau - motion_bound > threshold + slack; } -/** True iff the representative configuration is a definite violation. */ +/** True iff the representative configuration is a definite violation. +@ingroup planning_collision_checker */ inline bool IsDefiniteViolation(double phi_hat, double tau, double threshold) { return phi_hat + tau < threshold; } diff --git a/planning/continuous_collision/options.h b/planning/continuous_collision/options.h index e27784465163..8439f8ab2ecc 100644 --- a/planning/continuous_collision/options.h +++ b/planning/continuous_collision/options.h @@ -4,7 +4,7 @@ #include #include -#include +#include #include "drake/common/parallelism.h" #include "drake/geometry/geometry_ids.h" @@ -14,7 +14,8 @@ namespace drake { namespace planning { namespace continuous_collision { -/** Search modes for certification (the search algorithm). */ +/** Search modes for certification (the search algorithm). +@ingroup planning_collision_checker */ enum class SearchMode { /** Return on the first definite violation; serial execution returns the earliest one in time. */ @@ -24,7 +25,8 @@ enum class SearchMode { kCertifyAll, }; -/** Outcome of a certification run (the problem statement). */ +/** Outcome of a certification run (the problem statement). +@ingroup planning_collision_checker */ enum class Verdict { /** Proof: every unfiltered pair keeps signed distance > margin + padding over the entire continuous time domain. */ @@ -39,7 +41,8 @@ enum class Verdict { }; /** Options controlling one certification call (the architecture; the numerical - * policy). */ + * policy). + * @ingroup planning_collision_checker */ struct Options { /** Global clearance margin δ in meters. The certificate proves signed distance > margin + padding for every pair at every time. */ @@ -56,7 +59,8 @@ struct Options { narrower than this become kInconclusive findings instead of splitting. */ double min_interval{1e-9}; /** Position coordinates whose junction continuity is checked modulo 2π - (GcsTrajectoryOptimization continuous-revolute convention). */ + (GcsTrajectoryOptimization continuous-revolute convention). + @see planning::trajectory_optimization::GetContinuousRevoluteJointIndices */ std::vector continuous_revolute_indices{}; /** Maximum polynomial degree accepted for monomial→Bernstein conversion. */ int max_conversion_degree{10}; @@ -67,30 +71,44 @@ struct Options { /** If true, every certification event is recorded into a Certificate that VerifyCertificate() can independently replay (the search algorithm). */ bool emit_certificate{false}; - drake::Parallelism parallelism{drake::Parallelism::Max()}; + Parallelism parallelism{Parallelism::Max()}; }; -/** Per-body-pair padding, mirroring drake::planning::CollisionChecker -semantics: the effective threshold for pair p is margin + padding(p). */ +/** Per-body-pair padding: the effective threshold for pair p is +margin + padding(p). + +Which of the two scalars applies to a pair is decided by *anchoring*, from +plant topology alone. A body is anchored iff no position coordinate of the +plant changes its pose relative to the world — the world body itself, and +everything welded to it directly or transitively. A pair is a self-collision +pair iff both of its bodies are non-anchored, and an environment pair +otherwise. The rule never depends on which trajectory is being checked. +@ingroup planning_collision_checker */ struct PaddingSpec { - /** Padding for robot-vs-environment pairs. */ + /** Padding for robot-vs-environment pairs, i.e. pairs with at least one + anchored body. */ double env_padding{0.0}; - /** Padding for robot-vs-robot (self-collision) pairs. */ + /** Padding for robot-vs-robot (self-collision) pairs, i.e. pairs whose two + bodies are both non-anchored. */ double self_padding{0.0}; - /** Optional dense symmetric matrix indexed by BodyIndex; when set it - overrides the two scalars for the pairs it covers. */ + /** Optional dense symmetric matrix indexed by BodyIndex, sized + num_bodies × num_bodies. Entry (a, b) overrides the scalars for that body + pair; a NaN entry means "not covered", and that pair falls back to + env_padding / self_padding. */ std::optional per_body_pair{}; }; -/** Identifies an unfiltered proximity geometry pair. */ +/** Identifies an unfiltered proximity geometry pair. +@ingroup planning_collision_checker */ struct PairId { - drake::geometry::GeometryId a; - drake::geometry::GeometryId b; - drake::multibody::BodyIndex body_a; - drake::multibody::BodyIndex body_b; + geometry::GeometryId a; + geometry::GeometryId b; + multibody::BodyIndex body_a; + multibody::BodyIndex body_b; }; -/** One violation or inconclusive record (the architecture). */ +/** One violation or inconclusive record (the architecture). +@ingroup planning_collision_checker */ struct Finding { /** Trajectory time of the witness configuration. */ double time{}; @@ -110,7 +128,8 @@ struct Finding { std::optional nearest_b_W{}; }; -/** Cost accounting for one certification call. */ +/** Cost accounting for one certification call. +@ingroup planning_collision_checker */ struct Statistics { uint64_t nodes{0}; uint64_t narrowphase_queries{0}; diff --git a/planning/continuous_collision/piecewise_bezier_path.cc b/planning/continuous_collision/piecewise_bezier_path.cc index 51b29ffabc7b..26ad5387cc50 100644 --- a/planning/continuous_collision/piecewise_bezier_path.cc +++ b/planning/continuous_collision/piecewise_bezier_path.cc @@ -4,12 +4,14 @@ #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" @@ -41,14 +43,6 @@ 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; -/* Streams `args` into one string. Used only on the throwing paths. */ -template -std::string StrCat(Args&&... args) { - std::ostringstream stream; - (stream << ... << args); - return stream.str(); -} - /* Pascal's triangle up to row `m`; table(j, a) = C(j, a) for a <= j, 0 otherwise. Exact in double for the degrees this file accepts (the default cap is 10; C(10, 5) = 252). */ @@ -80,11 +74,11 @@ void AppendBsplineSegments(const BsplineTrajectory& bspline, int source_index, std::vector* segments) { if (bspline.cols() != 1) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath: the BsplineTrajectory at segment index ", - source_index, " is ", bspline.rows(), "x", bspline.cols(), - "-valued; only column-vector-valued trajectories (cols() == 1) over " - "the plant's generalized positions are supported.")); + 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; @@ -133,11 +127,11 @@ void AppendBsplineSegments(const BsplineTrajectory& bspline, segments->push_back(std::move(segment)); } if (segments->size() == num_before) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath: the BsplineTrajectory at segment index ", - source_index, - " has an empty parameter domain; a trajectory must span a positive " - "time interval.")); + 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)); } } @@ -158,18 +152,19 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, const Options& options, int source_index, std::vector* segments) { if (pp.cols() != 1) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath: the PiecewisePolynomial at segment index ", - source_index, " is ", pp.rows(), "x", pp.cols(), - "-valued; only column-vector-valued trajectories (cols() == 1) over " - "the plant's generalized positions are supported.")); + 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( - StrCat("PiecewiseBezierPath: the PiecewisePolynomial at segment index ", - source_index, " has no segments.")); + 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; @@ -177,24 +172,23 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, m = std::max(m, pp.getSegmentPolynomialDegree(k, r, 0)); } if (m > options.max_conversion_degree) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath: PiecewisePolynomial segment ", k, - " (source segment index ", source_index, ") has polynomial degree ", - m, ", above options.max_conversion_degree = ", - options.max_conversion_degree, - ". The monomial-to-Bernstein change of basis is ill-conditioned at " - "high degree; either raise Options::max_conversion_degree " - "deliberately or re-express the trajectory with more, lower-degree " - "segments.")); + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: PiecewisePolynomial segment {} (source segment " + "index {}) has polynomial degree {}, above " + "options.max_conversion_degree = {}. The monomial-to-Bernstein " + "change of basis is ill-conditioned at high degree; either raise " + "Options::max_conversion_degree deliberately or re-express the " + "trajectory with more, lower-degree segments.", + k, source_index, m, options.max_conversion_degree)); } 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( - StrCat("PiecewiseBezierPath: PiecewisePolynomial segment ", k, - " (source segment index ", source_index, - ") has non-positive duration ", duration, ".")); + fmt::format("PiecewiseBezierPath: PiecewisePolynomial segment {} " + "(source segment index {}) has non-positive duration {}.", + k, source_index, duration)); } const Eigen::MatrixXd binomial = BinomialTable(m); BezierSegment segment; @@ -235,8 +229,9 @@ void AppendSegments(const Trajectory& trajectory, dynamic_cast*>(&trajectory)) { if (bezier->control_points().cols() < 1) { throw std::runtime_error( - StrCat("PiecewiseBezierPath: the BezierCurve at segment index ", - *source_index, " has no control points.")); + fmt::format("PiecewiseBezierPath: the BezierCurve at segment index " + "{} has no control points.", + *source_index)); } BezierSegment segment; segment.t_start = bezier->start_time(); @@ -251,9 +246,9 @@ void AppendSegments(const Trajectory& trajectory, const int num = composite->get_number_of_segments(); if (num < 1) { throw std::runtime_error( - StrCat("PiecewiseBezierPath: the CompositeTrajectory at segment " - "index ", - *source_index, " has no segments.")); + fmt::format("PiecewiseBezierPath: the CompositeTrajectory at " + "segment index {} has no segments.", + *source_index)); } for (int i = 0; i < num; ++i) { AppendSegments(composite->segment(i), options, source_index, segments); @@ -272,14 +267,14 @@ void AppendSegments(const Trajectory& trajectory, ++(*source_index); return; } - throw std::runtime_error(StrCat( - "PiecewiseBezierPath: unsupported trajectory type '", - NiceTypeName::Get(trajectory), "' at segment index ", *source_index, - ". Supported types are drake::trajectories::BezierCurve, " + 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.")); + "themselves supported.", + NiceTypeName::Get(trajectory), *source_index)); } /* Checks shape, time ordering/contiguity and C0 junctions (trajectory @@ -293,21 +288,21 @@ void ValidateSegments(int num_positions, const Options& options, 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(StrCat( - "PiecewiseBezierPath: segment ", i, " has ", - segment.control_points.rows(), " rows but the trajectory declares ", - num_positions, - " generalized positions; every segment must be valued in the same " - "position space.")); + 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(StrCat("PiecewiseBezierPath: segment ", i, - " has no control points.")); + throw std::runtime_error(fmt::format( + "PiecewiseBezierPath: segment {} has no control points.", i)); } if (!(segment.t_end >= segment.t_start)) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath: segment ", i, " spans [", segment.t_start, ", ", - segment.t_end, "], which runs backwards in time.")); + 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; @@ -316,11 +311,11 @@ void ValidateSegments(int num_positions, const Options& options, 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( - StrCat("PiecewiseBezierPath: segments are not contiguous in time — " - "segment ", - i - 1, " ends at ", previous_end, " but segment ", i, - " starts at ", segment.t_start, - ". Segments must be ordered and meet end-to-start.")); + 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)); } } } @@ -328,10 +323,11 @@ void ValidateSegments(int num_positions, const Options& options, std::vector is_continuous_revolute(num_positions, false); for (int index : options.continuous_revolute_indices) { if (index < 0 || index >= num_positions) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath: Options::continuous_revolute_indices contains ", - index, ", which is out of range for a trajectory with ", - num_positions, " generalized 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; } @@ -352,20 +348,17 @@ void ValidateSegments(int num_positions, const Options& options, gap -= kTwoPi * std::round(gap / kTwoPi); } if (std::abs(gap) > options.continuity_tolerance) { - throw std::runtime_error(StrCat( + 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 ", - i - 1, " and ", i, " in coordinate ", c, ": the gap is ", raw_gap, - (is_continuous_revolute[c] ? " (" : ""), - (is_continuous_revolute[c] ? StrCat(gap, " modulo 2π)") - : std::string()), - ", which exceeds Options::continuity_tolerance = ", - options.continuity_tolerance, - ". A discontinuous trajectory teleports; per-segment certificates " - "would not cover the jump. If coordinate ", - c, - " is a continuous revolute joint, list it in " - "Options::continuous_revolute_indices.")); + "segments {} and {} in coordinate {}: the gap is {}{}, which " + "exceeds Options::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, options.continuity_tolerance, c)); } } } @@ -376,11 +369,11 @@ void ValidateSegments(int num_positions, const Options& options, PiecewiseBezierPath PiecewiseBezierPath::FromTrajectory( const Trajectory& trajectory, const Options& options) { if (trajectory.cols() != 1) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath::FromTrajectory: the trajectory is ", - trajectory.rows(), "x", trajectory.cols(), - "-valued; only column-vector-valued trajectories (cols() == 1) over " - "the plant's generalized positions are supported.")); + 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) { @@ -406,10 +399,10 @@ PiecewiseBezierPath PiecewiseBezierPath::FromWaypoints( "rows; expected one row per generalized position."); } if (waypoints.cols() < 2) { - throw std::runtime_error(StrCat( + throw std::runtime_error(fmt::format( "PiecewiseBezierPath::FromWaypoints: at least 2 waypoints (columns) " - "are required to form a path; got ", - waypoints.cols(), ".")); + "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; @@ -459,15 +452,16 @@ void PiecewiseBezierPath::FinalizeMetadata(double continuity_tolerance) { } Eigen::VectorXd PiecewiseBezierPath::Value(double t) const { - DRAKE_THROW_UNLESS(!segments_.empty()); + 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(StrCat("PiecewiseBezierPath::Value: time ", t, - " is outside the path's domain [", t0, ", ", - tf, "].")); + 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 @@ -497,14 +491,16 @@ Eigen::VectorXd PiecewiseBezierPath::EvaluateSegment(int segment_index, double s) const { if (segment_index < 0 || segment_index >= static_cast(segments_.size())) { - throw std::runtime_error(StrCat( - "PiecewiseBezierPath::EvaluateSegment: segment index ", segment_index, - " is out of range; the path has ", segments_.size(), " segments.")); + 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( - StrCat("PiecewiseBezierPath::EvaluateSegment: parameter s = ", s, - " is outside the segment's domain [0, 1].")); + 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 = diff --git a/planning/continuous_collision/piecewise_bezier_path.h b/planning/continuous_collision/piecewise_bezier_path.h index 21eb672f082d..e82f09e0aa6f 100644 --- a/planning/continuous_collision/piecewise_bezier_path.h +++ b/planning/continuous_collision/piecewise_bezier_path.h @@ -2,8 +2,9 @@ #include -#include +#include +#include "drake/common/drake_copyable.h" #include "drake/common/trajectories/trajectory.h" #include "drake/planning/continuous_collision/options.h" @@ -12,7 +13,8 @@ namespace planning { namespace continuous_collision { /** One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1] (trajectory - * normalization). */ + * normalization). + * @ingroup planning_collision_checker */ struct BezierSegment { /** Original time interval (bookkeeping only; the certificate is a property of the path and is invariant under time reparametrization). */ @@ -32,9 +34,12 @@ 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). */ +Casteljau triangle at u is exactly q(u). +@ingroup planning_collision_checker */ 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). @@ -43,7 +48,7 @@ class PiecewiseBezierPath { options.continuity_tolerance (modulo 2π for coordinates in options.continuous_revolute_indices). */ static PiecewiseBezierPath FromTrajectory( - const drake::trajectories::Trajectory& trajectory, + const trajectories::Trajectory& trajectory, const Options& options); /** Normalizes an n × K waypoint matrix into K−1 order-1 segments (exact). @@ -90,7 +95,8 @@ class PiecewiseBezierPath { /** 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. */ +Allocation-free when the outputs are already correctly sized. +@ingroup planning_collision_checker */ void DeCasteljauSplitAtHalf(const Eigen::MatrixXd& cps, Eigen::MatrixXd* left, Eigen::MatrixXd* right, Eigen::VectorXd* mid); diff --git a/planning/continuous_collision/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc index d8eb4802aa4d..3979cd2fc322 100644 --- a/planning/continuous_collision/test/concurrency_test.cc +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -554,7 +554,7 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { std::vector>> lambda_expected; std::vector slack_expected; for (int p = 0; p < table_expected.num_pairs(); ++p) { - lambda_expected.push_back(table_expected.entries(p)); + lambda_expected.push_back(table_expected.GetEntries(p)); slack_expected.push_back(table_expected.carveout_slack(p)); } @@ -590,7 +590,7 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { continue; } for (int p = 0; p < table.num_pairs(); ++p) { - if (table.entries(p) != lambda_expected[p]) ++mismatches[t]; + if (table.GetEntries(p) != lambda_expected[p]) ++mismatches[t]; // The carve-out residual is part of Δ_p, so it has to be // bit-identical across threads too. if (table.carveout_slack(p) != slack_expected[p]) ++mismatches[t]; diff --git a/planning/continuous_collision/test/motion_bound_test.cc b/planning/continuous_collision/test/motion_bound_test.cc index 0ebb2cb43253..310c61b88931 100644 --- a/planning/continuous_collision/test/motion_bound_test.cc +++ b/planning/continuous_collision/test/motion_bound_test.cc @@ -518,15 +518,15 @@ GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { lower, upper, std::vector(nq, false), pairs); ASSERT_EQ(table.num_pairs(), 1); EXPECT_FALSE(table.pair_is_static(0)); - EXPECT_EQ(table.entries(0).size(), 2); + EXPECT_EQ(table.GetEntries(0).size(), 2); } { // One constant: only the other survives. std::vector constant(nq, false); constant[0] = true; const MotionBoundTable table = engine.ComputeMotionBoundTable(lower, upper, constant, pairs); - ASSERT_EQ(table.entries(0).size(), 1); - EXPECT_EQ(table.entries(0)[0].first, 1); + 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 @@ -616,9 +616,9 @@ GTEST_TEST(HalfSpaceRuleTest, AnchoredGroundPlaneIsAccepted) { const MotionBoundTable table = engine.ComputeMotionBoundTable( VectorXd::Constant(nq, -1.0), VectorXd::Constant(nq, 1.0), std::vector(nq, false), pairs); - ASSERT_EQ(table.entries(0).size(), 1); - EXPECT_GT(table.entries(0)[0].second, 0.0); - EXPECT_TRUE(std::isfinite(table.entries(0)[0].second)); + 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)); } GTEST_TEST(HalfSpaceRuleTest, RotatingHalfSpaceThrowsAtConstruction) { @@ -646,8 +646,8 @@ GTEST_TEST(HalfSpaceRuleTest, TranslatingHalfSpaceIsAccepted) { const MotionBoundTable table = engine.ComputeMotionBoundTable( VectorXd::Constant(nq, -1.0), VectorXd::Constant(nq, 1.0), std::vector(nq, false), pairs); - ASSERT_EQ(table.entries(0).size(), 1); - EXPECT_EQ(table.entries(0)[0].second, 1.0); + ASSERT_EQ(table.GetEntries(0).size(), 1); + EXPECT_EQ(table.GetEntries(0)[0].second, 1.0); } /* world --(revolute j0)--> b1 --(quaternion floating)--> b2 --(revolute j1)--> @@ -726,12 +726,12 @@ GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { } const MotionBoundTable table = engine.ComputeMotionBoundTable(lower, upper, constant, pairs); - ASSERT_EQ(table.entries(0).size(), 2); + 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.entries(0)) { + for (const auto& [c, lam] : table.GetEntries(0)) { if (c == j0.position_start()) lambda_j0 = lam; } EXPECT_GT(lambda_j0, p_FM.norm()); @@ -870,7 +870,7 @@ GTEST_TEST(ReachTest, RevoluteChainIsExactAndTight) { double lambda_top = 0.0; double lambda_slide = 0.0; - for (const auto& [c, lam] : table.entries(k)) { + for (const auto& [c, lam] : table.GetEntries(k)) { if (c == j_top.position_start()) lambda_top = lam; if (c == j_slide.position_start()) lambda_slide = lam; } @@ -932,7 +932,7 @@ GTEST_TEST(ReachTest, ScrewLambdaIncludesPitchAndIsNecessary) { lower, upper, std::vector(nq, false), pairs); double lambda_top = 0.0; - for (const auto& [c, lam] : table.entries(k)) { + for (const auto& [c, lam] : table.GetEntries(k)) { if (c == j_top.position_start()) lambda_top = lam; } const double pitch_term = kPitch / (2.0 * M_PI); @@ -1066,7 +1066,7 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, if (!constant[c]) expected.push_back(c); } std::vector actual; - for (const auto& [c, lam] : table.entries(k)) { + for (const auto& [c, lam] : table.GetEntries(k)) { actual.push_back(c); ASSERT_TRUE(std::isfinite(lam)); ASSERT_GE(lam, 0.0); @@ -1110,7 +1110,7 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, // ---- (1) Atomic, one coordinate at a time. ----------------------- bool single_distal_side = true; BodyIndex common_distal; - for (const auto& [c, lam] : table.entries(k)) { + 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; @@ -1348,10 +1348,10 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { upper[rot] = 0.5; const MotionBoundTable moving = engine.ComputeMotionBoundTable( lower, upper, std::vector(nq, false), pairs); - ASSERT_EQ(moving.entries(0).size(), 2); + 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.entries(0)) { + for (const auto& [c, lam] : moving.GetEntries(0)) { if (c == rot) lambda_rot = lam; } ASSERT_GT(lambda_rot, 0.0); @@ -1366,8 +1366,8 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { upper[rot] = kRange; // upper − lower is exactly kRange in binary FP. const MotionBoundTable carved = engine.ComputeMotionBoundTable(lower, upper, constant, pairs); - ASSERT_EQ(carved.entries(0).size(), 1); - EXPECT_EQ(carved.entries(0)[0].first, slide); + 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. @@ -1602,7 +1602,7 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { const MotionBoundTable table = engine.ComputeMotionBoundTable(lower, upper, constant, pairs); - ASSERT_EQ(table.entries(0).size(), 1); // Only the revolute survives. + 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 " diff --git a/planning/continuous_collision/vpolytope_ingestion.cc b/planning/continuous_collision/vpolytope_ingestion.cc index 77653c3a65f6..8df4ea2061eb 100644 --- a/planning/continuous_collision/vpolytope_ingestion.cc +++ b/planning/continuous_collision/vpolytope_ingestion.cc @@ -2,6 +2,8 @@ #include +#include + #include "drake/common/drake_throw.h" #include "drake/geometry/shape_specification.h" #include "drake/multibody/plant/coulomb_friction.h" @@ -22,28 +24,28 @@ namespace { constexpr double kDefaultFriction = 1.0; } // namespace -GeometryId AddVPolytopeObstacle( - MultibodyPlant* plant, - const drake::geometry::optimization::VPolytope& vpoly, - const RigidTransformd& X_WG, const std::string& name) { +GeometryId AddVPolytopeObstacle(MultibodyPlant* plant, + const geometry::optimization::VPolytope& vpoly, + const RigidTransformd& X_WG, + const std::string& name) { DRAKE_THROW_UNLESS(plant != nullptr); if (plant->is_finalized()) { - throw std::runtime_error( - "AddVPolytopeObstacle(): cannot add obstacle '" + name + - "' because the plant is already finalized; register V-polytope " - "obstacles before calling MultibodyPlant::Finalize()."); + throw std::runtime_error(fmt::format( + "AddVPolytopeObstacle(): cannot add obstacle '{}' because the plant " + "is already finalized; register V-polytope obstacles before calling " + "MultibodyPlant::Finalize().", + name)); } if (vpoly.ambient_dimension() != 3) { - throw std::runtime_error( - "AddVPolytopeObstacle(): obstacle '" + name + - "' has ambient " - "dimension " + - std::to_string(vpoly.ambient_dimension()) + - "; only 3-dimensional V-polytopes can be registered as geometry."); + throw std::runtime_error(fmt::format( + "AddVPolytopeObstacle(): obstacle '{}' has ambient dimension {}; only " + "3-dimensional V-polytopes can be registered as geometry.", + name, vpoly.ambient_dimension())); } if (vpoly.vertices().cols() == 0) { - throw std::runtime_error("AddVPolytopeObstacle(): obstacle '" + name + - "' has an empty vertex set."); + throw std::runtime_error(fmt::format( + "AddVPolytopeObstacle(): obstacle '{}' has an empty vertex set.", + name)); } // Drake's pinned VPolytope -> Convex entry point; it forwards the vertex diff --git a/planning/continuous_collision/vpolytope_ingestion.h b/planning/continuous_collision/vpolytope_ingestion.h index dec84477c447..9b673bb1d58c 100644 --- a/planning/continuous_collision/vpolytope_ingestion.h +++ b/planning/continuous_collision/vpolytope_ingestion.h @@ -38,11 +38,12 @@ redundant or degenerate vertex sets. @throws std::exception if `plant` is null or already finalized, if `vpoly.ambient_dimension() != 3`, if the vertex set is empty, or if Drake rejects the resulting hull (e.g. a degenerate vertex set that - its hull computation cannot inflate). */ -drake::geometry::GeometryId AddVPolytopeObstacle( - drake::multibody::MultibodyPlant* plant, - const drake::geometry::optimization::VPolytope& vpoly, - const drake::math::RigidTransform& X_WG, const std::string& name); + its hull computation cannot inflate). +@ingroup planning_collision_checker */ +geometry::GeometryId AddVPolytopeObstacle( + multibody::MultibodyPlant* plant, + const geometry::optimization::VPolytope& vpoly, + const math::RigidTransform& X_WG, const std::string& name); } // namespace continuous_collision } // namespace planning From e67dc92c87763fc3f56c364f8066ea20ee020701 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 27 Aug 2026 14:42:39 -0400 Subject: [PATCH 11/22] [planning] continuous_collision: use call-scoped parallel workers No Drake compute class owns persistent background threads; the upstream pattern is call-scoped fork-join. Replace internal::WorkerPool (parked threads owned by the checker, reserved through a Batch handle) with std::async(std::launch::async) futures collected in a local vector, the shape planning/iris/iris_from_clique_cover.cc uses. The lazy recruitment policy is structurally unchanged: the call still starts as a serial descent on the calling thread with sharing disabled and still hires N-1 helpers exactly once, when the run crosses the node threshold. Only the price of hiring changes, so the threshold moves with it: spawning a thread measures 34 us on the reference machine against ~6-7 us to notify a parked one, which pushes the break-even up about 4x, from 16 nodes to 64. A check that ends right after hiring loses at most a few hundred microseconds, and a check under 64 nodes never hires at all. The exception path is preserved exactly: every worker catches, aborts the work source, and the first exception is rethrown once all of them have finished. The futures are declared after everything their tasks reference so that ~future joins before those objects are destroyed, which is what the Batch destructor used to guarantee. The worker-count cap stays Parallelism::Max().num_threads(); with no pool left to bound, RunCertifier applies it to the requested width directly. Also rename certifier.cc to certifier_internal.cc so clang-format and cpplint treat certifier_internal.h as the related header again. Measured on the in-tree benchmark (--only profile, AMD 7950X3D): deep grazing workload at min_interval 1e-6 (12570 nodes) 5.9-6.1x at 16 threads against 6.2-6.4x pooled; the 1.6 ms difference is 0.51 ms of thread creation plus 0.64 ms of the 48 extra nodes the higher threshold runs serially. The 15-node PWL edge stays flat at 1.00-1.03x from Parallelism(1) to Parallelism(16), and the 146-node shelf check keeps a speedup above 1 at every width. --- planning/continuous_collision/BUILD.bazel | 6 +- .../{certifier.cc => certifier_internal.cc} | 271 ++++-------------- .../continuous_collision/certifier_internal.h | 92 +----- .../continuous_collision_checker.cc | 8 +- .../test/concurrency_test.cc | 6 +- 5 files changed, 67 insertions(+), 316 deletions(-) rename planning/continuous_collision/{certifier.cc => certifier_internal.cc} (85%) diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index 22eaf4de0ed9..f92793c653f0 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -141,14 +141,14 @@ drake_cc_library( ) # The certificate and the node recursion are mutually recursive translation -# units (certificate.cc replays the events that certifier.cc emits), so they -# form one library, exactly as they formed one module in the standalone +# units (certificate.cc replays the events that certifier_internal.cc emits), +# so they form one library, exactly as they formed one module in the standalone # package. drake_cc_library( name = "certifier", srcs = [ "certificate.cc", - "certifier.cc", + "certifier_internal.cc", ], hdrs = [ "certificate.h", diff --git a/planning/continuous_collision/certifier.cc b/planning/continuous_collision/certifier_internal.cc similarity index 85% rename from planning/continuous_collision/certifier.cc rename to planning/continuous_collision/certifier_internal.cc index 1f27ee36af84..ff76a6b4f625 100644 --- a/planning/continuous_collision/certifier.cc +++ b/planning/continuous_collision/certifier_internal.cc @@ -1,14 +1,14 @@ +#include "drake/planning/continuous_collision/certifier_internal.h" + #include #include #include #include -#include #include +#include #include #include #include -#include -#include #include #include @@ -16,7 +16,6 @@ #include "drake/common/parallelism.h" #include "drake/geometry/scene_graph.h" #include "drake/multibody/plant/multibody_plant.h" -#include "drake/planning/continuous_collision/certifier_internal.h" #include "drake/planning/continuous_collision/numerics.h" namespace drake { @@ -294,21 +293,23 @@ struct Recruitment { /* How many nodes a run must have visited before it hires helpers. - This is a measured break-even, not a taste knob. Hiring costs one WorkerPool - reservation, one ContextPool lease, the construction of the helper Worker - objects, one notification per helper and — at the end of the run — one wakeup - per helper before the lead can collect their statistics: about 6-7 us per - helper, ~65 us for a full fifteen, on the machine the benchmark suite - was measured on. A node on that machine costs ~7-13 us. Sixteen nodes of work - already done is therefore roughly a 3x margin over the price of the helpers, - and it bounds the damage in the one case lazy recruitment cannot avoid — a - check that ends immediately after hiring — to that same ~65 us (~15% of such a - check). + This is a measured break-even, not a taste knob. 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 before the lead + can collect their statistics. Thread creation dominates that list at tens of + microseconds per worker, which is where this threshold parts company with the + parked-thread pool it replaced: waking a parked thread cost ~6-7 us, so paying + for a fresh one instead moves the break-even up by roughly 4x, from 16 nodes + to 64. A node on the machine the benchmark suite was measured on costs + ~7-13 us, so 64 nodes of work already done is again roughly a 3x margin over + the price of a full fifteen helpers, and it bounds the damage in the one case + lazy recruitment cannot avoid — a check that ends immediately after hiring — + to a few hundred microseconds. Everything smaller than this runs at exactly serial speed at any Options::parallelism, which is the property that matters most in practice because Parallelism::Max() is the default value of that field. */ -constexpr std::uint64_t kNodesBeforeHiringHelpers = 16; +constexpr std::uint64_t kNodesBeforeHiringHelpers = 64; // --------------------------------------------------------------------------- // The node loop. @@ -894,193 +895,11 @@ ContextPool::Lease::~Lease() { if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); } -// --------------------------------------------------------------------------- -// WorkerPool. -// --------------------------------------------------------------------------- - -/* One parked thread. Each slot has its own mutex/condition variable so that - dispatching to n slots is n independent handoffs rather than a broadcast every - waiter has to filter. */ -struct WorkerPool::Slot { - std::mutex mutex; - std::condition_variable condition; - bool shutdown{false}; - bool has_task{false}; - const std::function* task{nullptr}; - int index{0}; - std::shared_ptr state; - std::thread thread; -}; - -/* Completion counter of one dispatch. Held by shared_ptr — the batch and every - slot that ran one of its tasks own a reference — because the slot signals - completion *through* this object and the waiter would otherwise be free to - destroy it while the signalling thread is still inside notify_all(). */ -struct WorkerPool::BatchState { - std::mutex mutex; - std::condition_variable condition; - int remaining{0}; -}; - -WorkerPool::WorkerPool() = default; - -WorkerPool::~WorkerPool() { - std::deque> slots; - { - std::lock_guard guard(mutex_); - shutdown_ = true; - slots.swap(slots_); - idle_.clear(); - } - // Outside the pool mutex: a slot thread never takes it, but keeping the - // teardown lock-free makes that independent of future edits. - for (const std::unique_ptr& slot : slots) { - { - std::lock_guard guard(slot->mutex); - slot->shutdown = true; - } - slot->condition.notify_one(); - } - for (const std::unique_ptr& slot : slots) { - if (slot->thread.joinable()) slot->thread.join(); - } -} - -WorkerPool::Batch WorkerPool::Reserve(int count) { - Batch batch; - batch.pool_ = this; - if (count <= 0) return batch; - // Bounding the pool by the machine's width keeps a program that runs many - // concurrent parallel checks from multiplying threads without limit; a call - // that finds nothing free simply runs with fewer workers, which is only a - // performance difference. The width comes from Parallelism::Max() rather - // than hardware_concurrency() directly, so the cap honours DRAKE_NUM_THREADS - // like the rest of Drake. - const int cap = Parallelism::Max().num_threads(); - std::lock_guard guard(mutex_); - if (shutdown_) return batch; - while (static_cast(batch.slots_.size()) < count && !idle_.empty()) { - const int index = idle_.back(); - idle_.pop_back(); - batch.slots_.push_back(index); - batch.handles_.push_back(slots_[index].get()); - } - while (static_cast(batch.slots_.size()) < count && - static_cast(slots_.size()) < cap) { - slots_.push_back(std::make_unique()); - Slot* const slot = slots_.back().get(); - const int index = static_cast(slots_.size()) - 1; - try { - slot->thread = std::thread([slot]() { - while (true) { - const std::function* task = nullptr; - int task_index = 0; - std::shared_ptr state; - { - std::unique_lock lock(slot->mutex); - slot->condition.wait(lock, [slot]() { - return slot->shutdown || slot->has_task; - }); - if (!slot->has_task) return; // shutdown - task = slot->task; - task_index = slot->index; - state = std::move(slot->state); - slot->task = nullptr; - slot->has_task = false; - } - // Tasks are documented not to throw (the certifier's worker lambda - // catches everything); swallowing here is the last line of defence - // against terminating the process and stranding the waiter. - try { - (*task)(task_index); - } catch (...) { // NOLINT(bugprone-empty-catch) - } - { - std::lock_guard state_guard(state->mutex); - --state->remaining; - state->condition.notify_all(); - } - } - }); - } catch (const std::system_error&) { - // Reserve() promises never to fail: running with fewer workers is only a - // performance difference. So when the system refuses a thread, drop the - // half-built slot and hand back what was already reserved. - slots_.pop_back(); - break; - } - batch.slots_.push_back(index); - batch.handles_.push_back(slot); - } - return batch; -} - -int WorkerPool::size() const { - std::lock_guard guard(mutex_); - return static_cast(slots_.size()); -} - -void WorkerPool::Release(const std::vector& slots) { - std::lock_guard guard(mutex_); - if (shutdown_) return; - for (const int slot : slots) idle_.push_back(slot); -} - -void WorkerPool::Batch::Dispatch(const std::function& task) { - if (handles_.empty()) return; - DRAKE_DEMAND(state_ == nullptr); - state_ = std::make_shared(); - state_->remaining = static_cast(handles_.size()); - for (int i = 0; i < static_cast(handles_.size()); ++i) { - Slot* const slot = handles_[i]; - { - std::lock_guard guard(slot->mutex); - slot->task = &task; - slot->index = i; - slot->state = state_; - slot->has_task = true; - } - slot->condition.notify_one(); - } -} - -void WorkerPool::Batch::Wait() { - if (state_ == nullptr) return; - { - std::unique_lock lock(state_->mutex); - state_->condition.wait(lock, [this]() { - return state_->remaining == 0; - }); - } - state_.reset(); -} - -WorkerPool::Batch& WorkerPool::Batch::operator=(Batch&& other) noexcept { - if (this == &other) return *this; - Wait(); - if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); - pool_ = other.pool_; - slots_ = std::move(other.slots_); - handles_ = std::move(other.handles_); - state_ = std::move(other.state_); - other.pool_ = nullptr; - other.slots_.clear(); - other.handles_.clear(); - other.state_.reset(); - return *this; -} - -WorkerPool::Batch::~Batch() { - Wait(); - if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); -} - // --------------------------------------------------------------------------- // RunCertifier. // --------------------------------------------------------------------------- -CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, - WorkerPool* workers) { +CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { DRAKE_DEMAND(input.model != nullptr); DRAKE_DEMAND(input.oracle != nullptr); DRAKE_DEMAND(input.table != nullptr); @@ -1112,7 +931,13 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, Statistics stats; std::vector records; - const int requested_threads = std::max(1, options.parallelism.num_threads()); + // 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, 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. @@ -1148,8 +973,6 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, } const bool have_work = !moving_pairs.empty() && num_segments > 0; - const int num_threads = - (workers == nullptr) ? 1 : std::max(1, requested_threads); const auto accumulate = [&](Worker* worker) { stats.nodes += worker->stats().nodes; stats.narrowphase_queries += worker->stats().narrowphase_queries; @@ -1209,39 +1032,47 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, if (first_error == nullptr) first_error = std::current_exception(); }; - // Declared before the batch so that the batch — whose destructor waits for - // the helpers — is torn down first on every path, including the throwing - // one. + // 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::function helper_task; - WorkerPool::Batch batch; + std::vector> helper_futures; Recruitment recruitment; recruitment.nodes_before_hire = kNodesBeforeHiringHelpers; + // 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 (requirement P1 covers the steady state, not this). recruitment.hire = [&]() { - batch = workers->Reserve(num_threads - 1); - const int hired = batch.size(); - if (hired == 0) return; // Pool exhausted: stay serial, still correct. + 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)); } - helper_task = [&](int index) { - try { - helpers[index]->Run(); - } catch (...) { - record_error(); - queue.Abort(); - } - }; // 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); - batch.Dispatch(helper_task); + 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); @@ -1251,7 +1082,9 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, record_error(); queue.Abort(); } - batch.Wait(); + // 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); accumulate(&lead); for (const std::unique_ptr& helper : helpers) { diff --git a/planning/continuous_collision/certifier_internal.h b/planning/continuous_collision/certifier_internal.h index c077efdad65c..c9d175950dd5 100644 --- a/planning/continuous_collision/certifier_internal.h +++ b/planning/continuous_collision/certifier_internal.h @@ -7,12 +7,11 @@ /// /// Nothing in this header is part of the public API; it exists so the facade /// (`continuous_collision_checker.cc`), the certificate replay -/// (`certificate.cc`) and the node loop (`certifier.cc`) can share +/// (`certificate.cc`) and the node loop (`certifier_internal.cc`) can share /// one set of per-call data structures without the core module depending on /// the api layer. #include -#include #include #include #include @@ -132,81 +131,6 @@ class ContextPool { mutable std::vector in_use_; }; -/** A pool of parked worker threads, reused across `Check*` calls. - -Why this exists: the driver used to spawn and join one `std::thread` per worker -per call, which the benchmark measured at ~23 µs per worker — 0.37 ms of pure -overhead on every 16-thread call, enough to make a sub-millisecond check -*slower* in parallel than in serial. Parked threads turn "hire 15 helpers" into -15 condition-variable notifications (a few µs), which is what makes the lazy -recruitment policy of RunCertifier() affordable: the driver can afford to start -serial and hire only once a run has proved itself big enough. - -Threads are created on demand (never at construction), capped at -`Parallelism::Max().num_threads()` per pool, parked on their own condition -variable when idle, and joined by the destructor. A `Batch` is a reservation of -some of them for the duration of one call; because reservations never block, -several concurrent `Check*` calls simply share out whatever threads exist and a -call that gets none just runs with fewer workers. */ -class WorkerPool { - private: - struct Slot; - struct BatchState; - - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(WorkerPool); - - /* Out of line (like the destructor) because Slot is incomplete here. */ - WorkerPool(); - ~WorkerPool(); - - /** Reserved threads for one call. Destruction waits for every dispatched - task to return and then releases the threads back to the pool. */ - class Batch { - public: - Batch() = default; - Batch(const Batch&) = delete; - Batch& operator=(const Batch&) = delete; - Batch(Batch&& other) noexcept { *this = std::move(other); } - Batch& operator=(Batch&& other) noexcept; - ~Batch(); - - /** How many threads were actually reserved (≤ the requested count). */ - int size() const { return static_cast(handles_.size()); } - - /** Runs `task(i)` for every i in [0, size()) on the reserved threads. The - referenced callable must outlive Wait(). Call at most once. */ - void Dispatch(const std::function& task); - - /** Blocks until every dispatched task has returned. Idempotent. */ - void Wait(); - - private: - friend class WorkerPool; - WorkerPool* pool_{}; - std::vector slots_; - std::vector handles_; - std::shared_ptr state_; - }; - - /** Reserves at most `count` currently idle threads, creating new ones (a - cold path) while the pool is below its cap. Never blocks. */ - Batch Reserve(int count); - - /** Number of threads the pool has created (for tests/diagnostics). */ - int size() const; - - private: - void Release(const std::vector& slots); - - mutable std::mutex mutex_; - /* A deque so that growing never invalidates the Slot addresses already - handed out to live batches. */ - std::deque> slots_; - std::vector idle_; - bool shutdown_{false}; -}; - /** Per-pair broadphase data for the free-sphere prefilter (the interval certificate): 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. @@ -296,9 +220,10 @@ are unchanged. visited `kNodesBeforeHiringHelpers` nodes. A check whose whole workload is smaller than that (a PWL edge, a shallow shelf check) therefore runs at exactly serial speed no matter what `Options::parallelism` says — which - matters because `Parallelism::Max()` is the default. Helpers come from a - `WorkerPool` that outlives the call, so hiring costs notifications rather - than thread creation. + matters because `Parallelism::Max()` is the default. Helpers are call-scoped + threads, spawned once per check when (and only when) that threshold is + crossed and joined before the call returns; nothing here owns a background + thread between calls. - **Determinism policy — unchanged, because sharing moves nodes between workers without changing which nodes exist.** Every node's decisions depend only on its own control points and its inherited active set, so the tree, the @@ -320,13 +245,12 @@ dependent place, and 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. -`pool` supplies the per-thread contexts; `workers` supplies the helper threads -and may be null, in which case every call runs serially on the calling thread. +`pool` supplies the per-thread contexts. Helper threads, if any are hired, are +created and joined within this call. @throws std::exception if the oracle throws for any pair; a parallel run waits for every worker first and rethrows the first failure. */ -CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool, - WorkerPool* workers); +CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool); // --------------------------------------------------------------------------- // Certificate assembly + independent replay (implemented in certificate.cc). diff --git a/planning/continuous_collision/continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc index 4d2d6626290d..4f823a945e85 100644 --- a/planning/continuous_collision/continuous_collision_checker.cc +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -454,8 +454,7 @@ class ContinuousCollisionChecker::Impl { input.prefilter = &prefilter_; input.options = options; - internal::CertifierOutput output = - internal::RunCertifier(input, &pool_, &worker_pool_); + internal::CertifierOutput output = internal::RunCertifier(input, &pool_); CertificationResult result; result.verdict = output.verdict; @@ -483,11 +482,6 @@ class ContinuousCollisionChecker::Impl { std::vector tau_base_; internal::PrefilterTable prefilter_; mutable internal::ContextPool pool_; - /** Parked helper threads, created on demand by the first call that hires - any and reused by every later call (see internal::WorkerPool). Declared last - so that its destructor — which joins every parked thread — runs before the - contexts and tables those threads worked on are torn down. */ - mutable internal::WorkerPool worker_pool_; }; // --------------------------------------------------------------------------- diff --git a/planning/continuous_collision/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc index 3979cd2fc322..47d0df3e791d 100644 --- a/planning/continuous_collision/test/concurrency_test.cc +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -23,7 +23,7 @@ /// inside one segment gets measurably faster with threads, and a check /// too small to pay for workers is no slower at Parallelism::Max() than /// serially. These are the two regressions the driver rework of -/// certifier.cc fixed, as the benchmark suite's thread-scaling +/// certifier_internal.cc fixed, as the benchmark suite's thread-scaling /// results measured; the deep /// workload is also where the sharing path gets its TSan coverage, since /// the corpus cases of claims 1-3 are far too small to hire a helper. @@ -606,8 +606,8 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { // 4. Per-call parallel scaling. // --------------------------------------------------------------------------- // -// These pin the two properties the driver rework of certifier.cc exists -// for, and that the benchmark suite's thread-scaling results measured +// These pin the two properties the driver rework of certifier_internal.cc +// exists for, and that the benchmark suite's thread-scaling results measured // the old driver failing: // // a) a deep tree inside a single segment actually spreads over the workers From ce3f8c1c4545987cee1f0eda303c13d0c762c037 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 27 Aug 2026 15:28:22 -0400 Subject: [PATCH 12/22] [planning] continuous_collision: fit tests to CI budgets across build flavors The suite was written against Release timings and a Release corpus, so several targets either failed or overran under the instrumented build flavors. Each fix is local to the flavor that needed it. Timing claims out of concurrency_test. DeepWorkloadIsFasterInParallel and SmallCheckIsNotSlowerInParallel are wall-clock claims in a file that is otherwise all equalities. Valgrind serializes threads, which inverts "parallel is faster than serial" and fails them for a reason unrelated to the driver. They move to concurrency_timing_test, which carries disable_in_compilation_mode_dbg and no_valgrind_tools; the determinism and invariance cases stay in concurrency_test and keep running everywhere, sanitizers included. The corpus, the deep workload and the option defaults they share now live in test/concurrency_test_utilities.h. The skip predicate also consults VALGRIND_OPTS, ASAN_OPTIONS, LSAN_OPTIONS, TSAN_OPTIONS and UBSAN_OPTIONS, since a tool that instruments at runtime is invisible to the compile-time tests it used to rely on (the same check limit_malloc.cc and gcs_trajectory_optimization_test.cc make). soundness_fuzz_test. Corpus composition was asserted as absolute counts, which pinned the test to a 200-case corpus. The floors become fractions of kNumCases, and kNumCases drops to 50 under sanitizers or memcheck, where the dense cross-check is one to two orders of magnitude slower. The floors still demand at least one case of every kind at that size, which a static_assert enforces. asan and lsan are excluded outright (the corpus is deliberately leaked), the timeout goes to "long", and the file's budget note now says which margins are Release-only. Smaller items: motion_bound_test gains a moderate timeout; certifier_test declares num_threads = 8 for the eight caller threads it spawns; the two temp_directory() calls and all three unchecked std::ofstream writes are gone, since the cube mesh is now Drake's shipped geometry/test/ quad_cube.obj scaled to the same half-extents through the non-uniform Mesh/Convex scale argument, and both L-prisms -- which encode analytic expectations no shipped asset matches -- are built as InMemoryMesh. The benchmark loses its manual tag and becomes testonly so CI compiles it, defaults --out to TEST_TMPDIR, and gains a two-second smoke test on the cheapest scenario. Verified: full suite green; concurrency_test clean under --config=tsan (concurrency_timing_test is filtered out there by its own no_tsan tag, as intended); soundness_fuzz_test passes its fractional assertions with the shrunk corpus forced on under both defines. --- planning/continuous_collision/BUILD.bazel | 94 +++- .../benchmark/iiwa_benchmark.cc | 13 +- .../test/bounding_sphere_test.cc | 27 +- .../test/concurrency_test.cc | 503 +----------------- .../test/concurrency_test_utilities.h | 392 ++++++++++++++ .../test/concurrency_timing_test.cc | 158 ++++++ .../test/distance_oracle_test.cc | 77 ++- .../test/soundness_fuzz_test.cc | 71 ++- 8 files changed, 775 insertions(+), 560 deletions(-) create mode 100644 planning/continuous_collision/test/concurrency_test_utilities.h create mode 100644 planning/continuous_collision/test/concurrency_timing_test.cc diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index f92793c653f0..6c4f6c37534d 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -221,6 +221,7 @@ drake_cc_googletest( # T2 — 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", "//geometry:geometry_roles", @@ -239,7 +240,8 @@ drake_cc_googletest( deps = [ ":bounding_sphere", "//common:essential", - "//common:temp_directory", + "//common:memory_file", + "//geometry:in_memory_mesh", "//geometry:shape_specification", "//geometry/proximity:polygon_surface_mesh", "//math:geometric_transform", @@ -249,11 +251,14 @@ drake_cc_googletest( # T3 — oracle accuracy, probe classification, half-space fallback, V-polytope. drake_cc_googletest( name = "distance_oracle_test", + data = ["//geometry:test_obj_files"], deps = [ ":distance_oracle", ":vpolytope_ingestion", - "//common:temp_directory", + "//common:find_resource", + "//common:memory_file", "//geometry:geometry_instance", + "//geometry:in_memory_mesh", "//geometry:proximity_properties", "//geometry:scene_graph", "//geometry:shape_specification", @@ -270,6 +275,8 @@ drake_cc_googletest( # T4/T6 — certifier semantics on a focused, hand-built corpus. drake_cc_googletest( name = "certifier_test", + # Eight caller threads, each asking for Parallelism(2). + num_threads = 8, deps = [ ":continuous_collision_checker", "//common:parallelism", @@ -288,9 +295,26 @@ drake_cc_googletest( # cross-checked against dense sampling and against the certificate replay. # 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 shrink recovers, and the corpus is deliberately leaked. drake_cc_googletest( name = "soundness_fuzz_test", - timeout = "moderate", + timeout = "long", + # The two settings are mutually exclusive (each dynamic-analysis config + # defines exactly one of them), so this select is unambiguous. + defines = select({ + "//tools:using_memcheck": ["DRAKE_CCD_FUZZ_SMALL_CORPUS"], + "//tools:using_sanitizer": ["DRAKE_CCD_FUZZ_SMALL_CORPUS"], + "//conditions:default": [], + }), + tags = [ + "no_asan", + "no_lsan", + ], deps = [ ":continuous_collision_checker", "//common:parallelism", @@ -345,11 +369,11 @@ drake_cc_googletest( ], ) -# T8 — concurrency determinism. Running with many threads is the point of -# this test: it pins the answer at Parallelism {1, 2, 8, 16}. -drake_cc_googletest( - name = "concurrency_test", - num_threads = 16, +# T8 — the corpus and the deep workload both concurrency targets run on. +drake_cc_library( + name = "concurrency_test_utilities", + testonly = 1, + hdrs = ["test/concurrency_test_utilities.h"], deps = [ ":continuous_collision_checker", "//common:parallelism", @@ -360,6 +384,36 @@ drake_cc_googletest( "//multibody/tree", "//planning:robot_diagram", "//planning:robot_diagram_builder", + "@googletest//:gtest", + ], +) + +# T8 — 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. +drake_cc_googletest( + name = "concurrency_test", + num_threads = 16, + deps = [ + ":concurrency_test_utilities", + "//common:parallelism", + ], +) + +# T8 — the two per-call scaling claims. Split from concurrency_test because +# they are wall-clock claims: Valgrind serializes threads, which inverts +# "parallel is faster than serial" and fails the test for a reason that has +# nothing to do with the driver. disable_in_compilation_mode_dbg already +# excludes the sanitizers and memcheck; no_valgrind_tools adds drd and +# helgrind, which serialize the same way. +drake_cc_googletest( + name = "concurrency_timing_test", + disable_in_compilation_mode_dbg = True, + num_threads = 16, + tags = ["no_valgrind_tools"], + deps = [ + ":concurrency_test_utilities", + "//common:parallelism", ], ) @@ -389,12 +443,18 @@ drake_cc_googletest( # === benchmark/ === -# The performance benchmark suite. Not part of the test suite: a full run -# takes minutes and reports measurements rather than assertions. Run it with +# The performance benchmark suite. A full run takes minutes and reports +# measurements rather than assertions, so it is not a test; run it with # bazel run //planning/continuous_collision:iiwa_benchmark -- \ # --out /tmp/ccd --drake_commit $(git rev-parse HEAD) +# It is not tagged manual, though, so that CI compiles it: the benchmark shares +# every header the library exposes, and a benchmark that stopped building would +# otherwise go unnoticed until someone next needed a measurement. The smoke +# test that comes with it runs the cheapest scenario at one repetition (about +# two seconds) purely to prove the binary still starts and finishes. drake_cc_binary( name = "iiwa_benchmark", + testonly = 1, srcs = [ "benchmark/benchmark_util.cc", "benchmark/benchmark_util.h", @@ -402,10 +462,22 @@ drake_cc_binary( "benchmark/scenario_worlds.cc", "benchmark/scenario_worlds.h", ], + add_test_rule = 1, data = [ "@drake_models//:iiwa_description", ], - tags = ["manual"], + test_rule_args = [ + "--only", + "dual", + "--reps", + "1", + "--warmup", + "0", + "--dense-samples", + "200", + ], + test_rule_size = "small", + test_rule_timeout = "moderate", deps = [ ":continuous_collision_checker", "//common:copyable_unique_ptr", diff --git a/planning/continuous_collision/benchmark/iiwa_benchmark.cc b/planning/continuous_collision/benchmark/iiwa_benchmark.cc index bc2983fe9b5d..cf3d40c655d1 100644 --- a/planning/continuous_collision/benchmark/iiwa_benchmark.cc +++ b/planning/continuous_collision/benchmark/iiwa_benchmark.cc @@ -20,10 +20,12 @@ /// [--dense-samples N] [--batch N] [--only NAME] /// [--drake_commit SHA] /// -/// `--out` defaults to the current directory, and `--drake_commit` (the -/// Drake revision this binary was built from, "unknown" by default) is -/// recorded verbatim in every result file so a JSON result identifies the -/// code it measured. +/// `--out` defaults to the current directory, or to $TEST_TMPDIR when that is +/// set — the sandbox is the only writable directory under `bazel test`, and +/// the smoke-test rule in BUILD.bazel relies on this so it needs no --out of +/// its own. `--drake_commit` (the Drake revision this binary was built from, +/// "unknown" by default) is recorded verbatim in every result file so a JSON +/// result identifies the code it measured. #include #include @@ -1004,6 +1006,9 @@ void RunProfile(const Config& config, const MachineInfo& machine, int Main(int argc, char** argv) { Config config; + if (const char* const test_tmpdir = std::getenv("TEST_TMPDIR")) { + config.out_dir = test_tmpdir; + } for (int i = 1; i < argc; ++i) { const std::string arg = argv[i]; const auto next = [&]() -> std::string { diff --git a/planning/continuous_collision/test/bounding_sphere_test.cc b/planning/continuous_collision/test/bounding_sphere_test.cc index e82e3d9c4993..9d10e6a5f521 100644 --- a/planning/continuous_collision/test/bounding_sphere_test.cc +++ b/planning/continuous_collision/test/bounding_sphere_test.cc @@ -10,10 +10,9 @@ #include "drake/planning/continuous_collision/bounding_sphere.h" #include -#include -#include #include #include +#include #include #include #include @@ -22,7 +21,8 @@ #include #include "drake/common/fmt_eigen.h" -#include "drake/common/temp_directory.h" +#include "drake/common/memory_file.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" @@ -349,14 +349,12 @@ GTEST_TEST(BoundingSphereTest, ConvexContainsHullVertices) { "construction to make this test meaningful"; } -/* Writes a small nonconvex OBJ (an L-shaped prism) so the Mesh path exercises - hull-vs-mesh semantics, not just a convex primitive in disguise. */ -std::string WriteLShapedObj() { - const std::filesystem::path dir = - std::filesystem::path(drake::temp_directory()) / "ccd_bounding_sphere"; - std::filesystem::create_directories(dir); - const std::filesystem::path path = dir / "l_prism.obj"; - std::ofstream out(path); +/* Builds 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}}; @@ -378,16 +376,15 @@ std::string WriteLShapedObj() { out << "f " << a << " " << b << " " << b + 6 << "\n"; out << "f " << a << " " << b + 6 << " " << a + 6 << "\n"; } - out.close(); - return path.string(); + return geometry::InMemoryMesh{ + MemoryFile(out.str(), ".obj", "ccd_l_prism.obj")}; } GTEST_TEST(BoundingSphereTest, MeshContainsHullVertices) { Rng rng(0x5eed0007); - const std::string obj = WriteLShapedObj(); for (const Vector3d& scale3 : {Vector3d(1.0, 1.0, 1.0), Vector3d(0.4, 1.7, 1.0)}) { - const Mesh shape(obj, scale3); + const Mesh shape(LShapedObj(), scale3); const auto& hull = shape.GetConvexHull(); ASSERT_GT(hull.num_vertices(), 3); CheckContainment(shape, HullVertexSampler(hull), diff --git a/planning/continuous_collision/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc index 47d0df3e791d..424e7d9590fc 100644 --- a/planning/continuous_collision/test/concurrency_test.cc +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -2,9 +2,10 @@ /// T8 — concurrency determinism (test plan T8; performance requirement /// P7; parallelism and determinism). /// -/// Four claims are pinned here. The first three run on a fixed corpus of ten -/// T4-style random cases (a mix of free and violating); the fourth builds one -/// deliberately deep workload out of that corpus: +/// Four claims are pinned here, on the fixed corpus of ten T4-style random +/// cases (a mix of free and violating) that +/// concurrency_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} in both search @@ -18,15 +19,17 @@ /// 3. The public Check* methods are safe to call concurrently on one checker /// instance: eight threads hammering one checker get the same answers as /// running the same calls one after another. -/// 4. Per-call parallelism actually distributes work, and never costs -/// anything when there is not enough of it to distribute: a deep tree -/// inside one segment gets measurably faster with threads, and a check -/// too small to pay for workers is no slower at Parallelism::Max() than -/// serially. These are the two regressions the driver rework of -/// certifier_internal.cc fixed, as the benchmark suite's thread-scaling -/// results measured; the deep -/// workload is also where the sharing path gets its TSan coverage, since -/// the corpus cases of claims 1-3 are far too small to hire a helper. +/// 4. The deep workload — the only one big enough that the driver actually +/// hires helpers, which no corpus case is — explores the same tree and +/// reports the same findings at every thread count, and keeps doing so +/// when several callers ask for it at once. This is where the sharing +/// path gets its coverage, TSan's included. +/// +/// Every case here is an equality, not a wall-clock claim, so this target runs +/// under every build flavor. The two timing claims that used to live here — a +/// deep tree gets faster with threads, a small check does not get slower — are +/// in concurrency_timing_test.cc, which is excluded from the build flavors +/// that make a duration meaningless. /// /// TSan. This file is the test to run under ThreadSanitizer. Drake's /// build carries a `tsan` config, so the invocation is: @@ -52,13 +55,8 @@ /// rooted in a /// continuous_collision frame is a real bug. -#include -#include -#include +#include #include -#include -#include -#include #include #include #include @@ -67,293 +65,14 @@ #include #include "drake/common/parallelism.h" -#include "drake/common/trajectories/bezier_curve.h" -#include "drake/geometry/shape_specification.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/robot_diagram.h" -#include "drake/planning/robot_diagram_builder.h" +#include "drake/planning/continuous_collision/test/concurrency_test_utilities.h" namespace drake { namespace planning { namespace continuous_collision { +namespace test { namespace { -using drake::Parallelism; -using drake::geometry::Box; -using drake::geometry::Capsule; -using drake::geometry::Cylinder; -using drake::geometry::HalfSpace; -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 Eigen::Vector3d; -using Eigen::VectorXd; - -constexpr double kMargin = 0.005; -/// Ten cases keeps the full 4-thread-count × 2-mode sweep (80 certification -/// runs) plus the concurrent-call test under a second in Release, which is what -/// makes this affordable to run again under TSan (~100× slower). -constexpr int kNumCases = 10; -constexpr int kMinFreeCases = 3; -constexpr int kMinViolatingCases = 3; - -CoulombFriction Friction() { - return CoulombFriction(1.0, 1.0); -} - -SpatialInertia Inertia() { - return SpatialInertia::SolidSphereWithMass(1.0, 0.05); -} - -/// A four-link chain of revolute and prismatic joints with primitive geometry, -/// four anchored obstacles and (on odd seeds) a HalfSpace floor, so the corpus -/// exercises the native narrowphase route and the analytic one. -std::unique_ptr> MakeWorld(uint64_t seed) { - std::mt19937_64 rng(seed); - const auto uniform = [&rng](double lo, double hi) { - return std::uniform_real_distribution(lo, hi)(rng); - }; - // Every helper below sequences its draws through named locals: the order in - // which a compiler evaluates sibling constructor or operator arguments is - // unspecified, so drawing inline would make the corpus toolchain-dependent - // and could silently shift the free/violating balance this file relies on. - const auto vector3 = [&uniform](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); - }; - const auto direction = [&vector3]() { - Vector3d v; - do { - v = vector3(-1, 1); - } while (v.norm() < 1e-3 || v.norm() > 1.0); - return v.normalized(); - }; - const auto offset = [&direction, &uniform](double lo, double hi) { - const Vector3d unit = direction(); - const double length = uniform(lo, hi); - return Vector3d(unit * length); - }; - const auto pose = [&vector3, &offset](double lo, double hi) { - const Vector3d rpy = vector3(-3, 3); - const Vector3d p = offset(lo, hi); - return RigidTransformd(RollPitchYawd(rpy), p); - }; - - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - std::vector*> links; - for (int i = 0; i < 4; ++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 = vector3(-0.5, 0.5); - const RigidTransformd X_PF(RollPitchYawd(rpy_PF), offset(0.22, 0.32)); - const Vector3d axis = direction(); - if (i == 2) { - plant.AddJoint("j" + std::to_string(i), parent, X_PF, - body, RigidTransformd(), axis); - } else { - plant.AddJoint("j" + std::to_string(i), parent, X_PF, body, - RigidTransformd(), axis); - } - const RigidTransformd X_LG(offset(0.10, 0.16)); - if (i % 2 == 0) { - const double radius = uniform(0.02, 0.04); - const double length = uniform(0.05, 0.10); - plant.RegisterCollisionGeometry(body, X_LG, Capsule(radius, length), - name + "_geom", Friction()); - } else { - const Vector3d size = vector3(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 < 4; ++i) { - const std::string name = "obstacle" + std::to_string(i); - const RigidBody& body = plant.AddRigidBody(name, Inertia()); - plant.WeldFrames(plant.world_frame(), body.body_frame(), pose(0.30, 0.75)); - if (i % 3 == 0) { - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Sphere(uniform(0.05, 0.12)), - name + "_geom", Friction()); - } else if (i % 3 == 1) { - const Vector3d size = vector3(0.08, 0.20); - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Box(size.x(), size.y(), size.z()), - name + "_geom", Friction()); - } else { - const double radius = uniform(0.04, 0.09); - const double length = uniform(0.08, 0.18); - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Cylinder(radius, length), name + "_geom", - Friction()); - } - } - if (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(); -} - -/// A quintic Bézier with random control points, so the corpus has real curved -/// trajectories rather than straight edges. -Eigen::MatrixXd MakeControlPoints(uint64_t seed, int num_positions) { - std::mt19937_64 rng(seed ^ 0xa5a5'5a5a'0f0f'f0f0ull); - std::uniform_real_distribution value(-1.4, 1.4); - Eigen::MatrixXd points(num_positions, 6); - for (int j = 0; j < 6; ++j) { - for (int i = 0; i < num_positions; ++i) points(i, j) = value(rng); - } - return points; -} - -Options BaseOptions(Parallelism parallelism, SearchMode mode) { - Options options; - options.margin = kMargin; - options.parallelism = parallelism; - options.mode = mode; - // Bounded cost per run: the whole sweep is executed 8 times per case. - options.min_interval = 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 deliberately 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.) -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 = MakeWorld(seed); - ContinuousCollisionChecker::Params params; - params.model = entry->model; - params.default_options = - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); - entry->checker = std::make_unique(params); - entry->control_points = - MakeControlPoints(seed, entry->model->plant().num_positions()); - const CertificationResult result = entry->checker->CheckTrajectory( - entry->trajectory(), - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); - entry->serial_verdict = result.verdict; - // Keep the corpus balanced: stop taking more of whichever kind is - // already well represented. - const bool is_free = result.verdict == Verdict::kCertifiedFree; - const bool is_violating = result.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 findings. Nothing here is a tolerance: two runs -/// of the same deterministic computation either agree exactly or the claim of -/// determinism is false. -::testing::AssertionResult FindingsIdentical(const std::vector& a, - const std::vector& b) { - if (a.size() != b.size()) { - return ::testing::AssertionFailure() - << "finding counts differ: " << a.size() << " vs " << b.size(); - } - for (std::size_t i = 0; i < a.size(); ++i) { - if (a[i].time != b[i].time) { - return ::testing::AssertionFailure() - << "finding " << i << " time " << a[i].time << " vs " << b[i].time; - } - if (a[i].q.size() != b[i].q.size() || - !(a[i].q.array() == b[i].q.array()).all()) { - return ::testing::AssertionFailure() - << "finding " << i << " witness configuration differs"; - } - if (a[i].pair.a != b[i].pair.a || a[i].pair.b != b[i].pair.b) { - return ::testing::AssertionFailure() - << "finding " << i << " pair differs"; - } - if (a[i].distance != b[i].distance || - a[i].motion_bound != b[i].motion_bound || - a[i].definite != b[i].definite) { - return ::testing::AssertionFailure() - << "finding " << i << " payload differs"; - } - if (a[i].nearest_a_W.has_value() != b[i].nearest_a_W.has_value() || - (a[i].nearest_a_W.has_value() && - *a[i].nearest_a_W != *b[i].nearest_a_W)) { - return ::testing::AssertionFailure() - << "finding " << i << " witness point A differs"; - } - if (a[i].nearest_b_W.has_value() != b[i].nearest_b_W.has_value() || - (a[i].nearest_b_W.has_value() && - *a[i].nearest_b_W != *b[i].nearest_b_W)) { - return ::testing::AssertionFailure() - << "finding " << i << " witness point B differs"; - } - } - return ::testing::AssertionSuccess(); -} - -::testing::AssertionResult EarliestWitnessIdentical( - const CertificationResult& a, const CertificationResult& b) { - if (a.findings.empty() != b.findings.empty()) { - return ::testing::AssertionFailure() - << "one run reported findings and the other did not"; - } - if (a.findings.empty()) return ::testing::AssertionSuccess(); - return FindingsIdentical({a.findings.front()}, {b.findings.front()}); -} - // --------------------------------------------------------------------------- // 1. The answer does not depend on the thread count. // --------------------------------------------------------------------------- @@ -603,133 +322,14 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { } // --------------------------------------------------------------------------- -// 4. Per-call parallel scaling. +// 4. The deep workload explores the same tree at every thread count. // --------------------------------------------------------------------------- // -// These pin the two properties the driver rework of certifier_internal.cc -// exists for, and that the benchmark suite's thread-scaling results measured -// the old driver failing: -// -// a) a deep tree inside a single segment actually spreads over the workers -// (the old depth-seeded driver got 0.98× at 16 threads on 12 570 nodes, -// because one fixed seed held essentially the whole tree); -// b) a check too small to pay for workers never loses by being asked for -// them — which matters because Parallelism::Max() is the *default* value -// of Options::parallelism. -// -// Both are timing claims, so both are written to survive a loaded machine: a -// ratio with a wide margin, best-of-three, and a skip when the hardware or the -// build cannot support the claim at all. They are not benchmarks — the numbers -// live in benchmark/results/ — they are regression detectors, and they should -// only ever fire on a driver that has stopped distributing work. - -/// True when the build cannot support a meaningful wall-clock claim: a -/// sanitizer build serializes and inflates everything, an unoptimized build -/// changes the ratios, and fewer than eight hardware threads means there is no -/// parallelism to measure. -bool TimingClaimsAreMeaningless() { -#if defined(__SANITIZE_THREAD__) || defined(__SANITIZE_ADDRESS__) - return true; -#elif defined(__has_feature) -#if __has_feature(thread_sanitizer) || __has_feature(address_sanitizer) - return true; -#endif -#endif -#ifndef NDEBUG - return true; -#else - return std::thread::hardware_concurrency() < 8; -#endif -} - -template -double BestOfThreeSeconds(F&& body) { - body(); // Warm up: first-touch page faults, the worker pool's threads. - double best = std::numeric_limits::infinity(); - for (int i = 0; i < 3; ++i) { - const auto start = std::chrono::steady_clock::now(); - body(); - best = std::min(best, std::chrono::duration( - std::chrono::steady_clock::now() - start) - .count()); - } - return best; -} - -/// The bisection's node budget below doubles as the deep workload's size: the -/// margin it converges to is the largest one still certifiable inside this -/// budget, so the tree it produces has just under this many nodes. Large -/// enough that a run takes tens of milliseconds (a wall-clock ratio then means -/// something) and that no fixed seeding depth could ever have covered it; -/// small enough that the ~40 probes that find it, and the timed repetitions -/// that use it, stay cheap — under a sanitizer too. -constexpr uint64_t kProbeBudget = 6000; -constexpr uint64_t kMinDeepNodes = 3000; - -/// A corpus case run at a margin just below its own swept clearance, which is -/// what makes the subdivision tree deep and *narrow* (the soundness argument): -/// certifying a node needs φ̂ − τ − Δ > m, so as the threshold m 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 — exactly the shape a depth-seeded work -/// queue cannot split, and the shape the benchmark suite's thread-scaling -/// results measured the old driver getting 0.98× on. -/// -/// That margin is found by bisection rather than hard-coded, so the workload -/// survives any change to the random worlds, the bounds, or Drake: the largest -/// margin still certifiable within kProbeBudget nodes is by construction the -/// one that costs about kProbeBudget nodes. -struct DeepWorkload { - const Case* entry{}; - double margin{0.0}; - double min_interval{1e-8}; - uint64_t nodes{0}; - - Options options(Parallelism parallelism) const { - Options options = BaseOptions(parallelism, SearchMode::kCertifyAll); - options.margin = margin; - options.min_interval = min_interval; - return options; - } -}; - -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; - options.max_nodes = kProbeBudget; - return deep->entry->checker - ->CheckTrajectory(deep->entry->trajectory(), options) - .verdict == Verdict::kCertifiedFree; - }; - 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->nodes = deep->entry->checker - ->CheckTrajectory(deep->entry->trajectory(), - deep->options(Parallelism::None())) - .stats.nodes; - return deep; - }(); - return *workload; -} +// The sharing path only ever runs on a workload big enough to hire a helper, +// which the corpus cases of claims 1-3 never are. These cases are where it +// gets its coverage — including its TSan coverage — and they are equalities, +// so unlike the wall-clock claims in concurrency_timing_test.cc they run +// everywhere. GTEST_TEST(ConcurrencyTest, DeepWorkloadIsBigEnoughToBeWorthSpreading) { // Without this the two tests below could silently degenerate into measuring @@ -804,61 +404,8 @@ GTEST_TEST(ConcurrencyTest, DeepWorkloadSurvivesConcurrentParallelCalls) { for (int t = 0; t < kThreads; ++t) EXPECT_EQ(mismatches[t], 0); } -GTEST_TEST(ConcurrencyTest, DeepWorkloadIsFasterInParallel) { - if (TimingClaimsAreMeaningless()) GTEST_SKIP(); - const DeepWorkload& deep = Deep(); - ASSERT_NE(deep.entry, nullptr); - const BezierCurve trajectory = deep.entry->trajectory(); - const Options serial_options = deep.options(Parallelism::None()); - const Options parallel_options = deep.options(Parallelism(8)); - - const double serial = BestOfThreeSeconds([&]() { - deep.entry->checker->CheckTrajectory(trajectory, serial_options); - }); - const double parallel = BestOfThreeSeconds([&]() { - deep.entry->checker->CheckTrajectory(trajectory, parallel_options); - }); - std::cout << "\n[ T8 ] deep workload: serial " << 1e3 * serial - << " ms, Parallelism(8) " << 1e3 * parallel << " ms (" - << serial / parallel << "x)\n\n"; - // Eight threads measure ~6x on the benchmark machine; 1.43x is the bound - // that separates "the driver distributes deep work" from the old driver's - // 0.98x without being a performance assertion in disguise. - EXPECT_LT(parallel, 0.7 * serial); -} - -GTEST_TEST(ConcurrencyTest, SmallCheckIsNotSlowerInParallel) { - if (TimingClaimsAreMeaningless()) GTEST_SKIP(); - // A two-waypoint edge in one of the corpus worlds is the small check: a - // handful of nodes, dominated by the serial breakpoint pass. Asked for the - // default Parallelism::Max(), the driver must decline to hire anyone rather - // than pay a worker-startup bill several times the size of the work. - const Case& entry = *Corpus().front(); - const VectorXd q1 = entry.control_points.col(0); - const VectorXd q2 = entry.control_points.rightCols(1); - const Options serial_options = - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); - const Options parallel_options = - BaseOptions(Parallelism::Max(), SearchMode::kCertifyAll); - ASSERT_LT(entry.checker->CheckEdge(q1, q2, serial_options).stats.nodes, 100u); - - const double serial = BestOfThreeSeconds([&]() { - entry.checker->CheckEdge(q1, q2, serial_options); - }); - const double parallel = BestOfThreeSeconds([&]() { - entry.checker->CheckEdge(q1, q2, parallel_options); - }); - std::cout << "\n[ T8 ] small check: serial " << 1e3 * serial - << " ms, Parallelism::Max() " << 1e3 * parallel << " ms (" - << serial / parallel << "x)\n\n"; - // Parity is what the driver actually delivers (it never hires for a check - // this small, so the two paths run the same code); the 1.5x bound leaves - // room for scheduler noise on a loaded machine without letting a return of - // the old 2.6x slowdown through. - EXPECT_LT(parallel, 1.5 * serial); -} - } // namespace +} // namespace test } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/test/concurrency_test_utilities.h b/planning/continuous_collision/test/concurrency_test_utilities.h new file mode 100644 index 000000000000..38ba2068cae0 --- /dev/null +++ b/planning/continuous_collision/test/concurrency_test_utilities.h @@ -0,0 +1,392 @@ +#pragma once + +/// @file +/// The shared fixture of the two T8 concurrency targets: the random corpus +/// that `concurrency_test.cc` pins the driver's determinism against, and the +/// deliberately deep workload that both it and `concurrency_timing_test.cc` +/// need. It lives in a header because each target wants its own copy — the +/// corpus and the deep workload are lazily built per binary — and because +/// duplicating three hundred lines of world generation between the two files +/// would be worse than sharing them. +/// +/// Nothing here asserts; the claims live in the two test files. + +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/common/trajectories/bezier_curve.h" +#include "drake/geometry/shape_specification.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/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::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 Eigen::Vector3d; +using Eigen::VectorXd; + +constexpr double kMargin = 0.005; +/// Ten cases keeps the full 4-thread-count × 2-mode sweep (80 certification +/// runs) plus the concurrent-call test under a second in Release, which is what +/// makes this affordable to run again under TSan (~100× slower). +constexpr int kNumCases = 10; +constexpr int kMinFreeCases = 3; +constexpr int kMinViolatingCases = 3; + +inline CoulombFriction Friction() { + return CoulombFriction(1.0, 1.0); +} + +inline SpatialInertia Inertia() { + return SpatialInertia::SolidSphereWithMass(1.0, 0.05); +} + +/// A four-link chain of revolute and prismatic joints with primitive geometry, +/// four anchored obstacles and (on odd seeds) a HalfSpace floor, so the corpus +/// exercises the native narrowphase route and the analytic one. +inline std::unique_ptr> MakeWorld(uint64_t seed) { + std::mt19937_64 rng(seed); + const auto uniform = [&rng](double lo, double hi) { + return std::uniform_real_distribution(lo, hi)(rng); + }; + // Every helper below sequences its draws through named locals: the order in + // which a compiler evaluates sibling constructor or operator arguments is + // unspecified, so drawing inline would make the corpus toolchain-dependent + // and could silently shift the free/violating balance this file relies on. + const auto vector3 = [&uniform](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); + }; + const auto direction = [&vector3]() { + Vector3d v; + do { + v = vector3(-1, 1); + } while (v.norm() < 1e-3 || v.norm() > 1.0); + return v.normalized(); + }; + const auto offset = [&direction, &uniform](double lo, double hi) { + const Vector3d unit = direction(); + const double length = uniform(lo, hi); + return Vector3d(unit * length); + }; + const auto pose = [&vector3, &offset](double lo, double hi) { + const Vector3d rpy = vector3(-3, 3); + const Vector3d p = offset(lo, hi); + return RigidTransformd(RollPitchYawd(rpy), p); + }; + + RobotDiagramBuilder builder; + MultibodyPlant& plant = builder.plant(); + std::vector*> links; + for (int i = 0; i < 4; ++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 = vector3(-0.5, 0.5); + const RigidTransformd X_PF(RollPitchYawd(rpy_PF), offset(0.22, 0.32)); + const Vector3d axis = direction(); + if (i == 2) { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, + body, RigidTransformd(), axis); + } else { + plant.AddJoint("j" + std::to_string(i), parent, X_PF, body, + RigidTransformd(), axis); + } + const RigidTransformd X_LG(offset(0.10, 0.16)); + if (i % 2 == 0) { + const double radius = uniform(0.02, 0.04); + const double length = uniform(0.05, 0.10); + plant.RegisterCollisionGeometry(body, X_LG, Capsule(radius, length), + name + "_geom", Friction()); + } else { + const Vector3d size = vector3(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 < 4; ++i) { + const std::string name = "obstacle" + std::to_string(i); + const RigidBody& body = plant.AddRigidBody(name, Inertia()); + plant.WeldFrames(plant.world_frame(), body.body_frame(), pose(0.30, 0.75)); + if (i % 3 == 0) { + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Sphere(uniform(0.05, 0.12)), + name + "_geom", Friction()); + } else if (i % 3 == 1) { + const Vector3d size = vector3(0.08, 0.20); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Box(size.x(), size.y(), size.z()), + name + "_geom", Friction()); + } else { + const double radius = uniform(0.04, 0.09); + const double length = uniform(0.08, 0.18); + plant.RegisterCollisionGeometry(body, RigidTransformd(), + Cylinder(radius, length), name + "_geom", + Friction()); + } + } + if (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(); +} + +/// A quintic Bézier 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) { + std::mt19937_64 rng(seed ^ 0xa5a5'5a5a'0f0f'f0f0ull); + std::uniform_real_distribution value(-1.4, 1.4); + Eigen::MatrixXd points(num_positions, 6); + for (int j = 0; j < 6; ++j) { + for (int i = 0; i < num_positions; ++i) points(i, j) = value(rng); + } + return points; +} + +inline Options BaseOptions(Parallelism parallelism, SearchMode mode) { + Options options; + options.margin = kMargin; + options.parallelism = parallelism; + options.mode = mode; + // Bounded cost per run: the whole sweep is executed 8 times per case. + options.min_interval = 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 deliberately 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 = MakeWorld(seed); + ContinuousCollisionChecker::Params params; + params.model = entry->model; + params.default_options = + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); + entry->checker = std::make_unique(params); + entry->control_points = + MakeControlPoints(seed, entry->model->plant().num_positions()); + const CertificationResult result = entry->checker->CheckTrajectory( + entry->trajectory(), + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); + entry->serial_verdict = result.verdict; + // Keep the corpus balanced: stop taking more of whichever kind is + // already well represented. + const bool is_free = result.verdict == Verdict::kCertifiedFree; + const bool is_violating = result.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 findings. 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 FindingsIdentical( + const std::vector& a, const std::vector& b) { + if (a.size() != b.size()) { + return ::testing::AssertionFailure() + << "finding counts differ: " << a.size() << " vs " << b.size(); + } + for (std::size_t i = 0; i < a.size(); ++i) { + if (a[i].time != b[i].time) { + return ::testing::AssertionFailure() + << "finding " << i << " time " << a[i].time << " vs " << b[i].time; + } + if (a[i].q.size() != b[i].q.size() || + !(a[i].q.array() == b[i].q.array()).all()) { + return ::testing::AssertionFailure() + << "finding " << i << " witness configuration differs"; + } + if (a[i].pair.a != b[i].pair.a || a[i].pair.b != b[i].pair.b) { + return ::testing::AssertionFailure() + << "finding " << i << " pair differs"; + } + if (a[i].distance != b[i].distance || + a[i].motion_bound != b[i].motion_bound || + a[i].definite != b[i].definite) { + return ::testing::AssertionFailure() + << "finding " << i << " payload differs"; + } + if (a[i].nearest_a_W.has_value() != b[i].nearest_a_W.has_value() || + (a[i].nearest_a_W.has_value() && + *a[i].nearest_a_W != *b[i].nearest_a_W)) { + return ::testing::AssertionFailure() + << "finding " << i << " witness point A differs"; + } + if (a[i].nearest_b_W.has_value() != b[i].nearest_b_W.has_value() || + (a[i].nearest_b_W.has_value() && + *a[i].nearest_b_W != *b[i].nearest_b_W)) { + return ::testing::AssertionFailure() + << "finding " << i << " witness point B differs"; + } + } + return ::testing::AssertionSuccess(); +} + +inline ::testing::AssertionResult EarliestWitnessIdentical( + const CertificationResult& a, const CertificationResult& b) { + if (a.findings.empty() != b.findings.empty()) { + return ::testing::AssertionFailure() + << "one run reported findings and the other did not"; + } + if (a.findings.empty()) return ::testing::AssertionSuccess(); + return FindingsIdentical({a.findings.front()}, {b.findings.front()}); +} + +/// The bisection's node budget in Deep() doubles as the deep workload's size: +/// the margin it converges to is the largest one still certifiable inside this +/// budget, so the tree it produces has just under this many nodes. Large +/// enough that a run takes tens of milliseconds (a wall-clock ratio then means +/// something) and that no fixed seeding depth could ever have covered it; +/// small enough that the ~40 probes that find it, and the timed repetitions +/// concurrency_timing_test.cc runs on it, stay cheap — under a sanitizer too. +/// +/// 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 = 3000; + +/// A corpus case run at a margin just below its own swept clearance, which is +/// what makes the subdivision tree deep and *narrow* (the soundness argument): +/// certifying a node needs φ̂ − τ − Δ > m, so as the threshold m 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 — exactly the shape a depth-seeded work +/// queue cannot split, and the shape the benchmark suite's thread-scaling +/// results measured the old driver getting 0.98× on. +/// +/// That margin is found by bisection rather than hard-coded, so the workload +/// survives any change to the random worlds, the bounds, or Drake: the largest +/// margin still certifiable within kProbeBudget nodes is by construction the +/// one that costs about kProbeBudget nodes. +struct DeepWorkload { + const Case* entry{}; + double margin{0.0}; + double min_interval{1e-8}; + uint64_t nodes{0}; + + Options options(Parallelism parallelism) const { + Options options = BaseOptions(parallelism, SearchMode::kCertifyAll); + options.margin = margin; + options.min_interval = min_interval; + 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; + options.max_nodes = kProbeBudget; + return deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), options) + .verdict == Verdict::kCertifiedFree; + }; + 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->nodes = deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), + deep->options(Parallelism::None())) + .stats.nodes; + return deep; + }(); + return *workload; +} + +} // namespace test +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/test/concurrency_timing_test.cc b/planning/continuous_collision/test/concurrency_timing_test.cc new file mode 100644 index 000000000000..e6239790a33b --- /dev/null +++ b/planning/continuous_collision/test/concurrency_timing_test.cc @@ -0,0 +1,158 @@ +/// @file +/// T8 — the two per-call parallel *scaling* claims, split out of +/// concurrency_test.cc because they are wall-clock claims and it is not. +/// +/// Every other T8 case is an equality and runs under every build flavor. A +/// duration, by contrast, means nothing under an instrumented build: Valgrind +/// serializes threads outright, so `parallel < serial` inverts and the case +/// fails for a reason that has nothing to do with the driver. Hence the +/// separate target, which carries disable_in_compilation_mode_dbg and the +/// no_valgrind_tools tag, and hence TimingClaimsAreMeaningless() below, which +/// skips whatever the build tags did not already exclude. +/// +/// The corpus, the deep workload and the option defaults are shared with +/// concurrency_test.cc through concurrency_test_utilities.h. + +#include +#include +#include +#include +#include +#include + +#include + +#include "drake/common/parallelism.h" +#include "drake/planning/continuous_collision/test/concurrency_test_utilities.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace test { +namespace { + +// --------------------------------------------------------------------------- +// 4. Per-call parallel scaling. +// --------------------------------------------------------------------------- +// +// These pin the two properties the driver rework of certifier_internal.cc +// exists for, and that the benchmark suite's thread-scaling results measured +// the old driver failing: +// +// a) a deep tree inside a single segment actually spreads over the workers +// (the old depth-seeded driver got 0.98× at 16 threads on 12 570 nodes, +// because one fixed seed held essentially the whole tree); +// b) a check too small to pay for workers never loses by being asked for +// them — which matters because Parallelism::Max() is the *default* value +// of Options::parallelism. +// +// Both are timing claims, so both are written to survive a loaded machine: a +// ratio with a wide margin, best-of-three, and a skip when the hardware or the +// build cannot support the claim at all. They are not benchmarks — the numbers +// live in benchmark/results/ — they are regression detectors, and they should +// only ever fire on a driver that has stopped distributing work. + +/// True when the build cannot support a meaningful wall-clock claim: a +/// sanitizer build serializes and inflates everything, an unoptimized build +/// changes the ratios, and fewer than eight hardware threads means there is no +/// parallelism to measure. +/// +/// The compile-time tests below only see the sanitizers this translation unit +/// was itself instrumented with. Valgrind instruments nothing at compile time, +/// and a sanitizer runtime linked in from elsewhere is equally invisible, so +/// the environment is consulted too: the tools that make a duration +/// meaningless all announce themselves through an options variable. That is +/// the same test limit_malloc.cc uses to disarm itself, and the same +/// VALGRIND_OPTS check gcs_trajectory_optimization_test.cc uses. +bool TimingClaimsAreMeaningless() { +#if defined(__SANITIZE_THREAD__) || defined(__SANITIZE_ADDRESS__) + return true; +#elif defined(__has_feature) +#if __has_feature(thread_sanitizer) || __has_feature(address_sanitizer) + return true; +#endif +#endif +#ifndef NDEBUG + return true; +#else + for (const char* variable : {"VALGRIND_OPTS", "ASAN_OPTIONS", "LSAN_OPTIONS", + "TSAN_OPTIONS", "UBSAN_OPTIONS"}) { + if (std::getenv(variable) != nullptr) return true; + } + return std::thread::hardware_concurrency() < 8; +#endif +} + +template +double BestOfThreeSeconds(F&& body) { + body(); // Warm up: first-touch page faults, Drake's own lazy caches. + double best = std::numeric_limits::infinity(); + for (int i = 0; i < 3; ++i) { + const auto start = std::chrono::steady_clock::now(); + body(); + best = std::min(best, std::chrono::duration( + std::chrono::steady_clock::now() - start) + .count()); + } + return best; +} + +GTEST_TEST(ConcurrencyTest, DeepWorkloadIsFasterInParallel) { + if (TimingClaimsAreMeaningless()) GTEST_SKIP(); + const DeepWorkload& deep = Deep(); + ASSERT_NE(deep.entry, nullptr); + const BezierCurve trajectory = deep.entry->trajectory(); + const Options serial_options = deep.options(Parallelism::None()); + const Options parallel_options = deep.options(Parallelism(8)); + + const double serial = BestOfThreeSeconds([&]() { + deep.entry->checker->CheckTrajectory(trajectory, serial_options); + }); + const double parallel = BestOfThreeSeconds([&]() { + deep.entry->checker->CheckTrajectory(trajectory, parallel_options); + }); + std::cout << "\n[ T8 ] deep workload: serial " << 1e3 * serial + << " ms, Parallelism(8) " << 1e3 * parallel << " ms (" + << serial / parallel << "x)\n\n"; + // Eight threads measure ~6x on the benchmark machine; 1.43x is the bound + // that separates "the driver distributes deep work" from the old driver's + // 0.98x without being a performance assertion in disguise. + EXPECT_LT(parallel, 0.7 * serial); +} + +GTEST_TEST(ConcurrencyTest, SmallCheckIsNotSlowerInParallel) { + if (TimingClaimsAreMeaningless()) GTEST_SKIP(); + // A two-waypoint edge in one of the corpus worlds is the small check: a + // handful of nodes, dominated by the serial breakpoint pass. Asked for the + // default Parallelism::Max(), the driver must decline to hire anyone rather + // than pay a worker-startup bill several times the size of the work. + const Case& entry = *Corpus().front(); + const VectorXd q1 = entry.control_points.col(0); + const VectorXd q2 = entry.control_points.rightCols(1); + const Options serial_options = + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); + const Options parallel_options = + BaseOptions(Parallelism::Max(), SearchMode::kCertifyAll); + ASSERT_LT(entry.checker->CheckEdge(q1, q2, serial_options).stats.nodes, 100u); + + const double serial = BestOfThreeSeconds([&]() { + entry.checker->CheckEdge(q1, q2, serial_options); + }); + const double parallel = BestOfThreeSeconds([&]() { + entry.checker->CheckEdge(q1, q2, parallel_options); + }); + std::cout << "\n[ T8 ] small check: serial " << 1e3 * serial + << " ms, Parallelism::Max() " << 1e3 * parallel << " ms (" + << serial / parallel << "x)\n\n"; + // Parity is what the driver actually delivers (it never hires for a check + // this small, so the two paths run the same code); the 1.5x bound leaves + // room for scheduler noise on a loaded machine without letting a return of + // the old 2.6x slowdown through. + EXPECT_LT(parallel, 1.5 * serial); +} + +} // 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 index be5fb470a4df..52cb075a06b4 100644 --- a/planning/continuous_collision/test/distance_oracle_test.cc +++ b/planning/continuous_collision/test/distance_oracle_test.cc @@ -10,21 +10,22 @@ #include #include -#include -#include #include #include #include #include #include +#include #include #include #include #include -#include "drake/common/temp_directory.h" +#include "drake/common/find_resource.h" +#include "drake/common/memory_file.h" #include "drake/geometry/geometry_instance.h" +#include "drake/geometry/in_memory_mesh.h" #include "drake/geometry/optimization/vpolytope.h" #include "drake/geometry/proximity_properties.h" #include "drake/geometry/query_object.h" @@ -52,6 +53,7 @@ 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; @@ -249,31 +251,26 @@ const Vector3d& CubeHalf() { return half; } -/// Writes a closed triangulated box OBJ; returns its path. +/// The cube of the mesh cases, as Drake's own shipped unit cube (vertices at +/// ±1) scaled to CubeHalf(). Using the shipped asset instead of writing one +/// keeps the test off the filesystem and off any assumption about which OBJ +/// dialect Drake's reader accepts; the non-uniform Mesh/Convex scale argument +/// reproduces exactly the half-extents the analytic expectations below use, so +/// its vertices coincide with BoxCorners(CubeHalf()) to the last bit. const std::string& CubeObjPath() { - static const std::string path = [] { - const std::filesystem::path p = - std::filesystem::path(drake::temp_directory()) / "ccd_cube.obj"; - std::ofstream out(p); - const Vector3d& h = CubeHalf(); - const double xs[8] = {-1, 1, 1, -1, -1, 1, 1, -1}; - const double ys[8] = {-1, -1, 1, 1, -1, -1, 1, 1}; - const double zs[8] = {-1, -1, -1, -1, 1, 1, 1, 1}; - for (int i = 0; i < 8; ++i) { - out << "v " << xs[i] * h.x() << " " << ys[i] * h.y() << " " - << zs[i] * h.z() << "\n"; - } - const int faces[12][3] = {{1, 4, 3}, {1, 3, 2}, {5, 6, 7}, {5, 7, 8}, - {1, 2, 6}, {1, 6, 5}, {3, 4, 8}, {3, 8, 7}, - {4, 1, 5}, {4, 5, 8}, {2, 3, 7}, {2, 7, 6}}; - for (const auto& f : faces) { - out << "f " << f[0] << " " << f[1] << " " << f[2] << "\n"; - } - return p.string(); - }(); + 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() { @@ -284,12 +281,14 @@ const std::vector& LProfile() { constexpr double kLHalfHeight = 0.5; -/// Writes a closed, genuinely non-convex L-prism OBJ; returns its path. -const std::string& LPrismObjPath() { - static const std::string path = [] { - const std::filesystem::path p = - std::filesystem::path(drake::temp_directory()) / "ccd_l_prism.obj"; - std::ofstream out(p); +/// A closed, genuinely non-convex L-prism. This one stays generated — it +/// encodes the analytic expectations of the mesh-vs-hull cases below and no +/// shipped asset matches them — but it is generated into memory rather than +/// into a file, so the test needs neither a temp directory nor a write that +/// could fail unnoticed. +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}) { @@ -308,9 +307,9 @@ const std::string& LPrismObjPath() { out << "f 1 " << i + 2 << " " << i + 1 << "\n"; out << "f " << 1 + n << " " << i + 1 + n << " " << i + 2 + n << "\n"; } - return p.string(); + return out.str(); }(); - return path; + return InMemoryMesh{MemoryFile(contents, ".obj", "ccd_l_prism.obj")}; } // ========================================================================== @@ -588,7 +587,7 @@ class AllShapesTest : public ::testing::Test { Vector3d(-1.5, 1.5, 1)); AddShapeBody(&plant, "convex", Convex(BoxCorners(CubeHalf()), "convex"), Vector3d(-0.5, 1.5, 1)); - AddShapeBody(&plant, "mesh", Mesh(CubeObjPath()), 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(), @@ -838,7 +837,7 @@ GTEST_TEST(DistanceOracleMesh, MeshDistanceEqualsConvexHullDistance) { // 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", Mesh(CubeObjPath()), Vector3d(-1, 0, 0)); + AddShapeBody(&plant, "mesh", CubeMesh(), Vector3d(-1, 0, 0)); const auto& convex_body = AddShapeBody(&plant, "convex", Convex(BoxCorners(CubeHalf()), "cube"), Vector3d(1, 0, 0)); @@ -886,9 +885,9 @@ GTEST_TEST(DistanceOracleMesh, RobotDiagramBuilder builder(0.0); MultibodyPlant& plant = builder.plant(); const auto& l_mesh_body = - AddShapeBody(&plant, "l_mesh", Mesh(LPrismObjPath()), Vector3d(0, 0, 0)); - const auto& l_convex_body = AddShapeBody( - &plant, "l_convex", Convex(LPrismObjPath()), Vector3d(0, 0, 0)); + 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)); @@ -939,9 +938,9 @@ GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { RobotDiagramBuilder builder(0.0); MultibodyPlant& plant = builder.plant(); const auto& mesh_body = - AddShapeBody(&plant, "mesh", Mesh(CubeObjPath()), Vector3d(0, 0, 1)); + AddShapeBody(&plant, "mesh", CubeMesh(), Vector3d(0, 0, 1)); const auto& convex_body = - AddShapeBody(&plant, "convex", Convex(CubeObjPath()), Vector3d(0, 3, 1)); + AddShapeBody(&plant, "convex", CubeConvex(), Vector3d(0, 3, 1)); const GeometryId ground = plant.RegisterCollisionGeometry( plant.world_body(), RigidTransformd::Identity(), HalfSpace(), "ground_geometry", Friction()); diff --git a/planning/continuous_collision/test/soundness_fuzz_test.cc b/planning/continuous_collision/test/soundness_fuzz_test.cc index 7ac640893efc..576508091493 100644 --- a/planning/continuous_collision/test/soundness_fuzz_test.cc +++ b/planning/continuous_collision/test/soundness_fuzz_test.cc @@ -22,9 +22,18 @@ /// Budget. 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. /// kNumCases = 200 clears test-plan T4's ≥ 150 (world, trajectory) pairs per CI -/// run by a third and measures ~14 s in Release here — a 10× margin against the -/// ~3 min budget, so the suite still fits on a CI machine an order of magnitude -/// slower. The spare budget is spent on resolution rather than on more +/// run by a third and measures ~14 s in Release here, a 10× margin against the +/// ~3 min budget — but that margin is a *Release* margin, and it is the only +/// flavor in which it is that comfortable. An instrumented build slows the +/// dense sweep by one to two orders of magnitude, which would spend the whole +/// budget and more, so two things give: asan and lsan are excluded outright +/// (BUILD.bazel tags), and the flavors that do run the fuzz — tsan, ubsan, +/// memcheck — take a quarter corpus (`DRAKE_CCD_FUZZ_SMALL_CORPUS`), which +/// buys back a 4× and leaves the timeout, raised to "long", to absorb the +/// rest. Composition is asserted as fractions of kNumCases so both corpus +/// sizes are held to the same standard. +/// +/// The spare Release budget is spent on resolution rather than on more /// shallowly-checked cases: kDenseSamples = 10⁴ resolves any clearance dip /// wider than ~10⁻⁴ of the domain, and every 10th certified case gets the 10⁵ /// sweep, which resolves 10× finer at 10× the cost. (Sample counts are per @@ -99,7 +108,31 @@ 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 (BUILD.bazel selects +/// this on //tools:using_sanitizer and //tools:using_memcheck). Nothing about +/// the case *recipes* changes, +/// 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 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. The fractions are the ones +/// the 200-case corpus has always been checked against. +constexpr int kMinCertified = kNumCases / 5; // 20% +constexpr int kMinViolation = kNumCases / 10; // 10% +constexpr int kMinInconclusive = kNumCases / 40; // 2.5% +constexpr int kMinDefiniteFindings = kNumCases / 10; // 10% +constexpr int kMinInconclusiveFindings = kNumCases / 40; // 2.5% +constexpr int kMinPerTrajectoryFamily = kNumCases / 10; // 10% each +constexpr int kMaxBudgetExhausted = kNumCases / 20; // 5% +constexpr int kMinScanQueries = 500 * kNumCases; + constexpr uint64_t kBaseSeed = 0x5eed'0000'0000'0000ull; constexpr int kDenseSamples = 10000; constexpr int kDeepDenseSamples = 100000; @@ -964,27 +997,39 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { // 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; T4's case + // count is satisfied by the uninstrumented run CI also performs. What it + // still has to be is large enough for the composition floors below to say + // something — at 50 cases the thinnest of them still demands a case. + 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, "test plan T4 asks for >= 150 (world, trajectory) cases per CI run"); - EXPECT_GE(tally.certified, 40); - EXPECT_GE(tally.violation, 20); - EXPECT_GE(tally.inconclusive, 5) +#endif + static_assert(kMinInconclusive >= 1 && kMinInconclusiveFindings >= 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"; - EXPECT_GE(tally.definite_findings, 20); - EXPECT_GE(tally.inconclusive_findings, 5); + EXPECT_GE(tally.definite_findings, kMinDefiniteFindings); + EXPECT_GE(tally.inconclusive_findings, kMinInconclusiveFindings); // The node budget exists to bound a pathological case, not to be the usual // answer: if it starts firing often, the corpus has stopped cross-checking // anything and the numbers above would quietly stop meaning what they say. - EXPECT_LE(tally.budget, kNumCases / 20); + EXPECT_LE(tally.budget, kMaxBudgetExhausted); // All three trajectory families of trajectory normalization must be // represented. - EXPECT_GE(tally.pwl, 20); - EXPECT_GE(tally.bezier, 20); - EXPECT_GE(tally.bspline, 20); + 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, 100000); + EXPECT_GT(tally.scan_queries, kMinScanQueries); // Every supported geometry class must have appeared somewhere in the corpus, // including the analytic HalfSpace route: a fuzz that only ever built spheres // and boxes would leave the τ_p table's expensive rows (capsule, cylinder, From 1c675b7bfb1d6b343c28fc95678f15442c747e25 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 27 Aug 2026 15:45:25 -0400 Subject: [PATCH 13/22] [bindings] Add pydrake bindings for planning continuous_collision Binds the public surface of planning/continuous_collision into a pydrake.planning.continuous_collision submodule: the Options/Verdict/ SearchMode configuration surface, ContinuousCollisionChecker (+ Params) and VerifyCertificate, Certificate records, PiecewiseBezierPath, MotionBoundTable / KinematicsEngine, DistanceOracle, ComputeBoundingSphere, and AddVPolytopeObstacle. Unlike graph_algorithms and trajectory_optimization, which flatten into pydrake.planning because their type names are self-qualifying, this sub-namespace exports names -- Options, Statistics, Certificate, Finding, PairId -- that are only meaningful when namespace-qualified, so it gets its own submodule following the pydrake.geometry.optimization precedent. The two out-param signatures (DeCasteljauSplitAtHalf and DistanceOracle::SignedDistance) are bound as lambdas returning tuples, with the deviation from the C++ signature noted in their docstrings. --- bindings/generated_docstrings/BUILD.bazel | 1 + .../planning_continuous_collision.h | 1128 +++++++++++++++++ bindings/pydrake/planning/BUILD.bazel | 10 + bindings/pydrake/planning/planning_py.cc | 6 + bindings/pydrake/planning/planning_py.h | 3 + .../planning_py_continuous_collision.cc | 495 ++++++++ .../test/continuous_collision_test.py | 297 +++++ 7 files changed, 1940 insertions(+) create mode 100644 bindings/generated_docstrings/planning_continuous_collision.h create mode 100644 bindings/pydrake/planning/planning_py_continuous_collision.cc create mode 100644 bindings/pydrake/planning/test/continuous_collision_test.py 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..cf5a095b0c70 --- /dev/null +++ b/bindings/generated_docstrings/planning_continuous_collision.h @@ -0,0 +1,1128 @@ +#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/bounding_sphere.h" +// #include "drake/planning/continuous_collision/certificate.h" +// #include "drake/planning/continuous_collision/continuous_collision_checker.h" +// #include "drake/planning/continuous_collision/distance_oracle.h" +// #include "drake/planning/continuous_collision/motion_bound_table.h" +// #include "drake/planning/continuous_collision/numerics.h" +// #include "drake/planning/continuous_collision/options.h" +// #include "drake/planning/continuous_collision/piecewise_bezier_path.h" +// #include "drake/planning/continuous_collision/vpolytope_ingestion.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::AddVPolytopeObstacle + struct /* AddVPolytopeObstacle */ { + // Source: drake/planning/continuous_collision/vpolytope_ingestion.h + const char* doc = +R"""(Registers a V-polytope as an anchored obstacle with a collision role +(the geometry-support scope, "V-polytopes as first-class geometry", +ingestion route (b)). + +The polytope is converted to ``drake::geometry::Convex`` through +Drake's own ``VPolytope::ToShapeConvex()`` entry point (a thin wrapper +over the ``Convex(Eigen::Matrix3X points, std::string label, +double scale)`` constructor pinned at M0), then registered on the +plant's world body. The result therefore rides the ordinary native +narrowphase path end to end: the proximity engine and the certifier's +radius/support code all read the same ``Convex::GetConvexHull()`` +object, so the certificate stays sound even for redundant or +degenerate vertex sets. + +Parameter ``plant``: + The plant to register on. Must be non-null, must already be a + registered SceneGraph source, and must NOT be finalized. + +Parameter ``vpoly``: + The polytope. Its vertices are interpreted in the geometry frame + G, i.e. the world-frame obstacle is ``X_WG * + conv(vpoly.vertices())``. Must be 3-dimensional with at least one + vertex. + +Parameter ``X_WG``: + Pose of the geometry frame in the world frame. + +Parameter ``name``: + Geometry name; also used as the ``Convex`` shape's label (which + Drake only uses in its own warning/error messages). Must not + contain a newline. + +Returns: + The id of the newly registered collision geometry. + +Raises: + RuntimeError if ``plant`` is null or already finalized, if + ``vpoly.ambient_dimension() != 3``, if the vertex set is empty, or + if Drake rejects the resulting hull (e.g. a degenerate vertex set + that its hull computation cannot inflate).)"""; + } AddVPolytopeObstacle; + // Symbol: drake::planning::continuous_collision::BezierSegment + struct /* BezierSegment */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1] (trajectory +normalization).)"""; + // Symbol: drake::planning::continuous_collision::BezierSegment::control_points + struct /* control_points */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(n × (m+1); column j is control point P_j.)"""; + } control_points; + // Symbol: drake::planning::continuous_collision::BezierSegment::t_end + struct /* t_end */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = R"""()"""; + } t_end; + // Symbol: drake::planning::continuous_collision::BezierSegment::t_start + struct /* t_start */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(Original time interval (bookkeeping only; the certificate is a +property of the path and is invariant under time reparametrization).)"""; + } t_start; + } BezierSegment; + // Symbol: drake::planning::continuous_collision::BoundingSphere + struct /* BoundingSphere */ { + // Source: drake/planning/continuous_collision/bounding_sphere.h + const char* doc = +R"""(A sphere, expressed in the owning body (link) frame L, that contains a +proximity geometry at every configuration of the body.)"""; + // Symbol: drake::planning::continuous_collision::BoundingSphere::center_L + struct /* center_L */ { + // Source: drake/planning/continuous_collision/bounding_sphere.h + const char* doc = R"""(Sphere center in the body frame.)"""; + } center_L; + // Symbol: drake::planning::continuous_collision::BoundingSphere::radius + struct /* radius */ { + // Source: drake/planning/continuous_collision/bounding_sphere.h + const char* doc = R"""()"""; + } radius; + } BoundingSphere; + // Symbol: drake::planning::continuous_collision::Certificate + struct /* Certificate */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = +R"""(Audit trail of every certification event of a run; an independent +replay (VerifyCertificate, declared in the api header) re-evaluates +every record and checks interval coverage of the full domain per pair.)"""; + // Symbol: drake::planning::continuous_collision::Certificate::pairs + struct /* pairs */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = +R"""(Pair table snapshot the indices refer to.)"""; + } pairs; + // Symbol: drake::planning::continuous_collision::Certificate::records + struct /* records */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } records; + } Certificate; + // Symbol: drake::planning::continuous_collision::CertificateRecord + struct /* CertificateRecord */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = +R"""(One certification event: pair ``pair_index`` was certified over the +parameter interval [s_start, s_end] of segment ``segment`` from +representative configuration qc (the search algorithm).)"""; + // Symbol: drake::planning::continuous_collision::CertificateRecord::motion_bound + struct /* motion_bound */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } motion_bound; + // Symbol: drake::planning::continuous_collision::CertificateRecord::pair_index + struct /* pair_index */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } pair_index; + // Symbol: drake::planning::continuous_collision::CertificateRecord::phi_hat + struct /* phi_hat */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } phi_hat; + // Symbol: drake::planning::continuous_collision::CertificateRecord::qc + struct /* qc */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } qc; + // Symbol: drake::planning::continuous_collision::CertificateRecord::s_end + struct /* s_end */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } s_end; + // Symbol: drake::planning::continuous_collision::CertificateRecord::s_start + struct /* s_start */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } s_start; + // Symbol: drake::planning::continuous_collision::CertificateRecord::segment + struct /* segment */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } segment; + // Symbol: drake::planning::continuous_collision::CertificateRecord::threshold + struct /* threshold */ { + // Source: drake/planning/continuous_collision/certificate.h + const char* doc = R"""()"""; + } threshold; + } CertificateRecord; + // Symbol: drake::planning::continuous_collision::CertificationResult + struct /* CertificationResult */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Result of one certification call (the architecture).)"""; + // Symbol: drake::planning::continuous_collision::CertificationResult::certificate + struct /* certificate */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""(Present iff Options::emit_certificate.)"""; + } certificate; + // Symbol: drake::planning::continuous_collision::CertificationResult::findings + struct /* findings */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""(Earliest-first.)"""; + } findings; + // Symbol: drake::planning::continuous_collision::CertificationResult::stats + struct /* stats */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } stats; + // Symbol: drake::planning::continuous_collision::CertificationResult::verdict + struct /* verdict */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } verdict; + } CertificationResult; + // Symbol: drake::planning::continuous_collision::ComputeBoundingSphere + struct /* ComputeBoundingSphere */ { + // Source: drake/planning/continuous_collision/bounding_sphere.h + const char* doc = +R"""(Computes a bounding sphere, in the body frame, of shape ``shape`` +posed at X_LG in the body frame (the geometry-support scope). + +The sphere is centered at the shape's natural center (tighter for the +broadphase prefilter than the white paper's origin-centered radius +R_g; 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. + +λ soundness dies quietly if any formula under-bounds, so this function +switches on the closed set of supported shape types and + +Raises: + RuntimeError on anything else (HalfSpace included — halfspaces are + handled by dedicated rules, never through a bounding sphere).)"""; + } ComputeBoundingSphere; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker + struct /* ContinuousCollisionChecker */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Certifies — not samples — that a trajectory is collision-free over its +entire continuous time domain (the problem statement). + +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 margin + padding(A, B) +— under the stated assumptions: exact real arithmetic up to the +configured numerical slack, a distance oracle accurate to its stated +tolerance, and the geometry semantics of the geometry-support scope +(Mesh ≡ convex hull). This is a statement about the continuum of +configurations, not about samples. The certificate is a property of +the path, so retiming the trajectory afterwards does not invalidate +it. + +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 deliberately 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.)"""; + } CheckEdge; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::CheckPath + struct /* CheckPath */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Certifies a piecewise-linear path through the given waypoint columns.)"""; + } 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 (any supported Drake trajectory type).)"""; + } CheckTrajectory; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::ComputeMotionBounds + struct /* ComputeMotionBounds */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } ComputeMotionBounds; + // 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, topology tables, and runs the +capability probe (throws on unsupported geometry pairs; the +geometry-support scope).)"""; + } ctor; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Normalize + struct /* Normalize */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Introspection / testing seams (all const, thread-safe).)"""; + } Normalize; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params + struct /* Params */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params::default_options + struct /* default_options */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } default_options; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params::model + struct /* model */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Plant + scene graph; the plant must be finalized.)"""; + } model; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params::padding + struct /* padding */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Per-body-pair padding; see PaddingSpec for the env/self rule.)"""; + } padding; + } Params; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::distance_oracle + struct /* distance_oracle */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } distance_oracle; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::kinematics_engine + struct /* kinematics_engine */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } kinematics_engine; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::model + struct /* model */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } model; + // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::pairs + struct /* pairs */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = R"""()"""; + } pairs; + } ContinuousCollisionChecker; + // Symbol: drake::planning::continuous_collision::DeCasteljauSplitAtHalf + struct /* DeCasteljauSplitAtHalf */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(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.)"""; + } DeCasteljauSplitAtHalf; + // Symbol: drake::planning::continuous_collision::DistanceOracle + struct /* DistanceOracle */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(Narrowphase distance abstraction (the distance-oracle contract). +Stateless per query and thread-compatible: configuration comes in via +the caller's QueryObject. + +Contract: SignedDistance returns φ̂ with |φ̂ − φ_true| ≤ tolerance() +whenever φ_true is at or above −tolerance(), and returns a definitely +negative value when the shapes interpenetrate beyond tolerance. Only +over-reporting a distance at or above threshold could fake a +certificate (the soundness argument), 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.)"""; + // Symbol: drake::planning::continuous_collision::DistanceOracle::DistanceOracle + struct /* ctor */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(Runs the capability probe: enumerates the unfiltered proximity pairs +from the model's SceneGraph inspector (collision filter state +snapshotted at construction), classifies every (shape, shape) +combination as {native, halfspace-fallback, unsupported}, and + +Raises: + RuntimeError immediately naming the offending geometries if any + pair is unsupported (deformables; halfspace–halfspace). Never + discovers an unsupported pair mid-certification.)"""; + } ctor; + // Symbol: drake::planning::continuous_collision::DistanceOracle::SignedDistance + struct /* SignedDistance */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(Signed distance for one pair at the configuration already set in the +context that produced ``query_object``. Optionally reports world-frame +closest points when the route provides them. + +``pair`` need not be an element of pairs(): the facade copies the +probe's records and rewrites their thresholds, so only ``pair.id`` and +``pair.route`` are read here. Both routes always fill the optional +out-params. + +Raises: + RuntimeError if ``pair`` carries a halfspace route but its + geometries were not classified by this oracle's capability probe + (i.e. the record did not come from pairs()).)"""; + } SignedDistance; + // Symbol: drake::planning::continuous_collision::DistanceOracle::pairs + struct /* pairs */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(The unfiltered pairs found by the probe (thresholds default 0; the +facade rewrites them from margin + padding).)"""; + } pairs; + // Symbol: drake::planning::continuous_collision::DistanceOracle::support_report + struct /* support_report */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(Human-readable probe report: one line per distinct shape-type +combination and its route (includes the "Mesh certified as convex +hull" notices; the risk register).)"""; + } support_report; + // Symbol: drake::planning::continuous_collision::DistanceOracle::tolerance + struct /* tolerance */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(τ used in the certificate arithmetic (the numerical policy).)"""; + } tolerance; + } DistanceOracle; + // Symbol: drake::planning::continuous_collision::DistanceRoute + struct /* DistanceRoute */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(How the oracle computes signed distance for one pair, resolved once by +the capability probe (the geometry-support scope; the distance-oracle +contract): no per-query dispatch decisions.)"""; + // Symbol: drake::planning::continuous_collision::DistanceRoute::kHalfSpaceA + struct /* kHalfSpaceA */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(Analytic halfspace support-function fallback; geometry ``a`` is the +halfspace.)"""; + } kHalfSpaceA; + // Symbol: drake::planning::continuous_collision::DistanceRoute::kHalfSpaceB + struct /* kHalfSpaceB */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = R"""(Same, geometry ``b`` is the halfspace.)"""; + } kHalfSpaceB; + // Symbol: drake::planning::continuous_collision::DistanceRoute::kNative + struct /* kNative */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(QueryObject::ComputeSignedDistancePairClosestPoints.)"""; + } kNative; + } DistanceRoute; + // Symbol: drake::planning::continuous_collision::Finding + struct /* Finding */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(One violation or inconclusive record (the architecture).)"""; + // Symbol: drake::planning::continuous_collision::Finding::definite + struct /* definite */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(true ⇒ definite violation; false ⇒ grazing / inconclusive.)"""; + } definite; + // Symbol: drake::planning::continuous_collision::Finding::distance + struct /* distance */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Oracle signed distance at q for this pair.)"""; + } distance; + // Symbol: drake::planning::continuous_collision::Finding::motion_bound + struct /* motion_bound */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Motion bound Δ_p at the terminal node (0 for breakpoint findings).)"""; + } motion_bound; + // Symbol: drake::planning::continuous_collision::Finding::nearest_a_W + struct /* nearest_a_W */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Closest points in world frame at q, when the narrowphase provides them +(violation findings; planners use these to push trajectories out of +collision).)"""; + } nearest_a_W; + // Symbol: drake::planning::continuous_collision::Finding::nearest_b_W + struct /* nearest_b_W */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } nearest_b_W; + // Symbol: drake::planning::continuous_collision::Finding::pair + struct /* pair */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } pair; + // Symbol: drake::planning::continuous_collision::Finding::q + struct /* q */ { + // Source: drake/planning/continuous_collision/options.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/options.h + const char* doc = +R"""(Trajectory time of the witness configuration.)"""; + } time; + } Finding; + // Symbol: drake::planning::continuous_collision::IsCertified + struct /* IsCertified */ { + // Source: drake/planning/continuous_collision/numerics.h + const char* doc = +R"""(True iff the pair is certified on the whole node.)"""; + } IsCertified; + // Symbol: drake::planning::continuous_collision::IsDefiniteViolation + struct /* IsDefiniteViolation */ { + // Source: drake/planning/continuous_collision/numerics.h + const char* doc = +R"""(True iff the representative configuration is a definite violation.)"""; + } IsDefiniteViolation; + // Symbol: drake::planning::continuous_collision::KinematicsEngine + struct /* KinematicsEngine */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Construction-time kinematic analysis of a plant (the displacement +lemma): 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.body_spheres(b) for the +prefilter; - once per Check* call: +engine.ComputeMotionBoundTable(path, pairs); - once per node, per +pair: table.MotionBound(pair_index, w).)"""; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::ComputeMotionBoundTable + struct /* ComputeMotionBoundTable */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc_2args = +R"""(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; the displacement lemma). 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. + +Raises: + RuntimeError naming the joint if the path moves a coordinate of an + unsupported joint type (quaternion floating, ball).)"""; + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc_4args = +R"""(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. + +Raises: + RuntimeError 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).)"""; + } ComputeMotionBoundTable; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::CoordinatesAffectingPair + struct /* CoordinatesAffectingPair */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(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.)"""; + } CoordinatesAffectingPair; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::KinematicsEngine + struct /* ctor */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(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 (constant-coordinate carve-out, the +joint-support scope). + +``model`` is aliased and must outlive this object. + +Raises: + RuntimeError if a HalfSpace geometry is on the *distal* side of a + rotational coordinate relative to an unfiltered partner (unbounded + reach). A HalfSpace that is merely the static partner of a + rotating body — the anchored ground plane under a robot arm, the + overwhelmingly common case — is accepted: λ then bounds the + partner's points, and signed distance is symmetric, so the + certificate still holds. + +Raises: + RuntimeError if the plant is not finalized, if a joint is + "reversed" (its declared parent body is outboard of its declared + child body in the multibody tree — a documented v1 exclusion), or + if any proximity geometry has a shape ComputeBoundingSphere() + rejects.)"""; + } ctor; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_has_halfspace + struct /* body_has_halfspace */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(True iff ``body`` carries at least one HalfSpace proximity geometry.)"""; + } body_has_halfspace; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_radius + struct /* body_radius */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Radius, about the body frame origin, of a sphere containing every +proximity geometry of ``body`` — the start of the reach chain. Zero +for a body with no (non-HalfSpace) proximity geometry.)"""; + } body_radius; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_sphere_geometries + struct /* body_sphere_geometries */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(The geometry ids matching body_spheres(body), element for element.)"""; + } body_sphere_geometries; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_spheres + struct /* body_spheres */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Bounding spheres (body frame) of every proximity geometry of ``body``, +used by the reach chain start and by the certifier's sphere prefilter. +HalfSpace geometries have no bounding sphere and are omitted.)"""; + } body_spheres; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::geometry_sphere + struct /* geometry_sphere */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(The bounding sphere (in its body's frame) of one proximity geometry. + +Raises: + RuntimeError if ``id`` is not a proximity geometry of this model + or is a HalfSpace (which has none).)"""; + } geometry_sphere; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::num_positions + struct /* num_positions */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = R"""()"""; + } num_positions; + // Symbol: drake::planning::continuous_collision::KinematicsEngine::plant + struct /* plant */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = R"""()"""; + } plant; + } KinematicsEngine; + // Symbol: drake::planning::continuous_collision::MotionBoundTable + struct /* MotionBoundTable */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Per-pair motion-bound coefficients in CSR layout (the displacement +lemma): 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. λ has units of meters of worst-case point +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 +(trajectory normalization; the joint-support scope) removed from J(p). +"Constant" there is a *tolerance* — a coordinate whose global +control-box range is at most Options::continuity_tolerance — not an +identity, so a carved coordinate may still displace the pair's distal +side by up to λ̃_j · range_j. That residual is charged unconditionally +inside MotionBound(), which is what makes Δ_p a true upper bound on +the pair's relative motion over the whole trajectory rather than one +that ignores the carved coordinates. It is exactly zero — bit for bit +— whenever every carved coordinate is *exactly* constant, which is the +case for every path whose control points repeat a coordinate's value +verbatim.)"""; + // Symbol: drake::planning::continuous_collision::MotionBoundTable::GetEntries + struct /* GetEntries */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Introspection for tests: the (coordinate, λ) entries of one pair, +ordered by increasing coordinate index.)"""; + } GetEntries; + // Symbol: drake::planning::continuous_collision::MotionBoundTable::MotionBound + struct /* MotionBound */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j — a sparse dot +product against the node's per-coordinate deviations w, plus the +carved coordinates' residual (the interval certificate, requirement +P3).)"""; + } MotionBound; + // Symbol: drake::planning::continuous_collision::MotionBoundTable::MotionBoundTable + struct /* ctor */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc_0args = R"""(Constructs an empty table (zero pairs).)"""; + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc_4args = +R"""(Constructs the CSR table directly from its four arrays. + +Parameter ``row_start``: + Size num_pairs + 1, starting at 0 and non-decreasing; + row_start.back() is the total entry count. + +Parameter ``coord``: + Position-coordinate index of every entry. + +Parameter ``lambda``: + λ of every entry, element for element with ``coord``. + +Parameter ``carveout_slack``: + One residual per pair. + +Raises: + RuntimeError if the arrays do not satisfy those invariants.)"""; + } ctor; + // Symbol: drake::planning::continuous_collision::MotionBoundTable::carveout_slack + struct /* carveout_slack */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Σ 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. Zero when every carved +coordinate is exactly constant.)"""; + } carveout_slack; + // Symbol: drake::planning::continuous_collision::MotionBoundTable::num_entries + struct /* num_entries */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(Total number of (coordinate, λ) entries over all pairs.)"""; + } num_entries; + // Symbol: drake::planning::continuous_collision::MotionBoundTable::num_pairs + struct /* num_pairs */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = R"""()"""; + } num_pairs; + // Symbol: drake::planning::continuous_collision::MotionBoundTable::pair_is_static + struct /* pair_is_static */ { + // Source: drake/planning/continuous_collision/motion_bound_table.h + const char* doc = +R"""(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.)"""; + } pair_is_static; + } MotionBoundTable; + // Symbol: drake::planning::continuous_collision::Options + struct /* Options */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Options controlling one certification call (the architecture; the +numerical policy).)"""; + // Symbol: drake::planning::continuous_collision::Options::certificate_slack + struct /* certificate_slack */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(ε_slack: swallows floating-point noise in the bound arithmetic.)"""; + } certificate_slack; + // Symbol: drake::planning::continuous_collision::Options::continuity_tolerance + struct /* continuity_tolerance */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Junction C0-continuity tolerance (per coordinate; modulo 2π for +coordinates listed in continuous_revolute_indices).)"""; + } continuity_tolerance; + // Symbol: drake::planning::continuous_collision::Options::continuous_revolute_indices + struct /* continuous_revolute_indices */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Position coordinates whose junction continuity is checked modulo 2π +(GcsTrajectoryOptimization continuous-revolute convention). + +See also: + planning::trajectory_optimization::GetContinuousRevoluteJointIndices)"""; + } continuous_revolute_indices; + // Symbol: drake::planning::continuous_collision::Options::emit_certificate + struct /* emit_certificate */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(If true, every certification event is recorded into a Certificate that +VerifyCertificate() can independently replay (the search algorithm).)"""; + } emit_certificate; + // Symbol: drake::planning::continuous_collision::Options::margin + struct /* margin */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Global clearance margin δ in meters. The certificate proves signed +distance > margin + padding for every pair at every time.)"""; + } margin; + // Symbol: drake::planning::continuous_collision::Options::max_conversion_degree + struct /* max_conversion_degree */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Maximum polynomial degree accepted for monomial→Bernstein conversion.)"""; + } max_conversion_degree; + // Symbol: drake::planning::continuous_collision::Options::max_nodes + struct /* max_nodes */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Optional node budget; exceeded ⇒ Verdict::kBudgetExhausted.)"""; + } max_nodes; + // Symbol: drake::planning::continuous_collision::Options::max_reported_findings + struct /* max_reported_findings */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } max_reported_findings; + // Symbol: drake::planning::continuous_collision::Options::min_interval + struct /* min_interval */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Resolution floor as a fraction of a segment's parameter width; nodes +narrower than this become kInconclusive findings instead of splitting.)"""; + } min_interval; + // Symbol: drake::planning::continuous_collision::Options::mode + struct /* mode */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } mode; + // Symbol: drake::planning::continuous_collision::Options::parallelism + struct /* parallelism */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } parallelism; + // Symbol: drake::planning::continuous_collision::Options::query_tolerance + struct /* query_tolerance */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(τ: the distance oracle's accuracy contract in meters (the +distance-oracle contract; the numerical policy).)"""; + } query_tolerance; + } Options; + // Symbol: drake::planning::continuous_collision::PaddingSpec + struct /* PaddingSpec */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Per-body-pair padding: the effective threshold for pair p is margin + +padding(p). + +Which of the two scalars applies to a pair is decided by *anchoring*, +from plant topology alone. A body is anchored iff no position +coordinate of the plant changes its pose relative to the world — the +world body itself, and everything welded to it directly or +transitively. A pair is a self-collision pair iff both of its bodies +are non-anchored, and an environment pair otherwise. The rule never +depends on which trajectory is being checked.)"""; + // Symbol: drake::planning::continuous_collision::PaddingSpec::env_padding + struct /* env_padding */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Padding for robot-vs-environment pairs, i.e. pairs with at least one +anchored body.)"""; + } env_padding; + // Symbol: drake::planning::continuous_collision::PaddingSpec::per_body_pair + struct /* per_body_pair */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Optional dense symmetric matrix indexed by BodyIndex, sized num_bodies +× num_bodies. Entry (a, b) overrides the scalars for that body pair; a +NaN entry means "not covered", and that pair falls back to env_padding +/ self_padding.)"""; + } per_body_pair; + // Symbol: drake::planning::continuous_collision::PaddingSpec::self_padding + struct /* self_padding */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Padding for robot-vs-robot (self-collision) pairs, i.e. pairs whose +two bodies are both non-anchored.)"""; + } self_padding; + } PaddingSpec; + // Symbol: drake::planning::continuous_collision::PairId + struct /* PairId */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Identifies an unfiltered proximity geometry pair.)"""; + // Symbol: drake::planning::continuous_collision::PairId::a + struct /* a */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } a; + // Symbol: drake::planning::continuous_collision::PairId::b + struct /* b */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } b; + // Symbol: drake::planning::continuous_collision::PairId::body_a + struct /* body_a */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } body_a; + // Symbol: drake::planning::continuous_collision::PairId::body_b + struct /* body_b */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } body_b; + } PairId; + // Symbol: drake::planning::continuous_collision::PairRecord + struct /* PairRecord */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(One unfiltered proximity pair with its pre-resolved distance route and +effective threshold m_p = margin + padding(p).)"""; + // Symbol: drake::planning::continuous_collision::PairRecord::id + struct /* id */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = R"""()"""; + } id; + // Symbol: drake::planning::continuous_collision::PairRecord::route + struct /* route */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = R"""()"""; + } route; + // Symbol: drake::planning::continuous_collision::PairRecord::threshold + struct /* threshold */ { + // Source: drake/planning/continuous_collision/distance_oracle.h + const char* doc = +R"""(Filled by the facade from margin + PaddingSpec.)"""; + } threshold; + } PairRecord; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath + struct /* PiecewiseBezierPath */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(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 (trajectory normalization). + +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).)"""; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::EvaluateSegment + struct /* EvaluateSegment */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(Evaluates segment ``segment_index`` at local parameter s ∈ [0, 1].)"""; + } EvaluateSegment; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::FromTrajectory + struct /* FromTrajectory */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(Normalizes any supported Drake trajectory (BezierCurve, +CompositeTrajectory, BsplineTrajectory via knot insertion, +PiecewisePolynomial via monomial→Bernstein change of basis). + +Raises: + RuntimeError on unsupported segment types, degree above + options.max_conversion_degree, or junction discontinuity beyond + options.continuity_tolerance (modulo 2π for coordinates in + options.continuous_revolute_indices).)"""; + } FromTrajectory; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::FromWaypoints + struct /* FromWaypoints */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(Normalizes an n × K waypoint matrix into K−1 order-1 segments (exact). +Segment k spans time [k, k+1]. + +Raises: + RuntimeError if K < 2.)"""; + } FromWaypoints; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::PiecewiseBezierPath + struct /* ctor */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = R"""()"""; + } ctor; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::Value + struct /* Value */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(Evaluates the path at time t (for tests and breakpoint checks; the hot +loop never calls this — it uses de Casteljau apexes).)"""; + } Value; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::constant_coordinates + struct /* constant_coordinates */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(True for coordinates whose value is identical (within the continuity +tolerance) across all control points of all segments; such coordinates +are treated as welded for the check (trajectory normalization; the +joint-support scope).)"""; + } constant_coordinates; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::end_time + struct /* end_time */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = R"""()"""; + } end_time; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::global_lower_bound + struct /* global_lower_bound */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = +R"""(Per-coordinate global control-point box over all segments (trajectory +normalization); used for trajectory-adaptive prismatic reach bounds.)"""; + } global_lower_bound; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::global_upper_bound + struct /* global_upper_bound */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = R"""()"""; + } global_upper_bound; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::num_positions + struct /* num_positions */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = R"""()"""; + } num_positions; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::segments + struct /* segments */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = R"""()"""; + } segments; + // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::start_time + struct /* start_time */ { + // Source: drake/planning/continuous_collision/piecewise_bezier_path.h + const char* doc = R"""()"""; + } start_time; + } PiecewiseBezierPath; + // Symbol: drake::planning::continuous_collision::SearchMode + struct /* SearchMode */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Search modes for certification (the search algorithm).)"""; + // Symbol: drake::planning::continuous_collision::SearchMode::kCertifyAll + struct /* kCertifyAll */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Certify the full domain and return every violation / inconclusive +region found (bounded by Options::max_reported_findings).)"""; + } kCertifyAll; + // Symbol: drake::planning::continuous_collision::SearchMode::kFindFirstViolation + struct /* kFindFirstViolation */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Return on the first definite violation; serial execution returns the +earliest one in time.)"""; + } kFindFirstViolation; + } SearchMode; + // Symbol: drake::planning::continuous_collision::Statistics + struct /* Statistics */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Cost accounting for one certification call.)"""; + // Symbol: drake::planning::continuous_collision::Statistics::max_depth + struct /* max_depth */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } max_depth; + // Symbol: drake::planning::continuous_collision::Statistics::narrowphase_queries + struct /* narrowphase_queries */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } narrowphase_queries; + // Symbol: drake::planning::continuous_collision::Statistics::nodes + struct /* nodes */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } nodes; + // Symbol: drake::planning::continuous_collision::Statistics::sphere_certifications + struct /* sphere_certifications */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } sphere_certifications; + // Symbol: drake::planning::continuous_collision::Statistics::wall_time_s + struct /* wall_time_s */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = R"""()"""; + } wall_time_s; + } Statistics; + // Symbol: drake::planning::continuous_collision::Verdict + struct /* Verdict */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Outcome of a certification run (the problem statement).)"""; + // Symbol: drake::planning::continuous_collision::Verdict::kBudgetExhausted + struct /* kBudgetExhausted */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(The optional node budget was exhausted first.)"""; + } kBudgetExhausted; + // Symbol: drake::planning::continuous_collision::Verdict::kCertifiedFree + struct /* kCertifiedFree */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Proof: every unfiltered pair keeps signed distance > margin + padding +over the entire continuous time domain.)"""; + } kCertifiedFree; + // Symbol: drake::planning::continuous_collision::Verdict::kInconclusive + struct /* kInconclusive */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(Subdivision hit the resolution floor with some pair's clearance within +oracle tolerance of the threshold (grazing trajectory).)"""; + } kInconclusive; + // Symbol: drake::planning::continuous_collision::Verdict::kViolationFound + struct /* kViolationFound */ { + // Source: drake/planning/continuous_collision/options.h + const char* doc = +R"""(An exactly-on-trajectory configuration violates the threshold.)"""; + } kViolationFound; + } Verdict; + // Symbol: drake::planning::continuous_collision::VerifyCertificate + struct /* VerifyCertificate */ { + // Source: drake/planning/continuous_collision/continuous_collision_checker.h + const char* doc = +R"""(Independently replays every record of ``certificate`` (recomputing +node control boxes from freshly restricted control points and +re-querying distances) and checks interval coverage of the full domain +for every pair. Returns true iff the certificate holds (the search +algorithm).)"""; + } VerifyCertificate; + } continuous_collision; + } planning; + } drake; +} pydrake_doc_planning_continuous_collision; + +#if defined(__GNUG__) +#pragma GCC diagnostic pop +#endif 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..caa6c64ddbf1 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..a40939cbd720 --- /dev/null +++ b/bindings/pydrake/planning/planning_py_continuous_collision.cc @@ -0,0 +1,495 @@ +#include +#include +#include +#include +#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/bounding_sphere.h" +#include "drake/planning/continuous_collision/certificate.h" +#include "drake/planning/continuous_collision/continuous_collision_checker.h" +#include "drake/planning/continuous_collision/distance_oracle.h" +#include "drake/planning/continuous_collision/motion_bound_table.h" +#include "drake/planning/continuous_collision/numerics.h" +#include "drake/planning/continuous_collision/options.h" +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" +#include "drake/planning/continuous_collision/vpolytope_ingestion.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. +)"""; + + // options.h + { + using Class = SearchMode; + constexpr auto& cls_doc = doc.SearchMode; + py::enum_(m, "SearchMode", cls_doc.doc) + .value("kFindFirstViolation", Class::kFindFirstViolation, + cls_doc.kFindFirstViolation.doc) + .value("kCertifyAll", Class::kCertifyAll, cls_doc.kCertifyAll.doc); + } + + { + 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) + .value("kBudgetExhausted", Class::kBudgetExhausted, + cls_doc.kBudgetExhausted.doc); + } + + { + using Class = Options; + constexpr auto& cls_doc = doc.Options; + py::class_ cls(m, "Options", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("margin", &Class::margin, cls_doc.margin.doc) + .def_readwrite("continuity_tolerance", &Class::continuity_tolerance, + cls_doc.continuity_tolerance.doc) + .def_readwrite("query_tolerance", &Class::query_tolerance, + cls_doc.query_tolerance.doc) + .def_readwrite("certificate_slack", &Class::certificate_slack, + cls_doc.certificate_slack.doc) + .def_readwrite( + "min_interval", &Class::min_interval, cls_doc.min_interval.doc) + .def_readwrite("continuous_revolute_indices", + &Class::continuous_revolute_indices, + cls_doc.continuous_revolute_indices.doc) + .def_readwrite("max_conversion_degree", &Class::max_conversion_degree, + cls_doc.max_conversion_degree.doc) + .def_readwrite("mode", &Class::mode, cls_doc.mode.doc) + .def_readwrite("max_reported_findings", &Class::max_reported_findings, + cls_doc.max_reported_findings.doc) + .def_readwrite("max_nodes", &Class::max_nodes, cls_doc.max_nodes.doc) + .def_readwrite("emit_certificate", &Class::emit_certificate, + cls_doc.emit_certificate.doc) + .def_readwrite( + "parallelism", &Class::parallelism, cls_doc.parallelism.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = PaddingSpec; + constexpr auto& cls_doc = doc.PaddingSpec; + py::class_ cls(m, "PaddingSpec", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite( + "env_padding", &Class::env_padding, cls_doc.env_padding.doc) + .def_readwrite( + "self_padding", &Class::self_padding, cls_doc.self_padding.doc) + .def_readwrite( + "per_body_pair", &Class::per_body_pair, cls_doc.per_body_pair.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = PairId; + constexpr auto& cls_doc = doc.PairId; + py::class_ cls(m, "PairId", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("a", &Class::a, cls_doc.a.doc) + .def_readwrite("b", &Class::b, cls_doc.b.doc) + .def_readwrite("body_a", &Class::body_a, cls_doc.body_a.doc) + .def_readwrite("body_b", &Class::body_b, cls_doc.body_b.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = Finding; + constexpr auto& cls_doc = doc.Finding; + py::class_ cls(m, "Finding", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("time", &Class::time, cls_doc.time.doc) + .def_readwrite("q", &Class::q, cls_doc.q.doc) + .def_readwrite("pair", &Class::pair, cls_doc.pair.doc) + .def_readwrite("distance", &Class::distance, cls_doc.distance.doc) + .def_readwrite( + "motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) + .def_readwrite("definite", &Class::definite, cls_doc.definite.doc) + .def_readwrite( + "nearest_a_W", &Class::nearest_a_W, cls_doc.nearest_a_W.doc) + .def_readwrite( + "nearest_b_W", &Class::nearest_b_W, cls_doc.nearest_b_W.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = Statistics; + constexpr auto& cls_doc = doc.Statistics; + py::class_ cls(m, "Statistics", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("nodes", &Class::nodes, cls_doc.nodes.doc) + .def_readwrite("narrowphase_queries", &Class::narrowphase_queries, + cls_doc.narrowphase_queries.doc) + .def_readwrite("sphere_certifications", &Class::sphere_certifications, + cls_doc.sphere_certifications.doc) + .def_readwrite("max_depth", &Class::max_depth, cls_doc.max_depth.doc) + .def_readwrite( + "wall_time_s", &Class::wall_time_s, cls_doc.wall_time_s.doc); + DefCopyAndDeepCopy(&cls); + } + + // bounding_sphere.h + { + using Class = BoundingSphere; + constexpr auto& cls_doc = doc.BoundingSphere; + py::class_ cls(m, "BoundingSphere", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("center_L", &Class::center_L, cls_doc.center_L.doc) + .def_readwrite("radius", &Class::radius, cls_doc.radius.doc); + DefCopyAndDeepCopy(&cls); + } + + m.def("ComputeBoundingSphere", &ComputeBoundingSphere, py::arg("shape"), + py::arg("X_LG"), doc.ComputeBoundingSphere.doc); + + // piecewise_bezier_path.h + { + using Class = BezierSegment; + constexpr auto& cls_doc = doc.BezierSegment; + py::class_ cls(m, "BezierSegment", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("t_start", &Class::t_start, cls_doc.t_start.doc) + .def_readwrite("t_end", &Class::t_end, cls_doc.t_end.doc) + .def_readwrite("control_points", &Class::control_points, + cls_doc.control_points.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = PiecewiseBezierPath; + constexpr auto& cls_doc = doc.PiecewiseBezierPath; + py::class_ cls(m, "PiecewiseBezierPath", cls_doc.doc); + cls // BR + .def_static("FromTrajectory", &Class::FromTrajectory, + py::arg("trajectory"), py::arg("options"), + cls_doc.FromTrajectory.doc) + .def_static("FromWaypoints", &Class::FromWaypoints, + py::arg("waypoints"), py::arg("options"), cls_doc.FromWaypoints.doc) + .def("num_positions", &Class::num_positions, cls_doc.num_positions.doc) + .def("segments", &Class::segments, cls_doc.segments.doc) + .def("start_time", &Class::start_time, cls_doc.start_time.doc) + .def("end_time", &Class::end_time, cls_doc.end_time.doc) + .def("global_lower_bound", &Class::global_lower_bound, + cls_doc.global_lower_bound.doc) + .def("global_upper_bound", &Class::global_upper_bound, + cls_doc.global_upper_bound.doc) + .def("constant_coordinates", &Class::constant_coordinates, + cls_doc.constant_coordinates.doc) + .def("Value", &Class::Value, py::arg("t"), cls_doc.Value.doc) + .def("EvaluateSegment", &Class::EvaluateSegment, + py::arg("segment_index"), py::arg("s"), + cls_doc.EvaluateSegment.doc); + DefCopyAndDeepCopy(&cls); + } + + m.def( + "DeCasteljauSplitAtHalf", + [](const Eigen::MatrixXd& cps) { + Eigen::MatrixXd left; + Eigen::MatrixXd right; + Eigen::VectorXd mid; + DeCasteljauSplitAtHalf(cps, &left, &right, &mid); + return std::make_tuple( + std::move(left), std::move(right), std::move(mid)); + }, + py::arg("cps"), + (std::string(doc.DeCasteljauSplitAtHalf.doc) + + "\n\n" + "Note:\n" + " Unlike the C++ signature, which writes through output " + "pointers, this returns a tuple ``(left, right, mid)``.") + .c_str()); + + // motion_bound_table.h + { + using Class = MotionBoundTable; + constexpr auto& cls_doc = doc.MotionBoundTable; + py::class_ cls(m, "MotionBoundTable", cls_doc.doc); + cls // BR + .def(py::init<>(), cls_doc.ctor.doc_0args) + .def(py::init, std::vector, std::vector, + std::vector>(), + py::arg("row_start"), py::arg("coord"), py::arg("lambda"), + py::arg("carveout_slack"), cls_doc.ctor.doc_4args) + .def("num_pairs", &Class::num_pairs, cls_doc.num_pairs.doc) + .def("pair_is_static", &Class::pair_is_static, py::arg("pair_index"), + cls_doc.pair_is_static.doc) + .def("MotionBound", &Class::MotionBound, py::arg("pair_index"), + py::arg("w"), cls_doc.MotionBound.doc) + .def("carveout_slack", &Class::carveout_slack, py::arg("pair_index"), + cls_doc.carveout_slack.doc) + .def("GetEntries", &Class::GetEntries, py::arg("pair_index"), + cls_doc.GetEntries.doc) + .def("num_entries", &Class::num_entries, cls_doc.num_entries.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = KinematicsEngine; + constexpr auto& cls_doc = doc.KinematicsEngine; + py::class_ cls(m, "KinematicsEngine", cls_doc.doc); + cls // BR + .def(py::init&>(), py::arg("model"), + // Keep the model alive as long as the engine: the C++ object + // aliases it (see the constructor's documentation). + py::keep_alive<1, 2>(), cls_doc.ctor.doc) + .def("CoordinatesAffectingPair", &Class::CoordinatesAffectingPair, + py::arg("body_a"), py::arg("body_b"), + cls_doc.CoordinatesAffectingPair.doc) + .def("ComputeMotionBoundTable", + overload_cast_explicit&>(&Class::ComputeMotionBoundTable), + py::arg("path"), py::arg("pairs"), + cls_doc.ComputeMotionBoundTable.doc_2args) + .def("ComputeMotionBoundTable", + overload_cast_explicit&, + const std::vector&>(&Class::ComputeMotionBoundTable), + py::arg("lower"), py::arg("upper"), py::arg("constant_coordinates"), + py::arg("pairs"), cls_doc.ComputeMotionBoundTable.doc_4args) + .def("body_spheres", &Class::body_spheres, py::arg("body"), + cls_doc.body_spheres.doc) + .def("body_sphere_geometries", &Class::body_sphere_geometries, + py::arg("body"), cls_doc.body_sphere_geometries.doc) + .def("geometry_sphere", &Class::geometry_sphere, py::arg("id"), + cls_doc.geometry_sphere.doc) + .def("body_has_halfspace", &Class::body_has_halfspace, py::arg("body"), + cls_doc.body_has_halfspace.doc) + .def("body_radius", &Class::body_radius, py::arg("body"), + cls_doc.body_radius.doc) + .def("num_positions", &Class::num_positions, cls_doc.num_positions.doc) + .def("plant", &Class::plant, py_rvp::reference_internal, + cls_doc.plant.doc); + } + + // distance_oracle.h + { + using Class = DistanceRoute; + constexpr auto& cls_doc = doc.DistanceRoute; + py::enum_(m, "DistanceRoute", cls_doc.doc) + .value("kNative", Class::kNative, cls_doc.kNative.doc) + .value("kHalfSpaceA", Class::kHalfSpaceA, cls_doc.kHalfSpaceA.doc) + .value("kHalfSpaceB", Class::kHalfSpaceB, cls_doc.kHalfSpaceB.doc); + } + + { + using Class = PairRecord; + constexpr auto& cls_doc = doc.PairRecord; + py::class_ cls(m, "PairRecord", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("id", &Class::id, cls_doc.id.doc) + .def_readwrite("route", &Class::route, cls_doc.route.doc) + .def_readwrite("threshold", &Class::threshold, cls_doc.threshold.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = DistanceOracle; + constexpr auto& cls_doc = doc.DistanceOracle; + py::class_ cls(m, "DistanceOracle", cls_doc.doc); + cls // BR + .def(py::init&, double>(), py::arg("model"), + py::arg("query_tolerance"), cls_doc.ctor.doc) + .def("pairs", &Class::pairs, cls_doc.pairs.doc) + .def( + "SignedDistance", + [](const Class& self, + const geometry::QueryObject& query_object, + const PairRecord& pair) { + Eigen::Vector3d nearest_a_W = Eigen::Vector3d::Zero(); + Eigen::Vector3d nearest_b_W = Eigen::Vector3d::Zero(); + const double distance = self.SignedDistance( + query_object, pair, &nearest_a_W, &nearest_b_W); + return std::make_tuple(distance, nearest_a_W, nearest_b_W); + }, + py::arg("query_object"), py::arg("pair"), + (std::string(cls_doc.SignedDistance.doc) + + "\n\n" + "Note:\n" + " Unlike the C++ signature, which reports the closest " + "points through optional output pointers, this returns a " + "tuple ``(distance, nearest_a_W, nearest_b_W)``.") + .c_str()) + .def("tolerance", &Class::tolerance, cls_doc.tolerance.doc) + .def("support_report", &Class::support_report, + cls_doc.support_report.doc); + DefCopyAndDeepCopy(&cls); + } + + // certificate.h + { + using Class = CertificateRecord; + constexpr auto& cls_doc = doc.CertificateRecord; + py::class_ cls(m, "CertificateRecord", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("segment", &Class::segment, cls_doc.segment.doc) + .def_readwrite("s_start", &Class::s_start, cls_doc.s_start.doc) + .def_readwrite("s_end", &Class::s_end, cls_doc.s_end.doc) + .def_readwrite("pair_index", &Class::pair_index, cls_doc.pair_index.doc) + .def_readwrite("qc", &Class::qc, cls_doc.qc.doc) + .def_readwrite("phi_hat", &Class::phi_hat, cls_doc.phi_hat.doc) + .def_readwrite( + "motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) + .def_readwrite("threshold", &Class::threshold, cls_doc.threshold.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = Certificate; + constexpr auto& cls_doc = doc.Certificate; + py::class_ cls(m, "Certificate", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("records", &Class::records, cls_doc.records.doc) + .def_readwrite("pairs", &Class::pairs, cls_doc.pairs.doc); + DefCopyAndDeepCopy(&cls); + } + + // continuous_collision_checker.h + { + using Class = CertificationResult; + constexpr auto& cls_doc = doc.CertificationResult; + py::class_ cls(m, "CertificationResult", cls_doc.doc); + cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_readwrite("verdict", &Class::verdict, cls_doc.verdict.doc) + .def_readwrite("findings", &Class::findings, cls_doc.findings.doc) + .def_readwrite("stats", &Class::stats, cls_doc.stats.doc) + .def_readwrite( + "certificate", &Class::certificate, cls_doc.certificate.doc); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = ContinuousCollisionChecker; + constexpr auto& cls_doc = doc.ContinuousCollisionChecker; + py::class_ cls(m, "ContinuousCollisionChecker", cls_doc.doc); + + { + using Nested = Class::Params; + constexpr auto& nested_doc = cls_doc.Params; + py::class_ nested_cls(cls, "Params", nested_doc.doc); + nested_cls // BR + .def(py::init<>()) + .def(ParamInit()) + .def_property( + "model", + [](const Nested& self) -> const RobotDiagram* { + return self.model.get(); + }, + [](Nested& self, py::object model) { + // Add a python reference to model (owned by the shared + // pointer), and transfer that to the c++ params struct. + self.model = + make_shared_ptr_from_py_object>(model); + }, + nested_doc.model.doc) + .def_readwrite("padding", &Nested::padding, nested_doc.padding.doc) + .def_readwrite("default_options", &Nested::default_options, + nested_doc.default_options.doc); + } + + py::object params_ctor = cls.attr("Params"); + cls // BR + .def( + py::init([params_ctor](py::object model, const py::kwargs& kwargs) { + // For lifetime management, we need to treat pointer-like + // arguments separately. Start by creating a Params object in + // Python with all of the other non-pointer kwargs. + py::object params_py = params_ctor(**kwargs); + auto* params = params_py.cast(); + DRAKE_DEMAND(params != nullptr); + // Now, add a python reference to model (owned by the shared + // pointer), and transfer that to the c++ checker. + params->model = + make_shared_ptr_from_py_object>(model); + return std::make_unique(std::move(*params)); + }), + py::kw_only(), py::arg("model"), + (std::string(cls_doc.ctor.doc) + + "\n\n" + "See :class:`pydrake.planning.continuous_collision" + ".ContinuousCollisionChecker.Params` for the list of " + "properties available here as kwargs.") + .c_str()) + .def(py::init(), py::arg("params"), cls_doc.ctor.doc) + .def("CheckTrajectory", &Class::CheckTrajectory, py::arg("trajectory"), + py::arg("options") = std::nullopt, cls_doc.CheckTrajectory.doc) + .def("CheckPath", &Class::CheckPath, py::arg("waypoints"), + py::arg("options") = std::nullopt, cls_doc.CheckPath.doc) + .def("CheckEdge", &Class::CheckEdge, py::arg("q1"), py::arg("q2"), + py::arg("options") = std::nullopt, cls_doc.CheckEdge.doc) + .def("Normalize", &Class::Normalize, py::arg("trajectory"), + py::arg("options") = std::nullopt, cls_doc.Normalize.doc) + .def("ComputeMotionBounds", &Class::ComputeMotionBounds, + py::arg("path"), cls_doc.ComputeMotionBounds.doc) + .def("distance_oracle", &Class::distance_oracle, + py_rvp::reference_internal, cls_doc.distance_oracle.doc) + .def("kinematics_engine", &Class::kinematics_engine, + py_rvp::reference_internal, cls_doc.kinematics_engine.doc) + .def("pairs", &Class::pairs, cls_doc.pairs.doc) + .def("model", &Class::model, py_rvp::reference_internal, + cls_doc.model.doc); + } + + m.def("VerifyCertificate", &VerifyCertificate, py::arg("checker"), + py::arg("path"), py::arg("certificate"), doc.VerifyCertificate.doc); + + // vpolytope_ingestion.h + m.def("AddVPolytopeObstacle", &AddVPolytopeObstacle, py::arg("plant"), + py::arg("vpoly"), py::arg("X_WG"), py::arg("name"), + doc.AddVPolytopeObstacle.doc); + + // numerics.h + m.def("IsCertified", &IsCertified, py::arg("phi_hat"), py::arg("tau"), + py::arg("motion_bound"), py::arg("threshold"), py::arg("slack"), + doc.IsCertified.doc); + + m.def("IsDefiniteViolation", &IsDefiniteViolation, py::arg("phi_hat"), + py::arg("tau"), py::arg("threshold"), doc.IsDefiniteViolation.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..7320ef029120 --- /dev/null +++ b/bindings/pydrake/planning/test/continuous_collision_test.py @@ -0,0 +1,297 @@ +import pydrake.planning.continuous_collision as mut + +import unittest + +import numpy as np + +from pydrake.common import Parallelism +from pydrake.geometry import Box, Sphere +from pydrake.geometry.optimization import VPolytope +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 BezierCurve + + +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 _make_arm_builder(): + """A planar 2-dof arm (revolute, then prismatic) with one anchored post + obstacle -- the same world planning/continuous_collision/test/api_test.cc + uses, so the verdicts asserted below match the C++ suite. Returns the + not-yet-built RobotDiagramBuilder (its plant is not finalized). + """ + 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=CoulombFriction(1.0, 1.0)) + plant.RegisterCollisionGeometry( + body=tool, X_BG=RigidTransform(), shape=Sphere(0.04), + name="tool_geom", coulomb_friction=CoulombFriction(1.0, 1.0)) + 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=CoulombFriction(1.0, 1.0)) + return builder + + +def _serial_options(): + options = mut.Options() + options.parallelism = Parallelism(num_threads=1) + return options + + +class TestContinuousCollision(unittest.TestCase): + def setUp(self): + self.model = _make_arm_builder().Build() + self.checker = mut.ContinuousCollisionChecker( + model=self.model, default_options=_serial_options()) + + def test_options(self): + """Exercises the Options / PaddingSpec / enum surface.""" + dut = mut.Options() + self.assertEqual(dut.margin, 0.0) + self.assertEqual(dut.mode, mut.SearchMode.kCertifyAll) + self.assertIsNone(dut.max_nodes) + dut.margin = 0.01 + dut.continuity_tolerance = 1e-6 + dut.query_tolerance = 1e-5 + dut.certificate_slack = 1e-8 + dut.min_interval = 1e-8 + dut.continuous_revolute_indices = [0] + dut.max_conversion_degree = 8 + dut.mode = mut.SearchMode.kFindFirstViolation + dut.max_reported_findings = 4 + dut.max_nodes = 10000 + dut.emit_certificate = True + dut.parallelism = Parallelism(num_threads=1) + self.assertEqual(dut.margin, 0.01) + self.assertEqual(dut.max_nodes, 10000) + self.assertTrue(dut.emit_certificate) + self.assertEqual(dut.parallelism.num_threads(), 1) + self.assertEqual(dut.continuous_revolute_indices, [0]) + + # kwargs-init round trip. + kwargs_dut = mut.Options(margin=0.02, max_reported_findings=7) + self.assertEqual(kwargs_dut.margin, 0.02) + self.assertEqual(kwargs_dut.max_reported_findings, 7) + + padding = mut.PaddingSpec(env_padding=0.001, self_padding=0.002) + self.assertEqual(padding.env_padding, 0.001) + self.assertEqual(padding.self_padding, 0.002) + self.assertIsNone(padding.per_body_pair) + + # The enums are complete. + self.assertEqual(len(mut.Verdict.__members__), 4) + self.assertEqual(len(mut.SearchMode.__members__), 2) + self.assertEqual(len(mut.DistanceRoute.__members__), 3) + + def test_params_and_introspection(self): + params = mut.ContinuousCollisionChecker.Params() + params.model = self.model + params.padding = mut.PaddingSpec(env_padding=0.0) + params.default_options = _serial_options() + self.assertIs(params.model, self.model) + checker = mut.ContinuousCollisionChecker(params=params) + + self.assertIs(checker.model(), self.model) + self.assertGreater(len(checker.pairs()), 0) + self.assertIsInstance(checker.pairs()[0], mut.PairRecord) + self.assertIsInstance(checker.pairs()[0].id, mut.PairId) + self.assertIsInstance(checker.pairs()[0].route, mut.DistanceRoute) + + oracle = checker.distance_oracle() + self.assertIsInstance(oracle, mut.DistanceOracle) + self.assertGreater(oracle.tolerance(), 0.0) + self.assertIsInstance(oracle.support_report(), str) + self.assertGreater(len(oracle.support_report()), 0) + + engine = checker.kinematics_engine() + self.assertIsInstance(engine, mut.KinematicsEngine) + self.assertEqual(engine.num_positions(), 2) + pair = checker.pairs()[0].id + coords = engine.CoordinatesAffectingPair(body_a=pair.body_a, + body_b=pair.body_b) + self.assertIsInstance(coords, list) + + def test_check_edge_free_and_colliding(self): + """A free edge certifies; a sweep past the post reports a violation.""" + free = self.checker.CheckEdge(q1=[0.0, 0.0], q2=[0.3, 0.05]) + self.assertEqual(free.verdict, mut.Verdict.kCertifiedFree) + self.assertEqual(len(free.findings), 0) + self.assertGreater(free.stats.nodes, 0) + self.assertGreaterEqual(free.stats.max_depth, 0) + self.assertIsNone(free.certificate) + + # Sweeping theta from 0 to 2.4 rad with the tool extended drives the + # tool sphere through the anchored post. + hit = self.checker.CheckEdge(q1=[0.0, 0.25], q2=[2.4, 0.25]) + self.assertEqual(hit.verdict, mut.Verdict.kViolationFound) + self.assertGreater(len(hit.findings), 0) + finding = hit.findings[0] + self.assertIsInstance(finding, mut.Finding) + self.assertTrue(finding.definite) + self.assertEqual(len(finding.q), 2) + self.assertIsInstance(finding.pair, mut.PairId) + self.assertLess(finding.distance, 1.0) + + def test_check_path_and_trajectory(self): + waypoints = np.array([[0.0, 0.3], [0.0, 0.05]]) + result = self.checker.CheckPath(waypoints=waypoints) + self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) + + trajectory = BezierCurve(0.0, 1.0, waypoints) + result = self.checker.CheckTrajectory(trajectory=trajectory) + self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) + + # Normalize + ComputeMotionBounds introspection seams. + path = self.checker.Normalize(trajectory=trajectory) + self.assertIsInstance(path, mut.PiecewiseBezierPath) + table = self.checker.ComputeMotionBounds(path=path) + self.assertIsInstance(table, mut.MotionBoundTable) + self.assertEqual(table.num_pairs(), len(self.checker.pairs())) + w = np.full(path.num_positions(), 0.1) + self.assertGreaterEqual(table.MotionBound(pair_index=0, w=w), 0.0) + self.assertGreaterEqual(table.carveout_slack(pair_index=0), 0.0) + self.assertIsInstance(table.pair_is_static(pair_index=0), bool) + self.assertIsInstance(table.GetEntries(pair_index=0), list) + self.assertGreaterEqual(table.num_entries(), 0) + + def test_certificate_round_trip(self): + options = _serial_options() + options.emit_certificate = True + q1 = np.array([0.0, 0.0]) + q2 = np.array([0.3, 0.05]) + result = self.checker.CheckEdge(q1=q1, q2=q2, options=options) + self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) + certificate = result.certificate + self.assertIsInstance(certificate, mut.Certificate) + self.assertGreater(len(certificate.records), 0) + self.assertGreater(len(certificate.pairs), 0) + record = certificate.records[0] + self.assertIsInstance(record, mut.CertificateRecord) + self.assertGreaterEqual(record.s_end, record.s_start) + self.assertEqual(len(record.qc), 2) + + # CheckEdge normalizes exactly this waypoint matrix, so the replay + # runs against the same path the certificate was recorded on. + path = mut.PiecewiseBezierPath.FromWaypoints( + waypoints=np.column_stack([q1, q2]), options=options) + self.assertTrue(mut.VerifyCertificate( + checker=self.checker, path=path, certificate=certificate)) + + # A tampered certificate must not verify. + tampered = mut.Certificate(records=list(certificate.records), + pairs=list(certificate.pairs)) + bad = tampered.records[0] + bad.phi_hat = bad.phi_hat + 100.0 + tampered.records = [bad] + list(tampered.records[1:]) + self.assertFalse(mut.VerifyCertificate( + checker=self.checker, path=path, certificate=tampered)) + + def test_piecewise_bezier_path(self): + options = mut.Options() + waypoints = np.array([[0.0, 0.3, 0.6], [0.0, 0.05, 0.10]]) + dut = mut.PiecewiseBezierPath.FromWaypoints(waypoints=waypoints, + options=options) + self.assertEqual(dut.num_positions(), 2) + self.assertEqual(len(dut.segments()), 2) + self.assertIsInstance(dut.segments()[0], mut.BezierSegment) + self.assertEqual(dut.start_time(), 0.0) + self.assertEqual(dut.end_time(), 2.0) + np.testing.assert_allclose(dut.Value(t=0.0), waypoints[:, 0]) + np.testing.assert_allclose(dut.Value(t=2.0), waypoints[:, 2]) + np.testing.assert_allclose(dut.EvaluateSegment(segment_index=0, s=0.0), + waypoints[:, 0]) + np.testing.assert_allclose(dut.global_lower_bound(), waypoints[:, 0]) + np.testing.assert_allclose(dut.global_upper_bound(), waypoints[:, 2]) + self.assertEqual(len(dut.constant_coordinates()), 2) + + trajectory = BezierCurve(0.0, 1.0, waypoints) + from_traj = mut.PiecewiseBezierPath.FromTrajectory( + trajectory=trajectory, options=options) + self.assertEqual(from_traj.num_positions(), 2) + + # The out-params of DeCasteljauSplitAtHalf come back as a tuple. + left, right, mid = mut.DeCasteljauSplitAtHalf(cps=waypoints) + self.assertEqual(left.shape, waypoints.shape) + self.assertEqual(right.shape, waypoints.shape) + np.testing.assert_allclose(mid, from_traj.Value(t=0.5)) + + def test_bounding_sphere(self): + dut = mut.ComputeBoundingSphere( + shape=Sphere(0.25), X_LG=RigidTransform([1.0, 2.0, 3.0])) + self.assertIsInstance(dut, mut.BoundingSphere) + self.assertEqual(dut.radius, 0.25) + np.testing.assert_allclose(dut.center_L, [1.0, 2.0, 3.0]) + + box = mut.ComputeBoundingSphere(shape=Box(2.0, 2.0, 2.0), + X_LG=RigidTransform()) + self.assertAlmostEqual(box.radius, np.sqrt(3.0)) + + def test_add_vpolytope_obstacle(self): + """AddVPolytopeObstacle runs on a pre-finalize plant.""" + builder = _make_arm_builder() + plant = builder.plant() + vertices = np.array([ + [0.0, 0.1, 0.0, 0.0], + [0.0, 0.0, 0.1, 0.0], + [0.0, 0.0, 0.0, 0.1], + ]) + geometry_id = mut.AddVPolytopeObstacle( + plant=plant, vpoly=VPolytope(vertices), + X_WG=RigidTransform([0.0, -0.60, 0.0]), name="vpoly_obstacle") + self.assertIsNotNone(geometry_id) + # The new obstacle rides the ordinary narrowphase path, so a checker + # built on the finalized diagram picks it up as an extra pair. + model = builder.Build() + checker = mut.ContinuousCollisionChecker( + model=model, default_options=_serial_options()) + ids = set() + for pair in checker.pairs(): + ids.add(pair.id.a) + ids.add(pair.id.b) + self.assertIn(geometry_id, ids) + + def test_numerics(self): + self.assertTrue(mut.IsCertified(phi_hat=1.0, tau=1e-6, + motion_bound=0.1, threshold=0.0, + slack=1e-9)) + self.assertFalse(mut.IsCertified(phi_hat=0.05, tau=1e-6, + motion_bound=0.1, threshold=0.0, + slack=1e-9)) + self.assertTrue(mut.IsDefiniteViolation(phi_hat=-0.1, tau=1e-6, + threshold=0.0)) + self.assertFalse(mut.IsDefiniteViolation(phi_hat=0.1, tau=1e-6, + threshold=0.0)) From 65c8b6a261fde1e3d09c7367f0c39245d879760e Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 27 Aug 2026 16:20:09 -0400 Subject: [PATCH 14/22] [planning] continuous_collision: adapt to current Drake master Rebased onto upstream/master 23e8561d0b (2026-08-27), ~1072 commits and ten months past the previous base. Purely mechanical adaptation to upstream API/tooling changes; no behavior was redesigned. Build rules (#24413, "Use opt_out_condition for excluding dynamic_analysis configurations"): - concurrency_timing_test: `disable_in_compilation_mode_dbg = True` plus `tags = ["no_valgrind_tools"]` are both gone; replaced by the single `opt_out_conditions = ["//tools:unoptimized"]`, which now expands to the sanitizers, dbg, kcov and every Valgrind tool -- the same set the two old spellings covered together. - soundness_fuzz_test: `tags = ["no_asan", "no_lsan"]` -> `opt_out_conditions = ["//tools/asan:enabled", "//tools/lsan:enabled"]`, and the small-corpus select key `//tools:using_memcheck` (deleted) -> `//tools/valgrind:enabled`. pydrake, aligning with the pybind11 -> nanobind port (#24840 and the "align with nanobind ... API" series). The module still builds under pybind11 by default; these spellings are the ones valid under both: - `py::module` -> `py::module_` in the definition and in planning_py.h. - `.def_readwrite` -> `.def_rw` (56 sites); `.def_property` -> `.def_prop_rw` (#24562, #24567). - `py::class_` -> pydrake's own `class_` alias (#24669), which also registers weak-referenceability and trampoline type aliases. - ContinuousCollisionChecker's kwargs constructor: `py::init(lambda)` -> a placement-new `.def("__init__", ...)`, since nanobind's `nb::init` takes types only (#24742, #24600); member `params_py.cast()` -> free `py::cast(params_py)` (#24583); and `py::arg("kwargs")` added under `#ifdef PYDRAKE_USE_NANOBIND`, which nanobind requires once any argument is named. - test/continuous_collision_test.py: `# ruff: isort: skip` on the `mut` import (#23672) and reformatted with the pinned ruff. Vendored docstrings: regenerated bindings/generated_docstrings/ planning_continuous_collision.h on the new base. The only change is upstream #24127, which rewrites `::` as U+2237 inside docstring prose; :diff_test passes. Regenerating the other 64 headers reproduced them byte-for-byte, confirming the toolchain matches. Reformatted planning_py_continuous_collision.cc with the repo's pinned clang-format (now 22.1.8) and planning/continuous_collision/BUILD.bazel with the pinned buildifier. Also declares pydrake/planning/continuous_collision.pyi in PYI_FILES. That entry was missing from the original bindings commit rather than being rebase drift, but stubgen fails without it for any module that calls def_submodule, exactly as upstream's own planning/experimental submodule shows. --- .../planning_continuous_collision.h | 34 ++-- bindings/pydrake/BUILD.bazel | 1 + bindings/pydrake/planning/planning_py.h | 2 +- .../planning_py_continuous_collision.cc | 173 ++++++++-------- .../test/continuous_collision_test.py | 186 ++++++++++++------ planning/continuous_collision/BUILD.bazel | 47 +++-- 6 files changed, 249 insertions(+), 194 deletions(-) diff --git a/bindings/generated_docstrings/planning_continuous_collision.h b/bindings/generated_docstrings/planning_continuous_collision.h index cf5a095b0c70..c6d4ac909b19 100644 --- a/bindings/generated_docstrings/planning_continuous_collision.h +++ b/bindings/generated_docstrings/planning_continuous_collision.h @@ -38,13 +38,13 @@ R"""(Registers a V-polytope as an anchored obstacle with a collision role (the geometry-support scope, "V-polytopes as first-class geometry", ingestion route (b)). -The polytope is converted to ``drake::geometry::Convex`` through -Drake's own ``VPolytope::ToShapeConvex()`` entry point (a thin wrapper -over the ``Convex(Eigen::Matrix3X points, std::string label, -double scale)`` constructor pinned at M0), then registered on the -plant's world body. The result therefore rides the ordinary native -narrowphase path end to end: the proximity engine and the certifier's -radius/support code all read the same ``Convex::GetConvexHull()`` +The polytope is converted to ``drake∷geometry∷Convex`` through Drake's +own ``VPolytope∷ToShapeConvex()`` entry point (a thin wrapper over the +``Convex(Eigen∷Matrix3X points, std∷string label, double +scale)`` constructor pinned at M0), then registered on the plant's +world body. The result therefore rides the ordinary native narrowphase +path end to end: the proximity engine and the certifier's +radius/support code all read the same ``Convex∷GetConvexHull()`` object, so the certificate stays sound even for redundant or degenerate vertex sets. @@ -192,7 +192,7 @@ R"""(Result of one certification call (the architecture).)"""; // Symbol: drake::planning::continuous_collision::CertificationResult::certificate struct /* certificate */ { // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""(Present iff Options::emit_certificate.)"""; + const char* doc = R"""(Present iff Options∷emit_certificate.)"""; } certificate; // Symbol: drake::planning::continuous_collision::CertificationResult::findings struct /* findings */ { @@ -230,7 +230,7 @@ Formulas are exact containment per shape: - 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 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. @@ -248,7 +248,7 @@ switches on the closed set of supported shape types and R"""(Certifies — not samples — that a trajectory is collision-free over its entire continuous time domain (the problem statement). -Guarantee: if a check returns Verdict::kCertifiedFree, then for every +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 margin + padding(A, B) — under the stated assumptions: exact real arithmetic up to the @@ -262,7 +262,7 @@ it. 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 deliberately stronger than -planning::CollisionChecker, whose documentation requires a per-thread +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 @@ -452,7 +452,7 @@ halfspace.)"""; struct /* kNative */ { // Source: drake/planning/continuous_collision/distance_oracle.h const char* doc = -R"""(QueryObject::ComputeSignedDistancePairClosestPoints.)"""; +R"""(QueryObject∷ComputeSignedDistancePairClosestPoints.)"""; } kNative; } DistanceRoute; // Symbol: drake::planning::continuous_collision::Finding @@ -546,7 +546,7 @@ control-point box (prismatic chain contributions use the box, so the bound is trajectory-adaptive; the displacement lemma). 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. +MotionBoundTable∷carveout_slack() instead. Raises: RuntimeError naming the joint if the path moves a coordinate of an @@ -672,7 +672,7 @@ Each pair also carries a scalar ``carveout_slack(p)``, the residual motion of the coordinates the constant-coordinate carve-out (trajectory normalization; the joint-support scope) removed from J(p). "Constant" there is a *tolerance* — a coordinate whose global -control-box range is at most Options::continuity_tolerance — not an +control-box range is at most Options∷continuity_tolerance — not an identity, so a carved coordinate may still displace the pair's distal side by up to λ̃_j · range_j. That residual is charged unconditionally inside MotionBound(), which is what makes Δ_p a true upper bound on @@ -780,7 +780,7 @@ R"""(Position coordinates whose junction continuity is checked modulo 2π (GcsTrajectoryOptimization continuous-revolute convention). See also: - planning::trajectory_optimization::GetContinuousRevoluteJointIndices)"""; + planning∷trajectory_optimization∷GetContinuousRevoluteJointIndices)"""; } continuous_revolute_indices; // Symbol: drake::planning::continuous_collision::Options::emit_certificate struct /* emit_certificate */ { @@ -806,7 +806,7 @@ R"""(Maximum polynomial degree accepted for monomial→Bernstein conversion.)""" struct /* max_nodes */ { // Source: drake/planning/continuous_collision/options.h const char* doc = -R"""(Optional node budget; exceeded ⇒ Verdict::kBudgetExhausted.)"""; +R"""(Optional node budget; exceeded ⇒ Verdict∷kBudgetExhausted.)"""; } max_nodes; // Symbol: drake::planning::continuous_collision::Options::max_reported_findings struct /* max_reported_findings */ { @@ -1035,7 +1035,7 @@ R"""(Search modes for certification (the search algorithm).)"""; // Source: drake/planning/continuous_collision/options.h const char* doc = R"""(Certify the full domain and return every violation / inconclusive -region found (bounded by Options::max_reported_findings).)"""; +region found (bounded by Options∷max_reported_findings).)"""; } kCertifyAll; // Symbol: drake::planning::continuous_collision::SearchMode::kFindFirstViolation struct /* kFindFirstViolation */ { 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/planning_py.h b/bindings/pydrake/planning/planning_py.h index caa6c64ddbf1..18129e867a11 100644 --- a/bindings/pydrake/planning/planning_py.h +++ b/bindings/pydrake/planning/planning_py.h @@ -21,7 +21,7 @@ void DefinePlanningCollisionChecker(py::module_ m); void DefinePlanningCollisionCheckerInterfaceTypes(py::module_ m); /* Defines bindings per planning_py_continuous_collision.cc. */ -void DefinePlanningContinuousCollision(py::module m); +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 index a40939cbd720..ead4c618a1d7 100644 --- a/bindings/pydrake/planning/planning_py_continuous_collision.cc +++ b/bindings/pydrake/planning/planning_py_continuous_collision.cc @@ -22,7 +22,7 @@ namespace drake { namespace pydrake { namespace internal { -void DefinePlanningContinuousCollision(py::module m) { +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 @@ -61,47 +61,43 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = Options; constexpr auto& cls_doc = doc.Options; - py::class_ cls(m, "Options", cls_doc.doc); + class_ cls(m, "Options", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("margin", &Class::margin, cls_doc.margin.doc) - .def_readwrite("continuity_tolerance", &Class::continuity_tolerance, + .def_rw("margin", &Class::margin, cls_doc.margin.doc) + .def_rw("continuity_tolerance", &Class::continuity_tolerance, cls_doc.continuity_tolerance.doc) - .def_readwrite("query_tolerance", &Class::query_tolerance, + .def_rw("query_tolerance", &Class::query_tolerance, cls_doc.query_tolerance.doc) - .def_readwrite("certificate_slack", &Class::certificate_slack, + .def_rw("certificate_slack", &Class::certificate_slack, cls_doc.certificate_slack.doc) - .def_readwrite( - "min_interval", &Class::min_interval, cls_doc.min_interval.doc) - .def_readwrite("continuous_revolute_indices", + .def_rw("min_interval", &Class::min_interval, cls_doc.min_interval.doc) + .def_rw("continuous_revolute_indices", &Class::continuous_revolute_indices, cls_doc.continuous_revolute_indices.doc) - .def_readwrite("max_conversion_degree", &Class::max_conversion_degree, + .def_rw("max_conversion_degree", &Class::max_conversion_degree, cls_doc.max_conversion_degree.doc) - .def_readwrite("mode", &Class::mode, cls_doc.mode.doc) - .def_readwrite("max_reported_findings", &Class::max_reported_findings, + .def_rw("mode", &Class::mode, cls_doc.mode.doc) + .def_rw("max_reported_findings", &Class::max_reported_findings, cls_doc.max_reported_findings.doc) - .def_readwrite("max_nodes", &Class::max_nodes, cls_doc.max_nodes.doc) - .def_readwrite("emit_certificate", &Class::emit_certificate, + .def_rw("max_nodes", &Class::max_nodes, cls_doc.max_nodes.doc) + .def_rw("emit_certificate", &Class::emit_certificate, cls_doc.emit_certificate.doc) - .def_readwrite( - "parallelism", &Class::parallelism, cls_doc.parallelism.doc); + .def_rw("parallelism", &Class::parallelism, cls_doc.parallelism.doc); DefCopyAndDeepCopy(&cls); } { using Class = PaddingSpec; constexpr auto& cls_doc = doc.PaddingSpec; - py::class_ cls(m, "PaddingSpec", cls_doc.doc); + class_ cls(m, "PaddingSpec", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite( - "env_padding", &Class::env_padding, cls_doc.env_padding.doc) - .def_readwrite( - "self_padding", &Class::self_padding, cls_doc.self_padding.doc) - .def_readwrite( + .def_rw("env_padding", &Class::env_padding, cls_doc.env_padding.doc) + .def_rw("self_padding", &Class::self_padding, cls_doc.self_padding.doc) + .def_rw( "per_body_pair", &Class::per_body_pair, cls_doc.per_body_pair.doc); DefCopyAndDeepCopy(&cls); } @@ -109,53 +105,49 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = PairId; constexpr auto& cls_doc = doc.PairId; - py::class_ cls(m, "PairId", cls_doc.doc); + class_ cls(m, "PairId", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("a", &Class::a, cls_doc.a.doc) - .def_readwrite("b", &Class::b, cls_doc.b.doc) - .def_readwrite("body_a", &Class::body_a, cls_doc.body_a.doc) - .def_readwrite("body_b", &Class::body_b, cls_doc.body_b.doc); + .def_rw("a", &Class::a, cls_doc.a.doc) + .def_rw("b", &Class::b, cls_doc.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); DefCopyAndDeepCopy(&cls); } { using Class = Finding; constexpr auto& cls_doc = doc.Finding; - py::class_ cls(m, "Finding", cls_doc.doc); + class_ cls(m, "Finding", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("time", &Class::time, cls_doc.time.doc) - .def_readwrite("q", &Class::q, cls_doc.q.doc) - .def_readwrite("pair", &Class::pair, cls_doc.pair.doc) - .def_readwrite("distance", &Class::distance, cls_doc.distance.doc) - .def_readwrite( - "motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) - .def_readwrite("definite", &Class::definite, cls_doc.definite.doc) - .def_readwrite( - "nearest_a_W", &Class::nearest_a_W, cls_doc.nearest_a_W.doc) - .def_readwrite( - "nearest_b_W", &Class::nearest_b_W, cls_doc.nearest_b_W.doc); + .def_rw("time", &Class::time, cls_doc.time.doc) + .def_rw("q", &Class::q, cls_doc.q.doc) + .def_rw("pair", &Class::pair, cls_doc.pair.doc) + .def_rw("distance", &Class::distance, cls_doc.distance.doc) + .def_rw("motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) + .def_rw("definite", &Class::definite, cls_doc.definite.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 = Statistics; constexpr auto& cls_doc = doc.Statistics; - py::class_ cls(m, "Statistics", cls_doc.doc); + class_ cls(m, "Statistics", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("nodes", &Class::nodes, cls_doc.nodes.doc) - .def_readwrite("narrowphase_queries", &Class::narrowphase_queries, + .def_rw("nodes", &Class::nodes, cls_doc.nodes.doc) + .def_rw("narrowphase_queries", &Class::narrowphase_queries, cls_doc.narrowphase_queries.doc) - .def_readwrite("sphere_certifications", &Class::sphere_certifications, + .def_rw("sphere_certifications", &Class::sphere_certifications, cls_doc.sphere_certifications.doc) - .def_readwrite("max_depth", &Class::max_depth, cls_doc.max_depth.doc) - .def_readwrite( - "wall_time_s", &Class::wall_time_s, cls_doc.wall_time_s.doc); + .def_rw("max_depth", &Class::max_depth, cls_doc.max_depth.doc) + .def_rw("wall_time_s", &Class::wall_time_s, cls_doc.wall_time_s.doc); DefCopyAndDeepCopy(&cls); } @@ -163,12 +155,12 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = BoundingSphere; constexpr auto& cls_doc = doc.BoundingSphere; - py::class_ cls(m, "BoundingSphere", cls_doc.doc); + class_ cls(m, "BoundingSphere", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("center_L", &Class::center_L, cls_doc.center_L.doc) - .def_readwrite("radius", &Class::radius, cls_doc.radius.doc); + .def_rw("center_L", &Class::center_L, cls_doc.center_L.doc) + .def_rw("radius", &Class::radius, cls_doc.radius.doc); DefCopyAndDeepCopy(&cls); } @@ -179,13 +171,13 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = BezierSegment; constexpr auto& cls_doc = doc.BezierSegment; - py::class_ cls(m, "BezierSegment", cls_doc.doc); + class_ cls(m, "BezierSegment", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("t_start", &Class::t_start, cls_doc.t_start.doc) - .def_readwrite("t_end", &Class::t_end, cls_doc.t_end.doc) - .def_readwrite("control_points", &Class::control_points, + .def_rw("t_start", &Class::t_start, cls_doc.t_start.doc) + .def_rw("t_end", &Class::t_end, cls_doc.t_end.doc) + .def_rw("control_points", &Class::control_points, cls_doc.control_points.doc); DefCopyAndDeepCopy(&cls); } @@ -193,7 +185,7 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = PiecewiseBezierPath; constexpr auto& cls_doc = doc.PiecewiseBezierPath; - py::class_ cls(m, "PiecewiseBezierPath", cls_doc.doc); + class_ cls(m, "PiecewiseBezierPath", cls_doc.doc); cls // BR .def_static("FromTrajectory", &Class::FromTrajectory, py::arg("trajectory"), py::arg("options"), @@ -239,7 +231,7 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = MotionBoundTable; constexpr auto& cls_doc = doc.MotionBoundTable; - py::class_ cls(m, "MotionBoundTable", cls_doc.doc); + class_ cls(m, "MotionBoundTable", cls_doc.doc); cls // BR .def(py::init<>(), cls_doc.ctor.doc_0args) .def(py::init, std::vector, std::vector, @@ -262,7 +254,7 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = KinematicsEngine; constexpr auto& cls_doc = doc.KinematicsEngine; - py::class_ cls(m, "KinematicsEngine", cls_doc.doc); + class_ cls(m, "KinematicsEngine", cls_doc.doc); cls // BR .def(py::init&>(), py::arg("model"), // Keep the model alive as long as the engine: the C++ object @@ -310,20 +302,20 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = PairRecord; constexpr auto& cls_doc = doc.PairRecord; - py::class_ cls(m, "PairRecord", cls_doc.doc); + class_ cls(m, "PairRecord", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("id", &Class::id, cls_doc.id.doc) - .def_readwrite("route", &Class::route, cls_doc.route.doc) - .def_readwrite("threshold", &Class::threshold, cls_doc.threshold.doc); + .def_rw("id", &Class::id, cls_doc.id.doc) + .def_rw("route", &Class::route, cls_doc.route.doc) + .def_rw("threshold", &Class::threshold, cls_doc.threshold.doc); DefCopyAndDeepCopy(&cls); } { using Class = DistanceOracle; constexpr auto& cls_doc = doc.DistanceOracle; - py::class_ cls(m, "DistanceOracle", cls_doc.doc); + class_ cls(m, "DistanceOracle", cls_doc.doc); cls // BR .def(py::init&, double>(), py::arg("model"), py::arg("query_tolerance"), cls_doc.ctor.doc) @@ -357,31 +349,30 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = CertificateRecord; constexpr auto& cls_doc = doc.CertificateRecord; - py::class_ cls(m, "CertificateRecord", cls_doc.doc); + class_ cls(m, "CertificateRecord", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("segment", &Class::segment, cls_doc.segment.doc) - .def_readwrite("s_start", &Class::s_start, cls_doc.s_start.doc) - .def_readwrite("s_end", &Class::s_end, cls_doc.s_end.doc) - .def_readwrite("pair_index", &Class::pair_index, cls_doc.pair_index.doc) - .def_readwrite("qc", &Class::qc, cls_doc.qc.doc) - .def_readwrite("phi_hat", &Class::phi_hat, cls_doc.phi_hat.doc) - .def_readwrite( - "motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) - .def_readwrite("threshold", &Class::threshold, cls_doc.threshold.doc); + .def_rw("segment", &Class::segment, cls_doc.segment.doc) + .def_rw("s_start", &Class::s_start, cls_doc.s_start.doc) + .def_rw("s_end", &Class::s_end, cls_doc.s_end.doc) + .def_rw("pair_index", &Class::pair_index, cls_doc.pair_index.doc) + .def_rw("qc", &Class::qc, cls_doc.qc.doc) + .def_rw("phi_hat", &Class::phi_hat, cls_doc.phi_hat.doc) + .def_rw("motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) + .def_rw("threshold", &Class::threshold, cls_doc.threshold.doc); DefCopyAndDeepCopy(&cls); } { using Class = Certificate; constexpr auto& cls_doc = doc.Certificate; - py::class_ cls(m, "Certificate", cls_doc.doc); + class_ cls(m, "Certificate", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("records", &Class::records, cls_doc.records.doc) - .def_readwrite("pairs", &Class::pairs, cls_doc.pairs.doc); + .def_rw("records", &Class::records, cls_doc.records.doc) + .def_rw("pairs", &Class::pairs, cls_doc.pairs.doc); DefCopyAndDeepCopy(&cls); } @@ -389,31 +380,30 @@ collision-free over its entire continuous time domain, rather than sampling it. { using Class = CertificationResult; constexpr auto& cls_doc = doc.CertificationResult; - py::class_ cls(m, "CertificationResult", cls_doc.doc); + class_ cls(m, "CertificationResult", cls_doc.doc); cls // BR .def(py::init<>()) .def(ParamInit()) - .def_readwrite("verdict", &Class::verdict, cls_doc.verdict.doc) - .def_readwrite("findings", &Class::findings, cls_doc.findings.doc) - .def_readwrite("stats", &Class::stats, cls_doc.stats.doc) - .def_readwrite( - "certificate", &Class::certificate, cls_doc.certificate.doc); + .def_rw("verdict", &Class::verdict, cls_doc.verdict.doc) + .def_rw("findings", &Class::findings, cls_doc.findings.doc) + .def_rw("stats", &Class::stats, cls_doc.stats.doc) + .def_rw("certificate", &Class::certificate, cls_doc.certificate.doc); DefCopyAndDeepCopy(&cls); } { using Class = ContinuousCollisionChecker; constexpr auto& cls_doc = doc.ContinuousCollisionChecker; - py::class_ cls(m, "ContinuousCollisionChecker", cls_doc.doc); + class_ cls(m, "ContinuousCollisionChecker", cls_doc.doc); { using Nested = Class::Params; constexpr auto& nested_doc = cls_doc.Params; - py::class_ nested_cls(cls, "Params", nested_doc.doc); + class_ nested_cls(cls, "Params", nested_doc.doc); nested_cls // BR .def(py::init<>()) .def(ParamInit()) - .def_property( + .def_prop_rw( "model", [](const Nested& self) -> const RobotDiagram* { return self.model.get(); @@ -425,28 +415,33 @@ collision-free over its entire continuous time domain, rather than sampling it. make_shared_ptr_from_py_object>(model); }, nested_doc.model.doc) - .def_readwrite("padding", &Nested::padding, nested_doc.padding.doc) - .def_readwrite("default_options", &Nested::default_options, + .def_rw("padding", &Nested::padding, nested_doc.padding.doc) + .def_rw("default_options", &Nested::default_options, nested_doc.default_options.doc); } py::object params_ctor = cls.attr("Params"); cls // BR .def( - py::init([params_ctor](py::object model, const py::kwargs& kwargs) { + "__init__", + [params_ctor]( + Class* self, py::object model, const py::kwargs& kwargs) { // For lifetime management, we need to treat pointer-like // arguments separately. Start by creating a Params object in // Python with all of the other non-pointer kwargs. py::object params_py = params_ctor(**kwargs); - auto* params = params_py.cast(); + auto* params = py::cast(params_py); DRAKE_DEMAND(params != nullptr); // Now, add a python reference to model (owned by the shared // pointer), and transfer that to the c++ checker. params->model = make_shared_ptr_from_py_object>(model); - return std::make_unique(std::move(*params)); - }), + new (self) Class(std::move(*params)); + }, py::kw_only(), py::arg("model"), +#ifdef PYDRAKE_USE_NANOBIND + py::arg("kwargs"), +#endif (std::string(cls_doc.ctor.doc) + "\n\n" "See :class:`pydrake.planning.continuous_collision" diff --git a/bindings/pydrake/planning/test/continuous_collision_test.py b/bindings/pydrake/planning/test/continuous_collision_test.py index 7320ef029120..0c0d86402951 100644 --- a/bindings/pydrake/planning/test/continuous_collision_test.py +++ b/bindings/pydrake/planning/test/continuous_collision_test.py @@ -1,4 +1,4 @@ -import pydrake.planning.continuous_collision as mut +import pydrake.planning.continuous_collision as mut # ruff: isort: skip import unittest @@ -21,8 +21,11 @@ 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)) + 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 _make_arm_builder(): @@ -35,34 +38,56 @@ def _make_arm_builder(): 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.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=CoulombFriction(1.0, 1.0)) + body=link, + X_BG=RigidTransform([0.15, 0.0, 0.0]), + shape=Box(0.30, 0.05, 0.05), + name="link_geom", + coulomb_friction=CoulombFriction(1.0, 1.0), + ) plant.RegisterCollisionGeometry( - body=tool, X_BG=RigidTransform(), shape=Sphere(0.04), - name="tool_geom", coulomb_friction=CoulombFriction(1.0, 1.0)) + body=tool, + X_BG=RigidTransform(), + shape=Sphere(0.04), + name="tool_geom", + coulomb_friction=CoulombFriction(1.0, 1.0), + ) 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.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=CoulombFriction(1.0, 1.0)) + body=post, + X_BG=RigidTransform(), + shape=Sphere(0.08), + name="post_geom", + coulomb_friction=CoulombFriction(1.0, 1.0), + ) return builder @@ -76,7 +101,8 @@ class TestContinuousCollision(unittest.TestCase): def setUp(self): self.model = _make_arm_builder().Build() self.checker = mut.ContinuousCollisionChecker( - model=self.model, default_options=_serial_options()) + model=self.model, default_options=_serial_options() + ) def test_options(self): """Exercises the Options / PaddingSpec / enum surface.""" @@ -141,8 +167,9 @@ def test_params_and_introspection(self): self.assertIsInstance(engine, mut.KinematicsEngine) self.assertEqual(engine.num_positions(), 2) pair = checker.pairs()[0].id - coords = engine.CoordinatesAffectingPair(body_a=pair.body_a, - body_b=pair.body_b) + coords = engine.CoordinatesAffectingPair( + body_a=pair.body_a, body_b=pair.body_b + ) self.assertIsInstance(coords, list) def test_check_edge_free_and_colliding(self): @@ -207,24 +234,33 @@ def test_certificate_round_trip(self): # CheckEdge normalizes exactly this waypoint matrix, so the replay # runs against the same path the certificate was recorded on. path = mut.PiecewiseBezierPath.FromWaypoints( - waypoints=np.column_stack([q1, q2]), options=options) - self.assertTrue(mut.VerifyCertificate( - checker=self.checker, path=path, certificate=certificate)) + waypoints=np.column_stack([q1, q2]), options=options + ) + self.assertTrue( + mut.VerifyCertificate( + checker=self.checker, path=path, certificate=certificate + ) + ) # A tampered certificate must not verify. - tampered = mut.Certificate(records=list(certificate.records), - pairs=list(certificate.pairs)) + tampered = mut.Certificate( + records=list(certificate.records), pairs=list(certificate.pairs) + ) bad = tampered.records[0] bad.phi_hat = bad.phi_hat + 100.0 tampered.records = [bad] + list(tampered.records[1:]) - self.assertFalse(mut.VerifyCertificate( - checker=self.checker, path=path, certificate=tampered)) + self.assertFalse( + mut.VerifyCertificate( + checker=self.checker, path=path, certificate=tampered + ) + ) def test_piecewise_bezier_path(self): options = mut.Options() waypoints = np.array([[0.0, 0.3, 0.6], [0.0, 0.05, 0.10]]) - dut = mut.PiecewiseBezierPath.FromWaypoints(waypoints=waypoints, - options=options) + dut = mut.PiecewiseBezierPath.FromWaypoints( + waypoints=waypoints, options=options + ) self.assertEqual(dut.num_positions(), 2) self.assertEqual(len(dut.segments()), 2) self.assertIsInstance(dut.segments()[0], mut.BezierSegment) @@ -232,15 +268,17 @@ def test_piecewise_bezier_path(self): self.assertEqual(dut.end_time(), 2.0) np.testing.assert_allclose(dut.Value(t=0.0), waypoints[:, 0]) np.testing.assert_allclose(dut.Value(t=2.0), waypoints[:, 2]) - np.testing.assert_allclose(dut.EvaluateSegment(segment_index=0, s=0.0), - waypoints[:, 0]) + np.testing.assert_allclose( + dut.EvaluateSegment(segment_index=0, s=0.0), waypoints[:, 0] + ) np.testing.assert_allclose(dut.global_lower_bound(), waypoints[:, 0]) np.testing.assert_allclose(dut.global_upper_bound(), waypoints[:, 2]) self.assertEqual(len(dut.constant_coordinates()), 2) trajectory = BezierCurve(0.0, 1.0, waypoints) from_traj = mut.PiecewiseBezierPath.FromTrajectory( - trajectory=trajectory, options=options) + trajectory=trajectory, options=options + ) self.assertEqual(from_traj.num_positions(), 2) # The out-params of DeCasteljauSplitAtHalf come back as a tuple. @@ -251,33 +289,41 @@ def test_piecewise_bezier_path(self): def test_bounding_sphere(self): dut = mut.ComputeBoundingSphere( - shape=Sphere(0.25), X_LG=RigidTransform([1.0, 2.0, 3.0])) + shape=Sphere(0.25), X_LG=RigidTransform([1.0, 2.0, 3.0]) + ) self.assertIsInstance(dut, mut.BoundingSphere) self.assertEqual(dut.radius, 0.25) np.testing.assert_allclose(dut.center_L, [1.0, 2.0, 3.0]) - box = mut.ComputeBoundingSphere(shape=Box(2.0, 2.0, 2.0), - X_LG=RigidTransform()) + box = mut.ComputeBoundingSphere( + shape=Box(2.0, 2.0, 2.0), X_LG=RigidTransform() + ) self.assertAlmostEqual(box.radius, np.sqrt(3.0)) def test_add_vpolytope_obstacle(self): """AddVPolytopeObstacle runs on a pre-finalize plant.""" builder = _make_arm_builder() plant = builder.plant() - vertices = np.array([ - [0.0, 0.1, 0.0, 0.0], - [0.0, 0.0, 0.1, 0.0], - [0.0, 0.0, 0.0, 0.1], - ]) + vertices = np.array( + [ + [0.0, 0.1, 0.0, 0.0], + [0.0, 0.0, 0.1, 0.0], + [0.0, 0.0, 0.0, 0.1], + ] + ) geometry_id = mut.AddVPolytopeObstacle( - plant=plant, vpoly=VPolytope(vertices), - X_WG=RigidTransform([0.0, -0.60, 0.0]), name="vpoly_obstacle") + plant=plant, + vpoly=VPolytope(vertices), + X_WG=RigidTransform([0.0, -0.60, 0.0]), + name="vpoly_obstacle", + ) self.assertIsNotNone(geometry_id) # The new obstacle rides the ordinary narrowphase path, so a checker # built on the finalized diagram picks it up as an extra pair. model = builder.Build() checker = mut.ContinuousCollisionChecker( - model=model, default_options=_serial_options()) + model=model, default_options=_serial_options() + ) ids = set() for pair in checker.pairs(): ids.add(pair.id.a) @@ -285,13 +331,27 @@ def test_add_vpolytope_obstacle(self): self.assertIn(geometry_id, ids) def test_numerics(self): - self.assertTrue(mut.IsCertified(phi_hat=1.0, tau=1e-6, - motion_bound=0.1, threshold=0.0, - slack=1e-9)) - self.assertFalse(mut.IsCertified(phi_hat=0.05, tau=1e-6, - motion_bound=0.1, threshold=0.0, - slack=1e-9)) - self.assertTrue(mut.IsDefiniteViolation(phi_hat=-0.1, tau=1e-6, - threshold=0.0)) - self.assertFalse(mut.IsDefiniteViolation(phi_hat=0.1, tau=1e-6, - threshold=0.0)) + self.assertTrue( + mut.IsCertified( + phi_hat=1.0, + tau=1e-6, + motion_bound=0.1, + threshold=0.0, + slack=1e-9, + ) + ) + self.assertFalse( + mut.IsCertified( + phi_hat=0.05, + tau=1e-6, + motion_bound=0.1, + threshold=0.0, + slack=1e-9, + ) + ) + self.assertTrue( + mut.IsDefiniteViolation(phi_hat=-0.1, tau=1e-6, threshold=0.0) + ) + self.assertFalse( + mut.IsDefiniteViolation(phi_hat=0.1, tau=1e-6, threshold=0.0) + ) diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index 6c4f6c37534d..87336122eed1 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -303,18 +303,18 @@ drake_cc_googletest( # the shrink recovers, and the corpus is deliberately leaked. 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 - # defines exactly one of them), so this select is unambiguous. + # matches exactly one of them), so this select is unambiguous. defines = select({ - "//tools:using_memcheck": ["DRAKE_CCD_FUZZ_SMALL_CORPUS"], + "//tools/valgrind:enabled": ["DRAKE_CCD_FUZZ_SMALL_CORPUS"], "//tools:using_sanitizer": ["DRAKE_CCD_FUZZ_SMALL_CORPUS"], "//conditions:default": [], }), - tags = [ - "no_asan", - "no_lsan", - ], deps = [ ":continuous_collision_checker", "//common:parallelism", @@ -403,14 +403,13 @@ drake_cc_googletest( # T8 — the two per-call scaling claims. Split from concurrency_test because # they are wall-clock claims: Valgrind serializes threads, which inverts # "parallel is faster than serial" and fails the test for a reason that has -# nothing to do with the driver. disable_in_compilation_mode_dbg already -# excludes the sanitizers and memcheck; no_valgrind_tools adds drd and -# helgrind, which serialize the same way. +# nothing to do with the driver. //tools:unoptimized covers every flavor that +# does so: the sanitizers, dbg, kcov, and all of the Valgrind tools (memcheck +# plus drd and helgrind, which serialize the same way). drake_cc_googletest( name = "concurrency_timing_test", - disable_in_compilation_mode_dbg = True, + opt_out_conditions = ["//tools:unoptimized"], num_threads = 16, - tags = ["no_valgrind_tools"], deps = [ ":concurrency_test_utilities", "//common:parallelism", @@ -462,22 +461,9 @@ drake_cc_binary( "benchmark/scenario_worlds.cc", "benchmark/scenario_worlds.h", ], - add_test_rule = 1, data = [ "@drake_models//:iiwa_description", ], - test_rule_args = [ - "--only", - "dual", - "--reps", - "1", - "--warmup", - "0", - "--dense-samples", - "200", - ], - test_rule_size = "small", - test_rule_timeout = "moderate", deps = [ ":continuous_collision_checker", "//common:copyable_unique_ptr", @@ -498,6 +484,19 @@ drake_cc_binary( "//planning:scene_graph_collision_checker", "@eigen", ], + add_test_rule = 1, + test_rule_args = [ + "--only", + "dual", + "--reps", + "1", + "--warmup", + "0", + "--dense-samples", + "200", + ], + test_rule_size = "small", + test_rule_timeout = "moderate", ) add_lint_tests() From 91342677a20b401f2eed0de1477bc6ad98314b96 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 27 Aug 2026 18:27:36 -0400 Subject: [PATCH 15/22] [planning] continuous_collision: revise comments to Drake conventions --- planning/continuous_collision/BUILD.bazel | 32 +-- .../benchmark/benchmark_util.cc | 8 +- .../benchmark/benchmark_util.h | 121 +++++---- .../benchmark/iiwa_benchmark.cc | 156 ++++++----- .../benchmark/scenario_worlds.cc | 10 +- .../benchmark/scenario_worlds.h | 54 ++-- .../continuous_collision/bounding_sphere.cc | 40 ++- .../continuous_collision/bounding_sphere.h | 22 +- planning/continuous_collision/certificate.cc | 33 ++- planning/continuous_collision/certificate.h | 2 +- .../certifier_internal.cc | 130 +++++----- .../continuous_collision/certifier_internal.h | 185 ++++++------- .../continuous_collision_checker.cc | 168 +++++------- .../continuous_collision_checker.h | 101 ++++++-- .../continuous_collision/distance_oracle.cc | 48 ++-- .../continuous_collision/distance_oracle.h | 53 ++-- .../motion_bound_table.cc | 155 ++++++----- .../continuous_collision/motion_bound_table.h | 96 ++++--- planning/continuous_collision/numerics.h | 20 +- planning/continuous_collision/options.h | 22 +- .../piecewise_bezier_path.cc | 21 +- .../piecewise_bezier_path.h | 29 ++- .../continuous_collision/test/api_test.cc | 112 ++++---- .../test/bounding_sphere_test.cc | 43 ++-- .../test/certificate_test.cc | 187 +++++++------- .../test/certifier_test.cc | 119 ++++----- .../test/concurrency_test.cc | 128 ++++----- .../test/concurrency_test_utilities.h | 96 ++++--- .../test/concurrency_timing_test.cc | 90 +++---- .../test/distance_oracle_test.cc | 95 ++++--- .../test/motion_bound_test.cc | 190 ++++++-------- .../test/piecewise_bezier_path_test.cc | 23 +- .../test/soundness_fuzz_test.cc | 242 ++++++++---------- .../test/thin_obstacle_test.cc | 117 ++++----- .../vpolytope_ingestion.h | 8 +- 35 files changed, 1388 insertions(+), 1568 deletions(-) diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index 87336122eed1..4370206d35de 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -142,8 +142,7 @@ drake_cc_library( # The certificate and the node recursion are mutually recursive translation # units (certificate.cc replays the events that certifier_internal.cc emits), -# so they form one library, exactly as they formed one module in the standalone -# package. +# so they form one library. drake_cc_library( name = "certifier", srcs = [ @@ -203,7 +202,7 @@ drake_cc_library( # === test/ === -# T1 — curve module acceptance tests. +# Curve module acceptance tests. drake_cc_googletest( name = "piecewise_bezier_path_test", deps = [ @@ -218,7 +217,7 @@ drake_cc_googletest( ], ) -# T2 — the displacement lemma, the lambda table and the J(p) subtree logic. +# The displacement lemma, the lambda table and the J(p) subtree logic. drake_cc_googletest( name = "motion_bound_test", timeout = "moderate", @@ -234,7 +233,7 @@ drake_cc_googletest( ], ) -# T2 — the bounding-sphere radius property test. +# The bounding-sphere radius property test. drake_cc_googletest( name = "bounding_sphere_test", deps = [ @@ -248,7 +247,7 @@ drake_cc_googletest( ], ) -# T3 — oracle accuracy, probe classification, half-space fallback, V-polytope. +# Oracle accuracy, probe classification, half-space fallback, V-polytope. drake_cc_googletest( name = "distance_oracle_test", data = ["//geometry:test_obj_files"], @@ -272,7 +271,8 @@ drake_cc_googletest( ], ) -# T4/T6 — certifier semantics on a focused, hand-built corpus. +# Certifier semantics, including retiming invariance, on a focused, +# hand-built corpus. drake_cc_googletest( name = "certifier_test", # Eight caller threads, each asking for Parallelism(2). @@ -291,7 +291,7 @@ drake_cc_googletest( ], ) -# T4 — the randomized soundness fuzz: random worlds x random trajectories, +# The randomized soundness fuzz: random worlds x random trajectories, # cross-checked against dense sampling and against the certificate replay. # The dense cross-check (~1e7 signed-distance queries) is what makes this # test long rather than the certification itself. @@ -300,7 +300,7 @@ drake_cc_googletest( # 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 shrink recovers, and the corpus is deliberately leaked. +# the quarter corpus recovers. drake_cc_googletest( name = "soundness_fuzz_test", opt_out_conditions = [ @@ -334,7 +334,9 @@ drake_cc_googletest( ], ) -# T5 — thin-obstacle regression (the reason this library exists). +# 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 = [ @@ -353,7 +355,7 @@ drake_cc_googletest( ], ) -# T7 — certificate audit trail + mutation test. +# Certificate audit trail + mutation test. drake_cc_googletest( name = "certificate_test", deps = [ @@ -369,7 +371,7 @@ drake_cc_googletest( ], ) -# T8 — the corpus and the deep workload both concurrency targets run on. +# The corpus and the deep workload both concurrency targets run on. drake_cc_library( name = "concurrency_test_utilities", testonly = 1, @@ -388,7 +390,7 @@ drake_cc_library( ], ) -# T8 — concurrency determinism. Running with many threads is the point of +# 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. drake_cc_googletest( @@ -400,7 +402,7 @@ drake_cc_googletest( ], ) -# T8 — the two per-call scaling claims. Split from concurrency_test because +# The two per-call scaling claims. Split from concurrency_test because # they are wall-clock claims: Valgrind serializes threads, which inverts # "parallel is faster than serial" and fails the test for a reason that has # nothing to do with the driver. //tools:unoptimized covers every flavor that @@ -416,7 +418,7 @@ drake_cc_googletest( ], ) -# T9 — API / UX clear-throw tests. +# API / UX clear-throw tests. drake_cc_googletest( name = "api_test", deps = [ diff --git a/planning/continuous_collision/benchmark/benchmark_util.cc b/planning/continuous_collision/benchmark/benchmark_util.cc index bef4f05a1af1..ddb7be16f02c 100644 --- a/planning/continuous_collision/benchmark/benchmark_util.cc +++ b/planning/continuous_collision/benchmark/benchmark_util.cc @@ -32,7 +32,7 @@ using drake::trajectories::Trajectory; using Eigen::MatrixXd; using Eigen::VectorXd; -/// Formats a double with enough digits to round-trip through the JSON. +// Formats a double with enough digits to round-trip through the JSON. std::string FormatDouble(double v) { if (std::isnan(v)) return "null"; if (std::isinf(v)) return v > 0 ? "1e999" : "-1e999"; @@ -64,7 +64,7 @@ std::string Escape(const std::string& s) { return out; } -/// One (t, min-distance-over-all-pairs, min-distance-over-env-pairs) probe. +// One (t, min-distance-over-all-pairs, min-distance-over-env-pairs) probe. struct Probe { double all{0.0}; double env{0.0}; @@ -92,8 +92,8 @@ Probe ProbeAt(const RobotDiagram& diagram, Context* root, return p; } -/// Golden-section minimization of `f` on [lo, hi]; the sampled bracket around -/// a dense-sample argmin is unimodal in practice for these smooth curves. +// Golden-section minimization of `f` on [lo, hi]; the sampled bracket around +// a dense-sample argmin is unimodal in practice for these smooth curves. std::pair GoldenSectionMin( const std::function& f, double lo, double hi, int iterations) { diff --git a/planning/continuous_collision/benchmark/benchmark_util.h b/planning/continuous_collision/benchmark/benchmark_util.h index a919046b351b..300548b0b4d9 100644 --- a/planning/continuous_collision/benchmark/benchmark_util.h +++ b/planning/continuous_collision/benchmark/benchmark_util.h @@ -1,15 +1,12 @@ #pragma once -/// @file -/// Small, deliberately boring helpers shared by the benchmark scenarios -/// (the benchmark suite): a hand-rolled JSON writer, steady_clock timing with -/// medians, machine identification, quintic composite-Bézier construction, and -/// a dense ground-truth swept-clearance sampler used to *verify* — never to -/// certify — the clearance of every scenario trajectory. -/// -/// No third-party benchmark framework is used on purpose: the measurements -/// here are milliseconds-scale wall clock repeated by hand, and the JSON is -/// consumed by the white-paper author and by CI tracking. +// Helpers shared by the benchmark scenarios: a hand-rolled JSON writer, +// steady_clock timing with medians, machine identification, quintic +// composite-Bézier construction, and a dense ground-truth swept-clearance +// sampler that verifies, but never certifies, the clearance of every scenario +// trajectory. No third-party benchmark framework is involved: the measurements +// here are millisecond-scale wall clock repeated by hand, and the JSON is +// consumed by CI tracking. #include #include @@ -35,18 +32,24 @@ namespace internal { // JSON // --------------------------------------------------------------------------- -/// Minimal streaming JSON writer: enough for the fixed result schema, with no -/// dependency and no cleverness. Callers must balance Begin*/End* calls. +// Minimal streaming JSON writer: enough for the fixed result schema, with no +// dependency. Callers must balance Begin*/End* calls. class JsonWriter { public: JsonWriter() = default; + // Opens an object as the next element of the innermost array. void BeginObject(); + // Opens an object under `key` in the innermost object. void BeginObject(const std::string& key); + // Closes the innermost object. void EndObject(); + // Opens an array under `key` in the innermost object. void BeginArray(const std::string& key); + // Closes the innermost array. void EndArray(); + // Writes one `key`: `value` member into the innermost object. void Write(const std::string& key, double value); void Write(const std::string& key, int value); void Write(const std::string& key, long value); // NOLINT @@ -54,10 +57,12 @@ class JsonWriter { void Write(const std::string& key, bool value); void Write(const std::string& key, const char* value); void Write(const std::string& key, const std::string& value); - /// Appends a bare double to the innermost array. + // Appends a bare double to the innermost array. void WriteArrayValue(double value); + // Appends a bare string to the innermost array. void WriteArrayValue(const std::string& value); + // Returns the document written so far, newline-terminated. std::string str() const { return out_ + "\n"; } private: @@ -69,13 +74,14 @@ class JsonWriter { int depth_{0}; }; -/// Writes `text` to `path`, creating parent directories as needed. +// Writes `text` to `path`, creating parent directories as needed. void WriteTextFile(const std::string& path, const std::string& text); // --------------------------------------------------------------------------- // Timing // --------------------------------------------------------------------------- +// Wall-clock summary of one repeated measurement, in milliseconds. struct TimingSummary { double median_ms{0.0}; double min_ms{0.0}; @@ -83,9 +89,11 @@ struct TimingSummary { int reps{0}; }; -/// Runs `body` `warmup` times untimed, then `reps` times timed, and reduces -/// the sample to median/min/max. No pinning, no frequency control: the numbers -/// are what a user on this machine would see (the benchmark suite). +// Runs `body` `warmup` times untimed, then `reps` times timed, and reduces the +// sample to median/min/max. No pinning and no frequency control: the numbers +// are what a user on this machine would see. An exception thrown by `body` +// propagates and no summary is produced. +// @pre reps >= 1; the summary reads the ends of a sample of `reps` entries. template TimingSummary TimeRepeatedly(int warmup, int reps, F&& body) { for (int i = 0; i < warmup; ++i) { @@ -109,6 +117,7 @@ TimingSummary TimeRepeatedly(int warmup, int reps, F&& body) { return s; } +// Writes `t` as an object under `key`: median, min, max and reps. void WriteTiming(JsonWriter* json, const std::string& key, const TimingSummary& t); @@ -116,6 +125,7 @@ void WriteTiming(JsonWriter* json, const std::string& key, // Machine identification // --------------------------------------------------------------------------- +// Identification of the machine and the build a result file was produced on. struct MachineInfo { std::string cpu_model; int core_count{0}; @@ -123,42 +133,47 @@ struct MachineInfo { std::string drake_version_note; }; -/// Reads the CPU model from /proc/cpuinfo and records `drake_commit` (the -/// Drake revision the caller was built from, passed through verbatim) so -/// every result file self-identifies. +// Reads the CPU model from /proc/cpuinfo and records `drake_commit` (the +// Drake revision the caller was built from, passed through verbatim) so +// every result file self-identifies. MachineInfo GetMachineInfo(const std::string& drake_commit); +// Writes `machine` as the "machine" object of a result file. void WriteMachine(JsonWriter* json, const MachineInfo& machine); // --------------------------------------------------------------------------- // Trajectories // --------------------------------------------------------------------------- -/// Builds a C2 composite quintic Bézier through the columns of `waypoints` -/// (n × K) at the given `times` (K values, strictly increasing). Waypoint -/// velocities come from centred finite differences (zero at both ends) and -/// waypoint accelerations are zero, which is exactly the smooth composite -/// Bézier a GCS/B-spline planner would hand us — degree 5, K−1 segments. -/// -/// Control points per segment (duration h, endpoint velocities v0, v1): -/// P0 = q0, P5 = q1, -/// P1 = P0 + h v0/5, P4 = P5 − h v1/5, -/// P2 = P0 + 2 h v0/5, P3 = P5 − 2 h v1/5, -/// which reproduces q(t0)=q0, q̇(t0)=v0, q̈(t0)=0 and likewise at t1. +// Builds a C2 composite quintic Bézier through the columns of `waypoints` +// (n × K) at `times` (K strictly increasing values): the smooth composite +// Bézier a GCS/B-spline planner would hand us, degree 5 with K−1 segments. +// Waypoint velocities are centred finite differences (zero at both ends) and +// waypoint accelerations are zero. Per segment, duration h, velocities v0, v1: +// clang-format off +// P0 = q0, P5 = q1, +// P1 = P0 + h v0/5, P4 = P5 − h v1/5, +// P2 = P0 + 2 h v0/5, P3 = P5 − 2 h v1/5, +// clang-format on +// so q(t0)=q0, q̇(t0)=v0, q̈(t0)=0, and likewise at t1. +// @throws std::exception if waypoints.cols() < 2. +// @throws std::exception if times.size() != waypoints.cols(). +// @pre times is strictly increasing; the centred differences divide by +// times[i+1] - times[i-1]. std::shared_ptr> MakeQuinticCompositeBezier(const Eigen::MatrixXd& waypoints, const std::vector& times); -/// Path length in the plant's default edge metric: the unweighted Euclidean -/// configuration distance (LinearDistanceAndInterpolationProvider's default -/// weights are 1 for every non-quaternion coordinate), integrated along the -/// trajectory with `num_samples` chords. Used to derive the number of samples -/// a sampled checker would take at a given edge_step_size. +// Path length in the plant's default edge metric: the unweighted Euclidean +// configuration distance (LinearDistanceAndInterpolationProvider's default +// weights are 1 for every non-quaternion coordinate), integrated along the +// trajectory with `num_samples` chords. Used to derive the number of samples +// a sampled checker would take at a given edge_step_size. double PathLengthInEdgeMetric(const trajectories::Trajectory& t, int num_samples); -/// Samples `count` configurations uniformly in trajectory time (inclusive of -/// both endpoints). +// Samples `count` configurations uniformly in trajectory time (inclusive of +// both endpoints). std::vector SampleTrajectory( const trajectories::Trajectory& trajectory, int count); @@ -166,12 +181,10 @@ std::vector SampleTrajectory( // Ground-truth swept clearance // --------------------------------------------------------------------------- -/// True minimum signed distance along a trajectory, obtained by dense -/// sampling plus a golden-section refinement of the sampled argmin. This is -/// the benchmark's independent oracle: it is what "achieved clearance" means -/// in the result files. `min_env` restricts the minimum to robot-vs- -/// environment pairs (the quantity a shelf shift/scale actually controls); -/// `min_all` also includes robot-vs-robot pairs. +// The result of MeasureSweptClearance. `min_env` restricts the minimum to +// robot-vs-environment pairs (the quantity a shelf shift/scale actually +// controls); `min_all` also includes robot-vs-robot pairs. `t_all` and `t_env` +// are the trajectory times at which those minima occur. struct ClearanceReport { double min_all{0.0}; double t_all{0.0}; @@ -180,23 +193,29 @@ struct ClearanceReport { int samples{0}; }; -/// Geometry ids belonging to bodies of the named model instances. +// Geometry ids belonging to bodies of the named model instances. std::unordered_set CollectGeometryIds( const RobotDiagram& diagram, const std::vector& model_instance_names); -/// Dense-samples `trajectory` (`num_samples` configurations, split over -/// `num_threads` cloned contexts) and refines the minimum by golden section. -/// Distances beyond `max_distance` are not resolved; if no pair comes within -/// it the reported minimum saturates at `max_distance`. +// True minimum signed distance along `trajectory`, obtained by dense sampling +// (`num_samples` configurations split over `num_threads` cloned contexts) plus +// a golden-section refinement of the sampled argmin. This is the benchmark's +// independent oracle: it is what "achieved clearance" means in the result +// files. Distances beyond `max_distance` are not resolved; if no pair comes +// within it the reported minimum saturates at `max_distance`. A `num_threads` +// below 1 is treated as 1. +// @throws std::exception if a configuration on `trajectory` does not have +// diagram.plant().num_positions() rows, or holds a non-finite value. +// @pre num_samples >= 2; the sample times divide by num_samples - 1. ClearanceReport MeasureSweptClearance( const RobotDiagram& diagram, const trajectories::Trajectory& trajectory, const std::unordered_set& env_ids, int num_samples, int num_threads, double max_distance); -/// Bisects `f` (assumed non-decreasing) on [lo, hi] for f(x) = target. -/// Returns x. Used to place the shelf at a requested swept clearance. +// Bisects `f` (assumed non-decreasing) on [lo, hi] for f(x) = target. +// Returns x. Used to place the shelf at a requested swept clearance. double BisectMonotone(const std::function& f, double lo, double hi, double target, int iterations); diff --git a/planning/continuous_collision/benchmark/iiwa_benchmark.cc b/planning/continuous_collision/benchmark/iiwa_benchmark.cc index cf3d40c655d1..3277e33ff32e 100644 --- a/planning/continuous_collision/benchmark/iiwa_benchmark.cc +++ b/planning/continuous_collision/benchmark/iiwa_benchmark.cc @@ -1,31 +1,18 @@ -/// @file -/// The `continuous_collision` performance benchmark suite (the performance -/// targets and the benchmark deliverable of the white paper), adapted to what -/// exists on this machine: no trajectory optimizer is invoked, the smooth -/// composite Bézier trajectories are hand-constructed in -/// benchmark/scenario_worlds.cc, and every scenario's *true* swept clearance is -/// verified by dense sampling before it is benchmarked. -/// -/// Scenarios -/// a) iiwa14 + bookcase, three tiers at ~2 mm / 1 cm / 5 cm swept clearance -/// b) a two-waypoint PWL edge in the same world -/// c) dual-arm iiwa handover (self-collision heavy) -/// d) the grazing pathological case (kInconclusive cost at the floor) -/// e) thread scaling over a 1000-check batch, two ways -/// -/// Scenarios (a, 1 cm tier) and (b) are additionally compared against Drake's -/// own sampled `SceneGraphCollisionChecker` on the *same* RobotDiagram. -/// -/// Usage: iiwa_benchmark [--out DIR] [--reps N] [--warmup N] -/// [--dense-samples N] [--batch N] [--only NAME] -/// [--drake_commit SHA] -/// -/// `--out` defaults to the current directory, or to $TEST_TMPDIR when that is -/// set — the sandbox is the only writable directory under `bazel test`, and -/// the smoke-test rule in BUILD.bazel relies on this so it needs no --out of -/// its own. `--drake_commit` (the Drake revision this binary was built from, -/// "unknown" by default) is recorded verbatim in every result file so a JSON -/// result identifies the code it measured. +// The `continuous_collision` performance benchmark suite. No trajectory +// optimizer is invoked: the smooth composite Bézier trajectories are +// hand-constructed in benchmark/scenario_worlds.cc, and every scenario's +// *true* swept clearance is verified by dense sampling before it is +// benchmarked. +// +// Scenarios +// a) iiwa14 + bookcase, three tiers at ~2 mm / 1 cm / 5 cm swept clearance +// b) a two-waypoint PWL edge in the same world +// c) dual-arm iiwa handover (self-collision heavy) +// d) the grazing pathological case (kInconclusive cost at the floor) +// e) thread scaling over a 1000-check batch, two ways +// +// Scenarios (a, 1 cm tier) and (b) are additionally compared against Drake's +// own sampled `SceneGraphCollisionChecker` on the *same* RobotDiagram. #include #include @@ -67,18 +54,17 @@ using drake::trajectories::Trajectory; using Eigen::MatrixXd; using Eigen::VectorXd; -/// drake::planning::CollisionCheckerParams::edge_step_size has NO library -/// default: the field is value-initialized to 0 and the CollisionChecker -/// constructor rejects any non-positive value, so every caller must choose -/// one. 0.05 rad is the value that appears most often in Drake's own tests -/// and examples; the other common choices (0.125, 0.1, 0.01) are measured and -/// reported too, so the comparison cannot be accused of picking a flattering -/// resolution. +// drake::planning::CollisionCheckerParams::edge_step_size has NO library +// default: the field is value-initialized to 0 and the CollisionChecker +// constructor rejects any non-positive value, so every caller must choose one. +// 0.05 rad is the value that appears most often in Drake's own tests and +// examples; the other common choices (0.125, 0.1, 0.01) are measured and +// reported too. constexpr double kEdgeStepSize = 0.05; constexpr double kReportedEdgeStepSizes[] = {0.125, 0.1, 0.05, 0.01}; -/// Distances beyond this are irrelevant to every scenario here; the ground -/// truth sampler saturates at it. +// Distances beyond this are irrelevant to every scenario here; the ground +// truth sampler saturates at it. constexpr double kMaxProbeDistance = 0.30; struct Config { @@ -112,20 +98,20 @@ std::string ModeName(SearchMode m) { return m == SearchMode::kCertifyAll ? "kCertifyAll" : "kFindFirstViolation"; } -/// A world plus both checkers built on the *same* RobotDiagram. The sampled -/// checker is constructed first on purpose: its constructor pushes its -/// nominal filtered-collision matrix into the SceneGraph, so building our -/// checker afterwards guarantees the two see a bit-identical unfiltered pair -/// set. Anything else would make the comparison unfair in our favour. +// A world plus both checkers built on the *same* RobotDiagram. The two +// checkers must see the same pair set for the comparison to mean anything, so +// the sampled checker is constructed first: its constructor pushes its nominal +// filtered-collision matrix into the SceneGraph, and building the continuous +// checker afterwards makes the two see a bit-identical unfiltered pair set. struct World { std::shared_ptr> diagram; std::unique_ptr sampled; std::unique_ptr certified; std::unordered_set env_ids; int pair_count{0}; - /// SceneGraph's own unfiltered-candidate count *after* the sampled checker - /// pushed its filters in. Equality with pair_count is the evidence that - /// both checkers are looking at exactly the same pairs. + // SceneGraph's own unfiltered-candidate count *after* the sampled checker + // pushed its filters in. Equality with pair_count is the evidence that + // both checkers are looking at exactly the same pairs. int scene_graph_candidates{0}; }; @@ -208,7 +194,7 @@ void WriteClearance(JsonWriter* json, const ClearanceReport& clearance) { json->EndObject(); } -/// One certification measurement. +// One certification measurement. struct CertRun { Verdict verdict{}; Statistics stats; @@ -254,15 +240,15 @@ void WriteCertRun(JsonWriter* json, const std::string& key, } // --------------------------------------------------------------------------- -// The sampled-checker comparison (the performance requirements, the headline -// number) +// The sampled-checker comparison: the cost of certifying a path against the +// cost of sampling it at the resolutions a practitioner would use. // --------------------------------------------------------------------------- -/// For a curved trajectory a practitioner checks it the only way a sampled -/// checker allows: walk the path and call CheckConfigCollisionFree at the -/// same resolution the checker would use for an edge, i.e. one sample per -/// `edge_step_size` of path length in the plant's edge metric. We report the -/// implied sample count and the wall time of exactly that sweep. +// For a curved trajectory a practitioner checks it the only way a sampled +// checker allows: walk the path and call CheckConfigCollisionFree at the +// same resolution the checker would use for an edge, i.e. one sample per +// `edge_step_size` of path length in the plant's edge metric. We report the +// implied sample count and the wall time of exactly that sweep. void MeasureSampledPathSweep(JsonWriter* json, const SceneGraphCollisionChecker& sampled, const Trajectory& trajectory, int warmup, @@ -341,9 +327,9 @@ void MeasureSampledEdge(JsonWriter* json, // Shared plumbing // --------------------------------------------------------------------------- -/// Places the bookcase so the fixed trajectory's robot-vs-environment swept -/// clearance equals `target` (bisection on the shelf scale, which is monotone -/// non-decreasing over [0.010, 0.090]). +// Places the bookcase so the fixed trajectory's robot-vs-environment swept +// clearance equals `target` (bisection on the shelf scale, which is monotone +// non-decreasing over [0.010, 0.090]). double TuneShelfScale(const Config& config, double target) { const MatrixXd waypoints = ShelfTrajectoryWaypoints(); const auto trajectory = @@ -360,14 +346,14 @@ double TuneShelfScale(const Config& config, double target) { config.tune_iterations); } -/// Repetition policy. Every millisecond-scale measurement gets the full -/// `--reps` after `--warmup` untimed runs. The grazing scenario at the -/// default 1e-9 resolution floor costs tens of seconds per call, where 20 -/// repetitions would blow the suite's time budget for no statistical gain -/// (the relative spread of a 40 s measurement is far below that of a 2 ms -/// one), so expensive cases fall back to a small fixed count. The chosen -/// count is recorded in every timing block, so no result is silently -/// under-sampled. +// Repetition policy. Every millisecond-scale measurement gets the full +// `--reps` after `--warmup` untimed runs. The grazing scenario at the +// default 1e-9 resolution floor costs tens of seconds per call, where 20 +// repetitions would blow the suite's time budget for no statistical gain +// (the relative spread of a 40 s measurement is far below that of a 2 ms +// one), so expensive cases fall back to a small fixed count. The chosen +// count is recorded in every timing block, so no result is silently +// under-sampled. void PlanReps(const Config& config, double single_run_ms, int* warmup, int* reps) { if (single_run_ms > 1000.0) { @@ -379,7 +365,7 @@ void PlanReps(const Config& config, double single_run_ms, int* warmup, } } -/// Times one certification once, untimed, to price the case for PlanReps. +// Times one certification once, untimed, to price the case for PlanReps. double ProbeCost(const ContinuousCollisionChecker& checker, const Trajectory& trajectory, const Options& options) { const auto t0 = std::chrono::steady_clock::now(); @@ -446,8 +432,8 @@ void RunPwlEdge(const Config& config, const MachineInfo& machine, // quintic composite Bezier through the same two waypoints. With two // waypoints that quintic's endpoint velocities are zero, so its control // points collapse to {q1, q1, q1, q2, q2, q2} and it traces exactly the same - // straight joint-space segment under a different time parametrization — - // which is why the clearance it measures is the certified edge's clearance. + // straight joint-space segment under a different time parametrization. That + // is why the clearance it measures is the certified edge's clearance. json.Write("description", "two-waypoint PWL edge in the 1 cm shelf world, healthy " "clearance; certified as a single order-1 Bezier segment, with " @@ -732,8 +718,7 @@ void RunGrazing(const Config& config, const MachineInfo& machine, WriteClearance(&json, clearance); // The resolution floor is the knob that prices the pathological case: cost - // at the floor grows like log2(1 / min_interval) (the soundness argument's - // termination proof). + // at the floor grows like log2(1 / min_interval). constexpr double kFloors[] = {1e-9, 1e-6, 1e-4, 1e-2}; CertRun default_run; json.BeginArray("min_interval_sweep"); @@ -762,7 +747,8 @@ void RunGrazing(const Config& config, const MachineInfo& machine, json.EndArray(); // kFindFirstViolation at the same floor: with no definite violation // anywhere on the trajectory the earliest-witness bound never prunes, so - // this is expected to cost the same as kCertifyAll — measured, not assumed. + // this should cost the same as kCertifyAll. The row below measures that + // rather than assuming it. const Options first_options = MakeOptions(SearchMode::kFindFirstViolation, Parallelism::None()); int first_warmup = 0; @@ -789,8 +775,8 @@ void RunGrazing(const Config& config, const MachineInfo& machine, // --------------------------------------------------------------------------- // (f) performance review: where the time goes, and why per-call parallelism -// saturates. Not a standard scenario — this exists to back the gap analysis -// in the benchmark write-up with measurements rather than assertions. +// saturates. Not one of the standard scenarios; it attributes cost and probes +// parallel granularity by measurement rather than by assertion. // --------------------------------------------------------------------------- void RunProfile(const Config& config, const MachineInfo& machine, @@ -887,11 +873,9 @@ void RunProfile(const Config& config, const MachineInfo& machine, // This measures the ceiling that *segment-root seeding* imposes: if the // parallel driver's only work units are whole segments, the best per-call // speedup a 6-segment trajectory can reach is total work / heaviest segment. - // Certifying each segment on its own measures it directly. The driver no - // longer works that way — it shares sub-segment nodes on demand (see - // certifier_internal.h) — so this row is now a *reference* bound - // that the measured per-call speedup is allowed to exceed, and the record of - // why the old driver could not. + // Certifying each segment on its own measures it directly. The driver shares + // sub-segment nodes on demand (see certifier_internal.h), so this row is a + // *reference* bound that the measured per-call speedup is allowed to exceed. { const PiecewiseBezierPath path = world.certified->Normalize( shelf_trajectory, @@ -944,10 +928,10 @@ void RunProfile(const Config& config, const MachineInfo& machine, // --- Parallel granularity ------------------------------------------------- // Per-call parallelism is measured on three workloads spanning three orders - // of magnitude in node count, holding everything else fixed. The three sit - // on either side of the driver's lazy-recruitment threshold on purpose: the - // 15-node edge is below it (and must therefore be exactly serial at every p) - // while the other two are above it. + // of magnitude in node count, holding everything else fixed. The three + // straddle the driver's lazy-recruitment threshold: the 15-node edge is + // below it (and must therefore be exactly serial at every p) while the other + // two are above it. std::printf( "[f profile] tuning the grazing world for the long " "workload ...\n"); @@ -1004,6 +988,16 @@ void RunProfile(const Config& config, const MachineInfo& machine, std::printf("\n"); } +// Usage: iiwa_benchmark [--out DIR] [--reps N] [--warmup N] +// [--dense-samples N] [--batch N] [--only NAME] +// [--drake_commit SHA] +// +// `--out` defaults to the current directory, or to $TEST_TMPDIR when that is +// set: the sandbox is the only writable directory under `bazel test`, and the +// smoke-test rule in BUILD.bazel relies on this, so it needs no --out of its +// own. `--drake_commit` (the Drake revision this binary was built from, +// "unknown" by default) is recorded verbatim in every result file, so a JSON +// result identifies the code it measured. int Main(int argc, char** argv) { Config config; if (const char* const test_tmpdir = std::getenv("TEST_TMPDIR")) { diff --git a/planning/continuous_collision/benchmark/scenario_worlds.cc b/planning/continuous_collision/benchmark/scenario_worlds.cc index 20d7ce8e8dc6..d0d8a2f0a542 100644 --- a/planning/continuous_collision/benchmark/scenario_worlds.cc +++ b/planning/continuous_collision/benchmark/scenario_worlds.cc @@ -35,7 +35,7 @@ CoulombFriction Friction() { return CoulombFriction(1.0, 1.0); } -/// Adds one anchored box to the "environment" model instance. +// Adds one anchored box to the "environment" model instance. void AddAnchoredBox(MultibodyPlant* plant, const std::string& name, const Vector3d& size, const RigidTransformd& X_WB) { if (!plant->HasModelInstanceNamed("environment")) { @@ -58,10 +58,10 @@ void AddTable(MultibodyPlant* plant) { RigidTransformd(Vector3d(0.0, 0.0, -0.05))); } -/// The bookcase: two side panels, four horizontal boards (bottom, the two -/// bounding the reached-into bay, and top) and a back panel — seven anchored -/// boxes, all in the "environment" instance so they form one welded subgraph -/// with the world and with each other. +// The bookcase: seven anchored boxes, namely two side panels, four horizontal +// boards (bottom, the two bounding the reached-into bay, and top) and a back +// panel. All are in the "environment" instance, so they form one welded +// subgraph with the world and with each other. void AddShelf(MultibodyPlant* plant, double s) { using S = ShelfGeometry; const double x_front = S::kFrontX + s; diff --git a/planning/continuous_collision/benchmark/scenario_worlds.h b/planning/continuous_collision/benchmark/scenario_worlds.h index f3b7ea8cf162..89a561677d4f 100644 --- a/planning/continuous_collision/benchmark/scenario_worlds.h +++ b/planning/continuous_collision/benchmark/scenario_worlds.h @@ -1,10 +1,8 @@ #pragma once -/// @file -/// The fixed, versioned benchmark worlds and trajectories (the benchmark -/// suite). Every world is built from the cached `drake_models` iiwa14 -/// dense-sphere collision model plus programmatic anchored boxes, so a run is -/// reproducible from this file alone. +// The fixed benchmark worlds and trajectories. Every world is built from the +// cached `drake_models` iiwa14 dense-sphere collision model plus programmatic +// anchored boxes, so a run is reproducible from this file alone. #include #include @@ -19,15 +17,12 @@ namespace planning { namespace continuous_collision { namespace internal { -/// The dense-sphere iiwa14 collision variant: 46 collision spheres over -/// links 0-7, i.e. realistic proximity-pair counts (the benchmark suite asks -/// for the realistic model, not the 4-primitive one). +// The dense-sphere iiwa14 collision variant: 46 collision spheres over links +// 0-7, i.e. realistic proximity-pair counts rather than the 4-primitive model. extern const char* const kIiwaUrl; -/// Nominal shelf geometry. `shelf_scale` s translates the whole bookcase by -/// +s in x *and* opens the reached-into bay by s on each side, so the swept -/// clearance of the fixed benchmark trajectory is monotone non-decreasing in -/// s over the useful range. This one scalar is what the tier bisection turns. +// Nominal shelf geometry, in metres: the fixed dimensions of the bookcase that +// MakeShelfWorld builds. struct ShelfGeometry { static constexpr double kBayCentreZ = 0.60; static constexpr double kFrontX = 0.62; @@ -39,31 +34,36 @@ struct ShelfGeometry { static constexpr double kTopZ = 1.25; }; -/// iiwa14 welded to the world origin, a 3 m table slab, and a seven-box -/// bookcase in reach. Model instances are named "iiwa14" and "environment". +// iiwa14 welded to the world origin, a 3 m table slab, and a seven-box +// bookcase in reach. Model instances are named "iiwa14" and "environment". +// `shelf_scale` s translates the whole bookcase by +s in x *and* opens the +// reached-into bay by s on each side, so the swept clearance of the fixed +// benchmark trajectory is monotone non-decreasing in s over the useful range. +// This one scalar is what the tier bisection turns. std::shared_ptr> MakeShelfWorld(double shelf_scale); -/// Two iiwa14s welded to the world `base_separation` apart along +x, the -/// second rotated 180 degrees about z so the arms face each other, over the -/// same table slab. Model instances: "iiwa14", "iiwa14_1", "environment". +// Two iiwa14s welded to the world `base_separation` apart along +x, the +// second rotated 180 degrees about z so the arms face each other, over the +// same table slab. Model instances: "iiwa14", "iiwa14_1", "environment". std::shared_ptr> MakeDualArmWorld(double base_separation); -/// The 7 x 7 joint-space waypoint matrix of the shelf-reaching trajectory: -/// home, up-and-over on the +y side, into the bay mouth, deep inside the bay, -/// out on the -y side, and home. Solved once offline with -/// drake::multibody::InverseKinematics (position + tool-axis + minimum- -/// distance constraints) against the shelf-free world, then frozen here so -/// the benchmark has no solver dependency and no run-to-run drift. +// The 7 x 7 joint-space waypoint matrix of the shelf-reaching trajectory: +// home, up-and-over on the +y side, into the bay mouth, deep inside the bay, +// out on the -y side, and home. Solved once offline with +// drake::multibody::InverseKinematics (position + tool-axis + minimum- +// distance constraints) against the shelf-free world, then frozen here so +// the benchmark has no solver dependency and no run-to-run drift. Eigen::MatrixXd ShelfTrajectoryWaypoints(); -/// Times of the shelf waypoints (0, 1, ..., 6): 6 quintic Bézier segments. +// Times of the shelf waypoints (0, 1, ..., 6): 6 quintic Bézier segments. std::vector ShelfTrajectoryTimes(); -/// The 14 x 5 waypoint matrix of the dual-arm handover: both arms home, half -/// way, at the handover poses (end-effectors passing within a few cm), a -/// slightly different half-way pose on the way back, and home. +// The 14 x 5 waypoint matrix of the dual-arm handover: both arms home, half +// way, at the handover poses (end-effectors passing within a few cm), a +// slightly different half-way pose on the way back, and home. Eigen::MatrixXd DualArmTrajectoryWaypoints(); +// Times of the dual-arm waypoints (0, 1, ..., 4): 4 quintic Bézier segments. std::vector DualArmTrajectoryTimes(); } // namespace internal diff --git a/planning/continuous_collision/bounding_sphere.cc b/planning/continuous_collision/bounding_sphere.cc index bdd13cc354d5..f80aeefaf1c4 100644 --- a/planning/continuous_collision/bounding_sphere.cc +++ b/planning/continuous_collision/bounding_sphere.cc @@ -38,15 +38,14 @@ using drake::math::RigidTransform; 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 (a - sound relaxation of the geometry-support scope's exact per-shape R_g, by the - triangle inequality); the tighter centre is what the broadphase prefilter + 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. - λ soundness dies quietly if any formula under-bounds, 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 (the geometry-support scope). */ + 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) @@ -76,15 +75,14 @@ class BoundingSphereReifier final : public ShapeReifier { } void ImplementGeometry(const Cylinder& cylinder, void*) final { - // The farthest point from Go is always on a rim (the geometry-support - // scope). For a point p = z·ẑ + r'·û with |z| ≤ L/2, r' ≤ r and û ⊥ ẑ, + // 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. (The same rim argument in the geometry-support - // scope's origin-centred form picks up the ‖t‖ cross terms; here the centre - // rides along with the geometry, so only the canonical-frame extent - // matters.) + // 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())); } @@ -102,7 +100,7 @@ class BoundingSphereReifier final : public ShapeReifier { 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 (the geometry-support scope). + // actually checked. SetFromHull(mesh.GetConvexHull()); } @@ -112,11 +110,10 @@ class BoundingSphereReifier final : public ShapeReifier { "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 the dedicated rules in the " - "geometry-support scope " - "(anchored, or translation-only relative motion to its partner); any " - "other shape must be replaced by a Convex/Mesh approximation before " - "it can be certified.", + "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)); } @@ -164,9 +161,8 @@ BoundingSphere ComputeBoundingSphere(const Shape& shape, BoundingSphereReifier reifier(X_LG); shape.Reify(&reifier); const BoundingSphere& result = reifier.sphere(); - // A silently-zero or non-finite radius is the exact failure mode the - // geometry-support scope warns about, so re-assert the postcondition every - // caller relies on. + // 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; diff --git a/planning/continuous_collision/bounding_sphere.h b/planning/continuous_collision/bounding_sphere.h index c2cd4007c37c..0d2bcacaa890 100644 --- a/planning/continuous_collision/bounding_sphere.h +++ b/planning/continuous_collision/bounding_sphere.h @@ -19,16 +19,15 @@ struct BoundingSphere { }; /** Computes a bounding sphere, in the body frame, of shape `shape` posed at -X_LG in the body frame (the geometry-support scope). +X_LG in the body frame. -The sphere is centered at the shape's natural center (tighter for the -broadphase prefilter than the white paper's origin-centered radius R_g; 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: +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. + - 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). @@ -38,10 +37,11 @@ containment per shape: engine's hull bakes in scale and degeneracy inflation, and the radius must bound the geometry actually checked. -λ soundness dies quietly if any formula under-bounds, so this function -switches on the closed set of supported shape types and -@throws std::exception on anything else (HalfSpace included — halfspaces are -handled by dedicated rules, never through a bounding sphere). +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. @ingroup planning_collision_checker */ BoundingSphere ComputeBoundingSphere(const geometry::Shape& shape, const math::RigidTransform& X_LG); diff --git a/planning/continuous_collision/certificate.cc b/planning/continuous_collision/certificate.cc index bd1253dad60f..a34ca886be45 100644 --- a/planning/continuous_collision/certificate.cc +++ b/planning/continuous_collision/certificate.cc @@ -21,11 +21,11 @@ namespace internal { namespace { /* Slop allowed between the certifier's arithmetic and the replay's. The two - compute the same quantities by *different* routes — repeated halving versus a - pair of arbitrary-u de Casteljau subdivisions — so they agree only to rounding - (both routes are sequences of convex combinations, hence numerically benign, - and in practice differ by ~1e-15·‖q‖). This tolerance sits far below anything - a tamperer could hide in and far above the true rounding gap. */ + compute the same quantities by *different* routes, repeated halving versus a + pair of arbitrary-u de Casteljau subdivisions, so they agree only to rounding. + Both routes are sequences of convex combinations, hence numerically benign. + This tolerance sits far below anything a tamperer could hide in and far above + the rounding gap. */ constexpr double kReplayTolerance = 1e-9; /* One de Casteljau subdivision at u ∈ [0, 1]: `left` receives the control @@ -193,8 +193,8 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, // carve-out slack for any w: the residual of the coordinates the carve-out // removed, which is nonzero only when some of them are constant merely to // within Options::continuity_tolerance. Charging it here keeps the replay's - // Δ at least as large as the certifier's — a certificate emitted against a - // slack-inflated bound must not verify against a smaller one. + // Δ at least as large as the certifier's: a certificate emitted against + // a slack-inflated bound must not verify against a smaller one. double motion_bound = table.carveout_slack(p); if (!is_static) { // Re-restrict the segment's control points to the record's interval and @@ -243,12 +243,11 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, // record: the recomputed Δ above would be the full node's bound and the // test below would reject it. // - // "Static" is relative to the constant-coordinate carve-out (trajectory - // normalization; the displacement lemma), so coordinates this path happens - // to hold fixed still move the pair in general — which means the - // representative configuration has to be pinned to the path, exactly as the - // certifier pins it (q(t0)), or a record could be re-based onto an off-path - // configuration that measures more clearance. + // "Static" is relative to the constant-coordinate carve-out, so + // coordinates this path happens to hold fixed still move the pair in + // general. The representative configuration therefore has to be pinned to + // the path, exactly as the certifier pins it (q(t0)), or a record could be + // re-based onto an off-path configuration that measures more clearance. if (is_static) { const Eigen::VectorXd q0 = path.segments()[0].control_points.col(0); const double qc_error = (q0 - record.qc).cwiseAbs().maxCoeff(); @@ -274,7 +273,7 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, input.oracle->SignedDistance(context.query_object(), pairs[p]); const double tau_p = tau[p]; // Coherence: a record may legitimately store *less* than the narrowphase - // reports (the sphere-prefilter branch stores a lower bound on φ), but it + // reports (the sphere-prefilter branch stores a lower bound on ϕ), but it // may never claim more than the oracle's own contract allows. if (!(record.phi_hat <= phi_replay + tau_p + kReplayTolerance)) { return fail(fmt::format( @@ -283,9 +282,9 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, r, record.phi_hat, phi_replay, p)); } // The certificate test runs on min(stored, re-measured), so an inflated - // φ̂ can never buy a record anything: only the value this replay measured + // ϕ̂ can never buy a record anything: only the value this replay measured // for itself can carry the inequality. Both are lower bounds we are - // entitled to charge τ_p against, and for an honest record the stored + // entitled to charge τ_p against, and for an untampered record the stored // value is the smaller one (identical for a narrowphase record, the // sphere bound for a prefilter record), so nothing legitimate is lost. const double effective_phi = std::min(record.phi_hat, phi_replay); @@ -333,7 +332,7 @@ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, double covered_to = 0.0; for (std::size_t i = begin; i < cursor; ++i) { // Sorted by lo, so a start beyond the covered prefix is a real gap. - // Compared exactly and deliberately: the certifier's intervals are + // Compared exactly: the certifier's intervals are // dyadic and abut bit-for-bit (a child's endpoint *is* the parent's // computed midpoint), so any slack here would only buy a forged // certificate the right to excise a sliver at every one of its diff --git a/planning/continuous_collision/certificate.h b/planning/continuous_collision/certificate.h index 248aee26b05c..b79d7d6ec2ea 100644 --- a/planning/continuous_collision/certificate.h +++ b/planning/continuous_collision/certificate.h @@ -12,7 +12,7 @@ namespace continuous_collision { /** One certification event: pair `pair_index` was certified over the parameter interval [s_start, s_end] of segment `segment` from representative -configuration qc (the search algorithm). +configuration qc. @ingroup planning_collision_checker */ struct CertificateRecord { int segment{}; diff --git a/planning/continuous_collision/certifier_internal.cc b/planning/continuous_collision/certifier_internal.cc index ff76a6b4f625..146d58110cf4 100644 --- a/planning/continuous_collision/certifier_internal.cc +++ b/planning/continuous_collision/certifier_internal.cc @@ -33,7 +33,7 @@ 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 certificate is - invariant under time reparametrization (the soundness argument, T6). */ + invariant under time reparametrization. */ double TimeOf(const BezierSegment& seg, double s) { return seg.t_start + s * (seg.t_end - seg.t_start); } @@ -42,10 +42,10 @@ double TimeOf(const BezierSegment& seg, double s) { // Per-node world-frame geometry sphere centers. // --------------------------------------------------------------------------- -/* Caches one world-frame bounding-sphere center per geometry per node - (requirement P2: 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). */ +/* 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) @@ -95,9 +95,9 @@ class FindingSink { std::lock_guard guard(mutex_); Insert(&definite_, std::move(finding)); } - // Branch-and-bound bound for kFindFirstViolation (the search algorithm; - // parallelism and determinism): workers skip nodes whose interval starts at - // or after the earliest witness known so far. The bound decreases + // Branch-and-bound bound for kFindFirstViolation: 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); @@ -164,7 +164,7 @@ class FindingSink { }; // --------------------------------------------------------------------------- -// Shared work source for the parallel driver (parallelism and determinism). +// Shared work source for the parallel driver. // --------------------------------------------------------------------------- /* One unit of shared work: a node, self-contained so a worker can pick it up @@ -172,8 +172,8 @@ class FindingSink { 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. Requirement P1 (no per-node heap allocation) survives that - because the queue recycles item *shells*: a popped shell goes back on a free + 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. */ @@ -189,8 +189,8 @@ struct WorkItem { /* 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 / violation bound (parallelism and determinism), which is - what makes the driver TSan-clean by construction. */ + atomic node counter / violation bound, 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 @@ -293,22 +293,16 @@ struct Recruitment { /* How many nodes a run must have visited before it hires helpers. - This is a measured break-even, not a taste knob. 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 before the lead - can collect their statistics. Thread creation dominates that list at tens of - microseconds per worker, which is where this threshold parts company with the - parked-thread pool it replaced: waking a parked thread cost ~6-7 us, so paying - for a fresh one instead moves the break-even up by roughly 4x, from 16 nodes - to 64. A node on the machine the benchmark suite was measured on costs - ~7-13 us, so 64 nodes of work already done is again roughly a 3x margin over - the price of a full fifteen helpers, and it bounds the damage in the one case - lazy recruitment cannot avoid — a check that ends immediately after hiring — - to a few hundred microseconds. - - Everything smaller than this runs at exactly serial speed at any - Options::parallelism, which is the property that matters most in practice - because Parallelism::Max() is the default value of that field. */ + 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 before the lead can collect their statistics. Thread creation + dominates that list at tens of microseconds per worker, while a node costs + ~7-13 us on the machine the benchmark suite was measured on, 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; // --------------------------------------------------------------------------- @@ -327,7 +321,7 @@ struct NodeFrame { }; /* A worker owns all per-thread scratch of the node recursion; nothing in the - steady-state loop allocates (requirement P1): + 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 @@ -398,7 +392,7 @@ class Worker { } } - /* Appends one certification event to the audit trail (the search algorithm). + /* Appends one certification event to the audit trail. */ void RecordCertification(int segment, double s_lo, double s_hi, int pair, double phi_hat, double motion_bound, @@ -466,16 +460,15 @@ void Worker::RunItem(WorkItem* item) { stack_.pop_back(); const int k = static_cast(stack_.size()); - // Branch-and-bound on time (the search algorithm; parallelism and - // determinism): a node starting at or after the earliest witness known so - // far cannot contain an earlier one. + // Branch-and-bound on time: a node starting at or after the earliest + // witness known so far cannot contain an earlier one. if (find_first_ && TimeOf(segment, frame.s_lo) >= sink_->best_violation_time()) { continue; } if (node_counter_->fetch_add(1, std::memory_order_relaxed) >= max_nodes) { // Budget exhausted: stop here and report the earliest node this worker - // leaves uncovered — which is exactly this one, because a left-first DFS + // leaves uncovered, which is exactly this one, because a left-first DFS // pops in increasing parameter order and every frame still on the stack // starts at or after this node's end. sink_->ReportPending( @@ -504,14 +497,13 @@ void Worker::RunItem(WorkItem* item) { // 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 (trajectory normalization; the search - // algorithm). + // 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 - // (the interval certificate). + w_.setZero(); for (int j = 0; j < cols; ++j) { for (int i = 0; i < rows; ++i) { @@ -519,7 +511,7 @@ void Worker::RunItem(WorkItem* item) { } } - // One FK per node (requirement P2); body poses and the query object are + // 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(); @@ -544,13 +536,13 @@ void Worker::RunItem(WorkItem* item) { const PairRecord& pair = pairs[p]; const double threshold = pair.threshold; const double tau_p = tau[p]; - // Δ_p(ν) = Σ_{j ∈ J(p)} λ(j,p)·w_j — a sparse dot product over this - // pair's CSR row (requirement P3). + // Δ_p(ν) = Σ_{j ∈ J(p)} λ(j,p)·w_j, a sparse dot product over this + // pair's CSR row. const double motion_bound = table.MotionBound(p, w_); - // --- Early-out 1: the free-sphere prefilter (requirements P4, P5). --- - // φ_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 + // --- 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 @@ -579,9 +571,8 @@ void Worker::RunItem(WorkItem* item) { if (IsDefiniteViolation(phi_hat, tau_p, threshold)) { // qc is exactly on the trajectory (it is the de Casteljau apex), so - // φ_true(qc) ≤ φ̂ + τ_p < m_p is a definite violation of the - // continuum statement, not a sampling artifact (the problem statement; - // the interval certificate). + // ϕ_true(qc) ≤ ϕ̂ + τ_p < m_p is a definite violation of the + // continuum statement, not a sampling artifact. Finding finding; finding.time = t_mid; finding.q = q_mid_; @@ -606,11 +597,12 @@ void Worker::RunItem(WorkItem* item) { // can refine the witness toward the earliest violating time. } else if (IsCertified(phi_hat, tau_p, motion_bound, threshold, slack)) { // 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_p + ε, + // ϕ_p(q(s)) ≥ ϕ_true(qc) − Σ_{j∈J(p)} λ(j,p)·|q_j(s) − qc_j| + // ≥ (ϕ̂ − τ_p) − Δ_p(ν) > m_p + ε, // 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 — the dominant work saver. + // pair drops out of the entire subtree, which is the dominant work + // saver. if (emit_certificate_) { RecordCertification(item->segment, frame.s_lo, frame.s_hi, p, phi_hat, motion_bound, threshold); @@ -649,7 +641,7 @@ void Worker::RunItem(WorkItem* item) { // 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 + // 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; @@ -665,8 +657,8 @@ void Worker::RunItem(WorkItem* item) { 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 (the search algorithm). + // 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. } @@ -674,7 +666,7 @@ void Worker::RunItem(WorkItem* item) { } // --------------------------------------------------------------------------- -// Breakpoint pre-pass and static-pair resolution (the search algorithm, steps 1 +// Breakpoint pre-pass and static-pair resolution (steps 1 // and 2). // --------------------------------------------------------------------------- @@ -711,8 +703,8 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, 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 — "static" then means genuinely - // immobile — but a pair whose whole J_topo(p) was carved out on a + // 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. @@ -747,7 +739,7 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, continue; } } else if (lower_bound >= threshold) { - // A definite violation needs φ̂ + τ_p < m_p, and φ̂ ≥ φ_true − τ_p ≥ + // A definite violation needs ϕ̂ + τ_p < m_p, and ϕ̂ ≥ ϕ_true − τ_p ≥ // lower_bound − τ_p, so lower_bound ≥ m_p rules one out with no query. continue; } @@ -797,7 +789,7 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, } /* Orders the audit trail so that a run is comparable across thread counts and - across time reparametrizations (T6). */ + across time reparametrizations. */ void SortRecords(std::vector* records) { std::sort(records->begin(), records->end(), [](const CertificateRecord& a, const CertificateRecord& b) { @@ -951,7 +943,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { // 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. + // 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)) @@ -987,7 +979,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { if (have_work && num_threads <= 1) { // Serial: one worker, one local stack, no shared queue and no thread - // interleaving ⇒ bit-deterministic results and stats (requirement P7). + // interleaving => bit-deterministic results and stats. Worker worker(input, &lease[0], &sink, &node_counter, nullptr, nullptr); for (int k = 0; k < num_segments; ++k) { WorkItem item; @@ -1000,8 +992,8 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { accumulate(&worker); } else if (have_work) { // Parallel driver: lazy recruitment + occupancy-driven sharing. The full - // rationale — and the deviation from parallelism and determinism's static - // seeding — is documented on RunCertifier() in certifier_internal.h. + // rationale, and why static seeding is not used, is documented on + // RunCertifier() in certifier_internal.h. WorkQueue queue; { // Seeded in reverse so the LIFO hands segment 0 out first. Before any @@ -1021,8 +1013,8 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { } // 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 + // 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; @@ -1032,8 +1024,8 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { 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, + // 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; @@ -1044,7 +1036,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { // 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 (requirement P1 covers the steady state, not this). + // loop is turning; only the steady state is allocation-free. recruitment.hire = [&]() { const int hired = num_threads - 1; if (hired <= 0) return; @@ -1108,7 +1100,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { options.mode == SearchMode::kFindFirstViolation) { // The branch-and-bound recursion refines toward the earliest witness and // the sink keeps entries earliest-first, so this *is* the earliest witness - // the run found — identical serially and in parallel. + // the run found, identical serially and in parallel. findings.push_back(sink.definite().front()); } else { findings = sink.definite(); @@ -1118,7 +1110,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { if (budget_exhausted && sink.pending_valid()) { // Report what the budget left uncovered as a non-definite finding at the - // earliest uncovered time (the search algorithm: truncate in parameter + // earliest uncovered time (truncate in parameter // order, report the remainder). Finding finding; finding.time = sink.pending_time(); diff --git a/planning/continuous_collision/certifier_internal.h b/planning/continuous_collision/certifier_internal.h index c9d175950dd5..4379d90d1d2b 100644 --- a/planning/continuous_collision/certifier_internal.h +++ b/planning/continuous_collision/certifier_internal.h @@ -1,15 +1,9 @@ #pragma once -/// @file -/// Internal driver of the adaptive interval certifier (the search algorithm) -/// and of the independent certificate replay (the search algorithm, -/// "certificate audit trail"). -/// -/// Nothing in this header is part of the public API; it exists so the facade -/// (`continuous_collision_checker.cc`), the certificate replay -/// (`certificate.cc`) and the node loop (`certifier_internal.cc`) can share -/// one set of per-call data structures without the core module depending on -/// the api layer. +// Internal driver of the adaptive interval certifier and of the independent +// certificate replay. Nothing here is part of the public API; it exists so +// that continuous_collision_checker.cc, certificate.cc and +// certifier_internal.cc can share one set of per-call data structures. #include #include @@ -37,28 +31,27 @@ namespace planning { namespace continuous_collision { namespace internal { -/** One thread's view of the model: a root diagram context plus the plant and +/* One thread's view of the model: a root diagram context plus the plant and scene-graph sub-contexts pulled out of it once, so the hot loop pays a single -`SetPositions` per node (the performance requirements, P2) and no context -bookkeeping. */ +`SetPositions` per node and no context bookkeeping. */ class ThreadContext { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ThreadContext); - /** Allocates a root context of `model`. `model` is aliased and must outlive + /* Allocates a root context of `model`. `model` is aliased and must outlive this object. */ explicit ThreadContext(const RobotDiagram& model); - /** The one FK trigger per node: sets the plant's generalized positions. + /* 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. */ + /* The scene graph's query object at the configuration last set. */ const geometry::QueryObject& query_object() const; - /** World pose of `body` at the configuration last set (Drake's cache + /* 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; @@ -70,8 +63,8 @@ class ThreadContext { const systems::Context* scene_graph_context_{}; }; -/** A checkout pool of ThreadContexts (parallelism and determinism: -"construction allocates `parallelism.num_threads()` RobotDiagram contexts"). +/* 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: @@ -82,11 +75,11 @@ class ContextPool { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ContextPool); - /** Pre-warms `initial_size` contexts of `model`, which is aliased and must + /* 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. */ + /* RAII handle for a set of leased contexts. */ class Lease { public: Lease(const Lease&) = delete; @@ -114,10 +107,10 @@ class ContextPool { std::vector slots_; }; - /** Leases exactly `count` contexts, growing the pool if it is exhausted. */ + /* Leases exactly `count` contexts, growing the pool if it is exhausted. */ Lease Acquire(int count) const; - /** Number of contexts currently held by the pool (for tests/diagnostics). */ + /* Number of contexts currently held by the pool (for tests/diagnostics). */ int size() const; private: @@ -131,56 +124,55 @@ class ContextPool { mutable std::vector in_use_; }; -/** Per-pair broadphase data for the free-sphere prefilter (the interval -certificate): 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. -*/ +/* 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 + /* 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 + /* 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 certification run needs; assembled by the facade. All +/* Everything one certification run needs; assembled by the facade. All pointers are aliased and must outlive the call. */ struct CertifierInput { const RobotDiagram* model{}; const DistanceOracle* oracle{}; const MotionBoundTable* table{}; const PiecewiseBezierPath* path{}; - /** Pair records with `threshold` = margin + padding resolved for this call. + /* Pair records with `threshold` = margin + padding resolved for this call. Indexed consistently with `table`, `tau` and `prefilter`. */ const std::vector* pairs{}; - /** Per-pair oracle tolerance τ_p (a refinement of the numerical policy; see - the table in the facade). */ + /* Per-pair oracle tolerance τ_p; see the accuracy table in + continuous_collision_checker.cc. */ const std::vector* tau{}; const PrefilterTable* prefilter{}; Options options; }; -/** Result of one run, converted to a CertificationResult by the facade. */ +/* Result of one run, converted to a CertificationResult by the facade. */ struct CertifierOutput { Verdict verdict{Verdict::kCertifiedFree}; - /** Earliest-first, capped at Options::max_reported_findings. */ + /* Earliest-first, capped at Options::max_reported_findings. */ std::vector findings; Statistics stats; - /** Filled iff Options::emit_certificate; records are sorted by + /* Filled iff Options::emit_certificate; records are sorted by (segment, s_start, pair_index) so a run is comparable across thread counts and across time reparametrizations. - A *complete* audit trail — one whose certified intervals cover the whole - domain for every pair, which is what ReplayCertificate() demands — is - produced only by a run that ends Verdict::kCertifiedFree. A run that found a + Only a run that ends Verdict::kCertifiedFree produces a *complete* audit + trail, i.e. one whose certified intervals cover the whole domain for every + pair, which is what ReplayCertificate() demands. A run that found a violation, hit the resolution floor, exhausted its budget, or pruned the search (kFindFirstViolation) leaves the uncertified parts uncovered by construction; its records are still individually valid, but they do not @@ -188,65 +180,46 @@ struct CertifierOutput { Certificate certificate; }; -/** Runs the breakpoint pre-pass, the static-pair resolution and the adaptive -node recursion of the search algorithm over every segment of `input.path`, -serially or in parallel according to `input.options.parallelism`. - -

Parallel driver (supersedes the white paper's seeding sketch)

- -The white paper sketches "a work-stealing deque of nodes (seeded with all -segments' roots, or the top few bisection levels for small segment counts)". -That *static* seeding is what the first implementation did, and it does not -work: the certifier's trees are wildly unbalanced (a grazing trajectory -concentrates all of its subdivision in a band a few 10⁻³ wide in segment -parameter), so whatever fixed set of seeds is cut, one of them holds essentially -the whole tree. Measured: 0.98× at 16 threads on a 12.5k-node workload. Three -policies replace it; those *contracts* (per-thread contexts, one atomic -earliest- violation bound, findings sink under a mutex, only those three shared) -are unchanged. - -- **Sharing policy — occupancy-driven, not depth-driven.** There is one shared - LIFO work source. A worker that has just split a node consults the queue's - length: if it is below the number of live workers, the worker pushes its - *right* child there and keeps the left one on its local stack; otherwise it - keeps both. Sharing is therefore self-throttling (a saturated queue costs - nothing) and, crucially, does not stop at any depth — a worker sitting 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 kFindFirstViolation's bound tighten - early. -- **Recruitment policy — lazy.** The call starts as a plain serial descent on - the calling thread with sharing disabled, and hires helpers only after it has - visited `kNodesBeforeHiringHelpers` nodes. A check whose whole workload is - smaller than that (a PWL edge, a shallow shelf check) therefore runs at - exactly serial speed no matter what `Options::parallelism` says — which - matters because `Parallelism::Max()` is the default. Helpers are call-scoped - threads, spawned once per check when (and only when) that threshold is - crossed and joined before the call returns; nothing here owns a background - thread between calls. -- **Determinism policy — unchanged, because sharing moves nodes between - workers without changing which nodes exist.** Every node's decisions depend - only on its own control points and its inherited active set, so the tree, the - statistics summed over workers, and the findings are identical serially and - at any thread count in kCertifyAll. The two documented order-dependent - features are untouched: kFindFirstViolation's branch-and-bound (which prunes - only nodes starting at or after a witness already found, so the *reported* - witness is invariant while the statistics are not) and the `max_nodes` budget - (which truncates at a thread-count dependent place). - -Determinism (performance requirement P7; parallelism and -determinism): the serial path is bit-deterministic. In -kFindFirstViolation the *reported witness* is identical serially and at any -thread count — the branch-and-bound bound only ever prunes nodes that start at -or after a witness already found, so no node that could hold an earlier one is -ever skipped — while the statistics are not. Two documented exceptions to the -witness claim: a run that exhausts `max_nodes` truncates at a thread-count -dependent place, and 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. - -`pool` supplies the per-thread contexts. Helper threads, if any are hired, are -created and joined within this call. +/* 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. + +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, 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 +kFindFirstViolation's 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 tree, the statistics summed over +workers, and the findings are identical serially and at any thread count in +kCertifyAll. In kFindFirstViolation the *reported witness* is identical too, +because the bound only ever prunes nodes that start at or after a witness +already found; the statistics are not. Two exceptions: a run that exhausts +`max_nodes` truncates at a thread-count dependent place, and 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. */ @@ -256,7 +229,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool); // Certificate assembly + independent replay (implemented in certificate.cc). // --------------------------------------------------------------------------- -/** Restricts the Bézier control points `cps` (n × (m+1)) of a segment to the +/* Restricts the Bézier control points `cps` (n × (m+1)) of a segment to the sub-interval [a, b] ⊆ [0, 1] by two de Casteljau subdivisions, writing the n × (m+1) control points of the restricted curve into `out`. @@ -266,11 +239,11 @@ be an independent check. */ void RestrictBezier(const Eigen::MatrixXd& cps, double a, double b, Eigen::MatrixXd* out); -/** Evaluates the Bézier curve with control points `cps` at u ∈ [0, 1] by de +/* Evaluates the Bézier curve with control points `cps` at u ∈ [0, 1] by de Casteljau (the apex of the triangle). Cold path. */ Eigen::VectorXd EvaluateBezier(const Eigen::MatrixXd& cps, double u); -/** Inputs of the independent certificate replay. All pointers are aliased. */ +/* Inputs of the independent certificate replay. All pointers are aliased. */ struct ReplayInput { const RobotDiagram* model{}; const DistanceOracle* oracle{}; @@ -281,11 +254,11 @@ struct ReplayInput { double slack{1e-9}; }; -/** Independently re-evaluates every record of `certificate` and checks that -the certified intervals cover the whole domain for every pair (the search -algorithm). Returns true iff the certificate is a complete, self-consistent -proof that every pair stays above its recorded threshold everywhere on the path. -When it returns false and `message` is non-null, `*message` explains why. */ +/* Independently re-evaluates every record of `certificate` and checks that +the certified intervals cover the whole domain for every pair. Returns true iff +the certificate is a complete, self-consistent proof that every pair stays +above its recorded threshold everywhere on the path. When it returns false and +`message` is non-null, `*message` explains why. */ bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, std::string* message); diff --git a/planning/continuous_collision/continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc index 4f823a945e85..5259aa764aa4 100644 --- a/planning/continuous_collision/continuous_collision_checker.cc +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -1,23 +1,3 @@ -/// @file -/// The public facade (the architecture). It owns the construction-time analysis -/// — kinematics engine, distance oracle and capability probe, per-pair padding, -/// per-pair oracle tolerances, the broadphase sphere table and the per-thread -/// context pool — assembles the per-call inputs of the node-loop driver in -/// certifier.{h,cc}, and hosts the independent certificate replay entry -/// point. -/// -/// The guarantee this file implements, stated verbatim as in the header: -/// -/// 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 margin + padding(A, B) — -/// under the stated assumptions: exact real arithmetic up to the configured -/// numerical slack, a distance oracle accurate to its stated tolerance, and -/// the geometry semantics of the geometry-support scope (Mesh ≡ convex hull). -/// This is a statement about the continuum of configurations, not about -/// samples. The certificate is a property of the path, so retiming the -/// trajectory afterwards does not invalidate it. - #include "drake/planning/continuous_collision/continuous_collision_checker.h" #include @@ -51,54 +31,36 @@ using drake::multibody::BodyIndex; using drake::planning::RobotDiagram; // --------------------------------------------------------------------------- -// Per-pair oracle tolerance τ_p (a deliberate refinement of the numerical -// policy's uniform τ policy). +// Per-pair oracle tolerance τ_p. // --------------------------------------------------------------------------- // -// The numerical policy charges every pair one global τ = -// Options::query_tolerance (default 1e-6 m). Drake's *documented* accuracy for -// QueryObject::ComputeSignedDistancePairClosestPoints() is worse than that for -// several shape combinations — up to 5e-5 m for Cylinder-Ellipsoid — because -// those combinations run an iterative GJK/EPA-style solver with a hard-coded -// iteration limit. -// -// Trusting the oracle to 1e-6 m where Drake only promises 5e-5 m is exactly -// the one failure mode that can fake a certificate: the soundness argument -// shows that oracle misbehaviour *below* the threshold cannot produce a false -// "free", but over-reporting a distance at or above the threshold can. So the -// checker uses -// -// τ_p = max(Options::query_tolerance, documented_accuracy(shape_a, -// shape_b)) -// -// everywhere the white paper says τ — in the node certificate test, in the -// definite violation test, at breakpoints and in the certificate replay. Pairs -// routed through the analytic halfspace fallback are exact (closed-form support -// functions, the geometry-support scope), so they carry τ_p = -// Options::query_tolerance and nothing more. -// -// The table below is transcribed from Table 4 of -// drake/geometry/query_object.h ("Worst observed error (in m) for 2mm -// penetration/separation between geometries approximately 20cm in size" for -// T = double) in the pinned Drake (~v1.45). Mesh is certified as its convex -// hull, so its row/column duplicates Convex's, exactly as the Drake table's -// footnote states. Anything the checker cannot classify is charged the worst -// documented value. +// Drake documents ComputeSignedDistancePairClosestPoints() accuracy as bad as +// 5e-5 m for some shape pairs, well outside the 1e-6 m default of +// Options::query_tolerance, 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, certificate replay) +// therefore takes τ_p = max(Options::query_tolerance, +// documented_accuracy(shape_a, shape_b)); pairs routed through the analytic +// halfspace fallback are closed-form and keep the raw +// Options::query_tolerance. // -// | | Box | Capsule | Convex | Cylinder | Ellipsoid | Mesh | -// Sphere | | 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 | (= Convex row) | -// 3e-15 | | | Sphere | 3e-15 | 6e-15 | 3e-6 | 5e-15 | 4e-5 -// | 3e-6 | 6e-15 | +// 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. // -// If the pinned Drake ever tightens these numbers the table may be relaxed; -// it must never be relaxed ahead of Drake's own documentation. - -/** The closed set of shape classes the τ_p table knows. */ +// 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 | 3e-15 | 6e-15 | 3e-6 | 5e-15 | 4e-5 | 3e-6 | 6e-15 | +// clang-format on + +/* The closed set of shape classes the τ_p table knows. */ enum class ShapeClass { kSphere = 0, kBox = 1, @@ -112,10 +74,10 @@ enum class ShapeClass { }; constexpr int kNumShapeClasses = 9; -/** Worst documented error over the whole table; charged to any shape the -checker cannot classify (it never reaches the narrowphase anyway — the -capability probe refuses unknown shapes at construction — but the default must -be the conservative one). */ +/* 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; ShapeClass Classify(const drake::geometry::Shape& shape) { @@ -185,21 +147,20 @@ const AccuracyTable& DocumentedAccuracyTable() { 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 - // in this library anyway — the capability probe routes every one of them - // through the analytic support-function fallback, which is exact, and - // ComputeTauTable() below never consults this table for a non-native route - // — so these entries are belt-and-braces. They are filled with the - // documented value where there is one and with the worst documented value - // otherwise, so that a future routing change cannot silently inherit a - // τ of zero. + // 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`, given the call's query tolerance. */ +/* τ_p for every pair of `pairs`, given the call's query tolerance. */ std::vector ComputeTauTable(const RobotDiagram& model, const std::vector& pairs, double query_tolerance) { @@ -223,22 +184,13 @@ std::vector ComputeTauTable(const RobotDiagram& model, // PaddingSpec mirrors drake::planning::CollisionChecker: a pair's effective // threshold is m_p = margin + padding(p), where padding comes from the dense // per-body-pair matrix when one is supplied and otherwise from the {env, self} -// scalars. -// -// Environment-vs-self rule used here (documented as required): a body is -// *anchored* iff no position coordinate of the plant changes its pose relative -// to the world — that is, iff KinematicsEngine::CoordinatesAffectingPair(world, -// body) is empty, which covers the world body itself and everything welded -// (directly or transitively) to it. A pair is then -// -// - self iff BOTH bodies are non-anchored (a robot-vs-robot pair), and -// - env otherwise (at least one side is the world or rigidly attached to -// it). -// -// The rule is pure topology, so a pair's padding never depends on which -// trajectory is being checked; in particular the constant-coordinate carve-out -// of trajectory normalization (which can make a *moving* body behave as if -// welded for one trajectory) deliberately does not enter here. +// scalars. A pair is self iff both bodies are non-anchored and env otherwise, +// where a body is anchored iff KinematicsEngine::CoordinatesAffectingPair( +// world, body) is empty, which covers the world body and everything welded to +// it, directly or transitively. The rule is pure topology, so padding never +// depends on which trajectory is being checked; in particular the +// constant-coordinate carve-out, which can make a moving body behave as if +// welded for one trajectory, does not enter here. std::vector ComputePaddingTable(const KinematicsEngine& engine, const std::vector& pairs, @@ -303,9 +255,9 @@ internal::PrefilterTable ComputePrefilterTable( std::unordered_map slot_of; const auto slot = [&](GeometryId id, BodyIndex body) { - // HalfSpace has no bounding sphere (the geometry-support scope): such pairs - // skip the prefilter entirely and go straight to the analytic oracle route, - // which is cheap anyway. + // HalfSpace has no bounding sphere, so such pairs skip the prefilter + // entirely and go straight to the analytic oracle route, which is cheap + // anyway. if (Classify(inspector.GetShape(id)) == ShapeClass::kHalfSpace) return -1; const auto it = slot_of.find(id); if (it != slot_of.end()) return it->second; @@ -476,9 +428,9 @@ class ContinuousCollisionChecker::Impl { DistanceOracle oracle_; std::vector pairs_; std::vector pair_ids_; - /** padding(p) alone; the margin is added per call. */ + /* padding(p) alone; the margin is added per call. */ std::vector padding_; - /** Drake's documented accuracy per pair; τ_p = max(query_tolerance, this). */ + /* Drake's documented accuracy per pair; τ_p = max(query_tolerance, this). */ std::vector tau_base_; internal::PrefilterTable prefilter_; mutable internal::ContextPool pool_; @@ -583,23 +535,17 @@ const RobotDiagram& ContinuousCollisionChecker::model() const { // VerifyCertificate. // --------------------------------------------------------------------------- // -// Deliberately written against the checker's *public* introspection seams -// only: it re-derives the λ table from the path, re-restricts every record's -// control points with its own de Casteljau code, recomputes w about the -// record's qc, re-queries the oracle at qc from a fresh context, and re-checks -// the interval-certificate inequality with τ_p. It then verifies that the +// Written against the checker's *public* introspection seams only: it +// re-derives the λ table from the path, re-restricts every record's control +// points with its own de Casteljau code, recomputes w about the record's qc, +// re-queries the oracle at qc from a fresh context, and re-checks the +// interval-certificate inequality with τ_p. It then verifies that the // certified intervals cover [0, 1] of every segment for every pair. Nothing of // the certifier's own bookkeeping is trusted. // // The replay charges the checker's construction-time query tolerance and the -// documented Options::certificate_slack default; a run made with a *larger* -// slack (a stricter certificate) therefore still verifies. -// -// It returns true only for a *complete* proof. A certificate from a run that -// found a violation, ended inconclusive, exhausted its node budget, or pruned -// the search (kFindFirstViolation) necessarily leaves part of the domain -// uncovered, and the coverage check reports that as a failure — which is the -// correct answer to "does this certificate prove the path is free?". +// Options::certificate_slack default, so a run made with a *larger* slack, and +// hence a stricter certificate, still verifies. bool VerifyCertificate(const ContinuousCollisionChecker& checker, const PiecewiseBezierPath& path, diff --git a/planning/continuous_collision/continuous_collision_checker.h b/planning/continuous_collision/continuous_collision_checker.h index 20bf9073fa4b..bab6bca8a9ff 100644 --- a/planning/continuous_collision/continuous_collision_checker.h +++ b/planning/continuous_collision/continuous_collision_checker.h @@ -19,7 +19,7 @@ namespace drake { namespace planning { namespace continuous_collision { -/** Result of one certification call (the architecture). +/** Result of one certification call. @ingroup planning_collision_checker */ struct CertificationResult { Verdict verdict{}; @@ -30,25 +30,23 @@ struct CertificationResult { std::optional certificate; }; -/** Certifies — not samples — that a trajectory is collision-free over its -entire continuous time domain (the problem statement). +/** 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 margin + padding(A, B) — under the stated +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 margin + padding(A, B). That holds under three assumptions: exact real arithmetic up to the configured numerical slack, a -distance oracle accurate to its stated tolerance, and the geometry semantics -of the geometry-support scope (Mesh ≡ convex hull). This is a statement about -the continuum of configurations, not about samples. The certificate is a -property of the path, so retiming the trajectory afterwards does not invalidate -it. +distance oracle accurate to its stated tolerance, and Mesh ≡ convex hull. The +certificate is a property of the path, so retiming the trajectory afterwards +does not invalidate it. 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 deliberately 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. +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: @@ -62,33 +60,76 @@ class ContinuousCollisionChecker { Options default_options{}; }; - /** Builds contexts, bounding spheres, topology tables, and runs the - capability probe (throws on unsupported geometry pairs; the geometry-support - scope). */ + /** Builds contexts, bounding spheres and topology tables, and runs the + capability probe. + @throws std::exception if Params::model is null. + @throws std::exception if the plant is not finalized. + @throws std::exception if Params::default_options is invalid; see + CheckTrajectory() for the conditions. + @throws std::exception if PaddingSpec::per_body_pair is supplied and is not + num_bodies × num_bodies, or if any pair's padding is not finite. + @throws std::exception if the capability probe finds an unsupported pair; + see DistanceOracle's constructor. + @throws std::exception if the plant's topology or geometry defeats the + motion bound; see KinematicsEngine's constructor. */ explicit ContinuousCollisionChecker(Params params); ~ContinuousCollisionChecker(); - /** Certifies a trajectory (any supported Drake trajectory type). */ + /** Certifies a trajectory (any supported Drake trajectory type). + @throws std::exception if the trajectory cannot be normalized; see + PiecewiseBezierPath::FromTrajectory(). + @throws std::exception if the trajectory's row count differs from the + plant's number of generalized positions. + @throws std::exception if Options::margin is not finite, if + Options::query_tolerance or Options::certificate_slack is not a finite + nonnegative distance, if Options::min_interval is outside (0, 1], if + Options::max_reported_findings is below 1, or if Options::max_nodes is set + to 0. + @throws std::exception if margin + padding is negative for any pair; filter + such a pair out instead of padding it below zero. + @throws std::exception if the trajectory moves a coordinate of an + unsupported joint type, or moves a HalfSpace across a rotational + coordinate; see KinematicsEngine::ComputeMotionBoundTable(). */ CertificationResult CheckTrajectory( const trajectories::Trajectory& trajectory, const std::optional& options = {}) const; - /** Certifies a piecewise-linear path through the given waypoint columns. */ + /** Certifies a piecewise-linear path through the given waypoint columns. + @throws std::exception if `waypoints` has fewer than two columns. + @throws std::exception if `waypoints` does not have one row per generalized + position of the plant. + @throws std::exception under every condition CheckTrajectory() lists. */ CertificationResult CheckPath( const Eigen::MatrixXd& waypoints, const std::optional& options = {}) const; - /** Certifies the straight configuration-space edge q1 → q2. */ + /** 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. */ CertificationResult CheckEdge( const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const std::optional& options = {}) const; - /** Introspection / testing seams (all const, thread-safe). */ + /** Converts `trajectory` to the internal piecewise-Bézier form, for + introspection and testing. All const, and safe from arbitrary threads. + @throws std::exception if the trajectory cannot be normalized; see + PiecewiseBezierPath::FromTrajectory(). + @throws std::exception if the trajectory's row count differs from the + plant's number of generalized positions. */ PiecewiseBezierPath Normalize( const trajectories::Trajectory& trajectory, const std::optional& options = {}) const; + + /** The λ table this checker would use for `path`, for introspection and + testing. + @throws std::exception if the path's row count differs from the plant's + number of generalized positions. + @throws std::exception if the path moves a coordinate of an unsupported + joint type, or moves a HalfSpace across a rotational coordinate. */ MotionBoundTable ComputeMotionBounds(const PiecewiseBezierPath& path) const; + const DistanceOracle& distance_oracle() const; const KinematicsEngine& kinematics_engine() const; const std::vector& pairs() const; @@ -99,10 +140,18 @@ class ContinuousCollisionChecker { std::unique_ptr impl_; }; -/** Independently replays every record of `certificate` (recomputing node +/** Independently replays every record of `certificate`, recomputing node control boxes from freshly restricted control points and re-querying -distances) and checks interval coverage of the full domain for every pair. -Returns true iff the certificate holds (the search algorithm). +distances, then checks interval coverage of the full domain for every pair. +@param checker Supplies the model, the distance oracle and the pair + table the replay is checked against; the certificate must + have been produced by this checker. +@param path The normalized path the certificate was produced for. +@param certificate The audit trail to replay. +@returns true iff the certificate is a complete proof that `path` is free. +A certificate from a run that found a violation, ended inconclusive, exhausted +its node budget, or pruned the search leaves part of the domain uncovered, and +so returns false. @ingroup planning_collision_checker */ bool VerifyCertificate(const ContinuousCollisionChecker& checker, const PiecewiseBezierPath& path, diff --git a/planning/continuous_collision/distance_oracle.cc b/planning/continuous_collision/distance_oracle.cc index 3577def5a156..ed6a60c5862f 100644 --- a/planning/continuous_collision/distance_oracle.cc +++ b/planning/continuous_collision/distance_oracle.cc @@ -34,10 +34,9 @@ using drake::geometry::QueryObject; using drake::geometry::SceneGraphInspector; using drake::math::RigidTransformd; -/** The closed set of shape classes the oracle recognizes. Anything outside it -is `kUnsupported` and is refused by the capability probe (mirroring the -throw-on-unknown-shape rule the radius table uses; the geometry-support scope). -*/ +/* The closed set of shape classes the oracle recognizes. Anything outside it +is `kUnsupported` and is refused by the capability probe, mirroring the +throw-on-unknown-shape rule ComputeBoundingSphere() uses. */ enum class ShapeClass { kSphere, kBox, @@ -76,18 +75,18 @@ ShapeClass Classify(const drake::geometry::Shape& shape) { }); } -/** Everything the analytic halfspace fallback needs about the *non*-halfspace +/* 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. */ + /* Sphere / Capsule / Cylinder radius. */ double radius{0.0}; - /** Half the axial length of a Capsule / Cylinder. */ + /* Half the axial length of a Capsule / Cylinder. */ double half_length{0.0}; - /** Box half-sizes, or Ellipsoid semi-axes (a, b, c). */ + /* 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 + /* 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; @@ -145,7 +144,7 @@ SupportData MakeSupportData(const drake::geometry::Shape& shape) { return data; } -/** Returns argmax over x ∈ C of d_W·x, with C the shape described by `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. @@ -244,7 +243,7 @@ std::string ClassName(ShapeClass klass) { return ""; } -/** "geometry_name (ShapeType)", for error messages and the report. */ +/* "geometry_name (ShapeType)", for error messages and the report. */ std::string Describe(const SceneGraphInspector& inspector, GeometryId id) { std::ostringstream out; @@ -253,12 +252,12 @@ std::string Describe(const SceneGraphInspector& inspector, return out.str(); } -/** One row of the probe report: a distinct unordered shape-type combination +/* One row of the probe report: a distinct unordered shape-type combination and the route it resolved to. */ struct ComboRow { DistanceRoute route{DistanceRoute::kNative}; int pair_count{0}; - /** A representative pair, used for the probe query and error messages. */ + /* A representative pair, used for the probe query and error messages. */ GeometryId example_a; GeometryId example_b; }; @@ -266,7 +265,7 @@ struct ComboRow { } // namespace struct DistanceOracle::Impl { - /** Closed-form support data for every geometry that partners a halfspace. + /* 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; @@ -283,7 +282,7 @@ DistanceOracle::DistanceOracle(const RobotDiagram& model, const SceneGraphInspector& inspector = scene_graph.model_inspector(); const drake::multibody::MultibodyPlant& plant = model.plant(); - // --- Deformables are out of scope: refuse, naming them. ---------- + // --- Deformables are out of scope: refuse, naming them. ----------------- const std::vector deformables = inspector.GetAllDeformableGeometryIds(); if (!deformables.empty()) { @@ -358,8 +357,7 @@ DistanceOracle::DistanceOracle(const RobotDiagram& model, } } - // Meshes are certified as their convex hulls; say so, loudly (the risk - // register). + // Meshes are certified as their convex hulls; the report says so. if (class_a == ShapeClass::kMesh) mesh_names.insert(inspector.GetName(id_a)); if (class_b == ShapeClass::kMesh) @@ -444,9 +442,9 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, const drake::geometry::SignedDistancePair result = query_object.ComputeSignedDistancePairClosestPoints(pair.id.a, pair.id.b); - // Drake reports the pair in its own fixed (deliberately undocumented) - // order, which may be the reverse of ours; the witness points come back - // in *its* A/B geometry frames, so undo any swap explicitly. + // 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.id.a && result.id_B == pair.id.b) { @@ -473,13 +471,13 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, // 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. + // ϕ = 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 + // 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 -φ, + // 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). @@ -504,7 +502,7 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, 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̂. + // boundary plane; the witness displacement is then exactly ϕ·n̂. const Eigen::Vector3d plane_W = x_W - phi * n_W; if (nearest_a_W != nullptr) { diff --git a/planning/continuous_collision/distance_oracle.h b/planning/continuous_collision/distance_oracle.h index 7199692f6490..ea9b00557469 100644 --- a/planning/continuous_collision/distance_oracle.h +++ b/planning/continuous_collision/distance_oracle.h @@ -1,9 +1,5 @@ #pragma once -// NOTE(interface): This header is owned by the distance module. The class -// and file names and the documented semantics are fixed; internal details -// may be refined by the implementation. - #include #include #include @@ -19,9 +15,8 @@ namespace drake { namespace planning { namespace continuous_collision { -/** How the oracle computes signed distance for one pair, resolved once by -the capability probe (the geometry-support scope; the distance-oracle contract): -no per-query dispatch decisions. +/** How the oracle computes signed distance for one pair, resolved once by the +capability probe: no per-query dispatch decisions. @ingroup planning_collision_checker */ enum class DistanceRoute { /** QueryObject::ComputeSignedDistancePairClosestPoints. */ @@ -43,16 +38,15 @@ struct PairRecord { double threshold{0.0}; }; -/** Narrowphase distance abstraction (the distance-oracle contract). Stateless -per query and thread-compatible: configuration comes in via the caller's -QueryObject. +/** Narrowphase distance abstraction. Stateless per query and +thread-compatible: configuration comes in via the caller's QueryObject. -Contract: SignedDistance returns φ̂ with |φ̂ − φ_true| ≤ tolerance() -whenever φ_true is at or above −tolerance(), and returns a definitely +Contract: SignedDistance returns ϕ̂ with |ϕ̂ − ϕ_true| ≤ tolerance() +whenever ϕ_true is at or above −tolerance(), and returns a definitely negative value when the shapes interpenetrate beyond tolerance. Only -over-reporting a distance at or above threshold could fake a certificate -(the soundness argument), which is why the capability probe keeps any -not-a-true-distance backend out of the loop entirely. +over-reporting a distance at or above 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 @@ -64,12 +58,13 @@ class DistanceOracle { 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), classifies every (shape, shape) combination as - {native, halfspace-fallback, unsupported}, and - @throws std::exception immediately naming the offending geometries if any - pair is unsupported (deformables; halfspace–halfspace). Never discovers an - unsupported pair mid-certification. */ + 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. */ DistanceOracle(const RobotDiagram& model, double query_tolerance); /** The unfiltered pairs found by the probe (thresholds default 0; the @@ -92,23 +87,23 @@ class DistanceOracle { Eigen::Vector3d* nearest_a_W = nullptr, Eigen::Vector3d* nearest_b_W = nullptr) const; - /** τ used in the certificate arithmetic (the numerical policy). */ + /** τ used in the certificate arithmetic. */ double tolerance() const { return tolerance_; } /** Human-readable probe report: one line per distinct shape-type - combination and its route (includes the "Mesh certified as convex hull" - notices; the risk register). */ + combination and its route, including the "Mesh certified as convex hull" + notices. */ const std::string& support_report() const; private: std::vector pairs_; double tolerance_{1e-6}; - /** Immutable capability-probe results: closed-form support data for every - halfspace partner, the resolved per-shape-combination routes, and the - rendered report. Held by shared_ptr so the oracle stays cheaply copyable - and thread-compatible (the probe output is never mutated after - construction). */ + /* Immutable capability-probe results: closed-form support data for every + halfspace partner, the resolved per-shape-combination routes, and the + rendered report. 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_; }; diff --git a/planning/continuous_collision/motion_bound_table.cc b/planning/continuous_collision/motion_bound_table.cc index ca6c64164485..0d69a2514966 100644 --- a/planning/continuous_collision/motion_bound_table.cc +++ b/planning/continuous_collision/motion_bound_table.cc @@ -137,7 +137,7 @@ void KinematicsEngine::BuildTopology() { translation_known = true; } else if (rec.type_name == "planar") { rec.kind = JointKind::kPlanar; - // q = (x, y, θ) — see PlanarJoint's class documentation. + // 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) { @@ -267,8 +267,10 @@ void KinematicsEngine::BuildTopology() { 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). Loop " - "topologies are not supported in v1.", + "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)); } } @@ -305,11 +307,10 @@ void KinematicsEngine::BuildTopology() { 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 " + "KinematicsEngine: joint '{}' ({}) is reversed: its declared parent " "body '{}' is outboard of its declared child body '{}' in the " - "multibody tree. Reversed mobilizers are a documented v1 exclusion " - "because the frame that stays fixed under the joint's motion is then " - "on the outboard side, which the reach chain does not model. " + "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())); @@ -321,8 +322,9 @@ void KinematicsEngine::BuildTopology() { 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. This model's " - "topology is not supported in v1.", + "'{}' ({}) 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); @@ -368,9 +370,9 @@ void KinematicsEngine::BuildGeometry() { 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 the - // dedicated rule in CheckHalfSpaceRule() (the geometry-support scope) - // keeps them off the distal side of any rotational coordinate. + // 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); @@ -435,9 +437,9 @@ void KinematicsEngine::CheckHalfSpaceRule() const { "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 (the geometry-support " - "scope). Fix the model by anchoring the half space, filtering the " - "pair, or replacing the half space with a large Box.", + "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)); @@ -453,8 +455,7 @@ std::vector KinematicsEngine::CoordinatesAffectingPair( 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 (the displacement - // lemma). + // 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) { @@ -482,16 +483,15 @@ double KinematicsEngine::Reach(int joint_ord, BodyIndex body, 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 deliberately NOT - // included. + // 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 — reach chain walk did not reach the " - "requested joint. This indicates inconsistent topology tables."); + "KinematicsEngine: internal error: the reach chain walk did not reach " + "the requested joint. This indicates inconsistent topology tables."); } MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( @@ -513,7 +513,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( 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={}, " + "KinematicsEngine: control-box size mismatch: got lower={}, upper={}, " "constant_coordinates={} for a plant with {} positions.", lower.size(), upper.size(), constant_coordinates.size(), num_positions_)); @@ -532,8 +532,9 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( 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 Options::continuity_tolerance — a tolerance, - // not an identity. `range` is exactly what the residual is charged against. + // range collapses to within Options::continuity_tolerance. 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]; }; @@ -543,7 +544,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // 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 (the displacement lemma). + // adaptive. // ------------------------------------------------------------------ std::vector box_hop(joints_.size(), 0.0); for (int k = 0; k < static_cast(joints_.size()); ++k) { @@ -571,17 +572,17 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( 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 excluded in v1 (the " - "joint-support scope). 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. See the white paper's future " - "extensions for the manifold-curve extension.", + "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)); } } @@ -643,8 +644,8 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // // 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 + // (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 @@ -654,10 +655,10 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // ‖(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; + // 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π. + // 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 @@ -666,22 +667,21 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // 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 (the - // soundness argument), so no penetration-depth regularity is needed. + // 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 (trajectory normalization; the - // joint-support scope) drops coordinate j from J(p) when its *whole* - // control-point range fits inside Options::continuity_tolerance. 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 — small (≈ 1e-7 m for a metre-scale reach), but two orders of - // magnitude above Options::certificate_slack and unaccounted anywhere, so the - // certificate inequality could pass with the true clearance below threshold - // by that much. Instead of ignoring the step we charge it at its worst case, + // The constant-coordinate carve-out drops coordinate j from J(p) when its + // *whole* control-point range fits inside Options::continuity_tolerance. + // 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 Options::certificate_slack, 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): // @@ -692,11 +692,10 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // 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, - // and that is the case for every path whose control points repeat the - // coordinate's value verbatim — the overwhelmingly common way a coordinate - // becomes constant. λ̃_j, per coordinate kind: + // 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, + // * 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. // @@ -704,9 +703,9 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // 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| — 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 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 @@ -720,7 +719,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // // * QuaternionFloating quaternion coefficients: λ̃ = 2r/m ≤ 4r, with // m = min over the control box of ‖q‖ (computed above). Derivation. - // Drake normalizes internally — X_FM uses R(q/‖q‖) — so the map from + // 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 @@ -736,20 +735,18 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // 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 actually produces — a box of diameter - // ρ ≤ continuity_tolerance around a unit quaternion — m ≥ 1 − ρ, so - // 2r/m ≤ 2r/(1 − ρ) ≤ 4r for any ρ ≤ 1/2: the shipped 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 — Drake's own normalization is - // undefined there — and throws. + // In the regime the carve-out produces, a box of diameter + // ρ ≤ continuity_tolerance 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 λ̃. Such a coordinate must be - // *exactly* constant; anything else throws (the geometry-support scope). - // Accepting a merely tolerance-constant one, as the code did before the - // residual was charged, is the one case where the residual is genuinely - // unbounded. + // 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; @@ -798,8 +795,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( 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 (the " - "geometry-support scope).", + "space has unbounded reach, so no finite λ exists.", body_halfspace_name_[distal], plant_->get_body(distal).name(), rec.name, rec.type_name)); } @@ -831,13 +827,12 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( "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 " + "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 λ (the geometry-support scope): a " - "half space may only " - "sit across a rotational coordinate that is EXACTLY constant. " - "Fix the trajectory so that coordinate's control points are " + "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(), @@ -883,7 +878,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( lam = 1.0; break; case JointKind::kPlanar: - // q = (x, y, θ) — see PlanarJoint's class documentation. + // q = (x, y, θ); see PlanarJoint's class documentation. lam = (c == ps + 2) ? reach() : 1.0; break; case JointKind::kScrew: @@ -892,7 +887,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( case JointKind::kWeld: case JointKind::kUnsupported: throw std::runtime_error(fmt::format( - "KinematicsEngine: internal error — joint '{}' ({}) reached " + "KinematicsEngine: internal error: joint '{}' ({}) reached " "the λ assembly with an unsupported kind.", rec.name, rec.type_name)); } diff --git a/planning/continuous_collision/motion_bound_table.h b/planning/continuous_collision/motion_bound_table.h index da48cf6a3498..f402153c1f63 100644 --- a/planning/continuous_collision/motion_bound_table.h +++ b/planning/continuous_collision/motion_bound_table.h @@ -1,9 +1,5 @@ #pragma once -// NOTE(interface): This header is owned by the kinematics module. The class -// and file names and the documented semantics are fixed; internal details -// (private members, helper structs) may be refined by the implementation. - #include #include #include @@ -24,24 +20,16 @@ namespace drake { namespace planning { namespace continuous_collision { -/** Per-pair motion-bound coefficients in CSR layout (the displacement lemma): -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. λ has -units of meters of worst-case point 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 (trajectory normalization; the -joint-support scope) removed from J(p). "Constant" there is a *tolerance* — a -coordinate whose global control-box range is at most -Options::continuity_tolerance — not an identity, so a carved coordinate may -still displace the pair's distal side by up to λ̃_j · range_j. That residual is -charged unconditionally inside MotionBound(), which is what makes Δ_p a true -upper bound on the pair's relative motion over the whole trajectory rather than -one that ignores the carved coordinates. It is exactly zero — bit for bit — -whenever every carved coordinate is *exactly* constant, which is the case for -every path whose control points repeat a coordinate's value verbatim. +/** 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. @ingroup planning_collision_checker */ class MotionBoundTable { public: @@ -67,14 +55,17 @@ class MotionBoundTable { 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. */ + 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]; } - /** Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j — a sparse dot + /** Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j: a sparse dot product against the node's per-coordinate deviations w, plus the carved - coordinates' residual (the interval certificate, requirement P3). */ + 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 { double delta = carveout_slack_[pair_index]; for (int e = row_start_[pair_index]; e < row_start_[pair_index + 1]; ++e) { @@ -86,14 +77,18 @@ class MotionBoundTable { /** Σ 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. Zero when every carved coordinate is - exactly constant. */ + coordinates the table no longer tracks. The carve-out's "constant" is a + tolerance, a coordinate whose global control-box range is at most + Options::continuity_tolerance, 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. */ + ordered by increasing coordinate index. + @throws std::exception if pair_index is outside [0, num_pairs()). */ std::vector> GetEntries(int pair_index) const; /** Total number of (coordinate, λ) entries over all pairs. */ @@ -106,8 +101,8 @@ class MotionBoundTable { std::vector carveout_slack_; }; -/** Construction-time kinematic analysis of a plant (the displacement lemma): -joint classification, per-hop fixed-transform translations, per-body proximity +/** 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. @@ -126,21 +121,23 @@ class 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 (constant-coordinate carve-out, the - joint-support scope). + 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 (unbounded reach). - A HalfSpace that is merely the static partner of a rotating body — the - anchored ground plane under a robot arm, the overwhelmingly common case — is - accepted: λ then bounds the partner's points, and signed distance is - symmetric, so the certificate still holds. - @throws std::exception if the plant is not finalized, if a joint is - "reversed" (its declared parent body is outboard of its declared child body - in the multibody tree — a documented v1 exclusion), or if any proximity - geometry has a shape ComputeBoundingSphere() rejects. */ + 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 @@ -150,10 +147,12 @@ class KinematicsEngine { 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; the displacement lemma). 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. + 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( @@ -197,8 +196,8 @@ class KinematicsEngine { bool body_has_halfspace(multibody::BodyIndex body) const; /** Radius, about the body frame origin, of a sphere containing every - proximity geometry of `body` — the start of the reach chain. Zero for a - body with no (non-HalfSpace) proximity geometry. */ + 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_; } @@ -206,8 +205,7 @@ class KinematicsEngine { const multibody::MultibodyPlant& plant() const { return *plant_; } private: - /* The λ rule a joint's coordinates follow (the displacement lemma; the - * joint-support scope). */ + /* The λ rule a joint's coordinates follow. */ enum class JointKind { kWeld, // 0 dof; contributes fixed translations to reach only. kRevolute, // λ = r. diff --git a/planning/continuous_collision/numerics.h b/planning/continuous_collision/numerics.h index e9e74248ec7d..83aeedb6bdb9 100644 --- a/planning/continuous_collision/numerics.h +++ b/planning/continuous_collision/numerics.h @@ -1,23 +1,25 @@ #pragma once /** @file -Single home of the numerical accounting used everywhere (the numerical policy). +Single home of the numerical accounting used everywhere. -Let φ̂ be the oracle's reported signed distance at the node's representative -configuration, τ the oracle accuracy contract (|φ̂ − φ_true| ≤ τ on the +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 effective threshold (margin + padding), and ε the certificate slack. - - Certified: φ̂ − τ − Δ > m + ε (sound by the displacement lemma: + - Certified: ϕ̂ − τ − Δ > m + ε (sound by the displacement lemma: every configuration on the node keeps clearance > m). - - Definite violation: φ̂ + τ < m (the true clearance at an exactly + - Definite violation: ϕ̂ + τ < m (the true clearance at an exactly on-trajectory configuration is below threshold). - Otherwise the pair is gray and drives subdivision. -The certificate is mathematical modulo τ and ε: the library does not use -directed rounding (that hardening is a future extension); ε defaults -to 1e-9 m which dominates the accumulated FP error of the w/λ/dot-product -expression depths involved. */ +The certificate is mathematical modulo τ and ε. ε defaults to 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. */ namespace drake { namespace planning { diff --git a/planning/continuous_collision/options.h b/planning/continuous_collision/options.h index 8439f8ab2ecc..254b3c998d1e 100644 --- a/planning/continuous_collision/options.h +++ b/planning/continuous_collision/options.h @@ -14,7 +14,7 @@ namespace drake { namespace planning { namespace continuous_collision { -/** Search modes for certification (the search algorithm). +/** Search modes for certification. @ingroup planning_collision_checker */ enum class SearchMode { /** Return on the first definite violation; serial execution returns the @@ -25,7 +25,7 @@ enum class SearchMode { kCertifyAll, }; -/** Outcome of a certification run (the problem statement). +/** Outcome of a certification run. @ingroup planning_collision_checker */ enum class Verdict { /** Proof: every unfiltered pair keeps signed distance > margin + padding @@ -40,9 +40,8 @@ enum class Verdict { kBudgetExhausted, }; -/** Options controlling one certification call (the architecture; the numerical - * policy). - * @ingroup planning_collision_checker */ +/** Options controlling one certification call. +@ingroup planning_collision_checker */ struct Options { /** Global clearance margin δ in meters. The certificate proves signed distance > margin + padding for every pair at every time. */ @@ -50,8 +49,7 @@ struct Options { /** Junction C0-continuity tolerance (per coordinate; modulo 2π for coordinates listed in continuous_revolute_indices). */ double continuity_tolerance{1e-7}; - /** τ: the distance oracle's accuracy contract in meters (the distance-oracle - * contract; the numerical policy). */ + /** τ: the distance oracle's accuracy contract in meters. */ double query_tolerance{1e-6}; /** ε_slack: swallows floating-point noise in the bound arithmetic. */ double certificate_slack{1e-9}; @@ -66,10 +64,10 @@ struct Options { int max_conversion_degree{10}; SearchMode mode{SearchMode::kCertifyAll}; int max_reported_findings{32}; - /** Optional node budget; exceeded ⇒ Verdict::kBudgetExhausted. */ + /** Optional node budget; exceeded => Verdict::kBudgetExhausted. */ std::optional max_nodes{}; /** If true, every certification event is recorded into a Certificate that - VerifyCertificate() can independently replay (the search algorithm). */ + VerifyCertificate() can independently replay. */ bool emit_certificate{false}; Parallelism parallelism{Parallelism::Max()}; }; @@ -79,7 +77,7 @@ margin + padding(p). Which of the two scalars applies to a pair is decided by *anchoring*, from plant topology alone. A body is anchored iff no position coordinate of the -plant changes its pose relative to the world — the world body itself, and +plant changes its pose relative to the world, i.e. the world body itself and everything welded to it directly or transitively. A pair is a self-collision pair iff both of its bodies are non-anchored, and an environment pair otherwise. The rule never depends on which trajectory is being checked. @@ -107,7 +105,7 @@ struct PairId { multibody::BodyIndex body_b; }; -/** One violation or inconclusive record (the architecture). +/** One violation or inconclusive record. @ingroup planning_collision_checker */ struct Finding { /** Trajectory time of the witness configuration. */ @@ -119,7 +117,7 @@ struct Finding { double distance{}; /** Motion bound Δ_p at the terminal node (0 for breakpoint findings). */ double motion_bound{}; - /** true ⇒ definite violation; false ⇒ grazing / inconclusive. */ + /** true => definite violation; false => grazing / inconclusive. */ bool definite{}; /** Closest points in world frame at q, when the narrowphase provides them (violation findings; planners use these to push trajectories out diff --git a/planning/continuous_collision/piecewise_bezier_path.cc b/planning/continuous_collision/piecewise_bezier_path.cc index 26ad5387cc50..ee8b47b06c04 100644 --- a/planning/continuous_collision/piecewise_bezier_path.cc +++ b/planning/continuous_collision/piecewise_bezier_path.cc @@ -68,8 +68,8 @@ nonempty span [t_i, t_{i+1}) satisfies t_{i-p+1} = ... = t_i and t_{i+1} = ... 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 the test plan's T1 (1e-10 over >= 1e4 dense samples) guards the -indexing. */ +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) { @@ -177,7 +177,7 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, "index {}) has polynomial degree {}, above " "options.max_conversion_degree = {}. The monomial-to-Bernstein " "change of basis is ill-conditioned at high degree; either raise " - "Options::max_conversion_degree deliberately or re-express the " + "Options::max_conversion_degree or re-express the " "trajectory with more, lower-degree segments.", k, source_index, m, options.max_conversion_degree)); } @@ -220,7 +220,7 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, /* 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 (trajectory normalization, item 3). +visited so far and appears in error messages. */ void AppendSegments(const Trajectory& trajectory, const Options& options, int* source_index, @@ -312,7 +312,7 @@ void ValidateSegments(int num_positions, const Options& options, 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 {} " + "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)); @@ -337,7 +337,7 @@ void ValidateSegments(int num_positions, const Options& options, // 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 (trajectory normalization). + // 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; @@ -412,7 +412,7 @@ PiecewiseBezierPath PiecewiseBezierPath::FromWaypoints( 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}} (trajectory normalization, item 1). Segment + // 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; @@ -443,7 +443,7 @@ void PiecewiseBezierPath::FinalizeMetadata(double continuity_tolerance) { // 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 (trajectory normalization; the joint-support scope). + // treated as welded. constant_coordinates_.assign(n, false); for (int i = 0; i < n; ++i) { constant_coordinates_[i] = @@ -469,7 +469,7 @@ Eigen::VectorXd PiecewiseBezierPath::Value(double t) const { // drake::trajectories::PiecewiseTrajectory::get_segment_index(). The choice // is observable only when a junction carries a legitimate 2πk offset in a // continuous-revolute coordinate, where the two sides are different - // representatives of the same configuration (trajectory normalization). + // representatives of the same configuration. int low = 0; int high = static_cast(segments_.size()) - 1; while (low < high) { @@ -544,8 +544,7 @@ void DeCasteljauSplitAtHalf(const Eigen::MatrixXd& cps, Eigen::MatrixXd* left, // 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 (trajectory normalization; the search - // algorithm). + // the right child's first control point. *right = cps; left->col(0) = cps.col(0); for (int r = 1; r <= m; ++r) { diff --git a/planning/continuous_collision/piecewise_bezier_path.h b/planning/continuous_collision/piecewise_bezier_path.h index e82f09e0aa6f..9dc5c80e5c79 100644 --- a/planning/continuous_collision/piecewise_bezier_path.h +++ b/planning/continuous_collision/piecewise_bezier_path.h @@ -12,9 +12,8 @@ namespace drake { namespace planning { namespace continuous_collision { -/** One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1] (trajectory - * normalization). - * @ingroup planning_collision_checker */ +/** One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1]. +@ingroup planning_collision_checker */ struct BezierSegment { /** Original time interval (bookkeeping only; the certificate is a property of the path and is invariant under time reparametrization). */ @@ -26,7 +25,7 @@ struct BezierSegment { /** 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 (trajectory normalization). +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}, @@ -52,7 +51,9 @@ class PiecewiseBezierPath { const Options& options); /** Normalizes an n × K waypoint matrix into K−1 order-1 segments (exact). - Segment k spans time [k, k+1]. @throws std::exception if K < 2. */ + Segment k spans time [k, k+1]. + @throws std::exception if `waypoints` has fewer than two columns. + @throws std::exception if `waypoints` has zero rows. */ static PiecewiseBezierPath FromWaypoints(const Eigen::MatrixXd& waypoints, const Options& options); @@ -61,24 +62,28 @@ class PiecewiseBezierPath { 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 (trajectory - normalization); used for trajectory-adaptive prismatic reach bounds. */ + /** 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 the continuity tolerance) across all control points of all segments; such coordinates are - treated as welded for the check (trajectory normalization; the joint-support - scope). */ + 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 never calls this — it uses de Casteljau apexes). */ + /** Evaluates the path at time t, for tests and breakpoint checks; the hot + loop uses de Casteljau apexes instead. + @pre t lies in [start_time(), end_time()], up to a parameter slack. + @throws std::exception if t is outside that domain. */ Eigen::VectorXd Value(double t) const; - /** Evaluates segment `segment_index` at local parameter s ∈ [0, 1]. */ + /** Evaluates segment `segment_index` at local parameter s ∈ [0, 1]. + @pre 0 <= segment_index < segments().size(). + @pre s lies in [0, 1], up to a parameter slack. + @throws std::exception if either precondition is violated. */ Eigen::VectorXd EvaluateSegment(int segment_index, double s) const; private: diff --git a/planning/continuous_collision/test/api_test.cc b/planning/continuous_collision/test/api_test.cc index fef80a28553f..97b857d5d045 100644 --- a/planning/continuous_collision/test/api_test.cc +++ b/planning/continuous_collision/test/api_test.cc @@ -1,22 +1,9 @@ -/// @file -/// T9 — API / UX (test plan T9: the joint-support and geometry-support -/// scopes, and the architecture). -/// -/// Every refusal this library makes has to be *actionable*: the message must -/// name the joint, geometry, coordinate, index or size the caller has to go and -/// fix. These tests therefore assert on message content, not just that -/// something was thrown — a bare EXPECT_THROW would pass for a message reading -/// "error" and leave a user with nothing to act on. -/// -/// Coverage notes for two items of test-plan T9: -/// * Python bindings do not exist yet, so the pydrake-style smoke -/// tests are out of scope here. -/// * An *unfinalized* plant cannot reach the checker through Drake's public -/// API on this pin: RobotDiagramBuilder::Build() finalizes the plant -/// unconditionally and RobotDiagram's constructor is private to the -/// builder, so there is no way to construct the input that guard rejects. -/// The guard is therefore defensive; the adjacent, reachable guards (null -/// model) are pinned instead. See NullModelIsRefused below. +// 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 pydrake surface is covered separately, in +// bindings/pydrake/planning/test/continuous_collision_test.py. #include #include @@ -86,9 +73,9 @@ SpatialInertia Inertia() { return SpatialInertia::SolidSphereWithMass(1.0, 0.05); } -/// Runs `call`, requires it to throw, and returns the message so the caller can -/// assert on the identifiers it must contain. Reports the actual message on -/// every failure path, so a message regression is diagnosable from the log. +// Runs `call`, requires it to throw, and returns the message so the caller can +// assert on the identifiers it must contain. Reports the actual message on +// every failure path, so a message regression is diagnosable from the log. template std::string ThrowMessage(Callable&& call) { try { @@ -114,8 +101,8 @@ std::unique_ptr MakeChecker( return std::make_unique(params); } -/// A planar 2-dof arm (revolute, prismatic) with one anchored obstacle: the -/// well-formed world the dimension / options / trajectory tests use. +// 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(); @@ -139,9 +126,9 @@ std::unique_ptr> MakeArmWorld() { 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)]. +// 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(); @@ -163,7 +150,7 @@ std::unique_ptr> MakeFloatingBaseWorld() { return builder.Build(); } -/// The name Drake gave the quaternion floating joint it added at Finalize(). +// 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); @@ -173,8 +160,8 @@ std::string FloatingJointName(const MultibodyPlant& plant) { return {}; } -/// q for MakeFloatingBaseWorld(): identity quaternion, `p` for the base -/// position, `elbow` for the joint. +// 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; @@ -182,9 +169,8 @@ VectorXd FloatingQ(const Vector3d& p, double elbow) { } // --------------------------------------------------------------------------- -// 1. Joint scope (the joint-support scope): quaternion bases, and the -// constant-coordinate -// carve-out that makes them usable anyway. +// 1. Joint scope: quaternion bases, and the constant-coordinate carve-out that +// makes them usable anyway. // --------------------------------------------------------------------------- GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { @@ -193,7 +179,7 @@ GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { const std::string joint_name = FloatingJointName(model->plant()); ASSERT_FALSE(joint_name.empty()); - // Move a *quaternion* coordinate: straight-line interpolation of quaternion + // 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. Eigen::MatrixXd points(8, 2); @@ -222,11 +208,10 @@ GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { } GTEST_TEST(ApiTest, ConstantQuaternionBaseIsAcceptedEndToEnd) { - // The joint-support carve-out: a floating base whose pose is *constant* along - // the trajectory is treated as welded, so a floating-base robot is fully - // usable as long as the given trajectory does not move the base. This is the - // end-to-end version of that promise — not just "does not throw", but a real - // verdict with a real certificate. + // 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. Checked end to end: a verdict and a certificate, not + // just "does not throw". std::shared_ptr> model = MakeFloatingBaseWorld(); const auto checker = MakeChecker(model); @@ -265,13 +250,12 @@ GTEST_TEST(ApiTest, ConstantQuaternionBaseIsAcceptedEndToEnd) { } GTEST_TEST(ApiTest, ToleranceConstantQuaternionBaseChargesItsResidualEndToEnd) { - // The carve-out flags a coordinate constant on a *tolerance*, so a base held + // The carve-out flags a coordinate constant on a tolerance, so a base held // only to within continuity_tolerance is carved even though it still moves. // Its residual is charged to MotionBoundTable::carveout_slack(), and that has - // to survive all the way through the certifier — including the static-pair - // shortcut, which never evaluates a per-node Δ — and the certificate replay, - // which recomputes Δ from scratch and would reject a record whose bound came - // out smaller than the one the certifier used. + // to survive the static-pair shortcut, which never evaluates a per-node Δ, + // and the certificate replay, which recomputes Δ from scratch and would + // reject a record whose bound came out smaller than the certifier's. std::shared_ptr> model = MakeFloatingBaseWorld(); const auto checker = MakeChecker(model); @@ -339,12 +323,11 @@ GTEST_TEST(ApiTest, ToleranceConstantQuaternionBaseChargesItsResidualEndToEnd) { } // --------------------------------------------------------------------------- -// 2. Geometry scope (the geometry-support scope): rotating half spaces and -// deformables. +// 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 + // A half space on a body that rotates relative to an unfiltered partner has // unbounded reach, so no finite λ exists for that pair. This must be refused // when the checker is built, not discovered mid-certification. RobotDiagramBuilder builder; @@ -373,9 +356,8 @@ GTEST_TEST(ApiTest, RotatingHalfSpaceThrowsAtConstruction) { GTEST_TEST(ApiTest, AnchoredHalfSpaceUnderARotatingArmIsAccepted) { // The complement, so the rule above is not read as "half spaces are - // unsupported": the overwhelmingly common case — an anchored ground plane - // under a rotating arm — is accepted, because λ then bounds the *arm's* - // points and signed distance is symmetric. + // unsupported". An anchored ground plane under a rotating arm is accepted, + // because λ then bounds the arm's points and signed distance is symmetric. RobotDiagramBuilder builder; MultibodyPlant& plant = builder.plant(); const RigidBody& link = plant.AddRigidBody("link", Inertia()); @@ -402,12 +384,10 @@ GTEST_TEST(ApiTest, AnchoredHalfSpaceUnderARotatingArmIsAccepted) { } GTEST_TEST(ApiTest, DeformableGeometryIsRefusedNamingIt) { - // Deformables are out of scope (the geometry-support scope): their motion is - // not described by the plant's generalized positions, so no motion bound - // exists for them at all. Registering one is possible on this Drake pin (the - // plant must be discrete, which RobotDiagramBuilder's default time step - // already is), so the refusal is exercised on a real model rather than argued - // about. + // 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()); @@ -447,7 +427,7 @@ GTEST_TEST(ApiTest, DeformableGeometryIsRefusedNamingIt) { } // --------------------------------------------------------------------------- -// 3. Dimensions (trajectory normalization; the architecture). +// 3. Dimensions. // --------------------------------------------------------------------------- // The displacement lemma is proved in the separated regime only, so a @@ -501,7 +481,7 @@ GTEST_TEST(ApiTest, DimensionMismatchMessagesNameTheSizes) { } // --------------------------------------------------------------------------- -// 4. Trajectory validation (trajectory normalization). +// 4. Trajectory validation. // --------------------------------------------------------------------------- GTEST_TEST(ApiTest, DiscontinuousTrajectoryThrowsNamingTheJunction) { @@ -532,7 +512,7 @@ GTEST_TEST(ApiTest, DegreeAboveConversionCapThrows) { std::shared_ptr> model = MakeArmWorld(); const auto checker = MakeChecker(model); - // 13 interpolation nodes ⇒ one polynomial segment of degree 12, above the + // 13 interpolation nodes => one polynomial segment of degree 12, above the // default max_conversion_degree of 10. const int kNodes = 13; VectorXd times(kNodes); @@ -551,7 +531,7 @@ GTEST_TEST(ApiTest, DegreeAboveConversionCapThrows) { ExpectContains(message, "polynomial degree 12"); ExpectContains(message, "max_conversion_degree"); - // Raising the cap deliberately is the documented escape hatch, and it works. + // Raising the cap is the documented escape hatch. Options options; options.parallelism = Parallelism::None(); options.max_conversion_degree = 12; @@ -572,7 +552,7 @@ GTEST_TEST(ApiTest, UnsupportedTrajectoryTypeThrowsNamingTheType) { }); ExpectContains(message, "unsupported trajectory type"); ExpectContains(message, "PiecewiseQuaternionSlerp"); - // The message must list what *is* accepted. + // The message must list what is accepted. ExpectContains(message, "BezierCurve"); ExpectContains(message, "BsplineTrajectory"); } @@ -652,8 +632,10 @@ GTEST_TEST(ApiTest, NullModelIsRefused) { ContinuousCollisionChecker checker(params); }); ExpectContains(message, "Params::model is null"); - // The message points at the requirement the (unreachable-through-Drake's - // public API) finalization guard also enforces. + // 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. ExpectContains(message, "finalized"); } @@ -687,8 +669,8 @@ GTEST_TEST(ApiTest, MaxReportedFindingsIsRespected) { // would be satisfied by a regression that returned nothing, which would // also make the prefix check below vacuous. ASSERT_EQ(static_cast(result.findings.size()), cap); - // The cap keeps the *earliest* findings, so a capped run is a prefix of the - // uncapped one — dropping the latest entry can never remove an earlier one. + // The cap keeps the earliest findings, so a capped run is a prefix of the + // uncapped one: dropping the latest entry never removes an earlier one. for (std::size_t i = 0; i < result.findings.size(); ++i) { EXPECT_EQ(result.findings[i].time, uncapped.findings[i].time); EXPECT_EQ(result.findings[i].definite, uncapped.findings[i].definite); diff --git a/planning/continuous_collision/test/bounding_sphere_test.cc b/planning/continuous_collision/test/bounding_sphere_test.cc index 9d10e6a5f521..4525def6e591 100644 --- a/planning/continuous_collision/test/bounding_sphere_test.cc +++ b/planning/continuous_collision/test/bounding_sphere_test.cc @@ -1,11 +1,9 @@ -/* T2 (the test plan) — the bounding-sphere radius property test. - * - * For every supported shape class, at many random poses X_LG, every sampled - * surface point must lie inside the reported sphere. A shape that silently - * picks up another shape's radius formula is a *silent* λ soundness bug, so - * this test is deliberately exhaustive over the closed set of supported shapes - * and also pins the throw-on-unsupported behaviour. Never loosen the tolerance - * to make a case pass (the implementation notes, item 2). */ +// 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 λ 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 "drake/planning/continuous_collision/bounding_sphere.h" @@ -49,13 +47,13 @@ using Eigen::Vector3d; constexpr int kNumPoses = 100; constexpr int kNumSurfaceSamples = 1000; -/* The containment claim is exact mathematics; this only absorbs the rounding - of re-evaluating it. Note the slack is taken relative to the *origin-centred* - radius R_g = ‖c_L‖ + ρ, exactly as the test plan's T2 states the property: 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. */ +/* 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; using Rng = std::mt19937_64; @@ -124,7 +122,8 @@ Sampler CapsuleSampler(double r, double length) { const double half = 0.5 * length; return [r, half](Rng* rng) { // Total area is split between the cylindrical barrel and the two caps; - // exact area weighting is irrelevant here — every region must be sampled. + // exact area weighting is irrelevant here, but every region must be + // sampled. if (std::uniform_int_distribution(0, 1)(*rng) == 0) { const double phi = Uniform(rng, 0.0, 2.0 * M_PI); return Vector3d(r * std::cos(phi), r * std::sin(phi), @@ -161,8 +160,7 @@ Sampler EllipsoidSampler(double a, double b, double c) { /* 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 exactly the claim the - geometry-support scope makes. */ + engine collides, so containing all of them is the whole claim. */ Sampler HullVertexSampler( const drake::geometry::PolygonSurfaceMesh& hull) { return [&hull](Rng* rng) { @@ -332,8 +330,8 @@ GTEST_TEST(BoundingSphereTest, ConvexContainsHullVertices) { hull = &shape.GetConvexHull(); } catch (const std::exception& e) { // Drake rejects hulls it considers degenerate; the checker inherits - // that decision, and nothing about our radius is claimed for a shape - // the proximity engine cannot build either. + // 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; } @@ -394,9 +392,8 @@ GTEST_TEST(BoundingSphereTest, MeshContainsHullVertices) { } } -/* The closed-set requirement of the geometry-support scope: a shape that is not - on the supported list must throw, never silently inherit some other shape's - formula. */ +/* A shape that is not on the supported list must throw, never silently inherit + some other shape's formula. */ GTEST_TEST(BoundingSphereTest, ThrowsOnHalfSpace) { const HalfSpace shape; const RigidTransform X_LG = RigidTransform::Identity(); diff --git a/planning/continuous_collision/test/certificate_test.cc b/planning/continuous_collision/test/certificate_test.cc index caf51dd54440..ae0c7ad44085 100644 --- a/planning/continuous_collision/test/certificate_test.cc +++ b/planning/continuous_collision/test/certificate_test.cc @@ -1,23 +1,15 @@ -/// @file -/// T7 — certificate audit (test plan T7; the search algorithm's -/// "certificate audit trail"). -/// -/// `VerifyCertificate` is the library's second, independent line of defence: it -/// replays every certification event from the checker's *public* seams, -/// re-restricting control points, recomputing motion bounds and re-querying -/// distances, and then checks that the certified intervals tile the whole -/// domain for every pair. This file audits the auditor. -/// -/// Structure: a small corpus of certified runs — two random worlds plus one -/// hand-built world whose pair structure is designed (one pair that only -/// certifies after deep subdivision, one that certifies at the root) — and a -/// table of adversarial mutations, each applied to *every* corpus case. A -/// mutation that any case accepts is a hole in the audit. -/// -/// certifier_test.cc already covers a handful of single-case mutations on its -/// own world; this file is the sweep, plus the mutation classes that need a -/// designed pair structure (record relabelling) or a second run -/// (kFindFirstViolation and non-free verdicts). +// Tests VerifyCertificate, which replays every certification event from the +// checker's public seams, re-restricting control points, recomputing motion +// bounds and re-querying distances, then checks that the certified intervals +// tile the whole domain for every pair. +// +// The corpus is three certified runs: one hand-built world whose two pairs are +// built to certify at very different depths, plus two small random worlds. +// Below it is a table of mutations, each applied to every corpus case; a +// mutation any case accepts is a hole in the audit. certifier_test.cc covers a +// handful of single-case mutations on its own world, so this file is the sweep +// plus the mutation classes that need a designed pair structure (record +// relabelling) or a second run (kFindFirstViolation and non-free verdicts). #include #include @@ -68,10 +60,9 @@ using drake::trajectories::BezierCurve; using Eigen::Vector3d; using Eigen::VectorXd; -/// A non-zero margin *and* a non-zero environment padding, so that -/// m_p = margin + padding is a number a tamperer could plausibly try to lower -/// and the "threshold below what the options call for" branch has something to -/// bite on. +// A non-zero margin and a non-zero environment padding, so m_p = margin + +// padding is a number a tamperer could plausibly try to lower and the +// "threshold below what the options call for" branch has something to bite on. constexpr double kMargin = 0.005; constexpr double kEnvPadding = 0.002; @@ -108,16 +99,14 @@ std::unique_ptr MakeChecker( // A 2-dof Cartesian gantry (prismatic x, prismatic y) carrying a 5 mm sphere, // with exactly two unfiltered pairs: // -// * tool vs. "near_plate" — a 1 mm plate parallel to the travel, offset in y -// so the clearance is a constant 12 mm. With m_p = 0.007 and λ = 1 for the -// moving x coordinate, certification needs Δ = w_x < 0.012 − 0.007 − τ ≈ -// 0.005, i.e. a node no wider than ~1/64 of the 0.6 m travel: this pair -// only certifies at depth 6, producing dozens of records. -// * tool vs. "far_ball" — 3 m away, certified by the sphere prefilter at the -// root: exactly one record per segment. +// * tool vs. "near_plate": a 1 mm plate offset in y so the clearance is a +// constant 12 mm. With m_p = 0.007 and λ = 1 for the moving x coordinate, +// certification needs Δ = w_x < 0.012 − 0.007 − τ ≈ 0.005 against 0.6 m of +// travel, so it first certifies at depth 6 and produces dozens of records. +// * tool vs. "far_ball": 3 m away, certified by the sphere prefilter at the +// root, so exactly one record per segment. // -// Two pairs that could not be more different in how hard they are to certify is -// exactly what the record-relabelling mutation needs. +// The record-relabelling mutation needs two pairs this far apart in difficulty. std::unique_ptr> MakeDesignedWorld() { RobotDiagramBuilder builder; MultibodyPlant& plant = builder.plant(); @@ -146,7 +135,8 @@ std::unique_ptr> MakeDesignedWorld() { } // --------------------------------------------------------------------------- -// Worlds 2, 3 (small random): a trimmed copy of the T4 generator. +// Worlds 2, 3 (small random): a trimmed copy of the generator in +// soundness_fuzz_test.cc. // --------------------------------------------------------------------------- std::unique_ptr> MakeRandomWorld(uint64_t seed) { @@ -155,8 +145,8 @@ std::unique_ptr> MakeRandomWorld(uint64_t seed) { return std::uniform_real_distribution(lo, hi)(rng); }; // Named locals throughout: sibling constructor arguments are evaluated in an - // unspecified order, so drawing variates inline would make these worlds — and - // therefore which seeds land in the corpus — depend on the toolchain. + // unspecified order, so drawing variates inline would make these worlds, and + // therefore which seeds land in the corpus, depend on the toolchain. const auto vector3 = [&uniform](double lo, double hi) { const double x = uniform(lo, hi); const double y = uniform(lo, hi); @@ -221,11 +211,10 @@ std::unique_ptr> MakeRandomWorld(uint64_t seed) { return builder.Build(); } -/// A cubic Bézier whose control points are equally spaced from `start` to -/// `end` — the straight segment, but with four control points, so a mutation -/// can perturb an *interior* one without moving either endpoint (which would -/// change the path's start configuration and short-circuit the check we mean to -/// exercise). +// A cubic Bézier whose control points are equally spaced from `start` to `end`. +// It is the straight segment, but with four control points, so a mutation can +// perturb an interior one without moving either endpoint; moving an endpoint +// would change the path's start configuration and short-circuit the check. Eigen::MatrixXd CubicControlPoints(const VectorXd& start, const VectorXd& end) { Eigen::MatrixXd points(start.size(), 4); for (int j = 0; j < 4; ++j) { @@ -246,11 +235,11 @@ struct AuditCase { Eigen::MatrixXd control_points; std::optional path; Certificate certificate; - /// true when this case's two pairs were designed to have wildly different - /// certification depths (only the hand-built world). + // True when this case's two pairs were built to have wildly different + // certification depths (only the hand-built world). bool designed{false}; - /// The path a verifier would be handed if one control point were nudged. + // The path a verifier would be handed if one control point were nudged. PiecewiseBezierPath PerturbedPath(double delta) const { Eigen::MatrixXd points = control_points; points(0, 1) += delta; @@ -263,18 +252,17 @@ struct AuditCase { } }; -/// Builds the corpus once. Everything in it is a run that ended -/// Verdict::kCertifiedFree with an emitted certificate; a case that failed to -/// certify is *not* added, so CorpusIsBuiltAndVerifies (which requires three -/// cases, the designed one first) is the single place that reports the problem. -/// No gtest assertion is used in here: this initializer runs inside whichever -/// test happens to touch Corpus() first, which changes under --gtest_filter or -/// --gtest_shuffle, and a failure charged to an arbitrary test is a failure -/// nobody can read. -/// -/// The vector is deliberately 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.) +// Builds the corpus once. Every entry is a run that ended +// Verdict::kCertifiedFree with an emitted certificate; a case that failed to +// certify is dropped rather than added, so CorpusIsBuiltAndVerifies is the +// single place that reports a short corpus. No gtest assertion belongs here: +// this initializer runs inside whichever test touches Corpus() first, which +// changes under --gtest_filter or --gtest_shuffle, and a failure charged to an +// arbitrary test is a failure nobody can read. +// +// The vector is allocated and never freed because it owns RobotDiagrams and +// checkers whose destruction would otherwise race Drake's static teardown. LSan +// will report it if an asan preset is ever added. const std::vector>& Corpus() { static const std::vector>* corpus = [] { auto* cases = new std::vector>(); @@ -302,9 +290,9 @@ const std::vector>& Corpus() { } } - // 2. Small random worlds — the first two seeds whose trajectory certifies. - // Sweeping deterministically (rather than hard-coding lucky seeds) keeps - // the corpus honest if the geometry ever shifts underneath it. + // 2. Small random worlds: the first two seeds whose trajectory certifies. + // Sweeping deterministically, rather than hard-coding lucky seeds, still + // fills the corpus if the geometry ever shifts underneath it. for (uint64_t seed = 1; seed <= 40 && cases->size() < 3; ++seed) { auto entry = std::make_unique(); entry->name = "random_world_seed_" + std::to_string(seed); @@ -331,7 +319,7 @@ const std::vector>& Corpus() { return *corpus; } -/// Record counts per pair, for picking "the hardest" and "the easiest" pair. +// Record counts per pair, for picking "the hardest" and "the easiest" pair. std::vector RecordsPerPair(const AuditCase& entry) { std::vector counts(entry.certificate.pairs.size(), 0); for (const CertificateRecord& record : entry.certificate.records) { @@ -340,10 +328,10 @@ std::vector RecordsPerPair(const AuditCase& entry) { return counts; } -/// True iff `pair`'s records cover [0, 1] of every segment — the same coverage -/// property VerifyCertificate checks, re-derived here so a test can assert that -/// a mutation left coverage *intact* and therefore had to be caught by the -/// per-record arithmetic instead. +// True iff `pair`'s records cover [0, 1] of every segment. This is the coverage +// property VerifyCertificate checks, re-derived here so a test can assert that +// a mutation left coverage intact and therefore had to be caught by the +// per-record arithmetic instead. bool TilesEverySegment(const Certificate& certificate, int pair, std::size_t num_segments) { for (std::size_t segment = 0; segment < num_segments; ++segment) { @@ -365,8 +353,8 @@ bool TilesEverySegment(const Certificate& certificate, int pair, return true; } -/// Index of a record whose pair the trajectory actually moves and whose -/// interval is a proper sub-interval — the kind a tamperer would target. +// Index of a record whose pair the trajectory actually moves and whose interval +// is a proper sub-interval, which is the kind a tamperer would target. int MovingRecordIndex(const AuditCase& entry) { const MotionBoundTable table = entry.checker->ComputeMotionBounds(*entry.path); @@ -412,11 +400,11 @@ GTEST_TEST(CertificateAuditTest, DesignedWorldHasTheIntendedPairStructure) { const int easiest = *std::min_element(counts.begin(), counts.end()); // The far pair certifies at the root: exactly one record, for the path's one // segment. The 12 mm pair needs Δ = w_x < 0.012 − 0.007 − τ ≈ 0.005 against - // 0.6 m of travel, i.e. a node half-width of 0.3/2^d < 0.005 ⇒ d = 6, and a - // constant clearance means *every* depth-6 node certifies it: 2^6 = 64 - // records. Pinned exactly, so a regression that loosened (or tightened) the - // motion bound by even one bisection level shows up here rather than hiding - // behind an inequality. + // 0.6 m of travel, i.e. a node half-width of 0.3/2^d < 0.005 => d = 6, and a + // constant clearance means every depth-6 node certifies it: 2^6 = 64 records. + // Pinned exactly, so a regression that loosened or tightened the motion bound + // by even one bisection level shows up here rather than hiding behind an + // inequality. EXPECT_EQ(easiest, 1); EXPECT_EQ(hardest, 64); // Every pair's records must claim the same, correct threshold. @@ -426,13 +414,13 @@ GTEST_TEST(CertificateAuditTest, DesignedWorldHasTheIntendedPairStructure) { } // --------------------------------------------------------------------------- -// 2. Adversarial mutations, applied to every corpus case. +// 2. Mutations, each applied to every corpus case. // --------------------------------------------------------------------------- using Mutation = std::function; -/// Applies `mutate` to every corpus case and requires the result to be -/// rejected. `mutate` returns false when the case cannot host the mutation. +// Applies `mutate` to every corpus case and requires the result to be rejected. +// `mutate` returns false when the case cannot host the mutation. void ExpectRejectedEverywhere(const std::string& what, const Mutation& mutate) { int applied = 0; for (const auto& entry : Corpus()) { @@ -482,7 +470,7 @@ GTEST_TEST(CertificateAuditTest, RejectsShiftedRepresentativeConfiguration) { GTEST_TEST(CertificateAuditTest, RejectsDeletedRecord) { // The certifier's intervals tile the domain disjointly, so deleting any - // record punches a coverage hole — even one whose own arithmetic was sound. + // record punches a coverage hole, even one whose own arithmetic was sound. ExpectRejectedEverywhere( "delete a record", [](const AuditCase&, Certificate* certificate) { if (certificate->records.size() < 2) return false; @@ -502,7 +490,7 @@ GTEST_TEST(CertificateAuditTest, RejectsTruncatedRecords) { } GTEST_TEST(CertificateAuditTest, RejectsLoweredThreshold) { - // Lower *every* record of one pair, so the replay's self-consistency check + // Lower every record of one pair, so the replay's self-consistency check // ("all records of a pair claim the same threshold") passes and the mutation // has to be caught by the check that actually matters: the claimed threshold // must be at least the margin + padding the options call for. @@ -535,11 +523,11 @@ GTEST_TEST(CertificateAuditTest, RejectsPairTableMismatch) { } GTEST_TEST(CertificateAuditTest, RejectsRelabelledPairRecords) { - // Relabelling records between two pairs of *similar* difficulty can be a true + // Relabelling records between two pairs of similar difficulty can be a true // statement about a claim nobody made, so this mutation is only meaningful // where the pair structure is designed: give the 12 mm pair the far ball's - // single root-wide record and its motion bound (half the 0.6 m travel) - // swamps its 5 mm of slack. + // single root-wide record and its motion bound, half the 0.6 m travel, swamps + // its 5 mm of slack. ASSERT_FALSE(Corpus().empty()); const AuditCase& entry = *Corpus().front(); ASSERT_TRUE(entry.designed); @@ -558,8 +546,8 @@ GTEST_TEST(CertificateAuditTest, RejectsRelabelledPairRecords) { record.pair_index = hardest; } } - // Relabelling permutes two complete tilings, so coverage is *not* what - // catches this — verified rather than asserted, because a mutation that + // Relabelling permutes two complete tilings, so coverage is not what catches + // this. That is verified rather than assumed, because a mutation that // happened to break coverage would make the test pass for the wrong reason // and leave the arithmetic untested. for (int pair = 0; pair < static_cast(certificate.pairs.size()); @@ -574,9 +562,9 @@ GTEST_TEST(CertificateAuditTest, RejectsRelabelledPairRecords) { } GTEST_TEST(CertificateAuditTest, RejectsPerturbedPath) { - // The certificate is a statement about one specific path. Handing the - // verifier a path with a nudged interior control point must not verify: every - // record's qc stops being the midpoint apex of the interval it names. + // The certificate is a statement about one specific path. A path with a + // nudged interior control point must not verify: every record's qc stops + // being the midpoint apex of the interval it names. for (const auto& entry : Corpus()) { SCOPED_TRACE(entry->name); const PiecewiseBezierPath perturbed = entry->PerturbedPath(0.05); @@ -586,7 +574,7 @@ GTEST_TEST(CertificateAuditTest, RejectsPerturbedPath) { } // --------------------------------------------------------------------------- -// 3. What a *valid* transformation looks like — pinned deliberately. +// 3. What a valid transformation looks like. // --------------------------------------------------------------------------- GTEST_TEST(CertificateAuditTest, AcceptsReorderedRecords) { @@ -595,8 +583,8 @@ GTEST_TEST(CertificateAuditTest, AcceptsReorderedRecords) { // sorts the intervals itself before checking coverage and every record is // checked independently, so order carries no information. Pinning this keeps // a future "records must arrive sorted" shortcut from being mistaken for a - // security property — and keeps the mutations above honest, since a verifier - // that rejected everything would pass all of them. + // security property, and it rules out a verifier that rejects everything, + // which would pass every mutation above. std::mt19937 rng(20260826); int shuffled = 0; for (const auto& entry : Corpus()) { @@ -613,7 +601,7 @@ GTEST_TEST(CertificateAuditTest, AcceptsReorderedRecords) { } // --------------------------------------------------------------------------- -// 4. Certificates a run cannot honestly produce. +// 4. Runs whose certificate is not a proof. // --------------------------------------------------------------------------- GTEST_TEST(CertificateAuditTest, NoCertificateUnlessRequested) { @@ -628,8 +616,8 @@ GTEST_TEST(CertificateAuditTest, NoCertificateUnlessRequested) { EXPECT_FALSE(result.certificate.has_value()); } -/// The designed world again, but driven straight through the 1 mm plate at -/// y = 0.0175: q(t) sweeps y from 0 to 0.05 while x crosses the plate's span. +// The designed world again, but driven straight through the 1 mm plate at +// y = 0.0175: q(t) sweeps y from 0 to 0.05 while x crosses the plate's span. Eigen::MatrixXd ViolatingControlPoints() { VectorXd start(2), end(2); start << -0.3, 0.0; @@ -638,12 +626,11 @@ Eigen::MatrixXd ViolatingControlPoints() { } GTEST_TEST(CertificateAuditTest, NonFreeVerdictCertificateIsNotAProof) { - // Pinned behaviour for "the certificate of a non-free run is absent or - // unusable": the field is *present* whenever emit_certificate was asked for, - // and the records the run did make are individually valid — but a run that - // found a violation dropped that pair from the subtree instead of certifying - // it, so the trail cannot cover the domain and the replay refuses it. The - // resolution is "usable as an audit trail, unusable as a proof". + // The certificate field is present whenever emit_certificate was asked for, + // and the records the run did make are individually valid. A run that found a + // violation dropped that pair from the subtree instead of certifying it, so + // the trail cannot cover the domain and the replay refuses it: usable as an + // audit trail, unusable as a proof. ASSERT_FALSE(Corpus().empty()); const AuditCase& entry = *Corpus().front(); const Options options = AuditOptions(); @@ -658,9 +645,9 @@ GTEST_TEST(CertificateAuditTest, NonFreeVerdictCertificateIsNotAProof) { EXPECT_FALSE(VerifyCertificate(*entry.checker, path, *violating.certificate)) << "a certificate from a violating run must not read as a proof"; - // Same for a run stopped by the node budget. That needs the *free* - // trajectory: a definite violation outranks budget exhaustion in the verdict - // reduction, so the budget branch is only reachable when nothing violates. + // Same for a run stopped by the node budget. That needs the free trajectory: + // a definite violation outranks budget exhaustion in the verdict reduction, + // so the budget branch is only reachable when nothing violates. Options budgeted = options; budgeted.max_nodes = 3; const BezierCurve free_trajectory(0.0, 1.0, entry.control_points); @@ -670,7 +657,7 @@ GTEST_TEST(CertificateAuditTest, NonFreeVerdictCertificateIsNotAProof) { ASSERT_TRUE(truncated.certificate.has_value()); EXPECT_FALSE(entry.Verify(*truncated.certificate)); - // ... and for kFindFirstViolation, which additionally *prunes* the search: + // ... and for kFindFirstViolation, which additionally prunes the search: // every node starting after the witness is skipped, so whole stretches of the // domain are never visited at all. Options find_first = options; diff --git a/planning/continuous_collision/test/certifier_test.cc b/planning/continuous_collision/test/certifier_test.cc index d6e52836f50e..510a83bee9c1 100644 --- a/planning/continuous_collision/test/certifier_test.cc +++ b/planning/continuous_collision/test/certifier_test.cc @@ -1,11 +1,10 @@ -/// @file -/// End-to-end tests of the certifier core and the public facade (the test plan, -/// T4/T6/T7 restricted to a focused corpus; the large randomized T4 fuzz -/// corpus is a separate milestone and deliberately not 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. +// 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 and is not 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 @@ -65,14 +64,14 @@ SpatialInertia UnitInertia() { return SpatialInertia::SolidSphereWithMass(1.0, 0.05); } -/// A planar 3-dof arm (revolute, revolute, prismatic) in the z = 0 plane: -/// -/// 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] -/// -/// 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. +// 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", UnitInertia()); const RigidBody& link2 = plant->AddRigidBody("link2", UnitInertia()); @@ -106,9 +105,9 @@ void AddWeldedSphere(MultibodyPlant* plant, const std::string& name, 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. +// 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(); @@ -134,9 +133,9 @@ std::shared_ptr> MakeArmWorld() { return std::shared_ptr>(builder.Build()); } -/// 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 — exactly r_tool + r_graze + kMargin. +// 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. std::shared_ptr> MakeGrazeWorld() { RobotDiagramBuilder builder; MultibodyPlant& plant = builder.plant(); @@ -145,9 +144,9 @@ std::shared_ptr> MakeGrazeWorld() { return std::shared_ptr>(builder.Build()); } -/// A genuinely free squeeze: the tool slides between two spheres that leave -/// only 5 mm of clearance over the margin, so the certificate is real but has -/// to be earned by subdividing (the mirror image of the tangency world). +// A genuinely free squeeze: the tool slides between two spheres that leave +// only 5 mm of clearance over the margin, so the certificate is real but has +// to be earned by subdividing (the mirror image of the tangency world). std::shared_ptr> MakeGapWorld() { RobotDiagramBuilder builder; MultibodyPlant& plant = builder.plant(); @@ -174,9 +173,9 @@ Options SerialOptions() { return options; } -/// A cubic Bézier from `start` to `end` with linearly spaced control points -/// (so the curve is the straight segment, traversed with a nontrivial -/// parametrization) over the time interval [t0, t1]. +// A cubic Bézier from `start` to `end` with linearly spaced control points +// (so the curve is the straight segment, traversed with a nontrivial +// parametrization) over the time interval [t0, t1]. BezierCurve MakeBezier(const VectorXd& start, const VectorXd& end, int order, double t0, double t1) { Eigen::MatrixXd control_points(start.size(), order + 1); @@ -187,17 +186,18 @@ BezierCurve MakeBezier(const VectorXd& start, const VectorXd& end, return BezierCurve(t0, t1, control_points); } -/// Result of the dense-sampling cross-check. +// 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. + // 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 (separately tested, T3) distance oracle so that -/// halfspace pairs are handled the same way. +// 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 ContinuousCollisionChecker& checker, const PiecewiseBezierPath& path, int samples_per_segment, double threshold) { @@ -229,8 +229,8 @@ SampledClearance SampleClearance(const ContinuousCollisionChecker& checker, return result; } -/// Re-evaluates one finding's configuration from scratch and returns the -/// oracle distance of its pair there. +// Re-evaluates one finding's configuration from scratch and returns the +// oracle distance of its pair there. double DistanceAtFinding(const ContinuousCollisionChecker& checker, const Finding& finding) { const RobotDiagram& model = checker.model(); @@ -377,7 +377,7 @@ GTEST_TEST(CertifierTest, FindFirstReturnsEarliestWitness) { } // --------------------------------------------------------------------------- -// 3. Grazing tangency is inconclusive — never certified free. +// 3. Grazing tangency is inconclusive, never certified free. // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { @@ -420,9 +420,9 @@ GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { // filters every pair *within* a welded subgraph, so two anchored obstacles (or // two members of a welded cluster on the robot) never even reach the checker // as a candidate pair. The reachable source of J(p) = ∅ is therefore the -// constant-coordinate carve-out of trajectory normalization; the joint-support -// scope: a coordinate that no control point of the trajectory moves is removed -// from every J(p), and pairs left with an empty set are resolved once at q(t0). +// constant-coordinate carve-out: a coordinate that no control point of the +// trajectory moves is removed from every J(p), and pairs left with an empty +// set are resolved once at q(t0). GTEST_TEST(CertifierTest, StaticPairsResolvedOnce) { const auto model = MakeArmWorld(); const auto checker = MakeChecker(model, SerialOptions()); @@ -450,8 +450,8 @@ GTEST_TEST(CertifierTest, StaticPairsResolvedOnce) { ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); ASSERT_TRUE(result.certificate.has_value()); - // A static pair is certified exactly once — one full-segment record per - // segment, all sharing the single representative configuration q(t0) — and + // A static pair is certified exactly once: one full-segment record per + // segment, all sharing the single representative configuration q(t0). It // never appears in a node record. const int num_segments = static_cast(path.segments().size()); std::vector records_per_pair(table.num_pairs(), 0); @@ -493,8 +493,8 @@ GTEST_TEST(CertifierTest, PaddingSemantics) { }; // The one robot-vs-robot pair (link1, tool) keeps ≈ 0.27 m of clearance on - // this trajectory, so 0.4 m of *self* padding must break it — and nothing - // else, because every other pair has an anchored side and takes the (zero) + // this trajectory, so 0.4 m of *self* padding must break it, and nothing + // else: every other pair has an anchored side and takes the (zero) // environment padding. { ContinuousCollisionChecker::Params params; @@ -550,7 +550,7 @@ GTEST_TEST(CertifierTest, PaddingSemantics) { } // --------------------------------------------------------------------------- -// 5. Retiming invariance (T6): the certificate is a property of the path. +// 5. Retiming invariance: the certificate is a property of the path. // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, RetimingInvariance) { @@ -592,7 +592,7 @@ GTEST_TEST(CertifierTest, RetimingInvariance) { } // --------------------------------------------------------------------------- -// 6. Certificate emission, replay and mutation (T7). +// 6. Certificate emission, replay and mutation. // --------------------------------------------------------------------------- class CertificateFixture : public ::testing::Test { @@ -608,8 +608,8 @@ class CertificateFixture : public ::testing::Test { result_ = checker_.CheckTrajectory(trajectory_, options); } - /// Index of a record belonging to a pair the trajectory actually moves (so - /// the record carries a real node interval, not the global static one). + // Index of a record belonging to a pair the trajectory actually moves (so + // the record carries a real node interval, not the global static one). int MovingRecordIndex() const { const MotionBoundTable table = checker_.ComputeMotionBounds(path_); for (int i = 0; i < static_cast(result_.certificate->records.size()); @@ -714,8 +714,8 @@ GTEST_TEST(CertifierTest, CertificateRejectsRebasedStaticRecord) { EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); // Both directions: rotating θ1 toward the obstacles reduces the clearance - // the replay measures, while rotating away *increases* it — the case only - // the "static records are pinned to q(t0)" check can catch. + // the replay measures, while rotating away *increases* it. Only the "static + // records are pinned to q(t0)" check catches that second case. for (const double delta : {1.5, -1.5}) { Certificate certificate = *result.certificate; int tampered = 0; @@ -849,8 +849,8 @@ GTEST_TEST(CertifierTest, ParallelMatchesSerialOnViolation) { GTEST_TEST(CertifierTest, ConcurrentChecksAreIndependent) { // The Check* methods are const and documented thread-safe: concurrent calls - // must lease disjoint contexts from the pool (the full T8 sweep is a later - // milestone; this is the smoke test for the lease). + // must lease disjoint contexts from the pool. test/concurrency_test.cc + // sweeps that; this is the smoke test for the lease. const auto model = MakeArmWorld(); const auto checker = MakeChecker(model, SerialOptions()); const BezierCurve free_trajectory = @@ -903,8 +903,8 @@ GTEST_TEST(CertifierTest, ViolationExactlyAtStartTime) { GTEST_TEST(CertifierTest, ViolationAtAJunctionIsReported) { const auto model = MakeArmWorld(); const auto checker = MakeChecker(model, SerialOptions()); - // A 3-waypoint path whose middle waypoint — the junction between segments, - // at t = 1 — is inside the post. + // A 3-waypoint path whose middle waypoint (the junction between segments, + // at t = 1) is inside the post. 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); @@ -926,10 +926,11 @@ GTEST_TEST(CertifierTest, ViolationAtAJunctionIsReported) { } // --------------------------------------------------------------------------- -// 9b. A small seeded soundness sweep. The full T4 corpus (random worlds, -// B-splines, 10^5 samples, hundreds of cases) is a separate milestone; -// this is the cheap standing guard that no kCertifiedFree of *this* driver -// survives dense sampling, and that every definite witness really violates. +// 9b. 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; this is the cheap standing guard that no +// kCertifiedFree of *this* driver survives dense sampling, and that every +// definite witness really violates. // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, RandomTrajectoriesAreSoundAgainstDenseSampling) { @@ -976,7 +977,7 @@ GTEST_TEST(CertifierTest, RandomTrajectoriesAreSoundAgainstDenseSampling) { } // --------------------------------------------------------------------------- -// 10. API guardrails (the full T9 suite lives in api_test). +// 10. API guardrails (the full suite lives in test/api_test.cc). // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, ApiThrowsOnDimensionMismatch) { diff --git a/planning/continuous_collision/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc index 424e7d9590fc..020fa227d929 100644 --- a/planning/continuous_collision/test/concurrency_test.cc +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -1,59 +1,39 @@ -/// @file -/// T8 — concurrency determinism (test plan T8; performance requirement -/// P7; parallelism and determinism). -/// -/// Four claims are pinned here, on the fixed corpus of ten T4-style random -/// cases (a mix of free and violating) that -/// concurrency_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} in both search -/// modes, and in kCertifyAll so are `nodes` and `narrowphase_queries` — -/// the parallel driver explores the same tree, only in a different order. -/// (In kFindFirstViolation the branch-and-bound bound arrives at different -/// times, so the *statistics* are explicitly not deterministic; the -/// reported witness still is.) -/// 2. Serial mode is bit-deterministic: two runs produce byte-identical -/// findings and statistics. -/// 3. The public Check* methods are safe to call concurrently on one checker -/// instance: eight threads hammering one checker get the same answers as -/// running the same calls one after another. -/// 4. The deep workload — the only one big enough that the driver actually -/// hires helpers, which no corpus case is — explores the same tree and -/// reports the same findings at every thread count, and keeps doing so -/// when several callers ask for it at once. This is where the sharing -/// path gets its coverage, TSan's included. -/// -/// Every case here is an equality, not a wall-clock claim, so this target runs -/// under every build flavor. The two timing claims that used to live here — a -/// deep tree gets faster with threads, a small check does not get slower — are -/// in concurrency_timing_test.cc, which is excluded from the build flavors -/// that make a duration meaningless. -/// -/// TSan. This file is the test to run under ThreadSanitizer. Drake's -/// build carries a `tsan` config, so the invocation is: -/// -/// 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 with -/// "unexpected memory mapping" before main ever runs; running the test -/// binary under `setarch $(uname -m) -R` (or lowering vm.mmap_rnd_bits to -/// 28) is the standard workaround. -/// -/// Result on Drake ~v1.45 at the time of writing: clean — no data races -/// reported over repeated runs, so no suppression file is shipped. That was -/// measured against a prebuilt (uninstrumented) Drake, so TSan saw only -/// continuous_collision frames. It sees all of the -/// driver's shared mutable state, though — the work queue, the findings sink, -/// the atomic node counter and bound, and the context pool are all ours — which -/// is exactly the surface the design claims is the only one there is. If a -/// future pin -/// does produce reports rooted entirely in Drake, triage them and park -/// them in a suppression file (TSAN_OPTIONS=suppressions=...); anything -/// rooted in a -/// continuous_collision frame is a real bug. +// Concurrency determinism. +// +// Four claims are pinned here, on the fixed corpus of ten random cases (a mix +// of free and violating) that concurrency_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} in both search +// modes, and in kCertifyAll so are `nodes` and `narrowphase_queries`: +// the parallel driver explores the same tree in a different order. (In +// kFindFirstViolation the branch-and-bound bound arrives at different +// times, so the statistics are not deterministic; the reported witness +// still is.) +// 2. Serial mode is bit-deterministic: two runs produce byte-identical +// findings and statistics. +// 3. The public Check* methods are safe to call concurrently on one checker +// instance: eight threads sharing one checker get the same answers as +// the same calls made one after another. +// 4. 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 findings at every thread count, and keeps doing so when several +// callers ask for it at once. +// +// Every case is an equality, not a wall-clock claim, so this target runs under +// every build flavor; the timing claims live in concurrency_timing_test.cc. +// +// 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 +// (TSAN_OPTIONS=suppressions=...). #include #include @@ -113,8 +93,8 @@ GTEST_TEST(ConcurrencyTest, VerdictAndEarliestWitnessAreThreadCountInvariant) { GTEST_TEST(ConcurrencyTest, CertifyAllIsFullyThreadCountInvariant) { // In kCertifyAll every node's decision depends only on its own control points - // and inherited active set, so the *whole* tree — and therefore every - // statistic and every finding — is thread-count independent, not just the + // and inherited active set, so the *whole* tree, and therefore every + // statistic and every finding, is thread-count independent, not just the // earliest witness. for (const auto& entry : Corpus()) { const BezierCurve trajectory = entry->trajectory(); @@ -138,11 +118,10 @@ GTEST_TEST(ConcurrencyTest, CertifyAllIsFullyThreadCountInvariant) { GTEST_TEST(ConcurrencyTest, FindFirstViolationStatisticsAreAllowedToDiffer) { // The complement of the test above, pinned so that a future reader does not - // "fix" a statistics mismatch that the design explicitly permits: under - // branch-and-bound the number of nodes a run visits depends on when the - // atomic bound tightens, which depends on timing. Only the answer is - // deterministic. (The assertion is therefore on the *witness*, and the - // statistics are merely reported.) + // "fix" an expected statistics mismatch: under branch-and-bound the number of + // nodes a run visits depends on when the atomic bound tightens, which depends + // on timing. Only the answer is deterministic, so the assertion is on the + // *witness* and the statistics are merely reported. int cases_with_differing_stats = 0; int examined = 0; for (const auto& entry : Corpus()) { @@ -165,15 +144,15 @@ GTEST_TEST(ConcurrencyTest, FindFirstViolationStatisticsAreAllowedToDiffer) { } // Without this the `continue` above could silently empty the test. EXPECT_GE(examined, kMinViolatingCases); - std::cout << "\n[ T8 ] kFindFirstViolation: node counts differed between 1 " - "and 16 threads on " + std::cout << "\n[ concurrency ] kFindFirstViolation: node counts differed " + "between 1 and 16 threads on " << cases_with_differing_stats << " of the " << examined << " violating cases; the reported witness was identical on all of " "them.\n\n"; } // --------------------------------------------------------------------------- -// 2. Serial mode is bit-deterministic (the performance requirements, P7). +// 2. Serial mode is bit-deterministic. // --------------------------------------------------------------------------- GTEST_TEST(ConcurrencyTest, SerialModeIsBitDeterministic) { @@ -326,10 +305,9 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { // --------------------------------------------------------------------------- // // The sharing path only ever runs on a workload big enough to hire a helper, -// which the corpus cases of claims 1-3 never are. These cases are where it -// gets its coverage — including its TSan coverage — and they are equalities, -// so unlike the wall-clock claims in concurrency_timing_test.cc they run -// everywhere. +// which the corpus cases of claims 1-3 never are. These cases are where it gets +// its coverage, TSan's included, and they are equalities, so unlike the +// wall-clock claims in concurrency_timing_test.cc they run everywhere. GTEST_TEST(ConcurrencyTest, DeepWorkloadIsBigEnoughToBeWorthSpreading) { // Without this the two tests below could silently degenerate into measuring @@ -337,16 +315,16 @@ GTEST_TEST(ConcurrencyTest, DeepWorkloadIsBigEnoughToBeWorthSpreading) { const DeepWorkload& deep = Deep(); ASSERT_NE(deep.entry, nullptr); EXPECT_GE(deep.nodes, kMinDeepNodes) << "grazing margin " << deep.margin; - std::cout << "\n[ T8 ] deep workload: " << deep.entry->name << ", margin " - << deep.margin << ", " << deep.nodes << " nodes at min_interval " - << deep.min_interval << "\n\n"; + std::cout << "\n[ concurrency ] deep workload: " << deep.entry->name + << ", margin " << deep.margin << ", " << deep.nodes + << " nodes at min_interval " << deep.min_interval << "\n\n"; } GTEST_TEST(ConcurrencyTest, DeepWorkloadIsThreadCountInvariant) { // The scaling test below only proves work moved between threads; this proves // the *same* work moved. It runs in every build, sanitizers included, and is - // where the sharing path gets its TSan coverage — the corpus cases of the - // tests above are too small to ever hire a helper. + // where the sharing path gets its TSan coverage, because the corpus cases of + // the tests above are too small to ever hire a helper. const DeepWorkload& deep = Deep(); ASSERT_NE(deep.entry, nullptr); const BezierCurve trajectory = deep.entry->trajectory(); diff --git a/planning/continuous_collision/test/concurrency_test_utilities.h b/planning/continuous_collision/test/concurrency_test_utilities.h index 38ba2068cae0..8d79dc0f5152 100644 --- a/planning/continuous_collision/test/concurrency_test_utilities.h +++ b/planning/continuous_collision/test/concurrency_test_utilities.h @@ -1,15 +1,12 @@ #pragma once -/// @file -/// The shared fixture of the two T8 concurrency targets: the random corpus -/// that `concurrency_test.cc` pins the driver's determinism against, and the -/// deliberately deep workload that both it and `concurrency_timing_test.cc` -/// need. It lives in a header because each target wants its own copy — the -/// corpus and the deep workload are lazily built per binary — and because -/// duplicating three hundred lines of world generation between the two files -/// would be worse than sharing them. -/// -/// Nothing here asserts; the claims live in the two test files. +// The shared fixture of the two concurrency targets: the random corpus that +// `concurrency_test.cc` pins the driver's determinism against, and the deep +// workload that both it and `concurrency_timing_test.cc` need. Each target +// builds its own copy lazily, so the fixture lives in a header rather than +// duplicating three hundred lines of world generation between the two files. +// +// Nothing here asserts; the claims live in the two test files. #include #include @@ -60,9 +57,9 @@ using Eigen::Vector3d; using Eigen::VectorXd; constexpr double kMargin = 0.005; -/// Ten cases keeps the full 4-thread-count × 2-mode sweep (80 certification -/// runs) plus the concurrent-call test under a second in Release, which is what -/// makes this affordable to run again under TSan (~100× slower). +// Ten cases keeps the full 4-thread-count × 2-mode sweep (80 certification +// runs) plus the concurrent-call test under a second in Release, which is what +// makes this affordable to run again under TSan (~100× slower). constexpr int kNumCases = 10; constexpr int kMinFreeCases = 3; constexpr int kMinViolatingCases = 3; @@ -75,9 +72,9 @@ inline SpatialInertia Inertia() { return SpatialInertia::SolidSphereWithMass(1.0, 0.05); } -/// A four-link chain of revolute and prismatic joints with primitive geometry, -/// four anchored obstacles and (on odd seeds) a HalfSpace floor, so the corpus -/// exercises the native narrowphase route and the analytic one. +// A four-link chain of revolute and prismatic joints with primitive geometry, +// four anchored obstacles and (on odd seeds) a HalfSpace floor, so the corpus +// exercises the native narrowphase route and the analytic one. inline std::unique_ptr> MakeWorld(uint64_t seed) { std::mt19937_64 rng(seed); const auto uniform = [&rng](double lo, double hi) { @@ -174,8 +171,8 @@ inline std::unique_ptr> MakeWorld(uint64_t seed) { return builder.Build(); } -/// A quintic Bézier with random control points, so the corpus has real curved -/// trajectories rather than straight edges. +// A quintic Bézier 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) { std::mt19937_64 rng(seed ^ 0xa5a5'5a5a'0f0f'f0f0ull); std::uniform_real_distribution value(-1.4, 1.4); @@ -208,13 +205,12 @@ struct Case { } }; -/// 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 deliberately 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.) +// 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>(); @@ -253,9 +249,9 @@ inline const std::vector>& Corpus() { return *corpus; } -/// Bit-for-bit equality of two findings. Nothing here is a tolerance: two runs -/// of the same deterministic computation either agree exactly or the claim of -/// determinism is false. +// Bit-for-bit equality of two findings. 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 FindingsIdentical( const std::vector& a, const std::vector& b) { if (a.size() != b.size()) { @@ -308,32 +304,30 @@ inline ::testing::AssertionResult EarliestWitnessIdentical( return FindingsIdentical({a.findings.front()}, {b.findings.front()}); } -/// The bisection's node budget in Deep() doubles as the deep workload's size: -/// the margin it converges to is the largest one still certifiable inside this -/// budget, so the tree it produces has just under this many nodes. Large -/// enough that a run takes tens of milliseconds (a wall-clock ratio then means -/// something) and that no fixed seeding depth could ever have covered it; -/// small enough that the ~40 probes that find it, and the timed repetitions -/// concurrency_timing_test.cc runs on it, stay cheap — under a sanitizer too. -/// -/// kMinDeepNodes is the floor concurrency_test.cc holds the result to, so the -/// workload cannot silently degenerate if the corpus or the bisection drifts. +// The bisection's node budget in Deep() doubles as the deep workload's size: +// the margin it converges to is the largest one still certifiable inside this +// budget, so the tree it produces has just under this many nodes. Large enough +// that a run takes tens of milliseconds (a wall-clock ratio then means +// something) and that no fixed seeding depth could ever have covered it; small +// enough that the ~40 probes that find it, and the timed repetitions +// concurrency_timing_test.cc runs on it, stay cheap, sanitizers included. +// +// 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 = 3000; -/// A corpus case run at a margin just below its own swept clearance, which is -/// what makes the subdivision tree deep and *narrow* (the soundness argument): -/// certifying a node needs φ̂ − τ − Δ > m, so as the threshold m 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 — exactly the shape a depth-seeded work -/// queue cannot split, and the shape the benchmark suite's thread-scaling -/// results measured the old driver getting 0.98× on. -/// -/// That margin is found by bisection rather than hard-coded, so the workload -/// survives any change to the random worlds, the bounds, or Drake: the largest -/// margin still certifiable within kProbeBudget nodes is by construction the -/// one that costs about kProbeBudget nodes. +// 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 +// ϕ̂ − τ − Δ > m, so as the threshold m 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: the largest +// margin still certifiable within kProbeBudget nodes is by construction the +// one that costs about kProbeBudget nodes. struct DeepWorkload { const Case* entry{}; double margin{0.0}; diff --git a/planning/continuous_collision/test/concurrency_timing_test.cc b/planning/continuous_collision/test/concurrency_timing_test.cc index e6239790a33b..cb1e34687c6f 100644 --- a/planning/continuous_collision/test/concurrency_timing_test.cc +++ b/planning/continuous_collision/test/concurrency_timing_test.cc @@ -1,17 +1,16 @@ -/// @file -/// T8 — the two per-call parallel *scaling* claims, split out of -/// concurrency_test.cc because they are wall-clock claims and it is not. -/// -/// Every other T8 case is an equality and runs under every build flavor. A -/// duration, by contrast, means nothing under an instrumented build: Valgrind -/// serializes threads outright, so `parallel < serial` inverts and the case -/// fails for a reason that has nothing to do with the driver. Hence the -/// separate target, which carries disable_in_compilation_mode_dbg and the -/// no_valgrind_tools tag, and hence TimingClaimsAreMeaningless() below, which -/// skips whatever the build tags did not already exclude. -/// -/// The corpus, the deep workload and the option defaults are shared with -/// concurrency_test.cc through concurrency_test_utilities.h. +// The two per-call parallel *scaling* claims, split out of concurrency_test.cc +// because they are wall-clock claims and it is not. +// +// Every case in concurrency_test.cc is an equality and runs under every build +// flavor. A duration, by contrast, means nothing under an instrumented build: +// Valgrind serializes threads outright, so `parallel < serial` inverts and the +// case fails for a reason that has nothing to do with the driver. Hence the +// separate target, which carries disable_in_compilation_mode_dbg and the +// no_valgrind_tools tag, and hence TimingClaimsAreMeaningless() below, which +// skips whatever the build tags did not already exclude. +// +// The corpus, the deep workload and the option defaults are shared with +// concurrency_test.cc through concurrency_test_utilities.h. #include #include @@ -31,39 +30,33 @@ namespace continuous_collision { namespace test { namespace { -// --------------------------------------------------------------------------- -// 4. Per-call parallel scaling. -// --------------------------------------------------------------------------- +// Per-call parallel scaling: the two properties the driver in +// certifier_internal.cc exists for. // -// These pin the two properties the driver rework of certifier_internal.cc -// exists for, and that the benchmark suite's thread-scaling results measured -// the old driver failing: -// -// a) a deep tree inside a single segment actually spreads over the workers -// (the old depth-seeded driver got 0.98× at 16 threads on 12 570 nodes, -// because one fixed seed held essentially the whole tree); +// a) a deep tree inside a single segment spreads over the workers, instead +// of sitting behind one fixed seed that no other worker can split; // b) a check too small to pay for workers never loses by being asked for -// them — which matters because Parallelism::Max() is the *default* value +// them, which matters because Parallelism::Max() is the *default* value // of Options::parallelism. // // Both are timing claims, so both are written to survive a loaded machine: a // ratio with a wide margin, best-of-three, and a skip when the hardware or the -// build cannot support the claim at all. They are not benchmarks — the numbers -// live in benchmark/results/ — they are regression detectors, and they should -// only ever fire on a driver that has stopped distributing work. +// build cannot support the claim at all. They are regression detectors rather +// than benchmarks (the numbers live in benchmark/results/), and should only +// ever fire on a driver that has stopped distributing work. -/// True when the build cannot support a meaningful wall-clock claim: a -/// sanitizer build serializes and inflates everything, an unoptimized build -/// changes the ratios, and fewer than eight hardware threads means there is no -/// parallelism to measure. -/// -/// The compile-time tests below only see the sanitizers this translation unit -/// was itself instrumented with. Valgrind instruments nothing at compile time, -/// and a sanitizer runtime linked in from elsewhere is equally invisible, so -/// the environment is consulted too: the tools that make a duration -/// meaningless all announce themselves through an options variable. That is -/// the same test limit_malloc.cc uses to disarm itself, and the same -/// VALGRIND_OPTS check gcs_trajectory_optimization_test.cc uses. +// True when the build cannot support a meaningful wall-clock claim: a sanitizer +// build serializes and inflates everything, an unoptimized build changes the +// ratios, and fewer than eight hardware threads means there is no parallelism +// to measure. +// +// The compile-time tests below only see the sanitizers this translation unit +// was itself instrumented with. Valgrind instruments nothing at compile time, +// and a sanitizer runtime linked in from elsewhere is equally invisible, so the +// environment is consulted too: the tools that make a duration meaningless all +// announce themselves through an options variable. That is the same test +// limit_malloc.cc uses to disarm itself, and the same VALGRIND_OPTS check +// gcs_trajectory_optimization_test.cc uses. bool TimingClaimsAreMeaningless() { #if defined(__SANITIZE_THREAD__) || defined(__SANITIZE_ADDRESS__) return true; @@ -111,12 +104,12 @@ GTEST_TEST(ConcurrencyTest, DeepWorkloadIsFasterInParallel) { const double parallel = BestOfThreeSeconds([&]() { deep.entry->checker->CheckTrajectory(trajectory, parallel_options); }); - std::cout << "\n[ T8 ] deep workload: serial " << 1e3 * serial + std::cout << "\n[ concurrency ] deep workload: serial " << 1e3 * serial << " ms, Parallelism(8) " << 1e3 * parallel << " ms (" << serial / parallel << "x)\n\n"; - // Eight threads measure ~6x on the benchmark machine; 1.43x is the bound - // that separates "the driver distributes deep work" from the old driver's - // 0.98x without being a performance assertion in disguise. + // 1.43x is the bound that separates a driver that distributes deep work from + // one that leaves it on a single worker, without being a performance + // assertion in disguise. EXPECT_LT(parallel, 0.7 * serial); } @@ -141,13 +134,12 @@ GTEST_TEST(ConcurrencyTest, SmallCheckIsNotSlowerInParallel) { const double parallel = BestOfThreeSeconds([&]() { entry.checker->CheckEdge(q1, q2, parallel_options); }); - std::cout << "\n[ T8 ] small check: serial " << 1e3 * serial + std::cout << "\n[ concurrency ] small check: serial " << 1e3 * serial << " ms, Parallelism::Max() " << 1e3 * parallel << " ms (" << serial / parallel << "x)\n\n"; - // Parity is what the driver actually delivers (it never hires for a check - // this small, so the two paths run the same code); the 1.5x bound leaves - // room for scheduler noise on a loaded machine without letting a return of - // the old 2.6x slowdown through. + // The driver never hires for a check this small, so the two paths run the + // same code and parity is what to expect; the 1.5x bound leaves room for + // scheduler noise on a loaded machine without admitting a real slowdown. EXPECT_LT(parallel, 1.5 * serial); } diff --git a/planning/continuous_collision/test/distance_oracle_test.cc b/planning/continuous_collision/test/distance_oracle_test.cc index 52cb075a06b4..1ba50ae02517 100644 --- a/planning/continuous_collision/test/distance_oracle_test.cc +++ b/planning/continuous_collision/test/distance_oracle_test.cc @@ -1,10 +1,8 @@ -/// @file -/// T3 (the test plan): distance oracle accuracy, capability-probe -/// classification, the analytic halfspace fallback, Mesh-as-convex-hull -/// semantics, and the V-polytope ingestion round trip. -/// -/// Every world is built programmatically with RobotDiagramBuilder and every -/// randomized case uses a fixed seed, so the suite is deterministic. +// Distance oracle accuracy, capability-probe classification, the analytic +// halfspace fallback, Mesh-as-convex-hull semantics, and the V-polytope +// ingestion round trip. 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" @@ -73,17 +71,17 @@ using Eigen::Matrix3Xd; using Eigen::Vector3d; constexpr double kTau = 1e-6; -/// Exactness bar for the analytic halfspace fallback and for round trips that -/// must land on identical code paths. +// 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. +// 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. +// A built RobotDiagram plus a context, with convenience accessors. class World { public: explicit World(std::unique_ptr> diagram) @@ -97,7 +95,7 @@ class World { return diagram_->plant().GetMyMutableContextFromRoot(context_.get()); } - /// Re-evaluates the query output port; call after every pose change. + // 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>( @@ -108,7 +106,7 @@ class World { diagram_->plant().SetFreeBodyPose(&plant_context(), body, X_WB); } - /// Randomizes every floating body's pose. + // Randomizes every floating body's pose. void RandomizeAll(std::mt19937* rng, double range); private: @@ -120,8 +118,8 @@ CoulombFriction Friction() { return CoulombFriction(1.0, 1.0); } -/// Deterministic random pose: uniform translation in [-range, range]^3 and a -/// uniformly distributed orientation. +// 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); @@ -139,9 +137,9 @@ void World::RandomizeAll(std::mt19937* rng, double 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. +// 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, @@ -154,7 +152,7 @@ const RigidBody& AddShapeBody(MultibodyPlant* plant, return body; } -/// The single collision geometry registered on `body_name`. +// The single collision geometry registered on `body_name`. GeometryId GeometryOf(const MultibodyPlant& plant, const std::string& body_name) { const auto& ids = @@ -163,7 +161,7 @@ GeometryId GeometryOf(const MultibodyPlant& plant, return ids.front(); } -/// Finds the probe record for the unordered pair {a, b}. +// 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()) { @@ -179,7 +177,7 @@ const PairRecord& FindPair(const DistanceOracle& oracle, GeometryId a, // Independently derived ground truth. // -------------------------------------------------------------------------- -/// Distance from a point to a box, both in the box's frame; zero inside. +// 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(); } @@ -229,10 +227,10 @@ double HalfSpaceVertices(const Vector3d& n, const Vector3d& p0, } // -------------------------------------------------------------------------- -// On-disk meshes (written once per process into Drake's temp directory). +// Mesh fixtures for the hull-semantics cases. // -------------------------------------------------------------------------- -/// The 8 corners of a box centered on its frame origin. +// The 8 corners of a box centered on its frame origin. Matrix3Xd BoxCorners(const Vector3d& half) { Matrix3Xd v(3, 8); int col = 0; @@ -251,12 +249,9 @@ const Vector3d& CubeHalf() { return half; } -/// The cube of the mesh cases, as Drake's own shipped unit cube (vertices at -/// ±1) scaled to CubeHalf(). Using the shipped asset instead of writing one -/// keeps the test off the filesystem and off any assumption about which OBJ -/// dialect Drake's reader accepts; the non-uniform Mesh/Convex scale argument -/// reproduces exactly the half-extents the analytic expectations below use, so -/// its vertices coincide with BoxCorners(CubeHalf()) to the last bit. +// 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"); @@ -271,8 +266,8 @@ 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. +// 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}}; @@ -281,11 +276,9 @@ const std::vector& LProfile() { constexpr double kLHalfHeight = 0.5; -/// A closed, genuinely non-convex L-prism. This one stays generated — it -/// encodes the analytic expectations of the mesh-vs-hull cases below and no -/// shipped asset matches them — but it is generated into memory rather than -/// into a file, so the test needs neither a temp directory nor a write that -/// could fail unnoticed. +// 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; @@ -410,8 +403,8 @@ GTEST_TEST(DistanceOracleAccuracy, SphereBoxMatchesAnalyticDistance) { // 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. +// 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 { @@ -436,8 +429,8 @@ class HalfSpaceFallbackTest : public ::testing::Test { halfspace_id_ = GeometryOf(world_->plant(), "halfspace"); } - /// A deliberately asymmetric tetrahedron: its hull vertices are exactly the - /// four input points, so the reference minimum can be written down. + // 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); @@ -573,7 +566,7 @@ TEST_F(HalfSpaceFallbackTest, ReportNamesEveryCombinationAndRoute) { // Capability probe: classification snapshot and refusals. // ========================================================================== -/// A world with one geometry of every supported shape class. +// A world with one geometry of every supported shape class. class AllShapesTest : public ::testing::Test { protected: void SetUp() override { @@ -719,10 +712,9 @@ TEST_F(AllShapesTest, IdOrderingIsSymmetricForHalfSpacePairs) { EXPECT_EQ(checked, kDynamicBodies); } -/// Records, for the certifier and benchmark authors, which (shape, shape) -/// combinations Drake's native narrowphase actually supports on the pinned -/// build. The oracle routes every halfspace pair through the analytic fallback -/// precisely because of the rows this test prints. +// Records which (shape, shape) combinations Drake's native narrowphase supports +// on the pinned build. The oracle routes every halfspace pair through the +// analytic fallback because of the rows this test prints. TEST_F(AllShapesTest, NativeSupportTableSnapshot) { const auto& inspector = world_->diagram().scene_graph().model_inspector(); const QueryObject& query = world_->query(); @@ -745,7 +737,7 @@ TEST_F(AllShapesTest, NativeSupportTableSnapshot) { } std::cout << table << std::flush; EXPECT_EQ(supported + threw, static_cast(oracle_->pairs().size())); - // All non-halfspace combinations must work natively -- exactly what the + // All non-halfspace combinations must work natively, which is what the // capability probe asserted at construction. EXPECT_GE(supported, kNativePairs); } @@ -876,10 +868,9 @@ GTEST_TEST(DistanceOracleMesh, MeshDistanceEqualsConvexHullDistance) { EXPECT_GT(penetrating, 0); } -/// Documents the semantics loudly (the geometry-support scope; the risk -/// register): 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. +// 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); @@ -976,7 +967,7 @@ GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { // V-polytope ingestion round trip. // ========================================================================== -/// A deliberately lopsided polytope. +// A lopsided polytope. Matrix3Xd PolytopeVertices() { Matrix3Xd v(3, 6); v.col(0) = Vector3d(0.00, 0.00, 0.00); @@ -988,7 +979,7 @@ Matrix3Xd PolytopeVertices() { return v; } -/// The same polytope with interior points that add nothing to the hull. +// The same polytope with interior points that add nothing to the hull. Matrix3Xd RedundantPolytopeVertices() { const Matrix3Xd v = PolytopeVertices(); Matrix3Xd r(3, v.cols() + 3); diff --git a/planning/continuous_collision/test/motion_bound_test.cc b/planning/continuous_collision/test/motion_bound_test.cc index 310c61b88931..bf0fff0fad07 100644 --- a/planning/continuous_collision/test/motion_bound_test.cc +++ b/planning/continuous_collision/test/motion_bound_test.cc @@ -1,32 +1,18 @@ -/* T2 (the test plan) — the displacement lemma and the J(p) subtree logic. This - * is the load-bearing test of the whole library: if any λ(j, p) under-bounds - * the true motion of a pair's distal side, the certifier will happily certify a - * colliding trajectory. Any failure here is a soundness bug in the kinematics - * module and must be fixed there, never by loosening this test (the - * implementation notes, item 2). - * - * Three complementary property tests run over the same random-plant corpus: - * - * (1) Atomic, per coordinate. Move exactly one coordinate j ∈ J(p) and check - * that every sampled material point of the pair's distal side D(j, p) — - * the body inside S_j — displaces, measured in the *other* body's frame, - * by at most λ(j,p)·|Δq_j|. This is the elementary step the lemma's proof - * telescopes over, and it pins λ directly. - * - * (2) Aggregate, multi-coordinate. Move all coordinates at once and check - * that the distance between any material point of A's geometry and any - * material point of B's geometry changes by at most Σ λ(j,p)·|Δq_j|. This - * is the pairwise-distance form the certifier actually consumes. Note - * that a one-sided statement ("points of B in A's frame") is NOT valid in - * general for a self-collision pair, because the distal side changes from - * joint to joint along J(p); the sum survives only because each - * telescoping step is bounded in the frame of *that step's* static side, - * and point-to-point distance is frame invariant. - * - * (3) One-sided aggregate, for pairs whose whole J(p) shares a single distal - * side (every robot-vs-environment pair): then the stronger statement - * does hold and is checked. - */ +/* 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 @@ -94,10 +80,10 @@ using Rng = std::mt19937_64; /* Absolute slack on every displacement assertion. The claims are exact mathematics; this only absorbs floating-point noise in Drake's forward - kinematics and in our own accumulation (both ~1e-15 at these magnitudes). */ + kinematics and in this test's own accumulation (both ~1e-15 here). */ constexpr double kSlack = 1e-9; -/* Options::continuity_tolerance's default — the width below which the curve +/* Options::continuity_tolerance's default: the width below which the curve module flags a coordinate constant and the carve-out removes it from every J(p). A coordinate carved on that *tolerance* can still move by up to this much, which is what MotionBoundTable::carveout_slack() charges for. */ @@ -367,7 +353,7 @@ std::vector CollisionPairs(const RobotDiagram& diagram) { } // --------------------------------------------------------------------------- -// Part 1 — J(p) subtree logic on hand-built plants. +// Part 1. J(p) subtree logic on hand-built plants. // --------------------------------------------------------------------------- /* Convenience: the position coordinates of a named joint. */ @@ -541,15 +527,13 @@ GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { } // --------------------------------------------------------------------------- -// Part 1b — the joint-type and half-space carve-outs (the joint-support scope; -// the geometry-support scope). +// Part 1b. The joint-type and half-space carve-outs. // --------------------------------------------------------------------------- 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 v1 rejects it by name (the - // displacement lemma). + // 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", UnitInertia()); @@ -636,8 +620,7 @@ GTEST_TEST(HalfSpaceRuleTest, RotatingHalfSpaceThrowsAtConstruction) { GTEST_TEST(HalfSpaceRuleTest, TranslatingHalfSpaceIsAccepted) { // Pure translation keeps every point of the half space moving by |Δq|, so - // λ = 1 is finite and correct even though the reach is not (the - // geometry-support scope). + // λ = 1 is finite and correct even though the reach is not. auto diagram = MakeHalfSpaceModel(/* halfspace_on_link = */ true, true); const KinematicsEngine engine(*diagram); const std::vector pairs = CollisionPairs(*diagram); @@ -652,8 +635,8 @@ GTEST_TEST(HalfSpaceRuleTest, TranslatingHalfSpaceIsAccepted) { /* 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 exactly where its X_FM - translation must be picked up from the control box. */ + 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(); @@ -766,20 +749,17 @@ GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { } // --------------------------------------------------------------------------- -// Part 1c — an exactly tight reach chain. +// Part 1c. An exactly tight reach chain. // -// The randomized corpus below is excellent at catching *structural* errors -// (a dropped term, a wrong distal side), but the chain walk's triangle -// inequalities are strictly slack at random poses, so a term that is merely -// too small can hide inside that slack. This model removes the slack: every -// offset lies along +x with identity rotation, so ‖a + b‖ = ‖a‖ + ‖b‖ at every -// hop and the reach is *exactly attained* by a specific material point. Each -// contribution to r therefore shows up in λ digit for digit. +// 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, +// 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 @@ -958,9 +938,8 @@ GTEST_TEST(ReachTest, ScrewLambdaIncludesPitchAndIsNecessary) { EXPECT_LE(displacement, lambda_top * 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·Δθ. This - // is the direct evidence that dropping the pitch term would be UNSOUND, not - // merely conservative. + // displacement is √(r² + (pitch/2π)²)·Δθ, strictly larger than r·Δθ, so + // dropping the pitch term would be unsound, not merely conservative. EXPECT_GT(displacement, chain.expected_reach * dtheta * (1.0 + 1e-6)) << "a screw λ of r alone would under-bound this motion"; const double exact = std::hypot(chain.expected_reach, pitch_term) * dtheta; @@ -968,7 +947,7 @@ GTEST_TEST(ReachTest, ScrewLambdaIncludesPitchAndIsNecessary) { } // --------------------------------------------------------------------------- -// Part 2 — the displacement lemma property test. +// Part 2. The displacement lemma property test. // --------------------------------------------------------------------------- struct LemmaStats { @@ -1144,8 +1123,8 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, } if (table.pair_is_static(k)) { - // A static pair may move only by the carve-out residual — exactly zero - // when every carved coordinate is exactly constant. + // A static pair may move only by the carve-out residual, which is + // exactly zero when every carved coordinate is exactly constant. Matrix3Xd before(3, pts_b.cols()); Matrix3Xd after(3, pts_b.cols()); plant.SetPositions(&ctx, q); @@ -1213,9 +1192,9 @@ GTEST_TEST(DisplacementLemmaTest, RandomPlants) { 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 that the exactly-constant case and the (load-bearing) - // sub-tolerance case each get ~500 plants: the latter is the one where the - // carved coordinates still move and carveout_slack() has to pay for them. + // three regimes so the exactly-constant and sub-tolerance cases each get + // ~500 plants; in the sub-tolerance case the carved coordinates still move + // and carveout_slack() has to pay for them. const RandomWorld world = MakeRandomWorld(&rng, /* allow_screw = */ trial % 3 == 0, 128); stats.screw_joints += world.num_screw_joints; @@ -1231,9 +1210,8 @@ GTEST_TEST(DisplacementLemmaTest, RandomPlants) { EXPECT_GE(stats.atomic_checks, 20000); EXPECT_GE(stats.aggregate_checks, 10000); EXPECT_GE(stats.one_sided_checks, 2000); - // Screw joints must actually appear: the joint-support scope lists them as - // should-have, and this test is what decides whether they are supported or - // excluded. + // Screw joints must actually appear in the corpus, 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 λ. @@ -1289,16 +1267,16 @@ GTEST_TEST(DisplacementLemmaTest, ScrewChain) { } // --------------------------------------------------------------------------- -// Part 3 — the constant-coordinate carve-out's residual. +// Part 3. The constant-coordinate carve-out's residual. // // The curve module flags a coordinate constant when its whole control-point -// range fits inside Options::continuity_tolerance. 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. If that -// residual went uncharged the certificate inequality could pass with the true -// clearance below threshold by ~1e-7 m — two orders of magnitude above -// Options::certificate_slack. MotionBoundTable::carveout_slack() is what pays -// for it, and these tests are what pin it. +// range fits inside Options::continuity_tolerance. 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 +// Options::certificate_slack. MotionBoundTable::carveout_slack() pays for it, +// and these tests pin it. // --------------------------------------------------------------------------- /* world --j_rot(revolute, ẑ)--> l1 --j_slide(prismatic, x̂)--> l2, with a @@ -1335,8 +1313,8 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { const int rot = plant.GetJointByName("j_rot").position_start(); const int slide = 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 + // 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); @@ -1390,12 +1368,12 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { 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 in v1, 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. */ +/* 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(); @@ -1482,15 +1460,14 @@ GTEST_TEST(CarveOutSlackTest, } // --------------------------------------------------------------------------- -// Part 3b — the residual of a *floating base* held constant on the tolerance. +// 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: the -// joint kinds are excluded in v1 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, with the arm coordinate -// pinned so that the slack is the *whole* bound and the check is sensitive to -// it, and then again with everything moving. +// 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: first with the arm coordinate pinned so +// the slack is the whole bound, then again with everything moving. // --------------------------------------------------------------------------- /* world --base(rpy or quaternion floating)--> b1 --jr(revolute ŷ)--> b2, with @@ -1547,12 +1524,11 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { 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 + // 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 is - // therefore both the honest test (the sample is in the box the bound is - // stated over) and the same thing Drake would compute. + // 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 = RandomRotation(&rng).ToQuaternion(); @@ -1584,11 +1560,11 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { 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 and the probe below — which drives that one - // coordinate from one end of its interval to the other — pins that single - // coefficient rather than a seven-term sum. This is what makes the corpus - // sensitive to an under-bound in any one λ̃. + // 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 = @@ -1649,8 +1625,8 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { 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. + // Σ_{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) @@ -1690,18 +1666,18 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantQuaternionFloatingBase) { } // --------------------------------------------------------------------------- -// Part 3c — an exactly tight floating-base λ̃. +// 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 inside that slack 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. Every λ̃ shows up digit for digit. +// small can hide inside that slack 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. Every λ̃ +// shows up digit for digit. // --------------------------------------------------------------------------- std::unique_ptr> MakeTightFloatingChain(bool quaternion, diff --git a/planning/continuous_collision/test/piecewise_bezier_path_test.cc b/planning/continuous_collision/test/piecewise_bezier_path_test.cc index 0dbe0d5aac8b..55d9f3655191 100644 --- a/planning/continuous_collision/test/piecewise_bezier_path_test.cc +++ b/planning/continuous_collision/test/piecewise_bezier_path_test.cc @@ -1,4 +1,4 @@ -/* T1 — curve module acceptance tests (the test plan, T1). +/* 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 @@ -183,7 +183,7 @@ GTEST_TEST(BezierEvaluation, MatchesDrakeBezierCurve) { /* 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 (trajectory normalization). */ +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); @@ -232,7 +232,8 @@ GTEST_TEST(DeCasteljau, ChildrenReproduceParent) { } /* The hot loop pre-sizes its outputs; re-splitting into already-correctly -sized buffers must not reallocate them (the performance requirements, P1). */ +sized buffers must not reallocate them, so the steady-state loop does not +allocate. */ GTEST_TEST(DeCasteljau, PreSizedOutputsAreNotReallocated) { std::mt19937_64 generator(7); const Eigen::MatrixXd parent = RandomMatrix(6, 4, &generator); @@ -353,8 +354,7 @@ BsplineTrajectory MakeBsplineFromBasis( } /* Shared checker: the conversion must reproduce the B-spline to 1e-10 over ->= 1e4 dense samples, and the segments must tile the domain (trajectory -normalization; the test plan). */ +>= 1e4 dense samples, and the segments must tile the domain. */ void CheckBsplineEquivalence(const BsplineTrajectory& bspline) { const PiecewiseBezierPath path = PiecewiseBezierPath::FromTrajectory(bspline, Options{}); @@ -362,7 +362,7 @@ void CheckBsplineEquivalence(const BsplineTrajectory& bspline) { 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 + // 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()); } @@ -420,7 +420,7 @@ GTEST_TEST(BsplineConversion, NonUniformKnots) { 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 + // interior knot 2.0 with multiplicity 3 (C0 there, the extreme case that // still passes junction validation). 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}; @@ -557,7 +557,7 @@ GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCap) { Eigen::VectorXd times(num_points); Eigen::MatrixXd samples(2, num_points); for (int i = 0; i < num_points; ++i) { - // A deliberately non-unit segment duration: the monomial coefficients + // 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]); @@ -592,7 +592,7 @@ GTEST_TEST(PiecewisePolynomialConversion, DegreeAboveCapThrows) { }, "max_conversion_degree"); - // Raising the cap deliberately makes it work. + // Raising the cap makes it work. Options relaxed; relaxed.max_conversion_degree = degree; const PiecewiseBezierPath path = @@ -707,8 +707,7 @@ GTEST_TEST(JunctionValidation, NonMultipleOfTwoPiOffsetThrowsEvenWhenRevolute) { } /* 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 (trajectory -normalization). */ +be left exactly as it is: the segments are NOT re-aligned. */ GTEST_TEST(JunctionValidation, ControlPointsAreNotRealignedAcrossTwoPiJunction) { std::mt19937_64 generator(864213); @@ -901,7 +900,7 @@ GTEST_TEST(Composite, NestedCompositeRecursion) { } /* A CompositeTrajectory whose segments are B-splines and PiecewisePolynomials -recurses through the same rules (trajectory normalization, item 3). */ +recurses through the same rules. */ GTEST_TEST(Composite, MixedSegmentTypes) { std::mt19937_64 generator(11111); const int num_positions = 2; diff --git a/planning/continuous_collision/test/soundness_fuzz_test.cc b/planning/continuous_collision/test/soundness_fuzz_test.cc index 576508091493..fefc8afc6edd 100644 --- a/planning/continuous_collision/test/soundness_fuzz_test.cc +++ b/planning/continuous_collision/test/soundness_fuzz_test.cc @@ -1,44 +1,18 @@ -/// @file -/// T4 — end-to-end soundness fuzz (test plan T4; implementation note 2). -/// -/// Random worlds × random trajectories, cross-checked three ways: -/// -/// * a single sampled configuration whose clearance reaches the threshold -/// would refute a `kCertifiedFree` verdict outright, so every certified -/// case is searched for one — hard (10⁴ configurations, 10⁵ on a subset) — -/// and its emitted certificate is independently replayed; -/// * every definite `Finding` is re-evaluated exactly at its witness -/// configuration, from a context this run never touched, and must really -/// violate; -/// * every non-definite `Finding` that claims to be a resolution-floor -/// grazing record must be backed by a clearance that really sits within -/// 10·(τ_p + ε) of the threshold near the reported time. -/// -/// Any cross-check failure here is a soundness bug in the library, never a -/// reason to loosen the test (the implementation notes, item 2). Failure -/// messages carry the complete repro — seed, world recipe, trajectory control -/// points — so a failing case can be reconstructed from the CI log alone. -/// -/// Budget. 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. -/// kNumCases = 200 clears test-plan T4's ≥ 150 (world, trajectory) pairs per CI -/// run by a third and measures ~14 s in Release here, a 10× margin against the -/// ~3 min budget — but that margin is a *Release* margin, and it is the only -/// flavor in which it is that comfortable. An instrumented build slows the -/// dense sweep by one to two orders of magnitude, which would spend the whole -/// budget and more, so two things give: asan and lsan are excluded outright -/// (BUILD.bazel tags), and the flavors that do run the fuzz — tsan, ubsan, -/// memcheck — take a quarter corpus (`DRAKE_CCD_FUZZ_SMALL_CORPUS`), which -/// buys back a 4× and leaves the timeout, raised to "long", to absorb the -/// rest. Composition is asserted as fractions of kNumCases so both corpus -/// sizes are held to the same standard. -/// -/// The spare Release budget is spent on resolution rather than on more -/// shallowly-checked cases: kDenseSamples = 10⁴ resolves any clearance dip -/// wider than ~10⁻⁴ of the domain, and every 10th certified case gets the 10⁵ -/// sweep, which resolves 10× finer at 10× the cost. (Sample counts are per -/// case and approximate: they are split evenly across segments and each segment -/// gets both endpoints, so the true count is total + #segments.) +// End-to-end soundness fuzz: random worlds × random trajectories, cross-checked +// three 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) and its certificate is replayed +// independently; +// * every definite `Finding` is re-evaluated at its witness configuration, +// from a context this run never touched, and must really violate; +// * every non-definite `Finding` claiming to be a resolution-floor grazing +// record 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 @@ -109,21 +83,27 @@ 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 (BUILD.bazel selects -/// this on //tools:using_sanitizer and //tools:using_memcheck). Nothing about -/// the case *recipes* changes, -/// 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. +// A quarter corpus for instrumented builds, where the dense cross-check runs +// one to two orders of magnitude slower than in Release (BUILD.bazel selects +// this on //tools:using_sanitizer and //tools:using_memcheck). 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. asan and lsan are +// excluded outright (BUILD.bazel tags), and the instrumented flavors that do +// run the fuzz take the quarter corpus above, with the timeout raised to +// "long" to absorb the rest. Composition is asserted as fractions of kNumCases +// so both corpus sizes are held to the same standard. 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. The fractions are the ones -/// the 200-case corpus has always been checked against. +// 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% @@ -134,18 +114,22 @@ constexpr int kMaxBudgetExhausted = kNumCases / 20; // 5% 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, which resolves +// 10× finer at 10× the cost. Sample counts are per case and approximate: they +// are split evenly across segments and each segment gets both endpoints, so the +// true count is total + #segments. constexpr int kDenseSamples = 10000; constexpr int kDeepDenseSamples = 100000; -/// Every kDeepEvery-th certified case gets the 10⁵-sample sweep. constexpr int kDeepEvery = 10; -/// Samples used to locate a trajectory's minimum clearance when building a -/// deliberately grazing case. +// 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). The checker -/// charges each pair its own τ_p ≥ Options::query_tolerance; the tests below -/// only ever need an upper bound on it, and this is it. +// The worst signed-distance accuracy Drake documents for any supported shape +// combination (query_object.h Table 4, Cylinder–Ellipsoid). The checker charges +// each pair its own τ_p ≥ Options::query_tolerance; the tests below only ever +// need an upper bound on it, and this is it. constexpr double kWorstTau = 5e-5; // --------------------------------------------------------------------------- @@ -164,8 +148,8 @@ enum class ShapeKind { 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. + // 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()}; }; @@ -187,8 +171,8 @@ std::string Name(ShapeKind kind) { return "?"; } -/// A regular tetrahedron of circumradius √3·`scale`, as a vertex matrix; Drake -/// takes the convex hull of these points. +// 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); @@ -221,14 +205,14 @@ std::unique_ptr MakeShape(const ShapeSpec& spec) { enum class JointKind { kRevolute, kPrismatic }; struct LinkSpec { - /// Index into WorldRecipe::links, or -1 for the world body. + // 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. + // 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. + // The link geometry's pose in the link frame. Vector3d p_LG{Vector3d::Zero()}; ShapeSpec shape; }; @@ -243,7 +227,7 @@ struct WorldRecipe { uint64_t seed{0}; std::vector links; std::vector obstacles; - /// An anchored HalfSpace floor (exercises the analytic distance route). + // An anchored HalfSpace floor (exercises the analytic distance route). bool floor{false}; double floor_z{-0.45}; @@ -255,9 +239,9 @@ 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. + // 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). + // n × K: waypoints (PWL) or control points (Bézier / B-spline). Eigen::MatrixXd points; std::string Describe() const; }; @@ -333,17 +317,17 @@ class Rng { return std::uniform_int_distribution(lo, hi)(engine_); } bool Bernoulli(double p) { return std::bernoulli_distribution(p)(engine_); } - /// Note the named locals: the order in which a compiler evaluates sibling - /// constructor arguments is unspecified, so drawing three variates inline - /// would make the corpus depend on the toolchain. Every draw in this file is - /// sequenced explicitly for that reason. + // Note the named locals: the order in which a compiler evaluates sibling + // constructor arguments is unspecified, so drawing three variates inline + // would make the corpus depend on the toolchain. Every draw in this file is + // sequenced explicitly for that reason. 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); } - /// A uniformly distributed direction (rejection-sampled, so no pole bias). + // A uniformly distributed direction (rejection-sampled, so no pole bias). Vector3d Direction() { while (true) { const Vector3d v = UniformVector(-1.0, 1.0); @@ -351,7 +335,7 @@ class Rng { if (n > 1e-3 && n <= 1.0) return v / n; } } - /// A direction scaled by a length drawn *after* it. + // A direction scaled by a length drawn *after* it. Vector3d Offset(double lo, double hi) { const Vector3d direction = Direction(); const double length = Uniform(lo, hi); @@ -362,12 +346,12 @@ class Rng { 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. Adjacent links therefore have -/// real clearance in most configurations but can genuinely fold into each -/// other, which is what makes the self-collision half of the corpus nontrivial. -/// (MultibodyPlant::Finalize only filters *welded* subgraphs, so every -/// parent/child pair here is a live, unfiltered pair.) +// Link geometries stay small (≤ 5 cm half-extent) and sit ~12–18 cm out along +// the link, while joints are ~25–35 cm apart. Adjacent links therefore have +// real clearance in most configurations but can genuinely fold into each +// other, which is what makes the self-collision half of the corpus nontrivial. +// (MultibodyPlant::Finalize only filters *welded* subgraphs, so every +// parent/child pair here is a live, unfiltered pair.) ShapeSpec RandomLinkShape(Rng* rng) { ShapeSpec spec; const int roll = rng->Int(0, 11); @@ -498,10 +482,10 @@ std::unique_ptr> BuildWorld(const WorldRecipe& recipe) { 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). +// 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); @@ -570,13 +554,12 @@ std::unique_ptr> BuildTrajectory( // The independent dense cross-check. // --------------------------------------------------------------------------- -/// Radius of the smallest sphere about the *geometry frame origin* containing -/// the shape. Deliberately re-derived here — six exact one-liners, each -/// obviously correct — 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 is auditing. std::nullopt means "no finite radius available" (HalfSpace) -/// or "not worth deriving here" (Convex / Mesh); such pairs always take the -/// narrowphase. +// 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 is auditing. std::nullopt means "no finite radius available" +// (HalfSpace) or "not derived here" (Convex / Mesh); such pairs always take the +// narrowphase. std::optional LocalRadius(const Shape& shape) { return shape.Visit>( [](const auto& s) -> std::optional { @@ -597,8 +580,8 @@ std::optional LocalRadius(const Shape& shape) { }); } -/// 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. +// 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 ContinuousCollisionChecker& checker) @@ -622,16 +605,16 @@ class DenseScanner { } } - /// Worst (most negative) value of φ_p(q) − threshold over the dense samples, - /// with the time and pair that attained it. + // Worst (most negative) value of ϕ_p(q) − threshold over the dense samples, + // with the time and pair that attained it. struct Result { double min_slack{std::numeric_limits::infinity()}; double worst_time{std::numeric_limits::quiet_NaN()}; int worst_pair{-1}; }; - /// `threshold` is m_p, which this fuzz keeps uniform across pairs because it - /// never sets a PaddingSpec (the case loop asserts that). + // `threshold` is m_p, which this fuzz keeps uniform across pairs because it + // never sets a PaddingSpec (the case loop asserts that). Result Scan(const PiecewiseBezierPath& path, int total_samples, double threshold) { const int num_segments = static_cast(path.segments().size()); @@ -649,8 +632,8 @@ class DenseScanner { 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. + // 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) { @@ -672,8 +655,8 @@ class DenseScanner { int64_t narrowphase_queries() const { return narrowphase_queries_; } - /// Signed distance of one pair at an arbitrary configuration, from this - /// scanner's own fresh context. + // 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_; @@ -681,7 +664,7 @@ class DenseScanner { query_object(), checker_->pairs()[pair_index]); } - /// Index of the checker's pair matching `id`, or -1. + // Index of the checker's pair matching `id`, or -1. int FindPair(const PairId& id) const { const auto& pairs = checker_->pairs(); for (int p = 0; p < static_cast(pairs.size()); ++p) { @@ -713,7 +696,7 @@ class DenseScanner { 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 + // ϕ_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 @@ -764,17 +747,17 @@ struct Tally { int bspline{0}; int64_t scan_queries{0}; int floors{0}; - /// One counter per ShapeKind, over every geometry of every world built. + // 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. + // 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. +// Base options shared by every case. Options FuzzOptions(double margin) { Options options; options.margin = margin; @@ -829,14 +812,14 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { const std::unique_ptr> trajectory = BuildTrajectory(trajectory_recipe); - // Both halves of test-plan T4's 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 exercise the kInconclusive branch (and its - // cross-check) on random geometry. Without it the corpus would never - // produce a grazing case, because a random trajectory is tangent to a - // random obstacle with probability zero. + // 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 exercise the kInconclusive branch (and its cross-check) + // on random geometry. Without it the corpus would never produce a grazing + // case, because a random trajectory is tangent to a random obstacle with + // probability zero. double margin = (case_index % 2 == 0) ? 0.0 : 0.01; bool grazing = (case_index % 5) == 3; if (grazing) { @@ -952,9 +935,8 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { // the synthesized "here is where the budget stopped us" finding is // exempt, because its clearance carries no claim. ++tally.inconclusive_findings; - // Test plan T4: a grazing record must be backed by a clearance that - // sits within 10·(τ_p + ε) of the threshold somewhere near the - // reported time. + // A grazing record must be backed by a clearance that sits within + // 10·(τ_p + ε) of the threshold somewhere near the reported time. const double tolerance = 10.0 * (kWorstTau + options.certificate_slack); const double window = 0.01 * std::max(1e-12, path.end_time() - path.start_time()); @@ -971,7 +953,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { tally.scan_queries += scanner.narrowphase_queries(); } - std::cout << "\n[ T4 FUZZ SUMMARY ] cases = " << kNumCases + std::cout << "\n[ fuzz summary ] cases = " << kNumCases << " certified = " << tally.certified << " violation = " << tally.violation << " inconclusive = " << tally.inconclusive @@ -998,17 +980,17 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { // 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; T4's case - // count is satisfied by the uninstrumented run CI also performs. What it - // still has to be is large enough for the composition floors below to say - // something — at 50 cases the thinnest of them still demands a case. + // The shrunk corpus is an instrumentation-only configuration; the full case + // count is satisfied by the uninstrumented run CI also performs. It still has + // to be large enough for the composition floors below to say something; at 50 + // cases the thinnest of them still demands a case. 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, - "test plan T4 asks for >= 150 (world, trajectory) cases per CI run"); + static_assert(kNumCases >= 150, + "the soundness sweep needs at least 150 (world, trajectory) " + "cases per run"); #endif static_assert(kMinInconclusive >= 1 && kMinInconclusiveFindings >= 1, "every composition floor must demand at least one case"); @@ -1020,7 +1002,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { EXPECT_GE(tally.inconclusive_findings, kMinInconclusiveFindings); // The node budget exists to bound a pathological case, not to be the usual // answer: if it starts firing often, the corpus has stopped cross-checking - // anything and the numbers above would quietly stop meaning what they say. + // anything and the numbers above stop meaning what they say. EXPECT_LE(tally.budget, kMaxBudgetExhausted); // All three trajectory families of trajectory normalization must be // represented. diff --git a/planning/continuous_collision/test/thin_obstacle_test.cc b/planning/continuous_collision/test/thin_obstacle_test.cc index 59e8c193cdf8..5d65b6ff0ffe 100644 --- a/planning/continuous_collision/test/thin_obstacle_test.cc +++ b/planning/continuous_collision/test/thin_obstacle_test.cc @@ -1,17 +1,8 @@ -/// @file -/// T5 — the reason this library exists (test plan T5; the motivation for -/// the library). -/// -/// Drake's `drake::planning::SceneGraphCollisionChecker` checks an edge by -/// interpolating it at `edge_step_size` increments and running a *discrete* -/// check at each sample. A thin obstacle that sits between two samples is -/// invisible to it, however carefully the planner was written. This file builds -/// exactly that situation, pins Drake's miss, and shows that our continuum -/// certificate catches it — then shows the mirror image: a genuinely free -/// squeeze through a millimetre-scale gap that we certify with a bounded node -/// budget, so the gain is not bought with useless conservatism. -/// -/// Everything here is programmatic and deterministic: no RNG, no model files. +// 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 @@ -70,7 +61,7 @@ using Eigen::VectorXd; // 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 rather than a plausible story. +// statement about the sampled check. // // The edge runs from q1 = (-0.5, 0) to q2 = (+0.5, 0). // @@ -82,26 +73,23 @@ using Eigen::VectorXd; // 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 — 21 -// configurations 0.05 m apart in x, at x = -0.50, -0.45, …, 0.00, 0.05, …, -// 0.50. (Reconstructed and measured below rather than trusted.) +// * 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 = 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. -// -// A sampled checker at this resolution cannot tell this edge from an empty -// world. A certificate over the continuum can. 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. +// Half-width, in x, of the set of configurations that touch the plate. constexpr double kContactHalfWidth = kToolRadius + 0.5 * kPlateThickness; CoulombFriction Friction() { @@ -112,9 +100,9 @@ SpatialInertia Inertia() { return SpatialInertia::SolidSphereWithMass(1.0, 0.05); } -/// 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". +// 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 = @@ -139,8 +127,8 @@ void AddAnchoredBox(MultibodyPlant* plant, const std::string& name, name + "_geom", Friction()); } -/// The thin-plate world: 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. +// The thin-plate world: 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; @@ -152,8 +140,8 @@ std::unique_ptr> MakePlateWorld(double plate_x, 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. +// 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(); @@ -166,10 +154,10 @@ std::unique_ptr> MakeSlotWorld(double half_gap) { return builder.Build(); } -/// Drake's sampled edge checker, always on its own freshly built RobotDiagram -/// (the "clone of the same model" of test-plan T5): SceneGraphCollisionChecker -/// rewrites collision filters on the model it is handed, which would otherwise -/// perturb the pair table our checker snapshots at construction. +// 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)); @@ -207,9 +195,9 @@ Eigen::MatrixXd Waypoints(const VectorXd& q1, const VectorXd& q2) { return waypoints; } -/// Signed distance of `finding`'s pair, re-measured from a fresh context at the -/// witness configuration: the independent confirmation that the witness is a -/// real contact and not an artifact of the search. +// Signed distance of `finding`'s pair, re-measured from a fresh context at the +// witness configuration: an independent confirmation that the witness is a real +// contact and not an artifact of the search. double DistanceAtFinding(const ContinuousCollisionChecker& checker, const Finding& finding) { const RobotDiagram& model = checker.model(); @@ -242,19 +230,17 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { // The distance the sample count is derived from is exactly the edge length. EXPECT_NEAR(drake_checker.ComputeConfigurationDistance(q1, q2), 1.0, 1e-15); - // The headline: 1 mm of plate between the waypoints, and the sampled check - // calls the edge free. + // 1 mm of plate between the waypoints, and the sampled check calls the edge + // free. EXPECT_TRUE(drake_checker.CheckEdgeCollisionFree(q1, q2)) - << "the premise of this test — that default-resolution sampling misses a " - "1 mm plate — no longer holds on this Drake pin"; - - // Show *why*. The model of Drake's behaviour 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. That model is *measured*, not assumed, by sliding the plate across - // one sample period and comparing Drake's verdict against the prediction - // "caught iff the plate's mid-plane is within the contact half-width of some - // multiple of 0.05". Every mismatch would mean the sample grid is not what - // the arithmetic above claims. + << "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)); @@ -271,9 +257,8 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { } // With the grid confirmed, walk it through Drake's own interpolation function - // and measure the clearance at every sample. The nearest one is 25 mm from - // the plate's mid-plane, i.e. 19.5 mm of clearance: the sampled check is not - // remotely close to seeing it. + // 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(); @@ -290,8 +275,7 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { << "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. Replacing "shrink it and - // hope" with a proof is what this library is for. + // the very same sampled checker finds the plate. const SceneGraphCollisionChecker fine_checker = MakeDrakeChecker(MakePlateWorld(kPlateX, kPlateThickness), 0.002); EXPECT_FALSE(fine_checker.CheckEdgeCollisionFree(q1, q2)); @@ -360,8 +344,8 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { const VectorXd q1 = MakeQ(-0.3, 0.0); const VectorXd q2 = MakeQ(0.3, 0.0); - // Sampling passes here too — but this time it is *right*, and the point is - // that we agree without having to sample. + // Sampling passes here too, and this time it is right; the certified checker + // agrees without sampling. const SceneGraphCollisionChecker drake_checker = MakeDrakeChecker(MakeSlotWorld(kHalfGap), kDrakeEdgeStepSize); EXPECT_TRUE(drake_checker.CheckEdgeCollisionFree(q1, q2)); @@ -372,8 +356,8 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { // 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, + // 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 @@ -402,7 +386,7 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { } // --------------------------------------------------------------------------- -// 4. Thickness sweep — reported, not asserted (test plan T5's diagnostic half). +// 4. Thickness sweep: reported, not asserted. // --------------------------------------------------------------------------- GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { @@ -410,12 +394,11 @@ GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { // 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. - // (thickness = 0.040 is the exact tangency, where the nearest sample's signed - // distance is 0 and "collision" — φ < 0 — is a coin toss decided by rounding; - // it is in the sweep because it is the interesting number, and the assertion - // at the end is a window, not an equality, for exactly that reason.) - // Our verdict must be kViolationFound at *every* thickness in the sweep: the - // plate is genuinely crossed in all of them. + // 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 assertion + // at the end 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); const std::vector thicknesses = {0.001, 0.002, 0.005, 0.010, @@ -446,14 +429,14 @@ GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { : "OTHER ") << "\t" << (0.5 * thickness + kToolRadius) << "\n"; - // The assertion half of the sweep: our verdict is stable throughout. + // The assertion half of the sweep: the verdict is stable throughout. EXPECT_EQ(result.verdict, Verdict::kViolationFound); } std::cout << " --> Drake's sampled checker first sees the plate at " "thickness = " << first_caught << " m; predicted crossover 2*(0.025 - " << kToolRadius << ") = " << 2.0 * (0.025 - kToolRadius) << " m\n\n"; - // A report, not a gate — but the crossover must land in the right decade, + // A report, not a gate. The crossover must still land in the right decade, // otherwise the sweep is measuring something other than the resolution gap. EXPECT_GT(first_caught, 0.03); EXPECT_LT(first_caught, 0.06); diff --git a/planning/continuous_collision/vpolytope_ingestion.h b/planning/continuous_collision/vpolytope_ingestion.h index 9b673bb1d58c..208c42ea2472 100644 --- a/planning/continuous_collision/vpolytope_ingestion.h +++ b/planning/continuous_collision/vpolytope_ingestion.h @@ -11,14 +11,12 @@ namespace drake { namespace planning { namespace continuous_collision { -/** Registers a V-polytope as an anchored obstacle with a collision role -(the geometry-support scope, "V-polytopes as first-class geometry", ingestion -route (b)). +/** Registers a V-polytope as an anchored obstacle with a collision role. The polytope is converted to `drake::geometry::Convex` through Drake's own -`VPolytope::ToShapeConvex()` entry point (a thin wrapper over the +`VPolytope::ToShapeConvex()` entry point, a thin wrapper over the `Convex(Eigen::Matrix3X points, std::string label, double scale)` -constructor pinned at M0), then registered on the plant's world body. The +constructor, then registered on the plant's world body. The result therefore rides the ordinary native narrowphase path end to end: the proximity engine and the certifier's radius/support code all read the same `Convex::GetConvexHull()` object, so the certificate stays sound even for From 80f9992d139c33ef159a84a1d2bc0a9708e18061 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Fri, 28 Aug 2026 10:42:26 -0400 Subject: [PATCH 16/22] [planning] continuous_collision: move the benchmark out of tree --- planning/continuous_collision/BUILD.bazel | 60 - .../benchmark/benchmark_util.cc | 481 ------- .../benchmark/benchmark_util.h | 225 ---- .../benchmark/iiwa_benchmark.cc | 1172 ----------------- .../benchmark/scenario_worlds.cc | 182 --- .../benchmark/scenario_worlds.h | 72 - .../certifier_internal.cc | 12 +- .../test/concurrency_timing_test.cc | 4 +- 8 files changed, 8 insertions(+), 2200 deletions(-) delete mode 100644 planning/continuous_collision/benchmark/benchmark_util.cc delete mode 100644 planning/continuous_collision/benchmark/benchmark_util.h delete mode 100644 planning/continuous_collision/benchmark/iiwa_benchmark.cc delete mode 100644 planning/continuous_collision/benchmark/scenario_worlds.cc delete mode 100644 planning/continuous_collision/benchmark/scenario_worlds.h diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index 4370206d35de..bc7182301043 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -1,7 +1,6 @@ load("//tools/lint:lint.bzl", "add_lint_tests") load( "//tools/skylark:drake_cc.bzl", - "drake_cc_binary", "drake_cc_googletest", "drake_cc_library", "drake_cc_package_library", @@ -442,63 +441,4 @@ drake_cc_googletest( ], ) -# === benchmark/ === - -# The performance benchmark suite. A full run takes minutes and reports -# measurements rather than assertions, so it is not a test; run it with -# bazel run //planning/continuous_collision:iiwa_benchmark -- \ -# --out /tmp/ccd --drake_commit $(git rev-parse HEAD) -# It is not tagged manual, though, so that CI compiles it: the benchmark shares -# every header the library exposes, and a benchmark that stopped building would -# otherwise go unnoticed until someone next needed a measurement. The smoke -# test that comes with it runs the cheapest scenario at one repetition (about -# two seconds) purely to prove the binary still starts and finishes. -drake_cc_binary( - name = "iiwa_benchmark", - testonly = 1, - srcs = [ - "benchmark/benchmark_util.cc", - "benchmark/benchmark_util.h", - "benchmark/iiwa_benchmark.cc", - "benchmark/scenario_worlds.cc", - "benchmark/scenario_worlds.h", - ], - data = [ - "@drake_models//:iiwa_description", - ], - deps = [ - ":continuous_collision_checker", - "//common:copyable_unique_ptr", - "//common:parallelism", - "//common/trajectories:bezier_curve", - "//common/trajectories:composite_trajectory", - "//common/trajectories:trajectory", - "//geometry:geometry_ids", - "//geometry:scene_graph", - "//geometry:shape_specification", - "//math:geometric_transform", - "//multibody/parsing:parser", - "//multibody/plant", - "//multibody/tree", - "//planning:collision_checker_params", - "//planning:robot_diagram", - "//planning:robot_diagram_builder", - "//planning:scene_graph_collision_checker", - "@eigen", - ], - add_test_rule = 1, - test_rule_args = [ - "--only", - "dual", - "--reps", - "1", - "--warmup", - "0", - "--dense-samples", - "200", - ], - test_rule_size = "small", - test_rule_timeout = "moderate", -) - add_lint_tests() diff --git a/planning/continuous_collision/benchmark/benchmark_util.cc b/planning/continuous_collision/benchmark/benchmark_util.cc deleted file mode 100644 index ddb7be16f02c..000000000000 --- a/planning/continuous_collision/benchmark/benchmark_util.cc +++ /dev/null @@ -1,481 +0,0 @@ -#include "drake/planning/continuous_collision/benchmark/benchmark_util.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drake/common/copyable_unique_ptr.h" -#include "drake/common/trajectories/bezier_curve.h" -#include "drake/geometry/query_object.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace internal { -namespace { - -using drake::geometry::GeometryId; -using drake::geometry::QueryObject; -using drake::geometry::SignedDistancePair; -using drake::planning::RobotDiagram; -using drake::systems::Context; -using drake::trajectories::BezierCurve; -using drake::trajectories::CompositeTrajectory; -using drake::trajectories::Trajectory; -using Eigen::MatrixXd; -using Eigen::VectorXd; - -// Formats a double with enough digits to round-trip through the JSON. -std::string FormatDouble(double v) { - if (std::isnan(v)) return "null"; - if (std::isinf(v)) return v > 0 ? "1e999" : "-1e999"; - char buf[64]; - std::snprintf(buf, sizeof(buf), "%.10g", v); - return buf; -} - -std::string Escape(const std::string& s) { - std::string out; - for (const char c : s) { - switch (c) { - case '"': - out += "\\\""; - break; - case '\\': - out += "\\\\"; - break; - case '\n': - out += "\\n"; - break; - case '\t': - out += "\\t"; - break; - default: - out += c; - } - } - return out; -} - -// One (t, min-distance-over-all-pairs, min-distance-over-env-pairs) probe. -struct Probe { - double all{0.0}; - double env{0.0}; -}; - -Probe ProbeAt(const RobotDiagram& diagram, Context* root, - const VectorXd& q, const std::unordered_set& env_ids, - const drake::geometry::SceneGraphInspector& inspector, - double max_distance) { - diagram.plant().SetPositions(&diagram.mutable_plant_context(root), q); - const auto& query_object = diagram.scene_graph() - .get_query_output_port() - .template Eval>( - diagram.scene_graph_context(*root)); - const std::vector> pairs = - query_object.ComputeSignedDistancePairwiseClosestPoints(max_distance); - Probe p{max_distance, max_distance}; - for (const auto& pair : pairs) { - if (pair.distance < p.all) p.all = pair.distance; - const bool a_env = env_ids.count(pair.id_A) > 0; - const bool b_env = env_ids.count(pair.id_B) > 0; - if (a_env != b_env && pair.distance < p.env) p.env = pair.distance; - (void)inspector; - } - return p; -} - -// Golden-section minimization of `f` on [lo, hi]; the sampled bracket around -// a dense-sample argmin is unimodal in practice for these smooth curves. -std::pair GoldenSectionMin( - const std::function& f, double lo, double hi, - int iterations) { - constexpr double kInvPhi = 0.6180339887498949; - double a = lo; - double b = hi; - double c = b - kInvPhi * (b - a); - double d = a + kInvPhi * (b - a); - double fc = f(c); - double fd = f(d); - for (int i = 0; i < iterations; ++i) { - if (fc < fd) { - b = d; - d = c; - fd = fc; - c = b - kInvPhi * (b - a); - fc = f(c); - } else { - a = c; - c = d; - fc = fd; - d = a + kInvPhi * (b - a); - fd = f(d); - } - } - return (fc < fd) ? std::make_pair(fc, c) : std::make_pair(fd, d); -} - -} // namespace - -// --------------------------------------------------------------------------- -// JsonWriter -// --------------------------------------------------------------------------- - -void JsonWriter::Indent() { - out_.append(static_cast(2 * depth_), ' '); -} - -void JsonWriter::Separator() { - if (!first_.empty()) { - if (first_.back()) { - first_.back() = false; - } else { - out_ += ","; - } - out_ += "\n"; - Indent(); - } -} - -void JsonWriter::BeginObject() { - Separator(); - out_ += "{"; - first_.push_back(true); - ++depth_; -} - -void JsonWriter::BeginObject(const std::string& key) { - Separator(); - out_ += "\"" + Escape(key) + "\": {"; - first_.push_back(true); - ++depth_; -} - -void JsonWriter::EndObject() { - const bool empty = first_.back(); - first_.pop_back(); - --depth_; - if (!empty) { - out_ += "\n"; - Indent(); - } - out_ += "}"; -} - -void JsonWriter::BeginArray(const std::string& key) { - Separator(); - out_ += "\"" + Escape(key) + "\": ["; - first_.push_back(true); - ++depth_; -} - -void JsonWriter::EndArray() { - const bool empty = first_.back(); - first_.pop_back(); - --depth_; - if (!empty) { - out_ += "\n"; - Indent(); - } - out_ += "]"; -} - -void JsonWriter::Write(const std::string& key, double value) { - Separator(); - out_ += "\"" + Escape(key) + "\": " + FormatDouble(value); -} - -void JsonWriter::Write(const std::string& key, int value) { - Separator(); - out_ += "\"" + Escape(key) + "\": " + std::to_string(value); -} - -void JsonWriter::Write(const std::string& key, long value) { // NOLINT - Separator(); - out_ += "\"" + Escape(key) + "\": " + std::to_string(value); -} - -void JsonWriter::Write(const std::string& key, - unsigned long value) { // NOLINT - Separator(); - out_ += "\"" + Escape(key) + "\": " + std::to_string(value); -} - -void JsonWriter::Write(const std::string& key, bool value) { - Separator(); - out_ += "\"" + Escape(key) + "\": " + (value ? "true" : "false"); -} - -void JsonWriter::Write(const std::string& key, const char* value) { - Write(key, std::string(value)); -} - -void JsonWriter::Write(const std::string& key, const std::string& value) { - Separator(); - out_ += "\"" + Escape(key) + "\": \"" + Escape(value) + "\""; -} - -void JsonWriter::WriteArrayValue(double value) { - Separator(); - out_ += FormatDouble(value); -} - -void JsonWriter::WriteArrayValue(const std::string& value) { - Separator(); - out_ += "\"" + Escape(value) + "\""; -} - -void WriteTextFile(const std::string& path, const std::string& text) { - const std::filesystem::path p(path); - if (p.has_parent_path()) { - std::filesystem::create_directories(p.parent_path()); - } - std::ofstream file(path); - if (!file) throw std::runtime_error("cannot open for writing: " + path); - file << text; -} - -void WriteTiming(JsonWriter* json, const std::string& key, - const TimingSummary& t) { - json->BeginObject(key); - json->Write("median", t.median_ms); - json->Write("min", t.min_ms); - json->Write("max", t.max_ms); - json->Write("reps", t.reps); - json->EndObject(); -} - -// --------------------------------------------------------------------------- -// Machine -// --------------------------------------------------------------------------- - -MachineInfo GetMachineInfo(const std::string& drake_commit) { - MachineInfo info; - info.core_count = static_cast(std::thread::hardware_concurrency()); - { - std::ifstream cpuinfo("/proc/cpuinfo"); - std::string line; - while (std::getline(cpuinfo, line)) { - const size_t colon = line.find(':'); - if (colon == std::string::npos) continue; - if (line.compare(0, 10, "model name") != 0) continue; - info.cpu_model = line.substr(colon + 1); - const size_t start = info.cpu_model.find_first_not_of(" \t"); - if (start != std::string::npos) - info.cpu_model = info.cpu_model.substr(start); - break; - } - } - // The Drake revision is not discoverable from inside the binary, so the - // caller passes it in (--drake_commit) and it is recorded verbatim. - info.drake_commit = drake_commit; - info.drake_version_note = "built from the Drake source tree"; - return info; -} - -void WriteMachine(JsonWriter* json, const MachineInfo& machine) { - json->BeginObject("machine"); - json->Write("cpu_model", machine.cpu_model); - json->Write("core_count", machine.core_count); - json->Write("drake_commit", machine.drake_commit); - json->Write("drake_version", machine.drake_version_note); - json->EndObject(); -} - -// --------------------------------------------------------------------------- -// Trajectories -// --------------------------------------------------------------------------- - -std::shared_ptr> MakeQuinticCompositeBezier( - const MatrixXd& waypoints, const std::vector& times) { - const int n = static_cast(waypoints.rows()); - const int k = static_cast(waypoints.cols()); - if (k < 2 || static_cast(times.size()) != k) { - throw std::runtime_error("MakeQuinticCompositeBezier: bad sizes"); - } - MatrixXd velocity = MatrixXd::Zero(n, k); - for (int i = 1; i + 1 < k; ++i) { - velocity.col(i) = (waypoints.col(i + 1) - waypoints.col(i - 1)) / - (times[i + 1] - times[i - 1]); - } - std::vector>> segments; - for (int i = 0; i + 1 < k; ++i) { - const double h = times[i + 1] - times[i]; - MatrixXd cps(n, 6); - const VectorXd p0 = waypoints.col(i); - const VectorXd p5 = waypoints.col(i + 1); - const VectorXd v0 = velocity.col(i); - const VectorXd v1 = velocity.col(i + 1); - cps.col(0) = p0; - cps.col(1) = p0 + h * v0 / 5.0; - cps.col(2) = p0 + 2.0 * h * v0 / 5.0; - cps.col(3) = p5 - 2.0 * h * v1 / 5.0; - cps.col(4) = p5 - h * v1 / 5.0; - cps.col(5) = p5; - segments.emplace_back( - std::make_unique>(times[i], times[i + 1], cps)); - } - return std::make_shared>(std::move(segments)); -} - -std::vector SampleTrajectory(const Trajectory& trajectory, - int count) { - const double t0 = trajectory.start_time(); - const double t1 = trajectory.end_time(); - std::vector out; - out.reserve(count); - for (int i = 0; i < count; ++i) { - const double t = (count == 1) ? t0 - : t0 + (t1 - t0) * static_cast(i) / - static_cast(count - 1); - out.push_back(trajectory.value(t).col(0)); - } - return out; -} - -double PathLengthInEdgeMetric(const Trajectory& trajectory, - int num_samples) { - const std::vector qs = SampleTrajectory(trajectory, num_samples); - double length = 0.0; - for (size_t i = 1; i < qs.size(); ++i) { - length += (qs[i] - qs[i - 1]).norm(); - } - return length; -} - -// --------------------------------------------------------------------------- -// Ground-truth swept clearance -// --------------------------------------------------------------------------- - -std::unordered_set CollectGeometryIds( - const RobotDiagram& diagram, - const std::vector& model_instance_names) { - const auto& plant = diagram.plant(); - const auto& inspector = diagram.scene_graph().model_inspector(); - std::unordered_set ids; - for (const std::string& name : model_instance_names) { - if (!plant.HasModelInstanceNamed(name)) continue; - const auto instance = plant.GetModelInstanceByName(name); - for (const auto& body_index : plant.GetBodyIndices(instance)) { - const auto frame_id = plant.GetBodyFrameIdOrThrow(body_index); - for (const auto& id : inspector.GetGeometries( - frame_id, drake::geometry::Role::kProximity)) { - ids.insert(id); - } - } - } - return ids; -} - -ClearanceReport MeasureSweptClearance( - const RobotDiagram& diagram, const Trajectory& trajectory, - const std::unordered_set& env_ids, int num_samples, - int num_threads, double max_distance) { - const double t0 = trajectory.start_time(); - const double t1 = trajectory.end_time(); - const auto& inspector = diagram.scene_graph().model_inspector(); - - const int threads = std::max(1, num_threads); - std::vector best_all(threads, max_distance); - std::vector best_env(threads, max_distance); - std::vector arg_all(threads, 0); - std::vector arg_env(threads, 0); - - const auto worker = [&](int tid) { - auto root = diagram.CreateDefaultContext(); - for (int i = tid; i < num_samples; i += threads) { - const double t = t0 + (t1 - t0) * static_cast(i) / - static_cast(num_samples - 1); - const Probe p = ProbeAt(diagram, root.get(), trajectory.value(t).col(0), - env_ids, inspector, max_distance); - if (p.all < best_all[tid]) { - best_all[tid] = p.all; - arg_all[tid] = i; - } - if (p.env < best_env[tid]) { - best_env[tid] = p.env; - arg_env[tid] = i; - } - } - }; - - if (threads == 1) { - worker(0); - } else { - std::vector pool; - pool.reserve(threads); - for (int i = 0; i < threads; ++i) pool.emplace_back(worker, i); - for (auto& th : pool) th.join(); - } - - ClearanceReport report; - report.samples = num_samples; - report.min_all = max_distance; - report.min_env = max_distance; - int i_all = 0; - int i_env = 0; - for (int i = 0; i < threads; ++i) { - if (best_all[i] < report.min_all) { - report.min_all = best_all[i]; - i_all = arg_all[i]; - } - if (best_env[i] < report.min_env) { - report.min_env = best_env[i]; - i_env = arg_env[i]; - } - } - - // Refine each sampled argmin by golden section on the neighbouring bracket. - auto root = diagram.CreateDefaultContext(); - const double dt = (t1 - t0) / static_cast(num_samples - 1); - const auto time_of = [&](int i) { - return std::min(t1, std::max(t0, t0 + dt * static_cast(i))); - }; - const auto refine = [&](int index, bool env_only, double* value, - double* argt) { - const double lo = time_of(index - 1); - const double hi = time_of(index + 1); - if (hi <= lo) { - *argt = time_of(index); - return; - } - const auto f = [&](double t) { - const Probe p = ProbeAt(diagram, root.get(), trajectory.value(t).col(0), - env_ids, inspector, max_distance); - return env_only ? p.env : p.all; - }; - const auto [best, at] = GoldenSectionMin(f, lo, hi, 60); - if (best < *value) *value = best; - *argt = at; - }; - refine(i_all, false, &report.min_all, &report.t_all); - refine(i_env, true, &report.min_env, &report.t_env); - return report; -} - -double BisectMonotone(const std::function& f, double lo, - double hi, double target, int iterations) { - double a = lo; - double b = hi; - for (int i = 0; i < iterations; ++i) { - const double mid = 0.5 * (a + b); - if (f(mid) < target) { - a = mid; - } else { - b = mid; - } - } - return 0.5 * (a + b); -} - -} // namespace internal -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/benchmark/benchmark_util.h b/planning/continuous_collision/benchmark/benchmark_util.h deleted file mode 100644 index 300548b0b4d9..000000000000 --- a/planning/continuous_collision/benchmark/benchmark_util.h +++ /dev/null @@ -1,225 +0,0 @@ -#pragma once - -// Helpers shared by the benchmark scenarios: a hand-rolled JSON writer, -// steady_clock timing with medians, machine identification, quintic -// composite-Bézier construction, and a dense ground-truth swept-clearance -// sampler that verifies, but never certifies, the clearance of every scenario -// trajectory. No third-party benchmark framework is involved: the measurements -// here are millisecond-scale wall clock repeated by hand, and the JSON is -// consumed by CI tracking. - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "drake/common/trajectories/composite_trajectory.h" -#include "drake/common/trajectories/trajectory.h" -#include "drake/geometry/geometry_ids.h" -#include "drake/planning/robot_diagram.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace internal { - -// --------------------------------------------------------------------------- -// JSON -// --------------------------------------------------------------------------- - -// Minimal streaming JSON writer: enough for the fixed result schema, with no -// dependency. Callers must balance Begin*/End* calls. -class JsonWriter { - public: - JsonWriter() = default; - - // Opens an object as the next element of the innermost array. - void BeginObject(); - // Opens an object under `key` in the innermost object. - void BeginObject(const std::string& key); - // Closes the innermost object. - void EndObject(); - // Opens an array under `key` in the innermost object. - void BeginArray(const std::string& key); - // Closes the innermost array. - void EndArray(); - - // Writes one `key`: `value` member into the innermost object. - void Write(const std::string& key, double value); - void Write(const std::string& key, int value); - void Write(const std::string& key, long value); // NOLINT - void Write(const std::string& key, unsigned long value); // NOLINT - void Write(const std::string& key, bool value); - void Write(const std::string& key, const char* value); - void Write(const std::string& key, const std::string& value); - // Appends a bare double to the innermost array. - void WriteArrayValue(double value); - // Appends a bare string to the innermost array. - void WriteArrayValue(const std::string& value); - - // Returns the document written so far, newline-terminated. - std::string str() const { return out_ + "\n"; } - - private: - void Separator(); - void Indent(); - - std::string out_; - std::vector first_; // per open container: "nothing written yet" - int depth_{0}; -}; - -// Writes `text` to `path`, creating parent directories as needed. -void WriteTextFile(const std::string& path, const std::string& text); - -// --------------------------------------------------------------------------- -// Timing -// --------------------------------------------------------------------------- - -// Wall-clock summary of one repeated measurement, in milliseconds. -struct TimingSummary { - double median_ms{0.0}; - double min_ms{0.0}; - double max_ms{0.0}; - int reps{0}; -}; - -// Runs `body` `warmup` times untimed, then `reps` times timed, and reduces the -// sample to median/min/max. No pinning and no frequency control: the numbers -// are what a user on this machine would see. An exception thrown by `body` -// propagates and no summary is produced. -// @pre reps >= 1; the summary reads the ends of a sample of `reps` entries. -template -TimingSummary TimeRepeatedly(int warmup, int reps, F&& body) { - for (int i = 0; i < warmup; ++i) { - body(); - } - std::vector ms; - ms.reserve(reps); - for (int i = 0; i < reps; ++i) { - const auto t0 = std::chrono::steady_clock::now(); - body(); - const auto t1 = std::chrono::steady_clock::now(); - ms.push_back(std::chrono::duration(t1 - t0).count()); - } - std::sort(ms.begin(), ms.end()); - TimingSummary s; - s.reps = reps; - s.min_ms = ms.front(); - s.max_ms = ms.back(); - s.median_ms = - (reps % 2 == 1) ? ms[reps / 2] : 0.5 * (ms[reps / 2 - 1] + ms[reps / 2]); - return s; -} - -// Writes `t` as an object under `key`: median, min, max and reps. -void WriteTiming(JsonWriter* json, const std::string& key, - const TimingSummary& t); - -// --------------------------------------------------------------------------- -// Machine identification -// --------------------------------------------------------------------------- - -// Identification of the machine and the build a result file was produced on. -struct MachineInfo { - std::string cpu_model; - int core_count{0}; - std::string drake_commit; - std::string drake_version_note; -}; - -// Reads the CPU model from /proc/cpuinfo and records `drake_commit` (the -// Drake revision the caller was built from, passed through verbatim) so -// every result file self-identifies. -MachineInfo GetMachineInfo(const std::string& drake_commit); - -// Writes `machine` as the "machine" object of a result file. -void WriteMachine(JsonWriter* json, const MachineInfo& machine); - -// --------------------------------------------------------------------------- -// Trajectories -// --------------------------------------------------------------------------- - -// Builds a C2 composite quintic Bézier through the columns of `waypoints` -// (n × K) at `times` (K strictly increasing values): the smooth composite -// Bézier a GCS/B-spline planner would hand us, degree 5 with K−1 segments. -// Waypoint velocities are centred finite differences (zero at both ends) and -// waypoint accelerations are zero. Per segment, duration h, velocities v0, v1: -// clang-format off -// P0 = q0, P5 = q1, -// P1 = P0 + h v0/5, P4 = P5 − h v1/5, -// P2 = P0 + 2 h v0/5, P3 = P5 − 2 h v1/5, -// clang-format on -// so q(t0)=q0, q̇(t0)=v0, q̈(t0)=0, and likewise at t1. -// @throws std::exception if waypoints.cols() < 2. -// @throws std::exception if times.size() != waypoints.cols(). -// @pre times is strictly increasing; the centred differences divide by -// times[i+1] - times[i-1]. -std::shared_ptr> -MakeQuinticCompositeBezier(const Eigen::MatrixXd& waypoints, - const std::vector& times); - -// Path length in the plant's default edge metric: the unweighted Euclidean -// configuration distance (LinearDistanceAndInterpolationProvider's default -// weights are 1 for every non-quaternion coordinate), integrated along the -// trajectory with `num_samples` chords. Used to derive the number of samples -// a sampled checker would take at a given edge_step_size. -double PathLengthInEdgeMetric(const trajectories::Trajectory& t, - int num_samples); - -// Samples `count` configurations uniformly in trajectory time (inclusive of -// both endpoints). -std::vector SampleTrajectory( - const trajectories::Trajectory& trajectory, int count); - -// --------------------------------------------------------------------------- -// Ground-truth swept clearance -// --------------------------------------------------------------------------- - -// The result of MeasureSweptClearance. `min_env` restricts the minimum to -// robot-vs-environment pairs (the quantity a shelf shift/scale actually -// controls); `min_all` also includes robot-vs-robot pairs. `t_all` and `t_env` -// are the trajectory times at which those minima occur. -struct ClearanceReport { - double min_all{0.0}; - double t_all{0.0}; - double min_env{0.0}; - double t_env{0.0}; - int samples{0}; -}; - -// Geometry ids belonging to bodies of the named model instances. -std::unordered_set CollectGeometryIds( - const RobotDiagram& diagram, - const std::vector& model_instance_names); - -// True minimum signed distance along `trajectory`, obtained by dense sampling -// (`num_samples` configurations split over `num_threads` cloned contexts) plus -// a golden-section refinement of the sampled argmin. This is the benchmark's -// independent oracle: it is what "achieved clearance" means in the result -// files. Distances beyond `max_distance` are not resolved; if no pair comes -// within it the reported minimum saturates at `max_distance`. A `num_threads` -// below 1 is treated as 1. -// @throws std::exception if a configuration on `trajectory` does not have -// diagram.plant().num_positions() rows, or holds a non-finite value. -// @pre num_samples >= 2; the sample times divide by num_samples - 1. -ClearanceReport MeasureSweptClearance( - const RobotDiagram& diagram, - const trajectories::Trajectory& trajectory, - const std::unordered_set& env_ids, int num_samples, - int num_threads, double max_distance); - -// Bisects `f` (assumed non-decreasing) on [lo, hi] for f(x) = target. -// Returns x. Used to place the shelf at a requested swept clearance. -double BisectMonotone(const std::function& f, double lo, - double hi, double target, int iterations); - -} // namespace internal -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/benchmark/iiwa_benchmark.cc b/planning/continuous_collision/benchmark/iiwa_benchmark.cc deleted file mode 100644 index 3277e33ff32e..000000000000 --- a/planning/continuous_collision/benchmark/iiwa_benchmark.cc +++ /dev/null @@ -1,1172 +0,0 @@ -// The `continuous_collision` performance benchmark suite. No trajectory -// optimizer is invoked: the smooth composite Bézier trajectories are -// hand-constructed in benchmark/scenario_worlds.cc, and every scenario's -// *true* swept clearance is verified by dense sampling before it is -// benchmarked. -// -// Scenarios -// a) iiwa14 + bookcase, three tiers at ~2 mm / 1 cm / 5 cm swept clearance -// b) a two-waypoint PWL edge in the same world -// c) dual-arm iiwa handover (self-collision heavy) -// d) the grazing pathological case (kInconclusive cost at the floor) -// e) thread scaling over a 1000-check batch, two ways -// -// Scenarios (a, 1 cm tier) and (b) are additionally compared against Drake's -// own sampled `SceneGraphCollisionChecker` on the *same* RobotDiagram. - -#include -#include -#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/geometry/query_object.h" -#include "drake/planning/collision_checker_params.h" -#include "drake/planning/continuous_collision/benchmark/benchmark_util.h" -#include "drake/planning/continuous_collision/benchmark/scenario_worlds.h" -#include "drake/planning/continuous_collision/continuous_collision_checker.h" -#include "drake/planning/scene_graph_collision_checker.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace internal { -namespace { - -using drake::Parallelism; -using drake::planning::CollisionCheckerParams; -using drake::planning::RobotDiagram; -using drake::planning::SceneGraphCollisionChecker; -using drake::trajectories::CompositeTrajectory; -using drake::trajectories::Trajectory; -using Eigen::MatrixXd; -using Eigen::VectorXd; - -// drake::planning::CollisionCheckerParams::edge_step_size has NO library -// default: the field is value-initialized to 0 and the CollisionChecker -// constructor rejects any non-positive value, so every caller must choose one. -// 0.05 rad is the value that appears most often in Drake's own tests and -// examples; the other common choices (0.125, 0.1, 0.01) are measured and -// reported too. -constexpr double kEdgeStepSize = 0.05; -constexpr double kReportedEdgeStepSizes[] = {0.125, 0.1, 0.05, 0.01}; - -// Distances beyond this are irrelevant to every scenario here; the ground -// truth sampler saturates at it. -constexpr double kMaxProbeDistance = 0.30; - -struct Config { - std::string out_dir = "."; - std::string drake_commit = "unknown"; - int reps = 20; - int warmup = 3; - int dense_samples = 100000; - int tune_samples = 3000; - int tune_iterations = 24; - int batch = 1000; - int max_threads = 16; - std::string only; -}; - -std::string VerdictName(Verdict v) { - switch (v) { - case Verdict::kCertifiedFree: - return "kCertifiedFree"; - case Verdict::kViolationFound: - return "kViolationFound"; - case Verdict::kInconclusive: - return "kInconclusive"; - case Verdict::kBudgetExhausted: - return "kBudgetExhausted"; - } - return "unknown"; -} - -std::string ModeName(SearchMode m) { - return m == SearchMode::kCertifyAll ? "kCertifyAll" : "kFindFirstViolation"; -} - -// A world plus both checkers built on the *same* RobotDiagram. The two -// checkers must see the same pair set for the comparison to mean anything, so -// the sampled checker is constructed first: its constructor pushes its nominal -// filtered-collision matrix into the SceneGraph, and building the continuous -// checker afterwards makes the two see a bit-identical unfiltered pair set. -struct World { - std::shared_ptr> diagram; - std::unique_ptr sampled; - std::unique_ptr certified; - std::unordered_set env_ids; - int pair_count{0}; - // SceneGraph's own unfiltered-candidate count *after* the sampled checker - // pushed its filters in. Equality with pair_count is the evidence that - // both checkers are looking at exactly the same pairs. - int scene_graph_candidates{0}; -}; - -World MakeWorld(std::shared_ptr> diagram, - const std::vector& robot_instance_names) { - World world; - world.diagram = std::move(diagram); - const auto& plant = world.diagram->plant(); - - CollisionCheckerParams params; - params.model = world.diagram; - for (const std::string& name : robot_instance_names) { - params.robot_model_instances.push_back(plant.GetModelInstanceByName(name)); - } - params.edge_step_size = kEdgeStepSize; - params.env_collision_padding = 0.0; - params.self_collision_padding = 0.0; - params.implicit_context_parallelism = Parallelism::None(); - world.sampled = - std::make_unique(std::move(params)); - - ContinuousCollisionChecker::Params cparams; - cparams.model = world.diagram; - world.certified = std::make_unique(cparams); - - world.env_ids = CollectGeometryIds(*world.diagram, {"environment"}); - world.pair_count = static_cast(world.certified->pairs().size()); - world.scene_graph_candidates = static_cast(world.diagram->scene_graph() - .model_inspector() - .GetCollisionCandidates() - .size()); - return world; -} - -Options MakeOptions(SearchMode mode, Parallelism parallelism, - double min_interval = 1e-9) { - Options options; - options.margin = 0.0; - options.mode = mode; - options.parallelism = parallelism; - options.min_interval = min_interval; - return options; -} - -void WriteOptions(JsonWriter* json, const Options& options) { - json->BeginObject("options"); - json->Write("margin", options.margin); - json->Write("query_tolerance", options.query_tolerance); - json->Write("certificate_slack", options.certificate_slack); - json->Write("min_interval", options.min_interval); - json->Write("mode", ModeName(options.mode)); - json->Write("max_reported_findings", options.max_reported_findings); - json->Write("emit_certificate", options.emit_certificate); - json->Write("parallelism", options.parallelism.num_threads()); - json->EndObject(); -} - -void WriteStats(JsonWriter* json, const Statistics& stats) { - json->BeginObject("stats"); - json->Write("nodes", stats.nodes); - json->Write("narrowphase_queries", stats.narrowphase_queries); - json->Write("sphere_certifications", stats.sphere_certifications); - json->Write("max_depth", stats.max_depth); - json->EndObject(); -} - -void WriteClearance(JsonWriter* json, const ClearanceReport& clearance) { - json->BeginObject("achieved_clearance"); - json->Write("min_all_pairs_m", clearance.min_all); - json->Write("t_at_min_all", clearance.t_all); - json->Write("min_robot_env_pairs_m", clearance.min_env); - json->Write("t_at_min_env", clearance.t_env); - json->Write("dense_samples", clearance.samples); - json->Write("note", - "ground truth from dense sampling plus golden-section " - "refinement; the iiwa14 dense-sphere model has an intrinsic " - "~25.2 mm self-clearance floor (link_0 vs link_2 spheres) that " - "no shelf placement can raise, so min_all_pairs saturates " - "there once the environment clearance exceeds it"); - json->EndObject(); -} - -// One certification measurement. -struct CertRun { - Verdict verdict{}; - Statistics stats; - TimingSummary timing; - int num_findings{0}; -}; - -CertRun MeasureCertify(const ContinuousCollisionChecker& checker, - const Trajectory& trajectory, - const Options& options, int warmup, int reps) { - CertRun run; - run.timing = TimeRepeatedly(warmup, reps, [&]() { - const CertificationResult result = - checker.CheckTrajectory(trajectory, options); - run.verdict = result.verdict; - run.stats = result.stats; - run.num_findings = static_cast(result.findings.size()); - }); - return run; -} - -CertRun MeasureCertifyEdge(const ContinuousCollisionChecker& checker, - const VectorXd& q1, const VectorXd& q2, - const Options& options, int warmup, int reps) { - CertRun run; - run.timing = TimeRepeatedly(warmup, reps, [&]() { - const CertificationResult result = checker.CheckEdge(q1, q2, options); - run.verdict = result.verdict; - run.stats = result.stats; - run.num_findings = static_cast(result.findings.size()); - }); - return run; -} - -void WriteCertRun(JsonWriter* json, const std::string& key, - const CertRun& run) { - json->BeginObject(key); - json->Write("verdict", VerdictName(run.verdict)); - json->Write("num_findings", run.num_findings); - WriteStats(json, run.stats); - WriteTiming(json, "wall_ms", run.timing); - json->EndObject(); -} - -// --------------------------------------------------------------------------- -// The sampled-checker comparison: the cost of certifying a path against the -// cost of sampling it at the resolutions a practitioner would use. -// --------------------------------------------------------------------------- - -// For a curved trajectory a practitioner checks it the only way a sampled -// checker allows: walk the path and call CheckConfigCollisionFree at the -// same resolution the checker would use for an edge, i.e. one sample per -// `edge_step_size` of path length in the plant's edge metric. We report the -// implied sample count and the wall time of exactly that sweep. -void MeasureSampledPathSweep(JsonWriter* json, - const SceneGraphCollisionChecker& sampled, - const Trajectory& trajectory, int warmup, - int reps) { - const double length = PathLengthInEdgeMetric(trajectory, 20001); - json->BeginObject("sampled_comparison"); - json->Write("checker", "drake::planning::SceneGraphCollisionChecker"); - json->Write("edge_step_size_default_in_drake", - "none - CollisionCheckerParams::edge_step_size is a required " - "positive parameter with no library default"); - json->Write("edge_step_size", sampled.edge_step_size()); - json->Write("edge_metric", - "unweighted Euclidean (LinearDistanceAndInterpolationProvider " - "default weights = 1)"); - json->Write("path_length_edge_metric_rad", length); - json->BeginArray("sweeps"); - for (const double step : kReportedEdgeStepSizes) { - const int implied = static_cast(std::ceil(length / step)) + 1; - const std::vector configs = SampleTrajectory(trajectory, implied); - bool free = true; - const TimingSummary timing = TimeRepeatedly(warmup, reps, [&]() { - bool ok = true; - for (const VectorXd& q : configs) { - ok = sampled.CheckConfigCollisionFree(q) && ok; - } - free = ok; - }); - json->BeginObject(); - json->Write("edge_step_size", step); - json->Write("implied_samples", implied); - json->Write("collision_free", free); - WriteTiming(json, "sampled_wall_ms", timing); - json->EndObject(); - } - json->EndArray(); - json->EndObject(); -} - -void MeasureSampledEdge(JsonWriter* json, - const SceneGraphCollisionChecker& sampled, - const VectorXd& q1, const VectorXd& q2, int warmup, - int reps) { - const double length = sampled.ComputeConfigurationDistance(q1, q2); - json->BeginObject("sampled_comparison"); - json->Write("checker", "drake::planning::SceneGraphCollisionChecker"); - json->Write("edge_step_size_default_in_drake", - "none - CollisionCheckerParams::edge_step_size is a required " - "positive parameter with no library default"); - json->Write("edge_metric", - "unweighted Euclidean (LinearDistanceAndInterpolationProvider " - "default weights = 1)"); - json->Write("path_length_edge_metric_rad", length); - json->BeginArray("sweeps"); - // A SceneGraphCollisionChecker is not copy-assignable, so vary the step - // size on a mutable clone rather than rebuilding the model. - std::unique_ptr clone = sampled.Clone(); - for (const double step : kReportedEdgeStepSizes) { - clone->set_edge_step_size(step); - const int implied = static_cast(std::ceil(length / step)) + 1; - bool free = true; - const TimingSummary timing = TimeRepeatedly(warmup, reps, [&]() { - free = clone->CheckEdgeCollisionFree(q1, q2); - }); - json->BeginObject(); - json->Write("edge_step_size", step); - json->Write("implied_samples", implied); - json->Write("collision_free", free); - WriteTiming(json, "sampled_wall_ms", timing); - json->EndObject(); - } - json->EndArray(); - json->EndObject(); -} - -// --------------------------------------------------------------------------- -// Shared plumbing -// --------------------------------------------------------------------------- - -// Places the bookcase so the fixed trajectory's robot-vs-environment swept -// clearance equals `target` (bisection on the shelf scale, which is monotone -// non-decreasing over [0.010, 0.090]). -double TuneShelfScale(const Config& config, double target) { - const MatrixXd waypoints = ShelfTrajectoryWaypoints(); - const auto trajectory = - MakeQuinticCompositeBezier(waypoints, ShelfTrajectoryTimes()); - const auto clearance_of = [&](double scale) { - const auto diagram = MakeShelfWorld(scale); - const auto env_ids = CollectGeometryIds(*diagram, {"environment"}); - return MeasureSweptClearance(*diagram, *trajectory, env_ids, - config.tune_samples, config.max_threads, - kMaxProbeDistance) - .min_env; - }; - return BisectMonotone(clearance_of, 0.010, 0.090, target, - config.tune_iterations); -} - -// Repetition policy. Every millisecond-scale measurement gets the full -// `--reps` after `--warmup` untimed runs. The grazing scenario at the -// default 1e-9 resolution floor costs tens of seconds per call, where 20 -// repetitions would blow the suite's time budget for no statistical gain -// (the relative spread of a 40 s measurement is far below that of a 2 ms -// one), so expensive cases fall back to a small fixed count. The chosen -// count is recorded in every timing block, so no result is silently -// under-sampled. -void PlanReps(const Config& config, double single_run_ms, int* warmup, - int* reps) { - if (single_run_ms > 1000.0) { - *warmup = 0; - *reps = std::min(config.reps, 3); - } else { - *warmup = config.warmup; - *reps = config.reps; - } -} - -// Times one certification once, untimed, to price the case for PlanReps. -double ProbeCost(const ContinuousCollisionChecker& checker, - const Trajectory& trajectory, const Options& options) { - const auto t0 = std::chrono::steady_clock::now(); - checker.CheckTrajectory(trajectory, options); - const auto t1 = std::chrono::steady_clock::now(); - return std::chrono::duration(t1 - t0).count(); -} - -void PrintHeader() { - std::printf(" %-26s %-18s %8s %8s %10s %10s %7s\n", "case", "verdict", - "med_ms", "min_ms", "nodes", "np_query", "depth"); -} - -void PrintRow(const std::string& label, const CertRun& run) { - std::printf( - " %-26s %-18s %8.3f %8.3f %10llu %10llu %7d\n", label.c_str(), - VerdictName(run.verdict).c_str(), run.timing.median_ms, run.timing.min_ms, - static_cast(run.stats.nodes), // NOLINT(runtime/int) - static_cast( // NOLINT(runtime/int) - run.stats.narrowphase_queries), - run.stats.max_depth); -} - -// --------------------------------------------------------------------------- -// (b) the PWL edge, run inside the 1 cm shelf world. -// --------------------------------------------------------------------------- - -void RunPwlEdge(const Config& config, const MachineInfo& machine, - const World& world, const MatrixXd& shelf_waypoints, - double scale) { - const VectorXd q1 = shelf_waypoints.col(0); - const VectorXd q2 = shelf_waypoints.col(1); - MatrixXd edge(q1.size(), 2); - edge.col(0) = q1; - edge.col(1) = q2; - const auto edge_trajectory = MakeQuinticCompositeBezier(edge, {0.0, 1.0}); - const ClearanceReport clearance = MeasureSweptClearance( - *world.diagram, *edge_trajectory, world.env_ids, config.dense_samples, - config.max_threads, kMaxProbeDistance); - const CertRun certify_all = MeasureCertifyEdge( - *world.certified, q1, q2, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), config.warmup, - config.reps); - const CertRun find_first = MeasureCertifyEdge( - *world.certified, q1, q2, - MakeOptions(SearchMode::kFindFirstViolation, Parallelism::None()), - config.warmup, config.reps); - - std::printf( - "[b pwl edge] length=%.4f rad clearance all=%.6f m " - "env=%.6f m\n", - (q2 - q1).norm(), clearance.min_all, clearance.min_env); - PrintHeader(); - PrintRow("certify_all serial", certify_all); - PrintRow("find_first serial", find_first); - std::printf("\n"); - - JsonWriter json; - json.BeginObject(); - json.Write("scenario", "b_pwl_edge"); - // The certified object and the ground-truth object are not the same - // trajectory, only the same point set: CheckEdge certifies a single order-1 - // Bezier segment, while the clearance written below is measured on the - // quintic composite Bezier through the same two waypoints. With two - // waypoints that quintic's endpoint velocities are zero, so its control - // points collapse to {q1, q1, q1, q2, q2, q2} and it traces exactly the same - // straight joint-space segment under a different time parametrization. That - // is why the clearance it measures is the certified edge's clearance. - json.Write("description", - "two-waypoint PWL edge in the 1 cm shelf world, healthy " - "clearance; certified as a single order-1 Bezier segment, with " - "the clearance ground truth measured on a quintic composite " - "Bezier tracing the same joint-space point set"); - json.Write("model", kIiwaUrl); - json.Write("shelf_scale", scale); - json.Write("pair_count", world.pair_count); - json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); - json.Write("num_positions", world.diagram->plant().num_positions()); - json.Write("edge_length_rad", (q2 - q1).norm()); - WriteClearance(&json, clearance); - WriteOptions(&json, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); - json.Write("verdict", VerdictName(certify_all.verdict)); - WriteStats(&json, certify_all.stats); - WriteTiming(&json, "wall_ms", certify_all.timing); - WriteCertRun(&json, "certify_all_serial", certify_all); - WriteCertRun(&json, "find_first_serial", find_first); - MeasureSampledEdge(&json, *world.sampled, q1, q2, config.warmup, config.reps); - WriteMachine(&json, machine); - json.EndObject(); - WriteTextFile(config.out_dir + "/pwl_edge.json", json.str()); -} - -// --------------------------------------------------------------------------- -// (e) thread scaling. -// --------------------------------------------------------------------------- - -void RunThreadScaling(const Config& config, const MachineInfo& machine, - const World& world, const MatrixXd& shelf_waypoints, - double scale) { - std::printf( - "[e threads] building a batch of %d certified-free " - "trajectories ...\n", - config.batch); - std::mt19937 rng(20260826); - std::uniform_real_distribution jitter(-0.02, 0.02); - std::vector candidates; - const int max_candidates = 8 * config.batch; - candidates.reserve(max_candidates); - for (int i = 0; i < max_candidates; ++i) { - MatrixXd w = shelf_waypoints; - if (i > 0) { - for (int c = 0; c < w.cols(); ++c) { - for (int r = 0; r < w.rows(); ++r) w(r, c) += jitter(rng); - } - } - candidates.push_back(w); - } - - // Screen in parallel: only trajectories the checker *proves* free join the - // batch, so the throughput numbers are all full certifications. - std::vector ok(candidates.size(), 0); - { - std::atomic cursor{0}; - const auto screen = [&]() { - const Options options = - MakeOptions(SearchMode::kCertifyAll, Parallelism::None()); - for (;;) { - const size_t i = cursor.fetch_add(1); - if (i >= candidates.size()) return; - const auto traj = - MakeQuinticCompositeBezier(candidates[i], ShelfTrajectoryTimes()); - ok[i] = world.certified->CheckTrajectory(*traj, options).verdict == - Verdict::kCertifiedFree; - } - }; - std::vector pool; - pool.reserve(config.max_threads); - for (int t = 0; t < config.max_threads; ++t) pool.emplace_back(screen); - for (auto& th : pool) th.join(); - } - - std::vector>> accepted; - int screened = 0; - for (size_t i = 0; i < candidates.size() && - static_cast(accepted.size()) < config.batch; - ++i) { - ++screened; - if (!ok[i]) continue; - accepted.push_back( - MakeQuinticCompositeBezier(candidates[i], ShelfTrajectoryTimes())); - } - const double acceptance = - screened > 0 - ? static_cast(accepted.size()) / static_cast(screened) - : 0.0; - std::printf("[e threads] accepted %zu of %d screened (%.1f%%)\n", - accepted.size(), screened, 100.0 * acceptance); - - // Independent re-verification of a sample of the accepted batch: the - // certificate says free, dense sampling must agree. - double verify_min = kMaxProbeDistance; - const int verify_count = std::min(16, static_cast(accepted.size())); - for (int i = 0; i < verify_count; ++i) { - const size_t index = static_cast(i) * accepted.size() / - static_cast(verify_count); - const ClearanceReport r = - MeasureSweptClearance(*world.diagram, *accepted[index], world.env_ids, - 20000, config.max_threads, kMaxProbeDistance); - verify_min = std::min(verify_min, r.min_all); - } - std::printf( - "[e threads] re-verified %d sampled members; worst dense " - "clearance %.6f m\n", - verify_count, verify_min); - - JsonWriter json; - json.BeginObject(); - json.Write("scenario", "e_thread_scaling"); - json.Write("description", - "batch of certification calls on the 1 cm tier trajectory and " - "jittered variants (+/-0.02 rad on every waypoint coordinate) " - "that remain certified free"); - json.Write("model", kIiwaUrl); - json.Write("shelf_scale", scale); - json.Write("pair_count", world.pair_count); - json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); - json.Write("batch_size", static_cast(accepted.size())); - json.Write("candidates_screened", screened); - json.Write("acceptance_rate", acceptance); - json.Write("reverified_members", verify_count); - json.Write("reverified_worst_clearance_m", verify_min); - WriteOptions(&json, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); - WriteMachine(&json, machine); - - const int thread_counts[] = {1, 8, 16}; - json.BeginArray("thread_scaling"); - double baseline_per_call = 0.0; - for (const int p : thread_counts) { - const Options options = - MakeOptions(SearchMode::kCertifyAll, Parallelism(p)); - const auto t0 = std::chrono::steady_clock::now(); - for (const auto& traj : accepted) { - world.certified->CheckTrajectory(*traj, options); - } - const auto t1 = std::chrono::steady_clock::now(); - const double seconds = std::chrono::duration(t1 - t0).count(); - const double throughput = static_cast(accepted.size()) / seconds; - if (p == 1) baseline_per_call = throughput; - json.BeginObject(); - json.Write("mode", "per_call_parallelism"); - json.Write("threads", p); - json.Write("wall_s", seconds); - json.Write("checks_per_s", throughput); - json.Write("speedup", throughput / baseline_per_call); - json.EndObject(); - std::printf( - "[e threads] per-call p=%2d %8.3f s %9.1f checks/s " - "%5.2fx\n", - p, seconds, throughput, throughput / baseline_per_call); - } - double baseline_caller = 0.0; - for (const int t : thread_counts) { - const Options options = - MakeOptions(SearchMode::kCertifyAll, Parallelism::None()); - std::atomic cursor{0}; - const auto worker = [&]() { - for (;;) { - const size_t i = cursor.fetch_add(1); - if (i >= accepted.size()) return; - world.certified->CheckTrajectory(*accepted[i], options); - } - }; - const auto s0 = std::chrono::steady_clock::now(); - std::vector pool; - pool.reserve(t); - for (int k = 0; k < t; ++k) pool.emplace_back(worker); - for (auto& th : pool) th.join(); - const auto s1 = std::chrono::steady_clock::now(); - const double seconds = std::chrono::duration(s1 - s0).count(); - const double throughput = static_cast(accepted.size()) / seconds; - if (t == 1) baseline_caller = throughput; - json.BeginObject(); - json.Write("mode", "caller_threads_serial_checks"); - json.Write("threads", t); - json.Write("wall_s", seconds); - json.Write("checks_per_s", throughput); - json.Write("speedup", throughput / baseline_caller); - json.EndObject(); - std::printf( - "[e threads] caller t=%2d %8.3f s %9.1f checks/s " - "%5.2fx\n", - t, seconds, throughput, throughput / baseline_caller); - } - json.EndArray(); - json.EndObject(); - WriteTextFile(config.out_dir + "/thread_scaling.json", json.str()); - std::printf("\n"); -} - -// --------------------------------------------------------------------------- -// (c) dual-arm handover. -// --------------------------------------------------------------------------- - -void RunDualArm(const Config& config, const MachineInfo& machine) { - constexpr double kBaseSeparation = 1.00; - const MatrixXd waypoints = DualArmTrajectoryWaypoints(); - const auto trajectory = - MakeQuinticCompositeBezier(waypoints, DualArmTrajectoryTimes()); - World world = - MakeWorld(MakeDualArmWorld(kBaseSeparation), {"iiwa14", "iiwa14_1"}); - const ClearanceReport clearance = MeasureSweptClearance( - *world.diagram, *trajectory, world.env_ids, config.dense_samples, - config.max_threads, kMaxProbeDistance); - std::printf( - "[c dual arm] separation=%.3f m clearance all=%.6f m " - "env=%.6f m pairs=%d\n", - kBaseSeparation, clearance.min_all, clearance.min_env, world.pair_count); - - const CertRun serial_all = - MeasureCertify(*world.certified, *trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), - config.warmup, config.reps); - const CertRun par16 = - MeasureCertify(*world.certified, *trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism(16)), - config.warmup, config.reps); - PrintHeader(); - PrintRow("certify_all serial", serial_all); - PrintRow("certify_all 16 threads", par16); - std::printf("\n"); - - JsonWriter json; - json.BeginObject(); - json.Write("scenario", "c_dual_arm_handover"); - json.Write("description", - "two iiwa14 dense-sphere arms welded 1.00 m apart facing each " - "other; 4-segment quintic composite Bezier bringing the " - "end-effectors past each other and back"); - json.Write("model", kIiwaUrl); - json.Write("base_separation_m", kBaseSeparation); - json.Write("pair_count", world.pair_count); - json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); - json.Write("num_positions", world.diagram->plant().num_positions()); - json.Write("trajectory_segments", static_cast(waypoints.cols()) - 1); - json.Write("trajectory_degree", 5); - WriteClearance(&json, clearance); - WriteOptions(&json, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); - json.Write("verdict", VerdictName(serial_all.verdict)); - WriteStats(&json, serial_all.stats); - WriteTiming(&json, "wall_ms", serial_all.timing); - WriteCertRun(&json, "certify_all_serial", serial_all); - WriteCertRun(&json, "certify_all_16_threads", par16); - WriteMachine(&json, machine); - json.EndObject(); - WriteTextFile(config.out_dir + "/dual_arm.json", json.str()); -} - -// --------------------------------------------------------------------------- -// (d) grazing. -// --------------------------------------------------------------------------- - -void RunGrazing(const Config& config, const MachineInfo& machine, - const Trajectory& shelf_trajectory) { - std::printf("[d grazing] tuning shelf placement for zero clearance ...\n"); - const double scale = TuneShelfScale(config, 0.0); - World world = MakeWorld(MakeShelfWorld(scale), {"iiwa14"}); - const ClearanceReport clearance = MeasureSweptClearance( - *world.diagram, shelf_trajectory, world.env_ids, config.dense_samples, - config.max_threads, kMaxProbeDistance); - std::printf( - "[d grazing] shelf_scale=%.6f clearance env=%.9f m " - "all=%.9f m\n", - scale, clearance.min_env, clearance.min_all); - - JsonWriter json; - json.BeginObject(); - json.Write("scenario", "d_grazing"); - json.Write("description", - "the same shelf world placed so the trajectory's swept " - "clearance sits within the oracle tolerance of zero: the " - "conservative certifier must refine to the resolution floor and " - "report kInconclusive rather than a certificate"); - json.Write("model", kIiwaUrl); - json.Write("shelf_scale", scale); - json.Write("pair_count", world.pair_count); - json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); - WriteClearance(&json, clearance); - - // The resolution floor is the knob that prices the pathological case: cost - // at the floor grows like log2(1 / min_interval). - constexpr double kFloors[] = {1e-9, 1e-6, 1e-4, 1e-2}; - CertRun default_run; - json.BeginArray("min_interval_sweep"); - PrintHeader(); - for (const double floor : kFloors) { - const Options options = - MakeOptions(SearchMode::kCertifyAll, Parallelism::None(), floor); - int warmup = 0; - int reps = 0; - PlanReps(config, ProbeCost(*world.certified, shelf_trajectory, options), - &warmup, &reps); - const CertRun run = MeasureCertify(*world.certified, shelf_trajectory, - options, warmup, reps); - if (floor == 1e-9) default_run = run; - json.BeginObject(); - json.Write("min_interval", floor); - json.Write("verdict", VerdictName(run.verdict)); - json.Write("num_findings", run.num_findings); - WriteStats(&json, run.stats); - WriteTiming(&json, "wall_ms", run.timing); - json.EndObject(); - char label[64]; - std::snprintf(label, sizeof(label), "min_interval=%g", floor); - PrintRow(label, run); - } - json.EndArray(); - // kFindFirstViolation at the same floor: with no definite violation - // anywhere on the trajectory the earliest-witness bound never prunes, so - // this should cost the same as kCertifyAll. The row below measures that - // rather than assuming it. - const Options first_options = - MakeOptions(SearchMode::kFindFirstViolation, Parallelism::None()); - int first_warmup = 0; - int first_reps = 0; - PlanReps(config, ProbeCost(*world.certified, shelf_trajectory, first_options), - &first_warmup, &first_reps); - const CertRun find_first = - MeasureCertify(*world.certified, shelf_trajectory, first_options, - first_warmup, first_reps); - PrintRow("find_first serial", find_first); - std::printf("\n"); - - WriteOptions(&json, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); - json.Write("verdict", VerdictName(default_run.verdict)); - WriteStats(&json, default_run.stats); - WriteTiming(&json, "wall_ms", default_run.timing); - WriteCertRun(&json, "certify_all_serial", default_run); - WriteCertRun(&json, "find_first_serial", find_first); - WriteMachine(&json, machine); - json.EndObject(); - WriteTextFile(config.out_dir + "/grazing.json", json.str()); -} - -// --------------------------------------------------------------------------- -// (f) performance review: where the time goes, and why per-call parallelism -// saturates. Not one of the standard scenarios; it attributes cost and probes -// parallel granularity by measurement rather than by assertion. -// --------------------------------------------------------------------------- - -void RunProfile(const Config& config, const MachineInfo& machine, - const Trajectory& shelf_trajectory) { - std::printf("[f profile] rebuilding the 1 cm world ...\n"); - const double scale = TuneShelfScale(config, 0.010); - World world = MakeWorld(MakeShelfWorld(scale), {"iiwa14"}); - - JsonWriter json; - json.BeginObject(); - json.Write("scenario", "f_profile"); - json.Write("description", - "cost attribution and parallel-granularity probe backing the " - "gap analysis in the benchmark write-up; not one of the standard " - "scenarios"); - json.Write("model", kIiwaUrl); - json.Write("shelf_scale", scale); - json.Write("pair_count", world.pair_count); - json.Write("scene_graph_collision_candidates", world.scene_graph_candidates); - - // --- Cost attribution ----------------------------------------------------- - // Two microbenchmarks over the same inner loop isolate the marginal cost of - // a narrowphase query from the fixed per-configuration cost (SetPositions - // plus the pose/broadphase update the first query forces). - constexpr int kInner = 2000; - constexpr int kManyQueries = 27; // ~ the observed queries per node - const auto& oracle = world.certified->distance_oracle(); - const auto& pairs = world.certified->pairs(); - const auto& plant = world.diagram->plant(); - const auto& scene_graph = world.diagram->scene_graph(); - auto root = world.diagram->CreateDefaultContext(); - const std::vector configs = - SampleTrajectory(shelf_trajectory, kInner); - // Accumulator so the optimizer cannot discard the timed queries. - double sink = 0.0; - const auto sweep = [&](int queries_per_config) { - return TimeRepeatedly(config.warmup, config.reps, [&]() { - for (int i = 0; i < kInner; ++i) { - plant.SetPositions(&world.diagram->mutable_plant_context(root.get()), - configs[i]); - const auto& query_object = - scene_graph.get_query_output_port() - .Eval>( - world.diagram->scene_graph_context(*root)); - for (int k = 0; k < queries_per_config; ++k) { - sink += oracle.SignedDistance( - query_object, - pairs[static_cast(i * kManyQueries + k) % pairs.size()]); - } - } - }); - }; - const TimingSummary one = sweep(1); - const TimingSummary many = sweep(kManyQueries); - const double us_per_query = - 1000.0 * (many.median_ms - one.median_ms) / (kInner * (kManyQueries - 1)); - const double us_per_config = 1000.0 * one.median_ms / kInner - us_per_query; - - const CertRun reference = - MeasureCertify(*world.certified, shelf_trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), - config.warmup, config.reps); - const double predicted_ms = - (static_cast(reference.stats.nodes) * us_per_config + - static_cast(reference.stats.narrowphase_queries) * - us_per_query) / - 1000.0; - - json.BeginObject("cost_attribution"); - json.Write("us_per_configuration_fk_and_pose_update", us_per_config); - json.Write("us_per_narrowphase_query", us_per_query); - json.Write("nodes", reference.stats.nodes); - json.Write("narrowphase_queries", reference.stats.narrowphase_queries); - json.Write("sphere_certifications", reference.stats.sphere_certifications); - json.Write("predicted_ms", predicted_ms); - json.Write("measured_ms", reference.timing.median_ms); - json.Write("residual_ms", reference.timing.median_ms - predicted_ms); - json.Write("residual_note", - "residual covers the sphere prefilter, the lambda/Delta sparse " - "dot products, de Casteljau splitting and driver bookkeeping"); - json.Write("summed_distances_m", sink); - json.EndObject(); - std::printf( - "[f profile] %.3f us / configuration, %.3f us / narrowphase " - "query\n", - us_per_config, us_per_query); - std::printf( - "[f profile] predicted %.3f ms vs measured %.3f ms " - "(residual %.3f ms)\n", - predicted_ms, reference.timing.median_ms, - reference.timing.median_ms - predicted_ms); - - // --- Per-segment work distribution --------------------------------------- - // This measures the ceiling that *segment-root seeding* imposes: if the - // parallel driver's only work units are whole segments, the best per-call - // speedup a 6-segment trajectory can reach is total work / heaviest segment. - // Certifying each segment on its own measures it directly. The driver shares - // sub-segment nodes on demand (see certifier_internal.h), so this row is a - // *reference* bound that the measured per-call speedup is allowed to exceed. - { - const PiecewiseBezierPath path = world.certified->Normalize( - shelf_trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); - uint64_t total = 0; - uint64_t heaviest = 0; - double heaviest_ms = 0.0; - double serial_sum_ms = 0.0; - json.BeginArray("per_segment_work"); - for (size_t i = 0; i < path.segments().size(); ++i) { - const auto& segment = path.segments()[i]; - const drake::trajectories::BezierCurve curve( - segment.t_start, segment.t_end, segment.control_points); - const CertRun run = MeasureCertify( - *world.certified, curve, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), - config.warmup, config.reps); - total += run.stats.nodes; - heaviest = std::max(heaviest, run.stats.nodes); - heaviest_ms = std::max(heaviest_ms, run.timing.median_ms); - serial_sum_ms += run.timing.median_ms; - json.BeginObject(); - json.Write("segment", static_cast(i)); - json.Write("nodes", run.stats.nodes); - json.Write("narrowphase_queries", run.stats.narrowphase_queries); - WriteTiming(&json, "wall_ms", run.timing); - json.EndObject(); - } - json.EndArray(); - json.BeginObject("per_segment_summary"); - json.Write("total_nodes_over_segments", total); - json.Write("heaviest_segment_nodes", heaviest); - json.Write("segment_seeding_bound_on_per_call_speedup", - serial_sum_ms / std::max(heaviest_ms, 1e-9)); - json.Write("note", - "each segment certified on its own; the sum exceeds the " - "whole-trajectory node count only by the per-segment " - "breakpoint work. segment_seeding_bound is the ceiling a " - "driver seeded with whole segments would hit; the current " - "driver shares sub-segment nodes on demand and is not bound " - "by it"); - json.EndObject(); - std::printf( - "[f profile] heaviest segment = %llu of %llu nodes; " - "segment-seeding bound on per-call speedup = %.2fx\n", - static_cast(heaviest), // NOLINT(runtime/int) - static_cast(total), // NOLINT(runtime/int) - serial_sum_ms / std::max(heaviest_ms, 1e-9)); - } - - // --- Parallel granularity ------------------------------------------------- - // Per-call parallelism is measured on three workloads spanning three orders - // of magnitude in node count, holding everything else fixed. The three - // straddle the driver's lazy-recruitment threshold: the 15-node edge is - // below it (and must therefore be exactly serial at every p) while the other - // two are above it. - std::printf( - "[f profile] tuning the grazing world for the long " - "workload ...\n"); - const double graze_scale = TuneShelfScale(config, 0.0); - World graze = MakeWorld(MakeShelfWorld(graze_scale), {"iiwa14"}); - const MatrixXd shelf_waypoints = ShelfTrajectoryWaypoints(); - const VectorXd q1 = shelf_waypoints.col(0); - const VectorXd q2 = shelf_waypoints.col(1); - - constexpr int kParallelism[] = {1, 2, 4, 8, 16}; - json.BeginArray("parallel_granularity"); - std::printf(" %-18s %5s %10s %10s %8s\n", "workload", "p", "nodes", "med_ms", - "speedup"); - for (const char* which : {"pwl_edge_15_nodes", "shelf_1cm_146_nodes", - "grazing_min_interval_1e-6"}) { - double baseline = 0.0; - for (const int p : kParallelism) { - CertRun run; - if (std::strcmp(which, "pwl_edge_15_nodes") == 0) { - run = MeasureCertifyEdge( - *world.certified, q1, q2, - MakeOptions(SearchMode::kCertifyAll, Parallelism(p)), config.warmup, - config.reps); - } else if (std::strcmp(which, "shelf_1cm_146_nodes") == 0) { - run = - MeasureCertify(*world.certified, shelf_trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism(p)), - config.warmup, config.reps); - } else { - run = MeasureCertify( - *graze.certified, shelf_trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism(p), 1e-6), - config.warmup, config.reps); - } - if (p == 1) baseline = run.timing.median_ms; - json.BeginObject(); - json.Write("workload", which); - json.Write("threads", p); - json.Write("nodes", run.stats.nodes); - json.Write("verdict", VerdictName(run.verdict)); - WriteTiming(&json, "wall_ms", run.timing); - json.Write("speedup", baseline / run.timing.median_ms); - json.EndObject(); - std::printf(" %-18s %5d %10llu %10.3f %8.2f\n", which, p, - static_cast( // NOLINT(runtime/int) - run.stats.nodes), - run.timing.median_ms, baseline / run.timing.median_ms); - } - } - json.EndArray(); - WriteMachine(&json, machine); - json.EndObject(); - WriteTextFile(config.out_dir + "/profile.json", json.str()); - std::printf("\n"); -} - -// Usage: iiwa_benchmark [--out DIR] [--reps N] [--warmup N] -// [--dense-samples N] [--batch N] [--only NAME] -// [--drake_commit SHA] -// -// `--out` defaults to the current directory, or to $TEST_TMPDIR when that is -// set: the sandbox is the only writable directory under `bazel test`, and the -// smoke-test rule in BUILD.bazel relies on this, so it needs no --out of its -// own. `--drake_commit` (the Drake revision this binary was built from, -// "unknown" by default) is recorded verbatim in every result file, so a JSON -// result identifies the code it measured. -int Main(int argc, char** argv) { - Config config; - if (const char* const test_tmpdir = std::getenv("TEST_TMPDIR")) { - config.out_dir = test_tmpdir; - } - for (int i = 1; i < argc; ++i) { - const std::string arg = argv[i]; - const auto next = [&]() -> std::string { - if (i + 1 >= argc) throw std::runtime_error("missing value for " + arg); - return argv[++i]; - }; - if (arg == "--out") { - config.out_dir = next(); - } else if (arg == "--reps") { - config.reps = std::stoi(next()); - } else if (arg == "--warmup") { - config.warmup = std::stoi(next()); - } else if (arg == "--dense-samples") { - config.dense_samples = std::stoi(next()); - } else if (arg == "--tune-samples") { - config.tune_samples = std::stoi(next()); - } else if (arg == "--batch") { - config.batch = std::stoi(next()); - } else if (arg == "--only") { - config.only = next(); - } else if (arg == "--drake_commit") { - config.drake_commit = next(); - } else { - std::fprintf(stderr, "unknown argument: %s\n", arg.c_str()); - return 1; - } - } - const auto wanted = [&](const std::string& name) { - return config.only.empty() || config.only == name; - }; - - const MachineInfo machine = GetMachineInfo(config.drake_commit); - std::printf("continuous_collision benchmark suite\n"); - std::printf(" cpu : %s (%d logical cores)\n", - machine.cpu_model.c_str(), machine.core_count); - std::printf(" drake pin : %s (%s)\n", machine.drake_commit.c_str(), - machine.drake_version_note.c_str()); - std::printf(" model : %s\n", kIiwaUrl); - std::printf(" reps : %d timed after %d warmup\n", config.reps, - config.warmup); - std::printf(" output : %s\n\n", config.out_dir.c_str()); - - const MatrixXd shelf_waypoints = ShelfTrajectoryWaypoints(); - const auto shelf_trajectory = - MakeQuinticCompositeBezier(shelf_waypoints, ShelfTrajectoryTimes()); - - struct Tier { - const char* name; - const char* file; - double target; - bool headline; - }; - constexpr Tier kTiers[] = { - {"a shelf 2mm", "shelf_2mm", 0.002, false}, - {"a shelf 1cm", "shelf_1cm", 0.010, true}, - {"a shelf 5cm", "shelf_5cm", 0.050, false}, - }; - - for (const Tier& tier : kTiers) { - const bool need_headline_world = - tier.headline && (wanted("pwl") || wanted("threads")); - if (!wanted("shelf") && !need_headline_world) continue; - - std::printf("[%s] tuning shelf placement for %.0f mm ...\n", tier.name, - 1000.0 * tier.target); - const double scale = TuneShelfScale(config, tier.target); - World world = MakeWorld(MakeShelfWorld(scale), {"iiwa14"}); - const ClearanceReport clearance = MeasureSweptClearance( - *world.diagram, *shelf_trajectory, world.env_ids, config.dense_samples, - config.max_threads, kMaxProbeDistance); - std::printf( - "[%s] shelf_scale=%.6f clearance env=%.6f m all=%.6f m " - "pairs=%d\n", - tier.name, scale, clearance.min_env, clearance.min_all, - world.pair_count); - - if (wanted("shelf")) { - const CertRun serial_all = MeasureCertify( - *world.certified, *shelf_trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None()), - config.warmup, config.reps); - const CertRun serial_first = MeasureCertify( - *world.certified, *shelf_trajectory, - MakeOptions(SearchMode::kFindFirstViolation, Parallelism::None()), - config.warmup, config.reps); - const CertRun par8 = - MeasureCertify(*world.certified, *shelf_trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism(8)), - config.warmup, config.reps); - const CertRun par16 = - MeasureCertify(*world.certified, *shelf_trajectory, - MakeOptions(SearchMode::kCertifyAll, Parallelism(16)), - config.warmup, config.reps); - - PrintHeader(); - PrintRow("certify_all serial", serial_all); - PrintRow("find_first serial", serial_first); - PrintRow("certify_all 8 threads", par8); - PrintRow("certify_all 16 threads", par16); - std::printf("\n"); - - JsonWriter json; - json.BeginObject(); - json.Write("scenario", std::string("a_") + tier.file); - json.Write("description", - "iiwa14 (dense-sphere collision model) welded to the world, " - "seven-box bookcase plus a table slab; 6-segment quintic " - "composite Bezier reaching into the shelf bay and back"); - json.Write("model", kIiwaUrl); - json.Write("shelf_scale", scale); - json.Write("target_clearance_m", tier.target); - json.Write("pair_count", world.pair_count); - json.Write("scene_graph_collision_candidates", - world.scene_graph_candidates); - json.Write("num_positions", world.diagram->plant().num_positions()); - json.Write("trajectory_segments", - static_cast(shelf_waypoints.cols()) - 1); - json.Write("trajectory_degree", 5); - WriteClearance(&json, clearance); - WriteOptions(&json, - MakeOptions(SearchMode::kCertifyAll, Parallelism::None())); - json.Write("verdict", VerdictName(serial_all.verdict)); - WriteStats(&json, serial_all.stats); - WriteTiming(&json, "wall_ms", serial_all.timing); - WriteCertRun(&json, "certify_all_serial", serial_all); - WriteCertRun(&json, "find_first_serial", serial_first); - WriteCertRun(&json, "certify_all_8_threads", par8); - WriteCertRun(&json, "certify_all_16_threads", par16); - if (tier.headline) { - MeasureSampledPathSweep(&json, *world.sampled, *shelf_trajectory, - config.warmup, config.reps); - } - WriteMachine(&json, machine); - json.EndObject(); - WriteTextFile(config.out_dir + "/" + tier.file + ".json", json.str()); - } - - if (tier.headline && wanted("pwl")) { - RunPwlEdge(config, machine, world, shelf_waypoints, scale); - } - if (tier.headline && wanted("threads")) { - RunThreadScaling(config, machine, world, shelf_waypoints, scale); - } - } - - if (wanted("dual")) RunDualArm(config, machine); - if (wanted("grazing")) RunGrazing(config, machine, *shelf_trajectory); - if (wanted("profile")) RunProfile(config, machine, *shelf_trajectory); - - std::printf("done; results in %s\n", config.out_dir.c_str()); - return 0; -} - -} // namespace -} // namespace internal -} // namespace continuous_collision -} // namespace planning -} // namespace drake - -int main(int argc, char** argv) { - try { - return drake::planning::continuous_collision::internal::Main(argc, argv); - } catch (const std::exception& e) { - std::fprintf(stderr, "benchmark failed: %s\n", e.what()); - return 1; - } -} diff --git a/planning/continuous_collision/benchmark/scenario_worlds.cc b/planning/continuous_collision/benchmark/scenario_worlds.cc deleted file mode 100644 index d0d8a2f0a542..000000000000 --- a/planning/continuous_collision/benchmark/scenario_worlds.cc +++ /dev/null @@ -1,182 +0,0 @@ -#include "drake/planning/continuous_collision/benchmark/scenario_worlds.h" - -#include - -#include "drake/geometry/shape_specification.h" -#include "drake/math/rigid_transform.h" -#include "drake/math/rotation_matrix.h" -#include "drake/multibody/parsing/parser.h" -#include "drake/multibody/plant/coulomb_friction.h" -#include "drake/multibody/plant/multibody_plant.h" -#include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/robot_diagram_builder.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace internal { -namespace { - -using drake::geometry::Box; -using drake::math::RigidTransformd; -using drake::math::RotationMatrixd; -using drake::multibody::CoulombFriction; -using drake::multibody::ModelInstanceIndex; -using drake::multibody::MultibodyPlant; -using drake::multibody::Parser; -using drake::multibody::RigidBody; -using drake::multibody::SpatialInertia; -using drake::planning::RobotDiagram; -using drake::planning::RobotDiagramBuilder; -using Eigen::MatrixXd; -using Eigen::Vector3d; - -CoulombFriction Friction() { - return CoulombFriction(1.0, 1.0); -} - -// Adds one anchored box to the "environment" model instance. -void AddAnchoredBox(MultibodyPlant* plant, const std::string& name, - const Vector3d& size, const RigidTransformd& X_WB) { - if (!plant->HasModelInstanceNamed("environment")) { - plant->AddModelInstance("environment"); - } - const ModelInstanceIndex instance = - plant->GetModelInstanceByName("environment"); - const RigidBody& body = - plant->AddRigidBody(name, instance, - SpatialInertia::SolidBoxWithMass( - 1.0, size.x(), size.y(), size.z())); - plant->WeldFrames(plant->world_frame(), body.body_frame(), X_WB); - plant->RegisterCollisionGeometry(body, RigidTransformd(), - Box(size.x(), size.y(), size.z()), - name + "_geom", Friction()); -} - -void AddTable(MultibodyPlant* plant) { - AddAnchoredBox(plant, "table", Vector3d(3.0, 3.0, 0.10), - RigidTransformd(Vector3d(0.0, 0.0, -0.05))); -} - -// The bookcase: seven anchored boxes, namely two side panels, four horizontal -// boards (bottom, the two bounding the reached-into bay, and top) and a back -// panel. All are in the "environment" instance, so they form one welded -// subgraph with the world and with each other. -void AddShelf(MultibodyPlant* plant, double s) { - using S = ShelfGeometry; - const double x_front = S::kFrontX + s; - const double x_back = x_front + S::kDepth; - const double bay_h = S::kBayHalfHeight + s; - const double x_mid = 0.5 * (x_front + x_back); - const double z_mid = 0.5 * (S::kBottomZ + S::kTopZ); - const double height = S::kTopZ - S::kBottomZ; - const double width = 2.0 * S::kHalfWidth; - - AddAnchoredBox( - plant, "shelf_side_l", Vector3d(S::kDepth, S::kPanel, height), - RigidTransformd(Vector3d(x_mid, S::kHalfWidth + 0.5 * S::kPanel, z_mid))); - AddAnchoredBox(plant, "shelf_side_r", Vector3d(S::kDepth, S::kPanel, height), - RigidTransformd( - Vector3d(x_mid, -S::kHalfWidth - 0.5 * S::kPanel, z_mid))); - AddAnchoredBox(plant, "shelf_board_bottom", - Vector3d(S::kDepth, width, S::kPanel), - RigidTransformd(Vector3d(x_mid, 0.0, S::kBottomZ))); - AddAnchoredBox(plant, "shelf_board_low", - Vector3d(S::kDepth, width, S::kPanel), - RigidTransformd(Vector3d( - x_mid, 0.0, S::kBayCentreZ - bay_h - 0.5 * S::kPanel))); - AddAnchoredBox(plant, "shelf_board_high", - Vector3d(S::kDepth, width, S::kPanel), - RigidTransformd(Vector3d( - x_mid, 0.0, S::kBayCentreZ + bay_h + 0.5 * S::kPanel))); - AddAnchoredBox(plant, "shelf_board_top", - Vector3d(S::kDepth, width, S::kPanel), - RigidTransformd(Vector3d(x_mid, 0.0, S::kTopZ))); - AddAnchoredBox( - plant, "shelf_back", Vector3d(S::kPanel, width + 2.0 * S::kPanel, height), - RigidTransformd(Vector3d(x_back + 0.5 * S::kPanel, 0.0, z_mid))); -} - -} // namespace - -const char* const kIiwaUrl = - "package://drake_models/iiwa_description/urdf/" - "iiwa14_spheres_dense_collision.urdf"; - -std::shared_ptr> MakeShelfWorld(double shelf_scale) { - RobotDiagramBuilder builder(0.0); - MultibodyPlant& plant = builder.plant(); - builder.parser().AddModelsFromUrl(kIiwaUrl); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"), - RigidTransformd()); - AddTable(&plant); - AddShelf(&plant, shelf_scale); - plant.Finalize(); - return std::shared_ptr>(builder.Build()); -} - -std::shared_ptr> MakeDualArmWorld(double base_separation) { - RobotDiagramBuilder builder(0.0); - MultibodyPlant& plant = builder.plant(); - builder.parser().SetAutoRenaming(true); - const ModelInstanceIndex arm_a = - builder.parser().AddModelsFromUrl(kIiwaUrl).at(0); - const ModelInstanceIndex arm_b = - builder.parser().AddModelsFromUrl(kIiwaUrl).at(0); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base", arm_a), - RigidTransformd()); - plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base", arm_b), - RigidTransformd(RotationMatrixd::MakeZRotation(M_PI), - Vector3d(base_separation, 0.0, 0.0))); - AddTable(&plant); - plant.Finalize(); - return std::shared_ptr>(builder.Build()); -} - -MatrixXd ShelfTrajectoryWaypoints() { - MatrixXd w(7, 7); - // clang-format off - w << 0.0, 0.196552, 0.096805, -0.016522, -0.138992, -0.254084, 0.0, - 0.0, -0.219376, 0.213452, 0.715028, 0.250007, -0.270004, 0.0, - 0.0, 0.220295, 0.105212, 0.034537, -0.061201, -0.171787, 0.0, - 0.0, -1.546472, -1.471150, -0.931648, -1.577882, -1.679656, 0.0, - 0.0, 0.035924, -0.011122, -0.017085, -0.005269, -0.019359, 0.0, - 0.0, 0.740798, 0.465992, 0.214658, 0.321424, 0.426071, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - // clang-format on - return w; -} - -std::vector ShelfTrajectoryTimes() { - return {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0}; -} - -MatrixXd DualArmTrajectoryWaypoints() { - Eigen::VectorXd reach(14); - // Arm A and arm B reach poses: both extend forward at ~0.49 m with a small - // base yaw so the wrists pass each other offset in y and z. - reach << 0.12, 0.0, 0.0, -1.40, 0.0, 1.10, 0.0, -0.12, 0.0, 0.0, -1.70, 0.0, - 0.80, 0.0; - Eigen::VectorXd offset = Eigen::VectorXd::Zero(14); - offset(0) = 0.10; - offset(3) = 0.08; - offset(7) = -0.10; - offset(10) = 0.08; - - MatrixXd w(14, 5); - w.col(0) = Eigen::VectorXd::Zero(14); - w.col(1) = 0.5 * reach; - w.col(2) = reach; - w.col(3) = 0.5 * reach + offset; - w.col(4) = Eigen::VectorXd::Zero(14); - return w; -} - -std::vector DualArmTrajectoryTimes() { - return {0.0, 1.0, 2.0, 3.0, 4.0}; -} - -} // namespace internal -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/benchmark/scenario_worlds.h b/planning/continuous_collision/benchmark/scenario_worlds.h deleted file mode 100644 index 89a561677d4f..000000000000 --- a/planning/continuous_collision/benchmark/scenario_worlds.h +++ /dev/null @@ -1,72 +0,0 @@ -#pragma once - -// The fixed benchmark worlds and trajectories. Every world is built from the -// cached `drake_models` iiwa14 dense-sphere collision model plus programmatic -// anchored boxes, so a run is reproducible from this file alone. - -#include -#include -#include - -#include - -#include "drake/planning/robot_diagram.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace internal { - -// The dense-sphere iiwa14 collision variant: 46 collision spheres over links -// 0-7, i.e. realistic proximity-pair counts rather than the 4-primitive model. -extern const char* const kIiwaUrl; - -// Nominal shelf geometry, in metres: the fixed dimensions of the bookcase that -// MakeShelfWorld builds. -struct ShelfGeometry { - static constexpr double kBayCentreZ = 0.60; - static constexpr double kFrontX = 0.62; - static constexpr double kDepth = 0.32; - static constexpr double kBayHalfHeight = 0.13; - static constexpr double kPanel = 0.03; - static constexpr double kHalfWidth = 0.45; - static constexpr double kBottomZ = 0.05; - static constexpr double kTopZ = 1.25; -}; - -// iiwa14 welded to the world origin, a 3 m table slab, and a seven-box -// bookcase in reach. Model instances are named "iiwa14" and "environment". -// `shelf_scale` s translates the whole bookcase by +s in x *and* opens the -// reached-into bay by s on each side, so the swept clearance of the fixed -// benchmark trajectory is monotone non-decreasing in s over the useful range. -// This one scalar is what the tier bisection turns. -std::shared_ptr> MakeShelfWorld(double shelf_scale); - -// Two iiwa14s welded to the world `base_separation` apart along +x, the -// second rotated 180 degrees about z so the arms face each other, over the -// same table slab. Model instances: "iiwa14", "iiwa14_1", "environment". -std::shared_ptr> MakeDualArmWorld(double base_separation); - -// The 7 x 7 joint-space waypoint matrix of the shelf-reaching trajectory: -// home, up-and-over on the +y side, into the bay mouth, deep inside the bay, -// out on the -y side, and home. Solved once offline with -// drake::multibody::InverseKinematics (position + tool-axis + minimum- -// distance constraints) against the shelf-free world, then frozen here so -// the benchmark has no solver dependency and no run-to-run drift. -Eigen::MatrixXd ShelfTrajectoryWaypoints(); - -// Times of the shelf waypoints (0, 1, ..., 6): 6 quintic Bézier segments. -std::vector ShelfTrajectoryTimes(); - -// The 14 x 5 waypoint matrix of the dual-arm handover: both arms home, half -// way, at the handover poses (end-effectors passing within a few cm), a -// slightly different half-way pose on the way back, and home. -Eigen::MatrixXd DualArmTrajectoryWaypoints(); - -// Times of the dual-arm waypoints (0, 1, ..., 4): 4 quintic Bézier segments. -std::vector DualArmTrajectoryTimes(); - -} // namespace internal -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/certifier_internal.cc b/planning/continuous_collision/certifier_internal.cc index 146d58110cf4..d3a9f10064cd 100644 --- a/planning/continuous_collision/certifier_internal.cc +++ b/planning/continuous_collision/certifier_internal.cc @@ -297,12 +297,12 @@ struct Recruitment { objects, one thread creation per helper and, at the end of the run, one join per helper before the lead can collect their statistics. Thread creation dominates that list at tens of microseconds per worker, while a node costs - ~7-13 us on the machine the benchmark suite was measured on, 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. */ + ~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; // --------------------------------------------------------------------------- diff --git a/planning/continuous_collision/test/concurrency_timing_test.cc b/planning/continuous_collision/test/concurrency_timing_test.cc index cb1e34687c6f..2ed7a86bbc5b 100644 --- a/planning/continuous_collision/test/concurrency_timing_test.cc +++ b/planning/continuous_collision/test/concurrency_timing_test.cc @@ -42,8 +42,8 @@ namespace { // Both are timing claims, so both are written to survive a loaded machine: a // ratio with a wide margin, best-of-three, and a skip when the hardware or the // build cannot support the claim at all. They are regression detectors rather -// than benchmarks (the numbers live in benchmark/results/), and should only -// ever fire on a driver that has stopped distributing work. +// than benchmarks, and should only ever fire on a driver that has stopped +// distributing work. // True when the build cannot support a meaningful wall-clock claim: a sanitizer // build serializes and inflates everything, an unoptimized build changes the From 4a65698956c074f7be6461b4b03a985110287142 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Fri, 28 Aug 2026 10:58:23 -0400 Subject: [PATCH 17/22] [planning] continuous_collision: reuse Drake utilities and remove dead code --- .../planning_continuous_collision.h | 22 +-- .../planning_py_continuous_collision.cc | 7 +- .../test/continuous_collision_test.py | 1 - planning/continuous_collision/BUILD.bazel | 21 ++- .../certifier_internal.cc | 127 +++++++----------- .../continuous_collision/certifier_internal.h | 23 ++-- .../continuous_collision_checker.cc | 91 ++++--------- .../continuous_collision/distance_oracle.cc | 85 +++--------- .../motion_bound_table.cc | 35 +---- .../continuous_collision/motion_bound_table.h | 18 +-- .../piecewise_bezier_path.cc | 54 +++----- planning/continuous_collision/shape_class.h | 70 ++++++++++ .../vpolytope_ingestion.cc | 17 +-- 13 files changed, 217 insertions(+), 354 deletions(-) create mode 100644 planning/continuous_collision/shape_class.h diff --git a/bindings/generated_docstrings/planning_continuous_collision.h b/bindings/generated_docstrings/planning_continuous_collision.h index c6d4ac909b19..b4fd48cedc3f 100644 --- a/bindings/generated_docstrings/planning_continuous_collision.h +++ b/bindings/generated_docstrings/planning_continuous_collision.h @@ -533,7 +533,7 @@ hold no mutable state, so concurrent ComputeMotionBoundTable() calls are safe. Typical use by the certifier: - once, at checker construction: -KinematicsEngine engine(model); engine.body_spheres(b) for the +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).)"""; @@ -621,20 +621,6 @@ R"""(Radius, about the body frame origin, of a sphere containing every proximity geometry of ``body`` — the start of the reach chain. Zero for a body with no (non-HalfSpace) proximity geometry.)"""; } body_radius; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_sphere_geometries - struct /* body_sphere_geometries */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(The geometry ids matching body_spheres(body), element for element.)"""; - } body_sphere_geometries; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_spheres - struct /* body_spheres */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Bounding spheres (body frame) of every proximity geometry of ``body``, -used by the reach chain start and by the certifier's sphere prefilter. -HalfSpace geometries have no bounding sphere and are omitted.)"""; - } body_spheres; // Symbol: drake::planning::continuous_collision::KinematicsEngine::geometry_sphere struct /* geometry_sphere */ { // Source: drake/planning/continuous_collision/motion_bound_table.h @@ -731,12 +717,6 @@ pair's two geometries can move relative to each other purely through the coordinates the table no longer tracks. Zero when every carved coordinate is exactly constant.)"""; } carveout_slack; - // Symbol: drake::planning::continuous_collision::MotionBoundTable::num_entries - struct /* num_entries */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Total number of (coordinate, λ) entries over all pairs.)"""; - } num_entries; // Symbol: drake::planning::continuous_collision::MotionBoundTable::num_pairs struct /* num_pairs */ { // Source: drake/planning/continuous_collision/motion_bound_table.h diff --git a/bindings/pydrake/planning/planning_py_continuous_collision.cc b/bindings/pydrake/planning/planning_py_continuous_collision.cc index ead4c618a1d7..7a8de8b2d0be 100644 --- a/bindings/pydrake/planning/planning_py_continuous_collision.cc +++ b/bindings/pydrake/planning/planning_py_continuous_collision.cc @@ -246,8 +246,7 @@ collision-free over its entire continuous time domain, rather than sampling it. .def("carveout_slack", &Class::carveout_slack, py::arg("pair_index"), cls_doc.carveout_slack.doc) .def("GetEntries", &Class::GetEntries, py::arg("pair_index"), - cls_doc.GetEntries.doc) - .def("num_entries", &Class::num_entries, cls_doc.num_entries.doc); + cls_doc.GetEntries.doc); DefCopyAndDeepCopy(&cls); } @@ -274,10 +273,6 @@ collision-free over its entire continuous time domain, rather than sampling it. const std::vector&>(&Class::ComputeMotionBoundTable), py::arg("lower"), py::arg("upper"), py::arg("constant_coordinates"), py::arg("pairs"), cls_doc.ComputeMotionBoundTable.doc_4args) - .def("body_spheres", &Class::body_spheres, py::arg("body"), - cls_doc.body_spheres.doc) - .def("body_sphere_geometries", &Class::body_sphere_geometries, - py::arg("body"), cls_doc.body_sphere_geometries.doc) .def("geometry_sphere", &Class::geometry_sphere, py::arg("id"), cls_doc.geometry_sphere.doc) .def("body_has_halfspace", &Class::body_has_halfspace, py::arg("body"), diff --git a/bindings/pydrake/planning/test/continuous_collision_test.py b/bindings/pydrake/planning/test/continuous_collision_test.py index 0c0d86402951..43553fd2bf49 100644 --- a/bindings/pydrake/planning/test/continuous_collision_test.py +++ b/bindings/pydrake/planning/test/continuous_collision_test.py @@ -213,7 +213,6 @@ def test_check_path_and_trajectory(self): self.assertGreaterEqual(table.carveout_slack(pair_index=0), 0.0) self.assertIsInstance(table.pair_is_static(pair_index=0), bool) self.assertIsInstance(table.GetEntries(pair_index=0), list) - self.assertGreaterEqual(table.num_entries(), 0) def test_certificate_round_trip(self): options = _serial_options() diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index bc7182301043..9f3189224652 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -29,6 +29,19 @@ drake_cc_library( hdrs = ["numerics.h"], ) +# Shape classification shared by the oracle, the tau table and the +# bounding-sphere pass. Header-only and build-system internal. +drake_cc_library( + name = "shape_class", + hdrs = ["shape_class.h"], + internal = True, + visibility = ["//visibility:private"], + deps = [ + "//common:unused", + "//geometry:shape_specification", + ], +) + drake_cc_library( name = "options", hdrs = ["options.h"], @@ -56,6 +69,7 @@ drake_cc_library( "//common/trajectories:bspline_trajectory", "//common/trajectories:composite_trajectory", "//common/trajectories:piecewise_polynomial", + "//math:binomial_coefficient", "@fmt", ], ) @@ -92,6 +106,7 @@ drake_cc_library( "@eigen", ], implementation_deps = [ + ":shape_class", "//geometry:geometry_roles", "//geometry:scene_graph_inspector", "//geometry:shape_specification", @@ -112,7 +127,7 @@ drake_cc_library( "@eigen", ], implementation_deps = [ - "//common:unused", + ":shape_class", "//geometry:scene_graph_inspector", "//geometry:shape_specification", "//geometry/proximity:polygon_surface_mesh", @@ -164,8 +179,8 @@ drake_cc_library( "//geometry:scene_graph", "//math:geometric_transform", "//multibody/tree:multibody_tree_indexes", + "//planning:collision_checker_context", "//planning:robot_diagram", - "//systems/framework:context", "@eigen", ], implementation_deps = [ @@ -190,7 +205,7 @@ drake_cc_library( "@eigen", ], implementation_deps = [ - "//common:unused", + ":shape_class", "//geometry:scene_graph", "//geometry:scene_graph_inspector", "//geometry:shape_specification", diff --git a/planning/continuous_collision/certifier_internal.cc b/planning/continuous_collision/certifier_internal.cc index d3a9f10064cd..a5101b9db731 100644 --- a/planning/continuous_collision/certifier_internal.cc +++ b/planning/continuous_collision/certifier_internal.cc @@ -14,7 +14,6 @@ #include "drake/common/drake_assert.h" #include "drake/common/parallelism.h" -#include "drake/geometry/scene_graph.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/planning/continuous_collision/numerics.h" @@ -38,6 +37,24 @@ double TimeOf(const BezierSegment& seg, double s) { return seg.t_start + s * (seg.t_end - seg.t_start); } +/* Assembles one Finding. Every field is set here, so the call sites below + differ only in the values they pass. */ +Finding MakeFinding(double time, const Eigen::VectorXd& q, const PairId& pair, + double distance, double motion_bound, bool definite, + const Eigen::Vector3d& nearest_a_W, + const Eigen::Vector3d& nearest_b_W) { + Finding finding; + finding.time = time; + finding.q = q; + finding.pair = pair; + finding.distance = distance; + finding.motion_bound = motion_bound; + finding.definite = definite; + finding.nearest_a_W = nearest_a_W; + finding.nearest_b_W = nearest_b_W; + return finding; +} + // --------------------------------------------------------------------------- // Per-node world-frame geometry sphere centers. // --------------------------------------------------------------------------- @@ -68,6 +85,13 @@ class GeometryCache { double radius(int slot) const { return table_->geometries[slot].radius; } + /* 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() - radius(slot_a) - + radius(slot_b); + } + private: const PrefilterTable* table_{}; const ThreadContext* context_{}; @@ -551,9 +575,7 @@ void Worker::RunItem(WorkItem* item) { const int slot_a = prefilter.slot_a[p]; const int slot_b = prefilter.slot_b[p]; if (slot_a >= 0 && slot_b >= 0) { - const double lower_bound = - (geometry_.Center(slot_a) - geometry_.Center(slot_b)).norm() - - geometry_.radius(slot_a) - geometry_.radius(slot_b); + const double lower_bound = geometry_.LowerBound(slot_a, slot_b); if (IsCertified(lower_bound, tau_p, motion_bound, threshold, slack)) { ++stats_.sphere_certifications; if (emit_certificate_) { @@ -573,16 +595,9 @@ void Worker::RunItem(WorkItem* item) { // qc is exactly on the trajectory (it is the de Casteljau apex), so // ϕ_true(qc) ≤ ϕ̂ + τ_p < m_p is a definite violation of the // continuum statement, not a sampling artifact. - Finding finding; - finding.time = t_mid; - finding.q = q_mid_; - finding.pair = pair.id; - finding.distance = phi_hat; - finding.motion_bound = motion_bound; - finding.definite = true; - finding.nearest_a_W = nearest_a_; - finding.nearest_b_W = nearest_b_; - sink_->AddDefinite(std::move(finding)); + sink_->AddDefinite(MakeFinding(t_mid, q_mid_, pair.id, phi_hat, + motion_bound, true, nearest_a_, + nearest_b_)); if (!find_first_ || at_floor) { // kCertifyAll (or a floor node, which has no children to refine // into): drop p from this subtree. Without this a single @@ -612,16 +627,9 @@ void Worker::RunItem(WorkItem* item) { // --- Gray: subdivide, unless we are already at the resolution floor. - if (at_floor) { - Finding finding; - finding.time = t_mid; - finding.q = q_mid_; - finding.pair = pair.id; - finding.distance = phi_hat; - finding.motion_bound = motion_bound; - finding.definite = false; - finding.nearest_a_W = nearest_a_; - finding.nearest_b_W = nearest_b_; - sink_->AddInconclusive(std::move(finding)); + sink_->AddInconclusive(MakeFinding(t_mid, q_mid_, pair.id, phi_hat, + motion_bound, false, nearest_a_, + nearest_b_)); } else { arena_[survivor_offset + survivor_count] = p; ++survivor_count; @@ -719,9 +727,7 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, 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->Center(slot_a) - geometry->Center(slot_b)).norm() - - geometry->radius(slot_a) - geometry->radius(slot_b); + lower_bound = geometry->LowerBound(slot_a, slot_b); } if (is_static) { @@ -749,16 +755,10 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, &nearest_a, &nearest_b); if (IsDefiniteViolation(phi_hat, tau_p, threshold)) { - Finding finding; - finding.time = time; - finding.q = q; - finding.pair = pair.id; - finding.distance = phi_hat; - finding.motion_bound = 0.0; - finding.definite = true; - finding.nearest_a_W = nearest_a; - finding.nearest_b_W = nearest_b; - sink->AddDefinite(std::move(finding)); + // A breakpoint is a single configuration, so the finding carries no + // motion bound. + sink->AddDefinite(MakeFinding(time, q, pair.id, phi_hat, 0.0, true, + nearest_a, nearest_b)); continue; } if (!is_static) continue; @@ -775,16 +775,8 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, // 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. - Finding finding; - finding.time = time; - finding.q = q; - finding.pair = pair.id; - finding.distance = phi_hat; - finding.motion_bound = static_bound; - finding.definite = false; - finding.nearest_a_W = nearest_a; - finding.nearest_b_W = nearest_b; - sink->AddInconclusive(std::move(finding)); + sink->AddInconclusive(MakeFinding(time, q, pair.id, phi_hat, static_bound, + false, nearest_a, nearest_b)); } } @@ -806,24 +798,12 @@ void SortRecords(std::vector* records) { // ThreadContext / ContextPool. // --------------------------------------------------------------------------- -ThreadContext::ThreadContext(const drake::planning::RobotDiagram& model) - : model_(&model), root_(model.CreateDefaultContext()) { - plant_context_ = &model.plant().GetMyMutableContextFromRoot(root_.get()); - scene_graph_context_ = &model.scene_graph().GetMyContextFromRoot(*root_); -} - void ThreadContext::SetPositions(const Eigen::VectorXd& q) { - model_->plant().SetPositions(plant_context_, q); -} - -const QueryObject& ThreadContext::query_object() const { - return model_->scene_graph() - .get_query_output_port() - .Eval>(*scene_graph_context_); + model_->plant().SetPositions(&context_.mutable_plant_context(), q); } const RigidTransformd& ThreadContext::EvalBodyPose(BodyIndex body) const { - return model_->plant().EvalBodyPoseInWorld(*plant_context_, + return model_->plant().EvalBodyPoseInWorld(context_.plant_context(), model_->plant().get_body(body)); } @@ -861,11 +841,6 @@ ContextPool::Lease ContextPool::Acquire(int count) const { return Lease(this, std::move(contexts), std::move(slots)); } -int ContextPool::size() const { - std::lock_guard guard(mutex_); - return static_cast(slots_.size()); -} - void ContextPool::Release(const std::vector& slots) const { std::lock_guard guard(mutex_); for (const int slot : slots) in_use_[slot] = false; @@ -1112,22 +1087,16 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { // Report what the budget left uncovered as a non-definite finding at the // earliest uncovered time (truncate in parameter // order, report the remainder). - Finding finding; - finding.time = sink.pending_time(); - finding.q = sink.pending_q(); - finding.pair = pairs[sink.pending_pair()].id; - finding.motion_bound = 0.0; - finding.definite = false; + const PairRecord& pending_pair = pairs[sink.pending_pair()]; + const Eigen::VectorXd q = sink.pending_q(); Eigen::Vector3d nearest_a; Eigen::Vector3d nearest_b; - lease[0].SetPositions(finding.q); - finding.distance = input.oracle->SignedDistance(lease[0].query_object(), - pairs[sink.pending_pair()], - &nearest_a, &nearest_b); - finding.nearest_a_W = nearest_a; - finding.nearest_b_W = nearest_b; + lease[0].SetPositions(q); + const double distance = input.oracle->SignedDistance( + lease[0].query_object(), pending_pair, &nearest_a, &nearest_b); ++stats.narrowphase_queries; - findings.push_back(std::move(finding)); + findings.push_back(MakeFinding(sink.pending_time(), q, pending_pair.id, + distance, 0.0, false, nearest_a, nearest_b)); } std::stable_sort(findings.begin(), findings.end(), diff --git a/planning/continuous_collision/certifier_internal.h b/planning/continuous_collision/certifier_internal.h index 4379d90d1d2b..6e4d467e2e03 100644 --- a/planning/continuous_collision/certifier_internal.h +++ b/planning/continuous_collision/certifier_internal.h @@ -18,29 +18,30 @@ #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/certificate.h" #include "drake/planning/continuous_collision/distance_oracle.h" #include "drake/planning/continuous_collision/motion_bound_table.h" #include "drake/planning/continuous_collision/options.h" #include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include "drake/planning/robot_diagram.h" -#include "drake/systems/framework/context.h" namespace drake { namespace planning { namespace continuous_collision { namespace internal { -/* One thread's view of the model: a root diagram context plus the plant and -scene-graph sub-contexts pulled out of it once, so the hot loop pays a single -`SetPositions` per node and no context bookkeeping. */ +/* 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); + 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 @@ -49,7 +50,9 @@ class ThreadContext { void SetPositions(const Eigen::VectorXd& q); /* The scene graph's query object at the configuration last set. */ - const geometry::QueryObject& query_object() const; + 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). */ @@ -58,9 +61,7 @@ class ThreadContext { private: const RobotDiagram* model_{}; - std::unique_ptr> root_; - systems::Context* plant_context_{}; - const systems::Context* scene_graph_context_{}; + CollisionCheckerContext context_; }; /* A checkout pool of ThreadContexts; construction allocates @@ -91,7 +92,6 @@ class ContextPool { Lease& operator=(Lease&& other) noexcept; ~Lease(); - int size() const { return static_cast(contexts_.size()); } ThreadContext& operator[](int i) const { return *contexts_[i]; } private: @@ -110,9 +110,6 @@ class ContextPool { /* Leases exactly `count` contexts, growing the pool if it is exhausted. */ Lease Acquire(int count) const; - /* Number of contexts currently held by the pool (for tests/diagnostics). */ - int size() const; - private: void Release(const std::vector& slots) const; diff --git a/planning/continuous_collision/continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc index 5259aa764aa4..4797dc408c72 100644 --- a/planning/continuous_collision/continuous_collision_checker.cc +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include @@ -14,12 +13,12 @@ #include #include "drake/common/drake_throw.h" -#include "drake/common/unused.h" #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_internal.h" +#include "drake/planning/continuous_collision/shape_class.h" namespace drake { namespace planning { @@ -29,6 +28,9 @@ namespace { using drake::geometry::GeometryId; using drake::multibody::BodyIndex; using drake::planning::RobotDiagram; +using internal::Classify; +using internal::kNumShapeClasses; +using internal::ShapeClass; // --------------------------------------------------------------------------- // Per-pair oracle tolerance τ_p. @@ -57,55 +59,15 @@ using drake::planning::RobotDiagram; // | 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 | 3e-15 | 6e-15 | 3e-6 | 5e-15 | 4e-5 | 3e-6 | 6e-15 | +// | Sphere | 4e-15 | 6e-15 | 3e-6 | 5e-15 | 4e-5 | 3e-6 | 6e-15 | // clang-format on -/* The closed set of shape classes the τ_p table knows. */ -enum class ShapeClass { - kSphere = 0, - kBox = 1, - kCapsule = 2, - kCylinder = 3, - kEllipsoid = 4, - kConvex = 5, - kMesh = 6, - kHalfSpace = 7, - kOther = 8, -}; -constexpr int kNumShapeClasses = 9; - /* 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; -ShapeClass Classify(const drake::geometry::Shape& shape) { - return shape.Visit([](const auto& s) { - using S = std::decay_t; - drake::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::kOther; - } - }); -} - using AccuracyTable = std::array, kNumShapeClasses>; @@ -119,7 +81,7 @@ const AccuracyTable& DocumentedAccuracyTable() { }; using S = ShapeClass; set(S::kSphere, S::kSphere, 6e-15); - set(S::kSphere, S::kBox, 3e-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); @@ -184,13 +146,11 @@ std::vector ComputeTauTable(const RobotDiagram& model, // PaddingSpec mirrors drake::planning::CollisionChecker: a pair's effective // threshold is m_p = margin + padding(p), where padding comes from the dense // per-body-pair matrix when one is supplied and otherwise from the {env, self} -// scalars. A pair is self iff both bodies are non-anchored and env otherwise, -// where a body is anchored iff KinematicsEngine::CoordinatesAffectingPair( -// world, body) is empty, which covers the world body and everything welded to -// it, directly or transitively. The rule is pure topology, so padding never -// depends on which trajectory is being checked; in particular the -// constant-coordinate carve-out, which can make a moving body behave as if -// welded for one trajectory, does not enter here. +// scalars. A pair is self iff both bodies are non-anchored and env otherwise. +// The rule is pure topology, so padding never depends on which trajectory is +// being checked; in particular the constant-coordinate carve-out, which can +// make a moving body behave as if welded for one trajectory, does not enter +// here. std::vector ComputePaddingTable(const KinematicsEngine& engine, const std::vector& pairs, @@ -210,10 +170,7 @@ std::vector ComputePaddingTable(const KinematicsEngine& engine, std::vector anchored(num_bodies, false); for (int b = 0; b < num_bodies; ++b) { - anchored[b] = - engine - .CoordinatesAffectingPair(plant.world_body().index(), BodyIndex(b)) - .empty(); + anchored[b] = plant.IsAnchored(plant.get_body(BodyIndex(b))); } std::vector result(pairs.size(), 0.0); @@ -245,20 +202,18 @@ std::vector ComputePaddingTable(const KinematicsEngine& engine, // --------------------------------------------------------------------------- internal::PrefilterTable ComputePrefilterTable( - const RobotDiagram& model, const KinematicsEngine& engine, - const std::vector& pairs) { - const drake::geometry::SceneGraphInspector& inspector = - model.scene_graph().model_inspector(); + const 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; - const auto slot = [&](GeometryId id, BodyIndex body) { - // HalfSpace has no bounding sphere, so such pairs skip the prefilter - // entirely and go straight to the analytic oracle route, which is cheap - // anyway. - if (Classify(inspector.GetShape(id)) == ShapeClass::kHalfSpace) return -1; + // 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 BoundingSphere& sphere = engine.geometry_sphere(id); @@ -270,8 +225,10 @@ internal::PrefilterTable ComputePrefilterTable( }; for (int p = 0; p < static_cast(pairs.size()); ++p) { - table.slot_a[p] = slot(pairs[p].id.a, pairs[p].id.body_a); - table.slot_b[p] = slot(pairs[p].id.b, pairs[p].id.body_b); + table.slot_a[p] = slot(pairs[p].id.a, pairs[p].id.body_a, + pairs[p].route == DistanceRoute::kHalfSpaceA); + table.slot_b[p] = slot(pairs[p].id.b, pairs[p].id.body_b, + pairs[p].route == DistanceRoute::kHalfSpaceB); } return table; } @@ -332,7 +289,7 @@ class ContinuousCollisionChecker::Impl { padding_(ComputePaddingTable(engine_, pairs_, params.padding)), tau_base_(ComputeTauTable(*model_, pairs_, /* query_tolerance = */ 0.0)), - prefilter_(ComputePrefilterTable(*model_, engine_, pairs_)), + prefilter_(ComputePrefilterTable(engine_, pairs_)), pool_(*model_, std::max(1, default_options_.parallelism.num_threads())) { pair_ids_.reserve(pairs_.size()); diff --git a/planning/continuous_collision/distance_oracle.cc b/planning/continuous_collision/distance_oracle.cc index ed6a60c5862f..9576141077b4 100644 --- a/planning/continuous_collision/distance_oracle.cc +++ b/planning/continuous_collision/distance_oracle.cc @@ -4,10 +4,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -15,65 +13,25 @@ #include #include "drake/common/drake_throw.h" -#include "drake/common/unused.h" #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/shape_class.h" namespace drake { namespace planning { namespace continuous_collision { namespace { -using drake::unused; using drake::geometry::GeometryId; using drake::geometry::QueryObject; using drake::geometry::SceneGraphInspector; using drake::math::RigidTransformd; - -/* The closed set of shape classes the oracle recognizes. Anything outside it -is `kUnsupported` and is refused by the capability probe, mirroring the -throw-on-unknown-shape rule ComputeBoundingSphere() uses. */ -enum class ShapeClass { - kSphere, - kBox, - kCapsule, - kCylinder, - kEllipsoid, - kConvex, - kMesh, - kHalfSpace, - kUnsupported, -}; - -ShapeClass Classify(const drake::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; - } - }); -} +using internal::Classify; +using internal::ShapeClass; /* Everything the analytic halfspace fallback needs about the *non*-halfspace partner, extracted once by the probe. Only the fields relevant to `klass` are @@ -246,10 +204,8 @@ std::string ClassName(ShapeClass klass) { /* "geometry_name (ShapeType)", for error messages and the report. */ std::string Describe(const SceneGraphInspector& inspector, GeometryId id) { - std::ostringstream out; - out << inspector.GetName(id) << " (" << inspector.GetShape(id).type_name() - << ")"; - return out.str(); + return fmt::format("{} ({})", inspector.GetName(id), + inspector.GetShape(id).type_name()); } /* One row of the probe report: a distinct unordered shape-type combination @@ -407,29 +363,22 @@ DistanceOracle::DistanceOracle(const RobotDiagram& model, } // --- Render the report. -------------------------------------------------- - std::ostringstream report; - report << "DistanceOracle capability probe: " << pairs_.size() - << " unfiltered pair(s), " << combos.size() - << " distinct shape-type combination(s), tolerance tau = " - << tolerance_ << " m.\n"; + std::string report = fmt::format( + "DistanceOracle capability probe: {} unfiltered pair(s), {} distinct " + "shape-type combination(s), tolerance tau = {} m.\n", + pairs_.size(), combos.size(), tolerance_); for (const auto& [combo, row] : combos) { - report << " " << ClassName(combo.first) << "-" << ClassName(combo.second) - << ": "; - switch (row.route) { - case DistanceRoute::kNative: - report << "native (ComputeSignedDistancePairClosestPoints, probed ok)"; - break; - case DistanceRoute::kHalfSpaceA: - case DistanceRoute::kHalfSpaceB: - report << "halfspace analytic support-function fallback (exact)"; - break; - } - report << "; " << row.pair_count << " pair(s)\n"; + const char* const route = + (row.route == DistanceRoute::kNative) + ? "native (ComputeSignedDistancePairClosestPoints, probed ok)" + : "halfspace analytic support-function fallback (exact)"; + report += fmt::format(" {}-{}: {}; {} pair(s)\n", ClassName(combo.first), + ClassName(combo.second), route, row.pair_count); } for (const std::string& name : mesh_names) { - report << " Mesh " << name << ": certified as its convex hull\n"; + report += fmt::format(" Mesh {}: certified as its convex hull\n", name); } - impl->report = report.str(); + impl->report = std::move(report); impl_ = std::move(impl); } diff --git a/planning/continuous_collision/motion_bound_table.cc b/planning/continuous_collision/motion_bound_table.cc index 0d69a2514966..da05537e27f0 100644 --- a/planning/continuous_collision/motion_bound_table.cc +++ b/planning/continuous_collision/motion_bound_table.cc @@ -23,13 +23,13 @@ #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/shape_class.h" namespace drake { namespace planning { namespace continuous_collision { using drake::geometry::GeometryId; -using drake::geometry::HalfSpace; using drake::geometry::Role; using drake::geometry::Shape; using drake::math::RigidTransform; @@ -39,16 +39,7 @@ using drake::multibody::JointIndex; using drake::multibody::MultibodyPlant; using drake::multibody::ScrewJoint; using drake::multibody::WeldJoint; - -namespace { - -constexpr double kTwoPi = 6.283185307179586476925286766559; - -bool IsHalfSpace(const Shape& shape) { - return dynamic_cast(&shape) != nullptr; -} - -} // namespace +using internal::IsHalfSpace; MotionBoundTable::MotionBoundTable(std::vector row_start, std::vector coord, @@ -354,8 +345,6 @@ void KinematicsEngine::BuildGeometry() { const MultibodyPlant& plant = *plant_; const auto& inspector = model_->scene_graph().model_inspector(); - body_spheres_.assign(num_bodies_, {}); - body_sphere_geoms_.assign(num_bodies_, {}); body_radius_.assign(num_bodies_, 0.0); body_has_halfspace_.assign(num_bodies_, false); body_halfspace_name_.assign(num_bodies_, std::string{}); @@ -393,8 +382,6 @@ void KinematicsEngine::BuildGeometry() { // triangle inequality on the sphere that contains it. body_radius_[b] = std::max(body_radius_[b], sphere.center_L.norm() + sphere.radius); - body_sphere_geoms_[b].push_back(gid); - body_spheres_[b].push_back(sphere); geometry_spheres_.emplace(gid, sphere); } } @@ -567,7 +554,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( 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) / kTwoPi; + 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) { @@ -847,7 +834,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( lam_tilde = reach(); break; case CoordRule::kScrewCoord: - lam_tilde = reach() + std::abs(rec.screw_pitch) / kTwoPi; + lam_tilde = reach() + std::abs(rec.screw_pitch) / (2 * M_PI); break; case CoordRule::kQuaternion: { const double m = quat_min_norm[k]; @@ -882,7 +869,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( lam = (c == ps + 2) ? reach() : 1.0; break; case JointKind::kScrew: - lam = reach() + std::abs(rec.screw_pitch) / kTwoPi; + lam = reach() + std::abs(rec.screw_pitch) / (2 * M_PI); break; case JointKind::kWeld: case JointKind::kUnsupported: @@ -904,18 +891,6 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( std::move(lambda), std::move(carveout_slack)); } -const std::vector& KinematicsEngine::body_spheres( - BodyIndex body) const { - DRAKE_THROW_UNLESS(body.is_valid() && body < num_bodies_); - return body_spheres_[body]; -} - -const std::vector& KinematicsEngine::body_sphere_geometries( - BodyIndex body) const { - DRAKE_THROW_UNLESS(body.is_valid() && body < num_bodies_); - return body_sphere_geoms_[body]; -} - const BoundingSphere& KinematicsEngine::geometry_sphere(GeometryId id) const { auto it = geometry_spheres_.find(id); if (it == geometry_spheres_.end()) { diff --git a/planning/continuous_collision/motion_bound_table.h b/planning/continuous_collision/motion_bound_table.h index f402153c1f63..2f9ebffc3df1 100644 --- a/planning/continuous_collision/motion_bound_table.h +++ b/planning/continuous_collision/motion_bound_table.h @@ -91,9 +91,6 @@ class MotionBoundTable { @throws std::exception if pair_index is outside [0, num_pairs()). */ std::vector> GetEntries(int pair_index) const; - /** Total number of (coordinate, λ) entries over all pairs. */ - int num_entries() const { return static_cast(coord_.size()); } - private: std::vector row_start_{0}; std::vector coord_; @@ -109,7 +106,8 @@ concurrent ComputeMotionBoundTable() calls are safe. Typical use by the certifier: - once, at checker construction: KinematicsEngine engine(model); - engine.body_spheres(b) for the prefilter; + 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). @ingroup planning_collision_checker */ @@ -177,16 +175,6 @@ class KinematicsEngine { const std::vector& constant_coordinates, const std::vector& pairs) const; - /** Bounding spheres (body frame) of every proximity geometry of `body`, - used by the reach chain start and by the certifier's sphere prefilter. - HalfSpace geometries have no bounding sphere and are omitted. */ - const std::vector& body_spheres( - multibody::BodyIndex body) const; - - /** The geometry ids matching body_spheres(body), element for element. */ - const std::vector& body_sphere_geometries( - multibody::BodyIndex body) 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). */ @@ -303,8 +291,6 @@ class KinematicsEngine { /* Position coordinate -> ordinal of the owning joint. */ std::vector coord_joint_; - std::vector> body_spheres_; - std::vector> body_sphere_geoms_; std::vector body_radius_; std::vector body_has_halfspace_; std::vector body_halfspace_name_; diff --git a/planning/continuous_collision/piecewise_bezier_path.cc b/planning/continuous_collision/piecewise_bezier_path.cc index ee8b47b06c04..ebcf39b20c21 100644 --- a/planning/continuous_collision/piecewise_bezier_path.cc +++ b/planning/continuous_collision/piecewise_bezier_path.cc @@ -18,6 +18,7 @@ #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" namespace drake { namespace planning { @@ -25,14 +26,13 @@ namespace continuous_collision { 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; -constexpr double kTwoPi = 6.2831853071795864769252867665590; - /* 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. */ @@ -43,20 +43,6 @@ 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; -/* Pascal's triangle up to row `m`; table(j, a) = C(j, a) for a <= j, 0 -otherwise. Exact in double for the degrees this file accepts (the default cap -is 10; C(10, 5) = 252). */ -Eigen::MatrixXd BinomialTable(int m) { - Eigen::MatrixXd table = Eigen::MatrixXd::Zero(m + 1, m + 1); - for (int j = 0; j <= m; ++j) { - table(j, 0) = 1.0; - for (int a = 1; a <= j; ++a) { - table(j, a) = table(j - 1, a - 1) + (a <= j - 1 ? table(j - 1, a) : 0.0); - } - } - return table; -} - /* Converts one BsplineTrajectory into Bézier segments (trajectory normalization, item 4). @@ -190,7 +176,6 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, "(source segment index {}) has non-positive duration {}.", k, source_index, duration)); } - const Eigen::MatrixXd binomial = BinomialTable(m); BezierSegment segment; segment.t_start = t_start; segment.t_end = t_end; @@ -209,7 +194,9 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, for (int j = 0; j <= m; ++j) { double sum = 0.0; for (int a = 0; a <= j; ++a) { - sum += (binomial(j, a) / binomial(m, a)) * alpha[a]; + sum += (static_cast(BinomialCoefficient(j, a)) / + BinomialCoefficient(m, a)) * + alpha[a]; } segment.control_points(r, j) = sum; } @@ -345,7 +332,7 @@ void ValidateSegments(int num_positions, const Options& options, const double raw_gap = next(c, 0) - previous(c, previous.cols() - 1); double gap = raw_gap; if (is_continuous_revolute[c]) { - gap -= kTwoPi * std::round(gap / kTwoPi); + gap -= 2 * M_PI * std::round(gap / (2 * M_PI)); } if (std::abs(gap) > options.continuity_tolerance) { const std::string modulo = is_continuous_revolute[c] @@ -464,27 +451,20 @@ Eigen::VectorXd PiecewiseBezierPath::Value(double t) const { 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, matching - // drake::trajectories::PiecewiseTrajectory::get_segment_index(). The choice - // is observable only when a junction carries a legitimate 2πk offset in a - // continuous-revolute coordinate, where the two sides are different - // representatives of the same configuration. - int low = 0; - int high = static_cast(segments_.size()) - 1; - while (low < high) { - const int mid = low + (high - low + 1) / 2; - if (segments_[mid].t_start <= clamped) { - low = mid; - } else { - high = mid - 1; - } - } - const BezierSegment& segment = segments_[low]; + // 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(low, std::clamp(s, 0.0, 1.0)); + return EvaluateSegment(index, std::clamp(s, 0.0, 1.0)); } Eigen::VectorXd PiecewiseBezierPath::EvaluateSegment(int segment_index, diff --git a/planning/continuous_collision/shape_class.h b/planning/continuous_collision/shape_class.h new file mode 100644 index 000000000000..9e4da12f294b --- /dev/null +++ b/planning/continuous_collision/shape_class.h @@ -0,0 +1,70 @@ +#pragma once + +#include + +#include "drake/common/unused.h" +#include "drake/geometry/shape_specification.h" + +namespace drake { +namespace planning { +namespace continuous_collision { +namespace internal { + +/* 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; +} + +} // namespace internal +} // namespace continuous_collision +} // namespace planning +} // namespace drake diff --git a/planning/continuous_collision/vpolytope_ingestion.cc b/planning/continuous_collision/vpolytope_ingestion.cc index 8df4ea2061eb..42e266fff1e3 100644 --- a/planning/continuous_collision/vpolytope_ingestion.cc +++ b/planning/continuous_collision/vpolytope_ingestion.cc @@ -29,19 +29,10 @@ GeometryId AddVPolytopeObstacle(MultibodyPlant* plant, const RigidTransformd& X_WG, const std::string& name) { DRAKE_THROW_UNLESS(plant != nullptr); - if (plant->is_finalized()) { - throw std::runtime_error(fmt::format( - "AddVPolytopeObstacle(): cannot add obstacle '{}' because the plant " - "is already finalized; register V-polytope obstacles before calling " - "MultibodyPlant::Finalize().", - name)); - } - if (vpoly.ambient_dimension() != 3) { - throw std::runtime_error(fmt::format( - "AddVPolytopeObstacle(): obstacle '{}' has ambient dimension {}; only " - "3-dimensional V-polytopes can be registered as geometry.", - name, vpoly.ambient_dimension())); - } + // A non-3D V-polytope is refused by VPolytope::ToShapeConvex() below, and a + // finalized plant by MultibodyPlant::RegisterCollisionGeometry(); neither + // needs a check here. An empty vertex set reaches the proximity engine + // undetected, so it does. if (vpoly.vertices().cols() == 0) { throw std::runtime_error(fmt::format( "AddVPolytopeObstacle(): obstacle '{}' has an empty vertex set.", From 0e6bf8f0a99f93eb304d7eef7b8acc02899261bf Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Fri, 28 Aug 2026 11:57:57 -0400 Subject: [PATCH 18/22] [planning] continuous_collision: consolidate and trim tests --- planning/continuous_collision/BUILD.bazel | 123 +- .../continuous_collision/test/api_test.cc | 524 ++++---- .../test/bounding_sphere_test.cc | 274 ++--- .../test/certificate_test.cc | 268 ++--- .../test/certifier_test.cc | 771 +++--------- .../test/concurrency_test.cc | 92 +- .../test/concurrency_test_utilities.h | 386 ------ .../test/concurrency_timing_test.cc | 150 --- .../test/distance_oracle_test.cc | 382 +++--- .../test/motion_bound_test.cc | 1062 +++++++---------- .../test/piecewise_bezier_path_test.cc | 421 +++---- .../test/soundness_fuzz_test.cc | 128 +- .../test/test_utilities.h | 563 +++++++++ .../test/thin_obstacle_test.cc | 184 +-- 14 files changed, 1996 insertions(+), 3332 deletions(-) delete mode 100644 planning/continuous_collision/test/concurrency_test_utilities.h delete mode 100644 planning/continuous_collision/test/concurrency_timing_test.cc create mode 100644 planning/continuous_collision/test/test_utilities.h diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index 9f3189224652..62818568084e 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -216,13 +216,37 @@ drake_cc_library( # === test/ === +# The helpers the tests share: seeded random primitives and surface samplers, +# the throw-message probe, the checker factory, 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", + "//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 = [ ":piecewise_bezier_path", "//common:copyable_unique_ptr", - "//common:polynomial", + "//common/test_utilities:expect_throws_message", + "//common/test_utilities:limit_malloc", "//common/trajectories:bezier_curve", "//common/trajectories:bspline_trajectory", "//common/trajectories:composite_trajectory", @@ -237,13 +261,11 @@ drake_cc_googletest( timeout = "moderate", deps = [ ":motion_bound_table", + ":test_utilities", "//geometry:geometry_roles", "//geometry:scene_graph_inspector", - "//geometry:shape_specification", - "//math:geometric_transform", - "//multibody/plant", "//multibody/tree", - "//planning:robot_diagram_builder", + "@fmt", ], ) @@ -252,8 +274,10 @@ drake_cc_googletest( name = "bounding_sphere_test", deps = [ ":bounding_sphere", + ":test_utilities", "//common:essential", "//common:memory_file", + "//common/test_utilities:expect_throws_message", "//geometry:in_memory_mesh", "//geometry:shape_specification", "//geometry/proximity:polygon_surface_mesh", @@ -270,6 +294,7 @@ drake_cc_googletest( ":vpolytope_ingestion", "//common:find_resource", "//common:memory_file", + "//common/test_utilities:expect_throws_message", "//geometry:geometry_instance", "//geometry:in_memory_mesh", "//geometry:proximity_properties", @@ -285,24 +310,11 @@ drake_cc_googletest( ], ) -# Certifier semantics, including retiming invariance, on a focused, -# hand-built corpus. +# Certifier semantics, including retiming invariance and the standing soundness +# guard, on a focused, hand-built corpus. drake_cc_googletest( name = "certifier_test", - # Eight caller threads, each asking for Parallelism(2). - num_threads = 8, - deps = [ - ":continuous_collision_checker", - "//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", - ], + deps = [":test_utilities"], ) # The randomized soundness fuzz: random worlds x random trajectories, @@ -354,17 +366,8 @@ drake_cc_googletest( drake_cc_googletest( name = "thin_obstacle_test", deps = [ - ":continuous_collision_checker", - "//common:parallelism", - "//common/trajectories:bezier_curve", - "//geometry:scene_graph", - "//geometry:shape_specification", - "//math:geometric_transform", - "//multibody/plant", - "//multibody/tree", + ":test_utilities", "//planning:collision_checker_params", - "//planning:robot_diagram", - "//planning:robot_diagram_builder", "//planning:scene_graph_collision_checker", ], ) @@ -372,36 +375,7 @@ drake_cc_googletest( # Certificate audit trail + mutation test. drake_cc_googletest( name = "certificate_test", - deps = [ - ":continuous_collision_checker", - "//common:parallelism", - "//common/trajectories:bezier_curve", - "//geometry:shape_specification", - "//math:geometric_transform", - "//multibody/plant", - "//multibody/tree", - "//planning:robot_diagram", - "//planning:robot_diagram_builder", - ], -) - -# The corpus and the deep workload both concurrency targets run on. -drake_cc_library( - name = "concurrency_test_utilities", - testonly = 1, - hdrs = ["test/concurrency_test_utilities.h"], - deps = [ - ":continuous_collision_checker", - "//common:parallelism", - "//common/trajectories:bezier_curve", - "//geometry:shape_specification", - "//math:geometric_transform", - "//multibody/plant", - "//multibody/tree", - "//planning:robot_diagram", - "//planning:robot_diagram_builder", - "@googletest//:gtest", - ], + deps = [":test_utilities"], ) # Concurrency determinism. Running with many threads is the point of @@ -411,23 +385,7 @@ drake_cc_googletest( name = "concurrency_test", num_threads = 16, deps = [ - ":concurrency_test_utilities", - "//common:parallelism", - ], -) - -# The two per-call scaling claims. Split from concurrency_test because -# they are wall-clock claims: Valgrind serializes threads, which inverts -# "parallel is faster than serial" and fails the test for a reason that has -# nothing to do with the driver. //tools:unoptimized covers every flavor that -# does so: the sanitizers, dbg, kcov, and all of the Valgrind tools (memcheck -# plus drd and helgrind, which serialize the same way). -drake_cc_googletest( - name = "concurrency_timing_test", - opt_out_conditions = ["//tools:unoptimized"], - num_threads = 16, - deps = [ - ":concurrency_test_utilities", + ":test_utilities", "//common:parallelism", ], ) @@ -436,23 +394,18 @@ drake_cc_googletest( drake_cc_googletest( name = "api_test", deps = [ - ":continuous_collision_checker", + ":test_utilities", "//common:copyable_unique_ptr", - "//common:parallelism", - "//common/trajectories:bezier_curve", + "//common/test_utilities:expect_throws_message", "//common/trajectories:composite_trajectory", "//common/trajectories:piecewise_polynomial", "//common/trajectories:piecewise_quaternion", "//common/trajectories:trajectory", "//geometry:geometry_instance", "//geometry:proximity_properties", - "//geometry:shape_specification", - "//math:geometric_transform", "//multibody/fem:deformable_body_config", "//multibody/plant", "//multibody/tree", - "//planning:robot_diagram", - "//planning:robot_diagram_builder", ], ) diff --git a/planning/continuous_collision/test/api_test.cc b/planning/continuous_collision/test/api_test.cc index 97b857d5d045..1a346aa165c6 100644 --- a/planning/continuous_collision/test/api_test.cc +++ b/planning/continuous_collision/test/api_test.cc @@ -5,100 +5,67 @@ // "error". The pydrake surface is covered separately, in // bindings/pydrake/planning/test/continuous_collision_test.py. +#include +#include #include #include #include #include #include +#include #include #include "drake/common/copyable_unique_ptr.h" -#include "drake/common/parallelism.h" -#include "drake/common/trajectories/bezier_curve.h" +#include "drake/common/test_utilities/expect_throws_message.h" #include "drake/common/trajectories/composite_trajectory.h" #include "drake/common/trajectories/piecewise_polynomial.h" #include "drake/common/trajectories/piecewise_quaternion.h" #include "drake/common/trajectories/trajectory.h" #include "drake/geometry/geometry_instance.h" #include "drake/geometry/proximity_properties.h" -#include "drake/geometry/shape_specification.h" -#include "drake/math/rigid_transform.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/joint.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/robot_diagram.h" -#include "drake/planning/robot_diagram_builder.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" namespace drake { namespace planning { namespace continuous_collision { namespace { -using drake::Parallelism; -using drake::geometry::Box; using drake::geometry::GeometryInstance; -using drake::geometry::HalfSpace; using drake::geometry::ProximityProperties; -using drake::geometry::Sphere; -using drake::math::RigidTransformd; -using drake::multibody::CoulombFriction; using drake::multibody::Joint; -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::CompositeTrajectory; using drake::trajectories::PiecewisePolynomial; using drake::trajectories::PiecewiseQuaternionSlerp; using drake::trajectories::Trajectory; using Eigen::Vector3d; using Eigen::VectorXd; - -CoulombFriction Friction() { - return CoulombFriction(1.0, 1.0); -} - -SpatialInertia Inertia() { - return SpatialInertia::SolidSphereWithMass(1.0, 0.05); -} - -// Runs `call`, requires it to throw, and returns the message so the caller can -// assert on the identifiers it must contain. Reports the actual message on -// every failure path, so a message regression is diagnosable from the log. -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 {}; -} - -void ExpectContains(const std::string& haystack, const std::string& needle) { - EXPECT_NE(haystack.find(needle), std::string::npos) - << "the message did not mention '" << needle << "'.\nMessage was:\n" - << haystack; -} - -std::unique_ptr MakeChecker( - std::shared_ptr> model) { - ContinuousCollisionChecker::Params params; - params.model = std::move(model); - params.default_options.parallelism = Parallelism::None(); - return std::make_unique(params); +using test::BezierCurve; +using test::Box; +using test::Friction; +using test::HalfSpace; +using test::Inertia; +using test::MakeChecker; +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 @@ -175,59 +142,64 @@ VectorXd FloatingQ(const Vector3d& p, double elbow) { GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { std::shared_ptr> model = MakeFloatingBaseWorld(); - const auto checker = MakeChecker(model); + const auto checker = MakeChecker(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. + // 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 - const std::string message = ThrowMessage([&]() { - checker->CheckTrajectory(BezierCurve(0.0, 1.0, points)); - }); - ExpectContains(message, joint_name); - ExpectContains(message, "quaternion_floating"); - // The message must also point at the way out. - ExpectContains(message, "constant-coordinate carve-out"); + 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); - const std::string translate_message = ThrowMessage([&]() { - checker->CheckTrajectory(BezierCurve(0.0, 1.0, translated)); - }); - ExpectContains(translate_message, joint_name); - ExpectContains(translate_message, "coordinate 4"); + 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. Checked end to end: a verdict and a certificate, not - // just "does not throw". +// 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 and owes no residual at all, while a +// base held only to within continuity_tolerance is still carved but owes +// lambda-tilde times its range, charged to MotionBoundTable::carveout_slack(). +// That residual has to survive the static-pair shortcut, which never evaluates +// a per-node Delta, and the certificate replay, which recomputes Delta from +// scratch and would reject a record whose bound came out smaller. +void CheckConstantFloatingBase(double wobble) { + SCOPED_TRACE("wobble = " + std::to_string(wobble)); std::shared_ptr> model = MakeFloatingBaseWorld(); - const auto checker = MakeChecker(model); - - Options options; - options.parallelism = Parallelism::None(); + Options options = SerialOptions(); options.emit_certificate = true; + ASSERT_LE(wobble, options.continuity_tolerance); + const auto checker = MakeChecker(model, options); 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; const BezierCurve trajectory(0.0, 1.0, points); - const PiecewiseBezierPath path = checker->Normalize(trajectory, options); + const PiecewiseBezierPath path = checker.Normalize(trajectory, options); const std::vector& constant = path.constant_coordinates(); ASSERT_EQ(constant.size(), 8u); for (int i = 0; i < 7; ++i) { @@ -235,86 +207,33 @@ GTEST_TEST(ApiTest, ConstantQuaternionBaseIsAcceptedEndToEnd) { << "base coordinate " << i << " should have been flagged constant"; } EXPECT_FALSE(constant[7]); - - const CertificationResult result = - checker->CheckTrajectory(trajectory, options); - EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); - ASSERT_TRUE(result.certificate.has_value()); - // Every base control point here is bit-identical, so the carve-out is exact - // and owes no residual at all. - const MotionBoundTable table = checker->ComputeMotionBounds(path); - for (int p = 0; p < table.num_pairs(); ++p) { - EXPECT_EQ(table.carveout_slack(p), 0.0) << "pair " << p; - } - EXPECT_TRUE(VerifyCertificate(*checker, path, *result.certificate)); -} - -GTEST_TEST(ApiTest, ToleranceConstantQuaternionBaseChargesItsResidualEndToEnd) { - // The carve-out flags a coordinate constant on a tolerance, so a base held - // only to within continuity_tolerance is carved even though it still moves. - // Its residual is charged to MotionBoundTable::carveout_slack(), and that has - // to survive the static-pair shortcut, which never evaluates a per-node Δ, - // and the certificate replay, which recomputes Δ from scratch and would - // reject a record whose bound came out smaller than the certifier's. - std::shared_ptr> model = MakeFloatingBaseWorld(); - const auto checker = MakeChecker(model); - - Options options; - options.parallelism = Parallelism::None(); - options.emit_certificate = true; - - 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); - } - // A sub-tolerance wobble in the base's y position: still "constant" to the - // curve module, but no longer exactly so. - constexpr double kWobble = 6e-8; - ASSERT_LE(kWobble, options.continuity_tolerance); - points(5, 1) += kWobble; - points(7, 1) = 0.35; // ... and the elbow still moves. - points(7, 2) = 0.70; - const BezierCurve trajectory(0.0, 1.0, points); - - const PiecewiseBezierPath path = checker->Normalize(trajectory, options); - const std::vector& constant = path.constant_coordinates(); - ASSERT_EQ(constant.size(), 8u); - for (int i = 0; i < 7; ++i) { - EXPECT_TRUE(constant[i]) << "base coordinate " << i; - } - EXPECT_FALSE(constant[7]); - // The control-point range of the wobbled coordinate: `kWobble` up to the + // The control-point range of the wobbled coordinate: `wobble` up to the // rounding of adding it to -0.10 and subtracting again. const double range = path.global_upper_bound()[5] - path.global_lower_bound()[5]; - EXPECT_GT(range, 0.0); - EXPECT_NEAR(range, kWobble, 1e-9 * kWobble); + EXPECT_NEAR(range, wobble, 1e-9 * std::max(wobble, 1e-12)); - const MotionBoundTable table = checker->ComputeMotionBounds(path); - bool any_slack = false; + const MotionBoundTable table = checker.ComputeMotionBounds(path); bool any_static_with_slack = false; for (int p = 0; p < table.num_pairs(); ++p) { - if (table.carveout_slack(p) > 0.0) { - any_slack = true; + if (wobble == 0.0) { + EXPECT_EQ(table.carveout_slack(p), 0.0) << "pair " << p; + } else if (table.carveout_slack(p) > 0.0) { if (table.pair_is_static(p)) any_static_with_slack = true; - // λ̃ = 1 for a floating base's translation coordinates, and only that one - // coordinate has a width, so the residual is exactly that width. + // lambda-tilde = 1 for a floating base's translation coordinates, and + // only that one coordinate has a width, so the residual is exactly it. EXPECT_DOUBLE_EQ(table.carveout_slack(p), range) << "pair " << p; } } - EXPECT_TRUE(any_slack); - EXPECT_TRUE(any_static_with_slack) + EXPECT_EQ(any_static_with_slack, wobble > 0.0) << "the base-vs-post pair depends only on the carved base coordinates, " "so it is static and must still owe the residual"; const CertificationResult result = - checker->CheckTrajectory(trajectory, options); + checker.CheckTrajectory(trajectory, options); EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); ASSERT_TRUE(result.certificate.has_value()); - // The replay recomputes Δ through MotionBound(), so it charges the residual - // too: a certificate emitted against the inflated bound verifies, and one - // emitted against a smaller bound would not. - EXPECT_TRUE(VerifyCertificate(*checker, path, *result.certificate)); + EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); for (const CertificateRecord& record : result.certificate->records) { if (table.pair_is_static(record.pair_index)) { EXPECT_GE(record.motion_bound, table.carveout_slack(record.pair_index)); @@ -322,14 +241,23 @@ GTEST_TEST(ApiTest, ToleranceConstantQuaternionBaseChargesItsResidualEndToEnd) { } } +GTEST_TEST(ApiTest, ExactlyConstantQuaternionBaseIsAcceptedEndToEnd) { + CheckConstantFloatingBase(0.0); +} + +GTEST_TEST(ApiTest, ToleranceConstantQuaternionBaseChargesItsResidual) { + CheckConstantFloatingBase(6e-8); +} + // --------------------------------------------------------------------------- // 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 λ exists for that pair. This must be refused - // when the checker is built, not discovered mid-certification. + // 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()); @@ -344,20 +272,18 @@ GTEST_TEST(ApiTest, RotatingHalfSpaceThrowsAtConstruction) { "post_geom", Friction()); std::shared_ptr> model = builder.Build(); - const std::string message = ThrowMessage([&]() { - MakeChecker(model); - }); - ExpectContains(message, "blade_halfspace"); - ExpectContains(message, "post_geom"); - ExpectContains(message, "spin"); - // ... and it must say what to do about it. - ExpectContains(message, "Box"); + EXPECT_THAT(ThrowMessage([&]() { + MakeChecker(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 λ then bounds the arm's points and signed distance is symmetric. + // 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()); @@ -373,12 +299,12 @@ GTEST_TEST(ApiTest, AnchoredHalfSpaceUnderARotatingArmIsAccepted) { "ground_halfspace", Friction()); std::shared_ptr> model = builder.Build(); - const auto checker = MakeChecker(model); + const auto checker = MakeChecker(model, SerialOptions()); // The probe report is part of the UX: it must say how each pair is routed. - const std::string report = checker->distance_oracle().support_report(); - ExpectContains(report, "HalfSpace"); + EXPECT_THAT(checker.distance_oracle().support_report(), + HasSubstr("HalfSpace")); EXPECT_EQ( - checker->CheckEdge(VectorXd::Constant(1, 0.0), VectorXd::Constant(1, 1.5)) + checker.CheckEdge(VectorXd::Constant(1, 0.0), VectorXd::Constant(1, 1.5)) .verdict, Verdict::kCertifiedFree); } @@ -419,74 +345,60 @@ GTEST_TEST(ApiTest, DeformableGeometryIsRefusedNamingIt) { .size(), 1u); - const std::string message = ThrowMessage([&]() { - MakeChecker(model); - }); - ExpectContains(message, "deformable"); - ExpectContains(message, "squishy_blob"); + EXPECT_THAT(ThrowMessage([&]() { + MakeChecker(model, SerialOptions()); + }), + AllOf(HasSubstr("deformable"), HasSubstr("squishy_blob"))); } // --------------------------------------------------------------------------- -// 3. Dimensions. +// 3. Dimensions, trajectory validation and options. // --------------------------------------------------------------------------- -// The displacement lemma is proved in the separated regime only, so a -// negative effective threshold (margin + padding < 0) is outside what the -// checker can certify and must be rejected, not silently "certified". GTEST_TEST(ApiTest, NegativeEffectiveThresholdIsRejected) { - const auto checker = MakeChecker(MakeArmWorld()); - Options options; - options.parallelism = Parallelism::None(); + // The displacement lemma is proved in the separated regime only, so a + // negative effective threshold (margin + padding < 0) is outside what the + // checker can certify and must be rejected, not silently "certified". + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + Options options = SerialOptions(); options.margin = -0.01; - const VectorXd q0 = VectorXd::Zero(2); - const VectorXd q1 = VectorXd::Constant(2, 0.1); - const std::string message = ThrowMessage([&]() { - checker->CheckEdge(q0, q1, options); - }); - ExpectContains(message, "negative"); - ExpectContains(message, "filter the pair"); + EXPECT_THAT(ThrowMessage([&]() { + checker.CheckEdge(VectorXd::Zero(2), VectorXd::Constant(2, 0.1), + options); + }), + AllOf(HasSubstr("negative"), HasSubstr("filter the pair"))); } GTEST_TEST(ApiTest, DimensionMismatchMessagesNameTheSizes) { std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model); + const auto checker = MakeChecker(model, SerialOptions()); ASSERT_EQ(model->plant().num_positions(), 2); - const std::string path_message = ThrowMessage([&]() { - checker->CheckPath(Eigen::MatrixXd::Zero(3, 4)); - }); - ExpectContains(path_message, "CheckPath"); - ExpectContains(path_message, "3 rows"); - ExpectContains(path_message, "2 generalized positions"); - ExpectContains(path_message, "waypoints are columns"); - - const std::string edge_message = ThrowMessage([&]() { - checker->CheckEdge(VectorXd::Zero(2), VectorXd::Zero(5)); - }); - ExpectContains(edge_message, "CheckEdge"); - ExpectContains(edge_message, "sizes 2 and 5"); - - const std::string trajectory_message = ThrowMessage([&]() { - checker->CheckTrajectory( - BezierCurve(0.0, 1.0, Eigen::MatrixXd::Zero(7, 3))); - }); - ExpectContains(trajectory_message, "7 rows"); - ExpectContains(trajectory_message, "2 generalized positions"); + 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. - const std::string single_message = ThrowMessage([&]() { - checker->CheckPath(Eigen::MatrixXd::Zero(2, 1)); - }); - ExpectContains(single_message, "at least 2 waypoints"); + DRAKE_EXPECT_THROWS_MESSAGE(checker.CheckPath(Eigen::MatrixXd::Zero(2, 1)), + ".*at least 2 waypoints.*"); } -// --------------------------------------------------------------------------- -// 4. Trajectory validation. -// --------------------------------------------------------------------------- - GTEST_TEST(ApiTest, DiscontinuousTrajectoryThrowsNamingTheJunction) { - std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); Eigen::MatrixXd first(2, 2); first << 0.0, 0.3, 0.0, 0.05; @@ -499,18 +411,16 @@ GTEST_TEST(ApiTest, DiscontinuousTrajectoryThrowsNamingTheJunction) { std::make_unique>(1.0, 2.0, second)); const CompositeTrajectory trajectory(std::move(segments)); - const std::string message = ThrowMessage([&]() { - checker->CheckTrajectory(trajectory); - }); - ExpectContains(message, "C0 discontinuity"); - ExpectContains(message, "segments 0 and 1"); - ExpectContains(message, "coordinate 1"); - ExpectContains(message, "continuity_tolerance"); + EXPECT_THAT( + ThrowMessage([&]() { + checker.CheckTrajectory(trajectory); + }), + AllOf(HasSubstr("C0 discontinuity"), HasSubstr("segments 0 and 1"), + HasSubstr("coordinate 1"), HasSubstr("continuity_tolerance"))); } GTEST_TEST(ApiTest, DegreeAboveConversionCapThrows) { - std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); // 13 interpolation nodes => one polynomial segment of degree 12, above the // default max_conversion_degree of 10. @@ -525,134 +435,130 @@ GTEST_TEST(ApiTest, DegreeAboveConversionCapThrows) { const PiecewisePolynomial trajectory = PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, samples); - const std::string message = ThrowMessage([&]() { - checker->CheckTrajectory(trajectory); - }); - ExpectContains(message, "polynomial degree 12"); - ExpectContains(message, "max_conversion_degree"); + EXPECT_THAT(ThrowMessage([&]() { + checker.CheckTrajectory(trajectory); + }), + AllOf(HasSubstr("polynomial degree 12"), + HasSubstr("max_conversion_degree"))); // Raising the cap is the documented escape hatch. - Options options; - options.parallelism = Parallelism::None(); + Options options = SerialOptions(); options.max_conversion_degree = 12; - EXPECT_NO_THROW(checker->Normalize(trajectory, options)); + EXPECT_NO_THROW(checker.Normalize(trajectory, options)); } GTEST_TEST(ApiTest, UnsupportedTrajectoryTypeThrowsNamingTheType) { - std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); const PiecewiseQuaternionSlerp trajectory( std::vector{0.0, 1.0}, std::vector>{ Eigen::Quaternion::Identity(), Eigen::Quaternion(0.7071067811865476, 0.0, 0.0, 0.7071067811865476)}); - const std::string message = ThrowMessage([&]() { - checker->CheckTrajectory(trajectory); - }); - ExpectContains(message, "unsupported trajectory type"); - ExpectContains(message, "PiecewiseQuaternionSlerp"); - // The message must list what is accepted. - ExpectContains(message, "BezierCurve"); - ExpectContains(message, "BsplineTrajectory"); + // The message must name the offending type and list what is accepted. + EXPECT_THAT(ThrowMessage([&]() { + checker.CheckTrajectory(trajectory); + }), + AllOf(HasSubstr("unsupported trajectory type"), + HasSubstr("PiecewiseQuaternionSlerp"), + HasSubstr("BezierCurve"), HasSubstr("BsplineTrajectory"))); } GTEST_TEST(ApiTest, ContinuousRevoluteIndexOutOfRangeThrows) { - std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model); - Options options; - options.parallelism = Parallelism::None(); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + Options options = SerialOptions(); options.continuous_revolute_indices = {0, 5}; Eigen::MatrixXd points(2, 2); points << 0.0, 0.2, 0.0, 0.05; - const std::string message = ThrowMessage([&]() { - checker->CheckTrajectory(BezierCurve(0.0, 1.0, points), options); - }); - ExpectContains(message, "continuous_revolute_indices contains 5,"); - ExpectContains(message, "2 generalized positions"); + EXPECT_THAT(ThrowMessage([&]() { + checker.CheckTrajectory(BezierCurve(0.0, 1.0, points), + options); + }), + AllOf(HasSubstr("continuous_revolute_indices contains 5,"), + HasSubstr("2 generalized positions"))); // A negative index is out of range too. options.continuous_revolute_indices = {-1}; - const std::string negative_message = ThrowMessage([&]() { - checker->CheckTrajectory(BezierCurve(0.0, 1.0, points), options); - }); - ExpectContains(negative_message, "continuous_revolute_indices contains -1,"); + DRAKE_EXPECT_THROWS_MESSAGE( + checker.CheckTrajectory(BezierCurve(0.0, 1.0, points), options), + ".*continuous_revolute_indices contains -1,.*"); } -// --------------------------------------------------------------------------- -// 5. Options and construction. -// --------------------------------------------------------------------------- - GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { - std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); Eigen::MatrixXd points(2, 2); points << 0.0, 0.2, 0.0, 0.05; const BezierCurve trajectory(0.0, 1.0, points); - const auto check_with = [&](const Options& options) { - return ThrowMessage([&]() { - checker->CheckTrajectory(trajectory, options); - }); - }; - - Options options; - options.parallelism = Parallelism::None(); - - Options bad = options; - bad.min_interval = 0.0; - ExpectContains(check_with(bad), "min_interval"); - bad.min_interval = 2.0; - ExpectContains(check_with(bad), "(0, 1]"); - bad = options; - bad.max_reported_findings = 0; - ExpectContains(check_with(bad), "max_reported_findings"); - - bad = options; - bad.query_tolerance = -1.0; - ExpectContains(check_with(bad), "query_tolerance"); - - bad = options; - bad.certificate_slack = -1e-9; - ExpectContains(check_with(bad), "certificate_slack"); - - bad = options; - bad.max_nodes = 0; - ExpectContains(check_with(bad), "max_nodes"); - - bad = options; - bad.margin = std::numeric_limits::quiet_NaN(); - ExpectContains(check_with(bad), "margin"); + // Each case names the option the caller has to fix. + const std::vector>> + cases = { + {"min_interval", + [](Options* o) { + o->min_interval = 0.0; + }}, + {R"((0, 1])", + [](Options* o) { + o->min_interval = 2.0; + }}, + {"max_reported_findings", + [](Options* o) { + o->max_reported_findings = 0; + }}, + {"query_tolerance", + [](Options* o) { + o->query_tolerance = -1.0; + }}, + {"certificate_slack", + [](Options* o) { + o->certificate_slack = -1e-9; + }}, + {"max_nodes", + [](Options* o) { + o->max_nodes = 0; + }}, + {"margin", + [](Options* o) { + o->margin = std::numeric_limits::quiet_NaN(); + }}, + }; + for (const auto& [needle, mutate] : cases) { + SCOPED_TRACE(needle); + Options bad = SerialOptions(); + mutate(&bad); + EXPECT_THAT(ThrowMessage([&]() { + checker.CheckTrajectory(trajectory, bad); + }), + HasSubstr(needle)); + } } GTEST_TEST(ApiTest, NullModelIsRefused) { - ContinuousCollisionChecker::Params params; - const std::string message = ThrowMessage([&]() { - ContinuousCollisionChecker checker(params); - }); - ExpectContains(message, "Params::model is null"); // 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. - ExpectContains(message, "finalized"); + ContinuousCollisionChecker::Params params; + EXPECT_THAT( + ThrowMessage([&]() { + ContinuousCollisionChecker checker(params); + }), + AllOf(HasSubstr("Params::model is null"), HasSubstr("finalized"))); } GTEST_TEST(ApiTest, MaxReportedFindingsIsRespected) { - std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model); - Options options; - options.parallelism = Parallelism::None(); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + const Options options = SerialOptions(); - // Sweep the arm out past the post at θ ≈ π/2 with the tool extended and back - // again: two segments, each with its own violating region, so kCertifyAll - // (which drops a violating pair once per subtree) has more than one finding - // to cap. + // Sweep the arm out past the post at theta ~ pi/2 with the tool extended and + // back again: two segments, each with its own violating region, so + // kCertifyAll (which drops a violating pair once per subtree) has more than + // one finding to cap. Eigen::MatrixXd waypoints(2, 3); waypoints << 0.0, 2.4, 0.0, 0.25, 0.25, 0.25; - const CertificationResult uncapped = checker->CheckPath(waypoints, options); + const CertificationResult uncapped = checker.CheckPath(waypoints, options); ASSERT_EQ(uncapped.verdict, Verdict::kViolationFound); ASSERT_GE(uncapped.findings.size(), 2u); EXPECT_LE(static_cast(uncapped.findings.size()), @@ -662,7 +568,7 @@ GTEST_TEST(ApiTest, MaxReportedFindingsIsRespected) { SCOPED_TRACE("cap = " + std::to_string(cap)); Options capped = options; capped.max_reported_findings = cap; - const CertificationResult result = checker->CheckPath(waypoints, capped); + const CertificationResult result = checker.CheckPath(waypoints, capped); EXPECT_EQ(result.verdict, Verdict::kViolationFound); // Exactly `cap`, not merely at most: the sink keeps the cap earliest // entries, and this run has more than `cap` of them. An "at most" assertion diff --git a/planning/continuous_collision/test/bounding_sphere_test.cc b/planning/continuous_collision/test/bounding_sphere_test.cc index 4525def6e591..92a39691a4c7 100644 --- a/planning/continuous_collision/test/bounding_sphere_test.cc +++ b/planning/continuous_collision/test/bounding_sphere_test.cc @@ -1,15 +1,14 @@ // 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 λ 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. +// 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 "drake/planning/continuous_collision/bounding_sphere.h" #include -#include -#include +#include #include #include #include @@ -20,11 +19,12 @@ #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/math/rotation_matrix.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" namespace drake { namespace planning { @@ -42,8 +42,9 @@ using drake::geometry::MeshcatCone; using drake::geometry::Shape; using drake::geometry::Sphere; using drake::math::RigidTransform; -using drake::math::RotationMatrix; using Eigen::Vector3d; +using test::Rng; +using test::Sampler; constexpr int kNumPoses = 100; constexpr int kNumSurfaceSamples = 1000; @@ -56,116 +57,13 @@ constexpr int kNumSurfaceSamples = 1000; decide the outcome for a millimetre-scale shape parked a metre away. */ constexpr double kRelativeSlack = 1e-12; -using Rng = std::mt19937_64; - -double Uniform(Rng* rng, double lo, double hi) { - return std::uniform_real_distribution(lo, hi)(*rng); -} - -Vector3d RandomUnitVector(Rng* rng) { - std::normal_distribution normal(0.0, 1.0); - Vector3d v; - do { - v = Vector3d(normal(*rng), normal(*rng), normal(*rng)); - } while (v.norm() < 1e-9); - return v.normalized(); -} - -RotationMatrix RandomRotation(Rng* rng) { - std::normal_distribution normal(0.0, 1.0); - Eigen::Quaterniond q; - do { - q = Eigen::Quaterniond(normal(*rng), normal(*rng), normal(*rng), - normal(*rng)); - } while (q.norm() < 1e-9); - q.normalize(); - return RotationMatrix(q); -} - -RigidTransform RandomTransform(Rng* rng, double translation_scale) { - return RigidTransform( - RandomRotation(rng), - Vector3d(Uniform(rng, -translation_scale, translation_scale), - Uniform(rng, -translation_scale, translation_scale), - Uniform(rng, -translation_scale, translation_scale))); -} - -/* Samples a point on the surface of the shape, expressed in its canonical - geometry frame G. */ -using Sampler = std::function; - -Sampler SphereSampler(double r) { - return [r](Rng* rng) -> Vector3d { - // The explicit return type materializes the Eigen product before the - // lambda returns; without it the deduced type is an expression template - // referencing the RandomUnitVector temporary, which dangles once the - // std::function wrapper converts the result. - return r * RandomUnitVector(rng); - }; -} - -Sampler BoxSampler(double w, double d, double h) { - const Vector3d half(0.5 * w, 0.5 * d, 0.5 * h); - return [half](Rng* rng) { - const int axis = std::uniform_int_distribution(0, 2)(*rng); - const double sign = - std::uniform_int_distribution(0, 1)(*rng) == 0 ? -1.0 : 1.0; - Vector3d p(Uniform(rng, -half.x(), half.x()), - Uniform(rng, -half.y(), half.y()), - Uniform(rng, -half.z(), half.z())); - p(axis) = sign * half(axis); - return p; - }; -} - -Sampler CapsuleSampler(double r, double length) { - const double half = 0.5 * length; - return [r, half](Rng* rng) { - // Total area is split between the cylindrical barrel and the two caps; - // exact area weighting is irrelevant here, but every region must be - // sampled. - if (std::uniform_int_distribution(0, 1)(*rng) == 0) { - const double phi = Uniform(rng, 0.0, 2.0 * M_PI); - return Vector3d(r * std::cos(phi), r * std::sin(phi), - Uniform(rng, -half, half)); - } - const Vector3d u = RandomUnitVector(rng); - const double z_center = u.z() >= 0.0 ? half : -half; - return Vector3d(r * u.x(), r * u.y(), z_center + r * u.z()); - }; -} - -Sampler CylinderSampler(double r, double length) { - const double half = 0.5 * length; - return [r, half](Rng* rng) { - const double phi = Uniform(rng, 0.0, 2.0 * M_PI); - if (std::uniform_int_distribution(0, 1)(*rng) == 0) { - return Vector3d(r * std::cos(phi), r * std::sin(phi), - Uniform(rng, -half, half)); - } - // Cap disk: sqrt keeps the sample uniform in area, and hits the rim. - const double rho = r * std::sqrt(Uniform(rng, 0.0, 1.0)); - const double z = - std::uniform_int_distribution(0, 1)(*rng) == 0 ? -half : half; - return Vector3d(rho * std::cos(phi), rho * std::sin(phi), z); - }; -} - -Sampler EllipsoidSampler(double a, double b, double c) { - return [a, b, c](Rng* rng) { - const Vector3d u = RandomUnitVector(rng); - return Vector3d(a * u.x(), b * u.y(), c * u.z()); - }; -} - /* 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 = - std::uniform_int_distribution(0, hull.num_vertices() - 1)(*rng); + const int v = test::UniformInt(rng, 0, hull.num_vertices() - 1); return Vector3d(hull.vertex(v)); }; } @@ -175,7 +73,8 @@ void CheckContainment(const Shape& shape, const Sampler& sampler, Rng* rng) { SCOPED_TRACE(label); for (int pose = 0; pose < kNumPoses; ++pose) { - const RigidTransform X_LG = RandomTransform(rng, translation_scale); + 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; @@ -200,58 +99,69 @@ void CheckContainment(const Shape& shape, const Sampler& sampler, } } -GTEST_TEST(BoundingSphereTest, SphereContainsSurface) { - Rng rng(0x5eed0001); - for (double r : {1e-4, 0.05, 1.0, 7.5}) { - const Sphere shape(r); - CheckContainment(shape, SphereSampler(r), fmt::format("Sphere({})", r), 2.0, - &rng); +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); + }); } -} - -GTEST_TEST(BoundingSphereTest, BoxContainsSurface) { - Rng rng(0x5eed0002); - const std::vector sizes{ - {1.0, 1.0, 1.0}, {0.01, 2.0, 0.3}, {5.0, 0.002, 0.002}, {0.4, 0.7, 1.9}}; - for (const Vector3d& s : sizes) { - const Box shape(s.x(), s.y(), s.z()); - CheckContainment(shape, BoxSampler(s.x(), s.y(), s.z()), - fmt::format("Box({}, {}, {})", s.x(), s.y(), s.z()), 2.0, - &rng); + 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); + }); } -} - -GTEST_TEST(BoundingSphereTest, CapsuleContainsSurface) { - Rng rng(0x5eed0003); - const std::vector> params{ - {0.1, 1.0}, {1.0, 0.01}, {0.001, 3.0}, {0.5, 0.5}}; - for (const auto& [r, length] : params) { - const Capsule shape(r, length); - CheckContainment(shape, CapsuleSampler(r, length), - fmt::format("Capsule({}, {})", r, length), 2.0, &rng); + 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); + }); } -} - -GTEST_TEST(BoundingSphereTest, CylinderContainsSurface) { - Rng rng(0x5eed0004); - const std::vector> params{ - {0.1, 1.0}, {2.0, 0.01}, {0.002, 4.0}, {0.5, 0.5}}; - for (const auto& [r, length] : params) { - const Cylinder shape(r, length); - CheckContainment(shape, CylinderSampler(r, length), - fmt::format("Cylinder({}, {})", r, length), 2.0, &rng); + 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, EllipsoidContainsSurface) { - Rng rng(0x5eed0005); - const std::vector radii{ - {1.0, 1.0, 1.0}, {0.01, 0.5, 2.0}, {3.0, 0.001, 0.001}, {0.2, 0.9, 0.05}}; - for (const Vector3d& e : radii) { - const Ellipsoid shape(e.x(), e.y(), e.z()); - CheckContainment(shape, EllipsoidSampler(e.x(), e.y(), e.z()), - fmt::format("Ellipsoid({}, {}, {})", e.x(), e.y(), e.z()), - 2.0, &rng); +GTEST_TEST(BoundingSphereTest, PrimitivesContainTheirSurface) { + Rng rng(0x5eed0001); + for (const PrimitiveCase& entry : PrimitiveCases()) { + CheckContainment(*entry.shape, entry.sampler, entry.label, 2.0, &rng); } } @@ -264,7 +174,7 @@ std::vector> MakeVertexSets(Rng* rng) { { // Generic cloud on a ball. Eigen::Matrix3Xd v(3, 30); for (int i = 0; i < v.cols(); ++i) { - v.col(i) = Uniform(rng, 0.2, 1.0) * RandomUnitVector(rng); + v.col(i) = test::Uniform(rng, 0.2, 1.0) * test::RandomUnitVector(rng); } out.emplace_back("convex/generic", v); } @@ -279,39 +189,40 @@ std::vector> MakeVertexSets(Rng* rng) { } } for (; col < v.cols(); ++col) { - v.col(col) = Vector3d(Uniform(rng, -0.4, 0.4), Uniform(rng, -0.4, 0.4), - Uniform(rng, -0.4, 0.4)); + 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(Uniform(rng, -1.0, 1.0), Uniform(rng, -1.0, 1.0), 0.0); + 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(Uniform(rng, -1.0, 1.0), Uniform(rng, -1.0, 1.0), - Uniform(rng, -5e-7, 5e-7)); + 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(Uniform(rng, -2.0, 2.0), Uniform(rng, -1e-5, 1e-5), - Uniform(rng, -1e-5, 1e-5)); + 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 * RandomUnitVector(rng); + v.col(i) = 1e-4 * test::RandomUnitVector(rng); } out.emplace_back("convex/tiny", v); } @@ -347,9 +258,9 @@ GTEST_TEST(BoundingSphereTest, ConvexContainsHullVertices) { "construction to make this test meaningful"; } -/* Builds 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 +/* 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; @@ -394,23 +305,12 @@ GTEST_TEST(BoundingSphereTest, MeshContainsHullVertices) { /* A shape that is not on the supported list must throw, never silently inherit some other shape's formula. */ -GTEST_TEST(BoundingSphereTest, ThrowsOnHalfSpace) { - const HalfSpace shape; - const RigidTransform X_LG = RigidTransform::Identity(); - EXPECT_THROW(ComputeBoundingSphere(shape, X_LG), std::exception); - try { - ComputeBoundingSphere(shape, X_LG); - GTEST_FAIL() << "expected a throw"; - } catch (const std::exception& e) { - const std::string what = e.what(); - EXPECT_NE(what.find("HalfSpace"), std::string::npos) << what; - } -} - -GTEST_TEST(BoundingSphereTest, ThrowsOnUnsupportedShape) { - const MeshcatCone shape(1.0, 0.5, 0.25); +GTEST_TEST(BoundingSphereTest, ThrowsOnUnsupportedShapes) { const RigidTransform X_LG = RigidTransform::Identity(); - EXPECT_THROW(ComputeBoundingSphere(shape, X_LG), std::exception); + DRAKE_EXPECT_THROWS_MESSAGE(ComputeBoundingSphere(HalfSpace(), X_LG), + ".*HalfSpace.*"); + EXPECT_THROW(ComputeBoundingSphere(MeshcatCone(1.0, 0.5, 0.25), X_LG), + std::exception); } } // namespace diff --git a/planning/continuous_collision/test/certificate_test.cc b/planning/continuous_collision/test/certificate_test.cc index ae0c7ad44085..ca4fc2903c7e 100644 --- a/planning/continuous_collision/test/certificate_test.cc +++ b/planning/continuous_collision/test/certificate_test.cc @@ -6,12 +6,10 @@ // The corpus is three certified runs: one hand-built world whose two pairs are // built to certify at very different depths, plus two small random worlds. // Below it is a table of mutations, each applied to every corpus case; a -// mutation any case accepts is a hole in the audit. certifier_test.cc covers a -// handful of single-case mutations on its own world, so this file is the sweep -// plus the mutation classes that need a designed pair structure (record -// relabelling) or a second run (kFindFirstViolation and non-free verdicts). +// mutation any case accepts is a hole in the audit. #include +#include #include #include #include @@ -22,43 +20,28 @@ #include -#include "drake/common/parallelism.h" -#include "drake/common/trajectories/bezier_curve.h" -#include "drake/geometry/shape_specification.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/robot_diagram.h" -#include "drake/planning/robot_diagram_builder.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" namespace drake { namespace planning { namespace continuous_collision { namespace { -using drake::Parallelism; -using drake::geometry::Box; -using drake::geometry::Capsule; -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 Eigen::Vector3d; using Eigen::VectorXd; +using test::BezierCurve; +using test::Box; +using test::Friction; +using test::Inertia; +using test::MakeRandomWorld; +using test::MultibodyPlant; +using test::Parallelism; +using test::PrismaticJoint; +using test::RigidBody; +using test::RigidTransformd; +using test::RobotDiagram; +using test::RobotDiagramBuilder; +using test::Sphere; // A non-zero margin and a non-zero environment padding, so m_p = margin + // padding is a number a tamperer could plausibly try to lower and the @@ -66,14 +49,6 @@ using Eigen::VectorXd; constexpr double kMargin = 0.005; constexpr double kEnvPadding = 0.002; -CoulombFriction Friction() { - return CoulombFriction(1.0, 1.0); -} - -SpatialInertia Inertia() { - return SpatialInertia::SolidSphereWithMass(1.0, 0.05); -} - Options AuditOptions() { Options options; options.margin = kMargin; @@ -82,20 +57,14 @@ Options AuditOptions() { return options; } -std::unique_ptr MakeChecker( +std::unique_ptr MakeAuditChecker( std::shared_ptr> model) { - ContinuousCollisionChecker::Params params; - params.model = std::move(model); - params.default_options = AuditOptions(); - params.padding.env_padding = kEnvPadding; - params.padding.self_padding = kEnvPadding; - return std::make_unique(params); + PaddingSpec padding; + padding.env_padding = kEnvPadding; + padding.self_padding = kEnvPadding; + return test::MakeCheckerPtr(std::move(model), AuditOptions(), padding); } -// --------------------------------------------------------------------------- -// World 1 (hand-built): a designed pair structure. -// --------------------------------------------------------------------------- -// // A 2-dof Cartesian gantry (prismatic x, prismatic y) carrying a 5 mm sphere, // with exactly two unfiltered pairs: // @@ -134,83 +103,6 @@ std::unique_ptr> MakeDesignedWorld() { return builder.Build(); } -// --------------------------------------------------------------------------- -// Worlds 2, 3 (small random): a trimmed copy of the generator in -// soundness_fuzz_test.cc. -// --------------------------------------------------------------------------- - -std::unique_ptr> MakeRandomWorld(uint64_t seed) { - std::mt19937_64 rng(seed); - const auto uniform = [&rng](double lo, double hi) { - return std::uniform_real_distribution(lo, hi)(rng); - }; - // Named locals throughout: sibling constructor arguments are evaluated in an - // unspecified order, so drawing variates inline would make these worlds, and - // therefore which seeds land in the corpus, depend on the toolchain. - const auto vector3 = [&uniform](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); - }; - const auto direction = [&vector3]() { - Vector3d v; - do { - v = vector3(-1, 1); - } while (v.norm() < 1e-3 || v.norm() > 1.0); - return v.normalized(); - }; - const auto offset = [&direction, &uniform](double lo, double hi) { - const Vector3d unit = direction(); - const double length = uniform(lo, hi); - return Vector3d(unit * length); - }; - - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - std::vector*> links; - for (int i = 0; i < 3; ++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 = vector3(-0.5, 0.5); - const RigidTransformd X_PF(RollPitchYawd(rpy_PF), offset(0.25, 0.35)); - const Vector3d axis = direction(); - if (i == 1) { - plant.AddJoint("j" + std::to_string(i), parent, X_PF, - body, RigidTransformd(), axis); - } else { - plant.AddJoint("j" + std::to_string(i), parent, X_PF, body, - RigidTransformd(), axis); - } - const RigidTransformd X_LG(offset(0.12, 0.18)); - const double radius = uniform(0.02, 0.04); - const double length = uniform(0.05, 0.10); - plant.RegisterCollisionGeometry(body, X_LG, Capsule(radius, length), - name + "_geom", Friction()); - links.push_back(&body); - } - for (int i = 0; i < 3; ++i) { - const std::string name = "obstacle" + std::to_string(i); - const RigidBody& body = plant.AddRigidBody(name, Inertia()); - const Vector3d rpy_W = vector3(-3, 3); - plant.WeldFrames(plant.world_frame(), body.body_frame(), - RigidTransformd(RollPitchYawd(rpy_W), offset(0.3, 0.8))); - if (i % 2 == 0) { - const Vector3d size = vector3(0.08, 0.2); - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Box(size.x(), size.y(), size.z()), - name + "_geom", Friction()); - } else { - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Sphere(uniform(0.05, 0.11)), - name + "_geom", Friction()); - } - } - return builder.Build(); -} - // A cubic Bézier whose control points are equally spaced from `start` to `end`. // It is the straight segment, but with four control points, so a mutation can // perturb an interior one without moving either endpoint; moving an endpoint @@ -224,10 +116,6 @@ Eigen::MatrixXd CubicControlPoints(const VectorXd& start, const VectorXd& end) { return points; } -// --------------------------------------------------------------------------- -// The corpus. -// --------------------------------------------------------------------------- - struct AuditCase { std::string name; std::shared_ptr> model; @@ -257,47 +145,49 @@ struct AuditCase { // certify is dropped rather than added, so CorpusIsBuiltAndVerifies is the // single place that reports a short corpus. No gtest assertion belongs here: // this initializer runs inside whichever test touches Corpus() first, which -// changes under --gtest_filter or --gtest_shuffle, and a failure charged to an -// arbitrary test is a failure nobody can read. +// changes under --gtest_filter or --gtest_shuffle. // // The vector is allocated and never freed because it owns RobotDiagrams and -// checkers whose destruction would otherwise race Drake's static teardown. LSan -// will report it if an asan preset is ever added. +// checkers whose destruction would otherwise race Drake's static teardown. const std::vector>& Corpus() { static const std::vector>* corpus = [] { auto* cases = new std::vector>(); const Options options = AuditOptions(); + const auto add = [&cases, &options](std::unique_ptr entry) { + const BezierCurve trajectory(0.0, 1.0, entry->control_points); + const CertificationResult result = + entry->checker->CheckTrajectory(trajectory, options); + if (result.verdict != Verdict::kCertifiedFree) return; + entry->path = entry->checker->Normalize(trajectory, options); + entry->certificate = *result.certificate; + cases->push_back(std::move(entry)); + }; - // 1. The designed world. - { + { // 1. The designed world. auto entry = std::make_unique(); entry->name = "designed_gantry"; entry->designed = true; entry->model = MakeDesignedWorld(); - entry->checker = MakeChecker(entry->model); + entry->checker = MakeAuditChecker(entry->model); VectorXd start(2), end(2); start << -0.3, 0.0; end << 0.3, 0.0; entry->control_points = CubicControlPoints(start, end); - const BezierCurve trajectory(0.0, 1.0, entry->control_points); - const CertificationResult result = - entry->checker->CheckTrajectory(trajectory, options); - if (result.verdict == Verdict::kCertifiedFree && - result.certificate.has_value()) { - entry->path = entry->checker->Normalize(trajectory, options); - entry->certificate = *result.certificate; - cases->push_back(std::move(entry)); - } + add(std::move(entry)); } // 2. Small random worlds: the first two seeds whose trajectory certifies. // Sweeping deterministically, rather than hard-coding lucky seeds, still // fills the corpus if the geometry ever shifts underneath it. - for (uint64_t seed = 1; seed <= 40 && cases->size() < 3; ++seed) { + for (uint64_t seed = 1; seed <= 60 && cases->size() < 3; ++seed) { auto entry = std::make_unique(); entry->name = "random_world_seed_" + std::to_string(seed); - entry->model = MakeRandomWorld(seed); - entry->checker = MakeChecker(entry->model); + test::WorldSpec spec; + spec.num_links = 3; + spec.num_obstacles = 3; + spec.floor = false; + entry->model = MakeRandomWorld(seed, spec); + entry->checker = MakeAuditChecker(entry->model); const int n = entry->model->plant().num_positions(); VectorXd start = VectorXd::Zero(n); VectorXd end = VectorXd::Zero(n); @@ -306,13 +196,7 @@ const std::vector>& Corpus() { end[i] = start[i] + 0.25; } entry->control_points = CubicControlPoints(start, end); - const BezierCurve trajectory(0.0, 1.0, entry->control_points); - const CertificationResult result = - entry->checker->CheckTrajectory(trajectory, options); - if (result.verdict != Verdict::kCertifiedFree) continue; - entry->path = entry->checker->Normalize(trajectory, options); - entry->certificate = *result.certificate; - cases->push_back(std::move(entry)); + add(std::move(entry)); } return cases; }(); @@ -396,8 +280,6 @@ GTEST_TEST(CertificateAuditTest, DesignedWorldHasTheIntendedPairStructure) { << "the designed world should present exactly the tool/plate and " "tool/ball pairs"; const std::vector counts = RecordsPerPair(entry); - const int hardest = *std::max_element(counts.begin(), counts.end()); - const int easiest = *std::min_element(counts.begin(), counts.end()); // The far pair certifies at the root: exactly one record, for the path's one // segment. The 12 mm pair needs Δ = w_x < 0.012 − 0.007 − τ ≈ 0.005 against // 0.6 m of travel, i.e. a node half-width of 0.3/2^d < 0.005 => d = 6, and a @@ -405,8 +287,8 @@ GTEST_TEST(CertificateAuditTest, DesignedWorldHasTheIntendedPairStructure) { // Pinned exactly, so a regression that loosened or tightened the motion bound // by even one bisection level shows up here rather than hiding behind an // inequality. - EXPECT_EQ(easiest, 1); - EXPECT_EQ(hardest, 64); + EXPECT_EQ(*std::min_element(counts.begin(), counts.end()), 1); + EXPECT_EQ(*std::max_element(counts.begin(), counts.end()), 64); // Every pair's records must claim the same, correct threshold. for (const CertificateRecord& record : entry.certificate.records) { EXPECT_DOUBLE_EQ(record.threshold, kMargin + kEnvPadding); @@ -435,13 +317,22 @@ void ExpectRejectedEverywhere(const std::string& what, const Mutation& mutate) { << "' was never applicable to any corpus case"; } -GTEST_TEST(CertificateAuditTest, RejectsInflatedClearance) { +GTEST_TEST(CertificateAuditTest, RejectsTamperedClearance) { ExpectRejectedEverywhere("inflate phi_hat", [](const AuditCase&, Certificate* certificate) { if (certificate->records.empty()) return false; certificate->records.front().phi_hat += 1.0; return true; }); + // ... and the mirror image: a record whose claimed clearance no longer + // exceeds its own threshold proves nothing. + ExpectRejectedEverywhere("shrink phi_hat to the threshold", + [](const AuditCase&, Certificate* certificate) { + if (certificate->records.empty()) return false; + certificate->records.front().phi_hat = + certificate->records.front().threshold; + return true; + }); } GTEST_TEST(CertificateAuditTest, RejectsWidenedInterval) { @@ -468,7 +359,7 @@ GTEST_TEST(CertificateAuditTest, RejectsShiftedRepresentativeConfiguration) { }); } -GTEST_TEST(CertificateAuditTest, RejectsDeletedRecord) { +GTEST_TEST(CertificateAuditTest, RejectsMissingRecords) { // The certifier's intervals tile the domain disjointly, so deleting any // record punches a coverage hole, even one whose own arithmetic was sound. ExpectRejectedEverywhere( @@ -477,9 +368,6 @@ GTEST_TEST(CertificateAuditTest, RejectsDeletedRecord) { certificate->records.erase(certificate->records.begin()); return true; }); -} - -GTEST_TEST(CertificateAuditTest, RejectsTruncatedRecords) { ExpectRejectedEverywhere( "truncate the record list", [](const AuditCase&, Certificate* certificate) { @@ -504,6 +392,17 @@ GTEST_TEST(CertificateAuditTest, RejectsLoweredThreshold) { } return true; }); + // Self-consistency is not enough either: a certificate whose records *all* + // agree on a threshold nobody asked for proves a claim nobody asked for. + ExpectRejectedEverywhere( + "lower every threshold uniformly", + [](const AuditCase&, Certificate* certificate) { + if (certificate->records.empty()) return false; + for (CertificateRecord& record : certificate->records) { + record.threshold = -1e9; + } + return true; + }); } GTEST_TEST(CertificateAuditTest, RejectsPairTableMismatch) { @@ -573,18 +472,12 @@ GTEST_TEST(CertificateAuditTest, RejectsPerturbedPath) { } } -// --------------------------------------------------------------------------- -// 3. What a valid transformation looks like. -// --------------------------------------------------------------------------- - GTEST_TEST(CertificateAuditTest, AcceptsReorderedRecords) { // Re-ordering is the one item on the classic mutation list that must NOT be - // rejected: a permutation of a valid proof is still a valid proof. The replay - // sorts the intervals itself before checking coverage and every record is - // checked independently, so order carries no information. Pinning this keeps - // a future "records must arrive sorted" shortcut from being mistaken for a - // security property, and it rules out a verifier that rejects everything, - // which would pass every mutation above. + // rejected: a permutation of a valid proof is still a valid proof. Pinning + // this keeps a future "records must arrive sorted" shortcut from being + // mistaken for a security property, and it rules out a verifier that rejects + // everything, which would pass every mutation above. std::mt19937 rng(20260826); int shuffled = 0; for (const auto& entry : Corpus()) { @@ -601,7 +494,7 @@ GTEST_TEST(CertificateAuditTest, AcceptsReorderedRecords) { } // --------------------------------------------------------------------------- -// 4. Runs whose certificate is not a proof. +// 3. Runs whose certificate is not a proof. // --------------------------------------------------------------------------- GTEST_TEST(CertificateAuditTest, NoCertificateUnlessRequested) { @@ -616,15 +509,6 @@ GTEST_TEST(CertificateAuditTest, NoCertificateUnlessRequested) { EXPECT_FALSE(result.certificate.has_value()); } -// The designed world again, but driven straight through the 1 mm plate at -// y = 0.0175: q(t) sweeps y from 0 to 0.05 while x crosses the plate's span. -Eigen::MatrixXd ViolatingControlPoints() { - VectorXd start(2), end(2); - start << -0.3, 0.0; - end << 0.3, 0.05; - return CubicControlPoints(start, end); -} - GTEST_TEST(CertificateAuditTest, NonFreeVerdictCertificateIsNotAProof) { // The certificate field is present whenever emit_certificate was asked for, // and the records the run did make are individually valid. A run that found a @@ -634,7 +518,13 @@ GTEST_TEST(CertificateAuditTest, NonFreeVerdictCertificateIsNotAProof) { ASSERT_FALSE(Corpus().empty()); const AuditCase& entry = *Corpus().front(); const Options options = AuditOptions(); - const BezierCurve trajectory(0.0, 1.0, ViolatingControlPoints()); + // The designed world driven straight through the 1 mm plate at y = 0.0175: + // q(t) sweeps y from 0 to 0.05 while x crosses the plate's span. + VectorXd start(2), end(2); + start << -0.3, 0.0; + end << 0.3, 0.05; + const BezierCurve trajectory(0.0, 1.0, + CubicControlPoints(start, end)); const PiecewiseBezierPath path = entry.checker->Normalize(trajectory, options); diff --git a/planning/continuous_collision/test/certifier_test.cc b/planning/continuous_collision/test/certifier_test.cc index 510a83bee9c1..52d9675cfd8d 100644 --- a/planning/continuous_collision/test/certifier_test.cc +++ b/planning/continuous_collision/test/certifier_test.cc @@ -1,6 +1,8 @@ // 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 and is not duplicated here. +// test/soundness_fuzz_test.cc, the certificate mutation sweep in +// test/certificate_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 @@ -8,62 +10,43 @@ #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/rigid_transform.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/robot_diagram.h" -#include "drake/planning/robot_diagram_builder.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" namespace drake { namespace planning { namespace continuous_collision { namespace { -using drake::Parallelism; -using drake::geometry::Box; -using drake::geometry::HalfSpace; -using drake::geometry::QueryObject; -using drake::geometry::Sphere; -using drake::math::RigidTransformd; -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::Vector3d; using Eigen::VectorXd; +using test::BezierCurve; +using test::Box; +using test::DistanceAtFinding; +using test::Friction; +using test::HalfSpace; +using test::Inertia; +using test::MakeChecker; +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; -CoulombFriction Friction() { - return CoulombFriction(1.0, 1.0); -} - -SpatialInertia UnitInertia() { - return SpatialInertia::SolidSphereWithMass(1.0, 0.05); -} - // 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] @@ -73,9 +56,9 @@ SpatialInertia UnitInertia() { // 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", UnitInertia()); - const RigidBody& link2 = plant->AddRigidBody("link2", UnitInertia()); - const RigidBody& tool = plant->AddRigidBody("tool", UnitInertia()); + 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()); @@ -98,7 +81,7 @@ void AddArm(MultibodyPlant* plant) { void AddWeldedSphere(MultibodyPlant* plant, const std::string& name, const Vector3d& p_W, double radius) { - const RigidBody& body = plant->AddRigidBody(name, UnitInertia()); + const RigidBody& body = plant->AddRigidBody(name, Inertia()); plant->WeldFrames(plant->world_frame(), body.body_frame(), RigidTransformd(p_W)); plant->RegisterCollisionGeometry(body, RigidTransformd(), Sphere(radius), @@ -117,53 +100,19 @@ std::shared_ptr> MakeArmWorld() { // 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", UnitInertia()); + 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", UnitInertia()); + 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 std::shared_ptr>(builder.Build()); -} - -// 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. -std::shared_ptr> MakeGrazeWorld() { - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - AddArm(&plant); - AddWeldedSphere(&plant, "graze", Vector3d(0.80, 0.11, 0.0), 0.05); - return std::shared_ptr>(builder.Build()); -} - -// A genuinely free squeeze: the tool slides between two spheres that leave -// only 5 mm of clearance over the margin, so the certificate is real but has -// to be earned by subdividing (the mirror image of the tangency world). -std::shared_ptr> MakeGapWorld() { - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - AddArm(&plant); - // Sphere surface to tool surface at the closest approach: - // 0.115 − 0.05 − 0.05 = 0.015 = kMargin + 0.005. - 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); - return std::shared_ptr>(builder.Build()); -} - -ContinuousCollisionChecker MakeChecker( - std::shared_ptr> model, Options options) { - ContinuousCollisionChecker::Params params; - params.model = std::move(model); - params.default_options = std::move(options); - return ContinuousCollisionChecker(params); + return builder.Build(); } Options SerialOptions() { @@ -173,11 +122,11 @@ Options SerialOptions() { return options; } -// A cubic Bézier from `start` to `end` with linearly spaced control points -// (so the curve is the straight segment, traversed with a nontrivial -// parametrization) over the time interval [t0, t1]. +// 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, double t1) { + 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; @@ -186,6 +135,12 @@ BezierCurve MakeBezier(const VectorXd& start, const VectorXd& end, return BezierCurve(t0, t1, control_points); } +VectorXd MakeQ(double theta1, double theta2, double d) { + VectorXd q(3); + q << theta1, theta2, d; + return q; +} + // Result of the dense-sampling cross-check. struct SampledClearance { double min_clearance{std::numeric_limits::infinity()}; @@ -194,8 +149,8 @@ struct SampledClearance { }; // 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 +// 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 ContinuousCollisionChecker& checker, @@ -229,42 +184,14 @@ SampledClearance SampleClearance(const ContinuousCollisionChecker& checker, return result; } -// Re-evaluates one finding's configuration from scratch and returns the -// oracle distance of its pair there. -double DistanceAtFinding(const ContinuousCollisionChecker& checker, - const Finding& finding) { - const RobotDiagram& model = checker.model(); - 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)); - for (const PairRecord& pair : checker.pairs()) { - if (pair.id.a == finding.pair.a && pair.id.b == finding.pair.b) { - return checker.distance_oracle().SignedDistance(query_object, pair); - } - } - ADD_FAILURE() << "the finding names a pair the checker does not know."; - return 0.0; -} - -VectorXd MakeQ(double theta1, double theta2, double d) { - VectorXd q(3); - q << theta1, theta2, d; - return q; -} - // --------------------------------------------------------------------------- -// 1. A free trajectory is certified, and dense sampling agrees. +// 1. Free trajectories are certified, and dense sampling agrees. // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, FreeTrajectoryCertified) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3); const CertificationResult result = checker.CheckTrajectory(trajectory); EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); @@ -279,14 +206,32 @@ GTEST_TEST(CertifierTest, FreeTrajectoryCertified) { SampleClearance(checker, path, 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) { - const auto model = MakeGapWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + // 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 auto checker = MakeChecker(builder.Build(), 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, 0.0, 1.0); + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.0, 0.0, 0.20), 1); Options options = SerialOptions(); options.emit_certificate = true; @@ -307,31 +252,14 @@ GTEST_TEST(CertifierTest, NarrowGapCertifiedBySubdivision) { EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); } -GTEST_TEST(CertifierTest, FreePathAndEdgeCertified) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - - 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); - const CertificationResult path_result = checker.CheckPath(waypoints); - EXPECT_EQ(path_result.verdict, Verdict::kCertifiedFree); - - const CertificationResult edge_result = - checker.CheckEdge(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10)); - EXPECT_EQ(edge_result.verdict, Verdict::kCertifiedFree); -} - // --------------------------------------------------------------------------- // 2. A sweeping trajectory that hits an obstacle. // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, ViolationFoundWithExactWitness) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1); const CertificationResult result = checker.CheckTrajectory(trajectory); ASSERT_EQ(result.verdict, Verdict::kViolationFound); @@ -342,22 +270,19 @@ GTEST_TEST(CertifierTest, ViolationFoundWithExactWitness) { 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. + // reported time must reproduce it; and re-querying the distance from a fresh + // context must confirm the violation. const PiecewiseBezierPath path = checker.Normalize(trajectory); EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), 1e-9); - - // ... and re-querying the distance from a fresh context must confirm the - // violation. const double phi = DistanceAtFinding(checker, finding); EXPECT_LT(phi, kMargin); EXPECT_NEAR(phi, finding.distance, 1e-12); } GTEST_TEST(CertifierTest, FindFirstReturnsEarliestWitness) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1); Options options = SerialOptions(); options.mode = SearchMode::kFindFirstViolation; @@ -376,17 +301,41 @@ GTEST_TEST(CertifierTest, FindFirstReturnsEarliestWitness) { EXPECT_LE(result.findings.front().time, sampled.first_crossing + 1e-9); } -// --------------------------------------------------------------------------- -// 3. Grazing tangency is inconclusive, never certified free. -// --------------------------------------------------------------------------- +GTEST_TEST(CertifierTest, CertifyAllReportsEveryViolation) { + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + // Sweeping θ1 from 0 to 3 rad passes the post (≈1.57 rad) and then the + // pillar (≈2.58 rad): two disjoint violating regions, different pairs. + const BezierCurve trajectory = + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(3.0, 0.0, 0.0), 1); + + const CertificationResult result = checker.CheckTrajectory(trajectory); + ASSERT_EQ(result.verdict, Verdict::kViolationFound); + ASSERT_GE(result.findings.size(), 2u); + int definite = 0; + for (std::size_t i = 0; i < result.findings.size(); ++i) { + if (i > 0) { + EXPECT_LE(result.findings[i - 1].time, result.findings[i].time) + << "findings must be earliest-first"; + } + if (result.findings[i].definite) { + ++definite; + EXPECT_LT(DistanceAtFinding(checker, result.findings[i]), kMargin); + } + } + EXPECT_GE(definite, 2); +} GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { - const auto model = MakeGrazeWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - // θ1 = θ2 = 0 throughout; only the prismatic coordinate moves, sliding the - // tool sphere past the obstacle at exactly margin distance. + // 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 auto checker = MakeChecker(builder.Build(), SerialOptions()); const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.0, 0.0, 0.20), 1, 0.0, 1.0); + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.0, 0.0, 0.20), 1); Options options = SerialOptions(); // A coarser floor keeps the cost of the tangency cascade bounded; the @@ -396,7 +345,6 @@ GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { checker.CheckTrajectory(trajectory, options); EXPECT_EQ(result.verdict, Verdict::kInconclusive); - EXPECT_NE(result.verdict, Verdict::kCertifiedFree); ASSERT_FALSE(result.findings.empty()); const Finding& finding = result.findings.front(); EXPECT_FALSE(finding.definite); @@ -413,23 +361,21 @@ GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { } // --------------------------------------------------------------------------- -// 4. Static pairs (J(p) = ∅) are resolved once and certified globally. +// 3. Static pairs (J(p) = ∅) are resolved once and certified globally. // --------------------------------------------------------------------------- -// Note on where static pairs come from: MultibodyPlant::Finalize() already -// filters every pair *within* a welded subgraph, so two anchored obstacles (or -// two members of a welded cluster on the robot) never even reach the checker -// as a candidate pair. The reachable source of J(p) = ∅ is therefore the -// constant-coordinate carve-out: a coordinate that no control point of the -// trajectory moves is removed from every J(p), and pairs left with an empty -// set are resolved once at q(t0). +// 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: a coordinate that no control point of the trajectory moves is +// removed from every J(p), and pairs left with an empty set are resolved once +// at q(t0). GTEST_TEST(CertifierTest, StaticPairsResolvedOnce) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); // Only the prismatic coordinate moves: θ1 and θ2 are constant, so every pair // whose relative pose depends only on them becomes static. const BezierCurve trajectory = - MakeBezier(MakeQ(0.3, -0.2, 0.0), MakeQ(0.3, -0.2, 0.15), 2, 0.0, 1.0); + MakeBezier(MakeQ(0.3, -0.2, 0.0), MakeQ(0.3, -0.2, 0.15), 2); const PiecewiseBezierPath path = checker.Normalize(trajectory); const MotionBoundTable table = checker.ComputeMotionBounds(path); @@ -473,17 +419,38 @@ GTEST_TEST(CertifierTest, StaticPairsResolvedOnce) { } } EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); + + // A static record must be measured at the path's own start configuration: + // "static" is relative to the carve-out, so a record re-based onto an + // off-path configuration would measure a different pair pose entirely. Both + // directions: rotating θ1 toward the obstacles reduces the clearance the + // replay measures, while rotating away *increases* it, and only the "static + // records are pinned to q(t0)" check catches that second case. + for (const double delta : {1.5, -1.5}) { + Certificate certificate = *result.certificate; + int tampered = 0; + for (CertificateRecord& record : certificate.records) { + if (table.pair_is_static(record.pair_index)) { + record.qc[0] += delta; + ++tampered; + break; + } + } + ASSERT_EQ(tampered, 1); + EXPECT_FALSE(VerifyCertificate(checker, path, certificate)) + << "delta = " << delta; + } } // --------------------------------------------------------------------------- -// 4b. Padding reaches the effective threshold, and the env/self split is the -// documented one (self = both bodies move relative to the world). +// 4. Padding reaches the effective threshold, and the env/self split is the +// documented one (self = both bodies move relative to the world). // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, PaddingSemantics) { const auto model = MakeArmWorld(); const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); + MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3); const auto is_arm_self_pair = [&model](const Finding& finding) { const auto& plant = model->plant(); const std::string a = plant.get_body(finding.pair.body_a).name(); @@ -495,58 +462,36 @@ GTEST_TEST(CertifierTest, PaddingSemantics) { // The one robot-vs-robot pair (link1, tool) keeps ≈ 0.27 m of clearance on // this trajectory, so 0.4 m of *self* padding must break it, and nothing // else: every other pair has an anchored side and takes the (zero) - // environment padding. - { - ContinuousCollisionChecker::Params params; - params.model = model; - params.default_options = SerialOptions(); - params.padding.self_padding = 0.40; - const ContinuousCollisionChecker checker(params); - const CertificationResult result = checker.CheckTrajectory(trajectory); - ASSERT_EQ(result.verdict, Verdict::kViolationFound); - for (const Finding& finding : result.findings) { - EXPECT_TRUE(is_arm_self_pair(finding)) - << "self padding must not apply to environment pairs"; - } - } - - // Mirrored: environment padding reaches the arm-vs-obstacle pairs (the - // ground halfspace is 0.45 m away) and leaves the self pair alone. - { - ContinuousCollisionChecker::Params params; - params.model = model; - params.default_options = SerialOptions(); - params.padding.env_padding = 0.50; - const ContinuousCollisionChecker checker(params); + // environment padding. Mirrored, 0.5 m of environment padding reaches the + // arm-vs-obstacle pairs (the ground halfspace is 0.45 m away) and leaves the + // self pair alone. + for (const bool self : {true, false}) { + SCOPED_TRACE(self ? "self padding" : "env padding"); + PaddingSpec padding; + (self ? padding.self_padding : padding.env_padding) = self ? 0.40 : 0.50; + const auto checker = MakeChecker(model, SerialOptions(), padding); const CertificationResult result = checker.CheckTrajectory(trajectory); ASSERT_EQ(result.verdict, Verdict::kViolationFound); for (const Finding& finding : result.findings) { - EXPECT_FALSE(is_arm_self_pair(finding)) - << "environment padding must not apply to robot self pairs"; + EXPECT_EQ(is_arm_self_pair(finding), self) + << "padding must apply to exactly one class of pair"; } } - // A per-body-pair matrix overrides the scalars. - { - ContinuousCollisionChecker::Params params; - params.model = model; - params.default_options = SerialOptions(); - params.padding.env_padding = 0.50; - params.padding.per_body_pair = Eigen::MatrixXd::Zero( - model->plant().num_bodies(), model->plant().num_bodies()); - const ContinuousCollisionChecker checker(params); - EXPECT_EQ(checker.CheckTrajectory(trajectory).verdict, - Verdict::kCertifiedFree); - } + // A per-body-pair matrix overrides the scalars ... + PaddingSpec overridden; + overridden.env_padding = 0.50; + overridden.per_body_pair = Eigen::MatrixXd::Zero(model->plant().num_bodies(), + model->plant().num_bodies()); + EXPECT_EQ(MakeChecker(model, SerialOptions(), overridden) + .CheckTrajectory(trajectory) + .verdict, + Verdict::kCertifiedFree); - // A mis-sized matrix is a clear throw. - { - ContinuousCollisionChecker::Params params; - params.model = model; - params.default_options = SerialOptions(); - params.padding.per_body_pair = Eigen::MatrixXd::Zero(2, 2); - EXPECT_THROW(ContinuousCollisionChecker{params}, std::exception); - } + // ... and a mis-sized matrix is a clear throw. + PaddingSpec mis_sized; + mis_sized.per_body_pair = Eigen::MatrixXd::Zero(2, 2); + EXPECT_THROW(MakeChecker(model, SerialOptions(), mis_sized), std::exception); } // --------------------------------------------------------------------------- @@ -554,17 +499,16 @@ GTEST_TEST(CertifierTest, PaddingSemantics) { // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, RetimingInvariance) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); const VectorXd start = MakeQ(0.0, 0.0, 0.0); const VectorXd end = MakeQ(0.8, -0.4, 0.10); - const BezierCurve fast = MakeBezier(start, end, 3, 0.0, 1.0); - const BezierCurve slow = MakeBezier(start, end, 3, -2.5, 4.2); Options options = SerialOptions(); options.emit_certificate = true; - const CertificationResult a = checker.CheckTrajectory(fast, options); - const CertificationResult b = checker.CheckTrajectory(slow, options); + const CertificationResult a = + checker.CheckTrajectory(MakeBezier(start, end, 3, 0.0, 1.0), options); + const CertificationResult b = + checker.CheckTrajectory(MakeBezier(start, end, 3, -2.5, 4.2), options); EXPECT_EQ(a.verdict, b.verdict); EXPECT_EQ(a.stats.nodes, b.stats.nodes); @@ -592,189 +536,11 @@ GTEST_TEST(CertifierTest, RetimingInvariance) { } // --------------------------------------------------------------------------- -// 6. Certificate emission, replay and mutation. +// 6. The node budget and breakpoint semantics. // --------------------------------------------------------------------------- -class CertificateFixture : public ::testing::Test { - protected: - CertificateFixture() - : model_(MakeArmWorld()), - checker_(MakeChecker(model_, SerialOptions())), - trajectory_(MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, - 0.0, 1.0)), - path_(checker_.Normalize(trajectory_)) { - Options options = SerialOptions(); - options.emit_certificate = true; - result_ = checker_.CheckTrajectory(trajectory_, options); - } - - // Index of a record belonging to a pair the trajectory actually moves (so - // the record carries a real node interval, not the global static one). - int MovingRecordIndex() const { - const MotionBoundTable table = checker_.ComputeMotionBounds(path_); - for (int i = 0; i < static_cast(result_.certificate->records.size()); - ++i) { - const CertificateRecord& record = result_.certificate->records[i]; - if (!table.pair_is_static(record.pair_index) && record.s_end < 1.0) { - return i; - } - } - return -1; - } - - std::shared_ptr> model_; - ContinuousCollisionChecker checker_; - BezierCurve trajectory_; - PiecewiseBezierPath path_; - CertificationResult result_; -}; - -TEST_F(CertificateFixture, VerifiesOnACertifiedRun) { - ASSERT_EQ(result_.verdict, Verdict::kCertifiedFree); - ASSERT_TRUE(result_.certificate.has_value()); - EXPECT_FALSE(result_.certificate->records.empty()); - EXPECT_TRUE(VerifyCertificate(checker_, path_, *result_.certificate)); -} - -TEST_F(CertificateFixture, RejectsShrunkClearance) { - Certificate certificate = *result_.certificate; - ASSERT_FALSE(certificate.records.empty()); - certificate.records[0].phi_hat = certificate.records[0].threshold; - EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); -} - -TEST_F(CertificateFixture, RejectsInflatedClearance) { - Certificate certificate = *result_.certificate; - ASSERT_FALSE(certificate.records.empty()); - certificate.records[0].phi_hat += 1.0; - EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); -} - -TEST_F(CertificateFixture, RejectsWidenedInterval) { - Certificate certificate = *result_.certificate; - const int index = MovingRecordIndex(); - ASSERT_GE(index, 0); - CertificateRecord& record = certificate.records[index]; - record.s_end = std::min(1.0, record.s_end + (record.s_end - record.s_start)); - EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); -} - -TEST_F(CertificateFixture, RejectsTamperedRepresentativeConfiguration) { - Certificate certificate = *result_.certificate; - const int index = MovingRecordIndex(); - ASSERT_GE(index, 0); - certificate.records[index].qc[0] += 0.1; - EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); -} - -TEST_F(CertificateFixture, RejectsDroppedCoverage) { - Certificate certificate = *result_.certificate; - const int index = MovingRecordIndex(); - ASSERT_GE(index, 0); - certificate.records.erase(certificate.records.begin() + index); - EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); -} - -TEST_F(CertificateFixture, RejectsLoweredThreshold) { - Certificate certificate = *result_.certificate; - ASSERT_GE(certificate.records.size(), 2u); - certificate.records[0].threshold -= 0.005; - EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); -} - -TEST_F(CertificateFixture, RejectsUniformlyLoweredThresholds) { - // Self-consistency is not enough: a certificate whose records *all* agree on - // a threshold nobody asked for proves a claim nobody asked for. - Certificate certificate = *result_.certificate; - ASSERT_FALSE(certificate.records.empty()); - for (CertificateRecord& record : certificate.records) { - record.threshold = -1e9; - } - EXPECT_FALSE(VerifyCertificate(checker_, path_, certificate)); -} - -GTEST_TEST(CertifierTest, CertificateRejectsRebasedStaticRecord) { - // A static record must be measured at the path's own start configuration: - // "static" is relative to the constant-coordinate carve-out, so a record - // re-based onto an off-path configuration would measure a different pair - // pose entirely. - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - const BezierCurve trajectory = - MakeBezier(MakeQ(0.3, -0.2, 0.0), MakeQ(0.3, -0.2, 0.15), 2, 0.0, 1.0); - Options options = SerialOptions(); - options.emit_certificate = true; - const CertificationResult result = - checker.CheckTrajectory(trajectory, options); - ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); - ASSERT_TRUE(result.certificate.has_value()); - - const PiecewiseBezierPath path = checker.Normalize(trajectory); - const MotionBoundTable table = checker.ComputeMotionBounds(path); - EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); - - // Both directions: rotating θ1 toward the obstacles reduces the clearance - // the replay measures, while rotating away *increases* it. Only the "static - // records are pinned to q(t0)" check catches that second case. - for (const double delta : {1.5, -1.5}) { - Certificate certificate = *result.certificate; - int tampered = 0; - for (CertificateRecord& record : certificate.records) { - if (table.pair_is_static(record.pair_index)) { - // θ1 is a coordinate this path holds constant, hence one the carve-out - // removed from J(p), but one that certainly moves the pair. - record.qc[0] += delta; - ++tampered; - break; - } - } - ASSERT_EQ(tampered, 1); - EXPECT_FALSE(VerifyCertificate(checker, path, certificate)) - << "delta = " << delta; - } -} - -// --------------------------------------------------------------------------- -// 7. Search modes, finding caps and the node budget. -// --------------------------------------------------------------------------- - -GTEST_TEST(CertifierTest, CertifyAllReportsEveryViolation) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - // Sweeping θ1 from 0 to 3 rad passes the post (≈1.57 rad) and then the - // pillar (≈2.58 rad): two disjoint violating regions, different pairs. - const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(3.0, 0.0, 0.0), 1, 0.0, 1.0); - - const CertificationResult result = checker.CheckTrajectory(trajectory); - ASSERT_EQ(result.verdict, Verdict::kViolationFound); - ASSERT_GE(result.findings.size(), 2u); - for (size_t i = 1; i < result.findings.size(); ++i) { - EXPECT_LE(result.findings[i - 1].time, result.findings[i].time) - << "findings must be earliest-first"; - } - int definite = 0; - for (const Finding& finding : result.findings) { - if (finding.definite) { - ++definite; - EXPECT_LT(DistanceAtFinding(checker, finding), kMargin); - } - } - EXPECT_GE(definite, 2); - - Options capped = SerialOptions(); - capped.max_reported_findings = 1; - const CertificationResult capped_result = - checker.CheckTrajectory(trajectory, capped); - EXPECT_EQ(capped_result.verdict, Verdict::kViolationFound); - EXPECT_EQ(capped_result.findings.size(), 1u); - EXPECT_NEAR(capped_result.findings.front().time, result.findings.front().time, - 1e-12); -} - GTEST_TEST(CertifierTest, NodeBudgetExhausted) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); Eigen::MatrixXd waypoints(3, 4); waypoints.col(0) = MakeQ(0.0, 0.0, 0.0); waypoints.col(1) = MakeQ(0.3, -0.1, 0.03); @@ -792,150 +558,51 @@ GTEST_TEST(CertifierTest, NodeBudgetExhausted) { EXPECT_GE(result.findings.front().time, 0.0); } -// --------------------------------------------------------------------------- -// 8. Parallel smoke: same verdict and same witness as serial. -// --------------------------------------------------------------------------- - -GTEST_TEST(CertifierTest, ParallelMatchesSerialOnFreeTrajectory) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); - - const CertificationResult serial = checker.CheckTrajectory(trajectory); - Options parallel_options = SerialOptions(); - parallel_options.parallelism = Parallelism(4); - const CertificationResult parallel = - checker.CheckTrajectory(trajectory, parallel_options); - - EXPECT_EQ(serial.verdict, parallel.verdict); - EXPECT_EQ(parallel.verdict, Verdict::kCertifiedFree); - EXPECT_TRUE(parallel.findings.empty()); - // The same tree is explored either way; only the order differs. - EXPECT_EQ(serial.stats.nodes, parallel.stats.nodes); - EXPECT_EQ(serial.stats.narrowphase_queries, - parallel.stats.narrowphase_queries); -} - -GTEST_TEST(CertifierTest, ParallelMatchesSerialOnViolation) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); - - Options serial_options = SerialOptions(); - serial_options.mode = SearchMode::kFindFirstViolation; - Options parallel_options = serial_options; - parallel_options.parallelism = Parallelism(4); - - const CertificationResult serial = - checker.CheckTrajectory(trajectory, serial_options); - const CertificationResult parallel = - checker.CheckTrajectory(trajectory, parallel_options); - - ASSERT_EQ(serial.verdict, Verdict::kViolationFound); - ASSERT_EQ(parallel.verdict, Verdict::kViolationFound); - ASSERT_EQ(serial.findings.size(), 1u); - ASSERT_EQ(parallel.findings.size(), 1u); - // The earliest witness is deterministic across thread counts (the stats are - // not). - EXPECT_NEAR(serial.findings.front().time, parallel.findings.front().time, - 1e-12); - EXPECT_LT((serial.findings.front().q - parallel.findings.front().q) - .cwiseAbs() - .maxCoeff(), - 1e-12); -} - -GTEST_TEST(CertifierTest, ConcurrentChecksAreIndependent) { - // The Check* methods are const and documented thread-safe: concurrent calls - // must lease disjoint contexts from the pool. test/concurrency_test.cc - // sweeps that; this is the smoke test for the lease. - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - const BezierCurve free_trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3, 0.0, 1.0); - const BezierCurve bad_trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1, 0.0, 1.0); - - Options options = SerialOptions(); - options.parallelism = Parallelism(2); - std::vector verdicts(8); - std::vector threads; - for (int i = 0; i < 8; ++i) { - threads.emplace_back([&, i]() { - verdicts[i] = - (i % 2 == 0) - ? checker.CheckTrajectory(free_trajectory, options).verdict - : checker.CheckTrajectory(bad_trajectory, options).verdict; - }); - } - for (std::thread& thread : threads) thread.join(); - for (int i = 0; i < 8; ++i) { - EXPECT_EQ(verdicts[i], (i % 2 == 0) ? Verdict::kCertifiedFree - : Verdict::kViolationFound); - } -} - -// --------------------------------------------------------------------------- -// 9. Breakpoint semantics. -// --------------------------------------------------------------------------- - -GTEST_TEST(CertifierTest, ViolationExactlyAtStartTime) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - // q(t0) puts the arm straight into the post. - const BezierCurve trajectory = - MakeBezier(MakeQ(1.5708, 0.0, 0.0), MakeQ(0.5, 0.0, 0.0), 1, 0.0, 1.0); - - const CertificationResult result = checker.CheckTrajectory(trajectory); - ASSERT_EQ(result.verdict, Verdict::kViolationFound); - ASSERT_FALSE(result.findings.empty()); - const Finding& finding = result.findings.front(); - // Only the breakpoint pre-pass can produce a witness *exactly* at t0; node - // midpoints are strictly interior. - EXPECT_EQ(finding.time, 0.0); - EXPECT_TRUE(finding.definite); - EXPECT_EQ(finding.motion_bound, 0.0); - EXPECT_LT(DistanceAtFinding(checker, finding), kMargin); -} - -GTEST_TEST(CertifierTest, ViolationAtAJunctionIsReported) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - // A 3-waypoint path whose middle waypoint (the junction between segments, - // at t = 1) is inside the post. +GTEST_TEST(CertifierTest, BreakpointWitnessesAreReported) { + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + + // q(t0) puts the arm straight into the post. Only the breakpoint pre-pass can + // produce a witness *exactly* at t0; node midpoints are strictly interior. + const CertificationResult 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_FALSE(at_start.findings.empty()); + const Finding& first = at_start.findings.front(); + EXPECT_EQ(first.time, 0.0); + EXPECT_TRUE(first.definite); + EXPECT_EQ(first.motion_bound, 0.0); + EXPECT_LT(DistanceAtFinding(checker, first), kMargin); + + // A 3-waypoint path whose middle waypoint (the junction between segments, at + // t = 1) is inside the post: the pre-pass must report the junction + // configuration itself, not only interior node midpoints. 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 CertificationResult result = checker.CheckPath(waypoints); - ASSERT_EQ(result.verdict, Verdict::kViolationFound); + const CertificationResult at_junction = checker.CheckPath(waypoints); + ASSERT_EQ(at_junction.verdict, Verdict::kViolationFound); bool found_junction_witness = false; - for (const Finding& finding : result.findings) { + for (const Finding& finding : at_junction.findings) { if (finding.time == 1.0 && finding.definite && finding.motion_bound == 0.0) { found_junction_witness = true; EXPECT_LT(DistanceAtFinding(checker, finding), kMargin); } } - EXPECT_TRUE(found_junction_witness) - << "the breakpoint pre-pass must report the junction configuration " - "itself, not only interior node midpoints"; + EXPECT_TRUE(found_junction_witness); } // --------------------------------------------------------------------------- -// 9b. 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; this is the cheap standing guard that no -// kCertifiedFree of *this* driver survives dense sampling, and that every -// definite witness really violates. +// 7. 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 = MakeChecker(model, SerialOptions()); + const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); std::mt19937 rng(1234); std::uniform_real_distribution theta1(-3.0, 3.0); std::uniform_real_distribution theta2(-2.0, 2.0); @@ -976,56 +643,6 @@ GTEST_TEST(CertifierTest, RandomTrajectoriesAreSoundAgainstDenseSampling) { EXPECT_GT(violating, 0); } -// --------------------------------------------------------------------------- -// 10. API guardrails (the full suite lives in test/api_test.cc). -// --------------------------------------------------------------------------- - -GTEST_TEST(CertifierTest, ApiThrowsOnDimensionMismatch) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - - Eigen::MatrixXd wrong_rows(2, 3); - wrong_rows.setZero(); - EXPECT_THROW(checker.CheckPath(wrong_rows), std::exception); - - EXPECT_THROW(checker.CheckEdge(VectorXd::Zero(2), VectorXd::Zero(3)), - std::exception); - - Eigen::MatrixXd control_points(5, 2); - control_points.setZero(); - const BezierCurve wrong_trajectory(0.0, 1.0, control_points); - EXPECT_THROW(checker.CheckTrajectory(wrong_trajectory), std::exception); - - // A single waypoint is not a path. - Eigen::MatrixXd single(3, 1); - single.setZero(); - EXPECT_THROW(checker.CheckPath(single), std::exception); -} - -GTEST_TEST(CertifierTest, ApiThrowsOnBadOptions) { - const auto model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); - const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.1, 0.0, 0.0), 1, 0.0, 1.0); - - Options bad = SerialOptions(); - bad.min_interval = 0.0; - EXPECT_THROW(checker.CheckTrajectory(trajectory, bad), std::exception); - - bad = SerialOptions(); - bad.max_reported_findings = 0; - EXPECT_THROW(checker.CheckTrajectory(trajectory, bad), std::exception); - - bad = SerialOptions(); - bad.query_tolerance = -1.0; - EXPECT_THROW(checker.CheckTrajectory(trajectory, bad), std::exception); -} - -GTEST_TEST(CertifierTest, ConstructorRejectsNullModel) { - ContinuousCollisionChecker::Params params; - EXPECT_THROW(ContinuousCollisionChecker{params}, std::exception); -} - } // namespace } // namespace continuous_collision } // namespace planning diff --git a/planning/continuous_collision/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc index 020fa227d929..1654bbba5d94 100644 --- a/planning/continuous_collision/test/concurrency_test.cc +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -1,30 +1,20 @@ -// Concurrency determinism. -// -// Four claims are pinned here, on the fixed corpus of ten random cases (a mix -// of free and violating) that concurrency_test_utilities.h builds, plus the -// deep workload it derives from that corpus: +// 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} in both search -// modes, and in kCertifyAll so are `nodes` and `narrowphase_queries`: -// the parallel driver explores the same tree in a different order. (In +// witness are identical at Parallelism {1, 2, 8, 16} in both search modes, +// and in kCertifyAll so are `nodes` and `narrowphase_queries`. (In // kFindFirstViolation the branch-and-bound bound arrives at different -// times, so the statistics are not deterministic; the reported witness -// still is.) -// 2. Serial mode is bit-deterministic: two runs produce byte-identical -// findings and statistics. -// 3. The public Check* methods are safe to call concurrently on one checker -// instance: eight threads sharing one checker get the same answers as -// the same calls made one after another. +// times, so the statistics are not deterministic; the witness still is.) +// 2. Serial mode is bit-deterministic. +// 3. The public Check* methods are safe to call concurrently on one instance. // 4. 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 findings at every thread count, and keeps doing so when several -// callers ask for it at once. +// same findings 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; the timing claims live in concurrency_timing_test.cc. -// -// This is the test to run under ThreadSanitizer: +// every build flavor. This is the test to run under ThreadSanitizer: // // bazel test --config=tsan //planning/continuous_collision:concurrency_test // @@ -32,8 +22,7 @@ // 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 -// (TSAN_OPTIONS=suppressions=...). +// real bug; one rooted entirely in Drake belongs in a suppression file. #include #include @@ -45,7 +34,7 @@ #include #include "drake/common/parallelism.h" -#include "drake/planning/continuous_collision/test/concurrency_test_utilities.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" namespace drake { namespace planning { @@ -53,10 +42,6 @@ namespace continuous_collision { namespace test { namespace { -// --------------------------------------------------------------------------- -// 1. The answer does not depend on the thread count. -// --------------------------------------------------------------------------- - GTEST_TEST(ConcurrencyTest, CorpusIsBalanced) { const auto& corpus = Corpus(); ASSERT_EQ(static_cast(corpus.size()), kNumCases); @@ -92,10 +77,9 @@ GTEST_TEST(ConcurrencyTest, VerdictAndEarliestWitnessAreThreadCountInvariant) { } GTEST_TEST(ConcurrencyTest, CertifyAllIsFullyThreadCountInvariant) { - // In kCertifyAll every node's decision depends only on its own control points - // and inherited active set, so the *whole* tree, and therefore every - // statistic and every finding, is thread-count independent, not just the - // earliest witness. + // In kCertifyAll every node's decision depends only on its own control + // points and inherited active set, so the *whole* tree, and therefore every + // statistic and every finding, is thread-count independent. for (const auto& entry : Corpus()) { const BezierCurve trajectory = entry->trajectory(); const CertificationResult serial = entry->checker->CheckTrajectory( @@ -120,8 +104,7 @@ GTEST_TEST(ConcurrencyTest, FindFirstViolationStatisticsAreAllowedToDiffer) { // The complement of the test above, pinned so that a future reader does not // "fix" an expected statistics mismatch: under branch-and-bound the number of // nodes a run visits depends on when the atomic bound tightens, which depends - // on timing. Only the answer is deterministic, so the assertion is on the - // *witness* and the statistics are merely reported. + // on timing. Only the *witness* is deterministic. int cases_with_differing_stats = 0; int examined = 0; for (const auto& entry : Corpus()) { @@ -151,10 +134,6 @@ GTEST_TEST(ConcurrencyTest, FindFirstViolationStatisticsAreAllowedToDiffer) { "them.\n\n"; } -// --------------------------------------------------------------------------- -// 2. Serial mode is bit-deterministic. -// --------------------------------------------------------------------------- - GTEST_TEST(ConcurrencyTest, SerialModeIsBitDeterministic) { for (const auto& entry : Corpus()) { for (const SearchMode mode : @@ -178,10 +157,6 @@ GTEST_TEST(ConcurrencyTest, SerialModeIsBitDeterministic) { } } -// --------------------------------------------------------------------------- -// 3. Concurrent Check* calls on one checker instance. -// --------------------------------------------------------------------------- - GTEST_TEST(ConcurrencyTest, ConcurrentCallsOnOneCheckerMatchSequential) { // Every worker hits the *same* checker object, so they contend for the // construction-time context pool; the lease must hand each call its own @@ -247,8 +222,8 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { entry.checker->CheckPath(waypoints, options); const MotionBoundTable table_expected = entry.checker->ComputeMotionBounds( entry.checker->Normalize(entry.trajectory(), options)); - // Snapshot every λ entry, not just the CSR's size: the row layout is fixed by - // topology and would survive any amount of corruption in the coefficients. + // Snapshot every lambda entry, not just the CSR's size: the row layout is + // fixed by topology and would survive any amount of coefficient corruption. std::vector>> lambda_expected; std::vector slack_expected; for (int p = 0; p < table_expected.num_pairs(); ++p) { @@ -289,7 +264,7 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { } for (int p = 0; p < table.num_pairs(); ++p) { if (table.GetEntries(p) != lambda_expected[p]) ++mismatches[t]; - // The carve-out residual is part of Δ_p, so it has to be + // The carve-out residual is part of Delta_p, so it has to be // bit-identical across threads too. if (table.carveout_slack(p) != slack_expected[p]) ++mismatches[t]; } @@ -300,31 +275,24 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { for (int t = 0; t < kThreads; ++t) EXPECT_EQ(mismatches[t], 0); } -// --------------------------------------------------------------------------- -// 4. The deep workload explores the same tree at every thread count. -// --------------------------------------------------------------------------- -// // The sharing path only ever runs on a workload big enough to hire a helper, -// which the corpus cases of claims 1-3 never are. These cases are where it gets -// its coverage, TSan's included, and they are equalities, so unlike the -// wall-clock claims in concurrency_timing_test.cc they run everywhere. +// which the corpus cases above never are. The three tests below are where it +// gets its coverage, TSan's included. GTEST_TEST(ConcurrencyTest, DeepWorkloadIsBigEnoughToBeWorthSpreading) { - // Without this the two tests below could silently degenerate into measuring - // a handful of nodes if the corpus or the bisection ever drifted. + // Without this the two tests below could silently degenerate into measuring a + // handful of nodes if the corpus or the bisection ever drifted. const DeepWorkload& deep = Deep(); ASSERT_NE(deep.entry, nullptr); EXPECT_GE(deep.nodes, kMinDeepNodes) << "grazing margin " << deep.margin; + EXPECT_GE(deep.max_depth, kMinDeepDepth) << "grazing margin " << deep.margin; std::cout << "\n[ concurrency ] deep workload: " << deep.entry->name << ", margin " << deep.margin << ", " << deep.nodes - << " nodes at min_interval " << deep.min_interval << "\n\n"; + << " nodes, depth " << deep.max_depth << ", at min_interval " + << deep.min_interval << "\n\n"; } GTEST_TEST(ConcurrencyTest, DeepWorkloadIsThreadCountInvariant) { - // The scaling test below only proves work moved between threads; this proves - // the *same* work moved. It runs in every build, sanitizers included, and is - // where the sharing path gets its TSan coverage, because the corpus cases of - // the tests above are too small to ever hire a helper. const DeepWorkload& deep = Deep(); ASSERT_NE(deep.entry, nullptr); const BezierCurve trajectory = deep.entry->trajectory(); @@ -348,17 +316,15 @@ GTEST_TEST(ConcurrencyTest, DeepWorkloadIsThreadCountInvariant) { 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 pool as well as - // its context pool, and the case where a reservation returning fewer threads - // than asked for is the normal outcome rather than an edge case. + // makes concurrent calls contend for the checker's worker pool as well as its + // context pool, and the case where a reservation returning fewer threads than + // asked for is the normal outcome rather than an edge case. const DeepWorkload& deep = Deep(); ASSERT_NE(deep.entry, nullptr); const CertificationResult expected = deep.entry->checker->CheckTrajectory( deep.entry->trajectory(), deep.options(Parallelism::None())); constexpr int kThreads = 4; - // gtest assertions are not safe off the main thread, so each worker counts - // its own mismatches and the main thread asserts after the join. std::vector mismatches(kThreads, 0); std::vector threads; for (int t = 0; t < kThreads; ++t) { diff --git a/planning/continuous_collision/test/concurrency_test_utilities.h b/planning/continuous_collision/test/concurrency_test_utilities.h deleted file mode 100644 index 8d79dc0f5152..000000000000 --- a/planning/continuous_collision/test/concurrency_test_utilities.h +++ /dev/null @@ -1,386 +0,0 @@ -#pragma once - -// The shared fixture of the two concurrency targets: the random corpus that -// `concurrency_test.cc` pins the driver's determinism against, and the deep -// workload that both it and `concurrency_timing_test.cc` need. Each target -// builds its own copy lazily, so the fixture lives in a header rather than -// duplicating three hundred lines of world generation between the two files. -// -// Nothing here asserts; the claims live in the two test files. - -#include -#include -#include -#include -#include -#include - -#include - -#include "drake/common/parallelism.h" -#include "drake/common/trajectories/bezier_curve.h" -#include "drake/geometry/shape_specification.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/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::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 Eigen::Vector3d; -using Eigen::VectorXd; - -constexpr double kMargin = 0.005; -// Ten cases keeps the full 4-thread-count × 2-mode sweep (80 certification -// runs) plus the concurrent-call test under a second in Release, which is what -// makes this affordable to run again under TSan (~100× slower). -constexpr int kNumCases = 10; -constexpr int kMinFreeCases = 3; -constexpr int kMinViolatingCases = 3; - -inline CoulombFriction Friction() { - return CoulombFriction(1.0, 1.0); -} - -inline SpatialInertia Inertia() { - return SpatialInertia::SolidSphereWithMass(1.0, 0.05); -} - -// A four-link chain of revolute and prismatic joints with primitive geometry, -// four anchored obstacles and (on odd seeds) a HalfSpace floor, so the corpus -// exercises the native narrowphase route and the analytic one. -inline std::unique_ptr> MakeWorld(uint64_t seed) { - std::mt19937_64 rng(seed); - const auto uniform = [&rng](double lo, double hi) { - return std::uniform_real_distribution(lo, hi)(rng); - }; - // Every helper below sequences its draws through named locals: the order in - // which a compiler evaluates sibling constructor or operator arguments is - // unspecified, so drawing inline would make the corpus toolchain-dependent - // and could silently shift the free/violating balance this file relies on. - const auto vector3 = [&uniform](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); - }; - const auto direction = [&vector3]() { - Vector3d v; - do { - v = vector3(-1, 1); - } while (v.norm() < 1e-3 || v.norm() > 1.0); - return v.normalized(); - }; - const auto offset = [&direction, &uniform](double lo, double hi) { - const Vector3d unit = direction(); - const double length = uniform(lo, hi); - return Vector3d(unit * length); - }; - const auto pose = [&vector3, &offset](double lo, double hi) { - const Vector3d rpy = vector3(-3, 3); - const Vector3d p = offset(lo, hi); - return RigidTransformd(RollPitchYawd(rpy), p); - }; - - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - std::vector*> links; - for (int i = 0; i < 4; ++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 = vector3(-0.5, 0.5); - const RigidTransformd X_PF(RollPitchYawd(rpy_PF), offset(0.22, 0.32)); - const Vector3d axis = direction(); - if (i == 2) { - plant.AddJoint("j" + std::to_string(i), parent, X_PF, - body, RigidTransformd(), axis); - } else { - plant.AddJoint("j" + std::to_string(i), parent, X_PF, body, - RigidTransformd(), axis); - } - const RigidTransformd X_LG(offset(0.10, 0.16)); - if (i % 2 == 0) { - const double radius = uniform(0.02, 0.04); - const double length = uniform(0.05, 0.10); - plant.RegisterCollisionGeometry(body, X_LG, Capsule(radius, length), - name + "_geom", Friction()); - } else { - const Vector3d size = vector3(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 < 4; ++i) { - const std::string name = "obstacle" + std::to_string(i); - const RigidBody& body = plant.AddRigidBody(name, Inertia()); - plant.WeldFrames(plant.world_frame(), body.body_frame(), pose(0.30, 0.75)); - if (i % 3 == 0) { - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Sphere(uniform(0.05, 0.12)), - name + "_geom", Friction()); - } else if (i % 3 == 1) { - const Vector3d size = vector3(0.08, 0.20); - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Box(size.x(), size.y(), size.z()), - name + "_geom", Friction()); - } else { - const double radius = uniform(0.04, 0.09); - const double length = uniform(0.08, 0.18); - plant.RegisterCollisionGeometry(body, RigidTransformd(), - Cylinder(radius, length), name + "_geom", - Friction()); - } - } - if (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(); -} - -// A quintic Bézier 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) { - std::mt19937_64 rng(seed ^ 0xa5a5'5a5a'0f0f'f0f0ull); - std::uniform_real_distribution value(-1.4, 1.4); - Eigen::MatrixXd points(num_positions, 6); - for (int j = 0; j < 6; ++j) { - for (int i = 0; i < num_positions; ++i) points(i, j) = value(rng); - } - return points; -} - -inline Options BaseOptions(Parallelism parallelism, SearchMode mode) { - Options options; - options.margin = kMargin; - options.parallelism = parallelism; - options.mode = mode; - // Bounded cost per run: the whole sweep is executed 8 times per case. - options.min_interval = 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 = MakeWorld(seed); - ContinuousCollisionChecker::Params params; - params.model = entry->model; - params.default_options = - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); - entry->checker = std::make_unique(params); - entry->control_points = - MakeControlPoints(seed, entry->model->plant().num_positions()); - const CertificationResult result = entry->checker->CheckTrajectory( - entry->trajectory(), - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); - entry->serial_verdict = result.verdict; - // Keep the corpus balanced: stop taking more of whichever kind is - // already well represented. - const bool is_free = result.verdict == Verdict::kCertifiedFree; - const bool is_violating = result.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 findings. 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 FindingsIdentical( - const std::vector& a, const std::vector& b) { - if (a.size() != b.size()) { - return ::testing::AssertionFailure() - << "finding counts differ: " << a.size() << " vs " << b.size(); - } - for (std::size_t i = 0; i < a.size(); ++i) { - if (a[i].time != b[i].time) { - return ::testing::AssertionFailure() - << "finding " << i << " time " << a[i].time << " vs " << b[i].time; - } - if (a[i].q.size() != b[i].q.size() || - !(a[i].q.array() == b[i].q.array()).all()) { - return ::testing::AssertionFailure() - << "finding " << i << " witness configuration differs"; - } - if (a[i].pair.a != b[i].pair.a || a[i].pair.b != b[i].pair.b) { - return ::testing::AssertionFailure() - << "finding " << i << " pair differs"; - } - if (a[i].distance != b[i].distance || - a[i].motion_bound != b[i].motion_bound || - a[i].definite != b[i].definite) { - return ::testing::AssertionFailure() - << "finding " << i << " payload differs"; - } - if (a[i].nearest_a_W.has_value() != b[i].nearest_a_W.has_value() || - (a[i].nearest_a_W.has_value() && - *a[i].nearest_a_W != *b[i].nearest_a_W)) { - return ::testing::AssertionFailure() - << "finding " << i << " witness point A differs"; - } - if (a[i].nearest_b_W.has_value() != b[i].nearest_b_W.has_value() || - (a[i].nearest_b_W.has_value() && - *a[i].nearest_b_W != *b[i].nearest_b_W)) { - return ::testing::AssertionFailure() - << "finding " << i << " witness point B differs"; - } - } - return ::testing::AssertionSuccess(); -} - -inline ::testing::AssertionResult EarliestWitnessIdentical( - const CertificationResult& a, const CertificationResult& b) { - if (a.findings.empty() != b.findings.empty()) { - return ::testing::AssertionFailure() - << "one run reported findings and the other did not"; - } - if (a.findings.empty()) return ::testing::AssertionSuccess(); - return FindingsIdentical({a.findings.front()}, {b.findings.front()}); -} - -// The bisection's node budget in Deep() doubles as the deep workload's size: -// the margin it converges to is the largest one still certifiable inside this -// budget, so the tree it produces has just under this many nodes. Large enough -// that a run takes tens of milliseconds (a wall-clock ratio then means -// something) and that no fixed seeding depth could ever have covered it; small -// enough that the ~40 probes that find it, and the timed repetitions -// concurrency_timing_test.cc runs on it, stay cheap, sanitizers included. -// -// 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 = 3000; - -// 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 -// ϕ̂ − τ − Δ > m, so as the threshold m 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: the largest -// margin still certifiable within kProbeBudget nodes is by construction the -// one that costs about kProbeBudget nodes. -struct DeepWorkload { - const Case* entry{}; - double margin{0.0}; - double min_interval{1e-8}; - uint64_t nodes{0}; - - Options options(Parallelism parallelism) const { - Options options = BaseOptions(parallelism, SearchMode::kCertifyAll); - options.margin = margin; - options.min_interval = min_interval; - 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; - options.max_nodes = kProbeBudget; - return deep->entry->checker - ->CheckTrajectory(deep->entry->trajectory(), options) - .verdict == Verdict::kCertifiedFree; - }; - 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->nodes = deep->entry->checker - ->CheckTrajectory(deep->entry->trajectory(), - deep->options(Parallelism::None())) - .stats.nodes; - return deep; - }(); - return *workload; -} - -} // namespace test -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/test/concurrency_timing_test.cc b/planning/continuous_collision/test/concurrency_timing_test.cc deleted file mode 100644 index 2ed7a86bbc5b..000000000000 --- a/planning/continuous_collision/test/concurrency_timing_test.cc +++ /dev/null @@ -1,150 +0,0 @@ -// The two per-call parallel *scaling* claims, split out of concurrency_test.cc -// because they are wall-clock claims and it is not. -// -// Every case in concurrency_test.cc is an equality and runs under every build -// flavor. A duration, by contrast, means nothing under an instrumented build: -// Valgrind serializes threads outright, so `parallel < serial` inverts and the -// case fails for a reason that has nothing to do with the driver. Hence the -// separate target, which carries disable_in_compilation_mode_dbg and the -// no_valgrind_tools tag, and hence TimingClaimsAreMeaningless() below, which -// skips whatever the build tags did not already exclude. -// -// The corpus, the deep workload and the option defaults are shared with -// concurrency_test.cc through concurrency_test_utilities.h. - -#include -#include -#include -#include -#include -#include - -#include - -#include "drake/common/parallelism.h" -#include "drake/planning/continuous_collision/test/concurrency_test_utilities.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace test { -namespace { - -// Per-call parallel scaling: the two properties the driver in -// certifier_internal.cc exists for. -// -// a) a deep tree inside a single segment spreads over the workers, instead -// of sitting behind one fixed seed that no other worker can split; -// b) a check too small to pay for workers never loses by being asked for -// them, which matters because Parallelism::Max() is the *default* value -// of Options::parallelism. -// -// Both are timing claims, so both are written to survive a loaded machine: a -// ratio with a wide margin, best-of-three, and a skip when the hardware or the -// build cannot support the claim at all. They are regression detectors rather -// than benchmarks, and should only ever fire on a driver that has stopped -// distributing work. - -// True when the build cannot support a meaningful wall-clock claim: a sanitizer -// build serializes and inflates everything, an unoptimized build changes the -// ratios, and fewer than eight hardware threads means there is no parallelism -// to measure. -// -// The compile-time tests below only see the sanitizers this translation unit -// was itself instrumented with. Valgrind instruments nothing at compile time, -// and a sanitizer runtime linked in from elsewhere is equally invisible, so the -// environment is consulted too: the tools that make a duration meaningless all -// announce themselves through an options variable. That is the same test -// limit_malloc.cc uses to disarm itself, and the same VALGRIND_OPTS check -// gcs_trajectory_optimization_test.cc uses. -bool TimingClaimsAreMeaningless() { -#if defined(__SANITIZE_THREAD__) || defined(__SANITIZE_ADDRESS__) - return true; -#elif defined(__has_feature) -#if __has_feature(thread_sanitizer) || __has_feature(address_sanitizer) - return true; -#endif -#endif -#ifndef NDEBUG - return true; -#else - for (const char* variable : {"VALGRIND_OPTS", "ASAN_OPTIONS", "LSAN_OPTIONS", - "TSAN_OPTIONS", "UBSAN_OPTIONS"}) { - if (std::getenv(variable) != nullptr) return true; - } - return std::thread::hardware_concurrency() < 8; -#endif -} - -template -double BestOfThreeSeconds(F&& body) { - body(); // Warm up: first-touch page faults, Drake's own lazy caches. - double best = std::numeric_limits::infinity(); - for (int i = 0; i < 3; ++i) { - const auto start = std::chrono::steady_clock::now(); - body(); - best = std::min(best, std::chrono::duration( - std::chrono::steady_clock::now() - start) - .count()); - } - return best; -} - -GTEST_TEST(ConcurrencyTest, DeepWorkloadIsFasterInParallel) { - if (TimingClaimsAreMeaningless()) GTEST_SKIP(); - const DeepWorkload& deep = Deep(); - ASSERT_NE(deep.entry, nullptr); - const BezierCurve trajectory = deep.entry->trajectory(); - const Options serial_options = deep.options(Parallelism::None()); - const Options parallel_options = deep.options(Parallelism(8)); - - const double serial = BestOfThreeSeconds([&]() { - deep.entry->checker->CheckTrajectory(trajectory, serial_options); - }); - const double parallel = BestOfThreeSeconds([&]() { - deep.entry->checker->CheckTrajectory(trajectory, parallel_options); - }); - std::cout << "\n[ concurrency ] deep workload: serial " << 1e3 * serial - << " ms, Parallelism(8) " << 1e3 * parallel << " ms (" - << serial / parallel << "x)\n\n"; - // 1.43x is the bound that separates a driver that distributes deep work from - // one that leaves it on a single worker, without being a performance - // assertion in disguise. - EXPECT_LT(parallel, 0.7 * serial); -} - -GTEST_TEST(ConcurrencyTest, SmallCheckIsNotSlowerInParallel) { - if (TimingClaimsAreMeaningless()) GTEST_SKIP(); - // A two-waypoint edge in one of the corpus worlds is the small check: a - // handful of nodes, dominated by the serial breakpoint pass. Asked for the - // default Parallelism::Max(), the driver must decline to hire anyone rather - // than pay a worker-startup bill several times the size of the work. - const Case& entry = *Corpus().front(); - const VectorXd q1 = entry.control_points.col(0); - const VectorXd q2 = entry.control_points.rightCols(1); - const Options serial_options = - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); - const Options parallel_options = - BaseOptions(Parallelism::Max(), SearchMode::kCertifyAll); - ASSERT_LT(entry.checker->CheckEdge(q1, q2, serial_options).stats.nodes, 100u); - - const double serial = BestOfThreeSeconds([&]() { - entry.checker->CheckEdge(q1, q2, serial_options); - }); - const double parallel = BestOfThreeSeconds([&]() { - entry.checker->CheckEdge(q1, q2, parallel_options); - }); - std::cout << "\n[ concurrency ] small check: serial " << 1e3 * serial - << " ms, Parallelism::Max() " << 1e3 * parallel << " ms (" - << serial / parallel << "x)\n\n"; - // The driver never hires for a check this small, so the two paths run the - // same code and parity is what to expect; the 1.5x bound leaves room for - // scheduler noise on a loaded machine without admitting a real slowdown. - EXPECT_LT(parallel, 1.5 * serial); -} - -} // 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 index 1ba50ae02517..9de118253609 100644 --- a/planning/continuous_collision/test/distance_oracle_test.cc +++ b/planning/continuous_collision/test/distance_oracle_test.cc @@ -9,19 +9,21 @@ #include #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/optimization/vpolytope.h" @@ -69,6 +71,7 @@ using drake::planning::RobotDiagramBuilder; using drake::systems::Context; using Eigen::Matrix3Xd; using Eigen::Vector3d; +using ::testing::HasSubstr; constexpr double kTau = 1e-6; // Exactness bar for the analytic halfspace fallback and for round trips that @@ -309,47 +312,69 @@ InMemoryMesh LPrismMesh() { // Accuracy vs analytic ground truth (native route). // ========================================================================== -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(), kTau); +// 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); - - std::mt19937 rng(20260826); + int separated = 0; int penetrating = 0; for (int trial = 0; trial < 250; ++trial) { - const RigidTransformd X_WA = RandomPose(&rng, 0.4); - const RigidTransformd X_WB = RandomPose(&rng, 0.4); - world.SetPose(body_a, X_WA); - world.SetPose(body_b, X_WB); + 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); - // Exact for two spheres on both branches. - const double expected = - (X_WB.translation() - X_WA.translation()).norm() - r_a - r_b; - EXPECT_NEAR(phi, expected, kNative) << "trial " << trial; - if (phi < 0.0) ++penetrating; + 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(), kTau); + + 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(); @@ -361,42 +386,23 @@ GTEST_TEST(DistanceOracleAccuracy, SphereBoxMatchesAnalyticDistance) { 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(), kTau); - ASSERT_EQ(oracle.pairs().size(), 1u); - const PairRecord& pair = oracle.pairs().front(); std::mt19937 rng(881); - int separated = 0; - int penetrating = 0; - for (int trial = 0; trial < 250; ++trial) { - const RigidTransformd X_WS = RandomPose(&rng, 0.5); - const RigidTransformd X_WB = RandomPose(&rng, 0.5); - world.SetPose(sphere_body, X_WS); - world.SetPose(box_body, X_WB); - - const Vector3d p_B = X_WB.inverse() * X_WS.translation(); - const double center_distance = PointBoxDistance(p_B, half); - if (center_distance <= 0.0) continue; // center inside the box - // 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. - const double expected = center_distance - radius; - - 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, expected, 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); + // 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); } // ========================================================================== @@ -457,11 +463,7 @@ class HalfSpaceFallbackTest : public ::testing::Test { TEST_F(HalfSpaceFallbackTest, EveryPartnerMatchesHandDerivedFormulaExactly) { using Reference = std::function; - struct Partner { - std::string name; - Reference reference; - }; - const std::vector partners = { + const std::vector> partners = { {"sphere", [](const Vector3d& n, const Vector3d& p0, const RigidTransformd& X) { return HalfSpaceSphere(n, p0, X, kRadius); @@ -495,9 +497,9 @@ TEST_F(HalfSpaceFallbackTest, EveryPartnerMatchesHandDerivedFormulaExactly) { const RigidTransformd X_WH = RandomPose(&rng, 0.3); world_->SetPose(Body("halfspace"), X_WH); std::vector poses; - for (const Partner& p : partners) { + for (const auto& [name, reference] : partners) { poses.push_back(RandomPose(&rng, 0.4)); - world_->SetPose(Body(p.name), poses.back()); + world_->SetPose(Body(name), poses.back()); } const QueryObject& query = world_->query(); const Vector3d n_W = X_WH.rotation().matrix().col(2); @@ -506,25 +508,20 @@ TEST_F(HalfSpaceFallbackTest, EveryPartnerMatchesHandDerivedFormulaExactly) { for (size_t i = 0; i < partners.size(); ++i) { const PairRecord& pair = FindPair(*oracle_, halfspace_id_, - GeometryOf(world_->plant(), partners[i].name)); - ASSERT_NE(pair.route, DistanceRoute::kNative) << partners[i].name; + 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); - const double expected = partners[i].reference(n_W, p0_W, poses[i]); - EXPECT_NEAR(phi, expected, kExact) - << partners[i].name << ", trial " << trial; + 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.id.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); - if (phi > 0.0) { - ++positive; - } else { - ++negative; - } + (phi > 0.0 ? positive : negative) += 1; } } EXPECT_GT(positive, 0); @@ -550,16 +547,13 @@ TEST_F(HalfSpaceFallbackTest, ReportNamesEveryCombinationAndRoute) { const std::string report = oracle_->support_report(); SCOPED_TRACE(report); // Rows are ordered by shape class, so HalfSpace is always the second name. - EXPECT_NE(report.find("Sphere-HalfSpace"), std::string::npos); - EXPECT_NE(report.find("Box-HalfSpace"), std::string::npos); - EXPECT_NE(report.find("Capsule-HalfSpace"), std::string::npos); - EXPECT_NE(report.find("Cylinder-HalfSpace"), std::string::npos); - EXPECT_NE(report.find("Ellipsoid-HalfSpace"), std::string::npos); - EXPECT_NE(report.find("Convex-HalfSpace"), std::string::npos); - EXPECT_NE(report.find("halfspace analytic support-function fallback"), - std::string::npos); - EXPECT_NE(report.find("native (ComputeSignedDistancePairClosestPoints"), - std::string::npos); + for (const char* row : + {"Sphere-HalfSpace", "Box-HalfSpace", "Capsule-HalfSpace", + "Cylinder-HalfSpace", "Ellipsoid-HalfSpace", "Convex-HalfSpace", + "halfspace analytic support-function fallback", + "native (ComputeSignedDistancePairClosestPoints"}) { + EXPECT_THAT(report, HasSubstr(row)); + } } // ========================================================================== @@ -626,13 +620,10 @@ TEST_F(AllShapesTest, ProbeClassifiesEveryPairSnapshot) { TEST_F(AllShapesTest, ReportAnnouncesMeshAsConvexHull) { const std::string report = oracle_->support_report(); SCOPED_TRACE(report); - EXPECT_NE(report.find("Mesh mesh_geometry: certified as its convex hull"), - std::string::npos); - EXPECT_NE(report.find("distinct shape-type combination(s)"), - std::string::npos); + EXPECT_THAT(report, + HasSubstr("Mesh mesh_geometry: certified as its convex hull")); // 8 distinct classes, each present once: 8*7/2 = 28 combinations. - EXPECT_NE(report.find("28 distinct shape-type combination(s)"), - std::string::npos); + EXPECT_THAT(report, HasSubstr("28 distinct shape-type combination(s)")); } TEST_F(AllShapesTest, WitnessPointsAreConsistentForSeparatedNativePairs) { @@ -655,17 +646,28 @@ TEST_F(AllShapesTest, WitnessPointsAreConsistentForSeparatedNativePairs) { EXPECT_GT(checked, 100); } -TEST_F(AllShapesTest, IdOrderingIsSymmetricForNativePairs) { +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 checked = 0; + int native = 0; + int halfspace = 0; for (const PairRecord& pair : oracle_->pairs()) { - if (pair.route != DistanceRoute::kNative) continue; PairRecord swapped = pair; std::swap(swapped.id.a, swapped.id.b); std::swap(swapped.id.body_a, swapped.id.body_b); + if (pair.route == DistanceRoute::kNative) { + ++native; + } else { + ++halfspace; + swapped.route = (pair.route == DistanceRoute::kHalfSpaceA) + ? DistanceRoute::kHalfSpaceB + : DistanceRoute::kHalfSpaceA; + } Vector3d a1; Vector3d b1; @@ -677,69 +679,9 @@ TEST_F(AllShapesTest, IdOrderingIsSymmetricForNativePairs) { EXPECT_EQ(phi, phi_swapped); EXPECT_EQ(a1, b2); EXPECT_EQ(b1, a2); - ++checked; - } - EXPECT_EQ(checked, kNativePairs); -} - -TEST_F(AllShapesTest, IdOrderingIsSymmetricForHalfSpacePairs) { - std::mt19937 rng(778); - world_->RandomizeAll(&rng, 0.5); - const QueryObject& query = world_->query(); - - int checked = 0; - for (const PairRecord& pair : oracle_->pairs()) { - if (pair.route == DistanceRoute::kNative) continue; - PairRecord swapped = pair; - std::swap(swapped.id.a, swapped.id.b); - std::swap(swapped.id.body_a, swapped.id.body_b); - 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); - ++checked; } - EXPECT_EQ(checked, kDynamicBodies); -} - -// Records which (shape, shape) combinations Drake's native narrowphase supports -// on the pinned build. The oracle routes every halfspace pair through the -// analytic fallback because of the rows this test prints. -TEST_F(AllShapesTest, NativeSupportTableSnapshot) { - const auto& inspector = world_->diagram().scene_graph().model_inspector(); - const QueryObject& query = world_->query(); - std::string table = - "Native ComputeSignedDistancePairClosestPoints support:\n"; - int supported = 0; - int threw = 0; - for (const PairRecord& pair : oracle_->pairs()) { - const std::string combo = - std::string(inspector.GetShape(pair.id.a).type_name()) + "-" + - std::string(inspector.GetShape(pair.id.b).type_name()); - try { - query.ComputeSignedDistancePairClosestPoints(pair.id.a, pair.id.b); - table += " " + combo + ": supported\n"; - ++supported; - } catch (const std::exception&) { - table += " " + combo + ": THROWS\n"; - ++threw; - } - } - std::cout << table << std::flush; - EXPECT_EQ(supported + threw, static_cast(oracle_->pairs().size())); - // All non-halfspace combinations must work natively, which is what the - // capability probe asserted at construction. - EXPECT_GE(supported, kNativePairs); + EXPECT_EQ(native, kNativePairs); + EXPECT_EQ(halfspace, kDynamicBodies); } GTEST_TEST(DistanceOracleProbe, HalfSpaceHalfSpacePairThrowsAtConstruction) { @@ -751,16 +693,13 @@ GTEST_TEST(DistanceOracleProbe, HalfSpaceHalfSpacePairThrowsAtConstruction) { AddShapeBody(&plant, "ceiling", HalfSpace(), Vector3d(0, 0, 2)); World world(builder.Build()); - try { - const DistanceOracle oracle(world.diagram(), kTau); - ADD_FAILURE() << "expected the capability probe to refuse the pair"; - } catch (const std::exception& e) { - const std::string what = e.what(); - SCOPED_TRACE(what); - EXPECT_NE(what.find("HalfSpace"), std::string::npos); - EXPECT_NE(what.find("ground_geometry"), std::string::npos); - EXPECT_NE(what.find("ceiling_geometry"), std::string::npos); - } + // Both geometries must be named, in whichever order the candidate set has + // them. + DRAKE_EXPECT_THROWS_MESSAGE( + DistanceOracle(world.diagram(), kTau), + "[\\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) { @@ -799,15 +738,8 @@ GTEST_TEST(DistanceOracleProbe, DeformableGeometryIsRefusedByName) { drake::multibody::fem::DeformableBodyConfig{}, 0.05); World world(builder.Build()); - try { - const DistanceOracle oracle(world.diagram(), kTau); - ADD_FAILURE() << "expected the probe to refuse the deformable geometry"; - } catch (const std::exception& e) { - const std::string what = e.what(); - SCOPED_TRACE(what); - EXPECT_NE(what.find("deformable"), std::string::npos); - EXPECT_NE(what.find("squishy"), std::string::npos); - } + DRAKE_EXPECT_THROWS_MESSAGE(DistanceOracle(world.diagram(), kTau), + "[\\s\\S]*deformable[\\s\\S]*squishy[\\s\\S]*"); } GTEST_TEST(DistanceOracleProbe, EmptyWorldProbesCleanly) { @@ -815,8 +747,7 @@ GTEST_TEST(DistanceOracleProbe, EmptyWorldProbesCleanly) { World world(builder.Build()); const DistanceOracle oracle(world.diagram(), kTau); EXPECT_TRUE(oracle.pairs().empty()); - EXPECT_NE(oracle.support_report().find("0 unfiltered pair(s)"), - std::string::npos); + EXPECT_THAT(oracle.support_report(), HasSubstr("0 unfiltered pair(s)")); } // ========================================================================== @@ -838,11 +769,11 @@ GTEST_TEST(DistanceOracleMesh, MeshDistanceEqualsConvexHullDistance) { World world(builder.Build()); const DistanceOracle oracle(world.diagram(), kTau); - const GeometryId mesh_id = GeometryOf(world.plant(), "mesh"); - const GeometryId convex_id = GeometryOf(world.plant(), "convex"); const GeometryId probe_id = GeometryOf(world.plant(), "probe"); - const PairRecord& mesh_pair = FindPair(oracle, mesh_id, probe_id); - const PairRecord& convex_pair = FindPair(oracle, convex_id, probe_id); + 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; @@ -856,13 +787,9 @@ GTEST_TEST(DistanceOracleMesh, MeshDistanceEqualsConvexHullDistance) { const QueryObject& query = world.query(); const double phi_mesh = oracle.SignedDistance(query, mesh_pair); - const double phi_convex = oracle.SignedDistance(query, convex_pair); - EXPECT_NEAR(phi_mesh, phi_convex, kExact) << "trial " << trial; - if (phi_mesh > 0) { - ++separated; - } else { - ++penetrating; - } + 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); @@ -885,44 +812,32 @@ GTEST_TEST(DistanceOracleMesh, World world(builder.Build()); const DistanceOracle oracle(world.diagram(), kTau); - const GeometryId mesh_id = GeometryOf(world.plant(), "l_mesh"); - const GeometryId convex_id = GeometryOf(world.plant(), "l_convex"); const GeometryId probe_id = GeometryOf(world.plant(), "probe"); - - // (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. - const double true_surface_clearance = 0.4 - probe_radius; - ASSERT_GT(true_surface_clearance, 0.0); - + 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()); - world.SetPose(probe_body, RigidTransformd(Vector3d(1.4, 1.4, 0.0))); - { + // (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, FindPair(oracle, mesh_id, probe_id)); - const double phi_convex = - oracle.SignedDistance(query, FindPair(oracle, convex_id, probe_id)); - EXPECT_NEAR(phi_mesh, phi_convex, kExact); - EXPECT_LT(phi_mesh, 0.0) + 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; } - - // Outside the hull too => both agree and both are positive. - world.SetPose(probe_body, RigidTransformd(Vector3d(1.9, 1.9, 0.0))); - { - const QueryObject& query = world.query(); - const double phi_mesh = - oracle.SignedDistance(query, FindPair(oracle, mesh_id, probe_id)); - const double phi_convex = - oracle.SignedDistance(query, FindPair(oracle, convex_id, probe_id)); - EXPECT_GT(phi_mesh, 0.0); - EXPECT_NEAR(phi_mesh, phi_convex, kExact); - } } GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { @@ -938,8 +853,10 @@ GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { World world(builder.Build()); const DistanceOracle oracle(world.diagram(), kTau); - const GeometryId mesh_id = GeometryOf(world.plant(), "mesh"); - const GeometryId convex_id = GeometryOf(world.plant(), "convex"); + 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); @@ -948,11 +865,9 @@ GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { world.SetPose(mesh_body, X_W); world.SetPose(convex_body, X_W); const QueryObject& query = world.query(); - const double phi_mesh = - oracle.SignedDistance(query, FindPair(oracle, ground, mesh_id)); - const double phi_convex = - oracle.SignedDistance(query, FindPair(oracle, ground, convex_id)); - EXPECT_NEAR(phi_mesh, phi_convex, kExact) << "trial " << trial; + 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(); @@ -1038,10 +953,9 @@ GTEST_TEST(VPolytopeIngestion, RoundTripMatchesDirectConvexRegistration) { oracle.SignedDistance(query, p_ingested, &a_i, &b_i); const double phi_direct = oracle.SignedDistance(query, p_direct, &a_d, &b_d); - const double phi_redundant = oracle.SignedDistance(query, p_redundant); - EXPECT_NEAR(phi_ingested, phi_direct, kExact) << "trial " << trial; - EXPECT_NEAR(phi_redundant, phi_direct, kExact) << "trial " << trial; + EXPECT_NEAR(oracle.SignedDistance(query, p_redundant), phi_direct, kExact) + << "trial " << trial; if (phi_direct > 1e-9) { ++separated; EXPECT_LT((a_i - a_d).norm(), kExact) << "trial " << trial; @@ -1070,8 +984,10 @@ GTEST_TEST(VPolytopeIngestion, RoundTripAlsoHoldsOnTheHalfSpaceFallbackRoute) { World world(builder.Build()); const DistanceOracle oracle(world.diagram(), kTau); - const GeometryId ingested_id = GeometryOf(world.plant(), "ingested"); - const GeometryId direct_id = GeometryOf(world.plant(), "direct"); + const PairRecord& ingested_pair = + FindPair(oracle, ground, GeometryOf(world.plant(), "ingested")); + const PairRecord& direct_pair = + FindPair(oracle, ground, GeometryOf(world.plant(), "direct")); std::mt19937 rng(1618); for (int trial = 0; trial < 100; ++trial) { @@ -1079,11 +995,9 @@ GTEST_TEST(VPolytopeIngestion, RoundTripAlsoHoldsOnTheHalfSpaceFallbackRoute) { world.SetPose(ingested_body, X_W); world.SetPose(direct_body, X_W); const QueryObject& query = world.query(); - const double phi_ingested = - oracle.SignedDistance(query, FindPair(oracle, ground, ingested_id)); - const double phi_direct = - oracle.SignedDistance(query, FindPair(oracle, ground, direct_id)); - EXPECT_NEAR(phi_ingested, phi_direct, kExact) << "trial " << trial; + const double phi_direct = oracle.SignedDistance(query, direct_pair); + EXPECT_NEAR(oracle.SignedDistance(query, ingested_pair), phi_direct, kExact) + << "trial " << trial; // Reference: the lowest transformed vertex, since the ground is z = 0. double lowest = std::numeric_limits::infinity(); for (int i = 0; i < vertices.cols(); ++i) { diff --git a/planning/continuous_collision/test/motion_bound_test.cc b/planning/continuous_collision/test/motion_bound_test.cc index bf0fff0fad07..a01a95e0528d 100644 --- a/planning/continuous_collision/test/motion_bound_test.cc +++ b/planning/continuous_collision/test/motion_bound_test.cc @@ -19,64 +19,61 @@ #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/geometry/shape_specification.h" -#include "drake/math/rigid_transform.h" -#include "drake/math/rotation_matrix.h" -#include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/ball_rpy_joint.h" #include "drake/multibody/tree/planar_joint.h" -#include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/quaternion_floating_joint.h" -#include "drake/multibody/tree/revolute_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/robot_diagram_builder.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" namespace drake { namespace planning { namespace continuous_collision { namespace { -using drake::geometry::Box; -using drake::geometry::Capsule; using drake::geometry::GeometryId; -using drake::geometry::HalfSpace; -using drake::geometry::Sphere; -using drake::math::RigidTransform; -using drake::math::RotationMatrix; using drake::multibody::BodyIndex; -using drake::multibody::CoulombFriction; using drake::multibody::JointIndex; -using drake::multibody::MultibodyPlant; using drake::multibody::PlanarJoint; -using drake::multibody::PrismaticJoint; using drake::multibody::QuaternionFloatingJoint; -using drake::multibody::RevoluteJoint; -using drake::multibody::RigidBody; +using drake::multibody::RpyFloatingJoint; using drake::multibody::ScrewJoint; -using drake::multibody::SpatialInertia; using drake::multibody::WeldJoint; -using drake::planning::RobotDiagram; -using drake::planning::RobotDiagramBuilder; using Eigen::Matrix3Xd; using Eigen::Vector3d; using Eigen::VectorXd; - -using Rng = std::mt19937_64; +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 @@ -89,90 +86,6 @@ constexpr double kSlack = 1e-9; much, which is what MotionBoundTable::carveout_slack() charges for. */ constexpr double kContinuityTolerance = 1e-7; -// --------------------------------------------------------------------------- -// Small random utilities (seeded, deterministic). -// --------------------------------------------------------------------------- - -double Uniform(Rng* rng, double lo, double hi) { - return std::uniform_real_distribution(lo, hi)(*rng); -} - -int UniformInt(Rng* rng, int lo, int hi) { - return std::uniform_int_distribution(lo, hi)(*rng); -} - -Vector3d RandomUnitVector(Rng* rng) { - std::normal_distribution normal(0.0, 1.0); - Vector3d v; - do { - v = Vector3d(normal(*rng), normal(*rng), normal(*rng)); - } while (v.norm() < 1e-6); - return v.normalized(); -} - -RotationMatrix RandomRotation(Rng* rng) { - std::normal_distribution normal(0.0, 1.0); - Eigen::Quaterniond q; - do { - q = Eigen::Quaterniond(normal(*rng), normal(*rng), normal(*rng), - normal(*rng)); - } while (q.norm() < 1e-6); - q.normalize(); - return RotationMatrix(q); -} - -RigidTransform RandomTransform(Rng* rng, double scale) { - return RigidTransform( - RandomRotation(rng), - Vector3d(Uniform(rng, -scale, scale), Uniform(rng, -scale, scale), - Uniform(rng, -scale, scale))); -} - -SpatialInertia UnitInertia() { - return SpatialInertia::SolidSphereWithMass(1.0, 0.05); -} - -// --------------------------------------------------------------------------- -// Surface sampling for the primitives the random worlds use. -// --------------------------------------------------------------------------- - -Matrix3Xd SampleSphereSurface(Rng* rng, double r, int n) { - Matrix3Xd p(3, n); - for (int i = 0; i < n; ++i) p.col(i) = r * RandomUnitVector(rng); - return p; -} - -Matrix3Xd SampleBoxSurface(Rng* rng, const Vector3d& size, int n) { - const Vector3d half = 0.5 * size; - Matrix3Xd p(3, n); - for (int i = 0; i < n; ++i) { - Vector3d v(Uniform(rng, -half.x(), half.x()), - Uniform(rng, -half.y(), half.y()), - Uniform(rng, -half.z(), half.z())); - const int axis = UniformInt(rng, 0, 2); - v(axis) = (UniformInt(rng, 0, 1) == 0 ? -1.0 : 1.0) * half(axis); - p.col(i) = v; - } - return p; -} - -Matrix3Xd SampleCapsuleSurface(Rng* rng, double r, double length, int n) { - const double half = 0.5 * length; - Matrix3Xd p(3, n); - for (int i = 0; i < n; ++i) { - if (UniformInt(rng, 0, 1) == 0) { - const double phi = Uniform(rng, 0.0, 2.0 * M_PI); - p.col(i) = Vector3d(r * std::cos(phi), r * std::sin(phi), - Uniform(rng, -half, half)); - } else { - const Vector3d u = RandomUnitVector(rng); - const double z0 = u.z() >= 0.0 ? half : -half; - p.col(i) = Vector3d(r * u.x(), r * u.y(), z0 + r * u.z()); - } - } - return p; -} - // --------------------------------------------------------------------------- // 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 @@ -192,32 +105,36 @@ struct RandomWorld { void AddRandomGeometry(Rng* rng, MultibodyPlant* plant, const RigidBody& body, const std::string& name, int num_samples, RandomWorld* world) { - const RigidTransform X_BG = RandomTransform(rng, 0.2); + 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, - CoulombFriction(1.0, 1.0)); - p_G = SampleSphereSurface(rng, r, num_samples); + Friction()); + p_G = test::SampleSurface(rng, num_samples, [r](Rng* g) { + return test::SampleSphere(g, r); + }); break; } case 1: { - const Vector3d size(Uniform(rng, 0.02, 0.3), Uniform(rng, 0.02, 0.3), - Uniform(rng, 0.02, 0.3)); + const Vector3d size = test::UniformVector(rng, 0.02, 0.3); gid = plant->RegisterCollisionGeometry(body, X_BG, Box(size), name, - CoulombFriction(1.0, 1.0)); - p_G = SampleBoxSurface(rng, size, num_samples); + 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, - CoulombFriction(1.0, 1.0)); - p_G = SampleCapsuleSurface(rng, r, length, num_samples); + 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; } } @@ -235,23 +152,22 @@ RandomWorld MakeRandomWorld(Rng* rng, bool allow_screw, int num_samples) { std::vector*> bodies{&plant.world_body()}; for (int i = 0; i < num_bodies; ++i) { const RigidBody& body = - plant.AddRigidBody(fmt::format("b{}", i), UnitInertia()); + 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 RigidTransform X_PF = RandomTransform(rng, 0.25); - const RigidTransform X_CM = RandomTransform(rng, 0.25); + 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); - const int kind = UniformInt(rng, 0, allow_screw ? 4 : 3); - switch (kind) { + switch (UniformInt(rng, 0, allow_screw ? 4 : 3)) { case 0: plant.AddJoint(jn, parent, X_PF, body, X_CM, - RandomUnitVector(rng)); + test::RandomUnitVector(rng)); break; case 1: plant.AddJoint(jn, parent, X_PF, body, X_CM, - RandomUnitVector(rng)); + test::RandomUnitVector(rng)); break; case 2: plant.AddJoint(jn, parent, X_PF, body, X_CM, @@ -259,11 +175,11 @@ RandomWorld MakeRandomWorld(Rng* rng, bool allow_screw, int num_samples) { break; case 3: plant.AddJoint(jn, parent, X_PF, body, X_CM, - RandomTransform(rng, 0.2)); + test::RandomTransform(rng, 0.2)); break; default: plant.AddJoint(jn, parent, X_PF, body, X_CM, - RandomUnitVector(rng), + test::RandomUnitVector(rng), Uniform(rng, 0.05, 0.6), 0.0); ++world.num_screw_joints; break; @@ -352,11 +268,38 @@ std::vector CollisionPairs(const RobotDiagram& diagram) { 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. // --------------------------------------------------------------------------- -/* Convenience: the position coordinates of a named joint. */ +/* 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); @@ -374,58 +317,40 @@ std::vector Merge(std::vector> groups) { return out; } -GTEST_TEST(JointSupportTest, SerialChain) { - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - const auto& env = plant.AddRigidBody("env", UnitInertia()); - const auto& l1 = plant.AddRigidBody("l1", UnitInertia()); - const auto& l2 = plant.AddRigidBody("l2", UnitInertia()); - const auto& l3 = plant.AddRigidBody("l3", UnitInertia()); - plant.AddJoint("w_env", plant.world_body(), {}, env, {}, - RigidTransform(Vector3d(1.0, 0.0, 0.0))); - plant.AddJoint("j1", plant.world_body(), {}, l1, {}, - Vector3d::UnitZ()); - plant.AddJoint("j2", l1, {}, l2, {}, Vector3d::UnitY()); - plant.AddJoint("j3", l2, {}, l3, {}, Vector3d::UnitX()); - auto diagram = builder.Build(); - const KinematicsEngine engine(*diagram); - const auto& p = diagram->plant(); - - const std::vector j1 = CoordsOf(p, "j1"); - const std::vector j2 = CoordsOf(p, "j2"); - const std::vector j3 = CoordsOf(p, "j3"); - - // Robot vs. anchored environment: the ancestors of the robot body. - EXPECT_EQ(engine.CoordinatesAffectingPair(env.index(), l3.index()), - Merge({j1, j2, j3})); - EXPECT_EQ(engine.CoordinatesAffectingPair(p.world_body().index(), l2.index()), - Merge({j1, j2})); - // Self pair through the common ancestor: the path between the two bodies. - EXPECT_EQ(engine.CoordinatesAffectingPair(l1.index(), l3.index()), - Merge({j2, j3})); - // A body against itself, and two anchored bodies, are static. - EXPECT_TRUE(engine.CoordinatesAffectingPair(l3.index(), l3.index()).empty()); - EXPECT_TRUE( - engine.CoordinatesAffectingPair(p.world_body().index(), env.index()) - .empty()); -} +/* One plant carrying every topology J(p) has to get right at once: -GTEST_TEST(JointSupportTest, BranchingTree) { + 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& b1 = plant.AddRigidBody("b1", UnitInertia()); - const auto& left = plant.AddRigidBody("left", UnitInertia()); - const auto& right = plant.AddRigidBody("right", UnitInertia()); - const auto& left_tip = plant.AddRigidBody("left_tip", UnitInertia()); + 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, {}, left_tip, {}, - Vector3d::UnitX()); + 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"); @@ -433,75 +358,48 @@ GTEST_TEST(JointSupportTest, BranchingTree) { const std::vector jt = CoordsOf(p, "jt"); ASSERT_EQ(jr.size(), 3); // A planar joint contributes three coordinates. - // Symmetric difference across the common ancestor b1: j0 affects both sides - // and drops out. - EXPECT_EQ(engine.CoordinatesAffectingPair(left_tip.index(), right.index()), - Merge({jl, jr, jt})); - EXPECT_EQ( - engine.CoordinatesAffectingPair(p.world_body().index(), left_tip.index()), - Merge({j0, jl, jt})); - EXPECT_EQ(engine.CoordinatesAffectingPair(left.index(), left_tip.index()), - Merge({jt})); -} - -GTEST_TEST(JointSupportTest, WeldedClusterMovesAsOneBody) { - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - const auto& arm = plant.AddRigidBody("arm", UnitInertia()); - const auto& hand = plant.AddRigidBody("hand", UnitInertia()); - const auto& finger = plant.AddRigidBody("finger", UnitInertia()); - const auto& anchored = plant.AddRigidBody("anchored", UnitInertia()); - plant.AddJoint("j0", plant.world_body(), {}, arm, {}, - Vector3d::UnitZ()); - plant.AddJoint("w1", arm, {}, hand, {}, - RigidTransform(Vector3d(0.2, 0.0, 0.0))); - plant.AddJoint("w2", hand, {}, finger, {}, - RigidTransform(Vector3d(0.05, 0.0, 0.0))); - plant.AddJoint("w3", plant.world_body(), {}, anchored, {}, - RigidTransform(Vector3d(0.0, 1.0, 0.0))); - auto diagram = builder.Build(); - const KinematicsEngine engine(*diagram); - const auto& p = diagram->plant(); - - // Everything inside a welded cluster is mutually static ... - EXPECT_TRUE( - engine.CoordinatesAffectingPair(arm.index(), finger.index()).empty()); - EXPECT_TRUE( - engine.CoordinatesAffectingPair(hand.index(), finger.index()).empty()); - EXPECT_TRUE( - engine.CoordinatesAffectingPair(p.world_body().index(), anchored.index()) - .empty()); - // ... and the whole cluster inherits the revolute coordinate of its chain. - EXPECT_EQ(engine.CoordinatesAffectingPair(anchored.index(), finger.index()), - CoordsOf(p, "j0")); + // 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", UnitInertia()); - const auto& l2 = plant.AddRigidBody("l2", UnitInertia()); + 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(), RigidTransform::Identity(), Sphere(0.1), - "g_world", CoulombFriction(1.0, 1.0)); - plant.RegisterCollisionGeometry( - l2, RigidTransform(Vector3d(0.3, 0, 0)), Sphere(0.05), "g_tip", - CoulombFriction(1.0, 1.0)); + 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); - ASSERT_EQ(pairs.size(), 1); - 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 = engine.ComputeMotionBoundTable( - lower, upper, std::vector(nq, false), pairs); + 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); @@ -510,7 +408,7 @@ GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { std::vector constant(nq, false); constant[0] = true; const MotionBoundTable table = - engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + OnePairTable(engine, pairs, lower, upper, constant); ASSERT_EQ(table.GetEntries(0).size(), 1); EXPECT_EQ(table.GetEntries(0)[0].first, 1); } @@ -518,28 +416,24 @@ GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { // 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 = engine.ComputeMotionBoundTable( - pinned, pinned, std::vector(nq, true), pairs); + 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); } } -// --------------------------------------------------------------------------- -// Part 1b. The joint-type and half-space carve-outs. -// --------------------------------------------------------------------------- - 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", UnitInertia()); - const auto& b = plant.AddRigidBody("body_b", UnitInertia()); + const auto& a = plant.AddRigidBody("body_a", Inertia()); + const auto& b = plant.AddRigidBody("body_b", Inertia()); plant.AddJoint("w", plant.world_body(), {}, b, {}, - RigidTransform(Vector3d(0.1, 0.0, 0.0))); + 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; @@ -548,22 +442,23 @@ GTEST_TEST(JointSupportTest, ReversedJointThrowsWithAnActionableMessage) { } catch (const std::exception& e) { GTEST_SKIP() << "this Drake refuses the model outright: " << e.what(); } - try { - const KinematicsEngine engine(*diagram); - GTEST_FAIL() << "expected a throw for a reversed joint"; - } catch (const std::exception& e) { - const std::string what = e.what(); - EXPECT_NE(what.find("reversed"), std::string::npos) << what; - } + EXPECT_THAT(ThrowMessage([&]() { + KinematicsEngine engine(*diagram); + }), + HasSubstr("reversed")); } -/* world --(revolute)--> link, with a half space on `halfspace_on_link` and a - sphere on the other body. */ +// --------------------------------------------------------------------------- +// 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", UnitInertia()); + const auto& link = plant.AddRigidBody("link", Inertia()); if (prismatic) { plant.AddJoint("j", plant.world_body(), {}, link, {}, Vector3d::UnitZ()); @@ -571,66 +466,58 @@ std::unique_ptr> MakeHalfSpaceModel(bool halfspace_on_link, plant.AddJoint("j", plant.world_body(), {}, link, {}, Vector3d::UnitY()); } - const CoulombFriction mu(1.0, 1.0); - const RigidTransform I = RigidTransform::Identity(); + const RigidTransformd I; if (halfspace_on_link) { - plant.RegisterCollisionGeometry(link, I, HalfSpace(), "hs", mu); + plant.RegisterCollisionGeometry(link, I, HalfSpace(), "hs", Friction()); plant.RegisterCollisionGeometry(plant.world_body(), - RigidTransform(Vector3d(0, 0, 1.0)), - Sphere(0.1), "ball", mu); + RigidTransformd(Vector3d(0, 0, 1.0)), + Sphere(0.1), "ball", Friction()); } else { plant.RegisterCollisionGeometry(plant.world_body(), I, HalfSpace(), "hs", - mu); - plant.RegisterCollisionGeometry(link, - RigidTransform(Vector3d(0.3, 0, 0)), - Sphere(0.1), "ball", mu); + Friction()); + plant.RegisterCollisionGeometry(link, RigidTransformd(Vector3d(0.3, 0, 0)), + Sphere(0.1), "ball", Friction()); } return builder.Build(); } -GTEST_TEST(HalfSpaceRuleTest, AnchoredGroundPlaneIsAccepted) { - // The canonical case: a ground plane on the world with a rotating arm above - // it. The half space is never the *distal* side, so λ bounds the arm's - // points and the pair is perfectly certifiable. - auto diagram = MakeHalfSpaceModel(/* halfspace_on_link = */ false, false); - const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); - const int nq = diagram->plant().num_positions(); - const MotionBoundTable table = engine.ComputeMotionBoundTable( - VectorXd::Constant(nq, -1.0), VectorXd::Constant(nq, 1.0), - std::vector(nq, false), pairs); - 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)); -} - -GTEST_TEST(HalfSpaceRuleTest, RotatingHalfSpaceThrowsAtConstruction) { - auto diagram = MakeHalfSpaceModel(/* halfspace_on_link = */ true, false); - try { - const KinematicsEngine engine(*diagram); - GTEST_FAIL() - << "expected a throw for a half space with revolute relative motion"; - } catch (const std::exception& e) { - const std::string what = e.what(); - EXPECT_NE(what.find("hs"), std::string::npos) << what; - EXPECT_NE(what.find("HalfSpace"), std::string::npos) << what; +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)); } -} -GTEST_TEST(HalfSpaceRuleTest, TranslatingHalfSpaceIsAccepted) { - // Pure translation keeps every point of the half space moving by |Δq|, so - // λ = 1 is finite and correct even though the reach is not. - auto diagram = MakeHalfSpaceModel(/* halfspace_on_link = */ true, true); - const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); - const int nq = diagram->plant().num_positions(); - const MotionBoundTable table = engine.ComputeMotionBoundTable( - VectorXd::Constant(nq, -1.0), VectorXd::Constant(nq, 1.0), - std::vector(nq, false), pairs); + 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)--> @@ -640,24 +527,21 @@ GTEST_TEST(HalfSpaceRuleTest, TranslatingHalfSpaceIsAccepted) { std::unique_ptr> MakeMidChainFloatingModel() { RobotDiagramBuilder builder; MultibodyPlant& plant = builder.plant(); - const auto& b1 = plant.AddRigidBody("b1", UnitInertia()); - const auto& b2 = plant.AddRigidBody("b2", UnitInertia()); - const auto& b3 = plant.AddRigidBody("b3", UnitInertia()); + 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, RigidTransform(Vector3d(0.1, 0.0, 0.0)), b2, - RigidTransform(Vector3d(0.0, 0.05, 0.0))); + "jf", b1, RigidTransformd(Vector3d(0.1, 0.0, 0.0)), b2, + RigidTransformd(Vector3d(0.0, 0.05, 0.0))); plant.AddJoint( - "j1", b2, RigidTransform(Vector3d(0.0, 0.0, 0.15)), b3, - RigidTransform(Vector3d(0.07, 0.0, 0.0)), Vector3d::UnitY()); - const CoulombFriction mu(1.0, 1.0); - plant.RegisterCollisionGeometry(plant.world_body(), - RigidTransform::Identity(), - Sphere(0.1), "g_world", mu); - plant.RegisterCollisionGeometry(b3, - RigidTransform(Vector3d(0.2, 0, 0)), - Sphere(0.05), "g_tip", mu); + "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(); } @@ -666,17 +550,13 @@ GTEST_TEST(JointSupportTest, MovingQuaternionFloatingJointThrows) { const KinematicsEngine engine(*diagram); const std::vector pairs = CollisionPairs(*diagram); const int nq = diagram->plant().num_positions(); - try { - engine.ComputeMotionBoundTable(VectorXd::Constant(nq, -0.5), - VectorXd::Constant(nq, 0.5), - std::vector(nq, false), pairs); - GTEST_FAIL() << "expected a throw for a moving quaternion floating joint"; - } catch (const std::exception& e) { - const std::string what = e.what(); - EXPECT_NE(what.find("jf"), std::string::npos) << what; - EXPECT_NE(what.find("quaternion_floating"), std::string::npos) << what; - EXPECT_NE(what.find("constant"), std::string::npos) << what; - } + 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) { @@ -684,7 +564,6 @@ GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { const MultibodyPlant& plant = diagram->plant(); const KinematicsEngine engine(*diagram); const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); const auto& jf = plant.GetJointByName("jf"); const int nq = plant.num_positions(); @@ -708,7 +587,7 @@ GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { upper[c] = 1.0; } const MotionBoundTable table = - engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + 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 @@ -724,9 +603,13 @@ GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); Rng rng(0xF10A7); const Matrix3Xd points_B = - SampleSphereSurface(&rng, 0.05, 128).colwise() + Vector3d(0.2, 0, 0); + 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(); - const auto& frame_world = plant.world_frame(); for (int trial = 0; trial < 200; ++trial) { VectorXd q = q0; VectorXd qp = q0; @@ -734,13 +617,8 @@ GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { q[c] = Uniform(&rng, lower[c], upper[c]); qp[c] = Uniform(&rng, lower[c], upper[c]); } - Matrix3Xd out_q(3, points_B.cols()); - Matrix3Xd out_qp(3, points_B.cols()); - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_q); - plant.SetPositions(&ctx, qp); - plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_qp); - const double displacement = (out_qp - out_q).colwise().norm().maxCoeff(); + 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 " @@ -778,14 +656,14 @@ TightChain MakeTightChain(bool screw_top, double screw_pitch) { 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 RigidTransform(Vector3d(x, 0.0, 0.0)); + return RigidTransformd(Vector3d(x, 0.0, 0.0)); }; RobotDiagramBuilder builder; MultibodyPlant& plant = builder.plant(); - const auto& b1 = plant.AddRigidBody("b1", UnitInertia()); - const auto& b2 = plant.AddRigidBody("b2", UnitInertia()); - const auto& b3 = plant.AddRigidBody("b3", UnitInertia()); + 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); @@ -796,11 +674,10 @@ TightChain MakeTightChain(bool screw_top, double screw_pitch) { plant.AddJoint("j_slide", b1, tx(kD1), b2, tx(-kD2), Vector3d::UnitX()); plant.AddJoint("j_weld", b2, tx(kE1), b3, tx(-kE3), tx(kE2)); - const CoulombFriction mu(1.0, 1.0); - plant.RegisterCollisionGeometry(plant.world_body(), - RigidTransform::Identity(), - Sphere(0.02), "g_world", mu); - plant.RegisterCollisionGeometry(b3, tx(kL3), Sphere(kRho), "g_tip", mu); + 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(); @@ -811,32 +688,22 @@ TightChain MakeTightChain(bool screw_top, double screw_pitch) { return out; } -/* Locates the (world geometry, tip geometry) pair. */ -int FindPairIndex(const RobotDiagram& diagram, - const std::vector& pairs, const std::string& name_a, - const std::string& name_b) { - const auto& inspector = diagram.scene_graph().model_inspector(); - for (int k = 0; k < static_cast(pairs.size()); ++k) { - const std::string a = inspector.GetName(pairs[k].a); - const std::string b = inspector.GetName(pairs[k].b); - if ((a.find(name_a) != std::string::npos && - b.find(name_b) != std::string::npos) || - (a.find(name_b) != std::string::npos && - b.find(name_a) != std::string::npos)) { - return k; - } - } - return -1; -} +/* 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{}; +}; -GTEST_TEST(ReachTest, RevoluteChainIsExactAndTight) { - const TightChain chain = MakeTightChain(/* screw_top = */ false, 0.0); +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 k = FindPairIndex(*chain.diagram, pairs, "g_world", "g_tip"); - ASSERT_GE(k, 0); - const int nq = plant.num_positions(); const auto& j_top = plant.GetJointByName("j_top"); const auto& j_slide = plant.GetJointByName("j_slide"); @@ -845,105 +712,65 @@ GTEST_TEST(ReachTest, RevoluteChainIsExactAndTight) { 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 = engine.ComputeMotionBoundTable( - lower, upper, std::vector(nq, false), pairs); - - double lambda_top = 0.0; - double lambda_slide = 0.0; - for (const auto& [c, lam] : table.GetEntries(k)) { - if (c == j_top.position_start()) lambda_top = lam; - if (c == j_slide.position_start()) lambda_slide = lam; + 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; } - // 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(lambda_top, chain.expected_reach, 1e-12); - EXPECT_EQ(lambda_slide, 1.0); - // And the bound is attained: with the slide at its box maximum and a small - // Δθ, the chord 2r·sin(Δθ/2) recovers r·Δθ to eight digits. 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; - const double dtheta = 1e-4; - qp[j_top.position_start()] = dtheta; + qp[j_top.position_start()] = probe.dtheta; Matrix3Xd p_B3(3, 1); p_B3.col(0) = chain.far_point_b3; - Matrix3Xd before(3, 1); - Matrix3Xd after(3, 1); - const auto& frame_tip = plant.GetBodyByName("b3").body_frame(); - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &before); - plant.SetPositions(&ctx, qp); - plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &after); - const double displacement = (after - before).norm(); - const double bound = lambda_top * dtheta; - EXPECT_LE(displacement, bound + kSlack); - EXPECT_GT(displacement / bound, 1.0 - 1e-8) + 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. - const VectorXd dq = (qp - q).cwiseAbs(); - EXPECT_LE(displacement, table.MotionBound(k, dq) + kSlack); + 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 MultibodyPlant& plant = chain.diagram->plant(); - const KinematicsEngine engine(*chain.diagram); - const std::vector pairs = CollisionPairs(*chain.diagram); - const int k = FindPairIndex(*chain.diagram, pairs, "g_world", "g_tip"); - ASSERT_GE(k, 0); - - 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 = engine.ComputeMotionBoundTable( - lower, upper, std::vector(nq, false), pairs); - - double lambda_top = 0.0; - for (const auto& [c, lam] : table.GetEntries(k)) { - if (c == j_top.position_start()) lambda_top = lam; - } + const TightChainProbe probe = ProbeTightChain(chain); const double pitch_term = kPitch / (2.0 * M_PI); - EXPECT_NEAR(lambda_top, chain.expected_reach + pitch_term, 1e-12); - - 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; - const double dtheta = 1e-4; - qp[j_top.position_start()] = dtheta; - Matrix3Xd p_B3(3, 1); - p_B3.col(0) = chain.far_point_b3; - Matrix3Xd before(3, 1); - Matrix3Xd after(3, 1); - const auto& frame_tip = plant.GetBodyByName("b3").body_frame(); - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &before); - plant.SetPositions(&ctx, qp); - plant.CalcPointsPositions(ctx, frame_tip, p_B3, plant.world_frame(), &after); - const double displacement = (after - before).norm(); - - EXPECT_LE(displacement, lambda_top * dtheta + kSlack); + 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(displacement, chain.expected_reach * dtheta * (1.0 + 1e-6)) + EXPECT_GT(probe.displacement, + chain.expected_reach * probe.dtheta * (1.0 + 1e-6)) << "a screw λ of r alone would under-bound this motion"; - const double exact = std::hypot(chain.expected_reach, pitch_term) * dtheta; - EXPECT_NEAR(displacement, exact, 1e-11); + EXPECT_NEAR(probe.displacement, + std::hypot(chain.expected_reach, pitch_term) * probe.dtheta, + 1e-11); } // --------------------------------------------------------------------------- @@ -1002,7 +829,11 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, const std::map> subtrees = SubtreeSets(plant); const std::vector owner = CoordinateOwners(plant); - // A random control box around a random nominal configuration. + // 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); @@ -1014,10 +845,6 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, constant[c] = is_constant; double half; if (is_constant) { - // 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. half = (carve_out == CarveOut::kSubTolerance) ? 0.5 * Uniform(rng, 0.02 * kContinuityTolerance, kContinuityTolerance) @@ -1105,14 +932,8 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, VectorXd q_step = q; q_step[c] = qp[c]; - Matrix3Xd before(3, pts.cols()); - Matrix3Xd after(3, pts.cols()); - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &before); - plant.SetPositions(&ctx, q_step); - plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &after); const double displacement = - (after - before).colwise().norm().maxCoeff(); + 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 " @@ -1122,23 +943,16 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, ++stats->atomic_checks; } - if (table.pair_is_static(k)) { - // A static pair may move only by the carve-out residual, which is - // exactly zero when every carved coordinate is exactly constant. - Matrix3Xd before(3, pts_b.cols()); - Matrix3Xd after(3, pts_b.cols()); - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_b, pts_b, frame_a, &before); - plant.SetPositions(&ctx, qp); - plant.CalcPointsPositions(ctx, frame_b, pts_b, frame_a, &after); - ASSERT_LE((after - before).colwise().norm().maxCoeff(), bound + kSlack) - << "pair " << k << " has empty J(p) but its relative pose moved by " - << "more than its carve-out slack " << bound; - continue; - } - // ---- (2) Aggregate: material-point distances. --------------------- - // Subsample: 24 × 24 point pairs is plenty to catch an under-bound and + // 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()); @@ -1163,19 +977,11 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, // ---- (3) One-sided aggregate when J(p) has a single distal side. --- if (single_distal_side && common_distal.is_valid()) { - const Matrix3Xd& pts = (common_distal == pair.body_a) ? pts_a : pts_b; - const auto& frame_d = - (common_distal == pair.body_a) ? frame_a : frame_b; - const auto& frame_o = - (common_distal == pair.body_a) ? frame_b : frame_a; - Matrix3Xd before(3, pts.cols()); - Matrix3Xd after(3, pts.cols()); - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &before); - plant.SetPositions(&ctx, qp); - plant.CalcPointsPositions(ctx, frame_d, pts, frame_o, &after); + const bool a_is_distal = (common_distal == pair.body_a); const double displacement = - (after - before).colwise().norm().maxCoeff(); + 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); @@ -1193,8 +999,7 @@ GTEST_TEST(DisplacementLemmaTest, RandomPlants) { 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; in the sub-tolerance case the carved coordinates still move - // and carveout_slack() has to pay for them. + // ~500 plants. const RandomWorld world = MakeRandomWorld(&rng, /* allow_screw = */ trial % 3 == 0, 128); stats.screw_joints += world.num_screw_joints; @@ -1210,15 +1015,14 @@ GTEST_TEST(DisplacementLemmaTest, RandomPlants) { EXPECT_GE(stats.atomic_checks, 20000); EXPECT_GE(stats.aggregate_checks, 10000); EXPECT_GE(stats.one_sided_checks, 2000); - // Screw joints must actually appear in the corpus, or the screw λ rule is - // never exercised. + // 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 be testing the exactly-constant case twice. + // 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( @@ -1241,12 +1045,12 @@ GTEST_TEST(DisplacementLemmaTest, ScrewChain) { 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), UnitInertia()); - plant.AddJoint( - fmt::format("j{}", i), *bodies.back(), RandomTransform(&rng, 0.25), - body, RandomTransform(&rng, 0.25), RandomUnitVector(&rng), - Uniform(&rng, -0.8, 0.8), 0.0); + 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); @@ -1261,9 +1065,6 @@ GTEST_TEST(DisplacementLemmaTest, ScrewChain) { EXPECT_GE(stats.plants, 75); EXPECT_GT(stats.atomic_checks, 500); EXPECT_GT(stats.max_tightness, 0.5); - GTEST_LOG_(INFO) << fmt::format("screw: plants={} atomic={} tightness={:.6f}", - stats.plants, stats.atomic_checks, - stats.max_tightness); } // --------------------------------------------------------------------------- @@ -1275,43 +1076,33 @@ GTEST_TEST(DisplacementLemmaTest, ScrewChain) { // 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 -// Options::certificate_slack. MotionBoundTable::carveout_slack() pays for it, -// and these tests pin it. +// Options::certificate_slack. MotionBoundTable::carveout_slack() pays for it. // --------------------------------------------------------------------------- -/* world --j_rot(revolute, ẑ)--> l1 --j_slide(prismatic, x̂)--> l2, with a - sphere on the world and one offset out along l2. */ -std::unique_ptr> MakeCarveOutChain() { +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", UnitInertia()); - const auto& l2 = plant.AddRigidBody("l2", UnitInertia()); + const auto& l1 = plant.AddRigidBody("l1", Inertia()); + const auto& l2 = plant.AddRigidBody("l2", Inertia()); plant.AddJoint("j_rot", plant.world_body(), {}, l1, - RigidTransform(Vector3d(-0.2, 0, 0)), + RigidTransformd(Vector3d(-0.2, 0, 0)), Vector3d::UnitZ()); plant.AddJoint("j_slide", l1, - RigidTransform(Vector3d(0.15, 0, 0)), - l2, {}, Vector3d::UnitX()); - const CoulombFriction mu(1.0, 1.0); - plant.RegisterCollisionGeometry(plant.world_body(), - RigidTransform::Identity(), - Sphere(0.1), "g_world", mu); - plant.RegisterCollisionGeometry(l2, - RigidTransform(Vector3d(0.3, 0, 0)), - Sphere(0.05), "g_tip", mu); - return builder.Build(); -} - -GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { - auto diagram = MakeCarveOutChain(); - const MultibodyPlant& plant = diagram->plant(); + 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); - ASSERT_EQ(pairs.size(), 1); - const int nq = plant.num_positions(); + const int nq = diagram->plant().num_positions(); ASSERT_EQ(nq, 2); - const int rot = plant.GetJointByName("j_rot").position_start(); - const int slide = plant.GetJointByName("j_slide").position_start(); + 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 @@ -1324,8 +1115,8 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { // (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 = engine.ComputeMotionBoundTable( - lower, upper, std::vector(nq, false), pairs); + 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; @@ -1343,7 +1134,7 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { lower[rot] = 0.0; upper[rot] = kRange; // upper − lower is exactly kRange in binary FP. const MotionBoundTable carved = - engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + 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; @@ -1357,14 +1148,14 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { lower[rot] = 0.0; upper[rot] = 0.0; const MotionBoundTable exact = - engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + 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 = engine.ComputeMotionBoundTable( - VectorXd::Constant(nq, -2.0), VectorXd::Constant(nq, 2.0), - std::vector(nq, false), pairs); + 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); } @@ -1377,61 +1168,45 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { std::unique_ptr> MakeCarvedHalfSpaceModel(bool rpy) { RobotDiagramBuilder builder; MultibodyPlant& plant = builder.plant(); - const auto& link = plant.AddRigidBody("link", UnitInertia()); + const auto& link = plant.AddRigidBody("link", Inertia()); if (rpy) { - plant.AddJoint( - "base", plant.world_body(), {}, link, {}); + plant.AddJoint("base", plant.world_body(), {}, link, {}); } else { plant.AddJoint("base", plant.world_body(), {}, link, {}); } - const CoulombFriction mu(1.0, 1.0); - plant.RegisterCollisionGeometry(link, RigidTransform::Identity(), - HalfSpace(), "hs", mu); + plant.RegisterCollisionGeometry(link, RigidTransformd(), HalfSpace(), "hs", + Friction()); plant.RegisterCollisionGeometry(plant.world_body(), - RigidTransform(Vector3d(0, 0, 1.0)), - Sphere(0.1), "ball", mu); + RigidTransformd(Vector3d(0, 0, 1.0)), + Sphere(0.1), "ball", Friction()); return builder.Build(); } -GTEST_TEST(CarveOutSlackTest, HalfSpaceAcrossAToleranceConstantRotationThrows) { - // 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 diagram = MakeCarvedHalfSpaceModel(/* rpy = */ false); - const KinematicsEngine engine(*diagram); // Must not throw: it is not a - // *supported* rotational kind. - const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); - const int nq = diagram->plant().num_positions(); +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); - VectorXd lower = VectorXd::Zero(nq); - VectorXd upper = VectorXd::Constant(nq, 5e-8); - try { - engine.ComputeMotionBoundTable(lower, upper, std::vector(nq, true), - pairs); - GTEST_FAIL() - << "expected a throw for a half space across a tolerance-constant " - "rotational coordinate"; - } catch (const std::exception& e) { - const std::string what = e.what(); - EXPECT_NE(what.find("hs"), std::string::npos) << what; - EXPECT_NE(what.find("base"), std::string::npos) << what; - EXPECT_NE(what.find("EXACTLY constant"), std::string::npos) << what; - } -} + 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"))); -GTEST_TEST(CarveOutSlackTest, - HalfSpaceAcrossAnExactlyConstantRotationIsAccepted) { - auto diagram = MakeCarvedHalfSpaceModel(/* rpy = */ false); - const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); - const int nq = diagram->plant().num_positions(); + // Exactly constant is accepted, and owes nothing. const VectorXd pinned = VectorXd::Constant(nq, 0.3); - const MotionBoundTable table = engine.ComputeMotionBoundTable( - pinned, pinned, std::vector(nq, true), pairs); + 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); } @@ -1442,19 +1217,17 @@ GTEST_TEST(CarveOutSlackTest, // space (every point of it moves by |Δq|), so only the *rotational* // coordinates have to be exactly constant. auto diagram = MakeCarvedHalfSpaceModel(/* rpy = */ true); - const MultibodyPlant& plant = diagram->plant(); const KinematicsEngine engine(*diagram); const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); - const int nq = plant.num_positions(); + 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 = engine.ComputeMotionBoundTable( - lower, upper, std::vector(nq, true), pairs); + 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); } @@ -1466,37 +1239,33 @@ GTEST_TEST(CarveOutSlackTest, // 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: first with the arm coordinate pinned so -// the slack is the whole bound, then again with everything moving. +// 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. */ +/* 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", UnitInertia()); - const auto& b2 = plant.AddRigidBody("b2", UnitInertia()); - const RigidTransform X_PF = RandomTransform(rng, 0.2); - const RigidTransform X_CM = RandomTransform(rng, 0.2); + 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("base", plant.world_body(), X_PF, b1, + X_CM); } - plant.AddJoint("jr", b1, RandomTransform(rng, 0.2), b2, - RandomTransform(rng, 0.2), - RandomUnitVector(rng)); - const CoulombFriction mu(1.0, 1.0); - plant.RegisterCollisionGeometry(plant.world_body(), - RigidTransform::Identity(), - Sphere(0.1), "g_world", mu); - plant.RegisterCollisionGeometry(b2, - RigidTransform(Vector3d(0.25, 0, 0)), - Sphere(0.06), "g_tip", mu); + 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(); } @@ -1508,7 +1277,6 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { int atomic_cases = 0; int aggregate_cases = 0; double max_ratio = 0.0; - double max_slack = 0.0; for (int trial = 0; trial < kTrials; ++trial) { SCOPED_TRACE(fmt::format("floating-base carve-out #{}", trial)); @@ -1516,7 +1284,6 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { const MultibodyPlant& plant = diagram->plant(); const KinematicsEngine engine(*diagram); const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); const int nq = plant.num_positions(); const int bs = plant.GetJointByName("base").position_start(); const int nb = plant.GetJointByName("base").num_positions(); @@ -1531,7 +1298,7 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { // the sample inside the box the bound is stated over. VectorXd q0(nq); if (quaternion) { - const Eigen::Quaterniond qb = RandomRotation(&rng).ToQuaternion(); + 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 { @@ -1543,17 +1310,16 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { auto root = diagram->CreateDefaultContext(); auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); const Matrix3Xd points_B = - SampleSphereSurface(&rng, 0.06, 64).colwise() + Vector3d(0.25, 0, 0); + 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& frame_world = plant.world_frame(); - Matrix3Xd out_q(3, points_B.cols()); - Matrix3Xd out_qp(3, points_B.cols()); const auto displacement = [&](const VectorXd& q, const VectorXd& qp) { - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_q); - plant.SetPositions(&ctx, qp); - plant.CalcPointsPositions(ctx, frame_tip, points_B, frame_world, &out_qp); - return (out_qp - out_q).colwise().norm().maxCoeff(); + return Displacement(plant, &ctx, points_B, frame_tip, plant.world_frame(), + q, qp); }; std::vector constant(nq, false); @@ -1577,14 +1343,13 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { upper[jr] = q0[jr] + 0.8; const MotionBoundTable table = - engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + 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; - max_slack = std::max(max_slack, slack); VectorXd q = q0; q[jr] = Uniform(&rng, lower[jr], upper[jr]); @@ -1613,10 +1378,9 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { upper[jr] = q0[jr] + 0.8; const MotionBoundTable table = - engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + OnePairTable(engine, pairs, lower, upper, constant); const double slack = table.carveout_slack(0); ASSERT_GT(slack, 0.0); - max_slack = std::max(max_slack, slack); ++aggregate_cases; VectorXd q(nq); @@ -1651,10 +1415,6 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { // ~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_LOG_(INFO) << fmt::format( - "{} base: atomic={} aggregate={} max_slack={:.3e} m max_ratio={:.6f}", - quaternion ? "quaternion floating" : "rpy floating", atomic_cases, - aggregate_cases, max_slack, max_ratio); } GTEST_TEST(CarveOutSlackTest, ToleranceConstantRpyFloatingBase) { @@ -1668,47 +1428,42 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantQuaternionFloatingBase) { // --------------------------------------------------------------------------- // Part 3c. An exactly tight floating-base λ̃. // -// The random corpus above catches structural errors but the chain walk's +// 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 inside that slack 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 +// 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. Every λ̃ -// shows up digit for digit. +// the chord 2R·sin(θ/2) recovers R·θ to fifteen digits at θ ~ 1e-7. // --------------------------------------------------------------------------- -std::unique_ptr> MakeTightFloatingChain(bool quaternion, - double radius) { +void RunTightFloatingBaseLambda(bool quaternion) { + constexpr double kRadius = 0.4; + constexpr double kWidth = 8e-8; // ≤ Options::continuity_tolerance. + Rng rng(quaternion ? 0x7168A7ull : 0x51DE12ull); + RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - const auto& link = plant.AddRigidBody("link", UnitInertia()); + MultibodyPlant& plant_in = builder.plant(); + const auto& link = plant_in.AddRigidBody("link", Inertia()); if (quaternion) { - plant.AddJoint("base", plant.world_body(), {}, - link, {}); + plant_in.AddJoint("base", plant_in.world_body(), + {}, link, {}); } else { - plant.AddJoint( - "base", plant.world_body(), {}, link, {}); + plant_in.AddJoint("base", plant_in.world_body(), {}, link, + {}); } - const CoulombFriction mu(1.0, 1.0); - plant.RegisterCollisionGeometry(link, RigidTransform::Identity(), - Sphere(radius), "g_link", mu); - plant.RegisterCollisionGeometry(plant.world_body(), - RigidTransform(Vector3d(0, 0, 3.0)), - Sphere(0.05), "g_world", mu); - return builder.Build(); -} + 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(); -void RunTightFloatingBaseLambda(bool quaternion) { - constexpr double kRadius = 0.4; - Rng rng(quaternion ? 0x7168A7ull : 0x51DE12ull); - auto diagram = MakeTightFloatingChain(quaternion, kRadius); const MultibodyPlant& plant = diagram->plant(); const KinematicsEngine engine(*diagram); const std::vector pairs = CollisionPairs(*diagram); - ASSERT_EQ(pairs.size(), 1); const int nq = plant.num_positions(); const auto& base = plant.GetJointByName("base"); const int bs = base.position_start(); @@ -1718,15 +1473,12 @@ void RunTightFloatingBaseLambda(bool quaternion) { // 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 = SampleSphereSurface(&rng, kRadius, 4096); + const Matrix3Xd points_B = test::SampleSurface(&rng, 4096, [kRadius](Rng* g) { + return test::SampleSphere(g, kRadius); + }); auto root = diagram->CreateDefaultContext(); auto& ctx = plant.GetMyMutableContextFromRoot(root.get()); - Matrix3Xd out_q(3, points_B.cols()); - Matrix3Xd out_qp(3, points_B.cols()); const auto& frame_link = plant.GetBodyByName("link").body_frame(); - const auto& frame_world = plant.world_frame(); - - constexpr double kWidth = 8e-8; // ≤ Options::continuity_tolerance. const std::vector constant(nq, true); for (int off = 0; off < nb; ++off) { @@ -1736,9 +1488,9 @@ void RunTightFloatingBaseLambda(bool 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.) + // λ̃ = 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(); @@ -1755,19 +1507,13 @@ void RunTightFloatingBaseLambda(bool quaternion) { lower[bs + off] = q0[bs + off] - 0.5 * kWidth; upper[bs + off] = q0[bs + off] + 0.5 * kWidth; const MotionBoundTable table = - engine.ComputeMotionBoundTable(lower, upper, constant, pairs); + 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); - VectorXd q = lower; - VectorXd qp = upper; - plant.SetPositions(&ctx, q); - plant.CalcPointsPositions(ctx, frame_link, points_B, frame_world, &out_q); - plant.SetPositions(&ctx, qp); - plant.CalcPointsPositions(ctx, frame_link, points_B, frame_world, &out_qp); - const double displacement = (out_qp - out_q).colwise().norm().maxCoeff(); - + 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) diff --git a/planning/continuous_collision/test/piecewise_bezier_path_test.cc b/planning/continuous_collision/test/piecewise_bezier_path_test.cc index 55d9f3655191..e7e84336ba3d 100644 --- a/planning/continuous_collision/test/piecewise_bezier_path_test.cc +++ b/planning/continuous_collision/test/piecewise_bezier_path_test.cc @@ -9,7 +9,7 @@ against themselves. */ #include #include -#include +#include #include #include #include @@ -17,11 +17,11 @@ against themselves. */ #include #include -#include #include #include "drake/common/copyable_unique_ptr.h" -#include "drake/common/polynomial.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" @@ -42,7 +42,6 @@ using drake::trajectories::BsplineTrajectory; using drake::trajectories::CompositeTrajectory; using drake::trajectories::PiecewisePolynomial; using drake::trajectories::Trajectory; -using ::testing::HasSubstr; constexpr double kTwoPi = 6.2831853071795864769252867665590; @@ -87,17 +86,6 @@ Eigen::VectorXd DrakeBezierValue(const Eigen::MatrixXd& control_points, return BezierCurve(0.0, 1.0, control_points).value(s); } -void ExpectThrowsWith(const std::function& statement, - const std::string& substring) { - try { - statement(); - ADD_FAILURE() << "Expected an exception whose message contains \"" - << substring << "\", but nothing was thrown."; - } catch (const std::exception& e) { - EXPECT_THAT(std::string(e.what()), HasSubstr(substring)); - } -} - /* 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, @@ -137,7 +125,7 @@ double MaxSampledError(const PiecewiseBezierPath& path, } // -------------------------------------------------------------------------- -// Bézier evaluation. +// Bézier evaluation and de Casteljau subdivision. // -------------------------------------------------------------------------- /* Our de Casteljau evaluation must agree with BezierCurve::value to 1e-12 over @@ -177,10 +165,6 @@ GTEST_TEST(BezierEvaluation, MatchesDrakeBezierCurve) { } } -// -------------------------------------------------------------------------- -// de Casteljau subdivision. -// -------------------------------------------------------------------------- - /* 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. */ @@ -208,10 +192,10 @@ GTEST_TEST(DeCasteljau, ChildrenReproduceParent) { ASSERT_EQ(right.cols(), order + 1); ASSERT_EQ(mid.size(), num_positions); - // The apex is exactly q(1/2). + // 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()); - // Children share the endpoints they must. worst = std::max(worst, (left.col(0) - parent.col(0)).cwiseAbs().maxCoeff()); worst = std::max( @@ -232,30 +216,23 @@ GTEST_TEST(DeCasteljau, ChildrenReproduceParent) { } /* The hot loop pre-sizes its outputs; re-splitting into already-correctly -sized buffers must not reallocate them, so the steady-state loop does not -allocate. */ -GTEST_TEST(DeCasteljau, PreSizedOutputsAreNotReallocated) { +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); - const double* left_data = left.data(); - const double* right_data = right.data(); - const double* mid_data = mid.data(); - DeCasteljauSplitAtHalf(parent, &left, &right, &mid); - EXPECT_EQ(left.data(), left_data); - EXPECT_EQ(right.data(), right_data); - EXPECT_EQ(mid.data(), mid_data); - // Splitting a child in place into the same buffers is the recursion the - // certifier runs; it must also be stable. + // certifier runs; it must be allocation-free too. const Eigen::MatrixXd child = left; - DeCasteljauSplitAtHalf(child, &left, &right, &mid); - EXPECT_EQ(left.data(), left_data); - EXPECT_EQ(right.data(), right_data); - EXPECT_EQ(mid.data(), mid_data); + { + 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 @@ -354,8 +331,10 @@ BsplineTrajectory MakeBsplineFromBasis( } /* Shared checker: the conversion must reproduce the B-spline to 1e-10 over ->= 1e4 dense samples, and the segments must tile the domain. */ -void CheckBsplineEquivalence(const BsplineTrajectory& bspline) { +>= 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, Options{}); EXPECT_EQ(path.num_positions(), bspline.rows()); @@ -366,53 +345,39 @@ void CheckBsplineEquivalence(const BsplineTrajectory& bspline) { // points, i.e. the degree of the source spline. EXPECT_EQ(segment.control_points.cols(), bspline.basis().order()); } - constexpr int kNumSamples = 10001; - EXPECT_LT(MaxSampledError(path, bspline, kNumSamples), 1e-10); + 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) { - const int num_basis_functions = order + 4; - const BsplineBasis basis(order, num_basis_functions, - KnotVectorType::kClampedUniform, 0.0, 3.0); - const BsplineTrajectory bspline = - MakeBsplineFromBasis(basis, 3, &generator); SCOPED_TRACE("order " + std::to_string(order)); - CheckBsplineEquivalence(bspline); - - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(bspline, Options{}); - // A clamped uniform basis has num_basis_functions - order + 1 nonempty - // spans. - EXPECT_EQ(static_cast(path.segments().size()), - num_basis_functions - order + 1); + 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; - for (int i = 0; i < order; ++i) { - knots.push_back(0.0); - } + std::vector knots(order, 0.0); for (double interior : {0.13, 0.29, 0.31, 1.70, 2.55}) { knots.push_back(interior); } - for (int i = 0; i < order; ++i) { - knots.push_back(3.0); - } - const BsplineBasis basis(order, knots); - const BsplineTrajectory bspline = - MakeBsplineFromBasis(basis, 4, &generator); - SCOPED_TRACE("order " + std::to_string(order)); - CheckBsplineEquivalence(bspline); - EXPECT_EQ( - static_cast(PiecewiseBezierPath::FromTrajectory(bspline, Options{}) - .segments() - .size()), + knots.insert(knots.end(), order, 3.0); + CheckBsplineEquivalence( + MakeBsplineFromBasis(BsplineBasis(order, knots), 4, &generator), 6); } } @@ -421,34 +386,24 @@ 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). + // 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}; - const BsplineBasis basis(4, knots); - const BsplineTrajectory bspline = - MakeBsplineFromBasis(basis, 2, &generator); - CheckBsplineEquivalence(bspline); - // Nonempty spans: [0,1], [1,2], [2,3], [3,3.5], [3.5,4]. - EXPECT_EQ( - static_cast(PiecewiseBezierPath::FromTrajectory(bspline, Options{}) - .segments() - .size()), - 5); + 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); - const BsplineBasis basis(4, 10, KnotVectorType::kClampedUniform, 0.0, - 5.0); - const BsplineTrajectory bspline = - MakeBsplineFromBasis(basis, 7, &generator); - CheckBsplineEquivalence(bspline); - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(bspline, Options{}); - EXPECT_EQ(static_cast(path.segments().size()), 7); - EXPECT_EQ(path.num_positions(), 7); + 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 @@ -456,23 +411,20 @@ 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) { - const BsplineBasis basis(order, order + 5, KnotVectorType::kUniform, - 0.0, 2.0); - const BsplineTrajectory bspline = - MakeBsplineFromBasis(basis, 3, &generator); SCOPED_TRACE("order " + std::to_string(order)); - CheckBsplineEquivalence(bspline); + 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 BsplineBasis basis(3, knots); - const BsplineTrajectory bspline = - MakeBsplineFromBasis(basis, 2, &generator); - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(bspline, Options{}); + const PiecewiseBezierPath path = PiecewiseBezierPath::FromTrajectory( + MakeBsplineFromBasis(BsplineBasis(3, knots), 2, &generator), + Options{}); 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) { @@ -486,11 +438,9 @@ GTEST_TEST(BsplineConversion, MatrixValuedThrows) { const BsplineTrajectory bspline( BsplineBasis(3, 6, KnotVectorType::kClampedUniform, 0.0, 1.0), control_points); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(bspline, Options{}); - }, - "column-vector-valued"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(bspline, Options{}), + "[\\s\\S]*column-vector-valued[\\s\\S]*"); } // -------------------------------------------------------------------------- @@ -507,12 +457,10 @@ GTEST_TEST(PiecewisePolynomialConversion, FirstOrderHold) { const PiecewiseBezierPath path = PiecewiseBezierPath::FromTrajectory(pp, Options{}); ASSERT_EQ(path.segments().size(), 5u); - for (const BezierSegment& segment : path.segments()) { - // A first-order hold is exactly an order-1 Bézier per segment. - EXPECT_EQ(segment.control_points.cols(), 2); - } - // Order-1 Bézier control points are the waypoints themselves. 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(), @@ -548,17 +496,19 @@ GTEST_TEST(PiecewisePolynomialConversion, CubicSplines) { EXPECT_LT(MaxSampledError(path_b, shape_preserving, 10001), 1e-10); } -/* A single high-degree polynomial segment, up to the default degree cap. */ -GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCap) { +/* A single high-degree polynomial segment, from degree 1 up to the default cap +and one past it. */ +GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCapAndBeyond) { const Options options; ASSERT_EQ(options.max_conversion_degree, 10); - for (int degree = 1; degree <= options.max_conversion_degree; ++degree) { + for (int degree = 1; degree <= options.max_conversion_degree + 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. + // 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]; @@ -566,39 +516,20 @@ GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCap) { const PiecewisePolynomial pp = PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, samples); + Options relaxed = options; + if (degree > options.max_conversion_degree) { + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(pp, options), + "[\\s\\S]*max_conversion_degree[\\s\\S]*"); + // Raising the cap is the documented escape hatch. + relaxed.max_conversion_degree = degree; + } const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(pp, options); + PiecewiseBezierPath::FromTrajectory(pp, relaxed); ASSERT_EQ(path.segments().size(), 1u); EXPECT_EQ(path.segments()[0].control_points.cols(), degree + 1); - EXPECT_LT(MaxSampledError(path, pp, 10001), 1e-10) << "degree " << degree; - } -} - -GTEST_TEST(PiecewisePolynomialConversion, DegreeAboveCapThrows) { - const int degree = 11; - const int num_points = degree + 1; - Eigen::VectorXd times(num_points); - Eigen::MatrixXd samples(1, num_points); - for (int i = 0; i < num_points; ++i) { - times[i] = 0.3 + 1.7 * static_cast(i) / degree; - samples(0, i) = std::sin(2.0 * times[i]); + EXPECT_LT(MaxSampledError(path, pp, 10001), 1e-10); } - const PiecewisePolynomial pp = - PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, - samples); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(pp, Options{}); - }, - "max_conversion_degree"); - - // Raising the cap makes it work. - Options relaxed; - relaxed.max_conversion_degree = degree; - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(pp, relaxed); - EXPECT_EQ(path.segments()[0].control_points.cols(), degree + 1); - EXPECT_LT(MaxSampledError(path, pp, 10001), 1e-10); } GTEST_TEST(PiecewisePolynomialConversion, MatrixValuedThrows) { @@ -608,11 +539,9 @@ GTEST_TEST(PiecewisePolynomialConversion, MatrixValuedThrows) { const std::vector times{0.0, 1.0}; const PiecewisePolynomial pp = PiecewisePolynomial::FirstOrderHold(times, samples); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(pp, Options{}); - }, - "column-vector-valued"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(pp, Options{}), + "[\\s\\S]*column-vector-valued[\\s\\S]*"); } // -------------------------------------------------------------------------- @@ -637,24 +566,16 @@ GTEST_TEST(JunctionValidation, InjectedDiscontinuityThrows) { std::mt19937_64 generator(90210); Eigen::VectorXd offset = Eigen::VectorXd::Zero(3); offset[1] = 1e-3; - const CompositeTrajectory trajectory = - MakeJunctionCase(offset, &generator); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); - }, - "C0 discontinuity"); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); - }, - "coordinate 1"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(MakeJunctionCase(offset, &generator), + Options{}), + "[\\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; - const CompositeTrajectory ok = MakeJunctionCase(tiny, &generator); - EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory(ok, Options{})); + EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory( + MakeJunctionCase(tiny, &generator), Options{})); } GTEST_TEST(JunctionValidation, TwoPiOffsetAcceptedOnlyWhenDeclaredRevolute) { @@ -664,46 +585,33 @@ GTEST_TEST(JunctionValidation, TwoPiOffsetAcceptedOnlyWhenDeclaredRevolute) { const CompositeTrajectory trajectory = MakeJunctionCase(offset, &generator); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); - }, - "C0 discontinuity"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(trajectory, Options{}), + "[\\s\\S]*C0 discontinuity[\\s\\S]*"); // Declaring the *wrong* coordinate does not help. Options wrong; wrong.continuous_revolute_indices = {0, 2}; - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(trajectory, wrong); - }, - "C0 discontinuity"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(trajectory, wrong), + "[\\s\\S]*C0 discontinuity[\\s\\S]*"); Options right; right.continuous_revolute_indices = {1}; EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory(trajectory, right)); - // Any integer multiple of 2π is fine. + // Any integer multiple of 2π is fine ... Eigen::VectorXd big_offset = Eigen::VectorXd::Zero(3); big_offset[1] = -3.0 * kTwoPi; - const CompositeTrajectory big = - MakeJunctionCase(big_offset, &generator); - EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory(big, right)); -} + EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory( + MakeJunctionCase(big_offset, &generator), right)); -GTEST_TEST(JunctionValidation, NonMultipleOfTwoPiOffsetThrowsEvenWhenRevolute) { - std::mt19937_64 generator(2468); - Eigen::VectorXd offset = Eigen::VectorXd::Zero(2); - offset[0] = kTwoPi + 1e-3; - const CompositeTrajectory trajectory = - MakeJunctionCase(offset, &generator); - Options options; - options.continuous_revolute_indices = {0, 1}; - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(trajectory, options); - }, - "C0 discontinuity"); + // ... 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 @@ -741,11 +649,9 @@ GTEST_TEST(JunctionValidation, OutOfRangeRevoluteIndexThrows) { waypoints << 0.0, 1.0, 2.0, 0.0, 0.0, 0.0; Options options; options.continuous_revolute_indices = {2}; - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromWaypoints(waypoints, options); - }, - "continuous_revolute_indices"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromWaypoints(waypoints, options), + "[\\s\\S]*continuous_revolute_indices[\\s\\S]*"); } /* A zero-order hold genuinely teleports at every break; certifying it @@ -754,13 +660,11 @@ 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; - const PiecewisePolynomial pp = - PiecewisePolynomial::ZeroOrderHold(times, samples); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(pp, Options{}); - }, - "C0 discontinuity"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory( + PiecewisePolynomial::ZeroOrderHold(times, samples), + Options{}), + "[\\s\\S]*C0 discontinuity[\\s\\S]*"); } // -------------------------------------------------------------------------- @@ -948,28 +852,19 @@ GTEST_TEST(Composite, UnknownSegmentTypeThrowsWithIndexAndTypeName) { const CompositeTrajectory trajectory = MakeComposite(std::move(pieces)); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); - }, - "segment index 1"); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); - }, - "UnsupportedTrajectory"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(trajectory, Options{}), + "[\\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); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromTrajectory(bare, Options{}); - }, - "segment index 0"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromTrajectory(bare, Options{}), + "[\\s\\S]*segment index 0[\\s\\S]*"); } // -------------------------------------------------------------------------- -// Waypoints. +// Waypoints and evaluation domain handling. // -------------------------------------------------------------------------- GTEST_TEST(Waypoints, OrderOneSegmentsAreExact) { @@ -993,9 +888,7 @@ GTEST_TEST(Waypoints, OrderOneSegmentsAreExact) { 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 k = 0; k + 1 < num_waypoints; ++k) { + // 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 = @@ -1008,23 +901,14 @@ GTEST_TEST(Waypoints, OrderOneSegmentsAreExact) { } GTEST_TEST(Waypoints, TooFewWaypointsThrows) { - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd::Zero(3, 1), - Options{}); - }, - "at least 2 waypoints"); - ExpectThrowsWith( - [&]() { - PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd(0, 4), Options{}); - }, - "zero rows"); + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromWaypoints( + Eigen::MatrixXd::Zero(3, 1), Options{}), + "[\\s\\S]*at least 2 waypoints[\\s\\S]*"); + DRAKE_EXPECT_THROWS_MESSAGE( + PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd(0, 4), Options{}), + "[\\s\\S]*zero rows[\\s\\S]*"); } -// -------------------------------------------------------------------------- -// Evaluation domain handling. -// -------------------------------------------------------------------------- - GTEST_TEST(Evaluation, DomainEdgesClampAndOutsideThrows) { Eigen::MatrixXd waypoints(2, 3); waypoints << 0.0, 1.0, 3.0, -1.0, 0.0, 1.0; @@ -1034,52 +918,27 @@ GTEST_TEST(Evaluation, DomainEdgesClampAndOutsideThrows) { 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(-1e-13)); EXPECT_NO_THROW(path.Value(2.0 + 1e-13)); EXPECT_TRUE(path.Value(-1e-13).isApprox(waypoints.col(0), 0.0)); - - ExpectThrowsWith( - [&]() { - path.Value(-1e-3); - }, - "outside the path's domain"); - ExpectThrowsWith( - [&]() { - path.Value(2.5); - }, - "outside the path's domain"); - ExpectThrowsWith( - [&]() { - path.EvaluateSegment(0, 1.5); - }, - "outside the segment's domain"); - ExpectThrowsWith( - [&]() { - path.EvaluateSegment(0, -0.5); - }, - "outside the segment's domain"); EXPECT_NO_THROW(path.EvaluateSegment(0, 1.0 + 1e-13)); -} -GTEST_TEST(Evaluation, SegmentIndexOutOfRangeThrows) { - Eigen::MatrixXd waypoints(2, 3); - waypoints << 0.0, 1.0, 3.0, -1.0, 0.0, 1.0; - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromWaypoints(waypoints, Options{}); - ExpectThrowsWith( - [&]() { - path.EvaluateSegment(2, 0.5); - }, - "out of range"); - ExpectThrowsWith( - [&]() { - path.EvaluateSegment(-1, 0.5); - }, - "out of range"); + 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 +/* 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) { @@ -1090,17 +949,17 @@ GTEST_TEST(Evaluation, JunctionTimeSelectsTheLaterSegment) { 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); - EXPECT_EQ(path.Value(0.0)[0], 0.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 be consistent there -regardless of which side the lookup lands on. */ +/* 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; diff --git a/planning/continuous_collision/test/soundness_fuzz_test.cc b/planning/continuous_collision/test/soundness_fuzz_test.cc index fefc8afc6edd..6ce7e9134753 100644 --- a/planning/continuous_collision/test/soundness_fuzz_test.cc +++ b/planning/continuous_collision/test/soundness_fuzz_test.cc @@ -1,15 +1,12 @@ // End-to-end soundness fuzz: random worlds × random trajectories, cross-checked -// three 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) and its certificate is replayed -// independently; -// * every definite `Finding` is re-evaluated at its witness configuration, -// from a context this run never touched, and must really violate; -// * every non-definite `Finding` claiming to be a resolution-floor grazing -// record must be backed by a clearance within 10·(τ_p + ε) of the -// threshold near the reported time. +// three 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) and its certificate is replayed +// independently; every definite `Finding` is re-evaluated at its witness +// configuration, from a context this run never touched, and must really +// violate; and every non-definite `Finding` claiming to be a resolution-floor +// grazing record 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. @@ -84,20 +81,15 @@ 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 (BUILD.bazel selects -// this on //tools:using_sanitizer and //tools:using_memcheck). 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. +// 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. asan and lsan are -// excluded outright (BUILD.bazel tags), and the instrumented flavors that do -// run the fuzz take the quarter corpus above, with the timeout raised to -// "long" to absorb the rest. Composition is asserted as fractions of kNumCases -// so both corpus sizes are held to the same standard. +// 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 @@ -115,10 +107,9 @@ 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, which resolves -// 10× finer at 10× the cost. Sample counts are per case and approximate: they -// are split evenly across segments and each segment gets both endpoints, so the -// true count is total + #segments. +// 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; @@ -127,15 +118,12 @@ constexpr int kDeepEvery = 10; constexpr int kGrazeProbeSamples = 2000; // The worst signed-distance accuracy Drake documents for any supported shape -// combination (query_object.h Table 4, Cylinder–Ellipsoid). The checker charges -// each pair its own τ_p ≥ Options::query_tolerance; the tests below only ever -// need an upper bound on it, and this is it. +// 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; -// --------------------------------------------------------------------------- -// 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. -// --------------------------------------------------------------------------- +// 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, @@ -153,22 +141,10 @@ struct ShapeSpec { Vector3d dims{Vector3d::Zero()}; }; -std::string Name(ShapeKind kind) { - switch (kind) { - case ShapeKind::kSphere: - return "Sphere"; - case ShapeKind::kBox: - return "Box"; - case ShapeKind::kCapsule: - return "Capsule"; - case ShapeKind::kCylinder: - return "Cylinder"; - case ShapeKind::kEllipsoid: - return "Ellipsoid"; - case ShapeKind::kConvex: - return "ConvexTetra"; - } - return "?"; +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 @@ -317,17 +293,16 @@ class Rng { return std::uniform_int_distribution(lo, hi)(engine_); } bool Bernoulli(double p) { return std::bernoulli_distribution(p)(engine_); } - // Note the named locals: the order in which a compiler evaluates sibling - // constructor arguments is unspecified, so drawing three variates inline - // would make the corpus depend on the toolchain. Every draw in this file is - // sequenced explicitly for that reason. + // 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); } - // A uniformly distributed direction (rejection-sampled, so no pole bias). + // Rejection-sampled, so no pole bias. Vector3d Direction() { while (true) { const Vector3d v = UniformVector(-1.0, 1.0); @@ -335,7 +310,6 @@ class Rng { if (n > 1e-3 && n <= 1.0) return v / n; } } - // A direction scaled by a length drawn *after* it. Vector3d Offset(double lo, double hi) { const Vector3d direction = Direction(); const double length = Uniform(lo, hi); @@ -347,11 +321,10 @@ class Rng { }; // Link geometries stay small (≤ 5 cm half-extent) and sit ~12–18 cm out along -// the link, while joints are ~25–35 cm apart. Adjacent links therefore have -// real clearance in most configurations but can genuinely fold into each -// other, which is what makes the self-collision half of the corpus nontrivial. +// 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 a live, unfiltered pair.) +// parent/child pair here is live.) ShapeSpec RandomLinkShape(Rng* rng) { ShapeSpec spec; const int roll = rng->Int(0, 11); @@ -557,9 +530,8 @@ std::unique_ptr> BuildTrajectory( // 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 is auditing. std::nullopt means "no finite radius available" -// (HalfSpace) or "not derived here" (Convex / Mesh); such pairs always take the -// narrowphase. +// 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 { @@ -765,10 +737,8 @@ Options FuzzOptions(double margin) { options.emit_certificate = true; options.parallelism = Parallelism::None(); // A coarser resolution floor than the 1e-9 default: a grazing pair still ends - // kInconclusive, but after ~20 bisections rather than ~30, which keeps the - // pathological cases of a 200-case corpus affordable. The node budget is the - // second guard; a case that hits it is counted and skipped, never silently - // accepted. + // kInconclusive, but after ~20 bisections rather than ~30. The node budget is + // the second guard; a case that hits it is counted, never silently accepted. options.min_interval = 1e-6; options.max_nodes = 300000; return options; @@ -816,10 +786,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { // 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 exercise the kInconclusive branch (and its cross-check) - // on random geometry. Without it the corpus would never produce a grazing - // case, because a random trajectory is tangent to a random obstacle with - // probability zero. + // 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) { @@ -928,15 +895,11 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { << "the reported distance is not reproducible at the witness"; } else if (result.verdict != Verdict::kBudgetExhausted) { // Every non-definite finding that is *not* a budget remainder is a - // resolution-floor grazing record, whether the run as a whole ended - // kInconclusive or kViolationFound (in kCertifyAll the sink's - // inconclusive list is appended to the definite one, so a violating run - // can carry grazing records too). All of them get the same audit; only - // the synthesized "here is where the budget stopped us" finding is - // exempt, because its clearance carries no claim. + // resolution-floor grazing record, and must be backed by a clearance + // that sits within 10·(τ_p + ε) of the threshold somewhere near the + // reported time. Only the synthesized "here is where the budget stopped + // us" finding is exempt, because its clearance carries no claim. ++tally.inconclusive_findings; - // A grazing record must be backed by a clearance that sits within - // 10·(τ_p + ε) of the threshold somewhere near the reported time. const double tolerance = 10.0 * (kWorstTau + options.certificate_slack); const double window = 0.01 * std::max(1e-12, path.end_time() - path.start_time()); @@ -981,9 +944,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { // 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. It still has - // to be large enough for the composition floors below to say something; at 50 - // cases the thinnest of them still demands a 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"); @@ -1012,10 +973,9 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { // 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 in the corpus, - // including the analytic HalfSpace route: 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 end to end. + // 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)) diff --git a/planning/continuous_collision/test/test_utilities.h b/planning/continuous_collision/test/test_utilities.h new file mode 100644 index 000000000000..bc396918a827 --- /dev/null +++ b/planning/continuous_collision/test/test_utilities.h @@ -0,0 +1,563 @@ +#pragma once + +// Helpers shared by this package's tests: seeded random primitives and surface +// samplers, the throw-message probe, the checker factory, 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 "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/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 {}; +} + +inline ContinuousCollisionChecker::Params CheckerParams( + std::shared_ptr> model, Options options, + PaddingSpec padding = {}) { + ContinuousCollisionChecker::Params params; + params.model = std::move(model); + params.default_options = std::move(options); + params.padding = std::move(padding); + return params; +} + +// The checker is neither copyable nor movable, so tests that need to own one +// inside a container take the pointer flavor. +inline ContinuousCollisionChecker MakeChecker( + std::shared_ptr> model, Options options, + PaddingSpec padding = {}) { + return ContinuousCollisionChecker( + CheckerParams(std::move(model), std::move(options), std::move(padding))); +} + +inline std::unique_ptr MakeCheckerPtr( + std::shared_ptr> model, Options options, + PaddingSpec padding = {}) { + return std::make_unique( + CheckerParams(std::move(model), std::move(options), std::move(padding))); +} + +// Signed distance of `finding`'s pair, re-measured at the witness +// configuration from a fresh context: an independent confirmation that the +// witness is a real contact and not an artifact of the search. +inline double DistanceAtFinding(const ContinuousCollisionChecker& checker, + const Finding& finding) { + const RobotDiagram& model = checker.model(); + 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)); + for (const PairRecord& pair : checker.pairs()) { + if (pair.id.a == finding.pair.a && pair.id.b == finding.pair.b) { + return checker.distance_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 x 2-mode sweep (80 certification +// runs) 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, SearchMode mode) { + Options options; + options.margin = kMargin; + options.parallelism = parallelism; + options.mode = mode; + // Bounded cost per run: the whole sweep is executed 8 times per case. + options.min_interval = 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); + ContinuousCollisionChecker::Params params; + params.model = entry->model; + params.default_options = + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); + entry->checker = std::make_unique(params); + entry->control_points = + MakeControlPoints(seed, entry->model->plant().num_positions()); + const CertificationResult result = entry->checker->CheckTrajectory( + entry->trajectory(), + BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); + entry->serial_verdict = result.verdict; + // Keep the corpus balanced: stop taking more of whichever kind is + // already well represented. + const bool is_free = result.verdict == Verdict::kCertifiedFree; + const bool is_violating = result.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 findings. 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 FindingsIdentical( + const std::vector& a, const std::vector& b) { + if (a.size() != b.size()) { + return ::testing::AssertionFailure() + << "finding counts differ: " << a.size() << " vs " << b.size(); + } + for (std::size_t i = 0; i < a.size(); ++i) { + if (a[i].time != b[i].time) { + return ::testing::AssertionFailure() + << "finding " << i << " time " << a[i].time << " vs " << b[i].time; + } + if (a[i].q.size() != b[i].q.size() || + !(a[i].q.array() == b[i].q.array()).all()) { + return ::testing::AssertionFailure() + << "finding " << i << " witness configuration differs"; + } + if (a[i].pair.a != b[i].pair.a || a[i].pair.b != b[i].pair.b) { + return ::testing::AssertionFailure() + << "finding " << i << " pair differs"; + } + if (a[i].distance != b[i].distance || + a[i].motion_bound != b[i].motion_bound || + a[i].definite != b[i].definite) { + return ::testing::AssertionFailure() + << "finding " << i << " payload differs"; + } + if (a[i].nearest_a_W.has_value() != b[i].nearest_a_W.has_value() || + (a[i].nearest_a_W.has_value() && + *a[i].nearest_a_W != *b[i].nearest_a_W)) { + return ::testing::AssertionFailure() + << "finding " << i << " witness point A differs"; + } + if (a[i].nearest_b_W.has_value() != b[i].nearest_b_W.has_value() || + (a[i].nearest_b_W.has_value() && + *a[i].nearest_b_W != *b[i].nearest_b_W)) { + return ::testing::AssertionFailure() + << "finding " << i << " witness point B differs"; + } + } + return ::testing::AssertionSuccess(); +} + +inline ::testing::AssertionResult EarliestWitnessIdentical( + const CertificationResult& a, const CertificationResult& b) { + if (a.findings.empty() != b.findings.empty()) { + return ::testing::AssertionFailure() + << "one run reported findings and the other did not"; + } + if (a.findings.empty()) return ::testing::AssertionSuccess(); + return FindingsIdentical({a.findings.front()}, {b.findings.front()}); +} + +// The bisection's node budget below doubles as the deep workload's size: the +// margin it converges to is the largest one still certifiable inside this +// budget, so the tree it produces has just under this many nodes. Large enough +// that no fixed seeding depth could ever have covered it; small enough that the +// ~40 probes that find it stay cheap, sanitizers included. 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; +// Floors concurrency_test.cc holds the result to, so the workload cannot +// silently degenerate if the corpus or the bisection drifts. Depth is the load +// bearing one: a deep, narrow spike is the shape a depth-seeded work queue +// cannot split, and it is what the sharing path exists for. +constexpr uint64_t kMinDeepNodes = 1000; +constexpr int kMinDeepDepth = 15; + +// 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. +struct DeepWorkload { + const Case* entry{}; + double margin{0.0}; + double min_interval{1e-8}; + uint64_t nodes{0}; + int max_depth{0}; + + Options options(Parallelism parallelism) const { + Options options = BaseOptions(parallelism, SearchMode::kCertifyAll); + options.margin = margin; + options.min_interval = min_interval; + 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; + options.max_nodes = kProbeBudget; + return deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), options) + .verdict == Verdict::kCertifiedFree; + }; + 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; + const Statistics stats = + deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), + deep->options(Parallelism::None())) + .stats; + deep->nodes = stats.nodes; + deep->max_depth = stats.max_depth; + 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 index 5d65b6ff0ffe..a071c69bfc5b 100644 --- a/planning/continuous_collision/test/thin_obstacle_test.cc +++ b/planning/continuous_collision/test/thin_obstacle_test.cc @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include @@ -16,19 +15,8 @@ #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/rigid_transform.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/spatial_inertia.h" #include "drake/planning/collision_checker_params.h" -#include "drake/planning/continuous_collision/continuous_collision_checker.h" -#include "drake/planning/robot_diagram.h" -#include "drake/planning/robot_diagram_builder.h" +#include "drake/planning/continuous_collision/test/test_utilities.h" #include "drake/planning/scene_graph_collision_checker.h" namespace drake { @@ -36,23 +24,24 @@ namespace planning { namespace continuous_collision { namespace { -using drake::Parallelism; -using drake::geometry::Box; -using drake::geometry::QueryObject; -using drake::geometry::Sphere; -using drake::math::RigidTransformd; -using drake::multibody::CoulombFriction; -using drake::multibody::MultibodyPlant; -using drake::multibody::PrismaticJoint; -using drake::multibody::RigidBody; -using drake::multibody::SpatialInertia; using drake::planning::CollisionCheckerParams; -using drake::planning::RobotDiagram; -using drake::planning::RobotDiagramBuilder; using drake::planning::SceneGraphCollisionChecker; -using drake::trajectories::BezierCurve; using Eigen::Vector3d; using Eigen::VectorXd; +using test::BezierCurve; +using test::Box; +using test::DistanceAtFinding; +using test::Friction; +using test::Inertia; +using test::MakeChecker; +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. @@ -92,14 +81,6 @@ 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; -CoulombFriction Friction() { - return CoulombFriction(1.0, 1.0); -} - -SpatialInertia Inertia() { - return SpatialInertia::SolidSphereWithMass(1.0, 0.05); -} - // 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". @@ -127,8 +108,8 @@ void AddAnchoredBox(MultibodyPlant* plant, const std::string& name, name + "_geom", Friction()); } -// The thin-plate world: 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. +// 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; @@ -173,13 +154,11 @@ SceneGraphCollisionChecker MakeDrakeChecker( return SceneGraphCollisionChecker(std::move(params)); } -ContinuousCollisionChecker MakeCertifiedChecker( - std::shared_ptr> model) { - ContinuousCollisionChecker::Params params; - params.model = std::move(model); - params.default_options.margin = 0.0; - params.default_options.parallelism = Parallelism::None(); - return ContinuousCollisionChecker(params); +Options CertifiedOptions() { + Options options; + options.margin = 0.0; + options.parallelism = Parallelism::None(); + return options; } VectorXd MakeQ(double x, double y) { @@ -195,28 +174,6 @@ Eigen::MatrixXd Waypoints(const VectorXd& q1, const VectorXd& q2) { return waypoints; } -// Signed distance of `finding`'s pair, re-measured from a fresh context at the -// witness configuration: an independent confirmation that the witness is a real -// contact and not an artifact of the search. -double DistanceAtFinding(const ContinuousCollisionChecker& checker, - const Finding& finding) { - const RobotDiagram& model = checker.model(); - 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)); - for (const PairRecord& pair : checker.pairs()) { - if (pair.id.a == finding.pair.a && pair.id.b == finding.pair.b) { - return checker.distance_oracle().SignedDistance(query_object, pair); - } - } - ADD_FAILURE() << "the finding names a pair the checker does not know."; - return std::numeric_limits::quiet_NaN(); -} - // --------------------------------------------------------------------------- // 1. Pin the failure mode: Drake's sampled checker reports the edge free. // --------------------------------------------------------------------------- @@ -227,11 +184,10 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { const SceneGraphCollisionChecker drake_checker = MakeDrakeChecker( MakePlateWorld(kPlateX, kPlateThickness), kDrakeEdgeStepSize); - // The distance the sample count is derived from is exactly the edge length. + // 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); - - // 1 mm of plate between the waypoints, and the sampled check calls the edge - // free. 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"; @@ -250,9 +206,8 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { SCOPED_TRACE("plate mid-plane at x = " + std::to_string(offset)); const SceneGraphCollisionChecker probe = MakeDrakeChecker( MakePlateWorld(offset, kPlateThickness), kDrakeEdgeStepSize); - const bool predicted_free = - gap_to_nearest_sample(offset) > kContactHalfWidth; - EXPECT_EQ(probe.CheckEdgeCollisionFree(q1, q2), predicted_free) + 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"; } @@ -276,9 +231,8 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { // The miss is a resolution gap, not a modelling one: shrink the step size and // the very same sampled checker finds the plate. - const SceneGraphCollisionChecker fine_checker = - MakeDrakeChecker(MakePlateWorld(kPlateX, kPlateThickness), 0.002); - EXPECT_FALSE(fine_checker.CheckEdgeCollisionFree(q1, q2)); + EXPECT_FALSE(MakeDrakeChecker(MakePlateWorld(kPlateX, kPlateThickness), 0.002) + .CheckEdgeCollisionFree(q1, q2)); } // --------------------------------------------------------------------------- @@ -288,9 +242,8 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { GTEST_TEST(ThinObstacleTest, CertifiedCheckerCatchesTheThinPlate) { const VectorXd q1 = MakeQ(-0.5, 0.0); const VectorXd q2 = MakeQ(0.5, 0.0); - std::shared_ptr> model = - MakePlateWorld(kPlateX, kPlateThickness); - const ContinuousCollisionChecker checker = MakeCertifiedChecker(model); + const auto checker = + MakeChecker(MakePlateWorld(kPlateX, kPlateThickness), CertifiedOptions()); const CertificationResult result = checker.CheckEdge(q1, q2); ASSERT_EQ(result.verdict, Verdict::kViolationFound); @@ -308,7 +261,6 @@ GTEST_TEST(ThinObstacleTest, CertifiedCheckerCatchesTheThinPlate) { 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); @@ -338,17 +290,14 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { constexpr double kClearance = kHalfGap - 0.5 * kPlateThickness - kToolRadius; static_assert(kClearance > 0.0); - std::shared_ptr> model = MakeSlotWorld(kHalfGap); - const ContinuousCollisionChecker checker = MakeCertifiedChecker(model); - + const auto checker = MakeChecker(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. - const SceneGraphCollisionChecker drake_checker = - MakeDrakeChecker(MakeSlotWorld(kHalfGap), kDrakeEdgeStepSize); - EXPECT_TRUE(drake_checker.CheckEdgeCollisionFree(q1, q2)); + EXPECT_TRUE(MakeDrakeChecker(MakeSlotWorld(kHalfGap), kDrakeEdgeStepSize) + .CheckEdgeCollisionFree(q1, q2)); const CertificationResult result = checker.CheckEdge(q1, q2); EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); @@ -362,8 +311,7 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { // 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. - constexpr std::uint64_t kNodeCeiling = 640; - EXPECT_LT(result.stats.nodes, kNodeCeiling) + EXPECT_LT(result.stats.nodes, uint64_t{640}) << "certifying a 3 mm gap should cost O(log(travel / clearance)) depth, " "not a blow-up"; EXPECT_GE(result.stats.max_depth, 6) @@ -372,9 +320,7 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { EXPECT_LE(result.stats.max_depth, 12); // ... and the certificate for this run replays independently. - Options options; - options.margin = 0.0; - options.parallelism = Parallelism::None(); + Options options = CertifiedOptions(); options.emit_certificate = true; const CertificationResult with_certificate = checker.CheckPath(Waypoints(q1, q2), options); @@ -386,58 +332,38 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { } // --------------------------------------------------------------------------- -// 4. Thickness sweep: reported, not asserted. +// 4. Thickness sweep: where the resolution gap closes. // --------------------------------------------------------------------------- -GTEST_TEST(ThinObstacleTest, ThicknessSweepReportsTheResolutionGap) { +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 assertion - // at the end 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. + // "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); - const std::vector thicknesses = {0.001, 0.002, 0.005, 0.010, - 0.020, 0.030, 0.038, 0.040, - 0.042, 0.050, 0.080}; double first_caught = std::numeric_limits::quiet_NaN(); - std::cout << "\n[ THIN-PLATE SWEEP ] plate mid-plane x = " << kPlateX - << " m, tool radius = " << kToolRadius - << " m, Drake edge_step_size = " << kDrakeEdgeStepSize - << " m (samples 0.05 m apart in x)\n" - << " thickness[m] drake_sampled continuous_collision " - "contact_half_width[m]\n"; - for (const double thickness : thicknesses) { + 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 SceneGraphCollisionChecker drake_checker = MakeDrakeChecker( - MakePlateWorld(kPlateX, thickness), kDrakeEdgeStepSize); - const bool drake_free = drake_checker.CheckEdgeCollisionFree(q1, q2); - - std::shared_ptr> model = - MakePlateWorld(kPlateX, thickness); - const ContinuousCollisionChecker checker = MakeCertifiedChecker(model); - const CertificationResult result = checker.CheckEdge(q1, q2); - + const bool drake_free = + MakeDrakeChecker(MakePlateWorld(kPlateX, thickness), kDrakeEdgeStepSize) + .CheckEdgeCollisionFree(q1, q2); if (!drake_free && std::isnan(first_caught)) first_caught = thickness; - std::cout << " " << thickness << "\t\t" - << (drake_free ? "free " : "IN COLLISION") << "\t" - << (result.verdict == Verdict::kViolationFound ? "violation" - : "OTHER ") - << "\t" << (0.5 * thickness + kToolRadius) << "\n"; - - // The assertion half of the sweep: the verdict is stable throughout. - EXPECT_EQ(result.verdict, Verdict::kViolationFound); + EXPECT_EQ( + MakeChecker(MakePlateWorld(kPlateX, thickness), CertifiedOptions()) + .CheckEdge(q1, q2) + .verdict, + Verdict::kViolationFound); } - std::cout << " --> Drake's sampled checker first sees the plate at " - "thickness = " - << first_caught << " m; predicted crossover 2*(0.025 - " - << kToolRadius << ") = " << 2.0 * (0.025 - kToolRadius) << " m\n\n"; - // A report, not a gate. The crossover must still land in the right decade, - // otherwise the sweep is measuring something other than the resolution gap. + // 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); } From 977fca648eea32cf1443badece4025867d5822f3 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Fri, 28 Aug 2026 13:30:42 -0400 Subject: [PATCH 19/22] [planning] continuous_collision: minimize the public API --- .../planning_continuous_collision.h | 1106 ++--------------- .../planning_py_continuous_collision.cc | 435 +------ .../test/continuous_collision_test.py | 307 +---- planning/continuous_collision/BUILD.bazel | 197 +-- .../continuous_collision/bounding_sphere.cc | 173 --- .../continuous_collision/bounding_sphere.h | 51 - planning/continuous_collision/certificate.cc | 359 ------ planning/continuous_collision/certificate.h | 40 - .../{certifier_internal.cc => certifier.cc} | 519 ++------ .../{certifier_internal.h => certifier.h} | 104 +- .../continuous_collision_checker.cc | 363 ++---- .../continuous_collision_checker.h | 196 ++- .../continuous_collision/distance_oracle.cc | 82 +- .../continuous_collision/distance_oracle.h | 79 +- planning/continuous_collision/internal.h | 143 +++ .../motion_bound_table.cc | 190 ++- .../continuous_collision/motion_bound_table.h | 114 +- planning/continuous_collision/numerics.h | 43 - planning/continuous_collision/options.h | 141 --- .../piecewise_bezier_path.cc | 58 +- .../piecewise_bezier_path.h | 63 +- planning/continuous_collision/shape_class.h | 70 -- .../continuous_collision/test/api_test.cc | 313 +---- .../test/bounding_sphere_test.cc | 5 +- .../test/certificate_test.cc | 583 --------- .../test/certifier_test.cc | 430 ++----- .../test/concurrency_test.cc | 325 ++--- .../test/distance_oracle_test.cc | 235 +--- .../test/motion_bound_test.cc | 48 +- .../test/piecewise_bezier_path_test.cc | 119 +- .../test/soundness_fuzz_test.cc | 215 ++-- .../test/test_utilities.h | 197 +-- .../test/thin_obstacle_test.cc | 46 +- .../vpolytope_ingestion.cc | 55 - .../vpolytope_ingestion.h | 48 - 35 files changed, 1597 insertions(+), 5855 deletions(-) delete mode 100644 planning/continuous_collision/bounding_sphere.cc delete mode 100644 planning/continuous_collision/bounding_sphere.h delete mode 100644 planning/continuous_collision/certificate.cc delete mode 100644 planning/continuous_collision/certificate.h rename planning/continuous_collision/{certifier_internal.cc => certifier.cc} (60%) rename planning/continuous_collision/{certifier_internal.h => certifier.h} (62%) create mode 100644 planning/continuous_collision/internal.h delete mode 100644 planning/continuous_collision/numerics.h delete mode 100644 planning/continuous_collision/options.h delete mode 100644 planning/continuous_collision/shape_class.h delete mode 100644 planning/continuous_collision/test/certificate_test.cc delete mode 100644 planning/continuous_collision/vpolytope_ingestion.cc delete mode 100644 planning/continuous_collision/vpolytope_ingestion.h diff --git a/bindings/generated_docstrings/planning_continuous_collision.h b/bindings/generated_docstrings/planning_continuous_collision.h index b4fd48cedc3f..eefe436edd51 100644 --- a/bindings/generated_docstrings/planning_continuous_collision.h +++ b/bindings/generated_docstrings/planning_continuous_collision.h @@ -12,15 +12,7 @@ #pragma GCC diagnostic ignored "-Wunused-variable" #endif -// #include "drake/planning/continuous_collision/bounding_sphere.h" -// #include "drake/planning/continuous_collision/certificate.h" // #include "drake/planning/continuous_collision/continuous_collision_checker.h" -// #include "drake/planning/continuous_collision/distance_oracle.h" -// #include "drake/planning/continuous_collision/motion_bound_table.h" -// #include "drake/planning/continuous_collision/numerics.h" -// #include "drake/planning/continuous_collision/options.h" -// #include "drake/planning/continuous_collision/piecewise_bezier_path.h" -// #include "drake/planning/continuous_collision/vpolytope_ingestion.h" // Symbol: pydrake_doc_planning_continuous_collision constexpr struct /* pydrake_doc_planning_continuous_collision */ { @@ -30,238 +22,24 @@ constexpr struct /* pydrake_doc_planning_continuous_collision */ { struct /* planning */ { // Symbol: drake::planning::continuous_collision struct /* continuous_collision */ { - // Symbol: drake::planning::continuous_collision::AddVPolytopeObstacle - struct /* AddVPolytopeObstacle */ { - // Source: drake/planning/continuous_collision/vpolytope_ingestion.h - const char* doc = -R"""(Registers a V-polytope as an anchored obstacle with a collision role -(the geometry-support scope, "V-polytopes as first-class geometry", -ingestion route (b)). - -The polytope is converted to ``drake∷geometry∷Convex`` through Drake's -own ``VPolytope∷ToShapeConvex()`` entry point (a thin wrapper over the -``Convex(Eigen∷Matrix3X points, std∷string label, double -scale)`` constructor pinned at M0), then registered on the plant's -world body. The result therefore rides the ordinary native narrowphase -path end to end: the proximity engine and the certifier's -radius/support code all read the same ``Convex∷GetConvexHull()`` -object, so the certificate stays sound even for redundant or -degenerate vertex sets. - -Parameter ``plant``: - The plant to register on. Must be non-null, must already be a - registered SceneGraph source, and must NOT be finalized. - -Parameter ``vpoly``: - The polytope. Its vertices are interpreted in the geometry frame - G, i.e. the world-frame obstacle is ``X_WG * - conv(vpoly.vertices())``. Must be 3-dimensional with at least one - vertex. - -Parameter ``X_WG``: - Pose of the geometry frame in the world frame. - -Parameter ``name``: - Geometry name; also used as the ``Convex`` shape's label (which - Drake only uses in its own warning/error messages). Must not - contain a newline. - -Returns: - The id of the newly registered collision geometry. - -Raises: - RuntimeError if ``plant`` is null or already finalized, if - ``vpoly.ambient_dimension() != 3``, if the vertex set is empty, or - if Drake rejects the resulting hull (e.g. a degenerate vertex set - that its hull computation cannot inflate).)"""; - } AddVPolytopeObstacle; - // Symbol: drake::planning::continuous_collision::BezierSegment - struct /* BezierSegment */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1] (trajectory -normalization).)"""; - // Symbol: drake::planning::continuous_collision::BezierSegment::control_points - struct /* control_points */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(n × (m+1); column j is control point P_j.)"""; - } control_points; - // Symbol: drake::planning::continuous_collision::BezierSegment::t_end - struct /* t_end */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = R"""()"""; - } t_end; - // Symbol: drake::planning::continuous_collision::BezierSegment::t_start - struct /* t_start */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(Original time interval (bookkeeping only; the certificate is a -property of the path and is invariant under time reparametrization).)"""; - } t_start; - } BezierSegment; - // Symbol: drake::planning::continuous_collision::BoundingSphere - struct /* BoundingSphere */ { - // Source: drake/planning/continuous_collision/bounding_sphere.h - const char* doc = -R"""(A sphere, expressed in the owning body (link) frame L, that contains a -proximity geometry at every configuration of the body.)"""; - // Symbol: drake::planning::continuous_collision::BoundingSphere::center_L - struct /* center_L */ { - // Source: drake/planning/continuous_collision/bounding_sphere.h - const char* doc = R"""(Sphere center in the body frame.)"""; - } center_L; - // Symbol: drake::planning::continuous_collision::BoundingSphere::radius - struct /* radius */ { - // Source: drake/planning/continuous_collision/bounding_sphere.h - const char* doc = R"""()"""; - } radius; - } BoundingSphere; - // Symbol: drake::planning::continuous_collision::Certificate - struct /* Certificate */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = -R"""(Audit trail of every certification event of a run; an independent -replay (VerifyCertificate, declared in the api header) re-evaluates -every record and checks interval coverage of the full domain per pair.)"""; - // Symbol: drake::planning::continuous_collision::Certificate::pairs - struct /* pairs */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = -R"""(Pair table snapshot the indices refer to.)"""; - } pairs; - // Symbol: drake::planning::continuous_collision::Certificate::records - struct /* records */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } records; - } Certificate; - // Symbol: drake::planning::continuous_collision::CertificateRecord - struct /* CertificateRecord */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = -R"""(One certification event: pair ``pair_index`` was certified over the -parameter interval [s_start, s_end] of segment ``segment`` from -representative configuration qc (the search algorithm).)"""; - // Symbol: drake::planning::continuous_collision::CertificateRecord::motion_bound - struct /* motion_bound */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } motion_bound; - // Symbol: drake::planning::continuous_collision::CertificateRecord::pair_index - struct /* pair_index */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } pair_index; - // Symbol: drake::planning::continuous_collision::CertificateRecord::phi_hat - struct /* phi_hat */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } phi_hat; - // Symbol: drake::planning::continuous_collision::CertificateRecord::qc - struct /* qc */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } qc; - // Symbol: drake::planning::continuous_collision::CertificateRecord::s_end - struct /* s_end */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } s_end; - // Symbol: drake::planning::continuous_collision::CertificateRecord::s_start - struct /* s_start */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } s_start; - // Symbol: drake::planning::continuous_collision::CertificateRecord::segment - struct /* segment */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } segment; - // Symbol: drake::planning::continuous_collision::CertificateRecord::threshold - struct /* threshold */ { - // Source: drake/planning/continuous_collision/certificate.h - const char* doc = R"""()"""; - } threshold; - } CertificateRecord; - // Symbol: drake::planning::continuous_collision::CertificationResult - struct /* CertificationResult */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = -R"""(Result of one certification call (the architecture).)"""; - // Symbol: drake::planning::continuous_collision::CertificationResult::certificate - struct /* certificate */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""(Present iff Options∷emit_certificate.)"""; - } certificate; - // Symbol: drake::planning::continuous_collision::CertificationResult::findings - struct /* findings */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""(Earliest-first.)"""; - } findings; - // Symbol: drake::planning::continuous_collision::CertificationResult::stats - struct /* stats */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - } stats; - // Symbol: drake::planning::continuous_collision::CertificationResult::verdict - struct /* verdict */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - } verdict; - } CertificationResult; - // Symbol: drake::planning::continuous_collision::ComputeBoundingSphere - struct /* ComputeBoundingSphere */ { - // Source: drake/planning/continuous_collision/bounding_sphere.h - const char* doc = -R"""(Computes a bounding sphere, in the body frame, of shape ``shape`` -posed at X_LG in the body frame (the geometry-support scope). - -The sphere is centered at the shape's natural center (tighter for the -broadphase prefilter than the white paper's origin-centered radius -R_g; 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. - -λ soundness dies quietly if any formula under-bounds, so this function -switches on the closed set of supported shape types and - -Raises: - RuntimeError on anything else (HalfSpace included — halfspaces are - handled by dedicated rules, never through a bounding sphere).)"""; - } ComputeBoundingSphere; // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker struct /* ContinuousCollisionChecker */ { // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Certifies — not samples — that a trajectory is collision-free over its -entire continuous time domain (the problem statement). +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 margin + padding(A, B) -— under the stated assumptions: exact real arithmetic up to the -configured numerical slack, a distance oracle accurate to its stated -tolerance, and the geometry semantics of the geometry-support scope -(Mesh ≡ convex hull). This is a statement about the continuum of -configurations, not about samples. The certificate is a property of -the path, so retiming the trajectory afterwards does not invalidate -it. +(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. 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 deliberately stronger than +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.)"""; @@ -269,835 +47,227 @@ is needed here. Construction and destruction are not thread-safe.)"""; struct /* CheckEdge */ { // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Certifies the straight configuration-space edge q1 → q2.)"""; +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 a piecewise-linear path through the given waypoint columns.)"""; +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 (any supported Drake trajectory type).)"""; +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∷min_interval is outside (0, 1]. + +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::ComputeMotionBounds - struct /* ComputeMotionBounds */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - } ComputeMotionBounds; // 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, topology tables, and runs the -capability probe (throws on unsupported geometry pairs; the -geometry-support scope).)"""; +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::Normalize - struct /* Normalize */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = -R"""(Introspection / testing seams (all const, thread-safe).)"""; - } Normalize; - // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params - struct /* Params */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params::default_options - struct /* default_options */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - } default_options; - // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params::model - struct /* model */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = -R"""(Plant + scene graph; the plant must be finalized.)"""; - } model; - // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::Params::padding - struct /* padding */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = -R"""(Per-body-pair padding; see PaddingSpec for the env/self rule.)"""; - } padding; - } Params; - // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::distance_oracle - struct /* distance_oracle */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - } distance_oracle; - // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::kinematics_engine - struct /* kinematics_engine */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - } kinematics_engine; // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::model struct /* model */ { // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = R"""()"""; } model; - // Symbol: drake::planning::continuous_collision::ContinuousCollisionChecker::pairs - struct /* pairs */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = R"""()"""; - } pairs; } ContinuousCollisionChecker; - // Symbol: drake::planning::continuous_collision::DeCasteljauSplitAtHalf - struct /* DeCasteljauSplitAtHalf */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(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.)"""; - } DeCasteljauSplitAtHalf; - // Symbol: drake::planning::continuous_collision::DistanceOracle - struct /* DistanceOracle */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(Narrowphase distance abstraction (the distance-oracle contract). -Stateless per query and thread-compatible: configuration comes in via -the caller's QueryObject. - -Contract: SignedDistance returns φ̂ with |φ̂ − φ_true| ≤ tolerance() -whenever φ_true is at or above −tolerance(), and returns a definitely -negative value when the shapes interpenetrate beyond tolerance. Only -over-reporting a distance at or above threshold could fake a -certificate (the soundness argument), 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.)"""; - // Symbol: drake::planning::continuous_collision::DistanceOracle::DistanceOracle - struct /* ctor */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(Runs the capability probe: enumerates the unfiltered proximity pairs -from the model's SceneGraph inspector (collision filter state -snapshotted at construction), classifies every (shape, shape) -combination as {native, halfspace-fallback, unsupported}, and - -Raises: - RuntimeError immediately naming the offending geometries if any - pair is unsupported (deformables; halfspace–halfspace). Never - discovers an unsupported pair mid-certification.)"""; - } ctor; - // Symbol: drake::planning::continuous_collision::DistanceOracle::SignedDistance - struct /* SignedDistance */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(Signed distance for one pair at the configuration already set in the -context that produced ``query_object``. Optionally reports world-frame -closest points when the route provides them. - -``pair`` need not be an element of pairs(): the facade copies the -probe's records and rewrites their thresholds, so only ``pair.id`` and -``pair.route`` are read here. Both routes always fill the optional -out-params. - -Raises: - RuntimeError if ``pair`` carries a halfspace route but its - geometries were not classified by this oracle's capability probe - (i.e. the record did not come from pairs()).)"""; - } SignedDistance; - // Symbol: drake::planning::continuous_collision::DistanceOracle::pairs - struct /* pairs */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(The unfiltered pairs found by the probe (thresholds default 0; the -facade rewrites them from margin + padding).)"""; - } pairs; - // Symbol: drake::planning::continuous_collision::DistanceOracle::support_report - struct /* support_report */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(Human-readable probe report: one line per distinct shape-type -combination and its route (includes the "Mesh certified as convex -hull" notices; the risk register).)"""; - } support_report; - // Symbol: drake::planning::continuous_collision::DistanceOracle::tolerance - struct /* tolerance */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(τ used in the certificate arithmetic (the numerical policy).)"""; - } tolerance; - } DistanceOracle; - // Symbol: drake::planning::continuous_collision::DistanceRoute - struct /* DistanceRoute */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(How the oracle computes signed distance for one pair, resolved once by -the capability probe (the geometry-support scope; the distance-oracle -contract): no per-query dispatch decisions.)"""; - // Symbol: drake::planning::continuous_collision::DistanceRoute::kHalfSpaceA - struct /* kHalfSpaceA */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(Analytic halfspace support-function fallback; geometry ``a`` is the -halfspace.)"""; - } kHalfSpaceA; - // Symbol: drake::planning::continuous_collision::DistanceRoute::kHalfSpaceB - struct /* kHalfSpaceB */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = R"""(Same, geometry ``b`` is the halfspace.)"""; - } kHalfSpaceB; - // Symbol: drake::planning::continuous_collision::DistanceRoute::kNative - struct /* kNative */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(QueryObject∷ComputeSignedDistancePairClosestPoints.)"""; - } kNative; - } DistanceRoute; // Symbol: drake::planning::continuous_collision::Finding struct /* Finding */ { - // Source: drake/planning/continuous_collision/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(One violation or inconclusive record (the architecture).)"""; - // Symbol: drake::planning::continuous_collision::Finding::definite - struct /* definite */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(true ⇒ definite violation; false ⇒ grazing / inconclusive.)"""; - } definite; +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/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Oracle signed distance at q for this pair.)"""; +R"""(Signed distance of the pair at q.)"""; } distance; - // Symbol: drake::planning::continuous_collision::Finding::motion_bound - struct /* motion_bound */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Motion bound Δ_p at the terminal node (0 for breakpoint findings).)"""; - } motion_bound; + // 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/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Closest points in world frame at q, when the narrowphase provides them -(violation findings; planners use these to push trajectories out of -collision).)"""; +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/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = R"""()"""; } nearest_b_W; - // Symbol: drake::planning::continuous_collision::Finding::pair - struct /* pair */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } pair; // Symbol: drake::planning::continuous_collision::Finding::q struct /* q */ { - // Source: drake/planning/continuous_collision/options.h + // 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/options.h + // 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::IsCertified - struct /* IsCertified */ { - // Source: drake/planning/continuous_collision/numerics.h - const char* doc = -R"""(True iff the pair is certified on the whole node.)"""; - } IsCertified; - // Symbol: drake::planning::continuous_collision::IsDefiniteViolation - struct /* IsDefiniteViolation */ { - // Source: drake/planning/continuous_collision/numerics.h - const char* doc = -R"""(True iff the representative configuration is a definite violation.)"""; - } IsDefiniteViolation; - // Symbol: drake::planning::continuous_collision::KinematicsEngine - struct /* KinematicsEngine */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Construction-time kinematic analysis of a plant (the displacement -lemma): 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).)"""; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::ComputeMotionBoundTable - struct /* ComputeMotionBoundTable */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc_2args = -R"""(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; the displacement lemma). 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. - -Raises: - RuntimeError naming the joint if the path moves a coordinate of an - unsupported joint type (quaternion floating, ball).)"""; - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc_4args = -R"""(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. - -Raises: - RuntimeError 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).)"""; - } ComputeMotionBoundTable; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::CoordinatesAffectingPair - struct /* CoordinatesAffectingPair */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(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.)"""; - } CoordinatesAffectingPair; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::KinematicsEngine - struct /* ctor */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(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 (constant-coordinate carve-out, the -joint-support scope). - -``model`` is aliased and must outlive this object. - -Raises: - RuntimeError if a HalfSpace geometry is on the *distal* side of a - rotational coordinate relative to an unfiltered partner (unbounded - reach). A HalfSpace that is merely the static partner of a - rotating body — the anchored ground plane under a robot arm, the - overwhelmingly common case — is accepted: λ then bounds the - partner's points, and signed distance is symmetric, so the - certificate still holds. - -Raises: - RuntimeError if the plant is not finalized, if a joint is - "reversed" (its declared parent body is outboard of its declared - child body in the multibody tree — a documented v1 exclusion), or - if any proximity geometry has a shape ComputeBoundingSphere() - rejects.)"""; - } ctor; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_has_halfspace - struct /* body_has_halfspace */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(True iff ``body`` carries at least one HalfSpace proximity geometry.)"""; - } body_has_halfspace; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::body_radius - struct /* body_radius */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Radius, about the body frame origin, of a sphere containing every -proximity geometry of ``body`` — the start of the reach chain. Zero -for a body with no (non-HalfSpace) proximity geometry.)"""; - } body_radius; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::geometry_sphere - struct /* geometry_sphere */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(The bounding sphere (in its body's frame) of one proximity geometry. - -Raises: - RuntimeError if ``id`` is not a proximity geometry of this model - or is a HalfSpace (which has none).)"""; - } geometry_sphere; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::num_positions - struct /* num_positions */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = R"""()"""; - } num_positions; - // Symbol: drake::planning::continuous_collision::KinematicsEngine::plant - struct /* plant */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = R"""()"""; - } plant; - } KinematicsEngine; - // Symbol: drake::planning::continuous_collision::MotionBoundTable - struct /* MotionBoundTable */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Per-pair motion-bound coefficients in CSR layout (the displacement -lemma): 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. λ has units of meters of worst-case point -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 -(trajectory normalization; the joint-support scope) removed from J(p). -"Constant" there is a *tolerance* — a coordinate whose global -control-box range is at most Options∷continuity_tolerance — not an -identity, so a carved coordinate may still displace the pair's distal -side by up to λ̃_j · range_j. That residual is charged unconditionally -inside MotionBound(), which is what makes Δ_p a true upper bound on -the pair's relative motion over the whole trajectory rather than one -that ignores the carved coordinates. It is exactly zero — bit for bit -— whenever every carved coordinate is *exactly* constant, which is the -case for every path whose control points repeat a coordinate's value -verbatim.)"""; - // Symbol: drake::planning::continuous_collision::MotionBoundTable::GetEntries - struct /* GetEntries */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Introspection for tests: the (coordinate, λ) entries of one pair, -ordered by increasing coordinate index.)"""; - } GetEntries; - // Symbol: drake::planning::continuous_collision::MotionBoundTable::MotionBound - struct /* MotionBound */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j — a sparse dot -product against the node's per-coordinate deviations w, plus the -carved coordinates' residual (the interval certificate, requirement -P3).)"""; - } MotionBound; - // Symbol: drake::planning::continuous_collision::MotionBoundTable::MotionBoundTable - struct /* ctor */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc_0args = R"""(Constructs an empty table (zero pairs).)"""; - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc_4args = -R"""(Constructs the CSR table directly from its four arrays. - -Parameter ``row_start``: - Size num_pairs + 1, starting at 0 and non-decreasing; - row_start.back() is the total entry count. - -Parameter ``coord``: - Position-coordinate index of every entry. - -Parameter ``lambda``: - λ of every entry, element for element with ``coord``. - -Parameter ``carveout_slack``: - One residual per pair. - -Raises: - RuntimeError if the arrays do not satisfy those invariants.)"""; - } ctor; - // Symbol: drake::planning::continuous_collision::MotionBoundTable::carveout_slack - struct /* carveout_slack */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(Σ 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. Zero when every carved -coordinate is exactly constant.)"""; - } carveout_slack; - // Symbol: drake::planning::continuous_collision::MotionBoundTable::num_pairs - struct /* num_pairs */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = R"""()"""; - } num_pairs; - // Symbol: drake::planning::continuous_collision::MotionBoundTable::pair_is_static - struct /* pair_is_static */ { - // Source: drake/planning/continuous_collision/motion_bound_table.h - const char* doc = -R"""(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.)"""; - } pair_is_static; - } MotionBoundTable; // Symbol: drake::planning::continuous_collision::Options struct /* Options */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Options controlling one certification call (the architecture; the -numerical policy).)"""; - // Symbol: drake::planning::continuous_collision::Options::certificate_slack - struct /* certificate_slack */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(ε_slack: swallows floating-point noise in the bound arithmetic.)"""; - } certificate_slack; - // Symbol: drake::planning::continuous_collision::Options::continuity_tolerance - struct /* continuity_tolerance */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Junction C0-continuity tolerance (per coordinate; modulo 2π for -coordinates listed in continuous_revolute_indices).)"""; - } continuity_tolerance; + // 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/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = R"""(Position coordinates whose junction continuity is checked modulo 2π -(GcsTrajectoryOptimization continuous-revolute convention). +(the GcsTrajectoryOptimization continuous-revolute convention). See also: planning∷trajectory_optimization∷GetContinuousRevoluteJointIndices)"""; } continuous_revolute_indices; - // Symbol: drake::planning::continuous_collision::Options::emit_certificate - struct /* emit_certificate */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(If true, every certification event is recorded into a Certificate that -VerifyCertificate() can independently replay (the search algorithm).)"""; - } emit_certificate; // Symbol: drake::planning::continuous_collision::Options::margin struct /* margin */ { - // Source: drake/planning/continuous_collision/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Global clearance margin δ in meters. The certificate proves signed -distance > margin + padding for every pair at every time.)"""; +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::max_conversion_degree - struct /* max_conversion_degree */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Maximum polynomial degree accepted for monomial→Bernstein conversion.)"""; - } max_conversion_degree; - // Symbol: drake::planning::continuous_collision::Options::max_nodes - struct /* max_nodes */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Optional node budget; exceeded ⇒ Verdict∷kBudgetExhausted.)"""; - } max_nodes; - // Symbol: drake::planning::continuous_collision::Options::max_reported_findings - struct /* max_reported_findings */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } max_reported_findings; // Symbol: drake::planning::continuous_collision::Options::min_interval struct /* min_interval */ { - // Source: drake/planning/continuous_collision/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Resolution floor as a fraction of a segment's parameter width; nodes -narrower than this become kInconclusive findings instead of splitting.)"""; +R"""(Resolution floor, as a fraction of a segment's parameter width; a node +narrower than this yields Verdict∷kInconclusive instead of splitting. +Must lie in (0, 1].)"""; } min_interval; - // Symbol: drake::planning::continuous_collision::Options::mode - struct /* mode */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } mode; // Symbol: drake::planning::continuous_collision::Options::parallelism struct /* parallelism */ { - // Source: drake/planning/continuous_collision/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = R"""()"""; } parallelism; - // Symbol: drake::planning::continuous_collision::Options::query_tolerance - struct /* query_tolerance */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(τ: the distance oracle's accuracy contract in meters (the -distance-oracle contract; the numerical policy).)"""; - } query_tolerance; } Options; - // Symbol: drake::planning::continuous_collision::PaddingSpec - struct /* PaddingSpec */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Per-body-pair padding: the effective threshold for pair p is margin + -padding(p). - -Which of the two scalars applies to a pair is decided by *anchoring*, -from plant topology alone. A body is anchored iff no position -coordinate of the plant changes its pose relative to the world — the -world body itself, and everything welded to it directly or -transitively. A pair is a self-collision pair iff both of its bodies -are non-anchored, and an environment pair otherwise. The rule never -depends on which trajectory is being checked.)"""; - // Symbol: drake::planning::continuous_collision::PaddingSpec::env_padding - struct /* env_padding */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Padding for robot-vs-environment pairs, i.e. pairs with at least one -anchored body.)"""; - } env_padding; - // Symbol: drake::planning::continuous_collision::PaddingSpec::per_body_pair - struct /* per_body_pair */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Optional dense symmetric matrix indexed by BodyIndex, sized num_bodies -× num_bodies. Entry (a, b) overrides the scalars for that body pair; a -NaN entry means "not covered", and that pair falls back to env_padding -/ self_padding.)"""; - } per_body_pair; - // Symbol: drake::planning::continuous_collision::PaddingSpec::self_padding - struct /* self_padding */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Padding for robot-vs-robot (self-collision) pairs, i.e. pairs whose -two bodies are both non-anchored.)"""; - } self_padding; - } PaddingSpec; - // Symbol: drake::planning::continuous_collision::PairId - struct /* PairId */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Identifies an unfiltered proximity geometry pair.)"""; - // Symbol: drake::planning::continuous_collision::PairId::a - struct /* a */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } a; - // Symbol: drake::planning::continuous_collision::PairId::b - struct /* b */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } b; - // Symbol: drake::planning::continuous_collision::PairId::body_a - struct /* body_a */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } body_a; - // Symbol: drake::planning::continuous_collision::PairId::body_b - struct /* body_b */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } body_b; - } PairId; - // Symbol: drake::planning::continuous_collision::PairRecord - struct /* PairRecord */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(One unfiltered proximity pair with its pre-resolved distance route and -effective threshold m_p = margin + padding(p).)"""; - // Symbol: drake::planning::continuous_collision::PairRecord::id - struct /* id */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = R"""()"""; - } id; - // Symbol: drake::planning::continuous_collision::PairRecord::route - struct /* route */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = R"""()"""; - } route; - // Symbol: drake::planning::continuous_collision::PairRecord::threshold - struct /* threshold */ { - // Source: drake/planning/continuous_collision/distance_oracle.h - const char* doc = -R"""(Filled by the facade from margin + PaddingSpec.)"""; - } threshold; - } PairRecord; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath - struct /* PiecewiseBezierPath */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(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 (trajectory normalization). - -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).)"""; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::EvaluateSegment - struct /* EvaluateSegment */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(Evaluates segment ``segment_index`` at local parameter s ∈ [0, 1].)"""; - } EvaluateSegment; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::FromTrajectory - struct /* FromTrajectory */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(Normalizes any supported Drake trajectory (BezierCurve, -CompositeTrajectory, BsplineTrajectory via knot insertion, -PiecewisePolynomial via monomial→Bernstein change of basis). - -Raises: - RuntimeError on unsupported segment types, degree above - options.max_conversion_degree, or junction discontinuity beyond - options.continuity_tolerance (modulo 2π for coordinates in - options.continuous_revolute_indices).)"""; - } FromTrajectory; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::FromWaypoints - struct /* FromWaypoints */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(Normalizes an n × K waypoint matrix into K−1 order-1 segments (exact). -Segment k spans time [k, k+1]. - -Raises: - RuntimeError if K < 2.)"""; - } FromWaypoints; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::PiecewiseBezierPath - struct /* ctor */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = R"""()"""; - } ctor; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::Value - struct /* Value */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(Evaluates the path at time t (for tests and breakpoint checks; the hot -loop never calls this — it uses de Casteljau apexes).)"""; - } Value; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::constant_coordinates - struct /* constant_coordinates */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(True for coordinates whose value is identical (within the continuity -tolerance) across all control points of all segments; such coordinates -are treated as welded for the check (trajectory normalization; the -joint-support scope).)"""; - } constant_coordinates; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::end_time - struct /* end_time */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = R"""()"""; - } end_time; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::global_lower_bound - struct /* global_lower_bound */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = -R"""(Per-coordinate global control-point box over all segments (trajectory -normalization); used for trajectory-adaptive prismatic reach bounds.)"""; - } global_lower_bound; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::global_upper_bound - struct /* global_upper_bound */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = R"""()"""; - } global_upper_bound; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::num_positions - struct /* num_positions */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = R"""()"""; - } num_positions; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::segments - struct /* segments */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = R"""()"""; - } segments; - // Symbol: drake::planning::continuous_collision::PiecewiseBezierPath::start_time - struct /* start_time */ { - // Source: drake/planning/continuous_collision/piecewise_bezier_path.h - const char* doc = R"""()"""; - } start_time; - } PiecewiseBezierPath; - // Symbol: drake::planning::continuous_collision::SearchMode - struct /* SearchMode */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Search modes for certification (the search algorithm).)"""; - // Symbol: drake::planning::continuous_collision::SearchMode::kCertifyAll - struct /* kCertifyAll */ { - // Source: drake/planning/continuous_collision/options.h + // 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"""(Certify the full domain and return every violation / inconclusive -region found (bounded by Options∷max_reported_findings).)"""; - } kCertifyAll; - // Symbol: drake::planning::continuous_collision::SearchMode::kFindFirstViolation - struct /* kFindFirstViolation */ { - // Source: drake/planning/continuous_collision/options.h +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"""(Return on the first definite violation; serial execution returns the -earliest one in time.)"""; - } kFindFirstViolation; - } SearchMode; - // Symbol: drake::planning::continuous_collision::Statistics - struct /* Statistics */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Cost accounting for one certification call.)"""; - // Symbol: drake::planning::continuous_collision::Statistics::max_depth - struct /* max_depth */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } max_depth; - // Symbol: drake::planning::continuous_collision::Statistics::narrowphase_queries - struct /* narrowphase_queries */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } narrowphase_queries; - // Symbol: drake::planning::continuous_collision::Statistics::nodes - struct /* nodes */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } nodes; - // Symbol: drake::planning::continuous_collision::Statistics::sphere_certifications - struct /* sphere_certifications */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = R"""()"""; - } sphere_certifications; - // Symbol: drake::planning::continuous_collision::Statistics::wall_time_s - struct /* wall_time_s */ { - // Source: drake/planning/continuous_collision/options.h +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"""()"""; - } wall_time_s; - } Statistics; + } verdict; + } Result; // Symbol: drake::planning::continuous_collision::Verdict struct /* Verdict */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(Outcome of a certification run (the problem statement).)"""; - // Symbol: drake::planning::continuous_collision::Verdict::kBudgetExhausted - struct /* kBudgetExhausted */ { - // Source: drake/planning/continuous_collision/options.h - const char* doc = -R"""(The optional node budget was exhausted first.)"""; - } kBudgetExhausted; + // 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/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Proof: every unfiltered pair keeps signed distance > margin + padding -over the entire continuous time domain.)"""; +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/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = R"""(Subdivision hit the resolution floor with some pair's clearance within -oracle tolerance of the threshold (grazing trajectory).)"""; +oracle tolerance of the threshold (a grazing trajectory).)"""; } kInconclusive; // Symbol: drake::planning::continuous_collision::Verdict::kViolationFound struct /* kViolationFound */ { - // Source: drake/planning/continuous_collision/options.h + // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = R"""(An exactly-on-trajectory configuration violates the threshold.)"""; } kViolationFound; } Verdict; - // Symbol: drake::planning::continuous_collision::VerifyCertificate - struct /* VerifyCertificate */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = -R"""(Independently replays every record of ``certificate`` (recomputing -node control boxes from freshly restricted control points and -re-querying distances) and checks interval coverage of the full domain -for every pair. Returns true iff the certificate holds (the search -algorithm).)"""; - } VerifyCertificate; } continuous_collision; } planning; } drake; diff --git a/bindings/pydrake/planning/planning_py_continuous_collision.cc b/bindings/pydrake/planning/planning_py_continuous_collision.cc index 7a8de8b2d0be..d393d7215c4a 100644 --- a/bindings/pydrake/planning/planning_py_continuous_collision.cc +++ b/bindings/pydrake/planning/planning_py_continuous_collision.cc @@ -1,21 +1,9 @@ -#include -#include -#include #include -#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/bounding_sphere.h" -#include "drake/planning/continuous_collision/certificate.h" #include "drake/planning/continuous_collision/continuous_collision_checker.h" -#include "drake/planning/continuous_collision/distance_oracle.h" -#include "drake/planning/continuous_collision/motion_bound_table.h" -#include "drake/planning/continuous_collision/numerics.h" -#include "drake/planning/continuous_collision/options.h" -#include "drake/planning/continuous_collision/piecewise_bezier_path.h" -#include "drake/planning/continuous_collision/vpolytope_ingestion.h" #include "drake/planning/robot_diagram.h" namespace drake { @@ -35,16 +23,6 @@ Certified continuous collision checking: proves that a trajectory is collision-free over its entire continuous time domain, rather than sampling it. )"""; - // options.h - { - using Class = SearchMode; - constexpr auto& cls_doc = doc.SearchMode; - py::enum_(m, "SearchMode", cls_doc.doc) - .value("kFindFirstViolation", Class::kFindFirstViolation, - cls_doc.kFindFirstViolation.doc) - .value("kCertifyAll", Class::kCertifyAll, cls_doc.kCertifyAll.doc); - } - { using Class = Verdict; constexpr auto& cls_doc = doc.Verdict; @@ -53,67 +31,8 @@ collision-free over its entire continuous time domain, rather than sampling it. "kCertifiedFree", Class::kCertifiedFree, cls_doc.kCertifiedFree.doc) .value("kViolationFound", Class::kViolationFound, cls_doc.kViolationFound.doc) - .value("kInconclusive", Class::kInconclusive, cls_doc.kInconclusive.doc) - .value("kBudgetExhausted", Class::kBudgetExhausted, - cls_doc.kBudgetExhausted.doc); - } - - { - 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("continuity_tolerance", &Class::continuity_tolerance, - cls_doc.continuity_tolerance.doc) - .def_rw("query_tolerance", &Class::query_tolerance, - cls_doc.query_tolerance.doc) - .def_rw("certificate_slack", &Class::certificate_slack, - cls_doc.certificate_slack.doc) - .def_rw("min_interval", &Class::min_interval, cls_doc.min_interval.doc) - .def_rw("continuous_revolute_indices", - &Class::continuous_revolute_indices, - cls_doc.continuous_revolute_indices.doc) - .def_rw("max_conversion_degree", &Class::max_conversion_degree, - cls_doc.max_conversion_degree.doc) - .def_rw("mode", &Class::mode, cls_doc.mode.doc) - .def_rw("max_reported_findings", &Class::max_reported_findings, - cls_doc.max_reported_findings.doc) - .def_rw("max_nodes", &Class::max_nodes, cls_doc.max_nodes.doc) - .def_rw("emit_certificate", &Class::emit_certificate, - cls_doc.emit_certificate.doc) - .def_rw("parallelism", &Class::parallelism, cls_doc.parallelism.doc); - DefCopyAndDeepCopy(&cls); - } - - { - using Class = PaddingSpec; - constexpr auto& cls_doc = doc.PaddingSpec; - class_ cls(m, "PaddingSpec", cls_doc.doc); - cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_rw("env_padding", &Class::env_padding, cls_doc.env_padding.doc) - .def_rw("self_padding", &Class::self_padding, cls_doc.self_padding.doc) - .def_rw( - "per_body_pair", &Class::per_body_pair, cls_doc.per_body_pair.doc); - DefCopyAndDeepCopy(&cls); - } - - { - using Class = PairId; - constexpr auto& cls_doc = doc.PairId; - class_ cls(m, "PairId", cls_doc.doc); - cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_rw("a", &Class::a, cls_doc.a.doc) - .def_rw("b", &Class::b, cls_doc.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); - DefCopyAndDeepCopy(&cls); + .value( + "kInconclusive", Class::kInconclusive, cls_doc.kInconclusive.doc); } { @@ -125,264 +44,42 @@ collision-free over its entire continuous time domain, rather than sampling it. .def(ParamInit()) .def_rw("time", &Class::time, cls_doc.time.doc) .def_rw("q", &Class::q, cls_doc.q.doc) - .def_rw("pair", &Class::pair, cls_doc.pair.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("motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) - .def_rw("definite", &Class::definite, cls_doc.definite.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 = Statistics; - constexpr auto& cls_doc = doc.Statistics; - class_ cls(m, "Statistics", cls_doc.doc); - cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_rw("nodes", &Class::nodes, cls_doc.nodes.doc) - .def_rw("narrowphase_queries", &Class::narrowphase_queries, - cls_doc.narrowphase_queries.doc) - .def_rw("sphere_certifications", &Class::sphere_certifications, - cls_doc.sphere_certifications.doc) - .def_rw("max_depth", &Class::max_depth, cls_doc.max_depth.doc) - .def_rw("wall_time_s", &Class::wall_time_s, cls_doc.wall_time_s.doc); - DefCopyAndDeepCopy(&cls); - } - - // bounding_sphere.h - { - using Class = BoundingSphere; - constexpr auto& cls_doc = doc.BoundingSphere; - class_ cls(m, "BoundingSphere", cls_doc.doc); - cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_rw("center_L", &Class::center_L, cls_doc.center_L.doc) - .def_rw("radius", &Class::radius, cls_doc.radius.doc); - DefCopyAndDeepCopy(&cls); - } - - m.def("ComputeBoundingSphere", &ComputeBoundingSphere, py::arg("shape"), - py::arg("X_LG"), doc.ComputeBoundingSphere.doc); - - // piecewise_bezier_path.h - { - using Class = BezierSegment; - constexpr auto& cls_doc = doc.BezierSegment; - class_ cls(m, "BezierSegment", cls_doc.doc); - cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_rw("t_start", &Class::t_start, cls_doc.t_start.doc) - .def_rw("t_end", &Class::t_end, cls_doc.t_end.doc) - .def_rw("control_points", &Class::control_points, - cls_doc.control_points.doc); - DefCopyAndDeepCopy(&cls); - } - - { - using Class = PiecewiseBezierPath; - constexpr auto& cls_doc = doc.PiecewiseBezierPath; - class_ cls(m, "PiecewiseBezierPath", cls_doc.doc); - cls // BR - .def_static("FromTrajectory", &Class::FromTrajectory, - py::arg("trajectory"), py::arg("options"), - cls_doc.FromTrajectory.doc) - .def_static("FromWaypoints", &Class::FromWaypoints, - py::arg("waypoints"), py::arg("options"), cls_doc.FromWaypoints.doc) - .def("num_positions", &Class::num_positions, cls_doc.num_positions.doc) - .def("segments", &Class::segments, cls_doc.segments.doc) - .def("start_time", &Class::start_time, cls_doc.start_time.doc) - .def("end_time", &Class::end_time, cls_doc.end_time.doc) - .def("global_lower_bound", &Class::global_lower_bound, - cls_doc.global_lower_bound.doc) - .def("global_upper_bound", &Class::global_upper_bound, - cls_doc.global_upper_bound.doc) - .def("constant_coordinates", &Class::constant_coordinates, - cls_doc.constant_coordinates.doc) - .def("Value", &Class::Value, py::arg("t"), cls_doc.Value.doc) - .def("EvaluateSegment", &Class::EvaluateSegment, - py::arg("segment_index"), py::arg("s"), - cls_doc.EvaluateSegment.doc); - DefCopyAndDeepCopy(&cls); - } - - m.def( - "DeCasteljauSplitAtHalf", - [](const Eigen::MatrixXd& cps) { - Eigen::MatrixXd left; - Eigen::MatrixXd right; - Eigen::VectorXd mid; - DeCasteljauSplitAtHalf(cps, &left, &right, &mid); - return std::make_tuple( - std::move(left), std::move(right), std::move(mid)); - }, - py::arg("cps"), - (std::string(doc.DeCasteljauSplitAtHalf.doc) + - "\n\n" - "Note:\n" - " Unlike the C++ signature, which writes through output " - "pointers, this returns a tuple ``(left, right, mid)``.") - .c_str()); - - // motion_bound_table.h - { - using Class = MotionBoundTable; - constexpr auto& cls_doc = doc.MotionBoundTable; - class_ cls(m, "MotionBoundTable", cls_doc.doc); - cls // BR - .def(py::init<>(), cls_doc.ctor.doc_0args) - .def(py::init, std::vector, std::vector, - std::vector>(), - py::arg("row_start"), py::arg("coord"), py::arg("lambda"), - py::arg("carveout_slack"), cls_doc.ctor.doc_4args) - .def("num_pairs", &Class::num_pairs, cls_doc.num_pairs.doc) - .def("pair_is_static", &Class::pair_is_static, py::arg("pair_index"), - cls_doc.pair_is_static.doc) - .def("MotionBound", &Class::MotionBound, py::arg("pair_index"), - py::arg("w"), cls_doc.MotionBound.doc) - .def("carveout_slack", &Class::carveout_slack, py::arg("pair_index"), - cls_doc.carveout_slack.doc) - .def("GetEntries", &Class::GetEntries, py::arg("pair_index"), - cls_doc.GetEntries.doc); - DefCopyAndDeepCopy(&cls); - } - - { - using Class = KinematicsEngine; - constexpr auto& cls_doc = doc.KinematicsEngine; - class_ cls(m, "KinematicsEngine", cls_doc.doc); - cls // BR - .def(py::init&>(), py::arg("model"), - // Keep the model alive as long as the engine: the C++ object - // aliases it (see the constructor's documentation). - py::keep_alive<1, 2>(), cls_doc.ctor.doc) - .def("CoordinatesAffectingPair", &Class::CoordinatesAffectingPair, - py::arg("body_a"), py::arg("body_b"), - cls_doc.CoordinatesAffectingPair.doc) - .def("ComputeMotionBoundTable", - overload_cast_explicit&>(&Class::ComputeMotionBoundTable), - py::arg("path"), py::arg("pairs"), - cls_doc.ComputeMotionBoundTable.doc_2args) - .def("ComputeMotionBoundTable", - overload_cast_explicit&, - const std::vector&>(&Class::ComputeMotionBoundTable), - py::arg("lower"), py::arg("upper"), py::arg("constant_coordinates"), - py::arg("pairs"), cls_doc.ComputeMotionBoundTable.doc_4args) - .def("geometry_sphere", &Class::geometry_sphere, py::arg("id"), - cls_doc.geometry_sphere.doc) - .def("body_has_halfspace", &Class::body_has_halfspace, py::arg("body"), - cls_doc.body_has_halfspace.doc) - .def("body_radius", &Class::body_radius, py::arg("body"), - cls_doc.body_radius.doc) - .def("num_positions", &Class::num_positions, cls_doc.num_positions.doc) - .def("plant", &Class::plant, py_rvp::reference_internal, - cls_doc.plant.doc); - } - - // distance_oracle.h - { - using Class = DistanceRoute; - constexpr auto& cls_doc = doc.DistanceRoute; - py::enum_(m, "DistanceRoute", cls_doc.doc) - .value("kNative", Class::kNative, cls_doc.kNative.doc) - .value("kHalfSpaceA", Class::kHalfSpaceA, cls_doc.kHalfSpaceA.doc) - .value("kHalfSpaceB", Class::kHalfSpaceB, cls_doc.kHalfSpaceB.doc); - } - - { - using Class = PairRecord; - constexpr auto& cls_doc = doc.PairRecord; - class_ cls(m, "PairRecord", cls_doc.doc); - cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_rw("id", &Class::id, cls_doc.id.doc) - .def_rw("route", &Class::route, cls_doc.route.doc) - .def_rw("threshold", &Class::threshold, cls_doc.threshold.doc); - DefCopyAndDeepCopy(&cls); - } - - { - using Class = DistanceOracle; - constexpr auto& cls_doc = doc.DistanceOracle; - class_ cls(m, "DistanceOracle", cls_doc.doc); - cls // BR - .def(py::init&, double>(), py::arg("model"), - py::arg("query_tolerance"), cls_doc.ctor.doc) - .def("pairs", &Class::pairs, cls_doc.pairs.doc) - .def( - "SignedDistance", - [](const Class& self, - const geometry::QueryObject& query_object, - const PairRecord& pair) { - Eigen::Vector3d nearest_a_W = Eigen::Vector3d::Zero(); - Eigen::Vector3d nearest_b_W = Eigen::Vector3d::Zero(); - const double distance = self.SignedDistance( - query_object, pair, &nearest_a_W, &nearest_b_W); - return std::make_tuple(distance, nearest_a_W, nearest_b_W); - }, - py::arg("query_object"), py::arg("pair"), - (std::string(cls_doc.SignedDistance.doc) + - "\n\n" - "Note:\n" - " Unlike the C++ signature, which reports the closest " - "points through optional output pointers, this returns a " - "tuple ``(distance, nearest_a_W, nearest_b_W)``.") - .c_str()) - .def("tolerance", &Class::tolerance, cls_doc.tolerance.doc) - .def("support_report", &Class::support_report, - cls_doc.support_report.doc); - DefCopyAndDeepCopy(&cls); - } - - // certificate.h - { - using Class = CertificateRecord; - constexpr auto& cls_doc = doc.CertificateRecord; - class_ cls(m, "CertificateRecord", cls_doc.doc); - cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_rw("segment", &Class::segment, cls_doc.segment.doc) - .def_rw("s_start", &Class::s_start, cls_doc.s_start.doc) - .def_rw("s_end", &Class::s_end, cls_doc.s_end.doc) - .def_rw("pair_index", &Class::pair_index, cls_doc.pair_index.doc) - .def_rw("qc", &Class::qc, cls_doc.qc.doc) - .def_rw("phi_hat", &Class::phi_hat, cls_doc.phi_hat.doc) - .def_rw("motion_bound", &Class::motion_bound, cls_doc.motion_bound.doc) - .def_rw("threshold", &Class::threshold, cls_doc.threshold.doc); - DefCopyAndDeepCopy(&cls); - } - - { - using Class = Certificate; - constexpr auto& cls_doc = doc.Certificate; - class_ cls(m, "Certificate", cls_doc.doc); + 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("records", &Class::records, cls_doc.records.doc) - .def_rw("pairs", &Class::pairs, cls_doc.pairs.doc); + .def_rw("margin", &Class::margin, cls_doc.margin.doc) + .def_rw("min_interval", &Class::min_interval, cls_doc.min_interval.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); } - // continuous_collision_checker.h { - using Class = CertificationResult; - constexpr auto& cls_doc = doc.CertificationResult; - class_ cls(m, "CertificationResult", cls_doc.doc); + 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("findings", &Class::findings, cls_doc.findings.doc) - .def_rw("stats", &Class::stats, cls_doc.stats.doc) - .def_rw("certificate", &Class::certificate, cls_doc.certificate.doc); + .def_rw("finding", &Class::finding, cls_doc.finding.doc) + .def_rw("num_nodes", &Class::num_nodes, cls_doc.num_nodes.doc); DefCopyAndDeepCopy(&cls); } @@ -390,94 +87,32 @@ collision-free over its entire continuous time domain, rather than sampling it. using Class = ContinuousCollisionChecker; constexpr auto& cls_doc = doc.ContinuousCollisionChecker; class_ cls(m, "ContinuousCollisionChecker", cls_doc.doc); - - { - using Nested = Class::Params; - constexpr auto& nested_doc = cls_doc.Params; - class_ nested_cls(cls, "Params", nested_doc.doc); - nested_cls // BR - .def(py::init<>()) - .def(ParamInit()) - .def_prop_rw( - "model", - [](const Nested& self) -> const RobotDiagram* { - return self.model.get(); - }, - [](Nested& self, py::object model) { - // Add a python reference to model (owned by the shared - // pointer), and transfer that to the c++ params struct. - self.model = - make_shared_ptr_from_py_object>(model); - }, - nested_doc.model.doc) - .def_rw("padding", &Nested::padding, nested_doc.padding.doc) - .def_rw("default_options", &Nested::default_options, - nested_doc.default_options.doc); - } - - py::object params_ctor = cls.attr("Params"); cls // BR .def( "__init__", - [params_ctor]( - Class* self, py::object model, const py::kwargs& kwargs) { - // For lifetime management, we need to treat pointer-like - // arguments separately. Start by creating a Params object in - // Python with all of the other non-pointer kwargs. - py::object params_py = params_ctor(**kwargs); - auto* params = py::cast(params_py); - DRAKE_DEMAND(params != nullptr); - // Now, add a python reference to model (owned by the shared - // pointer), and transfer that to the c++ checker. - params->model = - make_shared_ptr_from_py_object>(model); - new (self) Class(std::move(*params)); + [](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"), -#ifdef PYDRAKE_USE_NANOBIND - py::arg("kwargs"), -#endif - (std::string(cls_doc.ctor.doc) + - "\n\n" - "See :class:`pydrake.planning.continuous_collision" - ".ContinuousCollisionChecker.Params` for the list of " - "properties available here as kwargs.") - .c_str()) - .def(py::init(), py::arg("params"), cls_doc.ctor.doc) + py::arg("default_options") = Options{}, cls_doc.ctor.doc) .def("CheckTrajectory", &Class::CheckTrajectory, py::arg("trajectory"), - py::arg("options") = std::nullopt, cls_doc.CheckTrajectory.doc) + py::arg("options") = std::nullopt, + py::call_guard(), + cls_doc.CheckTrajectory.doc) .def("CheckPath", &Class::CheckPath, py::arg("waypoints"), - py::arg("options") = std::nullopt, cls_doc.CheckPath.doc) + 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, cls_doc.CheckEdge.doc) - .def("Normalize", &Class::Normalize, py::arg("trajectory"), - py::arg("options") = std::nullopt, cls_doc.Normalize.doc) - .def("ComputeMotionBounds", &Class::ComputeMotionBounds, - py::arg("path"), cls_doc.ComputeMotionBounds.doc) - .def("distance_oracle", &Class::distance_oracle, - py_rvp::reference_internal, cls_doc.distance_oracle.doc) - .def("kinematics_engine", &Class::kinematics_engine, - py_rvp::reference_internal, cls_doc.kinematics_engine.doc) - .def("pairs", &Class::pairs, cls_doc.pairs.doc) + py::arg("options") = std::nullopt, + py::call_guard(), cls_doc.CheckEdge.doc) .def("model", &Class::model, py_rvp::reference_internal, cls_doc.model.doc); } - - m.def("VerifyCertificate", &VerifyCertificate, py::arg("checker"), - py::arg("path"), py::arg("certificate"), doc.VerifyCertificate.doc); - - // vpolytope_ingestion.h - m.def("AddVPolytopeObstacle", &AddVPolytopeObstacle, py::arg("plant"), - py::arg("vpoly"), py::arg("X_WG"), py::arg("name"), - doc.AddVPolytopeObstacle.doc); - - // numerics.h - m.def("IsCertified", &IsCertified, py::arg("phi_hat"), py::arg("tau"), - py::arg("motion_bound"), py::arg("threshold"), py::arg("slack"), - doc.IsCertified.doc); - - m.def("IsDefiniteViolation", &IsDefiniteViolation, py::arg("phi_hat"), - py::arg("tau"), py::arg("threshold"), doc.IsDefiniteViolation.doc); } } // namespace internal diff --git a/bindings/pydrake/planning/test/continuous_collision_test.py b/bindings/pydrake/planning/test/continuous_collision_test.py index 43553fd2bf49..f4ac0b9bde1d 100644 --- a/bindings/pydrake/planning/test/continuous_collision_test.py +++ b/bindings/pydrake/planning/test/continuous_collision_test.py @@ -6,7 +6,6 @@ from pydrake.common import Parallelism from pydrake.geometry import Box, Sphere -from pydrake.geometry.optimization import VPolytope from pydrake.math import RigidTransform from pydrake.multibody.plant import CoulombFriction from pydrake.multibody.tree import ( @@ -17,7 +16,7 @@ UnitInertia, ) from pydrake.planning import RobotDiagramBuilder -from pydrake.trajectories import BezierCurve +from pydrake.trajectories import PiecewisePolynomial def _inertia(): @@ -28,11 +27,14 @@ def _inertia(): ) -def _make_arm_builder(): - """A planar 2-dof arm (revolute, then prismatic) with one anchored post - obstacle -- the same world planning/continuous_collision/test/api_test.cc - uses, so the verdicts asserted below match the C++ suite. Returns the - not-yet-built RobotDiagramBuilder (its plant is not finalized). +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() @@ -66,14 +68,14 @@ def _make_arm_builder(): X_BG=RigidTransform([0.15, 0.0, 0.0]), shape=Box(0.30, 0.05, 0.05), name="link_geom", - coulomb_friction=CoulombFriction(1.0, 1.0), + coulomb_friction=_friction(), ) plant.RegisterCollisionGeometry( body=tool, X_BG=RigidTransform(), shape=Sphere(0.04), name="tool_geom", - coulomb_friction=CoulombFriction(1.0, 1.0), + coulomb_friction=_friction(), ) post = plant.AddRigidBody(name="post", M_BBo_B=_inertia()) plant.WeldFrames( @@ -86,271 +88,68 @@ def _make_arm_builder(): X_BG=RigidTransform(), shape=Sphere(0.08), name="post_geom", - coulomb_friction=CoulombFriction(1.0, 1.0), + coulomb_friction=_friction(), ) - return builder - - -def _serial_options(): - options = mut.Options() - options.parallelism = Parallelism(num_threads=1) - return options + return builder.Build() class TestContinuousCollision(unittest.TestCase): def setUp(self): - self.model = _make_arm_builder().Build() + self.model = _make_model() + options = mut.Options() + options.parallelism = Parallelism(num_threads=1) self.checker = mut.ContinuousCollisionChecker( - model=self.model, default_options=_serial_options() + model=self.model, default_options=options ) - def test_options(self): - """Exercises the Options / PaddingSpec / enum surface.""" + def test_options_round_trip(self): dut = mut.Options() self.assertEqual(dut.margin, 0.0) - self.assertEqual(dut.mode, mut.SearchMode.kCertifyAll) - self.assertIsNone(dut.max_nodes) dut.margin = 0.01 - dut.continuity_tolerance = 1e-6 - dut.query_tolerance = 1e-5 - dut.certificate_slack = 1e-8 dut.min_interval = 1e-8 dut.continuous_revolute_indices = [0] - dut.max_conversion_degree = 8 - dut.mode = mut.SearchMode.kFindFirstViolation - dut.max_reported_findings = 4 - dut.max_nodes = 10000 - dut.emit_certificate = True - dut.parallelism = Parallelism(num_threads=1) + dut.parallelism = Parallelism(num_threads=2) self.assertEqual(dut.margin, 0.01) - self.assertEqual(dut.max_nodes, 10000) - self.assertTrue(dut.emit_certificate) - self.assertEqual(dut.parallelism.num_threads(), 1) + self.assertEqual(dut.min_interval, 1e-8) self.assertEqual(dut.continuous_revolute_indices, [0]) + self.assertEqual(dut.parallelism.num_threads(), 2) + self.assertIsInstance(mut.Options(margin=0.02), mut.Options) - # kwargs-init round trip. - kwargs_dut = mut.Options(margin=0.02, max_reported_findings=7) - self.assertEqual(kwargs_dut.margin, 0.02) - self.assertEqual(kwargs_dut.max_reported_findings, 7) - - padding = mut.PaddingSpec(env_padding=0.001, self_padding=0.002) - self.assertEqual(padding.env_padding, 0.001) - self.assertEqual(padding.self_padding, 0.002) - self.assertIsNone(padding.per_body_pair) - - # The enums are complete. - self.assertEqual(len(mut.Verdict.__members__), 4) - self.assertEqual(len(mut.SearchMode.__members__), 2) - self.assertEqual(len(mut.DistanceRoute.__members__), 3) - - def test_params_and_introspection(self): - params = mut.ContinuousCollisionChecker.Params() - params.model = self.model - params.padding = mut.PaddingSpec(env_padding=0.0) - params.default_options = _serial_options() - self.assertIs(params.model, self.model) - checker = mut.ContinuousCollisionChecker(params=params) - - self.assertIs(checker.model(), self.model) - self.assertGreater(len(checker.pairs()), 0) - self.assertIsInstance(checker.pairs()[0], mut.PairRecord) - self.assertIsInstance(checker.pairs()[0].id, mut.PairId) - self.assertIsInstance(checker.pairs()[0].route, mut.DistanceRoute) - - oracle = checker.distance_oracle() - self.assertIsInstance(oracle, mut.DistanceOracle) - self.assertGreater(oracle.tolerance(), 0.0) - self.assertIsInstance(oracle.support_report(), str) - self.assertGreater(len(oracle.support_report()), 0) - - engine = checker.kinematics_engine() - self.assertIsInstance(engine, mut.KinematicsEngine) - self.assertEqual(engine.num_positions(), 2) - pair = checker.pairs()[0].id - coords = engine.CoordinatesAffectingPair( - body_a=pair.body_a, body_b=pair.body_b - ) - self.assertIsInstance(coords, list) + def test_model(self): + self.assertIs(self.checker.model(), self.model) - def test_check_edge_free_and_colliding(self): - """A free edge certifies; a sweep past the post reports a violation.""" - free = self.checker.CheckEdge(q1=[0.0, 0.0], q2=[0.3, 0.05]) - self.assertEqual(free.verdict, mut.Verdict.kCertifiedFree) - self.assertEqual(len(free.findings), 0) - self.assertGreater(free.stats.nodes, 0) - self.assertGreaterEqual(free.stats.max_depth, 0) - self.assertIsNone(free.certificate) - - # Sweeping theta from 0 to 2.4 rad with the tool extended drives the - # tool sphere through the anchored post. - hit = self.checker.CheckEdge(q1=[0.0, 0.25], q2=[2.4, 0.25]) - self.assertEqual(hit.verdict, mut.Verdict.kViolationFound) - self.assertGreater(len(hit.findings), 0) - finding = hit.findings[0] - self.assertIsInstance(finding, mut.Finding) - self.assertTrue(finding.definite) - self.assertEqual(len(finding.q), 2) - self.assertIsInstance(finding.pair, mut.PairId) - self.assertLess(finding.distance, 1.0) - - def test_check_path_and_trajectory(self): - waypoints = np.array([[0.0, 0.3], [0.0, 0.05]]) - result = self.checker.CheckPath(waypoints=waypoints) + 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) - trajectory = BezierCurve(0.0, 1.0, waypoints) - result = self.checker.CheckTrajectory(trajectory=trajectory) - self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) - - # Normalize + ComputeMotionBounds introspection seams. - path = self.checker.Normalize(trajectory=trajectory) - self.assertIsInstance(path, mut.PiecewiseBezierPath) - table = self.checker.ComputeMotionBounds(path=path) - self.assertIsInstance(table, mut.MotionBoundTable) - self.assertEqual(table.num_pairs(), len(self.checker.pairs())) - w = np.full(path.num_positions(), 0.1) - self.assertGreaterEqual(table.MotionBound(pair_index=0, w=w), 0.0) - self.assertGreaterEqual(table.carveout_slack(pair_index=0), 0.0) - self.assertIsInstance(table.pair_is_static(pair_index=0), bool) - self.assertIsInstance(table.GetEntries(pair_index=0), list) - - def test_certificate_round_trip(self): - options = _serial_options() - options.emit_certificate = True - q1 = np.array([0.0, 0.0]) - q2 = np.array([0.3, 0.05]) - result = self.checker.CheckEdge(q1=q1, q2=q2, options=options) - self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) - certificate = result.certificate - self.assertIsInstance(certificate, mut.Certificate) - self.assertGreater(len(certificate.records), 0) - self.assertGreater(len(certificate.pairs), 0) - record = certificate.records[0] - self.assertIsInstance(record, mut.CertificateRecord) - self.assertGreaterEqual(record.s_end, record.s_start) - self.assertEqual(len(record.qc), 2) - - # CheckEdge normalizes exactly this waypoint matrix, so the replay - # runs against the same path the certificate was recorded on. - path = mut.PiecewiseBezierPath.FromWaypoints( - waypoints=np.column_stack([q1, q2]), options=options - ) - self.assertTrue( - mut.VerifyCertificate( - checker=self.checker, path=path, certificate=certificate - ) - ) - - # A tampered certificate must not verify. - tampered = mut.Certificate( - records=list(certificate.records), pairs=list(certificate.pairs) - ) - bad = tampered.records[0] - bad.phi_hat = bad.phi_hat + 100.0 - tampered.records = [bad] + list(tampered.records[1:]) - self.assertFalse( - mut.VerifyCertificate( - checker=self.checker, path=path, certificate=tampered - ) - ) - - def test_piecewise_bezier_path(self): - options = mut.Options() - waypoints = np.array([[0.0, 0.3, 0.6], [0.0, 0.05, 0.10]]) - dut = mut.PiecewiseBezierPath.FromWaypoints( - waypoints=waypoints, options=options - ) - self.assertEqual(dut.num_positions(), 2) - self.assertEqual(len(dut.segments()), 2) - self.assertIsInstance(dut.segments()[0], mut.BezierSegment) - self.assertEqual(dut.start_time(), 0.0) - self.assertEqual(dut.end_time(), 2.0) - np.testing.assert_allclose(dut.Value(t=0.0), waypoints[:, 0]) - np.testing.assert_allclose(dut.Value(t=2.0), waypoints[:, 2]) - np.testing.assert_allclose( - dut.EvaluateSegment(segment_index=0, s=0.0), waypoints[:, 0] - ) - np.testing.assert_allclose(dut.global_lower_bound(), waypoints[:, 0]) - np.testing.assert_allclose(dut.global_upper_bound(), waypoints[:, 2]) - self.assertEqual(len(dut.constant_coordinates()), 2) - - trajectory = BezierCurve(0.0, 1.0, waypoints) - from_traj = mut.PiecewiseBezierPath.FromTrajectory( + 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(from_traj.num_positions(), 2) - - # The out-params of DeCasteljauSplitAtHalf come back as a tuple. - left, right, mid = mut.DeCasteljauSplitAtHalf(cps=waypoints) - self.assertEqual(left.shape, waypoints.shape) - self.assertEqual(right.shape, waypoints.shape) - np.testing.assert_allclose(mid, from_traj.Value(t=0.5)) - - def test_bounding_sphere(self): - dut = mut.ComputeBoundingSphere( - shape=Sphere(0.25), X_LG=RigidTransform([1.0, 2.0, 3.0]) - ) - self.assertIsInstance(dut, mut.BoundingSphere) - self.assertEqual(dut.radius, 0.25) - np.testing.assert_allclose(dut.center_L, [1.0, 2.0, 3.0]) - - box = mut.ComputeBoundingSphere( - shape=Box(2.0, 2.0, 2.0), X_LG=RigidTransform() - ) - self.assertAlmostEqual(box.radius, np.sqrt(3.0)) - - def test_add_vpolytope_obstacle(self): - """AddVPolytopeObstacle runs on a pre-finalize plant.""" - builder = _make_arm_builder() - plant = builder.plant() - vertices = np.array( - [ - [0.0, 0.1, 0.0, 0.0], - [0.0, 0.0, 0.1, 0.0], - [0.0, 0.0, 0.0, 0.1], - ] - ) - geometry_id = mut.AddVPolytopeObstacle( - plant=plant, - vpoly=VPolytope(vertices), - X_WG=RigidTransform([0.0, -0.60, 0.0]), - name="vpoly_obstacle", - ) - self.assertIsNotNone(geometry_id) - # The new obstacle rides the ordinary narrowphase path, so a checker - # built on the finalized diagram picks it up as an extra pair. - model = builder.Build() - checker = mut.ContinuousCollisionChecker( - model=model, default_options=_serial_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]]) ) - ids = set() - for pair in checker.pairs(): - ids.add(pair.id.a) - ids.add(pair.id.b) - self.assertIn(geometry_id, ids) + self.assertEqual(result.verdict, mut.Verdict.kCertifiedFree) - def test_numerics(self): - self.assertTrue( - mut.IsCertified( - phi_hat=1.0, - tau=1e-6, - motion_bound=0.1, - threshold=0.0, - slack=1e-9, - ) - ) - self.assertFalse( - mut.IsCertified( - phi_hat=0.05, - tau=1e-6, - motion_bound=0.1, - threshold=0.0, - slack=1e-9, - ) - ) - self.assertTrue( - mut.IsDefiniteViolation(phi_hat=-0.1, tau=1e-6, threshold=0.0) - ) - self.assertFalse( - mut.IsDefiniteViolation(phi_hat=0.1, tau=1e-6, threshold=0.0) - ) + 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 index 62818568084e..27c8ec11b380 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -11,45 +11,21 @@ package(default_visibility = ["//visibility:public"]) drake_cc_package_library( name = "continuous_collision", visibility = ["//visibility:public"], - deps = [ - ":bounding_sphere", - ":certifier", - ":continuous_collision_checker", - ":distance_oracle", - ":motion_bound_table", - ":numerics", - ":options", - ":piecewise_bezier_path", - ":vpolytope_ingestion", - ], -) - -drake_cc_library( - name = "numerics", - hdrs = ["numerics.h"], + deps = [":continuous_collision_checker"], ) -# Shape classification shared by the oracle, the tau table and the -# bounding-sphere pass. Header-only and build-system internal. +# Numerical policy, shape classification and the pair record, shared by every +# translation unit here. drake_cc_library( - name = "shape_class", - hdrs = ["shape_class.h"], + name = "internal", + hdrs = ["internal.h"], internal = True, visibility = ["//visibility:private"], deps = [ "//common:unused", - "//geometry:shape_specification", - ], -) - -drake_cc_library( - name = "options", - hdrs = ["options.h"], - deps = [ - "//common:parallelism", "//geometry:geometry_ids", + "//geometry:shape_specification", "//multibody/tree:multibody_tree_indexes", - "@eigen", ], ) @@ -57,13 +33,15 @@ drake_cc_library( name = "piecewise_bezier_path", srcs = ["piecewise_bezier_path.cc"], hdrs = ["piecewise_bezier_path.h"], + internal = True, + visibility = ["//visibility:private"], deps = [ - ":options", "//common:essential", "//common/trajectories:trajectory", "@eigen", ], implementation_deps = [ + ":internal", "//common:nice_type_name", "//common/trajectories:bezier_curve", "//common/trajectories:bspline_trajectory", @@ -74,42 +52,29 @@ drake_cc_library( ], ) -drake_cc_library( - name = "bounding_sphere", - srcs = ["bounding_sphere.cc"], - hdrs = ["bounding_sphere.h"], - deps = [ - "//geometry:shape_specification", - "//math:geometric_transform", - "@eigen", - ], - implementation_deps = [ - "//common:essential", - "//geometry/proximity:polygon_surface_mesh", - "@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 = [ - ":bounding_sphere", - ":options", + ":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 = [ - ":shape_class", "//geometry:geometry_roles", "//geometry:scene_graph_inspector", "//geometry:shape_specification", + "//geometry/proximity:polygon_surface_mesh", "//multibody/tree", "@fmt", ], @@ -119,15 +84,16 @@ drake_cc_library( name = "distance_oracle", srcs = ["distance_oracle.cc"], hdrs = ["distance_oracle.h"], + internal = True, + visibility = ["//visibility:private"], deps = [ - ":options", + ":internal", "//common:essential", "//geometry:scene_graph", "//planning:robot_diagram", "@eigen", ], implementation_deps = [ - ":shape_class", "//geometry:scene_graph_inspector", "//geometry:shape_specification", "//geometry/proximity:polygon_surface_mesh", @@ -137,79 +103,37 @@ drake_cc_library( ], ) +# 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. drake_cc_library( - name = "vpolytope_ingestion", - srcs = ["vpolytope_ingestion.cc"], - hdrs = ["vpolytope_ingestion.h"], - deps = [ - "//geometry:geometry_ids", - "//geometry/optimization:convex_set", - "//math:geometric_transform", - "//multibody/plant", - ], - implementation_deps = [ - "//common:essential", - "//geometry:shape_specification", - "@fmt", - ], -) - -# The certificate and the node recursion are mutually recursive translation -# units (certificate.cc replays the events that certifier_internal.cc emits), -# so they form one library. -drake_cc_library( - name = "certifier", + name = "continuous_collision_checker", srcs = [ - "certificate.cc", - "certifier_internal.cc", - ], - hdrs = [ - "certificate.h", - "certifier_internal.h", + "certifier.cc", + "certifier.h", + "continuous_collision_checker.cc", ], - install_hdrs_exclude = ["certifier_internal.h"], + hdrs = ["continuous_collision_checker.h"], deps = [ - ":distance_oracle", - ":motion_bound_table", - ":numerics", - ":options", - ":piecewise_bezier_path", "//common:essential", "//common:parallelism", - "//geometry:scene_graph", - "//math:geometric_transform", + "//common/trajectories:trajectory", + "//geometry:geometry_ids", "//multibody/tree:multibody_tree_indexes", - "//planning:collision_checker_context", "//planning:robot_diagram", "@eigen", ], implementation_deps = [ - "//multibody/plant", - "@fmt", - ], -) - -drake_cc_library( - name = "continuous_collision_checker", - srcs = ["continuous_collision_checker.cc"], - hdrs = ["continuous_collision_checker.h"], - deps = [ - ":certifier", ":distance_oracle", + ":internal", ":motion_bound_table", - ":options", ":piecewise_bezier_path", - "//common:essential", - "//common/trajectories:trajectory", - "//planning:robot_diagram", - "@eigen", - ], - implementation_deps = [ - ":shape_class", "//geometry:scene_graph", "//geometry:scene_graph_inspector", "//geometry:shape_specification", + "//math:geometric_transform", "//multibody/plant", + "//planning:collision_checker_context", "@fmt", ], ) @@ -217,15 +141,16 @@ drake_cc_library( # === test/ === # The helpers the tests share: seeded random primitives and surface samplers, -# the throw-message probe, the checker factory, the random world generator two -# corpora are built from, and the corpus plus deep workload concurrency_test.cc -# pins the driver's determinism against. +# 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", @@ -243,6 +168,7 @@ drake_cc_library( drake_cc_googletest( name = "piecewise_bezier_path_test", deps = [ + ":internal", ":piecewise_bezier_path", "//common:copyable_unique_ptr", "//common/test_utilities:expect_throws_message", @@ -262,6 +188,7 @@ drake_cc_googletest( deps = [ ":motion_bound_table", ":test_utilities", + "//common/test_utilities:expect_throws_message", "//geometry:geometry_roles", "//geometry:scene_graph_inspector", "//multibody/tree", @@ -269,13 +196,12 @@ drake_cc_googletest( ], ) -# The bounding-sphere radius property test. +# The bounding-sphere containment property test. drake_cc_googletest( name = "bounding_sphere_test", deps = [ - ":bounding_sphere", + ":motion_bound_table", ":test_utilities", - "//common:essential", "//common:memory_file", "//common/test_utilities:expect_throws_message", "//geometry:in_memory_mesh", @@ -285,13 +211,12 @@ drake_cc_googletest( ], ) -# Oracle accuracy, probe classification, half-space fallback, V-polytope. +# 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", - ":vpolytope_ingestion", "//common:find_resource", "//common:memory_file", "//common/test_utilities:expect_throws_message", @@ -300,7 +225,6 @@ drake_cc_googletest( "//geometry:proximity_properties", "//geometry:scene_graph", "//geometry:shape_specification", - "//geometry/optimization:convex_set", "//math:geometric_transform", "//multibody/fem:deformable_body_config", "//multibody/plant", @@ -314,13 +238,16 @@ drake_cc_googletest( # guard, on a focused, hand-built corpus. drake_cc_googletest( name = "certifier_test", - deps = [":test_utilities"], + deps = [ + ":piecewise_bezier_path", + ":test_utilities", + ], ) # The randomized soundness fuzz: random worlds x random trajectories, -# cross-checked against dense sampling and against the certificate replay. -# The dense cross-check (~1e7 signed-distance queries) is what makes this -# test long rather than the certification itself. +# 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 @@ -342,21 +269,14 @@ drake_cc_googletest( "//conditions:default": [], }), deps = [ - ":continuous_collision_checker", - "//common:parallelism", - "//common/trajectories:bezier_curve", + ":distance_oracle", + ":internal", + ":piecewise_bezier_path", + ":test_utilities", "//common/trajectories:bspline_trajectory", "//common/trajectories:piecewise_polynomial", - "//common/trajectories:trajectory", - "//geometry:scene_graph", "//geometry:scene_graph_inspector", - "//geometry:shape_specification", "//math:bspline_basis", - "//math:geometric_transform", - "//multibody/plant", - "//multibody/tree", - "//planning:robot_diagram", - "//planning:robot_diagram_builder", ], ) @@ -372,14 +292,8 @@ drake_cc_googletest( ], ) -# Certificate audit trail + mutation test. -drake_cc_googletest( - name = "certificate_test", - deps = [":test_utilities"], -) - -# 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 +# 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. drake_cc_googletest( name = "concurrency_test", @@ -395,12 +309,7 @@ drake_cc_googletest( name = "api_test", deps = [ ":test_utilities", - "//common:copyable_unique_ptr", "//common/test_utilities:expect_throws_message", - "//common/trajectories:composite_trajectory", - "//common/trajectories:piecewise_polynomial", - "//common/trajectories:piecewise_quaternion", - "//common/trajectories:trajectory", "//geometry:geometry_instance", "//geometry:proximity_properties", "//multibody/fem:deformable_body_config", diff --git a/planning/continuous_collision/bounding_sphere.cc b/planning/continuous_collision/bounding_sphere.cc deleted file mode 100644 index f80aeefaf1c4..000000000000 --- a/planning/continuous_collision/bounding_sphere.cc +++ /dev/null @@ -1,173 +0,0 @@ -#include "drake/planning/continuous_collision/bounding_sphere.h" - -#include -#include -#include -#include - -#include - -#include "drake/common/drake_assert.h" -#include "drake/geometry/proximity/polygon_surface_mesh.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -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::Shape; -using drake::geometry::ShapeReifier; -using drake::geometry::Sphere; -using drake::math::RigidTransform; - -/* 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; -} - -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/bounding_sphere.h b/planning/continuous_collision/bounding_sphere.h deleted file mode 100644 index 0d2bcacaa890..000000000000 --- a/planning/continuous_collision/bounding_sphere.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include - -#include "drake/geometry/shape_specification.h" -#include "drake/math/rigid_transform.h" - -namespace drake { -namespace planning { -namespace continuous_collision { - -/** A sphere, expressed in the owning body (link) frame L, that contains a -proximity geometry at every configuration of the body. -@ingroup planning_collision_checker */ -struct BoundingSphere { - /** Sphere center in the body frame. */ - 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. -@ingroup planning_collision_checker */ -BoundingSphere ComputeBoundingSphere(const geometry::Shape& shape, - const math::RigidTransform& X_LG); - -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/certificate.cc b/planning/continuous_collision/certificate.cc deleted file mode 100644 index a34ca886be45..000000000000 --- a/planning/continuous_collision/certificate.cc +++ /dev/null @@ -1,359 +0,0 @@ -#include "drake/planning/continuous_collision/certificate.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "drake/common/drake_assert.h" -#include "drake/planning/continuous_collision/certifier_internal.h" -#include "drake/planning/continuous_collision/numerics.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace internal { -namespace { - -/* Slop allowed between the certifier's arithmetic and the replay's. The two - compute the same quantities by *different* routes, repeated halving versus a - pair of arbitrary-u de Casteljau subdivisions, so they agree only to rounding. - Both routes are sequences of convex combinations, hence numerically benign. - This tolerance sits far below anything a tamperer could hide in and far above - the rounding gap. */ -constexpr double kReplayTolerance = 1e-9; - -/* One de Casteljau subdivision at u ∈ [0, 1]: `left` receives the control - points of the restriction to [0, u] and `right` those of the restriction to - [u, 1] (both n × (m+1)). The triangle b_j^r = (1−u)·b_j^{r−1} + u·b_{j+1}^{r−1} - is built in place inside `right`; its first column after round r is the left - child's r-th control point and the column it leaves at m−r is the right - child's. Written out here, rather than reused from the curve module, so that - the replay is genuinely independent of the code path it audits. */ -void SplitAt(const Eigen::MatrixXd& cps, double u, Eigen::MatrixXd* left, - Eigen::MatrixXd* right) { - const int m = static_cast(cps.cols()) - 1; - left->resize(cps.rows(), cps.cols()); - *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) = (1.0 - u) * right->col(j) + u * right->col(j + 1); - } - left->col(r) = right->col(0); - } -} - -} // namespace - -void RestrictBezier(const Eigen::MatrixXd& cps, double a, double b, - Eigen::MatrixXd* out) { - DRAKE_DEMAND(out != nullptr); - const double lo = std::clamp(a, 0.0, 1.0); - const double hi = std::clamp(b, 0.0, 1.0); - if (lo <= 0.0 && hi >= 1.0) { - *out = cps; - return; - } - Eigen::MatrixXd scratch; - Eigen::MatrixXd tail; - if (lo <= 0.0) { - tail = cps; - } else { - SplitAt(cps, lo, &scratch, &tail); - } - // `tail` is the curve on [lo, 1] in its own parameter v ∈ [0, 1]; the - // original parameter hi lands at v = (hi − lo)/(1 − lo). - const double span = 1.0 - lo; - const double v = (span > 0.0) ? std::clamp((hi - lo) / span, 0.0, 1.0) : 1.0; - if (v >= 1.0) { - *out = std::move(tail); - return; - } - SplitAt(tail, v, out, &scratch); -} - -Eigen::VectorXd EvaluateBezier(const Eigen::MatrixXd& cps, double u) { - const int m = static_cast(cps.cols()) - 1; - Eigen::MatrixXd work = cps; - 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); -} - -bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, - std::string* message) { - DRAKE_DEMAND(input.model != nullptr); - 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); - - const auto fail = [message](std::string reason) { - if (message != nullptr) *message = std::move(reason); - return false; - }; - - const PiecewiseBezierPath& path = *input.path; - const std::vector& pairs = *input.pairs; - const std::vector& tau = *input.tau; - const MotionBoundTable& table = *input.table; - const int num_pairs = static_cast(pairs.size()); - const int num_segments = static_cast(path.segments().size()); - const int num_positions = path.num_positions(); - - // --- 1. The pair snapshot must be the checker's own table. --------------- - // Without this the record indices mean nothing, and every later check could - // be aimed at the wrong geometries. - if (static_cast(certificate.pairs.size()) != num_pairs) { - return fail( - fmt::format("certificate covers {} pair(s) but the checker has {}.", - certificate.pairs.size(), num_pairs)); - } - for (int p = 0; p < num_pairs; ++p) { - if (certificate.pairs[p].a != pairs[p].id.a || - certificate.pairs[p].b != pairs[p].id.b) { - return fail(fmt::format( - "certificate pair {} does not match the checker's pair table.", p)); - } - } - if (table.num_pairs() != num_pairs) { - return fail("the motion-bound table does not match the pair table."); - } - - // --- 2. Replay every record. --------------------------------------------- - ThreadContext context(*input.model); - Eigen::MatrixXd restricted; - Eigen::VectorXd w(num_positions); - Eigen::VectorXd last_qc; - std::vector claimed_threshold( - num_pairs, std::numeric_limits::quiet_NaN()); - - for (int r = 0; r < static_cast(certificate.records.size()); ++r) { - const CertificateRecord& record = certificate.records[r]; - const int p = record.pair_index; - if (p < 0 || p >= num_pairs) { - return fail( - fmt::format("record {} names pair index {}, out of range.", r, p)); - } - if (record.segment < 0 || record.segment >= num_segments) { - return fail(fmt::format("record {} names segment {}, out of range.", r, - record.segment)); - } - if (!(record.s_start >= 0.0) || !(record.s_end <= 1.0) || - !(record.s_start < record.s_end)) { - return fail(fmt::format( - "record {} has a degenerate or out-of-range interval [{}, {}].", r, - record.s_start, record.s_end)); - } - if (record.qc.size() != num_positions) { - return fail(fmt::format( - "record {} carries a representative configuration of size {}; the " - "plant has {} positions.", - r, record.qc.size(), num_positions)); - } - if (!std::isfinite(record.phi_hat) || !std::isfinite(record.motion_bound) || - !std::isfinite(record.threshold) || record.motion_bound < 0.0) { - return fail( - fmt::format("record {} carries non-finite or negative data.", r)); - } - // Every record of a pair must claim the same threshold: a certificate that - // silently lowers m_p on some intervals proves nothing coherent. - if (std::isnan(claimed_threshold[p])) { - claimed_threshold[p] = record.threshold; - } else if (claimed_threshold[p] != record.threshold) { - return fail(fmt::format( - "pair {} is certified against two different thresholds ({} and {}).", - p, claimed_threshold[p], record.threshold)); - } - // ... and the threshold it claims must be at least the one the caller - // expects. Checking only self-consistency would let a certificate whose - // records all say "threshold = -1e9" verify: it would be a true statement - // about a claim nobody asked for. - const double expected = pairs[p].threshold; - if (!(record.threshold >= - expected - kReplayTolerance * std::max(1.0, std::abs(expected)))) { - return fail(fmt::format( - "record {}: pair {} is certified only against threshold {}, which is " - "below the {} the checker's options call for.", - r, p, record.threshold, expected)); - } - - const bool is_static = table.pair_is_static(p); - // A static pair's J(p) is empty, so MotionBound() would return exactly the - // carve-out slack for any w: the residual of the coordinates the carve-out - // removed, which is nonzero only when some of them are constant merely to - // within Options::continuity_tolerance. Charging it here keeps the replay's - // Δ at least as large as the certifier's: a certificate emitted against - // a slack-inflated bound must not verify against a smaller one. - double motion_bound = table.carveout_slack(p); - if (!is_static) { - // Re-restrict the segment's control points to the record's interval and - // recompute w about the record's qc from scratch. This is the half of - // the certificate the checker must not be believed on. - RestrictBezier(path.segments()[record.segment].control_points, - record.s_start, record.s_end, &restricted); - - // qc must be the node's own midpoint apex, i.e. a configuration exactly - // on the trajectory. (A qc merely inside the control box would still be - // sound by the displacement lemma, but pinning it to the apex is what - // the certifier emits, and it makes a tampered qc detectable.) - const Eigen::VectorXd apex = EvaluateBezier(restricted, 0.5); - const double qc_error = (apex - record.qc).cwiseAbs().maxCoeff(); - // Relative: a plant with large coordinate values (an unbounded prismatic - // joint, say) rounds proportionally, and the check must not turn into a - // scale-dependent false alarm. - const double qc_limit = - kReplayTolerance * std::max(1.0, record.qc.cwiseAbs().maxCoeff()); - if (!(qc_error <= qc_limit)) { - return fail(fmt::format( - "record {}: the stored representative configuration is not the " - "midpoint of the interval it claims (off by {}).", - r, qc_error)); - } - - w.setZero(); - for (int j = 0; j < restricted.cols(); ++j) { - for (int i = 0; i < num_positions; ++i) { - w[i] = std::max(w[i], std::abs(restricted(i, j) - record.qc[i])); - } - } - motion_bound = table.MotionBound(p, w); - if (!(record.motion_bound >= - motion_bound - kReplayTolerance * std::max(1.0, motion_bound))) { - return fail(fmt::format( - "record {}: the stored motion bound {} understates the recomputed " - "bound {}.", - r, record.motion_bound, motion_bound)); - } - } - // For a static pair J(p) = ∅: no coordinate the trajectory *moves* changes - // the pair's relative pose, so Δ_p is the constant carve-out slack (0 in - // every case but a tolerance-constant coordinate) and one measurement - // certifies the whole domain. A *non*-static pair cannot smuggle in such a - // record: the recomputed Δ above would be the full node's bound and the - // test below would reject it. - // - // "Static" is relative to the constant-coordinate carve-out, so - // coordinates this path happens to hold fixed still move the pair in - // general. The representative configuration therefore has to be pinned to - // the path, exactly as the certifier pins it (q(t0)), or a record could be - // re-based onto an off-path configuration that measures more clearance. - if (is_static) { - const Eigen::VectorXd q0 = path.segments()[0].control_points.col(0); - const double qc_error = (q0 - record.qc).cwiseAbs().maxCoeff(); - if (!(qc_error <= kReplayTolerance * - std::max(1.0, record.qc.cwiseAbs().maxCoeff()))) { - return fail(fmt::format( - "record {}: pair {} is certified statically from a configuration " - "that is not the path's start (off by {}).", - r, p, qc_error)); - } - } - - // Re-measure the distance ourselves. Records are emitted sorted, so the - // several pairs certified at one node arrive adjacently and share a qc; - // skipping the redundant SetPositions saves that many forward-kinematics - // evaluations on what is otherwise a linear scan of the whole audit trail. - if (last_qc.size() != record.qc.size() || - !(last_qc.array() == record.qc.array()).all()) { - context.SetPositions(record.qc); - last_qc = record.qc; - } - const double phi_replay = - input.oracle->SignedDistance(context.query_object(), pairs[p]); - const double tau_p = tau[p]; - // Coherence: a record may legitimately store *less* than the narrowphase - // reports (the sphere-prefilter branch stores a lower bound on ϕ), but it - // may never claim more than the oracle's own contract allows. - if (!(record.phi_hat <= phi_replay + tau_p + kReplayTolerance)) { - return fail(fmt::format( - "record {}: the stored clearance {} over-reports the independently " - "measured {} (pair {}).", - r, record.phi_hat, phi_replay, p)); - } - // The certificate test runs on min(stored, re-measured), so an inflated - // ϕ̂ can never buy a record anything: only the value this replay measured - // for itself can carry the inequality. Both are lower bounds we are - // entitled to charge τ_p against, and for an untampered record the stored - // value is the smaller one (identical for a narrowphase record, the - // sphere bound for a prefilter record), so nothing legitimate is lost. - const double effective_phi = std::min(record.phi_hat, phi_replay); - if (!IsCertified(effective_phi, tau_p, motion_bound, record.threshold, - input.slack)) { - return fail(fmt::format( - "record {}: phi_hat {} - tau {} - Delta {} does not exceed threshold " - "{} + slack {} (pair {}, segment {}, [{}, {}]).", - r, effective_phi, tau_p, motion_bound, record.threshold, input.slack, - p, record.segment, record.s_start, record.s_end)); - } - } - - // --- 3. Coverage. -------------------------------------------------------- - // The certified intervals must tile [0, 1] of every segment for every pair. - // Without this a certificate could consist of a handful of perfectly valid - // records and still prove nothing about the parts of the path they miss. - struct Interval { - int pair; - int segment; - double lo; - double hi; - }; - std::vector intervals; - intervals.reserve(certificate.records.size()); - for (const CertificateRecord& record : certificate.records) { - intervals.push_back(Interval{record.pair_index, record.segment, - record.s_start, record.s_end}); - } - std::sort(intervals.begin(), intervals.end(), - [](const Interval& a, const Interval& b) { - if (a.pair != b.pair) return a.pair < b.pair; - if (a.segment != b.segment) return a.segment < b.segment; - return a.lo < b.lo; - }); - - std::size_t cursor = 0; - for (int p = 0; p < num_pairs; ++p) { - for (int k = 0; k < num_segments; ++k) { - const std::size_t begin = cursor; - while (cursor < intervals.size() && intervals[cursor].pair == p && - intervals[cursor].segment == k) { - ++cursor; - } - double covered_to = 0.0; - for (std::size_t i = begin; i < cursor; ++i) { - // Sorted by lo, so a start beyond the covered prefix is a real gap. - // Compared exactly: the certifier's intervals are - // dyadic and abut bit-for-bit (a child's endpoint *is* the parent's - // computed midpoint), so any slack here would only buy a forged - // certificate the right to excise a sliver at every one of its - // thousands of record boundaries. - if (intervals[i].lo > covered_to) break; - covered_to = std::max(covered_to, intervals[i].hi); - } - if (!(covered_to >= 1.0)) { - return fail(fmt::format( - "pair {} is certified only up to s = {} of segment {}; the " - "certificate does not cover the whole path.", - p, covered_to, k)); - } - } - } - - if (message != nullptr) message->clear(); - return true; -} - -} // namespace internal -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/certificate.h b/planning/continuous_collision/certificate.h deleted file mode 100644 index b79d7d6ec2ea..000000000000 --- a/planning/continuous_collision/certificate.h +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include - -#include - -#include "drake/planning/continuous_collision/options.h" - -namespace drake { -namespace planning { -namespace continuous_collision { - -/** One certification event: pair `pair_index` was certified over the -parameter interval [s_start, s_end] of segment `segment` from representative -configuration qc. -@ingroup planning_collision_checker */ -struct CertificateRecord { - int segment{}; - double s_start{}; - double s_end{}; - int pair_index{}; - Eigen::VectorXd qc; - double phi_hat{}; - double motion_bound{}; - double threshold{}; -}; - -/** Audit trail of every certification event of a run; an independent -replay (VerifyCertificate, declared in the api header) re-evaluates every -record and checks interval coverage of the full domain per pair. -@ingroup planning_collision_checker */ -struct Certificate { - std::vector records; - /** Pair table snapshot the indices refer to. */ - std::vector pairs; -}; - -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/certifier_internal.cc b/planning/continuous_collision/certifier.cc similarity index 60% rename from planning/continuous_collision/certifier_internal.cc rename to planning/continuous_collision/certifier.cc index a5101b9db731..ba2374abc398 100644 --- a/planning/continuous_collision/certifier_internal.cc +++ b/planning/continuous_collision/certifier.cc @@ -1,4 +1,4 @@ -#include "drake/planning/continuous_collision/certifier_internal.h" +#include "drake/planning/continuous_collision/certifier.h" #include #include @@ -15,7 +15,6 @@ #include "drake/common/drake_assert.h" #include "drake/common/parallelism.h" #include "drake/multibody/plant/multibody_plant.h" -#include "drake/planning/continuous_collision/numerics.h" namespace drake { namespace planning { @@ -31,34 +30,29 @@ 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 certificate is - invariant under time reparametrization. */ + 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); } -/* Assembles one Finding. Every field is set here, so the call sites below - differ only in the values they pass. */ -Finding MakeFinding(double time, const Eigen::VectorXd& q, const PairId& pair, - double distance, double motion_bound, bool definite, +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.pair = pair; + 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.motion_bound = motion_bound; - finding.definite = definite; finding.nearest_a_W = nearest_a_W; finding.nearest_b_W = nearest_b_W; return finding; } -// --------------------------------------------------------------------------- -// Per-node world-frame geometry sphere centers. -// --------------------------------------------------------------------------- - /* 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 @@ -83,13 +77,12 @@ class GeometryCache { return center_W_[slot]; } - double radius(int slot) const { return table_->geometries[slot].radius; } - /* 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() - radius(slot_a) - - radius(slot_b); + return (Center(slot_a) - Center(slot_b)).norm() - + table_->geometries[slot_a].radius - + table_->geometries[slot_b].radius; } private: @@ -100,28 +93,20 @@ class GeometryCache { std::uint64_t stamp_{1}; }; -// --------------------------------------------------------------------------- -// Findings sink. -// --------------------------------------------------------------------------- - -/* Collects findings from every worker. Cold path: guarded by one mutex. - Each list keeps only the `cap` earliest entries, so memory stays bounded no - matter how many violating nodes a pathological trajectory produces, while the - "earliest-first" contract of CertificationResult::findings is preserved - exactly (dropping the *latest* entry can never remove an earlier one). */ +/* Collects the earliest violation and the earliest inconclusive witness from + every worker. Cold path: guarded by one mutex. */ class FindingSink { public: - explicit FindingSink(int cap) : cap_(std::max(1, cap)) {} - void AddDefinite(Finding finding) { const double time = finding.time; { std::lock_guard guard(mutex_); - Insert(&definite_, std::move(finding)); + if (!definite_.has_value() || time < definite_->time) { + definite_ = std::move(finding); + } } - // Branch-and-bound bound for kFindFirstViolation: workers skip nodes - // whose interval starts at or after the earliest witness known so far. - // The bound decreases + // 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); @@ -132,80 +117,39 @@ class FindingSink { void AddInconclusive(Finding finding) { std::lock_guard guard(mutex_); - Insert(&inconclusive_, std::move(finding)); + 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); } - /* Reports a node that was left unexplored when the node budget ran out; the - earliest such node over all workers is what the run reports (the search - algorithm: the budget "truncates in parameter order and reports the - remainder"). */ - void ReportPending(double time, const Eigen::VectorXd& q, int pair_index) { - std::lock_guard guard(mutex_); - if (!pending_valid_ || time < pending_time_) { - pending_valid_ = true; - pending_time_ = time; - pending_q_ = q; - pending_pair_ = pair_index; - } - } - - bool pending_valid() const { return pending_valid_; } - double pending_time() const { return pending_time_; } - const Eigen::VectorXd& pending_q() const { return pending_q_; } - int pending_pair() const { return pending_pair_; } - - const std::vector& definite() const { return definite_; } - const std::vector& inconclusive() const { return inconclusive_; } + std::optional& definite() { return definite_; } + std::optional& inconclusive() { return inconclusive_; } private: - void Insert(std::vector* list, Finding&& finding) { - if (static_cast(list->size()) >= cap_ && - finding.time >= list->back().time) { - return; - } - const auto position = - std::upper_bound(list->begin(), list->end(), finding.time, - [](double time, const Finding& other) { - return time < other.time; - }); - list->insert(position, std::move(finding)); - if (static_cast(list->size()) > cap_) list->pop_back(); - } - - const int cap_; std::mutex mutex_; - std::vector definite_; - std::vector inconclusive_; + std::optional definite_; + std::optional inconclusive_; std::atomic best_violation_time_{kInfinity}; - bool pending_valid_{false}; - double pending_time_{kInfinity}; - Eigen::VectorXd pending_q_; - int pending_pair_{0}; }; -// --------------------------------------------------------------------------- -// Shared work source for the parallel driver. -// --------------------------------------------------------------------------- - /* 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. */ + 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}; - int depth{0}; Eigen::MatrixXd control_points; std::vector active; }; @@ -213,8 +157,8 @@ struct WorkItem { /* 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 / violation bound, which is what makes the driver - TSan-clean by construction. */ + 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 @@ -275,12 +219,12 @@ class WorkQueue { condition_.notify_all(); } - /* The sharing policy (see certifier_internal.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. */ + /* 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); @@ -290,10 +234,6 @@ class WorkQueue { num_workers_.store(count, std::memory_order_relaxed); } - /* Items never picked up; used to report what the node budget left - uncovered. Call only after every worker has finished. */ - std::vector& remaining() { return items_; } - private: std::mutex mutex_; std::condition_variable condition_; @@ -310,7 +250,6 @@ class WorkQueue { 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_before_hire{0}; std::uint64_t nodes{0}; std::function hire; }; @@ -319,27 +258,21 @@ struct Recruitment { 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 before the lead can collect their statistics. 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. */ + 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; -// --------------------------------------------------------------------------- -// The node loop. -// --------------------------------------------------------------------------- - /* 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 depth{0}; int active_offset{0}; int active_length{0}; }; @@ -371,9 +304,7 @@ class Worker { node_counter_(node_counter), queue_(queue), recruit_(recruit), - geometry_(*input.prefilter, *context), - find_first_(input.options.mode == SearchMode::kFindFirstViolation), - emit_certificate_(input.options.emit_certificate) { + geometry_(*input.prefilter, *context) { const int n = input_.path->num_positions(); q_mid_.resize(n); w_.resize(n); @@ -392,9 +323,6 @@ class Worker { /* Serial entry point (and the body of the parallel one). */ void RunItem(WorkItem* item); - const Statistics& stats() const { return stats_; } - std::vector& records() { return records_; } - 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. */ @@ -416,15 +344,6 @@ class Worker { } } - /* Appends one certification event to the audit trail. - */ - void RecordCertification(int segment, double s_lo, double s_hi, int pair, - double phi_hat, double motion_bound, - double threshold) { - records_.push_back(CertificateRecord{segment, s_lo, s_hi, pair, q_mid_, - phi_hat, motion_bound, threshold}); - } - const CertifierInput& input_; ThreadContext* context_{}; FindingSink* sink_{}; @@ -433,8 +352,6 @@ class Worker { /* Non-null only for the lead worker, and only until it has hired. */ Recruitment* recruit_{}; GeometryCache geometry_; - const bool find_first_{false}; - const bool emit_certificate_{false}; /* Reused buffers for the queue's two directions (see WorkItem). */ WorkItem item_; @@ -450,25 +367,19 @@ class Worker { Eigen::VectorXd w_; Eigen::Vector3d nearest_a_; Eigen::Vector3d nearest_b_; - - Statistics stats_; - std::vector records_; }; void Worker::RunItem(WorkItem* item) { - const PiecewiseBezierPath& path = *input_.path; - const BezierSegment& segment = path.segments()[item->segment]; + 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 Options& options = input_.options; - const double slack = options.certificate_slack; + const double threshold = input_.options.margin; + const double min_interval = input_.options.min_interval; const int rows = static_cast(item->control_points.rows()); const int cols = static_cast(item->control_points.cols()); - const std::uint64_t max_nodes = - options.max_nodes.value_or(std::numeric_limits::max()); // Seed the local stack with this work item. EnsureSlabs(2, rows, cols); @@ -476,7 +387,7 @@ void Worker::RunItem(WorkItem* item) { 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, item->depth, 0, + stack_.push_back(NodeFrame{item->s_lo, item->s_hi, 0, static_cast(item->active.size())}); while (!stack_.empty()) { @@ -486,31 +397,13 @@ void Worker::RunItem(WorkItem* item) { // Branch-and-bound on time: a node starting at or after the earliest // witness known so far cannot contain an earlier one. - if (find_first_ && - TimeOf(segment, frame.s_lo) >= sink_->best_violation_time()) { - continue; - } - if (node_counter_->fetch_add(1, std::memory_order_relaxed) >= max_nodes) { - // Budget exhausted: stop here and report the earliest node this worker - // leaves uncovered, which is exactly this one, because a left-first DFS - // pops in increasing parameter order and every frame still on the stack - // starts at or after this node's end. - sink_->ReportPending( - TimeOf(segment, frame.s_lo), slabs_[k].col(0), - frame.active_length > 0 ? arena_[frame.active_offset] : 0); - if (queue_ != nullptr) queue_->Abort(); - stack_.clear(); - return; - } - ++stats_.nodes; - stats_.max_depth = std::max(stats_.max_depth, frame.depth); - - // Lazy recruitment (see certifier_internal.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 >= recruit_->nodes_before_hire) { + 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(); @@ -526,8 +419,7 @@ void Worker::RunItem(WorkItem* item) { &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 - + // 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) { @@ -535,8 +427,8 @@ void Worker::RunItem(WorkItem* item) { } } - // One FK per node; body poses and the query object are - // pulled lazily below, and only for pairs that survive that far. + // 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(); @@ -547,7 +439,7 @@ void Worker::RunItem(WorkItem* item) { // midpoint no longer separates the endpoints in double arithmetic the node // cannot be split any further, whatever min_interval says. Without it a // pathologically small min_interval would spin forever. - const bool at_floor = (frame.s_hi - frame.s_lo) <= options.min_interval || + const bool at_floor = (frame.s_hi - frame.s_lo) <= min_interval || !(s_mid > frame.s_lo && s_mid < frame.s_hi); const int survivor_offset = frame.active_offset + frame.active_length; @@ -558,10 +450,9 @@ void Worker::RunItem(WorkItem* item) { e < frame.active_offset + frame.active_length; ++e) { const int p = arena_[e]; const PairRecord& pair = pairs[p]; - const double threshold = pair.threshold; const double tau_p = tau[p]; - // Δ_p(ν) = Σ_{j ∈ J(p)} λ(j,p)·w_j, a sparse dot product over this - // pair's CSR row. + // Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p)·w_j, a sparse dot + // product over this pair's CSR row. const double motion_bound = table.MotionBound(p, w_); // --- Early-out 1: the free-sphere prefilter. --- @@ -571,82 +462,58 @@ void Worker::RunItem(WorkItem* item) { // 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 certificate replay's arithmetic uniform. + // 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) { - const double lower_bound = geometry_.LowerBound(slot_a, slot_b); - if (IsCertified(lower_bound, tau_p, motion_bound, threshold, slack)) { - ++stats_.sphere_certifications; - if (emit_certificate_) { - RecordCertification(item->segment, frame.s_lo, frame.s_hi, p, - lower_bound, motion_bound, threshold); - } - continue; - } + if (slot_a >= 0 && slot_b >= 0 && + IsCertified(geometry_.LowerBound(slot_a, slot_b), tau_p, motion_bound, + threshold)) { + continue; } // --- Narrowphase. ---------------------------------------------------- - ++stats_.narrowphase_queries; 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_p is a definite violation of the - // continuum statement, not a sampling artifact. - sink_->AddDefinite(MakeFinding(t_mid, q_mid_, pair.id, phi_hat, - motion_bound, true, nearest_a_, - nearest_b_)); - if (!find_first_ || at_floor) { - // kCertifyAll (or a floor node, which has no children to refine - // into): drop p from this subtree. Without this a single - // violating pair would report one finding per node all the way down - // to the resolution floor; the earliest-first ordering and the - // max_reported_findings cap still apply, and every *disjoint* - // violating region of p is still reported because sibling subtrees - // carry their own copy of the active set. - continue; - } - // kFindFirstViolation: keep p active so the branch-and-bound recursion - // can refine the witness toward the earliest violating time. - } else if (IsCertified(phi_hat, tau_p, motion_bound, threshold, slack)) { + // ϕ_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_p + ε, + // ≥ (ϕ̂ − τ_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. - if (emit_certificate_) { - RecordCertification(item->segment, frame.s_lo, frame.s_hi, p, phi_hat, - motion_bound, threshold); - } + 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; } - // --- Gray: subdivide, unless we are already at the resolution floor. - - if (at_floor) { - sink_->AddInconclusive(MakeFinding(t_mid, q_mid_, pair.id, phi_hat, - motion_bound, false, nearest_a_, - nearest_b_)); - } else { - arena_[survivor_offset + survivor_count] = p; - ++survivor_count; - } + 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, frame.depth + 1, survivor_offset, - survivor_count}; - const NodeFrame left{frame.s_lo, s_mid, frame.depth + 1, survivor_offset, - survivor_count}; + 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_internal.h): the shared queue - // is running dry, so hand the right child over and carry on down the left + // 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 @@ -654,7 +521,6 @@ void Worker::RunItem(WorkItem* item) { share_.segment = item->segment; share_.s_lo = right.s_lo; share_.s_hi = right.s_hi; - share_.depth = right.depth; share_.control_points = split_scratch_; share_.active.assign(arena_.begin() + survivor_offset, arena_.begin() + survivor_offset + survivor_count); @@ -673,11 +539,6 @@ void Worker::RunItem(WorkItem* item) { } } -// --------------------------------------------------------------------------- -// Breakpoint pre-pass and static-pair resolution (steps 1 -// and 2). -// --------------------------------------------------------------------------- - /* 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 @@ -688,15 +549,12 @@ void Worker::RunItem(WorkItem* item) { 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, - Statistics* stats, - std::vector* records) { + 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 slack = input.options.certificate_slack; - const int num_segments = static_cast(input.path->segments().size()); + const double threshold = input.options.margin; context->SetPositions(q); geometry->NewConfiguration(); @@ -706,7 +564,6 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, Eigen::Vector3d nearest_b; for (int p = 0; p < static_cast(pairs.size()); ++p) { const PairRecord& pair = pairs[p]; - const double threshold = pair.threshold; 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 @@ -718,9 +575,8 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, // 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 duplicate its finding (crowding out genuine ones - // under max_reported_findings) and pay a narrowphase query per junction. + // 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; @@ -734,70 +590,33 @@ void RunBreakpointPass(const CertifierInput& input, ThreadContext* context, // Δ_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, slack)) { - ++stats->sphere_certifications; - if (records != nullptr) { - for (int k = 0; k < num_segments; ++k) { - records->push_back(CertificateRecord{k, 0.0, 1.0, p, q, lower_bound, - static_bound, threshold}); - } - } - continue; - } + if (IsCertified(lower_bound, tau_p, static_bound, threshold)) continue; } else if (lower_bound >= threshold) { - // A definite violation needs ϕ̂ + τ_p < m_p, and ϕ̂ ≥ ϕ_true − τ_p ≥ - // lower_bound − τ_p, so lower_bound ≥ m_p rules one out with no query. + // A definite violation needs ϕ̂ + τ_p < m, and ϕ̂ ≥ ϕ_true − τ_p ≥ + // lower_bound − τ_p, so lower_bound ≥ m rules one out with no query. continue; } - ++stats->narrowphase_queries; const double phi_hat = input.oracle->SignedDistance(query_object, pair, &nearest_a, &nearest_b); if (IsDefiniteViolation(phi_hat, tau_p, threshold)) { - // A breakpoint is a single configuration, so the finding carries no - // motion bound. - sink->AddDefinite(MakeFinding(time, q, pair.id, phi_hat, 0.0, true, - nearest_a, nearest_b)); + 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, slack)) { - if (records != nullptr) { - for (int k = 0; k < num_segments; ++k) { - records->push_back(CertificateRecord{k, 0.0, 1.0, p, q, phi_hat, - static_bound, threshold}); - } - } - 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.id, phi_hat, static_bound, - false, nearest_a, nearest_b)); + sink->AddInconclusive( + MakeFinding(time, q, pair, phi_hat, nearest_a, nearest_b)); } } -/* Orders the audit trail so that a run is comparable across thread counts and - across time reparametrizations. */ -void SortRecords(std::vector* records) { - std::sort(records->begin(), records->end(), - [](const CertificateRecord& a, const CertificateRecord& b) { - if (a.segment != b.segment) return a.segment < b.segment; - if (a.s_start != b.s_start) return a.s_start < b.s_start; - if (a.s_end != b.s_end) return a.s_end < b.s_end; - return a.pair_index < b.pair_index; - }); -} - } // namespace -// --------------------------------------------------------------------------- -// ThreadContext / ContextPool. -// --------------------------------------------------------------------------- - void ThreadContext::SetPositions(const Eigen::VectorXd& q) { model_->plant().SetPositions(&context_.mutable_plant_context(), q); } @@ -862,12 +681,7 @@ ContextPool::Lease::~Lease() { if (pool_ != nullptr && !slots_.empty()) pool_->Release(slots_); } -// --------------------------------------------------------------------------- -// RunCertifier. -// --------------------------------------------------------------------------- - -CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { - DRAKE_DEMAND(input.model != nullptr); +Result RunCertifier(const CertifierInput& input, ContextPool* pool) { DRAKE_DEMAND(input.oracle != nullptr); DRAKE_DEMAND(input.table != nullptr); DRAKE_DEMAND(input.path != nullptr); @@ -876,34 +690,19 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { DRAKE_DEMAND(input.prefilter != nullptr); DRAKE_DEMAND(pool != nullptr); - const Options& options = input.options; const PiecewiseBezierPath& path = *input.path; - const std::vector& pairs = *input.pairs; - const int num_pairs = static_cast(pairs.size()); + const int num_pairs = static_cast(input.pairs->size()); const int num_segments = static_cast(path.segments().size()); - const bool emit = options.emit_certificate; - const std::uint64_t max_nodes = - options.max_nodes.value_or(std::numeric_limits::max()); - - CertifierOutput output; - if (emit) { - output.certificate.pairs.reserve(num_pairs); - for (const PairRecord& pair : pairs) { - output.certificate.pairs.push_back(pair.id); - } - } - FindingSink sink(options.max_reported_findings); + FindingSink sink; std::atomic node_counter{0}; - Statistics stats; - std::vector records; // 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, options.parallelism.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 @@ -927,8 +726,7 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { 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, &stats, - emit ? &records : nullptr); + /* resolve_static = */ k == 0, &sink); } } @@ -939,22 +737,9 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { if (!input.table->pair_is_static(p)) moving_pairs.push_back(p); } - const bool have_work = !moving_pairs.empty() && num_segments > 0; - const auto accumulate = [&](Worker* worker) { - stats.nodes += worker->stats().nodes; - stats.narrowphase_queries += worker->stats().narrowphase_queries; - stats.sphere_certifications += worker->stats().sphere_certifications; - stats.max_depth = std::max(stats.max_depth, worker->stats().max_depth); - if (emit) { - records.insert(records.end(), - std::make_move_iterator(worker->records().begin()), - std::make_move_iterator(worker->records().end())); - } - }; - - if (have_work && num_threads <= 1) { + 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 and stats. + // interleaving => bit-deterministic results. Worker worker(input, &lease[0], &sink, &node_counter, nullptr, nullptr); for (int k = 0; k < num_segments; ++k) { WorkItem item; @@ -962,25 +747,22 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { item.control_points = path.segments()[k].control_points; item.active = moving_pairs; worker.RunItem(&item); - if (node_counter.load(std::memory_order_relaxed) > max_nodes) break; } - accumulate(&worker); - } else if (have_work) { + } 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_internal.h. + // 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 kFindFirstViolation's bound + // 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.depth = 0; seed.control_points = path.segments()[k].control_points; seed.active = moving_pairs; queue.Push(&seed); @@ -1007,7 +789,6 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { std::vector> helper_futures; Recruitment recruitment; - recruitment.nodes_before_hire = kNodesBeforeHiringHelpers; // 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 @@ -1053,76 +834,22 @@ CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool) { // 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); - accumulate(&lead); - for (const std::unique_ptr& helper : helpers) { - accumulate(helper.get()); - } - // Anything the budget left in the queue is uncovered too. - for (const WorkItem& item : queue.remaining()) { - sink.ReportPending(TimeOf(path.segments()[item.segment], item.s_lo), - item.control_points.col(0), - item.active.empty() ? 0 : item.active.front()); - } } - // --- Step 4: reduce per the search mode. --------------------------------- - const bool budget_exhausted = - options.max_nodes.has_value() && - node_counter.load(std::memory_order_relaxed) > *options.max_nodes; - - std::vector findings; - if (!sink.definite().empty() && - options.mode == SearchMode::kFindFirstViolation) { - // The branch-and-bound recursion refines toward the earliest witness and - // the sink keeps entries earliest-first, so this *is* the earliest witness - // the run found, identical serially and in parallel. - findings.push_back(sink.definite().front()); + 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 { - findings = sink.definite(); - findings.insert(findings.end(), sink.inconclusive().begin(), - sink.inconclusive().end()); - } - - if (budget_exhausted && sink.pending_valid()) { - // Report what the budget left uncovered as a non-definite finding at the - // earliest uncovered time (truncate in parameter - // order, report the remainder). - const PairRecord& pending_pair = pairs[sink.pending_pair()]; - const Eigen::VectorXd q = sink.pending_q(); - Eigen::Vector3d nearest_a; - Eigen::Vector3d nearest_b; - lease[0].SetPositions(q); - const double distance = input.oracle->SignedDistance( - lease[0].query_object(), pending_pair, &nearest_a, &nearest_b); - ++stats.narrowphase_queries; - findings.push_back(MakeFinding(sink.pending_time(), q, pending_pair.id, - distance, 0.0, false, nearest_a, nearest_b)); - } - - std::stable_sort(findings.begin(), findings.end(), - [](const Finding& a, const Finding& b) { - return a.time < b.time; - }); - const int cap = std::max(1, options.max_reported_findings); - if (static_cast(findings.size()) > cap) findings.resize(cap); - - if (!sink.definite().empty()) { - output.verdict = Verdict::kViolationFound; - } else if (budget_exhausted) { - output.verdict = Verdict::kBudgetExhausted; - } else if (!sink.inconclusive().empty()) { - output.verdict = Verdict::kInconclusive; - } else { - output.verdict = Verdict::kCertifiedFree; - } - - output.findings = std::move(findings); - output.stats = stats; - if (emit) { - SortRecords(&records); - output.certificate.records = std::move(records); + result.verdict = Verdict::kCertifiedFree; } - return output; + return result; } } // namespace internal diff --git a/planning/continuous_collision/certifier_internal.h b/planning/continuous_collision/certifier.h similarity index 62% rename from planning/continuous_collision/certifier_internal.h rename to planning/continuous_collision/certifier.h index 6e4d467e2e03..3b601a0d161d 100644 --- a/planning/continuous_collision/certifier_internal.h +++ b/planning/continuous_collision/certifier.h @@ -1,14 +1,12 @@ #pragma once -// Internal driver of the adaptive interval certifier and of the independent -// certificate replay. Nothing here is part of the public API; it exists so -// that continuous_collision_checker.cc, certificate.cc and -// certifier_internal.cc can share one set of per-call data structures. +// 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 @@ -19,10 +17,10 @@ #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/certificate.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/options.h" #include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include "drake/planning/robot_diagram.h" @@ -140,15 +138,13 @@ struct PrefilterTable { std::vector slot_b; }; -/* Everything one certification run needs; assembled by the facade. All -pointers are aliased and must outlive the call. */ +/* Everything one run needs; assembled by the facade. All pointers are aliased +and must outlive the call. */ struct CertifierInput { - const RobotDiagram* model{}; const DistanceOracle* oracle{}; const MotionBoundTable* table{}; const PiecewiseBezierPath* path{}; - /* Pair records with `threshold` = margin + padding resolved for this call. - Indexed consistently with `table`, `tau` and `prefilter`. */ + /* 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. */ @@ -157,39 +153,22 @@ struct CertifierInput { Options options; }; -/* Result of one run, converted to a CertificationResult by the facade. */ -struct CertifierOutput { - Verdict verdict{Verdict::kCertifiedFree}; - /* Earliest-first, capped at Options::max_reported_findings. */ - std::vector findings; - Statistics stats; - /* Filled iff Options::emit_certificate; records are sorted by - (segment, s_start, pair_index) so a run is comparable across thread counts - and across time reparametrizations. - - Only a run that ends Verdict::kCertifiedFree produces a *complete* audit - trail, i.e. one whose certified intervals cover the whole domain for every - pair, which is what ReplayCertificate() demands. A run that found a - violation, hit the resolution floor, exhausted its budget, or pruned the - search (kFindFirstViolation) leaves the uncertified parts uncovered by - construction; its records are still individually valid, but they do not - amount to a proof and ReplayCertificate() will say so. */ - Certificate certificate; -}; - /* 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, and a findings sink under a -mutex. +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 @@ -197,8 +176,8 @@ 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 -kFindFirstViolation's bound tighten early. +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 @@ -209,55 +188,16 @@ 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 tree, the statistics summed over -workers, and the findings are identical serially and at any thread count in -kCertifyAll. In kFindFirstViolation the *reported witness* is identical too, -because the bound only ever prunes nodes that start at or after a witness -already found; the statistics are not. Two exceptions: a run that exhausts -`max_nodes` truncates at a thread-count dependent place, and 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. +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. */ -CertifierOutput RunCertifier(const CertifierInput& input, ContextPool* pool); - -// --------------------------------------------------------------------------- -// Certificate assembly + independent replay (implemented in certificate.cc). -// --------------------------------------------------------------------------- - -/* Restricts the Bézier control points `cps` (n × (m+1)) of a segment to the -sub-interval [a, b] ⊆ [0, 1] by two de Casteljau subdivisions, writing the -n × (m+1) control points of the restricted curve into `out`. - -This is a local, cold-path implementation used only by the certificate replay: -the replay must not reuse the certifier's own subdivision code path if it is to -be an independent check. */ -void RestrictBezier(const Eigen::MatrixXd& cps, double a, double b, - Eigen::MatrixXd* out); - -/* Evaluates the Bézier curve with control points `cps` at u ∈ [0, 1] by de -Casteljau (the apex of the triangle). Cold path. */ -Eigen::VectorXd EvaluateBezier(const Eigen::MatrixXd& cps, double u); - -/* Inputs of the independent certificate replay. All pointers are aliased. */ -struct ReplayInput { - const RobotDiagram* model{}; - const DistanceOracle* oracle{}; - const MotionBoundTable* table{}; - const PiecewiseBezierPath* path{}; - const std::vector* pairs{}; - const std::vector* tau{}; - double slack{1e-9}; -}; - -/* Independently re-evaluates every record of `certificate` and checks that -the certified intervals cover the whole domain for every pair. Returns true iff -the certificate is a complete, self-consistent proof that every pair stays -above its recorded threshold everywhere on the path. When it returns false and -`message` is non-null, `*message` explains why. */ -bool ReplayCertificate(const ReplayInput& input, const Certificate& certificate, - std::string* message); +Result RunCertifier(const CertifierInput& input, ContextPool* pool); } // namespace internal } // namespace continuous_collision diff --git a/planning/continuous_collision/continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc index 4797dc408c72..a0cdec1bc875 100644 --- a/planning/continuous_collision/continuous_collision_checker.cc +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -2,23 +2,20 @@ #include #include -#include #include #include -#include #include #include #include #include -#include "drake/common/drake_throw.h" #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_internal.h" -#include "drake/planning/continuous_collision/shape_class.h" +#include "drake/planning/continuous_collision/certifier.h" +#include "drake/planning/continuous_collision/internal.h" namespace drake { namespace planning { @@ -27,9 +24,10 @@ namespace { using drake::geometry::GeometryId; using drake::multibody::BodyIndex; -using drake::planning::RobotDiagram; using internal::Classify; +using internal::DistanceRoute; using internal::kNumShapeClasses; +using internal::PairRecord; using internal::ShapeClass; // --------------------------------------------------------------------------- @@ -37,14 +35,13 @@ using internal::ShapeClass; // --------------------------------------------------------------------------- // // Drake documents ComputeSignedDistancePairClosestPoints() accuracy as bad as -// 5e-5 m for some shape pairs, well outside the 1e-6 m default of -// Options::query_tolerance, 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, certificate replay) -// therefore takes τ_p = max(Options::query_tolerance, -// documented_accuracy(shape_a, shape_b)); pairs routed through the analytic -// halfspace fallback are closed-form and keep the raw -// Options::query_tolerance. +// 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 @@ -122,87 +119,26 @@ const AccuracyTable& DocumentedAccuracyTable() { return table; } -/* τ_p for every pair of `pairs`, given the call's query tolerance. */ +/* τ_p for every pair of `pairs`. */ std::vector ComputeTauTable(const RobotDiagram& model, - const std::vector& pairs, - double query_tolerance) { + const std::vector& pairs) { const drake::geometry::SceneGraphInspector& inspector = model.scene_graph().model_inspector(); const AccuracyTable& table = DocumentedAccuracyTable(); - std::vector tau(pairs.size(), query_tolerance); + 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].id.a))); - const int b = static_cast(Classify(inspector.GetShape(pairs[p].id.b))); - tau[p] = std::max(query_tolerance, table[a][b]); + 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; } -// --------------------------------------------------------------------------- -// Padding. -// --------------------------------------------------------------------------- -// -// PaddingSpec mirrors drake::planning::CollisionChecker: a pair's effective -// threshold is m_p = margin + padding(p), where padding comes from the dense -// per-body-pair matrix when one is supplied and otherwise from the {env, self} -// scalars. A pair is self iff both bodies are non-anchored and env otherwise. -// The rule is pure topology, so padding never depends on which trajectory is -// being checked; in particular the constant-coordinate carve-out, which can -// make a moving body behave as if welded for one trajectory, does not enter -// here. - -std::vector ComputePaddingTable(const KinematicsEngine& engine, - const std::vector& pairs, - const PaddingSpec& padding) { - const drake::multibody::MultibodyPlant& plant = engine.plant(); - const int num_bodies = plant.num_bodies(); - if (padding.per_body_pair.has_value()) { - const Eigen::MatrixXd& matrix = *padding.per_body_pair; - if (matrix.rows() != num_bodies || matrix.cols() != num_bodies) { - throw std::runtime_error(fmt::format( - "ContinuousCollisionChecker: PaddingSpec::per_body_pair is " - "{}x{} but must be {}x{} (one row and column per BodyIndex of the " - "plant).", - matrix.rows(), matrix.cols(), num_bodies, num_bodies)); - } - } - - std::vector anchored(num_bodies, false); - for (int b = 0; b < num_bodies; ++b) { - anchored[b] = plant.IsAnchored(plant.get_body(BodyIndex(b))); - } - - std::vector result(pairs.size(), 0.0); - for (int p = 0; p < static_cast(pairs.size()); ++p) { - const int a = static_cast(pairs[p].id.body_a); - const int b = static_cast(pairs[p].id.body_b); - double value = (!anchored[a] && !anchored[b]) ? padding.self_padding - : padding.env_padding; - if (padding.per_body_pair.has_value()) { - const double entry = (*padding.per_body_pair)(a, b); - // A NaN entry means "not covered by the matrix"; fall back to the - // scalars for that pair. - if (!std::isnan(entry)) value = entry; - } - if (!std::isfinite(value)) { - throw std::runtime_error( - fmt::format("ContinuousCollisionChecker: padding for the body pair " - "({}, {}) is not finite.", - plant.get_body(BodyIndex(a)).name(), - plant.get_body(BodyIndex(b)).name())); - } - result[p] = value; - } - return result; -} - -// --------------------------------------------------------------------------- -// Prefilter table. -// --------------------------------------------------------------------------- - +/* Per-pair bounding-sphere slots for the broadphase prefilter. */ internal::PrefilterTable ComputePrefilterTable( - const KinematicsEngine& engine, const std::vector& pairs) { + 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); @@ -216,7 +152,7 @@ internal::PrefilterTable ComputePrefilterTable( if (is_half_space) return -1; const auto it = slot_of.find(id); if (it != slot_of.end()) return it->second; - const BoundingSphere& sphere = engine.geometry_sphere(id); + 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}); @@ -225,229 +161,145 @@ internal::PrefilterTable ComputePrefilterTable( }; for (int p = 0; p < static_cast(pairs.size()); ++p) { - table.slot_a[p] = slot(pairs[p].id.a, pairs[p].id.body_a, + table.slot_a[p] = slot(pairs[p].a, pairs[p].body_a, pairs[p].route == DistanceRoute::kHalfSpaceA); - table.slot_b[p] = slot(pairs[p].id.b, pairs[p].id.body_b, + table.slot_b[p] = slot(pairs[p].b, pairs[p].body_b, pairs[p].route == DistanceRoute::kHalfSpaceB); } return table; } void ValidateOptions(const Options& options) { - if (!std::isfinite(options.margin)) { - throw std::runtime_error( - "ContinuousCollisionChecker: Options::margin must be finite."); - } - if (!(options.query_tolerance >= 0.0) || - !std::isfinite(options.query_tolerance)) { - throw std::runtime_error(fmt::format( - "ContinuousCollisionChecker: Options::query_tolerance must be " - "a finite non-negative distance; got {}.", - options.query_tolerance)); - } - if (!(options.certificate_slack >= 0.0) || - !std::isfinite(options.certificate_slack)) { + // 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::certificate_slack must " - "be a finite non-negative distance; got {}.", - options.certificate_slack)); + "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.min_interval > 0.0) || !(options.min_interval <= 1.0)) { throw std::runtime_error(fmt::format( - "ContinuousCollisionChecker: Options::min_interval is a " - "fraction of a segment's parameter width and must lie in (0, 1]; got " - "{}.", + "ContinuousCollisionChecker: Options::min_interval is a fraction of a " + "segment's parameter width and must lie in (0, 1]; got {}.", options.min_interval)); } - if (options.max_reported_findings < 1) { - throw std::runtime_error(fmt::format( - "ContinuousCollisionChecker: Options::max_reported_findings " - "must be at least 1; got {}.", - options.max_reported_findings)); - } - if (options.max_nodes.has_value() && *options.max_nodes == 0) { - throw std::runtime_error( - "ContinuousCollisionChecker: Options::max_nodes must be at " - "least 1 when set."); - } } } // namespace -// --------------------------------------------------------------------------- -// Impl. -// --------------------------------------------------------------------------- - class ContinuousCollisionChecker::Impl { public: - explicit Impl(Params params) - : model_(std::move(params.model)), - default_options_(std::move(params.default_options)), + Impl(std::shared_ptr> model, + const Options& default_options) + : model_(std::move(model)), + default_options_(default_options), engine_(*model_), - oracle_(*model_, default_options_.query_tolerance), + oracle_(*model_), pairs_(oracle_.pairs()), - padding_(ComputePaddingTable(engine_, pairs_, params.padding)), - tau_base_(ComputeTauTable(*model_, pairs_, - /* query_tolerance = */ 0.0)), + tau_(ComputeTauTable(*model_, pairs_)), prefilter_(ComputePrefilterTable(engine_, pairs_)), pool_(*model_, - std::max(1, default_options_.parallelism.num_threads())) { - pair_ids_.reserve(pairs_.size()); - for (int p = 0; p < static_cast(pairs_.size()); ++p) { - pair_ids_.push_back(pairs_[p].id); - // pairs() reports the checker's default thresholds; every call rewrites - // its own copy from that call's margin. - pairs_[p].threshold = default_options_.margin + padding_[p]; - } - } + std::max(1, default_options_.parallelism.num_threads())) {} - const Options& default_options() const { return default_options_; } const RobotDiagram& model() const { return *model_; } - const KinematicsEngine& engine() const { return engine_; } - const DistanceOracle& oracle() const { return oracle_; } - const std::vector& pairs() const { return pairs_; } - const std::vector& pair_ids() const { return pair_ids_; } const Options& Resolve(const std::optional& options) const { return options.has_value() ? *options : default_options_; } - void ValidatePath(const PiecewiseBezierPath& path) const { + 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.", + "ContinuousCollisionChecker: the trajectory has {} rows but the " + "plant has {} generalized positions.", path.num_positions(), expected)); } - } - - CertificationResult Check(const PiecewiseBezierPath& path, - const Options& options) const { - ValidateOptions(options); - ValidatePath(path); - const auto start = std::chrono::steady_clock::now(); - // Per-call: the λ table (it depends on the trajectory's control box), the - // effective thresholds and the per-pair oracle tolerances. - const MotionBoundTable table = - engine_.ComputeMotionBoundTable(path, pair_ids_); - std::vector pairs = pairs_; - std::vector tau(pairs.size()); - for (int p = 0; p < static_cast(pairs.size()); ++p) { - pairs[p].threshold = options.margin + padding_[p]; - tau[p] = std::max(options.query_tolerance, tau_base_[p]); - // The displacement lemma argues entirely in the separated regime, so - // the certificate is meaningless for a negative effective threshold: a - // pair meant to touch must be collision-filtered, not padded below - // zero. Negative padding is therefore rejected rather than certified. - if (pairs[p].threshold < 0.0) { - throw std::runtime_error(fmt::format( - "ContinuousCollisionChecker: margin ({}) + padding ({}) " - "is negative for the pair on bodies {} and {}. The certificate " - "is only proven for nonnegative thresholds; filter the pair out " - "instead of using negative padding.", - options.margin, padding_[p], - model_->plant().get_body(pairs[p].id.body_a).name(), - model_->plant().get_body(pairs[p].id.body_b).name())); - } - } + // 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.model = model_.get(); input.oracle = &oracle_; input.table = &table; input.path = &path; - input.pairs = &pairs; - input.tau = τ + input.pairs = &pairs_; + input.tau = &tau_; input.prefilter = &prefilter_; input.options = options; - - internal::CertifierOutput output = internal::RunCertifier(input, &pool_); - - CertificationResult result; - result.verdict = output.verdict; - result.findings = std::move(output.findings); - result.stats = output.stats; - result.stats.wall_time_s = - std::chrono::duration(std::chrono::steady_clock::now() - start) - .count(); - if (options.emit_certificate) { - result.certificate = std::move(output.certificate); - } - return result; + return internal::RunCertifier(input, &pool_); } private: std::shared_ptr> model_; Options default_options_; - KinematicsEngine engine_; - DistanceOracle oracle_; + internal::KinematicsEngine engine_; + internal::DistanceOracle oracle_; std::vector pairs_; - std::vector pair_ids_; - /* padding(p) alone; the margin is added per call. */ - std::vector padding_; - /* Drake's documented accuracy per pair; τ_p = max(query_tolerance, this). */ - std::vector tau_base_; + /* τ_p: max(kQueryTolerance, Drake's documented accuracy for the pair). */ + std::vector tau_; internal::PrefilterTable prefilter_; mutable internal::ContextPool pool_; }; -// --------------------------------------------------------------------------- -// ContinuousCollisionChecker. -// --------------------------------------------------------------------------- - -ContinuousCollisionChecker::ContinuousCollisionChecker(Params params) { - if (params.model == nullptr) { +ContinuousCollisionChecker::ContinuousCollisionChecker( + std::shared_ptr> model, + const Options& default_options) { + if (model == nullptr) { throw std::runtime_error( - "ContinuousCollisionChecker: Params::model is null; supply a " - "RobotDiagram whose plant is finalized."); + "ContinuousCollisionChecker: the model is null; supply a RobotDiagram " + "whose plant is finalized."); } - if (!params.model->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(params.default_options); - impl_ = std::make_unique(std::move(params)); + ValidateOptions(default_options); + impl_ = std::make_unique(std::move(model), default_options); } ContinuousCollisionChecker::~ContinuousCollisionChecker() = default; -CertificationResult ContinuousCollisionChecker::CheckTrajectory( +Result ContinuousCollisionChecker::CheckTrajectory( const drake::trajectories::Trajectory& trajectory, const std::optional& options) const { const Options& resolved = impl_->Resolve(options); - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, resolved); - return impl_->Check(path, resolved); + return impl_->Check(internal::PiecewiseBezierPath::FromTrajectory( + trajectory, resolved.continuous_revolute_indices), + resolved); } -CertificationResult ContinuousCollisionChecker::CheckPath( +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 " + "ContinuousCollisionChecker::CheckPath: the waypoint matrix has {} " + "rows but the plant has {} generalized positions (waypoints are " "columns).", waypoints.rows(), expected)); } - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromWaypoints(waypoints, resolved); - return impl_->Check(path, resolved); + return impl_->Check(internal::PiecewiseBezierPath::FromWaypoints(waypoints), + resolved); } -CertificationResult ContinuousCollisionChecker::CheckEdge( +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.", + "ContinuousCollisionChecker::CheckEdge: the endpoints have sizes {} " + "and {} but the plant has {} generalized positions.", q1.size(), q2.size(), expected)); } Eigen::MatrixXd waypoints(expected, 2); @@ -456,73 +308,10 @@ CertificationResult ContinuousCollisionChecker::CheckEdge( return CheckPath(waypoints, options); } -PiecewiseBezierPath ContinuousCollisionChecker::Normalize( - const drake::trajectories::Trajectory& trajectory, - const std::optional& options) const { - const Options& resolved = impl_->Resolve(options); - PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, resolved); - impl_->ValidatePath(path); - return path; -} - -MotionBoundTable ContinuousCollisionChecker::ComputeMotionBounds( - const PiecewiseBezierPath& path) const { - impl_->ValidatePath(path); - return impl_->engine().ComputeMotionBoundTable(path, impl_->pair_ids()); -} - -const DistanceOracle& ContinuousCollisionChecker::distance_oracle() const { - return impl_->oracle(); -} - -const KinematicsEngine& ContinuousCollisionChecker::kinematics_engine() const { - return impl_->engine(); -} - -const std::vector& ContinuousCollisionChecker::pairs() const { - return impl_->pairs(); -} - const RobotDiagram& ContinuousCollisionChecker::model() const { return impl_->model(); } -// --------------------------------------------------------------------------- -// VerifyCertificate. -// --------------------------------------------------------------------------- -// -// Written against the checker's *public* introspection seams only: it -// re-derives the λ table from the path, re-restricts every record's control -// points with its own de Casteljau code, recomputes w about the record's qc, -// re-queries the oracle at qc from a fresh context, and re-checks the -// interval-certificate inequality with τ_p. It then verifies that the -// certified intervals cover [0, 1] of every segment for every pair. Nothing of -// the certifier's own bookkeeping is trusted. -// -// The replay charges the checker's construction-time query tolerance and the -// Options::certificate_slack default, so a run made with a *larger* slack, and -// hence a stricter certificate, still verifies. - -bool VerifyCertificate(const ContinuousCollisionChecker& checker, - const PiecewiseBezierPath& path, - const Certificate& certificate) { - const MotionBoundTable table = checker.ComputeMotionBounds(path); - const std::vector& pairs = checker.pairs(); - const std::vector tau = ComputeTauTable( - checker.model(), pairs, checker.distance_oracle().tolerance()); - - internal::ReplayInput input; - input.model = &checker.model(); - input.oracle = &checker.distance_oracle(); - input.table = &table; - input.path = &path; - input.pairs = &pairs; - input.tau = τ - input.slack = Options{}.certificate_slack; - return internal::ReplayCertificate(input, certificate, nullptr); -} - } // 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 index bab6bca8a9ff..6fe9fa4af55f 100644 --- a/planning/continuous_collision/continuous_collision_checker.h +++ b/planning/continuous_collision/continuous_collision_checker.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -7,27 +8,75 @@ #include #include "drake/common/drake_copyable.h" +#include "drake/common/parallelism.h" #include "drake/common/trajectories/trajectory.h" -#include "drake/planning/continuous_collision/certificate.h" -#include "drake/planning/continuous_collision/distance_oracle.h" -#include "drake/planning/continuous_collision/motion_bound_table.h" -#include "drake/planning/continuous_collision/options.h" -#include "drake/planning/continuous_collision/piecewise_bezier_path.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 { -/** Result of one certification call. +/** Outcome of one check. @ingroup planning_collision_checker */ -struct CertificationResult { +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, + /** Subdivision hit the resolution floor with some pair's clearance within + oracle tolerance of the threshold (a grazing trajectory). */ + 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, as a fraction of a segment's parameter width; a node + narrower than this yields Verdict::kInconclusive instead of splitting. Must + lie in (0, 1]. */ + double min_interval{1e-9}; + /** 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{}; - /** Earliest-first. */ - std::vector findings; - Statistics stats; - /** Present iff Options::emit_certificate. */ - std::optional certificate; + /** 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 @@ -35,11 +84,11 @@ 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 margin + padding(A, B). That holds under three -assumptions: exact real arithmetic up to the configured numerical slack, a +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 -certificate is a property of the path, so retiming the trajectory afterwards -does not invalidate it. +proof is a property of the path, so retiming the trajectory afterwards does not +invalidate it. 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 @@ -52,87 +101,51 @@ class ContinuousCollisionChecker { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ContinuousCollisionChecker); - struct Params { - /** Plant + scene graph; the plant must be finalized. */ - std::shared_ptr> model; - /** Per-body-pair padding; see PaddingSpec for the env/self rule. */ - PaddingSpec padding{}; - Options default_options{}; - }; - /** Builds contexts, bounding spheres and topology tables, and runs the capability probe. - @throws std::exception if Params::model is null. - @throws std::exception if the plant is not finalized. - @throws std::exception if Params::default_options is invalid; see - CheckTrajectory() for the conditions. - @throws std::exception if PaddingSpec::per_body_pair is supplied and is not - num_bodies × num_bodies, or if any pair's padding is not finite. - @throws std::exception if the capability probe finds an unsupported pair; - see DistanceOracle's constructor. - @throws std::exception if the plant's topology or geometry defeats the - motion bound; see KinematicsEngine's constructor. */ - explicit ContinuousCollisionChecker(Params params); + @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 (any supported Drake trajectory type). - @throws std::exception if the trajectory cannot be normalized; see - PiecewiseBezierPath::FromTrajectory(). + /** 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::min_interval is outside (0, 1]. @throws std::exception if the trajectory's row count differs from the plant's number of generalized positions. - @throws std::exception if Options::margin is not finite, if - Options::query_tolerance or Options::certificate_slack is not a finite - nonnegative distance, if Options::min_interval is outside (0, 1], if - Options::max_reported_findings is below 1, or if Options::max_nodes is set - to 0. - @throws std::exception if margin + padding is negative for any pair; filter - such a pair out instead of padding it below zero. + @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, or moves a HalfSpace across a rotational - coordinate; see KinematicsEngine::ComputeMotionBoundTable(). */ - CertificationResult CheckTrajectory( - const trajectories::Trajectory& trajectory, - const std::optional& options = {}) const; - - /** Certifies a piecewise-linear path through the given waypoint columns. - @throws std::exception if `waypoints` has fewer than two columns. - @throws std::exception if `waypoints` does not have one row per generalized - position of the plant. + 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. */ - CertificationResult CheckPath( - const Eigen::MatrixXd& waypoints, - const std::optional& options = {}) const; + 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. */ - CertificationResult CheckEdge( - const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, - const std::optional& options = {}) const; - - /** Converts `trajectory` to the internal piecewise-Bézier form, for - introspection and testing. All const, and safe from arbitrary threads. - @throws std::exception if the trajectory cannot be normalized; see - PiecewiseBezierPath::FromTrajectory(). - @throws std::exception if the trajectory's row count differs from the - plant's number of generalized positions. */ - PiecewiseBezierPath Normalize( - const trajectories::Trajectory& trajectory, - const std::optional& options = {}) const; - - /** The λ table this checker would use for `path`, for introspection and - testing. - @throws std::exception if the path's row count differs from the plant's - number of generalized positions. - @throws std::exception if the path moves a coordinate of an unsupported - joint type, or moves a HalfSpace across a rotational coordinate. */ - MotionBoundTable ComputeMotionBounds(const PiecewiseBezierPath& path) const; - - const DistanceOracle& distance_oracle() const; - const KinematicsEngine& kinematics_engine() const; - const std::vector& pairs() const; + Result CheckEdge(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, + const std::optional& options = {}) const; + const RobotDiagram& model() const; private: @@ -140,23 +153,6 @@ class ContinuousCollisionChecker { std::unique_ptr impl_; }; -/** Independently replays every record of `certificate`, recomputing node -control boxes from freshly restricted control points and re-querying -distances, then checks interval coverage of the full domain for every pair. -@param checker Supplies the model, the distance oracle and the pair - table the replay is checked against; the certificate must - have been produced by this checker. -@param path The normalized path the certificate was produced for. -@param certificate The audit trail to replay. -@returns true iff the certificate is a complete proof that `path` is free. -A certificate from a run that found a violation, ended inconclusive, exhausted -its node budget, or pruned the search leaves part of the domain uncovered, and -so returns false. -@ingroup planning_collision_checker */ -bool VerifyCertificate(const ContinuousCollisionChecker& checker, - const PiecewiseBezierPath& path, - const Certificate& certificate); - } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/distance_oracle.cc b/planning/continuous_collision/distance_oracle.cc index 9576141077b4..3b431fd3b729 100644 --- a/planning/continuous_collision/distance_oracle.cc +++ b/planning/continuous_collision/distance_oracle.cc @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include @@ -12,26 +11,24 @@ #include -#include "drake/common/drake_throw.h" #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/shape_class.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; -using internal::Classify; -using internal::ShapeClass; /* Everything the analytic halfspace fallback needs about the *non*-halfspace partner, extracted once by the probe. Only the fields relevant to `klass` are @@ -201,19 +198,17 @@ std::string ClassName(ShapeClass klass) { return ""; } -/* "geometry_name (ShapeType)", for error messages and the report. */ +/* "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()); } -/* One row of the probe report: a distinct unordered shape-type combination -and the route it resolved to. */ +/* 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}; - int pair_count{0}; - /* A representative pair, used for the probe query and error messages. */ GeometryId example_a; GeometryId example_b; }; @@ -225,13 +220,9 @@ struct DistanceOracle::Impl { Keyed by geometry id because the facade hands back its own PairRecord copies, so SignedDistance() cannot index into pairs_. */ std::unordered_map support; - std::string report; }; -DistanceOracle::DistanceOracle(const RobotDiagram& model, - double query_tolerance) { - DRAKE_THROW_UNLESS(query_tolerance >= 0.0); - tolerance_ = query_tolerance; +DistanceOracle::DistanceOracle(const RobotDiagram& model) { auto impl = std::make_shared(); const drake::geometry::SceneGraph& scene_graph = model.scene_graph(); @@ -254,11 +245,9 @@ DistanceOracle::DistanceOracle(const RobotDiagram& model, } // --- Snapshot the unfiltered pairs and classify each one. ---------------- - // GetCollisionCandidates() returns a sorted std::set and std::map keeps the - // report ordering fixed, so both pairs_ and support_report() are + // GetCollisionCandidates() returns a sorted std::set, so pairs_ is // deterministic for a given model. std::map, ComboRow> combos; - std::set mesh_names; for (const auto& [id_a, id_b] : inspector.GetCollisionCandidates()) { const drake::multibody::RigidBody* body_a = @@ -313,23 +302,12 @@ DistanceOracle::DistanceOracle(const RobotDiagram& model, } } - // Meshes are certified as their convex hulls; the report says so. - if (class_a == ShapeClass::kMesh) - mesh_names.insert(inspector.GetName(id_a)); - if (class_b == ShapeClass::kMesh) - mesh_names.insert(inspector.GetName(id_b)); - const auto key = std::minmax(class_a, class_b); - const std::pair combo{key.first, key.second}; - auto it = combos.find(combo); - if (it == combos.end()) { - combos.emplace(combo, ComboRow{route, 1, id_a, id_b}); - } else { - ++it->second.pair_count; - } + combos.emplace(std::pair{key.first, key.second}, + ComboRow{route, id_a, id_b}); - pairs_.push_back(PairRecord{ - PairId{id_a, id_b, body_a->index(), body_b->index()}, route, 0.0}); + pairs_.push_back( + PairRecord{id_a, id_b, body_a->index(), body_b->index(), route}); } // --- One probe query per distinct native combination. -------------------- @@ -362,24 +340,6 @@ DistanceOracle::DistanceOracle(const RobotDiagram& model, } } - // --- Render the report. -------------------------------------------------- - std::string report = fmt::format( - "DistanceOracle capability probe: {} unfiltered pair(s), {} distinct " - "shape-type combination(s), tolerance tau = {} m.\n", - pairs_.size(), combos.size(), tolerance_); - for (const auto& [combo, row] : combos) { - const char* const route = - (row.route == DistanceRoute::kNative) - ? "native (ComputeSignedDistancePairClosestPoints, probed ok)" - : "halfspace analytic support-function fallback (exact)"; - report += fmt::format(" {}-{}: {}; {} pair(s)\n", ClassName(combo.first), - ClassName(combo.second), route, row.pair_count); - } - for (const std::string& name : mesh_names) { - report += fmt::format(" Mesh {}: certified as its convex hull\n", name); - } - impl->report = std::move(report); - impl_ = std::move(impl); } @@ -389,17 +349,16 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, Eigen::Vector3d* nearest_b_W) const { if (pair.route == DistanceRoute::kNative) { const drake::geometry::SignedDistancePair result = - query_object.ComputeSignedDistancePairClosestPoints(pair.id.a, - pair.id.b); + 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.id.a && result.id_B == pair.id.b) { + 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.id.b && result.id_B == pair.id.a) { + } else if (result.id_A == pair.b && result.id_B == pair.a) { p_ACa = result.p_BCb; p_BCb = result.p_ACa; } else { @@ -408,10 +367,10 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, "different geometry pair than the one queried."); } if (nearest_a_W != nullptr) { - *nearest_a_W = query_object.GetPoseInWorld(pair.id.a) * p_ACa; + *nearest_a_W = query_object.GetPoseInWorld(pair.a) * p_ACa; } if (nearest_b_W != nullptr) { - *nearest_b_W = query_object.GetPoseInWorld(pair.id.b) * p_BCb; + *nearest_b_W = query_object.GetPoseInWorld(pair.b) * p_BCb; } return result.distance; } @@ -431,8 +390,8 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, // 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.id.a : pair.id.b; - const GeometryId partner_id = a_is_halfspace ? pair.id.b : pair.id.a; + 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()) { @@ -463,10 +422,7 @@ double DistanceOracle::SignedDistance(const QueryObject& query_object, return phi; } -const std::string& DistanceOracle::support_report() const { - return impl_->report; -} - +} // 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 index ea9b00557469..95e68fe82729 100644 --- a/planning/continuous_collision/distance_oracle.h +++ b/planning/continuous_collision/distance_oracle.h @@ -1,113 +1,74 @@ #pragma once #include -#include #include #include #include "drake/common/drake_copyable.h" #include "drake/geometry/query_object.h" -#include "drake/planning/continuous_collision/options.h" +#include "drake/planning/continuous_collision/internal.h" #include "drake/planning/robot_diagram.h" namespace drake { namespace planning { namespace continuous_collision { +namespace internal { -/** How the oracle computes signed distance for one pair, resolved once by the -capability probe: no per-query dispatch decisions. -@ingroup planning_collision_checker */ -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 pair with its pre-resolved distance route and -effective threshold m_p = margin + padding(p). -@ingroup planning_collision_checker */ -struct PairRecord { - PairId id; - DistanceRoute route{DistanceRoute::kNative}; - /** Filled by the facade from margin + PaddingSpec. */ - double threshold{0.0}; -}; - -/** Narrowphase distance abstraction. Stateless per query and +/* Narrowphase distance abstraction. Stateless per query and thread-compatible: configuration comes in via the caller's QueryObject. -Contract: SignedDistance returns ϕ̂ with |ϕ̂ − ϕ_true| ≤ tolerance() -whenever ϕ_true is at or above −tolerance(), and returns a definitely -negative value when the shapes interpenetrate beyond tolerance. Only -over-reporting a distance at or above threshold could fake a certificate, -which is why the capability probe keeps any not-a-true-distance backend out of -the loop entirely. +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. -@ingroup planning_collision_checker */ +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 + /* 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. */ - DistanceOracle(const RobotDiagram& model, double query_tolerance); + 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 (thresholds default 0; the - facade rewrites them from margin + padding). */ + /* 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 reports world-frame - closest points when the route provides them. - - `pair` need not be an element of pairs(): the facade copies the probe's - records and rewrites their thresholds, so only `pair.id` and `pair.route` - are read here. Both routes always fill the optional out-params. + /* 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 (i.e. the - record did not come from pairs()). */ + 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; - /** τ used in the certificate arithmetic. */ - double tolerance() const { return tolerance_; } - - /** Human-readable probe report: one line per distinct shape-type - combination and its route, including the "Mesh certified as convex hull" - notices. */ - const std::string& support_report() const; - private: std::vector pairs_; - double tolerance_{1e-6}; /* Immutable capability-probe results: closed-form support data for every - halfspace partner, the resolved per-shape-combination routes, and the - rendered report. Held by shared_ptr so the oracle stays cheaply copyable + 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..283a4e70dea1 --- /dev/null +++ b/planning/continuous_collision/internal.h @@ -0,0 +1,143 @@ +#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. + +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 index da05537e27f0..990c301548d1 100644 --- a/planning/continuous_collision/motion_bound_table.cc +++ b/planning/continuous_collision/motion_bound_table.cc @@ -18,16 +18,18 @@ #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/shape_class.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; @@ -39,24 +41,158 @@ using drake::multibody::JointIndex; using drake::multibody::MultibodyPlant; using drake::multibody::ScrewJoint; using drake::multibody::WeldJoint; -using internal::IsHalfSpace; - -MotionBoundTable::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)) { - DRAKE_THROW_UNLESS(!row_start_.empty()); - DRAKE_THROW_UNLESS(row_start_.front() == 0); - for (int i = 1; i < static_cast(row_start_.size()); ++i) { - DRAKE_THROW_UNLESS(row_start_[i] >= row_start_[i - 1]); - } - DRAKE_THROW_UNLESS(coord_.size() == lambda_.size()); - DRAKE_THROW_UNLESS(static_cast(coord_.size()) == row_start_.back()); - DRAKE_THROW_UNLESS(carveout_slack_.size() + 1 == row_start_.size()); + +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( @@ -482,7 +618,8 @@ double KinematicsEngine::Reach(int joint_ord, BodyIndex body, } MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( - const PiecewiseBezierPath& path, const std::vector& pairs) const { + 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 {}.", @@ -496,7 +633,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( const Eigen::VectorXd& lower, const Eigen::VectorXd& upper, const std::vector& constant_coordinates, - const std::vector& pairs) const { + 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( @@ -519,7 +656,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( 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 Options::continuity_tolerance. That is a + // 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) { @@ -661,12 +798,12 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // 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 Options::continuity_tolerance. + // *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 Options::certificate_slack, so the + // 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 @@ -723,7 +860,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( // ‖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 - // ρ ≤ continuity_tolerance around a unit quaternion, m ≥ 1 − ρ, so + // ρ ≤ 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, @@ -756,7 +893,7 @@ MotionBoundTable KinematicsEngine::ComputeMotionBoundTable( return r; }; - for (const PairId& pair : pairs) { + 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_ || @@ -912,6 +1049,7 @@ double KinematicsEngine::body_radius(BodyIndex body) const { 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 index 2f9ebffc3df1..17bd89fe22c7 100644 --- a/planning/continuous_collision/motion_bound_table.h +++ b/planning/continuous_collision/motion_bound_table.h @@ -9,18 +9,53 @@ #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/bounding_sphere.h" -#include "drake/planning/continuous_collision/options.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 { -/** Per-pair motion-bound coefficients in CSR layout: for pair index k, a +/* 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 @@ -29,29 +64,17 @@ 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. -@ingroup planning_collision_checker */ +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). */ + /* Constructs an empty table (zero pairs). */ MotionBoundTable() = default; - /** Constructs the CSR table directly from its four arrays. - @param row_start Size num_pairs + 1, starting at 0 and non-decreasing; - row_start.back() is the total entry count. - @param coord Position-coordinate index of every entry. - @param lambda λ of every entry, element for element with `coord`. - @param carveout_slack One residual per pair. - @throws std::exception if the arrays do not satisfy those invariants. */ - MotionBoundTable(std::vector row_start, std::vector coord, - std::vector lambda, - std::vector carveout_slack); - int num_pairs() const { return static_cast(row_start_.size()) - 1; } - /** True iff J(p) is empty after the constant-coordinate carve-out: no + /* 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 @@ -61,7 +84,7 @@ class MotionBoundTable { return row_start_[pair_index] == row_start_[pair_index + 1]; } - /** Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j: a sparse dot + /* Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j: a sparse dot product against the node's per-coordinate deviations w, plus the carved coordinates' residual. @pre 0 <= pair_index < num_pairs(). @@ -74,50 +97,61 @@ class MotionBoundTable { return delta; } - /** Σ over the coordinates of J_topo(p) that the carve-out removed of + /* Σ 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 - Options::continuity_tolerance, so this is zero exactly when every carved - coordinate is exactly constant. + 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, + /* 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. +/* 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). -@ingroup planning_collision_checker */ +- 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. + /* 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. @@ -138,13 +172,13 @@ class KinematicsEngine { ComputeBoundingSphere() rejects. */ explicit KinematicsEngine(const RobotDiagram& model); - /** The position-coordinate indices whose motion changes the relative pose + /* 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 + /* 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 @@ -154,9 +188,10 @@ class KinematicsEngine { @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; + const PiecewiseBezierPath& path, + const std::vector& pairs) const; - /** Raw-data overload of the above, for callers (and tests) that already + /* 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 @@ -173,17 +208,17 @@ class KinematicsEngine { MotionBoundTable ComputeMotionBoundTable( const Eigen::VectorXd& lower, const Eigen::VectorXd& upper, const std::vector& constant_coordinates, - const std::vector& pairs) const; + const std::vector& pairs) const; - /** The bounding sphere (in its body's frame) of one proximity geometry. + /* 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. */ + /* 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 + /* 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; @@ -297,6 +332,7 @@ class KinematicsEngine { std::unordered_map geometry_spheres_; }; +} // namespace internal } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/numerics.h b/planning/continuous_collision/numerics.h deleted file mode 100644 index 83aeedb6bdb9..000000000000 --- a/planning/continuous_collision/numerics.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -/** @file -Single home of the numerical accounting used everywhere. - -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 -effective threshold (margin + padding), and ε the certificate 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. - -The certificate is mathematical modulo τ and ε. ε defaults to 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. */ - -namespace drake { -namespace planning { -namespace continuous_collision { - -/** True iff the pair is certified on the whole node. -@ingroup planning_collision_checker */ -inline bool IsCertified(double phi_hat, double tau, double motion_bound, - double threshold, double slack) { - return phi_hat - tau - motion_bound > threshold + slack; -} - -/** True iff the representative configuration is a definite violation. -@ingroup planning_collision_checker */ -inline bool IsDefiniteViolation(double phi_hat, double tau, double threshold) { - return phi_hat + tau < threshold; -} - -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/options.h b/planning/continuous_collision/options.h deleted file mode 100644 index 254b3c998d1e..000000000000 --- a/planning/continuous_collision/options.h +++ /dev/null @@ -1,141 +0,0 @@ -#pragma once - -#include -#include -#include - -#include - -#include "drake/common/parallelism.h" -#include "drake/geometry/geometry_ids.h" -#include "drake/multibody/tree/multibody_tree_indexes.h" - -namespace drake { -namespace planning { -namespace continuous_collision { - -/** Search modes for certification. -@ingroup planning_collision_checker */ -enum class SearchMode { - /** Return on the first definite violation; serial execution returns the - earliest one in time. */ - kFindFirstViolation, - /** Certify the full domain and return every violation / inconclusive - region found (bounded by Options::max_reported_findings). */ - kCertifyAll, -}; - -/** Outcome of a certification run. -@ingroup planning_collision_checker */ -enum class Verdict { - /** Proof: every unfiltered pair keeps signed distance > margin + padding - over the entire continuous time domain. */ - kCertifiedFree, - /** An exactly-on-trajectory configuration violates the threshold. */ - kViolationFound, - /** Subdivision hit the resolution floor with some pair's clearance within - oracle tolerance of the threshold (grazing trajectory). */ - kInconclusive, - /** The optional node budget was exhausted first. */ - kBudgetExhausted, -}; - -/** Options controlling one certification call. -@ingroup planning_collision_checker */ -struct Options { - /** Global clearance margin δ in meters. The certificate proves signed - distance > margin + padding for every pair at every time. */ - double margin{0.0}; - /** Junction C0-continuity tolerance (per coordinate; modulo 2π for - coordinates listed in continuous_revolute_indices). */ - double continuity_tolerance{1e-7}; - /** τ: the distance oracle's accuracy contract in meters. */ - double query_tolerance{1e-6}; - /** ε_slack: swallows floating-point noise in the bound arithmetic. */ - double certificate_slack{1e-9}; - /** Resolution floor as a fraction of a segment's parameter width; nodes - narrower than this become kInconclusive findings instead of splitting. */ - double min_interval{1e-9}; - /** Position coordinates whose junction continuity is checked modulo 2π - (GcsTrajectoryOptimization continuous-revolute convention). - @see planning::trajectory_optimization::GetContinuousRevoluteJointIndices */ - std::vector continuous_revolute_indices{}; - /** Maximum polynomial degree accepted for monomial→Bernstein conversion. */ - int max_conversion_degree{10}; - SearchMode mode{SearchMode::kCertifyAll}; - int max_reported_findings{32}; - /** Optional node budget; exceeded => Verdict::kBudgetExhausted. */ - std::optional max_nodes{}; - /** If true, every certification event is recorded into a Certificate that - VerifyCertificate() can independently replay. */ - bool emit_certificate{false}; - Parallelism parallelism{Parallelism::Max()}; -}; - -/** Per-body-pair padding: the effective threshold for pair p is -margin + padding(p). - -Which of the two scalars applies to a pair is decided by *anchoring*, from -plant topology alone. A body is anchored iff no position coordinate of the -plant changes its pose relative to the world, i.e. the world body itself and -everything welded to it directly or transitively. A pair is a self-collision -pair iff both of its bodies are non-anchored, and an environment pair -otherwise. The rule never depends on which trajectory is being checked. -@ingroup planning_collision_checker */ -struct PaddingSpec { - /** Padding for robot-vs-environment pairs, i.e. pairs with at least one - anchored body. */ - double env_padding{0.0}; - /** Padding for robot-vs-robot (self-collision) pairs, i.e. pairs whose two - bodies are both non-anchored. */ - double self_padding{0.0}; - /** Optional dense symmetric matrix indexed by BodyIndex, sized - num_bodies × num_bodies. Entry (a, b) overrides the scalars for that body - pair; a NaN entry means "not covered", and that pair falls back to - env_padding / self_padding. */ - std::optional per_body_pair{}; -}; - -/** Identifies an unfiltered proximity geometry pair. -@ingroup planning_collision_checker */ -struct PairId { - geometry::GeometryId a; - geometry::GeometryId b; - multibody::BodyIndex body_a; - multibody::BodyIndex body_b; -}; - -/** One violation or inconclusive record. -@ingroup planning_collision_checker */ -struct Finding { - /** Trajectory time of the witness configuration. */ - double time{}; - /** The witness configuration, exactly on the trajectory. */ - Eigen::VectorXd q; - PairId pair; - /** Oracle signed distance at q for this pair. */ - double distance{}; - /** Motion bound Δ_p at the terminal node (0 for breakpoint findings). */ - double motion_bound{}; - /** true => definite violation; false => grazing / inconclusive. */ - bool definite{}; - /** Closest points in world frame at q, when the narrowphase provides - them (violation findings; planners use these to push trajectories out - of collision). */ - std::optional nearest_a_W{}; - std::optional nearest_b_W{}; -}; - -/** Cost accounting for one certification call. -@ingroup planning_collision_checker */ -struct Statistics { - uint64_t nodes{0}; - uint64_t narrowphase_queries{0}; - uint64_t sphere_certifications{0}; - int max_depth{0}; - double wall_time_s{0.0}; -}; - -} // 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 index ebcf39b20c21..a3584f2693fa 100644 --- a/planning/continuous_collision/piecewise_bezier_path.cc +++ b/planning/continuous_collision/piecewise_bezier_path.cc @@ -19,10 +19,12 @@ #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; @@ -132,10 +134,9 @@ coefficients become alpha_a = c_a * (t_end - t_start)^a, and the exact monomial 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 options.max_conversion_ -degree. */ +The map is increasingly ill-conditioned in m, hence kMaxConversionDegree. */ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, - const Options& options, int source_index, + int source_index, std::vector* segments) { if (pp.cols() != 1) { throw std::runtime_error(fmt::format( @@ -157,15 +158,14 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, for (int r = 0; r < num_positions; ++r) { m = std::max(m, pp.getSegmentPolynomialDegree(k, r, 0)); } - if (m > options.max_conversion_degree) { + if (m > kMaxConversionDegree) { throw std::runtime_error(fmt::format( "PiecewiseBezierPath: PiecewisePolynomial segment {} (source segment " - "index {}) has polynomial degree {}, above " - "options.max_conversion_degree = {}. The monomial-to-Bernstein " - "change of basis is ill-conditioned at high degree; either raise " - "Options::max_conversion_degree or re-express the " - "trajectory with more, lower-degree segments.", - k, source_index, m, options.max_conversion_degree)); + "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); @@ -209,8 +209,7 @@ void AppendPiecewisePolynomialSegments(const PiecewisePolynomial& pp, recursing through CompositeTrajectory. `source_index` counts source segments visited so far and appears in error messages. */ -void AppendSegments(const Trajectory& trajectory, - const Options& options, int* source_index, +void AppendSegments(const Trajectory& trajectory, int* source_index, std::vector* segments) { if (const auto* bezier = dynamic_cast*>(&trajectory)) { @@ -238,7 +237,7 @@ void AppendSegments(const Trajectory& trajectory, *source_index)); } for (int i = 0; i < num; ++i) { - AppendSegments(composite->segment(i), options, source_index, segments); + AppendSegments(composite->segment(i), source_index, segments); } return; } @@ -250,7 +249,7 @@ void AppendSegments(const Trajectory& trajectory, } if (const auto* pp = dynamic_cast*>(&trajectory)) { - AppendPiecewisePolynomialSegments(*pp, options, *source_index, segments); + AppendPiecewisePolynomialSegments(*pp, *source_index, segments); ++(*source_index); return; } @@ -266,7 +265,8 @@ void AppendSegments(const Trajectory& trajectory, /* Checks shape, time ordering/contiguity and C0 junctions (trajectory * normalization). */ -void ValidateSegments(int num_positions, const Options& options, +void ValidateSegments(int num_positions, + const std::vector& continuous_revolute_indices, const std::vector& segments) { if (segments.empty()) { throw std::runtime_error( @@ -308,7 +308,7 @@ void ValidateSegments(int num_positions, const Options& options, } std::vector is_continuous_revolute(num_positions, false); - for (int index : options.continuous_revolute_indices) { + for (int index : continuous_revolute_indices) { if (index < 0 || index >= num_positions) { throw std::runtime_error(fmt::format( "PiecewiseBezierPath: Options::continuous_revolute_indices contains " @@ -334,18 +334,18 @@ void ValidateSegments(int num_positions, const Options& options, if (is_continuous_revolute[c]) { gap -= 2 * M_PI * std::round(gap / (2 * M_PI)); } - if (std::abs(gap) > options.continuity_tolerance) { + 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 Options::continuity_tolerance = {}. A discontinuous " + "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, options.continuity_tolerance, c)); + i - 1, i, c, raw_gap, modulo, kContinuityTolerance, c)); } } } @@ -354,7 +354,8 @@ void ValidateSegments(int num_positions, const Options& options, } // namespace PiecewiseBezierPath PiecewiseBezierPath::FromTrajectory( - const Trajectory& trajectory, const Options& options) { + 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; " @@ -372,14 +373,14 @@ PiecewiseBezierPath PiecewiseBezierPath::FromTrajectory( PiecewiseBezierPath path; path.num_positions_ = num_positions; int source_index = 0; - AppendSegments(trajectory, options, &source_index, &path.segments_); - ValidateSegments(num_positions, options, path.segments_); - path.FinalizeMetadata(options.continuity_tolerance); + 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, const Options& options) { + const Eigen::MatrixXd& waypoints) { if (waypoints.rows() < 1) { throw std::runtime_error( "PiecewiseBezierPath::FromWaypoints: the waypoint matrix has zero " @@ -410,12 +411,12 @@ PiecewiseBezierPath PiecewiseBezierPath::FromWaypoints( segment.control_points.col(1) = waypoints.col(k + 1); path.segments_.push_back(std::move(segment)); } - ValidateSegments(num_positions, options, path.segments_); - path.FinalizeMetadata(options.continuity_tolerance); + ValidateSegments(num_positions, {}, path.segments_); + path.FinalizeMetadata(); return path; } -void PiecewiseBezierPath::FinalizeMetadata(double continuity_tolerance) { +void PiecewiseBezierPath::FinalizeMetadata() { const int n = num_positions_; global_lower_ = Eigen::VectorXd::Constant(n, std::numeric_limits::infinity()); @@ -434,7 +435,7 @@ void PiecewiseBezierPath::FinalizeMetadata(double continuity_tolerance) { constant_coordinates_.assign(n, false); for (int i = 0; i < n; ++i) { constant_coordinates_[i] = - (global_upper_[i] - global_lower_[i]) <= continuity_tolerance; + (global_upper_[i] - global_lower_[i]) <= kContinuityTolerance; } } @@ -536,6 +537,7 @@ void DeCasteljauSplitAtHalf(const Eigen::MatrixXd& cps, Eigen::MatrixXd* left, *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 index 9dc5c80e5c79..bd7fc1d2444a 100644 --- a/planning/continuous_collision/piecewise_bezier_path.h +++ b/planning/continuous_collision/piecewise_bezier_path.h @@ -6,24 +6,23 @@ #include "drake/common/drake_copyable.h" #include "drake/common/trajectories/trajectory.h" -#include "drake/planning/continuous_collision/options.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]. -@ingroup planning_collision_checker */ +/* One Bézier segment q(s) = Σ_j B_{j,m}(s) P_j, s ∈ [0, 1]. */ struct BezierSegment { - /** Original time interval (bookkeeping only; the certificate is a property - of the path and is invariant under time reparametrization). */ + /* 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. */ + /* 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 +/* 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. @@ -33,62 +32,58 @@ 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). -@ingroup planning_collision_checker */ +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, + /* 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 - options.max_conversion_degree, or junction discontinuity beyond - options.continuity_tolerance (modulo 2π for coordinates in - options.continuous_revolute_indices). */ + kMaxConversionDegree, or junction discontinuity beyond kContinuityTolerance + (modulo 2π for coordinates in `continuous_revolute_indices`). */ static PiecewiseBezierPath FromTrajectory( const trajectories::Trajectory& trajectory, - const Options& options); + const std::vector& continuous_revolute_indices); - /** Normalizes an n × K waypoint matrix into K−1 order-1 segments (exact). + /* 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. - @throws std::exception if `waypoints` has zero rows. */ - static PiecewiseBezierPath FromWaypoints(const Eigen::MatrixXd& waypoints, - const Options& options); + @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 + /* 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 the continuity - tolerance) across all control points of all segments; such coordinates are - treated as welded for the check. */ + /* 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 + /* Evaluates the path at time t, for tests and breakpoint checks; the hot loop uses de Casteljau apexes instead. - @pre t lies in [start_time(), end_time()], up to a parameter slack. - @throws std::exception if t is outside that domain. */ + @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]. - @pre 0 <= segment_index < segments().size(). - @pre s lies in [0, 1], up to a parameter slack. - @throws std::exception if either precondition is violated. */ + /* 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(double continuity_tolerance); + void FinalizeMetadata(); int num_positions_{}; std::vector segments_; @@ -97,14 +92,14 @@ class PiecewiseBezierPath { std::vector constant_coordinates_; }; -/** Splits the Bézier control matrix `cps` (n × (m+1)) at u = 1/2 by de +/* 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. -@ingroup planning_collision_checker */ +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/shape_class.h b/planning/continuous_collision/shape_class.h deleted file mode 100644 index 9e4da12f294b..000000000000 --- a/planning/continuous_collision/shape_class.h +++ /dev/null @@ -1,70 +0,0 @@ -#pragma once - -#include - -#include "drake/common/unused.h" -#include "drake/geometry/shape_specification.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace internal { - -/* 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; -} - -} // 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 index 1a346aa165c6..33c486c6a8a1 100644 --- a/planning/continuous_collision/test/api_test.cc +++ b/planning/continuous_collision/test/api_test.cc @@ -2,10 +2,14 @@ // 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 pydrake surface is covered separately, in +// "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 @@ -16,12 +20,7 @@ #include #include -#include "drake/common/copyable_unique_ptr.h" #include "drake/common/test_utilities/expect_throws_message.h" -#include "drake/common/trajectories/composite_trajectory.h" -#include "drake/common/trajectories/piecewise_polynomial.h" -#include "drake/common/trajectories/piecewise_quaternion.h" -#include "drake/common/trajectories/trajectory.h" #include "drake/geometry/geometry_instance.h" #include "drake/geometry/proximity_properties.h" #include "drake/multibody/fem/deformable_body_config.h" @@ -37,10 +36,6 @@ namespace { using drake::geometry::GeometryInstance; using drake::geometry::ProximityProperties; using drake::multibody::Joint; -using drake::trajectories::CompositeTrajectory; -using drake::trajectories::PiecewisePolynomial; -using drake::trajectories::PiecewiseQuaternionSlerp; -using drake::trajectories::Trajectory; using Eigen::Vector3d; using Eigen::VectorXd; using test::BezierCurve; @@ -48,7 +43,7 @@ using test::Box; using test::Friction; using test::HalfSpace; using test::Inertia; -using test::MakeChecker; +using test::MakeCheckerPtr; using test::MultibodyPlant; using test::Parallelism; using test::PrismaticJoint; @@ -142,7 +137,7 @@ VectorXd FloatingQ(const Vector3d& p, double elbow) { GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { std::shared_ptr> model = MakeFloatingBaseWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeCheckerPtr(model, SerialOptions()); const std::string joint_name = FloatingJointName(model->plant()); ASSERT_FALSE(joint_name.empty()); @@ -156,7 +151,7 @@ GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { points(0, 1) = 0.7071067811865476; // w points(3, 1) = 0.7071067811865476; // z EXPECT_THAT(ThrowMessage([&]() { - checker.CheckTrajectory(BezierCurve(0.0, 1.0, points)); + checker->CheckTrajectory(BezierCurve(0.0, 1.0, points)); }), AllOf(HasSubstr(joint_name), HasSubstr("quaternion_floating"), HasSubstr("constant-coordinate carve-out"))); @@ -168,87 +163,35 @@ GTEST_TEST(ApiTest, MovingQuaternionBaseThrowsNamingTheJoint) { translated.col(1) = FloatingQ(Vector3d(0.2, 0.0, 0.0), 0.0); EXPECT_THAT( ThrowMessage([&]() { - checker.CheckTrajectory(BezierCurve(0.0, 1.0, translated)); + checker->CheckTrajectory(BezierCurve(0.0, 1.0, translated)); }), AllOf(HasSubstr(joint_name), HasSubstr("coordinate 4"))); } -// 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 and owes no residual at all, while a -// base held only to within continuity_tolerance is still carved but owes -// lambda-tilde times its range, charged to MotionBoundTable::carveout_slack(). -// That residual has to survive the static-pair shortcut, which never evaluates -// a per-node Delta, and the certificate replay, which recomputes Delta from -// scratch and would reject a record whose bound came out smaller. -void CheckConstantFloatingBase(double wobble) { - SCOPED_TRACE("wobble = " + std::to_string(wobble)); - std::shared_ptr> model = MakeFloatingBaseWorld(); - Options options = SerialOptions(); - options.emit_certificate = true; - ASSERT_LE(wobble, options.continuity_tolerance); - const auto checker = MakeChecker(model, options); - - 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; - const BezierCurve trajectory(0.0, 1.0, points); - - const PiecewiseBezierPath path = checker.Normalize(trajectory, options); - const std::vector& constant = path.constant_coordinates(); - ASSERT_EQ(constant.size(), 8u); - for (int i = 0; i < 7; ++i) { - EXPECT_TRUE(constant[i]) - << "base coordinate " << i << " should have been flagged constant"; - } - EXPECT_FALSE(constant[7]); - // The control-point range of the wobbled coordinate: `wobble` up to the - // rounding of adding it to -0.10 and subtracting again. - const double range = - path.global_upper_bound()[5] - path.global_lower_bound()[5]; - EXPECT_NEAR(range, wobble, 1e-9 * std::max(wobble, 1e-12)); - - const MotionBoundTable table = checker.ComputeMotionBounds(path); - bool any_static_with_slack = false; - for (int p = 0; p < table.num_pairs(); ++p) { - if (wobble == 0.0) { - EXPECT_EQ(table.carveout_slack(p), 0.0) << "pair " << p; - } else if (table.carveout_slack(p) > 0.0) { - if (table.pair_is_static(p)) any_static_with_slack = true; - // lambda-tilde = 1 for a floating base's translation coordinates, and - // only that one coordinate has a width, so the residual is exactly it. - EXPECT_DOUBLE_EQ(table.carveout_slack(p), range) << "pair " << p; - } - } - EXPECT_EQ(any_static_with_slack, wobble > 0.0) - << "the base-vs-post pair depends only on the carved base coordinates, " - "so it is static and must still owe the residual"; - - const CertificationResult result = - checker.CheckTrajectory(trajectory, options); - EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); - ASSERT_TRUE(result.certificate.has_value()); - EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); - for (const CertificateRecord& record : result.certificate->records) { - if (table.pair_is_static(record.pair_index)) { - EXPECT_GE(record.motion_bound, table.carveout_slack(record.pair_index)); +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); } } -GTEST_TEST(ApiTest, ExactlyConstantQuaternionBaseIsAcceptedEndToEnd) { - CheckConstantFloatingBase(0.0); -} - -GTEST_TEST(ApiTest, ToleranceConstantQuaternionBaseChargesItsResidual) { - CheckConstantFloatingBase(6e-8); -} - // --------------------------------------------------------------------------- // 2. Geometry scope: rotating half spaces and deformables. // --------------------------------------------------------------------------- @@ -273,7 +216,7 @@ GTEST_TEST(ApiTest, RotatingHalfSpaceThrowsAtConstruction) { std::shared_ptr> model = builder.Build(); EXPECT_THAT(ThrowMessage([&]() { - MakeChecker(model, SerialOptions()); + MakeCheckerPtr(model, SerialOptions()); }), AllOf(HasSubstr("blade_halfspace"), HasSubstr("post_geom"), HasSubstr("spin"), HasSubstr("Box"))); @@ -297,14 +240,10 @@ GTEST_TEST(ApiTest, AnchoredHalfSpaceUnderARotatingArmIsAccepted) { RigidTransformd(Vector3d(0.0, 0.0, -0.4))); plant.RegisterCollisionGeometry(ground, RigidTransformd(), HalfSpace(), "ground_halfspace", Friction()); - std::shared_ptr> model = builder.Build(); - const auto checker = MakeChecker(model, SerialOptions()); - // The probe report is part of the UX: it must say how each pair is routed. - EXPECT_THAT(checker.distance_oracle().support_report(), - HasSubstr("HalfSpace")); + const auto checker = MakeCheckerPtr(builder.Build(), SerialOptions()); EXPECT_EQ( - checker.CheckEdge(VectorXd::Constant(1, 0.0), VectorXd::Constant(1, 1.5)) + checker->CheckEdge(VectorXd::Constant(1, 0.0), VectorXd::Constant(1, 1.5)) .verdict, Verdict::kCertifiedFree); } @@ -346,7 +285,7 @@ GTEST_TEST(ApiTest, DeformableGeometryIsRefusedNamingIt) { 1u); EXPECT_THAT(ThrowMessage([&]() { - MakeChecker(model, SerialOptions()); + MakeCheckerPtr(model, SerialOptions()); }), AllOf(HasSubstr("deformable"), HasSubstr("squishy_blob"))); } @@ -355,143 +294,44 @@ GTEST_TEST(ApiTest, DeformableGeometryIsRefusedNamingIt) { // 3. Dimensions, trajectory validation and options. // --------------------------------------------------------------------------- -GTEST_TEST(ApiTest, NegativeEffectiveThresholdIsRejected) { - // The displacement lemma is proved in the separated regime only, so a - // negative effective threshold (margin + padding < 0) is outside what the - // checker can certify and must be rejected, not silently "certified". - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - Options options = SerialOptions(); - options.margin = -0.01; - EXPECT_THAT(ThrowMessage([&]() { - checker.CheckEdge(VectorXd::Zero(2), VectorXd::Constant(2, 0.1), - options); - }), - AllOf(HasSubstr("negative"), HasSubstr("filter the pair"))); -} - GTEST_TEST(ApiTest, DimensionMismatchMessagesNameTheSizes) { std::shared_ptr> model = MakeArmWorld(); - const auto checker = MakeChecker(model, SerialOptions()); + const auto checker = MakeCheckerPtr(model, SerialOptions()); ASSERT_EQ(model->plant().num_positions(), 2); EXPECT_THAT(ThrowMessage([&]() { - checker.CheckPath(Eigen::MatrixXd::Zero(3, 4)); + 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)); + checker->CheckEdge(VectorXd::Zero(2), VectorXd::Zero(5)); }), AllOf(HasSubstr("CheckEdge"), HasSubstr("sizes 2 and 5"))); EXPECT_THAT(ThrowMessage([&]() { - checker.CheckTrajectory( + 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)), + DRAKE_EXPECT_THROWS_MESSAGE(checker->CheckPath(Eigen::MatrixXd::Zero(2, 1)), ".*at least 2 waypoints.*"); } -GTEST_TEST(ApiTest, DiscontinuousTrajectoryThrowsNamingTheJunction) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - - Eigen::MatrixXd first(2, 2); - first << 0.0, 0.3, 0.0, 0.05; - Eigen::MatrixXd second(2, 2); - // Coordinate 1 teleports by 0.4 m at the junction. - second << 0.3, 0.6, 0.45, 0.50; - std::vector>> segments; - segments.emplace_back(std::make_unique>(0.0, 1.0, first)); - segments.emplace_back( - std::make_unique>(1.0, 2.0, second)); - const CompositeTrajectory trajectory(std::move(segments)); - - EXPECT_THAT( - ThrowMessage([&]() { - checker.CheckTrajectory(trajectory); - }), - AllOf(HasSubstr("C0 discontinuity"), HasSubstr("segments 0 and 1"), - HasSubstr("coordinate 1"), HasSubstr("continuity_tolerance"))); -} - -GTEST_TEST(ApiTest, DegreeAboveConversionCapThrows) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - - // 13 interpolation nodes => one polynomial segment of degree 12, above the - // default max_conversion_degree of 10. - const int kNodes = 13; - VectorXd times(kNodes); - Eigen::MatrixXd samples(2, kNodes); - for (int i = 0; i < kNodes; ++i) { - times[i] = i; - samples(0, i) = 0.1 * ((i % 3) - 1); - samples(1, i) = 0.02 * ((i % 5) - 2); - } - const PiecewisePolynomial trajectory = - PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, - samples); - EXPECT_THAT(ThrowMessage([&]() { - checker.CheckTrajectory(trajectory); - }), - AllOf(HasSubstr("polynomial degree 12"), - HasSubstr("max_conversion_degree"))); - - // Raising the cap is the documented escape hatch. - Options options = SerialOptions(); - options.max_conversion_degree = 12; - EXPECT_NO_THROW(checker.Normalize(trajectory, options)); -} - -GTEST_TEST(ApiTest, UnsupportedTrajectoryTypeThrowsNamingTheType) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - const PiecewiseQuaternionSlerp trajectory( - std::vector{0.0, 1.0}, - std::vector>{ - Eigen::Quaternion::Identity(), - Eigen::Quaternion(0.7071067811865476, 0.0, 0.0, - 0.7071067811865476)}); - // The message must name the offending type and list what is accepted. - EXPECT_THAT(ThrowMessage([&]() { - checker.CheckTrajectory(trajectory); - }), - AllOf(HasSubstr("unsupported trajectory type"), - HasSubstr("PiecewiseQuaternionSlerp"), - HasSubstr("BezierCurve"), HasSubstr("BsplineTrajectory"))); -} - -GTEST_TEST(ApiTest, ContinuousRevoluteIndexOutOfRangeThrows) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - Options options = SerialOptions(); - options.continuous_revolute_indices = {0, 5}; - - Eigen::MatrixXd points(2, 2); - points << 0.0, 0.2, 0.0, 0.05; - EXPECT_THAT(ThrowMessage([&]() { - checker.CheckTrajectory(BezierCurve(0.0, 1.0, points), - options); - }), - AllOf(HasSubstr("continuous_revolute_indices contains 5,"), - HasSubstr("2 generalized positions"))); - - // A negative index is out of range too. - options.continuous_revolute_indices = {-1}; - DRAKE_EXPECT_THROWS_MESSAGE( - checker.CheckTrajectory(BezierCurve(0.0, 1.0, points), options), - ".*continuous_revolute_indices contains -1,.*"); -} - GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + 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. + // 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". const std::vector>> cases = { {"min_interval", @@ -502,21 +342,9 @@ GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { [](Options* o) { o->min_interval = 2.0; }}, - {"max_reported_findings", - [](Options* o) { - o->max_reported_findings = 0; - }}, - {"query_tolerance", + {"nonnegative", [](Options* o) { - o->query_tolerance = -1.0; - }}, - {"certificate_slack", - [](Options* o) { - o->certificate_slack = -1e-9; - }}, - {"max_nodes", - [](Options* o) { - o->max_nodes = 0; + o->margin = -0.01; }}, {"margin", [](Options* o) { @@ -528,7 +356,7 @@ GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { Options bad = SerialOptions(); mutate(&bad); EXPECT_THAT(ThrowMessage([&]() { - checker.CheckTrajectory(trajectory, bad); + checker->CheckTrajectory(trajectory, bad); }), HasSubstr(needle)); } @@ -539,49 +367,10 @@ GTEST_TEST(ApiTest, NullModelIsRefused) { // 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. - ContinuousCollisionChecker::Params params; - EXPECT_THAT( - ThrowMessage([&]() { - ContinuousCollisionChecker checker(params); - }), - AllOf(HasSubstr("Params::model is null"), HasSubstr("finalized"))); -} - -GTEST_TEST(ApiTest, MaxReportedFindingsIsRespected) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - const Options options = SerialOptions(); - - // Sweep the arm out past the post at theta ~ pi/2 with the tool extended and - // back again: two segments, each with its own violating region, so - // kCertifyAll (which drops a violating pair once per subtree) has more than - // one finding to cap. - Eigen::MatrixXd waypoints(2, 3); - waypoints << 0.0, 2.4, 0.0, 0.25, 0.25, 0.25; - - const CertificationResult uncapped = checker.CheckPath(waypoints, options); - ASSERT_EQ(uncapped.verdict, Verdict::kViolationFound); - ASSERT_GE(uncapped.findings.size(), 2u); - EXPECT_LE(static_cast(uncapped.findings.size()), - options.max_reported_findings); - - for (const int cap : {1, 2}) { - SCOPED_TRACE("cap = " + std::to_string(cap)); - Options capped = options; - capped.max_reported_findings = cap; - const CertificationResult result = checker.CheckPath(waypoints, capped); - EXPECT_EQ(result.verdict, Verdict::kViolationFound); - // Exactly `cap`, not merely at most: the sink keeps the cap earliest - // entries, and this run has more than `cap` of them. An "at most" assertion - // would be satisfied by a regression that returned nothing, which would - // also make the prefix check below vacuous. - ASSERT_EQ(static_cast(result.findings.size()), cap); - // The cap keeps the earliest findings, so a capped run is a prefix of the - // uncapped one: dropping the latest entry never removes an earlier one. - for (std::size_t i = 0; i < result.findings.size(); ++i) { - EXPECT_EQ(result.findings[i].time, uncapped.findings[i].time); - EXPECT_EQ(result.findings[i].definite, uncapped.findings[i].definite); - } - } + EXPECT_THAT(ThrowMessage([&]() { + ContinuousCollisionChecker checker(nullptr); + }), + AllOf(HasSubstr("model is null"), HasSubstr("finalized"))); } } // namespace diff --git a/planning/continuous_collision/test/bounding_sphere_test.cc b/planning/continuous_collision/test/bounding_sphere_test.cc index 92a39691a4c7..cbcfa6f04429 100644 --- a/planning/continuous_collision/test/bounding_sphere_test.cc +++ b/planning/continuous_collision/test/bounding_sphere_test.cc @@ -5,8 +5,6 @@ // set of supported shapes and pins the throw-on-unsupported behaviour. Never // loosen the tolerance to make a case pass. -#include "drake/planning/continuous_collision/bounding_sphere.h" - #include #include #include @@ -24,11 +22,13 @@ #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; @@ -314,6 +314,7 @@ GTEST_TEST(BoundingSphereTest, ThrowsOnUnsupportedShapes) { } } // namespace +} // namespace internal } // namespace continuous_collision } // namespace planning } // namespace drake diff --git a/planning/continuous_collision/test/certificate_test.cc b/planning/continuous_collision/test/certificate_test.cc deleted file mode 100644 index ca4fc2903c7e..000000000000 --- a/planning/continuous_collision/test/certificate_test.cc +++ /dev/null @@ -1,583 +0,0 @@ -// Tests VerifyCertificate, which replays every certification event from the -// checker's public seams, re-restricting control points, recomputing motion -// bounds and re-querying distances, then checks that the certified intervals -// tile the whole domain for every pair. -// -// The corpus is three certified runs: one hand-built world whose two pairs are -// built to certify at very different depths, plus two small random worlds. -// Below it is a table of mutations, each applied to every corpus case; a -// mutation any case accepts is a hole in the audit. - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "drake/planning/continuous_collision/test/test_utilities.h" - -namespace drake { -namespace planning { -namespace continuous_collision { -namespace { - -using Eigen::Vector3d; -using Eigen::VectorXd; -using test::BezierCurve; -using test::Box; -using test::Friction; -using test::Inertia; -using test::MakeRandomWorld; -using test::MultibodyPlant; -using test::Parallelism; -using test::PrismaticJoint; -using test::RigidBody; -using test::RigidTransformd; -using test::RobotDiagram; -using test::RobotDiagramBuilder; -using test::Sphere; - -// A non-zero margin and a non-zero environment padding, so m_p = margin + -// padding is a number a tamperer could plausibly try to lower and the -// "threshold below what the options call for" branch has something to bite on. -constexpr double kMargin = 0.005; -constexpr double kEnvPadding = 0.002; - -Options AuditOptions() { - Options options; - options.margin = kMargin; - options.parallelism = Parallelism::None(); - options.emit_certificate = true; - return options; -} - -std::unique_ptr MakeAuditChecker( - std::shared_ptr> model) { - PaddingSpec padding; - padding.env_padding = kEnvPadding; - padding.self_padding = kEnvPadding; - return test::MakeCheckerPtr(std::move(model), AuditOptions(), padding); -} - -// A 2-dof Cartesian gantry (prismatic x, prismatic y) carrying a 5 mm sphere, -// with exactly two unfiltered pairs: -// -// * tool vs. "near_plate": a 1 mm plate offset in y so the clearance is a -// constant 12 mm. With m_p = 0.007 and λ = 1 for the moving x coordinate, -// certification needs Δ = w_x < 0.012 − 0.007 − τ ≈ 0.005 against 0.6 m of -// travel, so it first certifies at depth 6 and produces dozens of records. -// * tool vs. "far_ball": 3 m away, certified by the sphere prefilter at the -// root, so exactly one record per segment. -// -// The record-relabelling mutation needs two pairs this far apart in difficulty. -std::unique_ptr> MakeDesignedWorld() { - RobotDiagramBuilder builder; - MultibodyPlant& plant = builder.plant(); - const RigidBody& carriage = plant.AddRigidBody("carriage", Inertia()); - const RigidBody& tool = plant.AddRigidBody("tool", Inertia()); - plant.AddJoint("gantry_x", plant.world_body(), {}, carriage, - {}, Vector3d::UnitX()); - plant.AddJoint("gantry_y", carriage, {}, tool, {}, - Vector3d::UnitY()); - plant.RegisterCollisionGeometry(tool, RigidTransformd(), Sphere(0.005), - "tool_geom", Friction()); - - const RigidBody& plate = plant.AddRigidBody("near_plate", Inertia()); - plant.WeldFrames(plant.world_frame(), plate.body_frame(), - RigidTransformd(Vector3d(0.0, 0.0175, 0.0))); - plant.RegisterCollisionGeometry(plate, RigidTransformd(), - Box(0.9, 0.001, 0.6), "near_plate_geom", - Friction()); - - const RigidBody& ball = plant.AddRigidBody("far_ball", Inertia()); - plant.WeldFrames(plant.world_frame(), ball.body_frame(), - RigidTransformd(Vector3d(0.0, 3.0, 0.0))); - plant.RegisterCollisionGeometry(ball, RigidTransformd(), Sphere(0.05), - "far_ball_geom", Friction()); - return builder.Build(); -} - -// A cubic Bézier whose control points are equally spaced from `start` to `end`. -// It is the straight segment, but with four control points, so a mutation can -// perturb an interior one without moving either endpoint; moving an endpoint -// would change the path's start configuration and short-circuit the check. -Eigen::MatrixXd CubicControlPoints(const VectorXd& start, const VectorXd& end) { - Eigen::MatrixXd points(start.size(), 4); - for (int j = 0; j < 4; ++j) { - const double u = j / 3.0; - points.col(j) = (1.0 - u) * start + u * end; - } - return points; -} - -struct AuditCase { - std::string name; - std::shared_ptr> model; - std::unique_ptr checker; - Eigen::MatrixXd control_points; - std::optional path; - Certificate certificate; - // True when this case's two pairs were built to have wildly different - // certification depths (only the hand-built world). - bool designed{false}; - - // The path a verifier would be handed if one control point were nudged. - PiecewiseBezierPath PerturbedPath(double delta) const { - Eigen::MatrixXd points = control_points; - points(0, 1) += delta; - return checker->Normalize(BezierCurve(0.0, 1.0, points), - AuditOptions()); - } - - bool Verify(const Certificate& certificate_in) const { - return VerifyCertificate(*checker, *path, certificate_in); - } -}; - -// Builds the corpus once. Every entry is a run that ended -// Verdict::kCertifiedFree with an emitted certificate; a case that failed to -// certify is dropped rather than added, so CorpusIsBuiltAndVerifies is the -// single place that reports a short corpus. No gtest assertion belongs here: -// this initializer runs inside whichever test touches Corpus() first, which -// changes under --gtest_filter or --gtest_shuffle. -// -// The vector is allocated and never freed because it owns RobotDiagrams and -// checkers whose destruction would otherwise race Drake's static teardown. -const std::vector>& Corpus() { - static const std::vector>* corpus = [] { - auto* cases = new std::vector>(); - const Options options = AuditOptions(); - const auto add = [&cases, &options](std::unique_ptr entry) { - const BezierCurve trajectory(0.0, 1.0, entry->control_points); - const CertificationResult result = - entry->checker->CheckTrajectory(trajectory, options); - if (result.verdict != Verdict::kCertifiedFree) return; - entry->path = entry->checker->Normalize(trajectory, options); - entry->certificate = *result.certificate; - cases->push_back(std::move(entry)); - }; - - { // 1. The designed world. - auto entry = std::make_unique(); - entry->name = "designed_gantry"; - entry->designed = true; - entry->model = MakeDesignedWorld(); - entry->checker = MakeAuditChecker(entry->model); - VectorXd start(2), end(2); - start << -0.3, 0.0; - end << 0.3, 0.0; - entry->control_points = CubicControlPoints(start, end); - add(std::move(entry)); - } - - // 2. Small random worlds: the first two seeds whose trajectory certifies. - // Sweeping deterministically, rather than hard-coding lucky seeds, still - // fills the corpus if the geometry ever shifts underneath it. - for (uint64_t seed = 1; seed <= 60 && cases->size() < 3; ++seed) { - auto entry = std::make_unique(); - entry->name = "random_world_seed_" + std::to_string(seed); - test::WorldSpec spec; - spec.num_links = 3; - spec.num_obstacles = 3; - spec.floor = false; - entry->model = MakeRandomWorld(seed, spec); - entry->checker = MakeAuditChecker(entry->model); - const int n = entry->model->plant().num_positions(); - VectorXd start = VectorXd::Zero(n); - VectorXd end = VectorXd::Zero(n); - for (int i = 0; i < n; ++i) { - start[i] = 0.15 * ((i % 2 == 0) ? 1.0 : -1.0); - end[i] = start[i] + 0.25; - } - entry->control_points = CubicControlPoints(start, end); - add(std::move(entry)); - } - return cases; - }(); - return *corpus; -} - -// Record counts per pair, for picking "the hardest" and "the easiest" pair. -std::vector RecordsPerPair(const AuditCase& entry) { - std::vector counts(entry.certificate.pairs.size(), 0); - for (const CertificateRecord& record : entry.certificate.records) { - ++counts[record.pair_index]; - } - return counts; -} - -// True iff `pair`'s records cover [0, 1] of every segment. This is the coverage -// property VerifyCertificate checks, re-derived here so a test can assert that -// a mutation left coverage intact and therefore had to be caught by the -// per-record arithmetic instead. -bool TilesEverySegment(const Certificate& certificate, int pair, - std::size_t num_segments) { - for (std::size_t segment = 0; segment < num_segments; ++segment) { - std::vector> intervals; - for (const CertificateRecord& record : certificate.records) { - if (record.pair_index == pair && - record.segment == static_cast(segment)) { - intervals.emplace_back(record.s_start, record.s_end); - } - } - std::sort(intervals.begin(), intervals.end()); - double covered_to = 0.0; - for (const auto& [lo, hi] : intervals) { - if (lo > covered_to) break; - covered_to = std::max(covered_to, hi); - } - if (!(covered_to >= 1.0)) return false; - } - return true; -} - -// Index of a record whose pair the trajectory actually moves and whose interval -// is a proper sub-interval, which is the kind a tamperer would target. -int MovingRecordIndex(const AuditCase& entry) { - const MotionBoundTable table = - entry.checker->ComputeMotionBounds(*entry.path); - for (int i = 0; i < static_cast(entry.certificate.records.size()); ++i) { - const CertificateRecord& record = entry.certificate.records[i]; - if (!table.pair_is_static(record.pair_index) && record.s_end < 1.0) { - return i; - } - } - return -1; -} - -// --------------------------------------------------------------------------- -// 1. Baseline. -// --------------------------------------------------------------------------- - -GTEST_TEST(CertificateAuditTest, CorpusIsBuiltAndVerifies) { - const auto& corpus = Corpus(); - ASSERT_GE(corpus.size(), 3u) - << "the corpus needs the designed world plus at least two random ones; a " - "case that failed to certify is dropped rather than added empty, so a " - "short corpus means a run stopped certifying"; - ASSERT_TRUE(corpus.front()->designed) - << "the designed world must be first: the mutations that need its pair " - "structure index Corpus().front()"; - for (const auto& entry : corpus) { - SCOPED_TRACE(entry->name); - EXPECT_FALSE(entry->certificate.records.empty()); - EXPECT_EQ(entry->certificate.pairs.size(), entry->checker->pairs().size()); - EXPECT_TRUE(entry->Verify(entry->certificate)); - } -} - -GTEST_TEST(CertificateAuditTest, DesignedWorldHasTheIntendedPairStructure) { - ASSERT_FALSE(Corpus().empty()); - const AuditCase& entry = *Corpus().front(); - ASSERT_TRUE(entry.designed); - ASSERT_EQ(entry.certificate.pairs.size(), 2u) - << "the designed world should present exactly the tool/plate and " - "tool/ball pairs"; - const std::vector counts = RecordsPerPair(entry); - // The far pair certifies at the root: exactly one record, for the path's one - // segment. The 12 mm pair needs Δ = w_x < 0.012 − 0.007 − τ ≈ 0.005 against - // 0.6 m of travel, i.e. a node half-width of 0.3/2^d < 0.005 => d = 6, and a - // constant clearance means every depth-6 node certifies it: 2^6 = 64 records. - // Pinned exactly, so a regression that loosened or tightened the motion bound - // by even one bisection level shows up here rather than hiding behind an - // inequality. - EXPECT_EQ(*std::min_element(counts.begin(), counts.end()), 1); - EXPECT_EQ(*std::max_element(counts.begin(), counts.end()), 64); - // Every pair's records must claim the same, correct threshold. - for (const CertificateRecord& record : entry.certificate.records) { - EXPECT_DOUBLE_EQ(record.threshold, kMargin + kEnvPadding); - } -} - -// --------------------------------------------------------------------------- -// 2. Mutations, each applied to every corpus case. -// --------------------------------------------------------------------------- - -using Mutation = std::function; - -// Applies `mutate` to every corpus case and requires the result to be rejected. -// `mutate` returns false when the case cannot host the mutation. -void ExpectRejectedEverywhere(const std::string& what, const Mutation& mutate) { - int applied = 0; - for (const auto& entry : Corpus()) { - SCOPED_TRACE(what + " on " + entry->name); - Certificate certificate = entry->certificate; - if (!mutate(*entry, &certificate)) continue; - ++applied; - EXPECT_FALSE(entry->Verify(certificate)) - << "VerifyCertificate accepted a certificate mutated by: " << what; - } - EXPECT_GT(applied, 0) << "the mutation '" << what - << "' was never applicable to any corpus case"; -} - -GTEST_TEST(CertificateAuditTest, RejectsTamperedClearance) { - ExpectRejectedEverywhere("inflate phi_hat", - [](const AuditCase&, Certificate* certificate) { - if (certificate->records.empty()) return false; - certificate->records.front().phi_hat += 1.0; - return true; - }); - // ... and the mirror image: a record whose claimed clearance no longer - // exceeds its own threshold proves nothing. - ExpectRejectedEverywhere("shrink phi_hat to the threshold", - [](const AuditCase&, Certificate* certificate) { - if (certificate->records.empty()) return false; - certificate->records.front().phi_hat = - certificate->records.front().threshold; - return true; - }); -} - -GTEST_TEST(CertificateAuditTest, RejectsWidenedInterval) { - ExpectRejectedEverywhere( - "widen a certified interval", - [](const AuditCase& entry, Certificate* certificate) { - const int index = MovingRecordIndex(entry); - if (index < 0) return false; - CertificateRecord& record = certificate->records[index]; - const double width = record.s_end - record.s_start; - record.s_end = std::min(1.0, record.s_end + width); - return record.s_end > entry.certificate.records[index].s_end; - }); -} - -GTEST_TEST(CertificateAuditTest, RejectsShiftedRepresentativeConfiguration) { - ExpectRejectedEverywhere( - "shift qc off the trajectory", - [](const AuditCase& entry, Certificate* certificate) { - const int index = MovingRecordIndex(entry); - if (index < 0) return false; - certificate->records[index].qc[0] += 0.05; - return true; - }); -} - -GTEST_TEST(CertificateAuditTest, RejectsMissingRecords) { - // The certifier's intervals tile the domain disjointly, so deleting any - // record punches a coverage hole, even one whose own arithmetic was sound. - ExpectRejectedEverywhere( - "delete a record", [](const AuditCase&, Certificate* certificate) { - if (certificate->records.size() < 2) return false; - certificate->records.erase(certificate->records.begin()); - return true; - }); - ExpectRejectedEverywhere( - "truncate the record list", - [](const AuditCase&, Certificate* certificate) { - if (certificate->records.size() < 4) return false; - certificate->records.resize(certificate->records.size() * 3 / 4); - return true; - }); -} - -GTEST_TEST(CertificateAuditTest, RejectsLoweredThreshold) { - // Lower every record of one pair, so the replay's self-consistency check - // ("all records of a pair claim the same threshold") passes and the mutation - // has to be caught by the check that actually matters: the claimed threshold - // must be at least the margin + padding the options call for. - ExpectRejectedEverywhere( - "lower one pair's threshold below margin + padding", - [](const AuditCase&, Certificate* certificate) { - if (certificate->records.empty()) return false; - const int pair = certificate->records.front().pair_index; - for (CertificateRecord& record : certificate->records) { - if (record.pair_index == pair) record.threshold -= 0.003; - } - return true; - }); - // Self-consistency is not enough either: a certificate whose records *all* - // agree on a threshold nobody asked for proves a claim nobody asked for. - ExpectRejectedEverywhere( - "lower every threshold uniformly", - [](const AuditCase&, Certificate* certificate) { - if (certificate->records.empty()) return false; - for (CertificateRecord& record : certificate->records) { - record.threshold = -1e9; - } - return true; - }); -} - -GTEST_TEST(CertificateAuditTest, RejectsPairTableMismatch) { - ExpectRejectedEverywhere("drop a pair from the snapshot", - [](const AuditCase&, Certificate* certificate) { - if (certificate->pairs.size() < 2) return false; - certificate->pairs.pop_back(); - return true; - }); - ExpectRejectedEverywhere("swap two entries of the pair snapshot", - [](const AuditCase&, Certificate* certificate) { - if (certificate->pairs.size() < 2) return false; - std::swap(certificate->pairs.front(), - certificate->pairs.back()); - return true; - }); -} - -GTEST_TEST(CertificateAuditTest, RejectsRelabelledPairRecords) { - // Relabelling records between two pairs of similar difficulty can be a true - // statement about a claim nobody made, so this mutation is only meaningful - // where the pair structure is designed: give the 12 mm pair the far ball's - // single root-wide record and its motion bound, half the 0.6 m travel, swamps - // its 5 mm of slack. - ASSERT_FALSE(Corpus().empty()); - const AuditCase& entry = *Corpus().front(); - ASSERT_TRUE(entry.designed); - const std::vector counts = RecordsPerPair(entry); - const int hardest = static_cast( - std::max_element(counts.begin(), counts.end()) - counts.begin()); - const int easiest = static_cast( - std::min_element(counts.begin(), counts.end()) - counts.begin()); - ASSERT_NE(hardest, easiest); - - Certificate certificate = entry.certificate; - for (CertificateRecord& record : certificate.records) { - if (record.pair_index == hardest) { - record.pair_index = easiest; - } else if (record.pair_index == easiest) { - record.pair_index = hardest; - } - } - // Relabelling permutes two complete tilings, so coverage is not what catches - // this. That is verified rather than assumed, because a mutation that - // happened to break coverage would make the test pass for the wrong reason - // and leave the arithmetic untested. - for (int pair = 0; pair < static_cast(certificate.pairs.size()); - ++pair) { - EXPECT_TRUE( - TilesEverySegment(certificate, pair, entry.path->segments().size())) - << "pair " << pair - << " lost its full tiling, so this mutation would " - "have been caught by the coverage check instead of the arithmetic"; - } - EXPECT_FALSE(entry.Verify(certificate)); -} - -GTEST_TEST(CertificateAuditTest, RejectsPerturbedPath) { - // The certificate is a statement about one specific path. A path with a - // nudged interior control point must not verify: every record's qc stops - // being the midpoint apex of the interval it names. - for (const auto& entry : Corpus()) { - SCOPED_TRACE(entry->name); - const PiecewiseBezierPath perturbed = entry->PerturbedPath(0.05); - EXPECT_FALSE( - VerifyCertificate(*entry->checker, perturbed, entry->certificate)); - } -} - -GTEST_TEST(CertificateAuditTest, AcceptsReorderedRecords) { - // Re-ordering is the one item on the classic mutation list that must NOT be - // rejected: a permutation of a valid proof is still a valid proof. Pinning - // this keeps a future "records must arrive sorted" shortcut from being - // mistaken for a security property, and it rules out a verifier that rejects - // everything, which would pass every mutation above. - std::mt19937 rng(20260826); - int shuffled = 0; - for (const auto& entry : Corpus()) { - SCOPED_TRACE(entry->name); - Certificate certificate = entry->certificate; - if (certificate.records.size() < 2) continue; - ++shuffled; - std::shuffle(certificate.records.begin(), certificate.records.end(), rng); - EXPECT_TRUE(entry->Verify(certificate)) - << "a permutation of a valid certificate is still a valid certificate"; - } - EXPECT_GE(shuffled, 3) << "every corpus case should have had a record list " - "long enough to permute"; -} - -// --------------------------------------------------------------------------- -// 3. Runs whose certificate is not a proof. -// --------------------------------------------------------------------------- - -GTEST_TEST(CertificateAuditTest, NoCertificateUnlessRequested) { - ASSERT_FALSE(Corpus().empty()); - const AuditCase& entry = *Corpus().front(); - Options options = AuditOptions(); - options.emit_certificate = false; - const BezierCurve trajectory(0.0, 1.0, entry.control_points); - const CertificationResult result = - entry.checker->CheckTrajectory(trajectory, options); - ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); - EXPECT_FALSE(result.certificate.has_value()); -} - -GTEST_TEST(CertificateAuditTest, NonFreeVerdictCertificateIsNotAProof) { - // The certificate field is present whenever emit_certificate was asked for, - // and the records the run did make are individually valid. A run that found a - // violation dropped that pair from the subtree instead of certifying it, so - // the trail cannot cover the domain and the replay refuses it: usable as an - // audit trail, unusable as a proof. - ASSERT_FALSE(Corpus().empty()); - const AuditCase& entry = *Corpus().front(); - const Options options = AuditOptions(); - // The designed world driven straight through the 1 mm plate at y = 0.0175: - // q(t) sweeps y from 0 to 0.05 while x crosses the plate's span. - VectorXd start(2), end(2); - start << -0.3, 0.0; - end << 0.3, 0.05; - const BezierCurve trajectory(0.0, 1.0, - CubicControlPoints(start, end)); - const PiecewiseBezierPath path = - entry.checker->Normalize(trajectory, options); - - const CertificationResult violating = - entry.checker->CheckTrajectory(trajectory, options); - ASSERT_EQ(violating.verdict, Verdict::kViolationFound); - ASSERT_TRUE(violating.certificate.has_value()); - EXPECT_FALSE(VerifyCertificate(*entry.checker, path, *violating.certificate)) - << "a certificate from a violating run must not read as a proof"; - - // Same for a run stopped by the node budget. That needs the free trajectory: - // a definite violation outranks budget exhaustion in the verdict reduction, - // so the budget branch is only reachable when nothing violates. - Options budgeted = options; - budgeted.max_nodes = 3; - const BezierCurve free_trajectory(0.0, 1.0, entry.control_points); - const CertificationResult truncated = - entry.checker->CheckTrajectory(free_trajectory, budgeted); - ASSERT_EQ(truncated.verdict, Verdict::kBudgetExhausted); - ASSERT_TRUE(truncated.certificate.has_value()); - EXPECT_FALSE(entry.Verify(*truncated.certificate)); - - // ... and for kFindFirstViolation, which additionally prunes the search: - // every node starting after the witness is skipped, so whole stretches of the - // domain are never visited at all. - Options find_first = options; - find_first.mode = SearchMode::kFindFirstViolation; - const CertificationResult pruned = - entry.checker->CheckTrajectory(trajectory, find_first); - ASSERT_EQ(pruned.verdict, Verdict::kViolationFound); - ASSERT_TRUE(pruned.certificate.has_value()); - EXPECT_FALSE(VerifyCertificate(*entry.checker, path, *pruned.certificate)); -} - -GTEST_TEST(CertificateAuditTest, - FindFirstViolationOnAFreeTrajectoryStillCovers) { - // The complement, pinned so the rule above is not mistaken for "the mode - // invalidates certificates": with nothing to find, kFindFirstViolation has - // nothing to prune against, explores the same tree as kCertifyAll, and its - // trail is a complete proof. - ASSERT_FALSE(Corpus().empty()); - const AuditCase& entry = *Corpus().front(); - Options options = AuditOptions(); - options.mode = SearchMode::kFindFirstViolation; - const BezierCurve trajectory(0.0, 1.0, entry.control_points); - const CertificationResult result = - entry.checker->CheckTrajectory(trajectory, options); - ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); - ASSERT_TRUE(result.certificate.has_value()); - EXPECT_TRUE(entry.Verify(*result.certificate)); -} - -} // namespace -} // 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 index 52d9675cfd8d..402cf0c29cba 100644 --- a/planning/continuous_collision/test/certifier_test.cc +++ b/planning/continuous_collision/test/certifier_test.cc @@ -1,8 +1,8 @@ // 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 certificate mutation sweep in -// test/certificate_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. +// 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 @@ -18,6 +18,7 @@ #include +#include "drake/planning/continuous_collision/piecewise_bezier_path.h" #include "drake/planning/continuous_collision/test/test_utilities.h" namespace drake { @@ -27,13 +28,14 @@ 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::MakeChecker; +using test::MakeCheckerPtr; using test::MultibodyPlant; using test::Parallelism; using test::PrismaticJoint; @@ -141,6 +143,10 @@ VectorXd MakeQ(double theta1, double theta2, double 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()}; @@ -153,26 +159,24 @@ struct SampledClearance { // 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 ContinuousCollisionChecker& checker, +SampledClearance SampleClearance(const RobotDiagram& model, const PiecewiseBezierPath& path, int samples_per_segment, double threshold) { - const RobotDiagram& model = checker.model(); 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 BezierSegment& segment = path.segments()[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; - const VectorXd q = path.EvaluateSegment(k, s); - model.plant().SetPositions(&plant_context, q); + 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 PairRecord& pair : checker.pairs()) { - const double phi = - checker.distance_oracle().SignedDistance(query_object, pair); + 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 = @@ -189,21 +193,19 @@ SampledClearance SampleClearance(const ContinuousCollisionChecker& checker, // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, FreeTrajectoryCertified) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + 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 CertificationResult result = checker.CheckTrajectory(trajectory); + const Result result = checker->CheckTrajectory(trajectory); EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); - EXPECT_TRUE(result.findings.empty()); - EXPECT_GT(result.stats.nodes, 0u); - EXPECT_GT(result.stats.sphere_certifications, 0u); - EXPECT_FALSE(result.certificate.has_value()); + 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 PiecewiseBezierPath path = checker.Normalize(trajectory); const SampledClearance sampled = - SampleClearance(checker, path, 10000, kMargin); + SampleClearance(*model, Normalize(trajectory), 10000, kMargin); EXPECT_GT(sampled.min_clearance, kMargin); EXPECT_TRUE(std::isnan(sampled.first_crossing)); @@ -212,9 +214,9 @@ GTEST_TEST(CertifierTest, FreeTrajectoryCertified) { 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->CheckPath(waypoints).verdict, Verdict::kCertifiedFree); EXPECT_EQ( - checker.CheckEdge(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10)).verdict, + checker->CheckEdge(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10)).verdict, Verdict::kCertifiedFree); } @@ -228,101 +230,59 @@ GTEST_TEST(CertifierTest, NarrowGapCertifiedBySubdivision) { 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 auto checker = MakeChecker(builder.Build(), SerialOptions()); + 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); - Options options = SerialOptions(); - options.emit_certificate = true; - const CertificationResult result = - checker.CheckTrajectory(trajectory, options); + const Result result = checker->CheckTrajectory(trajectory); EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); - EXPECT_TRUE(result.findings.empty()); + 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.stats.max_depth, 4); + EXPECT_GE(result.num_nodes, 16u); - const PiecewiseBezierPath path = checker.Normalize(trajectory); const SampledClearance sampled = - SampleClearance(checker, path, 10000, kMargin); + SampleClearance(*model, Normalize(trajectory), 10000, kMargin); EXPECT_GT(sampled.min_clearance, kMargin); EXPECT_LT(sampled.min_clearance, kMargin + 0.01); - ASSERT_TRUE(result.certificate.has_value()); - EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); } // --------------------------------------------------------------------------- -// 2. A sweeping trajectory that hits an obstacle. +// 2. A sweeping trajectory that hits an obstacle: the witness is exact, and it +// is the earliest one. // --------------------------------------------------------------------------- -GTEST_TEST(CertifierTest, ViolationFoundWithExactWitness) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); +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 CertificationResult result = checker.CheckTrajectory(trajectory); + const Result result = checker->CheckTrajectory(trajectory); ASSERT_EQ(result.verdict, Verdict::kViolationFound); - ASSERT_FALSE(result.findings.empty()); - const Finding& finding = result.findings.front(); - EXPECT_TRUE(finding.definite); + 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 = checker.Normalize(trajectory); + const PiecewiseBezierPath path = Normalize(trajectory); EXPECT_LT((path.Value(finding.time) - finding.q).cwiseAbs().maxCoeff(), 1e-9); - const double phi = DistanceAtFinding(checker, finding); + const double phi = DistanceAtFinding(*model, finding); EXPECT_LT(phi, kMargin); EXPECT_NEAR(phi, finding.distance, 1e-12); -} - -GTEST_TEST(CertifierTest, FindFirstReturnsEarliestWitness) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(2.0, 0.0, 0.0), 1); - - Options options = SerialOptions(); - options.mode = SearchMode::kFindFirstViolation; - const CertificationResult result = - checker.CheckTrajectory(trajectory, options); - ASSERT_EQ(result.verdict, Verdict::kViolationFound); - ASSERT_EQ(result.findings.size(), 1u); - const PiecewiseBezierPath path = checker.Normalize(trajectory); - const SampledClearance sampled = - SampleClearance(checker, path, 10000, kMargin); - ASSERT_FALSE(std::isnan(sampled.first_crossing)); // The branch-and-bound recursion drives the reported witness to the earliest // violating time, which dense sampling brackets from above. - EXPECT_NEAR(result.findings.front().time, sampled.first_crossing, 5e-3); - EXPECT_LE(result.findings.front().time, sampled.first_crossing + 1e-9); -} - -GTEST_TEST(CertifierTest, CertifyAllReportsEveryViolation) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - // Sweeping θ1 from 0 to 3 rad passes the post (≈1.57 rad) and then the - // pillar (≈2.58 rad): two disjoint violating regions, different pairs. - const BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(3.0, 0.0, 0.0), 1); - - const CertificationResult result = checker.CheckTrajectory(trajectory); - ASSERT_EQ(result.verdict, Verdict::kViolationFound); - ASSERT_GE(result.findings.size(), 2u); - int definite = 0; - for (std::size_t i = 0; i < result.findings.size(); ++i) { - if (i > 0) { - EXPECT_LE(result.findings[i - 1].time, result.findings[i].time) - << "findings must be earliest-first"; - } - if (result.findings[i].definite) { - ++definite; - EXPECT_LT(DistanceAtFinding(checker, result.findings[i]), kMargin); - } - } - EXPECT_GE(definite, 2); + 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) { @@ -333,7 +293,8 @@ GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { MultibodyPlant& plant = builder.plant(); AddArm(&plant); AddWeldedSphere(&plant, "graze", Vector3d(0.80, 0.11, 0.0), 0.05); - const auto checker = MakeChecker(builder.Build(), SerialOptions()); + 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); @@ -341,260 +302,105 @@ GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { // A coarser floor keeps the cost of the tangency cascade bounded; the // verdict is what matters here, not the depth. options.min_interval = 1e-4; - const CertificationResult result = - checker.CheckTrajectory(trajectory, options); + const Result result = checker->CheckTrajectory(trajectory, options); EXPECT_EQ(result.verdict, Verdict::kInconclusive); - ASSERT_FALSE(result.findings.empty()); - const Finding& finding = result.findings.front(); - EXPECT_FALSE(finding.definite); + ASSERT_TRUE(result.finding.has_value()); // The near-witness sits within a hair of the threshold. - EXPECT_NEAR(finding.distance, kMargin, 1e-3); - EXPECT_NEAR(DistanceAtFinding(checker, finding), finding.distance, 1e-12); + 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 PiecewiseBezierPath path = checker.Normalize(trajectory); const SampledClearance sampled = - SampleClearance(checker, path, 10000, kMargin); + SampleClearance(*model, Normalize(trajectory), 10000, kMargin); EXPECT_NEAR(sampled.min_clearance, kMargin, 1e-6); } // --------------------------------------------------------------------------- -// 3. Static pairs (J(p) = ∅) are resolved once and certified globally. +// 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). // --------------------------------------------------------------------------- -// 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: a coordinate that no control point of the trajectory moves is -// removed from every J(p), and pairs left with an empty set are resolved once -// at q(t0). -GTEST_TEST(CertifierTest, StaticPairsResolvedOnce) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - // Only the prismatic coordinate moves: θ1 and θ2 are constant, so every pair - // whose relative pose depends only on them becomes static. - const BezierCurve trajectory = - MakeBezier(MakeQ(0.3, -0.2, 0.0), MakeQ(0.3, -0.2, 0.15), 2); - - const PiecewiseBezierPath path = checker.Normalize(trajectory); - const MotionBoundTable table = checker.ComputeMotionBounds(path); +GTEST_TEST(CertifierTest, BreakpointWitnessesAreReported) { + const auto model = MakeArmWorld(); + const auto checker = MakeCheckerPtr(model, SerialOptions()); - int num_static = 0; - int num_moving = 0; - for (int p = 0; p < table.num_pairs(); ++p) { - (table.pair_is_static(p) ? num_static : num_moving) += 1; - } - EXPECT_GT(num_static, 0) << "the constant-coordinate carve-out should have " - "made the link1/link2 pairs static"; - EXPECT_GT(num_moving, 0); + // 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); - Options options = SerialOptions(); - options.emit_certificate = true; - const CertificationResult result = - checker.CheckTrajectory(trajectory, options); - ASSERT_EQ(result.verdict, Verdict::kCertifiedFree); - ASSERT_TRUE(result.certificate.has_value()); - - // A static pair is certified exactly once: one full-segment record per - // segment, all sharing the single representative configuration q(t0). It - // never appears in a node record. - const int num_segments = static_cast(path.segments().size()); - std::vector records_per_pair(table.num_pairs(), 0); - for (const CertificateRecord& record : result.certificate->records) { - ++records_per_pair[record.pair_index]; - if (table.pair_is_static(record.pair_index)) { - EXPECT_EQ(record.s_start, 0.0); - EXPECT_EQ(record.s_end, 1.0); - EXPECT_EQ(record.motion_bound, 0.0); - EXPECT_LT( - (record.qc - path.EvaluateSegment(0, 0.0)).cwiseAbs().maxCoeff(), - 1e-15); - } - } - for (int p = 0; p < table.num_pairs(); ++p) { - if (table.pair_is_static(p)) { - EXPECT_EQ(records_per_pair[p], num_segments) - << "static pair " << p << " was resolved more than once"; - } - } - EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)); - - // A static record must be measured at the path's own start configuration: - // "static" is relative to the carve-out, so a record re-based onto an - // off-path configuration would measure a different pair pose entirely. Both - // directions: rotating θ1 toward the obstacles reduces the clearance the - // replay measures, while rotating away *increases* it, and only the "static - // records are pinned to q(t0)" check catches that second case. - for (const double delta : {1.5, -1.5}) { - Certificate certificate = *result.certificate; - int tampered = 0; - for (CertificateRecord& record : certificate.records) { - if (table.pair_is_static(record.pair_index)) { - record.qc[0] += delta; - ++tampered; - break; - } - } - ASSERT_EQ(tampered, 1); - EXPECT_FALSE(VerifyCertificate(checker, path, certificate)) - << "delta = " << delta; - } + // 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); } -// --------------------------------------------------------------------------- -// 4. Padding reaches the effective threshold, and the env/self split is the -// documented one (self = both bodies move relative to the world). -// --------------------------------------------------------------------------- - -GTEST_TEST(CertifierTest, PaddingSemantics) { +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 BezierCurve trajectory = - MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.8, -0.4, 0.10), 3); - const auto is_arm_self_pair = [&model](const Finding& finding) { - const auto& plant = model->plant(); - const std::string a = plant.get_body(finding.pair.body_a).name(); - const std::string b = plant.get_body(finding.pair.body_b).name(); - return (a == "link1" || a == "link2" || a == "tool") && - (b == "link1" || b == "link2" || b == "tool"); - }; - - // The one robot-vs-robot pair (link1, tool) keeps ≈ 0.27 m of clearance on - // this trajectory, so 0.4 m of *self* padding must break it, and nothing - // else: every other pair has an anchored side and takes the (zero) - // environment padding. Mirrored, 0.5 m of environment padding reaches the - // arm-vs-obstacle pairs (the ground halfspace is 0.45 m away) and leaves the - // self pair alone. - for (const bool self : {true, false}) { - SCOPED_TRACE(self ? "self padding" : "env padding"); - PaddingSpec padding; - (self ? padding.self_padding : padding.env_padding) = self ? 0.40 : 0.50; - const auto checker = MakeChecker(model, SerialOptions(), padding); - const CertificationResult result = checker.CheckTrajectory(trajectory); - ASSERT_EQ(result.verdict, Verdict::kViolationFound); - for (const Finding& finding : result.findings) { - EXPECT_EQ(is_arm_self_pair(finding), self) - << "padding must apply to exactly one class of pair"; - } - } + const auto checker = MakeCheckerPtr(model, SerialOptions()); - // A per-body-pair matrix overrides the scalars ... - PaddingSpec overridden; - overridden.env_padding = 0.50; - overridden.per_body_pair = Eigen::MatrixXd::Zero(model->plant().num_bodies(), - model->plant().num_bodies()); - EXPECT_EQ(MakeChecker(model, SerialOptions(), overridden) - .CheckTrajectory(trajectory) + // 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); - // ... and a mis-sized matrix is a clear throw. - PaddingSpec mis_sized; - mis_sized.per_body_pair = Eigen::MatrixXd::Zero(2, 2); - EXPECT_THROW(MakeChecker(model, SerialOptions(), mis_sized), std::exception); + // 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); } // --------------------------------------------------------------------------- -// 5. Retiming invariance: the certificate is a property of the path. +// 4. Retiming invariance: the proof is a property of the path. // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, RetimingInvariance) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + 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); - Options options = SerialOptions(); - options.emit_certificate = true; - const CertificationResult a = - checker.CheckTrajectory(MakeBezier(start, end, 3, 0.0, 1.0), options); - const CertificationResult b = - checker.CheckTrajectory(MakeBezier(start, end, 3, -2.5, 4.2), options); + 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.stats.nodes, b.stats.nodes); - EXPECT_EQ(a.stats.narrowphase_queries, b.stats.narrowphase_queries); - EXPECT_EQ(a.stats.sphere_certifications, b.stats.sphere_certifications); - EXPECT_EQ(a.stats.max_depth, b.stats.max_depth); - - ASSERT_TRUE(a.certificate.has_value()); - ASSERT_TRUE(b.certificate.has_value()); - ASSERT_EQ(a.certificate->records.size(), b.certificate->records.size()); - for (size_t i = 0; i < a.certificate->records.size(); ++i) { - const CertificateRecord& ra = a.certificate->records[i]; - const CertificateRecord& rb = b.certificate->records[i]; - // The certified interval structure lives in parameter space, so it is - // bit-identical; only the reported *times* would differ. - EXPECT_EQ(ra.segment, rb.segment); - EXPECT_EQ(ra.s_start, rb.s_start); - EXPECT_EQ(ra.s_end, rb.s_end); - EXPECT_EQ(ra.pair_index, rb.pair_index); - EXPECT_EQ(ra.phi_hat, rb.phi_hat); - EXPECT_EQ(ra.motion_bound, rb.motion_bound); - EXPECT_EQ(ra.threshold, rb.threshold); - EXPECT_TRUE(ra.qc == rb.qc); - } -} - -// --------------------------------------------------------------------------- -// 6. The node budget and breakpoint semantics. -// --------------------------------------------------------------------------- - -GTEST_TEST(CertifierTest, NodeBudgetExhausted) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - Eigen::MatrixXd waypoints(3, 4); - waypoints.col(0) = MakeQ(0.0, 0.0, 0.0); - waypoints.col(1) = MakeQ(0.3, -0.1, 0.03); - waypoints.col(2) = MakeQ(0.6, -0.3, 0.07); - waypoints.col(3) = MakeQ(0.8, -0.4, 0.10); - - Options options = SerialOptions(); - options.max_nodes = 1; - const CertificationResult result = checker.CheckPath(waypoints, options); - EXPECT_EQ(result.verdict, Verdict::kBudgetExhausted); - ASSERT_FALSE(result.findings.empty()); - // The remainder is reported as a non-definite finding at the earliest - // uncovered time. - EXPECT_FALSE(result.findings.front().definite); - EXPECT_GE(result.findings.front().time, 0.0); -} - -GTEST_TEST(CertifierTest, BreakpointWitnessesAreReported) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); - - // q(t0) puts the arm straight into the post. Only the breakpoint pre-pass can - // produce a witness *exactly* at t0; node midpoints are strictly interior. - const CertificationResult 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_FALSE(at_start.findings.empty()); - const Finding& first = at_start.findings.front(); - EXPECT_EQ(first.time, 0.0); - EXPECT_TRUE(first.definite); - EXPECT_EQ(first.motion_bound, 0.0); - EXPECT_LT(DistanceAtFinding(checker, first), kMargin); - - // A 3-waypoint path whose middle waypoint (the junction between segments, at - // t = 1) is inside the post: the pre-pass must report the junction - // configuration itself, not only interior node midpoints. - 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 CertificationResult at_junction = checker.CheckPath(waypoints); - ASSERT_EQ(at_junction.verdict, Verdict::kViolationFound); - bool found_junction_witness = false; - for (const Finding& finding : at_junction.findings) { - if (finding.time == 1.0 && finding.definite && - finding.motion_bound == 0.0) { - found_junction_witness = true; - EXPECT_LT(DistanceAtFinding(checker, finding), kMargin); - } - } - EXPECT_TRUE(found_junction_witness); + EXPECT_EQ(a.verdict, Verdict::kCertifiedFree); + EXPECT_EQ(a.num_nodes, b.num_nodes); } // --------------------------------------------------------------------------- -// 7. A small seeded soundness sweep. The full corpus (random worlds, +// 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 @@ -602,7 +408,8 @@ GTEST_TEST(CertifierTest, BreakpointWitnessesAreReported) { // --------------------------------------------------------------------------- GTEST_TEST(CertifierTest, RandomTrajectoriesAreSoundAgainstDenseSampling) { - const auto checker = MakeChecker(MakeArmWorld(), SerialOptions()); + 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); @@ -616,25 +423,24 @@ GTEST_TEST(CertifierTest, RandomTrajectoriesAreSoundAgainstDenseSampling) { control_points.col(j) << theta1(rng), theta2(rng), slide(rng); } const BezierCurve trajectory(0.0, 1.0, control_points); - const CertificationResult result = checker.CheckTrajectory(trajectory); - const PiecewiseBezierPath path = checker.Normalize(trajectory); + const Result result = checker->CheckTrajectory(trajectory); + const PiecewiseBezierPath path = Normalize(trajectory); if (result.verdict == Verdict::kCertifiedFree) { ++certified; const SampledClearance sampled = - SampleClearance(checker, path, 2000, kMargin); + 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; - } - for (const Finding& finding : result.findings) { - if (!finding.definite) continue; + } 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(checker, finding), kMargin) + EXPECT_LT(DistanceAtFinding(*model, finding), kMargin) << "trial " << trial; } } diff --git a/planning/continuous_collision/test/concurrency_test.cc b/planning/continuous_collision/test/concurrency_test.cc index 1654bbba5d94..a08a814dd0e9 100644 --- a/planning/continuous_collision/test/concurrency_test.cc +++ b/planning/continuous_collision/test/concurrency_test.cc @@ -3,15 +3,13 @@ // 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} in both search modes, -// and in kCertifyAll so are `nodes` and `narrowphase_queries`. (In -// kFindFirstViolation the branch-and-bound bound arrives at different -// times, so the statistics are not deterministic; the witness still is.) -// 2. Serial mode is bit-deterministic. -// 3. The public Check* methods are safe to call concurrently on one instance. -// 4. The deep workload, which unlike any corpus case is big enough that the +// 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 findings at every thread count, concurrent callers included. +// 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: @@ -28,7 +26,6 @@ #include #include #include -#include #include #include @@ -42,6 +39,19 @@ 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); @@ -55,218 +65,73 @@ GTEST_TEST(ConcurrencyTest, CorpusIsBalanced) { EXPECT_GE(violating_count, kMinViolatingCases); } -GTEST_TEST(ConcurrencyTest, VerdictAndEarliestWitnessAreThreadCountInvariant) { - for (const SearchMode mode : - {SearchMode::kCertifyAll, SearchMode::kFindFirstViolation}) { - for (const auto& entry : Corpus()) { - const BezierCurve trajectory = entry->trajectory(); - const CertificationResult serial = entry->checker->CheckTrajectory( - trajectory, BaseOptions(Parallelism::None(), mode)); - for (const int threads : {2, 8, 16}) { - SCOPED_TRACE(entry->name + ", mode " + - (mode == SearchMode::kCertifyAll ? "kCertifyAll" - : "kFindFirstViolation") + - ", threads " + std::to_string(threads)); - const CertificationResult parallel = entry->checker->CheckTrajectory( - trajectory, BaseOptions(Parallelism(threads), mode)); - EXPECT_EQ(serial.verdict, parallel.verdict); - EXPECT_TRUE(EarliestWitnessIdentical(serial, parallel)); - } - } - } -} - -GTEST_TEST(ConcurrencyTest, CertifyAllIsFullyThreadCountInvariant) { - // In kCertifyAll every node's decision depends only on its own control - // points and inherited active set, so the *whole* tree, and therefore every - // statistic and every finding, is thread-count independent. +GTEST_TEST(ConcurrencyTest, ThreadCountInvariantAndSeriallyDeterministic) { for (const auto& entry : Corpus()) { const BezierCurve trajectory = entry->trajectory(); - const CertificationResult serial = entry->checker->CheckTrajectory( - trajectory, BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); + 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(entry->name + ", threads " + std::to_string(threads)); - const CertificationResult parallel = entry->checker->CheckTrajectory( - trajectory, - BaseOptions(Parallelism(threads), SearchMode::kCertifyAll)); - EXPECT_EQ(serial.stats.nodes, parallel.stats.nodes); - EXPECT_EQ(serial.stats.narrowphase_queries, - parallel.stats.narrowphase_queries); - EXPECT_EQ(serial.stats.sphere_certifications, - parallel.stats.sphere_certifications); - EXPECT_EQ(serial.stats.max_depth, parallel.stats.max_depth); - EXPECT_TRUE(FindingsIdentical(serial.findings, parallel.findings)); - } - } -} - -GTEST_TEST(ConcurrencyTest, FindFirstViolationStatisticsAreAllowedToDiffer) { - // The complement of the test above, pinned so that a future reader does not - // "fix" an expected statistics mismatch: under branch-and-bound the number of - // nodes a run visits depends on when the atomic bound tightens, which depends - // on timing. Only the *witness* is deterministic. - int cases_with_differing_stats = 0; - int examined = 0; - for (const auto& entry : Corpus()) { - if (entry->serial_verdict != Verdict::kViolationFound) continue; - ++examined; - const BezierCurve trajectory = entry->trajectory(); - const CertificationResult serial = entry->checker->CheckTrajectory( - trajectory, - BaseOptions(Parallelism::None(), SearchMode::kFindFirstViolation)); - const CertificationResult parallel = entry->checker->CheckTrajectory( - trajectory, - BaseOptions(Parallelism(16), SearchMode::kFindFirstViolation)); - ASSERT_EQ(serial.verdict, parallel.verdict); - ASSERT_EQ(serial.findings.size(), 1u); - ASSERT_EQ(parallel.findings.size(), 1u); - EXPECT_TRUE(EarliestWitnessIdentical(serial, parallel)); - if (serial.stats.nodes != parallel.stats.nodes) { - ++cases_with_differing_stats; - } - } - // Without this the `continue` above could silently empty the test. - EXPECT_GE(examined, kMinViolatingCases); - std::cout << "\n[ concurrency ] kFindFirstViolation: node counts differed " - "between 1 and 16 threads on " - << cases_with_differing_stats << " of the " << examined - << " violating cases; the reported witness was identical on all of " - "them.\n\n"; -} - -GTEST_TEST(ConcurrencyTest, SerialModeIsBitDeterministic) { - for (const auto& entry : Corpus()) { - for (const SearchMode mode : - {SearchMode::kCertifyAll, SearchMode::kFindFirstViolation}) { - SCOPED_TRACE(entry->name); - const Options options = BaseOptions(Parallelism::None(), mode); - const BezierCurve trajectory = entry->trajectory(); - const CertificationResult first = - entry->checker->CheckTrajectory(trajectory, options); - const CertificationResult second = - entry->checker->CheckTrajectory(trajectory, options); - EXPECT_EQ(first.verdict, second.verdict); - EXPECT_TRUE(FindingsIdentical(first.findings, second.findings)); - EXPECT_EQ(first.stats.nodes, second.stats.nodes); - EXPECT_EQ(first.stats.narrowphase_queries, - second.stats.narrowphase_queries); - EXPECT_EQ(first.stats.sphere_certifications, - second.stats.sphere_certifications); - EXPECT_EQ(first.stats.max_depth, second.stats.max_depth); + 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, 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. + // 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), SearchMode::kCertifyAll); + const Options options = BaseOptions(Parallelism(2)); - std::vector sequential; + std::vector trajectory_expected; + std::vector edge_expected; + std::vector path_expected; + std::vector waypoints; for (const auto& entry : corpus) { - sequential.push_back( + 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; - constexpr int kRepeats = 3; - std::vector> concurrent(kThreads * kRepeats); - std::vector threads; - for (int t = 0; t < kThreads; ++t) { - threads.emplace_back([&, t]() { - for (int r = 0; r < kRepeats; ++r) { - std::vector& slot = concurrent[t * kRepeats + r]; - for (const auto& entry : corpus) { - slot.push_back( - entry->checker->CheckTrajectory(entry->trajectory(), options)); - } - } - }); - } - for (std::thread& thread : threads) thread.join(); - - for (int i = 0; i < kThreads * kRepeats; ++i) { - ASSERT_EQ(concurrent[i].size(), sequential.size()); - for (std::size_t k = 0; k < sequential.size(); ++k) { - SCOPED_TRACE("worker " + std::to_string(i) + ", case " + corpus[k]->name); - EXPECT_EQ(concurrent[i][k].verdict, sequential[k].verdict); - EXPECT_TRUE( - FindingsIdentical(concurrent[i][k].findings, sequential[k].findings)); - EXPECT_EQ(concurrent[i][k].stats.nodes, sequential[k].stats.nodes); - EXPECT_EQ(concurrent[i][k].stats.narrowphase_queries, - sequential[k].stats.narrowphase_queries); - } - } -} - -GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { - // The same, through the other two public entry points and the const - // introspection seams, so that a mutable-state regression in any of them - // shows up here rather than in a user's planner. - const Case& entry = *Corpus().front(); - const Options options = BaseOptions(Parallelism(2), SearchMode::kCertifyAll); - const int n = entry.model->plant().num_positions(); - const VectorXd q1 = entry.control_points.col(0); - const VectorXd q2 = entry.control_points.rightCols(1); - Eigen::MatrixXd waypoints(n, 3); - waypoints.col(0) = q1; - waypoints.col(1) = 0.5 * (q1 + q2); - waypoints.col(2) = q2; - - const CertificationResult edge_expected = - entry.checker->CheckEdge(q1, q2, options); - const CertificationResult path_expected = - entry.checker->CheckPath(waypoints, options); - const MotionBoundTable table_expected = entry.checker->ComputeMotionBounds( - entry.checker->Normalize(entry.trajectory(), options)); - // Snapshot every lambda entry, not just the CSR's size: the row layout is - // fixed by topology and would survive any amount of coefficient corruption. - std::vector>> lambda_expected; - std::vector slack_expected; - for (int p = 0; p < table_expected.num_pairs(); ++p) { - lambda_expected.push_back(table_expected.GetEntries(p)); - slack_expected.push_back(table_expected.carveout_slack(p)); - } - - const auto same_result = [](const CertificationResult& a, - const CertificationResult& b) { - return a.verdict == b.verdict && a.stats.nodes == b.stats.nodes && - a.stats.narrowphase_queries == b.stats.narrowphase_queries && - a.stats.sphere_certifications == b.stats.sphere_certifications && - FindingsIdentical(a.findings, b.findings); - }; - 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 does the asserting - // after the join. + // 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 < 4; ++r) { - if (!same_result(entry.checker->CheckEdge(q1, q2, options), - edge_expected)) { - ++mismatches[t]; - } - if (!same_result(entry.checker->CheckPath(waypoints, options), - path_expected)) { - ++mismatches[t]; - } - const MotionBoundTable table = entry.checker->ComputeMotionBounds( - entry.checker->Normalize(entry.trajectory(), options)); - if (table.num_pairs() != table_expected.num_pairs()) { - ++mismatches[t]; - continue; - } - for (int p = 0; p < table.num_pairs(); ++p) { - if (table.GetEntries(p) != lambda_expected[p]) ++mismatches[t]; - // The carve-out residual is part of Delta_p, so it has to be - // bit-identical across threads too. - if (table.carveout_slack(p) != slack_expected[p]) ++mismatches[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]; + } } } }); @@ -276,52 +141,39 @@ GTEST_TEST(ConcurrencyTest, ConcurrentMixedApiCallsAreIndependent) { } // The sharing path only ever runs on a workload big enough to hire a helper, -// which the corpus cases above never are. The three tests below are where it -// gets its coverage, TSan's included. +// which the corpus cases above never are. The two tests below are where it gets +// its coverage, TSan's included. -GTEST_TEST(ConcurrencyTest, DeepWorkloadIsBigEnoughToBeWorthSpreading) { - // Without this the two tests below could silently degenerate into measuring a - // handful of nodes if the corpus or the bisection ever drifted. +GTEST_TEST(ConcurrencyTest, DeepWorkloadIsThreadCountInvariant) { const DeepWorkload& deep = Deep(); ASSERT_NE(deep.entry, nullptr); - EXPECT_GE(deep.nodes, kMinDeepNodes) << "grazing margin " << deep.margin; - EXPECT_GE(deep.max_depth, kMinDeepDepth) << "grazing margin " << deep.margin; + // 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.nodes - << " nodes, depth " << deep.max_depth << ", at min_interval " - << deep.min_interval << "\n\n"; -} + << ", margin " << deep.margin << ", " << deep.num_nodes + << " nodes\n\n"; -GTEST_TEST(ConcurrencyTest, DeepWorkloadIsThreadCountInvariant) { - const DeepWorkload& deep = Deep(); - ASSERT_NE(deep.entry, nullptr); const BezierCurve trajectory = deep.entry->trajectory(); - const CertificationResult serial = deep.entry->checker->CheckTrajectory( + 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)); - const CertificationResult parallel = deep.entry->checker->CheckTrajectory( - trajectory, deep.options(Parallelism(threads))); - EXPECT_EQ(serial.verdict, parallel.verdict); - EXPECT_EQ(serial.stats.nodes, parallel.stats.nodes); - EXPECT_EQ(serial.stats.narrowphase_queries, - parallel.stats.narrowphase_queries); - EXPECT_EQ(serial.stats.sphere_certifications, - parallel.stats.sphere_certifications); - EXPECT_EQ(serial.stats.max_depth, parallel.stats.max_depth); - EXPECT_TRUE(FindingsIdentical(serial.findings, parallel.findings)); + 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 pool as well as its - // context pool, and the case where a reservation returning fewer threads than - // asked for is the normal outcome rather than an edge case. + // 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 CertificationResult expected = deep.entry->checker->CheckTrajectory( + const Result expected = deep.entry->checker->CheckTrajectory( deep.entry->trajectory(), deep.options(Parallelism::None())); constexpr int kThreads = 4; @@ -330,15 +182,10 @@ GTEST_TEST(ConcurrencyTest, DeepWorkloadSurvivesConcurrentParallelCalls) { for (int t = 0; t < kThreads; ++t) { threads.emplace_back([&, t]() { for (int r = 0; r < 2; ++r) { - const CertificationResult result = deep.entry->checker->CheckTrajectory( - deep.entry->trajectory(), deep.options(Parallelism(4))); - if (result.verdict != expected.verdict || - result.stats.nodes != expected.stats.nodes || - result.stats.narrowphase_queries != - expected.stats.narrowphase_queries || - result.stats.sphere_certifications != - expected.stats.sphere_certifications || - !FindingsIdentical(result.findings, expected.findings)) { + if (!SameResult( + deep.entry->checker->CheckTrajectory( + deep.entry->trajectory(), deep.options(Parallelism(4))), + expected)) { ++mismatches[t]; } } diff --git a/planning/continuous_collision/test/distance_oracle_test.cc b/planning/continuous_collision/test/distance_oracle_test.cc index 9de118253609..7fd9f0caa3b6 100644 --- a/planning/continuous_collision/test/distance_oracle_test.cc +++ b/planning/continuous_collision/test/distance_oracle_test.cc @@ -1,8 +1,7 @@ // Distance oracle accuracy, capability-probe classification, the analytic -// halfspace fallback, Mesh-as-convex-hull semantics, and the V-polytope -// ingestion round trip. Every world is built programmatically with -// RobotDiagramBuilder and every randomized case uses a fixed seed, so the suite -// is deterministic. +// 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" @@ -26,7 +25,6 @@ #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/optimization/vpolytope.h" #include "drake/geometry/proximity_properties.h" #include "drake/geometry/query_object.h" #include "drake/geometry/shape_specification.h" @@ -37,13 +35,13 @@ #include "drake/multibody/plant/deformable_model.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/spatial_inertia.h" -#include "drake/planning/continuous_collision/vpolytope_ingestion.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; @@ -58,7 +56,6 @@ using drake::geometry::Mesh; using drake::geometry::QueryObject; using drake::geometry::Shape; using drake::geometry::Sphere; -using drake::geometry::optimization::VPolytope; using drake::math::RigidTransformd; using drake::math::RotationMatrixd; using drake::multibody::BodyIndex; @@ -73,7 +70,6 @@ using Eigen::Matrix3Xd; using Eigen::Vector3d; using ::testing::HasSubstr; -constexpr double kTau = 1e-6; // Exactness bar for the analytic halfspace fallback and for round trips that // must land on identical code paths. constexpr double kExact = 1e-12; @@ -168,7 +164,7 @@ GeometryId GeometryOf(const MultibodyPlant& plant, const PairRecord& FindPair(const DistanceOracle& oracle, GeometryId a, GeometryId b) { for (const PairRecord& p : oracle.pairs()) { - if ((p.id.a == a && p.id.b == b) || (p.id.a == b && p.id.b == a)) { + if ((p.a == a && p.b == b) || (p.a == b && p.b == a)) { return p; } } @@ -363,7 +359,7 @@ GTEST_TEST(DistanceOracleAccuracy, SphereSphereMatchesAnalyticDistance) { const auto& body_b = AddShapeBody(&plant, "sphere_b", Sphere(r_b), Vector3d(1, 0, 0)); World world(builder.Build()); - const DistanceOracle oracle(world.diagram(), kTau); + const DistanceOracle oracle(world.diagram()); std::mt19937 rng(20260826); // Exact for two spheres on both branches. @@ -386,7 +382,7 @@ GTEST_TEST(DistanceOracleAccuracy, SphereBoxMatchesAnalyticDistance) { 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(), kTau); + const DistanceOracle oracle(world.diagram()); std::mt19937 rng(881); // For a sphere whose center lies outside a convex body, the signed distance @@ -431,7 +427,7 @@ class HalfSpaceFallbackTest : public ::testing::Test { AddShapeBody(&plant, "tetra", Convex(TetraVertices(), "tetra"), Vector3d(0, -3, 0)); world_ = std::make_unique(builder.Build()); - oracle_ = std::make_unique(world_->diagram(), kTau); + oracle_ = std::make_unique(world_->diagram()); halfspace_id_ = GeometryOf(world_->plant(), "halfspace"); } @@ -518,7 +514,7 @@ TEST_F(HalfSpaceFallbackTest, EveryPartnerMatchesHandDerivedFormulaExactly) { << partners[i].first << ", trial " << trial; // Witnesses: separated by exactly |phi|, with the halfspace-side witness // on the boundary plane. - const Vector3d& on_plane = (pair.id.a == halfspace_id_) ? p_a_W : p_b_W; + 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; @@ -543,19 +539,6 @@ TEST_F(HalfSpaceFallbackTest, AxisParallelCylinderDirectionIsHandled) { EXPECT_NEAR((p_a_W - p_b_W).norm(), std::abs(phi), kExact); } -TEST_F(HalfSpaceFallbackTest, ReportNamesEveryCombinationAndRoute) { - const std::string report = oracle_->support_report(); - SCOPED_TRACE(report); - // Rows are ordered by shape class, so HalfSpace is always the second name. - for (const char* row : - {"Sphere-HalfSpace", "Box-HalfSpace", "Capsule-HalfSpace", - "Cylinder-HalfSpace", "Ellipsoid-HalfSpace", "Convex-HalfSpace", - "halfspace analytic support-function fallback", - "native (ComputeSignedDistancePairClosestPoints"}) { - EXPECT_THAT(report, HasSubstr(row)); - } -} - // ========================================================================== // Capability probe: classification snapshot and refusals. // ========================================================================== @@ -580,7 +563,7 @@ class AllShapesTest : public ::testing::Test { plant.world_body(), RigidTransformd::Identity(), HalfSpace(), "ground_geometry", Friction()); world_ = std::make_unique(builder.Build()); - oracle_ = std::make_unique(world_->diagram(), kTau); + oracle_ = std::make_unique(world_->diagram()); } static constexpr int kDynamicBodies = 7; @@ -599,10 +582,10 @@ TEST_F(AllShapesTest, ProbeClassifiesEveryPairSnapshot) { int halfspace_pairs = 0; int native_pairs = 0; for (const PairRecord& pair : oracle_->pairs()) { - if (pair.id.a == halfspace_id_) { + if (pair.a == halfspace_id_) { EXPECT_EQ(pair.route, DistanceRoute::kHalfSpaceA); ++halfspace_pairs; - } else if (pair.id.b == halfspace_id_) { + } else if (pair.b == halfspace_id_) { EXPECT_EQ(pair.route, DistanceRoute::kHalfSpaceB); ++halfspace_pairs; } else { @@ -610,22 +593,12 @@ TEST_F(AllShapesTest, ProbeClassifiesEveryPairSnapshot) { ++native_pairs; } // Every record carries the bodies its geometries hang from. - EXPECT_NE(pair.id.body_a, pair.id.body_b); - EXPECT_EQ(pair.threshold, 0.0) << "the facade owns thresholds"; + EXPECT_NE(pair.body_a, pair.body_b); } EXPECT_EQ(halfspace_pairs, kDynamicBodies); EXPECT_EQ(native_pairs, kNativePairs); } -TEST_F(AllShapesTest, ReportAnnouncesMeshAsConvexHull) { - const std::string report = oracle_->support_report(); - SCOPED_TRACE(report); - EXPECT_THAT(report, - HasSubstr("Mesh mesh_geometry: certified as its convex hull")); - // 8 distinct classes, each present once: 8*7/2 = 28 combinations. - EXPECT_THAT(report, HasSubstr("28 distinct shape-type combination(s)")); -} - TEST_F(AllShapesTest, WitnessPointsAreConsistentForSeparatedNativePairs) { std::mt19937 rng(31337); int checked = 0; @@ -658,8 +631,8 @@ TEST_F(AllShapesTest, IdOrderingIsSymmetric) { int halfspace = 0; for (const PairRecord& pair : oracle_->pairs()) { PairRecord swapped = pair; - std::swap(swapped.id.a, swapped.id.b); - std::swap(swapped.id.body_a, swapped.id.body_b); + std::swap(swapped.a, swapped.b); + std::swap(swapped.body_a, swapped.body_b); if (pair.route == DistanceRoute::kNative) { ++native; } else { @@ -696,7 +669,7 @@ GTEST_TEST(DistanceOracleProbe, HalfSpaceHalfSpacePairThrowsAtConstruction) { // Both geometries must be named, in whichever order the candidate set has // them. DRAKE_EXPECT_THROWS_MESSAGE( - DistanceOracle(world.diagram(), kTau), + DistanceOracle(world.diagram()), "[\\s\\S]*two HalfSpace geometries[\\s\\S]*" "(ground_geometry[\\s\\S]*ceiling_geometry" "|ceiling_geometry[\\s\\S]*ground_geometry)[\\s\\S]*"); @@ -709,14 +682,12 @@ GTEST_TEST(DistanceOracleProbe, ProbeSnapshotIsStableAcrossConstructions) { AddShapeBody(&plant, "box", Box(0.2, 0.2, 0.2), Vector3d(1, 0, 0)); World world(builder.Build()); - const DistanceOracle first(world.diagram(), kTau); - const DistanceOracle second(world.diagram(), kTau); - EXPECT_EQ(first.support_report(), second.support_report()); - EXPECT_EQ(first.tolerance(), kTau); + 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].id.a, second.pairs()[i].id.a); - EXPECT_EQ(first.pairs()[i].id.b, second.pairs()[i].id.b); + EXPECT_EQ(first.pairs()[i].a, second.pairs()[i].a); + EXPECT_EQ(first.pairs()[i].b, second.pairs()[i].b); } } @@ -738,16 +709,15 @@ GTEST_TEST(DistanceOracleProbe, DeformableGeometryIsRefusedByName) { drake::multibody::fem::DeformableBodyConfig{}, 0.05); World world(builder.Build()); - DRAKE_EXPECT_THROWS_MESSAGE(DistanceOracle(world.diagram(), kTau), + 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(), kTau); + const DistanceOracle oracle(world.diagram()); EXPECT_TRUE(oracle.pairs().empty()); - EXPECT_THAT(oracle.support_report(), HasSubstr("0 unfiltered pair(s)")); } // ========================================================================== @@ -767,7 +737,7 @@ GTEST_TEST(DistanceOracleMesh, MeshDistanceEqualsConvexHullDistance) { const auto& probe_body = AddShapeBody(&plant, "probe", Sphere(0.07), Vector3d(0, 2, 0)); World world(builder.Build()); - const DistanceOracle oracle(world.diagram(), kTau); + const DistanceOracle oracle(world.diagram()); const GeometryId probe_id = GeometryOf(world.plant(), "probe"); const PairRecord& mesh_pair = @@ -810,7 +780,7 @@ GTEST_TEST(DistanceOracleMesh, const auto& probe_body = AddShapeBody(&plant, "probe", Sphere(probe_radius), Vector3d(5, 5, 5)); World world(builder.Build()); - const DistanceOracle oracle(world.diagram(), kTau); + const DistanceOracle oracle(world.diagram()); const GeometryId probe_id = GeometryOf(world.plant(), "probe"); const PairRecord& mesh_pair = @@ -851,7 +821,7 @@ GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { plant.world_body(), RigidTransformd::Identity(), HalfSpace(), "ground_geometry", Friction()); World world(builder.Build()); - const DistanceOracle oracle(world.diagram(), kTau); + const DistanceOracle oracle(world.diagram()); const PairRecord& mesh_pair = FindPair(oracle, ground, GeometryOf(world.plant(), "mesh")); @@ -878,161 +848,8 @@ GTEST_TEST(DistanceOracleMesh, HalfSpaceFallbackAgainstMeshUsesTheSameHull) { } } -// ========================================================================== -// V-polytope ingestion round trip. -// ========================================================================== - -// A lopsided polytope. -Matrix3Xd PolytopeVertices() { - Matrix3Xd v(3, 6); - v.col(0) = Vector3d(0.00, 0.00, 0.00); - v.col(1) = Vector3d(0.28, 0.03, 0.01); - v.col(2) = Vector3d(0.05, 0.31, -0.02); - v.col(3) = Vector3d(-0.04, 0.06, 0.24); - v.col(4) = Vector3d(0.22, 0.25, 0.19); - v.col(5) = Vector3d(0.10, -0.18, 0.11); - return v; -} - -// The same polytope with interior points that add nothing to the hull. -Matrix3Xd RedundantPolytopeVertices() { - const Matrix3Xd v = PolytopeVertices(); - Matrix3Xd r(3, v.cols() + 3); - r.leftCols(v.cols()) = v; - r.col(v.cols() + 0) = v.rowwise().mean(); - r.col(v.cols() + 1) = 0.5 * (v.col(0) + v.col(4)); - r.col(v.cols() + 2) = 0.25 * (v.col(1) + v.col(2) + v.col(3) + v.col(5)); - return r; -} - -GTEST_TEST(VPolytopeIngestion, RoundTripMatchesDirectConvexRegistration) { - const Matrix3Xd vertices = PolytopeVertices(); - const VPolytope vpoly(vertices); - const VPolytope redundant_vpoly{RedundantPolytopeVertices()}; - - const RigidTransformd X_WG(RotationMatrixd(Eigen::AngleAxisd( - 0.7, Vector3d(0.3, -0.5, 0.8).normalized())), - Vector3d(0.2, -0.1, 0.35)); - - RobotDiagramBuilder builder(0.0); - MultibodyPlant& plant = builder.plant(); - const GeometryId ingested = - AddVPolytopeObstacle(&plant, vpoly, X_WG, "ingested_vpolytope"); - const GeometryId ingested_redundant = - AddVPolytopeObstacle(&plant, redundant_vpoly, X_WG, "ingested_redundant"); - const GeometryId direct = plant.RegisterCollisionGeometry( - plant.world_body(), X_WG, Convex(vertices, "direct_convex"), - "direct_convex", Friction()); - const auto& probe_body = - AddShapeBody(&plant, "probe", Sphere(0.06), Vector3d(2, 2, 2)); - World world(builder.Build()); - const DistanceOracle oracle(world.diagram(), kTau); - const GeometryId probe_id = GeometryOf(world.plant(), "probe"); - - // Three anchored obstacles against one dynamic probe. Anchored-anchored - // pairs are filtered by SceneGraph, so exactly three pairs survive. - ASSERT_EQ(oracle.pairs().size(), 3u); - - const PairRecord& p_ingested = FindPair(oracle, ingested, probe_id); - const PairRecord& p_redundant = - FindPair(oracle, ingested_redundant, probe_id); - const PairRecord& p_direct = FindPair(oracle, direct, probe_id); - - std::mt19937 rng(2718); - int separated = 0; - int penetrating = 0; - for (int trial = 0; trial < 120; ++trial) { - world.SetPose(probe_body, RandomPose(&rng, 0.5)); - const QueryObject& query = world.query(); - - Vector3d a_i; - Vector3d b_i; - Vector3d a_d; - Vector3d b_d; - const double phi_ingested = - oracle.SignedDistance(query, p_ingested, &a_i, &b_i); - const double phi_direct = - oracle.SignedDistance(query, p_direct, &a_d, &b_d); - EXPECT_NEAR(phi_ingested, phi_direct, kExact) << "trial " << trial; - EXPECT_NEAR(oracle.SignedDistance(query, p_redundant), phi_direct, kExact) - << "trial " << trial; - if (phi_direct > 1e-9) { - ++separated; - EXPECT_LT((a_i - a_d).norm(), kExact) << "trial " << trial; - EXPECT_LT((b_i - b_d).norm(), kExact) << "trial " << trial; - } else { - ++penetrating; - } - } - EXPECT_GT(separated, 0); - EXPECT_GT(penetrating, 0); -} - -GTEST_TEST(VPolytopeIngestion, RoundTripAlsoHoldsOnTheHalfSpaceFallbackRoute) { - const Matrix3Xd vertices = PolytopeVertices(); - RobotDiagramBuilder builder(0.0); - MultibodyPlant& plant = builder.plant(); - // Both copies ride floating bodies so the anchored halfspace can see them. - const VPolytope vpoly(vertices); - const auto& ingested_body = AddShapeBody( - &plant, "ingested", vpoly.ToShapeConvex("ingested"), Vector3d(0, 0, 1)); - const auto& direct_body = AddShapeBody( - &plant, "direct", Convex(vertices, "direct"), Vector3d(0, 2, 1)); - const GeometryId ground = plant.RegisterCollisionGeometry( - plant.world_body(), RigidTransformd::Identity(), HalfSpace(), - "ground_geometry", Friction()); - World world(builder.Build()); - const DistanceOracle oracle(world.diagram(), kTau); - - const PairRecord& ingested_pair = - FindPair(oracle, ground, GeometryOf(world.plant(), "ingested")); - const PairRecord& direct_pair = - FindPair(oracle, ground, GeometryOf(world.plant(), "direct")); - - std::mt19937 rng(1618); - for (int trial = 0; trial < 100; ++trial) { - const RigidTransformd X_W = RandomPose(&rng, 0.3); - world.SetPose(ingested_body, X_W); - world.SetPose(direct_body, X_W); - const QueryObject& query = world.query(); - const double phi_direct = oracle.SignedDistance(query, direct_pair); - EXPECT_NEAR(oracle.SignedDistance(query, ingested_pair), phi_direct, kExact) - << "trial " << trial; - // Reference: the lowest transformed vertex, since the ground is z = 0. - double lowest = std::numeric_limits::infinity(); - for (int i = 0; i < vertices.cols(); ++i) { - lowest = std::min(lowest, (X_W * Vector3d(vertices.col(i))).z()); - } - EXPECT_NEAR(phi_direct, lowest, kExact) << "trial " << trial; - } -} - -GTEST_TEST(VPolytopeIngestion, RejectsBadArguments) { - const VPolytope vpoly(PolytopeVertices()); - EXPECT_THROW( - AddVPolytopeObstacle(nullptr, vpoly, RigidTransformd::Identity(), "x"), - std::exception); - - { // 2-D polytope. - RobotDiagramBuilder builder(0.0); - Eigen::MatrixXd square(2, 4); - square << 0, 1, 1, 0, 0, 0, 1, 1; - const VPolytope flat(square); - EXPECT_THROW(AddVPolytopeObstacle(&builder.plant(), flat, - RigidTransformd::Identity(), "flat"), - std::exception); - } - { // Post-finalize. - RobotDiagramBuilder builder(0.0); - MultibodyPlant& plant = builder.plant(); - plant.Finalize(); - EXPECT_THROW(AddVPolytopeObstacle(&plant, vpoly, - RigidTransformd::Identity(), "late"), - std::exception); - } -} - } // 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 index a01a95e0528d..11bc6624c922 100644 --- a/planning/continuous_collision/test/motion_bound_test.cc +++ b/planning/continuous_collision/test/motion_bound_test.cc @@ -42,6 +42,7 @@ namespace drake { namespace planning { namespace continuous_collision { +namespace internal { namespace { using drake::geometry::GeometryId; @@ -80,12 +81,6 @@ using ::testing::HasSubstr; kinematics and in this test's own accumulation (both ~1e-15 here). */ constexpr double kSlack = 1e-9; -/* Options::continuity_tolerance's default: the width below which the curve - module flags a coordinate constant and the carve-out removes it from every - J(p). A coordinate carved on that *tolerance* can still move by up to this - much, which is what MotionBoundTable::carveout_slack() charges for. */ -constexpr double kContinuityTolerance = 1e-7; - // --------------------------------------------------------------------------- // 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 @@ -254,16 +249,16 @@ std::vector AngularCoordinates(const MultibodyPlant& plant) { return angular; } -std::vector CollisionPairs(const RobotDiagram& diagram) { +std::vector CollisionPairs(const RobotDiagram& diagram) { const MultibodyPlant& plant = diagram.plant(); const auto& inspector = diagram.scene_graph().model_inspector(); - std::vector pairs; + 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(PairId{ga, gb, ba, bb}); + pairs.push_back(PairRecord{ga, gb, ba, bb}); } return pairs; } @@ -271,7 +266,7 @@ std::vector CollisionPairs(const RobotDiagram& diagram) { /* 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 std::vector& pairs, const VectorXd& lower, const VectorXd& upper, const std::vector& constant) { EXPECT_EQ(pairs.size(), 1u); @@ -392,7 +387,7 @@ GTEST_TEST(JointSupportTest, ConstantCoordinateCarveOutEmptiesJp) { Sphere(0.05), "g_tip", Friction()); auto diagram = builder.Build(); const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*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); @@ -493,7 +488,7 @@ GTEST_TEST(HalfSpaceRuleTest, OnlyRotationRelativeToAHalfSpaceIsRefused) { auto ground = MakeHalfSpaceModel(/* halfspace_on_link = */ false, prismatic); const KinematicsEngine engine(*ground); - const std::vector pairs = CollisionPairs(*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), @@ -505,7 +500,7 @@ GTEST_TEST(HalfSpaceRuleTest, OnlyRotationRelativeToAHalfSpaceIsRefused) { auto translating = MakeHalfSpaceModel(/* halfspace_on_link = */ true, true); const KinematicsEngine engine(*translating); - const std::vector pairs = CollisionPairs(*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), @@ -548,7 +543,7 @@ std::unique_ptr> MakeMidChainFloatingModel() { GTEST_TEST(JointSupportTest, MovingQuaternionFloatingJointThrows) { auto diagram = MakeMidChainFloatingModel(); const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*diagram); + const std::vector pairs = CollisionPairs(*diagram); const int nq = diagram->plant().num_positions(); EXPECT_THAT(ThrowMessage([&]() { engine.ComputeMotionBoundTable( @@ -563,7 +558,7 @@ GTEST_TEST(JointSupportTest, ConstantFloatingBaseCarveOutIsSoundMidChain) { auto diagram = MakeMidChainFloatingModel(); const MultibodyPlant& plant = diagram->plant(); const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*diagram); + const std::vector pairs = CollisionPairs(*diagram); const auto& jf = plant.GetJointByName("jf"); const int nq = plant.num_positions(); @@ -703,7 +698,7 @@ struct TightChainProbe { TightChainProbe ProbeTightChain(const TightChain& chain) { const MultibodyPlant& plant = chain.diagram->plant(); const KinematicsEngine engine(*chain.diagram); - const std::vector pairs = CollisionPairs(*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"); @@ -821,7 +816,7 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, const RobotDiagram& diagram = *world.diagram; const MultibodyPlant& plant = diagram.plant(); const KinematicsEngine engine(diagram); - const std::vector pairs = CollisionPairs(diagram); + const std::vector pairs = CollisionPairs(diagram); if (pairs.empty()) return; const int nq = plant.num_positions(); @@ -904,7 +899,7 @@ void CheckWorld(Rng* rng, const RandomWorld& world, CarveOut carve_out, const VectorXd dq = (qp - q).cwiseAbs(); for (int k = 0; k < table.num_pairs(); ++k) { - const PairId& pair = 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(); @@ -1071,12 +1066,12 @@ GTEST_TEST(DisplacementLemmaTest, ScrewChain) { // Part 3. The constant-coordinate carve-out's residual. // // The curve module flags a coordinate constant when its whole control-point -// range fits inside Options::continuity_tolerance. That is a tolerance, not an +// 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 -// Options::certificate_slack. MotionBoundTable::carveout_slack() pays for it. +// kNumericalSlack. MotionBoundTable::carveout_slack() pays for it. // --------------------------------------------------------------------------- GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { @@ -1098,7 +1093,7 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantCoordinateIsChargedAtLambda) { Sphere(0.05), "g_tip", Friction()); auto diagram = builder.Build(); const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*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(); @@ -1191,7 +1186,7 @@ GTEST_TEST(CarveOutSlackTest, HalfSpaceNeedsExactlyConstantRotation) { // 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 std::vector pairs = CollisionPairs(*ball); const int nq = ball->plant().num_positions(); ASSERT_EQ(nq, 3); @@ -1218,7 +1213,7 @@ GTEST_TEST(CarveOutSlackTest, // coordinates have to be exactly constant. auto diagram = MakeCarvedHalfSpaceModel(/* rpy = */ true); const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*diagram); + const std::vector pairs = CollisionPairs(*diagram); const int nq = diagram->plant().num_positions(); ASSERT_EQ(nq, 6); // q = (rpy, p_FM). @@ -1283,7 +1278,7 @@ void RunFloatingBaseCarveOutCorpus(bool quaternion, std::uint64_t seed) { auto diagram = MakeFloatingBaseChain(&rng, quaternion); const MultibodyPlant& plant = diagram->plant(); const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*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(); @@ -1441,7 +1436,7 @@ GTEST_TEST(CarveOutSlackTest, ToleranceConstantQuaternionFloatingBase) { void RunTightFloatingBaseLambda(bool quaternion) { constexpr double kRadius = 0.4; - constexpr double kWidth = 8e-8; // ≤ Options::continuity_tolerance. + constexpr double kWidth = 8e-8; // ≤ kContinuityTolerance. Rng rng(quaternion ? 0x7168A7ull : 0x51DE12ull); RobotDiagramBuilder builder; @@ -1463,7 +1458,7 @@ void RunTightFloatingBaseLambda(bool quaternion) { const MultibodyPlant& plant = diagram->plant(); const KinematicsEngine engine(*diagram); - const std::vector pairs = CollisionPairs(*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(); @@ -1533,6 +1528,7 @@ GTEST_TEST(CarveOutSlackTest, QuaternionFloatingLambdaTildeIsExactAndTight) { } } // 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 index e7e84336ba3d..e34f538fedb6 100644 --- a/planning/continuous_collision/test/piecewise_bezier_path_test.cc +++ b/planning/continuous_collision/test/piecewise_bezier_path_test.cc @@ -28,10 +28,12 @@ against themselves. */ #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; @@ -142,7 +144,7 @@ GTEST_TEST(BezierEvaluation, MatchesDrakeBezierCurve) { const BezierCurve curve(t_start, t_end, control_points); const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(curve, Options{}); + PiecewiseBezierPath::FromTrajectory(curve, {}); ASSERT_EQ(path.num_positions(), num_positions); ASSERT_EQ(path.segments().size(), 1u); EXPECT_EQ(path.start_time(), t_start); @@ -336,7 +338,7 @@ BsplineTrajectory MakeBsplineFromBasis( void CheckBsplineEquivalence(const BsplineTrajectory& bspline, int expected_segments = 0) { const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(bspline, Options{}); + 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); @@ -423,8 +425,7 @@ 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), - Options{}); + 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) { @@ -438,9 +439,8 @@ GTEST_TEST(BsplineConversion, MatrixValuedThrows) { const BsplineTrajectory bspline( BsplineBasis(3, 6, KnotVectorType::kClampedUniform, 0.0, 1.0), control_points); - DRAKE_EXPECT_THROWS_MESSAGE( - PiecewiseBezierPath::FromTrajectory(bspline, Options{}), - "[\\s\\S]*column-vector-valued[\\s\\S]*"); + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(bspline, {}), + "[\\s\\S]*column-vector-valued[\\s\\S]*"); } // -------------------------------------------------------------------------- @@ -454,8 +454,7 @@ GTEST_TEST(PiecewisePolynomialConversion, FirstOrderHold) { const PiecewisePolynomial pp = PiecewisePolynomial::FirstOrderHold(times, samples); - const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(pp, Options{}); + 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 @@ -482,7 +481,7 @@ GTEST_TEST(PiecewisePolynomialConversion, CubicSplines) { PiecewisePolynomial::CubicWithContinuousSecondDerivatives( times, samples); const PiecewiseBezierPath path_a = - PiecewiseBezierPath::FromTrajectory(continuous_second, Options{}); + 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); @@ -492,16 +491,15 @@ GTEST_TEST(PiecewisePolynomialConversion, CubicSplines) { const PiecewisePolynomial shape_preserving = PiecewisePolynomial::CubicShapePreserving(times, samples); const PiecewiseBezierPath path_b = - PiecewiseBezierPath::FromTrajectory(shape_preserving, Options{}); + 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 default cap -and one past it. */ +/* A single high-degree polynomial segment, from degree 1 up to the cap and one +past it. */ GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCapAndBeyond) { - const Options options; - ASSERT_EQ(options.max_conversion_degree, 10); - for (int degree = 1; degree <= options.max_conversion_degree + 1; ++degree) { + 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); @@ -516,16 +514,13 @@ GTEST_TEST(PiecewisePolynomialConversion, LagrangeUpToDegreeCapAndBeyond) { const PiecewisePolynomial pp = PiecewisePolynomial::LagrangeInterpolatingPolynomial(times, samples); - Options relaxed = options; - if (degree > options.max_conversion_degree) { - DRAKE_EXPECT_THROWS_MESSAGE( - PiecewiseBezierPath::FromTrajectory(pp, options), - "[\\s\\S]*max_conversion_degree[\\s\\S]*"); - // Raising the cap is the documented escape hatch. - relaxed.max_conversion_degree = degree; + if (degree > kMaxConversionDegree) { + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(pp, {}), + "[\\s\\S]*polynomial degree 11[\\s\\S]*"); + continue; } const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(pp, relaxed); + 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); @@ -539,9 +534,8 @@ GTEST_TEST(PiecewisePolynomialConversion, MatrixValuedThrows) { const std::vector times{0.0, 1.0}; const PiecewisePolynomial pp = PiecewisePolynomial::FirstOrderHold(times, samples); - DRAKE_EXPECT_THROWS_MESSAGE( - PiecewiseBezierPath::FromTrajectory(pp, Options{}), - "[\\s\\S]*column-vector-valued[\\s\\S]*"); + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(pp, {}), + "[\\s\\S]*column-vector-valued[\\s\\S]*"); } // -------------------------------------------------------------------------- @@ -568,14 +562,14 @@ GTEST_TEST(JunctionValidation, InjectedDiscontinuityThrows) { offset[1] = 1e-3; DRAKE_EXPECT_THROWS_MESSAGE( PiecewiseBezierPath::FromTrajectory(MakeJunctionCase(offset, &generator), - Options{}), + {}), "[\\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), Options{})); + MakeJunctionCase(tiny, &generator), {})); } GTEST_TEST(JunctionValidation, TwoPiOffsetAcceptedOnlyWhenDeclaredRevolute) { @@ -586,18 +580,15 @@ GTEST_TEST(JunctionValidation, TwoPiOffsetAcceptedOnlyWhenDeclaredRevolute) { MakeJunctionCase(offset, &generator); DRAKE_EXPECT_THROWS_MESSAGE( - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}), + PiecewiseBezierPath::FromTrajectory(trajectory, {}), "[\\s\\S]*C0 discontinuity[\\s\\S]*"); // Declaring the *wrong* coordinate does not help. - Options wrong; - wrong.continuous_revolute_indices = {0, 2}; DRAKE_EXPECT_THROWS_MESSAGE( - PiecewiseBezierPath::FromTrajectory(trajectory, wrong), + PiecewiseBezierPath::FromTrajectory(trajectory, {0, 2}), "[\\s\\S]*C0 discontinuity[\\s\\S]*"); - Options right; - right.continuous_revolute_indices = {1}; + const std::vector right{1}; EXPECT_NO_THROW(PiecewiseBezierPath::FromTrajectory(trajectory, right)); // Any integer multiple of 2π is fine ... @@ -623,10 +614,8 @@ GTEST_TEST(JunctionValidation, offset[0] = kTwoPi; const CompositeTrajectory trajectory = MakeJunctionCase(offset, &generator); - Options options; - options.continuous_revolute_indices = {0}; const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, options); + PiecewiseBezierPath::FromTrajectory(trajectory, {0}); ASSERT_EQ(path.segments().size(), 2u); const Eigen::MatrixXd& first = path.segments()[0].control_points; @@ -645,12 +634,11 @@ GTEST_TEST(JunctionValidation, } GTEST_TEST(JunctionValidation, OutOfRangeRevoluteIndexThrows) { - Eigen::MatrixXd waypoints(2, 3); - waypoints << 0.0, 1.0, 2.0, 0.0, 0.0, 0.0; - Options options; - options.continuous_revolute_indices = {2}; + 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::FromWaypoints(waypoints, options), + PiecewiseBezierPath::FromTrajectory( + BezierCurve(0.0, 1.0, control_points), {2}), "[\\s\\S]*continuous_revolute_indices[\\s\\S]*"); } @@ -662,8 +650,7 @@ GTEST_TEST(JunctionValidation, ZeroOrderHoldIsRejected) { 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), - Options{}), + PiecewisePolynomial::ZeroOrderHold(times, samples), {}), "[\\s\\S]*C0 discontinuity[\\s\\S]*"); } @@ -688,7 +675,7 @@ GTEST_TEST(Metadata, GlobalControlBox) { const CompositeTrajectory trajectory = MakeComposite(std::move(pieces)); const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + PiecewiseBezierPath::FromTrajectory(trajectory, {}); Eigen::VectorXd expected_lower = Eigen::VectorXd::Constant(3, std::numeric_limits::infinity()); @@ -721,20 +708,12 @@ GTEST_TEST(Metadata, ConstantCoordinateFlags) { waypoints.row(3) << 0.0, 0.0, 2e-7, 0.0; const PiecewiseBezierPath path = - PiecewiseBezierPath::FromWaypoints(waypoints, Options{}); + 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]); - - // A looser tolerance sweeps coordinate 3 in as well. - Options loose; - loose.continuity_tolerance = 1e-5; - const PiecewiseBezierPath loose_path = - PiecewiseBezierPath::FromWaypoints(waypoints, loose); - EXPECT_TRUE(loose_path.constant_coordinates()[3]); - EXPECT_FALSE(loose_path.constant_coordinates()[0]); } // -------------------------------------------------------------------------- @@ -759,7 +738,7 @@ GTEST_TEST(Composite, BezierSegmentsRoundTrip) { MakeComposite(std::move(pieces)); const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + 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); @@ -795,7 +774,7 @@ GTEST_TEST(Composite, NestedCompositeRecursion) { MakeComposite(std::move(outer_pieces)); const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + 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); @@ -833,7 +812,7 @@ GTEST_TEST(Composite, MixedSegmentTypes) { MakeComposite(std::move(pieces)); const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + 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); @@ -853,14 +832,13 @@ GTEST_TEST(Composite, UnknownSegmentTypeThrowsWithIndexAndTypeName) { MakeComposite(std::move(pieces)); DRAKE_EXPECT_THROWS_MESSAGE( - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}), + 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, Options{}), - "[\\s\\S]*segment index 0[\\s\\S]*"); + DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromTrajectory(bare, {}), + "[\\s\\S]*segment index 0[\\s\\S]*"); } // -------------------------------------------------------------------------- @@ -874,7 +852,7 @@ GTEST_TEST(Waypoints, OrderOneSegmentsAreExact) { const Eigen::MatrixXd waypoints = RandomMatrix(num_positions, num_waypoints, &generator); const PiecewiseBezierPath path = - PiecewiseBezierPath::FromWaypoints(waypoints, Options{}); + PiecewiseBezierPath::FromWaypoints(waypoints); ASSERT_EQ(path.num_positions(), num_positions); ASSERT_EQ(static_cast(path.segments().size()), num_waypoints - 1); @@ -901,11 +879,11 @@ GTEST_TEST(Waypoints, OrderOneSegmentsAreExact) { } GTEST_TEST(Waypoints, TooFewWaypointsThrows) { - DRAKE_EXPECT_THROWS_MESSAGE(PiecewiseBezierPath::FromWaypoints( - Eigen::MatrixXd::Zero(3, 1), Options{}), - "[\\s\\S]*at least 2 waypoints[\\s\\S]*"); DRAKE_EXPECT_THROWS_MESSAGE( - PiecewiseBezierPath::FromWaypoints(Eigen::MatrixXd(0, 4), Options{}), + 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]*"); } @@ -913,7 +891,7 @@ 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, Options{}); + 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)); @@ -945,7 +923,7 @@ GTEST_TEST(Evaluation, JunctionTimeSelectsTheLaterSegment) { Eigen::MatrixXd waypoints(1, 4); waypoints << 0.0, 1.0, 3.0, 6.0; const PiecewiseBezierPath path = - PiecewiseBezierPath::FromWaypoints(waypoints, Options{}); + 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. @@ -976,7 +954,7 @@ GTEST_TEST(Evaluation, JunctionTimesAreConsistent) { const CompositeTrajectory trajectory = MakeComposite(std::move(pieces)); const PiecewiseBezierPath path = - PiecewiseBezierPath::FromTrajectory(trajectory, Options{}); + PiecewiseBezierPath::FromTrajectory(trajectory, {}); for (int i = 0; i < 4; ++i) { const double junction = 0.75 * i; EXPECT_LT((path.Value(junction) - trajectory.value(junction)) @@ -988,6 +966,7 @@ GTEST_TEST(Evaluation, JunctionTimesAreConsistent) { } } // 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 index 6ce7e9134753..94aba31d2f6a 100644 --- a/planning/continuous_collision/test/soundness_fuzz_test.cc +++ b/planning/continuous_collision/test/soundness_fuzz_test.cc @@ -1,12 +1,10 @@ // End-to-end soundness fuzz: random worlds × random trajectories, cross-checked -// three ways. A sampled configuration whose clearance reaches the threshold +// 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) and its certificate is replayed -// independently; every definite `Finding` is re-evaluated at its witness -// configuration, from a context this run never touched, and must really -// violate; and every non-definite `Finding` claiming to be a resolution-floor -// grazing record must be backed by a clearance within 10·(τ_p + ε) of the -// threshold near the reported time. +// 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. @@ -43,12 +41,16 @@ #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; @@ -96,13 +98,10 @@ constexpr int kNumCases = 200; // 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 kMinDefiniteFindings = kNumCases / 10; // 10% -constexpr int kMinInconclusiveFindings = kNumCases / 40; // 2.5% -constexpr int kMinPerTrajectoryFamily = kNumCases / 10; // 10% each -constexpr int kMaxBudgetExhausted = kNumCases / 20; // 5% +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; @@ -556,12 +555,13 @@ std::optional LocalRadius(const Shape& shape) { // that appear in some pair, their local radii, and each pair's two slots. class DenseScanner { public: - explicit DenseScanner(const ContinuousCollisionChecker& checker) - : checker_(&checker), - root_(checker.model().CreateDefaultContext()), + explicit DenseScanner(const RobotDiagram& model) + : model_(&model), + oracle_(model), + root_(model.CreateDefaultContext()), plant_context_( - &checker.model().plant().GetMyMutableContextFromRoot(root_.get())) { - const auto& inspector = checker.model().scene_graph().model_inspector(); + &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); @@ -571,27 +571,26 @@ class DenseScanner { centre_.push_back(Vector3d::Zero()); return static_cast(geometries_.size()) - 1; }; - for (const PairRecord& pair : checker.pairs()) { - slot_a_.push_back(slot(pair.id.a)); - slot_b_.push_back(slot(pair.id.b)); + 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 Result { + struct Worst { double min_slack{std::numeric_limits::infinity()}; double worst_time{std::numeric_limits::quiet_NaN()}; int worst_pair{-1}; }; - // `threshold` is m_p, which this fuzz keeps uniform across pairs because it - // never sets a PaddingSpec (the case loop asserts that). - Result Scan(const PiecewiseBezierPath& path, int total_samples, - double threshold) { + // `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); - Result result; + Worst result; for (int k = 0; k < num_segments; ++k) { const BezierSegment& segment = path.segments()[k]; for (int i = 0; i <= per_segment; ++i) { @@ -611,16 +610,16 @@ class DenseScanner { 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 = checker_->pairs()[pair_index]; + 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(checker_->distance_oracle().SignedDistance( - query_object(), pair) - - threshold)); + best = std::min( + best, + std::abs(oracle_.SignedDistance(query_object(), pair) - threshold)); } return best; } @@ -632,38 +631,37 @@ class DenseScanner { double DistanceAt(const VectorXd& q, int pair_index) { SetPositions(q); ++narrowphase_queries_; - return checker_->distance_oracle().SignedDistance( - query_object(), checker_->pairs()[pair_index]); + return oracle_.SignedDistance(query_object(), oracle_.pairs()[pair_index]); } - // Index of the checker's pair matching `id`, or -1. - int FindPair(const PairId& id) const { - const auto& pairs = checker_->pairs(); + // 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].id.a == id.a && pairs[p].id.b == id.b) return p; + if (pairs[p].a == a && pairs[p].b == b) return p; } return -1; } private: void SetPositions(const VectorXd& q) { - checker_->model().plant().SetPositions(plant_context_, q); + model_->plant().SetPositions(plant_context_, q); } const QueryObject& query_object() const { - const auto& scene_graph = checker_->model().scene_graph(); + 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, - Result* result) { + 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 = checker_->pairs(); + 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]]; @@ -679,9 +677,7 @@ class DenseScanner { if (lower > threshold) continue; } ++narrowphase_queries_; - const double slack = - checker_->distance_oracle().SignedDistance(query, pairs[p]) - - threshold; + const double slack = oracle_.SignedDistance(query, pairs[p]) - threshold; if (slack < result->min_slack) { result->min_slack = slack; result->worst_time = time; @@ -690,7 +686,8 @@ class DenseScanner { } } - const ContinuousCollisionChecker* checker_{}; + const RobotDiagram* model_{}; + DistanceOracle oracle_; std::unique_ptr> root_; drake::systems::Context* plant_context_{}; std::vector geometries_; @@ -709,10 +706,7 @@ struct Tally { int certified{0}; int violation{0}; int inconclusive{0}; - int budget{0}; int deep_scans{0}; - int definite_findings{0}; - int inconclusive_findings{0}; int graze_cases{0}; int pwl{0}; int bezier{0}; @@ -729,29 +723,17 @@ struct Tally { double tightest_certified_slack{std::numeric_limits::infinity()}; }; -// Base options shared by every case. +// Base options shared by every case. A coarser resolution floor than the 1e-9 +// default: a grazing pair still ends kInconclusive, but after ~20 bisections +// rather than ~30. Options FuzzOptions(double margin) { Options options; options.margin = margin; - options.mode = SearchMode::kCertifyAll; - options.emit_certificate = true; options.parallelism = Parallelism::None(); - // A coarser resolution floor than the 1e-9 default: a grazing pair still ends - // kInconclusive, but after ~20 bisections rather than ~30. The node budget is - // the second guard; a case that hits it is counted, never silently accepted. options.min_interval = 1e-6; - options.max_nodes = 300000; return options; } -ContinuousCollisionChecker MakeChecker( - std::shared_ptr> model, const Options& options) { - ContinuousCollisionChecker::Params params; - params.model = std::move(model); - params.default_options = options; - return ContinuousCollisionChecker(params); -} - GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { Tally tally; for (int case_index = 0; case_index < kNumCases; ++case_index) { @@ -790,12 +772,10 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { double margin = (case_index % 2 == 0) ? 0.0 : 0.01; bool grazing = (case_index % 5) == 3; if (grazing) { - const Options probe_options = FuzzOptions(0.0); - const ContinuousCollisionChecker probe = - MakeChecker(model, probe_options); - DenseScanner probe_scanner(probe); - const DenseScanner::Result probe_scan = probe_scanner.Scan( - probe.Normalize(*trajectory, probe_options), kGrazeProbeSamples, 0.0); + 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; @@ -810,28 +790,22 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { world.Describe() + trajectory_recipe.Describe()); const Options options = FuzzOptions(margin); - const ContinuousCollisionChecker checker = MakeChecker(model, options); - // This fuzz never sets a PaddingSpec, so m_p = margin for every pair; the - // dense scan relies on that to compare against one number. - for (const PairRecord& pair : checker.pairs()) { - ASSERT_EQ(pair.threshold, margin); - } - - const PiecewiseBezierPath path = checker.Normalize(*trajectory, options); - const CertificationResult result = - checker.CheckTrajectory(*trajectory, options); + const ContinuousCollisionChecker checker(model, options); + const PiecewiseBezierPath path = + PiecewiseBezierPath::FromTrajectory(*trajectory, {}); + const Result result = checker.CheckTrajectory(*trajectory, options); - DenseScanner scanner(checker); + DenseScanner scanner(*model); switch (result.verdict) { case Verdict::kCertifiedFree: { ++tally.certified; - ASSERT_TRUE(result.findings.empty()); - // (a) Dense sampling must find no configuration at or below the - // threshold. A single one would be a false certificate. + 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::Result scan = scanner.Scan( + const DenseScanner::Worst scan = scanner.Scan( path, deep ? kDeepDenseSamples : kDenseSamples, margin); tally.tightest_certified_slack = std::min(tally.tightest_certified_slack, scan.min_slack); @@ -841,70 +815,49 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { << " configurations) found clearance " << scan.min_slack << " m below the threshold at t = " << scan.worst_time << " for pair " << scan.worst_pair; - // (b) The audit trail must replay independently. - ASSERT_TRUE(result.certificate.has_value()); - EXPECT_TRUE(VerifyCertificate(checker, path, *result.certificate)) - << "CERTIFIED FREE but the emitted certificate does not verify"; break; } case Verdict::kViolationFound: ++tally.violation; - EXPECT_FALSE(result.findings.empty()); + ASSERT_TRUE(result.finding.has_value()); break; case Verdict::kInconclusive: ++tally.inconclusive; - EXPECT_FALSE(result.findings.empty()); + ASSERT_TRUE(result.finding.has_value()); break; - case Verdict::kBudgetExhausted: - // The node budget is a safety valve, not an expected outcome; a run - // that hits it reports the earliest node it left uncovered, and there - // is nothing to cross-check because nothing was proved. The corpus-wide - // bound on how often this may happen is asserted after the loop. - ++tally.budget; - EXPECT_FALSE(result.findings.empty()) - << "budget exhaustion must report the uncovered remainder"; - break; - } - - // Findings are earliest-first, always. - for (std::size_t i = 1; i < result.findings.size(); ++i) { - EXPECT_LE(result.findings[i - 1].time, result.findings[i].time); } - for (const Finding& finding : result.findings) { - const int pair_index = scanner.FindPair(finding.pair); - ASSERT_GE(pair_index, 0) << "finding names an unknown pair"; - const double threshold = checker.pairs()[pair_index].threshold; + 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 (finding.definite) { - ++tally.definite_findings; + 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 definite witness must be an on-trajectory configuration, " + << "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, threshold + kWorstTau) - << "definite violation at t = " << finding.time + EXPECT_LT(phi, margin + kWorstTau) + << "violation at t = " << finding.time << " re-measures at phi = " << phi << " against threshold " - << threshold; + << margin; EXPECT_NEAR(phi, finding.distance, 1e-9) << "the reported distance is not reproducible at the witness"; - } else if (result.verdict != Verdict::kBudgetExhausted) { - // Every non-definite finding that is *not* a budget remainder is a - // resolution-floor grazing record, and must be backed by a clearance - // that sits within 10·(τ_p + ε) of the threshold somewhere near the - // reported time. Only the synthesized "here is where the budget stopped - // us" finding is exempt, because its clearance carries no claim. - ++tally.inconclusive_findings; - const double tolerance = 10.0 * (kWorstTau + options.certificate_slack); + } else { + // An inconclusive witness is a resolution-floor grazing record, and + // must be backed by a clearance that sits within 10·(τ_p + ε) of the + // threshold somewhere near the reported time. + const double tolerance = 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, threshold, finding.time, + scanner.MinAbsSlackNear(path, pair_index, margin, finding.time, window, /* samples = */ 400); EXPECT_LE(best, tolerance) << "INCONCLUSIVE at t = " << finding.time @@ -919,15 +872,12 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { std::cout << "\n[ fuzz summary ] cases = " << kNumCases << " certified = " << tally.certified << " violation = " << tally.violation - << " inconclusive = " << tally.inconclusive - << " budget = " << tally.budget << "\n" + << " inconclusive = " << tally.inconclusive << "\n" << " trajectories: PWL = " << tally.pwl << ", Bezier = " << tally.bezier << ", B-spline = " << tally.bspline << "; grazing-margin cases = " << tally.graze_cases << "\n" << " deep (1e5-sample) scans = " << tally.deep_scans - << " definite findings = " << tally.definite_findings - << " inconclusive findings = " << tally.inconclusive_findings << "\n cross-check narrowphase queries = " << tally.scan_queries << "; tightest measured clearance above a certified threshold = " @@ -953,18 +903,12 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { "the soundness sweep needs at least 150 (world, trajectory) " "cases per run"); #endif - static_assert(kMinInconclusive >= 1 && kMinInconclusiveFindings >= 1, + 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"; - EXPECT_GE(tally.definite_findings, kMinDefiniteFindings); - EXPECT_GE(tally.inconclusive_findings, kMinInconclusiveFindings); - // The node budget exists to bound a pathological case, not to be the usual - // answer: if it starts firing often, the corpus has stopped cross-checking - // anything and the numbers above stop meaning what they say. - EXPECT_LE(tally.budget, kMaxBudgetExhausted); // All three trajectory families of trajectory normalization must be // represented. EXPECT_GE(tally.pwl, kMinPerTrajectoryFamily); @@ -985,6 +929,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { } } // 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 index bc396918a827..273441974ffc 100644 --- a/planning/continuous_collision/test/test_utilities.h +++ b/planning/continuous_collision/test/test_utilities.h @@ -1,9 +1,9 @@ #pragma once // Helpers shared by this package's tests: seeded random primitives and surface -// samplers, the throw-message probe, the checker factory, 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. +// 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. @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -32,6 +33,7 @@ #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" @@ -192,38 +194,20 @@ std::string ThrowMessage(Callable&& call) { return {}; } -inline ContinuousCollisionChecker::Params CheckerParams( - std::shared_ptr> model, Options options, - PaddingSpec padding = {}) { - ContinuousCollisionChecker::Params params; - params.model = std::move(model); - params.default_options = std::move(options); - params.padding = std::move(padding); - return params; -} - // The checker is neither copyable nor movable, so tests that need to own one // inside a container take the pointer flavor. -inline ContinuousCollisionChecker MakeChecker( - std::shared_ptr> model, Options options, - PaddingSpec padding = {}) { - return ContinuousCollisionChecker( - CheckerParams(std::move(model), std::move(options), std::move(padding))); -} - inline std::unique_ptr MakeCheckerPtr( - std::shared_ptr> model, Options options, - PaddingSpec padding = {}) { - return std::make_unique( - CheckerParams(std::move(model), std::move(options), std::move(padding))); + 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: an independent confirmation that the -// witness is a real contact and not an artifact of the search. -inline double DistanceAtFinding(const ContinuousCollisionChecker& checker, +// 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) { - const RobotDiagram& model = checker.model(); auto root = model.CreateDefaultContext(); auto& plant_context = model.plant().GetMyMutableContextFromRoot(root.get()); model.plant().SetPositions(&plant_context, finding.q); @@ -231,9 +215,10 @@ inline double DistanceAtFinding(const ContinuousCollisionChecker& checker, const auto& query_object = scene_graph.get_query_output_port().Eval>( scene_graph.GetMyContextFromRoot(*root)); - for (const PairRecord& pair : checker.pairs()) { - if (pair.id.a == finding.pair.a && pair.id.b == finding.pair.b) { - return checker.distance_oracle().SignedDistance(query_object, pair); + 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."; @@ -337,9 +322,9 @@ inline std::unique_ptr> MakeRandomWorld( // --------------------------------------------------------------------------- constexpr double kMargin = 0.005; -// Ten cases keeps the full 4-thread-count x 2-mode sweep (80 certification -// runs) plus the concurrent-call test under a second in Release, which is what -// makes this affordable to run again under TSan (~100x slower). +// 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; @@ -357,12 +342,11 @@ inline Eigen::MatrixXd MakeControlPoints(uint64_t seed, int num_positions) { return points; } -inline Options BaseOptions(Parallelism parallelism, SearchMode mode) { +inline Options BaseOptions(Parallelism parallelism) { Options options; options.margin = kMargin; options.parallelism = parallelism; - options.mode = mode; - // Bounded cost per run: the whole sweep is executed 8 times per case. + // Bounded cost per run: the whole sweep is executed several times per case. options.min_interval = 1e-6; return options; } @@ -395,21 +379,17 @@ inline const std::vector>& Corpus() { auto entry = std::make_unique(); entry->name = "seed_" + std::to_string(seed); entry->model = MakeRandomWorld(seed); - ContinuousCollisionChecker::Params params; - params.model = entry->model; - params.default_options = - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll); - entry->checker = std::make_unique(params); + entry->checker = + MakeCheckerPtr(entry->model, BaseOptions(Parallelism::None())); entry->control_points = MakeControlPoints(seed, entry->model->plant().num_positions()); - const CertificationResult result = entry->checker->CheckTrajectory( - entry->trajectory(), - BaseOptions(Parallelism::None(), SearchMode::kCertifyAll)); - entry->serial_verdict = result.verdict; + 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 = result.verdict == Verdict::kCertifiedFree; - const bool is_violating = result.verdict == Verdict::kViolationFound; + 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) { @@ -423,76 +403,35 @@ inline const std::vector>& Corpus() { return *corpus; } -// Bit-for-bit equality of two findings. 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 FindingsIdentical( - const std::vector& a, const std::vector& b) { - if (a.size() != b.size()) { +// 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() - << "finding counts differ: " << a.size() << " vs " << b.size(); + << "one run reported a finding and the other did not"; } - for (std::size_t i = 0; i < a.size(); ++i) { - if (a[i].time != b[i].time) { - return ::testing::AssertionFailure() - << "finding " << i << " time " << a[i].time << " vs " << b[i].time; - } - if (a[i].q.size() != b[i].q.size() || - !(a[i].q.array() == b[i].q.array()).all()) { - return ::testing::AssertionFailure() - << "finding " << i << " witness configuration differs"; - } - if (a[i].pair.a != b[i].pair.a || a[i].pair.b != b[i].pair.b) { - return ::testing::AssertionFailure() - << "finding " << i << " pair differs"; - } - if (a[i].distance != b[i].distance || - a[i].motion_bound != b[i].motion_bound || - a[i].definite != b[i].definite) { - return ::testing::AssertionFailure() - << "finding " << i << " payload differs"; - } - if (a[i].nearest_a_W.has_value() != b[i].nearest_a_W.has_value() || - (a[i].nearest_a_W.has_value() && - *a[i].nearest_a_W != *b[i].nearest_a_W)) { - return ::testing::AssertionFailure() - << "finding " << i << " witness point A differs"; - } - if (a[i].nearest_b_W.has_value() != b[i].nearest_b_W.has_value() || - (a[i].nearest_b_W.has_value() && - *a[i].nearest_b_W != *b[i].nearest_b_W)) { - return ::testing::AssertionFailure() - << "finding " << i << " witness point B differs"; - } - } - return ::testing::AssertionSuccess(); -} - -inline ::testing::AssertionResult EarliestWitnessIdentical( - const CertificationResult& a, const CertificationResult& b) { - if (a.findings.empty() != b.findings.empty()) { + if (!a.has_value()) return ::testing::AssertionSuccess(); + if (a->time != b->time) { return ::testing::AssertionFailure() - << "one run reported findings and the other did not"; + << "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.findings.empty()) return ::testing::AssertionSuccess(); - return FindingsIdentical({a.findings.front()}, {b.findings.front()}); + 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(); } -// The bisection's node budget below doubles as the deep workload's size: the -// margin it converges to is the largest one still certifiable inside this -// budget, so the tree it produces has just under this many nodes. Large enough -// that no fixed seeding depth could ever have covered it; small enough that the -// ~40 probes that find it stay cheap, sanitizers included. 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; -// Floors concurrency_test.cc holds the result to, so the workload cannot -// silently degenerate if the corpus or the bisection drifts. Depth is the load -// bearing one: a deep, narrow spike is the shape a depth-seeded work queue -// cannot split, and it is what the sharing path exists for. -constexpr uint64_t kMinDeepNodes = 1000; -constexpr int kMinDeepDepth = 15; - // 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 @@ -501,17 +440,22 @@ constexpr int kMinDeepDepth = 15; // 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 = 1000; + struct DeepWorkload { const Case* entry{}; double margin{0.0}; - double min_interval{1e-8}; - uint64_t nodes{0}; - int max_depth{0}; + uint64_t num_nodes{0}; Options options(Parallelism parallelism) const { - Options options = BaseOptions(parallelism, SearchMode::kCertifyAll); + Options options = BaseOptions(parallelism); options.margin = margin; - options.min_interval = min_interval; + options.min_interval = 1e-8; return options; } }; @@ -529,10 +473,10 @@ inline const DeepWorkload& Deep() { const auto certifiable_within_budget = [&](double margin) { Options options = deep->options(Parallelism::None()); options.margin = margin; - options.max_nodes = kProbeBudget; - return deep->entry->checker - ->CheckTrajectory(deep->entry->trajectory(), options) - .verdict == Verdict::kCertifiedFree; + 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; @@ -545,13 +489,10 @@ inline const DeepWorkload& Deep() { (certifiable_within_budget(mid) ? certifiable : grazing) = mid; } deep->margin = certifiable; - const Statistics stats = - deep->entry->checker - ->CheckTrajectory(deep->entry->trajectory(), - deep->options(Parallelism::None())) - .stats; - deep->nodes = stats.nodes; - deep->max_depth = stats.max_depth; + deep->num_nodes = deep->entry->checker + ->CheckTrajectory(deep->entry->trajectory(), + deep->options(Parallelism::None())) + .num_nodes; return deep; }(); return *workload; diff --git a/planning/continuous_collision/test/thin_obstacle_test.cc b/planning/continuous_collision/test/thin_obstacle_test.cc index a071c69bfc5b..72acbf3f39bf 100644 --- a/planning/continuous_collision/test/thin_obstacle_test.cc +++ b/planning/continuous_collision/test/thin_obstacle_test.cc @@ -28,12 +28,11 @@ using drake::planning::CollisionCheckerParams; using drake::planning::SceneGraphCollisionChecker; using Eigen::Vector3d; using Eigen::VectorXd; -using test::BezierCurve; using test::Box; using test::DistanceAtFinding; using test::Friction; using test::Inertia; -using test::MakeChecker; +using test::MakeCheckerPtr; using test::MultibodyPlant; using test::Parallelism; using test::PrismaticJoint; @@ -242,15 +241,15 @@ GTEST_TEST(ThinObstacleTest, DrakeSampledCheckerMissesTheThinPlate) { GTEST_TEST(ThinObstacleTest, CertifiedCheckerCatchesTheThinPlate) { const VectorXd q1 = MakeQ(-0.5, 0.0); const VectorXd q2 = MakeQ(0.5, 0.0); - const auto checker = - MakeChecker(MakePlateWorld(kPlateX, kPlateThickness), CertifiedOptions()); + const std::shared_ptr> model = + MakePlateWorld(kPlateX, kPlateThickness); + const auto checker = MakeCheckerPtr(model, CertifiedOptions()); - const CertificationResult result = checker.CheckEdge(q1, q2); + const Result result = checker->CheckEdge(q1, q2); ASSERT_EQ(result.verdict, Verdict::kViolationFound); - ASSERT_FALSE(result.findings.empty()); + ASSERT_TRUE(result.finding.has_value()); - const Finding& finding = result.findings.front(); - EXPECT_TRUE(finding.definite); + const Finding& finding = *result.finding; ASSERT_EQ(finding.q.size(), 2); // The witness lies inside the plate-crossing parameter interval. CheckEdge @@ -267,14 +266,14 @@ GTEST_TEST(ThinObstacleTest, CertifiedCheckerCatchesTheThinPlate) { // ... and a direct distance query at the witness, from a context this run // never touched, confirms the contact. - const double phi = DistanceAtFinding(checker, finding); + 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, + EXPECT_EQ(checker->CheckPath(Waypoints(q1, q2)).verdict, Verdict::kViolationFound); } @@ -290,7 +289,8 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { constexpr double kClearance = kHalfGap - 0.5 * kPlateThickness - kToolRadius; static_assert(kClearance > 0.0); - const auto checker = MakeChecker(MakeSlotWorld(kHalfGap), CertifiedOptions()); + const auto checker = + MakeCheckerPtr(MakeSlotWorld(kHalfGap), CertifiedOptions()); const VectorXd q1 = MakeQ(-0.3, 0.0); const VectorXd q2 = MakeQ(0.3, 0.0); @@ -299,9 +299,9 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { EXPECT_TRUE(MakeDrakeChecker(MakeSlotWorld(kHalfGap), kDrakeEdgeStepSize) .CheckEdgeCollisionFree(q1, q2)); - const CertificationResult result = checker.CheckEdge(q1, q2); + const Result result = checker->CheckEdge(q1, q2); EXPECT_EQ(result.verdict, Verdict::kCertifiedFree); - EXPECT_TRUE(result.findings.empty()); + 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 @@ -311,24 +311,12 @@ GTEST_TEST(ThinObstacleTest, NarrowGapCertifiedWithBoundedNodeBudget) { // 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.stats.nodes, uint64_t{640}) + 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.stats.max_depth, 6) + 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"; - EXPECT_LE(result.stats.max_depth, 12); - - // ... and the certificate for this run replays independently. - Options options = CertifiedOptions(); - options.emit_certificate = true; - const CertificationResult with_certificate = - checker.CheckPath(Waypoints(q1, q2), options); - ASSERT_EQ(with_certificate.verdict, Verdict::kCertifiedFree); - ASSERT_TRUE(with_certificate.certificate.has_value()); - const BezierCurve edge(0.0, 1.0, Waypoints(q1, q2)); - EXPECT_TRUE(VerifyCertificate(checker, checker.Normalize(edge, options), - *with_certificate.certificate)); } // --------------------------------------------------------------------------- @@ -356,8 +344,8 @@ GTEST_TEST(ThinObstacleTest, ThicknessSweepBracketsTheResolutionGap) { .CheckEdgeCollisionFree(q1, q2); if (!drake_free && std::isnan(first_caught)) first_caught = thickness; EXPECT_EQ( - MakeChecker(MakePlateWorld(kPlateX, thickness), CertifiedOptions()) - .CheckEdge(q1, q2) + MakeCheckerPtr(MakePlateWorld(kPlateX, thickness), CertifiedOptions()) + ->CheckEdge(q1, q2) .verdict, Verdict::kViolationFound); } diff --git a/planning/continuous_collision/vpolytope_ingestion.cc b/planning/continuous_collision/vpolytope_ingestion.cc deleted file mode 100644 index 42e266fff1e3..000000000000 --- a/planning/continuous_collision/vpolytope_ingestion.cc +++ /dev/null @@ -1,55 +0,0 @@ -#include "drake/planning/continuous_collision/vpolytope_ingestion.h" - -#include - -#include - -#include "drake/common/drake_throw.h" -#include "drake/geometry/shape_specification.h" -#include "drake/multibody/plant/coulomb_friction.h" - -namespace drake { -namespace planning { -namespace continuous_collision { - -using drake::geometry::GeometryId; -using drake::math::RigidTransformd; -using drake::multibody::CoulombFriction; -using drake::multibody::MultibodyPlant; - -namespace { -// Signed distance never reads friction, but RegisterCollisionGeometry demands -// some proximity properties. Unit friction is Drake's own conventional -// placeholder for "the caller does not care". -constexpr double kDefaultFriction = 1.0; -} // namespace - -GeometryId AddVPolytopeObstacle(MultibodyPlant* plant, - const geometry::optimization::VPolytope& vpoly, - const RigidTransformd& X_WG, - const std::string& name) { - DRAKE_THROW_UNLESS(plant != nullptr); - // A non-3D V-polytope is refused by VPolytope::ToShapeConvex() below, and a - // finalized plant by MultibodyPlant::RegisterCollisionGeometry(); neither - // needs a check here. An empty vertex set reaches the proximity engine - // undetected, so it does. - if (vpoly.vertices().cols() == 0) { - throw std::runtime_error(fmt::format( - "AddVPolytopeObstacle(): obstacle '{}' has an empty vertex set.", - name)); - } - - // Drake's pinned VPolytope -> Convex entry point; it forwards the vertex - // matrix to Convex(Matrix3X, label, scale=1). The hull is computed lazily - // by Convex::GetConvexHull() and is the object the proximity engine - // actually collides. - const drake::geometry::Convex convex = vpoly.ToShapeConvex(name); - - return plant->RegisterCollisionGeometry( - plant->world_body(), X_WG, convex, name, - CoulombFriction(kDefaultFriction, kDefaultFriction)); -} - -} // namespace continuous_collision -} // namespace planning -} // namespace drake diff --git a/planning/continuous_collision/vpolytope_ingestion.h b/planning/continuous_collision/vpolytope_ingestion.h deleted file mode 100644 index 208c42ea2472..000000000000 --- a/planning/continuous_collision/vpolytope_ingestion.h +++ /dev/null @@ -1,48 +0,0 @@ -#pragma once - -#include - -#include "drake/geometry/geometry_ids.h" -#include "drake/geometry/optimization/vpolytope.h" -#include "drake/math/rigid_transform.h" -#include "drake/multibody/plant/multibody_plant.h" - -namespace drake { -namespace planning { -namespace continuous_collision { - -/** Registers a V-polytope as an anchored obstacle with a collision role. - -The polytope is converted to `drake::geometry::Convex` through Drake's own -`VPolytope::ToShapeConvex()` entry point, a thin wrapper over the -`Convex(Eigen::Matrix3X points, std::string label, double scale)` -constructor, then registered on the plant's world body. The -result therefore rides the ordinary native narrowphase path end to end: the -proximity engine and the certifier's radius/support code all read the same -`Convex::GetConvexHull()` object, so the certificate stays sound even for -redundant or degenerate vertex sets. - -@param plant The plant to register on. Must be non-null, must already be a - registered SceneGraph source, and must NOT be finalized. -@param vpoly The polytope. Its vertices are interpreted in the geometry - frame G, i.e. the world-frame obstacle is - `X_WG * conv(vpoly.vertices())`. Must be 3-dimensional with at - least one vertex. -@param X_WG Pose of the geometry frame in the world frame. -@param name Geometry name; also used as the `Convex` shape's label (which - Drake only uses in its own warning/error messages). Must not - contain a newline. -@returns The id of the newly registered collision geometry. -@throws std::exception if `plant` is null or already finalized, if - `vpoly.ambient_dimension() != 3`, if the vertex set is empty, or if - Drake rejects the resulting hull (e.g. a degenerate vertex set that - its hull computation cannot inflate). -@ingroup planning_collision_checker */ -geometry::GeometryId AddVPolytopeObstacle( - multibody::MultibodyPlant* plant, - const geometry::optimization::VPolytope& vpoly, - const math::RigidTransform& X_WG, const std::string& name); - -} // namespace continuous_collision -} // namespace planning -} // namespace drake From a3d5a79688977e5df189e4c6f0609f1b24fd662b Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Mon, 31 Aug 2026 11:28:50 -0400 Subject: [PATCH 20/22] [planning] continuous_collision: fix CI (installed headers, clang warnings, dbg timeout) Installed headers. certifier.h sat in the srcs of :continuous_collision_checker, and drake_cc_library installs private headers found in srcs by default. It therefore landed in libdrake's drake_headers tree while the internal headers it includes (distance_oracle.h and friends) correctly did not, so mkdoc's parse of that tree died on a missing include and every platform failed to build //bindings/generated_docstrings:gen_planning_continuous_collision. It moves to hdrs with install_hdrs_exclude, the same shape multibody/parsing uses for detail_composite_parse.h; the package's only installed header is now continuous_collision_checker.h. The vendored docstrings are regenerated: they had drifted from the header, still carrying U+03C6 where the guarantee now reads U+03D5 and a two-line form for a doc comment that is one line today. Clang warnings. A lambda in motion_bound_test.cc captured a constexpr local it did not need, which -Wunused-lambda-capture rejects under the clang jobs' -Werror=all. Re-running every translation unit in the package, plus the pydrake bindings, through clang with the linux-clang job's exact flag set turned up no others. Dbg timeout. Deep() bisects for the margin whose certifying tree is largest within kProbeBudget nodes, and it used to bound each probe with Options::max_nodes. That option was withdrawn when the public API was minimized, so the budget became a post-hoc check on a run that had already finished, and the probes the search rejects -- roughly half of them, and the expensive half -- began exploring to exhaustion: up to 479k nodes each against the 1.5k the workload itself needs. Options::min_interval is the cost bound that remains, and the workload now simply inherits BaseOptions' 1e-6 rather than overriding it to 1e-8. The certifying side is unaffected (its tree is 19 levels deep, so it never approaches either floor) and the workload is unchanged at margin 0.101269 and 1479 nodes, while the rejected probes fall to about 5e3 nodes. DeepWorkloadIsThreadCountInvariant goes from 280.4 s to 7.1 s in a dbg build. The target also declares timeout = "moderate", since --config=debug and --config=lsan scale the short budget down, to 120 s and 72 s, rather than up. --- .../planning_continuous_collision.h | 5 ++--- planning/continuous_collision/BUILD.bazel | 18 +++++++++++++++--- .../test/motion_bound_test.cc | 2 +- .../continuous_collision/test/test_utilities.h | 17 ++++++++++++++++- 4 files changed, 34 insertions(+), 8 deletions(-) diff --git a/bindings/generated_docstrings/planning_continuous_collision.h b/bindings/generated_docstrings/planning_continuous_collision.h index eefe436edd51..fc3d4356fdcc 100644 --- a/bindings/generated_docstrings/planning_continuous_collision.h +++ b/bindings/generated_docstrings/planning_continuous_collision.h @@ -31,7 +31,7 @@ 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 +(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 @@ -146,8 +146,7 @@ R"""(Where the plan fails, or where it could not be decided.)"""; // 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.)"""; + const char* doc = R"""(Signed distance of the pair at q.)"""; } distance; // Symbol: drake::planning::continuous_collision::Finding::geometry_a struct /* geometry_a */ { diff --git a/planning/continuous_collision/BUILD.bazel b/planning/continuous_collision/BUILD.bazel index 27c8ec11b380..f000a860659f 100644 --- a/planning/continuous_collision/BUILD.bazel +++ b/planning/continuous_collision/BUILD.bazel @@ -105,15 +105,21 @@ drake_cc_library( # 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 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", - "certifier.h", "continuous_collision_checker.cc", ], - hdrs = ["continuous_collision_checker.h"], + hdrs = [ + "certifier.h", + "continuous_collision_checker.h", + ], + install_hdrs_exclude = ["certifier.h"], deps = [ "//common:essential", "//common:parallelism", @@ -295,8 +301,14 @@ drake_cc_googletest( # 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", diff --git a/planning/continuous_collision/test/motion_bound_test.cc b/planning/continuous_collision/test/motion_bound_test.cc index 11bc6624c922..6898bac72d5f 100644 --- a/planning/continuous_collision/test/motion_bound_test.cc +++ b/planning/continuous_collision/test/motion_bound_test.cc @@ -1468,7 +1468,7 @@ void RunTightFloatingBaseLambda(bool quaternion) { // 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, [kRadius](Rng* g) { + const Matrix3Xd points_B = test::SampleSurface(&rng, 4096, [](Rng* g) { return test::SampleSphere(g, kRadius); }); auto root = diagram->CreateDefaultContext(); diff --git a/planning/continuous_collision/test/test_utilities.h b/planning/continuous_collision/test/test_utilities.h index 273441974ffc..0d72576c0c73 100644 --- a/planning/continuous_collision/test/test_utilities.h +++ b/planning/continuous_collision/test/test_utilities.h @@ -447,6 +447,22 @@ inline ::testing::AssertionResult FindingIdentical( constexpr uint64_t kProbeBudget = 6000; constexpr uint64_t kMinDeepNodes = 1000; +// The workload runs at the resolution floor BaseOptions sets, and that floor +// is what keeps the bisection below affordable in an unoptimized build. Most +// of its probes land on a margin the search rejects, and a rejected probe is +// the expensive kind: it keeps subdividing until every leaf is either +// certified or narrower than Options::min_interval, so that floor is the only +// thing bounding it. Measured on this workload, the worst rejected probe in +// the band around the grazing margin costs about 5e3 nodes at a floor of 1e-6 +// but about 4.8e5 nodes at 1e-8, which is the difference between a bisection +// costing a tenth of a second and one costing four -- and, multiplied by the +// ~70x an unoptimized build charges, between fitting the dbg test budget and +// overrunning it. (Before Options::max_nodes was withdrawn from the public +// API the probe bounded itself directly and the floor did not have to.) +// +// The certifying side is indifferent to the choice: the tree the search +// converges on is 19 levels deep, so no leaf it visits comes near even 1e-6 +// wide, and it explores the identical tree at either floor. struct DeepWorkload { const Case* entry{}; double margin{0.0}; @@ -455,7 +471,6 @@ struct DeepWorkload { Options options(Parallelism parallelism) const { Options options = BaseOptions(parallelism); options.margin = margin; - options.min_interval = 1e-8; return options; } }; From c5bf9cda0b802b57304bda8fdd714c638e61b63a Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 3 Sep 2026 10:16:42 -0400 Subject: [PATCH 21/22] [planning] continuous_collision: replace min_interval with a distance-resolution floor --- .../planning_continuous_collision.h | 46 ++++++--- .../planning_py_continuous_collision.cc | 3 +- .../test/continuous_collision_test.py | 5 +- planning/continuous_collision/certifier.cc | 23 +++-- .../continuous_collision_checker.cc | 9 +- .../continuous_collision_checker.h | 38 +++++-- planning/continuous_collision/internal.h | 4 +- .../continuous_collision/motion_bound_table.h | 23 +++-- .../continuous_collision/test/api_test.cc | 16 ++- .../test/certifier_test.cc | 4 +- .../test/soundness_fuzz_test.cc | 98 ++++++++++++++++--- .../test/test_utilities.h | 40 ++++---- 12 files changed, 231 insertions(+), 78 deletions(-) diff --git a/bindings/generated_docstrings/planning_continuous_collision.h b/bindings/generated_docstrings/planning_continuous_collision.h index fc3d4356fdcc..c0c8c63f0c45 100644 --- a/bindings/generated_docstrings/planning_continuous_collision.h +++ b/bindings/generated_docstrings/planning_continuous_collision.h @@ -37,6 +37,23 @@ 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 @@ -79,7 +96,8 @@ PiecewisePolynomial, or a CompositeTrajectory of those). Raises: RuntimeError if Options∷margin is not a finite nonnegative - distance, or if Options∷min_interval is outside (0, 1]. + distance, or if Options∷distance_resolution is not a finite + positive distance. Raises: RuntimeError if the trajectory's row count differs from the @@ -197,6 +215,19 @@ R"""(Position coordinates whose junction continuity is checked modulo 2π 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 @@ -205,14 +236,6 @@ 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::min_interval - struct /* min_interval */ { - // Source: drake/planning/continuous_collision/continuous_collision_checker.h - const char* doc = -R"""(Resolution floor, as a fraction of a segment's parameter width; a node -narrower than this yields Verdict∷kInconclusive instead of splitting. -Must lie in (0, 1].)"""; - } min_interval; // Symbol: drake::planning::continuous_collision::Options::parallelism struct /* parallelism */ { // Source: drake/planning/continuous_collision/continuous_collision_checker.h @@ -257,8 +280,9 @@ entire continuous time domain.)"""; struct /* kInconclusive */ { // Source: drake/planning/continuous_collision/continuous_collision_checker.h const char* doc = -R"""(Subdivision hit the resolution floor with some pair's clearance within -oracle tolerance of the threshold (a grazing trajectory).)"""; +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 */ { diff --git a/bindings/pydrake/planning/planning_py_continuous_collision.cc b/bindings/pydrake/planning/planning_py_continuous_collision.cc index d393d7215c4a..a27086470239 100644 --- a/bindings/pydrake/planning/planning_py_continuous_collision.cc +++ b/bindings/pydrake/planning/planning_py_continuous_collision.cc @@ -62,7 +62,8 @@ collision-free over its entire continuous time domain, rather than sampling it. .def(py::init<>()) .def(ParamInit()) .def_rw("margin", &Class::margin, cls_doc.margin.doc) - .def_rw("min_interval", &Class::min_interval, cls_doc.min_interval.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) diff --git a/bindings/pydrake/planning/test/continuous_collision_test.py b/bindings/pydrake/planning/test/continuous_collision_test.py index f4ac0b9bde1d..83778cdea5f0 100644 --- a/bindings/pydrake/planning/test/continuous_collision_test.py +++ b/bindings/pydrake/planning/test/continuous_collision_test.py @@ -105,12 +105,13 @@ def setUp(self): 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.min_interval = 1e-8 + 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.min_interval, 1e-8) + 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) diff --git a/planning/continuous_collision/certifier.cc b/planning/continuous_collision/certifier.cc index ba2374abc398..55abced6196d 100644 --- a/planning/continuous_collision/certifier.cc +++ b/planning/continuous_collision/certifier.cc @@ -377,7 +377,7 @@ void Worker::RunItem(WorkItem* item) { const std::vector& tau = *input_.tau; const PrefilterTable& prefilter = *input_.prefilter; const double threshold = input_.options.margin; - const double min_interval = input_.options.min_interval; + 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()); @@ -435,12 +435,11 @@ void Worker::RunItem(WorkItem* item) { const double s_mid = 0.5 * (frame.s_lo + frame.s_hi); const double t_mid = TimeOf(segment, s_mid); - // The resolution floor, plus a hard floating-point backstop: once the - // midpoint no longer separates the endpoints in double arithmetic the node - // cannot be split any further, whatever min_interval says. Without it a - // pathologically small min_interval would spin forever. - const bool at_floor = (frame.s_hi - frame.s_lo) <= min_interval || - !(s_mid > frame.s_lo && s_mid < frame.s_hi); + // 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; @@ -453,7 +452,15 @@ void Worker::RunItem(WorkItem* item) { 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 motion_bound = table.MotionBound(p, w_); + 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, diff --git a/planning/continuous_collision/continuous_collision_checker.cc b/planning/continuous_collision/continuous_collision_checker.cc index a0cdec1bc875..33fa616b6d7f 100644 --- a/planning/continuous_collision/continuous_collision_checker.cc +++ b/planning/continuous_collision/continuous_collision_checker.cc @@ -180,11 +180,12 @@ void ValidateOptions(const Options& options) { "a negative margin.", options.margin)); } - if (!(options.min_interval > 0.0) || !(options.min_interval <= 1.0)) { + if (!(options.distance_resolution > 0.0) || + !std::isfinite(options.distance_resolution)) { throw std::runtime_error(fmt::format( - "ContinuousCollisionChecker: Options::min_interval is a fraction of a " - "segment's parameter width and must lie in (0, 1]; got {}.", - options.min_interval)); + "ContinuousCollisionChecker: Options::distance_resolution must be a " + "finite positive distance in meters; got {}.", + options.distance_resolution)); } } diff --git a/planning/continuous_collision/continuous_collision_checker.h b/planning/continuous_collision/continuous_collision_checker.h index 6fe9fa4af55f..9bebc5fb0803 100644 --- a/planning/continuous_collision/continuous_collision_checker.h +++ b/planning/continuous_collision/continuous_collision_checker.h @@ -26,8 +26,9 @@ enum class Verdict { kCertifiedFree, /** An exactly-on-trajectory configuration violates the threshold. */ kViolationFound, - /** Subdivision hit the resolution floor with some pair's clearance within - oracle tolerance of the threshold (a grazing trajectory). */ + /** 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, }; @@ -57,10 +58,14 @@ struct Options { > margin for every unfiltered pair at every time. Must be finite and nonnegative. */ double margin{0.0}; - /** Resolution floor, as a fraction of a segment's parameter width; a node - narrower than this yields Verdict::kInconclusive instead of splitting. Must - lie in (0, 1]. */ - double min_interval{1e-9}; + /** 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 */ @@ -90,6 +95,24 @@ 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 @@ -119,7 +142,8 @@ class 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::min_interval is outside (0, 1]. + 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, diff --git a/planning/continuous_collision/internal.h b/planning/continuous_collision/internal.h index 283a4e70dea1..96c45e63644e 100644 --- a/planning/continuous_collision/internal.h +++ b/planning/continuous_collision/internal.h @@ -13,7 +13,9 @@ threshold (Options::margin), and ε the numerical slack. 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. + - 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 diff --git a/planning/continuous_collision/motion_bound_table.h b/planning/continuous_collision/motion_bound_table.h index 17bd89fe22c7..26851c65c1af 100644 --- a/planning/continuous_collision/motion_bound_table.h +++ b/planning/continuous_collision/motion_bound_table.h @@ -84,17 +84,26 @@ class MotionBoundTable { return row_start_[pair_index] == row_start_[pair_index + 1]; } - /* Δ_p(ν) = carveout_slack(p) + Σ_{j ∈ J(p)} λ(j,p) · w_j: a sparse dot - product against the node's per-coordinate deviations w, plus the carved - coordinates' residual. + /* Σ_{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 MotionBound(int pair_index, const Eigen::VectorXd& w) const { - double delta = carveout_slack_[pair_index]; + 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) { - delta += lambda_[e] * w[coord_[e]]; + travel += lambda_[e] * w[coord_[e]]; } - return delta; + 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 diff --git a/planning/continuous_collision/test/api_test.cc b/planning/continuous_collision/test/api_test.cc index 33c486c6a8a1..50753f49a95b 100644 --- a/planning/continuous_collision/test/api_test.cc +++ b/planning/continuous_collision/test/api_test.cc @@ -334,13 +334,21 @@ GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { // threshold and silently "certified". const std::vector>> cases = { - {"min_interval", + {"distance_resolution", [](Options* o) { - o->min_interval = 0.0; + o->distance_resolution = 0.0; }}, - {R"((0, 1])", + {"positive", [](Options* o) { - o->min_interval = 2.0; + o->distance_resolution = -1e-3; + }}, + {"distance_resolution", + [](Options* o) { + o->distance_resolution = std::numeric_limits::quiet_NaN(); + }}, + {"finite", + [](Options* o) { + o->distance_resolution = std::numeric_limits::infinity(); }}, {"nonnegative", [](Options* o) { diff --git a/planning/continuous_collision/test/certifier_test.cc b/planning/continuous_collision/test/certifier_test.cc index 402cf0c29cba..1fbec4fc1357 100644 --- a/planning/continuous_collision/test/certifier_test.cc +++ b/planning/continuous_collision/test/certifier_test.cc @@ -299,9 +299,9 @@ GTEST_TEST(CertifierTest, GrazingTangencyIsInconclusive) { MakeBezier(MakeQ(0.0, 0.0, 0.0), MakeQ(0.0, 0.0, 0.20), 1); Options options = SerialOptions(); - // A coarser floor keeps the cost of the tangency cascade bounded; the + // A coarse resolution keeps the cost of the tangency cascade bounded; the // verdict is what matters here, not the depth. - options.min_interval = 1e-4; + options.distance_resolution = 1e-4; const Result result = checker->CheckTrajectory(trajectory, options); EXPECT_EQ(result.verdict, Verdict::kInconclusive); diff --git a/planning/continuous_collision/test/soundness_fuzz_test.cc b/planning/continuous_collision/test/soundness_fuzz_test.cc index 94aba31d2f6a..6474bc5abbd9 100644 --- a/planning/continuous_collision/test/soundness_fuzz_test.cc +++ b/planning/continuous_collision/test/soundness_fuzz_test.cc @@ -121,6 +121,13 @@ constexpr int kGrazeProbeSamples = 2000; // 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. @@ -708,6 +715,12 @@ struct Tally { 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}; @@ -723,17 +736,24 @@ struct Tally { double tightest_certified_slack{std::numeric_limits::infinity()}; }; -// Base options shared by every case. A coarser resolution floor than the 1e-9 -// default: a grazing pair still ends kInconclusive, but after ~20 bisections -// rather than ~30. -Options FuzzOptions(double margin) { +// 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.min_interval = 1e-6; + 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) { @@ -789,7 +809,8 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { std::to_string(margin) + (grazing ? " (grazing)" : "") + "\n" + world.Describe() + trajectory_recipe.Describe()); - const Options options = FuzzOptions(margin); + 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, {}); @@ -797,6 +818,35 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { 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; @@ -805,8 +855,8 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { // A single one would be a false certificate. const bool deep = (tally.certified % kDeepEvery) == 0; if (deep) ++tally.deep_scans; - const DenseScanner::Worst scan = scanner.Scan( - path, deep ? kDeepDenseSamples : kDenseSamples, margin); + 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) @@ -850,10 +900,23 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { EXPECT_NEAR(phi, finding.distance, 1e-9) << "the reported distance is not reproducible at the witness"; } else { - // An inconclusive witness is a resolution-floor grazing record, and - // must be backed by a clearance that sits within 10·(τ_p + ε) of the - // threshold somewhere near the reported time. - const double tolerance = 10.0 * (kWorstTau + kNumericalSlack); + // 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 = @@ -862,7 +925,7 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { EXPECT_LE(best, tolerance) << "INCONCLUSIVE at t = " << finding.time << " but the closest sampled clearance near it is " << best - << " m from the threshold, far outside 10*(tau + eps) = " + << " m from the threshold, outside r + 10*(tau + eps) = " << tolerance; } } @@ -876,6 +939,10 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { << " 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 = " @@ -909,6 +976,11 @@ GTEST_TEST(SoundnessFuzzTest, RandomWorldsAndTrajectories) { 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); diff --git a/planning/continuous_collision/test/test_utilities.h b/planning/continuous_collision/test/test_utilities.h index 0d72576c0c73..8de1c2b4a77e 100644 --- a/planning/continuous_collision/test/test_utilities.h +++ b/planning/continuous_collision/test/test_utilities.h @@ -347,7 +347,8 @@ inline Options BaseOptions(Parallelism parallelism) { options.margin = kMargin; options.parallelism = parallelism; // Bounded cost per run: the whole sweep is executed several times per case. - options.min_interval = 1e-6; + // See the note on DeepWorkload for why the resolution matters. + options.distance_resolution = 1e-6; return options; } @@ -445,24 +446,26 @@ inline ::testing::AssertionResult FindingIdentical( // 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 = 1000; - -// The workload runs at the resolution floor BaseOptions sets, and that floor -// is what keeps the bisection below affordable in an unoptimized build. Most -// of its probes land on a margin the search rejects, and a rejected probe is -// the expensive kind: it keeps subdividing until every leaf is either -// certified or narrower than Options::min_interval, so that floor is the only -// thing bounding it. Measured on this workload, the worst rejected probe in -// the band around the grazing margin costs about 5e3 nodes at a floor of 1e-6 -// but about 4.8e5 nodes at 1e-8, which is the difference between a bisection -// costing a tenth of a second and one costing four -- and, multiplied by the -// ~70x an unoptimized build charges, between fitting the dbg test budget and -// overrunning it. (Before Options::max_nodes was withdrawn from the public -// API the probe bounded itself directly and the floor did not have to.) +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 certifying side is indifferent to the choice: the tree the search -// converges on is 19 levels deep, so no leaf it visits comes near even 1e-6 -// wide, and it explores the identical tree at either floor. +// 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}; @@ -471,6 +474,7 @@ struct DeepWorkload { Options options(Parallelism parallelism) const { Options options = BaseOptions(parallelism); options.margin = margin; + options.distance_resolution = kDeepResolution; return options; } }; From 97d9d2cb08bedb5045cbef7c884d2300e62b8385 Mon Sep 17 00:00:00 2001 From: Peter Werner Date: Thu, 3 Sep 2026 10:52:12 -0400 Subject: [PATCH 22/22] [planning] continuous_collision: make the options validation test clang-format stable --- .../continuous_collision/test/api_test.cc | 47 +++++++------------ 1 file changed, 17 insertions(+), 30 deletions(-) diff --git a/planning/continuous_collision/test/api_test.cc b/planning/continuous_collision/test/api_test.cc index 50753f49a95b..3291f6c17026 100644 --- a/planning/continuous_collision/test/api_test.cc +++ b/planning/continuous_collision/test/api_test.cc @@ -10,7 +10,6 @@ // 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 @@ -332,37 +331,25 @@ GTEST_TEST(ApiTest, OptionsValidationMessagesAreActionable) { // 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". - const std::vector>> - cases = { - {"distance_resolution", - [](Options* o) { - o->distance_resolution = 0.0; - }}, - {"positive", - [](Options* o) { - o->distance_resolution = -1e-3; - }}, - {"distance_resolution", - [](Options* o) { - o->distance_resolution = std::numeric_limits::quiet_NaN(); - }}, - {"finite", - [](Options* o) { - o->distance_resolution = std::numeric_limits::infinity(); - }}, - {"nonnegative", - [](Options* o) { - o->margin = -0.01; - }}, - {"margin", - [](Options* o) { - o->margin = std::numeric_limits::quiet_NaN(); - }}, - }; - for (const auto& [needle, mutate] : cases) { + 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(); - mutate(&bad); + bad.*field = value; EXPECT_THAT(ThrowMessage([&]() { checker->CheckTrajectory(trajectory, bad); }),