"""Перенос трением на 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