Сортировочная ячейка 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,2 @@
|
||||
"""Robozon conveyor sorting cell for Isaac Sim: procedural scene + CRE-ROI v2b vision."""
|
||||
__version__ = "0.1.0"
|
||||
@@ -0,0 +1,265 @@
|
||||
"""Single source of truth for the cell's geometry and timing.
|
||||
|
||||
Every number here was measured or derived on the reference build; the docstrings say
|
||||
which, because several of them are not free parameters.
|
||||
"""
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parent.parent
|
||||
ASSETS = ROOT / "assets"
|
||||
CONFIG = ROOT / "config"
|
||||
MESHES = ASSETS / "meshes"
|
||||
MODELS = ASSETS / "models" # populated by scripts/fetch_models.py
|
||||
|
||||
# ---------------------------------------------------------------- line geometry
|
||||
BELT_Z = 1.781 # belt surface height (world Z)
|
||||
BELT_HALF_W = 0.225 # 450 mm belt
|
||||
FLOOR_Z = 0.0
|
||||
|
||||
SPAWN_X = 2.30 # items are released here, on the infeed belt
|
||||
INFEED_X0, INFEED_X1 = 0.0, 2.60 # infeed belt, upstream of the camera stand
|
||||
MAIN_X0, MAIN_X1 = -8.00, 0.0 # main run, items travel in -X (line ends at -8)
|
||||
CAM_X = -0.750 # inspection point, under the camera portal
|
||||
PUSH_X = -3.900 # pusher centre
|
||||
|
||||
BLADE_X0, BLADE_X1 = -4.25, -3.55 # blade footprint along the belt (0.70 m)
|
||||
BLADE_HOME_Y = -0.300 # retracted
|
||||
BLADE_OUT_Y = 0.420 # extended, item is clear onto the branch
|
||||
BLADE_STROKE = BLADE_OUT_Y - BLADE_HOME_Y
|
||||
|
||||
# branch that carries pushed items away, +Y, 40 mm below the main line is NOT used -
|
||||
# the reference branch sits at the same height
|
||||
# measured off the authored branch /World/ConveyorTrack_03/Belt_01
|
||||
BRANCH_X0, BRANCH_X1 = -4.366, -3.466
|
||||
BRANCH_Y0, BRANCH_Y1 = 0.219, 1.225
|
||||
|
||||
BIN_X0, BIN_X1 = -4.42, -3.42 # collection bin at the end of the branch
|
||||
BIN_Y0, BIN_Y1 = 1.27, 2.05
|
||||
BIN_FLOOR_Z = 1.20
|
||||
BIN_LIP_Z = 1.72
|
||||
|
||||
# laser through-beam, just upstream of the blade. The beam must start clear of the
|
||||
# blade's retracted footprint (y <= -0.27) or it simply reads the blade.
|
||||
GATE_X = PUSH_X + 0.16
|
||||
BEAM_Y0, BEAM_Y1 = -0.240, 0.300
|
||||
BEAM_Z = BELT_Z + 0.025
|
||||
|
||||
# ---------------------------------------------------------------- motion
|
||||
BELT_SPEED = 0.8 # m/s - slow enough that the plow can lean goods over
|
||||
# without the belt dragging them past the blade
|
||||
|
||||
# Blade speed is NOT free. The blade covers 0.70 m of belt and the beam trips with the
|
||||
# item centred at x=-3.61, so extend+retract must fit inside 0.64 m of travel = 0.64 s
|
||||
# at 1 m/s. Stroke 0.72 m => extend alone needs >1.12 m/s, a full cycle >2.25 m/s.
|
||||
# Measured: 1.0/1.5/2.0/2.5 m/s all deliver the item to the bin; 3.0 m/s throws it
|
||||
# (the kinematic blade injects too much impulse). 2.5 m/s = 0.29 s per direction.
|
||||
PUSHER_SPEED = 2.5
|
||||
PUSHER_MAX_SAFE = 2.5
|
||||
|
||||
# 700 mm pitch between goods. At 1 m/s that is 0.7 s head-to-head, and the pusher's own
|
||||
# cycle is ~0.58 s plus the clearing wait - so back-to-back class-D items WILL make the
|
||||
# retract interlock hold. See the pitch study in the README.
|
||||
RELEASE_GAP = 0.70 # metres between released items
|
||||
|
||||
# ---------------------------------------------------------------- plow (DiverterEnd)
|
||||
# The second diverter, at the far end of the main run. Unlike the pusher it does not shove
|
||||
# the item sideways; it swings a blade into the lane so goods are deflected as they arrive.
|
||||
# Used by scene/plow_cell.usd; see sim/plow.py.
|
||||
PLOW_ROOT = "/World/Diverters/DiverterEnd"
|
||||
PLOW_BASE = PLOW_ROOT + "/Base" # kinematic pedestal
|
||||
PLOW_ARM = PLOW_ROOT + "/Arm" # dynamic blade, gravity disabled, 12 kg
|
||||
PLOW_HINGE = PLOW_ROOT + "/ArmHinge" # revolute about Z, angular force drive
|
||||
|
||||
PLOW_POS = (-7.05, 0.0, 1.76) # authored placement (the prim also carries rotateZ=180)
|
||||
# 42 deg puts the tip at 0.6*sin42 = 0.402 m. The lane edge is at 0.450 m, but an item is
|
||||
# not a point: a ~150 mm box is taken by the lane once its near edge crosses, so a centre
|
||||
# at 0.402 delivers it. 30 deg (0.300 m) never could - that was the dead zone.
|
||||
PLOW_SWING = 42.0 # degrees either side of centre
|
||||
PLOW_LIMIT = 45.0 # joint hard limit, raised by fix_plow_reach_and_trays.py
|
||||
PLOW_HOLD = 1.5 # authored dwell at each end, seconds
|
||||
|
||||
# Authored gains on the angular drive, read back off plow_cell.usd. Force-type, so the
|
||||
# blade is compliant: it yields on contact instead of teleporting through cargo the way the
|
||||
# kinematic pusher blade does. (The earlier fixed.usd build used 50000/5000 - stiffer arm,
|
||||
# softer damping; these are the 90_degree.usd figures the scene actually ships with.)
|
||||
PLOW_ARM_MASS = 12.0 # kg, gravity disabled - the hinge alone holds the arm
|
||||
PLOW_STIFFNESS = 120000.0
|
||||
PLOW_DAMPING = 1500.0
|
||||
PLOW_MAX_FORCE = 1.0e6
|
||||
|
||||
# Blade motion is specified as a **tip speed**, not an angular rate, because tip speed is
|
||||
# what the cargo feels. The arm is PLOW_ARM_LEN long, so a tip speed v needs w = v / L:
|
||||
#
|
||||
# 0.8 m/s tip -> 1.333 rad/s -> 76.4 deg/s
|
||||
#
|
||||
# Matching the tip to the belt (0.8 m/s) means the blade never overtakes the goods it is
|
||||
# steering: it meets them at their own speed and leans them across instead of batting them.
|
||||
# The authored graph instead swings 30 deg in 7 ms (4125 deg/s, 72 rad/s) - a display
|
||||
# animation, not a sortable motion; at that rate the blade arrives as an impulse and throws
|
||||
# goods off the line, the same failure the pusher shows above 2.5 m/s.
|
||||
PLOW_ARM_LEN = 0.60 # metres, hinge to tip (after scripts/narrow_plow.py)
|
||||
PLOW_TIP_SPEED = 0.80 # m/s at the blade tip
|
||||
PLOW_RATE = 76.4 # deg/s = degrees(PLOW_TIP_SPEED / PLOW_ARM_LEN)
|
||||
PLOW_RATE_AUTHORED = 4125.29612494 # deg/s baked into the scene's OmniGraph script
|
||||
|
||||
# Ceiling on the drive's own angular velocity, the authored figure (72 rad/s). It is a
|
||||
# limit, not a demand: the commanded angle is ramped at PLOW_RATE, so the drive never gets
|
||||
# anywhere near this. Left at the authored value so the joint is not quietly re-specified.
|
||||
PLOW_MAX_ANGULAR_VELOCITY = 72.0 # rad/s
|
||||
|
||||
# --- sweeping, as opposed to leaning -------------------------------------------------
|
||||
# With the pivot at the belt centre and the free end pointing UPSTREAM, the blade cannot
|
||||
# work as an inclined plane: goods driven along it slide toward its downstream end, which is
|
||||
# the pivot at y = 0, so they are funnelled back to the middle. Measured ceiling 0.39 m and
|
||||
# goods wedged between blade and centreline.
|
||||
#
|
||||
# So the blade is used as a **sweeper** instead of a ramp. It waits at centre, and once the
|
||||
# item is in front of it it swings through, giving the item a lateral push it carries onto
|
||||
# the lane. That needs a brisk rate - a slow sweep just leans on the item - but it must stay
|
||||
# inside the range the pusher blade already proved safe (<= 2.5 m/s at the contact point).
|
||||
#
|
||||
# 200 deg/s at a 0.6 m arm = 2.09 m/s at the tip, under the 2.5 m/s limit.
|
||||
PLOW_SWEEP_RATE = 200.0 # deg/s during the push
|
||||
# The zone must start UPSTREAM of the blade's own leading tip, not at it. With the pivot
|
||||
# moved upstream the arm at rest lies along the centreline from -7.05 to -6.45, so its tip
|
||||
# at -6.45 stands in the flow: a centred item runs into it and stops before the plow has
|
||||
# been told to do anything. Observed live - four items piled at x -6.34..-6.45 with the
|
||||
# blade still at 0 deg. Starting the zone 250 mm earlier gives the sweep time to begin
|
||||
# while the item is still approaching.
|
||||
PLOW_SWEEP_X0, PLOW_SWEEP_X1 = -7.15, -6.20
|
||||
|
||||
# --- fork: the blade is a POINT, not a sweeper --------------------------------------
|
||||
# The discharge is now a Y: channel C runs straight on (y -0.45..0.00), channel B branches
|
||||
# 45 deg away (y 0.00..+1.73), and they share the inner edge y = 0 where the blade pivots.
|
||||
#
|
||||
# So the blade has two states, not a symmetric swing about centre:
|
||||
#
|
||||
# rest -> lies across the B mouth. Class C needs NO action at all, which removes every
|
||||
# timing failure the sweeper had: C is simply the default route.
|
||||
# B -> swings over, the B mouth opens and the C side is blocked instead.
|
||||
#
|
||||
# Which sign closes which is MEASURED, never reasoned: signs in this scene have come out
|
||||
# backwards three times. Run one class-B item and read lane_of().
|
||||
# The point SELECTS the route; it does not deliver on its own. With the pivot downstream at
|
||||
# the apex, goods sliding along a stationary blade travel toward that pivot - i.e. to y = 0,
|
||||
# the boundary between the channels - and are released there with no sideways speed at all.
|
||||
# They then straddle both belts, C pulling straight and B pulling at 45 deg, the two cancel,
|
||||
# and the item stands still against the blade. That is the stall in the viewport.
|
||||
#
|
||||
# So the blade holds a SHALLOW angle to select, then keeps turning while it is in contact:
|
||||
# the extra travel is the push that commits the item to one channel.
|
||||
# GUIDE, not gate. At 42-45 deg to the flow the blade's normal points mostly backwards: it
|
||||
# takes the item's forward speed away and the item stops dead against it - measured, goods
|
||||
# halted at x -6.945 with the belt still running under them. A shallow blade lets the item
|
||||
# keep its travel and only drifts it sideways, which is what a plough is supposed to do.
|
||||
#
|
||||
# With the pivot on the +Y edge the blade parks ACROSS the lane at a shallow angle, so C is
|
||||
# the default route and needs no command; for a B item it retracts flush to the edge and B
|
||||
# passes straight through. Only B actuates, and the blade never blocks anything.
|
||||
#
|
||||
# The sign that swings INTO the lane is to be MEASURED - signs in this scene have come out
|
||||
# backwards four times.
|
||||
# Pivot is on the -Y edge, so the blade can only drift goods toward +Y - i.e. into B. That
|
||||
# is the right way round: C is reached by doing nothing, which is what "C is the default"
|
||||
# has to mean mechanically. From the +Y edge B was simply unreachable, and both B items in
|
||||
# the last run ran straight on into C no matter what the blade did.
|
||||
PLOW_REST_ANGLE = 0.0 # C route (default): blade flush with the edge, lane clear
|
||||
PLOW_B_ANGLE = +26.0 # B route: enlarged so the deflection is visible
|
||||
# upstream of this an item counts as a new pass and the plow re-arms for it
|
||||
PLOW_REARM_X = -4.00
|
||||
PLOW_NUDGE_S = 0.35 # how long the blade stays out - then it releases
|
||||
PLOW_PUSH_ANGLE = 38.0 # while touching: a little more drift, still not a wall
|
||||
|
||||
# ---- pre-positioned steer, driven by the laser gate rather than by contact ----------
|
||||
# The blade used to wait until it FELT the item, then swing. That is too late: the item is
|
||||
# already against a moving edge, so the swing lands as a shove. Now the gate at SENSE_X
|
||||
# (1.02 m upstream of the blade's leading edge at x -7.32) names the class 1.28 s ahead at
|
||||
# 0.8 m/s, and the blade is already standing at the right angle when the item arrives. The
|
||||
# item then just grazes the face and is walked sideways while the belt keeps carrying it.
|
||||
#
|
||||
# Signs are measured, not reasoned: a POSITIVE swing deflects toward -Y.
|
||||
# B - the +Y branch -> negative
|
||||
# C - the -Y lane, centreline y -0.225 -> positive
|
||||
# Small on purpose: at 16 deg the 0.60 m face walks an item 0.60*tan(16) = 172 mm sideways
|
||||
# over its length, which reaches lane B's mouth (+0.16) and lane C's centreline without the
|
||||
# blade ever becoming a wall.
|
||||
PLOW_PRESET = {"B": -16.0, "C": +16.0, "D": 0.0}
|
||||
# past this x the item is clear of the blade, so the arm may go home for the next one
|
||||
PLOW_RELEASE_X = -8.10
|
||||
# where the blade pivots, measured after it was moved onto the fork apex. The queue serves
|
||||
# whichever armed item is closest to THIS, so it must track the pivot if the plow moves.
|
||||
PLOW_X = -7.85
|
||||
|
||||
# Gains for *sorting*, as opposed to the authored display gains above.
|
||||
#
|
||||
# Measured failure with the authored 120000/1500/1e6: a 0.6 kg item on a 1 m/s belt reaches
|
||||
# the blade, pushes it from 30 deg back to 10 deg, and the drive answers with a restoring
|
||||
# torque that saturates maxForce. The contact then resolves as an impulse and the item
|
||||
# leaves at 6 m/s, ending several kilometres below the floor. Traces show it every time:
|
||||
# t=10.53 arm=+30.0 v=0.50 -> t=10.73 arm=+19.6 v=0.50 -> t=11.36 v=5.98 (launched)
|
||||
# The blade only has to redirect goods that weigh well under a kilogram, so it needs enough
|
||||
# authority to hold its angle and no more. Softer and better damped, it deflects instead of
|
||||
# batting.
|
||||
# Second correction, from a measured limit cycle: at 3000/600 the arm did not settle at all
|
||||
# but oscillated between +21.4 and -21.4 deg at ~99 deg/s - faster than the 76.4 deg/s ramp
|
||||
# that was commanding it, which is the giveaway that the drive, not the command, was moving
|
||||
# it. With the arm's inertia around 1.1 kg m^2 a stiffness of 3000 puts the natural
|
||||
# frequency near 395 rad/s, far too high to be integrated at 120 Hz, so the solver rings.
|
||||
# Dropping to 300 puts it near 125 rad/s, comfortably resolved, and damping 150 keeps it
|
||||
# close to critically damped.
|
||||
PLOW_SORT_STIFFNESS = 300.0
|
||||
PLOW_SORT_DAMPING = 150.0
|
||||
PLOW_SORT_MAX_FORCE = 2000.0
|
||||
|
||||
# PhysX separates overlapping bodies by moving them apart, and by default it may do so at
|
||||
# any speed. A thin blade sweeping into a box penetrates deeply in one step, and the
|
||||
# uncapped separation is itself enough to fire the item off the line. Cap it for the items
|
||||
# and for the arm; 1 m/s is the belt speed, so separation can never outrun the process.
|
||||
MAX_DEPENETRATION = 1.0
|
||||
|
||||
# ---------------------------------------------------------------- plow friction
|
||||
# Goods piled up against the blade instead of sliding along it, and the reason is friction,
|
||||
# not geometry. Two surfaces decide whether a plow leads or blocks:
|
||||
#
|
||||
# the blade face - the item has to slide ALONG it. With no physics material authored the
|
||||
# arm ran on PhysX defaults, which is roughly rubber on rubber.
|
||||
# the belt - the carrying belt is deliberately grippy (1.1 / 0.95) so goods do not
|
||||
# slip while being driven. That same grip pins them against moving
|
||||
# sideways: to cross 0.45 m the blade must overcome mu * m * g the whole
|
||||
# way, and at mu = 0.95 it simply wins the tug of war by stopping them.
|
||||
#
|
||||
# Real plough sorters solve it exactly this way - a polished (UHMW) blade over a low-
|
||||
# friction slider bed on the diverting section only. The carrying sections keep their grip.
|
||||
PLOW_BLADE_FRICTION = (0.05, 0.04) # static, dynamic - polished blade face
|
||||
# Low friction belongs ONLY on the narrow handover plates, never on the carrying belt. A
|
||||
# belt moves goods by friction alone: at 0.30 the drive on a 0.6 kg item is about 1.8 N, and
|
||||
# any resistance at all - the blade edge, the belt lip, a skewed pose - stops it. Binding
|
||||
# this to /World/ConveyorTrack_04/Belt made the whole 2 m run through the plow slippery, so
|
||||
# goods could be nudged sideways but not carried onward. That is why neither the long belt
|
||||
# nor the short one helped: the run-out existed, it just had no grip.
|
||||
# 0.30/0.25 was too slippery to CARRY. The plates are driven (configure_lanes gives each a
|
||||
# surfaceVelocity pointing down its lane), but friction is what transmits that drive, and at
|
||||
# 0.30 it transmits almost nothing: every item nudged onto PlowTransition_B (x -8.03..-7.45)
|
||||
# coasted and stopped at x ~ -7.7 - the exact "pushed, then stops dead" trace. Raised to a
|
||||
# middle ground: enough grip to keep goods moving toward their lane, still far below the
|
||||
# carrying belt (1.10/0.95) so the blade can still slide them sideways.
|
||||
PLOW_SECTION_FRICTION = (0.70, 0.60)
|
||||
PLOW_SECTION_PLATES = ["/World/PlowTransition_B", "/World/PlowTransition_C"]
|
||||
|
||||
# ---------------------------------------------------------------- vision
|
||||
# Scene meshes are 0.49x real size, so metres -> real millimetres needs this divisor.
|
||||
DIM_SCALE = 1.0 / 2.041
|
||||
|
||||
ROI_FIXED = 320 # CRE-ROI v2b: fixed ROI side
|
||||
ROI_PAD_V, ROI_PAD_X = 24, 24
|
||||
DISP_PAD = 1.35 # left pad = DISP_PAD * max disparity
|
||||
Z_MIN = 0.35 # closest expected surface, sets the pad width
|
||||
MAX_MASK_FRAC = 0.08 # a blob bigger than this is belt, not cargo
|
||||
VIEW_CONSISTENCY = 0.10 # per-view centroid must agree within 10 cm
|
||||
|
||||
# B/C/D thresholds (millimetres, on the real-size scale)
|
||||
OVERSIZE_MAX = (451, 321, 321)
|
||||
MIN_DIM = 10
|
||||
ROUND_K = 0.80
|
||||
|
||||
CLASS_LABELS = {"B": "sortable", "C": "oversize", "D": "round"}
|
||||
@@ -0,0 +1 @@
|
||||
from .pipeline import CreRoiV2b, classify, roundness # noqa: F401
|
||||
@@ -0,0 +1,355 @@
|
||||
"""CRE-ROI v2b: FastSAM segment-everything -> cross-view common object -> fixed-320 ROI
|
||||
crop -> ONE batched CRE-Stereo pass over the 3 camera pairs -> 3-view fusion -> B/C/D.
|
||||
|
||||
Why each piece is there
|
||||
ROI crop the item covers a small part of a 1280-wide frame; cropping spends the
|
||||
network's resolution on the cargo instead of on belt.
|
||||
same L/R window disparity is invariant to an equal column shift, so the crop must use
|
||||
identical [x0,x1] in both eyes and be left-padded by the max disparity,
|
||||
or the right-hand counterpart falls outside the crop.
|
||||
batching the 3 crops go through CRE as one forward pass; that is the "v2b" part.
|
||||
gate pixel the inspection point is projected into every camera, and the blob covering
|
||||
it is kept - by construction all views then measure the SAME physical item.
|
||||
consistency a view whose 3D centroid disagrees with the median is dropped; that is the
|
||||
cross-view rule enforced again in 3D, where it is unambiguous.
|
||||
|
||||
The stereo rig must be RECTIFIED (parallel optical axes). Verged pairs break
|
||||
depth = fx*b/disp and the reconstruction lands metres away.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import torch
|
||||
import torch.nn.functional as F
|
||||
|
||||
from .. import config as C
|
||||
|
||||
|
||||
# ------------------------------------------------------------------ geometry helpers
|
||||
def _taubin(xy):
|
||||
"""algebraic circle fit; returns (cx, cy, R, mean relative residual)"""
|
||||
x = xy[:, 0].astype(np.float64)
|
||||
y = xy[:, 1].astype(np.float64)
|
||||
xm, ym = x.mean(), y.mean()
|
||||
u, v = x - xm, y - ym
|
||||
Suu, Svv, Suv = (u * u).sum(), (v * v).sum(), (u * v).sum()
|
||||
Suuu, Svvv = (u ** 3).sum(), (v ** 3).sum()
|
||||
Suvv, Svuu = (u * v * v).sum(), (v * u * u).sum()
|
||||
try:
|
||||
uc, vc = np.linalg.solve(np.array([[Suu, Suv], [Suv, Svv]]),
|
||||
0.5 * np.array([Suuu + Suvv, Svvv + Svuu]))
|
||||
except np.linalg.LinAlgError:
|
||||
return None
|
||||
cx, cy = uc + xm, vc + ym
|
||||
R = np.sqrt(max(uc * uc + vc * vc + (Suu + Svv) / len(x), 1e-12))
|
||||
if not np.isfinite(R) or R < 1e-6:
|
||||
return None
|
||||
return cx, cy, R, float(np.abs(np.hypot(x - cx, y - cy) - R).mean() / R)
|
||||
|
||||
|
||||
def _rin_rout(xy):
|
||||
"""K = inscribed / circumscribed radius of the convex hull; 1.0 for a perfect circle"""
|
||||
pts = np.ascontiguousarray(xy.astype(np.float32))
|
||||
if len(pts) < 3:
|
||||
return 0.0
|
||||
_, r_out = cv2.minEnclosingCircle(pts)
|
||||
if r_out < 1e-6:
|
||||
return 0.0
|
||||
xmin, ymin = xy.min(0)
|
||||
w, h = float(np.ptp(xy[:, 0])), float(np.ptp(xy[:, 1]))
|
||||
sc = 180.0 / max(w, h, 1e-6)
|
||||
img = np.zeros((int(h * sc) + 10, int(w * sc) + 10), np.uint8)
|
||||
try:
|
||||
hull = cv2.convexHull(pts).reshape(-1, 2)
|
||||
except cv2.error:
|
||||
return 0.0
|
||||
cv2.fillConvexPoly(img, ((hull - [xmin, ymin]) * sc + 5).astype(np.int32), 255)
|
||||
return float(cv2.distanceTransform(img, cv2.DIST_L2, 5).max()) / sc / r_out
|
||||
|
||||
|
||||
def _section_k(xy, res_tol=0.06, min_span=120.0):
|
||||
if len(xy) < 15:
|
||||
return 0.0
|
||||
fit = _taubin(xy)
|
||||
if fit is not None:
|
||||
cx, cy, R, res = fit
|
||||
ang = np.arctan2(xy[:, 1] - cy, xy[:, 0] - cx)
|
||||
span = np.unique((((ang + np.pi) / (2 * np.pi)) * 48).astype(int) % 48).size / 48.0 * 360.0
|
||||
ext = max(np.ptp(xy[:, 0]), np.ptp(xy[:, 1])) + 1e-9
|
||||
if res < res_tol and span >= min_span and 0.35 * ext < R < 2.0 * ext:
|
||||
return 1.0
|
||||
return _rin_rout(xy)
|
||||
|
||||
|
||||
def voxel(P, v=0.004):
|
||||
key = np.floor(P / v).astype(np.int64)
|
||||
_, idx = np.unique(key, axis=0, return_index=True)
|
||||
return P[idx]
|
||||
|
||||
|
||||
def roundness(P):
|
||||
"""max circular-section K over the top-down section and one belt-aligned cross section"""
|
||||
if len(P) < 60:
|
||||
return 0.0
|
||||
P = voxel(P)
|
||||
ks = [_section_k(P[:, :2])]
|
||||
xy = P[:, :2] - P[:, :2].mean(0)
|
||||
try:
|
||||
_, _, V = np.linalg.svd(xy, full_matrices=False)
|
||||
ax = V[0]
|
||||
except np.linalg.LinAlgError:
|
||||
ax = np.array([1.0, 0.0])
|
||||
along = xy @ ax
|
||||
perp = xy @ np.array([-ax[1], ax[0]])
|
||||
mid = np.abs(along - np.median(along)) < 0.15 * (np.ptp(along) + 1e-9)
|
||||
if mid.sum() > 15:
|
||||
ks.append(_section_k(np.c_[perp[mid], P[mid, 2]]))
|
||||
return max(ks)
|
||||
|
||||
|
||||
def classify(P, belt_z: float):
|
||||
"""belt-plane OBB -> dims in real millimetres -> B / C / D"""
|
||||
if len(P) < 60:
|
||||
return "?", [0, 0, 0], 0.0
|
||||
P = voxel(P)
|
||||
centre = P.mean(0)
|
||||
radial = np.linalg.norm(P - centre, axis=1)
|
||||
P = P[radial < np.percentile(radial, 94)] # trim segmentation fringe
|
||||
if len(P) < 40:
|
||||
return "?", [0, 0, 0], 0.0
|
||||
height = (np.percentile(P[:, 2], 98) - belt_z) / C.DIM_SCALE * 1000.0
|
||||
(_, _), (rw, rh), _ = cv2.minAreaRect(np.ascontiguousarray(P[:, :2].astype(np.float32)))
|
||||
foot = sorted([rw / C.DIM_SCALE * 1000.0, rh / C.DIM_SCALE * 1000.0])
|
||||
dims = sorted([height, foot[0], foot[1]], reverse=True)
|
||||
k = roundness(P)
|
||||
a, b, c = C.OVERSIZE_MAX
|
||||
if dims[0] > a or dims[1] > b or dims[2] > c or dims[2] < C.MIN_DIM:
|
||||
return "C", [round(x) for x in dims], k
|
||||
return ("D" if k > C.ROUND_K else "B"), [round(x) for x in dims], k
|
||||
|
||||
|
||||
# ------------------------------------------------------------------ the pipeline
|
||||
class CreRoiV2b:
|
||||
"""Holds the models, the render products and the per-frame decision."""
|
||||
|
||||
def __init__(self, calib_path: Path | str = None, device="cuda"):
|
||||
self.device = device
|
||||
calib_path = Path(calib_path or C.CONFIG / "calib.json")
|
||||
self.calib = json.loads(calib_path.read_text())
|
||||
self.belt_z = self.calib.get("belt_top", C.BELT_Z)
|
||||
self.aim = np.array(self.calib["center"], dtype=float)
|
||||
if not self.calib.get("rectified"):
|
||||
raise ValueError(
|
||||
f"{calib_path} is not marked rectified. depth = fx*b/disp assumes parallel "
|
||||
"optical axes; rebuild the rig with sim.scene.build_camera_rig()."
|
||||
)
|
||||
self._load_models()
|
||||
self.cams = {}
|
||||
|
||||
# -- models -------------------------------------------------------------
|
||||
def _load_models(self):
|
||||
import sys
|
||||
cre_dir = C.MODELS / "crestereo"
|
||||
if str(cre_dir) not in sys.path:
|
||||
sys.path.insert(0, str(cre_dir))
|
||||
from nets import Model # noqa: E402 (vendored CRE-Stereo)
|
||||
|
||||
self.cre = Model(max_disp=256, mixed_precision=False, test_mode=True)
|
||||
weights = C.MODELS / "crestereo_eth3d.pth"
|
||||
self.cre.load_state_dict(torch.load(weights, map_location="cpu"), strict=True)
|
||||
self.cre.to(self.device).eval()
|
||||
|
||||
from ultralytics import FastSAM # noqa: E402
|
||||
self.fsam = FastSAM(str(C.MODELS / "FastSAM-s.pt"))
|
||||
|
||||
# -- render products ----------------------------------------------------
|
||||
def attach_cameras(self):
|
||||
"""one RGB annotator per eye; call once, after the stage is populated"""
|
||||
import omni.replicator.core as rep
|
||||
|
||||
for name, cc in self.calib["cameras"].items():
|
||||
K = cc["intrinsics"]
|
||||
res = (K["width"], K["height"])
|
||||
ann = {}
|
||||
for side, path in (("L", cc["left_path"]), ("R", cc["right_path"])):
|
||||
rp = rep.create.render_product(path, res)
|
||||
a = rep.AnnotatorRegistry.get_annotator("rgb")
|
||||
a.attach(rp)
|
||||
ann[side] = a
|
||||
self.cams[name] = dict(K=K, b=cc["baseline_m"],
|
||||
LW=np.array(cc["left_world"], dtype=float), **ann)
|
||||
self.gate_px = {n: self._gate_px(n) for n in self.cams}
|
||||
return self.cams
|
||||
|
||||
async def warmup(self, steps=120, tries=4):
|
||||
"""Step the app until the eyes actually return an image.
|
||||
|
||||
`attach_cameras` creates the render products and returns at once, but RTX yields
|
||||
nothing for a fresh render product for a long while, so the first `get_data()` comes
|
||||
back PURE BLACK - RGB max 0 with alpha ~255, meaning geometry is being hit and is
|
||||
simply unresolved, not that the view is empty. FastSAM then segments nothing and
|
||||
every early item is logged unclassified, which reads as "the pipeline is broken".
|
||||
|
||||
Measured on this cell: 60 steps still black; 120 steps plus a second settle gives
|
||||
RGB max 92 / mean 21 on the D435 left eye. So this waits and CHECKS rather than
|
||||
trusting a fixed count - it returns the darkest eye it saw, for the run log.
|
||||
"""
|
||||
import isaacsim.core.experimental.utils.app as app_utils
|
||||
worst = 0
|
||||
for attempt in range(tries):
|
||||
await app_utils.update_app_async(steps=steps)
|
||||
worst = 255
|
||||
for c in self.cams.values():
|
||||
for side in ("L", "R"):
|
||||
a = np.asarray(c[side].get_data())
|
||||
worst = min(worst, int(a[..., :3].max()) if a.size else 0)
|
||||
if worst > 3:
|
||||
return dict(ok=True, attempts=attempt + 1, darkest_eye_max=worst)
|
||||
return dict(ok=False, attempts=tries, darkest_eye_max=worst)
|
||||
|
||||
def _gate_px(self, name):
|
||||
c = self.cams[name]
|
||||
K = c["K"]
|
||||
p = np.append(self.aim, 1.0) @ np.linalg.inv(c["LW"])
|
||||
d = max(-p[2], 1e-6)
|
||||
u = K["cx"] + K["fx"] * p[0] / d
|
||||
v = K["cy"] - K["fy"] * p[1] / d
|
||||
return int(np.clip(u, 0, K["width"] - 1)), int(np.clip(v, 0, K["height"] - 1))
|
||||
|
||||
# -- stages -------------------------------------------------------------
|
||||
def segment(self, rgb, gate):
|
||||
"""smallest plausible blob covering the gate pixel"""
|
||||
t0 = time.time()
|
||||
res = self.fsam(rgb[..., ::-1], device=self.device, retina_masks=True,
|
||||
imgsz=1024, conf=0.35, iou=0.9, verbose=False)
|
||||
ms = (time.time() - t0) * 1000
|
||||
if not res or res[0].masks is None:
|
||||
return None, ms
|
||||
gx, gy = gate
|
||||
best, best_area = None, np.inf
|
||||
for m in res[0].masks.data.cpu().numpy():
|
||||
mb = m > 0.5
|
||||
area = int(mb.sum())
|
||||
if area < 150 or area > C.MAX_MASK_FRAC * mb.size:
|
||||
continue
|
||||
if mb[gy, gx] and area < best_area:
|
||||
best, best_area = mb, area
|
||||
return best, ms
|
||||
|
||||
@staticmethod
|
||||
def crop(left, right, mask, K, baseline):
|
||||
"""identical column window in both eyes, left-padded by the max disparity"""
|
||||
H, W = mask.shape
|
||||
ys, xs = np.where(mask)
|
||||
max_disp = int(np.ceil(C.DISP_PAD * K["fx"] * baseline / C.Z_MIN))
|
||||
x0 = max(0, xs.min() - max_disp - C.ROI_PAD_X)
|
||||
x1 = min(W, xs.max() + C.ROI_PAD_X)
|
||||
y0 = max(0, ys.min() - C.ROI_PAD_V)
|
||||
y1 = min(H, ys.max() + C.ROI_PAD_V)
|
||||
cl, cr = left[y0:y1, x0:x1], right[y0:y1, x0:x1]
|
||||
h, w = cl.shape[:2]
|
||||
s = min(C.ROI_FIXED / max(h, w, 1), 2.5)
|
||||
if abs(s - 1) > 0.02:
|
||||
cl = cv2.resize(cl, (max(int(round(w * s)), 8), max(int(round(h * s)), 8)))
|
||||
cr = cv2.resize(cr, (cl.shape[1], cl.shape[0]))
|
||||
return np.ascontiguousarray(cl), np.ascontiguousarray(cr), (x0, y0, x1, y1), s, (h, w)
|
||||
|
||||
def infer_batch(self, crops, iters=20):
|
||||
"""all camera crops in ONE two-stage CRE pass, zero-padded to a common /8 canvas"""
|
||||
sizes = [c[0].shape[:2] for c in crops]
|
||||
Hp = (max(h for h, _ in sizes) + 7) // 8 * 8
|
||||
Wp = (max(w for _, w in sizes) + 7) // 8 * 8
|
||||
lb = np.zeros((len(crops), 3, Hp, Wp), np.float32)
|
||||
rb = np.zeros_like(lb)
|
||||
for i, (l, r) in enumerate(crops):
|
||||
h, w = l.shape[:2]
|
||||
lb[i, :, :h, :w] = l.transpose(2, 0, 1)
|
||||
rb[i, :, :h, :w] = r.transpose(2, 0, 1)
|
||||
il = torch.from_numpy(lb).to(self.device)
|
||||
ir = torch.from_numpy(rb).to(self.device)
|
||||
ild = F.interpolate(il, (Hp // 2, Wp // 2), mode="bilinear", align_corners=True)
|
||||
ird = F.interpolate(ir, (Hp // 2, Wp // 2), mode="bilinear", align_corners=True)
|
||||
with torch.inference_mode():
|
||||
init = self.cre(ild, ird, iters=iters, flow_init=None)
|
||||
flow = self.cre(il, ir, iters=iters, flow_init=init)
|
||||
disp = np.abs(flow[:, 0].detach().cpu().numpy())
|
||||
return [disp[i, :h, :w] for i, (h, w) in enumerate(sizes)]
|
||||
|
||||
def backproject(self, disp_small, mask, box, s, orig, K, baseline, LW):
|
||||
h, w = orig
|
||||
disp = cv2.resize(disp_small, (w, h)) / s
|
||||
depth = np.where(disp > 0.5, K["fx"] * baseline / np.maximum(disp, 1e-6), np.nan)
|
||||
x0, y0, x1, y1 = box
|
||||
# erode: silhouette pixels straddle the depth discontinuity and smear the cloud
|
||||
m = cv2.erode(mask[y0:y1, x0:x1].astype(np.uint8),
|
||||
np.ones((3, 3), np.uint8), iterations=2).astype(bool)
|
||||
vs, us = np.mgrid[y0:y1, x0:x1]
|
||||
sel = m & np.isfinite(depth) & (depth > 1e-3)
|
||||
u, v, d = us[sel], vs[sel], depth[sel]
|
||||
cam = np.stack([(u - K["cx"]) * d / K["fx"],
|
||||
-(v - K["cy"]) * d / K["fy"],
|
||||
-d, np.ones_like(d)], 1)
|
||||
return (cam @ LW)[:, :3]
|
||||
|
||||
def _in_workspace(self, P):
|
||||
return P[(np.abs(P[:, 0] - self.aim[0]) < 0.45) & (np.abs(P[:, 1]) < 0.35) &
|
||||
(P[:, 2] > self.belt_z + 0.012) & (P[:, 2] < self.belt_z + 0.80)]
|
||||
|
||||
# -- one decision -------------------------------------------------------
|
||||
def measure(self):
|
||||
"""capture -> segment -> crop -> batched CRE -> fuse -> classify"""
|
||||
t0 = time.time()
|
||||
crops, meta, seg_ms = [], [], []
|
||||
for name, c in self.cams.items():
|
||||
left = c["L"].get_data()[..., :3]
|
||||
right = c["R"].get_data()[..., :3]
|
||||
mask, ms = self.segment(left, self.gate_px[name])
|
||||
seg_ms.append(round(ms))
|
||||
if mask is None:
|
||||
continue
|
||||
cl, cr, box, s, orig = self.crop(left.astype(np.float32) / 255.0,
|
||||
right.astype(np.float32) / 255.0,
|
||||
mask, c["K"], c["b"])
|
||||
crops.append((cl, cr))
|
||||
meta.append((name, mask, box, s, orig))
|
||||
|
||||
if not crops:
|
||||
return dict(cls="?", dims=[0, 0, 0], k=0.0, views=0, pts=0,
|
||||
seg_ms=seg_ms, cre_ms=0, total_ms=round((time.time() - t0) * 1000))
|
||||
|
||||
t_cre = time.time()
|
||||
disps = self.infer_batch(crops)
|
||||
cre_ms = round((time.time() - t_cre) * 1000)
|
||||
|
||||
clouds = []
|
||||
for (name, mask, box, s, orig), disp in zip(meta, disps):
|
||||
c = self.cams[name]
|
||||
P = self._in_workspace(
|
||||
self.backproject(disp, mask, box, s, orig, c["K"], c["b"], c["LW"]))
|
||||
if len(P) >= 40:
|
||||
clouds.append((name, P))
|
||||
|
||||
dropped = []
|
||||
if len(clouds) > 1:
|
||||
cent = np.array([p.mean(0) for _, p in clouds])
|
||||
med = np.median(cent, 0)
|
||||
keep = [i for i in range(len(clouds))
|
||||
if np.linalg.norm(cent[i] - med) < C.VIEW_CONSISTENCY]
|
||||
dropped = [clouds[i][0] for i in range(len(clouds)) if i not in keep]
|
||||
if keep:
|
||||
clouds = [clouds[i] for i in keep]
|
||||
|
||||
if not clouds:
|
||||
return dict(cls="?", dims=[0, 0, 0], k=0.0, views=0, pts=0, seg_ms=seg_ms,
|
||||
cre_ms=cre_ms, total_ms=round((time.time() - t0) * 1000))
|
||||
|
||||
P = np.vstack([p for _, p in clouds])
|
||||
cls, dims, k = classify(P, self.belt_z)
|
||||
return dict(cls=cls, dims=dims, k=round(float(k), 3), views=len(clouds),
|
||||
dropped=dropped, pts=len(P), seg_ms=seg_ms, cre_ms=cre_ms,
|
||||
total_ms=round((time.time() - t0) * 1000))
|
||||
@@ -0,0 +1,5 @@
|
||||
"""Simulation side: scene loading, cell mechanics, the self-running feeder.
|
||||
|
||||
Submodules are imported lazily - importing them here eagerly creates a cycle, because
|
||||
mechanics imports names from scene while the package is still initialising.
|
||||
"""
|
||||
@@ -0,0 +1,91 @@
|
||||
"""Through-beams across each lane entry: did the item actually get onto its lane, and at
|
||||
what blade angle and sweep rate.
|
||||
|
||||
The delivery number alone cannot tune the plow. An item that ends on the floor and one that
|
||||
never left the belt both score zero, but they need opposite corrections - the first was
|
||||
pushed too hard, the second not hard enough. A beam at the lane entry separates them: it
|
||||
fires the moment the item crosses onto the lane, so a run yields, per item,
|
||||
|
||||
crossed yes/no - did the push reach the lane at all
|
||||
angle deg - where the blade was at the crossing
|
||||
rate deg/s - how fast it was sweeping at that instant
|
||||
speed m/s - how fast the item was going as it crossed
|
||||
|
||||
which is what the sweep rate is tuned against. A rate that crosses every item but at high
|
||||
speed is throwing them; one that crosses none is too slow.
|
||||
|
||||
The beams are real `raycast_closest` queries, like the gate before the pusher, placed
|
||||
**along the lane entry line** rather than across the belt - the item is travelling sideways
|
||||
here, so the beam has to lie along the direction it is leaving.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from .. import config as C
|
||||
|
||||
# Beams sit just inside each lane entry, spanning the lane's width in X, so anything pushed
|
||||
# across breaks one. Y is the entry edge after scripts/move_lanes_inboard.py.
|
||||
# Beams sit ON each lane, not at its entry line, so a break means "this item is riding the
|
||||
# lane" rather than "this item touched the boundary". Each is an origin + direction + length,
|
||||
# because lane C is laid at 45 deg and cannot be described by a y value the way B can.
|
||||
#
|
||||
# lane B perpendicular, x -7.03..-6.57, y -2.38..-0.38 -> beam across it at y = -0.80
|
||||
# lane C 45 deg, near edge y = x + 7.637 -> beam across it at y = +0.90,
|
||||
# where the lane occupies roughly x -7.6..-6.7
|
||||
BEAMS = {
|
||||
# lane C: straight run, belt x[-10.00,-8.00] y[-0.45,0.00]; beam across it at x = -8.60
|
||||
"lane_C": dict(o=(-8.60, -0.50, C.BELT_Z + 0.03), d=(0.0, 1.0, 0.0), L=0.55),
|
||||
# lane B: 45 deg band from (-7.84,0.16) to (-9.25,1.57); beam across it 0.7 m in,
|
||||
# so its direction is the lane's perpendicular (0.707, 0.707), not a world axis.
|
||||
"lane_B": dict(o=(-8.53, 0.46, C.BELT_Z + 0.03), d=(0.7071, 0.7071, 0.0), L=0.55),
|
||||
}
|
||||
ITEMS_PREFIX = "/World/Items/"
|
||||
|
||||
|
||||
class LaneBeams:
|
||||
"""crossing detector at each lane entry"""
|
||||
|
||||
def __init__(self, stage, cell, plow=None):
|
||||
self.stage = stage
|
||||
self.cell = cell
|
||||
self.plow = plow
|
||||
self.crossings: dict[str, dict] = {} # item -> first crossing record
|
||||
self._t = 0.0
|
||||
from omni.physx import get_physx_scene_query_interface
|
||||
self._q = get_physx_scene_query_interface()
|
||||
|
||||
def tick(self, dt):
|
||||
self._t += dt
|
||||
|
||||
def _hit(self, b):
|
||||
"""name of whatever breaks this beam, else None"""
|
||||
h = self._q.raycast_closest(list(b["o"]), list(b["d"]), b["L"])
|
||||
if not h or not h.get("hit"):
|
||||
return None
|
||||
path = str(h.get("rigidBody") or h.get("collision") or "")
|
||||
if not path.startswith(ITEMS_PREFIX):
|
||||
return None
|
||||
return path[len(ITEMS_PREFIX):].split("/")[0] or None
|
||||
|
||||
def poll(self, rate=None):
|
||||
"""call each physics step; records the first crossing of each item"""
|
||||
for lane, b in BEAMS.items():
|
||||
name = self._hit(b)
|
||||
if name is None or name in self.crossings:
|
||||
continue
|
||||
try:
|
||||
v = self.cell._rp[name].get_velocities()[0].numpy()[0]
|
||||
speed = float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5)
|
||||
except Exception:
|
||||
speed = 0.0
|
||||
self.crossings[name] = dict(
|
||||
item=name, lane=lane, t=round(self._t, 3),
|
||||
angle=round(self.plow.angle, 1) if self.plow else None,
|
||||
commanded=round(self.plow.commanded, 1) if self.plow else None,
|
||||
rate=None if rate is None else round(rate, 1),
|
||||
speed=round(speed, 2))
|
||||
|
||||
def crossed(self, name):
|
||||
return name in self.crossings
|
||||
|
||||
def report(self):
|
||||
return list(self.crossings.values())
|
||||
@@ -0,0 +1,168 @@
|
||||
"""Runtime side of the cell: releasing goods, the laser gate and the pusher stroke.
|
||||
|
||||
Two hard-won rules are encoded here and should not be "simplified" away:
|
||||
|
||||
1. During simulation, read poses from RigidPrim.get_world_poses(). BBoxCache / XformCache
|
||||
return the AUTHORED transform, so a moving item looks frozen and every gate misfires.
|
||||
2. The blade retracts only when (a) the pushed item has cleared the belt and (b) nothing
|
||||
else is inside the blade's footprint. Retracting blindly sweeps the blade back through
|
||||
the next item and knocks it over.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
from pxr import Gf, UsdGeom
|
||||
|
||||
from .. import config as C
|
||||
from .scene import BLADE as BLADE_PATH, ITEMS_ROOT, BLADE_PARENT_Y
|
||||
|
||||
|
||||
class Cell:
|
||||
def __init__(self, stage, items):
|
||||
self.stage = stage
|
||||
self.items = list(items)
|
||||
self._blade_op = self._blade_translate_op()
|
||||
self._blade_base = self._blade_op.Get()
|
||||
from isaacsim.core.experimental.prims import RigidPrim
|
||||
self._rp = {n: RigidPrim(paths=[f"{ITEMS_ROOT}/{n}"]) for n in self.items}
|
||||
from omni.physx import get_physx_scene_query_interface
|
||||
self._query = get_physx_scene_query_interface()
|
||||
self.blade_to(C.BLADE_HOME_Y)
|
||||
|
||||
# -- blade --------------------------------------------------------------
|
||||
def _blade_translate_op(self):
|
||||
prim = self.stage.GetPrimAtPath(BLADE_PATH)
|
||||
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
|
||||
return op
|
||||
raise RuntimeError(f"{BLADE_PATH} has no translate op to drive")
|
||||
|
||||
def blade_to(self, y):
|
||||
"""y is a WORLD coordinate; the op lives in the diverter's frame"""
|
||||
b = self._blade_base
|
||||
self._blade_op.Set(Gf.Vec3d(b[0], y - BLADE_PARENT_Y, b[2]))
|
||||
|
||||
async def stroke(self, app, out=True, speed=None):
|
||||
"""sweep the blade at a commanded m/s; fine steps keep the contact impulse sane.
|
||||
Above ~2.5 m/s the kinematic blade throws goods off the line."""
|
||||
speed = min(speed or C.PUSHER_SPEED, C.PUSHER_MAX_SAFE)
|
||||
a, b = (C.BLADE_HOME_Y, C.BLADE_OUT_Y) if out else (C.BLADE_OUT_Y, C.BLADE_HOME_Y)
|
||||
dt = 1.0 / 60.0
|
||||
steps = max(4, int(round(abs(b - a) / max(speed * dt, 1e-6))))
|
||||
for i in range(steps + 1):
|
||||
self.blade_to(a + (b - a) * i / steps)
|
||||
await app.update_app_async(steps=1)
|
||||
return steps * dt
|
||||
|
||||
# -- item state ---------------------------------------------------------
|
||||
def pose(self, name):
|
||||
return self._rp[name].get_world_poses()[0].numpy()[0]
|
||||
|
||||
def place(self, name, pos):
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}")
|
||||
for op in UsdGeom.Xformable(prim).GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
|
||||
op.Set(Gf.Vec3d(*pos))
|
||||
return
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTransform:
|
||||
M = Gf.Matrix4d(op.Get())
|
||||
M.SetTranslateOnly(Gf.Vec3d(*pos))
|
||||
op.Set(M)
|
||||
return
|
||||
|
||||
def _underside_gap(self, name):
|
||||
"""distance from the prim origin down to its lowest point, so it can be seated"""
|
||||
cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True)
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}")
|
||||
r = cache.ComputeWorldBound(prim).ComputeAlignedRange()
|
||||
if r.IsEmpty():
|
||||
return 0.0
|
||||
origin = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(0).ExtractTranslation()
|
||||
return origin[2] - r.GetMin()[2]
|
||||
|
||||
def park(self, name, index=0):
|
||||
self.place(name, (9.0 + 1.2 * index, 5.0, 0.4))
|
||||
|
||||
def park_all(self):
|
||||
"""park everything AND freeze it, so the queue does not fall out of the world.
|
||||
|
||||
Parked items are ordinary dynamic bodies sitting off to the side at y ~ +5, where
|
||||
there is no floor under them - so the whole undispatched queue free-falls for the
|
||||
entire run. Measured: parked stock at z = -665 after a minute and the stage bound
|
||||
reaching z = -20438, which also wrecks every "frame the whole scene" camera because
|
||||
the scene is suddenly 20 km tall. Freezing them costs nothing and they are woken in
|
||||
`release`, which clears the flag before placing the item on the belt.
|
||||
"""
|
||||
from pxr import UsdPhysics
|
||||
for i, n in enumerate(self.items):
|
||||
self.park(n, i)
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{n}")
|
||||
if prim.IsValid() and prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(True)
|
||||
|
||||
def _thaw(self, name):
|
||||
"""let a parked item fall under gravity again, just before it is released"""
|
||||
from pxr import UsdPhysics
|
||||
prim = self.stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}")
|
||||
if prim.IsValid() and prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(False)
|
||||
|
||||
def release(self, name, y=0.0):
|
||||
self._thaw(name)
|
||||
self.place(name, (C.SPAWN_X, y, C.BELT_Z + 0.005))
|
||||
self.place(name, (C.SPAWN_X, y, C.BELT_Z + self._underside_gap(name) + 0.008))
|
||||
|
||||
# -- laser gate ---------------------------------------------------------
|
||||
def laser(self):
|
||||
"""name of whatever breaks the beam, else None. A real raycast, not a coordinate test."""
|
||||
hit = self._query.raycast_closest(
|
||||
[C.GATE_X, C.BEAM_Y0, C.BEAM_Z], [0.0, 1.0, 0.0], C.BEAM_Y1 - C.BEAM_Y0)
|
||||
if not hit or not hit.get("hit"):
|
||||
return None
|
||||
path = str(hit.get("rigidBody") or hit.get("collision") or "")
|
||||
for n in self.items:
|
||||
if f"{ITEMS_ROOT}/{n}" in path:
|
||||
return n
|
||||
return None
|
||||
|
||||
def blade_path_busy(self, exclude):
|
||||
for n in self.items:
|
||||
if n == exclude:
|
||||
continue
|
||||
p = self.pose(n)
|
||||
if (C.BLADE_X0 - 0.12 < p[0] < C.BLADE_X1 + 0.12) and (-0.30 < p[1] < 0.45):
|
||||
return n
|
||||
return None
|
||||
|
||||
async def divert(self, app, name, speed=None, max_wait_s=1.5):
|
||||
"""full push cycle with both retract interlocks"""
|
||||
dt = 1.0 / 60.0
|
||||
t = await self.stroke(app, out=True, speed=speed)
|
||||
for _ in range(int(max_wait_s / dt)): # item off the main line
|
||||
if self.pose(name)[1] > 0.50:
|
||||
break
|
||||
await app.update_app_async(steps=1)
|
||||
t += dt
|
||||
held = 0.0
|
||||
for _ in range(int(max_wait_s / dt)): # path clear for the return
|
||||
if self.blade_path_busy(name) is None:
|
||||
break
|
||||
await app.update_app_async(steps=1)
|
||||
t += dt
|
||||
held += dt
|
||||
t += await self.stroke(app, out=False, speed=speed)
|
||||
return t, held
|
||||
|
||||
# -- outcome ------------------------------------------------------------
|
||||
def where(self, name):
|
||||
"""bin / branch / line-end / line, from the simulated pose"""
|
||||
p = self.pose(name)
|
||||
if C.BIN_X0 < p[0] < C.BIN_X1 and C.BIN_Y0 < p[1] < C.BIN_Y1 and p[2] < C.BIN_LIP_Z:
|
||||
return "bin"
|
||||
if p[2] < C.BELT_Z - 0.35: # dropped off the end of the run
|
||||
return "line-end"
|
||||
if p[1] > 0.5:
|
||||
return "branch"
|
||||
if p[0] < C.MAIN_X0 + 0.35: # the run stops at MAIN_X0, not beyond it
|
||||
return "line-end"
|
||||
return "line"
|
||||
@@ -0,0 +1,211 @@
|
||||
"""The plow diverter (``DiverterEnd``) - the blade at the far end of the main run.
|
||||
|
||||
Mechanically it is the opposite of the pusher. The pusher is a kinematic slab shoved
|
||||
across the belt from script; the plow is a **dynamic arm on a revolute joint driven by an
|
||||
angular force drive**, so it is compliant - it yields on contact instead of teleporting
|
||||
through cargo. That is why this module commands a drive target rather than writing a
|
||||
transform the way ``mechanics.Cell.blade_to`` does.
|
||||
|
||||
Two consequences of that choice, both load-bearing:
|
||||
|
||||
1. **A drive target is a request, not a position.** The arm arrives when the solver gets it
|
||||
there, and USD drive writes reach PhysX with a lag (about a second in the worst case
|
||||
measured on the pusher's prismatic joint). Never assume the blade is where you last
|
||||
commanded it - read :meth:`Plow.angle`, which measures the arm's actual pose.
|
||||
|
||||
2. **Rate is not free.** The authored graph swings 30 deg in 7 ms (72 rad/s). At that rate
|
||||
the blade is an impulse and throws goods off the line. :meth:`Plow.step_toward` ramps the
|
||||
target at ``config.PLOW_RATE`` instead, which is what makes the motion sortable.
|
||||
|
||||
The scene keeps its authored ``DiverterAnimGraph``, so pressing Play alone makes the cell
|
||||
demonstrate itself. ``plow_cell.prepare(..., script_control=True)`` switches that graph off;
|
||||
until it is off, the graph rewrites the drive target every tick and fights this module.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
from pxr import UsdGeom, UsdPhysics
|
||||
|
||||
from .. import config as C
|
||||
|
||||
|
||||
def _yaw_deg(quat) -> float:
|
||||
"""Z rotation of a [w, x, y, z] quaternion, in degrees"""
|
||||
w, x, y, z = (float(v) for v in quat)
|
||||
return math.degrees(math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)))
|
||||
|
||||
|
||||
class Plow:
|
||||
"""Angular control of the plow arm.
|
||||
|
||||
``angle`` is measured from the arm's rest pose, so it is signed the same way as the
|
||||
authored throw: positive swings one way into the lane, negative the other.
|
||||
"""
|
||||
|
||||
def __init__(self, stage, hinge_path: str | None = None, arm_path: str | None = None,
|
||||
base_path: str | None = None, kinematic: bool = True):
|
||||
self.stage = stage
|
||||
self.hinge_path = hinge_path or C.PLOW_HINGE
|
||||
self.arm_path = arm_path or C.PLOW_ARM
|
||||
|
||||
hinge = stage.GetPrimAtPath(self.hinge_path)
|
||||
if not hinge.IsValid():
|
||||
raise RuntimeError(
|
||||
f"{self.hinge_path} missing - plow_cell.usd is the scene with the plow; "
|
||||
"sorter.usd carries the pusher only")
|
||||
self.drive = UsdPhysics.DriveAPI(hinge, "angular")
|
||||
if not self.drive:
|
||||
raise RuntimeError(f"{self.hinge_path} has no angular drive to command")
|
||||
|
||||
from isaacsim.core.experimental.prims import RigidPrim
|
||||
self._arm = RigidPrim(paths=[self.arm_path])
|
||||
# Reference the angle to the *base*, not to whatever pose the arm happened to hold
|
||||
# when this object was built. The base is kinematic and both bodies read yaw +180
|
||||
# at rest, so `yaw(arm) - yaw(base)` is the true joint angle and reads 0 at rest.
|
||||
#
|
||||
# Taking a snapshot instead was wrong and hid every other plow fault: a run that
|
||||
# started with the arm left at -30 from the previous run reported "commanded 0.0 ->
|
||||
# reached +30.47" and "commanded +30.0 -> reached -34.56", which looks like a
|
||||
# broken drive rather than a broken measurement.
|
||||
self._base = RigidPrim(paths=[base_path or C.PLOW_BASE])
|
||||
self.commanded = 0.0
|
||||
|
||||
# Kinematic mode: rotate the arm directly instead of asking a force drive to hold
|
||||
# an angle. The compliant drive was tuned three times (120000 / 3000 / 300) and
|
||||
# never held its target - at 3000 it rang between +-21.4 deg at 99 deg/s, faster
|
||||
# than the 76.4 deg/s ramp commanding it, which is the drive moving the arm rather
|
||||
# than the command. A kinematic arm turned at the ramp rate goes exactly where it
|
||||
# is put, which is what the pusher blade has always done.
|
||||
#
|
||||
# It gives up compliance, so the arm no longer yields on contact. That is safe here
|
||||
# only because the tip speed matches the belt (0.8 m/s): the blade leans goods over
|
||||
# at their own speed rather than batting them. MAX_DEPENETRATION still caps how
|
||||
# violently PhysX may separate a deep overlap.
|
||||
self._rot_op = None
|
||||
if kinematic:
|
||||
self._rot_op = self._ensure_rot_op()
|
||||
|
||||
def _ensure_rot_op(self):
|
||||
"""the arm's own rotateZ op, created if the authored prim has none.
|
||||
|
||||
The hinge sits at the arm's origin (localPos0 = localPos1 = 0), so turning the arm
|
||||
about its own Z reproduces the joint exactly.
|
||||
"""
|
||||
xf = UsdGeom.Xformable(self.stage.GetPrimAtPath(self.arm_path))
|
||||
for op in xf.GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeRotateZ:
|
||||
return op
|
||||
return xf.AddRotateZOp()
|
||||
|
||||
# -- state --------------------------------------------------------------
|
||||
def _yaw_of(self, prim) -> float:
|
||||
"""world yaw from the SIMULATED pose (never XformCache: during simulation that
|
||||
returns the authored transform and the arm looks frozen)"""
|
||||
return _yaw_deg(prim.get_world_poses()[1].numpy()[0])
|
||||
|
||||
@property
|
||||
def angle(self) -> float:
|
||||
"""true joint angle in degrees: the arm's yaw relative to the base it hinges on"""
|
||||
d = self._yaw_of(self._arm) - self._yaw_of(self._base)
|
||||
return (d + 180.0) % 360.0 - 180.0
|
||||
|
||||
def at(self, deg: float, tol: float = 1.0) -> bool:
|
||||
return abs(self.angle - deg) <= tol
|
||||
|
||||
# -- command ------------------------------------------------------------
|
||||
def target(self, deg: float, velocity: float | None = None) -> float:
|
||||
"""command the drive; the value is clamped inside the joint's own limit"""
|
||||
deg = max(-C.PLOW_LIMIT, min(C.PLOW_LIMIT, float(deg)))
|
||||
if self._rot_op is not None: # kinematic: put the arm there
|
||||
self._rot_op.Set(float(deg))
|
||||
else: # compliant: ask the drive to get there
|
||||
self.drive.GetTargetPositionAttr().Set(deg)
|
||||
if velocity is not None:
|
||||
self.drive.GetTargetVelocityAttr().Set(float(velocity))
|
||||
self.commanded = deg
|
||||
return deg
|
||||
|
||||
def home(self) -> float:
|
||||
"""centre the blade, out of the lane"""
|
||||
return self.target(0.0, velocity=0.0)
|
||||
|
||||
def gains(self, stiffness=None, damping=None, max_force=None):
|
||||
"""re-apply the authored gains, or override them for an experiment"""
|
||||
self.drive.GetStiffnessAttr().Set(float(
|
||||
C.PLOW_STIFFNESS if stiffness is None else stiffness))
|
||||
self.drive.GetDampingAttr().Set(float(
|
||||
C.PLOW_DAMPING if damping is None else damping))
|
||||
self.drive.GetMaxForceAttr().Set(float(
|
||||
C.PLOW_MAX_FORCE if max_force is None else max_force))
|
||||
|
||||
# -- motion -------------------------------------------------------------
|
||||
def step_toward(self, deg: float, dt: float, rate: float | None = None) -> bool:
|
||||
"""advance the commanded target one physics step toward `deg`.
|
||||
|
||||
Ramping the target is what keeps the blade sortable: commanding the endpoint
|
||||
outright makes the solver deliver it as an impulse. Returns True once the command
|
||||
has reached `deg` - the arm itself follows a little later, so gate on
|
||||
:meth:`at` if you need the blade physically there.
|
||||
"""
|
||||
rate = C.PLOW_RATE if rate is None else rate
|
||||
step = rate * dt
|
||||
delta = deg - self.commanded
|
||||
if abs(delta) <= step:
|
||||
self.target(deg, velocity=0.0)
|
||||
return True
|
||||
self.target(self.commanded + math.copysign(step, delta),
|
||||
velocity=math.copysign(rate, delta))
|
||||
return False
|
||||
|
||||
async def swing(self, app, deg: float, rate: float | None = None,
|
||||
settle_s: float = 0.5, dt: float = 1.0 / 60.0) -> float:
|
||||
"""drive the blade to `deg` and wait for the arm to actually arrive"""
|
||||
rate = C.PLOW_RATE if rate is None else rate
|
||||
t = 0.0
|
||||
while not self.step_toward(deg, dt, rate):
|
||||
await app.update_app_async(steps=1)
|
||||
t += dt
|
||||
for _ in range(int(settle_s / dt)): # the arm lags the command
|
||||
if self.at(deg):
|
||||
break
|
||||
await app.update_app_async(steps=1)
|
||||
t += dt
|
||||
return t
|
||||
|
||||
async def divert(self, app, side: float = 1.0, dwell_s: float | None = None,
|
||||
rate: float | None = None) -> float:
|
||||
"""full cycle: swing into the lane, hold, return to centre"""
|
||||
dwell_s = C.PLOW_HOLD if dwell_s is None else dwell_s
|
||||
deg = math.copysign(C.PLOW_SWING, side)
|
||||
t = await self.swing(app, deg, rate)
|
||||
for _ in range(int(dwell_s / (1.0 / 60.0))):
|
||||
await app.update_app_async(steps=1)
|
||||
t += 1.0 / 60.0
|
||||
t += await self.swing(app, 0.0, rate)
|
||||
return t
|
||||
|
||||
# -- the authored profile, in Python -------------------------------------
|
||||
@staticmethod
|
||||
def authored_profile(t: float, rate: float | None = None) -> tuple[float, float]:
|
||||
"""(target_deg, target_deg_per_s) of the scene's own OmniGraph loop at time `t`.
|
||||
|
||||
Reimplemented so the demo motion is available from code. `rate` defaults to
|
||||
``config.PLOW_RATE`` rather than the authored 4125 deg/s; pass
|
||||
``config.PLOW_RATE_AUTHORED`` to reproduce the scene exactly, impulse and all.
|
||||
"""
|
||||
rate = C.PLOW_RATE if rate is None else rate
|
||||
a = C.PLOW_SWING
|
||||
hold, ts = C.PLOW_HOLD, a / rate
|
||||
segs = [(hold, 0.0, 0.0), (ts, a, rate), (hold, a, 0.0), (ts, 0.0, -rate),
|
||||
(hold, 0.0, 0.0), (ts, -a, -rate), (hold, -a, 0.0), (ts, 0.0, rate),
|
||||
(hold, 0.0, 0.0)]
|
||||
period = sum(s[0] for s in segs)
|
||||
c, t0, pos0 = t % period, 0.0, 0.0
|
||||
for dur, pos1, vel in segs:
|
||||
if c <= t0 + dur + 1e-9:
|
||||
u = 0.0 if dur <= 0 else (c - t0) / dur
|
||||
pos = pos0 + (pos1 - pos0) * u if abs(vel) > 1e-6 else pos1
|
||||
return float(pos), float(vel)
|
||||
t0, pos0 = t0 + dur, pos1
|
||||
return 0.0, 0.0
|
||||
@@ -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)
|
||||
@@ -0,0 +1,484 @@
|
||||
"""Runtime setup for scene/plow_cell_90_45_test.usd - the plow cell with the 90-degree
|
||||
corner exit (ConveyorTrack_06) replacing plow_cell.usd's 45-degree lane.
|
||||
|
||||
Topology differences from plow_cell.usd, all measured on the live stage (not assumed):
|
||||
* ConveyorTrack_01 is now part of the MAIN RUN (local +X -> world -X) instead of being
|
||||
the plow's own lane - it is what carries class C onward to its container.
|
||||
* ConveyorTrack_06 is new: a 90-degree corner that carries class B out to +Y.
|
||||
* config.PLOW_PRESET needs no change: B=-16 deg was measured driving items to +Y (onto
|
||||
ConveyorTrack_06 -> container B), C=+16 deg to -Y (onto ConveyorTrack_01 ->
|
||||
container C) - the same signs plow_sort.py already uses for the old layout.
|
||||
|
||||
Two bugs fixed here for good, both cost a session each to find:
|
||||
* `prim.SetActive(False)` on a ConveyorBeltGraph/DiverterAnimGraph does NOT stop an
|
||||
already-instantiated OmniGraph exec - it keeps writing zero into surfaceVelocity (or
|
||||
the plow's drive target) every tick regardless of the prim's active state. The graph
|
||||
node has to be REMOVED (`stage.RemovePrim`), not deactivated.
|
||||
* The plow's corner decks (PlowCornerDeck_B/C, PlowTransition_B/C) are static plates:
|
||||
an item that slides off the belt onto one, under only the sideways push the plow gave
|
||||
it, loses its drive the instant it clears the belt and stops dead on the plate -
|
||||
exactly plow_sort.py's "touches and then just sits there" symptom. They have to be
|
||||
driven too, toward whichever real belt segment is physically next - by MEASURED
|
||||
position, not by the deck's own name: PlowCornerDeck_B in this build sits on the
|
||||
geometric path toward container C, not container B.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pxr import Gf, Usd, UsdGeom, UsdLux, UsdPhysics, UsdShade
|
||||
|
||||
from .. import config as C
|
||||
from . import scene as _scene
|
||||
from .plow_cell import GRIP_MATERIAL, configure_plow, drive_belt
|
||||
|
||||
SCENE = C.ROOT / "scene" / "plow_cell_90_45_test.usd"
|
||||
|
||||
# _scene.BELTS (5: ConveyorTrack, _02, _03, _04, _01) is the SORTER scene's list and does
|
||||
# not cover this cell at all - it is missing ConveyorTrack_05, the entry segment items are
|
||||
# actually spawned onto (x 0..+2, the first belt in the run). Driven the same -X way as the
|
||||
# rest of the main run below. ConveyorTrack_06 (the 90-degree corner) is NOT in this list -
|
||||
# it needs a different world direction (0,+1,0) and is driven separately in configure_belts.
|
||||
BELTS = _scene.BELTS + ["/World/ConveyorTrack_05/Belt"]
|
||||
TRACKS = ("ConveyorTrack", "ConveyorTrack_01", "ConveyorTrack_02", "ConveyorTrack_03",
|
||||
"ConveyorTrack_04", "ConveyorTrack_05", "ConveyorTrack_06")
|
||||
|
||||
# Belt top z=1.781 everywhere on the main run; ConveyorTrack_05 is the line's entry, local
|
||||
# +X -> world +X (the only segment laid that way - everything else is world -X already).
|
||||
ENTRY_BELT = "/World/ConveyorTrack_05/Belt"
|
||||
ENTRY_X, ENTRY_Y = 1.80, 0.0 # near the +X (upstream) end of ConveyorTrack_05's 0..+2 span
|
||||
|
||||
GROUND_Z = C.FLOOR_Z # 0.0 - matches the sorter scene's own floor constant
|
||||
GROUND_PATH = "/World/_Ground"
|
||||
LIGHT_PATH = "/Environment/_BrightFill"
|
||||
|
||||
# Deck -> unit world direction aiming at the CENTRE of the real belt it physically feeds
|
||||
# into. Computed from UsdGeom.BBoxCache on the live stage, not guessed from the deck's
|
||||
# name - the names are stale (see module docstring). Re-derive if the scene is re-laid.
|
||||
DECK_DIR = {
|
||||
"/World/PlowTransition_B": (-0.9995, 0.0309, 0.0), # feeds ConveyorTrack_01 (class C)
|
||||
"/World/PlowCornerDeck_B": (-0.9716, 0.2367, 0.0), # feeds ConveyorTrack_01 (class C)
|
||||
"/World/PlowTransition_C": (-0.9945, -0.1047, 0.0), # feeds ConveyorTrack_06 (class B)
|
||||
"/World/PlowCornerDeck_C": (-0.9995, -0.0302, 0.0), # feeds ConveyorTrack_06 (class B)
|
||||
}
|
||||
|
||||
|
||||
PUSHER_GEOM = "/World/Diverters/DiverterY_Split/Pusher/Geom"
|
||||
# Footprint along the belt. The authored blade was 1200 mm - a near-wall - and 500 mm was
|
||||
# the requested replacement, but 500 mm is provably too narrow for THIS belt speed:
|
||||
# * momentum transfer falls off with blade speed (measured dy: 1.3 m/s -> 0.17..0.22 m,
|
||||
# 1.8 m/s -> 0.01..0.08 m), because a transform-driven kinematic blade shoves by
|
||||
# depenetration rather than by carrying - so the stroke wants to be SLOW;
|
||||
# * a slow stroke (0.82 m at 1.3 m/s = 0.63 s) needs 0.63 m of blade to stay in contact
|
||||
# at 1 m/s belt speed, but 500 mm only gives 0.50 s, so the item slid off the trailing
|
||||
# edge halfway through and left with a third of the needed displacement.
|
||||
# 800 mm satisfies both (0.80 s of contact for a 0.63 s stroke) and is still a third
|
||||
# shorter than the 1200 mm original.
|
||||
PUSHER_X_MM = 800.0
|
||||
|
||||
|
||||
def resize_pusher_blade(stage, x_mm=PUSHER_X_MM):
|
||||
"""the authored blade is a Cube scaled (1.2, 0.06, 0.3) - 1200 mm along the belt
|
||||
(X), a near-wall rather than a paddle. Only the X (along-belt) scale changes; Y
|
||||
(cross-belt thickness) and Z (height) are load-bearing as measured elsewhere and
|
||||
stay put. Idempotent: re-reads and re-derives from whatever scale is currently there."""
|
||||
prim = stage.GetPrimAtPath(PUSHER_GEOM)
|
||||
if not prim.IsValid():
|
||||
return None
|
||||
xf = UsdGeom.Xformable(prim)
|
||||
for op in xf.GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeScale:
|
||||
s = op.Get()
|
||||
op.Set(Gf.Vec3f(x_mm / 1000.0, s[1], s[2]))
|
||||
return (x_mm / 1000.0, s[1], s[2])
|
||||
return None
|
||||
|
||||
|
||||
PUSHER_GRIP_MATERIAL = "/World/_PusherGrip"
|
||||
|
||||
|
||||
def grip_pusher_blade(stage, static_f=1.1, dynamic_f=0.95):
|
||||
"""the blade face is bound to /World/Diverters/DiverterMaterial (static/dynamic
|
||||
friction 0.12/0.08) - deliberately slick for the PLOW's blade (config.PLOW_BLADE_
|
||||
FRICTION, so goods slide along its edge instead of piling up), but the pusher shares
|
||||
that same authored material and inherits the slickness for free. Measured on an
|
||||
isolated item: it picks up a brief lateral velocity spike on contact and then the
|
||||
blade sweeps clean past it - a flick, not a carry (0.42 m commanded stroke, item ends
|
||||
up 0.05 m over). A high-friction grip material, bound stronger-than-descendants same
|
||||
as the belts' own grip, is what a real pusher gate needs: it should carry the item
|
||||
with it, not glance off."""
|
||||
prim = stage.GetPrimAtPath(PUSHER_GEOM)
|
||||
if not prim.IsValid():
|
||||
return None
|
||||
grip = stage.GetPrimAtPath(PUSHER_GRIP_MATERIAL)
|
||||
if not grip.IsValid():
|
||||
grip = stage.DefinePrim(PUSHER_GRIP_MATERIAL, "Material")
|
||||
pm = UsdPhysics.MaterialAPI.Apply(grip)
|
||||
pm.CreateStaticFrictionAttr().Set(static_f)
|
||||
pm.CreateDynamicFrictionAttr().Set(dynamic_f)
|
||||
pm.CreateRestitutionAttr().Set(0.0)
|
||||
api = UsdShade.MaterialBindingAPI.Apply(prim)
|
||||
api.Bind(UsdShade.Material(grip), bindingStrength=UsdShade.Tokens.strongerThanDescendants,
|
||||
materialPurpose="physics")
|
||||
return (static_f, dynamic_f)
|
||||
|
||||
|
||||
PUSHER_XFORM = "/World/Diverters/DiverterY_Split/Pusher"
|
||||
PUSHER_CLEARANCE = 0.002 # target gap between the blade's bottom edge and the belt top
|
||||
|
||||
|
||||
def seat_pusher_blade(stage, clearance=PUSHER_CLEARANCE):
|
||||
"""scene.py's configure_pusher() seats the blade at a hardcoded local z=-0.135,
|
||||
which measured 14 mm above the belt (1.795 vs belt top 1.781) - fine for the boxy
|
||||
items it was tuned on, but taller than `plate` (9 mm) or `pen` (5 mm), which pass
|
||||
clean underneath no matter how the sweep speed/friction is tuned. Lower it to a
|
||||
small measured clearance above the belt instead of trusting the hardcoded offset."""
|
||||
blade = stage.GetPrimAtPath(PUSHER_XFORM)
|
||||
belt = stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt")
|
||||
if not blade.IsValid() or not belt.IsValid():
|
||||
return None
|
||||
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
|
||||
blade_bottom = bbc.ComputeWorldBound(blade).ComputeAlignedRange().GetMin()[2]
|
||||
belt_top = bbc.ComputeWorldBound(belt).ComputeAlignedRange().GetMax()[2]
|
||||
drop = (blade_bottom - belt_top) - clearance
|
||||
if drop <= 0:
|
||||
return blade_bottom, belt_top, 0.0
|
||||
for op in UsdGeom.Xformable(blade).GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
|
||||
v = op.Get()
|
||||
op.Set(Gf.Vec3d(v[0], v[1], v[2] - drop))
|
||||
return blade_bottom, belt_top, drop
|
||||
return None
|
||||
|
||||
|
||||
def _kill_stale_graphs(stage):
|
||||
"""remove (not deactivate) every ConveyorBeltGraph and the DiverterAnimGraph - see
|
||||
module docstring. Safe to call more than once; RemovePrim on a missing path is a no-op
|
||||
check via IsValid() first."""
|
||||
killed = []
|
||||
for track in TRACKS:
|
||||
for graph in (f"/World/{track}/ConveyorBeltGraph", f"/World/{track}/ConveyorBeltGraph_01"):
|
||||
p = stage.GetPrimAtPath(graph)
|
||||
if p.IsValid():
|
||||
stage.RemovePrim(p.GetPath())
|
||||
killed.append(graph)
|
||||
p = stage.GetPrimAtPath("/World/Diverters/DiverterAnimGraph")
|
||||
if p.IsValid():
|
||||
stage.RemovePrim(p.GetPath())
|
||||
killed.append("/World/Diverters/DiverterAnimGraph")
|
||||
return killed
|
||||
|
||||
|
||||
def add_ground_and_light(stage):
|
||||
"""this bare mechanical cell (see module docstring: no camera portal, no laser gate,
|
||||
no item library) also ships with no ground plane and a single DistantLight - fine for
|
||||
a dry mechanics smoke test, useless for watching goods over WebRTC: anything that
|
||||
overshoots a belt or a container (the pusher has thrown items tens of metres in this
|
||||
same cell before) free-falls forever and the scene reads as half-lit. A big static
|
||||
collider under the whole cell plus a bright DomeLight fix both, idempotently."""
|
||||
ground = stage.GetPrimAtPath(GROUND_PATH)
|
||||
if not ground.IsValid():
|
||||
cube = UsdGeom.Cube.Define(stage, GROUND_PATH)
|
||||
cube.CreateSizeAttr().Set(1.0) # unit cube, half-extent 0.5 before scale
|
||||
xf = UsdGeom.Xformable(cube.GetPrim())
|
||||
# covers x -15..+25 (both the conveyor/container area AND the item park slots
|
||||
# off at x 9..21), y -8..+10, top surface at GROUND_Z
|
||||
xf.AddTranslateOp().Set(Gf.Vec3d(5.0, 1.0, GROUND_Z - 0.5))
|
||||
xf.AddScaleOp().Set(Gf.Vec3f(40.0, 18.0, 1.0))
|
||||
prim = cube.GetPrim()
|
||||
UsdPhysics.CollisionAPI.Apply(prim)
|
||||
ground = prim
|
||||
UsdGeom.Imageable(ground).MakeVisible()
|
||||
|
||||
light = stage.GetPrimAtPath(LIGHT_PATH)
|
||||
if not light.IsValid():
|
||||
dome = UsdLux.DomeLight.Define(stage, LIGHT_PATH)
|
||||
dome.CreateIntensityAttr().Set(2500.0)
|
||||
dome.CreateColorAttr().Set(Gf.Vec3f(1.0, 1.0, 1.0))
|
||||
light = dome.GetPrim()
|
||||
UsdGeom.Imageable(light).MakeVisible()
|
||||
return dict(ground=str(ground.GetPath()), light=str(light.GetPath()))
|
||||
|
||||
|
||||
RAIL_PATH = "/World/_Rails"
|
||||
# Straight transport-only segments where NOTHING is ever meant to leave sideways.
|
||||
# ConveyorTrack_04 was already excluded (the plow deflects goods clear off its edge onto
|
||||
# the junction decks). Measured live and fixed here: ConveyorTrack_03 (the pusher shoves
|
||||
# goods off ITS +Y edge onto the branch), ConveyorTrack_06 and ConveyorTrack_01 (the
|
||||
# plow's own two deflection targets) all got the same treatment as _04 - and each grew a
|
||||
# rail directly across its own intended entry/exit, which is exactly the pile-up seen at
|
||||
# the plow and the "pusher pushes but the item just stays on the belt" symptom: the pusher
|
||||
# WAS working (an isolated single-item test got it 97% of the way to the branch) - it was
|
||||
# arriving at a wall this module had just built.
|
||||
RAIL_BELTS = ("/World/ConveyorTrack_05/Belt", "/World/ConveyorTrack/Belt",
|
||||
"/World/ConveyorTrack_02/Belt")
|
||||
RAIL_HEIGHT = 0.08 # low guard, enough to stop a bounce/overshoot, not a wall
|
||||
|
||||
|
||||
def add_side_rails(stage):
|
||||
"""low invisible guards along the long edges of straight runs, so a jostled item
|
||||
rolls back onto the belt instead of pitching off into open air (measured happening -
|
||||
the pusher alone has thrown items metres off the line before). Computed from each
|
||||
belt's OWN live bbox, not hand-picked numbers - segments are laid at different
|
||||
orientations and a constant y +-0.45 is wrong on at least one of them."""
|
||||
bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render])
|
||||
root = stage.GetPrimAtPath(RAIL_PATH)
|
||||
if not root.IsValid():
|
||||
UsdGeom.Xform.Define(stage, RAIL_PATH)
|
||||
built = []
|
||||
for belt in RAIL_BELTS:
|
||||
prim = stage.GetPrimAtPath(belt)
|
||||
if not prim.IsValid():
|
||||
continue
|
||||
r = bbc.ComputeWorldBound(prim).ComputeAlignedRange()
|
||||
mn, mx = r.GetMin(), r.GetMax()
|
||||
dx, dy = mx[0] - mn[0], mx[1] - mn[1]
|
||||
top = mx[2]
|
||||
long_axis_x = dx >= dy # which local axis is the belt's length vs its width
|
||||
safe_name = belt.replace("/", "_")
|
||||
for side, edge in ((0, mn), (1, mx)):
|
||||
path = f"{RAIL_PATH}/{safe_name}_{side}"
|
||||
if stage.GetPrimAtPath(path).IsValid():
|
||||
built.append(path)
|
||||
continue
|
||||
cube = UsdGeom.Cube.Define(stage, path)
|
||||
cube.CreateSizeAttr().Set(1.0)
|
||||
xf = UsdGeom.Xformable(cube.GetPrim())
|
||||
if long_axis_x:
|
||||
cx, hx = (mn[0] + mx[0]) / 2.0, dx / 2.0 + 0.05
|
||||
cy = edge[1]
|
||||
sx, sy = hx * 2.0, 0.02
|
||||
else:
|
||||
cx = edge[0]
|
||||
cy, hy = (mn[1] + mx[1]) / 2.0, dy / 2.0 + 0.05
|
||||
sx, sy = 0.02, hy * 2.0
|
||||
xf.AddTranslateOp().Set(Gf.Vec3d(cx, cy, top + RAIL_HEIGHT / 2.0))
|
||||
xf.AddScaleOp().Set(Gf.Vec3f(sx, sy, RAIL_HEIGHT))
|
||||
UsdPhysics.CollisionAPI.Apply(cube.GetPrim())
|
||||
UsdGeom.Imageable(cube.GetPrim()).MakeInvisible()
|
||||
built.append(path)
|
||||
return built
|
||||
|
||||
|
||||
def _ensure_grip_material(stage):
|
||||
"""drive_belt()'s default grip_path (plow_cell.GRIP_MATERIAL, /World/PlowCell/
|
||||
M_beltPhysics) is only ever CREATED inside plow_cell.configure_belts() - this module
|
||||
calls drive_belt() directly and never that function, so the material prim never
|
||||
existed, `grip.IsValid()` was False on every single call, and every deck/belt driven
|
||||
here kept whatever friction it already had (or nothing) instead of getting bound to
|
||||
the intended high-grip surface. The main belts happened to already carry their own
|
||||
per-track authored material (0.9/0.9) and looked fine by accident; the plow-junction
|
||||
decks have no such authored material and were the ones left exposed."""
|
||||
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)
|
||||
return grip
|
||||
|
||||
|
||||
def regrip_decks(stage, static_f=1.1, dynamic_f=0.95):
|
||||
"""configure_plow() runs after configure_belts() and rebinds the transition plates
|
||||
(PlowTransition_B/C) to /World/PlowCell/M_plowSection - a deliberately slippery
|
||||
material (0.7/0.6, config.PLOW_SECTION_FRICTION) by original design, so the plow's
|
||||
blade can slide an item across rather than have the plate fight it. This module also
|
||||
tries to conveyor-DRIVE those same plates (DECK_DIR), which needs grip, not slip - the
|
||||
two designs are in direct conflict, and 'strongerThanDescendants' meant the slippery
|
||||
one always won. Measured effect: items sitting on a plate that is moving under them
|
||||
but barely dragging them - the multi-second "stuck" crawl on the kinematics log.
|
||||
PlowCornerDeck_B/C had no material bound at all (checked live) for the same reason as
|
||||
_ensure_grip_material above. Re-bind all four, stronger again, after configure_plow."""
|
||||
grip = _ensure_grip_material(stage)
|
||||
mat = UsdShade.Material(grip)
|
||||
bound = []
|
||||
for path in DECK_DIR:
|
||||
prim = stage.GetPrimAtPath(path)
|
||||
if not prim.IsValid():
|
||||
continue
|
||||
api = UsdShade.MaterialBindingAPI.Apply(prim)
|
||||
api.Bind(mat, bindingStrength=UsdShade.Tokens.strongerThanDescendants,
|
||||
materialPurpose="physics")
|
||||
bound.append(path)
|
||||
return bound
|
||||
|
||||
|
||||
# The conveyor ART prim of each track (SM_ConveyorBelt_*) carries its own collider, and
|
||||
# that includes the blue SIDE RAILS running the full length of the track. At a plow/pusher
|
||||
# station the rails have to be cut away on the discharge side - goods leave the belt
|
||||
# sideways there by design. plow_sort.py documents this exactly ("Left in place they simply
|
||||
# stop everything at the lane entry, which is what 'nothing reaches the bins' looked like")
|
||||
# and provides open_junction() for it; this module never called it, so ConveyorTrack_04's
|
||||
# shell (y -0.58..+0.58, collision on) stood as a wall right where class-B goods are pushed
|
||||
# out - measured: B items deflected correctly to y~+0.48 then sat there for 55-58 s.
|
||||
# Only the decorative shell loses its collider; every Belt keeps its own, so goods still
|
||||
# ride on a real surface and cannot fall through.
|
||||
JUNCTION_SHELLS = (
|
||||
"/World/ConveyorTrack_04/SM_ConveyorBelt_A06_02", # the run through the plow
|
||||
"/World/ConveyorTrack_04/SM_ConveyorBelt_A06_Decal_02",
|
||||
"/World/ConveyorTrack_01/SM_ConveyorBelt_A06_02", # class-C lane
|
||||
"/World/ConveyorTrack_01/SM_ConveyorBelt_A06_Decal_02",
|
||||
"/World/ConveyorTrack_06/SM_ConveyorBelt_A03", # class-B lane (90 deg corner)
|
||||
"/World/ConveyorTrack_06/SM_ConveyorBelt_A03_Decal",
|
||||
"/World/ConveyorTrack_03/SM_ConveyorBelt_A21_02", # the pusher's own discharge
|
||||
"/World/ConveyorTrack_03/SM_ConveyorBelt_A21_Decal_02",
|
||||
)
|
||||
|
||||
|
||||
def open_junction(stage):
|
||||
"""drop the decorative shell colliders at the plow and pusher discharge points"""
|
||||
opened = []
|
||||
for path in JUNCTION_SHELLS:
|
||||
prim = stage.GetPrimAtPath(path)
|
||||
if not prim.IsValid():
|
||||
continue
|
||||
attr = prim.GetAttribute("physics:collisionEnabled")
|
||||
if not attr:
|
||||
attr = UsdPhysics.CollisionAPI.Apply(prim).CreateCollisionEnabledAttr()
|
||||
attr.Set(False)
|
||||
opened.append(path)
|
||||
return opened
|
||||
|
||||
|
||||
PUSH_SECTION_MATERIAL = "/World/_PushSectionSlip"
|
||||
|
||||
|
||||
def slip_pusher_section(stage, static_f=0.30, dynamic_f=0.25):
|
||||
"""lower the friction of the belt the pusher discharges from.
|
||||
|
||||
The grip material this module binds to every belt (1.1/0.95) is right for carrying
|
||||
goods along the line, but at the pusher it is the thing the blade has to fight: a
|
||||
0.6 kg item on mu=0.95 resists lateral motion with ~5.3 N, and the measured result was
|
||||
the blade sweeping its full 0.82 m stroke while the item slid only 0.15-0.22 m across
|
||||
it - a slip, not a transfer. The project's own plow code solves the same problem the
|
||||
same way (config.PLOW_SECTION_FRICTION 0.70/0.60 on the transition plates, and 0.05/
|
||||
0.04 on the blade face) so goods can slide sideways off the belt.
|
||||
|
||||
Applied to ConveyorTrack_03/Belt only - the pusher's own discharge section. Its
|
||||
surfaceVelocity still carries items along the line; 0.30/0.25 is ample for that at
|
||||
1 m/s while letting the blade drive them across.
|
||||
"""
|
||||
prim = stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt")
|
||||
if not prim.IsValid():
|
||||
return None
|
||||
mat_prim = stage.GetPrimAtPath(PUSH_SECTION_MATERIAL)
|
||||
if not mat_prim.IsValid():
|
||||
mat_prim = stage.DefinePrim(PUSH_SECTION_MATERIAL, "Material")
|
||||
pm = UsdPhysics.MaterialAPI.Apply(mat_prim)
|
||||
pm.CreateStaticFrictionAttr().Set(static_f)
|
||||
pm.CreateDynamicFrictionAttr().Set(dynamic_f)
|
||||
pm.CreateRestitutionAttr().Set(0.0)
|
||||
api = UsdShade.MaterialBindingAPI.Apply(prim)
|
||||
api.Bind(UsdShade.Material(mat_prim),
|
||||
bindingStrength=UsdShade.Tokens.strongerThanDescendants,
|
||||
materialPurpose="physics")
|
||||
return (static_f, dynamic_f)
|
||||
|
||||
|
||||
def configure_belts(stage, speed=None):
|
||||
"""drive all 7 main belts plus the 4 static plow-junction decks, each by its
|
||||
measured world direction. Must run AFTER _kill_stale_graphs - otherwise the graphs
|
||||
zero the velocity this sets a few physics steps after play()."""
|
||||
speed = speed if speed is not None else C.BELT_SPEED
|
||||
_ensure_grip_material(stage)
|
||||
driven = {}
|
||||
for path in BELTS:
|
||||
v = drive_belt(stage, path, (-1, 0, 0), speed)
|
||||
if v:
|
||||
driven[path] = v
|
||||
# NOT pure +Y. ConveyorTrack_06's belt spans x -8.97..-8.00, y +0.03..+1.05, and
|
||||
# container_B sits at x -9.26..-8.36, y +1.07..+1.87. A class-B item is deflected onto
|
||||
# _06 near its +X edge (~x -8.05); driving straight +Y then walks it up the belt at
|
||||
# CONSTANT x and it falls off the far edge at x~-8.02 - 0.34 m short of the container's
|
||||
# near wall. Measured exactly that: B items reached y +1.15/+1.28 and dropped to the
|
||||
# floor at x -8.03/-8.01. Aim the belt diagonally at the container centre instead.
|
||||
v = drive_belt(stage, "/World/ConveyorTrack_06/Belt", (-0.5447, 0.8386, 0), speed)
|
||||
if v:
|
||||
driven["/World/ConveyorTrack_06/Belt"] = v
|
||||
# the pusher's own branch - carries a pushed D item on from the shove into BinD.
|
||||
# plow_cell.py's configure_belts() drives this; this module's own list above never
|
||||
# did, so a pushed item landed on a branch with no belt force and just sat there.
|
||||
# Same pure-+Y bug as ConveyorTrack_06 had, measured the same way: an item placed on
|
||||
# Belt_01 at (-4.10,+0.70) rode +Y to y=1.92 at CONSTANT x=-4.10 and fell off the far
|
||||
# edge - BinD's floor is x -6.21..-4.95, so it missed by 0.85 m. The belt does carry
|
||||
# (friction 1.1/0.95, |v|=1.0 confirmed); it was simply pointed past the bin. Aim it
|
||||
# at the BinD floor centre instead.
|
||||
v = drive_belt(stage, _scene.BRANCH, (-0.6976, 0.7165, 0), speed)
|
||||
if v:
|
||||
driven[_scene.BRANCH] = v
|
||||
for path, direction in DECK_DIR.items():
|
||||
v = drive_belt(stage, path, direction, speed)
|
||||
if v:
|
||||
driven[path] = v
|
||||
return driven
|
||||
|
||||
|
||||
async def open_scene(usd_path=None):
|
||||
"""the SYNC `open_stage` + a settle margin, not `open_stage_async` - the async loader
|
||||
returns while background layer composition is still touching the stage on another
|
||||
thread, which trips Kit's 'Detected usd threading violation' guard the moment
|
||||
configure_physics() edits the stage. A live WebRTC stream keeps Hydra populating the
|
||||
freshly-opened ~360 prims on its own thread well after `is_stage_loading()` clears, so
|
||||
the margin here is generous on purpose - short margins measured flaky on this scene
|
||||
while streaming is active."""
|
||||
import asyncio
|
||||
import omni.usd
|
||||
import isaacsim.core.experimental.utils.app as app_utils
|
||||
path = str(usd_path or SCENE)
|
||||
omni.usd.get_context().open_stage(path)
|
||||
await app_utils.update_app_async(steps=120)
|
||||
await asyncio.sleep(3.0)
|
||||
await app_utils.update_app_async(steps=60)
|
||||
return omni.usd.get_context().get_stage()
|
||||
|
||||
|
||||
async def _retrying(fn, *args, tries=12, **kwargs):
|
||||
"""call fn(*args) with a small settle-and-retry loop.
|
||||
|
||||
UsdPhysics/PhysX edits on a just-opened stage race a live WebRTC session's background
|
||||
Hydra-populate thread: 'Detected usd threading violation' (pxr.Tf.ErrorException,
|
||||
which derives from BaseException, not Exception, and carries no message in str() - the
|
||||
diagnostic text is printed separately by Tf's own delegate). It clears within a step
|
||||
or two once that thread catches up, so each of prepare()'s five sub-calls gets its own
|
||||
short retry here rather than re-running the whole sequence from the top on every miss.
|
||||
"""
|
||||
import asyncio
|
||||
import isaacsim.core.experimental.utils.app as app_utils
|
||||
last_exc = None
|
||||
for attempt in range(tries):
|
||||
try:
|
||||
return fn(*args, **kwargs)
|
||||
except BaseException as exc:
|
||||
last_exc = exc
|
||||
await app_utils.update_app_async(steps=60)
|
||||
await asyncio.sleep(1.0)
|
||||
raise last_exc
|
||||
|
||||
|
||||
async def prepare(stage, belt_speed=None, script_control: bool = True, kinematic_arm: bool = True):
|
||||
"""everything the new-topology scene needs before the belts and the plow will run"""
|
||||
await _retrying(_scene.configure_physics, stage)
|
||||
killed = await _retrying(_kill_stale_graphs, stage)
|
||||
belts = await _retrying(configure_belts, stage, belt_speed)
|
||||
plow = await _retrying(configure_plow, stage, script_control, kinematic_arm)
|
||||
regripped = await _retrying(regrip_decks, stage)
|
||||
await _retrying(_scene.configure_pusher, stage)
|
||||
pusher_dims = await _retrying(resize_pusher_blade, stage)
|
||||
await _retrying(grip_pusher_blade, stage)
|
||||
seat = await _retrying(seat_pusher_blade, stage)
|
||||
# slip_pusher_section() is deliberately NOT called: lowering the pusher belt's
|
||||
# friction to 0.30/0.25 did not improve the push at all (dy stayed ~0.21 m, the
|
||||
# same value it holds across every blade speed, width and fire-timing tried) and
|
||||
# it cost a class-C delivery. Kept above for the record - the ~0.21 m ceiling is
|
||||
# not a friction problem.
|
||||
env = await _retrying(add_ground_and_light, stage)
|
||||
rails = await _retrying(add_side_rails, stage)
|
||||
opened = await _retrying(open_junction, stage)
|
||||
return dict(script_control=script_control, plow_ready=plow, belts=belts,
|
||||
graphs_removed=killed, env=env, pusher_dims=pusher_dims, rails=len(rails),
|
||||
pusher_seat=seat, decks_regripped=regripped, junction_opened=len(opened),
|
||||
belt_speed=C.BELT_SPEED if belt_speed is None else belt_speed)
|
||||
|
||||
|
||||
async def load(usd_path=None, belt_speed=None, script_control: bool = True):
|
||||
stage = await open_scene(usd_path)
|
||||
return stage, await prepare(stage, belt_speed, script_control)
|
||||
@@ -0,0 +1,153 @@
|
||||
"""Contact sensing on the plow blade.
|
||||
|
||||
**Why a contact report and not another beam.** The cell already has two through-beams: the
|
||||
laser gate before the pusher and the arming beam at x = -6.30 that pre-positions the plow.
|
||||
Both answer "something is about to arrive". Neither can answer "the blade is now touching
|
||||
*this* item", and that is the question that matters at the plow, because the arm is only
|
||||
useful while it is actually in contact - before that it is waving at nothing, and after it
|
||||
the item is already committed to a lane. A beam at the blade would also be broken by the
|
||||
blade itself as it swings, which is the trap the gate beam at y = -0.24 was placed to dodge.
|
||||
|
||||
So the sensor is a **PhysX contact report on the arm body**
|
||||
(``PhysxSchema.PhysxContactReportAPI``). It fires on the real collision pair, names both
|
||||
bodies, and needs no extra geometry that could foul the belt. Isaac's
|
||||
``sensors.experimental.physics.Contact`` wraps the same mechanism with an authored prim and
|
||||
a threshold; the raw report is used here because the plow needs the *identity* of what it
|
||||
touched, which is what carries the class through.
|
||||
|
||||
**Keeping the class.** Classification happens once, far upstream under the camera portal.
|
||||
That verdict is stored per item and travels with it:
|
||||
|
||||
camera portal ──▶ classes[item] = "B" | "C" | "D"
|
||||
│
|
||||
arming beam ────────▶ pre-position the blade for that class
|
||||
│
|
||||
blade contact ───────▶ CONFIRM against the same stored class, and hold the side while
|
||||
contact lasts - the item is steered by the class it was given,
|
||||
not by anything re-derived at the blade
|
||||
|
||||
:class:`PlowContact` therefore takes the same ``classes`` mapping the sorter uses, and
|
||||
reports, per touch: which item, what class it carries, the blade angle at first touch, and
|
||||
how long contact lasted. A touch whose class is unknown is reported as such rather than
|
||||
guessed - an unclassified item must not be steered anywhere.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pxr import PhysicsSchemaTools, PhysxSchema
|
||||
|
||||
from .. import config as C
|
||||
|
||||
ITEMS_PREFIX = "/World/Items/"
|
||||
|
||||
|
||||
class PlowContact:
|
||||
"""PhysX contact reporting on the plow arm, resolved to item + class"""
|
||||
|
||||
def __init__(self, stage, classes: dict, arm_path: str | None = None,
|
||||
plow=None, threshold: float = 0.0):
|
||||
"""
|
||||
classes : the SAME dict the sorter steers by - vision writes into it, so the
|
||||
sensor sees whatever verdict the item is carrying at the moment of touch
|
||||
plow : optional sim.plow.Plow, so the angle at contact can be recorded
|
||||
"""
|
||||
self.stage = stage
|
||||
self.classes = classes
|
||||
self.plow = plow
|
||||
self.arm_path = arm_path or C.PLOW_ARM
|
||||
|
||||
prim = stage.GetPrimAtPath(self.arm_path)
|
||||
if not prim.IsValid():
|
||||
raise RuntimeError(f"{self.arm_path} missing - is this plow_cell.usd?")
|
||||
api = PhysxSchema.PhysxContactReportAPI.Apply(prim)
|
||||
api.CreateThresholdAttr().Set(float(threshold)) # 0 = report every touch
|
||||
|
||||
self.touches: dict[str, dict] = {} # item -> first/last touch record
|
||||
self.in_contact: set[str] = set()
|
||||
self.events: list[dict] = []
|
||||
self._t = 0.0
|
||||
self._sub = None
|
||||
|
||||
# -- lifecycle ----------------------------------------------------------
|
||||
def install(self):
|
||||
from omni.physx import get_physx_simulation_interface
|
||||
if self._sub is None:
|
||||
self._sub = get_physx_simulation_interface(
|
||||
).subscribe_contact_report_events(self._on_report)
|
||||
return self
|
||||
|
||||
def remove(self):
|
||||
self._sub = None
|
||||
|
||||
def tick(self, dt):
|
||||
"""advance the sensor's clock; contact reports carry no timestamp of their own"""
|
||||
self._t += dt
|
||||
|
||||
# -- the report ---------------------------------------------------------
|
||||
def _item_of(self, path: str):
|
||||
if not path.startswith(ITEMS_PREFIX):
|
||||
return None
|
||||
name = path[len(ITEMS_PREFIX):].split("/")[0]
|
||||
return name or None
|
||||
|
||||
def _on_report(self, contact_headers, contact_data):
|
||||
touching = set()
|
||||
for h in contact_headers:
|
||||
a0 = str(PhysicsSchemaTools.intToSdfPath(h.actor0))
|
||||
a1 = str(PhysicsSchemaTools.intToSdfPath(h.actor1))
|
||||
if self.arm_path not in (a0, a1):
|
||||
continue
|
||||
other = a1 if self.arm_path == a0 else a0
|
||||
name = self._item_of(other)
|
||||
if name is None: # the blade also brushes belts and rails
|
||||
continue
|
||||
touching.add(name)
|
||||
self._register(name)
|
||||
# contact that has ended
|
||||
for gone in self.in_contact - touching:
|
||||
rec = self.touches.get(gone)
|
||||
if rec is not None:
|
||||
rec["released_t"] = round(self._t, 3)
|
||||
rec["duration"] = round(self._t - rec["first_t"], 3)
|
||||
self.in_contact = touching
|
||||
|
||||
def _register(self, name):
|
||||
cls = self.classes.get(name)
|
||||
angle = round(self.plow.angle, 1) if self.plow is not None else None
|
||||
rec = self.touches.get(name)
|
||||
if rec is None:
|
||||
rec = dict(item=name, cls=cls, classified=cls is not None,
|
||||
first_t=round(self._t, 3), angle_at_touch=angle,
|
||||
commanded_at_touch=(round(self.plow.commanded, 1)
|
||||
if self.plow is not None else None),
|
||||
angle_min=angle, angle_max=angle,
|
||||
released_t=None, duration=None, samples=0)
|
||||
self.touches[name] = rec
|
||||
self.events.append(dict(t=rec["first_t"], item=name, cls=cls,
|
||||
angle=angle, kind="touch"))
|
||||
rec["samples"] += 1
|
||||
rec["cls"] = cls if cls is not None else rec["cls"]
|
||||
if angle is not None:
|
||||
rec["angle_min"] = min(rec["angle_min"], angle)
|
||||
rec["angle_max"] = max(rec["angle_max"], angle)
|
||||
|
||||
# -- what the plow asks it ----------------------------------------------
|
||||
def is_touching(self, name: str) -> bool:
|
||||
return name in self.in_contact
|
||||
|
||||
def touched(self, name: str) -> bool:
|
||||
return name in self.touches
|
||||
|
||||
def side_for(self, name: str, mapping: dict, swing: float):
|
||||
"""the angle this item's stored class asks for, or None if it has no class.
|
||||
|
||||
Deliberately returns None rather than 0 for an unknown class: 0 is a real command
|
||||
(drive straight on) and must not double as "no idea".
|
||||
"""
|
||||
cls = self.classes.get(name)
|
||||
if cls is None:
|
||||
return None
|
||||
want = mapping.get(cls, "straight")
|
||||
return {"pos": swing, "neg": -swing}.get(want, 0.0)
|
||||
|
||||
def report(self):
|
||||
return dict(touches=list(self.touches.values()), events=self.events)
|
||||
@@ -0,0 +1,344 @@
|
||||
"""Two-way sorting at the plow: each arriving item is steered onto the lane its class
|
||||
belongs to.
|
||||
|
||||
Layout after `scripts/place_plow_lanes.py`:
|
||||
|
||||
+Y lane x -7.47..-7.02 y +0.05..+2.05 carries away in +Y
|
||||
────────────── plow at x=-7.05, 600 mm arm, hinge about Z, +-35 deg
|
||||
-Y lane x -7.48..-7.03 y -2.05..-0.05 carries away in -Y
|
||||
|
||||
Goods reach the plow having already passed the pusher, so class D is gone; what arrives is
|
||||
B and C, and the plow splits them.
|
||||
|
||||
Which sign of the plow angle feeds which lane is **measured, not assumed** - the arm sits
|
||||
on a prim that carries its own rotateZ=180, and the blade deflects toward the side it
|
||||
slopes away from, which is easy to get backwards. Call :func:`calibrate` once and it
|
||||
returns the mapping to hand to :class:`PlowSorter`.
|
||||
|
||||
The plow is compliant (angular force drive), so a commanded angle is a request. Everything
|
||||
here reads `Plow.angle` for the real pose and never assumes the arm arrived.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pxr import Gf, PhysxSchema, UsdPhysics, UsdShade
|
||||
|
||||
from .. import config as C
|
||||
from . import plow_cell as _cell
|
||||
from .plow import Plow
|
||||
from .plow_contact import PlowContact
|
||||
|
||||
LANE_NEG = "/ConveyorTrack_01/Belt" # perpendicular, carries -Y
|
||||
LANE_POS = "/World/ConveyorTrack_01/Belt" # its mirror, carries +Y
|
||||
|
||||
# the arm reaches to x=-6.52; trip the sensor upstream of that so the blade has time to
|
||||
# take up its angle before the item is on it
|
||||
SENSE_X = -6.30
|
||||
SENSE_Y0, SENSE_Y1 = -0.45, 0.45 # full belt width: a narrow gate misses edge-riders
|
||||
SENSE_Z = C.BELT_Z + 0.025 # where the visible stripe is drawn
|
||||
SENSE_HEIGHT = 0.40 # the curtain is cast from this high above the belt
|
||||
SENSE_CLEAR = 0.001 # stops 1 mm short of it: a 2.4 mm watch is still inside
|
||||
SENSE_RAYS = 181 # 5 mm spacing - narrower than the 6.4 mm `pen`
|
||||
GATE_WINDOW = 0.15 # no rays are cast unless an item is this close to the line
|
||||
|
||||
# lane near edges after scripts/place_plow_lanes.py
|
||||
LANE_SETTLED_Y = 0.50 # beyond this the item is committed to a lane
|
||||
|
||||
# Tray interiors, measured off the walls scripts/place_plow_lanes.py builds, NOT guessed:
|
||||
# B walls x -7.25 / -6.35, y -3.35 / -2.55 -> centre (-6.80, -2.95)
|
||||
# C walls x -8.67 / -7.77, y +1.88 / +2.68 -> centre (-8.22, +2.28)
|
||||
# The earlier values were a wall position rather than a centre, and were out by 0.45-0.50 m.
|
||||
# That mattered: an item resting exactly in tray C measured |x - cx| = 0.50, which failed the
|
||||
# `< CONTAINER_R` test, so a correct delivery was scored as a miss.
|
||||
CONTAINER_B = (-9.54, 1.82) # re-measured after the trays were moved onto the lane exits
|
||||
CONTAINER_C = (-10.45, -0.225) # stale values here score a correct delivery as a miss
|
||||
CONTAINER_R = 0.55 # tray half-width is 0.45; a little slack for the resting pose
|
||||
CONTAINER_LIP_Z = 1.72 # tray floor sits at 1.16, so anything inside is below this
|
||||
|
||||
|
||||
# Where each lane has to carry goods, in WORLD terms: lane B straight out along -Y, lane C
|
||||
# out toward its tray, which sits off at 45 deg. `_cell.drive_belt` resolves these into each
|
||||
# belt's own frame - the +Y lane is laid diagonally, so its local X is neither +X nor +Y.
|
||||
# Directions for the FORK layout (scripts/build_fork_v2.py + the channel split):
|
||||
# C runs straight on down the line, B branches 45 deg to +Y.
|
||||
# These were left over from the old T layout and drove both belts the wrong way - goods
|
||||
# reached the apex, were correctly routed to their side by the blade, and then sat there
|
||||
# because the branch under them was pulling across or backwards. Same class of fault as
|
||||
# ConveyorTrack_04 at the start: a direction not recomputed after the geometry moved.
|
||||
LANE_DIR = {
|
||||
LANE_POS: (-1.0, 0.0, 0.0), # /World/ConveyorTrack_01 - channel C, straight
|
||||
LANE_NEG: (-0.7071, 0.7071, 0.0), # /ConveyorTrack_01 - channel B, 45 deg +Y
|
||||
}
|
||||
|
||||
# The decks that bridge the junction were built as **static plates**, and that is where
|
||||
# goods died. The blade cams an item sideways only while the belt is still driving it into
|
||||
# the blade; the moment it slides off the driven belt onto a dead plate nothing pushes it
|
||||
# any more - not the belt, which no longer reaches it, and not the blade, which is holding a
|
||||
# fixed angle. It stops on the plate, exactly at the belt edge. Every "it touches and then
|
||||
# just sits there" observation is this.
|
||||
#
|
||||
# So the decks are driven too, each toward the lane it feeds. They are Mesh prims, so
|
||||
# `drive_belt` gives them a kinematic body first.
|
||||
DECK_DIR = {
|
||||
"/World/PlowCornerDeck_B": (-0.7071, 0.7071, 0.0),
|
||||
"/World/PlowCornerDeck_C": (-1.0, 0.0, 0.0),
|
||||
# the transition plates, extended inboard to |y| = 0.20 by
|
||||
# scripts/extend_transition_decks.py so they reach the band where the blade lets go
|
||||
"/World/PlowTransition_B": (-0.7071, 0.7071, 0.0),
|
||||
"/World/PlowTransition_C": (-1.0, 0.0, 0.0),
|
||||
}
|
||||
|
||||
|
||||
def configure_lanes(stage, speed=None):
|
||||
"""drive both plow lanes, and the decks that bridge them to the main run, outward"""
|
||||
speed = speed if speed is not None else C.BELT_SPEED
|
||||
driven = []
|
||||
for path, world_dir in list(LANE_DIR.items()) + list(DECK_DIR.items()):
|
||||
if _cell.drive_belt(stage, path, world_dir, speed) is not None:
|
||||
driven.append(path)
|
||||
return driven
|
||||
|
||||
|
||||
# The conveyor art carries its own collider (SM_ConveyorBelt_*_02, collision=True), and
|
||||
# that includes the blue side rails. At a plow station the rails are cut away on the
|
||||
# discharge side - goods have to leave the belt sideways. Left in place they simply stop
|
||||
# everything at the lane entry, which is what "nothing reaches the bins" looked like.
|
||||
JUNCTION_SHELLS = [
|
||||
"/World/ConveyorTrack_04/SM_ConveyorBelt_A06_02", # the run through the plow
|
||||
"/ConveyorTrack_01/SM_ConveyorBelt_A06_02", # lane B structure
|
||||
"/World/ConveyorTrack_01/SM_ConveyorBelt_A06_02", # lane C structure
|
||||
]
|
||||
|
||||
|
||||
def open_junction(stage):
|
||||
"""drop the shell colliders at the plow so goods can cross onto the lanes.
|
||||
|
||||
Only the decorative shell loses its collider; each Belt keeps its own, so goods still
|
||||
ride on a surface and cannot fall through.
|
||||
"""
|
||||
opened = []
|
||||
for path in JUNCTION_SHELLS:
|
||||
prim = stage.GetPrimAtPath(path)
|
||||
if not prim.IsValid():
|
||||
continue
|
||||
attr = prim.GetAttribute("physics:collisionEnabled")
|
||||
if not attr:
|
||||
attr = UsdPhysics.CollisionAPI.Apply(prim).CreateCollisionEnabledAttr()
|
||||
attr.Set(False)
|
||||
opened.append(path)
|
||||
return opened
|
||||
|
||||
|
||||
def keep_lanes_active(stage):
|
||||
"""`plow_cell.deactivate_stray()` switches the -Y lane off as a duplicate. It is not a
|
||||
duplicate - it is half the sorter."""
|
||||
prim = stage.GetPrimAtPath("/ConveyorTrack_01")
|
||||
if prim.IsValid() and not prim.IsActive():
|
||||
prim.SetActive(True)
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
class PlowSorter:
|
||||
"""steers each arriving item onto the lane its class maps to"""
|
||||
|
||||
def __init__(self, stage, cell, classes, mapping, swing=None, sense_x=SENSE_X,
|
||||
kinematic=True, contact_sensor=True):
|
||||
"""
|
||||
cell : mechanics.Cell, for item poses
|
||||
classes : dict name -> class letter
|
||||
mapping : dict class letter -> "pos" | "neg" | "straight"
|
||||
"""
|
||||
self.stage = stage
|
||||
self.cell = cell
|
||||
self.classes = dict(classes)
|
||||
self.mapping = dict(mapping)
|
||||
self.swing = C.PLOW_SWING if swing is None else swing
|
||||
self.sense_x = sense_x
|
||||
# kinematic by default: the arm is turned directly at PLOW_RATE rather than
|
||||
# asked to hold an angle, because the force drive never settled (see sim/plow.py)
|
||||
self.plow = Plow(stage, kinematic=kinematic)
|
||||
if not kinematic:
|
||||
self.plow.gains(stiffness=C.PLOW_SORT_STIFFNESS, damping=C.PLOW_SORT_DAMPING,
|
||||
max_force=C.PLOW_SORT_MAX_FORCE)
|
||||
self.plow.home()
|
||||
# Contact sensing on the blade itself. The arming beam upstream says something is
|
||||
# coming; this says the blade is touching *this* item, and it carries the class the
|
||||
# item was given at the camera - so the steer is driven by the stored verdict, never
|
||||
# by anything re-derived at the blade.
|
||||
self.contact = (PlowContact(stage, self.classes, plow=self.plow).install()
|
||||
if contact_sensor else None)
|
||||
self.angle_for = {}
|
||||
self.decided = {}
|
||||
self._holding = None
|
||||
self._latched = None # (item, angle) the blade is committed to
|
||||
self.returning = False # blade on its way back to centre
|
||||
self._nudge_left = 0.0 # seconds remaining in the current nudge
|
||||
self._nudge_angle = 0.0
|
||||
self.swept = set() # items the gate has already armed for
|
||||
self._active = None # (item, angle) the blade is holding right now
|
||||
self.pending = {} # item -> angle, everything armed and not yet past
|
||||
self.conflicts = set() # items that shared the zone with another class
|
||||
self.gate_log = [] # what the laser saw, for the run report
|
||||
self.homed = 0 # times it has finished a return
|
||||
from omni.physx import get_physx_scene_query_interface
|
||||
self._query = get_physx_scene_query_interface()
|
||||
|
||||
# -- sensing ------------------------------------------------------------
|
||||
def sensor(self):
|
||||
"""item crossing the gate, or None - a dense light curtain, armed only when needed.
|
||||
|
||||
Two earlier attempts failed and both are worth recording. A single horizontal beam
|
||||
is blind to flat stock: meshes are 0.49x real size, so `watch` (5 mm real) stands
|
||||
2.4 mm tall and drove under a beam 25 mm up. A sparse downward curtain fixed the
|
||||
tall-enough cases but still lost `pen`, which is 6.4 mm wide in scene and slipped
|
||||
between rays spaced 75 mm apart. An `overlap_box` query would have no blind spot at
|
||||
all, but it crashed the process outright, so it is not used here.
|
||||
|
||||
What works is a curtain dense enough that nothing fits between the rays - 5 mm
|
||||
spacing against a 6.4 mm minimum width - reaching to 1 mm off the belt so even the
|
||||
watch is inside it. That many rays every step would be wasteful, so a pose check
|
||||
gates the gate: unless some item is within GATE_WINDOW of the line, no ray is cast
|
||||
at all, which is most of the time.
|
||||
|
||||
The red stripe at /World/PlowLaserGate marks where it stands. It carries no
|
||||
collider, so it can never be what the rays hit.
|
||||
"""
|
||||
near = False
|
||||
for name in self.cell.items:
|
||||
if abs(float(self.cell.pose(name)[0]) - self.sense_x) < GATE_WINDOW:
|
||||
near = True
|
||||
break
|
||||
if not near:
|
||||
return None
|
||||
|
||||
z0 = C.BELT_Z + SENSE_HEIGHT
|
||||
reach = SENSE_HEIGHT - SENSE_CLEAR
|
||||
for i in range(SENSE_RAYS):
|
||||
y = SENSE_Y0 + (SENSE_Y1 - SENSE_Y0) * i / (SENSE_RAYS - 1)
|
||||
hit = self._query.raycast_closest(
|
||||
[self.sense_x, y, z0], [0.0, 0.0, -1.0], reach)
|
||||
if not hit or not hit.get("hit"):
|
||||
continue
|
||||
path = str(hit.get("rigidBody") or hit.get("collision") or "")
|
||||
for name in self.cell.items:
|
||||
if f"/World/Items/{name}" in path:
|
||||
return name
|
||||
return None
|
||||
|
||||
def side_for(self, name):
|
||||
"""+swing / -swing / 0, from the item's class"""
|
||||
want = self.mapping.get(self.classes.get(name), "straight")
|
||||
return {"pos": self.swing, "neg": -self.swing}.get(want, 0.0)
|
||||
|
||||
def preset_for(self, name):
|
||||
"""the angle the blade should ALREADY be holding when this item arrives"""
|
||||
return float(C.PLOW_PRESET.get(self.classes.get(name), 0.0))
|
||||
|
||||
# -- per-step -----------------------------------------------------------
|
||||
def update(self, dt):
|
||||
"""serve a QUEUE of armed items, always the one nearest the blade.
|
||||
|
||||
Holding one item at a time is fine at a 2.5 m pitch and wrong at 700 mm. The gate
|
||||
sits 1.62 m upstream of the blade's trailing edge, so at 1 m/s an item occupies the
|
||||
plow for 1.62 s while the next arrives every 0.70 s - one blade, three items in the
|
||||
zone. The single `_active` slot simply ignored the other two, which is exactly the
|
||||
"the shift does not fire" symptom: the blade was still committed to someone else.
|
||||
|
||||
So every item the gate sees is queued with its angle, and each step the blade serves
|
||||
whichever queued item is CLOSEST to the blade and not yet past it. That cannot make
|
||||
one blade sort two items that need opposite angles at the same instant - nothing
|
||||
can - so those cases are counted in `self.conflicts` and reported, rather than
|
||||
silently lost.
|
||||
"""
|
||||
if self.contact is not None:
|
||||
self.contact.tick(dt)
|
||||
|
||||
for _n in list(self.swept):
|
||||
if float(self.cell.pose(_n)[0]) > C.PLOW_REARM_X:
|
||||
self.swept.discard(_n)
|
||||
self.decided.pop(_n, None)
|
||||
self.pending.pop(_n, None)
|
||||
if self.contact is not None:
|
||||
self.contact.touches.pop(_n, None)
|
||||
|
||||
# ---- the laser arms the blade -------------------------------------
|
||||
seen = self.sensor()
|
||||
if seen is not None and seen not in self.swept:
|
||||
ang = self.preset_for(seen)
|
||||
self.swept.add(seen)
|
||||
self.decided[seen] = ang
|
||||
self.gate_log.append(dict(
|
||||
item=seen, cls=self.classes.get(seen), angle=round(ang, 1),
|
||||
x=round(float(self.cell.pose(seen)[0]), 3)))
|
||||
if abs(ang) > 1e-6:
|
||||
self.pending[seen] = ang
|
||||
|
||||
# ---- drop whatever is already past the blade ----------------------
|
||||
for n in list(self.pending):
|
||||
if float(self.cell.pose(n)[0]) < C.PLOW_RELEASE_X:
|
||||
self.pending.pop(n, None)
|
||||
|
||||
# ---- serve the one closest to the blade ---------------------------
|
||||
if self.pending:
|
||||
nearest = min(self.pending, key=lambda n: abs(float(self.cell.pose(n)[0]) - C.PLOW_X))
|
||||
ang = self.pending[nearest]
|
||||
wanted = {self.pending[n] for n in self.pending}
|
||||
if len(wanted) > 1:
|
||||
self.conflicts.add(nearest) # two classes in the zone, one blade
|
||||
self._active = (nearest, ang)
|
||||
self.plow.step_toward(ang, dt, rate=C.PLOW_SWEEP_RATE)
|
||||
return
|
||||
|
||||
self._active = None
|
||||
if abs(self.plow.commanded) > 0.5:
|
||||
self.returning = True
|
||||
elif self.returning:
|
||||
self.returning = False
|
||||
self.homed += 1
|
||||
self.plow.step_toward(C.PLOW_REST_ANGLE, dt, rate=C.PLOW_SWEEP_RATE)
|
||||
|
||||
def lane_of(self, name):
|
||||
"""where the item ended up: a container, a lane, still on the line, or lost"""
|
||||
p = self.cell.pose(name)
|
||||
x, y, z = float(p[0]), float(p[1]), float(p[2])
|
||||
# The pusher's D bin FIRST. It sits at y +1.27..+2.05, so the `y > LANE_SETTLED_Y`
|
||||
# test below claims it as "lane_C" and a delivered item is scored as a miss. That
|
||||
# hid a working pusher: 5 of 11 class-D items in the 25-object run were physically
|
||||
# in the bin (x -3.7..-4.0, y +1.42..+1.84, z 1.25) and every one was logged as
|
||||
# lane_C. Order of tests is not cosmetic here.
|
||||
if C.BIN_X0 < x < C.BIN_X1 and C.BIN_Y0 < y < C.BIN_Y1 and z < C.BIN_LIP_Z:
|
||||
return "bin"
|
||||
for tag, (cx, cy) in (("container_B", CONTAINER_B), ("container_C", CONTAINER_C)):
|
||||
if abs(x - cx) < CONTAINER_R and abs(y - cy) < CONTAINER_R and z < CONTAINER_LIP_Z:
|
||||
return tag
|
||||
# FORK layout: lane B is the +Y branch, lane C carries straight on down the run.
|
||||
# Reversed under the old T, and leaving it reported a correct branch as the other one.
|
||||
if y > LANE_SETTLED_Y:
|
||||
return "lane_B"
|
||||
if x < C.MAIN_X0 and abs(y) < LANE_SETTLED_Y:
|
||||
return "lane_C"
|
||||
if z < C.BELT_Z - 0.4:
|
||||
return "floor"
|
||||
return "line"
|
||||
|
||||
|
||||
def calibrate_mapping():
|
||||
"""class -> which way the blade swings, **measured on the running cell**.
|
||||
|
||||
The docstring at the top of this module warns that the sign is easy to get backwards,
|
||||
and the first version had it backwards. Observed with the default mapping: `barrel`,
|
||||
class C, mapped to "pos" (+30 deg), came to rest at y = -2.44 - the *-Y* lane, which
|
||||
feeds tray B. So a positive swing deflects toward -Y:
|
||||
|
||||
+swing -> -Y lane -> tray B
|
||||
-swing -> +Y lane -> tray C
|
||||
|
||||
Re-measure with a single item and `PlowSorter.lane_of` if the arm or the lanes are ever
|
||||
re-laid; do not reason it out from the geometry, the arm's parent carries rotateZ=180
|
||||
and the blade deflects away from the face it slopes toward.
|
||||
"""
|
||||
# FORK layout, plow on the apex: C runs straight on and must not be steered at all;
|
||||
# B is the only class that actuates. A positive swing deflects toward -Y (a property of
|
||||
# the arm mount, unchanged), and the B branch is at +Y, so B needs a NEGATIVE swing.
|
||||
# Measured this session: "pos" put bolts_cluster (B) at y -0.155, the wrong side.
|
||||
return {"B": "neg", "C": "straight", "D": "straight"}
|
||||
@@ -0,0 +1,144 @@
|
||||
"""Vision stack on top of the plow cell: infeed belt, item feeder, laser gate and the
|
||||
CRE-ROI v2b decision that tells the pusher what to divert.
|
||||
|
||||
Purely additive. `sim/plow_cell.py` still owns the belts and the plow, and nothing here
|
||||
edits the authored kinematics — the DiverterAnimGraph, the plow hinge and the pusher's own
|
||||
drive are left exactly as `plow_cell.prepare()` leaves them.
|
||||
|
||||
The layout the scene augmentation produced:
|
||||
|
||||
ConveyorTrack_05 x 0.00 .. +2.00 infeed, items are released at x=+1.70
|
||||
ConveyorTrack_02 x -2.00 .. 0.00 camera portal straddles x=-0.75
|
||||
ConveyorTrack_03 x -6.00 .. -2.00 laser gate at x=-3.74, pusher at x=-3.90
|
||||
Belt_01 branch to the bin
|
||||
|
||||
Goods run -X at the configured belt speed, so an item is released, measured under the
|
||||
portal, and reaches the gate about 3.4 s later at 1 m/s.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics, UsdShade
|
||||
|
||||
from .. import config as C
|
||||
from . import plow_cell as _cell
|
||||
from . import scene as _scene
|
||||
|
||||
# the conveyor added by scripts/add_vision_to_plow_cell.py
|
||||
INFEED = "/World/ConveyorTrack_05/Belt"
|
||||
INFEED_TRACK = "/World/ConveyorTrack_05"
|
||||
INFEED_X0, INFEED_X1 = 0.0, 2.0
|
||||
|
||||
# release point: on the infeed belt, clear of its upstream edge so the item settles before
|
||||
# it reaches the transfer to ConveyorTrack_02
|
||||
SPAWN_X = 1.70
|
||||
|
||||
ITEMS_ROOT = _scene.ITEMS_ROOT
|
||||
LASER_GATE = "/World/SortingRig/LaserGate"
|
||||
|
||||
|
||||
def configure_infeed(stage, speed=None):
|
||||
"""drive the added conveyor the same way as the rest of the line.
|
||||
|
||||
Its local X is +X in world (unlike the branch, which is rotated), so the surface
|
||||
velocity is simply -speed on X.
|
||||
"""
|
||||
speed = speed if speed is not None else C.BELT_SPEED
|
||||
prim = stage.GetPrimAtPath(INFEED)
|
||||
if not prim.IsValid():
|
||||
raise RuntimeError(
|
||||
f"{INFEED} missing - run scripts/add_vision_to_plow_cell.py first")
|
||||
|
||||
if not prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||
UsdPhysics.RigidBodyAPI.Apply(prim)
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(True)
|
||||
PhysxSchema.PhysxSurfaceVelocityAPI.Apply(prim)
|
||||
PhysxSchema.PhysxSurfaceVelocityAPI(prim).CreateSurfaceVelocityAttr().Set(
|
||||
Gf.Vec3f(-speed, 0.0, 0.0))
|
||||
|
||||
grip = stage.GetPrimAtPath(_cell.GRIP_MATERIAL)
|
||||
if grip.IsValid():
|
||||
api = UsdShade.MaterialBindingAPI.Apply(prim)
|
||||
api.Bind(UsdShade.Material(grip),
|
||||
bindingStrength=UsdShade.Tokens.strongerThanDescendants,
|
||||
materialPurpose="physics")
|
||||
|
||||
# the added track brings its own conveyor graph; it carries no speed and would only
|
||||
# fight the explicit surface velocity
|
||||
for suffix in ("", "_01"):
|
||||
g = stage.GetPrimAtPath(f"{INFEED_TRACK}/ConveyorBeltGraph{suffix}")
|
||||
if g.IsValid():
|
||||
g.SetActive(False)
|
||||
return prim
|
||||
|
||||
|
||||
def load_items(stage, meshes_dir=None):
|
||||
"""the bundled per-class test meshes, as dynamic rigid bodies parked off the line"""
|
||||
meshes_dir = Path(meshes_dir or C.MESHES)
|
||||
manifest = json.loads((meshes_dir / "manifest.json").read_text())
|
||||
UsdGeom.Xform.Define(stage, ITEMS_ROOT)
|
||||
items = {}
|
||||
for i, (name, meta) in enumerate(sorted(manifest.items())):
|
||||
usd = meshes_dir / f"{name}.usd"
|
||||
if not usd.exists():
|
||||
continue
|
||||
prim = UsdGeom.Xform.Define(stage, f"{ITEMS_ROOT}/{name}").GetPrim()
|
||||
refs = prim.GetReferences()
|
||||
refs.ClearReferences()
|
||||
refs.AddReference(str(usd))
|
||||
# Meshes flattened out of the working scene bring their own xformOp:translate at
|
||||
# float precision. ClearXformOpOrder() drops the *order*, not the attribute, so
|
||||
# adding a fresh double-precision op collides with what is already there and USD
|
||||
# raises. Match whatever precision the prim already carries.
|
||||
xf = UsdGeom.Xformable(prim)
|
||||
xf.ClearXformOpOrder()
|
||||
park = (9.0 + 1.2 * i, 5.0, 0.4)
|
||||
# Items exported from the working scene carry translate as float3, and
|
||||
# ClearXformOpOrder() drops the ORDER but keeps the attribute. AddTranslateOp() then
|
||||
# warns-as-raises about the precision mismatch (it still succeeds), and a retry hits
|
||||
# "already exists". Reuse the attribute that is there instead of adding anything.
|
||||
attr = prim.GetAttribute("xformOp:translate")
|
||||
if attr:
|
||||
op = UsdGeom.XformOp(attr)
|
||||
op.Set(Gf.Vec3f(*park) if str(attr.GetTypeName()) == "float3" else Gf.Vec3d(*park))
|
||||
xf.SetXformOpOrder([op])
|
||||
else:
|
||||
xf.AddTranslateOp().Set(Gf.Vec3d(*park))
|
||||
UsdPhysics.RigidBodyAPI.Apply(prim)
|
||||
# exported meshes arrive kinematic and hidden; both make them inert
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(False)
|
||||
UsdPhysics.MassAPI.Apply(prim).CreateMassAttr().Set(0.6)
|
||||
px = PhysxSchema.PhysxRigidBodyAPI.Apply(prim)
|
||||
px.CreateEnableCCDAttr().Set(True)
|
||||
px.CreateSolverPositionIterationCountAttr().Set(24)
|
||||
px.CreateSleepThresholdAttr().Set(0.0) # a settled item must stay draggable
|
||||
# without this a blade sweeping into the item separates them at whatever speed
|
||||
# PhysX picks, which fires the item off the line instead of deflecting it
|
||||
px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION)
|
||||
UsdGeom.Imageable(prim).MakeVisible()
|
||||
items[name] = meta
|
||||
return items
|
||||
|
||||
|
||||
def prepare(stage, belt_speed=None, script_control=True, meshes_dir=None):
|
||||
"""plow_cell.prepare() plus the infeed belt, the items and the camera housekeeping"""
|
||||
info = _cell.prepare(stage, belt_speed=belt_speed, script_control=script_control)
|
||||
configure_infeed(stage, belt_speed)
|
||||
hidden = _scene.hide_aim_markers(stage)
|
||||
items = load_items(stage, meshes_dir)
|
||||
|
||||
# mechanics.Cell releases at C.SPAWN_X; the plow cell's infeed is shorter than the
|
||||
# sorter's, so point it at this belt. run.py already sets C.BELT_SPEED the same way.
|
||||
C.SPAWN_X = SPAWN_X
|
||||
|
||||
calib_path = C.CONFIG / "calib.json"
|
||||
info.update(items=items, aim_markers_hidden=hidden, spawn_x=SPAWN_X,
|
||||
calib=json.loads(calib_path.read_text()) if calib_path.exists() else None)
|
||||
return info
|
||||
|
||||
|
||||
def load(usd_path=None, belt_speed=None, script_control=True, meshes_dir=None):
|
||||
stage = _cell.open_scene(usd_path)
|
||||
return stage, prepare(stage, belt_speed, script_control, meshes_dir)
|
||||
@@ -0,0 +1,170 @@
|
||||
"""Entry point: build the cell, start the belt, run CRE-ROI v2b on every item as it passes
|
||||
under the stand, and divert class D with the pusher.
|
||||
|
||||
./python.sh -m robozon_sorter.sim.run # windowed, watchable
|
||||
./python.sh -m robozon_sorter.sim.run --headless # batch, prints the log
|
||||
./python.sh -m robozon_sorter.sim.run --no-vision # mechanics only, uses ground truth
|
||||
|
||||
It also runs inside an already-open Isaac Sim: `from robozon_sorter.sim.run import main`.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
|
||||
|
||||
def parse_args(argv=None):
|
||||
p = argparse.ArgumentParser(description="Robozon conveyor sorting cell")
|
||||
p.add_argument("--headless", action="store_true", help="no window")
|
||||
p.add_argument("--no-vision", action="store_true",
|
||||
help="skip CRE-ROI and route on ground truth (mechanics smoke test)")
|
||||
p.add_argument("--loops", type=int, default=1, help="passes over the test items")
|
||||
p.add_argument("--speed", type=float, default=None, help="override belt speed, m/s")
|
||||
p.add_argument("--pusher", type=float, default=None, help="override blade speed, m/s")
|
||||
p.add_argument("--log", default=None, help="write the run log here as JSON")
|
||||
return p.parse_args(argv)
|
||||
|
||||
|
||||
async def _run(app_utils, stage, args):
|
||||
from .. import config as C
|
||||
from . import scene as S
|
||||
from .mechanics import Cell
|
||||
|
||||
if args.speed:
|
||||
C.BELT_SPEED = args.speed
|
||||
built = S.build(stage)
|
||||
items = built["items"]
|
||||
print(f"cell built: {len(items)} test items "
|
||||
f"({sorted({m['zone'] for m in items.values()})})")
|
||||
|
||||
vision = None
|
||||
if not args.no_vision:
|
||||
from ..cv.pipeline import CreRoiV2b
|
||||
vision = CreRoiV2b()
|
||||
vision.attach_cameras()
|
||||
print("CRE-ROI v2b ready; gate pixels:", vision.gate_px)
|
||||
|
||||
await app_utils.update_app_async(steps=40)
|
||||
cell = Cell(stage, items.keys())
|
||||
cell.park_all()
|
||||
await app_utils.update_app_async(steps=15)
|
||||
|
||||
import omni.timeline
|
||||
timeline = omni.timeline.get_timeline_interface()
|
||||
app_utils.play(commit=True)
|
||||
await app_utils.update_app_async(steps=20)
|
||||
|
||||
order = [n for _ in range(args.loops) for n in sorted(items)]
|
||||
dt = 1.0 / 60.0
|
||||
log, active, done = [], [], set()
|
||||
classified, diverted = set(), set()
|
||||
nxt, t = 0, 0.0
|
||||
print(f"\n{'t':>7} event")
|
||||
while t < 45.0 * args.loops * max(len(order), 1) / 6 and len(done) < len(order):
|
||||
await app_utils.update_app_async(steps=2)
|
||||
t += 2 * dt
|
||||
|
||||
if nxt < len(order) and (not active or cell.pose(active[-1])[0] < C.SPAWN_X - C.RELEASE_GAP):
|
||||
name = order[nxt]
|
||||
cell.release(name)
|
||||
active.append(name)
|
||||
nxt += 1
|
||||
print(f"{t:7.2f} release {name}")
|
||||
|
||||
for name in list(active):
|
||||
x = float(cell.pose(name)[0])
|
||||
|
||||
if name not in classified and abs(x - C.CAM_X) < 0.06:
|
||||
gt = items[name]["zone"]
|
||||
if vision is not None:
|
||||
was_playing = timeline.is_playing()
|
||||
res = vision.measure()
|
||||
# Replicator's step stops the timeline; resume or the line freezes
|
||||
if was_playing and not timeline.is_playing():
|
||||
timeline.play()
|
||||
await app_utils.update_app_async(steps=2)
|
||||
pred = res["cls"]
|
||||
print(f"{t:7.2f} vision {name:18s} pred={pred} gt={gt} "
|
||||
f"{'ok' if pred == gt else 'MISS'} dims={res['dims']} "
|
||||
f"K={res['k']:.2f} views={res['views']} cre={res['cre_ms']}ms")
|
||||
log.append(dict(item=name, gt=gt, **res))
|
||||
else:
|
||||
pred = gt
|
||||
print(f"{t:7.2f} route {name:18s} class={pred} (ground truth)")
|
||||
log.append(dict(item=name, gt=gt, cls=pred))
|
||||
cell.pred = getattr(cell, "pred", {})
|
||||
cell.pred[name] = pred
|
||||
classified.add(name)
|
||||
|
||||
if (name not in diverted and getattr(cell, "pred", {}).get(name) == "D"
|
||||
and cell.laser() == name):
|
||||
took, held = await cell.divert(app_utils, name, speed=args.pusher)
|
||||
diverted.add(name)
|
||||
msg = f" (retract held {held:.2f}s)" if held > 0.01 else ""
|
||||
print(f"{t:7.2f} divert {name:18s} cycle {took:.2f}s{msg}")
|
||||
t += took
|
||||
|
||||
place = cell.where(name)
|
||||
if place in ("bin", "line-end"):
|
||||
print(f"{t:7.2f} done {name:18s} -> {place}")
|
||||
for rec in log:
|
||||
if rec["item"] == name and "outcome" not in rec:
|
||||
rec["outcome"] = place
|
||||
active.remove(name)
|
||||
done.add(name)
|
||||
|
||||
app_utils.stop()
|
||||
await app_utils.update_app_async(steps=15)
|
||||
cell.blade_to(C.BLADE_HOME_Y)
|
||||
|
||||
graded = [r for r in log if "cls" in r and r["cls"] != "?"]
|
||||
hits = sum(1 for r in graded if r["cls"] == r["gt"])
|
||||
print(f"\n {len(log)} items, {hits}/{len(graded)} agreed with ground truth")
|
||||
if vision is not None and graded:
|
||||
cre = [r["cre_ms"] for r in graded if r.get("cre_ms")]
|
||||
tot = [r["total_ms"] for r in graded if r.get("total_ms")]
|
||||
if cre:
|
||||
print(f" CRE batched {sum(cre)/len(cre):.0f} ms/item, "
|
||||
f"end-to-end {sum(tot)/len(tot):.0f} ms/item")
|
||||
routed = [r for r in log if r.get("outcome")]
|
||||
if routed:
|
||||
print(" routing: " + ", ".join(f"{r['item']}->{r['outcome']}" for r in routed))
|
||||
if args.log:
|
||||
with open(args.log, "w") as fh:
|
||||
json.dump(log, fh, indent=2)
|
||||
print(f" log written to {args.log}")
|
||||
return log
|
||||
|
||||
|
||||
def main(argv=None):
|
||||
args = parse_args(argv)
|
||||
try:
|
||||
import omni.usd
|
||||
stage = omni.usd.get_context().get_stage()
|
||||
inside = stage is not None
|
||||
except Exception:
|
||||
inside = False
|
||||
|
||||
if not inside:
|
||||
from isaacsim import SimulationApp
|
||||
app = SimulationApp({"headless": args.headless, "width": 1600, "height": 900})
|
||||
import omni.usd
|
||||
import isaacsim.core.experimental.utils.stage as stage_utils
|
||||
stage_utils.create_new_stage()
|
||||
stage = omni.usd.get_context().get_stage()
|
||||
else:
|
||||
app = None
|
||||
|
||||
import asyncio
|
||||
import isaacsim.core.experimental.utils.app as app_utils
|
||||
loop = asyncio.get_event_loop()
|
||||
try:
|
||||
return loop.run_until_complete(_run(app_utils, stage, args))
|
||||
finally:
|
||||
if app is not None:
|
||||
app.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(0 if main() is not None else 1)
|
||||
@@ -0,0 +1,207 @@
|
||||
"""Loads the real sorting cell (scene/sorter.usd) and applies the runtime configuration
|
||||
it needs to actually run.
|
||||
|
||||
The scene file is the original build - conveyor art, diverters, camera portal, camera
|
||||
bodies, laser gate and collection bin exactly as authored. Nothing here rebuilds geometry.
|
||||
What this module does is re-apply the handful of runtime settings that USD does not carry
|
||||
and that the cell does not work without; each one is documented where it is applied,
|
||||
because every one of them was a silent failure at some point.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics, UsdShade
|
||||
|
||||
from .. import config as C
|
||||
|
||||
SCENE = C.ROOT / "scene" / "sorter.usd"
|
||||
|
||||
# --- prim paths in the authored scene -------------------------------------------------
|
||||
BELTS = ["/World/ConveyorTrack/Belt", "/World/ConveyorTrack_02/Belt",
|
||||
"/World/ConveyorTrack_03/Belt", "/World/ConveyorTrack_04/Belt",
|
||||
"/World/ConveyorTrack_01/Belt"]
|
||||
SPAWN_BELT = "/World/SortingRig/SpawnBelt"
|
||||
BRANCH = "/World/ConveyorTrack_03/Belt_01" # the branch the pusher feeds
|
||||
BLADE = "/World/Diverters/DiverterY_Split/Pusher"
|
||||
PUSHER_JOINT = "/World/Diverters/DiverterY_Split/PusherSlide"
|
||||
ANIM_GRAPH = "/World/Diverters/DiverterAnimGraph"
|
||||
CAMERA_BODIES = "/World/CameraBodies"
|
||||
ITEMS_ROOT = "/World/Items"
|
||||
RIG = "/RigRS"
|
||||
|
||||
# The blade's parent carries this offset; world_y = PARENT_Y + local_y.
|
||||
BLADE_PARENT_Y = -0.35
|
||||
|
||||
|
||||
def open_scene(usd_path: str | Path | None = None):
|
||||
"""open sorter.usd into the current context"""
|
||||
import omni.usd
|
||||
path = str(usd_path or SCENE)
|
||||
if not Path(path).exists():
|
||||
raise FileNotFoundError(
|
||||
f"{path} not found. 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()
|
||||
|
||||
|
||||
# --------------------------------------------------------------------- runtime config
|
||||
def configure_physics(stage):
|
||||
scene = stage.GetPrimAtPath("/World/PhysicsScene")
|
||||
if not scene.IsValid():
|
||||
scene = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim()
|
||||
UsdPhysics.Scene(scene).CreateGravityMagnitudeAttr().Set(9.81)
|
||||
px = PhysxSchema.PhysxSceneAPI.Apply(scene)
|
||||
# 120 Hz is what the cell was tuned and validated at, together with the 2.5 m/s blade.
|
||||
# Raising it changes the contact response and the pushed item stops landing in the bin,
|
||||
# so treat this number and PUSHER_SPEED as a matched pair.
|
||||
px.CreateTimeStepsPerSecondAttr().Set(120)
|
||||
px.CreateEnableCCDAttr().Set(True)
|
||||
px.CreateSolverTypeAttr().Set("TGS")
|
||||
|
||||
|
||||
def configure_belts(stage, speed=None, grip_path="/World/SortingRig/M_beltPhysics"):
|
||||
"""explicit surface velocities; the authored ConveyorBeltGraphs carry no speed and
|
||||
would only fight these, so they are switched off.
|
||||
|
||||
`grip_path` is where the belt friction material is authored. It defaults to a prim
|
||||
under the sorter's rig; plow_cell.usd has no SortingRig and passes its own path so the
|
||||
scene does not grow an empty one.
|
||||
"""
|
||||
speed = speed if speed is not None else C.BELT_SPEED
|
||||
|
||||
grip = stage.GetPrimAtPath(grip_path)
|
||||
if not grip.IsValid():
|
||||
grip = stage.DefinePrim(grip_path, "Material")
|
||||
pm = UsdPhysics.MaterialAPI.Apply(grip)
|
||||
pm.CreateStaticFrictionAttr().Set(1.1)
|
||||
pm.CreateDynamicFrictionAttr().Set(0.95)
|
||||
pm.CreateRestitutionAttr().Set(0.02)
|
||||
grip_mat = UsdShade.Material(grip)
|
||||
|
||||
def drive(path, vel):
|
||||
prim = stage.GetPrimAtPath(path)
|
||||
if not prim.IsValid():
|
||||
return False
|
||||
if not prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||
UsdPhysics.RigidBodyAPI.Apply(prim)
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(True)
|
||||
PhysxSchema.PhysxSurfaceVelocityAPI.Apply(prim)
|
||||
PhysxSchema.PhysxSurfaceVelocityAPI(prim).CreateSurfaceVelocityAttr().Set(Gf.Vec3f(*vel))
|
||||
api = UsdShade.MaterialBindingAPI.Apply(prim)
|
||||
api.Bind(grip_mat, bindingStrength=UsdShade.Tokens.strongerThanDescendants,
|
||||
materialPurpose="physics")
|
||||
return True
|
||||
|
||||
for path in BELTS + [SPAWN_BELT]:
|
||||
drive(path, (-speed, 0, 0))
|
||||
|
||||
# The branch is rotated: its LOCAL X points along world -Y. surfaceVelocity is given
|
||||
# in the body's local frame, so carrying goods toward the bin (+Y) needs (-speed,0,0).
|
||||
# Setting the "obvious" (0,+speed,0) drags them sideways and they sit there.
|
||||
drive(BRANCH, (-speed, 0, 0))
|
||||
|
||||
for track in ["ConveyorTrack", "ConveyorTrack_02", "ConveyorTrack_03",
|
||||
"ConveyorTrack_04", "ConveyorTrack_01"]:
|
||||
for graph in (f"/World/{track}/ConveyorBeltGraph", f"/World/{track}/ConveyorBeltGraph_01"):
|
||||
g = stage.GetPrimAtPath(graph)
|
||||
if g.IsValid():
|
||||
g.SetActive(False)
|
||||
|
||||
|
||||
def configure_pusher(stage):
|
||||
"""the blade is driven kinematically from script.
|
||||
|
||||
Its authored PhysicsPrismaticJoint is unusable at runtime: USD drive-target writes
|
||||
reach PhysX about a second late, so the blade never completes its stroke while the
|
||||
item is still in reach. The joint is disabled and the blade is moved directly.
|
||||
"""
|
||||
blade = stage.GetPrimAtPath(BLADE)
|
||||
if not blade.IsValid():
|
||||
raise RuntimeError(f"{BLADE} missing - is this the right scene?")
|
||||
UsdPhysics.RigidBodyAPI(blade).CreateKinematicEnabledAttr().Set(True)
|
||||
|
||||
joint = stage.GetPrimAtPath(PUSHER_JOINT)
|
||||
if joint.IsValid():
|
||||
joint.GetAttribute("physics:jointEnabled").Set(False)
|
||||
|
||||
graph = stage.GetPrimAtPath(ANIM_GRAPH)
|
||||
if graph.IsValid():
|
||||
graph.SetActive(False) # otherwise it rewrites the diverter targets every tick
|
||||
|
||||
# the blade must sweep through the conveyor rails rather than grind on them
|
||||
filt = UsdPhysics.FilteredPairsAPI.Apply(blade)
|
||||
rel = filt.CreateFilteredPairsRel()
|
||||
have = {str(t) for t in rel.GetTargets()}
|
||||
for path in BELTS + [SPAWN_BELT, BRANCH, "/World/Diverters/DiverterY_Split/Base"]:
|
||||
if stage.GetPrimAtPath(path).IsValid() and path not in have:
|
||||
rel.AddTarget(path)
|
||||
|
||||
# seat the blade just over the belt so flat items cannot slip underneath
|
||||
for op in UsdGeom.Xformable(blade).GetOrderedXformOps():
|
||||
if op.GetOpType() == UsdGeom.XformOp.TypeTranslate:
|
||||
v = op.Get()
|
||||
op.Set(Gf.Vec3d(v[0], C.BLADE_HOME_Y - BLADE_PARENT_Y, -0.135))
|
||||
break
|
||||
|
||||
|
||||
def hide_aim_markers(stage):
|
||||
"""the camera bodies carry cosmetic aim-ray cones that sit right over the inspection
|
||||
point; left visible they dominate the frame and segmentation locks onto them."""
|
||||
n = 0
|
||||
for rig in ["RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"]:
|
||||
p = stage.GetPrimAtPath(f"{CAMERA_BODIES}/{rig}/AimRay")
|
||||
if p.IsValid():
|
||||
UsdGeom.Imageable(p).MakeInvisible()
|
||||
n += 1
|
||||
return n
|
||||
|
||||
|
||||
def load_test_items(stage, meshes_dir=None):
|
||||
"""add the bundled per-class test meshes as dynamic rigid bodies"""
|
||||
meshes_dir = Path(meshes_dir or C.MESHES)
|
||||
manifest = json.loads((meshes_dir / "manifest.json").read_text())
|
||||
UsdGeom.Xform.Define(stage, ITEMS_ROOT)
|
||||
items = {}
|
||||
for i, (name, meta) in enumerate(sorted(manifest.items())):
|
||||
usd = meshes_dir / f"{name}.usd"
|
||||
if not usd.exists():
|
||||
continue
|
||||
prim = UsdGeom.Xform.Define(stage, f"{ITEMS_ROOT}/{name}").GetPrim()
|
||||
refs = prim.GetReferences()
|
||||
refs.ClearReferences() # idempotent: prepare() may run more than once
|
||||
refs.AddReference(str(usd))
|
||||
xf = UsdGeom.Xformable(prim)
|
||||
xf.ClearXformOpOrder()
|
||||
xf.AddTranslateOp().Set(Gf.Vec3d(9.0 + 1.2 * i, 5.0, 0.4))
|
||||
UsdPhysics.RigidBodyAPI.Apply(prim)
|
||||
# meshes exported from a streaming scene arrive kinematic and hidden - both make
|
||||
# them inert: kinematic ignores gravity and belt friction, hidden shows nothing
|
||||
UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(False)
|
||||
UsdPhysics.MassAPI.Apply(prim).CreateMassAttr().Set(0.6)
|
||||
px = PhysxSchema.PhysxRigidBodyAPI.Apply(prim)
|
||||
px.CreateEnableCCDAttr().Set(True)
|
||||
px.CreateSolverPositionIterationCountAttr().Set(24)
|
||||
px.CreateSleepThresholdAttr().Set(0.0) # a settled item must still be draggable
|
||||
UsdGeom.Imageable(prim).MakeVisible()
|
||||
items[name] = meta
|
||||
return items
|
||||
|
||||
|
||||
def prepare(stage, belt_speed=None, meshes_dir=None):
|
||||
"""everything the authored scene needs before it will run"""
|
||||
configure_physics(stage)
|
||||
configure_belts(stage, belt_speed)
|
||||
configure_pusher(stage)
|
||||
hidden = hide_aim_markers(stage)
|
||||
items = load_test_items(stage, meshes_dir)
|
||||
calib = json.loads((C.CONFIG / "calib.json").read_text())
|
||||
return dict(items=items, calib=calib, aim_markers_hidden=hidden)
|
||||
|
||||
|
||||
def load(usd_path=None, belt_speed=None, meshes_dir=None):
|
||||
stage = open_scene(usd_path)
|
||||
return stage, prepare(stage, belt_speed, meshes_dir)
|
||||
@@ -0,0 +1,135 @@
|
||||
"""Self-running item feeder: press Play and goods appear on the infeed belt one at a time,
|
||||
spaced by a fixed pitch along the belt.
|
||||
|
||||
It hooks a PhysX step callback rather than living in an outer async loop, so the scene runs
|
||||
on its own from the Play button - no driver script has to be babysitting it. The same
|
||||
callback also drives the laser gate and the pusher when `route` is enabled.
|
||||
|
||||
Pitch is measured along the belt between consecutive items, so the release condition is
|
||||
simply "the last one released has travelled PITCH from the spawn point".
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from .. import config as C
|
||||
|
||||
|
||||
class AutoFeeder:
|
||||
def __init__(self, cell, order=None, pitch=None, loop=False,
|
||||
route=None, on_event=None):
|
||||
"""
|
||||
cell : mechanics.Cell
|
||||
order : release order; defaults to every loaded item
|
||||
pitch : metres between consecutive items along the belt
|
||||
route : dict name -> class; when given, class D is diverted by the pusher
|
||||
on_event : optional callback(kind, name, payload) for logging
|
||||
"""
|
||||
self.cell = cell
|
||||
self.order = list(order or cell.items)
|
||||
self.pitch = pitch if pitch is not None else C.RELEASE_GAP
|
||||
self.loop = loop
|
||||
self.route = route or {}
|
||||
self.on_event = on_event
|
||||
self._sub = None
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.next_index = 0
|
||||
self.active = []
|
||||
self.released = []
|
||||
self.diverted = set()
|
||||
self.finished = {}
|
||||
self._busy = False # a push cycle owns the blade until it completes
|
||||
self._cycle = None
|
||||
|
||||
# ------------------------------------------------------------------ install
|
||||
def install(self):
|
||||
"""subscribe to the physics step; from here on the cell runs itself on Play"""
|
||||
from omni.physx import get_physx_interface
|
||||
if self._sub is None:
|
||||
self._sub = get_physx_interface().subscribe_physics_step_events(self._on_step)
|
||||
return self
|
||||
|
||||
def remove(self):
|
||||
self._sub = None
|
||||
|
||||
def _emit(self, kind, name, payload=None):
|
||||
if self.on_event:
|
||||
self.on_event(kind, name, payload or {})
|
||||
|
||||
# ------------------------------------------------------------------ per step
|
||||
def _on_step(self, dt):
|
||||
try:
|
||||
self._release_due()
|
||||
self._service_gate(dt)
|
||||
self._retire()
|
||||
except Exception as exc: # never let a callback kill the sim
|
||||
self._emit("error", "", {"exc": repr(exc)})
|
||||
|
||||
def _release_due(self):
|
||||
if self._busy or self.next_index >= len(self.order):
|
||||
if self.loop and self.next_index >= len(self.order) and not self.active:
|
||||
self.next_index = 0
|
||||
return
|
||||
if self.active:
|
||||
travelled = C.SPAWN_X - float(self.cell.pose(self.active[-1])[0])
|
||||
if travelled < self.pitch:
|
||||
return
|
||||
name = self.order[self.next_index]
|
||||
self.cell.release(name)
|
||||
self.active.append(name)
|
||||
self.released.append(name)
|
||||
self.next_index += 1
|
||||
self._emit("release", name, {"pitch": self.pitch})
|
||||
|
||||
def _service_gate(self, dt):
|
||||
"""laser gate -> pusher, as a small state machine so it spans several steps"""
|
||||
if self._cycle is not None:
|
||||
self._step_cycle(dt)
|
||||
return
|
||||
for name in list(self.active):
|
||||
if name in self.diverted or self.route.get(name) != "D":
|
||||
continue
|
||||
if self.cell.laser() == name:
|
||||
self._cycle = dict(name=name, phase="extend", t=0.0,
|
||||
y=C.BLADE_HOME_Y, held=0.0)
|
||||
self._busy = True
|
||||
self._emit("gate", name, {})
|
||||
return
|
||||
|
||||
def _step_cycle(self, dt):
|
||||
c = self._cycle
|
||||
name = c["name"]
|
||||
speed = C.PUSHER_SPEED
|
||||
if c["phase"] == "extend":
|
||||
c["y"] = min(C.BLADE_OUT_Y, c["y"] + speed * dt)
|
||||
self.cell.blade_to(c["y"])
|
||||
if c["y"] >= C.BLADE_OUT_Y - 1e-6:
|
||||
c["phase"] = "clear"
|
||||
elif c["phase"] == "clear":
|
||||
c["t"] += dt
|
||||
if float(self.cell.pose(name)[1]) > 0.50 or c["t"] > 1.5:
|
||||
c["phase"] = "wait"
|
||||
c["t"] = 0.0
|
||||
elif c["phase"] == "wait":
|
||||
# do not sweep the blade back through whatever has already arrived
|
||||
busy = self.cell.blade_path_busy(name)
|
||||
c["t"] += dt
|
||||
if busy is None or c["t"] > 1.5:
|
||||
c["held"] = c["t"]
|
||||
c["phase"] = "retract"
|
||||
elif c["phase"] == "retract":
|
||||
c["y"] = max(C.BLADE_HOME_Y, c["y"] - speed * dt)
|
||||
self.cell.blade_to(c["y"])
|
||||
if c["y"] <= C.BLADE_HOME_Y + 1e-6:
|
||||
self.diverted.add(name)
|
||||
self._busy = False
|
||||
self._cycle = None
|
||||
self._emit("divert", name, {"held": round(c["held"], 3)})
|
||||
|
||||
def _retire(self):
|
||||
for name in list(self.active):
|
||||
place = self.cell.where(name)
|
||||
if place in ("bin", "line-end"):
|
||||
self.finished[name] = place
|
||||
self.active.remove(name)
|
||||
self._emit("done", name, {"where": place})
|
||||
@@ -0,0 +1,132 @@
|
||||
"""Floor and lighting for the plow cell.
|
||||
|
||||
The authored scene has one distant light and no floor at all: goods that miss a tray fall
|
||||
for kilometres (traces from the first sorting runs end at z = -20000), which makes "dropped"
|
||||
and "thrown across the room" look identical in a log and gives the eye nothing to judge the
|
||||
cell against. A floor turns both into something you can see and measure.
|
||||
|
||||
The floor is a **static collider** - no rigid body - so it costs nothing to simulate and
|
||||
catches anything that leaves the line at the height a real floor would.
|
||||
|
||||
Lighting presets exist because the vision stack is measured under them. They are the same
|
||||
three the earlier flow evaluations used, so results stay comparable:
|
||||
|
||||
bright dome 1800 + strong key easy case, high contrast on the belt
|
||||
dim dome 350 + weak key near the sensor's noise floor
|
||||
harsh dome 120 + hard low key long shadows, specular blowout on the rails
|
||||
|
||||
`apply_lighting(stage, "dim")` swaps a preset without touching anything else, so a run can
|
||||
sweep them. Every light this module makes lives under /World/CellLighting; authored lights
|
||||
elsewhere are dimmed rather than deleted, so the scene file stays as built.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pxr import Gf, Sdf, UsdGeom, UsdLux, UsdPhysics, UsdShade
|
||||
|
||||
from .. import config as C
|
||||
|
||||
FLOOR = "/World/CellFloor"
|
||||
LIGHTS = "/World/CellLighting"
|
||||
|
||||
# dome intensity, key (distant) intensity, key rotation XYZ, dome colour
|
||||
PRESETS = {
|
||||
"bright": dict(dome=1800.0, key=3000.0, angle=(-45.0, 20.0, 0.0),
|
||||
tint=(1.0, 1.0, 1.0)),
|
||||
"dim": dict(dome=350.0, key=600.0, angle=(-50.0, -25.0, 0.0),
|
||||
tint=(0.92, 0.95, 1.0)),
|
||||
"harsh": dict(dome=120.0, key=5200.0, angle=(-16.0, 65.0, 0.0),
|
||||
tint=(1.0, 0.95, 0.86)),
|
||||
}
|
||||
DEFAULT_PRESET = "bright"
|
||||
|
||||
|
||||
def add_floor(stage, z=None, size=60.0, colour=(0.22, 0.23, 0.25)):
|
||||
"""a static floor under the whole cell.
|
||||
|
||||
Collision goes on the *child mesh*, with the position on the parent Xform: a Cube that
|
||||
is both scaled and collided reports the wrong bounds to PhysX and goods drop straight
|
||||
through it. Cube size is 2.0 so the scale op equals the half-extent.
|
||||
"""
|
||||
z = C.FLOOR_Z if z is None else z
|
||||
xf = UsdGeom.Xform.Define(stage, FLOOR)
|
||||
ops = UsdGeom.Xformable(xf.GetPrim())
|
||||
ops.ClearXformOpOrder()
|
||||
ops.AddTranslateOp().Set(Gf.Vec3d(-3.0, 0.0, z - 0.05))
|
||||
|
||||
mesh = UsdGeom.Cube.Define(stage, f"{FLOOR}/Mesh")
|
||||
mesh.CreateSizeAttr().Set(2.0)
|
||||
mops = UsdGeom.Xformable(mesh.GetPrim())
|
||||
mops.ClearXformOpOrder()
|
||||
mops.AddScaleOp().Set(Gf.Vec3f(size / 2.0, size / 2.0, 0.05))
|
||||
mesh.CreateDisplayColorAttr().Set([Gf.Vec3f(*colour)])
|
||||
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
|
||||
|
||||
mat = UsdPhysics.MaterialAPI.Apply(
|
||||
stage.DefinePrim(f"{FLOOR}/M_floor", "Material"))
|
||||
mat.CreateStaticFrictionAttr().Set(0.7)
|
||||
mat.CreateDynamicFrictionAttr().Set(0.6)
|
||||
mat.CreateRestitutionAttr().Set(0.0) # a dropped item must not bounce away
|
||||
api = UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim())
|
||||
api.Bind(UsdShade.Material(stage.GetPrimAtPath(f"{FLOOR}/M_floor")),
|
||||
bindingStrength=UsdShade.Tokens.strongerThanDescendants,
|
||||
materialPurpose="physics")
|
||||
return FLOOR
|
||||
|
||||
|
||||
def _dim_authored(stage):
|
||||
"""turn authored lights down instead of deleting them, so the file stays as built"""
|
||||
n = 0
|
||||
for prim in stage.Traverse():
|
||||
if LIGHTS in str(prim.GetPath()):
|
||||
continue
|
||||
a = prim.GetAttribute("inputs:intensity")
|
||||
if a and a.IsValid() and a.Get() is not None:
|
||||
a.Set(0.0)
|
||||
n += 1
|
||||
return n
|
||||
|
||||
|
||||
def apply_lighting(stage, preset=DEFAULT_PRESET):
|
||||
"""install (or re-point) the cell's dome + key light to a named preset"""
|
||||
if preset not in PRESETS:
|
||||
raise ValueError(f"unknown preset {preset!r}; have {sorted(PRESETS)}")
|
||||
p = PRESETS[preset]
|
||||
_dim_authored(stage)
|
||||
UsdGeom.Xform.Define(stage, LIGHTS)
|
||||
|
||||
dome = UsdLux.DomeLight.Define(stage, f"{LIGHTS}/Dome")
|
||||
dome.CreateIntensityAttr().Set(p["dome"])
|
||||
dome.CreateColorAttr().Set(Gf.Vec3f(*p["tint"]))
|
||||
|
||||
key = UsdLux.DistantLight.Define(stage, f"{LIGHTS}/Key")
|
||||
key.CreateIntensityAttr().Set(p["key"])
|
||||
key.CreateAngleAttr().Set(1.5 if preset != "harsh" else 0.3) # harsh = sharp shadows
|
||||
kops = UsdGeom.Xformable(key.GetPrim())
|
||||
kops.ClearXformOpOrder()
|
||||
kops.AddRotateXYZOp().Set(Gf.Vec3f(*p["angle"]))
|
||||
|
||||
stage.GetPrimAtPath(LIGHTS).SetCustomDataByKey("preset", preset)
|
||||
return dict(preset=preset, **p)
|
||||
|
||||
|
||||
def stage_cell(stage, preset=DEFAULT_PRESET, floor=True):
|
||||
"""floor + lighting in one call"""
|
||||
out = dict(lighting=apply_lighting(stage, preset))
|
||||
if floor:
|
||||
out["floor"] = add_floor(stage)
|
||||
|
||||
# Only NOW switch off the scene's own /Environment/defaultLight, and only if the preset
|
||||
# really did put lights in. Doing it first - as this did briefly - hides the one
|
||||
# authored light before its replacement exists, so any failure in between leaves the
|
||||
# stage with NO light at all: the viewport goes black and the only thing still visible
|
||||
# is the emissive laser stripe. The reason to switch it off at all is that two
|
||||
# uncoordinated rigs make the exposure visibly swim as RTX re-converges.
|
||||
from pxr import UsdGeom as _UG
|
||||
lit = stage.GetPrimAtPath("/World/CellLighting")
|
||||
dl = stage.GetPrimAtPath("/Environment/defaultLight")
|
||||
if lit.IsValid() and any(True for _ in lit.GetChildren()) and dl.IsValid():
|
||||
_UG.Imageable(dl).MakeInvisible()
|
||||
out["default_light_off"] = True
|
||||
else:
|
||||
out["default_light_off"] = False # replacement missing - keep the only light on
|
||||
return out
|
||||
Reference in New Issue
Block a user