Files
UA-.NETStandard/tests/Opc.Ua.OpenUsd.Tests/OpenUsdConnectorModelChangeGuardTests.cs
Marc Schier 81d726b7ad Coordinate the robot cell, and make the twin show what it is actually doing (#4162)
# 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 #4161

## Checklist

- [x] I have signed the
[CLA](https://opcfoundation.org/license/cla/ContributorLicenseAgreementv1.0.pdf)
and read the
[CONTRIBUTING](https://github.com/OPCFoundation/UA-.NETStandard/blob/master/CONTRIBUTING.md)
doc.
- [x] I have added tests that prove my fix is effective or that my
feature works and increased code coverage.
- [x] I have added all necessary documentation.
- [x] I have verified that my changes do not introduce (new) build or
analyzer warnings.
- [x] 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.

---------

Co-authored-by: Copilot <223556219+Copilot@users.noreply.github.com>
Copilot-Session: 8cbb8cd0-f0cb-4ab0-bea2-6202fbf69485
2026-08-04 06:58:29 +00:00

96 lines
3.9 KiB
C#

/* ========================================================================
* Copyright (c) 2005-2026 The OPC Foundation, Inc. All rights reserved.
*
* OPC Foundation MIT License 1.00
*
* Permission is hereby granted, free of charge, to any person
* obtaining a copy of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use,
* copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following
* conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*
* The complete license agreement can be found here:
* http://opcfoundation.org/License/MIT/1.00/
* ======================================================================*/
using System;
using NUnit.Framework;
namespace Opc.Ua.OpenUsd.Client.Tests
{
/// <summary>
/// Covers the guard that refuses to run a twin whose §5.13 reconciliation is dead.
/// </summary>
/// <remarks>
/// A connector whose model-change item was rejected keeps whatever composition it
/// happened to resolve at start-up and never reconciles again, so a Dynamic component
/// silently stops matching the server. That has to be loud: the same class of silent
/// staleness took a long time to find the last time it went unreported.
/// </remarks>
[TestFixture]
[Category("OpenUsd")]
public sealed class OpenUsdConnectorModelChangeGuardTests
{
[Test]
public void ARejectedModelChangeItemIsRefused()
{
var error = new ServiceResult(StatusCodes.BadNodeIdUnknown);
InvalidOperationException? thrown = Assert.Throws<InvalidOperationException>(
() => OpenUsdConnector.ThrowIfModelChangeRejected(error, Opc.Ua.ObjectIds.Server));
Assert.That(thrown!.Message, Does.Contain("model-change"));
Assert.That(thrown.Message, Does.Contain("i=2253"),
"The message must name the node the subscription was refused on.");
}
[Test]
public void EveryBadStatusIsRefused()
{
foreach (StatusCode status in new[]
{
StatusCodes.BadEventFilterInvalid,
StatusCodes.BadAttributeIdInvalid,
StatusCodes.BadMonitoredItemFilterUnsupported
})
{
Assert.Throws<InvalidOperationException>(
() => OpenUsdConnector.ThrowIfModelChangeRejected(
new ServiceResult(status), Opc.Ua.ObjectIds.Server),
$"{status} should have been refused.");
}
}
[Test]
public void AnAcceptedModelChangeItemIsAllowedThrough()
{
Assert.DoesNotThrow(
() => OpenUsdConnector.ThrowIfModelChangeRejected(
ServiceResult.Good, Opc.Ua.ObjectIds.Server));
}
[Test]
public void AnUnreportedStatusIsAllowedThrough()
{
// A monitored item that was never given a status is not a rejected one; the
// client stack leaves Error null until the server has answered for it.
Assert.DoesNotThrow(
() => OpenUsdConnector.ThrowIfModelChangeRejected(null, Opc.Ua.ObjectIds.Server));
}
}
}