Epic 021: Headless robot simulator — implementation (iter 3)

[DanielKow]

Summary

Iteration 3 of Epic 021 (Robot Simulator) — the collision-aware simulator, robot-program, teaching-point and FK-validation primitives land in RocketWelder.SDK.Robotics.Core.

Tasks included in this PR

• TASK-003 — MoveResult evolution with MoveFailureReason + CollisionResult (c2bf99f)
• TASK-004 — ICollisionSource + primitive hierarchy + PrimitiveCollisionSource (8c6c10b)
• TASK-005 — ToolModel hierarchy (None / Capsule / Mesh) (581a1d7)
• TASK-006 — CollisionEnvironment immutable configuration (77dd9e9)
• TASK-007 — CollisionDetector static class, 10 non-adjacent pairs + env delegation (f4d92d5)
• TASK-008 — RobotProgram + ProgramStep hierarchy + System.Text.Json round-trip (3bb50c0)
• TASK-009 — TeachingPointSet with JSON round-trip (15063de)
• TASK-010 — FkValidator + schema-versioned JSON export/import (7b6b224)
• TASK-011 — SimulatedRobot extensions: collision-aware moves, teaching-point CRUD, program execution (d66a018)

Still pending

• TASK-012 — Fairino preset fixes + FR3/FR10/FR16/FR20/FR30 additions. Blocked: fairino-preset-reference.md does not list d4 (wrist offset) per model, only FR5 has it. Awaiting clarification from team-lead. Follow-up commit will land here.

Testing

• dotnet test csharp/RocketWelder.SDK.Robotics.Core.Tests/ — 202 passed, 0 failed.

Design docs

Design and per-task specs are in the modelingevolution/project-management repo on branch feature/epic-021-iter3-implementation under epics/epic-021-robot-simulator/. Dev log: iterations/iteration-3/dev-log.md.

[DanielKow]

Reviewer findings — Epic 021 Iter 3 (TASK-003 … TASK-011)

Tests: 202/202 green in csharp/RocketWelder.SDK.Robotics.Core.Tests/. Architecture and ADR conformance look solid: SimulatedRobot composes CollisionEnvironment / TeachingPointSet / RobotProgram cleanly, FK/IK stay pure, collision is opt-in (ADR-006 headless), and TeachingPointSet is owned outside the robot lifetime (ADR-005).

Posting my findings as one global comment because several of them span multiple files. File:line citations inline.

---

[MUST] Finding #1 — Path-level collision checking is missing

Severity: BLOCKER
Category: Requirements
Evidence:

• docs/epics/epic-021-robot-simulator/iterations/iteration-2/test-scenarios.md FR-8 "Path validation stops at first collision":

  And a program with 5 steps where step 3's interpolated path collides
  When I execute the program
  Then the result includes trajectory up to the last safe interpolation step before the collision

• SimulatedRobot.cs:270 (ExecuteWaypoints) and SimulatedRobot.cs:375 (Execute) both only call FirstCollision(targetJoints) once, against the target configuration — not inside the interpolation loop.
• No test in csharp/RocketWelder.SDK.Robotics.Core.Tests/ asserts collision detection along an interpolated path whose endpoints are both collision-free.

Issue: A program whose endpoints are both collision-free but whose swept path clips an obstacle (common: elbow arc through a fixture between two safe poses) executes the full trajectory today with no failure reported. That is a silent safety bypass and directly contradicts the FR-8 scenario's "last safe interpolation step" language.

Question: Was path-level checking deferred intentionally? If so, the FR-8 "Path validation" scenario is expected to fail in iter 3 and that needs an explicit deferral from team-lead. Otherwise, please move the FirstCollision check into the per-interpolation loop and truncate steps at the last safe sub-step, plus add a regression test that has both endpoints safe and an intermediate config colliding.

---

[MUST] Finding #2 — Redundant/incorrect Equals/GetHashCode overrides on record types

Severity: MAJOR
Category: Standards / Design
Evidence:

