"""Full-line test of scene/plow_cell_90_45_test.usd with known (pre-assigned) classes: a laser curtain on ConveyorTrack_04 reads each item's pre-known class and shifts the plow right (-16 deg) for B - so it slides along the blade onto ConveyorTrack_06 into container B - and left (+16 deg) for C - so it nudges onto ConveyorTrack_01 into container C. A second curtain further upstream (x=-3.2, same spot the pusher already uses) intercepts class D for the pusher's own bin, unchanged from the already-verified pipeline. Class ground truth is NOT taken from the catalogue's `zone` fields - categories.json and manifest.json disagree with each other and with their own roundness numbers in several places (pouf is zone C in both yet k_round=0.994, i.e. round => class D; pen is C in one file and D in the other). Classes are asserted explicitly in ITEMS below, with the reason. Run inside the live Isaac Sim through the code editor's python server: python isaacsim_send.py --context plow9045 --file scripts/run_plow_9045_known_classes.py """ import asyncio import sys import time REPO = "/home/dasha/robozon-sorter" if REPO not in sys.path: sys.path.insert(0, REPO) for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: del sys.modules[_m] import importlib importlib.invalidate_caches() import numpy as np import omni.usd import omni.timeline import omni.kit.viewport.utility as vp import isaacsim.core.experimental.utils.app as app_utils from omni.physx import get_physx_interface, get_physx_scene_query_interface from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema from isaacsim.core.experimental.prims import RigidPrim from robozon_sorter import config as C from robozon_sorter.sim import plow_cell_9045, scene as _scene from robozon_sorter.sim.plow import Plow # Strict B/C/B/C alternation - the worst case for the blade, a full reversal every 0.7 s. # # The catalogue's own `zone` fields are NOT trustworthy and are not used to pick these: # * pouf is zone C in BOTH categories.json and manifest.json, but k_round = 0.994 - # it is round, so it is class **D** and belongs to the pusher, not the plow. Removed. # * pen is zone C in categories.json and zone D in manifest.json, and k_round = 0.842 # is over the 0.82 roundness threshold - genuinely ambiguous, so it is not used to # measure the plow either. (It is also 13x9 mm, thin enough to slip under a blade.) # The C slots below are items that are oversize by DIMENSION and clearly not round: # backpack 455x370x301 (k 0.82) and pillow 455x431x213 (k 0.905 but flat, not a solid of # revolution). B slots are unambiguous: lunchbox k 0.646, detergent k 0.742. # # box_300x200x200 / box_400x400x300 are also left out: the kinematics log measured them # dwelling 5.97 s and 54.61 s in the plow zone (vs ~1.4 s for everything else), and while # the blade is held by one stuck item every item behind it is starved of its own angle - # that measures the stall, not the swing. # FULL D/C/B run: 9 items, three of each class, repeating D -> C -> B so every consecutive # pair is a different class (the hardest ordering for a single blade + single pusher). # Classes asserted from the physical criteria, not the catalogue's `zone` fields: # D = round (k_round above the 0.82 operating threshold) -> pusher -> BinD # C = oversize by dimension, not round -> plow +20 -> container_C # B = fits the envelope, not round -> plow -20 -> container_B ITEMS = [ ("bag", "D"), # 202x175x170 k 0.896 round ("backpack", "C"), # 455x370x301 k 0.82 oversize ("lunchbox", "B"), # 201x152x62 k 0.646 ("helmet", "D"), # 354x297x280 k 0.895 round ("pillow", "C"), # 455x431x213 k 0.905 oversize (flat, not a solid of rev.) ("detergent", "B"), # 278x260x180 k 0.742 ("bucket", "D"), # 287x287x272 k 0.995 round ("box_400x400x300", "C"), # 401x400x301 k 0.716 oversize ("box_300x200x200", "B"), # 301x200x200 k 0.72 ] CLASSES = dict(ITEMS) ORDER = [n for n, _ in ITEMS] # 700 mm is the spec. It is also SHORTER than the deflection zone an item occupies # (T_zone*speed = 0.95 m), so two opposite-class items are inside the plow at once and # one blade cannot give both their own angle - injectable here to test that directly. PITCH = float(globals().get("pitch", 0.70)) # metres between items at SPEED SPEED = 1.0 # m/s # The blade itself occupies x -7.95..-7.32 (measured). The sensor has to sit far enough # UPSTREAM (+X) of -7.32 that a full B<->C reversal completes before the item touches it. # -7.20 (tried last round) was a mistake born of reading C.PLOW_SWEEP_X0=-7.15 as "the # blade": that constant is the upstream sweep WINDOW, not the blade body, so the sensor # ended up 0.12 m = 0.12 s ahead of the blade while a reversal needs ~0.175 s. The blade # provably could not arrive in time - the kinematics log showed served=NO / held +0 for # every single item that run. Keep >= ~1 m of lead. PLOW_SENSE_X = float(globals().get("plow_sense_x", -6.30)) # ~1.02 m / 1.02 s of lead PUSH_SENSE_X = C.PUSH_X + plow_cell_9045.PUSHER_X_MM / 2000.0 # the blade's own upstream # edge (half its 500 mm width ahead of centre), not a separate gate 700 mm further back - # detection and the stroke firing are now the same event, no lag for the belt to eat. # ---- derive PLOW_RATE / PLOW_ANGLE from the 1 m/s + 700 mm spec, instead of guessing ---- # T_pitch: time between two items at any fixed point. # T_lead : sensor-to-pivot warning time (plenty - the blade only needs a fraction of it). # T_zone : how long ONE item spends inside the active deflection zone (SWEEP_X0 to # RELEASE_X) - the real constraint, because a second item enters this zone # before the first clears it whenever T_zone > T_pitch: with a single blade, # two back-to-back opposite-class items then CANNOT both get a clean, # uninterrupted deflection window - there is an unavoidable overlap, independent # of how fast the blade turns. Sizing the blade speed only controls how much of # that overlap is wasted on the swing itself. # Injectable so the angle/rate can be swept without editing the file: # isaacsim_send.py --args-json '{"plow_angle": 28, "swing_margin": 0.25}' PLOW_ANGLE = float(globals().get("plow_angle", 20.0)) # inside PLOW_LIMIT=45 T_PITCH = PITCH / SPEED BLADE_LEADING_X = -7.32 # measured upstream face of the plow blade body BLADE_TRAILING_X = -7.95 # measured downstream face T_LEAD = abs(PLOW_SENSE_X - BLADE_LEADING_X) / SPEED # to the BLADE, not the pivot T_ZONE = abs(C.PLOW_RELEASE_X - C.PLOW_SWEEP_X0) / SPEED SWING_MARGIN = float(globals().get("swing_margin", 0.25)) # fraction of T_pitch allotted # to the swing itself; smaller => faster commanded blade PLOW_RATE = (2.0 * PLOW_ANGLE) / (SWING_MARGIN * T_PITCH) # worst case: full reversal # The return-to-centre leg had been sharing PLOW_RATE with the deflection swing - fine for # steering an item (where too fast caused overshoot: RATE=600 measured 0/3 on class C), # but a SLOW return with nothing to steer just leaves a residual angle live when the next # item arrives - measured misrouting B->C traffic that should have seen a clean 0. There is # no overshoot risk on an empty return (nothing is being deflected), so it can run flat out: # 3x PLOW_RATE reaches home well inside the same 0.5*T_pitch budget with margin to spare. PLOW_RETURN_RATE = 3.0 * PLOW_RATE PLOW_ANGLES = {"B": -PLOW_ANGLE, "C": PLOW_ANGLE, "D": 0.0} # Force-release timeout. 3*T_zone (2.85 s) measured TOO SHORT: items dwell 4.5-53 s in # the zone, so the blade released its angle long before the item actually reached the # blade body, and the item passed a neutral (0 deg) blade - which sends it +Y by # default, because ConveyorTrack_06 (y 0.025..1.048, driving +Y) claims anything at # y>0 at the end of Track_04. Every class-C miss this run is that: served=NO, held +0. PLOW_HOLD_MAX = float(globals().get("plow_hold_max", 3.0 * T_ZONE)) PLOW_X_LOG_HI = PLOW_SENSE_X + 0.20 # log window: a little before the sensor... PLOW_X_LOG_LO = C.PLOW_RELEASE_X - 0.20 # ...to a little past release print(f"\n===== PLOW TIMING (1 m/s, {PITCH*1000:.0f} mm pitch) =====") print(f" T_pitch (item spacing) = {T_PITCH:.3f} s") print(f" T_lead (sensor -> blade) = {T_LEAD:.3f} s") T_SWING_FULL = (2.0 * PLOW_ANGLE) / PLOW_RATE if PLOW_RATE else 0.0 print(f" T_swing (full B<->C reversal)= {T_SWING_FULL:.3f} s" + (" OK - blade arrives in time" if T_SWING_FULL < T_LEAD else " TOO SLOW - blade cannot arrive before the item does")) print(f" T_zone (in deflection zone)= {T_ZONE:.3f} s") if T_ZONE > T_PITCH: print(f" T_zone > T_pitch by {T_ZONE - T_PITCH:.3f} s: back-to-back opposite-class " f"items WILL overlap in the zone - this is geometry, not a rate problem.") print(f" PLOW_RATE = 2*{PLOW_ANGLE:.0f} / ({SWING_MARGIN}*{T_PITCH:.3f}) = {PLOW_RATE:.0f} deg/s " f"(config default {C.PLOW_SWEEP_RATE:.0f})") print(f" PLOW_RETURN_RATE = 3x PLOW_RATE = {PLOW_RETURN_RATE:.0f} deg/s (no overshoot risk " f"on an empty return, so it does not need the deflection swing's slower budget)") print(f" PUSH_SENSE_X = PUSH_X + blade_halfwidth = {PUSH_SENSE_X:.3f} (blade's own edge)") # C.PUSHER_MAX_SAFE (2.5 m/s) is a ceiling against throwing goods off the line, not a # measured-good speed - isolated single-item tests (pusher_diag*.py) found it FLICKS the # item (a brief velocity spike, then the blade outruns it: item ends up only 0.01-0.05 m # over against a 0.42 m commanded stroke). 0.6 m/s is too slow the other way - the item's # own belt-driven X motion carries it clean out of the blade's X window before the stroke # finishes. 1.3 m/s hit 0.407/0.42 m (97%) in the same isolated test - a real carry. # Contact-window arithmetic, measured not guessed. The blade spans 500 mm of belt, so at # 1 m/s an item is in front of it for only 0.50 s. The old 1.3 m/s over a 0.85 m stroke # takes 0.654 s: the pusher log showed the item entering at x=-3.83 and leaving at x=-4.42, # i.e. off the blade's trailing edge (-4.15) after ~0.33 s - barely half the stroke, giving # dy of only +0.17..+0.22 m against the ~0.5 m needed to reach the branch belt. Waiting for # the item to reach PUSH_X+0.08 first burned another 0.18 m of that window, so the stroke # now fires the instant the curtain sees the item. # stroke = BLADE_HOME_Y..PUSH_OUT_Y = 0.30 + 0.52 = 0.82 m # at 1.8 m/s that is 0.456 s < the 0.50 s window, with ~0.04 s of margin. PUSH_SPEED = 1.3 # best measured momentum transfer; the blade is sized for it above # The return leg carries nothing, so it does not need the carry speed: measured # 0.683 s at 1.3 m/s vs 0.367 s at 2.5 m/s over the same 0.85 m stroke. Getting the # blade home sooner is what lets a following D item be served at all. PUSH_RETURN_SPEED = C.PUSHER_MAX_SAFE # 2.5 m/s PUSH_OUT_Y = 0.52 # C.BLADE_OUT_Y (0.42) stops short of the branch belt's own start # (y=0.443, measured); 0.52 clears it with margin while keeping the # stroke short enough to finish inside the contact window above. SENSE_Y0, SENSE_Y1 = -0.45, 0.45 SENSE_RAYS = 121 GATE_WINDOW = 0.15 CONTAINER_B = (-8.81, 1.47) CONTAINER_C = (-10.45, -0.225) CONTAINER_R = 0.55 CONTAINER_Z = 1.30 # floor 1.14-1.18; anything below this is resting in the tray # real BinD geometry (/World/SortingRig/BinD_*), measured directly on this scene - # config.BIN_X0/X1/Y0/Y1 are the OLD sorter.usd's bin and do not apply here, same mistake # as SPAWN_X/BELTS earlier: every "shared" config constant needs re-verifying per scene. BIN_X0, BIN_X1 = -6.21, -4.95 BIN_Y0, BIN_Y1 = 1.57, 2.86 BIN_LIP_Z = 1.72 # ---------------------------------------------------------------- scene # NOT plow_cell_9045.load(): reopening this stage while the WebRTC stream is attached # races the background Hydra-populate thread and reliably throws 'Detected usd threading # violation' (measured over ~10 attempts here). The stage is already the right one # (confirmed via health_check) - prepare it in place instead. stage = omni.usd.get_context().get_stage() print("current stage:", stage.GetRootLayer().identifier) info = await plow_cell_9045.prepare(stage, belt_speed=SPEED, script_control=True) print(f"prepare: {info}") def _load_items(stage, names): """define each item, fully physics-ready (RigidBodyAPI, mass, CCD, collision), BEFORE the timeline ever plays - and NEVER change that API afterward. Two things measured broken in THIS session when tried during an already-playing simulation: (1) flipping kinematicEnabled True->False mid-play - the item's authored translate op and kinematic flag both "write" successfully (no exception) but the body never actually moves, forever kinematic in the solver's own copy of the actor; (2) applying UsdPhysics.RigidBodyAPI.Apply() fresh mid-play - same silent no-op. A plain xformOp:translate WRITE on a body that already has its RigidBodyAPI from before Play started, in contrast, is the pattern used successfully everywhere else in this project (mechanics.Cell.place/blade_to, the plow's own kinematic rotateZ) - so items get their physics now, sit on the new ground plane at their park slot, and are only ever teleported (never re-tagged) at release time. Each spawn is its own try/except: this Kit session raises even benign Tf warnings as exceptions (e.g. 'sneaker' fails on a float3-vs-double xformOp precision mismatch that Tf itself says it is proceeding past), so one bad mesh must not take the other ten down. """ items_dir = C.ROOT / "assets" / "items" UsdGeom.Xform.Define(stage, "/World/Items") ok = [] for i, name in enumerate(names): usd = items_dir / f"{name}.usd" try: prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() prim.GetReferences().ClearReferences() prim.GetReferences().AddReference(str(usd)) xf = UsdGeom.Xformable(prim) xf.ClearXformOpOrder() xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set( Gf.Vec3d(9.0 + 1.2 * i, 5.0, 0.4)) UsdPhysics.RigidBodyAPI.Apply(prim) # meshes exported from a streaming scene arrive kinematic (scene.py's own # load_test_items() docstring says so) - the referenced .usd itself authors # kinematicEnabled=True, so it must be forced False here explicitly, ONCE, # before Play. This is what was actually silently pinning every item in place # this whole time - not a mid-play toggle race, a stale authored default this # code never overrode. UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(False) UsdPhysics.MassAPI.Apply(prim).CreateMassAttr().Set(0.6) px = PhysxSchema.PhysxRigidBodyAPI.Apply(prim) px.CreateEnableCCDAttr().Set(True) px.CreateSolverPositionIterationCountAttr().Set(24) px.CreateSolverVelocityIterationCountAttr().Set(8) px.CreateSleepThresholdAttr().Set(0.0) # a settled item must still be draggable # bottle (tall, narrow, round) has been measured disappearing into the belt - # a contact-resolution/CCD tunnel, same failure mode plow_cell.py caps on the # plow arm with C.MAX_DEPENETRATION: an uncapped deep-penetration event lets # PhysX separate the overlap at whatever speed it likes, which can eject a # thin body clean through a thin collider in a single step. px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) for desc in Usd.PrimRange(prim): if desc.HasAPI(UsdPhysics.CollisionAPI): pxcol = PhysxSchema.PhysxCollisionAPI.Apply(desc) pxcol.CreateContactOffsetAttr().Set(0.004) # tighter than the ~5cm pxcol.CreateRestOffsetAttr().Set(0.001) # PhysX default for small items UsdGeom.Imageable(prim).MakeInvisible() # shown at release, not before ok.append(name) except BaseException as exc: print(f" WARNING: failed to load item {name!r}: {type(exc).__name__}") return ok loaded = _load_items(stage, ORDER) print(f"items loaded: {loaded}") ORDER = loaded # downstream code (spawn loop, report) only sees what actually loaded rp = {n: RigidPrim(paths=[f"/World/Items/{n}"]) for n in ORDER} # built now, physics is already live await app_utils.update_app_async(steps=20) plow = Plow(stage, kinematic=True) plow.home() query = get_physx_scene_query_interface() def _activate_item(name, x, y): """teleport + reveal an already-physics-ready item - see _load_items for why nothing else may change here once the timeline is playing.""" prim = stage.GetPrimAtPath(f"/World/Items/{name}") for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: op.Set(Gf.Vec3d(x, y, C.BELT_Z + 0.05)) break UsdGeom.Imageable(prim).MakeVisible() _last_pose = {} # name -> last successfully read pose, for when the tensor backend hiccups def item_pose(name): """`get_world_poses()` can raise 'Failed to get rigid body transforms from backend' if PhysX's tensor view for this actor is momentarily invalid (measured after a hard contact from the plow/pusher) - fall back to the last good read rather than crash the whole run over one body's one bad tick.""" try: p = rp[name].get_world_poses()[0].numpy()[0] _last_pose[name] = p return p except BaseException: if name in _last_pose: return _last_pose[name] raise # -- the pusher blade, driven directly (mechanics.Cell.blade_to/stroke, inlined - the # rest of Cell assumes the park/thaw item pattern this script deliberately does not use) def _blade_op(stage): prim = stage.GetPrimAtPath(_scene.BLADE) for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: return op raise RuntimeError(f"{_scene.BLADE} has no translate op") blade_op = _blade_op(stage) blade_base = blade_op.Get() def blade_to(y): b = blade_base blade_op.Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2])) blade_to(C.BLADE_HOME_Y) async def stroke(out=True, speed=None): """pace the blade by REAL elapsed sim time (tl.get_current_time()), not an assumed dt=1/60 - this scene's actual physics step has measured well under 60 Hz elsewhere in this project (verify_belts2.py found 83.33 ms, not 16.67 ms). Assuming 60 Hz here made each `update_app_async(steps=1)` cover several times the intended distance, so the blade arrived in a handful of big jumps instead of a smooth sweep - PROBE_PUSH_PHYSICS's own distinction between a genuine push (item picks up the blade's tangential speed) and a teleport (item gets a small depenetration nudge and stops dead): the user's own 'item stops in place on contact' report is exactly the teleport symptom.""" speed = min(speed or C.PUSHER_SPEED, C.PUSHER_MAX_SAFE) # C.BLADE_OUT_Y (0.42) lands 20 mm SHORT of where the branch belt (Belt_01) actually # starts (y=0.44, measured) - close enough that a pushed item straddles the boundary # and the main belt's -X drive keeps winning over the branch's +Y pull. PUSH_OUT_Y # gives real margin onto the branch instead of leaving it to a coin flip. a, b = (C.BLADE_HOME_Y, PUSH_OUT_Y) if out else (PUSH_OUT_Y, C.BLADE_HOME_Y) duration = abs(b - a) / max(speed, 1e-6) t0 = float(tl.get_current_time()) while True: u = min(1.0, (float(tl.get_current_time()) - t0) / max(duration, 1e-6)) blade_to(a + (b - a) * u) await app_utils.update_app_async(steps=1) if u >= 1.0: break def _curtain(x, exclude): near = any(abs(float(item_pose(n)[0]) - x) < GATE_WINDOW for n in ORDER if n not in exclude and n in rp) if not near: return None z0 = C.BELT_Z + 0.40 reach = 0.40 - 0.001 for i in range(SENSE_RAYS): y = SENSE_Y0 + (SENSE_Y1 - SENSE_Y0) * i / (SENSE_RAYS - 1) hit = query.raycast_closest([x, y, z0], [0.0, 0.0, -1.0], reach) if not hit or not hit.get("hit"): continue path = str(hit.get("rigidBody") or hit.get("collision") or "") for n in ORDER: if n in exclude: continue if f"/World/Items/{n}" in path: return n return None gate_log = [] push_swept = set() plow_swept = set() plow_pending = {} plow_active = None # (name, angle, commit_sim_t) the blade is currently committed to pushing = set() push_queue = [] # D items waiting for the blade to finish the item ahead of them push_log = [] # what the pusher actually did to each D item plow_trace = {n: [] for n in ORDER} # per-item kinematics while inside the sense-to-release sim_t = [0.0] # boxed so _step (no `global` needed) can advance it # ---------------------------------------------------------------- pusher state machine # Driven from the PHYSICS CALLBACK, exactly like the plow - not from an async coroutine. # That was the whole problem: `_do_push` used to `await update_app_async()` inside a task # fired by asyncio.ensure_future, while the main feed loop pumped the app too. With two # tasks pumping, the sim advanced further between consecutive blade_to() writes than the # stroke maths assumed, so the blade jumped in bigger steps - the teleport regime again. # Isolated (single pumper) the same blade+speed reached dy=+1.62; inside the full run it # managed +0.21. Advancing the blade by PUSH_SPEED*dt once per physics step removes the # dependency on who else is pumping. PUSH_HOLD_S = 0.15 # dwell at full extension before returning push_state = {"phase": "idle", "item": None, "y": C.BLADE_HOME_Y, "t": 0.0, "y0": 0.0, "x0": 0.0} def _push_begin(name): push_state.update(phase="out", item=name, t=0.0, y0=float(item_pose(name)[1]), x0=float(item_pose(name)[0])) pushing.add(name) def _push_step(dt): """advance the blade one physics step; returns nothing""" st = push_state if st["phase"] == "idle": return name = st["item"] if st["phase"] == "out": st["y"] = min(PUSH_OUT_Y, st["y"] + PUSH_SPEED * dt) blade_to(st["y"]) # Carry assist - the "impulse". A transform-driven kinematic blade transfers no # momentum of its own (PhysX sees a teleport, so the item gets only a # depenetration shove), which is why the bare blade plateaued at ~0.21 m. Rather # than one violent kick, the item's +Y velocity is matched to the blade's every # step while the blade is advancing: that is what a real carrying push does, and # it measured dy 1.62 -> 1.92 in isolation. X and Z are left alone so the belt # keeps driving it down the line normally. if name in rp: try: lin = rp[name].get_velocities()[0].numpy()[0] rp[name].set_velocities( np.array([[float(lin[0]), PUSH_SPEED, float(lin[2])]]), np.array([[0.0, 0.0, 0.0]])) except BaseException: pass if st["y"] >= PUSH_OUT_Y - 1e-6: st["phase"], st["t"] = "hold", 0.0 if name in rp: p = item_pose(name) push_log.append(dict(item=name, start_x=round(st["x0"], 3), start_y=round(st["y0"], 3), after_x=round(float(p[0]), 3), after_y=round(float(p[1]), 3), after_z=round(float(p[2]), 3), dy=round(float(p[1]) - st["y0"], 3))) elif st["phase"] == "hold": st["t"] += dt if st["t"] >= PUSH_HOLD_S: st["phase"] = "back" elif st["phase"] == "back": st["y"] = max(C.BLADE_HOME_Y, st["y"] - PUSH_RETURN_SPEED * dt) blade_to(st["y"]) if st["y"] <= C.BLADE_HOME_Y + 1e-6: pushing.discard(name) st.update(phase="idle", item=None) if push_queue: _push_begin(push_queue.pop(0)) def _step(dt): global plow_active sim_t[0] += dt try: # kinematics trace: every item still between the plow sensor and the release # point, every tick - what the plow tuning needs to actually be corrected from, # rather than re-guessed. Cheap: only items in this ~1.7 m window are sampled. for n in ORDER: if n not in rp: continue p = item_pose(n) x = float(p[0]) if PLOW_X_LOG_HI >= x >= PLOW_X_LOG_LO: plow_trace[n].append((round(sim_t[0], 4), round(x, 4), round(float(p[1]), 4), round(plow.commanded, 2), round(plow.angle, 2), n == (plow_active[0] if plow_active else None))) seen = _curtain(PUSH_SENSE_X, push_swept) if seen is not None: push_swept.add(seen) gate_log.append(("push", seen, CLASSES[seen])) if CLASSES[seen] == "D": if push_state["phase"] != "idle": push_queue.append(seen) else: _push_begin(seen) seen = _curtain(PLOW_SENSE_X, plow_swept) if seen is not None: plow_swept.add(seen) ang = float(PLOW_ANGLES.get(CLASSES[seen], 0.0)) gate_log.append(("plow", seen, CLASSES[seen], ang)) if abs(ang) > 1e-6: plow_pending[seen] = ang # Once the blade commits to an item, hold that angle until the item clears the # release point - a newer arrival with the opposite angle must NOT reassign the # target while the current item is still physically sliding along the blade, or # the blade reverses mid-deflection and both items end up misrouted (measured: # box_400x400x300 wanted +20, got dragged to container_B instead of C - a B item # 0.7 s ahead of it in the queue). # # PLOW_HOLD_MAX is a force-release timeout on top of the position check. The # kinematics log showed items occasionally taking 8-50 s to clear the zone # (expected ~T_zone=0.95s) - a deck-contact stick/jitter issue, not a plow one - # and while that is unresolved a position-only release leaves the blade locked # to one stalled item and unable to return to centre or serve anyone else for the # rest of the run. Releasing on a timeout keeps the blade responsive even when an # individual item is still slowly working itself loose behind it. if plow_active is not None: name, _, commit_t = plow_active x = float(item_pose(name)[0]) # release once the item is past the blade BODY (its downstream face), not the # further-downstream PLOW_RELEASE_X - by the blade's own trailing edge the # deflection has already happened and holding longer only starves the queue. if x < BLADE_TRAILING_X or (sim_t[0] - commit_t) > PLOW_HOLD_MAX: plow_active = None for n in list(plow_pending): if float(item_pose(n)[0]) < C.PLOW_RELEASE_X: # starved: it crossed release without ever being served its own angle - # picked up whatever the blade happened to be doing instead. Logged, not # silently dropped, because "nearest to PLOW_X" (the old rule below) could # cause exactly this: a stalled item still gets judged "far" while a NEWER # item that entered later but is moving normally overtakes it in raw # distance and keeps winning the slot - the case measured on lunchbox and # detergent, both starved behind an adjacent, slower-clearing C item. gate_log.append(("plow-starved", n, CLASSES[n], plow_pending[n])) plow_pending.pop(n, None) if plow_active is None and plow_pending: # FIFO, not nearest-to-PLOW_X: whichever item was DETECTED first is served # first. Nearest-distance let a normally-moving newer arrival leapfrog an # older one that had merely stalled a little, starving it (see above) - FIFO # cannot starve anyone, every pending item's turn always eventually comes. oldest = next(iter(plow_pending)) plow_active = (oldest, plow_pending.pop(oldest), sim_t[0]) if plow_active is not None: plow.step_toward(plow_active[1], dt, rate=PLOW_RATE) else: plow.step_toward(0.0, dt, rate=PLOW_RETURN_RATE) _push_step(dt) except BaseException as exc: # pxr.Tf.ErrorException (the stage-vs-Fabric sync race seen throughout this run) # derives from BaseException, not Exception - `except Exception` never sees it, and # missing one 1/60s physics tick of plow/sensor update is harmless; the next tick # retries on its own. gate_log.append(("step-error", "", repr(exc))) sub = get_physx_interface().subscribe_physics_step_events(_step) # tl.play()/tl.stop(), not app_utils.play()/stop(): pacing everything downstream off an # assumed 60 fps (steps=int(round(PITCH*60))) measured wrong on this scene before - the # timeline's actual step can run well under 60 Hz, so a "0.7 s" wait was really much # shorter and every item piled up at the entry belt instead of spreading out at 700 mm. # Pace off tl.get_current_time() instead, which is what verify_belts2.py/verify_plow2.py # (the only scripts that measured correct 1 m/s transport on this scene) actually do. tl = omni.timeline.get_timeline_interface() tl.play() await app_utils.update_app_async(steps=20) print("timeline playing:", tl.is_playing()) # ---------------------------------------------------------------- feed + shoot screenshots w = vp.get_active_viewport() shots = [] async def _shot(tag): await app_utils.update_app_async(steps=5) path = f"/tmp/plow9045_{tag}.png" vp.capture_viewport_to_file(w, file_path=path) await app_utils.update_app_async(steps=3) shots.append(path) async def _release(name): """_activate_item() during Play can still race the physics-Fabric sync thread the same way setup did, but a short retry is enough here - unlike the one-time setup race, this one resolves in a tick or two, and the loop's overall 0.7 s pitch tolerates jitter.""" for attempt in range(8): try: return _activate_item(name, plow_cell_9045.ENTRY_X, plow_cell_9045.ENTRY_Y) except BaseException: await app_utils.update_app_async(steps=2) return _activate_item(name, plow_cell_9045.ENTRY_X, plow_cell_9045.ENTRY_Y) # let it raise for real async def _wait_sim_seconds(seconds): """advance by SIM time, not an assumed frame count - this scene's actual physics step has measured well under 60 Hz before, and a fixed steps=N wait ran short as a result.""" target = float(tl.get_current_time()) + seconds while float(tl.get_current_time()) < target: await app_utils.update_app_async(steps=5) sim_t0 = float(tl.get_current_time()) for i, name in enumerate(ORDER): await _release(name) print(f" {i * PITCH:5.2f}s released {name} ({CLASSES[name]})") await _wait_sim_seconds(PITCH) if i == 0: await app_utils.update_app_async(steps=10) print(f" {name} position 0.1s+ after release: {item_pose(name)}" f" (spawned at {plow_cell_9045.ENTRY_X:.2f},{plow_cell_9045.ENTRY_Y:.2f}) " f"- should have moved if belts + gravity are live") if i % 3 == 0: await _shot(f"feed_{i:02d}_{name}") # ---------------------------------------------------------------- wait for everything to settle MAX_SECONDS = 60.0 settled = {} def _outcome(name): if name not in rp: return None p = item_pose(name) x, y, z = float(p[0]), float(p[1]), float(p[2]) if BIN_X0 < x < BIN_X1 and BIN_Y0 < y < BIN_Y1 and z < BIN_LIP_Z: return "bin_D" if abs(x - CONTAINER_B[0]) < CONTAINER_R and abs(y - CONTAINER_B[1]) < CONTAINER_R and z < CONTAINER_Z: return "container_B" if abs(x - CONTAINER_C[0]) < CONTAINER_R and abs(y - CONTAINER_C[1]) < CONTAINER_R and z < CONTAINER_Z: return "container_C" if z < C.BELT_Z - 0.5 and x > -8.3: return "floor" return None wall_t0 = time.time() wall_budget = 240.0 # backstop in case the timeline stalls entirely - don't hang forever while (float(tl.get_current_time()) - sim_t0 < MAX_SECONDS + len(ORDER) * PITCH and time.time() - wall_t0 < wall_budget): await app_utils.update_app_async(steps=30) for n in ORDER: if n in settled: continue w_ = _outcome(n) if w_ is not None: settled[n] = w_ if len(settled) >= len(ORDER): break await _shot("final") # tl.stop() resets every rigid body to its authored (pre-Play) transform - mechanics.py's # own docstring warns of exactly this ("capture renders while playing"). Read final poses # NOW, while still playing, or the report shows everyone back at their park slot. final_pos = {n: item_pose(n).copy() for n in ORDER} sub = None tl.stop() await app_utils.update_app_async(steps=10) # ---------------------------------------------------------------- report EXPECT = {"D": "bin_D", "B": "container_B", "C": "container_C"} print("\n===== GATE LOG (first item at each gate) =====") seen_gates = set() for entry in gate_log: key = (entry[0], entry[1]) if key in seen_gates: continue seen_gates.add(key) print(" ", entry) print("\n===== DELIVERY =====") ok_n = 0 by_class = {"B": [0, 0], "C": [0, 0], "D": [0, 0]} # class -> [correct, total] for name, cls in ITEMS: if name not in loaded: print(f" {name:18s} class={cls} -> SKIPPED (failed to load)") continue outcome = settled.get(name, "line/unresolved") want = EXPECT[cls] ok = outcome == want ok_n += ok by_class[cls][1] += 1 by_class[cls][0] += int(ok) p = final_pos[name] print(f" {name:18s} class={cls} -> {outcome:14s} want={want:14s} " f"{'OK' if ok else 'FAIL'} final=({float(p[0]):+.2f},{float(p[1]):+.2f},{float(p[2]):+.2f})") print(f"\ndelivered {ok_n}/{len(ITEMS)}") print("\n===== ACCURACY BY CLASS (plow: B/C, pusher: D) =====") for cls in ("B", "C", "D"): hit, total = by_class[cls] rate = hit / total if total else 0.0 print(f" {cls}: {hit}/{total} ({rate*100:.0f}%)") print(f"\nPLOW_RATE used this run: {PLOW_RATE:.0f} deg/s (config default {C.PLOW_SWEEP_RATE:.0f})") print(f"PLOW_RETURN_RATE used this run: {PLOW_RETURN_RATE:.0f} deg/s") print(f"PLOW_HOLD_MAX used this run: {PLOW_HOLD_MAX:.2f} s") print(f"PUSH_SENSE_X used this run: {PUSH_SENSE_X:.3f} (blade's own edge)") print(f"PLOW_ANGLE used this run: +-{PLOW_ANGLE:.0f} deg (config default 16)") print("\nscreenshots:", shots) # ---------------------------------------------------------------- kinematics log import json KIN_LOG = "/tmp/plow9045_kinematics.json" json.dump(dict( timing=dict(T_pitch=T_PITCH, T_lead=T_LEAD, T_zone=T_ZONE, plow_rate=PLOW_RATE, plow_angle=PLOW_ANGLE, push_speed=PUSH_SPEED), items=[dict(name=n, cls=CLASSES[n], target_angle=PLOW_ANGLES.get(CLASSES[n], 0.0), outcome=settled.get(n, "unresolved"), samples=plow_trace[n]) for n in ORDER], ), open(KIN_LOG, "w"), indent=1) print(f"\nkinematics log -> {KIN_LOG} ({sum(len(plow_trace[n]) for n in ORDER)} samples)") print("\n===== PUSHER LOG (class D) =====") if not push_log: print(" the pusher never fired - no D item was detected at the curtain") for e in push_log: print(f" {e['item']:18s} stroke start x={e['start_x']:+.3f} y={e['start_y']:+.3f}" f" ==> after stroke x={e['after_x']:+.3f} y={e['after_y']:+.3f} " f"z={e['after_z']:+.3f} dy={e['dy']:+.3f}") print("\n===== KINEMATICS SUMMARY (plow zone only) =====") for n in ORDER: tr = plow_trace[n] if not tr: print(f" {n:18s} never entered the logged zone") continue t0, x0, y0, cmd0, ang0, active0 = tr[0] t1, x1, y1, cmd1, ang1, active1 = tr[-1] lag = max(abs(c - a) for _, _, _, c, a, _ in tr) active_frac = sum(1 for row in tr if row[5]) / len(tr) want = PLOW_ANGLES.get(CLASSES[n], 0.0) # lateral deflection actually achieved across the zone, and whether the blade was # holding this item's OWN angle when it mattered - the two numbers the angle/rate # tuning has to be read from. served = "yes" if abs(ang1 - want) < 5.0 else f"NO (held {ang1:+.0f})" print(f" {n:18s} cls={CLASSES[n]} want={want:+.0f} dy={y1-y0:+.3f}m " f"served={served:14s} active={active_frac*100:3.0f}% " f"dwell={t1-t0:.2f}s (T_pitch={T_PITCH:.2f}s)")