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>
160 lines
8.7 KiB
Python
160 lines
8.7 KiB
Python
"""Пушер и плуг новой сцены ШТАТНЫМ механизмом проекта.
|
||
|
||
Прошлый прогон был поставлен неверно: я командовал силовым приводом шарнира, а проект от
|
||
него отказался. configure_plow(kinematic_arm=True) делает лезвие КИНЕМАТИЧЕСКИМ, выключает
|
||
сам шарнир (physics:jointEnabled=False) и пишет угол напрямую - в комментарии сказано, что
|
||
привод перенастраивали трижды и он не держал, звеня на +-21.4 градуса быстрее, чем его
|
||
успевала вести команда. Нож пушера так же не ездит по своему призматическому суставу:
|
||
сустав выключен, а нож переносится записью трансформа (mechanics.Cell.blade_to).
|
||
Поэтому здесь всё идёт через plow_cell.prepare() + Plow + Cell.
|
||
|
||
Сверх штатного добавлено то, чего код проекта про эту сборку не знает:
|
||
* узлы ConveyorBeltGraph УДАЛЯЮТСЯ - deactivate недостаточно, уже собранный граф
|
||
продолжает обнулять surfaceVelocity на каждом тике;
|
||
* приводятся ConveyorTrack_05 и новая угловая ConveyorTrack_06: список лент в scene.py
|
||
заканчивается на _05 и седьмой дорожки не содержит.
|
||
|
||
Время берётся из таймлайна: заданная частота физики не применяется, фактический шаг
|
||
83.33 мс, и на предположении о 120 Гц скорости выходили ровно вдвое завышенными.
|
||
"""
|
||
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
|
||
from robozon_sorter.sim.plow import Plow
|
||
|
||
SPEED = 1.0
|
||
SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd"
|
||
|
||
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()]
|
||
for path in killed:
|
||
stage.RemovePrim(path)
|
||
await app_utils.update_app_async(steps=10)
|
||
|
||
info = plow_cell.prepare(stage, belt_speed=SPEED, script_control=True, kinematic_arm=True)
|
||
print(f"prepare: плуг готов={info['plow_ready']}, лент приведено={len(info['belts'])}, "
|
||
f"скорость={info['belt_speed']} (узлов графа удалено {len(killed)})")
|
||
|
||
# дорожки, которых нет в списке проекта
|
||
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():
|
||
v = plow_cell.drive_belt(stage, path, intent, SPEED)
|
||
PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True)
|
||
print(f" дополнительно приведена {path.split('/World/')[-1]}: v={v}")
|
||
for path in plow_cell.BELTS + [plow_cell.BRANCH]:
|
||
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_04/Belt")
|
||
).ComputeAlignedRange().GetMax()[2]
|
||
GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL)
|
||
|
||
def spawn(name, x, y, size=0.05, mass=0.5):
|
||
path = f"/World/_Goods/{name}"
|
||
c = UsdGeom.Cube.Define(stage, path); c.CreateSizeAttr().Set(2.0)
|
||
xf = UsdGeom.Xformable(c.GetPrim())
|
||
xf.AddTranslateOp().Set(Gf.Vec3d(x, y, TOP + size + 0.005))
|
||
xf.AddScaleOp().Set(Gf.Vec3f(size, size, size))
|
||
p = c.GetPrim()
|
||
UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p)
|
||
UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(mass)
|
||
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
|
||
|
||
# ---------- ПЛУГ: угол по классу, заранее ----------------------------------------------
|
||
plow = Plow(stage)
|
||
print(f"\nПЛУГ. углы по классам {C.PLOW_PRESET}, широкий B={C.PLOW_B_ANGLE}, "
|
||
f"ось плуга X={C.PLOW_X}, кинематическое лезвие")
|
||
print(f" {'класс':10s} {'цель°':>6s} {'угол лезвия°':>13s} {'смещ.Y,мм':>10s} "
|
||
f"{'скольж.,мм':>11s} {'конец X,Y':>16s} вывод")
|
||
print(" " + "-" * 92)
|
||
out = {}
|
||
for label, deg in (("D", C.PLOW_PRESET["D"]), ("B", C.PLOW_PRESET["B"]),
|
||
("C", C.PLOW_PRESET["C"]), ("B широкий", C.PLOW_B_ANGLE)):
|
||
if stage.GetPrimAtPath("/World/_Goods").IsValid():
|
||
stage.RemovePrim("/World/_Goods")
|
||
stage.DefinePrim("/World/_Goods", "Xform")
|
||
gp = spawn("item", -6.30, 0.0)
|
||
plow.target(deg) # ЗАРАНЕЕ, до подхода товара
|
||
tl.play()
|
||
await app_utils.update_app_async(steps=30)
|
||
reached = plow.angle
|
||
rp = RigidPrim(paths=[gp])
|
||
P, T = [], []
|
||
t_end = float(tl.get_current_time()) + 4.0
|
||
while float(tl.get_current_time()) < t_end:
|
||
P.append(rp.get_world_poses()[0].numpy()[0].copy())
|
||
T.append(float(tl.get_current_time()))
|
||
await app_utils.update_app_async(steps=3)
|
||
tl.stop(); await app_utils.update_app_async(steps=5)
|
||
y0, dy = float(P[0][1]), float(P[-1][1]) - float(P[0][1])
|
||
a = math.radians(reached)
|
||
ex, ey = math.cos(a), math.sin(a)
|
||
slide, prev = 0.0, None
|
||
for p in P:
|
||
if C.PLOW_SWEEP_X1 >= float(p[0]) >= C.PLOW_SWEEP_X0:
|
||
if prev is not None:
|
||
slide += abs((float(p[0])-prev[0])*ex + (float(p[1])-prev[1])*ey) * 1000
|
||
prev = (float(p[0]), float(p[1]))
|
||
side = "ушёл в +Y" if dy > 0.05 else ("ушёл в -Y" if dy < -0.05 else "прошёл прямо")
|
||
print(f" {label:10s} {deg:6.1f} {reached:13.1f} {dy*1000:10.0f} {slide:11.0f} "
|
||
f"({float(P[-1][0]):+6.2f},{float(P[-1][1]):+6.2f}) {side}")
|
||
out[label] = dict(target=deg, reached=round(reached, 1), dy_mm=round(dy*1000),
|
||
slide_mm=round(slide), end=[round(float(P[-1][0]), 2),
|
||
round(float(P[-1][1]), 2)])
|
||
|
||
# ---------- ПУШЕР ------------------------------------------------------------------------
|
||
print(f"\nПУШЕР. ход {C.BLADE_HOME_Y} -> {C.BLADE_OUT_Y} ({C.BLADE_STROKE*1000:.0f} мм), "
|
||
f"срабатывание у PUSH_X={C.PUSH_X}")
|
||
from robozon_sorter.sim.mechanics import Cell
|
||
if stage.GetPrimAtPath("/World/_Goods").IsValid():
|
||
stage.RemovePrim("/World/_Goods")
|
||
stage.DefinePrim("/World/_Goods", "Xform")
|
||
gp = spawn("push_D", -2.80, 0.0)
|
||
cell = Cell(stage, items={})
|
||
plow.target(C.PLOW_PRESET["D"])
|
||
tl.play(); await app_utils.update_app_async(steps=25)
|
||
rp = RigidPrim(paths=[gp])
|
||
P, fired, stroke_s = [], False, None
|
||
t_end = float(tl.get_current_time()) + 6.0
|
||
while float(tl.get_current_time()) < t_end:
|
||
p = rp.get_world_poses()[0].numpy()[0]
|
||
P.append(p.copy())
|
||
if not fired and float(p[0]) <= C.PUSH_X + 0.08:
|
||
print(f" товар дошёл до x={float(p[0]):+.2f} - ход ножа")
|
||
stroke_s = await cell.stroke(app_utils, out=True)
|
||
fired = True
|
||
await app_utils.update_app_async(steps=3)
|
||
tl.stop(); await app_utils.update_app_async(steps=5)
|
||
dy = float(P[-1][1]) - float(P[0][1])
|
||
print(f" ход ножа занял {stroke_s if stroke_s else 0:.2f} с")
|
||
print(f" товар: ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> "
|
||
f"({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f}), по Y {dy*1000:+.0f} мм")
|
||
print(f" {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.15 else 'товар НЕ уведён на ветку'}")
|
||
out["pusher"] = dict(dy_mm=round(dy*1000), fired=fired)
|
||
globals()["PLOW2"] = out
|