Сортировочная ячейка Isaac Sim: CV-пайплайн и меши товаров
Замкнутый контур "поток -> CV -> механика": товары идут по конвейеру с шагом 700 мм, класс определяется стереопайплайном во время движения, пушер и плуг реагируют физически. Состав: * control_test/ - ячейка и CV. run_sorting_cv.py + cv_worker.py (два процесса, потому что torch внутри Isaac роняет сцену), cell.py (физика лент, плуга, пушера), measure_plane.py (замер габаритов), README.md и .memory.md с замерами, проблемами и ловушками * robozon_sorter/ - модули симуляции, scripts/ - утилиты, scene/ - сцены * assets/ - меши товаров, плуг, объекты Objaverse Бейзлайн CV: DEFOM-Stereo vitl, вход 480, iters 24, кроп зоны осмотра, без сегментации. На потоке 700 мм - классы 8/9, габариты MAE 32.8 мм, 469 мс на товар при такте 700 мс. Веса моделей (4.5 ГБ) и пропсы конвейера NVIDIA (274 МБ) не включены - источники и команды скачивания в MODELS.md. Выход прогонов (captures/, runtime/) не включён: воспроизводится. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
dasha_f
@@ -0,0 +1,168 @@
|
||||
"""Runtime side of the cell: releasing goods, the laser gate and the pusher stroke.
|
||||
|
||||
Two hard-won rules are encoded here and should not be "simplified" away:
|
||||
|
||||
1. During simulation, read poses from RigidPrim.get_world_poses(). BBoxCache / XformCache
|
||||
return the AUTHORED transform, so a moving item looks frozen and every gate misfires.
|
||||
2. The blade retracts only when (a) the pushed item has cleared the belt and (b) nothing
|
||||
else is inside the blade's footprint. Retracting blindly sweeps the blade back through
|
||||
the next item and knocks it over.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
from pxr import Gf, UsdGeom
|
||||
|
||||
from .. import config as C
|
||||
from .scene import BLADE as BLADE_PATH, ITEMS_ROOT, BLADE_PARENT_Y
|
||||
|
||||
|
||||
class Cell:
|
||||
def __init__(self, stage, items):
|
||||
self.stage = stage
|
||||
self.items = list(items)
|
||||
self._blade_op = self._blade_translate_op()
|
||||
self._blade_base = self._blade_op.Get()
|
||||
from isaacsim.core.experimental.prims import RigidPrim
|
||||
self._rp = {n: RigidPrim(paths=[f"{ITEMS_ROOT}/{n}"]) for n in self.items}
|
||||
from omni.physx import get_physx_scene_query_interface
|
||||
self._query = get_physx_scene_query_interface()
|
||||
self.blade_to(C.BLADE_HOME_Y)
|
||||
|
||||
# -- blade --------------------------------------------------------------
|
||||
def _blade_translate_op(self):
|
||||
prim = self.stage.GetPrimAtPath(BLADE_PATH)
|
||||
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
|
||||
return op
|
||||
raise RuntimeError(f"{BLADE_PATH} has no translate op to drive")
|
||||
|
||||
def blade_to(self, y):
|
||||
"""y is a WORLD coordinate; the op lives in the diverter's frame"""
|
||||
b = self._blade_base
|
||||
self._blade_op.Set(Gf.Vec3d(b[0], y - BLADE_PARENT_Y, b[2]))
|
||||
|
||||
async def stroke(self, app, out=True, speed=None):
|
||||
"""sweep the blade at a commanded m/s; fine steps keep the contact impulse sane.
|
||||
Above ~2.5 m/s the kinematic blade throws goods off the line."""
|
||||
speed = min(speed or C.PUSHER_SPEED, C.PUSHER_MAX_SAFE)
|
||||
a, b = (C.BLADE_HOME_Y, C.BLADE_OUT_Y) if out else (C.BLADE_OUT_Y, C.BLADE_HOME_Y)
|
||||
dt = 1.0 / 60.0
|
||||
steps = max(4, int(round(abs(b - a) / max(speed * dt, 1e-6))))
|
||||
for i in range(steps + 1):
|
||||
self.blade_to(a + (b - a) * i / steps)
|
||||
await app.update_app_async(steps=1)
|
||||
return steps * dt
|
||||
|
||||
# -- item state ---------------------------------------------------------
|
||||
def pose(self, name):
|
||||
return self._rp[name].get_world_poses()[0].numpy()[0]
|
||||
|
||||
def place(self, name, pos):
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}")
|
||||
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
|
||||
op.Set(Gf.Vec3d(*pos))
|
||||
return
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTransform:
|
||||
M = Gf.Matrix4d(op.Get())
|
||||
M.SetTranslateOnly(Gf.Vec3d(*pos))
|
||||
op.Set(M)
|
||||
return
|
||||
|
||||
def _underside_gap(self, name):
|
||||
"""distance from the prim origin down to its lowest point, so it can be seated"""
|
||||
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}")
|
||||
r = cache.ComputeWorldBound(prim).ComputeAlignedRange()
|
||||
if r.IsEmpty():
|
||||
return 0.0
|
||||
origin = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(0).ExtractTranslation()
|
||||
return origin[2] - r.GetMin()[2]
|
||||
|
||||
def park(self, name, index=0):
|
||||
self.place(name, (9.0 + 1.2 * index, 5.0, 0.4))
|
||||
|
||||
def park_all(self):
|
||||
"""park everything AND freeze it, so the queue does not fall out of the world.
|
||||
|
||||
Parked items are ordinary dynamic bodies sitting off to the side at y ~ +5, where
|
||||
there is no floor under them - so the whole undispatched queue free-falls for the
|
||||
entire run. Measured: parked stock at z = -665 after a minute and the stage bound
|
||||
reaching z = -20438, which also wrecks every "frame the whole scene" camera because
|
||||
the scene is suddenly 20 km tall. Freezing them costs nothing and they are woken in
|
||||
`release`, which clears the flag before placing the item on the belt.
|
||||
"""
|
||||
from pxr import UsdPhysics
|
||||
for i, n in enumerate(self.items):
|
||||
self.park(n, i)
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{n}")
|
||||
if prim.IsValid() and prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(True)
|
||||
|
||||
def _thaw(self, name):
|
||||
"""let a parked item fall under gravity again, just before it is released"""
|
||||
from pxr import UsdPhysics
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}")
|
||||
if prim.IsValid() and prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(False)
|
||||
|
||||
def release(self, name, y=0.0):
|
||||
self._thaw(name)
|
||||
self.place(name, (C.SPAWN_X, y, C.BELT_Z + 0.005))
|
||||
self.place(name, (C.SPAWN_X, y, C.BELT_Z + self._underside_gap(name) + 0.008))
|
||||
|
||||
# -- laser gate ---------------------------------------------------------
|
||||
def laser(self):
|
||||
"""name of whatever breaks the beam, else None. A real raycast, not a coordinate test."""
|
||||
hit = self._query.raycast_closest(
|
||||
[C.GATE_X, C.BEAM_Y0, C.BEAM_Z], [0.0, 1.0, 0.0], C.BEAM_Y1 - C.BEAM_Y0)
|
||||
if not hit or not hit.get("hit"):
|
||||
return None
|
||||
path = str(hit.get("rigidBody") or hit.get("collision") or "")
|
||||
for n in self.items:
|
||||
if f"{ITEMS_ROOT}/{n}" in path:
|
||||
return n
|
||||
return None
|
||||
|
||||
def blade_path_busy(self, exclude):
|
||||
for n in self.items:
|
||||
if n == exclude:
|
||||
continue
|
||||
p = self.pose(n)
|
||||
if (C.BLADE_X0 - 0.12 < p[0] < C.BLADE_X1 + 0.12) and (-0.30 < p[1] < 0.45):
|
||||
return n
|
||||
return None
|
||||
|
||||
async def divert(self, app, name, speed=None, max_wait_s=1.5):
|
||||
"""full push cycle with both retract interlocks"""
|
||||
dt = 1.0 / 60.0
|
||||
t = await self.stroke(app, out=True, speed=speed)
|
||||
for _ in range(int(max_wait_s / dt)): # item off the main line
|
||||
if self.pose(name)[1] > 0.50:
|
||||
break
|
||||
await app.update_app_async(steps=1)
|
||||
t += dt
|
||||
held = 0.0
|
||||
for _ in range(int(max_wait_s / dt)): # path clear for the return
|
||||
if self.blade_path_busy(name) is None:
|
||||
break
|
||||
await app.update_app_async(steps=1)
|
||||
t += dt
|
||||
held += dt
|
||||
t += await self.stroke(app, out=False, speed=speed)
|
||||
return t, held
|
||||
|
||||
# -- outcome ------------------------------------------------------------
|
||||
def where(self, name):
|
||||
"""bin / branch / line-end / line, from the simulated pose"""
|
||||
p = self.pose(name)
|
||||
if C.BIN_X0 < p[0] < C.BIN_X1 and C.BIN_Y0 < p[1] < C.BIN_Y1 and p[2] < C.BIN_LIP_Z:
|
||||
return "bin"
|
||||
if p[2] < C.BELT_Z - 0.35: # dropped off the end of the run
|
||||
return "line-end"
|
||||
if p[1] > 0.5:
|
||||
return "branch"
|
||||
if p[0] < C.MAIN_X0 + 0.35: # the run stops at MAIN_X0, not beyond it
|
||||
return "line-end"
|
||||
return "line"
|
||||
Reference in New Issue
Block a user