Coordinate the robot cell, and make the twin show what it is actually doing

[marcschier]

Description

MinimalRobotServer drove its two mobile manipulators through each other. The platforms did clear — R1 and R2 trace figure-eights 1.2 m apart at closest approach, exactly as README.md claimed — but each arm reaches 1.611 m (link offsets 0.260 + 0.680 + 0.670 + 0.158 from robot.usda), so the arms swept straight through one another while the chassis politely missed. Clearance has to be argued about the deployed arm, not the footprint.

Three related gaps sat alongside it: motion was open-loop (two independent sine paths with no awareness of each other), only R1 had a gripper, and nothing was ever actually picked — PartA/PartB were static children of a single table and the arm replayed a canned eight-pose loop unrelated to where either the robot or the table was.

This replaces all of that with a coordinated transfer cycle.

Keeping them apart

The cell is divided into reservable zones, and a robot reserves the zone immediately ahead before entering it, releasing the one behind once clear — block signalling rather than a global schedule:

Zone	Extent	Occupancy
EndZoneA / EndZoneB	|x| > 1.3 m	one robot — a deployed arm needs the whole end
CorridorEastbound	|x| ≤ 1.3 m, northern lane (y = 0.35)	one robot
CorridorWestbound	|x| ≤ 1.3 m, southern lane (y = −1.15)	one robot
DockA / DockB	around each charging dock (y = −2.0)	one robot

Three rules follow:

• Opposite-direction traffic passes, being in lanes 1.5 m apart; same-direction traffic queues rather than overtaking.
• An arm may only leave its transport envelope inside an exclusively reserved end zone. This is the rule that actually fixes the reported defect.
• A robot with no work parks on its dock, releasing the end zone so the other robot can deliver into the station it serves.

That last rule is not cosmetic. Without it the cell deadlocks permanently on the first handover: an idle robot standing beside its own table blocks the very delivery it is waiting for. A state trace showed R1 blocked at x = 0.98 forever while R2 sat idle holding EndZoneB. The docks are in a southern layby rather than beside the stations, because a robot parked next to a station is close enough to foul one turning into it.

Where the part actually is

A carried part's pose is computed by forward kinematics (RobotKinematics) from the same six axis values the server publishes, not from the choreography that produced them — if the arm and the part were computed independently they would visibly drift apart. RobotArmSolver solves shoulder and elbow so the tool points down onto a slot, and is checked against that same forward kinematics, landing the tool on every slot of both tables to within 1e-6 m.

Also simulated

Trapezoidal accel/cruise/decel travel with the heading following the path; speed-and-separation slowdown near the other robot; an emergency stop that now halts motion rather than only blinking the beacon; battery drain with a charging detour; seeded grip-slip faults with floor recovery; and KPIs (parts moved, cycle count, last/average cycle time, utilisation, fault count). Both robots now carry a gripper.

Two hidden couplings this surfaced

• m_r1NodeId was keyed on "has a tool", so giving R2 a gripper silently moved the dynamic-tool demo and the speed-override command target onto R2. Now keyed on R1 explicitly.
• RobotPositionsDriveLiveOpenUsdSceneAsync sampled a fixed 1.2 s window and assumed the robot was always moving. Under a duty cycle a robot legitimately stands still while gripping, dwelling or charging, so it now polls until movement is observed — the assertion's intent is that live positions reach the scene, not that the robot never stops.

Invariants are asserted, not assumed

CellChoreographyTests runs ten minutes of simulated time at 50 ms and checks on every step that:

• oriented footprints never overlap (an oriented box, not a bounding circle — the circle is so pessimistic for a 1.15 × 0.60 m platform that it demands clearances no real cell would need);
• the two tool centre points never meet;
• a zone is never doubly occupied;
• arms only deploy inside an end zone;
• parts are conserved — never duplicated, lost or carried twice;
• a carried part sits exactly on the tool centre point.

Plus the round trip completing, KPIs accumulating, the emergency stop halting every robot, dropped parts being recovered, and the battery draining and recharging.

The README documents the zoning, traffic rules and cycle, corrects the misleading clearance claim, and lists ten further realism extensions that were deliberately not built (ISO 10218 / TS 15066 speed-and-separation monitoring, scanner cones, tool changer, inspection station, conveyor infeed, gate interlock, torque telemetry, maintenance counters, time-sampled pose history, traffic priority).

Making the cycle visible

Running the sample end to end against the OpenUSD connector showed the robots driving the cycle and the arms reaching the slots while nothing was picked up or moved. The choreographer tracked part poses and gripper opening purely in memory: the address space only ever exposed the platform poses and the twelve axis angles, so a connector had nothing to subscribe to for the parts and the blocks stayed wherever the asset authored them.

A CellTwin folder now holds a ThreeDCartesianCoordinates position and a ThreeDOrientation heading per part, plus a jaw position per robot, bound to /Cell/Parts/PartNN and to the gripper jaws. The variables are registered before the motion device system is built - a binding can only carry a SourceNodeId that already exists - and sit outside the robotics subtree, because a workpiece is not a device and the robotics builder validates every node below its root as one it allocated itself.

