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