Сортировочная ячейка Isaac Sim: CV-пайплайн и меши товаров
Замкнутый контур "поток -> CV -> механика": товары идут по конвейеру с шагом 700 мм, класс определяется стереопайплайном во время движения, пушер и плуг реагируют физически. Состав: * control_test/ - ячейка и CV. run_sorting_cv.py + cv_worker.py (два процесса, потому что torch внутри Isaac роняет сцену), cell.py (физика лент, плуга, пушера), measure_plane.py (замер габаритов), README.md и .memory.md с замерами, проблемами и ловушками * robozon_sorter/ - модули симуляции, scripts/ - утилиты, scene/ - сцены * assets/ - меши товаров, плуг, объекты Objaverse Бейзлайн CV: DEFOM-Stereo vitl, вход 480, iters 24, кроп зоны осмотра, без сегментации. На потоке 700 мм - классы 8/9, габариты MAE 32.8 мм, 469 мс на товар при такте 700 мс. Веса моделей (4.5 ГБ) и пропсы конвейера NVIDIA (274 МБ) не включены - источники и команды скачивания в MODELS.md. Выход прогонов (captures/, runtime/) не включён: воспроизводится. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
dasha_f
@@ -0,0 +1,732 @@
|
||||
"""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)")
|
||||
Reference in New Issue
Block a user