The poses are published as structured vectors, not as a matrix. A viewer composes a prim's matrix from the vector translate and rotate ops; the matrix4d profile is explicitly left unresolved by the reference connector, which drops a binding it cannot convert in silence. That is the shape the first attempt took, and it failed without a word in any log.

The jaws needed the asset restructured. A carrier cube and a finger mesh were siblings under the gripper body, so driving the carrier would have slid it out from under the finger it is bolted to. Each jaw is now one Xform owning both, declaring the single matrix op every live-bound prim declares, with the finger points rebased onto the jaw origin. Closed puts the finger faces on the 35 mm half-width of the block, so the gripper grips it rather than passing through it.

RobotAssetContractTests now holds every live-bound prim to the single-matrix contract, not just the two robot mount points.

No part starves

Collecting the lowest free slot meant a part that never landed in slot 0 was never collected at all: with two parts seeded on WorkTableA the second sat untouched for the life of the process while the robots shuttled the other one past it - which reads in the twin as the robots ignoring a block sitting in front of them. Parts are now stamped with the order in which they came to rest and the one that has waited longest is collected, which is what a real cell does with a buffer anyway.

Verified end to end: all three parts circulate between both tables, the heading follows the carrying robot and resets on placement, and the jaws stroke between open (0.060) and closed (0.047) as parts are gripped and released.

The OpenUSD packages move to 0.3.0-alpha.

The part was not in the gripper

Running it end to end showed a workpiece floating clear of the jaws that were supposed to be holding it.

The platform pose and the workpiece pose leave the server as separate bindings published by different loops: positioning at 5 Hz (UpdateIntervalMilliseconds = 200), the simulation tick at 20 Hz. A carried part rides the tool centre point, so it was being drawn against a platform up to four ticks stale. Measured against the live scene with the arm stowed — where the part-to-platform distance is fixed by construction — the part sat anywhere between 0.65 m and 0.90 m from its platform instead of at a single distance. A quarter of a metre of drift, three part widths, in front of a 96 mm jaw opening.

The platform now publishes at the tick rate, and a carried part is derived from the pose that was actually written to the address space, recorded at the SetFrameValue call site rather than where the sample is produced. Each value was individually correct the whole time; only their relationship was wrong, which is why nothing caught it.

RobotOpenUsdE2eTests now rebuilds the tool centre point from the values authored into the scene and asserts a carried part sits on it. It reproduced the defect at 0.49 m before the fix. The tolerance is the sampling straddle of reading two independently delivered bindings — one publish period is 43 mm at cruise — leaving an order of magnitude between it and the defect.

Two assertions in the positioning test wanted the robots a metre apart in X. They pass each other in opposite corridor lanes, where X is all but identical and the separation is entirely in Y; and before the first sample arrives both platforms sit on the zero frame and coincide exactly. They now measure separation in the plane, once both have been seen to move — this was failing intermittently on net48 already.

The opening view

Framing the bounds of an enclosed scene automatically puts the eye inside it: the viewport opened on a close-up of whichever robot happened to be nearest, looking through the fence.

The cell now authors /Cell/OverviewCamera — the whole floor, both work tables, the docks and the controller cabinet, from 12 m out at 28 degrees of elevation — and the connector opens on the first camera the served root layer declares. It already had the plumbing to honour a camera path; it was simply never given one. --camera still overrides the choice outright, and RobotAssetContractTests asserts the establishing shot is ordered before /Cell/TopDownCamera.

The mounted gripper was never reported

DynamicToolIsComposedAsync was failing on both TFMs, in isolation, and a longer timeout did not help. It predates this branch — I confirmed it at 58fd5e8ca with a clean worktree. It turned out not to be a flaky test at all.

Part 5 §9.32.2 lets only a node carrying a NodeVersion property trigger a ModelChangeEvent, and AsyncCustomNodeManager drops entries for nodes that lack one. Neither the mounted tool nor the robot it hangs off had a NodeVersion, so every addition was filtered out. Removal kept reporting, because a deleted node is no longer in the manager's index and so takes the "not mine, pass it through" branch of the same filter.

That asymmetry is what made it look like flakiness. A connector starting while the tool was mounted composed it and could still deactivate it later; one starting in the six-second gap was never told the tool appeared and never composed it at all, for the life of the process. Which happened depended only on the phase the fixture reached the test in — near-constant, hence a consistent failure rather than an intermittent one.

Measured directly before the fix: model-change events arrived only on detach, exactly 18 s apart, and every resolve returned zero components:

14:00:12.738 SERVER mounted        <- no event
14:00:24.752 SERVER detached
14:00:24.801 model-change event    <- only this

After enabling model-change tracking on the robot, the prim tracks the cycle — A = composed active (12 s mounted), i = inactive (6 s detached):

