"""Loads scene/plow_cell.usd - the bare mechanical cell: conveyors, the Y-split pusher and the plow, with no camera portal, no laser gate and no item library. This is the transfer of the authored 90_degree.usd build (see scripts/build_plow_cell.py). It is deliberately the *mechanics only*: cameras, speed scenarios and laser sensors are added on top of it later, and keeping them out means the belts and the plow can be brought up and watched without a vision stack attached. Two ways to run it: * **as authored** - open the scene and press Play. The scene's own ``DiverterAnimGraph`` script node sweeps the pusher and the plow on a fixed loop. Nothing else is needed; this is what the file looks like when it was built. * **under script control** - ``prepare(stage, script_control=True)`` switches that graph off and hands the plow to :class:`sim.plow.Plow`. The graph has to go: it rewrites the drive targets every tick and would overwrite anything Python commands. The belts are driven the same way as in the sorter scene - explicit ``PhysxSurfaceVelocityAPI`` on kinematic slabs - because the authored ``ConveyorBeltGraph`` nodes carry no speed of their own and only fight the explicit setting. """ from __future__ import annotations from pathlib import Path from pxr import Gf, PhysxSchema, Usd, UsdGeom, UsdPhysics, UsdShade from .. import config as C from . import scene as _scene SCENE = C.ROOT / "scene" / "plow_cell.usd" # Same belt topology as the sorter scene - sorter.usd was exported from the same build. BELTS = _scene.BELTS BRANCH = _scene.BRANCH ANIM_GRAPH = "/World/Diverters/DiverterAnimGraph" GRIP_MATERIAL = "/World/PlowCell/M_beltPhysics" # In the authored build this second ConveyorTrack_01 at stage root was a leftover duplicate # and `prepare()` switched it off. `scripts/place_plow_lanes.py` then moved it out to -Y and # made it **the plow's -Y sorting lane**, so switching it off now removes half the sorter and # everything the plow deflects that way drops through the gap. It stays active by default; # `deactivate_stray=True` is kept only for opening the pre-lanes scene. STRAY_TRACK = "/ConveyorTrack_01" def drive_belt(stage, path, world_dir, speed, grip_path=GRIP_MATERIAL): """carry goods along `world_dir` (a WORLD direction), whatever the belt's own frame is. `surfaceVelocity` is expressed in the body's **local** frame, and this build does not lay every track the same way round: measured on the authored scene, local +X maps to ConveyorTrack, _02, _03, _05 -> world +X ConveyorTrack_04 -> world -X (the run through the plow) ConveyorTrack_03/Belt_01 -> world -Y (the branch) /ConveyorTrack_01 -> world -Y (plow lane, -Y side) /World/ConveyorTrack_01 -> world (-0.71, +0.71) (plow lane, +Y side, 45 deg) So a hard-coded sign is right for four belts and backwards for the fifth. Driving ConveyorTrack_04 backwards is what made goods stop dead at x = -6.0: they arrive moving -X, meet a belt pushing +X, and balance on the transfer jittering in place. It reads exactly like a blocked junction, which is the wrong thing to go and fix. Resolve the axis instead of assuming it. """ prim = stage.GetPrimAtPath(path) if not prim.IsValid(): return None if not prim.HasAPI(UsdPhysics.RigidBodyAPI): UsdPhysics.RigidBodyAPI.Apply(prim) UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(True) world = Gf.Vec3d(*world_dir) world = world / (world.GetLength() or 1.0) M = UsdGeom.XformCache().GetLocalToWorldTransform(prim) local = M.GetInverse().TransformDir(world) n = local.GetLength() or 1.0 local = local / n # unit direction in the body's own frame # Scale the MAGNITUDE by what one local unit is worth in world, not by 1. A track with # a non-unit scale shrinks the velocity on its way back out: ConveyorTrack_04 carries # scale (0.5, 1, 1), so a local 0.8 came out as 0.40 m/s in world - the main run was # feeding the fork at half the speed the branches were pulling away at, and goods hung # on the boundary with nothing behind them. Direction was right; only the magnitude was # wrong, which is why checking the sign alone missed it twice. per_unit = M.TransformDir(local).GetLength() or 1.0 local = Gf.Vec3f(*(local * (speed / per_unit))) PhysxSchema.PhysxSurfaceVelocityAPI.Apply(prim) PhysxSchema.PhysxSurfaceVelocityAPI(prim).CreateSurfaceVelocityAttr().Set(local) grip = stage.GetPrimAtPath(grip_path) if grip.IsValid(): api = UsdShade.MaterialBindingAPI.Apply(prim) api.Bind(UsdShade.Material(grip), bindingStrength=UsdShade.Tokens.strongerThanDescendants, materialPurpose="physics") return tuple(round(v, 3) for v in local) def configure_belts(stage, speed=None): """drive every belt of the plow cell by its intended WORLD direction""" speed = speed if speed is not None else C.BELT_SPEED grip = stage.GetPrimAtPath(GRIP_MATERIAL) if not grip.IsValid(): grip = stage.DefinePrim(GRIP_MATERIAL, "Material") pm = UsdPhysics.MaterialAPI.Apply(grip) pm.CreateStaticFrictionAttr().Set(1.1) pm.CreateDynamicFrictionAttr().Set(0.95) pm.CreateRestitutionAttr().Set(0.02) driven = {} for path in BELTS: # the whole main run travels -X v = drive_belt(stage, path, (-1, 0, 0), speed) if v: driven[path] = v v = drive_belt(stage, BRANCH, (0, 1, 0), speed) # the pusher's branch, toward the bin if v: driven[BRANCH] = v for track in ("ConveyorTrack", "ConveyorTrack_01", "ConveyorTrack_02", "ConveyorTrack_03", "ConveyorTrack_04", "ConveyorTrack_05"): for graph in (f"/World/{track}/ConveyorBeltGraph", f"/World/{track}/ConveyorBeltGraph_01"): g = stage.GetPrimAtPath(graph) if g.IsValid(): g.SetActive(False) return driven def open_scene(usd_path: str | Path | None = None): import omni.usd path = str(usd_path or SCENE) if not Path(path).exists(): raise FileNotFoundError( f"{path} not found. Build it with scripts/build_plow_cell.py; the conveyor art " "it references lives in assets/conveyors/ - run scripts/fetch_assets.py if that " "folder is empty." ) omni.usd.get_context().open_stage(path) return omni.usd.get_context().get_stage() def _friction_material(stage, path, static_f, dynamic_f, bind_to=(), restitution=0.0): """author a physics material and bind it, physics-purpose, to the given prims. Binding is `strongerThanDescendants` so it beats the belt grip material that configure_belts() puts on the same belt - the plow section wants to be slippery even though every carrying section wants to grip. """ prim = stage.GetPrimAtPath(path) if not prim.IsValid(): prim = stage.DefinePrim(path, "Material") m = UsdPhysics.MaterialAPI.Apply(prim) m.CreateStaticFrictionAttr().Set(float(static_f)) m.CreateDynamicFrictionAttr().Set(float(dynamic_f)) m.CreateRestitutionAttr().Set(float(restitution)) mat = UsdShade.Material(prim) bound = [] for target in bind_to: t = stage.GetPrimAtPath(target) if not t.IsValid(): continue api = UsdShade.MaterialBindingAPI.Apply(t) api.Bind(mat, bindingStrength=UsdShade.Tokens.strongerThanDescendants, materialPurpose="physics") bound.append(target) return bound def configure_plow(stage, script_control: bool = True, kinematic_arm: bool = True): """make the plow controllable and put its arm at rest. The arm is a dynamic body with gravity disabled, held only by the hinge drive, so a scene that opens with a stale target has the blade already leaning into the lane. """ hinge = stage.GetPrimAtPath(C.PLOW_HINGE) if not hinge.IsValid(): raise RuntimeError(f"{C.PLOW_HINGE} missing - is this plow_cell.usd?") if script_control: graph = stage.GetPrimAtPath(ANIM_GRAPH) if graph.IsValid(): graph.SetActive(False) drive = UsdPhysics.DriveAPI(hinge, "angular") if drive: drive.GetTargetPositionAttr().Set(0.0) drive.GetTargetVelocityAttr().Set(0.0) base = stage.GetPrimAtPath(C.PLOW_BASE) if base.IsValid() and base.HasAPI(UsdPhysics.RigidBodyAPI): UsdPhysics.RigidBodyAPI(base).CreateKinematicEnabledAttr().Set(True) # The arm is thin and sweeps into cargo, so it penetrates deeply in a single step. # Uncapped, PhysX separates that overlap at whatever speed it likes and the item leaves # the cell at several m/s. Cap the separation and give the arm the solver iterations to # resolve the contact properly instead. # A plough leads goods across only if they can slide - along the blade, and sideways # over the belt. Both surfaces are given friction here; see config for the measurement # that made it necessary (goods piled against the blade and stopped). _friction_material(stage, "/World/PlowCell/M_bladeFace", *C.PLOW_BLADE_FRICTION, bind_to=[C.PLOW_ARM]) _friction_material(stage, "/World/PlowCell/M_plowSection", *C.PLOW_SECTION_FRICTION, bind_to=C.PLOW_SECTION_PLATES) # The pedestal is a WALL across the belt: measured x[-7.02,-6.98] y[-0.54,+0.54] # z[+1.72,+2.56], against a belt of y[-0.45,+0.45] - it spans the full width and stands # 780 mm proud of the deck, with collision on. Goods arrive at the full 0.80 m/s, hit it # at x = -6.98 and stop, whatever the blade is doing and wherever they have been nudged # to. That is the "does not move on after being displaced" symptom, and it is not the # arm: the arm is 180 mm wide and lies along the flow. # # The pedestal is structure, not a working surface - only the blade should ever touch # cargo, and the blade carries its own collider. Its collision is switched off. for base_prim in Usd.PrimRange(stage.GetPrimAtPath(C.PLOW_BASE)): a = base_prim.GetAttribute("physics:collisionEnabled") if a: a.Set(False) elif base_prim.HasAPI(UsdPhysics.CollisionAPI): UsdPhysics.CollisionAPI(base_prim).CreateCollisionEnabledAttr().Set(False) # The pedestal is authored with `physics:approximation = "convexHull"`. A convex hull is # the smallest convex volume enclosing every vertex, so every opening in the frame is # filled in: what looks like a gantry you can see through is, to PhysX, a solid brick - # measured y[-0.54,+0.54] z[+1.72,+2.56] against a belt of y[-0.45,+0.45]. Goods arrive # at the full 0.80 m/s, hit it at x = -6.98 and stop, wherever they have been nudged to. # Transparency is a shader property and has nothing to do with it. # # Switching the approximation to the mesh itself keeps the frame in the simulation as # real structure - its posts still collide - while the opening becomes a genuine # opening. Triangle-mesh colliders are legal here because the pedestal is kinematic. # The conveyor line itself is untouched. for base_prim in Usd.PrimRange(stage.GetPrimAtPath(C.PLOW_BASE)): if base_prim.HasAPI(UsdPhysics.MeshCollisionAPI): UsdPhysics.MeshCollisionAPI(base_prim).CreateApproximationAttr().Set("none") arm = stage.GetPrimAtPath(C.PLOW_ARM) if arm.IsValid(): px = PhysxSchema.PhysxRigidBodyAPI.Apply(arm) px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) px.CreateSolverPositionIterationCountAttr().Set(32) px.CreateSolverVelocityIterationCountAttr().Set(8) if kinematic_arm: # Turn the arm directly instead of asking the force drive to hold an angle. # The drive was tuned three times and never held: at stiffness 3000 the arm # rang between +-21.4 deg at 99 deg/s, faster than the 76.4 deg/s ramp that was # commanding it. Kinematic, it goes exactly where sim/plow.py puts it. UsdPhysics.RigidBodyAPI(arm).CreateKinematicEnabledAttr().Set(True) # CCD is invalid on a body that is ever kinematic - PhysX errors on it. px.CreateEnableCCDAttr().Set(False) # The hinge has to go too, or it drags the arm back toward its own drive target # every step while the script writes the transform somewhere else - the same # fight that made the pusher blade jitter for a whole run. hinge.GetAttribute("physics:jointEnabled").Set(False) else: px.CreateEnableCCDAttr().Set(True) return drive is not None def deactivate_stray(stage): prim = stage.GetPrimAtPath(STRAY_TRACK) if prim.IsValid() and prim.IsActive(): prim.SetActive(False) return True return False def prepare(stage, belt_speed=None, script_control: bool = True, deactivate_stray_track: bool = False, kinematic_arm: bool = True): """everything the authored scene needs before the belts and the plow will run""" _scene.configure_physics(stage) belts = configure_belts(stage, belt_speed) stray = deactivate_stray(stage) if deactivate_stray_track else False plow = configure_plow(stage, script_control, kinematic_arm) # The Y-split blade is moved by writing its transform (mechanics.Cell.blade_to). Its # authored PhysicsPrismaticJoint has to be switched off first or the two fight: the # joint drags the blade back toward its own drive target every step while the script # writes it somewhere else, and the blade jitters back and forth for the whole run - # including long after the last class-D item has gone by. Only the sorter scene used # to do this; the plow cell needs it just as much. _scene.configure_pusher(stage) return dict(script_control=script_control, plow_ready=plow, stray_deactivated=stray, belts=belts, belt_speed=C.BELT_SPEED if belt_speed is None else belt_speed) def load(usd_path=None, belt_speed=None, script_control: bool = True): stage = open_scene(usd_path) return stage, prepare(stage, belt_speed, script_control)