"""Почему лента не тянет: сырой прогон одной дорожки с печатью каждого шага.

Предыдущий вывод "стоит/упал" был ложным - я снимал положения ПОСЛЕ stop(), который
возвращает сцену в исходное состояние, поэтому все тела оказались в точках рождения
независимо от того, ехали они или нет. Здесь положение печатается ВО ВРЕМЯ прогона.

Первый подозреваемый - surfaceVelocityEnabled: PhysxSurfaceVelocityAPI можно применить
и задать вектор, но без включённого флага он не действует, и внешне это неотличимо от
нехватки трения.
"""
import sys
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, UsdPhysics, PhysxSchema, UsdShade
from isaacsim.core.experimental.prims import RigidPrim

stage = omni.usd.get_context().get_stage()
tl = omni.timeline.get_timeline_interface()
if tl.is_playing():
    tl.stop(); await app_utils.update_app_async(steps=10)

BELT = "/World/ConveyorTrack/Belt"
PROBE = "/World/_BeltProbe/main_00"

pr = stage.GetPrimAtPath(BELT)
print("=== состояние привода ленты ===")
for a in pr.GetAttributes():
    n = a.GetName()
    if "urfaceVelocity" in n or "kinematic" in n.lower():
        print(f"  {n} = {a.Get()}")
print("  применённые схемы:", [s for s in pr.GetAppliedSchemas()])

# включить флаг явно
api = PhysxSchema.PhysxSurfaceVelocityAPI.Apply(pr)
en = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled")
if not en or not en.IsValid():
    en = api.CreateSurfaceVelocityEnabledAttr()
en.Set(True)
print("  surfaceVelocityEnabled выставлен в True")

# трение: без материала на ленте тянуть нечем
MAT = "/World/_TestGrip"
m = stage.GetPrimAtPath(MAT)
if not m.IsValid():
    m = stage.DefinePrim(MAT, "Material")
pm = UsdPhysics.MaterialAPI.Apply(m)
pm.CreateStaticFrictionAttr().Set(1.1)
pm.CreateDynamicFrictionAttr().Set(0.95)
pm.CreateRestitutionAttr().Set(0.0)
for target in (BELT, PROBE):
    t = stage.GetPrimAtPath(target)
    if t.IsValid():
        UsdShade.MaterialBindingAPI.Apply(t).Bind(
            UsdShade.Material(m), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
            materialPurpose="physics")
print(f"  трение 1.1/0.95 привязано к ленте и к пробе")

probe = stage.GetPrimAtPath(PROBE)
print(f"  проба существует: {probe.IsValid()}")

tl.play()
await app_utils.update_app_async(steps=20)
rp = RigidPrim(paths=[PROBE])
print("\n=== ВО ВРЕМЯ прогона ===")
print("   шаг     x        y        z      dx за шаг")
prev = None
for i in range(12):
    pos, _ = rp.get_world_poses()
    p = pos.numpy()[0]
    d = "-" if prev is None else f"{(p[0]-prev)*1000:+7.1f} мм"
    print(f"   {i*10:4d}  {p[0]:+7.3f}  {p[1]:+7.3f}  {p[2]:+7.3f}   {d}")
    prev = p[0]
    await app_utils.update_app_async(steps=10)
tl.stop()
await app_utils.update_app_async(steps=5)