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