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