dasha_f
0d32f32db0
Замкнутый контур "поток -> 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>
271 lines
13 KiB
Python
271 lines
13 KiB
Python
"""ЗАМКНУТЫЙ КОНТУР: товары едут, CV определяет класс на лету, пушер и плуг реагируют.
|
||
|
||
Отличие от прежнего прогона с заранее заданными классами: класс здесь НЕ читается из
|
||
разметки, а приходит от стереопайплайна во время движения. Разметка используется только
|
||
для подсчёта ошибки в конце.
|
||
|
||
Как замыкается контур. torch внутри Isaac роняет процесс, поэтому CV живёт отдельным
|
||
процессом (cv_worker.py), а обмен идёт через каталог runtime/: при пересечении ворот
|
||
осмотра сцена рендерит шесть кадров и кладёт заявку, работник отвечает файлом с классом.
|
||
Времени хватает с большим запасом: от ворот (x = -0.750) до пушера (-3.900) товар едет
|
||
3.15 с, до плуга (-7.85) - 7.1 с, инференс - 469 мс.
|
||
|
||
Механика та же, что проверена раньше:
|
||
* пушер берёт класс D - нож двигается записью трансформа (Cell.blade_to), его призматический
|
||
сустав выключен, иначе сустав и скрипт тянут нож в разные стороны;
|
||
* плуг ставится ЗАРАНЕЕ по классу (B - в одну сторону, C - в другую), лезвие кинематическое,
|
||
шарнир выключен: силовой привод в этой сборке не держал угол и звенел.
|
||
"""
|
||
import sys, os, math, json, time, glob
|
||
CT = "/home/dasha/robozon-sorter/control_test"
|
||
REPO = "/home/dasha/robozon-sorter"
|
||
for p in (CT, REPO):
|
||
if p not in sys.path:
|
||
sys.path.insert(0, p)
|
||
|
||
import numpy as np
|
||
import omni.usd, omni.timeline
|
||
import omni.kit.viewport.utility as vp
|
||
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
|
||
import importlib
|
||
import cell as cellmod
|
||
importlib.reload(cellmod)
|
||
import classify as CL
|
||
import cam_configs as CC
|
||
from robozon_sorter import config as C
|
||
from robozon_sorter.sim.mechanics import Cell
|
||
from robozon_sorter.sim.plow import Plow
|
||
|
||
SPEED, PITCH = 1.0, 0.70
|
||
GATE_X = float(CC.TARGET[0])
|
||
RT = f"{CT}/runtime"
|
||
REQ, RES, SHOTS = f"{RT}/req", f"{RT}/res", f"{RT}/shots"
|
||
for d in (RT, REQ, RES, SHOTS):
|
||
os.makedirs(d, exist_ok=True)
|
||
for f in glob.glob(f"{REQ}/*") + glob.glob(f"{RES}/*"):
|
||
os.remove(f)
|
||
|
||
ITEMS = ["bag", "backpack", "lunchbox", "helmet", "pillow",
|
||
"detergent", "bucket", "box_400x400x300", "box_300x200x200"]
|
||
RIGS = ["RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"]
|
||
|
||
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(str(cellmod.SCENE))
|
||
await app_utils.update_app_async(steps=60)
|
||
stage = omni.usd.get_context().get_stage()
|
||
info = await cellmod.prepare(stage, belt_speed=SPEED, script_control=True)
|
||
print(f"ячейка готова: парковок обезврежено {info.get('parks_cleared')}, лент {len(info['belts'])}")
|
||
|
||
fps = stage.GetTimeCodesPerSecond() or 60.0
|
||
stage.SetEndTimeCode(stage.GetStartTimeCode() + fps * 180.0)
|
||
tl.set_end_time(float(stage.GetEndTimeCode()) / fps)
|
||
tl.set_looping(False)
|
||
|
||
lib = {i["name"]: i for i in CL.load_library(f"{CT}/items_flow") if "cls" in i}
|
||
bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
|
||
w = vp.get_active_viewport(); orig_cam = w.camera_path
|
||
|
||
ROOT = "/World/_CV"
|
||
if stage.GetPrimAtPath(ROOT).IsValid():
|
||
stage.RemovePrim(ROOT)
|
||
stage.DefinePrim(ROOT, "Xform")
|
||
|
||
|
||
def spawn(name, i):
|
||
"""товар из items_flow по ПРОВЕРЕННОМУ пути загрузки (как в flow_capture/run_pipeline).
|
||
|
||
Три вещи, которые я сделал неверно в первой версии и из-за которых поток не поехал:
|
||
меши items_flow УЖЕ в каталожном масштабе и УЖЕ посажены на z=0 (scale_items.py), так
|
||
что домасштабирование лишнее; коллайдеры в них УЖЕ есть, свои накладывать поверх нельзя;
|
||
и рождаться товар должен на парковке, а к ленте переноситься в момент выпуска.
|
||
"""
|
||
path = f"{ROOT}/{name}"
|
||
if stage.GetPrimAtPath(path).IsValid():
|
||
stage.RemovePrim(path)
|
||
prim = UsdGeom.Xform.Define(stage, path).GetPrim()
|
||
prim.GetReferences().ClearReferences()
|
||
prim.GetReferences().AddReference(lib[name]["path"])
|
||
xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder()
|
||
op = xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble)
|
||
op.Set(Gf.Vec3d(9.0 + 1.5 * i, 5.0, 0.4)) # парковка вне линии
|
||
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.CreateSleepThresholdAttr().Set(0.0)
|
||
px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION)
|
||
for d in Usd.PrimRange(prim):
|
||
if d.HasAPI(UsdPhysics.CollisionAPI):
|
||
pc = PhysxSchema.PhysxCollisionAPI.Apply(d)
|
||
pc.CreateContactOffsetAttr().Set(0.004)
|
||
pc.CreateRestOffsetAttr().Set(0.001)
|
||
im = UsdGeom.Imageable(d)
|
||
if im:
|
||
im.MakeVisible()
|
||
return prim, op
|
||
|
||
|
||
async def compose(prim):
|
||
for _ in range(4):
|
||
await app_utils.update_app_async(steps=2)
|
||
for d in Usd.PrimRange(prim):
|
||
im = UsdGeom.Imageable(d)
|
||
if im:
|
||
im.MakeVisible()
|
||
|
||
|
||
async def shoot(name):
|
||
"""шесть кадров в кроп-разрешении сцены; таймлайн на это время ставится на паузу"""
|
||
files = {}
|
||
was = tl.is_playing()
|
||
if was:
|
||
tl.pause()
|
||
for rig in RIGS:
|
||
for eye in ("Left", "Right"):
|
||
cam = f"/RigRS/{rig}_{eye}"
|
||
if not stage.GetPrimAtPath(cam).IsValid():
|
||
continue
|
||
w.camera_path = cam
|
||
await app_utils.update_app_async(steps=6)
|
||
f = f"{RT}/frames/{name}__{rig}_{eye}.png"
|
||
os.makedirs(os.path.dirname(f), exist_ok=True)
|
||
vp.capture_viewport_to_file(w, file_path=f)
|
||
await app_utils.update_app_async(steps=4)
|
||
files[f"{rig}_{eye}"] = f
|
||
w.camera_path = orig_cam
|
||
await app_utils.update_app_async(steps=3)
|
||
if was:
|
||
tl.play()
|
||
return files
|
||
|
||
|
||
print("ожидаю работника CV...")
|
||
for _ in range(600):
|
||
if os.path.exists(f"{RT}/worker_ready"):
|
||
break
|
||
await app_utils.update_app_async(steps=6)
|
||
print("работник готов:", os.path.exists(f"{RT}/worker_ready"))
|
||
|
||
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())
|
||
|
||
state = {} # name -> dict(prim, rp, shot, cls, pushed, plowed, gt)
|
||
order, nxt = [], 0.0
|
||
pusher_busy = -1.0
|
||
print(f"\nпоток: шаг {PITCH*1000:.0f} мм при {SPEED} м/с, ворота x={GATE_X:+.3f}, "
|
||
f"пушер x={C.PUSH_X:+.3f}, плуг x={C.PLOW_X:+.3f}\n")
|
||
|
||
while float(tl.get_current_time()) - t0 < 90.0:
|
||
t = float(tl.get_current_time()) - t0
|
||
if len(order) < len(ITEMS) and t >= nxt:
|
||
nm = ITEMS[len(order)]
|
||
prim, op = spawn(nm, len(order))
|
||
for _ in range(3):
|
||
await app_utils.update_app_async(steps=2) # ссылка компонуется не в тот же тик
|
||
op.Set(Gf.Vec3d(cellmod.ENTRY_X, cellmod.ENTRY_Y, C.BELT_Z + 0.02))
|
||
rp = RigidPrim(paths=[str(prim.GetPath())])
|
||
state[nm] = dict(prim=prim, rp=rp, shot=False, cls=None, pushed=False,
|
||
plowed=False, gt=lib[nm]["cls"], t_gate=None, t_cls=None)
|
||
order.append(nm); nxt += PITCH / SPEED
|
||
print(f" {t:5.2f}s выпущен {nm}")
|
||
|
||
# ворота осмотра: снять кадры и отправить заявку
|
||
for nm in order:
|
||
st = state[nm]
|
||
if st["shot"]:
|
||
continue
|
||
x = float(st["rp"].get_world_poses()[0].numpy()[0][0])
|
||
if x <= GATE_X + 0.02:
|
||
st["shot"] = True; st["t_gate"] = t
|
||
files = await shoot(nm)
|
||
json.dump(dict(name=nm, files=files), open(f"{REQ}/{nm}.json", "w"))
|
||
print(f" {t:5.2f}s {nm} на воротах x={x:+.3f} - заявка отправлена")
|
||
|
||
# ответы от работника
|
||
for f in glob.glob(f"{RES}/*.json"):
|
||
try:
|
||
a = json.load(open(f))
|
||
except Exception:
|
||
continue
|
||
nm = a["name"]
|
||
if nm in state and state[nm]["cls"] is None:
|
||
state[nm]["cls"] = a.get("cls")
|
||
state[nm]["dims"] = a.get("dims"); state[nm]["k"] = a.get("k")
|
||
state[nm]["t_cls"] = t
|
||
lat = (t - (state[nm]["t_gate"] or t))
|
||
print(f" {t:5.2f}s {nm} -> класс {a.get('cls')} (эталон {state[nm]['gt']}), "
|
||
f"dims={a.get('dims')}, задержка {lat:.2f}с, инференс {a.get('ms')}мс")
|
||
os.remove(f)
|
||
|
||
# пушер берёт класс D
|
||
if t > pusher_busy:
|
||
for nm in order:
|
||
st = state[nm]
|
||
if st["cls"] != "D" or st["pushed"]:
|
||
continue
|
||
x = float(st["rp"].get_world_poses()[0].numpy()[0][0])
|
||
if x <= C.PUSH_X + 0.06:
|
||
print(f" {t:5.2f}s ПУШЕР берёт {nm} (класс D) на x={x:+.3f}")
|
||
await cell.stroke(app_utils, out=True, speed=1.2)
|
||
await cell.stroke(app_utils, out=False, speed=1.5)
|
||
st["pushed"] = True
|
||
pusher_busy = float(tl.get_current_time()) - t0 + 0.2
|
||
break
|
||
|
||
# плуг ставится заранее по классу B/C
|
||
for nm in order:
|
||
st = state[nm]
|
||
if st["cls"] not in ("B", "C") or st["plowed"]:
|
||
continue
|
||
x = float(st["rp"].get_world_poses()[0].numpy()[0][0])
|
||
if x <= C.PLOW_X + 1.2:
|
||
ang = C.PLOW_PRESET[st["cls"]]
|
||
plow.target(ang)
|
||
st["plowed"] = True
|
||
print(f" {t:5.2f}s ПЛУГ на {ang:+.0f}° под {nm} (класс {st['cls']}) x={x:+.3f}")
|
||
|
||
await app_utils.update_app_async(steps=3)
|
||
|
||
# итог
|
||
print(f"\n {'товар':18s} {'эталон':>7s} {'предсказано':>12s} {'габариты, мм':>20s} "
|
||
f"{'пушер':>6s} {'плуг':>5s} {'конец X,Y':>16s}")
|
||
print(" " + "-" * 96)
|
||
rows = []
|
||
for nm in order:
|
||
st = state[nm]
|
||
q = st["rp"].get_world_poses()[0].numpy()[0]
|
||
ok = "верно" if st["cls"] == st["gt"] else "ОШИБКА"
|
||
print(f" {nm:18s} {st['gt']:>7s} {str(st['cls'])+' '+ok:>12s} "
|
||
f"{str(st.get('dims')):>20s} {'да' if st['pushed'] else '-':>6s} "
|
||
f"{'да' if st['plowed'] else '-':>5s} ({float(q[0]):+6.2f},{float(q[1]):+6.2f})")
|
||
rows.append(dict(name=nm, gt=st["gt"], pred=st["cls"], dims=st.get("dims"),
|
||
k=st.get("k"), pushed=st["pushed"], plowed=st["plowed"],
|
||
end=[round(float(q[0]), 2), round(float(q[1]), 2)],
|
||
lat=None if st["t_cls"] is None or st["t_gate"] is None
|
||
else round(st["t_cls"] - st["t_gate"], 2)))
|
||
got = [r for r in rows if r["pred"]]
|
||
acc = sum(1 for r in got if r["pred"] == r["gt"])
|
||
print(f"\n классов получено {len(got)}/{len(rows)}, верно {acc}/{len(got)}")
|
||
lats = [r["lat"] for r in got if r["lat"] is not None]
|
||
if lats:
|
||
print(f" задержка от ворот до класса: медиана {np.median(lats):.2f} с "
|
||
f"(до пушера {abs(C.PUSH_X-GATE_X)/SPEED:.2f} с, до плуга {abs(C.PLOW_X-GATE_X)/SPEED:.2f} с)")
|
||
json.dump(rows, open(f"{RT}/sorting_cv.json", "w"), indent=1, ensure_ascii=False)
|
||
|
||
# обзорный кадр
|
||
w.camera_path = orig_cam
|
||
await app_utils.update_app_async(steps=30)
|
||
vp.capture_viewport_to_file(w, file_path=f"{SHOTS}/overview.png")
|
||
await app_utils.update_app_async(steps=30)
|
||
tl.stop(); await app_utils.update_app_async(steps=5)
|
||
print(f"\n -> {RT}/sorting_cv.json, кадры в {SHOTS}")
|