Сортировочная ячейка 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,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)}")
|
||||
Reference in New Issue
Block a user