isaac/robozon_sorter/config.py

"""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"}