....AAAAAAAAAAAAAAAAAAAAAAAiiiiiiiiiiiiAAAAAAAAAAAAAAAAAAAAAAAAiiiiiiiiiiii

The test now asserts a transition, not a momentary state: the prim seen both active and inactive across more than one full cycle. Unlike the old check this cannot pass on a connector that only ever hears about one direction — which is exactly what the previous pair of tests could not distinguish, since the sibling DynamicToolIsDeactivatedWhenDetachedAsync was satisfied by the initial compose and never exercised reconciliation at all.

Two robustness gaps in the connector fell out of this: a rejected model-change monitored item was reported as success, leaving every Dynamic component frozen at whatever it resolved at start-up; and the initial compose ran before the subscription existed, so a change in between was seen by neither. The first now fails loudly, the second re-resolves once after subscribing.

Opc.Ua.Di.Tests is now 402/403 on net48 (was 3 failures) and on net10.0, across three consecutive runs of the dynamic-tool tests on each.

Related Issues

• Fixes MinimalRobotServer: the two robots drive their arms through each other #4161

Checklist

• I have signed the CLA and read the CONTRIBUTING doc.
• I have added tests that prove my fix is effective or that my feature works and increased code coverage.
• I have added all necessary documentation.
• I have verified that my changes do not introduce (new) build or analyzer warnings.
• I ran all tests locally using the UA.slnx solution against at least .net framework and .net 10, and all passed.
• I fixed all failing and flaky tests in the CI pipelines and all CodeQL warnings.
• I have addressed all PR feedback received.

Verification: full dotnet build UA.slnx -m:1 /p:UseSharedCompilation=false — 0 errors, no new warnings. Opc.Ua.Di.Tests 399/400 on both net10.0 and net48 (1 pre-existing skip), Opc.Ua.OpenUsd.Tests 635, Opc.Ua.Robotics.Tests 103.

[Copilot]

Pull request overview

Updates the MinimalRobotServer sample to run a coordinated two-robot transfer cycle (with zone reservations, arm stow/deploy rules, part pick/place, and richer simulation state) to prevent the manipulators from colliding while still allowing both robots to operate concurrently and pass in the corridor.

Changes:

• Adds a shared CellChoreographer-driven duty cycle (zones/lanes/docks, e-stop halting motion, charging/fault injection, KPIs) and publishes its poses/axes through the existing server model.
• Introduces forward/inverse kinematics (RobotKinematics, RobotArmSolver) and uses FK to keep carried parts rigidly aligned with the published axis values.
• Adds long-run invariant/regression tests (kinematics and choreography), updates E2E polling to account for legitimate robot idle periods, and updates the USD cell asset + README to match the new behavior.

Reviewed changes

Copilot reviewed 15 out of 15 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
tests/Opc.Ua.Di.Tests/RobotOpenUsdE2eTests.cs	Adjusts the OpenUSD live-position E2E to poll for motion under the new duty cycle.
tests/Opc.Ua.Di.Tests/RobotKinematicsTests.cs	Adds unit tests validating forward kinematics and the arm solver against FK.
tests/Opc.Ua.Di.Tests/Opc.Ua.Di.Tests.csproj	Skips the new robotics tests on net472 (where the MinimalRobotServer reference is excluded).
tests/Opc.Ua.Di.Tests/CellChoreographyTests.cs	Adds long-run invariant tests to prevent collisions, double-occupancy, and part/pose inconsistencies.
samples/MinimalRobotServer/RobotKinematics.cs	Implements rigid transforms + FK used to compute the tool center point from published axis values.
samples/MinimalRobotServer/RobotCell.cs	Gives R2 a tool, fixes the “R1 keyed by tool” coupling, and plumbs choreographer identity into axis runtime.
samples/MinimalRobotServer/RobotCell.Configure.cs	Drives published axis values from the choreographer; e-stop now halts choreography.
samples/MinimalRobotServer/RobotArmSolver.cs	Adds IK for placing the TCP onto target slot coordinates.
samples/MinimalRobotServer/RobotAgent.cs	Introduces the per-robot simulation state model (pose, axes, battery, zone state, etc.).
samples/MinimalRobotServer/README.md	Documents the new zoning/traffic rules, transfer cycle, and corrects the prior clearance claim.
samples/MinimalRobotServer/Program.cs	Registers CellChoreographer in DI for the hosted sample.
samples/MinimalRobotServer/MobileRobotPositionProvider.cs	Uses the shared choreographer for robot poses when hosted; retains options-driven motion for standalone tests.
samples/MinimalRobotServer/CellLayout.cs	Defines authoritative cell geometry constants (stations, lanes, docks, slot positions).
samples/MinimalRobotServer/CellChoreographer.cs	Implements zone reservation + coordinated motion + part handling + faults/charging/KPIs.
samples/MinimalRobotServer/Assets/Cell.usda	Updates the USD scene to two stations, lanes, docks, and live-authored circulating parts.