Сортировочная ячейка Isaac Sim: CV-пайплайн и меши товаров
Замкнутый контур "поток -> 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>
This commit is contained in:
dasha_f
@@ -0,0 +1,293 @@
|
||||
"""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)
|
||||
Reference in New Issue
Block a user