dasha_f/isaac
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Сортировочная ячейка Isaac Sim: CV-пайплайн и меши товаров

Browse Source 
Замкнутый контур "поток -> 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
2026-08-01 13:07:24 +00:00
parent 6ce460378a
commit 0d32f32db0
342 changed files with 18000 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/place_plow_lanes.py
+317
View File
@@ -0,0 +1,317 @@
#!/usr/bin/env python3
"""Place the two plow take-away lanes clear of the blade, and put a container at each end.
python scripts/place_plow_lanes.py
The plow (x = -7.05, 600 mm arm, +-35 deg about Z) sweeps goods off the side of the main
run. A take-away lane therefore has to start OUTSIDE the main belt, not on it: the first
attempt put both near edges at y = +-0.05, which is inside the 450 mm belt, so the lanes
sat under the blade and fouled its swing.
Final layout, near edges at y = +-0.25 (just past the belt edge at +-0.225):
lane C +Y side, rotated 45 deg near end (-7.05, +0.45) runs toward (-X, +Y)
lane B -Y side, perpendicular near end (-7.25, -0.45) runs -Y
container at the far end of each
Both sit at belt height (top z = 1.781) and start flush with ConveyorTrack_04's belt edge
at y = +-0.45, so they join the run the same way the D branch joins it upstream.
C is angled because a plow deflection carries goods sideways *and* downstream - they leave
the belt on a diagonal, and a 45 deg lane meets that trajectory instead of fighting it.
B stays square because the -Y throw is the shorter one.
Only these two tracks move. The plow, its hinge, its drive and the main run are untouched.
"""
from __future__ import annotations
import math
import shutil
import sys
from pathlib import Path
from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade
ROOT = Path(__file__).resolve().parent.parent
CELL = ROOT / "scene" / "plow_cell.usd"
LANE_B = "/ConveyorTrack_01" # -Y side, perpendicular
LANE_C = "/World/ConveyorTrack_01" # +Y side, 45 deg
BELT_Z = 1.781
LANE_SCALE = Gf.Vec3d(1.0, 0.5, 1.0) # same as every other track
# Height matters and was wrong the first time. The D branch (ConveyorTrack_03/Belt_01)
# sits at z 1.74..1.78, flush with the main belt, which is what makes it read as part of
# the conveyor. These tracks carried an authored -0.1 drop, putting them 100 mm low so they
# looked like separate furniture parked nearby. 0.0 puts their belt tops at 1.781 too.
LANE_DROP = 0.0
# Near ends must clear the ARM's swept envelope, not just the belt edge. The 600 mm arm
# pivots at (-7.05, 0) and reaches +-35 deg, so its tip traces out to
# 0.60*sin(35) = 0.344 m either side. Starting the lanes at +-0.45 leaves ~100 mm.
ARM_SWEEP_Y = 0.60 * math.sin(math.radians(35.0)) # 0.344 m
# A plow sweeps goods sideways while they are still ON the belt, so a take-away lane has to
# run ALONGSIDE it, its near edge touching the belt's side rail - not past the belt's end.
# The first placement put lane B at x -7.48..-7.03, entirely downstream of where the main
# belt stops (x = -7.00): goods would have had to leave the belt and cross a gap to reach
# it, which is why nothing ever arrived. Both lanes now sit inside the arm's working span
# (x -7.12..-6.52) with their near edges on the belt edge at y = +-0.45.
B_TRANSLATE = Gf.Vec3d(-6.80, -0.45, LANE_DROP)
B_YAW = -90.0 # travel -Y
# A 45 deg lane meeting a straight belt edge does not touch at its centreline: the near
# corner runs ahead of it. Measured overlap at y=0.45 was 159 mm into the belt, so the lane
# is offset by that much and its nearest corner then lands on the edge instead of inside it.
# x=-6.55 puts the lane's near corner at the arm's tip (-6.52) and inside the belt span
# (-7.00..-6.00), i.e. on the junction itself. At -6.90 the corner sat behind the plow, so
# a +Y deflection had nowhere to land and goods rode on past.
C_TRANSLATE = Gf.Vec3d(-6.55, 0.45 + 0.159, LANE_DROP)
C_YAW = 135.0 # travel (-X, +Y): the diagonal a plow throw makes
def yaw_quat(deg):
r = math.radians(deg) / 2.0
return Gf.Quatd(math.cos(r), Gf.Vec3d(0.0, 0.0, math.sin(r)))
def set_xform(layer, path, translate, yaw, scale=LANE_SCALE):
spec = layer.GetPrimAtPath(path)
if not spec:
return False
for name, value, vtype in (
("xformOp:translate", translate, Sdf.ValueTypeNames.Double3),
("xformOp:orient", yaw_quat(yaw), Sdf.ValueTypeNames.Quatd),
("xformOp:scale", scale, Sdf.ValueTypeNames.Double3)):
attr = spec.attributes.get(name) or Sdf.AttributeSpec(spec, name, vtype)
attr.default = value
order = spec.attributes.get("xformOpOrder") or Sdf.AttributeSpec(
spec, "xformOpOrder", Sdf.ValueTypeNames.TokenArray)
order.default = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
return True
# ---------------------------------------------------------------- containers
CONTAINERS = "/World/PlowContainers"
BIN_HALF = Gf.Vec3f(0.45, 0.40, 0.26) # inner half-extents
BIN_FLOOR_Z = 1.16
BIN_WALL_TOP = 1.70 # under the lane surface, so goods tip in
TH = 0.024
def _material(stage, path, rgb):
mat = UsdShade.Material.Define(stage, path)
sh = UsdShade.Shader.Define(stage, path + "/Shader")
sh.CreateIdAttr("UsdPreviewSurface")
sh.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*rgb))
sh.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.55)
mat.CreateSurfaceOutput().ConnectToSource(sh.ConnectableAPI(), "surface")
return mat
def _box(stage, path, centre, half, mat, collider=True):
cube = UsdGeom.Cube.Define(stage, path)
cube.GetSizeAttr().Set(2.0) # scale == half-extent
xf = UsdGeom.Xformable(cube.GetPrim())
xf.ClearXformOpOrder()
xf.AddTranslateOp().Set(Gf.Vec3d(*centre))
xf.AddScaleOp().Set(Gf.Vec3f(*half))
UsdShade.MaterialBindingAPI.Apply(cube.GetPrim())
UsdShade.MaterialBindingAPI(cube.GetPrim()).Bind(mat)
if collider:
UsdPhysics.CollisionAPI.Apply(cube.GetPrim())
return cube.GetPrim()
def add_container(stage, tag, centre, mat):
"""open-top box; the far wall rises above belt height so a moving item cannot skim over"""
cx, cy = centre
hx, hy, _ = BIN_HALF
wh = (BIN_WALL_TOP - BIN_FLOOR_Z) / 2
wz = BIN_FLOOR_Z + wh
back_top = BELT_Z + 0.22
back_h = (back_top - BIN_FLOOR_Z) / 2
base = f"{CONTAINERS}/{tag}"
_box(stage, f"{base}_Floor", (cx, cy, BIN_FLOOR_Z), (hx, hy, TH), mat)
for name, c, h in (
("W0", (cx, cy + hy, BIN_FLOOR_Z + back_h), (hx, TH, back_h)),
("W1", (cx, cy - hy, wz), (hx, TH, wh)),
("W2", (cx - hx, cy, wz), (TH, hy, wh)),
("W3", (cx + hx, cy, wz), (TH, hy, wh))):
_box(stage, f"{base}_{name}", c, h, mat)
for i, (lx, ly) in enumerate([(cx - hx + 0.06, cy - hy + 0.06), (cx + hx - 0.06, cy - hy + 0.06),
(cx - hx + 0.06, cy + hy - 0.06), (cx + hx - 0.06, cy + hy - 0.06)]):
_box(stage, f"{base}_Leg{i}", (lx, ly, BIN_FLOOR_Z / 2),
(0.024, 0.024, BIN_FLOOR_Z / 2), mat, collider=False)
# ---------------------------------------------------------------- transition plate
def add_transition(stage, lane_path, edge_y, sign, mat, edge_x0, edge_x1):
"""Deck the WHOLE discharge corner, not just the touching triangle.
An angled lane leaves gaps on BOTH sides of its end face: one between its near corner
and the belt edge, another beyond its far corner. Goods do not cross at a single point -
the plow can put them anywhere across the discharge width - so the plate has to span the
entire region between the belt edge and the lane's end face, from one side of the zone
to the other. A plate covering only the first triangle still drops anything pushed wide.
Built as the convex hull of the belt-edge segment and both end-face corners, extruded
down 20 mm, coplanar with both belt surfaces.
"""
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
xf = UsdGeom.XformCache()
belt = stage.GetPrimAtPath(f"{lane_path}/Belt")
M = xf.GetLocalToWorldTransform(belt)
r = cache.ComputeWorldBound(belt).ComputeAlignedRange()
top = r.GetMax()[2]
travel = M.TransformDir(Gf.Vec3d(1, 0, 0)).GetNormalized()
across = Gf.Vec3d(-travel[1], travel[0], 0.0)
half_w = 0.225
centre = Gf.Vec3d((r.GetMin()[0] + r.GetMax()[0]) / 2,
(r.GetMin()[1] + r.GetMax()[1]) / 2, 0.0)
near = centre - travel * 1.0
c0 = near + across * half_w
c1 = near - across * half_w
# everything the plate must reach: the belt edge across the discharge zone, and both
# corners of the lane's end face
xs = [edge_x0, edge_x1, float(c0[0]), float(c1[0])]
ys = [edge_y, edge_y, float(c0[1]), float(c1[1])]
far_y = max(ys) if sign > 0 else min(ys)
quad = [(min(xs), edge_y), (max(xs), edge_y), (max(xs), far_y), (min(xs), far_y)]
path = f"/World/PlowTransition_{'C' if sign > 0 else 'B'}"
if stage.GetPrimAtPath(path).IsValid():
stage.RemovePrim(path)
if abs(far_y - edge_y) < 0.002:
# a square lane meets the edge flush along its whole face - no gap to deck, and a
# zero-area mesh would be a degenerate collider
return None, []
mesh = UsdGeom.Mesh.Define(stage, path)
pts = [Gf.Vec3f(x, y, top) for x, y in quad] + [Gf.Vec3f(x, y, top - 0.02) for x, y in quad]
mesh.GetPointsAttr().Set(pts)
faces = [(0, 3, 2, 1), (4, 5, 6, 7), (0, 1, 5, 4), (1, 2, 6, 5), (2, 3, 7, 6), (3, 0, 4, 7)]
mesh.GetFaceVertexCountsAttr().Set([4] * len(faces))
mesh.GetFaceVertexIndicesAttr().Set([i for f in faces for i in f])
mesh.GetSubdivisionSchemeAttr().Set("none")
mesh.GetExtentAttr().Set([Gf.Vec3f(min(xs), min(edge_y, far_y), top - 0.02),
Gf.Vec3f(max(xs), max(edge_y, far_y), top)])
UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim())
UsdShade.MaterialBindingAPI(mesh.GetPrim()).Bind(mat)
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
return path, [(round(x, 3), round(y, 3)) for x, y in quad]
def add_corner_deck(stage, lane_path, edge_y, sign, mat, edge_far_x):
"""Close the right angle a SQUARE lane leaves against a wider belt.
Lane B meets the belt flush along its own face, but the belt is wider than the lane:
the run reaches x=-6.00 while the lane stops at x=-6.58. Anything the plow pushes
sideways in that leftover span has open air under it. A triangular fillet spanning the
belt edge out to the run's end and back down the lane's side turns that right angle
into a chute, so goods slide into the lane instead of dropping through.
"""
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
belt = stage.GetPrimAtPath(f"{lane_path}/Belt")
r = cache.ComputeWorldBound(belt).ComputeAlignedRange()
top = r.GetMax()[2]
lane_far_x = r.GetMax()[0] # the lane's edge nearest the open span
span = abs(edge_far_x - lane_far_x)
if span < 0.02:
return None, []
corner = (lane_far_x, edge_y)
along_belt = (edge_far_x, edge_y)
down_lane = (lane_far_x, edge_y + sign * span)
path = f"/World/PlowCornerDeck_{'C' if sign > 0 else 'B'}"
if stage.GetPrimAtPath(path).IsValid():
stage.RemovePrim(path)
tri = [corner, along_belt, down_lane]
mesh = UsdGeom.Mesh.Define(stage, path)
pts = [Gf.Vec3f(x, y, top) for x, y in tri] + [Gf.Vec3f(x, y, top - 0.02) for x, y in tri]
mesh.GetPointsAttr().Set(pts)
faces = [(0, 2, 1), (3, 4, 5), (0, 1, 4, 3), (1, 2, 5, 4), (2, 0, 3, 5)]
mesh.GetFaceVertexCountsAttr().Set([len(f) for f in faces])
mesh.GetFaceVertexIndicesAttr().Set([i for f in faces for i in f])
mesh.GetSubdivisionSchemeAttr().Set("none")
xs = [p[0] for p in tri]; ys = [p[1] for p in tri]
mesh.GetExtentAttr().Set([Gf.Vec3f(min(xs), min(ys), top - 0.02),
Gf.Vec3f(max(xs), max(ys), top)])
UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim())
UsdShade.MaterialBindingAPI(mesh.GetPrim()).Bind(mat)
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
return path, [(round(x, 3), round(y, 3)) for x, y in tri]
def main():
if not CELL.exists():
print(f"{CELL} not found")
return 1
backup = CELL.with_suffix(".usd.prelanes")
if not backup.exists():
shutil.copy(CELL, backup)
print(f"backup -> {backup.name}")
layer = Sdf.Layer.FindOrOpen(str(CELL))
set_xform(layer, LANE_B, B_TRANSLATE, B_YAW)
set_xform(layer, LANE_C, C_TRANSLATE, C_YAW)
layer.Save()
print(f"arm sweeps to y=+-{ARM_SWEEP_Y:.3f}; lanes start at +-0.45")
print(f"lane B (-Y, square) near end {tuple(B_TRANSLATE)[:2]}")
print(f"lane C (+Y, 45 deg) near end {tuple(C_TRANSLATE)[:2]}")
stage = Usd.Stage.Open(str(CELL))
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
xf = UsdGeom.XformCache()
if stage.GetPrimAtPath(CONTAINERS).IsValid():
stage.RemovePrim(CONTAINERS)
UsdGeom.Xform.Define(stage, CONTAINERS)
m_b = _material(stage, f"{CONTAINERS}/M_B", (0.85, 0.22, 0.20))
m_c = _material(stage, f"{CONTAINERS}/M_C", (0.25, 0.72, 0.32))
ends = {}
for tag, path in (("B", LANE_B), ("C", LANE_C)):
belt = stage.GetPrimAtPath(f"{path}/Belt")
r = cache.ComputeWorldBound(belt).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
d = xf.GetLocalToWorldTransform(belt).TransformDir(Gf.Vec3d(1, 0, 0)).GetNormalized()
centre = Gf.Vec3d((mn[0] + mx[0]) / 2, (mn[1] + mx[1]) / 2, 0)
# container just past the discharge end, along the lane's own travel direction
far = centre + d * ((max(mx[0] - mn[0], mx[1] - mn[1]) / 2) + BIN_HALF[1] + 0.10)
ends[tag] = (float(far[0]), float(far[1]))
print(f" lane {tag}: x[{mn[0]:6.2f}..{mx[0]:6.2f}] y[{mn[1]:6.2f}..{mx[1]:6.2f}] "
f"top={mx[2]:.3f} travel({d[0]:+.2f},{d[1]:+.2f})")
add_container(stage, "B", ends["B"], m_b)
add_container(stage, "C", ends["C"], m_c)
m_t = _material(stage, f"{CONTAINERS}/M_transition", (0.30, 0.31, 0.34))
for old in ("/World/PlowTransition_C", "/World/PlowTransition_B"):
if stage.GetPrimAtPath(old).IsValid():
stage.RemovePrim(old)
# deck both discharge corners across the full width of the junction (belt x -7.00..-6.00)
for lane, edge, sign in ((LANE_C, 0.45, +1), (LANE_B, -0.45, -1)):
path, corners = add_transition(stage, lane, edge, sign, m_t, -7.00, -6.00)
print(f" transition {path or 'not needed (lane meets flush)'}: {corners}")
# a flush lane still leaves the right angle where the belt runs on past it
cpath, ctri = add_corner_deck(stage, lane, edge, sign, m_t, -6.00)
print(f" corner deck {cpath or 'not needed'}: {ctri}")
stage.GetRootLayer().Save()
print("\ncontainers:")
for tag in ("B", "C"):
r = cache.ComputeWorldBound(
stage.GetPrimAtPath(f"{CONTAINERS}/{tag}_Floor")).ComputeAlignedRange()
mn, mx = r.GetMin(), r.GetMax()
print(f" {tag}: x[{mn[0]:6.2f}..{mx[0]:6.2f}] y[{mn[1]:6.2f}..{mx[1]:6.2f}] "
f"floor z={mx[2]:.2f}")
arm = cache.ComputeWorldBound(
stage.GetPrimAtPath("/World/Diverters/DiverterEnd/Arm")).ComputeAlignedRange()
print(f"\nplow arm x[{arm.GetMin()[0]:.2f}..{arm.GetMax()[0]:.2f}] "
f"y[{arm.GetMin()[1]:.2f}..{arm.GetMax()[1]:.2f}]; swept envelope +-{ARM_SWEEP_Y:.2f}")
return 0
if __name__ == "__main__":
sys.exit(main())