Сортировочная ячейка 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,527 @@
|
||||
"""control_test - run the sorting cell against whatever meshes are sitting in items/.
|
||||
|
||||
isaacsim_send.py --context ct --file control_test/run_pipeline.py
|
||||
isaacsim_send.py --context ct --file control_test/run_pipeline.py \
|
||||
--args-json '{"only": ["bag","backpack","lunchbox"], "pitch": 1.4}'
|
||||
|
||||
Every .usd in items/ is measured (control_test/classify.py) and classified by the
|
||||
documented rules, then fed onto the line in sorted order at PITCH spacing. Nothing is
|
||||
hard-coded per object: drop a mesh in the folder and it joins the next run.
|
||||
|
||||
D round, dimensions in envelope -> laser at the pusher -> blade -> Belt_01 -> BinD
|
||||
C undersize or oversize -> plow +PLOW_ANGLE -> ConveyorTrack_01 -> container C
|
||||
B in envelope, not round -> plow -PLOW_ANGLE -> ConveyorTrack_06 -> container B
|
||||
|
||||
Injectable args: pitch, speed, plow_angle, swing_margin, plow_hold_max, only, limit.
|
||||
"""
|
||||
import asyncio
|
||||
import pathlib
|
||||
import sys
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import omni.timeline
|
||||
import omni.usd
|
||||
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
|
||||
|
||||
# the python_server exec's this file as a string, so __file__ does not exist here;
|
||||
# `control_test_dir` can be injected to run the folder from somewhere else.
|
||||
HERE = pathlib.Path(globals().get("control_test_dir",
|
||||
"/home/dasha/robozon-sorter/control_test")).resolve()
|
||||
for extra in (str(HERE), "/home/dasha/robozon-sorter"):
|
||||
if extra not in sys.path:
|
||||
sys.path.insert(0, extra)
|
||||
for _m in [k for k in list(sys.modules) if k.startswith(("robozon_sorter", "cell", "classify"))]:
|
||||
del sys.modules[_m]
|
||||
import importlib
|
||||
importlib.invalidate_caches()
|
||||
|
||||
import cell
|
||||
import classify as CL
|
||||
from robozon_sorter import config as C
|
||||
from robozon_sorter.sim.plow import Plow
|
||||
|
||||
# ---------------------------------------------------------------- knobs
|
||||
SPEED = float(globals().get("speed", 1.0)) # belt m/s
|
||||
# 700 mm, the spec figure. Workable because the blade is only OWNED while an item is
|
||||
# physically on it (0.63 s), leaving a 0.07 s gap to change angle - see T_GAP below.
|
||||
PITCH = float(globals().get("pitch", 0.70)) # the spec figure
|
||||
PLOW_ANGLE = float(globals().get("plow_angle", 20.0))
|
||||
SWING_MARGIN = float(globals().get("swing_margin", 0.25))
|
||||
ONLY = globals().get("only") # optional list of item names
|
||||
LIMIT = int(globals().get("limit", 0)) # optional cap on how many to run
|
||||
|
||||
PLOW_SENSE_X = -6.30 # ~1.0 m of lead on the blade body at -7.32
|
||||
PUSH_SENSE_X = C.PUSH_X + cell.PUSHER_X_MM / 2000.0
|
||||
# The blade must be back home before the NEXT item reaches it, i.e. the whole
|
||||
# out-hold-return cycle has to fit inside T_pitch. At 0.70 m pitch that is 0.70 s, and the
|
||||
# old 1.3 m/s / 0.15 s dwell cycle took 0.63 + 0.15 + 0.33 = 1.11 s - the blade was still
|
||||
# in the lane when the following B/C item arrived, which knocked detergent onto the floor
|
||||
# and stopped lunchbox dead against it.
|
||||
#
|
||||
# Moving the blade fast no longer costs transfer, because the carry-assist below drives
|
||||
# the item rather than the blade face doing it: the assist velocity is set independently
|
||||
# (PUSH_ASSIST_V) so the blade can clear the lane quickly while the item still gets a
|
||||
# controlled 1.6 m/s across.
|
||||
# cycle = 0.77/2.5 + 0.03 + 0.77/2.5 = 0.31 + 0.03 + 0.31 = 0.65 s < 0.70 s
|
||||
PUSH_SPEED, PUSH_RETURN_SPEED, PUSH_OUT_Y, PUSH_HOLD_S = 2.5, 2.5, 0.52, 0.03
|
||||
PUSH_HOME_Y = -0.25 # was config's -0.30; the blade face still clears a 400 mm item
|
||||
# +Y velocity handed to the item while the blade advances. The out phase is only
|
||||
# (0.52+0.25)/2.5 = 0.31 s, and belt friction eats part of it: 1.6 m/s measured just
|
||||
# dy=+0.40 m, short of the branch belt's near edge at y=0.443, so class-D items stayed on
|
||||
# the main line and were caught by the plow instead. Scaled up to clear it with margin.
|
||||
# Calibrated on this cell, not guessed: 1.6 m/s -> dy +0.40 m (short of the branch belt's
|
||||
# near edge at y=0.443, item stays on the line); 2.4 m/s -> dy +0.69 m (shoots clean over
|
||||
# Belt_01, which ends at y=2.169, and lands on the floor). The response is close to linear
|
||||
# in between, so ~1.95 m/s puts the item at dy~0.52 - just past the near edge, nowhere near
|
||||
# the far one, and the branch belt takes it from there.
|
||||
PUSH_ASSIST_V = 2.1
|
||||
SENSE_Y0, SENSE_Y1, SENSE_RAYS, GATE_WINDOW = -0.45, 0.45, 121, 0.15
|
||||
BLADE_LEADING_X, BLADE_TRAILING_X = -7.32, -7.95
|
||||
BLADE_GUARD = 0.08 # keep the slow rate while an item is this close to the blade
|
||||
|
||||
T_PITCH = PITCH / SPEED
|
||||
# How long an item ACTUALLY owns the blade: the blade body is 0.63 m long, so at 1 m/s an
|
||||
# item is against it for 0.63 s. (config's PLOW_SWEEP_X0..PLOW_RELEASE_X spans 0.95 m and
|
||||
# was used as "T_zone" before - that is the wider sweep WINDOW, not the blade, and taking
|
||||
# it as the occupancy is what made a 0.70 m pitch look geometrically impossible.)
|
||||
T_BLADE = (BLADE_LEADING_X - BLADE_TRAILING_X) / SPEED
|
||||
# Item N leaves the blade T_BLADE after reaching it; item N+1 reaches it T_PITCH after N.
|
||||
# The blade is therefore EMPTY for this long between two consecutive items, and that is
|
||||
# the whole budget for a class change.
|
||||
T_GAP = T_PITCH - T_BLADE
|
||||
# Rate needed for the worst case (a full B<->C reversal) inside that gap, x1.5 margin.
|
||||
# Swinging this fast is safe because it only happens while the blade is empty - there is
|
||||
# nothing against it to bat. While an item IS on the blade the slower PLOW_RATE is used.
|
||||
PLOW_REPOSITION_RATE = min(2000.0, (2.0 * PLOW_ANGLE) / max(T_GAP, 0.02) * 1.5)
|
||||
PLOW_RATE = (2.0 * PLOW_ANGLE) / (SWING_MARGIN * T_PITCH)
|
||||
PLOW_ANGLES = {"B": -PLOW_ANGLE, "C": PLOW_ANGLE, "D": 0.0}
|
||||
|
||||
CONTAINER_B, CONTAINER_C, CONTAINER_R = (-8.81, 1.47), (-10.45, -0.225), 0.55
|
||||
# An item counts as delivered only if it is RESTING IN the tray, not merely above its
|
||||
# x/y footprint: the tray floors sit at z 1.14..1.18 and stand on legs, so a `z < 1.30`
|
||||
# test alone also accepts an item lying on the ground under the container. That is exactly
|
||||
# what hid this bug - B items were reported OK at z=+0.00 while sitting on the floor.
|
||||
CONTAINER_Z_MIN, CONTAINER_Z_MAX = 1.10, 1.72
|
||||
BIN_Z_MIN = 1.15
|
||||
BIN_X0, BIN_X1, BIN_Y0, BIN_Y1, BIN_LIP_Z = -6.21, -4.95, 1.57, 2.86, 1.72
|
||||
EXPECT = {"D": "bin_D", "B": "container_B", "C": "container_C"}
|
||||
|
||||
# ---------------------------------------------------------------- item library
|
||||
raw = CL.load_library(HERE / "items")
|
||||
lib = [r for r in raw if "error" not in r]
|
||||
skipped = [r for r in raw if "error" in r]
|
||||
if ONLY:
|
||||
lib = [r for r in lib if r["name"] in set(ONLY)]
|
||||
if LIMIT:
|
||||
lib = lib[:LIMIT]
|
||||
if not lib:
|
||||
raise SystemExit("no usable meshes in items/")
|
||||
|
||||
print(f"===== ITEM LIBRARY ({HERE / 'items'}, classes from labels.json) =====")
|
||||
print(f"{'item':>20} {'dims mm (label)':>24} {'k':>6} class")
|
||||
for r in lib:
|
||||
print(f"{r['name']:>20} {str(r['dims_mm']):>24} {r['k']:>6.3f} {r['cls']}")
|
||||
if skipped:
|
||||
print(f" skipped, no entry in labels.json: {[r['name'] for r in skipped]}")
|
||||
# a label whose own dims/k imply another class would surface as a baffling mechanical
|
||||
# failure, so it is caught here instead
|
||||
bad = CL.verify_labels(HERE / "items")
|
||||
if bad:
|
||||
print(" WARNING - labels inconsistent with the documented rules:")
|
||||
for b in bad:
|
||||
print(f" {b['name']}: labelled {b['labelled']}, rules imply {b['implied']} "
|
||||
f"(dims={b['dims_mm']} k={b['k']})")
|
||||
else:
|
||||
print(" labels consistent with the documented B/C/D rules")
|
||||
from collections import Counter
|
||||
print(" totals:", dict(Counter(r["cls"] for r in lib)))
|
||||
|
||||
ORDER = [r["name"] for r in lib]
|
||||
CLASSES = {r["name"]: r["cls"] for r in lib}
|
||||
PATHS = {r["name"]: r["path"] for r in lib}
|
||||
|
||||
print(f"\n===== TIMING =====")
|
||||
print(f" T_pitch (item spacing) = {T_PITCH:.3f} s")
|
||||
print(f" T_blade (item on the blade) = {T_BLADE:.3f} s")
|
||||
print(f" T_gap (blade empty between) = {T_GAP:.3f} s")
|
||||
print(f" reposition rate needed = {PLOW_REPOSITION_RATE:.0f} deg/s "
|
||||
f"(hold rate {PLOW_RATE:.0f} deg/s)")
|
||||
PUSH_CYCLE = (PUSH_OUT_Y - PUSH_HOME_Y) / PUSH_SPEED + PUSH_HOLD_S + \
|
||||
(PUSH_OUT_Y - PUSH_HOME_Y) / PUSH_RETURN_SPEED
|
||||
print(f" pusher cycle (out+hold+back) = {PUSH_CYCLE:.3f} s"
|
||||
+ (" OK - clears the lane before the next item"
|
||||
if PUSH_CYCLE < T_PITCH else
|
||||
f" TOO SLOW - blade still in the lane when the next item arrives"))
|
||||
if T_GAP <= 0:
|
||||
print(f" IMPOSSIBLE: an item still owns the blade when the next arrives. "
|
||||
f"Need pitch > {T_BLADE*SPEED:.2f} m at {SPEED} m/s.")
|
||||
|
||||
# ---------------------------------------------------------------- scene
|
||||
stage = omni.usd.get_context().get_stage()
|
||||
if not stage or "plow_cell_90_45" not in (stage.GetRootLayer().identifier or ""):
|
||||
raise SystemExit(f"open {cell.SCENE} first - see control_test/README.md")
|
||||
info = await cell.prepare(stage, belt_speed=SPEED, script_control=True)
|
||||
print(f"\nprepare: belts={len(info['belts'])} graphs_removed={len(info['graphs_removed'])} "
|
||||
f"junction_opened={info['junction_opened']} rails={info['rails']} "
|
||||
f"pusher={info['pusher_dims'][0]:.2f} m")
|
||||
|
||||
ITEMS_ROOT = "/World/CtrlItems"
|
||||
|
||||
|
||||
def _load(names):
|
||||
"""define every item physics-ready BEFORE play; never re-tag a body afterwards"""
|
||||
UsdGeom.Xform.Define(stage, ITEMS_ROOT)
|
||||
ok = []
|
||||
for i, name in enumerate(names):
|
||||
try:
|
||||
prim = UsdGeom.Xform.Define(stage, f"{ITEMS_ROOT}/{name}").GetPrim()
|
||||
prim.GetReferences().ClearReferences()
|
||||
prim.GetReferences().AddReference(PATHS[name])
|
||||
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)
|
||||
# exported meshes arrive kinematic; force dynamic once, before play
|
||||
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)
|
||||
px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION)
|
||||
for d in Usd.PrimRange(prim):
|
||||
if d.HasAPI(UsdPhysics.CollisionAPI):
|
||||
pc = PhysxSchema.PhysxCollisionAPI.Apply(d)
|
||||
pc.CreateContactOffsetAttr().Set(0.004)
|
||||
pc.CreateRestOffsetAttr().Set(0.001)
|
||||
UsdGeom.Imageable(prim).MakeInvisible()
|
||||
ok.append(name)
|
||||
except BaseException as exc:
|
||||
print(f" WARNING: {name} failed to load: {type(exc).__name__}")
|
||||
return ok
|
||||
|
||||
|
||||
ORDER = _load(ORDER)
|
||||
rp = {n: RigidPrim(paths=[f"{ITEMS_ROOT}/{n}"]) for n in ORDER}
|
||||
# ONE view over every item: get_world_poses() then costs a single backend round-trip
|
||||
# instead of nine. Benchmarked on this scene: 0.746 ms for nine separate reads vs
|
||||
# 0.081 ms batched. The physics callback used to do ~36 separate reads per step (trace
|
||||
# loop + two curtains + the plow schedule), i.e. ~3 ms of a 16.7 ms budget spent almost
|
||||
# entirely on GPU round-trips that also stall the render thread feeding WebRTC.
|
||||
items_view = RigidPrim(paths=[f"{ITEMS_ROOT}/{n}" for n in ORDER])
|
||||
_pose_cache = {}
|
||||
|
||||
|
||||
def refresh_poses():
|
||||
try:
|
||||
arr = items_view.get_world_poses()[0].numpy()
|
||||
for i, n in enumerate(ORDER):
|
||||
_pose_cache[n] = arr[i]
|
||||
_last[n] = arr[i]
|
||||
except BaseException:
|
||||
pass
|
||||
await app_utils.update_app_async(steps=20)
|
||||
|
||||
plow = Plow(stage, kinematic=True)
|
||||
plow.home()
|
||||
query = get_physx_scene_query_interface()
|
||||
_last = {}
|
||||
|
||||
|
||||
def item_pose(name):
|
||||
"""cached: refresh_poses() fills the whole cache in one call per step"""
|
||||
if name in _pose_cache:
|
||||
return _pose_cache[name]
|
||||
refresh_poses()
|
||||
if name in _pose_cache:
|
||||
return _pose_cache[name]
|
||||
if name in _last:
|
||||
return _last[name]
|
||||
raise KeyError(name)
|
||||
|
||||
|
||||
def activate(name):
|
||||
prim = stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}")
|
||||
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
|
||||
op.Set(Gf.Vec3d(cell.ENTRY_X, cell.ENTRY_Y, C.BELT_Z + 0.05))
|
||||
break
|
||||
UsdGeom.Imageable(prim).MakeVisible()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- pusher blade
|
||||
blade_prim = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher")
|
||||
blade_op = next(o for o in UsdGeom.Xformable(blade_prim).GetOrderedXformOps()
|
||||
if o.GetOpType() == UsdGeom.XformOp.TypeTranslate)
|
||||
blade_base = blade_op.Get()
|
||||
BLADE_PARENT_Y = -0.35
|
||||
|
||||
|
||||
def blade_to(y):
|
||||
blade_op.Set(Gf.Vec3d(blade_base[0], y - BLADE_PARENT_Y, blade_base[2]))
|
||||
|
||||
|
||||
blade_to(PUSH_HOME_Y)
|
||||
|
||||
gate_log, push_log = [], []
|
||||
push_swept, plow_swept, push_queue = set(), set(), []
|
||||
plow_done = set()
|
||||
sim_t = [0.0]
|
||||
push_state = {"phase": "idle", "item": None, "y": PUSH_HOME_Y, "t": 0.0, "y0": 0.0}
|
||||
|
||||
|
||||
def _curtain(x, exclude):
|
||||
if not any(abs(float(item_pose(n)[0]) - x) < GATE_WINDOW
|
||||
for n in ORDER if n not in exclude and n in rp):
|
||||
return None
|
||||
z0, reach = C.BELT_Z + 0.40, 0.399
|
||||
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 not in exclude and f"{ITEMS_ROOT}/{n}" in path:
|
||||
return n
|
||||
return None
|
||||
|
||||
|
||||
def _push_begin(name):
|
||||
push_state.update(phase="out", item=name, t=0.0, y0=float(item_pose(name)[1]))
|
||||
|
||||
|
||||
def _push_step(dt):
|
||||
"""the pusher, advanced from the PHYSICS step - never from an async task. Driving it
|
||||
from a coroutine that pumps the app concurrently with the feed loop made the blade
|
||||
jump further per sim-step than the stroke maths assumed, and the transfer collapsed
|
||||
from ~0.8 m to ~0.2 m."""
|
||||
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: a transform-driven kinematic blade imparts no momentum (PhysX sees
|
||||
# a teleport), so the item's +Y velocity is matched to the blade's each step - a
|
||||
# carry, not a single kick. Measured dy 0.21 m -> 0.80 m.
|
||||
if name in rp:
|
||||
try:
|
||||
lin = rp[name].get_velocities()[0].numpy()[0]
|
||||
rp[name].set_velocities(np.array([[float(lin[0]), PUSH_ASSIST_V, 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
|
||||
p = item_pose(name)
|
||||
push_log.append(dict(item=name, dy=round(float(p[1]) - st["y0"], 3),
|
||||
x=round(float(p[0]), 3), y=round(float(p[1]), 3)))
|
||||
elif st["phase"] == "hold":
|
||||
st["t"] += dt
|
||||
if st["t"] >= PUSH_HOLD_S:
|
||||
st["phase"] = "back"
|
||||
else:
|
||||
st["y"] = max(PUSH_HOME_Y, st["y"] - PUSH_RETURN_SPEED * dt)
|
||||
blade_to(st["y"])
|
||||
if st["y"] <= PUSH_HOME_Y + 1e-6:
|
||||
st.update(phase="idle", item=None)
|
||||
if push_queue:
|
||||
_push_begin(push_queue.pop(0))
|
||||
|
||||
|
||||
RUN_ACTIVE = [True]
|
||||
|
||||
|
||||
def _step(dt):
|
||||
# If the script dies mid-run the subscription can outlive it, and a callback still
|
||||
# touching dead RigidPrims while the timeline keeps playing is what left the app
|
||||
# pinned at full load and unreachable - which reads exactly like a crash. The finally
|
||||
# block below clears this flag no matter how the run ends.
|
||||
if not RUN_ACTIVE[0]:
|
||||
return
|
||||
sim_t[0] += dt
|
||||
refresh_poses() # one batched read; everything below uses the cache
|
||||
try:
|
||||
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":
|
||||
(push_queue.append if push_state["phase"] != "idle" else _push_begin)(seen)
|
||||
|
||||
seen = _curtain(PLOW_SENSE_X, plow_swept)
|
||||
if seen is not None:
|
||||
plow_swept.add(seen)
|
||||
gate_log.append(("plow", seen, CLASSES[seen],
|
||||
float(PLOW_ANGLES.get(CLASSES[seen], 0.0))))
|
||||
|
||||
# Purely POSITION-DRIVEN, no commit / hold / timeout - so nothing can be starved.
|
||||
# Whoever is physically on the blade owns the angle; the instant they clear its
|
||||
# trailing edge the blade is free and snaps to the next arrival's angle. The old
|
||||
# scheme committed at the sensor (x -6.30) and held to x -7.95, occupying the
|
||||
# blade for 1.65 s - 2.4 items' worth at a 0.70 m pitch, which is what starved
|
||||
# everyone behind and forced the pitch up to 1.4 m.
|
||||
on_blade = nearest = None
|
||||
for n in ORDER:
|
||||
if n not in rp or n in plow_done:
|
||||
continue
|
||||
x = float(item_pose(n)[0])
|
||||
if x < BLADE_TRAILING_X:
|
||||
plow_done.add(n)
|
||||
elif x <= BLADE_LEADING_X + BLADE_GUARD: # on the blade, or close enough
|
||||
if on_blade is None or x < on_blade[0]: # that a fast snap would bat it
|
||||
on_blade = (x, n) # deepest in = furthest along
|
||||
elif n in plow_swept: # sensed, still approaching
|
||||
if nearest is None or x < nearest[0]:
|
||||
nearest = (x, n) # smallest x = closest to the blade
|
||||
target = on_blade or nearest
|
||||
ang = float(PLOW_ANGLES.get(CLASSES[target[1]], 0.0)) if target else 0.0
|
||||
plow.step_toward(ang, dt,
|
||||
rate=PLOW_RATE if on_blade else PLOW_REPOSITION_RATE)
|
||||
_push_step(dt)
|
||||
except BaseException as exc:
|
||||
gate_log.append(("step-error", "", repr(exc)))
|
||||
|
||||
|
||||
sub = get_physx_interface().subscribe_physics_step_events(_step)
|
||||
tl = omni.timeline.get_timeline_interface()
|
||||
|
||||
# Everything from Play onward runs inside try/finally. Without it, any error in the run -
|
||||
# and several happened while building this - left the physics callback subscribed AND the
|
||||
# timeline playing, so the app kept simulating at full tilt with stale references. The
|
||||
# python_server then could not answer, WebRTC dropped with NVST_R_BUSY, and the whole
|
||||
# thing looked like a crash when it was really a leaked run.
|
||||
try:
|
||||
tl.play()
|
||||
await app_utils.update_app_async(steps=20)
|
||||
|
||||
w = vp.get_active_viewport()
|
||||
|
||||
|
||||
async def _wait(sec):
|
||||
target = float(tl.get_current_time()) + sec
|
||||
while float(tl.get_current_time()) < target:
|
||||
await app_utils.update_app_async(steps=5)
|
||||
await asyncio.sleep(0) # hand the event loop back to the
|
||||
# WebRTC streamer and the python server. Pumping update_app_async back-to-back for
|
||||
# the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client
|
||||
# disconnected from WebRTC server' on every run, and a second client cannot connect
|
||||
# at all - from outside that is indistinguishable from Isaac Sim having crashed.
|
||||
|
||||
|
||||
print(f"\n===== FEED ({len(ORDER)} items, {PITCH*1000:.0f} mm pitch @ {SPEED} m/s) =====")
|
||||
sim_t0 = float(tl.get_current_time())
|
||||
for i, name in enumerate(ORDER):
|
||||
for _ in range(8):
|
||||
try:
|
||||
activate(name)
|
||||
break
|
||||
except BaseException:
|
||||
await app_utils.update_app_async(steps=2)
|
||||
print(f" {i*PITCH/SPEED:6.2f}s {name} ({CLASSES[name]})")
|
||||
await _wait(PITCH / SPEED)
|
||||
|
||||
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 BIN_Z_MIN < z < BIN_LIP_Z:
|
||||
return "bin_D"
|
||||
for tag, (cx, cy) in (("container_B", CONTAINER_B), ("container_C", CONTAINER_C)):
|
||||
if (abs(x - cx) < CONTAINER_R and abs(y - cy) < CONTAINER_R
|
||||
and CONTAINER_Z_MIN < z < CONTAINER_Z_MAX):
|
||||
return tag
|
||||
if z < 0.25: # ended up on the ground, wherever that was
|
||||
return "floor"
|
||||
return None
|
||||
|
||||
|
||||
wall0 = time.time()
|
||||
while (float(tl.get_current_time()) - sim_t0 < 60.0 + len(ORDER) * PITCH / SPEED
|
||||
and time.time() - wall0 < 90.0):
|
||||
await app_utils.update_app_async(steps=10)
|
||||
await asyncio.sleep(0) # hand the event loop back to the
|
||||
# WebRTC streamer and the python server. Pumping update_app_async back-to-back for
|
||||
# the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client
|
||||
# disconnected from WebRTC server' on every run, and a second client cannot connect
|
||||
# at all - from outside that is indistinguishable from Isaac Sim having crashed.
|
||||
for n in ORDER:
|
||||
if n not in settled and (w_ := outcome(n)) is not None:
|
||||
settled[n] = w_
|
||||
if len(settled) >= len(ORDER):
|
||||
break
|
||||
|
||||
await app_utils.update_app_async(steps=5)
|
||||
await asyncio.sleep(0) # hand the event loop back to the
|
||||
# WebRTC streamer and the python server. Pumping update_app_async back-to-back for
|
||||
# the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client
|
||||
# disconnected from WebRTC server' on every run, and a second client cannot connect
|
||||
# at all - from outside that is indistinguishable from Isaac Sim having crashed.
|
||||
vp.capture_viewport_to_file(w, file_path="/tmp/control_test_final.png")
|
||||
await app_utils.update_app_async(steps=5)
|
||||
await asyncio.sleep(0) # hand the event loop back to the
|
||||
# WebRTC streamer and the python server. Pumping update_app_async back-to-back for
|
||||
# the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client
|
||||
# disconnected from WebRTC server' on every run, and a second client cannot connect
|
||||
# at all - from outside that is indistinguishable from Isaac Sim having crashed.
|
||||
final = {n: item_pose(n).copy() for n in ORDER}
|
||||
sub = None
|
||||
tl.stop()
|
||||
await app_utils.update_app_async(steps=10)
|
||||
await asyncio.sleep(0) # hand the event loop back to the
|
||||
# WebRTC streamer and the python server. Pumping update_app_async back-to-back for
|
||||
# the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client
|
||||
# disconnected from WebRTC server' on every run, and a second client cannot connect
|
||||
# at all - from outside that is indistinguishable from Isaac Sim having crashed.
|
||||
|
||||
|
||||
finally:
|
||||
RUN_ACTIVE[0] = False
|
||||
sub = None
|
||||
try:
|
||||
tl.stop()
|
||||
except BaseException:
|
||||
pass
|
||||
await app_utils.update_app_async(steps=10)
|
||||
await asyncio.sleep(0) # hand the event loop back to the
|
||||
# WebRTC streamer and the python server. Pumping update_app_async back-to-back for
|
||||
# the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client
|
||||
# disconnected from WebRTC server' on every run, and a second client cannot connect
|
||||
# at all - from outside that is indistinguishable from Isaac Sim having crashed.
|
||||
print("cleanup: physics callback released, timeline stopped")
|
||||
|
||||
print("\n===== DELIVERY =====")
|
||||
ok_n = 0
|
||||
by_class = {"B": [0, 0], "C": [0, 0], "D": [0, 0]}
|
||||
for name in ORDER:
|
||||
cls = CLASSES[name]
|
||||
got, want = settled.get(name, "line/unresolved"), EXPECT[cls]
|
||||
ok = got == want
|
||||
ok_n += ok
|
||||
by_class[cls][1] += 1
|
||||
by_class[cls][0] += int(ok)
|
||||
p = final[name]
|
||||
print(f" {name:20s} {cls} -> {got:16s} want={want:14s} {'OK' if ok else 'FAIL'}"
|
||||
f" ({float(p[0]):+.2f},{float(p[1]):+.2f},{float(p[2]):+.2f})")
|
||||
print(f"\ndelivered {ok_n}/{len(ORDER)}")
|
||||
for c in ("B", "C", "D"):
|
||||
h, t = by_class[c]
|
||||
print(f" {c}: {h}/{t}" + (f" ({100*h/t:.0f}%)" if t else ""))
|
||||
if push_log:
|
||||
print("\n===== PUSHER =====")
|
||||
for e in push_log:
|
||||
print(f" {e['item']:20s} dy={e['dy']:+.3f} -> ({e['x']:+.2f},{e['y']:+.2f})")
|
||||
print("\nscreenshot: /tmp/control_test_final.png")
|
||||
Reference in New Issue
Block a user