• docs/standards/dotnet.md §Type Selection: records "provide boilerplate code generation (equality, GetHashCode, ToString, …)".
• ToolModel.cs:27-31 — CapsuleToolModel manually overrides Equals(CapsuleToolModel?) and GetHashCode().
• ToolModel.cs:42-46 — MeshToolModel does the same; GetHashCode unconditionally calls Id.GetHashCode() with no null-guard, and neither override consults EqualityContract.

Issue: Both records are sealed and all their value fields are positional, so the compiler-generated structural equality is already correct. The manual overrides (a) shadow the generated equality / bypass EqualityContract, (b) risk silent drift if a new property is later added to the primary constructor, and (c) violate Principle #2 ("delete the part"). They add risk without adding behaviour.

Question: Any reason behind these overrides that I'm missing? If not, please delete both pairs.

---

[MUST] Finding #3 — .ToList() / .ToArray() violates dotnet standards

Severity: MAJOR
Category: Standards
Evidence:

• docs/standards/dotnet.md §Concurrency and Performance: "FORBIDDEN: ToList(), ToArray(), ToDictionary(). These methods are FORBIDDEN in regular code."
• FkValidator.cs:46 — records.Select(...).ToList() in Export.
• FkValidator.cs:61 — records.Select(...).ToList() in Import.
• TeachingPointSet.cs:75 — _order.Select(...).ToList() in ToJson.
• CollisionDetector.cs:72,76 — hits.ToArray().
• CollisionEnvironment.cs:44 — linkRadii.ToArray() in the constructor.

Issue: None of these sites fall under the "tolerated" carve-outs in the standards doc (startup reflection, dictionary-for-O(1)-lookup). System.Text.Json serialises IEnumerable<T> directly, so the DTO projections can drop the .ToList(). CollisionDetector.CheckCollision can return IReadOnlyList<CollisionResult> (or allocate the array once) instead of List<T> → ToArray. CollisionEnvironment's defensive copy can use ImmutableArray<double>.CreateRange(linkRadii) and skip the copy when the caller already passes an ImmutableArray.

Question: Can these be reworked to stay within the standard, or is there a justification putting them in the tolerated bucket?

---

[MUST] Finding #4 — Box/Cylinder segment-distance uses 17-point sampling

Severity: MAJOR
Category: Design (correctness, safety-critical)
Evidence:

• design.md §CollisionDetector promises closed-form queries ("10 capsule-capsule closed-form checks").
• CollisionMath.cs:97-122 — ClosestPointOnSegmentToBox iterates t = 0..16/16 and takes min sdf.
• CollisionMath.cs:152-193 — ClosestPointOnSegmentToCylinderZ does the same.
• iteration-3/dev-log.md TASK-004 acknowledges the approximation.

Issue: Box / cylinder SDF is convex outside and concave inside. When a segment penetrates through opposite faces, the deepest penetration can be between the 17 sample points; a segment of length L samples every L/16 mm, and for link segments ~300 mm that's ~19 mm — larger than the link radius plus a typical safety margin. In the worst case the detector reports "no collision" while the link passes through the obstacle, which defeats the subsystem's purpose for a welding-cell safety check.

Segment-vs-AABB and segment-vs-Z-cylinder both have closed-form solutions; please swap to those, or at minimum prove the sampling error bound against min(linkRadius) + safetyMargin and document it on the method.

Question: Was sampling a deliberate cost tradeoff, or is closed-form blocked by something? If the former, can we add the error-bound analysis (and a test that constructs a worst-case "between samples" penetration) — or (preferred) replace with closed-form?

---

[SHOULD] Finding #5 — Inconsistent collision-failure signalling on the non-Try API

Severity: MINOR
Category: Design
Evidence:

• SimulatedRobot.cs:147-150 — MoveJoint throws InvalidOperationException on collision.
• SimulatedRobot.cs:165-166 — MoveLin returns -2 on collision.
• TryMoveJoint / TryMoveLin both return MoveResult.RejectedByCollision(...).

Issue: The two non-Try entry points have different failure semantics for the same condition. Mixing them inside one implementation is surprising and will lead to either forgotten exception handlers or missed return-code checks in caller code.

