dasha_f/isaac
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Сортировочная ячейка Isaac Sim: CV-пайплайн и меши товаров

Browse Source 
Замкнутый контур "поток -> 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
2026-08-01 13:07:24 +00:00
parent 6ce460378a
commit 0d32f32db0
342 changed files with 18000 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/add_vision_to_plow_cell.py
+161
View File
@@ -0,0 +1,161 @@
#!/usr/bin/env python3
"""Give scene/plow_cell.usd an infeed belt, the camera portal and the laser gate.
python scripts/add_vision_to_plow_cell.py
Purely additive: the conveyors, the Y-split pusher and the plow are left exactly as
authored, including their drives and the DiverterAnimGraph. Nothing existing is edited,
so the cell's kinematics are untouched.
What gets added
ConveyorTrack_05 a fourth infeed belt, upstream of ConveyorTrack_02. It references the
SAME ConveyorBelt_A06.usd with the same scale as the existing tracks,
so it is the same conveyor with the same textures, not a lookalike.
Goods run -X, so "behind" ConveyorTrack_02 (which spans x -2..0) means
x 0..+2.
CameraMountFrame the portal, camera bodies and the three rectified stereo pairs, copied
CameraBodies verbatim from sorter.usd. They already sit over x=-0.75, which is
/RigRS inside ConveyorTrack_02's belt, so no repositioning is needed.
SortingRig/LaserGate the through-beam gate at the Y-split pusher (x=-3.74).
Re-running is safe: existing prims are replaced rather than duplicated.
"""
from __future__ import annotations
import shutil
import sys
from pathlib import Path
from pxr import Gf, Sdf, Usd, UsdGeom
ROOT = Path(__file__).resolve().parent.parent
CELL = ROOT / "scene" / "plow_cell.usd"
SORTER = ROOT / "scene" / "sorter.usd"
# copied from sorter.usd unchanged - they are already aligned with this belt
FROM_SORTER = [
"/World/CameraMountFrame",
"/World/CameraBodies",
"/RigRS",
"/World/SortingRig", # carries LaserGate (and its materials)
]
# sorter.usd's SortingRig carries a plain box SpawnBelt at x 0..2.6. The real conveyor
# added below occupies the same lane, so the box and its rails/legs are dropped after the
# copy - otherwise two belts sit inside each other. LaserGate, the bin and the materials stay.
DROP_AFTER_COPY = [
"/World/SortingRig/SpawnBelt",
"/World/SortingRig/SpawnRail_p", "/World/SortingRig/SpawnRail_n",
"/World/SortingRig/Spawn_Leg0", "/World/SortingRig/Spawn_Leg1",
"/World/SortingRig/Spawn_Leg2", "/World/SortingRig/Spawn_Leg3",
]
INFEED_PRIM = "/World/ConveyorTrack_05"
INFEED_ASSET = "../assets/conveyors/ConveyorBelt_A06.usd"
# ConveyorTrack_02 sits at translate x=-2 and spans x -2..0, so the asset occupies
# [tx, tx+2]. Upstream of it is therefore tx=0 -> x 0..+2.
INFEED_TRANSLATE = Gf.Vec3d(0.0, 0.0, 0.0)
INFEED_SCALE = Gf.Vec3d(1.0, 0.5, 1.0) # identical to the other tracks
def _drop(layer: Sdf.Layer, path: str):
"""remove a prim spec if present, so the script is idempotent"""
spec = layer.GetPrimAtPath(path)
if not spec:
return False
parent = layer.GetPrimAtPath(str(Sdf.Path(path).GetParentPath())) or layer.pseudoRoot
name = Sdf.Path(path).name
if name in parent.nameChildren:
del parent.nameChildren[name]
return True
return False
def add_infeed(layer: Sdf.Layer):
"""a fourth conveyor upstream of ConveyorTrack_02, same asset and scale"""
_drop(layer, INFEED_PRIM)
spec = Sdf.CreatePrimInLayer(layer, INFEED_PRIM)
spec.specifier = Sdf.SpecifierDef
spec.typeName = "Xform"
spec.referenceList.prependedItems.append(Sdf.Reference(INFEED_ASSET))
for name, value, vtype in (
("xformOp:translate", INFEED_TRANSLATE, Sdf.ValueTypeNames.Double3),
("xformOp:scale", INFEED_SCALE, Sdf.ValueTypeNames.Double3)):
attr = Sdf.AttributeSpec(spec, name, vtype)
attr.default = value
order = Sdf.AttributeSpec(spec, "xformOpOrder", Sdf.ValueTypeNames.TokenArray)
order.default = ["xformOp:translate", "xformOp:scale"]
return INFEED_PRIM
def copy_from_sorter(layer: Sdf.Layer, src: Sdf.Layer):
copied = []
for path in FROM_SORTER:
if not src.GetPrimAtPath(path):
print(f" skip {path} - not in sorter.usd")
continue
_drop(layer, path)
if Sdf.CopySpec(src, Sdf.Path(path), layer, Sdf.Path(path)):
copied.append(path)
return copied
def main():
if not CELL.exists():
print(f"{CELL} not found - build it with scripts/build_plow_cell.py")
return 1
if not SORTER.exists():
print(f"{SORTER} not found - the camera stand is copied from it")
return 1
backup = CELL.with_suffix(".usd.bak")
if backup.exists():
print(f"backup {backup.name} already exists - keeping the pre-vision copy")
else:
shutil.copy(CELL, backup)
print(f"backup -> {backup.name}")
layer = Sdf.Layer.FindOrOpen(str(CELL))
src = Sdf.Layer.FindOrOpen(str(SORTER))
infeed = add_infeed(layer)
print(f"added {infeed} (references {INFEED_ASSET}, translate {tuple(INFEED_TRANSLATE)})")
for path in copy_from_sorter(layer, src):
print(f"copied {path}")
for path in DROP_AFTER_COPY:
if _drop(layer, path):
print(f"dropped {path} (superseded by the real conveyor)")
layer.Save()
print(f"saved {CELL}")
# verify by composing the result
stage = Usd.Stage.Open(str(CELL))
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
print("\nverification:")
ok = True
for path in [f"{INFEED_PRIM}/Belt", "/World/ConveyorTrack_02/Belt",
"/World/CameraMountFrame", "/World/SortingRig/LaserGate"]:
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
print(f" MISSING {path}")
ok = False
continue
r = cache.ComputeWorldBound(prim).ComputeAlignedRange()
if r.IsEmpty():
print(f" EMPTY {path}")
ok = False
continue
mn, mx = r.GetMin(), r.GetMax()
print(f" ok {path}: x[{mn[0]:7.3f}..{mx[0]:7.3f}] y[{mn[1]:6.3f}..{mx[1]:6.3f}] "
f"top_z={mx[2]:.3f}")
cams = stage.GetPrimAtPath("/RigRS")
n = len(cams.GetChildren()) if cams.IsValid() else 0
print(f" {'ok ' if n == 6 else 'PROBLEM'} /RigRS: {n} cameras")
return 0 if ok and n == 6 else 1
if __name__ == "__main__":
sys.exit(main())
scripts/bin_scan.py
+40
View File
@@ -0,0 +1,40 @@
"""Find the real D-item collection bin/container geometry near the pusher branch."""
import omni.usd
from pxr import Usd, UsdGeom
stage = omni.usd.get_context().get_stage()
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
print("=== config bin constants (old sorter.usd) ===")
import sys
sys.path.insert(0, "/home/dasha/robozon-sorter")
from robozon_sorter import config as C
print(f"BIN_X0={C.BIN_X0} BIN_X1={C.BIN_X1} BIN_Y0={C.BIN_Y0} BIN_Y1={C.BIN_Y1} BIN_LIP_Z={C.BIN_LIP_Z}")
print("\n=== SortingRig subtree (candidate bin location) ===")
p = stage.GetPrimAtPath("/World/SortingRig")
if p.IsValid():
for c in p.GetChildren():
r = bbc.ComputeWorldBound(c).ComputeAlignedRange()
if not r.IsEmpty():
mn, mx = r.GetMin(), r.GetMax()
print(f" {c.GetPath()} x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]")
else:
print(" MISSING")
print("\n=== ConveyorTrack_03 subtree (pusher branch) ===")
p = stage.GetPrimAtPath("/World/ConveyorTrack_03")
for c in p.GetChildren():
r = bbc.ComputeWorldBound(c).ComputeAlignedRange()
if not r.IsEmpty():
mn, mx = r.GetMin(), r.GetMax()
print(f" {c.GetPath()} x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]")
print("\n=== search whole stage for Bin-like names ===")
for pr in stage.Traverse():
nm = pr.GetName().lower()
if "bin" in nm:
r = bbc.ComputeWorldBound(pr).ComputeAlignedRange()
if not r.IsEmpty():
mn, mx = r.GetMin(), r.GetMax()
print(f" {pr.GetPath()} x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]")
scripts/bottle_collision_check.py
+23
View File
@@ -0,0 +1,23 @@
"""Does bottle's referenced mesh actually carry collision, compared to a working item?"""
import omni.usd
from pxr import Usd, UsdGeom, UsdPhysics
import sys
sys.path.insert(0, "/home/dasha/robozon-sorter")
from robozon_sorter import config as C
for name in ["bottle", "box_300x200x200", "bag", "helmet"]:
tmp_stage = Usd.Stage.CreateInMemory()
prim = tmp_stage.DefinePrim("/probe", "Xform")
prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{name}.usd"))
print(f"=== {name} ===")
n_col = 0
n_mesh = 0
for p in Usd.PrimRange(prim):
if p.IsA(UsdGeom.Mesh):
n_mesh += 1
has_col = p.HasAPI(UsdPhysics.CollisionAPI) or p.HasAPI(UsdPhysics.MeshCollisionAPI)
if has_col:
n_col += 1
approx = p.GetAttribute("physics:approximation")
print(f" {p.GetPath()} type={p.GetTypeName()} approx={approx.Get() if approx else None}")
print(f" meshes={n_mesh} prims_with_collision={n_col}")
scripts/branch_check.py
+97
View File
@@ -0,0 +1,97 @@
"""1) Does Belt_01 actually CARRY an item, and does it carry it to BinD?
2) How long does the pusher take to stroke out and return home?"""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene, plow_cell_9045
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True)
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
b01 = stage.GetPrimAtPath(_scene.BRANCH)
r = bbc.ComputeWorldBound(b01).ComputeAlignedRange()
print(f"Belt_01 bbox x[{r.GetMin()[0]:+.3f}..{r.GetMax()[0]:+.3f}] "
f"y[{r.GetMin()[1]:+.3f}..{r.GetMax()[1]:+.3f}] top_z={r.GetMax()[2]:+.3f}")
sv = b01.GetAttribute("physxSurfaceVelocity:surfaceVelocity").Get()
en = b01.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled")
xc = UsdGeom.XformCache()
wv = xc.GetLocalToWorldTransform(b01).TransformDir(Gf.Vec3d(*sv))
print(f" local surfaceVelocity={sv} enabled={en.Get() if en else None}")
print(f" WORLD drive = ({wv[0]:+.3f},{wv[1]:+.3f},{wv[2]:+.3f}) |v|={wv.GetLength():.3f}")
api = UsdShade.MaterialBindingAPI(b01)
mat, _ = api.ComputeBoundMaterial(materialPurpose="physics")
if mat:
m = UsdPhysics.MaterialAPI(mat.GetPrim())
print(f" friction static/dynamic = {m.GetStaticFrictionAttr().Get()}/{m.GetDynamicFrictionAttr().Get()}")
bd = bbc.ComputeWorldBound(stage.GetPrimAtPath("/World/SortingRig/BinD_Floor")).ComputeAlignedRange()
print(f"BinD floor x[{bd.GetMin()[0]:+.2f}..{bd.GetMax()[0]:+.2f}] y[{bd.GetMin()[1]:+.2f}..{bd.GetMax()[1]:+.2f}] "
f"centre=({(bd.GetMin()[0]+bd.GetMax()[0])/2:+.2f},{(bd.GetMin()[1]+bd.GetMax()[1])/2:+.2f})")
ipath = "/World/Items/_branchprobe"
def spawn(x, y):
if stage.GetPrimAtPath(ipath).IsValid():
stage.RemovePrim(ipath)
prim = UsdGeom.Xform.Define(stage, ipath).GetPrim()
prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / "bag.usd"))
xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(x, y, C.BELT_Z + 0.06))
UsdPhysics.RigidBodyAPI.Apply(prim)
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)
UsdGeom.Imageable(prim).MakeVisible()
return RigidPrim(paths=[ipath])
print("\n--- placing an item DIRECTLY on Belt_01 at (-4.10, +0.70): does it reach BinD? ---")
rp = spawn(-4.10, 0.70)
tl.play(); await app_utils.update_app_async(steps=10)
p0 = rp.get_world_poses()[0].numpy()[0].copy()
for i in range(9):
await app_utils.update_app_async(steps=40)
p = rp.get_world_poses()[0].numpy()[0]
print(f" t~{(i+1)*40/60:4.1f}s x={float(p[0]):+.2f} y={float(p[1]):+.2f} z={float(p[2]):+.2f}")
pf = rp.get_world_poses()[0].numpy()[0]
in_bin = (-6.21 < float(pf[0]) < -4.95) and (1.57 < float(pf[1]) < 2.86)
print(f" travelled ({float(pf[0])-float(p0[0]):+.2f},{float(pf[1])-float(p0[1]):+.2f}) IN BIN_D: {in_bin}")
tl.stop(); await app_utils.update_app_async(steps=6)
print("\n--- pusher stroke-out / return timing ---")
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
bop = op; break
bbase = bop.Get()
def blade_to(y):
bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2]))
blade_to(C.BLADE_HOME_Y)
tl.play(); await app_utils.update_app_async(steps=5)
for label, a, b, spd in (("out ", C.BLADE_HOME_Y, 0.55, 1.3), ("back", 0.55, C.BLADE_HOME_Y, 1.3),
("back", 0.55, C.BLADE_HOME_Y, 2.5)):
blade_to(a); await app_utils.update_app_async(steps=3)
dur = abs(b - a) / spd
t0 = float(tl.get_current_time())
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / dur)
blade_to(a + (b - a) * u)
await app_utils.update_app_async(steps=1)
if u >= 1.0:
break
print(f" {label} @ {spd} m/s : {float(tl.get_current_time())-t0:.3f} s "
f"(stroke {abs(b-a):.2f} m)")
tl.stop()
scripts/build_fork_v2.py
+156
View File
@@ -0,0 +1,156 @@
#!/usr/bin/env python3
"""Lay the discharge out as a fork: C carries straight on, B branches, plow in the corner.
/home/whatevenif/isaacsim/python.sh scripts/build_fork_v2.py
Design, from the sketch:
main run ──────┬─────────────▶ lane C (straight on, same line as the run)
│
└───▶ lane B (branches away at 45 deg)
plow sits in this corner
Class C needs no action - it runs straight through, and the blade at rest closes the B
mouth, so C is the default route and the blade only leans on it if it wanders. Class B is
the only case that actuates: the blade swings over, the B mouth opens, and the item drives
into its branch instead of being shoved across a belt.
**How the tracks are actually built** - this is what the first attempt got wrong. Each
ConveyorTrack carries `translate + orient(quaternion) + scale`; there is no rotateZ to
write, so clearing the op order and adding one silently produced a different transform. The
`Belt` child then sits at a fixed local offset of +1.0 along the track's local X, scaled by
the track's own X scale. So:
belt centre = track origin + (local +X in world) * 1.0 * scale_x
Placing a branch therefore means: point the track's local X down the branch, and put the
track origin at the fork apex, which lands the belt centre one length-half down the branch.
Verification uses a **fresh** BBoxCache after every write. Reusing one is what made the
first attempt report "nothing moved" while the geometry underneath had in fact been
scattered.
"""
from __future__ import annotations
import math
import sys
from pathlib import Path
from pxr import Gf, Usd, UsdGeom
SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
APEX = Gf.Vec3d(-7.00, 0.0, 0.0) # downstream end of the main run
TRAYS = "/World/PlowContainers"
PLOW = "/World/Diverters/DiverterEnd"
LANE_C = "/World/ConveyorTrack_01" # straight on, 0 deg off the run
LANE_B = "/ConveyorTrack_01" # branches 45 deg toward -Y
C_DEG, B_DEG = 0.0, -45.0
TRAY_AT = 2.60 # how far down each branch its tray sits
def quat_z(deg):
h = math.radians(deg) / 2.0
return Gf.Quatd(math.cos(h), Gf.Vec3d(0, 0, math.sin(h)))
def world_dir(deg):
"""travel direction of a branch `deg` off the -X run"""
a = math.radians(180.0 + deg)
return Gf.Vec3d(math.cos(a), math.sin(a), 0.0)
def place_track(stage, path, deg):
"""point the track down its branch and hang its origin on the apex"""
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
return False
xf = UsdGeom.Xformable(prim)
for op in xf.GetOrderedXformOps():
n = op.GetOpName()
if n.endswith("translate"):
op.Set(APEX)
elif n.endswith("orient"):
op.Set(quat_z(180.0 + deg)) # local +X onto the branch direction
return True
def move_group(stage, prefix, to_xy):
"""shift a tray so its centre lands on `to_xy`, keeping its parts together"""
cache = UsdGeom.BBoxCache(0, ["default"])
parts = [c for c in stage.GetPrimAtPath(TRAYS).GetChildren()
if c.GetName().startswith(prefix)]
if not parts:
return 0
xs, ys = [], []
for c in parts:
r = cache.ComputeWorldBound(c).ComputeAlignedRange()
xs += [r.GetMin()[0], r.GetMax()[0]]
ys += [r.GetMin()[1], r.GetMax()[1]]
dx = to_xy[0] - (min(xs) + max(xs)) / 2.0
dy = to_xy[1] - (min(ys) + max(ys)) / 2.0
n = 0
for c in parts:
for op in UsdGeom.Xformable(c).GetOrderedXformOps():
if op.GetOpName().endswith("translate"):
t = op.Get()
op.Set(type(t)(t[0] + dx, t[1] + dy, t[2]))
n += 1
break
return n
def report(stage, path, label):
"""measure with a FRESH cache - a reused one reports the state before the write"""
r = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
stage.GetPrimAtPath(path)).ComputeAlignedRange()
if r.IsEmpty():
print(f" {label:22s} (empty)")
return
print(f" {label:22s} x[{r.GetMin()[0]:+.2f},{r.GetMax()[0]:+.2f}] "
f"y[{r.GetMin()[1]:+.2f},{r.GetMax()[1]:+.2f}] top z={r.GetMax()[2]:.3f}")
def main():
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
stage = Usd.Stage.Open(str(SCENE))
for path, deg, tag in ((LANE_C, C_DEG, "C"), (LANE_B, B_DEG, "B")):
if not place_track(stage, path, deg):
print(f" {path} missing"); continue
d = world_dir(deg)
tray = (APEX[0] + d[0] * TRAY_AT, APEX[1] + d[1] * TRAY_AT)
moved = move_group(stage, f"{tag}_", tray)
print(f"branch {tag}: {deg:+.0f} deg, dir ({d[0]:+.3f},{d[1]:+.3f}), "
f"tray -> ({tray[0]:+.2f},{tray[1]:+.2f}) [{moved} parts]")
# the plow sits in the corner between the two branches
pxf = UsdGeom.Xformable(stage.GetPrimAtPath(PLOW))
for op in pxf.GetOrderedXformOps():
if op.GetOpName().endswith("translate"):
t = op.Get()
op.Set(Gf.Vec3d(APEX[0], APEX[1], t[2]))
break
stage.GetRootLayer().Save()
print("\n--- measured after the write (fresh cache each time) ---")
report(stage, "/World/ConveyorTrack_04/Belt", "main run")
report(stage, f"{LANE_C}/Belt", "lane C (straight)")
report(stage, f"{LANE_B}/Belt", "lane B (45 deg)")
report(stage, f"{PLOW}/Arm", "plow arm")
for tag in ("B", "C"):
cache = UsdGeom.BBoxCache(0, ["default"])
xs, ys = [], []
for c in stage.GetPrimAtPath(TRAYS).GetChildren():
if c.GetName().startswith(f"{tag}_"):
r = cache.ComputeWorldBound(c).ComputeAlignedRange()
xs += [r.GetMin()[0], r.GetMax()[0]]; ys += [r.GetMin()[1], r.GetMax()[1]]
if xs:
print(f" tray {tag} centre "
f"({(min(xs)+max(xs))/2:+.2f},{(min(ys)+max(ys))/2:+.2f})")
print(f"\nsaved {SCENE}")
if __name__ == "__main__":
main()
scripts/build_plow_cell.py
+122
View File
@@ -0,0 +1,122 @@
#!/usr/bin/env python3
"""Build scene/plow_cell.usd from the authored 90_degree.usd.
python scripts/build_plow_cell.py [path/to/90_degree.usd]
The source is the original authored cell - conveyor art, the Y-split pusher, the plow
(``DiverterEnd``) and its OmniGraph drive script exactly as built. This script does not
regenerate geometry; it only makes the file self-contained inside this repo:
1. **References re-pointed.** The source pulls conveyor art straight off the Omniverse S3
bucket and the plow meshes from its own folder. Both are re-pointed at the repo's
``assets/`` tree so the scene composes offline (``scripts/fetch_assets.py`` fills
``assets/conveyors/``).
2. **Baked drive animation stripped.** The pusher's ``PusherSlide`` carries a long
``targetPosition.timeSamples`` track. Time samples outrank the attribute default, so
anything that tries to *control* that drive - the authored script node or Python - is
overwritten every frame while the timeline runs. The track is dropped; the drive keeps
its authored gains and limits. (The plow's own ``ArmHinge`` is already clean in
90_degree.usd; the earlier fixed.usd bakes it too, hence the pattern matches both.)
The authored ``DiverterAnimGraph`` script node is deliberately kept: open the scene, press
Play, and the cell demonstrates itself the way it was built. ``sim/plow_cell.py`` switches
that graph off when you want to drive the plow from code instead.
"""
from __future__ import annotations
import re
import shutil
import subprocess
import sys
import tempfile
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
OUT = ROOT / "scene" / "plow_cell.usd"
DEFAULT_SRC = (Path.home() / "Desktop" / "isaac_sim_project" / "test_isassc" / "90_degree.usd")
S3 = ("https://omniverse-content-production.s3-us-west-2.amazonaws.com/"
"Assets/Isaac/6.0/Isaac/Props/Conveyors/")
# asset path in the source -> path relative to scene/
REFS = {
f"{S3}ConveyorBelt_A06.usd": "../assets/conveyors/ConveyorBelt_A06.usd",
f"{S3}ConveyorBelt_A24.usd": "../assets/conveyors/ConveyorBelt_A24.usd",
"./plow_base.usd": "../assets/plow/plow_base.usd",
"./plow_arm.usd": "../assets/plow/plow_arm.usd",
}
# Baked drive tracks fight every attempt to control a diverter. In 90_degree.usd only the
# pusher's linear drive carries one (the plow's angular drive is already clean), but the
# earlier fixed.usd bakes the plow too - match both so either source builds the same way.
BAKED = re.compile(r"drive:(linear|angular):physics:targetPosition\.timeSamples")
def usdcat(src: Path, dst: Path) -> None:
if not shutil.which("usdcat"):
sys.exit("usdcat not found - it ships with USD / Isaac Sim and is needed to "
"convert the binary .usd to text and back")
subprocess.run(["usdcat", str(src), "-o", str(dst)], check=True)
def strip_baked_track(text: str) -> tuple[str, int]:
"""drop `<drive target>.timeSamples = { ... }` blocks"""
out, skipping, dropped = [], False, 0
for line in text.splitlines(keepends=True):
if not skipping and BAKED.search(line) and line.rstrip().endswith("{"):
skipping, dropped = True, dropped + 1
continue
if skipping:
if line.strip() == "}":
skipping = False
continue
out.append(line)
return "".join(out), dropped
def repoint_refs(text: str) -> tuple[str, dict[str, int]]:
counts = {}
for old, new in REFS.items():
n = text.count(f"@{old}@")
if n:
text = text.replace(f"@{old}@", f"@{new}@")
counts[old] = n
return text, counts
def build(src: Path) -> Path:
if not src.exists():
sys.exit(f"source scene not found: {src}")
OUT.parent.mkdir(parents=True, exist_ok=True)
with tempfile.TemporaryDirectory() as tmp:
flat = Path(tmp) / "src.usda"
usdcat(src, flat)
text = flat.read_text()
text, refs = repoint_refs(text)
text, dropped = strip_baked_track(text)
missing = [k for k, v in refs.items() if v == 0]
if missing:
print("warning: reference not found in source (layout may have changed):")
for m in missing:
print(f" {m}")
edited = Path(tmp) / "edited.usda"
edited.write_text(text)
usdcat(edited, OUT)
print(f"built {OUT.relative_to(ROOT)} from {src}")
for old, new in REFS.items():
print(f" ref {refs[old]}x {Path(old).name:24s} -> {new}")
print(f" drop {dropped}x baked drive targetPosition.timeSamples")
left = re.findall(r"@(https?://[^@]+)@", OUT.read_text(errors="ignore")) if OUT.suffix == ".usda" else []
if left:
print(f" warning: {len(left)} remote reference(s) still present")
return OUT
if __name__ == "__main__":
build(Path(sys.argv[1]) if len(sys.argv) > 1 else DEFAULT_SRC)
scripts/build_y_fork.py
+129
View File
@@ -0,0 +1,129 @@
#!/usr/bin/env python3
"""Rebuild the discharge as a Y fork with the plow at its apex.
/home/whatevenif/isaacsim/python.sh scripts/build_y_fork.py
The layout so far was a T: lane B perpendicular, lane C at 45 deg, meeting the main run at
different x, with the blade out in the middle of the belt trying to shove goods 400 mm
sideways onto them. Every failure this session came from that - the dead zone, the wedges,
goods stalling on the lip - because a push was being asked to do the job of a route.
A fork does not need the push. Both branches leave one apex, the blade sits in it as a
railway point, and goods **drive** into their branch:
rest (class C) blade closes the B mouth -> everything runs straight on to C
B arrives blade swings over -> the B mouth opens and takes it
Geometry, all from the apex at the downstream end of the main run:
apex x -7.00, y 0
branch C 15 deg up from the run (travel -0.966, +0.259)
branch B 35 deg down from the run (travel -0.819, -0.574)
Each track is placed by measurement, not by assumption: the script reads where the belt slab
currently sits relative to its own prim origin, then sets the transform so the slab's near
end lands on the apex pointing along its branch. The trays follow their branches.
"""
from __future__ import annotations
import math
import sys
from pathlib import Path
from pxr import Gf, Usd, UsdGeom
SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
APEX = (-7.00, 0.0)
TRAYS = "/World/PlowContainers"
# track prim -> (branch angle from the -X run, tray prefix, tray distance along the branch)
BRANCHES = {
"/World/ConveyorTrack_01": dict(deg=+15.0, tray="C_", tray_at=2.30, name="C"),
"/ConveyorTrack_01": dict(deg=-35.0, tray="B_", tray_at=2.30, name="B"),
}
def _dir(deg):
"""world travel direction of a branch `deg` off the -X run"""
a = math.radians(180.0 - deg) # -X is 180 deg; +deg swings toward +Y
return math.cos(a), math.sin(a)
def _belt_of(stage, track):
for p in (f"{track}/Belt", f"{track}/Belt_01"):
if stage.GetPrimAtPath(p).IsValid():
return p
return None
def _set_xform(stage, path, tx, ty, tz, rot_deg):
xf = UsdGeom.Xformable(stage.GetPrimAtPath(path))
xf.ClearXformOpOrder()
xf.AddTranslateOp().Set(Gf.Vec3d(tx, ty, tz))
xf.AddRotateZOp().Set(float(rot_deg))
def main():
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
stage = Usd.Stage.Open(str(SCENE))
bb = UsdGeom.BBoxCache(0, ["default"])
xc = UsdGeom.XformCache()
for track, b in BRANCHES.items():
prim = stage.GetPrimAtPath(track)
if not prim.IsValid():
print(f" {track} missing"); continue
belt = _belt_of(stage, track)
if belt is None:
print(f" {track} has no Belt"); continue
# where the slab sits now, relative to this track's own origin
org = xc.GetLocalToWorldTransform(prim).ExtractTranslation()
r = bb.ComputeWorldBound(stage.GetPrimAtPath(belt)).ComputeAlignedRange()
span_x, span_y = r.GetMax()[0] - r.GetMin()[0], r.GetMax()[1] - r.GetMin()[1]
length = max(span_x, span_y)
top_z = r.GetMax()[2]
# the slab's centre offset from the origin, in the track's own frame
cx = (r.GetMin()[0] + r.GetMax()[0]) / 2.0 - org[0]
cy = (r.GetMin()[1] + r.GetMax()[1]) / 2.0 - org[1]
off = math.hypot(cx, cy)
dx, dy = _dir(b["deg"])
# put the slab centre half a length down the branch from the apex
tx = APEX[0] + dx * (length / 2.0) - (dx * off - dx * off)
ty = APEX[1] + dy * (length / 2.0)
_set_xform(stage, track, tx - cx, ty - cy, org[2], 180.0 - b["deg"])
r2 = bb.ComputeWorldBound(stage.GetPrimAtPath(belt)).ComputeAlignedRange()
print(f" branch {b['name']} {b['deg']:+.0f} deg dir ({dx:+.3f},{dy:+.3f}) "
f"length {length:.2f} m")
print(f" slab now x[{r2.GetMin()[0]:+.2f},{r2.GetMax()[0]:+.2f}] "
f"y[{r2.GetMin()[1]:+.2f},{r2.GetMax()[1]:+.2f}] top z={r2.GetMax()[2]:.3f}")
# the tray rides to the end of its branch
tex, tey = APEX[0] + dx * b["tray_at"], APEX[1] + dy * b["tray_at"]
moved = 0
for c in stage.GetPrimAtPath(TRAYS).GetChildren():
if not c.GetName().startswith(b["tray"]):
continue
cr = bb.ComputeWorldBound(c).ComputeAlignedRange()
ccx = (cr.GetMin()[0] + cr.GetMax()[0]) / 2.0
ccy = (cr.GetMin()[1] + cr.GetMax()[1]) / 2.0
cxf = UsdGeom.Xformable(c)
for op in cxf.GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
t = op.Get()
op.Set(type(t)(t[0] + (tex - ccx), t[1] + (tey - ccy), t[2]))
moved += 1
break
print(f" tray {b['name']} -> ({tex:+.2f},{tey:+.2f}) {moved} parts moved")
stage.GetRootLayer().Save()
print(f"saved {SCENE}")
print("NOTE: the blade's rest position is now 'B closed', not 0 - re-measure which sign")
print(" closes B before running a sort.")
if __name__ == "__main__":
main()
scripts/cam_check.py
+71
View File
@@ -0,0 +1,71 @@
"""Какая камера сдвинута: сверка живой сцены с тем, что записано в файле.
В сцене несколько камер разного назначения - шесть стереокамер стенда в /RigRS, корпуса
камер в /World/CameraBodies и служебная перспектива вьюпорта. Сдвиг любой из них
выглядит одинаково, а чинятся они по-разному, поэтому сначала находится ТА САМАЯ.
Живая сцена читается из памяти, эталон - из слоя на диске: несовпадение и есть правка,
сделанная мышью во вьюпорте.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd
from pxr import Usd, UsdGeom, Gf
live = omni.usd.get_context().get_stage()
path = live.GetRootLayer().identifier
print(f"живая сцена: {path}\n")
disk = Usd.Stage.Open(path)
cache_l = UsdGeom.XformCache()
cache_d = UsdGeom.XformCache()
cams = [p for p in live.Traverse() if p.IsA(UsdGeom.Camera)]
print(f"камер в сцене: {len(cams)}\n")
print(f" {'камера':44s} {'положение сейчас':>34s} расхождение с файлом")
print(" " + "-" * 104)
moved = []
for c in cams:
p = str(c.GetPath())
Ml = cache_l.GetLocalToWorldTransform(c)
tl_ = Ml.ExtractTranslation()
d = disk.GetPrimAtPath(p)
if not d or not d.IsValid():
print(f" {p:44s} ({tl_[0]:+7.3f},{tl_[1]:+7.3f},{tl_[2]:+7.3f}) нет в файле")
continue
Md = cache_d.GetLocalToWorldTransform(d)
td = Md.ExtractTranslation()
dt = (tl_ - td).GetLength()
# угловое расхождение по направлению взгляда камеры (-Z в её системе)
vl = Ml.TransformDir(Gf.Vec3d(0, 0, -1)); vd = Md.TransformDir(Gf.Vec3d(0, 0, -1))
vl = vl / (vl.GetLength() or 1); vd = vd / (vd.GetLength() or 1)
import math
ang = math.degrees(math.acos(max(-1.0, min(1.0, vl[0]*vd[0] + vl[1]*vd[1] + vl[2]*vd[2]))))
flag = "СДВИНУТА" if (dt > 0.001 or ang > 0.1) else "совпадает"
print(f" {p:44s} ({tl_[0]:+7.3f},{tl_[1]:+7.3f},{tl_[2]:+7.3f}) "
f"{dt*1000:7.1f} мм / {ang:5.2f}° {flag}")
if flag == "СДВИНУТА":
moved.append((p, td, tl_, dt, ang))
# служебная перспектива вьюпорта - её в файле обычно нет, она хранится в сессии
persp = live.GetPrimAtPath("/OmniverseKit_Persp")
if persp.IsValid():
M = cache_l.GetLocalToWorldTransform(persp)
t = M.ExtractTranslation()
print(f"\n перспектива вьюпорта /OmniverseKit_Persp: "
f"({t[0]:+.2f}, {t[1]:+.2f}, {t[2]:+.2f})")
print()
if moved:
print(f"СДВИНУТО КАМЕР: {len(moved)}")
for p, td, tlv, dt, ang in moved:
print(f" {p}")
print(f" было в файле: ({td[0]:+7.3f},{td[1]:+7.3f},{td[2]:+7.3f})")
print(f" стало сейчас: ({tlv[0]:+7.3f},{tlv[1]:+7.3f},{tlv[2]:+7.3f})")
print(f" расхождение {dt*1000:.1f} мм, поворот {ang:.2f}°")
else:
print("Ни одна камера-прем не сдвинута относительно файла.")
print("Значит двигали перспективу вьюпорта - она в файл не пишется и на прогоны не влияет.")
scripts/cam_reset.py
+52
View File
@@ -0,0 +1,52 @@
"""Вернуть перспективу вьюпорта на обзор ячейки.
Сдвинута оказалась только служебная камера /OmniverseKit_Persp - она уехала в
(-8.96, -5.49, -6.70), то есть ПОД пол, и смотрела снизу. Шесть стереокамер стенда в
/RigRS не тронуты (0.0 мм, 0.00°), их править нечего - а именно они участвуют в прогонах.
Перспектива вьюпорта в файл не пишется и на физику с замерами не влияет: это только то,
что видит человек. Поэтому здесь она ставится по фактическому габариту ячейки, а не по
запомненному числу - сцена менялась, и старая точка могла бы снова смотреть мимо.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.rendering_manager import ViewportManager
from pxr import Usd, UsdGeom, Gf
stage = omni.usd.get_context().get_stage()
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
# габарит по дорожкам и плугу - то, на что осмысленно смотреть
lo = Gf.Vec3d(1e9, 1e9, 1e9); hi = Gf.Vec3d(-1e9, -1e9, -1e9)
for p in stage.Traverse():
n = p.GetName()
if not (n.startswith("ConveyorTrack") or n == "Diverters"):
continue
r = bb.ComputeWorldBound(p).ComputeAlignedRange()
a, b = r.GetMin(), r.GetMax()
for i in range(3):
lo[i] = min(lo[i], a[i]); hi[i] = max(hi[i], b[i])
ctr = Gf.Vec3d((lo[0]+hi[0])/2, (lo[1]+hi[1])/2, (lo[2]+hi[2])/2)
span = max(hi[0]-lo[0], hi[1]-lo[1])
print(f"габарит ячейки: x {lo[0]:+.2f}..{hi[0]:+.2f} y {lo[1]:+.2f}..{hi[1]:+.2f} "
f"z {lo[2]:+.2f}..{hi[2]:+.2f}")
print(f"центр ({ctr[0]:+.2f}, {ctr[1]:+.2f}, {ctr[2]:+.2f}), протяжённость {span:.1f} м")
# три четверти сверху-сбоку: вся линия в кадре, плуг и пушер видны не с торца
eye = Gf.Vec3d(ctr[0] + span * 0.45, ctr[1] - span * 0.65, ctr[2] + span * 0.55)
tgt = Gf.Vec3d(ctr[0], ctr[1], 1.60)
ViewportManager.set_camera_view("/OmniverseKit_Persp",
eye=[eye[0], eye[1], eye[2]],
target=[tgt[0], tgt[1], tgt[2]])
await app_utils.update_app_async(steps=30)
cache = UsdGeom.XformCache()
t = cache.GetLocalToWorldTransform(stage.GetPrimAtPath("/OmniverseKit_Persp")).ExtractTranslation()
print(f"\nперспектива возвращена: ({t[0]:+.2f}, {t[1]:+.2f}, {t[2]:+.2f}) "
f"-> смотрит на ({tgt[0]:+.2f}, {tgt[1]:+.2f}, {tgt[2]:+.2f})")
print("камеры стенда /RigRS не трогались")
scripts/contact_and_decks.py
+56
View File
@@ -0,0 +1,56 @@
"""1) Blade vs item contact height at the pusher. 2) Actual authored deck surfaceVelocity
magnitudes vs the commanded 1.0 m/s - the log showed items crawling 8-50s across ~1-2m
of deck (expected ~1-2s), which smells like the drive_belt() scale-correction being wrong
for these particular Cube prims (same class of bug as ConveyorTrack_04's documented
0.5-scale issue)."""
import omni.usd
from pxr import Usd, UsdGeom, PhysxSchema, Gf
stage = omni.usd.get_context().get_stage()
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
print("=== pusher blade vs belt height ===")
blade = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom")
r = bbc.ComputeWorldBound(blade).ComputeAlignedRange()
print(f" blade z[{r.GetMin()[2]:+.3f}..{r.GetMax()[2]:+.3f}]")
belt = stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt")
r2 = bbc.ComputeWorldBound(belt).ComputeAlignedRange()
print(f" ConveyorTrack_03/Belt top z={r2.GetMax()[2]:+.3f}")
print("\n=== item rest heights (bottom of bbox) for the 11 named items, unposed ===")
import sys
sys.path.insert(0, "/home/dasha/robozon-sorter")
from robozon_sorter import config as C
items_dir = C.ROOT / "assets" / "items"
for name in ["bottle","box_300x200x200","box_400x400x300","lunchbox","bag","detergent",
"pouf","pen","plate","cylinder","helmet"]:
tmp_stage = Usd.Stage.CreateInMemory()
prim = tmp_stage.DefinePrim("/probe", "Xform")
prim.GetReferences().AddReference(str(items_dir / f"{name}.usd"))
bb2 = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
rr = bb2.ComputeWorldBound(prim).ComputeAlignedRange()
if rr.IsEmpty():
print(f" {name:18s} EMPTY BBOX"); continue
mn, mx = rr.GetMin(), rr.GetMax()
print(f" {name:18s} local z[{mn[2]:+.3f}..{mx[2]:+.3f}] height={mx[2]-mn[2]:.3f}")
print("\n=== deck actual surfaceVelocity (authored) vs commanded speed=1.0 ===")
for path in ["/World/PlowTransition_B", "/World/PlowCornerDeck_B",
"/World/PlowTransition_C", "/World/PlowCornerDeck_C"]:
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
print(f" {path} MISSING"); continue
sv = prim.GetAttribute("physxSurfaceVelocity:surfaceVelocity")
v = sv.Get() if sv else None
mag = (v[0]**2+v[1]**2+v[2]**2)**0.5 if v else 0.0
# world-space check: transform local surfaceVelocity to world using current xform
xc = UsdGeom.XformCache()
M = xc.GetLocalToWorldTransform(prim)
world_v = M.TransformDir(Gf.Vec3d(*v)) if v else Gf.Vec3d(0,0,0)
world_mag = world_v.GetLength()
scale_op = None
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeScale:
scale_op = op.Get()
print(f" {path}")
print(f" local surfaceVelocity={v} |local|={mag:.3f} |world|={world_mag:.3f} scale={scale_op}")
scripts/diag_belts.py
+71
View File
@@ -0,0 +1,71 @@
"""Почему пробы не поехали: есть ли коллизия на лентах и куда падают тела.
Ни одна проба не сдвинулась, две улетели. Это картина не буксования, а отсутствия опоры:
если у према Belt нет коллайдера, тело проваливается сквозь ленту и дальше поведение
случайно. Поэтому проверяется опора, а не сила трения.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema
from isaacsim.core.experimental.prims import RigidPrim
stage = omni.usd.get_context().get_stage()
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
BELTS = ["/World/ConveyorTrack_05/Belt", "/World/ConveyorTrack/Belt",
"/World/ConveyorTrack_03/Belt", "/World/ConveyorTrack_04/Belt",
"/World/ConveyorTrack_01/Belt", "/World/ConveyorTrack_06/Belt",
"/World/ConveyorTrack_03/Belt_01"]
print("ОПОРА ПОД ТОВАРОМ:\n")
for path in BELTS:
pr = stage.GetPrimAtPath(path)
if not pr.IsValid():
print(f" {path}: НЕТ ПРЕМА"); continue
r = bb.ComputeWorldBound(pr).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
own = pr.HasAPI(UsdPhysics.CollisionAPI)
rb = pr.HasAPI(UsdPhysics.RigidBodyAPI)
kin = UsdPhysics.RigidBodyAPI(pr).GetKinematicEnabledAttr().Get() if rb else None
# коллайдеры среди потомков
kids, meshes = [], []
for d in Usd.PrimRange(pr):
if d == pr:
continue
if d.HasAPI(UsdPhysics.CollisionAPI):
kids.append(d.GetName())
if d.IsA(UsdGeom.Mesh):
meshes.append(d.GetName())
print(f" {path}")
print(f" тип={pr.GetTypeName()} коллизия_на_себе={own} rigid={rb} кинематик={kin}")
print(f" габарит z {mn[2]:.3f}..{mx[2]:.3f} xy {mn[0]:.2f}..{mx[0]:.2f} / {mn[1]:.2f}..{mx[1]:.2f}")
print(f" потомков с коллизией: {len(kids)} {kids[:4]} мешей: {len(meshes)} {meshes[:4]}")
# что вообще есть под точкой (-1.0, 0.0): все коллайдеры, чей габарит её накрывает
print("\nЧТО НАКРЫВАЕТ ТОЧКУ (-1.0, 0.0) сверху вниз:")
hits = []
for p in stage.Traverse():
if not p.HasAPI(UsdPhysics.CollisionAPI):
continue
r = bb.ComputeWorldBound(p).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
if mn[0] <= -1.0 <= mx[0] and mn[1] <= 0.0 <= mx[1]:
hits.append((mx[2], str(p.GetPath()), mn[2]))
for top, path, bot in sorted(hits, reverse=True)[:10]:
print(f" z {bot:6.3f}..{top:6.3f} {path}")
if not hits:
print(" НИЧЕГО - под товаром нет ни одного коллайдера")
# где сейчас лежат пробы
probe = stage.GetPrimAtPath("/World/_BeltProbe")
if probe.IsValid():
print("\nГДЕ ОКАЗАЛИСЬ ПРОБЫ:")
for c in probe.GetChildren():
r = bb.ComputeWorldBound(c).ComputeAlignedRange()
m = r.GetMidpoint()
print(f" {c.GetName():10s} ({m[0]:+7.2f}, {m[1]:+7.2f}, {m[2]:+7.2f})")
scripts/diag_drive.py
+79
View File
@@ -0,0 +1,79 @@
"""Почему лента не тянет: сырой прогон одной дорожки с печатью каждого шага.
Предыдущий вывод "стоит/упал" был ложным - я снимал положения ПОСЛЕ stop(), который
возвращает сцену в исходное состояние, поэтому все тела оказались в точках рождения
независимо от того, ехали они или нет. Здесь положение печатается ВО ВРЕМЯ прогона.
Первый подозреваемый - surfaceVelocityEnabled: PhysxSurfaceVelocityAPI можно применить
и задать вектор, но без включённого флага он не действует, и внешне это неотличимо от
нехватки трения.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
BELT = "/World/ConveyorTrack/Belt"
PROBE = "/World/_BeltProbe/main_00"
pr = stage.GetPrimAtPath(BELT)
print("=== состояние привода ленты ===")
for a in pr.GetAttributes():
n = a.GetName()
if "urfaceVelocity" in n or "kinematic" in n.lower():
print(f" {n} = {a.Get()}")
print(" применённые схемы:", [s for s in pr.GetAppliedSchemas()])
# включить флаг явно
api = PhysxSchema.PhysxSurfaceVelocityAPI.Apply(pr)
en = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled")
if not en or not en.IsValid():
en = api.CreateSurfaceVelocityEnabledAttr()
en.Set(True)
print(" surfaceVelocityEnabled выставлен в True")
# трение: без материала на ленте тянуть нечем
MAT = "/World/_TestGrip"
m = stage.GetPrimAtPath(MAT)
if not m.IsValid():
m = stage.DefinePrim(MAT, "Material")
pm = UsdPhysics.MaterialAPI.Apply(m)
pm.CreateStaticFrictionAttr().Set(1.1)
pm.CreateDynamicFrictionAttr().Set(0.95)
pm.CreateRestitutionAttr().Set(0.0)
for target in (BELT, PROBE):
t = stage.GetPrimAtPath(target)
if t.IsValid():
UsdShade.MaterialBindingAPI.Apply(t).Bind(
UsdShade.Material(m), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
materialPurpose="physics")
print(f" трение 1.1/0.95 привязано к ленте и к пробе")
probe = stage.GetPrimAtPath(PROBE)
print(f" проба существует: {probe.IsValid()}")
tl.play()
await app_utils.update_app_async(steps=20)
rp = RigidPrim(paths=[PROBE])
print("\n=== ВО ВРЕМЯ прогона ===")
print(" шаг x y z dx за шаг")
prev = None
for i in range(12):
pos, _ = rp.get_world_poses()
p = pos.numpy()[0]
d = "-" if prev is None else f"{(p[0]-prev)*1000:+7.1f} мм"
print(f" {i*10:4d} {p[0]:+7.3f} {p[1]:+7.3f} {p[2]:+7.3f} {d}")
prev = p[0]
await app_utils.update_app_async(steps=10)
tl.stop()
await app_utils.update_app_async(steps=5)
scripts/diag_junction.py
+72
View File
@@ -0,0 +1,72 @@
"""Why goods stop at x=-6.0: place one item either side of the belt junction and watch.
Run A parks an item at x=-5.5, upstream of the transfer, and lets it drive at it.
Run B starts one already at x=-6.3, past the transfer, on the wide belt through the plow.
If A stalls and B runs, the transfer is blocked by structure, not by a dead belt. The
suspect is the downstream end frame of ConveyorTrack_03: `open_junction()` clears the shell
collider on the plow track and both lanes, but not on the track goods arrive *on*, so its
end plate stands across the path at exactly x=-6.0.
"""
import sys, time
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
# The live Isaac process keeps every module it has ever imported, so an edited
# robozon_sorter/ on disk is invisible to a second run in the same session. Drop the
# package from sys.modules first or you spend the evening re-testing the old code.
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
del sys.modules[_m]
import importlib
importlib.invalidate_caches() # a *new* module file is invisible until the finder is reset
import omni.timeline, isaacsim.core.experimental.utils.app as app_utils
from pxr import UsdPhysics
from robozon_sorter import config as C
from robozon_sorter.sim import plow_sort, plow_vision
from robozon_sorter.sim.mechanics import Cell
OPEN_03 = bool(globals().get("open_03", False)) # also clear ConveyorTrack_03's shell
START_X = float(globals().get("start_x", -5.5))
ITEM = globals().get("item", "barrel")
SECONDS = float(globals().get("seconds", 6.0))
stage, info = plow_vision.load(belt_speed=1.0, script_control=True)
plow_sort.keep_lanes_active(stage)
plow_sort.configure_lanes(stage, 1.0)
opened = plow_sort.open_junction(stage)
extra = None
if OPEN_03:
p = stage.GetPrimAtPath("/World/ConveyorTrack_03/SM_ConveyorBelt_A24_02")
a = p.GetAttribute("physics:collisionEnabled") or \
UsdPhysics.CollisionAPI.Apply(p).CreateCollisionEnabledAttr()
a.Set(False)
extra = str(p.GetPath())
print(f"opened {len(opened)} shells, extra={extra}")
items = {k: v["zone"] for k, v in info["items"].items()}
await app_utils.update_app_async(steps=30)
cell = Cell(stage, items.keys())
cell.park_all()
await app_utils.update_app_async(steps=10)
cell.place(ITEM, (START_X, 0.0, C.BELT_Z + 0.10))
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
print(f"\n{ITEM} from x={START_X} (plow at x={C.PLOW_POS[0]})")
t0, last = time.time(), None
while time.time() - t0 < SECONDS:
await app_utils.update_app_async(steps=12)
p = cell.pose(ITEM)
x, y, z = (float(v) for v in p[:3])
moved = "" if last is None else f" dx={x - last:+.3f}"
print(f" t={time.time() - t0:4.1f} x={x:+.3f} y={y:+.3f} z={z:+.3f}{moved}")
last = x
if z < C.BELT_Z - 0.5:
print(" -> fell off"); break
app_utils.stop()
await app_utils.update_app_async(steps=10)
print(f"verdict: {'REACHED PLOW' if last is not None and last < -6.6 else 'STALLED at x=%.2f' % (last or 0)}")
scripts/diag_stall.py
+89
View File
@@ -0,0 +1,89 @@
"""Что держит товар на x = -3.15: список коллайдеров в этой полосе + трасса остановки.
Пушер проверить не вышло - товар не доехал до точки срабатывания (PUSH_X = -3.9) и встал
раньше. Прежде чем править пушер, надо понять, упирается товар в препятствие или теряет
привод: это разные неисправности, и внешне они неотличимы.
"""
import sys, math
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
print("КОЛЛАЙДЕРЫ, накрывающие полосу x -3.4..-3.0 на высоте ленты (z 1.75..2.10):")
for p in stage.Traverse():
if not p.HasAPI(UsdPhysics.CollisionAPI):
continue
en = p.GetAttribute("physics:collisionEnabled")
if en and en.Get() is False:
continue
r = bb.ComputeWorldBound(p).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
if mx[0] < -3.4 or mn[0] > -3.0:
continue
if mx[2] < 1.75 or mn[2] > 2.10:
continue
if abs(mn[1]) > 0.6 and abs(mx[1]) > 0.6 and mn[1] * mx[1] > 0:
continue
print(f" x {mn[0]:+7.3f}..{mx[0]:+7.3f} y {mn[1]:+6.2f}..{mx[1]:+6.2f} "
f"z {mn[2]:+6.3f}..{mx[2]:+6.3f} {p.GetPath()}")
# какая лента под этой точкой и что у неё со скоростью
print("\nЛЕНТЫ ПОД x=-3.15:")
for path in list(plow_cell.BELTS) + [plow_cell.BRANCH]:
pr = stage.GetPrimAtPath(path)
if not pr.IsValid():
continue
r = bb.ComputeWorldBound(pr).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
if mn[0] <= -3.15 <= mx[0]:
v = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocity").Get()
e = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled")
print(f" {path}: v={v} включено={e.Get() if e else None} "
f"y {mn[1]:+.2f}..{mx[1]:+.2f}")
# трасса: пустить товар и печатать x, пока не встанет
TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt")
).ComputeAlignedRange().GetMax()[2]
if stage.GetPrimAtPath("/World/_Stall").IsValid():
stage.RemovePrim("/World/_Stall")
stage.DefinePrim("/World/_Stall", "Xform")
c = UsdGeom.Cube.Define(stage, "/World/_Stall/item"); c.CreateSizeAttr().Set(2.0)
xf = UsdGeom.Xformable(c.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(-2.40, 0.0, TOP + 0.055))
xf.AddScaleOp().Set(Gf.Vec3f(0.05, 0.05, 0.05))
p = c.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(0.5)
PhysxSchema.PhysxRigidBodyAPI.Apply(p).CreateSolverPositionIterationCountAttr().Set(32)
g = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL)
if g.IsValid():
UsdShade.MaterialBindingAPI.Apply(p).Bind(
UsdShade.Material(g), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
materialPurpose="physics")
tl.play(); await app_utils.update_app_async(steps=25)
rp = RigidPrim(paths=["/World/_Stall/item"])
print("\nТРАССА (старт x=-2.40):")
prev, t0 = None, float(tl.get_current_time())
for i in range(28):
q = rp.get_world_poses()[0].numpy()[0]
t = float(tl.get_current_time())
d = "-" if prev is None else f"{(float(q[0])-prev)*1000:+7.1f} мм"
print(f" t={t-t0:5.2f}s x={float(q[0]):+7.3f} y={float(q[1]):+6.3f} "
f"z={float(q[2]):+6.3f} {d}")
prev = float(q[0])
await app_utils.update_app_async(steps=6)
tl.stop(); await app_utils.update_app_async(steps=5)
scripts/diag_zero.py
+54
View File
@@ -0,0 +1,54 @@
"""Кто обнуляет surfaceVelocity: значение читается до play и НЕСКОЛЬКО РАЗ во время.
drive_belt вернул (-1.0, 0, 0), а в атрибуте лежит (-0, 0, 0). Два разных объяснения:
запись не дошла до према, либо её перетирают во время прогона. Отличить их можно только
чтением атрибута в обоих состояниях - что и делается здесь.
Подозреваемый - авторские узлы ConveyorBeltGraph: по документации сборки собственной
скорости они не несут и на каждом тике пишут свою (нулевую), забивая явную установку.
Их деактивация в предыдущем прогоне могла не подействовать на уже созданный граф.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, PhysxSchema
from isaacsim.core.experimental.prims import RigidPrim
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
BELT = "/World/ConveyorTrack/Belt"
PROBE = "/World/_BeltProbe/main_00"
pr = stage.GetPrimAtPath(BELT)
attr = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocity")
# состояние графов конвейера
graphs = [p for p in stage.Traverse() if "ConveyorBeltGraph" in p.GetName()]
print("графы ConveyorBeltGraph:")
for g in graphs:
print(f" {g.GetPath()} активен={g.IsActive()}")
attr.Set(Gf.Vec3f(-1.0, 0.0, 0.0))
print(f"\nзаписал -1.0 -> читается ДО play: {attr.Get()}")
tl.play()
await app_utils.update_app_async(steps=5)
print(f"после play, 5 шагов: {attr.Get()}")
await app_utils.update_app_async(steps=20)
print(f"после play, 25 шагов: {attr.Get()}")
rp = RigidPrim(paths=[PROBE])
x0 = rp.get_world_poses()[0].numpy()[0][0]
for i in range(6):
await app_utils.update_app_async(steps=20)
x = rp.get_world_poses()[0].numpy()[0][0]
print(f" шаг {25+(i+1)*20:4d}: v={attr.Get()} проба x={x:+.3f} прошла {(x-x0)*1000:+7.1f} мм")
tl.stop()
await app_utils.update_app_async(steps=5)
print(f"\nпосле stop: {attr.Get()}")
scripts/env_scan.py
+43
View File
@@ -0,0 +1,43 @@
"""Check ConveyorTrack_05 geometry, existing floor/ground, and lighting in the fresh stage."""
import omni.usd
from pxr import Usd, UsdGeom, UsdLux
stage = omni.usd.get_context().get_stage()
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
xc = UsdGeom.XformCache()
def report(path):
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
print(f"{path} MISSING"); return
r = bbc.ComputeWorldBound(prim).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
M = xc.GetLocalToWorldTransform(prim)
lx = M.TransformDir((1,0,0)); lx = lx/(lx.GetLength() or 1)
print(f"{path}")
print(f" bbox x[{mn[0]:+.3f}..{mx[0]:+.3f}] y[{mn[1]:+.3f}..{mx[1]:+.3f}] z[{mn[2]:+.3f}..{mx[2]:+.3f}]")
print(f" local+X in world = ({lx[0]:+.3f},{lx[1]:+.3f},{lx[2]:+.3f})")
for p in ["/World/ConveyorTrack_05", "/World/ConveyorTrack_05/Belt"]:
report(p)
print("\n=== whole /World bbox ===")
w = stage.GetPrimAtPath("/World")
r = bbc.ComputeWorldBound(w).ComputeAlignedRange()
print(f" x[{r.GetMin()[0]:+.2f}..{r.GetMax()[0]:+.2f}] y[{r.GetMin()[1]:+.2f}..{r.GetMax()[1]:+.2f}] z[{r.GetMin()[2]:+.2f}..{r.GetMax()[2]:+.2f}]")
print("\n=== lights ===")
n = 0
for p in stage.Traverse():
if p.IsA(UsdLux.BoundableLightBase) or p.IsA(UsdLux.NonboundableLightBase):
n += 1
inten = p.GetAttribute("inputs:intensity")
vis = UsdGeom.Imageable(p).ComputeVisibility()
print(f" {p.GetPath()} type={p.GetTypeName()} intensity={inten.Get() if inten else '?'} vis={vis}")
print(f" total lights: {n}")
print("\n=== any ground/floor plane already present? ===")
for p in stage.Traverse():
name = p.GetName().lower()
if "floor" in name or "ground" in name or "plane" in name:
print(" candidate:", p.GetPath(), p.GetTypeName())
scripts/exc_test.py
+18
View File
@@ -0,0 +1,18 @@
"""Minimal repro: can Python catch the Tf threading-violation error at all?"""
import omni.usd
from pxr import UsdPhysics
import isaacsim.core.experimental.utils.app as app_utils
omni.usd.get_context().open_stage("/home/dasha/robozon-sorter/scene/plow_cell_90_45_test.usd")
await app_utils.update_app_async(steps=30)
stage = omni.usd.get_context().get_stage()
print("attempting Scene.Define with bare except:")
try:
scene = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim()
print("SUCCESS, no exception")
except:
import traceback
print("CAUGHT something:")
traceback.print_exc()
print("after try/except block, script continues")
scripts/exc_test2.py
+27
View File
@@ -0,0 +1,27 @@
"""Does the threading violation clear if we just retry Scene.Define a few times?
Never let an exception escape this script so stdout survives regardless of outcome."""
import asyncio
import omni.usd
from pxr import UsdPhysics
import isaacsim.core.experimental.utils.app as app_utils
omni.usd.get_context().open_stage("/home/dasha/robozon-sorter/scene/plow_cell_90_45_test.usd")
print("opened, waiting...")
await app_utils.update_app_async(steps=120)
await asyncio.sleep(3.0)
await app_utils.update_app_async(steps=60)
stage = omni.usd.get_context().get_stage()
print("stage settled, prim count:", len(list(stage.Traverse())))
for attempt in range(8):
try:
scene = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim()
print(f"attempt {attempt}: SUCCESS, prim valid={scene.IsValid()}")
break
except BaseException as exc:
print(f"attempt {attempt}: FAILED, type={type(exc).__name__}")
await app_utils.update_app_async(steps=60)
await asyncio.sleep(1.0)
else:
print("all attempts failed")
print("done")
scripts/export_item_library.py
+76
View File
@@ -0,0 +1,76 @@
#!/usr/bin/env python3
"""Flatten the classified objects out of the working scene into assets/items/.
/home/whatevenif/isaacsim/python.sh scripts/export_item_library.py
The six meshes bundled in `assets/meshes/` are a smoke-test set, two per class. A sorting
run wants the whole catalogue, and the classified objects live inside
`robozon_conveyor_scaled.usd` under `/World/CVObjects`, referencing `.glb` files that are
not in this repo. Each one is therefore *flattened* on export so the result carries its own
geometry and composes anywhere - the recipe in CLAUDE.md, applied in bulk.
Ground truth comes from `categories.json`: `zone` (B/C/D) and `obb_extents_m`, which the
run harness reports predicted dimensions against. Objects larger than the 500 mm incoming
envelope are skipped - they could never reach this conveyor.
"""
from __future__ import annotations
import json
import sys
from pathlib import Path
from pxr import Usd, UsdGeom
ASSETS = Path("/home/dasha/isaac_assets")
SCENE = ASSETS / "robozon_conveyor_scaled.usd"
CATS = ASSETS / "categories.json"
OUT = Path(__file__).resolve().parent.parent / "assets" / "items"
ENVELOPE_MM = 500.0
def main():
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
cats = json.loads(CATS.read_text())
OUT.mkdir(parents=True, exist_ok=True)
src = Usd.Stage.Open(str(SCENE))
root = src.GetPrimAtPath("/World/CVObjects")
if not root.IsValid():
sys.exit("/World/CVObjects missing")
present = {p.GetName() for p in root.GetChildren()}
manifest, skipped = {}, []
for name, meta in sorted(cats.items()):
if name not in present:
skipped.append((name, "not in scene"))
continue
ext_mm = [e * 1000.0 for e in meta["obb_extents_m"]]
if max(ext_mm) > ENVELOPE_MM:
skipped.append((name, f"oversize {max(ext_mm):.0f} mm"))
continue
ns = Usd.Stage.CreateInMemory()
item = UsdGeom.Xform.Define(ns, "/Item")
item.GetPrim().GetReferences().AddReference(str(SCENE), f"/World/CVObjects/{name}")
ns.SetDefaultPrim(item.GetPrim())
dst = OUT / f"{name}.usd"
ns.Flatten().Export(str(dst))
manifest[name] = dict(zone=meta["zone"],
gt_dims_mm=[round(v) for v in sorted(ext_mm, reverse=True)],
k_round=round(meta.get("k_round", 0.0), 3),
label_ru=meta.get("label_ru", ""))
print(f" {name:22s} {meta['zone']} {manifest[name]['gt_dims_mm']} "
f"{dst.stat().st_size / 1024:.0f} KB")
(OUT / "manifest.json").write_text(json.dumps(manifest, indent=2, ensure_ascii=False))
from collections import Counter
print(f"\nexported {len(manifest)} -> {OUT} "
f"{dict(Counter(v['zone'] for v in manifest.values()))}")
if skipped:
print(f"skipped {len(skipped)}: " + ", ".join(f"{n} ({w})" for n, w in skipped[:8]))
if __name__ == "__main__":
main()
scripts/extend_transition_decks.py
+132
View File
@@ -0,0 +1,132 @@
#!/usr/bin/env python3
"""Carry the driven surface inboard, to where the plow can actually deliver.
/home/whatevenif/isaacsim/python.sh scripts/extend_transition_decks.py
Measured with one item and a full 42 deg sweep: the blade imparts a real push (item picks up
0.59 m/s and travels 139 mm sideways) but leaves it at **y = 0.261**, and every driven
surface past the plow starts at **|y| = 0.380**:
belt ConveyorTrack_04 x -7.00..-6.00 y -0.450..+0.450
deck PlowTransition_C x -7.00..-6.00 y +0.380..+0.698
lane C x -8.12..-6.39 y +0.380..+2.112
lane B x -7.03..-6.58 y -2.379..-0.380 (no transition deck at all)
So there is a 119 mm band where a swept item sits on the very lip of the main belt with
nothing driving it toward its lane. That is the gap the goods die in - not a hole they fall
through, a strip with no traction, right where the blade lets go of them.
This closes it from the inside: each transition plate is brought in to |y| = 0.20, well
short of where the blade releases, and B gets the plate it never had. `plow_sort.DECK_DIR`
already drives both toward their lanes, so an item landing here is carried on instead of
stopping.
Plates are static Cubes with collision, coplanar with the belt at z 1.7805, and are made
kinematic + surface-driven at load time like every other deck.
"""
from __future__ import annotations
import sys
from pathlib import Path
from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics
SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
TOP_Z = 1.7805
THICK = 0.01
INBOARD = 0.20 # how far in the driven surface now reaches
# Lane B is perpendicular, so a straight strip meets it flush.
PLATES_STRAIGHT = {"PlowTransition_B": (-7.03, -6.45, -0.380)}
# Lane C is laid at 45 deg. Its bounding box, x -8.123..-6.391 by y 0.380..2.112, is the
# AABB of a rotated rectangle and describes a footprint the belt does not have: the near
# side is a single CORNER at (-7.257, 0.380), and the edge runs away from it at 45 deg,
#
# y = x + 7.637
#
# so at x -7.03 the lane really starts at y 0.607, and at x -6.39 at y 1.247 - not at 0.380
# anywhere except that one corner. A straight plate ending at y 0.380 therefore leaves a
# widening wedge of open air, which is the dark triangle in the viewport and where
# `bolts_cluster` fell through after the blade had successfully pushed it to y +0.425.
#
# The C plate is a trapezoid instead: inboard edge at |y| = INBOARD, outer edge ON the
# lane's diagonal.
LANE_C_EDGE = lambda x: x + 7.637
def _plate(stage, name, x0, x1, y_in, y_out):
path = f"/World/{name}"
prim = stage.GetPrimAtPath(path)
if prim.IsValid():
stage.RemovePrim(path)
cube = UsdGeom.Cube.Define(stage, path)
cube.CreateSizeAttr().Set(2.0) # size 2 so the scale op IS the half-extent
cx, cy = (x0 + x1) / 2.0, (y_in + y_out) / 2.0
hx, hy = abs(x1 - x0) / 2.0, abs(y_out - y_in) / 2.0
xf = UsdGeom.Xformable(cube.GetPrim())
xf.ClearXformOpOrder()
xf.AddTranslateOp().Set(Gf.Vec3d(cx, cy, TOP_Z - THICK))
xf.AddScaleOp().Set(Gf.Vec3f(hx, hy, THICK))
cube.CreateDisplayColorAttr().Set([Gf.Vec3f(0.30, 0.31, 0.33)])
UsdPhysics.CollisionAPI.Apply(cube.GetPrim())
return path, (round(cx - hx, 3), round(cx + hx, 3), round(cy - hy, 3), round(cy + hy, 3))
def _trapezoid(stage, name, quad):
"""a thin prism whose top face is the given 4 corners, coplanar with the belt.
A Cube cannot do this - the plate has to follow a 45 deg edge, so it is authored as an
explicit mesh. Given thickness rather than left as a zero-height quad: a flat sheet is a
poor collider and goods catch on its rim.
"""
path = f"/World/{name}"
if stage.GetPrimAtPath(path).IsValid():
stage.RemovePrim(path)
mesh = UsdGeom.Mesh.Define(stage, path)
top = [Gf.Vec3f(x, y, TOP_Z) for x, y in quad]
bot = [Gf.Vec3f(x, y, TOP_Z - THICK) for x, y in quad]
pts = top + bot
mesh.CreatePointsAttr().Set(pts)
faces, counts = [], []
faces += [0, 1, 2, 3]; counts.append(4) # top
faces += [7, 6, 5, 4]; counts.append(4) # bottom
for i in range(4): # sides
j = (i + 1) % 4
faces += [i, 4 + i, 4 + j, j]; counts.append(4)
mesh.CreateFaceVertexIndicesAttr().Set(faces)
mesh.CreateFaceVertexCountsAttr().Set(counts)
xs = [p[0] for p in pts]; ys = [p[1] for p in pts]; zs = [p[2] for p in pts]
mesh.CreateExtentAttr().Set([Gf.Vec3f(min(xs), min(ys), min(zs)),
Gf.Vec3f(max(xs), max(ys), max(zs))])
mesh.CreateDisplayColorAttr().Set([Gf.Vec3f(0.30, 0.31, 0.33)])
mesh.CreateSubdivisionSchemeAttr().Set("none")
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
UsdPhysics.MeshCollisionAPI.Apply(mesh.GetPrim()).CreateApproximationAttr().Set("convexHull")
return path
def main():
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
stage = Usd.Stage.Open(str(SCENE))
for name, (x0, x1, y_out) in PLATES_STRAIGHT.items():
y_in = INBOARD if y_out > 0 else -INBOARD
path, span = _plate(stage, name, x0, x1, y_in, y_out)
print(f" {name:20s} straight x[{span[0]:+.3f},{span[1]:+.3f}] "
f"y[{span[2]:+.3f},{span[3]:+.3f}]")
x0, x1 = -7.03, -6.39
quad = [(x0, INBOARD), (x1, INBOARD), (x1, LANE_C_EDGE(x1)), (x0, LANE_C_EDGE(x0))]
_trapezoid(stage, "PlowTransition_C", quad)
print(f" PlowTransition_C trapezoid corners "
+ " ".join(f"({a:+.2f},{b:+.2f})" for a, b in quad))
print(f" outer edge follows the lane diagonal y = x + 7.637 "
f"({LANE_C_EDGE(x0):+.3f} at x={x0}, {LANE_C_EDGE(x1):+.3f} at x={x1})")
stage.GetRootLayer().Save()
print(f"saved {SCENE}")
print(f"driven surface now reaches |y| = {INBOARD}; the blade releases goods at ~0.26")
if __name__ == "__main__":
main()
scripts/fetch_assets.py
Executable
+83
View File
@@ -0,0 +1,83 @@
#!/usr/bin/env python3
"""Fetch the conveyor art that scene/sorter.usd references.
python scripts/fetch_assets.py
~270 MB, too big for git. Downloads in resumable Range chunks: the Omniverse bucket is
slow enough that a plain GET truncates, and a half-written USD fails to compose with a
confusing "could not open asset" rather than an obvious size error.
"""
from __future__ import annotations
import os, sys, time, urllib.request
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
DEST = ROOT / "assets" / "conveyors"
BASE = ("https://omniverse-content-production.s3-us-west-2.amazonaws.com/"
"Assets/Isaac/6.0/Isaac/Props/Conveyors/")
CHUNK, TRIES = 1 << 21, 12
# the scene composes these; the Textures/ and Material Library/ folders come with them
FILES = ["ConveyorBelt_A06.usd", "ConveyorBelt_A24.usd"]
TEXTURES = [
"Textures/M_ConveyorBelt_A01_Belt.usd", "Textures/M_ConveyorBelt_A01_Decal.usd",
"Textures/MetalPainted_Blue_Glossy_A.usd", "Textures/Plastic_Orange_A.usd",
"Textures/Plastic_Rough_Black_A.usd", "Textures/Steel_A.usd",
"Textures/T_ConveyorBelt_A01_Belt_Albedo.png", "Textures/T_ConveyorBelt_A01_Belt_Normal.png",
"Textures/T_ConveyorBelt_A01_Belt_ORM.png",
"Textures/T_ConveyorsBelt_A01_Decal_Albedo.png", "Textures/T_ConveyorsBelt_A01_Decal_Alpha.png",
"Textures/T_ConveyorsBelt_A01_Decal_ORM.png",
"Material%20Library/physics_material.usd",
]
def size_of(url):
try:
req = urllib.request.Request(url, method="HEAD")
with urllib.request.urlopen(req, timeout=60) as r:
return int(r.headers.get("Content-Length", 0))
except Exception:
return 0
def grab(url, out: Path):
total = size_of(url)
have = out.stat().st_size if out.exists() else 0
if total and have == total:
print(f" {out.name}: present"); return True
out.parent.mkdir(parents=True, exist_ok=True)
if not total:
urllib.request.urlretrieve(url, out); return out.exists()
pos = have if have < total else 0
with open(out, "r+b" if pos else "wb") as fh:
fh.seek(pos); t0 = time.time()
while pos < total:
end = min(pos + CHUNK - 1, total - 1)
for a in range(TRIES):
try:
req = urllib.request.Request(url)
req.add_header("Range", f"bytes={pos}-{end}")
with urllib.request.urlopen(req, timeout=120) as r:
data = r.read()
if len(data) != end - pos + 1:
raise IOError("short chunk")
fh.write(data); fh.flush(); pos += len(data)
print(f"\r {out.name}: {100.0*pos/total:5.1f}%"
f" {(pos-have)/max(time.time()-t0,1e-6)/1024:6.0f} KB/s", end="", flush=True)
break
except Exception as exc:
if a == TRIES - 1:
print(f"\n {out.name}: FAILED at {pos}: {exc}"); return False
time.sleep(min(2 ** a, 20))
print()
return out.stat().st_size == total
def main():
print(f"fetching conveyor art into {DEST}")
ok = True
for rel in FILES + TEXTURES:
local = DEST / rel.replace("%20", " ")
ok &= grab(BASE + rel, local)
print("\nall present" if ok else "\nincomplete - re-run, downloads resume")
return 0 if ok else 1
if __name__ == "__main__":
sys.exit(main())
scripts/fetch_models.py
+111
View File
@@ -0,0 +1,111 @@
#!/usr/bin/env python3
"""Fetch the model weights the pipeline needs. They are far too big for git.
python scripts/fetch_models.py
Downloads in small Range chunks with per-chunk retries: the CRE weights come off a slow
mirror and a plain single-stream GET truncates silently, leaving an unloadable file.
"""
from __future__ import annotations
import os
import sys
import time
import urllib.request
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
MODELS = ROOT / "assets" / "models"
CHUNK = 1 << 21
TRIES = 12
SOURCES = {
"FastSAM-s.pt":
"https://huggingface.co/Ultralytics/FastSAM/resolve/main/FastSAM-s.pt?download=true",
# CRE-Stereo ETH3D weights, from the megvii research release mirror
"crestereo_eth3d.pth":
"https://github.com/ibaiGorordo/CREStereo-Pytorch/releases/download/0.0.1/crestereo_eth3d.pth",
}
CRE_REPO = "https://github.com/ibaiGorordo/CREStereo-Pytorch"
def content_length(url):
req = urllib.request.Request(url, method="HEAD")
with urllib.request.urlopen(req, timeout=60) as r:
if r.status in (301, 302) and r.headers.get("Location"):
return content_length(r.headers["Location"])
return int(r.headers.get("Content-Length", 0))
def get_range(url, a, b):
req = urllib.request.Request(url)
req.add_header("Range", f"bytes={a}-{b}")
with urllib.request.urlopen(req, timeout=120) as r:
return r.read()
def fetch(name, url, out_dir):
out = out_dir / name
total = content_length(url)
have = out.stat().st_size if out.exists() else 0
if total and have == total:
print(f" {name}: already complete ({total/1e6:.1f} MB)")
return True
if not total: # server refuses HEAD: plain download
print(f" {name}: streaming (no content-length)")
urllib.request.urlretrieve(url, out)
return out.exists()
if have > total:
have = 0
mode = "r+b" if have else "wb"
pos = have
with open(out, mode) as fh:
fh.seek(pos)
t0 = time.time()
while pos < total:
end = min(pos + CHUNK - 1, total - 1)
for attempt in range(TRIES):
try:
data = get_range(url, pos, end)
if len(data) != end - pos + 1:
raise IOError("short chunk")
fh.write(data)
fh.flush()
pos += len(data)
pct = 100.0 * pos / total
rate = (pos - have) / max(time.time() - t0, 1e-6) / 1024
print(f"\r {name}: {pct:5.1f}% {rate:6.0f} KB/s", end="", flush=True)
break
except Exception as exc:
if attempt == TRIES - 1:
print(f"\n {name}: FAILED at byte {pos}: {exc}")
return False
time.sleep(min(2 ** attempt, 20))
print()
ok = out.stat().st_size == total
print(f" {name}: {'ok' if ok else 'SIZE MISMATCH'} ({out.stat().st_size/1e6:.1f} MB)")
return ok
def main():
MODELS.mkdir(parents=True, exist_ok=True)
print(f"fetching model weights into {MODELS}")
results = {n: fetch(n, u, MODELS) for n, u in SOURCES.items()}
cre_dir = MODELS / "crestereo"
if not (cre_dir / "nets").exists():
print(f"\nCRE-Stereo network code is not vendored here. Clone it next to the weights:")
print(f" git clone {CRE_REPO} {cre_dir}")
print(" (only the `nets` package is imported)")
missing = [n for n, ok in results.items() if not ok]
if missing:
print(f"\nincomplete: {missing} - re-run, downloads resume where they stopped")
return 1
print("\nall weights present")
return 0
if __name__ == "__main__":
sys.exit(main())
scripts/fix_plow_reach_and_trays.py
+110
View File
@@ -0,0 +1,110 @@
#!/usr/bin/env python3
"""Close the two geometry faults that stop goods reaching a tray.
/home/whatevenif/isaacsim/python.sh scripts/fix_plow_reach_and_trays.py
**1. Tray B is walled shut on the side goods arrive from.** Measured:
B_W0 (near, y -2.55) z 1.16 .. 2.00 <- 220 mm ABOVE the lane belt (1.7805)
B_W1 (far, y -3.35) z 1.16 .. 1.70
tray C, correctly built:
C_W1 (near, y +1.88) z 1.16 .. 1.70 <- 80 mm BELOW the belt, goods slide over
C_W0 (far, y +2.68) z 1.16 .. 2.00
B has its tall backboard on the near face instead of the far one, so the lane runs
goods straight into a wall. The two heights are swapped to match C.
**2. The plow cannot reach the lane.** The arm is 600 mm on a hinge at the belt centre, so
its tip reaches ``0.6 * sin(limit)``. At the authored +-35 deg that is 344 mm, while
the lanes start at |y| = 450 mm: a 106 mm dead zone no command can cross. Goods are
nudged to about y 0.15 and left on the line, which is exactly what every trace shows.
Raising the joint limit to +-45 deg gives 424 mm of tip travel. That is still short of
450 mm *at the centre of the item*, but an item is not a point: a 150 mm-wide box is
carried by the lane once its near edge crosses, i.e. at a centre of about 375 mm, so
42 deg (402 mm) delivers it with margin. Moving the lanes inboard instead was rejected -
they would overlap the main belt, and two coincident belt colliders at the same height
is its own failure.
Both edits are written back into scene/plow_cell.usd.
"""
from __future__ import annotations
import sys
from pathlib import Path
from pxr import Usd, UsdGeom, UsdPhysics
SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
HINGE = "/World/Diverters/DiverterEnd/ArmHinge"
TRAYS = "/World/PlowContainers"
NEW_LIMIT = 45.0 # joint hard limit, degrees either side
NEAR_WALL_TOP = 1.70 # must sit below the lane belt at 1.7805
FAR_WALL_TOP = 2.00
def _range(stage, path):
return UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
stage.GetPrimAtPath(path)).ComputeAlignedRange()
def _set_top(stage, path, top):
"""scale a wall Cube about its base so its top lands at `top`"""
prim = stage.GetPrimAtPath(path)
r = _range(stage, path)
z0, z1 = r.GetMin()[2], r.GetMax()[2]
if abs(z1 - top) < 1e-4:
return None
want = max(top - z0, 0.02)
xf = UsdGeom.Xformable(prim)
for op in xf.GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeScale:
s = op.Get()
op.Set(type(s)(s[0], s[1], s[2] * (want / max(z1 - z0, 1e-6))))
break
else:
return None
# keep the base where it was: scaling a centred cube moves it by half the delta
for op in xf.GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
t = op.Get()
op.Set(type(t)(t[0], t[1], t[2] + (want - (z1 - z0)) / 2.0))
break
return (round(z1, 3), round(_range(stage, path).GetMax()[2], 3))
def main():
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
stage = Usd.Stage.Open(str(SCENE))
print("tray B - swap the tall backboard to the far face")
for wall, top, tag in ((f"{TRAYS}/B_W0", NEAR_WALL_TOP, "near (goods arrive)"),
(f"{TRAYS}/B_W1", FAR_WALL_TOP, "far (backboard)")):
if not stage.GetPrimAtPath(wall).IsValid():
print(f" {wall} missing"); continue
changed = _set_top(stage, wall, top)
print(f" {wall.rsplit('/', 1)[1]:6s} {tag:22s} "
+ (f"top {changed[0]} -> {changed[1]}" if changed else "already correct"))
print(f"plow hinge - raise the limit so the arm can reach the lane")
hinge = stage.GetPrimAtPath(HINGE)
if hinge.IsValid():
j = UsdPhysics.RevoluteJoint(hinge)
lo, hi = j.GetLowerLimitAttr().Get(), j.GetUpperLimitAttr().Get()
j.GetLowerLimitAttr().Set(-NEW_LIMIT)
j.GetUpperLimitAttr().Set(NEW_LIMIT)
import math
print(f" limits {lo:+.0f}/{hi:+.0f} -> {-NEW_LIMIT:+.0f}/{NEW_LIMIT:+.0f} "
f"(tip reach {0.6 * math.sin(math.radians(abs(hi))):.3f} -> "
f"{0.6 * math.sin(math.radians(NEW_LIMIT)):.3f} m, lane edge at 0.450)")
else:
print(f" {HINGE} missing")
stage.GetRootLayer().Save()
print(f"saved {SCENE}")
if __name__ == "__main__":
main()
scripts/geom_scan.py
+47
View File
@@ -0,0 +1,47 @@
"""Scan the new 90/45 scene: exact belt geometry, plow decks, and containers."""
import omni.usd
from pxr import Usd, UsdGeom, UsdPhysics
stage = omni.usd.get_context().get_stage()
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
xc = UsdGeom.XformCache()
def report(path):
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
print(f"{path} MISSING")
return
r = bbc.ComputeWorldBound(prim).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
M = xc.GetLocalToWorldTransform(prim)
localx = M.TransformDir((1,0,0))
localx = localx / (localx.GetLength() or 1)
active = prim.IsActive()
print(f"{path} active={active}")
print(f" bbox x[{mn[0]:+.3f}..{mx[0]:+.3f}] y[{mn[1]:+.3f}..{mx[1]:+.3f}] z[{mn[2]:+.3f}..{mx[2]:+.3f}]")
print(f" local+X in world = ({localx[0]:+.3f},{localx[1]:+.3f},{localx[2]:+.3f})")
kids = [c.GetName() for c in prim.GetChildren()]
print(f" children: {kids}")
print("=== root-level /ConveyorTrack_01 duplicate check ===")
p = stage.GetPrimAtPath("/ConveyorTrack_01")
print("exists:", p.IsValid())
for path in ["/World/ConveyorTrack_04", "/World/ConveyorTrack_04/Belt",
"/World/ConveyorTrack_06", "/World/ConveyorTrack_06/Belt",
"/World/ConveyorTrack_01", "/World/ConveyorTrack_01/Belt",
"/World/PlowCornerDeck_B", "/World/PlowCornerDeck_C",
"/World/PlowTransition_B", "/World/PlowTransition_C",
"/World/PlowContainers", "/World/Diverters/DiverterEnd"]:
report(path)
print()
print("=== PlowContainers full subtree ===")
pc = stage.GetPrimAtPath("/World/PlowContainers")
if pc.IsValid():
for c in Usd.PrimRange(pc):
r = bbc.ComputeWorldBound(c).ComputeAlignedRange()
if not r.IsEmpty():
mn, mx = r.GetMin(), r.GetMax()
print(f" {c.GetPath()} type={c.GetTypeName()} "
f"x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]")
scripts/gravity_check.py
+43
View File
@@ -0,0 +1,43 @@
"""Is gravity actually simulated at all right now? Check PhysicsScene + one item's live state."""
import omni.usd, omni.timeline
from pxr import UsdPhysics, PhysxSchema, UsdGeom, Usd
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
print("timeline playing:", tl.is_playing())
sc = stage.GetPrimAtPath("/World/PhysicsScene")
print("PhysicsScene valid:", sc.IsValid())
if sc.IsValid():
physxScene = PhysxSchema.PhysxSceneAPI(sc)
print(" gravityMagnitude:", sc.GetAttribute("physics:gravityMagnitude").Get())
print(" gravityDirection:", sc.GetAttribute("physics:gravityDirection").Get())
print(" timeStepsPerSecond:", sc.GetAttribute("physxScene:timeStepsPerSecond").Get())
print(" enableCCD:", sc.GetAttribute("physxScene:enableCCD").Get())
name = "bottle"
prim = stage.GetPrimAtPath(f"/World/Items/{name}")
print(f"\n{name} prim valid:", prim.IsValid())
print(" applied schemas:", list(prim.GetAppliedSchemas()))
print(" kinematicEnabled:", prim.GetAttribute("physics:kinematicEnabled").Get())
attr = prim.GetAttribute("xformOp:translate")
print(" authored translate:", attr.Get())
rp = RigidPrim(paths=[f"/World/Items/{name}"])
print(" RigidPrim world pose (fabric):", rp.get_world_poses()[0].numpy()[0])
# check mass / collision on descendant meshes
n_col = 0
for p in Usd.PrimRange(prim):
if p.HasAPI(UsdPhysics.CollisionAPI) or p.HasAPI(UsdPhysics.MeshCollisionAPI):
n_col += 1
print(" descendant prims with CollisionAPI:", n_col)
print("\n=== stepping physics 60 more times, watching bottle's Z ===")
if not tl.is_playing():
tl.play()
for i in range(6):
await app_utils.update_app_async(steps=10)
p = rp.get_world_poses()[0].numpy()[0]
print(f" step batch {i}: pos={p} playing={tl.is_playing()} time={tl.get_current_time():.3f}")
scripts/impulse_probe.py
+105
View File
@@ -0,0 +1,105 @@
"""1) find the working RigidPrim velocity-set signature
2) does Belt_01 (now aimed diagonally) actually carry an item into BinD?
3) does a per-step 'carry assist' (matching the item's +Y velocity to the blade's)
get a pushed item across, where the bare blade tops out at ~0.21 m?"""
import sys, inspect
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, omni.timeline
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene, plow_cell_9045
print("set_velocities signature:", inspect.signature(RigidPrim.set_velocities))
print("get_velocities signature:", inspect.signature(RigidPrim.get_velocities))
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
info = await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True)
print("blade width:", info["pusher_dims"][0], "m")
ipath = "/World/Items/_imp"
def spawn(x, y, mesh="bag"):
if stage.GetPrimAtPath(ipath).IsValid():
stage.RemovePrim(ipath)
prim = UsdGeom.Xform.Define(stage, ipath).GetPrim()
prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{mesh}.usd"))
xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(x, y, C.BELT_Z + 0.06))
UsdPhysics.RigidBodyAPI.Apply(prim)
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)
UsdGeom.Imageable(prim).MakeVisible()
return RigidPrim(paths=[ipath])
# ---------- 2) Belt_01 -> BinD ----------
print("\n--- item placed on Belt_01 at (-4.10,+0.70), does it reach BinD? ---")
rp = spawn(-4.10, 0.70)
tl.play(); await app_utils.update_app_async(steps=10)
for i in range(8):
await app_utils.update_app_async(steps=45)
p = rp.get_world_poses()[0].numpy()[0]
print(f" t~{(i+1)*45/60:4.1f}s x={float(p[0]):+.2f} y={float(p[1]):+.2f} z={float(p[2]):+.2f}")
p = rp.get_world_poses()[0].numpy()[0]
print(" IN BIN_D:", (-6.21 < float(p[0]) < -4.95) and (1.57 < float(p[1]) < 2.86))
tl.stop(); await app_utils.update_app_async(steps=6)
# ---------- 1)+3) velocity API and carry assist ----------
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
bop = op; break
bbase = bop.Get()
def blade_to(y):
bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2]))
SENSE_X = C.PUSH_X + plow_cell_9045.PUSHER_X_MM / 2000.0
for mode in ("bare blade", "blade + carry assist"):
blade_to(C.BLADE_HOME_Y)
rp = spawn(-3.05, 0.0)
tl.play(); await app_utils.update_app_async(steps=8)
for _ in range(400):
if float(rp.get_world_poses()[0].numpy()[0][0]) <= SENSE_X:
break
await app_utils.update_app_async(steps=1)
p0 = rp.get_world_poses()[0].numpy()[0].copy()
a, b, spd = C.BLADE_HOME_Y, 0.52, 1.3
dur = abs(b - a) / spd
t0 = float(tl.get_current_time()); err = None
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / dur)
blade_to(a + (b - a) * u)
if mode.endswith("assist") and u < 1.0:
try:
lin = rp.get_velocities()[0].numpy()[0]
rp.set_velocities(np.array([[float(lin[0]), spd, float(lin[2])]]),
np.array([[0.0, 0.0, 0.0]]))
except BaseException as exc:
err = f"{type(exc).__name__}: {exc}"
break
await app_utils.update_app_async(steps=1)
if u >= 1.0:
break
if err:
print(f"\n {mode}: set_velocities FAILED -> {err}")
else:
for _ in range(90):
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
onbranch = float(p[1]) > 0.45
print(f"\n {mode}: dy={float(p[1])-float(p0[1]):+.3f} final=({float(p[0]):+.2f},"
f"{float(p[1]):+.2f},{float(p[2]):+.2f}) reached_branch={onbranch}")
tl.stop(); await app_utils.update_app_async(steps=6)
scripts/live_demo_webrtc.py
+86
View File
@@ -0,0 +1,86 @@
"""Self-running plow demo for watching over WebRTC.
Sent into the live streaming Kit. Unlike the test harness this does **not** block in a
loop: it hooks the feeder and the plow onto the physics step, aims the viewport at the plow,
presses Play and returns. The cell then runs on its own for as long as the session lives,
which is what makes it watchable in the browser - a blocking script would hold the
interpreter and the stream would show a frozen frame.
The subscriptions are stashed in the module namespace on purpose. A PhysX step
subscription dies the moment its Python handle is garbage-collected, so a demo that forgets
to keep a reference stops after the call returns and looks like the scene simply ignoring
the Play button.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import importlib
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
del sys.modules[_m]
importlib.invalidate_caches()
import omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.rendering_manager import ViewportManager
from robozon_sorter import config as C
from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.spawner import AutoFeeder
SPEED = float(globals().get("speed", 0.8))
PITCH = float(globals().get("pitch", 3.0))
RATE = float(globals().get("rate", 300.0))
N = int(globals().get("n", 8))
C.BELT_SPEED = SPEED
C.PLOW_SWEEP_RATE = RATE
stage, info = plow_vision.load(belt_speed=SPEED, script_control=True,
meshes_dir=f"{REPO}/assets/items")
staging.stage_cell(stage, preset="bright", floor=True)
plow_sort.keep_lanes_active(stage)
lanes = plow_sort.configure_lanes(stage, SPEED)
plow_sort.open_junction(stage)
items = {k: v["zone"] for k, v in info["items"].items()}
await app_utils.update_app_async(steps=40)
cell = Cell(stage, items.keys())
cell.park_all()
await app_utils.update_app_async(steps=15)
order = ([n for n in sorted(items) if items[n] == "B"][:3]
+ [n for n in sorted(items) if items[n] == "C"][:3]
+ [n for n in sorted(items) if items[n] == "D"][:2])[:N]
sorter = plow_sort.PlowSorter(stage, cell, items, plow_sort.calibrate_mapping())
beams = lane_beams.LaneBeams(stage, cell, plow=sorter.plow)
def _step(dt):
try:
sorter.update(dt)
beams.tick(dt)
beams.poll(rate=RATE)
except Exception:
pass
from omni.physx import get_physx_interface
# keep the handles alive in the namespace or the callbacks are collected and the cell stops
STEP_SUB = get_physx_interface().subscribe_physics_step_events(_step)
FEEDER = AutoFeeder(cell, order=order, pitch=PITCH, route=dict(items), loop=True).install()
# look at the plow from the discharge side so the sweep and both lanes are in frame
ViewportManager.set_camera_view("/OmniverseKit_Persp",
eye=[-5.2, -3.4, 3.2], target=[-7.0, 0.0, 1.9])
await app_utils.update_app_async(steps=20)
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
print(f"LIVE: {len(order)} items looping, pitch {PITCH} m @ {SPEED} m/s, "
f"sweep {RATE} deg/s (tip {C.PLOW_ARM_LEN * RATE * 3.14159 / 180:.2f} m/s)")
print(f"order: {order}")
print(f"lanes/decks driven: {len(lanes)} | mapping {sorter.mapping}")
print("running on the physics step - the stream stays live, nothing is blocking")
scripts/lower_plow_to_belt.py
+81
View File
@@ -0,0 +1,81 @@
#!/usr/bin/env python3
"""Drop the plow so its blade sweeps at belt level instead of over the goods.
/home/whatevenif/isaacsim/python.sh scripts/lower_plow_to_belt.py
Measured on the built scene:
blade z 1.810 .. 1.890
belt top z 1.781
gap under the blade = 29 mm
29 mm is enough for flat goods to pass straight under the blade, and tall ones get caught
near their top edge and tipped rather than led across. It is the reason items reached only
y ~ 0.24 while the arm was correctly holding 42 deg with 402 mm of reach: the blade was not
touching them at all, so no amount of angle or lane geometry could have helped.
The whole ``DiverterEnd`` is moved, not just the arm: base and arm keep their relative
placement, so the hinge stays consistent whether the arm is driven kinematically or by its
joint. The pedestal sinks the same 27 mm, which is invisible - it stands on the floor.
Target clearance is 2 mm: enough that the blade is not grinding on the belt collider,
little enough that nothing rides under it.
"""
from __future__ import annotations
import sys
from pathlib import Path
from pxr import Gf, Usd, UsdGeom
SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
DIVERTER = "/World/Diverters/DiverterEnd"
ARM = DIVERTER + "/Arm"
BELT = "/World/ConveyorTrack_04/Belt"
CLEARANCE = 0.002
def _range(stage, path):
return UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
stage.GetPrimAtPath(path)).ComputeAlignedRange()
def main():
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
stage = Usd.Stage.Open(str(SCENE))
arm = _range(stage, ARM)
belt = _range(stage, BELT)
if arm.IsEmpty() or belt.IsEmpty():
sys.exit("arm or belt has no bounds - wrong scene?")
blade_bottom, belt_top = arm.GetMin()[2], belt.GetMax()[2]
gap = blade_bottom - belt_top
drop = gap - CLEARANCE
print(f"blade bottom z {blade_bottom:.4f} | belt top z {belt_top:.4f}")
print(f"gap {gap * 1000:.0f} mm -> target {CLEARANCE * 1000:.0f} mm, dropping {drop * 1000:.0f} mm")
if abs(drop) < 1e-4:
print("already at height, nothing to do")
return
prim = stage.GetPrimAtPath(DIVERTER)
xf = UsdGeom.Xformable(prim)
for op in xf.GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
t = op.Get()
op.Set(Gf.Vec3d(t[0], t[1], t[2] - drop))
break
else:
sys.exit(f"{DIVERTER} has no translate op to move")
stage.GetRootLayer().Save()
after = _range(stage, ARM)
print(f"blade now z[{after.GetMin()[2]:.4f}, {after.GetMax()[2]:.4f}] "
f"-> clearance {(after.GetMin()[2] - belt_top) * 1000:.0f} mm")
print(f"saved {SCENE}")
if __name__ == "__main__":
main()
scripts/move_lanes_inboard.py
+102
View File
@@ -0,0 +1,102 @@
#!/usr/bin/env python3
"""Bring both plow lanes inboard so the blade can actually reach them.
/home/whatevenif/isaacsim/python.sh scripts/move_lanes_inboard.py [--shift 0.07]
Measured over the 25-object run: **no item ever crossed y = 0.39**, while the lanes start
at |y| = 0.45. 16 of 25 came to rest against the blade still on the belt. 0.39 is not a
coincidence - it is where the blade tip is: a 600 mm arm at 42 deg reaches 0.6*sin42 =
0.402 m, and the item is pushed to the tip and no further, because that is where the blade
ends. Raising the angle cannot close it either: at the joint's 45 deg limit the tip reaches
0.424, still short.
So the lane comes to the blade. Each lane moves 70 mm toward the centreline, putting its
near edge at |y| = 0.38 - inside the tip's reach with ~20 mm to spare.
**Its tray moves with it.** Moving the lane alone would widen the gap between the lane end
and the tray wall from 100 mm to 170 mm, and goods would fall short onto the floor instead
of into the tray. Lane and tray are one assembly and are shifted by the same vector.
Known cost: the lane now overlaps the carrying belt by 70 mm (the belt is +-0.45 wide).
Two belt colliders share that strip at the same height, with different surface velocities.
That strip is exactly the hand-over region, so goods there being pulled by both is the
intended behaviour rather than a defect - but it is the thing to look at first if items
start jittering at the lane entry.
"""
from __future__ import annotations
import argparse
import sys
from pathlib import Path
from pxr import Gf, Usd, UsdGeom
SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
# prim -> how far to move it in +y (toward the centreline from its own side)
GROUPS = {
"B": dict(shift=+1.0, prims=["/ConveyorTrack_01", "/World/PlowCornerDeck_B"],
tray_prefix="B_"),
"C": dict(shift=-1.0, prims=["/World/ConveyorTrack_01", "/World/PlowCornerDeck_C",
"/World/PlowTransition_C"],
tray_prefix="C_"),
}
TRAYS = "/World/PlowContainers"
def _shift_y(stage, path, dy):
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
return False
xf = UsdGeom.Xformable(prim)
for op in xf.GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
t = op.Get()
op.Set(type(t)(t[0], t[1] + dy, t[2]))
return True
if op.GetOpType() == UsdGeom.XformOp.TypeTransform:
M = Gf.Matrix4d(op.Get())
tr = M.ExtractTranslation()
M.SetTranslateOnly(Gf.Vec3d(tr[0], tr[1] + dy, tr[2]))
op.Set(M)
return True
xf.AddTranslateOp().Set(Gf.Vec3d(0.0, dy, 0.0))
return True
def _edge(stage, path):
r = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
stage.GetPrimAtPath(path)).ComputeAlignedRange()
return None if r.IsEmpty() else (r.GetMin()[1], r.GetMax()[1])
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--shift", type=float, default=0.07, help="metres toward the centreline")
args = ap.parse_args()
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
stage = Usd.Stage.Open(str(SCENE))
for tag, g in GROUPS.items():
dy = g["shift"] * args.shift
before = _edge(stage, g["prims"][0])
moved = [p for p in g["prims"] if _shift_y(stage, p, dy)]
trays = [c.GetPath().pathString
for c in stage.GetPrimAtPath(TRAYS).GetChildren()
if c.GetName().startswith(g["tray_prefix"])]
moved += [p for p in trays if _shift_y(stage, p, dy)]
after = _edge(stage, g["prims"][0])
near_before = min(abs(v) for v in before) if before else float("nan")
near_after = min(abs(v) for v in after) if after else float("nan")
print(f"lane {tag}: dy {dy:+.3f} m, {len(moved)} prims "
f"({len(trays)} of them tray parts)")
print(f" near edge |y| {near_before:.3f} -> {near_after:.3f} "
f"(blade tip reaches 0.402)")
stage.GetRootLayer().Save()
print(f"saved {SCENE}")
if __name__ == "__main__":
main()
scripts/narrow_plow.py
+126
View File
@@ -0,0 +1,126 @@
#!/usr/bin/env python3
"""Bring the plow arm to a 600 mm sweep width.
python scripts/narrow_plow.py [--width 0.60]
The authored arm is 730 mm along its own long axis. That is wider than the 450 mm belt by
enough that it overhangs both rails: it clips goods it should have passed and shoulders
others off the lane instead of steering them. 600 mm still spans the belt with margin while
leaving the lane edges clear.
Only the scale on `DiverterEnd/Arm/Geom` changes. The hinge, its drive, the limits and the
arm's rigid body are untouched, so the kinematics are exactly as authored - the arm is
simply shorter.
Which local axis carries the length is not obvious: Geom is rotated -90 deg about Z and its
parent another 180 deg, so the mesh's own X and Y do not map to the world axes you would
guess. The script measures instead of assuming.
"""
from __future__ import annotations
import argparse
import shutil
import sys
from pathlib import Path
import numpy as np
from pxr import Gf, Usd, UsdGeom
ROOT = Path(__file__).resolve().parent.parent
CELL = ROOT / "scene" / "plow_cell.usd"
ARM_GEOM = "/World/Diverters/DiverterEnd/Arm/Geom"
ARM = "/World/Diverters/DiverterEnd/Arm"
def arm_length(stage):
"""longest principal extent of the arm's mesh points, in world metres"""
cache = UsdGeom.XformCache()
pts = []
for prim in Usd.PrimRange(stage.GetPrimAtPath(ARM)):
mesh = UsdGeom.Mesh(prim)
if not mesh:
continue
p = mesh.GetPointsAttr().Get()
if not p:
continue
M = cache.GetLocalToWorldTransform(prim)
pts.append(np.array([M.Transform(Gf.Vec3d(*q)) for q in p]))
if not pts:
return None
P = np.vstack(pts)
Q = P - P.mean(0)
_, _, vt = np.linalg.svd(Q, full_matrices=False)
return float(np.ptp(Q @ vt[0]))
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--width", type=float, default=0.60, help="target sweep width, metres")
args = ap.parse_args()
if not CELL.exists():
print(f"{CELL} not found")
return 1
stage = Usd.Stage.Open(str(CELL))
geom = stage.GetPrimAtPath(ARM_GEOM)
if not geom.IsValid():
print(f"{ARM_GEOM} missing - is this plow_cell.usd?")
return 1
before = arm_length(stage)
if not before:
print("arm carries no mesh points - is assets/plow/ populated?")
return 1
print(f"arm length now {before*1000:.0f} mm, target {args.width*1000:.0f} mm")
xf = UsdGeom.Xformable(geom)
scale_op = None
for op in xf.GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeScale:
scale_op = op
if scale_op is None:
scale_op = xf.AddScaleOp()
scale_op.Set(Gf.Vec3f(1, 1, 1))
base = Gf.Vec3f(scale_op.Get() or Gf.Vec3f(1, 1, 1))
# find which local axis the length rides on, by testing rather than reasoning about
# the two stacked rotations
factor = args.width / before
best = None
for axis in (0, 1, 2):
trial = Gf.Vec3f(base)
trial[axis] = base[axis] * factor
scale_op.Set(trial)
got = arm_length(stage)
print(f" scale on local {'XYZ'[axis]} -> {got*1000:.0f} mm")
if best is None or abs(got - args.width) < abs(best[1] - args.width):
best = (axis, got, trial)
axis, got, trial = best
scale_op.Set(trial)
if abs(got - args.width) > 0.005:
print(f"closest achievable was {got*1000:.0f} mm on local {'XYZ'[axis]} - "
"the arm's length may not lie on a single local axis")
return 1
backup = CELL.with_suffix(".usd.prewidth")
if not backup.exists():
shutil.copy(CELL, backup)
print(f"backup -> {backup.name}")
stage.GetRootLayer().Save()
check = Usd.Stage.Open(str(CELL))
final = arm_length(check)
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
r = cache.ComputeWorldBound(check.GetPrimAtPath(ARM)).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
print(f"\nsaved. arm is now {final*1000:.0f} mm "
f"(scale {tuple(round(v,4) for v in trial)} on local {'XYZ'[axis]})")
print(f" world AABB x[{mn[0]:.3f}..{mx[0]:.3f}] y[{mn[1]:.3f}..{mx[1]:.3f}] "
f"z[{mn[2]:.3f}..{mx[2]:.3f}]")
return 0
if __name__ == "__main__":
sys.exit(main())
scripts/place_plow_lanes.py
+317
View File
@@ -0,0 +1,317 @@
#!/usr/bin/env python3
"""Place the two plow take-away lanes clear of the blade, and put a container at each end.
python scripts/place_plow_lanes.py
The plow (x = -7.05, 600 mm arm, +-35 deg about Z) sweeps goods off the side of the main
run. A take-away lane therefore has to start OUTSIDE the main belt, not on it: the first
attempt put both near edges at y = +-0.05, which is inside the 450 mm belt, so the lanes
sat under the blade and fouled its swing.
Final layout, near edges at y = +-0.25 (just past the belt edge at +-0.225):
lane C +Y side, rotated 45 deg near end (-7.05, +0.45) runs toward (-X, +Y)
lane B -Y side, perpendicular near end (-7.25, -0.45) runs -Y
container at the far end of each
Both sit at belt height (top z = 1.781) and start flush with ConveyorTrack_04's belt edge
at y = +-0.45, so they join the run the same way the D branch joins it upstream.
C is angled because a plow deflection carries goods sideways *and* downstream - they leave
the belt on a diagonal, and a 45 deg lane meets that trajectory instead of fighting it.
B stays square because the -Y throw is the shorter one.
Only these two tracks move. The plow, its hinge, its drive and the main run are untouched.
"""
from __future__ import annotations
import math
import shutil
import sys
from pathlib import Path
from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade
ROOT = Path(__file__).resolve().parent.parent
CELL = ROOT / "scene" / "plow_cell.usd"
LANE_B = "/ConveyorTrack_01" # -Y side, perpendicular
LANE_C = "/World/ConveyorTrack_01" # +Y side, 45 deg
BELT_Z = 1.781
LANE_SCALE = Gf.Vec3d(1.0, 0.5, 1.0) # same as every other track
# Height matters and was wrong the first time. The D branch (ConveyorTrack_03/Belt_01)
# sits at z 1.74..1.78, flush with the main belt, which is what makes it read as part of
# the conveyor. These tracks carried an authored -0.1 drop, putting them 100 mm low so they
# looked like separate furniture parked nearby. 0.0 puts their belt tops at 1.781 too.
LANE_DROP = 0.0
# Near ends must clear the ARM's swept envelope, not just the belt edge. The 600 mm arm
# pivots at (-7.05, 0) and reaches +-35 deg, so its tip traces out to
# 0.60*sin(35) = 0.344 m either side. Starting the lanes at +-0.45 leaves ~100 mm.
ARM_SWEEP_Y = 0.60 * math.sin(math.radians(35.0)) # 0.344 m
# A plow sweeps goods sideways while they are still ON the belt, so a take-away lane has to
# run ALONGSIDE it, its near edge touching the belt's side rail - not past the belt's end.
# The first placement put lane B at x -7.48..-7.03, entirely downstream of where the main
# belt stops (x = -7.00): goods would have had to leave the belt and cross a gap to reach
# it, which is why nothing ever arrived. Both lanes now sit inside the arm's working span
# (x -7.12..-6.52) with their near edges on the belt edge at y = +-0.45.
B_TRANSLATE = Gf.Vec3d(-6.80, -0.45, LANE_DROP)
B_YAW = -90.0 # travel -Y
# A 45 deg lane meeting a straight belt edge does not touch at its centreline: the near
# corner runs ahead of it. Measured overlap at y=0.45 was 159 mm into the belt, so the lane
# is offset by that much and its nearest corner then lands on the edge instead of inside it.
# x=-6.55 puts the lane's near corner at the arm's tip (-6.52) and inside the belt span
# (-7.00..-6.00), i.e. on the junction itself. At -6.90 the corner sat behind the plow, so
# a +Y deflection had nowhere to land and goods rode on past.
C_TRANSLATE = Gf.Vec3d(-6.55, 0.45 + 0.159, LANE_DROP)
C_YAW = 135.0 # travel (-X, +Y): the diagonal a plow throw makes
def yaw_quat(deg):
r = math.radians(deg) / 2.0
return Gf.Quatd(math.cos(r), Gf.Vec3d(0.0, 0.0, math.sin(r)))
def set_xform(layer, path, translate, yaw, scale=LANE_SCALE):
spec = layer.GetPrimAtPath(path)
if not spec:
return False
for name, value, vtype in (
("xformOp:translate", translate, Sdf.ValueTypeNames.Double3),
("xformOp:orient", yaw_quat(yaw), Sdf.ValueTypeNames.Quatd),
("xformOp:scale", scale, Sdf.ValueTypeNames.Double3)):
attr = spec.attributes.get(name) or Sdf.AttributeSpec(spec, name, vtype)
attr.default = value
order = spec.attributes.get("xformOpOrder") or Sdf.AttributeSpec(
spec, "xformOpOrder", Sdf.ValueTypeNames.TokenArray)
order.default = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
return True
# ---------------------------------------------------------------- containers
CONTAINERS = "/World/PlowContainers"
BIN_HALF = Gf.Vec3f(0.45, 0.40, 0.26) # inner half-extents
BIN_FLOOR_Z = 1.16
BIN_WALL_TOP = 1.70 # under the lane surface, so goods tip in
TH = 0.024
def _material(stage, path, rgb):
mat = UsdShade.Material.Define(stage, path)
sh = UsdShade.Shader.Define(stage, path + "/Shader")
sh.CreateIdAttr("UsdPreviewSurface")
sh.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*rgb))
sh.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.55)
mat.CreateSurfaceOutput().ConnectToSource(sh.ConnectableAPI(), "surface")
return mat
def _box(stage, path, centre, half, mat, collider=True):
cube = UsdGeom.Cube.Define(stage, path)
cube.GetSizeAttr().Set(2.0) # scale == half-extent
xf = UsdGeom.Xformable(cube.GetPrim())
xf.ClearXformOpOrder()
xf.AddTranslateOp().Set(Gf.Vec3d(*centre))
xf.AddScaleOp().Set(Gf.Vec3f(*half))
UsdShade.MaterialBindingAPI.Apply(cube.GetPrim())
UsdShade.MaterialBindingAPI(cube.GetPrim()).Bind(mat)
if collider:
UsdPhysics.CollisionAPI.Apply(cube.GetPrim())
return cube.GetPrim()
def add_container(stage, tag, centre, mat):
"""open-top box; the far wall rises above belt height so a moving item cannot skim over"""
cx, cy = centre
hx, hy, _ = BIN_HALF
wh = (BIN_WALL_TOP - BIN_FLOOR_Z) / 2
wz = BIN_FLOOR_Z + wh
back_top = BELT_Z + 0.22
back_h = (back_top - BIN_FLOOR_Z) / 2
base = f"{CONTAINERS}/{tag}"
_box(stage, f"{base}_Floor", (cx, cy, BIN_FLOOR_Z), (hx, hy, TH), mat)
for name, c, h in (
("W0", (cx, cy + hy, BIN_FLOOR_Z + back_h), (hx, TH, back_h)),
("W1", (cx, cy - hy, wz), (hx, TH, wh)),
("W2", (cx - hx, cy, wz), (TH, hy, wh)),
("W3", (cx + hx, cy, wz), (TH, hy, wh))):
_box(stage, f"{base}_{name}", c, h, mat)
for i, (lx, ly) in enumerate([(cx - hx + 0.06, cy - hy + 0.06), (cx + hx - 0.06, cy - hy + 0.06),
(cx - hx + 0.06, cy + hy - 0.06), (cx + hx - 0.06, cy + hy - 0.06)]):
_box(stage, f"{base}_Leg{i}", (lx, ly, BIN_FLOOR_Z / 2),
(0.024, 0.024, BIN_FLOOR_Z / 2), mat, collider=False)
# ---------------------------------------------------------------- transition plate
def add_transition(stage, lane_path, edge_y, sign, mat, edge_x0, edge_x1):
"""Deck the WHOLE discharge corner, not just the touching triangle.
An angled lane leaves gaps on BOTH sides of its end face: one between its near corner
and the belt edge, another beyond its far corner. Goods do not cross at a single point -
the plow can put them anywhere across the discharge width - so the plate has to span the
entire region between the belt edge and the lane's end face, from one side of the zone
to the other. A plate covering only the first triangle still drops anything pushed wide.
Built as the convex hull of the belt-edge segment and both end-face corners, extruded
down 20 mm, coplanar with both belt surfaces.
"""
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
xf = UsdGeom.XformCache()
belt = stage.GetPrimAtPath(f"{lane_path}/Belt")
M = xf.GetLocalToWorldTransform(belt)
r = cache.ComputeWorldBound(belt).ComputeAlignedRange()
top = r.GetMax()[2]
travel = M.TransformDir(Gf.Vec3d(1, 0, 0)).GetNormalized()
across = Gf.Vec3d(-travel[1], travel[0], 0.0)
half_w = 0.225
centre = Gf.Vec3d((r.GetMin()[0] + r.GetMax()[0]) / 2,
(r.GetMin()[1] + r.GetMax()[1]) / 2, 0.0)
near = centre - travel * 1.0
c0 = near + across * half_w
c1 = near - across * half_w
# everything the plate must reach: the belt edge across the discharge zone, and both
# corners of the lane's end face
xs = [edge_x0, edge_x1, float(c0[0]), float(c1[0])]
ys = [edge_y, edge_y, float(c0[1]), float(c1[1])]
far_y = max(ys) if sign > 0 else min(ys)
quad = [(min(xs), edge_y), (max(xs), edge_y), (max(xs), far_y), (min(xs), far_y)]
path = f"/World/PlowTransition_{'C' if sign > 0 else 'B'}"
if stage.GetPrimAtPath(path).IsValid():
stage.RemovePrim(path)
if abs(far_y - edge_y) < 0.002:
# a square lane meets the edge flush along its whole face - no gap to deck, and a
# zero-area mesh would be a degenerate collider
return None, []
mesh = UsdGeom.Mesh.Define(stage, path)
pts = [Gf.Vec3f(x, y, top) for x, y in quad] + [Gf.Vec3f(x, y, top - 0.02) for x, y in quad]
mesh.GetPointsAttr().Set(pts)
faces = [(0, 3, 2, 1), (4, 5, 6, 7), (0, 1, 5, 4), (1, 2, 6, 5), (2, 3, 7, 6), (3, 0, 4, 7)]
mesh.GetFaceVertexCountsAttr().Set([4] * len(faces))
mesh.GetFaceVertexIndicesAttr().Set([i for f in faces for i in f])
mesh.GetSubdivisionSchemeAttr().Set("none")
mesh.GetExtentAttr().Set([Gf.Vec3f(min(xs), min(edge_y, far_y), top - 0.02),
Gf.Vec3f(max(xs), max(edge_y, far_y), top)])
UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim())
UsdShade.MaterialBindingAPI(mesh.GetPrim()).Bind(mat)
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
return path, [(round(x, 3), round(y, 3)) for x, y in quad]
def add_corner_deck(stage, lane_path, edge_y, sign, mat, edge_far_x):
"""Close the right angle a SQUARE lane leaves against a wider belt.
Lane B meets the belt flush along its own face, but the belt is wider than the lane:
the run reaches x=-6.00 while the lane stops at x=-6.58. Anything the plow pushes
sideways in that leftover span has open air under it. A triangular fillet spanning the
belt edge out to the run's end and back down the lane's side turns that right angle
into a chute, so goods slide into the lane instead of dropping through.
"""
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
belt = stage.GetPrimAtPath(f"{lane_path}/Belt")
r = cache.ComputeWorldBound(belt).ComputeAlignedRange()
top = r.GetMax()[2]
lane_far_x = r.GetMax()[0] # the lane's edge nearest the open span
span = abs(edge_far_x - lane_far_x)
if span < 0.02:
return None, []
corner = (lane_far_x, edge_y)
along_belt = (edge_far_x, edge_y)
down_lane = (lane_far_x, edge_y + sign * span)
path = f"/World/PlowCornerDeck_{'C' if sign > 0 else 'B'}"
if stage.GetPrimAtPath(path).IsValid():
stage.RemovePrim(path)
tri = [corner, along_belt, down_lane]
mesh = UsdGeom.Mesh.Define(stage, path)
pts = [Gf.Vec3f(x, y, top) for x, y in tri] + [Gf.Vec3f(x, y, top - 0.02) for x, y in tri]
mesh.GetPointsAttr().Set(pts)
faces = [(0, 2, 1), (3, 4, 5), (0, 1, 4, 3), (1, 2, 5, 4), (2, 0, 3, 5)]
mesh.GetFaceVertexCountsAttr().Set([len(f) for f in faces])
mesh.GetFaceVertexIndicesAttr().Set([i for f in faces for i in f])
mesh.GetSubdivisionSchemeAttr().Set("none")
xs = [p[0] for p in tri]; ys = [p[1] for p in tri]
mesh.GetExtentAttr().Set([Gf.Vec3f(min(xs), min(ys), top - 0.02),
Gf.Vec3f(max(xs), max(ys), top)])
UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim())
UsdShade.MaterialBindingAPI(mesh.GetPrim()).Bind(mat)
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
return path, [(round(x, 3), round(y, 3)) for x, y in tri]
def main():
if not CELL.exists():
print(f"{CELL} not found")
return 1
backup = CELL.with_suffix(".usd.prelanes")
if not backup.exists():
shutil.copy(CELL, backup)
print(f"backup -> {backup.name}")
layer = Sdf.Layer.FindOrOpen(str(CELL))
set_xform(layer, LANE_B, B_TRANSLATE, B_YAW)
set_xform(layer, LANE_C, C_TRANSLATE, C_YAW)
layer.Save()
print(f"arm sweeps to y=+-{ARM_SWEEP_Y:.3f}; lanes start at +-0.45")
print(f"lane B (-Y, square) near end {tuple(B_TRANSLATE)[:2]}")
print(f"lane C (+Y, 45 deg) near end {tuple(C_TRANSLATE)[:2]}")
stage = Usd.Stage.Open(str(CELL))
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
xf = UsdGeom.XformCache()
if stage.GetPrimAtPath(CONTAINERS).IsValid():
stage.RemovePrim(CONTAINERS)
UsdGeom.Xform.Define(stage, CONTAINERS)
m_b = _material(stage, f"{CONTAINERS}/M_B", (0.85, 0.22, 0.20))
m_c = _material(stage, f"{CONTAINERS}/M_C", (0.25, 0.72, 0.32))
ends = {}
for tag, path in (("B", LANE_B), ("C", LANE_C)):
belt = stage.GetPrimAtPath(f"{path}/Belt")
r = cache.ComputeWorldBound(belt).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
d = xf.GetLocalToWorldTransform(belt).TransformDir(Gf.Vec3d(1, 0, 0)).GetNormalized()
centre = Gf.Vec3d((mn[0] + mx[0]) / 2, (mn[1] + mx[1]) / 2, 0)
# container just past the discharge end, along the lane's own travel direction
far = centre + d * ((max(mx[0] - mn[0], mx[1] - mn[1]) / 2) + BIN_HALF[1] + 0.10)
ends[tag] = (float(far[0]), float(far[1]))
print(f" lane {tag}: x[{mn[0]:6.2f}..{mx[0]:6.2f}] y[{mn[1]:6.2f}..{mx[1]:6.2f}] "
f"top={mx[2]:.3f} travel({d[0]:+.2f},{d[1]:+.2f})")
add_container(stage, "B", ends["B"], m_b)
add_container(stage, "C", ends["C"], m_c)
m_t = _material(stage, f"{CONTAINERS}/M_transition", (0.30, 0.31, 0.34))
for old in ("/World/PlowTransition_C", "/World/PlowTransition_B"):
if stage.GetPrimAtPath(old).IsValid():
stage.RemovePrim(old)
# deck both discharge corners across the full width of the junction (belt x -7.00..-6.00)
for lane, edge, sign in ((LANE_C, 0.45, +1), (LANE_B, -0.45, -1)):
path, corners = add_transition(stage, lane, edge, sign, m_t, -7.00, -6.00)
print(f" transition {path or 'not needed (lane meets flush)'}: {corners}")
# a flush lane still leaves the right angle where the belt runs on past it
cpath, ctri = add_corner_deck(stage, lane, edge, sign, m_t, -6.00)
print(f" corner deck {cpath or 'not needed'}: {ctri}")
stage.GetRootLayer().Save()
print("\ncontainers:")
for tag in ("B", "C"):
r = cache.ComputeWorldBound(
stage.GetPrimAtPath(f"{CONTAINERS}/{tag}_Floor")).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
print(f" {tag}: x[{mn[0]:6.2f}..{mx[0]:6.2f}] y[{mn[1]:6.2f}..{mx[1]:6.2f}] "
f"floor z={mx[2]:.2f}")
arm = cache.ComputeWorldBound(
stage.GetPrimAtPath("/World/Diverters/DiverterEnd/Arm")).ComputeAlignedRange()
print(f"\nplow arm x[{arm.GetMin()[0]:.2f}..{arm.GetMax()[0]:.2f}] "
f"y[{arm.GetMin()[1]:.2f}..{arm.GetMax()[1]:.2f}]; swept envelope +-{ARM_SWEEP_Y:.2f}")
return 0
if __name__ == "__main__":
sys.exit(main())
scripts/plow_barrier_scan.py
+43
View File
@@ -0,0 +1,43 @@
"""Find what's colliding right at the plow pile-up point, and check Track_06's drive."""
import omni.usd
from pxr import Usd, UsdGeom, UsdPhysics, PhysxSchema
stage = omni.usd.get_context().get_stage()
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
print("=== everything with a collider near the plow (x -8.3..-7.0, y -0.6..0.9, z 1.7..2.6) ===")
for p in stage.Traverse():
has_col = (p.HasAPI(UsdPhysics.CollisionAPI) or p.HasAPI(UsdPhysics.MeshCollisionAPI))
if not has_col:
continue
attr = p.GetAttribute("physics:collisionEnabled")
enabled = attr.Get() if attr and attr.HasAuthoredValue() else True
if not enabled:
continue
r = bbc.ComputeWorldBound(p).ComputeAlignedRange()
if r.IsEmpty():
continue
mn, mx = r.GetMin(), r.GetMax()
if mx[0] < -8.3 or mn[0] > -7.0 or mx[1] < -0.6 or mn[1] > 0.9 or mx[2] < 1.7:
continue
approx = None
if p.HasAPI(UsdPhysics.MeshCollisionAPI):
a = p.GetAttribute("physics:approximation")
approx = a.Get() if a else None
print(f" {p.GetPath()} type={p.GetTypeName()} approx={approx}")
print(f" x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]")
print("\n=== ConveyorTrack_06/Belt drive + friction ===")
belt = stage.GetPrimAtPath("/World/ConveyorTrack_06/Belt")
print(" applied schemas:", list(belt.GetAppliedSchemas()))
sv = belt.GetAttribute("physxSurfaceVelocity:surfaceVelocity")
print(" surfaceVelocity:", sv.Get() if sv else None)
en = belt.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled")
print(" surfaceVelocityEnabled:", en.Get() if en else None)
from pxr import UsdShade
api = UsdShade.MaterialBindingAPI(belt)
mat, rel = api.ComputeBoundMaterial(materialPurpose="physics")
print(" physics material:", mat.GetPath() if mat else None)
if mat:
m = UsdPhysics.MaterialAPI(mat.GetPrim())
print(" static/dynamic friction:", m.GetStaticFrictionAttr().Get(), m.GetDynamicFrictionAttr().Get())
scripts/prep_inplace.py
+20
View File
@@ -0,0 +1,20 @@
"""Skip reopening the stage - it's already loaded (367 prims). Just prepare() it in place
to test whether the threading violation is specific to the reopen, not to editing per se."""
import sys
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 omni.usd
import isaacsim.core.experimental.utils.app as app_utils
from robozon_sorter.sim import plow_cell_9045
stage = omni.usd.get_context().get_stage()
print("current stage:", stage.GetRootLayer().identifier, "prims:", len(list(stage.Traverse())))
info = await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True)
print("prepare OK:", info)
scripts/probe_push_physics.py
+110
View File
@@ -0,0 +1,110 @@
"""Does the kinematic blade actually PUSH, or does it pass through and let PhysX untangle?
The question the whole plow rests on and which nothing so far has answered. A kinematic body
moved with `setKinematicTarget` sweeps: PhysX derives a velocity from the pose delta and
transfers momentum to whatever it meets. A body moved by writing its pose is a **teleport**:
it reappears somewhere else, and anything it now overlaps is pushed apart by depenetration
only - a shove with no momentum behind it, roughly proportional to how deep the overlap is
rather than to how fast the blade was going.
The two look identical in a viewport and identical in a contact sensor. They differ in one
measurable: the item's velocity while the blade is on it.
push item picks up lateral speed close to the blade's tangential speed
teleport item barely moves, gets a small separation nudge, and stops
This puts one item against a stationary blade, sweeps the blade through it, and records the
item's velocity every step. It also confirms the arm's simulated pose actually changes - if
PhysX never sees the rotation, the blade is a ghost and no amount of rate tuning matters.
"""
import sys
import time
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import importlib
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
del sys.modules[_m]
importlib.invalidate_caches()
import omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import UsdPhysics
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell, plow_vision, staging
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.plow import Plow
ITEM = globals().get("item", "box_300x200x200")
RATE = float(globals().get("rate", 300.0))
BELT = bool(globals().get("belt", True)) # is the belt driving the item at the time
stage, info = plow_vision.load(belt_speed=0.8 if BELT else 0.0, script_control=True,
meshes_dir=f"{REPO}/assets/items")
staging.stage_cell(stage, preset="bright", floor=True)
items = {k: v["zone"] for k, v in info["items"].items()}
await app_utils.update_app_async(steps=30)
cell = Cell(stage, items.keys())
cell.park_all()
await app_utils.update_app_async(steps=10)
arm = stage.GetPrimAtPath(C.PLOW_ARM)
print("--- what the arm IS ---")
print(" kinematic :", UsdPhysics.RigidBodyAPI(arm).GetKinematicEnabledAttr().Get())
print(" hinge on :", stage.GetPrimAtPath(C.PLOW_HINGE).GetAttribute(
"physics:jointEnabled").Get())
plow = Plow(stage, kinematic=True)
plow.target(0.0)
# put the item just in front of the blade, offset to the side the blade sweeps toward
cell.place(ITEM, (-6.75, 0.10, C.BELT_Z + 0.06))
tl = omni.timeline.get_timeline_interface()
tl.play()
await app_utils.update_app_async(steps=40)
def vel():
v = cell._rp[ITEM].get_velocities()[0].numpy()[0]
return float(v[0]), float(v[1]), float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5)
p0 = cell.pose(ITEM)
print(f"\n--- before: item at x={float(p0[0]):+.3f} y={float(p0[1]):+.3f}, "
f"arm measured {plow.angle:+.1f} deg ---")
print(f"\n--- sweeping to +42 deg at {RATE} deg/s ---")
print(f"{'step':>4} {'cmd':>7} {'arm':>7} {'item y':>8} {'vy':>7} {'|v|':>6}")
dt = 1.0 / 120.0
rows = []
for i in range(220):
done = plow.step_toward(42.0, dt, rate=RATE)
await app_utils.update_app_async(steps=1)
p = cell.pose(ITEM)
vx, vy, sp = vel()
rows.append((plow.commanded, plow.angle, float(p[1]), vy, sp))
if i % 12 == 0 or (done and i % 4 == 0):
print(f"{i:4d} {plow.commanded:7.1f} {plow.angle:7.1f} {float(p[1]):8.3f} "
f"{vy:7.2f} {sp:6.2f}")
if done and i > 60:
break
arm_moved = max(abs(r[1]) for r in rows)
peak_vy = max(abs(r[3]) for r in rows)
y_gain = max(r[2] for r in rows) - float(p0[1])
tip_speed = C.PLOW_ARM_LEN * RATE * 3.14159 / 180.0
print(f"\n--- verdict ---")
print(f" arm reached {arm_moved:.1f} deg (commanded 42)")
print(f" blade tip speed {tip_speed:.2f} m/s")
print(f" item peak |vy| {peak_vy:.2f} m/s")
print(f" item lateral travel {y_gain:+.3f} m")
if arm_moved < 5:
print(" => PhysX never saw the rotation: the blade is a ghost")
elif peak_vy < 0.15 * tip_speed:
print(" => TELEPORT, not a sweep: the arm arrives without momentum and the item only")
print(" gets a depenetration nudge. Rate tuning cannot fix this.")
else:
print(" => real push: the item takes up a fair share of the blade's tip speed")
tl.stop()
scripts/push_method.py
+133
View File
@@ -0,0 +1,133 @@
"""Blade drive METHOD comparison - the speed sweep proved speed is not the variable.
A: write xformOp:translate (what the pipeline does now) = a TELEPORT. PhysX sees no
velocity; the item gets only a depenetration shove, so a FASTER blade pushes LESS -
exactly what the sweep measured (1.3->99mm, 2.6->17mm). This is probe_push_physics.py's
documented teleport signature.
B: RigidPrim.set_world_poses() -> sets the KINEMATIC TARGET on the physics backend, so
PhysX derives velocity = delta/dt and transfers real momentum.
C: dynamic blade + set_velocities() -> a genuine moving mass carrying momentum.
"""
import sys
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, omni.timeline
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene, plow_cell_9045
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True)
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
blade_rp = RigidPrim(paths=[_scene.BLADE])
def _bop():
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
return op
bop = _bop(); bbase = bop.Get()
def blade_xform_to(y):
bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2]))
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
r0 = bbc.ComputeWorldBound(blade_prim).ComputeAlignedRange()
BLADE_WORLD_X = (r0.GetMin()[0] + r0.GetMax()[0]) / 2.0
BLADE_WORLD_Z = (r0.GetMin()[2] + r0.GetMax()[2]) / 2.0
SENSE_X = r0.GetMax()[0]
print(f"blade centre x={BLADE_WORLD_X:+.3f} z={BLADE_WORLD_Z:+.3f}, sense {SENSE_X:+.3f}")
ipath = "/World/Items/_pushprobe2"
def spawn():
if stage.GetPrimAtPath(ipath).IsValid():
stage.RemovePrim(ipath)
prim = UsdGeom.Xform.Define(stage, ipath).GetPrim()
prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / "box_300x200x200.usd"))
xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(-3.05, 0.0, C.BELT_Z + 0.05))
UsdPhysics.RigidBodyAPI.Apply(prim)
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.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION)
UsdGeom.Imageable(prim).MakeVisible()
return RigidPrim(paths=[ipath])
SPEED = 1.3
A, B = C.BLADE_HOME_Y, 0.55
async def ride_to_blade(rp):
for _ in range(400):
if float(rp.get_world_poses()[0].numpy()[0][0]) <= SENSE_X:
return True
await app_utils.update_app_async(steps=1)
return False
print(f"\n{'method':>34} {'y_gain':>8} {'final_y':>8} {'final_z':>8} verdict")
print("-" * 76)
for method in ("A: usd xform write (current)", "B: RigidPrim.set_world_poses",
"C: dynamic + set_velocities"):
# reset blade to kinematic home
UsdPhysics.RigidBodyAPI(blade_prim).CreateKinematicEnabledAttr().Set(True)
blade_xform_to(A)
rp = spawn()
tl.play(); await app_utils.update_app_async(steps=8)
await ride_to_blade(rp)
p0 = rp.get_world_poses()[0].numpy()[0].copy()
dur = abs(B - A) / SPEED
t0 = float(tl.get_current_time())
if method.startswith("C"):
UsdPhysics.RigidBodyAPI(blade_prim).CreateKinematicEnabledAttr().Set(False)
UsdPhysics.MassAPI.Apply(blade_prim).CreateMassAttr().Set(200.0)
PhysxSchema.PhysxRigidBodyAPI.Apply(blade_prim).CreateDisableGravityAttr().Set(True)
await app_utils.update_app_async(steps=2)
while True:
t = float(tl.get_current_time()) - t0
u = min(1.0, t / dur)
y = A + (B - A) * u
try:
if method.startswith("A"):
blade_xform_to(y)
elif method.startswith("B"):
blade_rp.set_world_poses(positions=np.array([[BLADE_WORLD_X, y, BLADE_WORLD_Z]]))
else:
vy = 0.0 if u >= 1.0 else SPEED
blade_rp.set_velocities(np.array([[0.0, vy, 0.0, 0.0, 0.0, 0.0]]))
except BaseException as exc:
print(f" {method}: drive call failed: {type(exc).__name__}")
break
await app_utils.update_app_async(steps=1)
if u >= 1.0:
break
for _ in range(60):
if method.startswith("C"):
try:
blade_rp.set_velocities(np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]))
except BaseException:
pass
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
gain = float(p[1]) - float(p0[1])
verdict = ("FELL" if float(p[2]) < 1.2 else
"EJECTED" if abs(float(p[1])) > 3.0 else
"DELIVERED" if float(p[1]) > 0.45 else "short")
print(f"{method:>34} {gain:8.3f} {float(p[1]):8.3f} {float(p[2]):8.3f} {verdict}")
tl.stop(); await app_utils.update_app_async(steps=6)
UsdPhysics.RigidBodyAPI(blade_prim).CreateKinematicEnabledAttr().Set(True)
blade_xform_to(A)
scripts/push_sweep.py
+119
View File
@@ -0,0 +1,119 @@
"""Pusher speed sweep with the CURRENT grip material + 500 mm blade.
Earlier speed tuning (pusher_diag*.py -> PUSH_SPEED=1.3) was measured while the blade was
still bound to the SLIPPERY DiverterMaterial (0.12/0.08); the grip fix (1.1/0.95) makes
those numbers stale. Also tests firing IMMEDIATELY at detection vs waiting to PUSH_X+0.08:
geometry says the item has only 0.5 m of blade (0.5 s at 1 m/s) and the wait burns 0.17 s
of it, so the wait may be why the stroke never completes on the item.
Outcome per trial: y_gain (needs > ~0.45 to reach the branch belt) and whether the item
survived at belt height or was ejected / fell through.
"""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene, plow_cell_9045
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
info = await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True)
print("blade dims:", info["pusher_dims"], " seat:", info["pusher_seat"])
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
def _bop():
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
return op
bop = _bop(); bbase = bop.Get()
def blade_to(y):
bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2]))
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
r = bbc.ComputeWorldBound(stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom")).ComputeAlignedRange()
BLADE_X0, BLADE_X1 = r.GetMin()[0], r.GetMax()[0]
SENSE_X = BLADE_X1 # leading (downstream-facing) edge
print(f"blade x[{BLADE_X0:+.3f}..{BLADE_X1:+.3f}] sense at {SENSE_X:+.3f} "
f"contact window = {(BLADE_X1-BLADE_X0)/1.0:.3f} s at 1 m/s")
ITEM = "box_300x200x200"
ipath = "/World/Items/_pushprobe"
def spawn():
if stage.GetPrimAtPath(ipath).IsValid():
stage.RemovePrim(ipath)
prim = UsdGeom.Xform.Define(stage, ipath).GetPrim()
prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{ITEM}.usd"))
xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(-3.05, 0.0, C.BELT_Z + 0.05))
UsdPhysics.RigidBodyAPI.Apply(prim)
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.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION)
UsdGeom.Imageable(prim).MakeVisible()
return RigidPrim(paths=[ipath])
print(f"\n{'speed':>6} {'wait':>6} {'y_gain':>8} {'final_x':>8} {'final_y':>8} {'final_z':>8} verdict")
print("-" * 78)
results = []
for speed in (1.3, 1.7, 2.1, 2.6):
for wait_to_center in (False, True):
rp = spawn()
blade_to(C.BLADE_HOME_Y)
tl.play(); await app_utils.update_app_async(steps=8)
# ride until the leading edge sees it
for _ in range(400):
if float(rp.get_world_poses()[0].numpy()[0][0]) <= SENSE_X:
break
await app_utils.update_app_async(steps=1)
if wait_to_center:
for _ in range(60):
if float(rp.get_world_poses()[0].numpy()[0][0]) <= C.PUSH_X + 0.08:
break
await app_utils.update_app_async(steps=1)
p0 = rp.get_world_poses()[0].numpy()[0].copy()
a, b = C.BLADE_HOME_Y, 0.55
dur = abs(b - a) / speed
t0 = float(tl.get_current_time())
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / dur)
blade_to(a + (b - a) * u)
await app_utils.update_app_async(steps=1)
if u >= 1.0:
break
for _ in range(60): # let it settle / travel on
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
gain = float(p[1]) - float(p0[1])
if float(p[2]) < 1.2:
verdict = "FELL/LOST"
elif abs(float(p[1])) > 3.0:
verdict = "EJECTED"
elif float(p[1]) > 0.45:
verdict = "DELIVERED"
else:
verdict = "short - stayed on main belt"
print(f"{speed:6.1f} {str(wait_to_center):>6} {gain:8.3f} {float(p[0]):8.3f} "
f"{float(p[1]):8.3f} {float(p[2]):8.3f} {verdict}")
results.append((speed, wait_to_center, gain, verdict))
tl.stop(); await app_utils.update_app_async(steps=6)
blade_to(C.BLADE_HOME_Y)
good = [r for r in results if r[3] == "DELIVERED"]
print(f"\nDELIVERED configs: {[(s, w) for s, w, g, v in good]}")
scripts/pusher_diag.py
+101
View File
@@ -0,0 +1,101 @@
"""Isolated pusher diagnostic: place one item right at the blade, sweep it, log velocity
every step. Determines push (item picks up tangential speed) vs teleport (item barely
moves, gets a depenetration nudge, stops) - probe_push_physics.py's own distinction.
Reuses the CURRENTLY prepared stage (belts/plow/pusher/rails already configured by the
last full run) rather than reopening.
"""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop()
await app_utils.update_app_async(steps=10)
print("=== blade geometry now ===")
geom = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom")
bbc = UsdGeom.BBoxCache(0, ["default", "render"])
r = bbc.ComputeWorldBound(geom).ComputeAlignedRange()
print(f" blade bbox x[{r.GetMin()[0]:+.3f}..{r.GetMax()[0]:+.3f}] "
f"y[{r.GetMin()[1]:+.3f}..{r.GetMax()[1]:+.3f}] z[{r.GetMin()[2]:+.3f}..{r.GetMax()[2]:+.3f}]")
print(f" BLADE_HOME_Y={C.BLADE_HOME_Y} BLADE_OUT_Y={C.BLADE_OUT_Y} PUSH_X={C.PUSH_X} BELT_Z={C.BELT_Z}")
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
print(" blade kinematic:", UsdPhysics.RigidBodyAPI(blade_prim).GetKinematicEnabledAttr().Get())
print(" blade collision enabled (self):", blade_prim.GetAttribute("physics:collisionEnabled").Get())
for c in blade_prim.GetChildren():
print(" child", c.GetPath(), "collisionEnabled:", c.GetAttribute("physics:collisionEnabled").Get())
# reset blade to home
def blade_op():
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
return op
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)
# place a fresh item just upstream of the blade, in its path
name = "probe_item"
items_dir = C.ROOT / "assets" / "items"
prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim()
prim.GetReferences().ClearReferences()
prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd"))
xf = UsdGeom.Xformable(prim)
xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.05, 0.0, C.BELT_Z + 0.10))
UsdPhysics.RigidBodyAPI.Apply(prim)
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.CreateSleepThresholdAttr().Set(0.0)
UsdGeom.Imageable(prim).MakeVisible()
rp = RigidPrim(paths=[f"/World/Items/{name}"])
tl.play()
await app_utils.update_app_async(steps=40)
p0 = rp.get_world_poses()[0].numpy()[0]
print(f"\n--- settled at x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f} ---")
print("\n--- sweeping blade out at PUSHER_MAX_SAFE, logging item state ---")
speed = C.PUSHER_MAX_SAFE
a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y
duration = abs(b - a) / speed
t0 = float(tl.get_current_time())
i = 0
print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'item_z':>8} {'vx':>7} {'vy':>7}")
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / duration)
blade_to(a + (b - a) * u)
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
v = rp.get_velocities()[0].numpy()[0]
if i % 3 == 0 or u >= 1.0:
print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} "
f"{p[0]:8.3f} {p[1]:8.3f} {p[2]:8.3f} {v[0]:7.3f} {v[1]:7.3f}")
i += 1
if u >= 1.0:
break
p_final = rp.get_world_poses()[0].numpy()[0]
print(f"\n--- after stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (started y={p0[1]:+.3f}, moved {p_final[1]-p0[1]:+.3f}) ---")
tl.stop()
scripts/pusher_diag2.py
+82
View File
@@ -0,0 +1,82 @@
"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide
clean past the blade's x-window before the sweep even started - fire the sweep the
instant the item is placed, matching the real pipeline's timing exactly."""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop()
await app_utils.update_app_async(steps=10)
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
def blade_op():
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
return op
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)
name = "probe_item2"
items_dir = C.ROOT / "assets" / "items"
prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim()
prim.GetReferences().ClearReferences()
prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd"))
xf = UsdGeom.Xformable(prim)
xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10))
UsdPhysics.RigidBodyAPI.Apply(prim)
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.CreateSleepThresholdAttr().Set(0.0)
UsdGeom.Imageable(prim).MakeVisible()
rp = RigidPrim(paths=[f"/World/Items/{name}"])
tl.play()
await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget
p0 = rp.get_world_poses()[0].numpy()[0]
print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}")
print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}")
speed = C.PUSHER_MAX_SAFE
a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y
duration = abs(b - a) / speed
t0 = float(tl.get_current_time())
i = 0
print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}")
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / duration)
blade_to(a + (b - a) * u)
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
v = rp.get_velocities()[0].numpy()[0]
print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} "
f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}")
i += 1
if u >= 1.0:
break
p_final = rp.get_world_poses()[0].numpy()[0]
print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})")
tl.stop()
scripts/pusher_diag3.py
+82
View File
@@ -0,0 +1,82 @@
"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide
clean past the blade's x-window before the sweep even started - fire the sweep the
instant the item is placed, matching the real pipeline's timing exactly."""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop()
await app_utils.update_app_async(steps=10)
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
def blade_op():
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
return op
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)
name = "probe_item2"
items_dir = C.ROOT / "assets" / "items"
prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim()
prim.GetReferences().ClearReferences()
prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd"))
xf = UsdGeom.Xformable(prim)
xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10))
UsdPhysics.RigidBodyAPI.Apply(prim)
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.CreateSleepThresholdAttr().Set(0.0)
UsdGeom.Imageable(prim).MakeVisible()
rp = RigidPrim(paths=[f"/World/Items/{name}"])
tl.play()
await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget
p0 = rp.get_world_poses()[0].numpy()[0]
print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}")
print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}")
speed = 1.0
a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y
duration = abs(b - a) / speed
t0 = float(tl.get_current_time())
i = 0
print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}")
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / duration)
blade_to(a + (b - a) * u)
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
v = rp.get_velocities()[0].numpy()[0]
print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} "
f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}")
i += 1
if u >= 1.0:
break
p_final = rp.get_world_poses()[0].numpy()[0]
print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})")
tl.stop()
scripts/pusher_diag4.py
+82
View File
@@ -0,0 +1,82 @@
"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide
clean past the blade's x-window before the sweep even started - fire the sweep the
instant the item is placed, matching the real pipeline's timing exactly."""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop()
await app_utils.update_app_async(steps=10)
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
def blade_op():
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
return op
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)
name = "probe_item2"
items_dir = C.ROOT / "assets" / "items"
prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim()
prim.GetReferences().ClearReferences()
prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd"))
xf = UsdGeom.Xformable(prim)
xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10))
UsdPhysics.RigidBodyAPI.Apply(prim)
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.CreateSleepThresholdAttr().Set(0.0)
UsdGeom.Imageable(prim).MakeVisible()
rp = RigidPrim(paths=[f"/World/Items/{name}"])
tl.play()
await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget
p0 = rp.get_world_poses()[0].numpy()[0]
print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}")
print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}")
speed = 0.6
a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y
duration = abs(b - a) / speed
t0 = float(tl.get_current_time())
i = 0
print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}")
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / duration)
blade_to(a + (b - a) * u)
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
v = rp.get_velocities()[0].numpy()[0]
print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} "
f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}")
i += 1
if u >= 1.0:
break
p_final = rp.get_world_poses()[0].numpy()[0]
print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})")
tl.stop()
scripts/pusher_diag5.py
+82
View File
@@ -0,0 +1,82 @@
"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide
clean past the blade's x-window before the sweep even started - fire the sweep the
instant the item is placed, matching the real pipeline's timing exactly."""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import scene as _scene
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop()
await app_utils.update_app_async(steps=10)
blade_prim = stage.GetPrimAtPath(_scene.BLADE)
def blade_op():
for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
return op
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)
name = "probe_item2"
items_dir = C.ROOT / "assets" / "items"
prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim()
prim.GetReferences().ClearReferences()
prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd"))
xf = UsdGeom.Xformable(prim)
xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10))
UsdPhysics.RigidBodyAPI.Apply(prim)
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.CreateSleepThresholdAttr().Set(0.0)
UsdGeom.Imageable(prim).MakeVisible()
rp = RigidPrim(paths=[f"/World/Items/{name}"])
tl.play()
await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget
p0 = rp.get_world_poses()[0].numpy()[0]
print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}")
print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}")
speed = 1.3
a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y
duration = abs(b - a) / speed
t0 = float(tl.get_current_time())
i = 0
print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}")
while True:
u = min(1.0, (float(tl.get_current_time()) - t0) / duration)
blade_to(a + (b - a) * u)
await app_utils.update_app_async(steps=1)
p = rp.get_world_poses()[0].numpy()[0]
v = rp.get_velocities()[0].numpy()[0]
print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} "
f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}")
i += 1
if u >= 1.0:
break
p_final = rp.get_world_poses()[0].numpy()[0]
print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})")
tl.stop()
scripts/pusher_geom.py
+19
View File
@@ -0,0 +1,19 @@
"""Inspect the pusher blade's actual geometry and any existing D-push helpers."""
import omni.usd
from pxr import Usd, UsdGeom
stage = omni.usd.get_context().get_stage()
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
for path in ["/World/Diverters/DiverterY_Split", "/World/Diverters/DiverterY_Split/Pusher",
"/World/Diverters/DiverterY_Split/Pusher/Geom"]:
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
print(path, "MISSING"); continue
r = bbc.ComputeWorldBound(prim).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
print(f"{path}")
print(f" bbox x[{mn[0]:+.3f}..{mx[0]:+.3f}] ({(mx[0]-mn[0])*1000:.0f}mm) "
f"y[{mn[1]:+.3f}..{mx[1]:+.3f}] ({(mx[1]-mn[1])*1000:.0f}mm) "
f"z[{mn[2]:+.3f}..{mx[2]:+.3f}]")
print(f" children: {[c.GetName() for c in prim.GetChildren()]}")
scripts/pusher_mat.py
+12
View File
@@ -0,0 +1,12 @@
import omni.usd
from pxr import UsdShade, UsdPhysics
stage = omni.usd.get_context().get_stage()
prim = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom")
api = UsdShade.MaterialBindingAPI(prim)
mat, rel = api.ComputeBoundMaterial(materialPurpose="physics")
print("physics material bound:", mat.GetPath() if mat else None)
if mat:
m = UsdPhysics.MaterialAPI(mat.GetPrim())
print(" static friction:", m.GetStaticFrictionAttr().Get())
print(" dynamic friction:", m.GetDynamicFrictionAttr().Get())
print(" restitution:", m.GetRestitutionAttr().Get())
scripts/pusher_ops.py
+9
View File
@@ -0,0 +1,9 @@
import omni.usd
from pxr import UsdGeom
stage = omni.usd.get_context().get_stage()
for path in ["/World/Diverters/DiverterY_Split/Pusher", "/World/Diverters/DiverterY_Split/Pusher/Geom"]:
prim = stage.GetPrimAtPath(path)
print(path, prim.GetTypeName())
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
print(" ", op.GetOpName(), op.GetOpType(), op.Get())
print(" refs:", [str(r) for r in prim.GetPrimStack()][:2])
scripts/reverse_plow_mount.py
+92
View File
@@ -0,0 +1,92 @@
#!/usr/bin/env python3
"""Turn the plow round: pivot upstream, free end downstream at the discharge edge.
/home/whatevenif/isaacsim/python.sh scripts/reverse_plow_mount.py
Measured on the built scene, with goods travelling in **-X**:
pivot x = -7.050 <- DOWNSTREAM end
free tip x = -6.658 (at 42 deg) <- UPSTREAM end, y +0.353
That is a plough mounted backwards. A plough is an inclined plane: the belt drives the item
along the blade toward the blade's **downstream** end, and the item leaves there. With the
downstream end sitting at the pivot on the belt centreline (y = 0), goods are funnelled
*inward*, slip past the pivot and carry on down the line. They can never discharge.
It explains every symptom: the 0.39 m ceiling is the brief shove from the sweep, after
which the item slides back toward the centre; moving the lanes inboard changed nothing
because the lane edge was never what goods were failing to reach; and goods pile in the
wedge between the blade and the belt centre, which is what the viewport shows.
The fix is the mounting, not the length:
pivot x -7.050 -> -6.522 (upstream end of the same physical span)
arm extends -X instead of +X (rotateZ 180 -> 0)
so at 42 deg the free end lands near x -6.92, y +-0.353 - downstream of the pivot and out
at the discharge side. Goods now slide *outward and forward* along the blade.
**The swing sign flips with the mount.** `plow_sort.calibrate_mapping()` says to measure it
rather than reason about it; re-measure after running this.
"""
from __future__ import annotations
import math
import sys
from pathlib import Path
from pxr import Gf, Usd, UsdGeom
SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd"
DIVERTER = "/World/Diverters/DiverterEnd"
ARM = DIVERTER + "/Arm"
def main():
if not SCENE.exists():
sys.exit(f"{SCENE} not found")
stage = Usd.Stage.Open(str(SCENE))
prim = stage.GetPrimAtPath(DIVERTER)
if not prim.IsValid():
sys.exit(f"{DIVERTER} missing")
xc = UsdGeom.XformCache()
piv = xc.GetLocalToWorldTransform(stage.GetPrimAtPath(ARM)).ExtractTranslation()
r = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
stage.GetPrimAtPath(ARM)).ComputeAlignedRange()
reach = r.GetMax()[0] - piv[0] # +0.528: arm points upstream today
print(f"before: pivot x={piv[0]:.3f}, arm reaches {reach:+.3f} m in X "
f"({'UPSTREAM - wrong way' if reach > 0 else 'downstream'})")
xf = UsdGeom.Xformable(prim)
moved = flipped = False
for op in xf.GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate and not moved:
t = op.Get()
op.Set(Gf.Vec3d(t[0] + reach, t[1], t[2])) # pivot to the upstream end
moved = True
elif op.GetOpType() == UsdGeom.XformOp.TypeRotateZ and not flipped:
op.Set(float((op.Get() or 0.0) + 180.0) % 360.0) # arm now points downstream
flipped = True
if not (moved and flipped):
sys.exit(f"{DIVERTER} needs both a translate and a rotateZ op "
f"(moved={moved}, flipped={flipped})")
stage.GetRootLayer().Save()
xc2 = UsdGeom.XformCache()
piv2 = xc2.GetLocalToWorldTransform(stage.GetPrimAtPath(ARM)).ExtractTranslation()
r2 = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound(
stage.GetPrimAtPath(ARM)).ComputeAlignedRange()
L = abs(reach)
print(f"after : pivot x={piv2[0]:.3f}, arm spans x[{r2.GetMin()[0]:.3f}, "
f"{r2.GetMax()[0]:.3f}]")
for a in (0, 20, 42):
print(f" {a:2d} deg: free end x={piv2[0] - L * math.cos(math.radians(a)):+.3f} "
f"y={L * math.sin(math.radians(a)):+.3f} (downstream of pivot = correct)")
print(f"saved {SCENE}")
print("NOTE: the swing sign flips with the mount - re-measure calibrate_mapping()")
if __name__ == "__main__":
main()
scripts/run_demo.py
+302
View File
@@ -0,0 +1,302 @@
#!/usr/bin/env python3
"""One-command demonstration of the whole cell: staging, conveyor, vision, pusher, plow.
./python.sh scripts/run_demo.py # 50 dispatches, lit, windowed
./python.sh scripts/run_demo.py --headless --repeats 2
./python.sh scripts/run_demo.py --preset harsh --no-vision
./python.sh scripts/run_demo.py --items bucket,barrel,box_300x200x200
This is the entry point for someone who has not built the scenario before: it stages the
cell, dispatches the item library, and writes one JSON with everything needed to judge the
result. Nothing has to be assembled by hand first.
It reports two things that are easy to conflate and must be kept apart:
* **classification** - did the vision stack name the class correctly? Measured against
ground truth from the manifest, with a confusion matrix.
* **delivery** - did the item physically reach the tray its class routes to? A correct
class that ends up on the floor is a delivery failure, not a vision failure, and the
reverse happens too - a misread item can still land somewhere by luck.
Per dispatch it also records the plow's state *at the moment the item is level with it*:
the commanded angle, the angle the arm actually reached, and its angular rate. Those three
are different numbers because the plow is a compliant force drive, and the difference is
usually what explains a miss.
"""
from __future__ import annotations
import argparse
import json
import math
import sys
import time
from datetime import datetime
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
USER_SITE = "/home/dasha/.local/lib/python3.12/site-packages" # ultralytics lives here
if Path(USER_SITE).exists() and USER_SITE not in sys.path:
sys.path.append(USER_SITE)
def parse_args(argv=None):
p = argparse.ArgumentParser(description="Robozon sorting cell - full demonstration run")
p.add_argument("--headless", action="store_true")
p.add_argument("--preset", default="bright", choices=["bright", "dim", "harsh"],
help="lighting preset the run is staged under")
p.add_argument("--no-floor", action="store_true", help="skip the catch floor")
p.add_argument("--repeats", type=int, default=2,
help="passes over the library; 2 x 25 items = 50 dispatches")
p.add_argument("--items", default=None, help="comma-separated subset, for a quick check")
p.add_argument("--no-vision", action="store_true",
help="route on ground truth instead of running CRE-ROI v2b")
p.add_argument("--pitch", type=float, default=2.5, help="metres between dispatches")
p.add_argument("--speed", type=float, default=None, help="belt speed override, m/s")
p.add_argument("--settle", type=float, default=6.0,
help="seconds to let the last item come to rest before scoring")
p.add_argument("--out", default=None, help="where to write the run log")
return p.parse_args(argv)
# --------------------------------------------------------------------------- scoring
def confusion(records):
labels = ["B", "C", "D"]
m = {g: {p: 0 for p in labels + ["?"]} for g in labels}
for r in records:
if r["gt"] in m:
m[r["gt"]][r["pred"] if r["pred"] in m[r["gt"]] else "?"] += 1
return m
def summarise(records, expect):
graded = [r for r in records if r.get("pred") not in (None, "?")]
hits = sum(1 for r in graded if r["pred"] == r["gt"])
delivered = [r for r in records if r.get("delivered")]
by_class = {}
for cls in ("B", "C", "D"):
same = [r for r in records if r["gt"] == cls]
if same:
by_class[cls] = dict(
dispatched=len(same),
delivered=sum(1 for r in same if r.get("delivered")),
classified=sum(1 for r in same if r.get("pred") == cls),
target=expect.get(cls))
where = {}
for r in records:
where[r["outcome"]] = where.get(r["outcome"], 0) + 1
return dict(
dispatched=len(records),
classification=dict(graded=len(graded), correct=hits,
accuracy=round(hits / len(graded), 3) if graded else None,
confusion=confusion(records)),
delivery=dict(delivered=len(delivered),
rate=round(len(delivered) / len(records), 3) if records else None,
by_class=by_class, resting_places=where),
)
async def _run(app_utils, args):
from robozon_sorter import config as C
from robozon_sorter.sim import plow_sort as PS
from robozon_sorter.sim import plow_vision as PV
from robozon_sorter.sim import staging
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.spawner import AutoFeeder
if args.speed:
C.BELT_SPEED = args.speed
print("=" * 74)
print(f" Robozon sorting cell - {datetime.now():%Y-%m-%d %H:%M}")
print(f" lighting {args.preset}, belt {C.BELT_SPEED} m/s, pitch {args.pitch} m, "
f"vision {'off' if args.no_vision else 'on'}")
print("=" * 74)
# ---- 1. scene ----------------------------------------------------------
stage, info = PV.load(script_control=True)
await app_utils.update_app_async(steps=50)
staged = staging.stage_cell(stage, preset=args.preset, floor=not args.no_floor)
PS.keep_lanes_active(stage)
lanes = PS.configure_lanes(stage)
opened = PS.open_junction(stage)
print(f"[scene ] lighting={staged['lighting']['preset']} "
f"floor={'yes' if 'floor' in staged else 'no'} lanes={len(lanes)} "
f"junction shells opened={len(opened)}")
# ---- 2. item library ---------------------------------------------------
lib = ROOT / "assets" / "items"
if not (lib / "manifest.json").exists():
lib = C.MESHES
print(f"[items ] assets/items missing - falling back to {lib.name} "
"(run scripts/export_item_library.py for the full catalogue)")
items_meta = PV.load_items(stage, meshes_dir=lib)
classes = {k: v["zone"] for k, v in items_meta.items()}
await app_utils.update_app_async(steps=30)
from collections import Counter
print(f"[items ] {len(classes)} loaded from {lib.name}: {dict(Counter(classes.values()))}")
cell = Cell(stage, classes.keys())
cell.park_all()
await app_utils.update_app_async(steps=15)
# ---- 3. vision ---------------------------------------------------------
vision = None
if not args.no_vision:
from robozon_sorter.cv.pipeline import CreRoiV2b
vision = CreRoiV2b()
vision.attach_cameras()
print(f"[vision] CRE-ROI v2b ready, gate pixels {vision.gate_px}")
# ---- 4. dispatch order -------------------------------------------------
if args.items:
order = [n.strip() for n in args.items.split(",") if n.strip() in classes]
else:
order = [n for _ in range(args.repeats) for n in sorted(classes)]
mapping = PS.calibrate_mapping()
expect = {"D": "bin", "B": "container_B", "C": "container_C"}
sorter = PS.PlowSorter(stage, cell, classes, mapping)
print(f"[plan ] {len(order)} dispatches, plow mapping {mapping}, expect {expect}")
# ---- 5. run ------------------------------------------------------------
route, records, seen = {}, {}, set()
def rec(name):
return records.setdefault(name + f"#{len([k for k in records if k.startswith(name)])}"
if False else name, dict(item=name, gt=classes[name]))
log_events = []
def on_event(kind, name, payload):
log_events.append(dict(kind=kind, item=name, **payload))
if kind in ("release", "divert", "done"):
print(f" {kind:8s} {name:20s} {payload if payload else ''}")
feeder = AutoFeeder(cell, order=order, pitch=args.pitch, route=route,
on_event=on_event).install()
import omni.timeline
timeline = omni.timeline.get_timeline_interface()
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
dt_block, blocks = 15, 0
prev_angle, prev_t = sorter.plow.angle, time.time()
while blocks < 900 and len(feeder.finished) < len(order):
await app_utils.update_app_async(steps=dt_block)
blocks += 1
sorter.update(dt_block / 60.0)
for name in list(feeder.active):
r = records.setdefault(name, dict(item=name, gt=classes[name], pred=None,
dims=None, k=None, cre_ms=None, views=None,
commanded=None, reached=None, rate=None,
outcome=None, delivered=False))
x = float(cell.pose(name)[0])
# vision, once, while the item is under the portal
if name not in seen and abs(x - C.CAM_X) < 0.08:
if vision is not None:
playing = timeline.is_playing()
res = vision.measure()
if playing and not timeline.is_playing():
timeline.play() # Replicator stops the timeline
await app_utils.update_app_async(steps=2)
r.update(pred=res["cls"], dims=res["dims"], k=res["k"],
views=res["views"], cre_ms=res["cre_ms"])
else:
r["pred"] = classes[name]
route[name] = r["pred"]
seen.add(name)
mark = "ok " if r["pred"] == r["gt"] else "MISS"
print(f" vision {name:20s} pred={r['pred']} gt={r['gt']} {mark} "
f"dims={r['dims']} K={r['k']}")
# plow state at the moment the item is level with the blade
if r["commanded"] is None and abs(x - C.PLOW_POS[0]) < 0.25:
now = time.time()
ang = sorter.plow.angle
r["commanded"] = sorter.decided.get(name)
r["reached"] = round(ang, 2)
r["rate"] = round((ang - prev_angle) / max(now - prev_t, 1e-6), 1)
print(f" plow {name:20s} cmd={r['commanded']} reached={r['reached']} "
f"rate={r['rate']} deg/s")
prev_angle, prev_t = sorter.plow.angle, time.time()
# let the stragglers come to rest before scoring
for _ in range(int(args.settle * 60 / dt_block)):
await app_utils.update_app_async(steps=dt_block)
sorter.update(dt_block / 60.0)
for name in order:
r = records.setdefault(name, dict(item=name, gt=classes[name], pred=None,
outcome=None, delivered=False))
p = cell.pose(name)
r["outcome"] = sorter.lane_of(name)
r["final"] = [round(float(v), 3) for v in p]
r["expected"] = expect.get(r["gt"])
r["delivered"] = (r["outcome"] == r["expected"])
app_utils.stop()
await app_utils.update_app_async(steps=15)
feeder.remove()
cell.blade_to(C.BLADE_HOME_Y)
sorter.plow.home()
# ---- 6. report ---------------------------------------------------------
recs = list(records.values())
summary = summarise(recs, expect)
print("\n" + "=" * 74)
print(f" dispatched {summary['dispatched']}")
cls_s = summary["classification"]
if cls_s["graded"]:
print(f" classification {cls_s['correct']}/{cls_s['graded']} "
f"= {cls_s['accuracy']:.0%}")
print(f" {'gt\\pred':>8} " + " ".join(f"{p:>5}" for p in ["B", "C", "D", "?"]))
for g, row in cls_s["confusion"].items():
print(f" {g:>8} " + " ".join(f"{row[p]:>5}" for p in ["B", "C", "D", "?"]))
d = summary["delivery"]
print(f" delivery {d['delivered']}/{summary['dispatched']} = {d['rate']:.0%}")
for cls, row in d["by_class"].items():
print(f" class {cls} -> {row['target']:<12} "
f"delivered {row['delivered']}/{row['dispatched']}, "
f"classified {row['classified']}/{row['dispatched']}")
print(f" came to rest {d['resting_places']}")
print("=" * 74)
out = Path(args.out) if args.out else ROOT / "runs" / f"demo_{datetime.now():%Y%m%d_%H%M%S}.json"
out.parent.mkdir(parents=True, exist_ok=True)
out.write_text(json.dumps(dict(
config=dict(preset=args.preset, floor=not args.no_floor, speed=C.BELT_SPEED,
pitch=args.pitch, vision=not args.no_vision, repeats=args.repeats,
mapping=mapping, expect=expect, library=lib.name),
summary=summary, items=recs, events=log_events[-400:]), indent=2))
print(f" log -> {out}")
return summary
def main(argv=None):
args = parse_args(argv)
try:
import omni.usd
inside = omni.usd.get_context().get_stage() is not None
except Exception:
inside = False
app = None
if not inside:
from isaacsim import SimulationApp
app = SimulationApp({"headless": args.headless, "width": 1600, "height": 900})
import asyncio
import isaacsim.core.experimental.utils.app as app_utils
loop = asyncio.get_event_loop()
try:
return loop.run_until_complete(_run(app_utils, args))
finally:
if app is not None:
app.close()
if __name__ == "__main__":
sys.exit(0 if main() else 1)
scripts/run_flow.py
+260
View File
@@ -0,0 +1,260 @@
"""Поток товаров с шагом 700 мм: лазерная завеса определяет класс, пушер берёт класс D.
Луч теперь ДАТЧИК, а не преграда - коллизия снята прямо в файле. Раньше он перекрывал всю
ширину полотна на 19 мм над лентой, и товар вставал на x = -3.044, не доходя до пушера.
КАК ДАТЧИК ОПРЕДЕЛЯЕТ КРУГОВОЕ СЕЧЕНИЕ. Одиночный луч даёт только факт прохода. Завеса
из 181 луча поперёк ленты меряет ШИРИНУ товара, а по мере его проезда набирается профиль
ширины вдоль хода. У коробки он прямоугольный - ширина постоянна почти до конца; у тела
кругового сечения он дугообразный, ширина плавно нарастает и спадает.
Различаются они отношением средней ширины к наибольшей. Для прямоугольника оно стремится
к 1.0, для круга даёт площадь полукруга к описанному прямоугольнику, то есть pi/4 = 0.785.
Порог 0.90 разделяет их с запасом и не требует ни камеры, ни обучения - только геометрия.
Спавн идёт по времени, а не расстановкой заранее: при 1 м/с шаг 700 мм это ровно 0.70 с
между выпусками. Точка выпуска x = 1.90, а не C.SPAWN_X = 2.30 - в этой сборке подающая
секция ConveyorTrack_05 кончается на x = 2.001, и 2.30 висит в воздухе.
"""
import sys, math
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from omni.physx import get_physx_scene_query_interface
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.plow import Plow
SPEED = 1.0
PITCH = 0.70 # м между товарами
RELEASE_X = 1.90
SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd"
GATE_X = -3.20 # где стоит луч
RAYS, Y0, Y1 = 31, -0.45, 0.45
CURTAIN_H = 0.40
ROUND_T = 0.90 # средняя/наибольшая ширина: ниже - круглое сечение
# что пускаем: цилиндры - класс D (круговое сечение), коробки - не D
PLAN = [("D_cyl_1", "cyl"), ("box_1", "box"), ("D_cyl_2", "cyl"), ("box_2", "box"),
("D_cyl_3", "cyl"), ("box_3", "box"), ("D_cyl_4", "cyl"), ("box_4", "box")]
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
omni.usd.get_context().open_stage(SCENE)
await app_utils.update_app_async(steps=60)
stage = omni.usd.get_context().get_stage()
killed = [p.GetPath() for p in stage.Traverse()
if "ConveyorBeltGraph" in p.GetName() or "DiverterAnimGraph" in p.GetName()]
for path in killed:
stage.RemovePrim(path)
print(f"удалено узлов графов: {len(killed)} {[str(k).split(chr(47))[-1] for k in killed]}")
await app_utils.update_app_async(steps=10)
info = plow_cell.prepare(stage, belt_speed=SPEED, script_control=True, kinematic_arm=True)
for path, intent in (("/World/ConveyorTrack_05/Belt", (-1, 0, 0)),
("/World/ConveyorTrack_06/Belt", (0, 1, 0))):
pr = stage.GetPrimAtPath(path)
if pr.IsValid():
plow_cell.drive_belt(stage, path, intent, SPEED)
for path in list(plow_cell.BELTS) + [plow_cell.BRANCH,
"/World/ConveyorTrack_05/Belt", "/World/ConveyorTrack_06/Belt"]:
pr = stage.GetPrimAtPath(path)
if pr.IsValid():
PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True)
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_05/Belt")
).ComputeAlignedRange().GetMax()[2]
GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL)
print(f"сцена готова: лент {len(info['belts'])}+2, скорость {SPEED} м/с, "
f"верх ленты z={TOP:.3f}, луч на x={GATE_X}")
ROOT = "/World/_Flow"
if stage.GetPrimAtPath(ROOT).IsValid():
stage.RemovePrim(ROOT)
stage.DefinePrim(ROOT, "Xform")
def make(name, kind):
path = f"{ROOT}/{name}"
if kind == "cyl":
g = UsdGeom.Cylinder.Define(stage, path)
g.CreateRadiusAttr().Set(0.045); g.CreateHeightAttr().Set(0.10)
g.CreateAxisAttr().Set("Z")
half = 0.05
else:
g = UsdGeom.Cube.Define(stage, path); g.CreateSizeAttr().Set(2.0)
half = 0.045
xf = UsdGeom.Xformable(g.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(RELEASE_X, 0.0, TOP + half + 0.006))
if kind == "box":
xf.AddScaleOp().Set(Gf.Vec3f(0.045, 0.045, 0.045))
p = g.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(0.4)
rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p)
rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32)
if GRIP.IsValid():
UsdShade.MaterialBindingAPI.Apply(p).Bind(
UsdShade.Material(GRIP), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
materialPurpose="physics")
return path
query = get_physx_scene_query_interface()
# Положения тел ЧИТАЮТСЯ ЧЕРЕЗ RigidPrim, а не через BBoxCache. BBoxCache берёт авторские
# трансформы из слоя USD, а физика пишет состояние в Fabric - во время прогона эти два
# источника расходятся, и замер по BBoxCache показывает позы, которых на экране нет.
# Именно из-за этого предыдущий прогон отчитался, что все восемь товаров стоят ровно в
# точках выпуска, хотя таймлайн отработал полные 26 секунд.
_views = {}
def pos_of(path):
v = _views.get(path)
if v is None:
v = RigidPrim(paths=[path]); _views[path] = v
q = v.get_world_poses()[0].numpy()[0]
return float(q[0]), float(q[1]), float(q[2])
def curtain_width():
"""ширина того, что сейчас под завесой, в мм. Луч, попавший выше полотна, - товар."""
z0 = TOP + CURTAIN_H
hit_y = []
for i in range(RAYS):
y = Y0 + (Y1 - Y0) * i / (RAYS - 1)
h = query.raycast_closest((GATE_X, y, z0), (0.0, 0.0, -1.0), CURTAIN_H - 0.001)
if h and h.get("hit"):
zh = z0 - h["distance"]
if zh > TOP + 0.006: # выше полотна на 6 мм - значит товар
hit_y.append(y)
if not hit_y:
return 0.0
return (max(hit_y) - min(hit_y)) * 1000.0 + (Y1 - Y0) / (RAYS - 1) * 1000.0
# Таймлайн сцены кончается на своём endTimeCode и останавливается сам. В прошлом прогоне
# из-за этого прошло только 5.1 с вместо 26: головной товар едва дошёл до створа, и
# завеса не успела ничего измерить. Продлеваем ленту времени под длительность опыта.
fps = stage.GetTimeCodesPerSecond() or 24.0
print(f"таймлайн: {stage.GetStartTimeCode()}..{stage.GetEndTimeCode()} кадров при {fps} к/с "
f"= {(stage.GetEndTimeCode()-stage.GetStartTimeCode())/fps:.1f} с - продлеваем")
stage.SetEndTimeCode(stage.GetStartTimeCode() + fps * 90.0)
tl.set_end_time(float(stage.GetEndTimeCode()) / fps)
tl.set_looping(False)
cell = Cell(stage, items={})
plow = Plow(stage); plow.target(C.PLOW_PRESET["D"])
tl.play()
await app_utils.update_app_async(steps=20)
t0 = float(tl.get_current_time())
released, profiles, done, order = [], {}, {}, []
next_release = 0.0
pusher_busy_until = -1.0
print(f"\nвыпуск каждые {PITCH/SPEED:.2f} с (шаг {PITCH*1000:.0f} мм при {SPEED} м/с)")
print("\n событие")
print(" " + "-" * 76)
T_END = 26.0
import time as _wall
_it, _w0 = 0, _wall.time()
while float(tl.get_current_time()) - t0 < T_END:
t = float(tl.get_current_time()) - t0
_it += 1
if _it % 40 == 0:
print(f" [цикл] итерация {_it}: сим t={t:5.2f}s, стена {_wall.time()-_w0:5.1f}s, "
f"играет={tl.is_playing()}, товаров={len(released)}")
if not tl.is_playing():
# play() после stop() перематывает в начало и сбрасывает физику: в прошлом прогоне
# это возвращало все товары в точки выпуска и обесценивало весь замер.
print(f" [цикл] ТАЙМЛАЙН ОСТАНОВИЛСЯ САМ на t={t:.2f}s - прерываю, "
f"перезапуск обнулил бы опыт")
break
# выпуск потока
if len(released) < len(PLAN) and t >= next_release:
name, kind = PLAN[len(released)]
path = make(name, kind)
released.append((name, kind, path))
order.append(name)
next_release += PITCH / SPEED
print(f" {t:5.2f}s выпущен {name} ({'цилиндр' if kind=='cyl' else 'коробка'})")
# завеса: набрать профиль ширины для того, кто сейчас в створе
w = curtain_width()
if w > 5.0:
# чей это профиль - ближайший по x к воротам
best, bd = None, 1e9
for name, kind, path in released:
if not stage.GetPrimAtPath(path).IsValid():
continue
d = abs(pos_of(path)[0] - GATE_X)
if d < bd:
best, bd = name, d
if best is not None and bd < 0.20:
profiles.setdefault(best, []).append(w)
# решение по классу, когда товар вышел из створа
for name, kind, path in released:
if name in done or name not in profiles:
continue
if not stage.GetPrimAtPath(path).IsValid():
continue
x = pos_of(path)[0]
if x < GATE_X - 0.09 and len(profiles[name]) >= 3:
prof = profiles[name]
ratio = (sum(prof) / len(prof)) / max(prof)
cls = "D" if ratio < ROUND_T else "B/C"
done[name] = dict(gt=("D" if kind == "cyl" else "B/C"), pred=cls,
ratio=round(ratio, 3), wmax=round(max(prof)),
n=len(prof), pushed=False)
print(f" {t:5.2f}s завеса: {name} ширина макс {max(prof):.0f} мм, "
f"проб {len(prof)}, ср/макс {ratio:.3f} -> класс {cls}")
# пушер: взять класс D, когда он дошёл до ножа
if t > pusher_busy_until:
for name, kind, path in released:
d = done.get(name)
if not d or d["pred"] != "D" or d["pushed"]:
continue
if not stage.GetPrimAtPath(path).IsValid():
continue
x = pos_of(path)[0]
if x <= C.PUSH_X + 0.06:
print(f" {t:5.2f}s ПУШЕР берёт {name} на x={x:+.2f}")
await cell.stroke(app_utils, out=True, speed=1.2)
await cell.stroke(app_utils, out=False, speed=1.5)
d["pushed"] = True
pusher_busy_until = float(tl.get_current_time()) - t0 + 0.15
break
await app_utils.update_app_async(steps=2)
# итог
print("\n ИТОГ")
print(f" {'товар':10s} {'истина':7s} {'датчик':7s} {'ср/макс':>8s} {'шир,мм':>7s} "
f"{'пушер':>6s} {'конец X,Y':>16s} где")
print(" " + "-" * 84)
okc = okp = 0
for name, kind, path in released:
d = done.get(name, dict(gt=("D" if kind == "cyl" else "B/C"), pred="-", ratio=0,
wmax=0, pushed=False))
if stage.GetPrimAtPath(path).IsValid():
x, y, z = pos_of(path)
else:
x = y = z = float("nan")
where = ("ВЕТКА пушера" if y > 0.50 else
"упал" if z < TOP - 0.20 else
"+Y (угол)" if y > 0.10 else
"-Y" if y < -0.10 else "прямо")
if d["pred"] == d["gt"]:
okc += 1
if (d["gt"] == "D") == bool(d["pushed"]):
okp += 1
print(f" {name:10s} {d['gt']:7s} {d['pred']:7s} {d['ratio']:8.3f} {d['wmax']:7.0f} "
f"{'да' if d['pushed'] else 'нет':>6s} ({x:+6.2f},{y:+6.2f}) {where}")
print(f"\n цикл: {_it} итераций, сим {float(tl.get_current_time())-t0:.2f}s, "
f"стена {_wall.time()-_w0:.0f}s, играет={tl.is_playing()}")
tl.stop(); await app_utils.update_app_async(steps=5)
print(f"\n класс определён верно: {okc}/{len(released)} "
f"пушер сработал по назначению: {okp}/{len(released)}")
scripts/run_plow_9045_known_classes.py
+732
View File
@@ -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)")
scripts/run_plow_cell_vision.py
+158
View File
@@ -0,0 +1,158 @@
#!/usr/bin/env python3
"""Run the plow cell with the full vision stack.
./python.sh scripts/run_plow_cell_vision.py # windowed
./python.sh scripts/run_plow_cell_vision.py --headless
./python.sh scripts/run_plow_cell_vision.py --no-vision # route on ground truth
Items are released on the added infeed belt, measured by CRE-ROI v2b under the camera
portal, and the ones that come back class D are diverted by the Y-split pusher when they
break the laser beam. The plow and the authored kinematics are not touched.
"""
from __future__ import annotations
import argparse
import json
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
def parse_args(argv=None):
p = argparse.ArgumentParser(description="plow cell + CRE-ROI v2b")
p.add_argument("--headless", action="store_true")
p.add_argument("--no-vision", action="store_true",
help="skip inference and route on ground truth")
p.add_argument("--speed", type=float, default=None, help="belt speed, m/s")
p.add_argument("--pitch", type=float, default=None, help="metres between items")
p.add_argument("--items", default=None, help="comma-separated release order")
p.add_argument("--log", default=None)
return p.parse_args(argv)
async def _run(app_utils, args):
from robozon_sorter import config as C
from robozon_sorter.sim import plow_vision
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.spawner import AutoFeeder
if args.speed:
C.BELT_SPEED = args.speed
stage, info = plow_vision.load(belt_speed=args.speed, script_control=True)
items = {k: v["zone"] for k, v in info["items"].items()}
print(f"plow cell ready: {len(items)} items {sorted(set(items.values()))}, "
f"infeed release at x={info['spawn_x']}")
vision = None
if not args.no_vision:
from robozon_sorter.cv.pipeline import CreRoiV2b
vision = CreRoiV2b()
vision.attach_cameras()
print("CRE-ROI v2b ready; gate pixels:", vision.gate_px)
await app_utils.update_app_async(steps=40)
cell = Cell(stage, items.keys())
cell.park_all()
await app_utils.update_app_async(steps=15)
order = [n.strip() for n in args.items.split(",")] if args.items else sorted(items)
order = [n for n in order if n in items]
pitch = args.pitch if args.pitch is not None else C.RELEASE_GAP
log, seen = [], set()
route = dict(items) if args.no_vision else {}
def on_event(kind, name, payload):
print(f" {kind:8s} {name:18s} {payload if payload else ''}")
if kind == "done":
for rec in log:
if rec["item"] == name and "outcome" not in rec:
rec["outcome"] = payload.get("where")
feeder = AutoFeeder(cell, order=order, pitch=pitch, route=route,
on_event=on_event).install()
import omni.timeline
timeline = omni.timeline.get_timeline_interface()
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
print(f"\npitch {pitch} m at {C.BELT_SPEED} m/s\n{'kind':>10} detail")
for _ in range(400):
await app_utils.update_app_async(steps=15)
# classify each item once, while it sits under the portal
if vision is not None:
for name in list(feeder.active):
if name in seen:
continue
x = float(cell.pose(name)[0])
if abs(x - C.CAM_X) < 0.08:
was_playing = timeline.is_playing()
res = vision.measure()
# Replicator's step stops the timeline; resume or the line freezes
if was_playing and not timeline.is_playing():
timeline.play()
await app_utils.update_app_async(steps=2)
gt = items[name]
route[name] = res["cls"]
seen.add(name)
print(f" vision {name:18s} pred={res['cls']} gt={gt} "
f"{'ok' if res['cls'] == gt else 'MISS'} dims={res['dims']} "
f"K={res['k']:.2f} views={res['views']} cre={res['cre_ms']}ms")
log.append(dict(item=name, gt=gt, **res))
if len(feeder.finished) >= len(order):
break
app_utils.stop()
await app_utils.update_app_async(steps=15)
feeder.remove()
cell.blade_to(C.BLADE_HOME_Y)
print(f"\n outcomes: {feeder.finished}")
expected = {n: ("bin" if items[n] == "D" else "line-end") for n in order}
wrong = [n for n in expected if feeder.finished.get(n) != expected[n]]
print(f" expected: {expected}")
print(" routing matches ground truth" if not wrong else f" differs on: {wrong}")
graded = [r for r in log if r.get("cls") not in (None, "?")]
if graded:
hits = sum(1 for r in graded if r["cls"] == r["gt"])
cre = [r["cre_ms"] for r in graded if r.get("cre_ms")]
print(f" vision agreed with ground truth on {hits}/{len(graded)}"
+ (f", CRE {sum(cre)/len(cre):.0f} ms/item" if cre else ""))
if args.log:
Path(args.log).write_text(json.dumps(log, indent=2))
print(f" log -> {args.log}")
return log
def main(argv=None):
args = parse_args(argv)
try:
import omni.usd
inside = omni.usd.get_context().get_stage() is not None
except Exception:
inside = False
app = None
if not inside:
from isaacsim import SimulationApp
app = SimulationApp({"headless": args.headless, "width": 1600, "height": 900})
import asyncio
import isaacsim.core.experimental.utils.app as app_utils
loop = asyncio.get_event_loop()
try:
return loop.run_until_complete(_run(app_utils, args))
finally:
if app is not None:
app.close()
if __name__ == "__main__":
sys.exit(0 if main() is not None else 1)
scripts/run_plow_sorting.py
+266
View File
@@ -0,0 +1,266 @@
"""Full cell run: goods ride the line, vision classifies them, the pusher takes D and the
plow splits B and C into their trays. Sent into a live Isaac Sim:
isaacsim_send.py --context sort --file scripts/run_plow_sorting.py \
--args-json '{"vision": true, "pitch": 1.2}'
`scripts/run_plow_cell_vision.py` exercises the pusher only - it never constructs a
PlowSorter, so B and C simply ran off the end of the line. This one wires the plow in and,
more importantly, records *why* an item ended up where it did:
* **detection** - predicted vs ground-truth class, dims, roundness, view count, CRE time.
* **delivery** - the tray the item actually came to rest in, against the tray its class
maps to.
* **kinematics** - for every item, a trace sampled while it crosses the plow: its pose and
speed, the angle the plow was commanded to and the angle the arm actually reached. When
an item does not arrive, that trace is what says whether the sensor missed it, the blade
was still moving, or it was deflected and then stopped short.
The plow is a compliant force drive, so commanded and measured angle are different numbers
and both are logged; treating them as one is what hides a blade that never took up its
angle in time.
"""
import json
import sys
import time
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
# The live Isaac process keeps every module it has ever imported, so an edited
# robozon_sorter/ on disk is invisible to a second run in the same session. Drop the
# package from sys.modules first or you spend the evening re-testing the old code.
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
del sys.modules[_m]
import importlib
importlib.invalidate_caches() # a *new* module file is invisible until the finder is reset
import omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from robozon_sorter import config as C
from robozon_sorter.sim import plow_sort, plow_vision
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.spawner import AutoFeeder
USE_VISION = bool(globals().get("vision", True))
PITCH = float(globals().get("pitch", 1.20))
SPEED = float(globals().get("speed", C.BELT_SPEED))
MAX_SECONDS = float(globals().get("max_seconds", 90.0))
OUT = globals().get("out", "/home/dasha/robozon-sorter/runs/plow_sorting.json")
# where each class is supposed to end up
EXPECT = {"D": "bin", "B": "container_B", "C": "container_C"}
# ---------------------------------------------------------------- scene
C.BELT_SPEED = SPEED
stage, info = plow_vision.load(belt_speed=SPEED, script_control=True)
items = {k: v["zone"] for k, v in info["items"].items()}
# plow_cell.prepare() deactivates /ConveyorTrack_01 as a stray duplicate. In this scene it
# is not a duplicate, it is the -Y sorting lane, so it has to come back on before the lanes
# are driven - otherwise everything the plow deflects toward -Y falls through the gap.
relit = plow_sort.keep_lanes_active(stage)
lanes = plow_sort.configure_lanes(stage, SPEED)
opened = plow_sort.open_junction(stage)
print(f"scene: {len(items)} items {sorted(set(items.values()))} | lane restored={relit} "
f"| lanes driven={len(lanes)} | junction shells opened={len(opened)}")
vision = None
if USE_VISION:
from robozon_sorter.cv.pipeline import CreRoiV2b
vision = CreRoiV2b()
vision.attach_cameras()
print("CRE-ROI v2b ready")
await app_utils.update_app_async(steps=40)
cell = Cell(stage, items.keys())
cell.park_all()
await app_utils.update_app_async(steps=15)
order = sorted(items)
route = {} if USE_VISION else dict(items) # what the pusher acts on (class D)
classes = {} if USE_VISION else dict(items) # what the plow acts on (B / C)
sorter = plow_sort.PlowSorter(stage, cell, classes, plow_sort.calibrate_mapping())
print(f"plow mapping {sorter.mapping} | sensor x={sorter.sense_x} | swing {sorter.swing} deg")
# ---------------------------------------------------------------- logging
rec = {n: dict(item=n, gt=items[n], pred=None, dims=None, k=None, views=None,
cre_ms=None, sensed=False, commanded=None, angle_at_plow=None,
trace=[], max_speed=0.0, blowup=None,
outcome=None, expected=EXPECT.get(items[n]), ok=None)
for n in order}
events = []
def on_event(kind, name, payload):
events.append((round(t_sim, 2), kind, name, payload))
if kind in ("release", "gate", "divert", "error"):
print(f" {kind:8s} {name:18s} {payload if payload else ''}")
feeder = AutoFeeder(cell, order=order, pitch=PITCH, route=route, on_event=on_event)
# ---------------------------------------------------------------- physics hook
t_sim = 0.0
DECIMATE = 8 # 120 Hz / 8 = 15 samples/s: 60 of them span 4 s, the whole discharge
BLOWUP_MS = 5.0 # a belt runs at 1 m/s; anything past this is the solver, not the belt
_tick = 0
def _speed(name):
try:
v = cell._rp[name].get_velocities()[0].numpy()[0]
return float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5)
except Exception:
return 0.0
def _step(dt):
"""the plow has to be serviced from the physics step, like the pusher: the sensor is a
raycast and the blade target is ramped per-step.
The trace is decimated: at full rate 60 samples cover 0.5 m and run out before the item
even reaches the plow, which is how the first pass missed where goods were being thrown.
"""
global t_sim, _tick
t_sim += dt
_tick += 1
try:
sorter.update(dt)
for n in list(feeder.active):
p = cell.pose(n)
x, y, z = float(p[0]), float(p[1]), float(p[2])
r = rec[n]
if x < -5.6: # from the plow approach onward
spd = _speed(n)
if spd > r.get("max_speed", 0.0):
r["max_speed"] = round(spd, 2)
if spd > BLOWUP_MS and r.get("blowup") is None:
r["blowup"] = dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3),
z=round(z, 3), speed=round(spd, 1),
cmd=round(sorter.plow.commanded, 1),
arm=round(sorter.plow.angle, 1))
if _tick % DECIMATE == 0 and len(r["trace"]) < 60:
r["trace"].append(dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3),
z=round(z, 3), v=round(spd, 2),
cmd=round(sorter.plow.commanded, 1),
arm=round(sorter.plow.angle, 1)))
if n in sorter.decided and not r["sensed"]:
r["sensed"] = True
r["commanded"] = round(sorter.decided[n], 1)
if abs(x - C.PLOW_POS[0]) < 0.25 and r["angle_at_plow"] is None:
r["angle_at_plow"] = round(sorter.plow.angle, 1)
except Exception as exc:
events.append((round(t_sim, 2), "step-error", "", repr(exc)))
from omni.physx import get_physx_interface
sub = get_physx_interface().subscribe_physics_step_events(_step)
feeder.install()
timeline = omni.timeline.get_timeline_interface()
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
# ---------------------------------------------------------------- run
print(f"\nrunning: pitch {PITCH} m at {SPEED} m/s, vision={USE_VISION}")
seen = set()
t0 = time.time()
settled = {}
while time.time() - t0 < MAX_SECONDS:
await app_utils.update_app_async(steps=15)
if vision is not None:
for name in list(feeder.active):
if name in seen:
continue
if abs(float(cell.pose(name)[0]) - C.CAM_X) < 0.10:
was = timeline.is_playing()
res = vision.measure()
if was and not timeline.is_playing(): # Replicator's step stops the timeline
timeline.play()
await app_utils.update_app_async(steps=2)
seen.add(name)
r = rec[name]
r.update(pred=res["cls"], dims=res["dims"], k=round(res.get("k", 0.0), 3),
views=res.get("views"), cre_ms=res.get("cre_ms"))
route[name] = res["cls"]
classes[name] = res["cls"]
sorter.classes[name] = res["cls"]
print(f" vision {name:18s} pred={res['cls']} gt={items[name]} "
f"{'ok' if res['cls'] == items[name] else 'MISS'} "
f"dims={res['dims']} K={res.get('k', 0):.2f}")
for n in order: # freeze the outcome once it stops
if n in settled:
continue
p = cell.pose(n)
where = sorter.lane_of(n)
if where.startswith("container") or where == "floor":
settled[n] = where
elif where == "line" and float(p[0]) < C.MAIN_X0 + 0.35:
settled[n] = "line-end"
if len(settled) >= len(order):
break
# outcomes: the D bin is the pusher's, read through mechanics; the trays are the plow's
for n in order:
w = sorter.lane_of(n)
if w == "line" and cell.where(n) == "bin":
w = "bin"
rec[n]["outcome"] = settled.get(n, w)
rec[n]["ok"] = (rec[n]["outcome"] == rec[n]["expected"])
p = cell.pose(n)
rec[n]["final"] = [round(float(v), 3) for v in p[:3]]
app_utils.stop()
await app_utils.update_app_async(steps=10)
sub = None
feeder.remove()
# ---------------------------------------------------------------- report
print("\n===== DETECTION =====")
graded = [r for r in rec.values() if r["pred"] not in (None, "?")]
if graded:
hit = sum(1 for r in graded if r["pred"] == r["gt"])
cre = [r["cre_ms"] for r in graded if r["cre_ms"]]
print(f" class agreement {hit}/{len(graded)}"
+ (f" | CRE {sum(cre)/len(cre):.0f} ms/item" if cre else ""))
for r in sorted(graded, key=lambda r: r["item"]):
print(f" {r['item']:18s} gt={r['gt']} pred={r['pred']} "
f"{'ok' if r['pred'] == r['gt'] else 'MISS':4s} dims={r['dims']} K={r['k']}")
else:
print(" (no vision this run)")
print("\n===== DELIVERY =====")
for r in sorted(rec.values(), key=lambda r: r["item"]):
print(f" {r['item']:18s} gt={r['gt']} -> {str(r['outcome']):12s} "
f"want={str(r['expected']):12s} {'OK' if r['ok'] else 'FAIL'} "
f"final={r['final']}")
good = [r for r in rec.values() if r["ok"]]
print(f" delivered {len(good)}/{len(order)}")
bad = [r for r in rec.values() if not r["ok"]]
if bad:
print("\n===== KINEMATICS ON FAILURES =====")
for r in bad:
print(f" {r['item']} ({r['gt']}) -> {r['outcome']}")
print(f" sensed={r['sensed']} commanded={r['commanded']} "
f"arm_at_plow={r['angle_at_plow']}")
for s in r["trace"][:12]:
print(f" t={s['t']:6.2f} x={s['x']:+.2f} y={s['y']:+.2f} z={s['z']:+.2f} "
f"cmd={s['cmd']:+.1f} arm={s['arm']:+.1f}")
import os
os.makedirs(os.path.dirname(OUT), exist_ok=True)
json.dump(dict(config=dict(pitch=PITCH, speed=SPEED, vision=USE_VISION,
mapping=sorter.mapping, expect=EXPECT),
items=list(rec.values()),
events=[dict(t=t, kind=k, item=n, payload=str(p)) for t, k, n, p in events]),
open(OUT, "w"), indent=2)
print(f"\nlog -> {OUT}")
scripts/seam_check.py
+45
View File
@@ -0,0 +1,45 @@
"""Check for gaps/height-mismatches at every belt-to-belt handoff seam, and compare
plow_cell.usd's own reference grip-material setup against what we're using."""
import omni.usd
from pxr import Usd, UsdGeom, UsdShade, UsdPhysics
stage = omni.usd.get_context().get_stage()
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
def report(path):
prim = stage.GetPrimAtPath(path)
if not prim.IsValid():
print(f" {path} MISSING"); return None
r = bbc.ComputeWorldBound(prim).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
api = UsdShade.MaterialBindingAPI(prim)
mat, _ = api.ComputeBoundMaterial(materialPurpose="physics")
fric = None
if mat:
m = UsdPhysics.MaterialAPI(mat.GetPrim())
fric = (round(m.GetStaticFrictionAttr().Get(),2), round(m.GetDynamicFrictionAttr().Get(),2))
print(f" {path}")
print(f" x[{mn[0]:+.3f}..{mx[0]:+.3f}] y[{mn[1]:+.3f}..{mx[1]:+.3f}] z[{mn[2]:+.3f}..{mx[2]:+.3f}] friction={fric} mat={mat.GetPath() if mat else None}")
return mn, mx
print("=== pusher handoff: ConveyorTrack_03/Belt -> Belt_01 ===")
b1 = report("/World/ConveyorTrack_03/Belt")
b2 = report("/World/ConveyorTrack_03/Belt_01")
if b1 and b2:
print(f" Y GAP (belt max_y to branch min_y): {b2[0][1]-b1[1][1]:+.3f} m Z step: {b2[0][2]-b1[1][2]:+.3f} m")
print("\n=== plow handoff: ConveyorTrack_04 -> decks -> ConveyorTrack_06 / _01 ===")
for path in ["/World/ConveyorTrack_04/Belt", "/World/PlowTransition_B", "/World/PlowCornerDeck_B",
"/World/ConveyorTrack_01/Belt", "/World/PlowTransition_C", "/World/PlowCornerDeck_C",
"/World/ConveyorTrack_06/Belt"]:
report(path)
print("\n=== reference: plow_cell.py's own GRIP_MATERIAL values ===")
import sys
sys.path.insert(0, "/home/dasha/robozon-sorter")
from robozon_sorter.sim import plow_cell as _pc
grip = stage.GetPrimAtPath(_pc.GRIP_MATERIAL)
print(" GRIP_MATERIAL path:", _pc.GRIP_MATERIAL, " valid:", grip.IsValid())
if grip.IsValid():
m = UsdPhysics.MaterialAPI(grip)
print(" static/dynamic:", m.GetStaticFrictionAttr().Get(), m.GetDynamicFrictionAttr().Get())
scripts/smoke_plow_cell.py
+68
View File
@@ -0,0 +1,68 @@
"""Smoke test for scene/plow_cell.usd: load the cell, run the belts, swing the plow under
script control and confirm the arm physically follows.
Run it inside a live Isaac Sim through the code editor's python server, e.g.
python isaacsim_send.py --context plow --file scripts/smoke_plow_cell.py
It asserts the things that were silent failures during the transfer: that the referenced
plow geometry actually composed (a stub resolves to 8 points, the real base to 72k), that
the belts got a surface velocity, and that commanding the drive moves the arm rather than
just setting an attribute nothing reads.
"""
import sys
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import PhysxSchema, UsdGeom
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
from robozon_sorter.sim.plow import Plow
stage, info = plow_cell.load(script_control=True)
print("loaded:", info)
# --- geometry actually composed? --------------------------------------------------------
for path, label, floor in ((C.PLOW_BASE + "/Geom/Mesh", "plow base", 1000),
(C.PLOW_ARM + "/Geom/Mesh", "plow arm", 1000)):
pts = UsdGeom.Mesh(stage.GetPrimAtPath(path)).GetPointsAttr().Get()
n = len(pts) if pts else 0
print(f" {label:10s} {n:>7} points {'OK' if n > floor else 'FAIL (stub or missing)'}")
# --- belts driven? ----------------------------------------------------------------------
driven = 0
for b in plow_cell.BELTS + [plow_cell.BRANCH]:
p = stage.GetPrimAtPath(b)
if p.IsValid() and p.HasAPI(PhysxSchema.PhysxSurfaceVelocityAPI):
v = PhysxSchema.PhysxSurfaceVelocityAPI(p).GetSurfaceVelocityAttr().Get()
if v and any(abs(c) > 1e-6 for c in v):
driven += 1
print(f" belts driven: {driven}/{len(plow_cell.BELTS) + 1}")
# --- plow moves? ------------------------------------------------------------------------
tl = omni.timeline.get_timeline_interface()
tl.play()
await app_utils.update_app_async(steps=30)
plow = Plow(stage)
rest = plow.angle
print(f" rest angle {rest:+.2f} deg")
await plow.swing(app_utils, C.PLOW_SWING)
out = plow.angle
print(f" swung to {out:+.2f} deg (commanded {C.PLOW_SWING:+.1f})")
await plow.swing(app_utils, 0.0)
back = plow.angle
print(f" returned {back:+.2f} deg")
tl.stop()
moved = abs(out - rest) > 0.5 * C.PLOW_SWING
homed = abs(back) < 5.0
print(f"RESULT: arm moved={moved} returned_home={homed} "
f"{'PASS' if moved and homed else 'FAIL'}")
scripts/spawn_debug.py
+62
View File
@@ -0,0 +1,62 @@
"""Isolated: does _activate_item's write actually stick, and is physics playing?"""
import sys
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 omni.usd, omni.timeline
from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema
import isaacsim.core.experimental.utils.app as app_utils
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
print("timeline playing (before):", tl.is_playing())
name = "bottle"
prim = stage.GetPrimAtPath(f"/World/Items/{name}")
print("prim valid:", prim.IsValid())
if not prim.IsValid():
UsdGeom.Xform.Define(stage, "/World/Items")
prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim()
prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{name}.usd"))
xf = UsdGeom.Xformable(prim)
xf.ClearXformOpOrder()
xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(9.0, 5.0, 0.4))
UsdGeom.Imageable(prim).MakeInvisible()
print("xform ops before activate:", [str(op.GetOpType()) for op in UsdGeom.Xformable(prim).GetOrderedXformOps()])
try:
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
op.Set(Gf.Vec3d(C.SPAWN_X, 0.0, C.BELT_Z + 0.05))
print("translate write: OK")
break
UsdPhysics.RigidBodyAPI.Apply(prim)
UsdPhysics.MassAPI.Apply(prim).CreateMassAttr().Set(0.6)
px = PhysxSchema.PhysxRigidBodyAPI.Apply(prim)
px.CreateEnableCCDAttr().Set(True)
UsdGeom.Imageable(prim).MakeVisible()
print("physics API apply: OK")
except BaseException as exc:
print("EXCEPTION during activate:", type(exc).__name__, exc)
# read back the AUTHORED attribute directly (not RigidPrim/Fabric)
attr = prim.GetAttribute("xformOp:translate")
print("authored translate now:", attr.Get() if attr else "NO ATTR")
rp = RigidPrim(paths=[f"/World/Items/{name}"])
print("RigidPrim world pose now:", rp.get_world_poses()[0].numpy()[0])
print("timeline playing (still):", tl.is_playing())
app_utils.play(commit=True)
print("timeline playing (after play() call):", tl.is_playing())
await app_utils.update_app_async(steps=30)
print("RigidPrim world pose after 30 steps of play:", rp.get_world_poses()[0].numpy()[0])
app_utils.stop()
scripts/test_sorting_run.py
+438
View File
@@ -0,0 +1,438 @@
"""Controlled sorting test over the whole item library, with per-item kinematics.
isaacsim_send.py --context test --file scripts/test_sorting_run.py \
--args-json '{"vision": true, "preset": "bright", "repeats": 2}'
What it records, per dispatched item:
* **dispatch** when it was released and with what ground-truth class
* **detection** predicted class, dimensions, roundness, views, CRE time
* **kinematics** at the moment the item is level with the plow: the commanded angle, the
angle the arm had actually reached, and the arm's **angular rate** in deg/s. Commanded
and reached are different numbers - the drive is compliant - and a blade that is still
travelling when the item arrives deflects it differently from one that has settled.
* **outcome** where it came to rest: tray B, tray C, the D bin, a lane, the line, or the
floor, plus the resting pose and whether it matches the tray its class maps to.
Two metric blocks are reported separately, because they fail independently: classification
(what the vision stack decided) and delivery (where the mechanics actually put it). An item
can be classified perfectly and still be left on the line, and the run is only useful if
those two are not conflated.
Lighting preset and the floor come from `sim/staging`, so a run can be repeated under
`bright` / `dim` / `harsh` to see how much of the classification error is illumination.
"""
import json
import os
import sys
import time
from collections import Counter, defaultdict
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import importlib
# The live Isaac process keeps every module it has ever imported, so an edited
# robozon_sorter/ on disk is invisible to a second run. Dropping the package is not enough
# on its own: a module file that did not exist when the directory was first scanned stays
# invisible until the import finder's cached listing is thrown away too.
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
del sys.modules[_m]
importlib.invalidate_caches()
import omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from robozon_sorter import config as C
from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.spawner import AutoFeeder
USE_VISION = bool(globals().get("vision", True))
PRESET = globals().get("preset", "bright")
REPEATS = int(globals().get("repeats", 1))
PITCH = float(globals().get("pitch", 2.5))
SPEED = float(globals().get("speed", 1.0))
LIMIT = int(globals().get("limit", 0)) # 0 = whole library
CLASSES_ONLY = set(str(globals().get("classes", "")).upper()) or None
# Explicit dispatch list, in order, repeats allowed. `limit`/`classes` cannot express
# "these exact items, plus an even 10/10/10 of the rest" when a class has fewer than 10
# unique members - the only way to balance is to send some of them twice.
ONLY = [n for n in str(globals().get("only", "")).split(",") if n.strip()]
# Items to score SEPARATELY as well as in the overall figures.
FOCUS = [n for n in str(globals().get("focus", "")).split(",") if n.strip()]
BUDGET = float(globals().get("max_seconds", 240.0))
TRACK_END_X = float(globals().get("track_end_x", -11.5)) # past both trays
ITEMS_DIR = globals().get("items_dir", f"{REPO}/assets/items")
OUT = globals().get("out", f"{REPO}/runs/test_sorting_{PRESET}.json")
EXPECT = {"D": "bin", "B": "container_B", "C": "container_C"}
CLASSES = ("B", "C", "D")
# ---------------------------------------------------------------- scene
C.BELT_SPEED = SPEED
stage, info = plow_vision.load(belt_speed=SPEED, script_control=True,
meshes_dir=ITEMS_DIR)
staged = staging.stage_cell(stage, preset=PRESET, floor=True)
plow_sort.keep_lanes_active(stage)
lanes = plow_sort.configure_lanes(stage, SPEED)
opened = plow_sort.open_junction(stage)
items = {k: v["zone"] for k, v in info["items"].items()}
gt_dims = {k: v.get("gt_dims_mm") for k, v in info["items"].items()}
print(f"library {len(items)} items {dict(Counter(items.values()))} | light={PRESET} "
f"| floor={'yes' if staged.get('floor') else 'no'} | lanes={len(lanes)} "
f"| junction opened={len(opened)}")
vision = None
if USE_VISION:
# The streaming launcher starts Kit WITHOUT the user site-packages, so ultralytics and
# torch installed under ~/.local are invisible to the running app even though
# `python.sh` imports them fine - it is the same interpreter (3.12.13), just a
# different sys.path. Appending (not prepending) leaves Kit's own bundled copies first.
for _sp in ("/home/dasha/.local/lib/python3.12/site-packages",):
if os.path.isdir(_sp) and _sp not in sys.path:
sys.path.append(_sp)
from robozon_sorter.cv.pipeline import CreRoiV2b
vision = CreRoiV2b()
vision.attach_cameras()
_w = await vision.warmup()
print(f"CRE-ROI v2b attached | прогрев камер: {'ок' if _w['ok'] else 'НЕ УДАЛСЯ'} "
f"за {_w['attempts']} подход(а), самый тёмный глаз max={_w['darkest_eye_max']}")
if not _w["ok"]:
print(" ВНИМАНИЕ: камеры всё ещё отдают чёрное - классификация будет пустой")
await app_utils.update_app_async(steps=40)
# Aim the viewport at the plow before anything else. The default Persp framing tries to
# fit the WHOLE stage, and the stage contains the parked queue off at x +37 - so the cell
# ends up a few pixels wide and the stream looks black with only the emissive laser stripe
# in it. That is what "renders wrong" was: aim, not lighting.
try:
from isaacsim.core.rendering_manager import ViewportManager
ViewportManager.set_camera_view("/OmniverseKit_Persp", eye=[-4.5, -5.0, 5.0],
target=[-6.5, 0.0, 1.8])
except Exception as _e:
print(" (камеру навести не удалось:", _e, ")")
cell = Cell(stage, items.keys())
cell.park_all()
await app_utils.update_app_async(steps=15)
base_order = sorted(items)
# Optional class filter, e.g. classes="BC" runs only the B and C items. Without it a small
# `limit` just takes the first N alphabetically, which can miss a whole class: limit=8 gave
# B=4 D=4 and not one C, so the C route went untested.
if ONLY:
missing = [n for n in ONLY if n not in items]
if missing:
print(f" ВНИМАНИЕ: нет в библиотеке: {missing}")
base_order = [n.strip() for n in ONLY if n.strip() in items]
elif CLASSES_ONLY:
base_order = [n for n in base_order if items[n] in CLASSES_ONLY]
if LIMIT:
base_order = base_order[:LIMIT]
order = base_order * max(1, REPEATS) # repeat the library to reach a dispatch count
route, classes = ({}, {}) if USE_VISION else (dict(items), dict(items))
sorter = plow_sort.PlowSorter(stage, cell, classes, plow_sort.calibrate_mapping())
BEAMS = lane_beams.LaneBeams(stage, cell, plow=sorter.plow)
print(f"dispatching {len(order)} ({len(base_order)} unique x{max(1, REPEATS)}) | "
f"mapping {sorter.mapping} | pitch {PITCH} m @ {SPEED} m/s")
# ---------------------------------------------------------------- logging
def blank(name, pas):
return dict(item=name, pass_no=pas, gt=items[name], gt_dims=gt_dims.get(name),
released_t=None, pred=None, dims=None, k=None, views=None, cre_ms=None,
sensed=False, commanded=None,
arm_at_plow=None, rate_at_plow=None, arm_max_rate=0.0,
max_speed=0.0, blowup=None, trace=[],
contact=[], contact_first=None, contact_last=None,
outcome=None, expected=EXPECT.get(items[name]), delivered=None,
final=None)
pas = defaultdict(int)
rec = {} # name -> record for the pass currently on the line
done_records = []
events = []
t_sim = 0.0
DECIMATE = 8
BLOWUP_MS = 5.0
_tick = 0
_prev_angle = 0.0
def on_event(kind, name, payload):
events.append(dict(t=round(t_sim, 2), kind=kind, item=name, payload=str(payload)))
if kind in ("release", "divert", "error"):
print(f" {kind:8s} {name:20s} {payload if payload else ''}")
feeder = AutoFeeder(cell, order=order, pitch=PITCH, route=route, on_event=on_event)
def _speed(name):
try:
v = cell._rp[name].get_velocities()[0].numpy()[0]
return float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5)
except Exception:
return 0.0
def _step(dt):
"""service the plow and sample kinematics as goods cross it"""
global t_sim, _tick, _prev_angle
t_sim += dt
_tick += 1
try:
sorter.update(dt)
BEAMS.tick(dt)
BEAMS.poll(rate=C.PLOW_SWEEP_RATE)
arm = sorter.plow.angle
rate = (arm - _prev_angle) / dt if dt > 0 else 0.0 # deg/s, measured not commanded
_prev_angle = arm
for n in list(feeder.active):
r = rec.get(n)
if r is None:
continue
p = cell.pose(n)
x, y, z = float(p[0]), float(p[1]), float(p[2])
if x >= -5.6:
continue
spd = _speed(n)
r["max_speed"] = max(r["max_speed"], round(spd, 2))
r["arm_max_rate"] = max(r["arm_max_rate"], round(abs(rate), 1))
if spd > BLOWUP_MS and r["blowup"] is None:
r["blowup"] = dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3),
z=round(z, 3), speed=round(spd, 1),
arm=round(arm, 1), rate=round(rate, 1))
if _tick % DECIMATE == 0 and len(r["trace"]) < 50:
r["trace"].append(dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3),
z=round(z, 3), v=round(spd, 2),
cmd=round(sorter.plow.commanded, 1),
arm=round(arm, 1), rate=round(rate, 1)))
if n in sorter.decided and not r["sensed"]:
r["sensed"] = True
r["commanded"] = round(sorter.decided[n], 1)
# the instant the item is level with the plow: this is the state that decides
if abs(x - C.PLOW_POS[0]) < 0.25 and r["arm_at_plow"] is None:
r["arm_at_plow"] = round(arm, 1)
r["rate_at_plow"] = round(rate, 1)
# CONTACT WINDOW: while the item is inside the arm's sweep radius, record how
# the blade is actually turning. This is what says whether it leaned the item
# over at tip speed or arrived as a hit - a single sample at the plow centre
# cannot tell those apart.
reach = (x - C.PLOW_POS[0]) ** 2 + (y - C.PLOW_POS[1]) ** 2
if reach < (C.PLOW_ARM_LEN + 0.10) ** 2:
if r["contact_first"] is None:
r["contact_first"] = dict(t=round(t_sim, 2), x=round(x, 3),
y=round(y, 3), arm=round(arm, 1),
rate=round(rate, 1), v=round(spd, 2))
if len(r["contact"]) < 40:
r["contact"].append(dict(t=round(t_sim, 2), y=round(y, 3),
arm=round(arm, 1), rate=round(rate, 1),
v=round(spd, 2)))
r["contact_last"] = dict(t=round(t_sim, 2), y=round(y, 3),
arm=round(arm, 1), v=round(spd, 2))
except Exception as exc:
events.append(dict(t=round(t_sim, 2), kind="step-error", item="", payload=repr(exc)))
from omni.physx import get_physx_interface
sub = get_physx_interface().subscribe_physics_step_events(_step)
feeder.install()
timeline = omni.timeline.get_timeline_interface()
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
# ---------------------------------------------------------------- run
seen, settled = set(), {}
t0 = time.time()
while time.time() - t0 < BUDGET:
await app_utils.update_app_async(steps=15)
for n in feeder.active: # open a record when an item is released
if n not in rec:
pas[n] += 1
rec[n] = blank(n, pas[n])
rec[n]["released_t"] = round(t_sim, 2)
if vision is not None:
for name in list(feeder.active):
if name in seen or name not in rec:
continue
if abs(float(cell.pose(name)[0]) - C.CAM_X) < 0.10:
was = timeline.is_playing()
res = vision.measure()
if was and not timeline.is_playing():
timeline.play()
await app_utils.update_app_async(steps=2)
seen.add(name)
r = rec[name]
r.update(pred=res.get("cls"), dims=res.get("dims"),
k=round(res.get("k", 0.0), 3), views=res.get("views"),
cre_ms=res.get("cre_ms"))
route[name] = res.get("cls")
sorter.classes[name] = res.get("cls")
for n in list(rec): # freeze an outcome once the item stops
if n in settled:
continue
where = sorter.lane_of(n)
if where == "line" and cell.where(n) == "bin":
where = "bin"
p = cell.pose(n)
resting = where.startswith("container") or where in ("bin", "floor")
# Freeze only once the item is genuinely done. The old cutoff was MAIN_X0 + 0.35 =
# -7.65, which is the fork apex - every item was declared "line-end" at full 0.80 m/s
# the instant it entered its branch, so no B or C delivery could ever be observed.
if resting or (where == "line" and float(p[0]) < TRACK_END_X):
settled[n] = where if resting else "line-end"
r = rec.pop(n)
r["outcome"] = settled[n]
r["final"] = [round(float(v), 3) for v in p[:3]]
r["delivered"] = (r["outcome"] == r["expected"])
done_records.append(r)
seen.discard(n)
settled.pop(n, None)
if len(done_records) >= len(order):
break
for n, r in list(rec.items()): # whatever is still on the line at the end
p = cell.pose(n)
r["outcome"] = sorter.lane_of(n)
r["final"] = [round(float(v), 3) for v in p[:3]]
r["delivered"] = (r["outcome"] == r["expected"])
done_records.append(r)
app_utils.stop()
await app_utils.update_app_async(steps=10)
sub = None
feeder.remove()
# ---------------------------------------------------------------- metrics
print(f"\n===== DISPATCHED {len(done_records)} =====")
print(f"{'item':22s} {'gt':2s} {'pred':4s} {'outcome':13s} {'want':13s} "
f"{'arm':>6s} {'rate':>8s} {'vmax':>6s}")
for r in done_records:
print(f"{r['item']:22s} {r['gt']:2s} {str(r['pred'] or '-'):4s} "
f"{str(r['outcome']):13s} {str(r['expected']):13s} "
f"{str(r['arm_at_plow']):>6s} {str(r['rate_at_plow']):>8s} "
f"{r['max_speed']:>6.1f} {'OK' if r['delivered'] else ''}")
# --- classification -------------------------------------------------------
graded = [r for r in done_records if r["pred"] in CLASSES]
print("\n===== CLASSIFICATION (CV) =====")
if graded:
conf = {a: Counter() for a in CLASSES}
for r in graded:
conf[r["gt"]][r["pred"]] += 1
hits = sum(conf[a][a] for a in CLASSES)
print(f" accuracy {hits}/{len(graded)} = {hits / len(graded):.2f}")
print(" confusion (rows GT, cols pred): " + " ".join(CLASSES))
for a in CLASSES:
print(f" {a}: " + " ".join(f"{conf[a][b]:3d}" for b in CLASSES))
for a in CLASSES:
tp = conf[a][a]
fp = sum(conf[g][a] for g in CLASSES) - tp
fn = sum(conf[a].values()) - tp
pr = tp / (tp + fp) if tp + fp else 0.0
rc = tp / (tp + fn) if tp + fn else 0.0
f1 = 2 * pr * rc / (pr + rc) if pr + rc else 0.0
print(f" {a}: precision {pr:.2f} recall {rc:.2f} F1 {f1:.2f} (n={tp + fn})")
cre = [r["cre_ms"] for r in graded if r.get("cre_ms")]
if cre:
print(f" CRE {sum(cre) / len(cre):.0f} ms/item over {len(cre)}")
if FOCUS:
fset = {n.strip() for n in FOCUS}
fg = [r for r in graded if r["item"] in fset]
print(f"\n ----- ОТДЕЛЬНО ПО НАЗВАННЫМ ТОВАРАМ ({len(fg)} из {len(fset)}) -----")
print(f" {'товар':<20} {'GT':<3} {'пред':<5} {'дim пред, мм':<20} {'GT дим, мм':<20} {'k':<6} верно")
okn = 0
for r in sorted(fg, key=lambda r: r["item"]):
good = r["pred"] == r["gt"]
okn += bool(good)
dp = "x".join(str(int(x)) for x in (r.get("dims") or [])) or "-"
dg = "x".join(str(int(x)) for x in (r.get("gt_dims") or [])) or "-"
print(f" {r['item']:<20} {r['gt']:<3} {str(r['pred']):<5} {dp:<20} {dg:<20} "
f"{(r.get('k') or 0):<6.3f} {'да' if good else 'НЕТ'}")
if fg:
print(f" точность по названным: {okn}/{len(fg)} = {okn / len(fg):.2f}")
miss = sorted(fset - {r["item"] for r in fg})
if miss:
print(f" не получили предсказания: {miss}")
else:
print(" no vision this run")
# --- delivery -------------------------------------------------------------
if sorter.contact is not None:
rep = sorter.contact.report()
print("\n===== PLOW CONTACT SENSOR =====")
print(f" {len(rep['touches'])} items touched the blade")
print(f" {'item':22s} {'cls':4s} {'angle@touch':>12s} {'range':>14s} {'dur s':>7s}")
for t in rep["touches"]:
print(f" {t['item']:22s} {str(t['cls']):4s} {str(t['angle_at_touch']):>12s} "
f"{str(t['angle_min']) + '..' + str(t['angle_max']):>14s} "
f"{str(t['duration']):>7s}"
+ ("" if t["classified"] else " UNCLASSIFIED - not steered"))
rep = BEAMS.report()
cf = sorted(getattr(sorter, "conflicts", set()))
print("\n===== КОНФЛИКТЫ ОЧЕРЕДИ ПЛУГА =====")
if not cf:
print(" нет: в зоне лезвия ни разу не оказалось двух классов одновременно")
else:
print(f" {len(cf)} товар(ов) делили зону лезвия с товаром ДРУГОГО класса.")
print(" Один нож не может держать два угла сразу - это предел подачи, не сбой:")
print(" " + ", ".join(cf))
print("\n===== ЛАЗЕР ПЕРЕД ПЛУГОМ (предустановка угла) =====")
gl = getattr(sorter, "gate_log", [])
if not gl:
print(" створ не сработал ни разу")
else:
print(f" сработал {len(gl)} раз | створ x={plow_sort.SENSE_X}, лезвие с x=-7.32")
print(f" {'товар':<20} {'класс':<6} {'угол':>7} {'x на срабатывании':>18}")
for g in gl:
print(f" {g['item']:<20} {str(g['cls']):<6} {g['angle']:>+7.1f} {g['x']:>18.2f}")
print("\n===== ЛАЗЕРНЫЕ ДАТЧИКИ НА ЛЕНТАХ B/C =====")
print(f" доехали до ленты: {len(rep)} из {len(done_records)} отправленных")
for c in rep:
print(f" {c['item']:20s} -> {c['lane']:7s} t={c['t']:6.2f}s угол ножа={c['angle']} v={c['speed']}")
if not rep:
print(" ни один товар не доехал ни до одной ленты")
print("\n===== DELIVERY (mechanics) =====")
ok = [r for r in done_records if r["delivered"]]
print(f" delivered {len(ok)}/{len(done_records)} = {len(ok) / max(len(done_records), 1):.2f}")
per_class = defaultdict(lambda: [0, 0])
for r in done_records:
per_class[r["gt"]][1] += 1
per_class[r["gt"]][0] += bool(r["delivered"])
for a in CLASSES:
got, tot = per_class[a]
if tot:
print(f" {a}: {got}/{tot} into {EXPECT[a]}")
print(" where everything ended up: " +
str(dict(Counter(r["outcome"] for r in done_records))))
thrown = [r for r in done_records if r["blowup"]]
stalled = [r for r in done_records if r["outcome"] in ("line", "lane_B", "lane_C")]
print(f" thrown by the mechanics: {len(thrown)} | stalled short of a tray: {len(stalled)}")
if thrown:
r = thrown[0]
print(f" e.g. {r['item']}: {r['blowup']}")
if stalled:
r = stalled[0]
print(f" e.g. {r['item']}: stopped at {r['final']} arm={r['arm_at_plow']}")
os.makedirs(os.path.dirname(OUT), exist_ok=True)
contact_report = sorter.contact.report() if sorter.contact else None
json.dump(dict(contact=contact_report, config=dict(preset=PRESET, vision=USE_VISION, pitch=PITCH, speed=SPEED,
repeats=REPEATS, mapping=sorter.mapping, expect=EXPECT,
staged=staged, items_dir=ITEMS_DIR),
records=done_records, events=events[-400:]),
open(OUT, "w"), indent=2, ensure_ascii=False)
print(f"\nlog -> {OUT}")
scripts/tune_sweep_rate.py
+117
View File
@@ -0,0 +1,117 @@
"""Find the plow sweep rate that actually lands goods on their lane.
isaacsim_send.py --context tune --file scripts/tune_sweep_rate.py \
--args-json '{"rates": [120, 200, 300, 450], "n": 4}'
Delivery alone cannot tune this: an item on the floor and an item still on the belt both
score zero and need opposite corrections. So each rate is judged on the lane-entry beams
(`sim/lane_beams`), which separate the two:
crossed the push reached the lane <- too slow if this is low
speed how fast it was going when it did <- throwing it if this is high
A usable rate crosses most items at a modest crossing speed. The sweep is a **push**, so
the blade returns to centre after each item and waits there - `PlowSorter` does that, and
the run reports how many times it completed a return, so a blade that stops homing shows up
as a number rather than as a mystery later.
"""
import json
import sys
import time
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import importlib
for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]:
del sys.modules[_m]
importlib.invalidate_caches()
import omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from robozon_sorter import config as C
from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.spawner import AutoFeeder
RATES = globals().get("rates", [120.0, 200.0, 300.0, 450.0])
N = int(globals().get("n", 4))
SPEED = float(globals().get("speed", 0.8))
PITCH = float(globals().get("pitch", 3.5))
BUDGET = float(globals().get("per_rate_seconds", 70.0))
OUT = globals().get("out", f"{REPO}/runs/sweep_tuning.json")
stage, info = plow_vision.load(belt_speed=SPEED, script_control=True,
meshes_dir=f"{REPO}/assets/items")
staging.stage_cell(stage, preset="bright", floor=True)
plow_sort.keep_lanes_active(stage)
plow_sort.configure_lanes(stage, SPEED)
plow_sort.open_junction(stage)
items = {k: v["zone"] for k, v in info["items"].items()}
await app_utils.update_app_async(steps=30)
cell = Cell(stage, items.keys())
order = [n for n in sorted(items) if items[n] in ("B", "C")][:N]
print(f"tuning on {len(order)} B/C items: {order}")
timeline = omni.timeline.get_timeline_interface()
results = []
for rate in RATES:
C.PLOW_SWEEP_RATE = float(rate)
cell.park_all()
await app_utils.update_app_async(steps=15)
sorter = plow_sort.PlowSorter(stage, cell, items, plow_sort.calibrate_mapping())
beams = lane_beams.LaneBeams(stage, cell, plow=sorter.plow)
def _step(dt, _s=sorter, _b=beams, _r=rate):
_s.update(dt)
_b.tick(dt)
_b.poll(rate=_r)
from omni.physx import get_physx_interface
sub = get_physx_interface().subscribe_physics_step_events(_step)
feeder = AutoFeeder(cell, order=order, pitch=PITCH, route={}).install()
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
t0 = time.time()
while time.time() - t0 < BUDGET:
await app_utils.update_app_async(steps=15)
if len(beams.crossings) >= len(order):
break
app_utils.stop()
await app_utils.update_app_async(steps=10)
feeder.remove()
sub = None
cr = beams.report()
speeds = [c["speed"] for c in cr]
where = {n: sorter.lane_of(n) for n in order}
delivered = sum(1 for n in order
if where[n] == {"B": "container_B", "C": "container_C"}[items[n]])
row = dict(rate=rate, crossed=len(cr), of=len(order),
mean_cross_speed=round(sum(speeds) / len(speeds), 2) if speeds else None,
max_cross_speed=round(max(speeds), 2) if speeds else None,
delivered=delivered, homed=sorter.homed, where=where,
crossings=cr)
results.append(row)
print(f" rate {rate:6.0f} deg/s -> crossed {len(cr)}/{len(order)} "
f"cross speed mean {row['mean_cross_speed']} max {row['max_cross_speed']} "
f"delivered {delivered} homed {sorter.homed}")
print("\n===== SWEEP RATE TUNING =====")
print(f"{'rate':>7} {'crossed':>9} {'mean v':>8} {'max v':>7} {'delivered':>10} {'homed':>6}")
for r in results:
print(f"{r['rate']:7.0f} {r['crossed']:4d}/{r['of']:<4d} "
f"{str(r['mean_cross_speed']):>8} {str(r['max_cross_speed']):>7} "
f"{r['delivered']:10d} {r['homed']:6d}")
best = max(results, key=lambda r: (r["delivered"], r["crossed"], -(r["max_cross_speed"] or 9)))
print(f"\nbest so far: {best['rate']:.0f} deg/s "
f"(tip {C.PLOW_ARM_LEN * best['rate'] * 3.14159 / 180:.2f} m/s)")
import os
os.makedirs(os.path.dirname(OUT), exist_ok=True)
json.dump(dict(rates=RATES, belt=SPEED, pitch=PITCH, results=results),
open(OUT, "w"), indent=2)
print(f"log -> {OUT}")
scripts/tune_sweep_standalone.py
+151
View File
@@ -0,0 +1,151 @@
#!/usr/bin/env python3
"""Sweep-rate tuning as a standalone Isaac Sim process.
/home/whatevenif/isaacsim/python.sh scripts/tune_sweep_standalone.py \
--rates 120,200,300,450 --items 4 --headless
Same experiment as `tune_sweep_rate.py`, but it brings up its own `SimulationApp` instead
of being sent into a live Kit through the code-editor socket. That socket stopped returning
results on anything longer than a minute or two - the code kept running (one run wrote its
log in full) but the reply never arrived, so six runs in a row looked like failures. A
standalone process writes its log itself and can be read afterwards, which removes the
connection from the experiment entirely.
Each rate is judged on the lane-entry beams, not on delivery alone: an item left on the belt
and an item thrown to the floor both score zero and need opposite corrections.
crossed the push reached the lane -> low means the sweep is too slow
speed how fast it crossed -> high means it is throwing them
homed completed returns to centre -> the blade must park in the middle between items
"""
from __future__ import annotations
import argparse
import json
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parent.parent
def parse_args(argv=None):
p = argparse.ArgumentParser(description="tune the plow sweep rate")
p.add_argument("--rates", default="120,200,300,450", help="deg/s, comma separated")
p.add_argument("--items", type=int, default=4, help="how many B/C items per rate")
p.add_argument("--speed", type=float, default=0.8, help="belt m/s")
p.add_argument("--pitch", type=float, default=3.5, help="metres between items")
p.add_argument("--seconds", type=float, default=60.0, help="budget per rate")
p.add_argument("--headless", action="store_true")
p.add_argument("--out", default=str(ROOT / "runs" / "sweep_tuning.json"))
return p.parse_args(argv)
async def _run(app_utils, args):
import omni.timeline
from omni.physx import get_physx_interface
from robozon_sorter import config as C
from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.spawner import AutoFeeder
rates = [float(r) for r in args.rates.split(",") if r.strip()]
stage, info = plow_vision.load(belt_speed=args.speed, script_control=True,
meshes_dir=str(ROOT / "assets" / "items"))
staging.stage_cell(stage, preset="bright", floor=True)
plow_sort.keep_lanes_active(stage)
lanes = plow_sort.configure_lanes(stage, args.speed)
plow_sort.open_junction(stage)
items = {k: v["zone"] for k, v in info["items"].items()}
print(f"scene ready: {len(items)} items, {len(lanes)} lanes/decks driven")
await app_utils.update_app_async(steps=40)
cell = Cell(stage, items.keys())
order = [n for n in sorted(items) if items[n] in ("B", "C")][:args.items]
want = {"B": "container_B", "C": "container_C"}
print(f"tuning on {len(order)} B/C items: {order}")
timeline = omni.timeline.get_timeline_interface()
results = []
for rate in rates:
C.PLOW_SWEEP_RATE = rate
cell.park_all()
await app_utils.update_app_async(steps=20)
sorter = plow_sort.PlowSorter(stage, cell, items, plow_sort.calibrate_mapping())
beams = lane_beams.LaneBeams(stage, cell, plow=sorter.plow)
def _step(dt, _s=sorter, _b=beams, _r=rate):
try:
_s.update(dt)
_b.tick(dt)
_b.poll(rate=_r)
except Exception:
pass
sub = get_physx_interface().subscribe_physics_step_events(_step)
feeder = AutoFeeder(cell, order=order, pitch=args.pitch, route={}).install()
app_utils.play(commit=True)
await app_utils.update_app_async(steps=20)
for _ in range(int(args.seconds * 4)):
await app_utils.update_app_async(steps=15)
if len(beams.crossings) >= len(order):
break
app_utils.stop()
await app_utils.update_app_async(steps=15)
feeder.remove()
sub = None
cr = beams.report()
sp = [c["speed"] for c in cr]
where = {n: sorter.lane_of(n) for n in order}
delivered = sum(1 for n in order if where[n] == want[items[n]])
row = dict(rate=rate, crossed=len(cr), of=len(order), delivered=delivered,
homed=sorter.homed,
mean_cross_speed=round(sum(sp) / len(sp), 2) if sp else None,
max_cross_speed=round(max(sp), 2) if sp else None,
where=where, crossings=cr)
results.append(row)
print(f" rate {rate:6.0f} deg/s (tip {C.PLOW_ARM_LEN * rate * 3.14159 / 180:.2f} m/s)"
f" -> crossed {len(cr)}/{len(order)} delivered {delivered} "
f"homed {sorter.homed} cross v mean {row['mean_cross_speed']} "
f"max {row['max_cross_speed']}")
print("\n===== SWEEP RATE TUNING =====")
print(f"{'rate':>7} {'tip m/s':>8} {'crossed':>9} {'delivered':>10} {'homed':>6} "
f"{'mean v':>8} {'max v':>7}")
for r in results:
print(f"{r['rate']:7.0f} {0.6 * r['rate'] * 3.14159 / 180:8.2f} "
f"{r['crossed']:4d}/{r['of']:<4d} {r['delivered']:10d} {r['homed']:6d} "
f"{str(r['mean_cross_speed']):>8} {str(r['max_cross_speed']):>7}")
if results:
best = max(results, key=lambda r: (r["delivered"], r["crossed"],
-(r["max_cross_speed"] or 99)))
print(f"\nbest: {best['rate']:.0f} deg/s "
f"crossed {best['crossed']}/{best['of']} delivered {best['delivered']}")
out = Path(args.out)
out.parent.mkdir(parents=True, exist_ok=True)
out.write_text(json.dumps(dict(rates=rates, belt=args.speed, pitch=args.pitch,
items=order, results=results), indent=2))
print(f"log -> {out}")
return results
def main(argv=None):
args = parse_args(argv)
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from isaacsim import SimulationApp
app = SimulationApp({"headless": args.headless, "width": 1280, "height": 800})
try:
import asyncio
import isaacsim.core.experimental.utils.app as app_utils
return asyncio.get_event_loop().run_until_complete(_run(app_utils, args))
finally:
app.close()
if __name__ == "__main__":
sys.exit(0 if main() else 1)
scripts/verify_belts.py
+190
View File
@@ -0,0 +1,190 @@
"""Проверка переноса товара трением на 1 м/с по ВСЕМ дорожкам новой сцены 90/45.
Почему это не просто "запустить существующий код". Список лент в scene.py жёстко
заканчивается на ConveyorTrack_05, а в новой сборке есть седьмая дорожка -
ConveyorTrack_06, угловая секция 1.04 x 1.05 м в точке (-8.0, +0.25). Кроме того из
сцены пропала корневая /ConveyorTrack_01 - 45-градусная дорожка плуга, которую этот
угол заменил. Поэтому ленты перечисляются здесь заново, по факту сцены.
surfaceVelocity задаётся в ЛОКАЛЬНОЙ системе тела, и дорожки уложены по-разному:
у ConveyorTrack_04 и _06 локальный +X смотрит в мировой -X. Направление выводится из
мировой цели, а величина делится на то, сколько мирового стоит одна локальная единица -
у _06 масштаб 0.5, и без этого деления скорость вышла бы вдвое меньше.
Скорость измеряется по фактическому перемещению тел, а не по заданному атрибуту:
атрибут можно выставить и не заметить, что трения не хватает и товар проскальзывает.
"""
import sys, math
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
SPEED = 1.0
SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd"
# дорожка -> мировое направление, куда она должна везти. Выведено из раскладки:
# главный ход идёт в -X, ветка пушера в +Y, угловая секция _06 уводит в +Y.
INTENT = {
"/World/ConveyorTrack_05/Belt": (-1, 0, 0),
"/World/ConveyorTrack/Belt": (-1, 0, 0),
"/World/ConveyorTrack_02/Belt": (-1, 0, 0),
"/World/ConveyorTrack_03/Belt": (-1, 0, 0),
"/World/ConveyorTrack_04/Belt": (-1, 0, 0),
"/World/ConveyorTrack_01/Belt": (-1, 0, 0),
"/World/ConveyorTrack_06/Belt": (0, 1, 0), # угол на 90 градусов
"/World/ConveyorTrack_03/Belt_01": (0, 1, 0), # ветка пушера
}
stage = omni.usd.get_context().get_stage()
if stage is None or SCENE not in stage.GetRootLayer().identifier:
omni.usd.get_context().open_stage(SCENE)
await app_utils.update_app_async(steps=60)
stage = omni.usd.get_context().get_stage()
print("сцена:", stage.GetRootLayer().identifier)
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop()
await app_utils.update_app_async(steps=10)
# --- физическая сцена -----------------------------------------------------------------
ps = None
for p in stage.Traverse():
if p.IsA(UsdPhysics.Scene):
ps = p; break
if ps is None:
ps = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim()
print("физическая сцена создана")
px = PhysxSchema.PhysxSceneAPI.Apply(ps)
hz = px.GetTimeStepsPerSecondAttr().Get() or 60
if hz < 120:
px.CreateTimeStepsPerSecondAttr().Set(120); hz = 120
px.CreateEnableCCDAttr().Set(True)
print(f"физика: {ps.GetPath()}, {hz} Гц, CCD включён")
# --- поверхности лент: измерить, а не угадать ------------------------------------------
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
surf = {}
for path in INTENT:
pr = stage.GetPrimAtPath(path)
if not pr.IsValid():
print(f" НЕТ ПРЕМА {path}"); continue
r = bb.ComputeWorldBound(pr).ComputeAlignedRange()
surf[path] = (r.GetMin(), r.GetMax())
# --- привод лент ----------------------------------------------------------------------
print(f"\nПРИВОД ЛЕНТ на {SPEED} м/с:")
driven = {}
for path, intent in INTENT.items():
if path not in surf:
continue
v = plow_cell.drive_belt(stage, path, intent, SPEED)
driven[path] = v
mn, mx = surf[path]
print(f" {path:36s} цель {intent} локальная v={v} верх z={mx[2]:.3f}")
# графы конвейера гасим на ВСЕХ семи дорожках - код проекта знает только шесть
off = 0
for p in stage.Traverse():
if "ConveyorBeltGraph" in p.GetName():
p.SetActive(False); off += 1
print(f" отключено графов ConveyorBeltGraph: {off}")
# --- пробные тела ----------------------------------------------------------------------
TESTS = [
("main_05", "/World/ConveyorTrack_05/Belt", (+1.20, 0.00)),
("main_00", "/World/ConveyorTrack/Belt", (-1.00, 0.00)),
("fork_03", "/World/ConveyorTrack_03/Belt", (-3.00, 0.00)),
("plow_04", "/World/ConveyorTrack_04/Belt", (-6.60, 0.00)),
("lane_01", "/World/ConveyorTrack_01/Belt", (-8.60, -0.22)),
("corner_06","/World/ConveyorTrack_06/Belt", (-8.45, 0.35)),
("branch", "/World/ConveyorTrack_03/Belt_01", (-3.90, 0.60)),
]
ROOT = "/World/_BeltProbe"
if stage.GetPrimAtPath(ROOT).IsValid():
stage.RemovePrim(ROOT)
stage.DefinePrim(ROOT, "Xform")
made = []
for name, belt, (x, y) in TESTS:
if belt not in surf:
print(f" проба {name}: нет ленты {belt}"); continue
top = surf[belt][1][2]
path = f"{ROOT}/{name}"
cube = UsdGeom.Cube.Define(stage, path)
cube.CreateSizeAttr().Set(2.0) # half-extent = 1, масштабом задаём 6 см
xf = UsdGeom.Xformable(cube.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(x, y, top + 0.045))
xf.AddScaleOp().Set(Gf.Vec3f(0.03, 0.03, 0.03))
pr = cube.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(pr)
UsdPhysics.CollisionAPI.Apply(pr)
UsdPhysics.MassAPI.Apply(pr).CreateMassAttr().Set(0.5)
rb = PhysxSchema.PhysxRigidBodyAPI.Apply(pr)
rb.CreateEnableCCDAttr().Set(True)
rb.CreateSolverPositionIterationCountAttr().Set(32)
rb.CreateSolverVelocityIterationCountAttr().Set(8)
made.append((name, belt, path, x, y, top))
print(f"\nпроб создано: {len(made)}")
# --- прогон ----------------------------------------------------------------------------
paths = [m[2] for m in made]
tl.play()
await app_utils.update_app_async(steps=30) # осадка на ленте
rp = RigidPrim(paths=paths)
SAMPLES, EVERY = 26, 8
import time
traj = []
for i in range(SAMPLES):
pos, _ = rp.get_world_poses()
traj.append(pos.numpy().copy())
await app_utils.update_app_async(steps=EVERY)
tl.stop()
await app_utils.update_app_async(steps=5)
dt = EVERY / float(hz)
print(f"\nШАГ ВЫБОРКИ {dt*1000:.1f} мс, всего {SAMPLES} выборок ({SAMPLES*dt:.2f} с)\n")
print(f" {'проба':10s} {'дорожка':22s} {'путь,мм':>9s} {'|v|,м/с':>8s} {'напр.':>16s} {'оценка':>12s}")
print(" " + "-" * 88)
res = {}
for k, (name, belt, path, x0, y0, top) in enumerate(made):
P = [t[k] for t in traj]
# установившаяся скорость: по второй половине выборки, чтобы отбросить осадку
h = len(P) // 2
d = P[-1] - P[h]
span = (len(P) - 1 - h) * dt
v = d / span
sp = float((v[0]**2 + v[1]**2) ** 0.5)
total = float(((P[-1][0]-P[0][0])**2 + (P[-1][1]-P[0][1])**2) ** 0.5) * 1000
dz = float(P[-1][2] - P[0][2])
want = INTENT[belt]
wn = math.sqrt(want[0]**2 + want[1]**2) or 1
cosang = (v[0]*want[0] + v[1]*want[1]) / (sp * wn) if sp > 1e-4 else 0.0
if dz < -0.05:
verdict = "УПАЛ"
elif sp < 0.15:
verdict = "СТОИТ"
elif cosang < 0.7:
verdict = "НЕ ТУДА"
elif sp < 0.80 * SPEED:
verdict = "буксует"
else:
verdict = "ок"
print(f" {name:10s} {belt.split('/World/')[-1]:22s} {total:9.0f} {sp:8.2f} "
f"({v[0]:+.2f},{v[1]:+.2f}) {verdict:>12s}")
res[name] = dict(speed=round(sp, 3), dir=[round(float(v[0]), 3), round(float(v[1]), 3)],
dz=round(dz, 3), verdict=verdict)
ok = sum(1 for v in res.values() if v["verdict"] == "ок")
print(f"\n ИТОГ: {ok} из {len(res)} дорожек везут товар на {SPEED} м/с в нужную сторону")
globals()["BELT_RESULT"] = res
scripts/verify_belts2.py
+208
View File
@@ -0,0 +1,208 @@
"""Перенос трением на 1 м/с по всем семи дорожкам. Чистая последовательность.
Что выяснилось предыдущими прогонами и почему порядок именно такой:
* Значение surfaceVelocity записывается верно, но ОБНУЛЯЕТСЯ в течение 5 шагов после
play. Пишет ноль авторский узел ConveyorBeltGraph: собственной скорости он не несёт и
на каждом тике кладёт свою. SetActive(False) на преме графа этого не останавливает -
прем перестаёт обходиться, но уже собранный граф OmniGraph продолжает работать.
Поэтому узлы УДАЛЯЮТСЯ, а сцена переоткрывается, чтобы граф не пережил правку.
* Список лент в scene.py заканчивается на ConveyorTrack_05, а в этой сборке семь дорожек:
добавлена угловая ConveyorTrack_06 (поворот на 90 градусов), и исчезла корневая
/ConveyorTrack_01 - прежняя 45-градусная дорожка плуга, которую этот угол заменил.
* surfaceVelocity задаётся в ЛОКАЛЬНОЙ системе тела, а дорожки уложены по-разному:
у _04 и _06 локальный +X смотрит в мировой -X. Направление выводится из мировой цели,
величина делится на то, сколько мирового стоит одна локальная единица - у _06 масштаб
0.5, и без деления скорость вышла бы вдвое меньше.
Скорость меряется по фактическому перемещению тел ВО ВРЕМЯ прогона: stop() возвращает
сцену в исходное состояние, и замер после него показывает точки рождения независимо от
того, ехал товар или нет.
"""
import sys, math
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
SPEED = 1.0
SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd"
INTENT = {
"/World/ConveyorTrack_05/Belt": (-1, 0, 0),
"/World/ConveyorTrack/Belt": (-1, 0, 0),
"/World/ConveyorTrack_02/Belt": (-1, 0, 0),
"/World/ConveyorTrack_03/Belt": (-1, 0, 0),
"/World/ConveyorTrack_04/Belt": (-1, 0, 0),
"/World/ConveyorTrack_01/Belt": (-1, 0, 0),
"/World/ConveyorTrack_06/Belt": (0, 1, 0),
"/World/ConveyorTrack_03/Belt_01": (0, 1, 0),
}
# Пробы ставятся в НАЧАЛО своей секции по ходу движения: в прошлом прогоне они
# проезжали секцию за 1.2 с и упирались, а скорость я считал по второй половине окна -
# то есть уже по стоящему телу. Отсюда были ложные "стоит" при пройденных 1475 мм.
TESTS = [
("main_05", "/World/ConveyorTrack_05/Belt", (+1.85, 0.00)),
("main_00", "/World/ConveyorTrack/Belt", (-0.15, 0.00)),
("fork_03", "/World/ConveyorTrack_03/Belt", (-2.15, 0.00)),
("plow_04", "/World/ConveyorTrack_04/Belt", (-6.15, 0.00)),
("lane_01", "/World/ConveyorTrack_01/Belt", (-8.15, -0.22)),
("corner_06", "/World/ConveyorTrack_06/Belt", (-8.45, 0.12)),
("branch", "/World/ConveyorTrack_03/Belt_01", (-4.20, 0.55)),
]
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
# 1. переоткрыть - чтобы не остался собранный граф от прошлой правки
omni.usd.get_context().open_stage(SCENE)
await app_utils.update_app_async(steps=60)
stage = omni.usd.get_context().get_stage()
print("сцена переоткрыта:", stage.GetRootLayer().identifier)
# 2. УДАЛИТЬ узлы конвейерного графа (гасить недостаточно)
doomed = [p.GetPath() for p in stage.Traverse() if "ConveyorBeltGraph" in p.GetName()]
for path in doomed:
stage.RemovePrim(path)
print(f"удалено узлов ConveyorBeltGraph: {len(doomed)}")
anim = stage.GetPrimAtPath(plow_cell.ANIM_GRAPH)
if anim.IsValid():
stage.RemovePrim(anim.GetPath()); print("удалён DiverterAnimGraph (иначе перетирает приводы)")
await app_utils.update_app_async(steps=20)
# 3. физика
ps = next((p for p in stage.Traverse() if p.IsA(UsdPhysics.Scene)), None)
if ps is None:
ps = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim()
px = PhysxSchema.PhysxSceneAPI.Apply(ps)
hz = px.GetTimeStepsPerSecondAttr().Get() or 60
if hz < 120:
px.CreateTimeStepsPerSecondAttr().Set(120); hz = 120
px.CreateEnableCCDAttr().Set(True)
px.CreateEnableStabilizationAttr().Set(True)
print(f"физика: {ps.GetPath()}, {hz} Гц")
# 4. привод лент
GRIP = "/World/_BeltGrip"
g = stage.GetPrimAtPath(GRIP)
if not g.IsValid():
g = stage.DefinePrim(GRIP, "Material")
pm = UsdPhysics.MaterialAPI.Apply(g)
pm.CreateStaticFrictionAttr().Set(1.1)
pm.CreateDynamicFrictionAttr().Set(0.95)
pm.CreateRestitutionAttr().Set(0.02)
print(f"\nПРИВОД на {SPEED} м/с:")
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
surf = {}
for path, intent in INTENT.items():
pr = stage.GetPrimAtPath(path)
if not pr.IsValid():
print(f" {path}: НЕТ ПРЕМА"); continue
surf[path] = bb.ComputeWorldBound(pr).ComputeAlignedRange()
v = plow_cell.drive_belt(stage, path, intent, SPEED, grip_path=GRIP)
en = PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr()
en.Set(True)
M = UsdGeom.XformCache().GetLocalToWorldTransform(pr)
per = M.TransformDir(Gf.Vec3d(1, 0, 0)).GetLength()
print(f" {path.split('/World/')[-1]:26s} цель{str(intent):12s} локальная v={v}"
f" мир/локаль по X = {per:.3f}")
# 5. пробы
ROOT = "/World/_BeltProbe"
if stage.GetPrimAtPath(ROOT).IsValid():
stage.RemovePrim(ROOT)
stage.DefinePrim(ROOT, "Xform")
made = []
for name, belt, (x, y) in TESTS:
if belt not in surf:
continue
top = surf[belt].GetMax()[2]
path = f"{ROOT}/{name}"
cube = UsdGeom.Cube.Define(stage, path)
cube.CreateSizeAttr().Set(2.0)
xf = UsdGeom.Xformable(cube.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(x, y, top + 0.035))
xf.AddScaleOp().Set(Gf.Vec3f(0.03, 0.03, 0.03))
pp = cube.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(pp)
UsdPhysics.CollisionAPI.Apply(pp)
UsdPhysics.MassAPI.Apply(pp).CreateMassAttr().Set(0.5)
rb = PhysxSchema.PhysxRigidBodyAPI.Apply(pp)
rb.CreateEnableCCDAttr().Set(True)
rb.CreateSolverPositionIterationCountAttr().Set(32)
UsdShade.MaterialBindingAPI.Apply(pp).Bind(
UsdShade.Material(g), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
materialPurpose="physics")
made.append((name, belt, path))
print(f"проб: {len(made)}")
# 6. прогон
tl.play()
await app_utils.update_app_async(steps=40)
belt0 = stage.GetPrimAtPath("/World/ConveyorTrack/Belt")
print("контроль после play: surfaceVelocity =",
belt0.GetAttribute("physxSurfaceVelocity:surfaceVelocity").Get())
rp = RigidPrim(paths=[m[2] for m in made])
EVERY, N = 5, 26
# Время берётся ИЗ ТАЙМЛАЙНА. Заданная timeStepsPerSecond и фактический шаг физики
# могут расходиться, а от этого напрямую зависит вычисленная скорость: при ошибке
# вдвое лента "поедет" вдвое быстрее, ничего на самом деле не изменив.
traj, tstamp = [], []
for i in range(N):
traj.append(rp.get_world_poses()[0].numpy().copy())
tstamp.append(float(tl.get_current_time()))
await app_utils.update_app_async(steps=EVERY)
spans = [b - a for a, b in zip(tstamp[:-1], tstamp[1:])]
dt = sum(spans) / len(spans) if spans else EVERY / float(hz)
print(f"фактический шаг таймлайна {dt*1000:.2f} мс против {EVERY/float(hz)*1000:.2f} мс "
f"по заданным {hz} Гц")
print(f"\nвыборка каждые {dt*1000:.0f} мс, {N} точек ({N*dt:.2f} с)\n")
print(f" {'проба':10s} {'дорожка':24s} {'путь,мм':>8s} {'v уст.,м/с':>10s} {'направление':>18s} оценка")
print(" " + "-" * 90)
res = {}
for k, (name, belt, path) in enumerate(made):
P = [t[k] for t in traj]
# мгновенные скорости между соседними выборками
inst = []
for j, (a, b) in enumerate(zip(P[:-1], P[1:])):
h = spans[j] if j < len(spans) and spans[j] > 1e-6 else dt
dx, dy = float(b[0]-a[0]), float(b[1]-a[1])
inst.append((math.hypot(dx, dy)/h, dx/h, dy/h))
moving = [t for t in inst if t[0] > 0.15]
total = float(math.hypot(P[-1][0]-P[0][0], P[-1][1]-P[0][1])) * 1000
dz = float(min(p[2] for p in P) - P[0][2])
if not moving:
sp, vx, vy = 0.0, 0.0, 0.0
else:
# установившаяся: медиана верхней половины, чтобы отбросить разгон и упор
moving.sort(key=lambda t: t[0])
top = moving[len(moving)//2:]
sp = sorted(t[0] for t in top)[len(top)//2]
vx = sum(t[1] for t in top)/len(top)
vy = sum(t[2] for t in top)/len(top)
w = INTENT[belt]; wn = math.hypot(w[0], w[1]) or 1
cos = (vx*w[0] + vy*w[1]) / (sp*wn) if sp > 1e-3 else 0.0
if dz < -0.10: verdict = "УПАЛ"
elif sp < 0.15: verdict = "СТОИТ"
elif cos < 0.7: verdict = "НЕ ТУДА"
elif sp < 0.80 * SPEED: verdict = f"буксует {sp/SPEED*100:.0f}%"
else: verdict = f"ок ({sp/SPEED*100:.0f}%)"
print(f" {name:10s} {belt.split('/World/')[-1]:24s} {total:8.0f} {sp:10.2f} "
f"({vx:+.2f},{vy:+.2f}) {verdict}")
res[name] = dict(v=round(sp, 3), path_mm=round(total), verdict=verdict)
tl.stop()
await app_utils.update_app_async(steps=5)
ok = sum(1 for r in res.values() if r["verdict"].startswith("ок"))
print(f"\n ИТОГ: {ok} из {len(res)} дорожек везут на {SPEED} м/с в нужную сторону")
globals()["BELTS_OK"] = res
scripts/verify_plow.py
+187
View File
@@ -0,0 +1,187 @@
"""Отработка пушера и плуга на новой сцене: поворот по классу и скольжение товара по лезвию.
Опирается на выясненное прогонами лент:
* узлы ConveyorBeltGraph нужно УДАЛЯТЬ - деактивация не мешает уже собранному графу
обнулять surfaceVelocity на каждом тике;
* DiverterAnimGraph тоже удаляется: он переписывает цели приводов каждый тик и затирает
всё, что задаёт Python;
* время берётся из таймлайна - заданная частота физики не применяется, фактический шаг
83.33 мс (60 Гц), и от этого напрямую зависит вычисленная скорость.
Плуг ставится в положение ДО подхода товара - это и есть предпозиционирование по классу.
Скольжение по лезвию меряется как путь товара ВДОЛЬ кромки за время контакта: если товар
только отбрасывается, поперечная составляющая есть, а продольной нет.
"""
import sys, math
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
SPEED = 1.0
SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd"
INTENT = {
"/World/ConveyorTrack_05/Belt": (-1, 0, 0), "/World/ConveyorTrack/Belt": (-1, 0, 0),
"/World/ConveyorTrack_02/Belt": (-1, 0, 0), "/World/ConveyorTrack_03/Belt": (-1, 0, 0),
"/World/ConveyorTrack_04/Belt": (-1, 0, 0), "/World/ConveyorTrack_01/Belt": (-1, 0, 0),
"/World/ConveyorTrack_06/Belt": (0, 1, 0), "/World/ConveyorTrack_03/Belt_01": (0, 1, 0),
}
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
omni.usd.get_context().open_stage(SCENE)
await app_utils.update_app_async(steps=60)
stage = omni.usd.get_context().get_stage()
for path in [p.GetPath() for p in stage.Traverse()
if "ConveyorBeltGraph" in p.GetName() or "DiverterAnimGraph" in p.GetName()]:
stage.RemovePrim(path)
await app_utils.update_app_async(steps=20)
ps = next((p for p in stage.Traverse() if p.IsA(UsdPhysics.Scene)), None)
if ps is None:
ps = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim()
PhysxSchema.PhysxSceneAPI.Apply(ps).CreateEnableCCDAttr().Set(True)
GRIP = "/World/_BeltGrip"
g = stage.GetPrimAtPath(GRIP)
if not g.IsValid():
g = stage.DefinePrim(GRIP, "Material")
pm = UsdPhysics.MaterialAPI.Apply(g)
pm.CreateStaticFrictionAttr().Set(1.1); pm.CreateDynamicFrictionAttr().Set(0.95)
pm.CreateRestitutionAttr().Set(0.02)
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
for path, intent in INTENT.items():
pr = stage.GetPrimAtPath(path)
if pr.IsValid():
plow_cell.drive_belt(stage, path, intent, SPEED, grip_path=GRIP)
PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True)
TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_04/Belt")
).ComputeAlignedRange().GetMax()[2]
print(f"сцена готова, ленты на {SPEED} м/с, верх ленты z={TOP:.3f}")
# --- механизмы ------------------------------------------------------------------------
hinge = stage.GetPrimAtPath(C.PLOW_HINGE)
slide = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/PusherSlide")
print(f"плуг {C.PLOW_HINGE}: {hinge.IsValid()} пушер PusherSlide: {slide.IsValid()}")
hdrive = UsdPhysics.DriveAPI(hinge, "angular")
pdrive = UsdPhysics.DriveAPI(slide, "linear")
arm = stage.GetPrimAtPath(C.PLOW_ARM)
def yaw_of(prim):
M = UsdGeom.XformCache().GetLocalToWorldTransform(prim)
d = M.TransformDir(Gf.Vec3d(1, 0, 0))
return math.degrees(math.atan2(d[1], d[0]))
def spawn(name, x, y, size=0.05, mass=0.5):
path = f"/World/_Goods/{name}"
c = UsdGeom.Cube.Define(stage, path)
c.CreateSizeAttr().Set(2.0)
xf = UsdGeom.Xformable(c.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(x, y, TOP + size + 0.005))
xf.AddScaleOp().Set(Gf.Vec3f(size, size, size))
p = c.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(mass)
rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p)
rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32)
UsdShade.MaterialBindingAPI.Apply(p).Bind(
UsdShade.Material(g), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
materialPurpose="physics")
return path
async def run_case(label, plow_deg, start_x=-6.30, start_y=0.0, seconds=4.0):
"""поставить плуг ЗАРАНЕЕ, пустить товар, проследить его через плуг"""
if stage.GetPrimAtPath("/World/_Goods").IsValid():
stage.RemovePrim("/World/_Goods")
stage.DefinePrim("/World/_Goods", "Xform")
path = spawn(label, start_x, start_y)
if hdrive:
hdrive.GetTargetPositionAttr().Set(float(plow_deg))
hdrive.CreateStiffnessAttr().Set(C.PLOW_STIFFNESS)
hdrive.CreateDampingAttr().Set(C.PLOW_DAMPING)
hdrive.CreateMaxForceAttr().Set(C.PLOW_MAX_FORCE)
tl.play()
await app_utils.update_app_async(steps=45) # дать лезвию встать ДО подхода
reached = yaw_of(arm) if arm.IsValid() else None
rp = RigidPrim(paths=[path])
T, P = [], []
t_end = float(tl.get_current_time()) + seconds
while float(tl.get_current_time()) < t_end:
P.append(rp.get_world_poses()[0].numpy()[0].copy())
T.append(float(tl.get_current_time()))
await app_utils.update_app_async(steps=3)
tl.stop(); await app_utils.update_app_async(steps=5)
return label, plow_deg, reached, T, P
CASES = [("D_прямо", C.PLOW_PRESET["D"]), ("B_влево", C.PLOW_PRESET["B"]),
("C_вправо", C.PLOW_PRESET["C"]), ("B_широкий", C.PLOW_B_ANGLE)]
print(f"\nуглы из конфига: preset={C.PLOW_PRESET} PLOW_B_ANGLE={C.PLOW_B_ANGLE} "
f"PLOW_X={C.PLOW_X}\n")
print(f" {'случай':11s} {'цель°':>6s} {'факт°':>6s} {'смещ.Y,мм':>10s} {'скольж.,мм':>11s} "
f"{'конец X,Y':>16s} сторона")
print(" " + "-" * 84)
out = {}
for label, deg in CASES:
label, deg, reached, T, P = await run_case(label, deg)
if len(P) < 5:
print(f" {label:11s} нет данных"); continue
y0 = float(P[0][1]); dy = float(P[-1][1]) - y0
# скольжение по лезвию: путь ВДОЛЬ кромки за время контакта с зоной плуга
slide_mm, prev = 0.0, None
for p in P:
if C.PLOW_SWEEP_X1 >= float(p[0]) >= C.PLOW_SWEEP_X0:
if prev is not None:
a = math.radians(reached if reached is not None else deg)
ex, ey = math.cos(a), math.sin(a) # направление кромки
slide_mm += abs((float(p[0])-prev[0])*ex + (float(p[1])-prev[1])*ey) * 1000
prev = (float(p[0]), float(p[1]))
side = "+Y (B)" if dy > 0.05 else ("-Y (C)" if dy < -0.05 else "прямо")
print(f" {label:11s} {deg:6.1f} {(reached if reached is not None else float('nan')):6.1f} "
f"{dy*1000:10.0f} {slide_mm:11.0f} ({P[-1][0]:+6.2f},{P[-1][1]:+6.2f}) {side}")
out[label] = dict(target=deg, reached=reached, dy_mm=round(dy*1000),
slide_mm=round(slide_mm), end=[round(float(P[-1][0]), 2),
round(float(P[-1][1]), 2)], side=side)
# --- пушер ------------------------------------------------------------------------------
print("\nПУШЕР:")
if stage.GetPrimAtPath("/World/_Goods").IsValid():
stage.RemovePrim("/World/_Goods")
stage.DefinePrim("/World/_Goods", "Xform")
gpath = spawn("push_D", -3.20, 0.0)
blade = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher")
tl.play(); await app_utils.update_app_async(steps=30)
rp = RigidPrim(paths=[gpath])
b0 = bb.ComputeWorldBound(blade).ComputeAlignedRange().GetMidpoint() if blade.IsValid() else None
fired = False
T, P, BY = [], [], []
t_end = float(tl.get_current_time()) + 5.0
while float(tl.get_current_time()) < t_end:
p = rp.get_world_poses()[0].numpy()[0]
P.append(p.copy()); T.append(float(tl.get_current_time()))
if not fired and float(p[0]) <= C.PUSH_X + 0.10:
if pdrive:
pdrive.GetTargetPositionAttr().Set(0.45) # выдвинуть
fired = True
print(f" команда пушеру при x={float(p[0]):+.2f} (PUSH_X={C.PUSH_X})")
if blade.IsValid():
BY.append(float(bb.ComputeWorldBound(blade).ComputeAlignedRange().GetMidpoint()[1]))
await app_utils.update_app_async(steps=3)
tl.stop(); await app_utils.update_app_async(steps=5)
if P:
dy = float(P[-1][1]) - float(P[0][1])
stroke = (max(BY) - min(BY)) * 1000 if BY else 0.0
print(f" ход лезвия пушера {stroke:.0f} мм")
print(f" товар: старт ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> "
f"конец ({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f}), смещение по Y {dy*1000:+.0f} мм")
print(f" {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.15 else 'товар НЕ уведён'}")
out["pusher"] = dict(stroke_mm=round(stroke), dy_mm=round(dy*1000))
globals()["PLOW_RESULT"] = out
scripts/verify_plow2.py
+159
View File
@@ -0,0 +1,159 @@
"""Пушер и плуг новой сцены ШТАТНЫМ механизмом проекта.
Прошлый прогон был поставлен неверно: я командовал силовым приводом шарнира, а проект от
него отказался. configure_plow(kinematic_arm=True) делает лезвие КИНЕМАТИЧЕСКИМ, выключает
сам шарнир (physics:jointEnabled=False) и пишет угол напрямую - в комментарии сказано, что
привод перенастраивали трижды и он не держал, звеня на +-21.4 градуса быстрее, чем его
успевала вести команда. Нож пушера так же не ездит по своему призматическому суставу:
сустав выключен, а нож переносится записью трансформа (mechanics.Cell.blade_to).
Поэтому здесь всё идёт через plow_cell.prepare() + Plow + Cell.
Сверх штатного добавлено то, чего код проекта про эту сборку не знает:
* узлы ConveyorBeltGraph УДАЛЯЮТСЯ - deactivate недостаточно, уже собранный граф
продолжает обнулять surfaceVelocity на каждом тике;
* приводятся ConveyorTrack_05 и новая угловая ConveyorTrack_06: список лент в scene.py
заканчивается на _05 и седьмой дорожки не содержит.
Время берётся из таймлайна: заданная частота физики не применяется, фактический шаг
83.33 мс, и на предположении о 120 Гц скорости выходили ровно вдвое завышенными.
"""
import sys, math
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
from robozon_sorter.sim.plow import Plow
SPEED = 1.0
SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd"
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
omni.usd.get_context().open_stage(SCENE)
await app_utils.update_app_async(steps=60)
stage = omni.usd.get_context().get_stage()
killed = [p.GetPath() for p in stage.Traverse() if "ConveyorBeltGraph" in p.GetName()]
for path in killed:
stage.RemovePrim(path)
await app_utils.update_app_async(steps=10)
info = plow_cell.prepare(stage, belt_speed=SPEED, script_control=True, kinematic_arm=True)
print(f"prepare: плуг готов={info['plow_ready']}, лент приведено={len(info['belts'])}, "
f"скорость={info['belt_speed']} (узлов графа удалено {len(killed)})")
# дорожки, которых нет в списке проекта
for path, intent in (("/World/ConveyorTrack_05/Belt", (-1, 0, 0)),
("/World/ConveyorTrack_06/Belt", (0, 1, 0))):
pr = stage.GetPrimAtPath(path)
if pr.IsValid():
v = plow_cell.drive_belt(stage, path, intent, SPEED)
PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True)
print(f" дополнительно приведена {path.split('/World/')[-1]}: v={v}")
for path in plow_cell.BELTS + [plow_cell.BRANCH]:
pr = stage.GetPrimAtPath(path)
if pr.IsValid():
PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True)
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_04/Belt")
).ComputeAlignedRange().GetMax()[2]
GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL)
def spawn(name, x, y, size=0.05, mass=0.5):
path = f"/World/_Goods/{name}"
c = UsdGeom.Cube.Define(stage, path); c.CreateSizeAttr().Set(2.0)
xf = UsdGeom.Xformable(c.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(x, y, TOP + size + 0.005))
xf.AddScaleOp().Set(Gf.Vec3f(size, size, size))
p = c.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(mass)
rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p)
rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32)
if GRIP.IsValid():
UsdShade.MaterialBindingAPI.Apply(p).Bind(
UsdShade.Material(GRIP), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
materialPurpose="physics")
return path
# ---------- ПЛУГ: угол по классу, заранее ----------------------------------------------
plow = Plow(stage)
print(f"\nПЛУГ. углы по классам {C.PLOW_PRESET}, широкий B={C.PLOW_B_ANGLE}, "
f"ось плуга X={C.PLOW_X}, кинематическое лезвие")
print(f" {'класс':10s} {'цель°':>6s} {'угол лезвия°':>13s} {'смещ.Y,мм':>10s} "
f"{'скольж.,мм':>11s} {'конец X,Y':>16s} вывод")
print(" " + "-" * 92)
out = {}
for label, deg in (("D", C.PLOW_PRESET["D"]), ("B", C.PLOW_PRESET["B"]),
("C", C.PLOW_PRESET["C"]), ("B широкий", C.PLOW_B_ANGLE)):
if stage.GetPrimAtPath("/World/_Goods").IsValid():
stage.RemovePrim("/World/_Goods")
stage.DefinePrim("/World/_Goods", "Xform")
gp = spawn("item", -6.30, 0.0)
plow.target(deg) # ЗАРАНЕЕ, до подхода товара
tl.play()
await app_utils.update_app_async(steps=30)
reached = plow.angle
rp = RigidPrim(paths=[gp])
P, T = [], []
t_end = float(tl.get_current_time()) + 4.0
while float(tl.get_current_time()) < t_end:
P.append(rp.get_world_poses()[0].numpy()[0].copy())
T.append(float(tl.get_current_time()))
await app_utils.update_app_async(steps=3)
tl.stop(); await app_utils.update_app_async(steps=5)
y0, dy = float(P[0][1]), float(P[-1][1]) - float(P[0][1])
a = math.radians(reached)
ex, ey = math.cos(a), math.sin(a)
slide, prev = 0.0, None
for p in P:
if C.PLOW_SWEEP_X1 >= float(p[0]) >= C.PLOW_SWEEP_X0:
if prev is not None:
slide += abs((float(p[0])-prev[0])*ex + (float(p[1])-prev[1])*ey) * 1000
prev = (float(p[0]), float(p[1]))
side = "ушёл в +Y" if dy > 0.05 else ("ушёл в -Y" if dy < -0.05 else "прошёл прямо")
print(f" {label:10s} {deg:6.1f} {reached:13.1f} {dy*1000:10.0f} {slide:11.0f} "
f"({float(P[-1][0]):+6.2f},{float(P[-1][1]):+6.2f}) {side}")
out[label] = dict(target=deg, reached=round(reached, 1), dy_mm=round(dy*1000),
slide_mm=round(slide), end=[round(float(P[-1][0]), 2),
round(float(P[-1][1]), 2)])
# ---------- ПУШЕР ------------------------------------------------------------------------
print(f"\nПУШЕР. ход {C.BLADE_HOME_Y} -> {C.BLADE_OUT_Y} ({C.BLADE_STROKE*1000:.0f} мм), "
f"срабатывание у PUSH_X={C.PUSH_X}")
from robozon_sorter.sim.mechanics import Cell
if stage.GetPrimAtPath("/World/_Goods").IsValid():
stage.RemovePrim("/World/_Goods")
stage.DefinePrim("/World/_Goods", "Xform")
gp = spawn("push_D", -2.80, 0.0)
cell = Cell(stage, items={})
plow.target(C.PLOW_PRESET["D"])
tl.play(); await app_utils.update_app_async(steps=25)
rp = RigidPrim(paths=[gp])
P, fired, stroke_s = [], False, None
t_end = float(tl.get_current_time()) + 6.0
while float(tl.get_current_time()) < t_end:
p = rp.get_world_poses()[0].numpy()[0]
P.append(p.copy())
if not fired and float(p[0]) <= C.PUSH_X + 0.08:
print(f" товар дошёл до x={float(p[0]):+.2f} - ход ножа")
stroke_s = await cell.stroke(app_utils, out=True)
fired = True
await app_utils.update_app_async(steps=3)
tl.stop(); await app_utils.update_app_async(steps=5)
dy = float(P[-1][1]) - float(P[0][1])
print(f" ход ножа занял {stroke_s if stroke_s else 0:.2f} с")
print(f" товар: ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> "
f"({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f}), по Y {dy*1000:+.0f} мм")
print(f" {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.15 else 'товар НЕ уведён на ветку'}")
out["pusher"] = dict(dy_mm=round(dy*1000), fired=fired)
globals()["PLOW2"] = out
scripts/verify_pusher.py
+96
View File
@@ -0,0 +1,96 @@
"""Пушер после снятия коллизии с луча лазера.
Товар вставал на x = -3.044 и не доходил до точки срабатывания PUSH_X = -3.9. Причина
найдена в списке коллайдеров: /World/SortingRig/LaserGate/Beam - визуализация луча
датчика - имеет ВКЛЮЧЁННУЮ коллизию и перекрывает всю ширину полотна (y -0.42..+0.42
при ленте -0.45..+0.45) на высоте 19 мм над лентой. Датчик должен смотреть, а не
преграждать; коллизия с него снимается, после чего проверяется сам пушер.
"""
import sys, math
REPO = "/home/dasha/robozon-sorter"
if REPO not in sys.path:
sys.path.insert(0, REPO)
import omni.usd, omni.timeline
import isaacsim.core.experimental.utils.app as app_utils
from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim
from robozon_sorter import config as C
from robozon_sorter.sim import plow_cell
from robozon_sorter.sim.mechanics import Cell
from robozon_sorter.sim.plow import Plow
stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
tl.stop(); await app_utils.update_app_async(steps=10)
# снять коллизию со всей визуализации датчиков
off = []
for p in stage.Traverse():
path = str(p.GetPath())
if "LaserGate" in path or "AimRay" in path or "TriggerRay" in path:
a = p.GetAttribute("physics:collisionEnabled")
if a:
a.Set(False); off.append(path)
elif p.HasAPI(UsdPhysics.CollisionAPI):
UsdPhysics.CollisionAPI(p).CreateCollisionEnabledAttr().Set(False); off.append(path)
print(f"коллизия снята с {len(off)} премов визуализации датчиков:")
for o in off:
print(f" {o}")
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt")
).ComputeAlignedRange().GetMax()[2]
GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL)
if stage.GetPrimAtPath("/World/_Goods").IsValid():
stage.RemovePrim("/World/_Goods")
stage.DefinePrim("/World/_Goods", "Xform")
c = UsdGeom.Cube.Define(stage, "/World/_Goods/push_D"); c.CreateSizeAttr().Set(2.0)
xf = UsdGeom.Xformable(c.GetPrim())
xf.AddTranslateOp().Set(Gf.Vec3d(-2.40, 0.0, TOP + 0.055))
xf.AddScaleOp().Set(Gf.Vec3f(0.05, 0.05, 0.05))
p = c.GetPrim()
UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(0.5)
rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p)
rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32)
if GRIP.IsValid():
UsdShade.MaterialBindingAPI.Apply(p).Bind(
UsdShade.Material(GRIP), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
materialPurpose="physics")
cell = Cell(stage, items={})
plow = Plow(stage); plow.target(C.PLOW_PRESET["D"])
print(f"\nход ножа {C.BLADE_HOME_Y} -> {C.BLADE_OUT_Y} ({C.BLADE_STROKE*1000:.0f} мм), "
f"срабатывание у PUSH_X={C.PUSH_X}, скорость ножа {C.PUSHER_SPEED} м/с")
tl.play(); await app_utils.update_app_async(steps=25)
rp = RigidPrim(paths=["/World/_Goods/push_D"])
P, fired, t_str = [], False, None
t0 = float(tl.get_current_time()); t_end = t0 + 7.0
print("\n t,с x y событие")
while float(tl.get_current_time()) < t_end:
q = rp.get_world_poses()[0].numpy()[0]
P.append(q.copy())
t = float(tl.get_current_time())
ev = ""
if not fired and float(q[0]) <= C.PUSH_X + 0.08:
ev = "КОМАНДА ножу"
print(f" {t-t0:5.2f} {float(q[0]):+7.3f} {float(q[1]):+6.3f} {ev}")
t_str = await cell.stroke(app_utils, out=True)
fired = True
ev = ""
if len(P) % 8 == 1:
print(f" {t-t0:5.2f} {float(q[0]):+7.3f} {float(q[1]):+6.3f} {ev}")
await app_utils.update_app_async(steps=4)
tl.stop(); await app_utils.update_app_async(steps=5)
dy = float(P[-1][1]) - float(P[0][1])
dx = float(P[-1][0]) - float(P[0][0])
print(f"\n нож сработал: {fired}, ход занял {t_str if t_str else 0:.2f} с")
print(f" товар: ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> "
f"({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f})")
print(f" смещение: по X {dx*1000:+.0f} мм, по Y {dy*1000:+.0f} мм")
print(f" ВЫВОД: {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.20 else 'товар НЕ уведён на ветку'}")