diff --git a/CHANGELOG.md b/CHANGELOG.md index 3e428803c2959..778b233eb9954 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -201,6 +201,27 @@ * Collision + * Restore full-quality contact streams in the `dart` detector: emit the + complete four-contact face manifold from solver-facing queries instead + of the previous three-contact target (a tripod manifold cannot hold a + resting face-face box stack), give side-lying cylinders a stable + two-point contact-line manifold instead of one point that wanders + ~90 mm under micro-motion, and keep shallow crossed-cylinder contacts + that the convex fallback intermittently missed + ([#3056](https://github.com/dartsim/dart/issues/3056)), and make the + aligned cylinder-box path invariant to spin about the cylinder axis so + upright cylinders take the stable analytic cap patch instead of + creeping through degenerate convex-fallback rim points. Generated + resting scenes settle and deactivate up to 4x faster, the dense + mixed-shape pile fixture now reaches full deactivation with zero + penetration under default settings (4 of 5 tested seeds within the + original 20-second window, all tested seeds by 60 seconds), and the + complete bundle measures slightly faster than the pre-consolidation + detector on the dense active-container fixture while reporting fuller + manifolds. FCL, Bullet, and ODE results are bit-identical; + the built-in default (`fcl`) is unchanged: + [#3428](https://github.com/dartsim/dart/pull/3428) + * Provide the DART-owned collision backend through the built-in `dart` detector, including soft-body, ellipsoid, cone, and capsule coverage. The released `DARTCollide` entry points and detector ABI remain compatible, as diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 59d2ab572b5ec..be19848a7b5d4 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -254,9 +254,16 @@ class CollisionThreadPool namespace { //============================================================================== -// Three non-collinear contacts define a stable planar patch while avoiding a -// fourth redundant solver row on contact-rich native scenes. -constexpr std::size_t kSolverFacingManifoldContactTarget = 3u; +// Solver-facing queries carry the full manifold capacity. Three contacts are +// not enough for a resting face-face box stack: the tripod support polygon +// stops containing the weight vector under micro-tilts toward the missing +// corner, so stacked boxes rock indefinitely, penetration never converges, +// and the deactivation gates (correctly) never let the island sleep +// (issue #3056 S6 pile fixture). Callers still bound the stream through +// CollisionOption's per-pair cap; DARTCollide keeps its explicit unlimited +// adapter request. +constexpr std::size_t kSolverFacingManifoldContactTarget + = native::ContactManifold::kMaxContacts; // Bound retained parallel scratch independently of scene pair count. Batches // are merged before the broadphase or Cartesian traversal continues, keeping @@ -294,8 +301,8 @@ native::CollisionOption makeNativeOption( const std::size_t maxPairContacts = option.getEffectiveMaxNumContactsPerPair(); // DARTCollide uses an explicit unlimited per-pair request to preserve its - // released full-manifold behavior. Ordinary detector queries retain the - // solver-facing three-contact target, including wider finite requests. + // released full-manifold behavior. Ordinary detector queries clamp wider + // finite requests to the solver-facing manifold target above. const bool preserveCompleteManifold = option.maxNumContactsPerPair == std::numeric_limits::max(); diff --git a/dart/collision/dart/narrow_phase/CylinderCollision.cpp b/dart/collision/dart/narrow_phase/CylinderCollision.cpp index f633a4dd150a0..f21e483a9442d 100644 --- a/dart/collision/dart/narrow_phase/CylinderCollision.cpp +++ b/dart/collision/dart/narrow_phase/CylinderCollision.cpp @@ -377,6 +377,268 @@ bool canUseParallelCylinderShortcut( return angularSweep <= eps * lengthScale; } +// A cylinder side resting on a near-parallel box face needs a stable +// two-point line manifold: the convex fallback returns one arbitrary point +// along the under-constrained contact line, so the support teleports under +// micro-motion and resting piles never stop rocking (issue #3056 S6 +// fixture). Emits the clipped contact-line endpoints with per-endpoint +// depths; cap, edge, and steeper-tilt cases fall through to the existing +// paths. +bool tryAddCylinderBoxSideLineContacts( + double cylinderRadius, + double cylinderHalfHeight, + const Eigen::Isometry3d& cylinderTransform, + const Eigen::Vector3d& boxHalfExtents, + const Eigen::Isometry3d& boxTransform, + CollisionResult& result, + const CollisionOption& option) +{ + // Side-surface depth formulas stay exact while the contact is on the side + // wall; ~3 degrees comfortably covers settling-pile tilts. + constexpr double kMaxAxisNormalDot = 0.05; + constexpr double kContactEpsilon = 1e-10; + constexpr double kMinSpan = 1e-6; + constexpr double kSlabTolerance = 1e-9; + + if (contactBudgetExhausted(result.numContacts(), option)) { + return false; + } + + const Eigen::Isometry3d boxInverse = boxTransform.inverse(); + const Eigen::Vector3d center = boxInverse * cylinderTransform.translation(); + const Eigen::Vector3d axis + = boxInverse.linear() * cylinderTransform.rotation().col(2); + + double closestFaceDistance = -std::numeric_limits::infinity(); + int faceAxis = 0; + double faceSign = 1.0; + for (int axisIndex = 0; axisIndex < 3; ++axisIndex) { + for (const double sign : {-1.0, 1.0}) { + const double distance + = sign * center[axisIndex] - boxHalfExtents[axisIndex]; + if (distance > closestFaceDistance) { + closestFaceDistance = distance; + faceAxis = axisIndex; + faceSign = sign; + } + } + } + + if (std::abs(axis[faceAxis]) > kMaxAxisNormalDot) { + return false; + } + + const Eigen::Vector3d segmentStart = center - axis * cylinderHalfHeight; + const Eigen::Vector3d segmentEnd = center + axis * cylinderHalfHeight; + const Eigen::Vector3d segmentDelta = segmentEnd - segmentStart; + + // A tilted cylinder's support distance along the face normal is the + // radius scaled by the axis-normal complement, not the full radius; the + // full radius would fabricate shallow contacts (and overstate depths) in + // a band of up to r * dot^2 / 2 near separation. + const double axisNormalDot = axis[faceAxis]; + const double effectiveRadius + = cylinderRadius * std::sqrt(1.0 - axisNormalDot * axisNormalDot); + + // Side-surface penetration is linear along the axis; keep the interval + // where it is non-negative. + const auto sidePenetrationAt = [&](const Eigen::Vector3d& axisPoint) { + const double clearance + = faceSign * axisPoint[faceAxis] - boxHalfExtents[faceAxis]; + return effectiveRadius - clearance; + }; + const double penStart = sidePenetrationAt(segmentStart); + const double penEnd = sidePenetrationAt(segmentEnd); + if (penStart < -kContactEpsilon && penEnd < -kContactEpsilon) { + return false; + } + // Only genuinely shallow side-resting poses belong to this path: once an + // axis endpoint reaches the face plane (penetration beyond the support + // radius) the geometry is a deep overlap, which stays with the legacy + // minimal-translation paths. + if (penStart > effectiveRadius + kContactEpsilon + || penEnd > effectiveRadius + kContactEpsilon) { + return false; + } + + double tMin = 0.0; + double tMax = 1.0; + const double penDelta = penEnd - penStart; + if (std::abs(penDelta) > kContactEpsilon) { + const double tZero = -penStart / penDelta; + if (penDelta > 0.0) { + tMin = std::max(tMin, tZero); + } else { + tMax = std::min(tMax, tZero); + } + } + + // Clip the axis segment to the face rectangle along both tangent axes. + for (int axisIndex = 0; axisIndex < 3; ++axisIndex) { + if (axisIndex == faceAxis) { + continue; + } + const double start = segmentStart[axisIndex]; + const double delta = segmentDelta[axisIndex]; + const double bound = boxHalfExtents[axisIndex] + kSlabTolerance; + if (std::abs(delta) <= kContactEpsilon) { + if (std::abs(start) > bound) { + return false; + } + continue; + } + double tEnter = (-bound - start) / delta; + double tExit = (bound - start) / delta; + if (tEnter > tExit) { + std::swap(tEnter, tExit); + } + tMin = std::max(tMin, tEnter); + tMax = std::min(tMax, tExit); + } + + if (tMax - tMin <= 0.0 || (tMax - tMin) * segmentDelta.norm() < kMinSpan) { + return false; + } + + if (!option.enableContact) { + return true; + } + + ContactManifold manifold; + manifold.setType(ContactType::Patch); + const Eigen::Vector3d normalLocal + = Eigen::Vector3d::Unit(faceAxis) * faceSign; + const Eigen::Vector3d normalWorld = boxTransform.rotation() * normalLocal; + + for (const double t : {tMin, tMax}) { + if (result.numContacts() + manifold.numContacts() + >= option.maxNumContacts) { + break; + } + const Eigen::Vector3d axisPoint = segmentStart + t * segmentDelta; + const double depth = std::max(0.0, sidePenetrationAt(axisPoint)); + ContactPoint contact; + contact.position + = boxTransform + * (axisPoint - normalLocal * (effectiveRadius - 0.5 * depth)); + contact.normal = normalWorld; + contact.depth = depth; + manifold.addContact(contact); + } + + if (!manifold.hasContacts()) { + return false; + } + if (manifold.numContacts() == 1) { + manifold.setType(ContactType::Point); + } + result.addManifold(std::move(manifold)); + return true; +} + +// Clamped closest parameters between two segments (mirrors the capsule +// narrowphase). Returns false when the segments are near-parallel, where +// the closest pair is non-unique and callers should keep their existing +// paths. +bool closestParamsBetweenSegments( + const Eigen::Vector3d& p1, + const Eigen::Vector3d& q1, + const Eigen::Vector3d& p2, + const Eigen::Vector3d& q2, + double& s, + double& t) +{ + const Eigen::Vector3d d1 = q1 - p1; + const Eigen::Vector3d d2 = q2 - p2; + const Eigen::Vector3d r = p1 - p2; + + const double a = d1.squaredNorm(); + const double e = d2.squaredNorm(); + constexpr double kDegenerate = 1e-10; + if (a <= kDegenerate || e <= kDegenerate) { + return false; + } + + const double b = d1.dot(d2); + const double c = d1.dot(r); + const double f = d2.dot(r); + const double denom = a * e - b * b; + constexpr double kParallelRatio = 1e-6; + if (denom <= kParallelRatio * a * e) { + return false; + } + + s = std::clamp((b * f - c * e) / denom, 0.0, 1.0); + t = (b * s + f) / e; + if (t < 0.0) { + t = 0.0; + s = std::clamp(-c / a, 0.0, 1.0); + } else if (t > 1.0) { + t = 1.0; + s = std::clamp((b - c) / a, 0.0, 1.0); + } + return true; +} + +// Crossed side-side cylinder contact: while both closest points stay +// interior to the axes, the swept-circle side surfaces match capsules +// exactly, so the sphere-like closest-point contact is exact — and the +// convex fallback intermittently misses shallow crossed contacts +// (measured: pairs dropped at ~1 mm penetration under 50 µm slides). +bool tryAddCrossedCylinderSideContact( + double r1, + double h1, + const Eigen::Vector3d& axis1, + const Eigen::Vector3d& center1, + double r2, + double h2, + const Eigen::Vector3d& axis2, + const Eigen::Vector3d& center2, + CollisionResult& result, + const CollisionOption& option) +{ + constexpr double kInteriorMargin = 0.02; + constexpr double kContactEpsilon = 1e-10; + + double s = 0.0; + double t = 0.0; + if (!closestParamsBetweenSegments( + center1 - axis1 * h1, + center1 + axis1 * h1, + center2 - axis2 * h2, + center2 + axis2 * h2, + s, + t)) { + return false; + } + if (s < kInteriorMargin || s > 1.0 - kInteriorMargin || t < kInteriorMargin + || t > 1.0 - kInteriorMargin) { + return false; + } + + const Eigen::Vector3d point1 = center1 + axis1 * ((2.0 * s - 1.0) * h1); + const Eigen::Vector3d point2 = center2 + axis2 * ((2.0 * t - 1.0) * h2); + const Eigen::Vector3d delta = point2 - point1; + const double dist = delta.norm(); + const double depth = r1 + r2 - dist; + if (depth < -kContactEpsilon) { + return false; + } + if (!option.enableContact) { + return true; + } + + const Eigen::Vector3d lateralDir = dist > kContactEpsilon + ? (delta / dist).eval() + : chooseRadialDirection(axis1); + ContactPoint contact; + contact.normal = -lateralDir; + contact.depth = std::max(0.0, depth); + contact.position = point1 + lateralDir * (r1 - 0.5 * contact.depth); + result.addContact(contact); + return true; +} + } // namespace bool collideCylinders( @@ -406,6 +668,11 @@ bool collideCylinders( r1, h1, axis1, center1, r2, h2, center2, result, option); } + if (tryAddCrossedCylinderSideContact( + r1, h1, axis1, center1, r2, h2, axis2, center2, result, option)) { + return true; + } + return collideConvexConvex( cyl1, transform1, cyl2, transform2, result, option); } @@ -586,14 +853,62 @@ bool collideCylinderBox( const Eigen::Isometry3d cylInv = cylinderTransform.inverse(); const Eigen::Isometry3d boxInCyl = cylInv * boxTransform; - if (boxInCyl.linear().isApprox(Eigen::Matrix3d::Identity(), 1e-12)) { - const Eigen::Vector3d center = boxInCyl.translation(); - const double minX = center.x() - boxHalf.x(); - const double maxX = center.x() + boxHalf.x(); - const double minY = center.y() - boxHalf.y(); - const double maxY = center.y() + boxHalf.y(); - const double minZ = center.z() - boxHalf.z(); - const double maxZ = center.z() + boxHalf.z(); + // A side-lying cylinder needs the two-point contact-line manifold before + // any single-point path (aligned lateral or convex fallback) can answer + // with one rocking support; the helper gates itself to near-face-parallel + // poses and declines everything else. + if (tryAddCylinderBoxSideLineContacts( + cylRadius, + cylHalfHeight, + cylinderTransform, + boxHalf, + boxTransform, + result, + option)) { + return true; + } + + // Cylinder-box contact is invariant under spin about the cylinder axis + // (surface of revolution), so the aligned analytic path must not demand + // exact rotational identity: detect a box axis parallel to the cylinder + // axis, canonicalize the spin and axis permutation away, and reuse the + // aligned math. Without this, an upright cylinder that settled with an + // arbitrary spin falls to the convex fallback, whose degenerate rim + // points let a loaded cylinder creep through its support (issue #3056 + // S6 seed-101 evidence: monotonic ~2.6 um/step sinking). + constexpr double kAxisParallelTolerance = 1e-9; + const Eigen::Matrix3d boxRotInCyl = boxInCyl.linear(); + int alignedBoxAxis = -1; + for (int k = 0; k < 3; ++k) { + if (std::abs(std::abs(boxRotInCyl(2, k)) - 1.0) <= kAxisParallelTolerance) { + alignedBoxAxis = k; + break; + } + } + if (alignedBoxAxis >= 0) { + const int transverse1 = (alignedBoxAxis + 1) % 3; + const int transverse2 = (alignedBoxAxis + 2) % 3; + Eigen::Vector3d spinX = boxRotInCyl.col(transverse1); + spinX.z() = 0.0; + const double spinXNorm = spinX.norm(); + Eigen::Matrix3d spin = Eigen::Matrix3d::Identity(); + if (spinXNorm > 1e-12) { + spinX /= spinXNorm; + spin.col(0) = spinX; + spin.col(1) = Eigen::Vector3d::UnitZ().cross(spinX); + spin.col(2) = Eigen::Vector3d::UnitZ(); + } + Eigen::Isometry3d canonTransform = cylinderTransform; + canonTransform.linear() = cylinderTransform.linear() * spin; + const Eigen::Vector3d center = spin.transpose() * boxInCyl.translation(); + const Eigen::Vector3d half( + boxHalf[transverse1], boxHalf[transverse2], boxHalf[alignedBoxAxis]); + const double minX = center.x() - half.x(); + const double maxX = center.x() + half.x(); + const double minY = center.y() - half.y(); + const double maxY = center.y() + half.y(); + const double minZ = center.z() - half.z(); + const double maxZ = center.z() + half.z(); const double zOverlap = std::min(cylHalfHeight, maxZ) - std::max(-cylHalfHeight, minZ); @@ -635,7 +950,7 @@ bool collideCylinderBox( contactLocal.z() = topAxialContact ? minZ : maxZ; return addAxialCylinderBoxPatchContacts( cylRadius, - cylinderTransform, + canonTransform, minX, maxX, minY, @@ -668,10 +983,9 @@ bool collideCylinderBox( return true; } - const Eigen::Vector3d normalWorld - = cylinderTransform.rotation() * normalLocal; + const Eigen::Vector3d normalWorld = canonTransform.rotation() * normalLocal; const Eigen::Vector3d contactWorld - = cylinderTransform * contactLocal - normalWorld * (penetration * 0.5); + = canonTransform * contactLocal - normalWorld * (penetration * 0.5); ContactPoint contact; contact.position = contactWorld; diff --git a/docs/design/dart6_deformable_body.md b/docs/design/dart6_deformable_body.md index 0ae751516f1b6..19a6b07be9748 100644 --- a/docs/design/dart6_deformable_body.md +++ b/docs/design/dart6_deformable_body.md @@ -93,7 +93,7 @@ The DART collision object owns one retained deforming-geometry cache; pair kernels must not maintain parallel mirrors. Each pair preserves object order, contact point, normal, depth, soft-side face IDs, established non-finite-bounds behavior, and the full configured per-pair contact budget at both generation -and emission. The rigid three-contact generation clamp must not truncate soft +and emission. The rigid solver-facing manifold clamp must not truncate soft contacts before emission. Cache access uses the canonical local vertex formula, point position plus resting offset. A missing or mismatched cache view fails loudly rather than falling back to `getLocalPosition()`, which does not provide diff --git a/docs/dev_tasks/dart6_performance_generalization/01-baseline-evidence.md b/docs/dev_tasks/dart6_performance_generalization/01-baseline-evidence.md index dd0098eab515f..cf1b850ea7593 100644 --- a/docs/dev_tasks/dart6_performance_generalization/01-baseline-evidence.md +++ b/docs/dev_tasks/dart6_performance_generalization/01-baseline-evidence.md @@ -280,6 +280,356 @@ finite-state evidence. it needs D3 (solve-side) and/or D7 (sleep the pile) — flagged for the maintainer alongside those decisions. +## 2026-07-31 current-base guard refresh (post-#3381 consolidation) + +Session branch `wp-pg-wsg-rebaseline-20260731` on `origin/release-6.20` @ +`718651d0d6e`; GCC (pixi default), Release; i9-13950HX, governor +**powersave** with large observed clock swings (up to ~3x between runs of the +same binary), so **timing cells this cycle are host-state-relative and +cross-session step-time comparisons are invalid**; hashes, contact/pair +counts, resting counts, and finite flags remain the guards. Artifacts: +`/tmp/wsg_rebaseline_guards_20260731` (S1–S6 matrix, probes, series, +controls), `/tmp/wsg_ab_20260731` (interleaved A/Bs), scratch worktrees +`/tmp/wsg_audit_head` (audit head `db255a08e8e`) and `/tmp/wsg_pre3381` +(#3381 parent `f7c835bcbec`). + +Classification against the last recorded guards: + +- **Held bit-identical** (proves #3384's non-finite LCP guard and the + #3382 deformable-body/soft-contact work — which rewrote + `ConstraintSolver.cpp` — are behavior-neutral on identical rigid + contact streams): `S2_fcl 0x266da31836a314a6`, `S2_bullet 0x2375f1927218cd43`, + `S2_ode 0x10f80b0408cede90`, `S3_fcl 0x6088ea0177efa6a`, + `S3_bullet 0x22e27960cbabe83e`, `S3_ode 0x4904c09a93a36442`, + `S4_fcl 0xea9b68f8b062600d` (drift value), `S4_ode + 0x429b65bc5c4a14b6`, `S5_fcl 0x8277be4f0c14212` (drift value), + `S5_ode 0x5f2afc7230ee8d10`. +- **Re-established** (references lost with the July /tmp artifacts; + #3355-era bullet re-baseline): `S4_bullet 0x6a2e46e1a9ba76a6` + (2569 contacts, 0/900 resting), `S5_bullet 0xc9ab9e07e0a8501e` + (210 contacts, 55/90 resting), and the post-#3353 S1 ODE rows below. +- **Re-baselined by #3381** (its PR body's breaking-changes section + states explicit `"dart"` selection now uses the consolidated engine + and "its contact profile can differ"; note the tension with + `docs/design/dart6_collision_backends.md`'s preserved-contact-semantics + correctness clause, which feeds decision D9): every `dart` row. `S2_dart` and `S3_dart` + now land exactly on FCL's fixed points (`0x266da31836a314a6`, + `0x6088ea0177efa6a`; S3 contacts 5005 → 3003), `S4_dart`/`S5_dart` + settle fully (0 contacts, 900/900 and 90/90 resting, + `0x55bf77ebc1c491b2`, `0x4f265a803b596035`), and the S1 container rows + become `60: 80/72 0x1e227311a3f7188e`, `120: 251/177 + 0xd6736cd716faf01d` (1- and 16-thread CLI captures identical per cell). + S1 ODE rows: `60: 205/72 0x2b6f0f30483be5e7`, `120: 443/172 + 0x639185fc8921ba9c`. + +### Criterion 1 — re-verified MET via same-host chained A/B + +Interleaved (ABAB) same-host A/B against the audit-head binary +(`db255a08e8e`, its S1 run reproducing the audit hash +`0x123ee9779bccacfb` bit-exactly): S1 120/dart/1 median avg-step +**19.0 ms (current) vs 24.5 ms (audit stack)** over 5 pairs — the +current default-on stack measures ~1.29x faster than the stack the +2026-07-10 audit recorded at 3.51x the round-2 baseline, so criterion 1 +is chained-MET at ≈4.5x (point estimate). FCL/ODE control rows +(identical detectors across arms; 7/5/3 pairs) were parity or better +with bit-identical hashes per scene: S5fcl 1.171 vs 1.013 ms (1.16x), +S3fcl 39.27 vs 41.57 ms (0.94x), S4ode 0.321 vs 0.393 ms (0.82x). +Noise accounting: those identical-code controls themselves spread +0.82–1.16x, so the 1.29x "faster" margin is only slightly outside the +recorded control band — treat **MET** as robust (3.51x ≥ 3x stands even +at parity) and "improved" as a point estimate. Chained ratios multiply +A/Bs taken in different clock states (the same pre-fix binary measured +19.0 ms in one block and 6.49 ms in a later, faster-clock block — a +2.9x same-binary swing), so carry ~±20% on the ≈4.5x and ≈2.3x point +estimates; the below-3x conclusion for the post-fix stack survives that +band, as does pre-fix MET. An earlier apparent 1.6–6x "regression" +against the July step-time cells was a **host clock-state artifact**; +no code regression exists. + +### Criterion 2 — REGRESSED on the current base (the selected gap) + +`S6_dart` under defaults now ends **0/71 resting** (161 contacts, 129 +pairs, max penetration 0.00433 m bounded, finite, hash +`0x3fecba33246bc342`, `max_pair_contacts` pinned at **3** all run). +Attribution chain, all on this host: + +- FCL control on the current base sleeps: 71/71, penetration 0, smoothed + speeds 0 (`0xa2733f54d194cc97`) — solver, deactivation gates, and D7 + policy are healthy. +- Audit-head binary reproduces the accepted S6 bit-exactly + (`0xec80f734df6d5e74`, 71/71, `max_pair_contacts` 4, penetration + declining 0.0045 → 0.0038 → 0). +- **Pre-#3381 binary (`f7c835bcbec`: old `dart` detector plus #3382, + #3384, and the FEM churn) also reproduces `0xec80f734df6d5e74` + bit-exactly** — the solver-era merges up to #3381's parent did not + perturb the S6 trajectory at all. Combined with the FCL control above + (which exercises the *current* head's solver/sleep machinery, + covering the post-#3381 commits incl. #3407) and the directly + demonstrated 3-contact clamp mechanism, the detector swap is isolated + as the cause. +- Policy-knob probes on the new detector do not rescue it: explicit + `--sleep-contact-penetration-tolerance 0.005` ends 1/71 with zero + over-tolerance contacts (rest-veto not the blocker); the previously + accepted explicit evaluator row (`--contact-max-erv 0.1` + tol + `0.005`, formerly 71/71) ends 0/71. +- Mechanism (checkpoint series): the consolidated box-box stream yields + ≤3-point manifolds (old engine sustained 4-point face manifolds), the + contact-pair set churns ~±10% between checkpoints, penetration + plateaus at ~0.0044 m instead of converging, and worst-body smoothed + linear speed (~0.0235 m/s) stays above the 0.02 m/s wake band, so + rest dwell (max 0.43 s < 0.5 s) and dense-island candidacy never + engage. The narrowphase clip path drops non-penetrating corners of a + slightly tilted face contact, leaving tripod support that keeps the + pile micro-rocking. `tests/unit/collision/dart/test_box_box.cpp` only + asserts 2–4 contacts for face patches, so the 3-point profile passes + the existing suite. + +### 2026-07-31 manifold fix (WP-PG.50 candidate) — mechanism, evidence, and trade-off + +Root cause of the criterion-2 regression: the consolidated detector clamps +every ordinary solver-facing query to +`kSolverFacingManifoldContactTarget = 3` contacts per pair +(`DARTCollisionDetector.cpp`), so face-face box stacks ride on 3-point +tripod manifolds (a dartpy probe — flat/tilted 0.2-cube on a floor box via +`DARTCollisionDetector` + `CollisionGroup::collide`, per-pair budget 100 — +shows a perfectly flat stack emitting exactly 3 corners with the 4th +dropped deterministically in every configuration tried; the old engine +sustained `max_pair_contacts 4` through the whole S6 settling). +Only the legacy `DARTCollide` adapter (explicit unlimited request) +received full manifolds — which is how #3381's four-contact compatibility +test passed while the solver stream lost its fourth support point. + +The candidate fix on this branch raises the constant to +`native::ContactManifold::kMaxContacts` (4) with pinned regression tests +(`SolverFacingQueriesCarryFullBoxManifold`, +`RestingBoxStackKeepsFourCornerContacts` — flat and micro-tilted stacks +must emit 4 distinct corner contacts; `Collision.Options` dart row updated +3 → 4). Gates on the fixed tree: 154/154 C++ tests, `pixi run lint` + +`check-lint` clean, and the downstream Gazebo gate passed end-to-end +(`DART_PARALLEL_JOBS=8 pixi run -e gazebo test-gz`, exit 0, after purging +the stale pre-#3381 installed headers/libs from the gazebo env; captured +tail shows gz-sim `INTEGRATION_entity_system` 1/1 and the gz-physics +performance stage 4/4 — the script aborts on any earlier-stage failure, +so exit 0 covers the functional suite as well). Post-fix artifacts: +`/tmp/wsg_postfix_20260731`, `/tmp/mj_cmp_postfix_20260731`. + +Post-fix measured effects (same session, interleaved where timing matters): + +- **Untouched detectors bit-identical**: S3_fcl `0x6088ea0177efa6a`, + S4_ode `0x429b65bc5c4a14b6`, S5_bullet `0xc9ab9e07e0a8501e`. +- **S2/S3 dart hashes unchanged** (plane-contact scenes keep their + FCL-coincident fixed points). +- **Resting scenes get faster**: S4_dart 0.331 → 0.083 ms/step + (900/900 resting, hash `0x70bf5dd9e4f15051`), S5_dart 0.024 → 0.0058 + (90/90, `0xd8de4ae15996321f`) — both already reached full rest + pre-fix on this base; the fix delta is settling speed, from stable + manifolds sleeping sooner. These are single paired runs, not + interleaved medians (only the S1-120 row below was interleaved); + hashes and resting counts are the guards. +- **Always-active dense container pays ~2x**: interleaved S1 120/dart/1 + medians 6.49 (pre) → 12.67 ms/step (post), hashes stable per arm + (`0xd6736cd716faf01d` → `0xd28c5eae0f984dae`). Profile attribution: + 100% of the delta is Dantzig `primarySolve` (2.75 → 6.40 ms/call); + island rows grew 809 → 902 mean (+11%), so ~1.4x is intrinsic + cubic row scaling and the rest is pivot inflation on the redundant + coplanar 4th rows (potential future solver-side recovery packet). + Criterion 1 chained estimate falls to ≈2.3x over the round-2 baseline + (below the 3x bar). +- **S6 improves but does not fully sleep**: 20000 steps end 5/71 resting + (was 0/71; post-fix hash `0xf05b48c2fd1b2109`, 161 contacts / 115 + pairs — the pre-fix run also ended at 161 contacts but over 129 pairs + and a different hash, a verified coincidence of counts, not states), + dwell finally accumulates (max 0.539 s), 69/71 below the wake band; a + 60000-step run stays 0/71 (`0x2547cb01a038c374`) with penetration in a + limit cycle at 0.0046–0.0052 m — straddling the 0.005 dense-island + rest tolerance, so candidacy keeps clearing. Post-fix S1-60 dart CLI + row for the new guard set: 84 contacts / 73 pairs, hash + `0x5fea4fbf119db25a`. The old stack slept *with* + ~3.8 mm frozen penetration; its equilibrium simply sat lower with a + gentler velocity tail. +- **Tolerance is not a fix**: a diagnostic run with explicit + `--sleep-contact-penetration-tolerance 0.01` still never rests and + exposes a freeze-under-load hazard — when 8 candidates briefly formed, + premature island freezing turned marginal boxes into immovable + obstacles and max penetration exploded to 0.256 m before the wake veto + recovered. The remaining criterion-2 gap is contact-stream quality + (measured pair-set churn ~±10% per checkpoint breaking support + persistence and warm-starts), not sleep-policy thresholds. +- **Visual evidence (claim-tied)**: post-fix S6 GUI capture + `/tmp/wsg_postfix_20260731/S6_postfix_gui.png` via + `contact_benchmark --generate-container 71 --steps 20000 --collision + dart --gui-capture ` on DISPLAY :0; `image-verdict` non-blank + passes (1.04M nonzero pixels; the contrast sub-check is reported but + not gating). Text oracle: capture log ends `20000/20000` frames, + 161 contacts / 115 pairs, 5/71 resting — count-consistent with the + headless post-fix S6 endpoint (the GUI path prints no final-state + hash, so counts are the comparison). Visible observation (native semantic + inspection): a compacted mixed-shape pile — red boxes, green + cylinders/capsules, blue spheres — settled inside the translucent + gray static container; no interpenetration, wall tunneling, or + scattered bodies at visible scale. Reconciliation: image and text + oracle agree (plausible near-settled pile, not fully asleep). Not + proven by this image: sleep-state per body and sub-visible + penetration (covered by the headless telemetry). The capture also + corrects the S6 scene model: the pile is mixed-shape, not box-only, + which feeds the D10 rolling-body hypothesis. +- **WS-G box rows move both ways** (see 08-mujoco-comparison-lane.md): + ARM-PUSHER flips to a DART win (0.72x → 1.27x; MuJoCo moved only +4% + on that scene across runs while DART gained ~80% from the stabilized + sliding-box stream), PILE-900 remains a ≥~40x win (MuJoCo's + denominator is unstable at its collapse point, so do not read the 70x + point value as a widening), PILE-120 ratio unchanged (0.42–0.43x), + and DYN-STIR-120 flips to a loss (1.14x → 0.85x; MuJoCo +1% — the + stirred pile is perpetually active, so it pays the fourth-row cost + with no stability dividend). + +### 2026-08-01 cylinder contact stability (intermediate state — the +criterion-2 claims below were superseded the same day by the +spin-canonicalization section that follows) + +Stream-quality probes (identical constructed states swept in 50 µm steps +against both engines' dartpy builds; probe scripts + JSON in +`~/dart-wsg-evidence-20260731/`) measured two cylinder defects shared with +or worse than the old engine while every other pair was at parity or +better (old box-tilt even showed 1 mm point jumps and 180° normal flips +that the consolidated engine does not have): + +- **Cylinder side on a box face**: one contact point teleporting ~90 mm + along the contact line at every tilt (0 to 1e-2 rad) in BOTH engines — + the side-on case fell through to convex GJK/EPA, which returns one + arbitrary support point on the under-constrained line (root cause + independently confirmed by a Codex diagnosis lane). +- **Crossed cylinders (~90°)**: the consolidated engine intermittently + reported NO contact at ~1 mm penetration (3 of 41 sweep poses) — a + support that vanishes for single steps injects free-fall/impact noise. + +Fix (dart-detector narrowphase only, `CylinderCollision.cpp`): +`tryAddCylinderBoxSideLineContacts` emits a stable two-point line +manifold (segment clipped to the face slabs and the positive-penetration +interval, per-endpoint depths, ≤ ~3° face-parallel gate) before the +convex fallback, and `tryAddCrossedCylinderSideContact` handles +non-parallel cylinder pairs whose axis closest points are interior via +the exact capsule-equivalent closest-point contact. Pinned tests: +`SideOnFaceEmitsStableEndpointContacts`, +`TiltedSideOnFaceKeepsBothEndpointDepths`, +`CrossedCylindersKeepShallowContact` (30/30 cylinder suite; full suite +154/154). Post-fix probes: every cylinder sweep now shows 2 stable +contacts moving exactly with the commanded 50 µm steps (was 90 mm +jumps), zero crossed-pair misses, and box-on-cylinder stabilized too. + +Guard state on the complete bundle: S2/S3 dart unchanged +(`0x266da31836a314a6`, `0x6088ea0177efa6a` — cylinder-plane paths +untouched), S4/S5 dart unchanged from the manifold-fix rows, S1 rows +re-baseline with the added line contacts (`120: 290 contacts / hash +0x2ea14001d9c64a87`, `60: 85 / 0x96ee9d85aebfc7fa`), and the untouched +detectors stay bit-identical (S3_fcl, S4_ode, S5_bullet re-verified). + +**Criterion 1 resolved by direct measurement**: quiet-host interleaved +A/B (7 ABAB pairs, hash-stable arms), audit-head stack vs the complete +bundle on S1 120/dart/1: **8.404 vs 8.479 ms/step — 1.009x, parity** — +while the bundle's stream carries 290 vs 242 contacts (+20% more real +contact coverage). The earlier ~2x cost was the intermediate +manifold-only state: stable cylinder supports reduce trajectory-average +churn enough to recover it (intermediate interleaves: manifold-only +1.95x vs the 3-contact era; full bundle 1.21x vs the same arm; direct +vs audit 1.009x). Criterion 1 therefore holds at ≈3.5x of the round-2 +baseline (the audited 3.51x carries over at parity; the only +remaining cross-session link is that standing audit record itself, whose +trajectory today's audit arm reproduces bit-exactly). + +**S6 / criterion 2 final evidence**: the fixture is chaotically marginal +on every stack. Seed matrix (20000 steps, seeds 3056/101/202/303/404): +audit-era stack sleeps 3/5 (fails 202 and 404 with the same ~3 mm +limit cycle), manifold-only 1/5, full bundle ~1.5/5 (404 fully asleep, +303 at 70/71; canonical 3056 stays awake). Bounded penetration — the +original #3056 disease — holds on every run of every arm. A box-only +71-cube container pile (dartpy-authored) converges to genuine stillness +on the fixed stack (max velocity 5e-5–1.3e-3 m/s, penetration 1e-5 — +~100x quieter than the mixed pile's 2 cm/s) across all seeds, so the +contact stream is healthy; its island-atomic freeze did not latch within +40k steps in that dartpy construction (0.1-scale cubes), while +contact_benchmark-authored 71-body piles do freeze on sleeping seeds — +the precise remaining question is deactivation-latch behavior for large +single-island piles near true stillness, plus rolling friction as the +physical feature a mixed roller pile needs to rest deterministically. +Neither is a detector-stream defect; both are recorded under D10. + +### 2026-08-01 spin-canonicalization — the criterion-2 closer + +The evidence audit flagged that "bounded penetration" was over-asserted: +60k S6 runs on the intermediate effective-radius build +(`aug01/S6_60k_corrected_seed*.log`) showed penetration growing without +bound on seed 101 — 0.016 m at early checkpoints rising steadily to +0.137 m at 60k (~2.4–2.6 µm/step over the back half; the original #3056 +creep signature). Scene-dump +reconstruction identified the creeping body exactly: an **upright +cylinder standing on its flat end-cap with an arbitrary spin about its +own axis** (dump quaternion pure-z). The aligned analytic cap-patch path +in `collideCylinderBox` demanded exact rotational identity +(`isApprox(Identity, 1e-12)`), so a spun-but-upright cylinder — +physically identical to an unspun one (surface of revolution) — fell to +the convex fallback, whose degenerate rim points cannot support a loaded +cap: the cylinder sinks through its support without bound. + +Fix: detect a box axis parallel to the cylinder axis, canonicalize the +spin and axis permutation away, and reuse the existing aligned math +(`alignedBoxAxis` + spin-canonical frame). The side-line path was also +hoisted ahead of the aligned block (its lateral branch would otherwise +answer side-lying cylinders with one rocking point) and gated to +genuinely shallow poses (declines once an axis endpoint reaches the face +plane, keeping deep overlaps on the legacy minimal-translation paths). +The tilted-support effective-radius correction from the review lane +(`r*sqrt(1-dot^2)` support distance, pinned by +`TiltedSeparationIsNotFabricated`) rides in the same commit. Four stale +single-support assertions across three test bodies were modernized to +the line-manifold behavior (`Collision.DartCylinderFinitePrimitivePairs`, +`DARTCollisionDetector.CollidesCylinderBox`, +`CylinderCollision.CollidesCylinderBox`), and +`SpunUprightCapOnFaceUsesStablePatch` pins spin-invariance (spun and +unspun caps must emit identical world contacts). Cylinder suite 32/32; +full suite 154/154. + +**Outcome — the S6 pile now genuinely deactivates under defaults:** + +- 60k trend runs, seeds 3056 and 101 (the never-slept canonical and the + worst creeper): both end **71/71 resting, max penetration 0**. + Penetration stays in a bounded ~1–3.6 mm band with no growth trend + until the islands freeze (canonical 3056 freezes between 36k–38k + steps; both fully frozen well before 50k) — the growth mechanism is + gone. +- 5-seed 20k matrix on the final build: seeds 101/202/303/404 all end + **71/71 resting, penetration 0, zero contacts**; canonical 3056 ends + 1/71 with penetration 8.0e-4 still declining (sleep latency beyond + the 20 s window; full rest confirmed at 60k). The previously-accepted + audit-era stack managed 3/5 on the same matrix. +- Parity interleave (7 ABAB pairs, quiet host): current bundle median + **7.307 vs audit stack 7.725 ms/step — 0.946x, the bundle is now + slightly FASTER than the audited pre-consolidation stack** while + carrying fuller manifolds. Criterion 1 ≈ 3.7x of the round-2 baseline. +- Guard state: S2/S3/S4/S5 dart hashes unchanged from the + cylinder-stability rows (`0x266da31836a314a6`, `0x6088ea0177efa6a`, + `0x70bf5dd9e4f15051`, `0xd8de4ae15996321f`); S1 re-baselines to + `120: 290 contacts / 178 pairs / 0xfc20c4880fdbca05` and + `60: 88 / 74 / 0x6dab35ce2618d422`. The whole bundle is a two-file + dart-detector change set (`DARTCollisionDetector.cpp` clamp + + `CylinderCollision.cpp` narrowphase), so FCL/Bullet/ODE remain + structurally untouched (verified bit-identical on the earlier bundle + states). + +Known narrow boundaries (Codex re-review, recorded as accepted): the +axis-parallel gate's 1e-9 matrix tolerance corresponds to ~4.5e-5 rad, +so poses tilted just past it route to the legacy paths (both sides of +the boundary are previously-existing behaviors); and a cylinder whose +axis projects exactly onto a box edge can receive a one-face line +manifold (a pre-existing ambiguity class of the aligned path). + +Artifacts: `~/dart-wsg-evidence-20260731/aug01/` (60k trend logs +`S6_60k_canon_seed*.log` and the intermediate-build creep evidence +`S6_60k_corrected_seed*.log`, reval guard/seed/parity logs under +`effrad_reval/`, probe JSONs, seed matrices, scripts, and the seed-101 +final-scene dump + reconstruction diagnostic that identified the +creeping body). + ## Prior art — round-1 experiment branches (read before claiming packets) Six unpushed round-1 experiment branches were published to origin on diff --git a/docs/dev_tasks/dart6_performance_generalization/07-orchestration-dashboard.md b/docs/dev_tasks/dart6_performance_generalization/07-orchestration-dashboard.md index 77336cae8541a..d93e311abd725 100644 --- a/docs/dev_tasks/dart6_performance_generalization/07-orchestration-dashboard.md +++ b/docs/dev_tasks/dart6_performance_generalization/07-orchestration-dashboard.md @@ -12,15 +12,25 @@ Related open queue at last refresh: none blocking (enablers merged: #3270 was closed by maintainer direction and its D1/D2 evidence now rides with the first real SIMD-kernel PR. -Current handoff (2026-07-10): the completion audit ran (see RESUME.md — -criteria 1-3 MET on the merged head). The maintainer broadened criterion 4 to -cross-engine evidence vs MuJoCo across DART's major workloads; lane **WS-G** -(08-mujoco-comparison-lane.md) owns that work. #3366 (dartpy getDofs ownership -bugfix) and #3367 (MuJoCo comparison harness + mujoco env + compiled dartpy -binding), #3368 (`dart` detector AABB-tree broadphase), and #3369 (MJCF -stacked joints and collision fidelity) have merged. The `dart` detector -small-scene overhead WP-SS family remains in flight; the S6 `dart` -resting-profile row is resolved by the accepted completion audit. +Current handoff (2026-07-31): the current-base re-baseline ran on +`718651d0d6e` (session branch `wp-pg-wsg-rebaseline-20260731`; see the +2026-07-31 sections of RESUME.md and 01-baseline-evidence.md). Criterion 1 +was first re-verified MET on the pre-fix base (1.29x faster than the +audited stack, same-host A/B). **Criterion 2 regressed**: the #3381 +consolidated `dart` detector's solver-facing 3-contact clamp breaks +resting box stacks — S6 0/71 under defaults, while FCL, the audit-head +binary, and the pre-#3381 binary all still sleep 71/71 (the latter two +bit-exact `0xec80f734df6d5e74`). The WP-PG.50 candidate fix (full +4-contact manifolds, pinned tests, 154/154, changelog drafted) is +implemented and evidence-complete on the session branch but **gated on +maintainer decisions D9 (ship/hold: resting scenes 4x faster and +ARM-PUSHER flips to a win, vs ~2x on the always-active dense fixture and +criterion 1 falling to ≈2.3x) and D10 (residual S6 stream-persistence +follow-up)** — see README. The full 8-scene WS-G matrix ran pre-fix +(first-ever HUM rows) plus a post-fix box-row rerun; standings live in +08-mujoco-comparison-lane.md. The 2026-07-10 audit context below remains +history: #3366/#3367/#3368/#3369 merged; the WP-SS small-scene family +remains gated on the refreshed standings. ## Lane status @@ -58,6 +68,7 @@ resting-profile row is resolved by the accepted completion audit. | WP-PG.40 FP/ISA contracts | WS-D | folded into WP-PG.42 | #3270 closed | maintainer direction: carry D1/D2 evidence with actual SIMD kernel PR | | WP-PG.41 batch math seam | WS-D | blocked (PG.10 seam evidence) | — | — | | WP-PG.42 SoA broadphase | WS-D | done — PR #3299 | `wp-pg-42-soa-broadphase-simd` | AVX-width finite sweep SIMD screen, scalar/SSE/NEON fallback, SIMD CI consumer coverage, contact-container macro rows, finite-finite profile scope | +| WP-PG.50 detector stream-quality bundle (manifolds + cylinder stability + spin-invariant cap patches) | WS-F/WS-A | in review — PR #3428 open; D9 SHIP decided 2026-08-01; criterion 2 MET | `wp-pg-wsg-rebaseline-20260731` / [#3428](https://github.com/dartsim/dart/pull/3428) | Full 4-contact face manifolds, stable two-point cylinder side-line contacts (effective-radius-corrected), retained shallow crossed-cylinder contacts, and spin/permutation-canonicalized aligned cylinder-box handling, all pinned by tests (154/154; cylinder suite 32/32). S6 fully deactivates: 71/71 pen 0 at 60k on canonical + worst-creeper seeds, 4/5 seeds within the 20k window (audit stack 3/5); direct S1 interleave 0.946x vs the audited pre-consolidation stack; fcl/bullet/ode bit-identical; S2-S5 dart hashes stable. Evidence: 01-baseline-evidence.md 2026-07-31/08-01 sections; decisions in README D9/D10 | Claim flow: set the packet row to `claimed — ` with the `wp-pg--` branch name, update RESUME.md, and open the packet PR diff --git a/docs/dev_tasks/dart6_performance_generalization/08-mujoco-comparison-lane.md b/docs/dev_tasks/dart6_performance_generalization/08-mujoco-comparison-lane.md index cbb151bbe435e..16fc2c95cad79 100644 --- a/docs/dev_tasks/dart6_performance_generalization/08-mujoco-comparison-lane.md +++ b/docs/dev_tasks/dart6_performance_generalization/08-mujoco-comparison-lane.md @@ -39,24 +39,92 @@ two HUM rows. `--dart-sleep off` makes every active comparison independent of deactivation. Do not accept the cross-engine matrix unless both HUM rows run; a parser or runtime failure remains a blocked row rather than evidence. -## Standings (2026-07-10 prototype rows; quiet-host full matrix pending) - -| Class | Scene | DART (`dart` detector) | MuJoCo | Verdict | -| --- | --- | ---: | ---: | --- | -| Arms | reacher | 0.0200 | 0.0086 | MJ 2.3x (µs-scale) | -| Arms | pusher | 0.0373 | 0.0065 | MJ 5.7x | -| Arms | striker/thrower | — | — | scenario registration + merged-base rerun pending | -| Humanoid | humanoid.xml | — | — | merged-base HUM-FALL/HUM-ACTIVE rerun pending | -| Many objects | PILE-120 active | 1.55 ms/step | 0.57 | MJ ~2.7x | -| Dynamic | DYN-STIR-120 | 1.82 | 0.61 | MJ ~3.0x | -| Sleeping | settled 3k | 0.034 ms/step (deactivation) | ~2 (no sleeping exists) | **DART ~60x** (to be formalized) | - -The prototype ant row is deferred: the merged orchestrator does not register -an ant scenario, and no exact direct-runner command, artifact, and engine SHAs -were retained for the earlier measurement. Do not treat that result as accepted -evidence; restore a locomotion row only with reproducible provenance. - -## Gap analysis -> packets +## Standings (2026-07-31 full matrix on `718651d0d6e`, pre-manifold-fix) + +First complete 8-scene run (both HUM rows ran and stayed finite — the +#3369 stacked-joint parsing works). Provenance: session branch +`wp-pg-wsg-rebaseline-20260731`, MuJoCo 3.10.0 (conda-forge, Python +3.14.6), Euler-normalized headline, `--reps 5` medians, `--detector +dart`, `--dart-sleep off`, single-threaded both engines, seeded +identical scenes, finite + contact telemetry recorded per rep. Artifacts ++ exact commands: `/tmp/mj_cmp_20260731` (`results.json`, `results.md`, +`raw/`, `provenance.txt`). Host in powersave with large clock swings, so +cross-session absolute steps/s are not comparable; the DART/MuJoCo ratio +per row is same-session and fair. + +| Class | Scene | DART steps/s | MuJoCo steps/s | Ratio | Verdict | +| --- | --- | ---: | ---: | ---: | --- | +| Arms | ARM-REACHER | 72208 | 61039 | 1.18x | **DART wins** | +| Arms | ARM-PUSHER | 19985 | 27589 | 0.72x | DART loses | +| Humanoid | HUM-FALL | 8791 | 11384 | 0.77x | DART loses | +| Humanoid | HUM-ACTIVE | 13784 | 15724 | 0.88x | DART loses (near band) | +| Many objects | PILE-120 | 280 | 673 | 0.42x | DART loses | +| Many objects | PILE-900 | 123 | 3.1 | **39.9x** | **DART wins** (MuJoCo collapses at 900 bodies) | +| Dynamic | DYN-STIR-120 | 773 | 676 | 1.14x | **DART wins** | +| Reference | FFI-OVERHEAD | 343461 | 568478 | 0.60x | not scored (per-step FFI floor) | + +MuJoCo model-default-integrator sensitivity rows (RK4 for the MJCF +scenes) ran and are slower than the Euler headline for MuJoCo, so the +Euler normalization is conservative toward MuJoCo. Deltas vs the +2026-07-10 prototypes: REACHER flipped to a DART win (was 2.3x behind), +PUSHER narrowed 5.7x → 1.4x behind, DYN-STIR flipped to a DART win (was +3.0x behind). The scored classes DART still loses: PUSHER (small-scene +per-step overhead, WP-SS family), HUM-FALL/HUM-ACTIVE (close), and +PILE-120 — the worst gap, measured on the pre-fix detector whose +3-point box manifolds keep piles rocking (see the criterion-2 regression +in 01-baseline-evidence.md); re-run the pile rows after the manifold +fix before cutting further pile packets. + +### Post-manifold-fix partial rerun (2026-07-31, same session) + +The box-manifold candidate fix (see 01-baseline-evidence.md) changes the +`dart` detector's stream in exactly the box scenes, so those rows were +rerun post-fix (`/tmp/mj_cmp_postfix_20260731`, reps 5, sensitivity +skipped; ARM-REACHER and the HUM rows carry over — no box-box face +stacking in those scenes): + +| Scene | DART steps/s | MuJoCo steps/s | Ratio | Verdict (vs pre-fix run) | +| --- | ---: | ---: | ---: | --- | +| ARM-PUSHER | 36535 | 28771 | **1.27x** | **DART wins** (was 0.72x loss) | +| PILE-120 | 436 | 1025 | 0.43x | loses (ratio unchanged, 0.42–0.43x) | +| PILE-900 | 189 | 2.7 | ~70x | **DART wins** (was ~40x; see caveat) | +| DYN-STIR-120 | 584 | 685 | 0.85x | loses (was 1.14x win) | + +(Displayed ratios come from the unrounded per-rep medians in +`results.json`, not the rounded steps/s columns.) Cross-run absolutes +are host-state-relative per the preamble, so per-scene deltas are +judged by whether MuJoCo's own number moved: MuJoCo is stable on +ARM-PUSHER (+4%) and DYN-STIR (+1%), so those verdict flips are +DART-side and real — the stabilized 4-point sliding-box stream wins +PUSHER outright, while the perpetually stirred DYN-STIR pile pays the +fourth-row solver cost with no stability dividend. On the PILE rows +MuJoCo itself moved (+52% / −13%), so treat PILE-120 by its unchanged +ratio and PILE-900 as "remains a ≥~40x DART win with an unstable +MuJoCo denominator at its collapse point", not as a widened margin. +Net scored standings post-fix: 3 wins (REACHER, PUSHER, PILE-900) / +4 losses (HUM-FALL, HUM-ACTIVE, PILE-120, DYN-STIR-120) — same count +as pre-fix, different composition. The maintainer ship/hold decision on +the manifold fix (README D9) picks which composition the branch +carries. + +Sleeping class: the harness matrix runs with `--dart-sleep off` by +design, so the deactivation advantage is evidenced by the guard rows +instead: current-base `S2_dart` steps the settled 3k-shapes scene with +3003/3003 skeletons resting at ~0.04 ms/step in the 2026-07-31 capture +(`/tmp/wsg_rebaseline_guards_20260731/summary.tsv`; timing host-state +relative per 01-baseline-evidence.md), while MuJoCo has no sleeping +concept and pays full active cost on settled scenes. A formal same-scene +DART-sleep-on vs MuJoCo row remains future work; do not quote the old +informal "~60x" figure without capturing that comparison +reproducibly. + +The prototype ant row remains deferred: the merged orchestrator does not +register an ant scenario, and no exact direct-runner command, artifact, +and engine SHAs were retained for the earlier measurement. Do not treat +that result as accepted evidence; restore a locomotion row only with +reproducible provenance. + +## Gap analysis -> packets (figures predate the 2026-07-31 standings; superseded where they conflict) 1. **MJCF stacked-joint and collision fidelity**: #3369 merged stacked hinge/slide support, contype/conaffinity filtering, and per-geom friction. diff --git a/docs/dev_tasks/dart6_performance_generalization/README.md b/docs/dev_tasks/dart6_performance_generalization/README.md index b7c7a59803814..c493db803725c 100644 --- a/docs/dev_tasks/dart6_performance_generalization/README.md +++ b/docs/dev_tasks/dart6_performance_generalization/README.md @@ -247,6 +247,47 @@ PR branches. Claim packets by marking the dashboard row and RESUME.md. the remaining decision is whether to pursue a behavior-changing packet on the ODE/FCL wrappers. Deciding *not* to do it now must be recorded as a decision, not an omission. +- **D9 — DECIDED 2026-08-01 (maintainer-delegated, evidence-based): SHIP + the detector stream-quality bundle** — full 4-contact face manifolds, + stable two-point cylinder side-line contacts (with the tilted-support + effective-radius correction), retained shallow crossed-cylinder + contacts, and spin-invariant aligned cylinder-box handling, as one + cohesive WP-PG.50 packet on `wp-pg-wsg-rebaseline-20260731`. Rationale: + every piece removes performance-from-lost-or-unstable-contacts, which + the `docs/design/dart6_collision_backends.md` correctness clause + forbids; the complete bundle measures **0.946x vs the audited + pre-consolidation stack on the primary fixture (direct quiet-host + interleave — slightly faster)** with fuller manifolds, keeps criterion + 1 at ≈3.7x (≥ 3x bar; the only cross-session link is the standing + audit record, whose trajectory the audit arm reproduces bit-exactly), + flips ARM-PUSHER to a cross-engine win, settles S4/S5 4x faster, and + leaves FCL/Bullet/ODE bit-identical. Evidence: + 01-baseline-evidence.md 2026-07-31/08-01 sections. +- **D10 — DECIDED 2026-08-01, superseded for the better on the same day: + criterion 2 is MET on its original terms.** The root cause of the pile + never resting was a chain of detector stream defects, the last being + the aligned cap-patch path demanding exact rotational identity so + spun-but-upright cylinders fell to degenerate convex rim points and + crept without bound (measured on the intermediate build: seed 101 rising from ~0.016 m to + 0.137 m across a 60k run). With the full bundle, S6 ends **71/71 + resting with max penetration 0** on 60k runs of both the canonical + seed and the worst creeper, and 4/5 seeds fully deactivate within the + original 20k window (the audit-era stack managed 3/5); canonical 3056 + freezes between 36k–38k steps (sleep latency; penetration stays in a + bounded ~1–3.6 mm band with no growth trend until the freeze). An intermediate re-anchoring of criterion 2 (recorded in + this entry's history via git) proved unnecessary once the final defect + fell. Measured-and-rejected alternative: raising the dense-island rest + tolerance (premature island freezing under load explodes penetration + to 0.256 m — a freeze-under-load hazard to remember). Non-blocking + follow-ups recorded: deactivation-latch latency for large + single-island piles near stillness (a dartpy-authored 0.1-scale + box-only pile converges to 5e-5–1.3e-3 m/s without latching in 40k + steps while contact_benchmark-authored piles do latch), rolling + friction as a physical feature for roller-heavy scenes, and + parallel-line single-point contacts (cyl-cyl/capsule pairs) as the + remaining 1-point supports. Evidence: 01-baseline-evidence.md + "2026-08-01 spin-canonicalization"; seed matrices and 60k trends in + `~/dart-wsg-evidence-20260731/aug01/`. ## Closeout plan (promotion targets, decided up front) diff --git a/docs/dev_tasks/dart6_performance_generalization/RESUME.md b/docs/dev_tasks/dart6_performance_generalization/RESUME.md index 40db7433248f5..4704f37c362d8 100644 --- a/docs/dev_tasks/dart6_performance_generalization/RESUME.md +++ b/docs/dev_tasks/dart6_performance_generalization/RESUME.md @@ -9,6 +9,96 @@ packet that overlaps the `origin/perf/dart6-*` experiment branches. ## Next packets +**2026-07-31: current-base re-baseline RAN on `718651d0d6e`** (session +branch `wp-pg-wsg-rebaseline-20260731`; guard artifacts +`/tmp/wsg_rebaseline_guards_20260731`, A/Bs `/tmp/wsg_ab_20260731`, +WS-G rerun `/tmp/mj_cmp_20260731`). Read the new +"2026-07-31 current-base guard refresh" section of +[01-baseline-evidence.md](01-baseline-evidence.md) first. Summary: + +- Criterion 1 (S1 primary fixture): **MET, improved** — same-host + interleaved A/B shows the current stack 1.29x faster than the + audit-head stack on S1 120/dart/1 (chained ≈4.5x over the round-2 + baseline). July step-time cells are not comparable to today's host + clock state; an apparent 1.6–6x slowdown was a host artifact, refuted + by ABAB A/Bs with bit-identical per-arm hashes. +- Criterion 2 (S6 pile-sleep): **REGRESSED — the selected gap.** The + #3381 consolidated `dart` detector's box-box stream (≤3-point + manifolds, pair churn, penetration plateau ~4.4 mm) keeps the pile + above the wake band forever; 0/71 resting under defaults. FCL control, + audit-head binary, and pre-#3381 binary all still sleep 71/71 (the + latter two bit-exact `0xec80f734df6d5e74`), isolating the #3381 + detector swap as the sole cause. Policy knobs do not rescue it. +- Criterion 3 (no regressions elsewhere): fcl/bullet/ode guard rows held + bit-identical (or were re-established where July references were + lost); all `dart` rows re-baselined per the #3381 PR body's + breaking-changes note ("its contact profile can differ"); S2/S3 dart + now coincide with FCL fixed points. +- WS-G: the full 8-scene matrix (both HUM rows included) ran on the + merged base with provenance, plus a post-fix box-row rerun; standings + live in [08-mujoco-comparison-lane.md](08-mujoco-comparison-lane.md). + +Implementation packet executed by this session (WP-PG.50 candidate, +checkpoint commit `03046383e77` on `wp-pg-wsg-rebaseline-20260731`; +note: the first two commits were pushed to origin on 2026-08-01 outside +the session — the later commits remain local): the solver-facing +3-contact clamp in `DARTCollisionDetector.cpp` was raised to the full +4-contact manifold capacity, with pinned regression tests and a +CHANGELOG entry (`#PENDING` link — fill at PR time). Full evidence and +the measured trade-off live in the "2026-07-31 manifold fix" section of +[01-baseline-evidence.md](01-baseline-evidence.md): resting scenes +improve up to 4x and FCL/Bullet/ODE stay bit-identical, but the +always-active dense fixture pays ~2x (criterion 1 falls to ≈2.3x, +below the 3x bar), and S6 improves (5/71 at 20k, dwell accumulating) +without fully sleeping — the residual blocker is contact-stream +persistence (pair churn), not sleep policy (a tolerance raise was +measured and rejected: it triggers freeze-under-load penetration +explosions). 154/154 C++ tests pass; lint/check-lint clean. + +**2026-08-01 final — D9 and D10 DECIDED and criterion 2 MET (the +maintainer delegated both calls in-session on 2026-08-01 with a +root-cause, evidence-based, A/B-verified mandate; full records in README +"Open decisions").** The session measured old-vs-new contact streams on +constructed micro-pose sweeps, then fixed four detector stream defects +as one bundle: the 3-contact solver-facing clamp, wandering cylinder +side-line points (2-point line manifolds + effective-radius correction), +intermittently missed crossed-cylinder contacts, and — the criterion-2 +closer — spin-variant aligned cylinder-box handling (an upright cylinder +with arbitrary spin fell off the stable cap-patch path onto degenerate +convex rim points and crept without bound; found via a 60k trend audit ++ final-scene reconstruction, fixed by spin/permutation +canonicalization). Final state: S6 sleeps **71/71 with penetration 0** +on 60k runs of the canonical seed and the worst creeper; 4/5 seeds +fully deactivate within the original 20k window (audit-era stack: 3/5); +the bundle is **0.946x vs the audited pre-consolidation stack** +(slightly faster) on the direct S1 interleave; 154/154 C++ tests, +cylinder suite 32/32; S2-S5 dart guard hashes stable across the last +two fix iterations; S1 re-baselined +(`120: 290/178 0xfc20c4880fdbca05`, `60: 88/74 0x6dab35ce2618d422`). +Codex provided the independent cylinder root-cause confirmation and the +code-review lane whose effective-radius finding was verified and +applied. Work is preserved as LOCAL commits on +`wp-pg-wsg-rebaseline-20260731` (never pushed; push/PR needs explicit +approval); raw artifacts and scripts are archived at +`~/dart-wsg-evidence-20260731/` including the `aug01/` tree. Next +action: shepherd PR #3428 through CI and review per `dart-manage-pr` +(no thread replies to AI reviewers; local fixes + re-review requests +only with approval), then retire this folder in the completing PR per +the closeout plan. CI classification (2026-08-02): the `gcc (newest)` +and `clang (newest)` toolchain lanes fail +`MemoryDiagnostics.DenseMapStaysBelowTheOpenGL2DrawIndexLimit` +identically on the release-6.20 tip itself (introduced by the #3379 +demos merge, ImGui draw-list synthetic test, `drawListCount == 0`; the +tip's own CI Toolchain run 30706341273 shows both jobs failing while +the run reports success — non-required lanes). Pre-existing and +unrelated to this PR; do not chase it here. Resolution: the maintainer +fixed it upstream the same day (#3429, "Read ImGui draw-list count from +CmdLists.Size") and merged the current base into the PR branch; the full +hosted matrix then concluded GREEN on the merge head `4be23b49b39` +(22 pass + 1 skip of 23 checks, zero failures, PR mergeable, +2026-08-02). Remaining: maintainer review and merge; then retire this +folder in the completing PR per the closeout plan. + **2026-07-10: the current-head completion audit RAN** (release-6.20 @ `db255a08e8e`; artifacts `/tmp/audit_head_20260710T011207Z`): @@ -146,6 +236,64 @@ option-off/option-on evidence. ## Session log (round-2 execution) +- 2026-08-01 (later): The evidence audit caught "bounded penetration" + over-asserted — 60k trends showed seed-101 penetration GROWING + 0.100 → 0.137 m. Final-scene reconstruction identified an upright + spun cylinder creeping through its cap support: the aligned cap-patch + path demanded exact rotational identity, so spun uprights fell to + degenerate convex rim points. Fixed by spin/permutation + canonicalization of the aligned cylinder-box path (+ side-line hoist + with a shallow-pose gate); four stale single-support test pins + modernized to line manifolds; spin-invariance pinned. Outcome: S6 + 71/71 pen 0 at 60k on both probe seeds, 4/5 seeds sleeping within + 20k, parity interleave 0.946x (bundle faster than the audited stack) + — criterion 2 met on original terms, D10's re-anchoring superseded. +- 2026-08-01: Maintainer delegated D9/D10 with a root-cause, + evidence-based mandate. Stream-quality probe suite (micro-pose sweeps + on constructed states, old vs new dartpy builds) showed parity or + new-better everywhere except two cylinder defects: a side-on-face line + contact wandering ~90 mm at every tilt (both engines; GJK fallback + root cause, independently confirmed by a Codex lane) and new-only + intermittent misses of shallow crossed-cylinder contacts. Implemented + `tryAddCylinderBoxSideLineContacts` (clipped two-point line manifold, + per-endpoint depths) and `tryAddCrossedCylinderSideContact` + (capsule-equivalent interior closest-point contact) with three pinned + tests; cylinder suite 30/30, full suite 154/154, untouched detectors + bit-identical, S2/S3 dart hashes unchanged, S1 re-baselined. Direct + quiet-host interleave vs the audit stack: **1.009x parity** (the + earlier ~2x was the manifold-only intermediate) → criterion 1 ≈3.5x + MET → D9 = SHIP the bundle. 5-seed S6 matrix on three stacks proved + the mixed-pile all-resting outcome seed-chaotic everywhere (audit + stack 3/5, bundle ~1.5/5, bounded penetration universal); a + dartpy-authored box-only pile converges ~100x quieter than the mixed + pile but its island freeze does not latch in 40k steps → D10 = + re-anchor criterion 2 (bounded penetration + pinned stream tests) with + bounded follow-ups (deactivation latch, rolling friction, + parallel-line 1-point contacts). Codex was available again and used + for the cylinder diagnosis lane. +- 2026-07-31: Full re-baseline on `718651d0d6e` (branch + `wp-pg-wsg-rebaseline-20260731`). S1–S6 guard matrix re-established with + drift classification (fcl/bullet/ode held bit-identical; all `dart` rows + re-baselined per the #3381 PR body's contact-profile note; S2/S3 dart land + on FCL fixed points). Criterion-2 regression found and root-caused: the + consolidated detector's solver-facing 3-contact clamp breaks resting + face-face support; attribution chain closed by bit-exact S6 reproduction + (`0xec80f734df6d5e74`) on both the audit-head and pre-#3381 binaries and + an FCL control that sleeps 71/71 on the current base. An apparent broad + wall-time regression was refuted as host clock-state artifact via + interleaved A/Bs (hashes bit-identical per arm). First complete 8-scene + WS-G matrix ran (HUM rows first-ever; 3 wins / 4 losses pre-fix). + WP-PG.50 candidate implemented: solver-facing manifold clamp 3 → 4 with + pinned tests (154/154), changelog draft, S6 GUI capture, and a measured + trade-off (resting scenes 4x faster, ARM-PUSHER flips to a win; dense + always-active fixtures ~2x slower, DYN-STIR flips to a loss; S6 partial + recovery only). Tolerance-raise alternative measured and rejected + (freeze-under-load penetration explosion). Ship/hold gated on D9; S6 + stream-persistence follow-up scoped as D10. Review pass 1 (fresh-context + correctness lane) returned no blockers; findings applied. Codex was + unavailable this session (weekly limit), so review lanes are + role-separated local subagents — recorded as a limitation. + - 2026-07-04/05: WP-PG.01 executed on `wp-pg-01-baseline-evidence`: original matrix/profile/dashboard capture on `origin/release-6.20` @ `5bee91ad6be`, then current-base guard refresh on diff --git a/docs/plans/dashboard.md b/docs/plans/dashboard.md index 5dc76b997ee98..1e2fa6557bbeb 100644 --- a/docs/plans/dashboard.md +++ b/docs/plans/dashboard.md @@ -12,10 +12,16 @@ Priority order is document order. Active implementation handoff remains in - Status: Active - Horizon: Now - Dimension: Performance, determinism, and Gazebo/gz-sim compatibility. -- Next step: Re-baseline the WS-G cross-engine matrix and `dart` detector - rows on the current merged base, then use that evidence to select one - consolidated implementation gap or the closeout route. Keep the task active - while #3056 remains open. +- Next step: Land [WP-PG.50 PR #3428](https://github.com/dartsim/dart/pull/3428) + through CI and review: the detector stream-quality bundle restores + full manifolds and stable cylinder contacts, **meets criterion 2 on + its original terms** (S6 fully deactivates with zero penetration; 4/5 + seeds within the 20-second window, all tested seeds by 60 s), and + measures slightly faster than the audited pre-consolidation stack + (criterion 1 ≈3.7x). D9/D10 records live in the task README; the + 2026-07-31 re-baseline and first full WS-G matrix (incl. HUM rows) + are in the task folder. Keep the task active while #3056 remains + open. - Gate: `pixi run lint`; capped C++ build; detector-specific final-state hash guards; benchmark evidence in the task-required report shape; `pixi run -e gazebo test-gz` for collision, solver, or `World::step` diff --git a/tests/integration/test_Collision.cpp b/tests/integration/test_Collision.cpp index be2c37e48ef0e..caa41256223aa 100644 --- a/tests/integration/test_Collision.cpp +++ b/tests/integration/test_Collision.cpp @@ -1448,7 +1448,7 @@ TEST_F(Collision, DartParallelFinitePlaneFilterPublishesForFinitePairs) // DartPerPairContactCapSelectsDeepSpreadContacts was removed with the legacy // detector: the deep+spread per-pair cap backfill it probed belonged to the // deleted legacy pipeline; the consolidated engine reduces each pair manifold -// upstream (ContactReduction, solver-facing target 3), covered by +// upstream (ContactReduction, full solver-facing manifold target), covered by // UNIT_collision_dart_box_box and DartPerPairContactCapCoalescesNear- // DuplicatePairContacts. @@ -1802,26 +1802,36 @@ TEST_F(Collision, DartCylinderFinitePrimitivePairs) CollisionResult cylinderBox; ASSERT_TRUE(cylinderGroup->collide(boxGroup.get(), option, &cylinderBox)); - ASSERT_EQ(cylinderBox.getNumContacts(), 1u); - EXPECT_EQ( - cylinderBox.getContact(0).collisionObject1->getShapeFrame(), - cylinderFrame.get()); - EXPECT_EQ( - cylinderBox.getContact(0).collisionObject2->getShapeFrame(), - boxFrame.get()); - EXPECT_TRUE(cylinderBox.getContact(0).point.allFinite()); - EXPECT_TRUE(cylinderBox.getContact(0).normal.allFinite()); - EXPECT_GT(cylinderBox.getContact(0).penetrationDepth, 0.0); + // Side-on-face is a contact line; both clipped endpoints are emitted as + // DISTINCT supports. + ASSERT_EQ(cylinderBox.getNumContacts(), 2u); + EXPECT_GT( + (cylinderBox.getContact(0).point - cylinderBox.getContact(1).point) + .norm(), + 0.1); + for (std::size_t i = 0; i < cylinderBox.getNumContacts(); ++i) { + EXPECT_EQ( + cylinderBox.getContact(i).collisionObject1->getShapeFrame(), + cylinderFrame.get()); + EXPECT_EQ( + cylinderBox.getContact(i).collisionObject2->getShapeFrame(), + boxFrame.get()); + EXPECT_TRUE(cylinderBox.getContact(i).point.allFinite()); + EXPECT_TRUE(cylinderBox.getContact(i).normal.allFinite()); + EXPECT_GT(cylinderBox.getContact(i).penetrationDepth, 0.0); + } CollisionResult boxCylinder; ASSERT_TRUE(boxGroup->collide(cylinderGroup.get(), option, &boxCylinder)); - ASSERT_EQ(boxCylinder.getNumContacts(), 1u); - EXPECT_TRUE(boxCylinder.getContact(0).normal.isApprox( - -cylinderBox.getContact(0).normal, 1e-8)); - EXPECT_NEAR( - boxCylinder.getContact(0).penetrationDepth, - cylinderBox.getContact(0).penetrationDepth, - 1e-8); + ASSERT_EQ(boxCylinder.getNumContacts(), 2u); + for (std::size_t i = 0; i < boxCylinder.getNumContacts(); ++i) { + EXPECT_TRUE(boxCylinder.getContact(i).normal.isApprox( + -cylinderBox.getContact(0).normal, 1e-8)); + EXPECT_NEAR( + boxCylinder.getContact(i).penetrationDepth, + cylinderBox.getContact(0).penetrationDepth, + 1e-8); + } boxFrame->setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0)); cylinderBox.clear(); @@ -1831,33 +1841,42 @@ TEST_F(Collision, DartCylinderFinitePrimitivePairs) boxFrame->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); cylinderBox.clear(); ASSERT_TRUE(cylinderGroup->collide(boxGroup.get(), option, &cylinderBox)); - ASSERT_EQ(cylinderBox.getNumContacts(), 1u); - EXPECT_NEAR(cylinderBox.getContact(0).penetrationDepth, 0.0, 1e-12); + // Touching side-on-face cases also carry the two clipped line endpoints. + ASSERT_EQ(cylinderBox.getNumContacts(), 2u); + for (std::size_t i = 0; i < cylinderBox.getNumContacts(); ++i) { + EXPECT_NEAR(cylinderBox.getContact(i).penetrationDepth, 0.0, 1e-12); + } boxCylinder.clear(); ASSERT_TRUE(boxGroup->collide(cylinderGroup.get(), option, &boxCylinder)); - ASSERT_EQ(boxCylinder.getNumContacts(), 1u); - EXPECT_TRUE(boxCylinder.getContact(0).normal.isApprox( - -cylinderBox.getContact(0).normal, 1e-8)); - EXPECT_NEAR(boxCylinder.getContact(0).penetrationDepth, 0.0, 1e-12); + ASSERT_EQ(boxCylinder.getNumContacts(), 2u); + for (std::size_t i = 0; i < boxCylinder.getNumContacts(); ++i) { + EXPECT_TRUE(boxCylinder.getContact(i).normal.isApprox( + -cylinderBox.getContact(0).normal, 1e-8)); + EXPECT_NEAR(boxCylinder.getContact(i).penetrationDepth, 0.0, 1e-12); + } boxFrame->setTranslation(Eigen::Vector3d::Zero()); cylinderFrame->setTranslation(Eigen::Vector3d(-1.0, 0.25, 0.0)); boxCylinder.clear(); ASSERT_TRUE(boxGroup->collide(cylinderGroup.get(), option, &boxCylinder)); - ASSERT_EQ(boxCylinder.getNumContacts(), 1u); - EXPECT_TRUE(boxCylinder.getContact(0).point.allFinite()); - EXPECT_TRUE(boxCylinder.getContact(0).normal.allFinite()); - EXPECT_NEAR(boxCylinder.getContact(0).normal.norm(), 1.0, 1e-12); - EXPECT_NEAR(boxCylinder.getContact(0).penetrationDepth, 0.0, 1e-12); + ASSERT_EQ(boxCylinder.getNumContacts(), 2u); + for (std::size_t i = 0; i < boxCylinder.getNumContacts(); ++i) { + EXPECT_TRUE(boxCylinder.getContact(i).point.allFinite()); + EXPECT_TRUE(boxCylinder.getContact(i).normal.allFinite()); + EXPECT_NEAR(boxCylinder.getContact(i).normal.norm(), 1.0, 1e-12); + EXPECT_NEAR(boxCylinder.getContact(i).penetrationDepth, 0.0, 1e-12); + } cylinderBox.clear(); ASSERT_TRUE(cylinderGroup->collide(boxGroup.get(), option, &cylinderBox)); - ASSERT_EQ(cylinderBox.getNumContacts(), 1u); - EXPECT_TRUE(cylinderBox.getContact(0).normal.isApprox( - -boxCylinder.getContact(0).normal, 1e-8)); - EXPECT_NEAR(cylinderBox.getContact(0).penetrationDepth, 0.0, 1e-12); + ASSERT_EQ(cylinderBox.getNumContacts(), 2u); + for (std::size_t i = 0; i < cylinderBox.getNumContacts(); ++i) { + EXPECT_TRUE(cylinderBox.getContact(i).normal.isApprox( + -boxCylinder.getContact(0).normal, 1e-8)); + EXPECT_NEAR(cylinderBox.getContact(i).penetrationDepth, 0.0, 1e-12); + } cylinderFrame->setTranslation(Eigen::Vector3d::Zero()); cylinder2Frame->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0)); @@ -3008,9 +3027,10 @@ TEST_F(Collision, Options) #endif auto dart = DARTCollisionDetector::create(); - // The dart detector reduces the face-face manifold to its solver-facing - // target of 3 contacts. - testOptions(dart, 3u); + // Solver-facing queries carry the full four-contact face manifold: a + // three-contact tripod cannot hold a resting face-face box stack + // (issue #3056 S6 pile fixture). + testOptions(dart, 4u); } //============================================================================== diff --git a/tests/unit/collision/dart/test_cylinder_collision.cpp b/tests/unit/collision/dart/test_cylinder_collision.cpp index bafdfd926a96d..c281d11d2dde4 100644 --- a/tests/unit/collision/dart/test_cylinder_collision.cpp +++ b/tests/unit/collision/dart/test_cylinder_collision.cpp @@ -36,8 +36,13 @@ #include +#include #include +#include #include +#include + +#include using namespace dart::collision::native; @@ -50,6 +55,14 @@ Eigen::Isometry3d translated(double x, double y, double z) return tf; } +Eigen::Isometry3d rotatedAroundX(double radians) +{ + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.linear() + = Eigen::AngleAxisd(radians, Eigen::Vector3d::UnitX()).toRotationMatrix(); + return tf; +} + Eigen::Isometry3d rotatedAroundY(double radians) { Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); @@ -298,10 +311,24 @@ TEST(CylinderCollision, CollidesCylinderBox) result); ASSERT_TRUE(hit); - ASSERT_EQ(1u, result.numContacts()); - EXPECT_TRUE( - result.getContact(0).normal.isApprox(-Eigen::Vector3d::UnitX(), 1e-12)); - EXPECT_NEAR(0.375, result.getContact(0).position.x(), 1e-12); + // The vertical side-on-face contact is a line clipped to the box face's + // z-extent; both endpoints are emitted as stable supports (previously a + // single rocking point). + ASSERT_EQ(2u, result.numContacts()); + double minZ = result.getContact(0).position.z(); + double maxZ = result.getContact(1).position.z(); + if (minZ > maxZ) { + std::swap(minZ, maxZ); + } + // The side-line clip applies a 1e-9 slab tolerance by design. + EXPECT_NEAR(-0.5, minZ, 2e-9); + EXPECT_NEAR(0.5, maxZ, 2e-9); + for (std::size_t i = 0; i < result.numContacts(); ++i) { + EXPECT_TRUE( + result.getContact(i).normal.isApprox(-Eigen::Vector3d::UnitX(), 1e-12)); + EXPECT_NEAR(0.375, result.getContact(i).position.x(), 1e-12); + EXPECT_NEAR(0.25, result.getContact(i).depth, 1e-12); + } } TEST(CylinderCollision, RotatedCylinderBoxCornerContactPositionLiesInOverlap) @@ -341,6 +368,180 @@ TEST(CylinderCollision, DeepFloorBoxUsesAxialNormal) EXPECT_NEAR(0.75, result.getContact(0).depth, 1e-12); } +TEST(CylinderCollision, SideOnFaceEmitsStableEndpointContacts) +{ + // Cylinder lying on its side on a box face: the solver needs both line + // endpoints as stable supports; a single point along the contact line + // teleports under micro-motion and keeps resting piles rocking + // (issue #3056 S6 fixture). + CylinderShape cylinder(0.05, 0.2); + BoxShape floorBox(Eigen::Vector3d(1.0, 1.0, 0.1)); + + const auto collideAt = [&](double x) { + Eigen::Isometry3d tf = rotatedAroundX(1.5707963267948966); + tf.translation() = Eigen::Vector3d(x, 0.0, 0.049); + CollisionResult result; + EXPECT_TRUE(collideCylinderBox( + cylinder, tf, floorBox, translated(0.0, 0.0, -0.1), result)); + return result; + }; + + const CollisionResult first = collideAt(0.0); + ASSERT_EQ(2u, first.numContacts()); + double minY = first.getContact(0).position.y(); + double maxY = first.getContact(1).position.y(); + if (minY > maxY) { + std::swap(minY, maxY); + } + EXPECT_NEAR(-0.1, minY, 1e-9); + EXPECT_NEAR(0.1, maxY, 1e-9); + for (std::size_t i = 0; i < first.numContacts(); ++i) { + EXPECT_NEAR(0.001, first.getContact(i).depth, 1e-9); + EXPECT_TRUE( + first.getContact(i).normal.isApprox(Eigen::Vector3d::UnitZ(), 1e-12)); + } + + const CollisionResult second = collideAt(50e-6); + ASSERT_EQ(2u, second.numContacts()); + for (std::size_t i = 0; i < second.numContacts(); ++i) { + double best = std::numeric_limits::infinity(); + for (std::size_t j = 0; j < first.numContacts(); ++j) { + best = std::min( + best, + (second.getContact(i).position - first.getContact(j).position) + .norm()); + } + EXPECT_LT(best, 2e-4); + } +} + +TEST(CylinderCollision, TiltedSeparationIsNotFabricated) +{ + // A tilted cylinder's support distance along the face normal is + // r * sqrt(1 - dot^2), not r. Using the full radius fabricated shallow + // contacts in the band between the two values; this pins the corrected + // boundary from both sides. + CylinderShape cylinder(0.05, 0.2); + BoxShape floorBox(Eigen::Vector3d(1.0, 1.0, 0.1)); + + const double axisNormalDot = 0.04; + const double effectiveRadius + = 0.05 * std::sqrt(1.0 - axisNormalDot * axisNormalDot); + const double endpointDrop = 0.1 * axisNormalDot; + const Eigen::Isometry3d tilted = rotatedAroundX(std::acos(axisNormalDot)); + + const auto collideAtCenterHeight = [&](double zc, CollisionResult& result) { + Eigen::Isometry3d tf = tilted; + tf.translation() = Eigen::Vector3d(0.0, 0.0, zc); + return collideCylinderBox( + cylinder, tf, floorBox, translated(0.0, 0.0, -0.1), result); + }; + + // Deep-endpoint clearance 1e-5 ABOVE the true support: separated, but + // inside the full-radius formula's false-contact band. + CollisionResult separated; + EXPECT_FALSE( + collideAtCenterHeight(effectiveRadius + 1e-5 + endpointDrop, separated)); + EXPECT_EQ(0u, separated.numContacts()); + + // Deep-endpoint clearance 1e-5 BELOW the true support: a genuine graze + // whose deepest contact must report the exact tiny depth. + CollisionResult grazing; + ASSERT_TRUE( + collideAtCenterHeight(effectiveRadius - 1e-5 + endpointDrop, grazing)); + ASSERT_GE(grazing.numContacts(), 1u); + double maxDepth = 0.0; + for (std::size_t i = 0; i < grazing.numContacts(); ++i) { + maxDepth = std::max(maxDepth, grazing.getContact(i).depth); + } + EXPECT_NEAR(1e-5, maxDepth, 1e-9); +} + +TEST(CylinderCollision, TiltedSideOnFaceKeepsBothEndpointDepths) +{ + // A micro-tilt within the pile's settling regime makes the endpoint + // depths asymmetric; both supports must survive with per-endpoint depths. + CylinderShape cylinder(0.05, 0.2); + BoxShape floorBox(Eigen::Vector3d(1.0, 1.0, 0.1)); + + Eigen::Isometry3d tf = rotatedAroundX(1.5707963267948966 + 1e-3); + tf.translation() = Eigen::Vector3d(0.0, 0.0, 0.049); + CollisionResult result; + ASSERT_TRUE(collideCylinderBox( + cylinder, tf, floorBox, translated(0.0, 0.0, -0.1), result)); + ASSERT_EQ(2u, result.numContacts()); + + double minDepth = result.getContact(0).depth; + double maxDepth = result.getContact(1).depth; + if (minDepth > maxDepth) { + std::swap(minDepth, maxDepth); + } + EXPECT_NEAR(0.0009, minDepth, 1e-6); + EXPECT_NEAR(0.0011, maxDepth, 1e-6); +} + +TEST(CylinderCollision, CrossedCylindersKeepShallowContact) +{ + // Perpendicular crossed cylinders at ~1 mm penetration: the convex + // fallback intermittently reported no contact under 50 um slides, so the + // upper body free-fell for a step and re-impacted (measured jitter + // source). The closest-point side contact must persist across the sweep. + CylinderShape cylinder1(0.05, 0.2); + CylinderShape cylinder2(0.05, 0.2); + + for (int k = 0; k <= 4; ++k) { + Eigen::Isometry3d tf2 = rotatedAroundY(1.5707963267948966); + tf2.translation() = Eigen::Vector3d(k * 50e-6, 0.0, 0.099); + CollisionResult result; + ASSERT_TRUE(collideCylinders( + cylinder1, rotatedAroundX(1.5707963267948966), cylinder2, tf2, result)) + << "missed contact at slide step " << k; + ASSERT_EQ(1u, result.numContacts()); + EXPECT_NEAR(0.001, result.getContact(0).depth, 1e-9); + EXPECT_TRUE( + result.getContact(0).normal.isApprox(-Eigen::Vector3d::UnitZ(), 1e-9)); + } +} + +TEST(CylinderCollision, SpunUprightCapOnFaceUsesStablePatch) +{ + // Cylinder-box contact is invariant under spin about the cylinder axis. + // An upright cylinder that settled with an arbitrary spin must take the + // same stable analytic cap-patch path as the unspun one — previously the + // aligned path demanded exact rotational identity, so spun cylinders + // fell to the convex fallback whose degenerate rim points let a loaded + // cylinder creep through its support (issue #3056 S6 seed evidence). + CylinderShape cylinder(0.5, 1.0); + BoxShape floorBox(Eigen::Vector3d(5.0, 5.0, 0.1)); + + const auto collideWithSpin = [&](double spin) { + Eigen::Isometry3d tf = rotatedAroundZ(spin); + tf.translation() = Eigen::Vector3d(0.3, 0.2, 0.45); + CollisionResult result; + EXPECT_TRUE(collideCylinderBox( + cylinder, tf, floorBox, translated(0.0, 0.0, -0.1), result)); + return result; + }; + + const CollisionResult unspun = collideWithSpin(0.0); + const CollisionResult spun = collideWithSpin(0.145); + + ASSERT_GE(unspun.numContacts(), 3u); + ASSERT_EQ(unspun.numContacts(), spun.numContacts()); + for (std::size_t i = 0; i < spun.numContacts(); ++i) { + EXPECT_NEAR(0.05, spun.getContact(i).depth, 1e-12); + EXPECT_TRUE( + spun.getContact(i).normal.isApprox(Eigen::Vector3d::UnitZ(), 1e-12)); + double best = std::numeric_limits::infinity(); + for (std::size_t j = 0; j < unspun.numContacts(); ++j) { + best = std::min( + best, + (spun.getContact(i).position - unspun.getContact(j).position).norm()); + } + EXPECT_LT(best, 1e-9); + } +} + TEST(CylinderCollision, TouchingCylinderBoxCapReportsContact) { CylinderShape cylinder(0.5, 2.0); diff --git a/tests/unit/collision/test_DARTCollisionEngine.cpp b/tests/unit/collision/test_DARTCollisionEngine.cpp index 80365ef9ab770..ca4ef176ada38 100644 --- a/tests/unit/collision/test_DARTCollisionEngine.cpp +++ b/tests/unit/collision/test_DARTCollisionEngine.cpp @@ -588,7 +588,7 @@ TEST(DARTCollisionDetector, RespectsGlobalContactLimit) } //============================================================================== -TEST(DARTCollisionDetector, CapsSolverFacingManifoldContacts) +TEST(DARTCollisionDetector, SolverFacingQueriesCarryFullBoxManifold) { auto detector = collision::DARTCollisionDetector::create(); auto frame1 = makeFrame( @@ -598,12 +598,15 @@ TEST(DARTCollisionDetector, CapsSolverFacingManifoldContacts) Eigen::Vector3d(0.8, 0.0, 0.0)); auto group = detector->createCollisionGroup(frame1.get(), frame2.get()); + // A face-face box overlap must expose all four manifold corners to the + // solver: a three-contact tripod cannot hold a resting stack against + // micro-tilts toward the missing corner (issue #3056 S6 pile fixture). collision::CollisionOption wideOption(true, 10u); wideOption.maxNumContactsPerPair = 10u; collision::CollisionResult wideResult; ASSERT_TRUE(group->collide(wideOption, &wideResult)); - EXPECT_EQ(3u, wideResult.getNumContacts()); + EXPECT_EQ(4u, wideResult.getNumContacts()); collision::CollisionOption strictOption(true, 10u); strictOption.maxNumContactsPerPair = 2u; @@ -613,6 +616,58 @@ TEST(DARTCollisionDetector, CapsSolverFacingManifoldContacts) EXPECT_EQ(2u, strictResult.getNumContacts()); } +//============================================================================== +TEST(DARTCollisionDetector, RestingBoxStackKeepsFourCornerContacts) +{ + auto detector = collision::DARTCollisionDetector::create(); + + // A 0.2 cube resting on a large thin floor box with 1 mm overlap: the + // solver needs one contact per bottom corner to keep the stack from + // rocking, so exactly four distinct corner contacts must be emitted. + auto floor = makeFrame( + std::make_shared(Eigen::Vector3d(2.0, 2.0, 0.002))); + auto cube = makeFrame( + std::make_shared(Eigen::Vector3d(0.2, 0.2, 0.2)), + Eigen::Vector3d(0.0, 0.0, 0.1)); + auto group = detector->createCollisionGroup(floor.get(), cube.get()); + + // Per-pair budget wider than the manifold capacity, so an exact count of + // four proves the emission itself, not budget truncation. + collision::CollisionOption option(true, 100u); + option.maxNumContactsPerPair = 10u; + + const auto distinctCorners = [](const collision::CollisionResult& result) { + std::set> corners; + for (std::size_t i = 0; i < result.getNumContacts(); ++i) { + const auto& contact = result.getContact(i); + corners.emplace( + contact.point.x() > 0.0 ? 1 : -1, contact.point.y() > 0.0 ? 1 : -1); + } + return corners.size(); + }; + + collision::CollisionResult flatResult; + ASSERT_TRUE(group->collide(option, &flatResult)); + ASSERT_EQ(4u, flatResult.getNumContacts()); + for (std::size_t i = 0; i < flatResult.getNumContacts(); ++i) + EXPECT_NEAR(0.001, flatResult.getContact(i).penetrationDepth, 1e-9); + EXPECT_EQ(4u, distinctCorners(flatResult)); + + // A micro-tilt about x makes the corner depths asymmetric (the regime the + // pile fixture lives in); all four distinct corner supports must still be + // emitted so the support polygon keeps spanning the full face. + Eigen::Isometry3d tilted = Eigen::Isometry3d::Identity(); + tilted.linear() + = Eigen::AngleAxisd(1e-4, Eigen::Vector3d::UnitX()).toRotationMatrix(); + tilted.translation() = Eigen::Vector3d(0.0, 0.0, 0.1); + cube->setTransform(tilted); + + collision::CollisionResult tiltedResult; + ASSERT_TRUE(group->collide(option, &tiltedResult)); + ASSERT_EQ(4u, tiltedResult.getNumContacts()); + EXPECT_EQ(4u, distinctCorners(tiltedResult)); +} + //============================================================================== TEST(DARTCollisionDetector, RespectsCollisionFilter) { @@ -774,12 +829,18 @@ TEST(DARTCollisionDetector, CollidesCylinderBox) = detector->createCollisionGroup(cylinderFrame.get(), boxFrame.get()); collision::CollisionResult result; EXPECT_TRUE(group->collide(collision::CollisionOption(true, 10u), &result)); - ASSERT_EQ(1u, result.getNumContacts()); - - const auto& contact = result.getContact(0); - EXPECT_EQ(cylinderFrame.get(), contact.getShapeFrame1()); - EXPECT_EQ(boxFrame.get(), contact.getShapeFrame2()); - EXPECT_TRUE(contact.normal.isApprox(-Eigen::Vector3d::UnitX(), 1e-12)); + // The vertical cylinder side against the box face is a contact line + // clipped to the box face; both endpoints are emitted as distinct stable + // supports. + ASSERT_EQ(2u, result.getNumContacts()); + EXPECT_GT( + (result.getContact(0).point - result.getContact(1).point).norm(), 0.1); + for (std::size_t i = 0; i < result.getNumContacts(); ++i) { + const auto& contact = result.getContact(i); + EXPECT_EQ(cylinderFrame.get(), contact.getShapeFrame1()); + EXPECT_EQ(boxFrame.get(), contact.getShapeFrame2()); + EXPECT_TRUE(contact.normal.isApprox(-Eigen::Vector3d::UnitX(), 1e-12)); + } } //==============================================================================