Question: Which shape matches IRobot's intended contract — return-code or exception on collision? Please align both.

---

[COULD] Finding #6 — LinkRadii[i] > 0 rejects zero; design only says "double[6] LinkRadii"

Severity: MINOR
Category: Design
Evidence:

• design.md §CollisionEnvironment: double[6] LinkRadii (no positivity constraint).
• CollisionEnvironment.cs:36 rejects linkRadii[i] <= 0.

Issue: Zero is a legitimate way to exclude a link from collision (thin tool-plate link, virtual base). Stricter validation isn't wrong, but it's undocumented and will surprise a CAD/CAM integrator.

Question: Is the > 0 check intentional? If so, update the design to say "positive link radii". Otherwise relax to >= 0 and keep the NaN guard.

---

[COULD] Finding #7 — ExtrudeAlongFlangeZ hand-rolls a ZYX rotation already available from Pose3/Drawing

Severity: MINOR
Category: Design / Coupling
Evidence:

• PrimitiveCollisionSource.cs:92-112 — manual ZYX euler → rotation-column math.
• ModelingEvolution.Drawing already provides Pose3<T> and Matrix4x4d transforms that ForwardKinematics consumes.

Issue: Duplicating the rotation inline (a) couples this file to a specific euler convention without a test pinning it, (b) is easy to get wrong (sign of sx in the zx/zy columns depends on convention), and (c) duplicates code that Drawing already owns. No test in ToolAndBaseTransformTests covers a capsule tool with a non-trivial flange orientation.

Question: Can this be replaced with state.FramePoses[5]'s rotation (via Drawing's matrix helpers) and guarded by a test with a non-zero J5 orientation?

---

— Claude (reviewer)

Tagged by Claude (moderator): #1–#4 [MUST] (FR-8 acceptance criterion, explicit standards violations, safety-critical design contract); #5 [SHOULD] (API inconsistency, not blocking); #6–#7 [COULD] (spec-doc gap / cleanup).

[DanielKow]

Round 1 MUST fixes — pushed as ea760f7 and 1eeba3e

MUST #1 — Path-level collision in ExecuteWaypoints / Execute
Moved FirstCollision(interpJoints) inside the interpolation loop in SimulatedRobot.cs. On collision the colliding sub-step is not appended to Steps, so Steps[^1] is always collision-free (truncate-at-last-safe-sub-step). Added regression ExecuteWaypoints_WithSafeEndpointsButCollidingIntermediate_TruncatesAtLastSafeStep with safe endpoints (J1=0 and J1=90 verified via direct CollisionDetector.CheckCollision) and a mid-swing box at (707, 707, 500); asserts every recorded step is collision-free.

MUST #2 — Manual record equality
Deleted hand-written Equals/GetHashCode from CapsuleToolModel and MeshToolModel. Records now rely on compiler-generated structural equality.

MUST #3 — Hot-path allocations

• CollisionDetector.CheckCollision now returns IReadOnlyList<CollisionResult>, accumulates into a lazily allocated List, no terminal ToArray.
• CollisionEnvironment.LinkRadii exposed as ImmutableArray<double> instead of IReadOnlyList<double> re-copied per call.
• FkValidator / TeachingPointSet export paths use pre-sized arrays instead of Select(...).ToList().

MUST #4 — Closed-form segment/box and segment/cylinder SDF
Replaced 17-point parametric sampling in ClosestPointOnSegmentToBox and ClosestPointOnSegmentToCylinderZ with closed-form break-point enumeration (endpoints + axis-center crossings + face crossings + outside-quadratic stationary point per sign cell for box; quadratic-root radial crossings + radial-projection minimum for cylinder). New CollisionMathTests.cs validates against a 10_001-sample brute-force minimum across pierce/tangent/outside geometries and adds a tangent-pierce regression the old uniform sampler missed.

All 217 tests pass. SHOULD #5 and COULD #6/#7 not yet addressed.

— Claude (dotnet-engineer)