diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4fbe3ef --- /dev/null +++ b/.gitignore @@ -0,0 +1,25 @@ +# Веса моделей в репозиторий не кладутся - ссылки на источники в MODELS.md +*.pth +*.pt +*.onnx +*.ckpt +*.safetensors + +# Пропсы конвейера NVIDIA (274 МБ): не наши и скачиваются штатным скриптом +assets/conveyors/ +# восстановить: python3 scripts/fetch_assets.py + +# Выход прогонов - воспроизводится, в истории не нужен +control_test/captures/ +control_test/runtime/ +control_test/diag/ +runs/ + +# Резервные копии, которые делались перед правками +*.bak +*.orig +*.pre* + +__pycache__/ +*.pyc +.DS_Store diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..feafd08 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,328 @@ +# Context for Claude + +Working notes for this repo: how the cell is put together, where the rest of the assets +live, and the traps that are not visible from the code. Read this before changing anything +in `robozon_sorter/sim/` — several constants here are load-bearing and look arbitrary. + +--- + +## 1. What this is + +A conveyor sorting cell in Isaac Sim 6.0 (Kit 110). Goods ride a belt, three stereo pairs +measure each one under a camera portal, and a pusher diverts the round ones onto a branch +belt and into a bin. + +``` +infeed belt ──▶ camera portal ──▶ laser gate ──▶ pusher ──▶ Belt_01 ──▶ bin + x=+2.3 x=-0.75 x=-3.74 x=-3.90 y=+1.3…2.0 + (CRE-ROI v2b) (class D only) +``` + +Goods travel in **−X** at 1 m/s. Belt surface is at **z = 1.781** everywhere on the main +run and the branch. + +**Classes:** B = sortable, C = oversize (any dim >450×320×320 mm or <10 mm), D = round +(`K = r_in/R_out > 0.8`). Only D is diverted. + +--- + +## 2. Where things are + +### This repo +`scene/sorter.usd` is the real authored cell, not a procedural rebuild. It was exported +from `90_degree.usd` with two changes: conveyor references re-pointed to +`../assets/conveyors/`, and the 48-object library dropped in favour of six bundled test +meshes with baked geometry. + +Do not "simplify" the scene by regenerating it from code. That was tried and rejected — +the result looked nothing like the real cell. + +`scene/plow_cell.usd` is the same authored build carried over whole, but as the **bare +mechanics**: conveyors, the Y-split pusher and the plow, with no camera portal, no laser +gate and no item library. Cameras, speed scenarios and laser sensors get added on top of +it. Rebuild it with `scripts/build_plow_cell.py`, which re-points the S3 conveyor +references at `assets/conveyors/`, points the plow at `assets/plow/`, and strips the baked +`targetPosition.timeSamples` track off the pusher drive — time samples outrank the +attribute default, so while they are present nothing can *control* that drive. + +The plow meshes matter: `assets/meshes/plow_*.usd` are 8-point placeholder boxes that +`sorter.usd` still uses, while `assets/plow/plow_*.usd` is the real geometry (72k / 23k +points). `scripts/smoke_plow_cell.py` asserts the point count precisely because a stub +composes without error and just looks wrong. + +### Remote machine (where Isaac Sim runs) + +``` +host dasha@46.39.224.77 +isaac /home/whatevenif/isaacsim (python.sh lives here) +project /home/dasha/robozon-sorter (deployed copy, ~308 MB, runnable) +assets /home/dasha/isaac_assets (the original working tree) +``` + +Connect with an SSH ControlMaster (plain repeated ssh exhausts local ephemeral ports on +long sessions) and tunnel the Kit python server: + +```bash +ssh -M -S ~/.ssh/cm/dasha -o ControlPersist=900 -fN dasha@46.39.224.77 +ssh -fN -L 8226:127.0.0.1:8226 dasha@46.39.224.77 # isaacsim.code_editor.python_server +``` + +Then send code with the `isaac-sim-remote` skill's `isaacsim_send.py`. Named contexts keep +state between calls, which is how the long experiments were run. + +### More meshes, if six are not enough + +| What | Where | Notes | +|---|---|---| +| 31 classified objects | `/home/dasha/isaac_assets/categories.json` | 13 C, 11 D, 7 B; has `zone`, `obb_extents_m`, `k_round`, `label_ru` | +| the objects themselves | `/home/dasha/isaac_assets/robozon_conveyor_scaled.usd` under `/World/CVObjects` | 48 prims, 31 of them classified | +| raw source geometry | `/home/dasha/.objaverse/hf-objaverse-v1/glbs/` | 19 `.glb`, 123 MB — what CVObjects reference | +| already-exported | `/home/dasha/isaac_assets/export_meshes/` | flattened USDs, the six in this repo came from here | +| conveyor art | `/home/dasha/isaac_assets/Props/Conveyors/` | 283 MB, mirrors the Omniverse S3 tree | + +To add a test mesh, flatten it out of the source scene so it carries no `.glb` reference: + +```python +ns = Usd.Stage.CreateInMemory() +root = UsdGeom.Xform.Define(ns, "/Item") +root.GetPrim().GetReferences().AddReference( + "/home/dasha/isaac_assets/robozon_conveyor_scaled.usd", f"/World/CVObjects/{name}") +ns.SetDefaultPrim(root.GetPrim()) +ns.Flatten().Export(f"assets/meshes/{name}.usd") +``` + +Then add an entry to `assets/meshes/manifest.json` with `zone` and `gt_dims_mm`. Objects in +that scene sit at **0.49× real size** (1/3-scale meshes × 1.4706), which is why +`config.DIM_SCALE = 1/2.041` converts metres to real millimetres. + +### Scene backups +`/home/dasha/isaac_assets/backups/` — `robozon_prescale_2252.usd` is the clean pre-scale +original, useful if the working scene ever gets damaged. + +--- + +## 3. Mechanics + +`sim/scene.py` opens the USD and re-applies the runtime settings USD does not carry. +`sim/mechanics.py` is the per-item behaviour. `sim/spawner.py` hooks a PhysX step callback +so the cell runs itself from the Play button. + +### Belts +Driven by `PhysxSurfaceVelocityAPI` on kinematic slabs, not by the authored +`ConveyorBeltGraph` nodes — those carry no velocity and only fight the explicit setting, so +they are switched off on load. + +**`surfaceVelocity` is expressed in the body's LOCAL frame.** `Belt_01` +(`/World/ConveyorTrack_03/Belt_01`) is rotated: its local X points along world −Y. Carrying +goods toward the bin therefore needs `(−speed, 0, 0)`. Setting the intuitive `(0, +speed, 0)` +drags them sideways and they sit there looking stuck. Resolve the local axes with +`XformCache.GetLocalToWorldTransform(prim).TransformDir(...)` before setting it. + +### Pusher +A kinematic blade moved directly from script. It is **not** joint-driven: the authored +`PusherSlide` prismatic drive is unusable because USD drive-target writes reach PhysX about +a second late, so the blade never finishes its stroke while the item is still in reach. +The joint is disabled and `DiverterAnimGraph` is switched off (it rewrites diverter targets +every tick). + +Blade collisions are filtered against every belt so it sweeps through the side rails +instead of grinding on them. + +**Speed and physics rate are a matched pair: 2.5 m/s at 120 Hz.** Do not change one alone. + +* The blade covers 0.70 m of belt; the beam trips with the item at x = −3.61, so the whole + cycle must fit in 0.64 m of travel = 0.64 s at 1 m/s. +* Stroke is 0.72 m → extension alone needs >1.12 m/s, extend+retract >2.25 m/s. +* Measured: 1.0 / 1.5 / 2.0 / 2.5 m/s all deliver to the bin. **3.0 m/s throws the item** + (ends up at y≈122, z≈−2070) — the kinematic blade injects too much impulse. +* Raising the solver rate to 480 Hz to "smooth" the impulse also breaks the landing. It was + tried; the item stopped reaching the bin. 120 Hz is the validated setting. + + +### Plow sorting station (`plow_cell.usd`) + +Two-way sort at the plow, verified by measurement. Rebuild the geometry with +`scripts/narrow_plow.py` then `scripts/place_plow_lanes.py`. + +**Arm width is 600 mm**, not the authored 730. The wider arm overhung both rails and +clipped goods it should have passed. `narrow_plow.py` does not guess which local axis +carries the length - it tries each and measures; the answer is **local Y, scale 0.8219**. +Hinge, drive and limits untouched. + +**Lane geometry, all verified against the belt (main belt x -7.00..-6.00, y +-0.45, top +z 1.7805):** + +| | span | contact | +|---|---|---| +| lane B, perpendicular, travel -Y | x -7.03..-6.58, y -2.45..-0.45 | height 0.0 mm, Y 0.0 mm | +| lane C, 45 deg, travel (-X,+Y) | x -8.12..-6.39, y +0.45..+2.18 | height 0.0 mm, Y 0.1 mm | + +Three placement mistakes were made and corrected, all of which looked fine in the tree and +wrong in the viewport: + +1. **Height.** These tracks carry an authored `-0.1` z offset, putting their belts at + z 1.681 - 100 mm below the run, so they read as separate furniture. Use `LANE_DROP = 0`. +2. **X position.** Lane B was first placed at x -7.48..-7.03, entirely *past* where the + main belt stops (-7.00). A plow sweeps goods sideways while they are still on the belt, + so a lane must run **alongside** it inside the arm's span (x -7.12..-6.52), never beyond + the end. Same for C: at x -6.90 its near corner sat behind the plow and +Y deflections + had nothing to land on; -6.55 puts the corner at the arm tip. +3. **A 45 deg lane does not meet a straight edge at its centreline.** Its near corner runs + ahead by 159 mm (measured), so the lane needs that much offset or it cuts into the belt. + +**An angled lane needs a transition DECK, not a corner patch.** Offsetting by 159 mm makes +its near corner touch, but only that one corner - the other stands off by +width/sqrt2 = 318 mm. Patching just that triangle is not enough either: the plow can put a +item anywhere across the discharge width, so anything pushed wide still drops through the +second gap. `place_plow_lanes.add_transition()` therefore decks the whole corner - the +convex span of the belt edge across the junction (x -7.00..-6.00) and both end-face +corners, giving y 0.45..0.768 - coplanar with both belt surfaces. A square lane like B +meets flush along its whole face, so the builder returns nothing rather than emitting a +zero-area collider. + +**Every lane also needs a corner deck**, flush or not. The belt is wider than the lane - +the run reaches x=-6.00 while lane B stops at -6.58 - so the right angle between them is +open air, and anything the plow pushes sideways in that leftover span drops through. +`add_corner_deck()` fills it with a triangular fillet (B: legs 575 mm, corners +(-6.575,-0.45), (-6.00,-0.45), (-6.575,-1.025); C: legs 391 mm), turning the right angle +into a chute. Deck plus fillet together leave no open surface across the discharge. + +Lanes start at y = +-0.45, which clears the arm's swept envelope: a 600 mm arm at +-35 deg +reaches 0.60*sin(35) = **0.344 m** either side. + +**The conveyor shell collides.** `SM_ConveyorBelt_*_02` has `collision=True`, and that +includes the blue side rails - they physically block goods from leaving the belt, which is +exactly what "nothing reaches the bins" looks like. `plow_sort.open_junction()` clears the +collider on the three shells at the junction, the way a real plow station has its rails cut +away. Each `Belt` keeps its own collider, so nothing falls through. + +**Plow delivery is geometrically impossible as currently placed - measured.** The arm is +600 mm and pivots at the belt centre (-7.05, 0), so its lateral reach is +0.60*sin(35) = **0.344 m**. Both lanes start at **y = +-0.45**. That leaves a **106 mm dead +band**: the blade can push an item to 0.344 and no further, the lane begins at 0.45, and the +item runs off the belt end and stops at x ~= -7.0 with v = 0, or falls. Every trace in +`runs/gt_run5.json` shows exactly that - barrel drifts from y=-0.09 to y=+0.148 and halts. + +The arm sits at z 1.810..1.890 while the lane surfaces are at 1.7805, i.e. **29 mm above +them**, so it would sweep *over* a lane edge rather than into it. The earlier reasoning that +lanes had to start beyond the swept envelope was therefore unnecessary, and it is what +opened the dead band. Closing it means one of: lanes in to ~y=0.33, a longer arm, or a +larger swing (the joint limit is +-35 authored). + +**The angle does not track the command.** Measured in a demo run: an item commanded 0.0 saw +the arm at **+30.5**, one commanded +30.0 saw **-34.6**. Both then went to the same lane. So +on top of the reach gap the control sign and the settling are wrong. Two things are known +about this path and both matter: + +* `Plow.target(deg)` writes the whole angle at once; calling it *and* `step_toward()` leaves + the ramp nothing to do and the blade snaps. Use one or the other, and `plow_sort` uses the + ramp. +* `Plow.home()` does not settle the arm - it sets a target, and the compliant drive needs + time. Consecutive trials therefore start from wherever the previous one left the blade + (measured: -26.4 and -14.4 at the start of runs meant to begin at 0). + +* A **positive command deflects to -Y (lane B)**, opposite to the natural reading. Measured, + not assumed, exactly as the module docstring warns. + +Until reach and tracking are fixed the plow does not deliver: `scripts/run_demo.py` reports +the classification honestly and a delivery rate near zero. + +### Retract interlocks +The blade returns only when (a) the pushed item has cleared to y > 0.5 and (b) no other item +is inside the blade's footprint. Retracting blindly sweeps the blade back through the next +item and knocks it over — that was a real observed failure, not a hypothetical. + +### Plow (`plow_cell.usd` only) +The second diverter, at x = −7.05. Mechanically the opposite of the pusher: a **dynamic +arm on a revolute joint with an angular force drive** (axis Z, limits ±35°, stiffness +120000, damping 1500, 12 kg with gravity disabled). Being force-driven it is compliant — +it yields on contact instead of teleporting through cargo — so `sim/plow.py` commands a +drive *target* rather than writing a transform. + +Two consequences: a target is a request, not a position (read `Plow.angle`, which measures +the arm's simulated pose, never assume it arrived), and the rate is not free. The authored +graph swings 30° in **7 ms** (72 rad/s), which is a display animation, not a sortable +motion — at that rate the blade lands as an impulse, the same failure the pusher shows +above 2.5 m/s. `Plow.step_toward` ramps the target at `config.PLOW_RATE` (180 °/s) instead; +`config.PLOW_RATE_AUTHORED` keeps the original figure for reference. + +The scene keeps its authored `DiverterAnimGraph`, so pressing Play alone demonstrates the +cell. `plow_cell.prepare(..., script_control=True)` switches that graph off — it has to go, +or it rewrites the drive target every tick and overwrites anything Python commands. + +There is also a stray second `ConveyorTrack_01` at stage root, outside `/World`, left over +from how the cell was assembled; it composes as a duplicate belt in the same place and is +deactivated on load rather than deleted, so the file stays as authored. + +### Laser gate +A genuine `raycast_closest` across the belt, not a coordinate test. The beam starts at +y = −0.24, which is deliberately **clear of the blade's retracted footprint** (the blade +spans y −0.33…−0.27). Start it any further out and the ray simply reads the blade and the +gate never sees cargo. + +--- + +## 4. Vision (CRE-ROI v2b) + +`cv/pipeline.py`. Per item, once, while it sits under the portal: FastSAM segment-everything +→ keep the blob covering the projected inspection point in every view → ROI crop to a fixed +320 px side → **one batched CRE-Stereo pass over all three crops** → fuse, dropping views +whose 3D centroid disagrees with the median by >10 cm → belt-plane OBB + `r_in/R_out`. + +The crop must use the **identical column window in both eyes**, left-padded by the maximum +disparity, or the right-hand counterpart falls outside the crop. Disparity is invariant to +an equal column shift, so depth stays correct. + +**The stereo rig must be rectified.** Both eyes of a pair share one orientation, with the +right eye offset along its X. Aiming each eye separately at the target verges the pair and +breaks `depth = fx·b/disp` — reconstruction came back at 2–5 m instead of 0.63 m. If +`config/calib.json` ever loses its `"rectified": true` flag, the pipeline refuses to load +rather than silently producing garbage. + +Measured: dimensions land within ~34 mm of ground truth on the largest edge; the batched CRE +pass costs ~170 ms per item. + +--- + +## 5. Traps that cost real time + +Each of these presented as a silent failure, not an error. + +- **Copied items arrive kinematic and hidden.** Kinematic bodies ignore gravity and belt + friction; hidden ones are invisible to the cameras while still simulating. Both must be + cleared, plus `sleepThreshold = 0` or a settled item is never woken by the belt. +- **`BBoxCache` / `XformCache` return the AUTHORED transform during simulation.** A moving + item looks frozen. Use `RigidPrim.get_world_poses()`. +- **`timeline.stop()` resets items to their authored poses**, so captures of a finished run + must be taken while still playing. +- **`rep.orchestrator.step_async()` stops the timeline.** Any live loop that classifies must + call `timeline.play()` again afterwards or the whole line freezes mid-run. +- **The viewport's active camera may not be Persp.** Setting Persp's pose then does nothing; + call `vp.set_active_camera("/OmniverseKit_Persp")` first. +- **Camera bodies carry cosmetic `AimRay` cones** that sit right over the inspection point + and dominate the frame. Hidden on load. +- **Stale composition:** if a stage was opened while a referenced asset was missing, USD + caches the failure. Dropping the file in later does not fix it — the prim stays valid with + `localErrors: none` but empty typeName and bbox. Diagnose with `GetPrimStack()`, fix by + re-opening the stage. +- **`Stage.TraverseAll()` crashes Isaac on this scene.** Use targeted `GetPrimAtPath`, or run + read-only checks headlessly via `/home/whatevenif/isaacsim/python.sh` (has pxr, no Kit). +- **Cube colliders:** use `size = 2.0` so the scale op equals the half-extent. Any other + arrangement makes PhysX use the wrong bounds and goods drop through the belt. + +--- + +## 6. Known limitations + +- **Classification is weak.** Metrology is sound but the roundness metric does not separate + classes at this scale: genuinely round items read K ≈ 0.75 against a 0.80 threshold while a + plain box reads 0.76. `config.ROUND_K` needs recalibration before the vision output should + drive the pusher for real. Use `--no-vision` to exercise mechanics on ground truth. +- **Consecutive class-D items are missed at the default 700 mm pitch.** Headway is 0.70 s and + one pusher cycle is 0.62 s; the gate is blind while a cycle runs, so a D item immediately + behind another D crosses the beam unseen. Verified: `D,C,D,B` sorts both D correctly, `D,D` + back-to-back loses the second. Fixing it needs a larger pitch, a slower belt or a second + diverter — not a faster blade, which is already at its stability limit. +- **The plow (`DiverterEnd`) is untouched in `sorter.usd`.** There its geometry was + restored and its arm returned to rest, but its control logic is left exactly as authored. + It is `scene/plow_cell.usd` that makes the plow controllable — see below. diff --git a/MODELS.md b/MODELS.md new file mode 100644 index 0000000..d9d2c03 --- /dev/null +++ b/MODELS.md @@ -0,0 +1,65 @@ +# Веса моделей + +В репозиторий не попадают: около 4.5 ГБ, и все скачиваются из первоисточников. Ниже — +откуда именно, чтобы окружение поднималось повторяемо. + +## DEFOM-Stereo — рабочий бейзлайн пайплайна + +Репозиторий: +Веса: + +```bash +git clone https://github.com/Insta360-Research-Team/DEFOM-Stereo.git +cd DEFOM-Stereo && mkdir -p checkpoints && cd checkpoints +gdown 1XuAM4vqzura_6NKN70hMW5lFD4TafnDL # defomstereo_vitl_sceneflow.pth 1.53 ГБ <- используется +gdown 1qyXKO-Nxq3ndl2H0deQpo6BSvwlGKYEg # defomstereo_vits_sceneflow.pth 173 МБ (лёгкий вариант) +wget https://huggingface.co/depth-anything/Depth-Anything-V2-Large/resolve/main/depth_anything_v2_vitl.pth +``` + +Энкодер DepthAnythingV2 обязателен: без него модель не поднимается. + +Скачивание папкой целиком (`gdown --folder`) не работает — перечисляет файлы и ничего не +качает. Только по одному идентификатору. + +В репозитории есть и штатные скрипты: `scripts/download_models.sh`, `scripts/download_dav2.sh`. + +## CREStereo — второй движок, точнее по габаритам + + — веса `crestereo_eth3d.pth` (95 МБ) по +ссылкам из его README. + +## Сегментация + +Обе модели в текущем бейзлайне **не используются** — товар отделяется от полотна +превышением над плоскостью. Оставлены для сравнительных прогонов. + +* FastSAM-s.pt (23 МБ) — +* yolo26n-seg.pt — (у нас вариант `n`, не `s`) + +## Fast FoundationStereo — проверялся, в бейзлайн не вошёл + + +Веса: + +Брался ONNX-экспорт `23_36_37_iters_4_res_320x736` (69 МБ, id `1p9vgRh_8R1FXA79l8VdnN28dDQ3NEEhf`) +и его yaml (`11oKt_6_jSdTqwBw1qotxmwqKxzYkvKdI`). + +Замер получился нечестным по времени: `onnxruntime-gpu` 1.28 требует CUDA 13, на сервере +12.8, колёс под CUDA 12 для python 3.12 нет, `onnx2torch` падает на динамическом `Clip`, +TensorRT не установлен — модель шла на CPU. Точность при этом от устройства не зависит и +вышла вчетверо хуже CRE (152.8 против 32.4 мм). + +## Пропсы конвейера NVIDIA + +`assets/conveyors/` (274 МБ) тоже не в репозитории — восстанавливаются штатным +`python3 scripts/fetch_assets.py`. Источник: `omniverse-content-production.s3-us-west-2.amazonaws.com`, +раздел `Assets/Isaac/6.0/Isaac/Props/Conveyors/`. + +## Куда смотрят пути в коде + +Сейчас они абсолютные и указывают на сервер: + +* `control_test/measure_flow.py`, `control_test/cv_worker.py` — константа `CV = "/home/dasha/isaac_assets/cv"` +* `control_test/measure_plane.py` — путь к `defom-stereo/checkpoints/` + +При переносе на другую машину поправить в этих трёх файлах. diff --git a/assets/items/air_conditioner.usd b/assets/items/air_conditioner.usd new file mode 100644 index 0000000..ee85068 Binary files /dev/null and b/assets/items/air_conditioner.usd differ diff --git a/assets/items/backpack.usd b/assets/items/backpack.usd new file mode 100644 index 0000000..0f7d68a Binary files /dev/null and b/assets/items/backpack.usd differ diff --git a/assets/items/bag.usd b/assets/items/bag.usd new file mode 100644 index 0000000..1a6ffc8 Binary files /dev/null and b/assets/items/bag.usd differ diff --git a/assets/items/banana.usd b/assets/items/banana.usd new file mode 100644 index 0000000..7aeffd8 Binary files /dev/null and b/assets/items/banana.usd differ diff --git a/assets/items/bolts_cluster.usd b/assets/items/bolts_cluster.usd new file mode 100644 index 0000000..5533e38 Binary files /dev/null and b/assets/items/bolts_cluster.usd differ diff --git a/assets/items/bottle.usd b/assets/items/bottle.usd new file mode 100644 index 0000000..ac10b1f Binary files /dev/null and b/assets/items/bottle.usd differ diff --git a/assets/items/box_300x200x200.usd b/assets/items/box_300x200x200.usd new file mode 100644 index 0000000..df89acc Binary files /dev/null and b/assets/items/box_300x200x200.usd differ diff --git a/assets/items/box_400x400x300.usd b/assets/items/box_400x400x300.usd new file mode 100644 index 0000000..173554e Binary files /dev/null and b/assets/items/box_400x400x300.usd differ diff --git a/assets/items/briefcase_hard.usd b/assets/items/briefcase_hard.usd new file mode 100644 index 0000000..5a72d51 Binary files /dev/null and b/assets/items/briefcase_hard.usd differ diff --git a/assets/items/bucket.usd b/assets/items/bucket.usd new file mode 100644 index 0000000..de9fd09 Binary files /dev/null and b/assets/items/bucket.usd differ diff --git a/assets/items/carton_large.usd b/assets/items/carton_large.usd new file mode 100644 index 0000000..5533f35 Binary files /dev/null and b/assets/items/carton_large.usd differ diff --git a/assets/items/chip_bag.usd b/assets/items/chip_bag.usd new file mode 100644 index 0000000..04a2e08 Binary files /dev/null and b/assets/items/chip_bag.usd differ diff --git a/assets/items/cleat_small.usd b/assets/items/cleat_small.usd new file mode 100644 index 0000000..4ecdd1c Binary files /dev/null and b/assets/items/cleat_small.usd differ diff --git a/assets/items/clothespin_flat.usd b/assets/items/clothespin_flat.usd new file mode 100644 index 0000000..8e21f5c Binary files /dev/null and b/assets/items/clothespin_flat.usd differ diff --git a/assets/items/cone.usd b/assets/items/cone.usd new file mode 100644 index 0000000..613660a Binary files /dev/null and b/assets/items/cone.usd differ diff --git a/assets/items/cooler_box.usd.heavy b/assets/items/cooler_box.usd.heavy new file mode 100644 index 0000000..9bae803 Binary files /dev/null and b/assets/items/cooler_box.usd.heavy differ diff --git a/assets/items/cooler_cube.usd b/assets/items/cooler_cube.usd new file mode 100644 index 0000000..9ef511a Binary files /dev/null and b/assets/items/cooler_cube.usd differ diff --git a/assets/items/cylinder.usd b/assets/items/cylinder.usd new file mode 100644 index 0000000..f7128a3 Binary files /dev/null and b/assets/items/cylinder.usd differ diff --git a/assets/items/detergent.usd b/assets/items/detergent.usd new file mode 100644 index 0000000..d76ecc1 Binary files /dev/null and b/assets/items/detergent.usd differ diff --git a/assets/items/duffel_bag.usd.heavy b/assets/items/duffel_bag.usd.heavy new file mode 100644 index 0000000..6b4a4e3 Binary files /dev/null and b/assets/items/duffel_bag.usd.heavy differ diff --git a/assets/items/duffel_round.usd b/assets/items/duffel_round.usd new file mode 100644 index 0000000..4833831 Binary files /dev/null and b/assets/items/duffel_round.usd differ diff --git a/assets/items/headphones.usd b/assets/items/headphones.usd new file mode 100644 index 0000000..a08bd0b Binary files /dev/null and b/assets/items/headphones.usd differ diff --git a/assets/items/helmet.usd b/assets/items/helmet.usd new file mode 100644 index 0000000..f9da2f8 Binary files /dev/null and b/assets/items/helmet.usd differ diff --git a/assets/items/lunchbox.usd b/assets/items/lunchbox.usd new file mode 100644 index 0000000..a9bb60f Binary files /dev/null and b/assets/items/lunchbox.usd differ diff --git a/assets/items/manifest.json b/assets/items/manifest.json new file mode 100644 index 0000000..6e9dd2b --- /dev/null +++ b/assets/items/manifest.json @@ -0,0 +1,252 @@ +{ + "backpack": { + "zone": "B", + "gt_dims_mm": [ + 455, + 370, + 301 + ], + "k_round": 0.82, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "bag": { + "zone": "D", + "gt_dims_mm": [ + 202, + 175, + 170 + ], + "k_round": 0.896, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "banana": { + "zone": "D", + "gt_dims_mm": [ + 183, + 71, + 33 + ], + "k_round": 0.94, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "bolts_cluster": { + "zone": "B", + "gt_dims_mm": [ + 194, + 136, + 53 + ], + "k_round": 0.718, + "label_ru": "Подходит для сортировки" + }, + "bottle": { + "zone": "D", + "gt_dims_mm": [ + 305, + 91, + 91 + ], + "k_round": 0.995, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "box_300x200x200": { + "zone": "B", + "gt_dims_mm": [ + 301, + 200, + 200 + ], + "k_round": 0.72, + "label_ru": "Подходит для сортировки" + }, + "box_400x400x300": { + "zone": "B", + "gt_dims_mm": [ + 401, + 400, + 300 + ], + "k_round": 0.716, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "bucket": { + "zone": "D", + "gt_dims_mm": [ + 287, + 287, + 272 + ], + "k_round": 0.995, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "chip_bag": { + "zone": "D", + "gt_dims_mm": [ + 250, + 162, + 69 + ], + "k_round": 0.811, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "cone": { + "zone": "D", + "gt_dims_mm": [ + 500, + 350, + 350 + ], + "k_round": 0.991, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "cylinder": { + "zone": "D", + "gt_dims_mm": [ + 435, + 50, + 43 + ], + "k_round": 0.867, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "detergent": { + "zone": "B", + "gt_dims_mm": [ + 278, + 260, + 180 + ], + "k_round": 0.742, + "label_ru": "Подходит для сортировки" + }, + "headphones": { + "zone": "D", + "gt_dims_mm": [ + 198, + 195, + 93 + ], + "k_round": 0.807, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "helmet": { + "zone": "D", + "gt_dims_mm": [ + 354, + 297, + 280 + ], + "k_round": 0.895, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "lunchbox": { + "zone": "B", + "gt_dims_mm": [ + 201, + 152, + 62 + ], + "k_round": 0.646, + "label_ru": "Подходит для сортировки" + }, + "mug": { + "zone": "D", + "gt_dims_mm": [ + 113, + 99, + 83 + ], + "k_round": 0.985, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "parcel_box": { + "zone": "B", + "gt_dims_mm": [ + 344, + 155, + 144 + ], + "k_round": 0.699, + "label_ru": "Подходит для сортировки" + }, + "pen": { + "zone": "D", + "gt_dims_mm": [ + 148, + 13, + 9 + ], + "k_round": 0.842, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "perfume": { + "zone": "D", + "gt_dims_mm": [ + 120, + 53, + 53 + ], + "k_round": 0.924, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "pillow": { + "zone": "C", + "gt_dims_mm": [ + 455, + 431, + 213 + ], + "k_round": 0.905, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "plate": { + "zone": "D", + "gt_dims_mm": [ + 209, + 209, + 27 + ], + "k_round": 0.998, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "pouf": { + "zone": "C", + "gt_dims_mm": [ + 489, + 489, + 264 + ], + "k_round": 0.994, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "sneaker": { + "zone": "B", + "gt_dims_mm": [ + 270, + 208, + 125 + ], + "k_round": 0.706, + "label_ru": "Подходит для сортировки" + }, + "tool_case": { + "zone": "B", + "gt_dims_mm": [ + 300, + 144, + 60 + ], + "k_round": 0.454, + "label_ru": "Подходит для сортировки" + }, + "watch": { + "zone": "C", + "gt_dims_mm": [ + 230, + 230, + 5 + ], + "k_round": 0.995, + "label_ru": "Не подходит для сортировки по габаритам" + } +} \ No newline at end of file diff --git a/assets/items/mug.usd b/assets/items/mug.usd new file mode 100644 index 0000000..699fcad Binary files /dev/null and b/assets/items/mug.usd differ diff --git a/assets/items/nailfile_mini.usd b/assets/items/nailfile_mini.usd new file mode 100644 index 0000000..90e1778 Binary files /dev/null and b/assets/items/nailfile_mini.usd differ diff --git a/assets/items/parcel_box.usd b/assets/items/parcel_box.usd new file mode 100644 index 0000000..4c60b97 Binary files /dev/null and b/assets/items/parcel_box.usd differ diff --git a/assets/items/pen.usd b/assets/items/pen.usd new file mode 100644 index 0000000..4196138 Binary files /dev/null and b/assets/items/pen.usd differ diff --git a/assets/items/perfume.usd b/assets/items/perfume.usd new file mode 100644 index 0000000..f3df47f Binary files /dev/null and b/assets/items/perfume.usd differ diff --git a/assets/items/pillow.usd b/assets/items/pillow.usd new file mode 100644 index 0000000..61fea87 Binary files /dev/null and b/assets/items/pillow.usd differ diff --git a/assets/items/plate.usd b/assets/items/plate.usd new file mode 100644 index 0000000..6e42972 Binary files /dev/null and b/assets/items/plate.usd differ diff --git a/assets/items/pouf.usd b/assets/items/pouf.usd new file mode 100644 index 0000000..9cdb655 Binary files /dev/null and b/assets/items/pouf.usd differ diff --git a/assets/items/printer_compact.usd b/assets/items/printer_compact.usd new file mode 100644 index 0000000..3bd3f8d Binary files /dev/null and b/assets/items/printer_compact.usd differ diff --git a/assets/items/printer_office.usd.heavy b/assets/items/printer_office.usd.heavy new file mode 100644 index 0000000..1f2c032 Binary files /dev/null and b/assets/items/printer_office.usd.heavy differ diff --git a/assets/items/safety_pin.usd b/assets/items/safety_pin.usd new file mode 100644 index 0000000..e80d10b Binary files /dev/null and b/assets/items/safety_pin.usd differ diff --git a/assets/items/sneaker.usd b/assets/items/sneaker.usd new file mode 100644 index 0000000..135d058 Binary files /dev/null and b/assets/items/sneaker.usd differ diff --git a/assets/items/suitcase_large.usd.heavy b/assets/items/suitcase_large.usd.heavy new file mode 100644 index 0000000..387338f Binary files /dev/null and b/assets/items/suitcase_large.usd.heavy differ diff --git a/assets/items/textures/air_conditioner_texture0.jpg b/assets/items/textures/air_conditioner_texture0.jpg new file mode 100644 index 0000000..e2f1607 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture0.jpg differ diff --git a/assets/items/textures/air_conditioner_texture1.png b/assets/items/textures/air_conditioner_texture1.png new file mode 100644 index 0000000..eb9d47d Binary files /dev/null and b/assets/items/textures/air_conditioner_texture1.png differ diff --git a/assets/items/textures/air_conditioner_texture10.png b/assets/items/textures/air_conditioner_texture10.png new file mode 100644 index 0000000..5d1e4ee Binary files /dev/null and b/assets/items/textures/air_conditioner_texture10.png differ diff --git a/assets/items/textures/air_conditioner_texture11.png b/assets/items/textures/air_conditioner_texture11.png new file mode 100644 index 0000000..9c33233 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture11.png differ diff --git a/assets/items/textures/air_conditioner_texture2.png b/assets/items/textures/air_conditioner_texture2.png new file mode 100644 index 0000000..a894e06 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture2.png differ diff --git a/assets/items/textures/air_conditioner_texture3.jpg b/assets/items/textures/air_conditioner_texture3.jpg new file mode 100644 index 0000000..2c47729 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture3.jpg differ diff --git a/assets/items/textures/air_conditioner_texture4.png b/assets/items/textures/air_conditioner_texture4.png new file mode 100644 index 0000000..c1969de Binary files /dev/null and b/assets/items/textures/air_conditioner_texture4.png differ diff --git a/assets/items/textures/air_conditioner_texture5.png b/assets/items/textures/air_conditioner_texture5.png new file mode 100644 index 0000000..ac3af00 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture5.png differ diff --git a/assets/items/textures/air_conditioner_texture6.jpg b/assets/items/textures/air_conditioner_texture6.jpg new file mode 100644 index 0000000..bd7ac45 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture6.jpg differ diff --git a/assets/items/textures/air_conditioner_texture7.png b/assets/items/textures/air_conditioner_texture7.png new file mode 100644 index 0000000..3527997 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture7.png differ diff --git a/assets/items/textures/air_conditioner_texture8.png b/assets/items/textures/air_conditioner_texture8.png new file mode 100644 index 0000000..415f12d Binary files /dev/null and b/assets/items/textures/air_conditioner_texture8.png differ diff --git a/assets/items/textures/air_conditioner_texture9.jpg b/assets/items/textures/air_conditioner_texture9.jpg new file mode 100644 index 0000000..16a1554 Binary files /dev/null and b/assets/items/textures/air_conditioner_texture9.jpg differ diff --git a/assets/items/textures/briefcase_hard_texture0.jpg b/assets/items/textures/briefcase_hard_texture0.jpg new file mode 100644 index 0000000..0edced2 Binary files /dev/null and b/assets/items/textures/briefcase_hard_texture0.jpg differ diff --git a/assets/items/textures/briefcase_hard_texture1.png b/assets/items/textures/briefcase_hard_texture1.png new file mode 100644 index 0000000..a109b68 Binary files /dev/null and b/assets/items/textures/briefcase_hard_texture1.png differ diff --git a/assets/items/textures/briefcase_hard_texture2.png b/assets/items/textures/briefcase_hard_texture2.png new file mode 100644 index 0000000..da0cffb Binary files /dev/null and b/assets/items/textures/briefcase_hard_texture2.png differ diff --git a/assets/items/textures/carton_large_texture0.jpg b/assets/items/textures/carton_large_texture0.jpg new file mode 100644 index 0000000..7c5da04 Binary files /dev/null and b/assets/items/textures/carton_large_texture0.jpg differ diff --git a/assets/items/textures/carton_large_texture1.png b/assets/items/textures/carton_large_texture1.png new file mode 100644 index 0000000..f413e4d Binary files /dev/null and b/assets/items/textures/carton_large_texture1.png differ diff --git a/assets/items/textures/carton_large_texture2.png b/assets/items/textures/carton_large_texture2.png new file mode 100644 index 0000000..d1f87c1 Binary files /dev/null and b/assets/items/textures/carton_large_texture2.png differ diff --git a/assets/items/textures/cleat_small_texture0.jpg b/assets/items/textures/cleat_small_texture0.jpg new file mode 100644 index 0000000..ec5255a Binary files /dev/null and b/assets/items/textures/cleat_small_texture0.jpg differ diff --git a/assets/items/textures/clothespin_flat_texture0.jpg b/assets/items/textures/clothespin_flat_texture0.jpg new file mode 100644 index 0000000..c23ec64 Binary files /dev/null and b/assets/items/textures/clothespin_flat_texture0.jpg differ diff --git a/assets/items/textures/cooler_box_texture0.jpg b/assets/items/textures/cooler_box_texture0.jpg new file mode 100644 index 0000000..f763b9e Binary files /dev/null and b/assets/items/textures/cooler_box_texture0.jpg differ diff --git a/assets/items/textures/cooler_cube_texture0.jpg b/assets/items/textures/cooler_cube_texture0.jpg new file mode 100644 index 0000000..e799116 Binary files /dev/null and b/assets/items/textures/cooler_cube_texture0.jpg differ diff --git a/assets/items/textures/cooler_cube_texture1.png b/assets/items/textures/cooler_cube_texture1.png new file mode 100644 index 0000000..d7527be Binary files /dev/null and b/assets/items/textures/cooler_cube_texture1.png differ diff --git a/assets/items/textures/cooler_cube_texture2.png b/assets/items/textures/cooler_cube_texture2.png new file mode 100644 index 0000000..93a7cd0 Binary files /dev/null and b/assets/items/textures/cooler_cube_texture2.png differ diff --git a/assets/items/textures/duffel_bag_texture0.jpg b/assets/items/textures/duffel_bag_texture0.jpg new file mode 100644 index 0000000..92b34d2 Binary files /dev/null and b/assets/items/textures/duffel_bag_texture0.jpg differ diff --git a/assets/items/textures/duffel_bag_texture1.jpg b/assets/items/textures/duffel_bag_texture1.jpg new file mode 100644 index 0000000..ef54f2f Binary files /dev/null and b/assets/items/textures/duffel_bag_texture1.jpg differ diff --git a/assets/items/textures/duffel_bag_texture2.png b/assets/items/textures/duffel_bag_texture2.png new file mode 100644 index 0000000..a9f19d3 Binary files /dev/null and b/assets/items/textures/duffel_bag_texture2.png differ diff --git a/assets/items/textures/duffel_bag_texture3.png b/assets/items/textures/duffel_bag_texture3.png new file mode 100644 index 0000000..e74a185 Binary files /dev/null and b/assets/items/textures/duffel_bag_texture3.png differ diff --git a/assets/items/textures/duffel_bag_texture4.png b/assets/items/textures/duffel_bag_texture4.png new file mode 100644 index 0000000..f175e3b Binary files /dev/null and b/assets/items/textures/duffel_bag_texture4.png differ diff --git a/assets/items/textures/duffel_bag_texture5.jpg b/assets/items/textures/duffel_bag_texture5.jpg new file mode 100644 index 0000000..5674858 Binary files /dev/null and b/assets/items/textures/duffel_bag_texture5.jpg differ diff --git a/assets/items/textures/duffel_bag_texture6.jpg b/assets/items/textures/duffel_bag_texture6.jpg new file mode 100644 index 0000000..5674858 Binary files /dev/null and b/assets/items/textures/duffel_bag_texture6.jpg differ diff --git a/assets/items/textures/duffel_round_texture0.jpg b/assets/items/textures/duffel_round_texture0.jpg new file mode 100644 index 0000000..354be4e Binary files /dev/null and b/assets/items/textures/duffel_round_texture0.jpg differ diff --git a/assets/items/textures/nailfile_mini_texture0.png b/assets/items/textures/nailfile_mini_texture0.png new file mode 100644 index 0000000..f75f05c Binary files /dev/null and b/assets/items/textures/nailfile_mini_texture0.png differ diff --git a/assets/items/textures/nailfile_mini_texture1.png b/assets/items/textures/nailfile_mini_texture1.png new file mode 100644 index 0000000..90f0a0e Binary files /dev/null and b/assets/items/textures/nailfile_mini_texture1.png differ diff --git a/assets/items/textures/printer_compact_texture0.png b/assets/items/textures/printer_compact_texture0.png new file mode 100644 index 0000000..3b9f75c Binary files /dev/null and b/assets/items/textures/printer_compact_texture0.png differ diff --git a/assets/items/textures/printer_compact_texture1.jpg b/assets/items/textures/printer_compact_texture1.jpg new file mode 100644 index 0000000..6634f0a Binary files /dev/null and b/assets/items/textures/printer_compact_texture1.jpg differ diff --git a/assets/items/textures/printer_office_texture0.jpg b/assets/items/textures/printer_office_texture0.jpg new file mode 100644 index 0000000..0830500 Binary files /dev/null and b/assets/items/textures/printer_office_texture0.jpg differ diff --git a/assets/items/textures/printer_office_texture1.jpg b/assets/items/textures/printer_office_texture1.jpg new file mode 100644 index 0000000..31ad44c Binary files /dev/null and b/assets/items/textures/printer_office_texture1.jpg differ diff --git a/assets/items/textures/printer_office_texture2.jpg b/assets/items/textures/printer_office_texture2.jpg new file mode 100644 index 0000000..dc1e441 Binary files /dev/null and b/assets/items/textures/printer_office_texture2.jpg differ diff --git a/assets/items/textures/printer_office_texture3.jpg b/assets/items/textures/printer_office_texture3.jpg new file mode 100644 index 0000000..7be7943 Binary files /dev/null and b/assets/items/textures/printer_office_texture3.jpg differ diff --git a/assets/items/textures/printer_office_texture4.jpg b/assets/items/textures/printer_office_texture4.jpg new file mode 100644 index 0000000..67ac7f7 Binary files /dev/null and b/assets/items/textures/printer_office_texture4.jpg differ diff --git a/assets/items/textures/safety_pin_texture0.png b/assets/items/textures/safety_pin_texture0.png new file mode 100644 index 0000000..7a25f38 Binary files /dev/null and b/assets/items/textures/safety_pin_texture0.png differ diff --git a/assets/items/textures/safety_pin_texture1.png b/assets/items/textures/safety_pin_texture1.png new file mode 100644 index 0000000..9b19e5b Binary files /dev/null and b/assets/items/textures/safety_pin_texture1.png differ diff --git a/assets/items/textures/safety_pin_texture2.png b/assets/items/textures/safety_pin_texture2.png new file mode 100644 index 0000000..cb55497 Binary files /dev/null and b/assets/items/textures/safety_pin_texture2.png differ diff --git a/assets/items/textures/suitcase_large_texture0.jpg b/assets/items/textures/suitcase_large_texture0.jpg new file mode 100644 index 0000000..2e7f8ab Binary files /dev/null and b/assets/items/textures/suitcase_large_texture0.jpg differ diff --git a/assets/items/textures/toaster_compact_texture0.png b/assets/items/textures/toaster_compact_texture0.png new file mode 100644 index 0000000..15e9a78 Binary files /dev/null and b/assets/items/textures/toaster_compact_texture0.png differ diff --git a/assets/items/textures/toaster_compact_texture1.png b/assets/items/textures/toaster_compact_texture1.png new file mode 100644 index 0000000..798b59b Binary files /dev/null and b/assets/items/textures/toaster_compact_texture1.png differ diff --git a/assets/items/textures/toaster_oven_texture0.png b/assets/items/textures/toaster_oven_texture0.png new file mode 100644 index 0000000..93e5dcd Binary files /dev/null and b/assets/items/textures/toaster_oven_texture0.png differ diff --git a/assets/items/textures/toaster_oven_texture1.png b/assets/items/textures/toaster_oven_texture1.png new file mode 100644 index 0000000..bdfbe4a Binary files /dev/null and b/assets/items/textures/toaster_oven_texture1.png differ diff --git a/assets/items/textures/toaster_oven_texture2.png b/assets/items/textures/toaster_oven_texture2.png new file mode 100644 index 0000000..5f9ec24 Binary files /dev/null and b/assets/items/textures/toaster_oven_texture2.png differ diff --git a/assets/items/textures/toaster_oven_texture3.png b/assets/items/textures/toaster_oven_texture3.png new file mode 100644 index 0000000..f949a1f Binary files /dev/null and b/assets/items/textures/toaster_oven_texture3.png differ diff --git a/assets/items/textures/toaster_oven_texture4.png b/assets/items/textures/toaster_oven_texture4.png new file mode 100644 index 0000000..30f9cce Binary files /dev/null and b/assets/items/textures/toaster_oven_texture4.png differ diff --git a/assets/items/textures/toaster_oven_texture5.png b/assets/items/textures/toaster_oven_texture5.png new file mode 100644 index 0000000..2234d80 Binary files /dev/null and b/assets/items/textures/toaster_oven_texture5.png differ diff --git a/assets/items/toaster_compact.usd b/assets/items/toaster_compact.usd new file mode 100644 index 0000000..b4aad83 Binary files /dev/null and b/assets/items/toaster_compact.usd differ diff --git a/assets/items/toaster_oven.usd b/assets/items/toaster_oven.usd new file mode 100644 index 0000000..f56a548 Binary files /dev/null and b/assets/items/toaster_oven.usd differ diff --git a/assets/items/tool_case.usd b/assets/items/tool_case.usd new file mode 100644 index 0000000..1c3abc3 Binary files /dev/null and b/assets/items/tool_case.usd differ diff --git a/assets/items/watch.usd b/assets/items/watch.usd new file mode 100644 index 0000000..7fc881c Binary files /dev/null and b/assets/items/watch.usd differ diff --git a/assets/meshes/backpack.usd b/assets/meshes/backpack.usd new file mode 100644 index 0000000..1daebae Binary files /dev/null and b/assets/meshes/backpack.usd differ diff --git a/assets/meshes/barrel.usd b/assets/meshes/barrel.usd new file mode 100644 index 0000000..6bfbe12 Binary files /dev/null and b/assets/meshes/barrel.usd differ diff --git a/assets/meshes/bolts_cluster.usd b/assets/meshes/bolts_cluster.usd new file mode 100644 index 0000000..23762c7 Binary files /dev/null and b/assets/meshes/bolts_cluster.usd differ diff --git a/assets/meshes/box_300x200x200.usd b/assets/meshes/box_300x200x200.usd new file mode 100644 index 0000000..2e535c6 Binary files /dev/null and b/assets/meshes/box_300x200x200.usd differ diff --git a/assets/meshes/bucket.usd b/assets/meshes/bucket.usd new file mode 100644 index 0000000..0fc59f0 Binary files /dev/null and b/assets/meshes/bucket.usd differ diff --git a/assets/meshes/manifest.json b/assets/meshes/manifest.json new file mode 100644 index 0000000..bea707a --- /dev/null +++ b/assets/meshes/manifest.json @@ -0,0 +1,92 @@ +{ + "box_300x200x200": { + "zone": "B", + "scene_dims_m": [ + 0.1584, + 0.1153, + 0.0983 + ], + "gt_dims_mm": [ + 301, + 200, + 200 + ], + "k_round": 0.72, + "label_ru": "\u041f\u043e\u0434\u0445\u043e\u0434\u0438\u0442 \u0434\u043b\u044f \u0441\u043e\u0440\u0442\u0438\u0440\u043e\u0432\u043a\u0438" + }, + "bolts_cluster": { + "zone": "B", + "scene_dims_m": [ + 0.1207, + 0.1191, + 0.0263 + ], + "gt_dims_mm": [ + 194, + 136, + 53 + ], + "k_round": 0.718, + "label_ru": "\u041f\u043e\u0434\u0445\u043e\u0434\u0438\u0442 \u0434\u043b\u044f \u0441\u043e\u0440\u0442\u0438\u0440\u043e\u0432\u043a\u0438" + }, + "barrel": { + "zone": "C", + "scene_dims_m": [ + 0.3238, + 0.3238, + 0.4314 + ], + "gt_dims_mm": [ + 880, + 542, + 542 + ], + "k_round": 0.995, + "label_ru": "\u041d\u0435 \u043f\u043e\u0434\u0445\u043e\u0434\u0438\u0442 \u0434\u043b\u044f \u0441\u043e\u0440\u0442\u0438\u0440\u043e\u0432\u043a\u0438 \u043f\u043e \u0433\u0430\u0431\u0430\u0440\u0438\u0442\u0430\u043c" + }, + "backpack": { + "zone": "C", + "scene_dims_m": [ + 0.1994, + 0.2502, + 0.2105 + ], + "gt_dims_mm": [ + 455, + 370, + 301 + ], + "k_round": 0.82, + "label_ru": "\u041d\u0435 \u043f\u043e\u0434\u0445\u043e\u0434\u0438\u0442 \u0434\u043b\u044f \u0441\u043e\u0440\u0442\u0438\u0440\u043e\u0432\u043a\u0438 \u043f\u043e \u0433\u0430\u0431\u0430\u0440\u0438\u0442\u0430\u043c" + }, + "bucket": { + "zone": "D", + "scene_dims_m": [ + 0.1903, + 0.2005, + 0.1582 + ], + "gt_dims_mm": [ + 287, + 287, + 272 + ], + "k_round": 0.995, + "label_ru": "\u041d\u0435 \u043f\u043e\u0434\u0445\u043e\u0434\u0438\u0442 \u0434\u043b\u044f \u0441\u043e\u0440\u0442\u0438\u0440\u043e\u0432\u043a\u0438 \u0431\u0435\u0437 \u0434\u043e\u0443\u043f\u0430\u043a\u043e\u0432\u043a\u0438" + }, + "mug": { + "zone": "D", + "scene_dims_m": [ + 0.0778, + 0.0594, + 0.0663 + ], + "gt_dims_mm": [ + 113, + 99, + 83 + ], + "k_round": 0.985, + "label_ru": "\u041d\u0435 \u043f\u043e\u0434\u0445\u043e\u0434\u0438\u0442 \u0434\u043b\u044f \u0441\u043e\u0440\u0442\u0438\u0440\u043e\u0432\u043a\u0438 \u0431\u0435\u0437 \u0434\u043e\u0443\u043f\u0430\u043a\u043e\u0432\u043a\u0438" + } +} \ No newline at end of file diff --git a/assets/meshes/mug.usd b/assets/meshes/mug.usd new file mode 100644 index 0000000..4334c5a Binary files /dev/null and b/assets/meshes/mug.usd differ diff --git a/assets/meshes/plow_arm.usd b/assets/meshes/plow_arm.usd new file mode 100644 index 0000000..03088f6 Binary files /dev/null and b/assets/meshes/plow_arm.usd differ diff --git a/assets/meshes/plow_base.usd b/assets/meshes/plow_base.usd new file mode 100644 index 0000000..fd319b2 Binary files /dev/null and b/assets/meshes/plow_base.usd differ diff --git a/assets/objaverse_c/air_conditioner.glb b/assets/objaverse_c/air_conditioner.glb new file mode 100644 index 0000000..3583e3f Binary files /dev/null and b/assets/objaverse_c/air_conditioner.glb differ diff --git a/assets/objaverse_c/briefcase_hard.glb b/assets/objaverse_c/briefcase_hard.glb new file mode 100644 index 0000000..c846dc3 Binary files /dev/null and b/assets/objaverse_c/briefcase_hard.glb differ diff --git a/assets/objaverse_c/carton_large.glb b/assets/objaverse_c/carton_large.glb new file mode 100644 index 0000000..4ef4618 Binary files /dev/null and b/assets/objaverse_c/carton_large.glb differ diff --git a/assets/objaverse_c/cooler_box.glb b/assets/objaverse_c/cooler_box.glb new file mode 100644 index 0000000..aa04aee Binary files /dev/null and b/assets/objaverse_c/cooler_box.glb differ diff --git a/assets/objaverse_c/cooler_cube.glb b/assets/objaverse_c/cooler_cube.glb new file mode 100644 index 0000000..05c3b30 Binary files /dev/null and b/assets/objaverse_c/cooler_cube.glb differ diff --git a/assets/objaverse_c/duffel_bag.glb b/assets/objaverse_c/duffel_bag.glb new file mode 100644 index 0000000..6b8d022 Binary files /dev/null and b/assets/objaverse_c/duffel_bag.glb differ diff --git a/assets/objaverse_c/duffel_round.glb b/assets/objaverse_c/duffel_round.glb new file mode 100644 index 0000000..f1567bd Binary files /dev/null and b/assets/objaverse_c/duffel_round.glb differ diff --git a/assets/objaverse_c/manifest_c.json b/assets/objaverse_c/manifest_c.json new file mode 100644 index 0000000..addb9c6 --- /dev/null +++ b/assets/objaverse_c/manifest_c.json @@ -0,0 +1,146 @@ +{ + "suitcase_large": { + "uid": "e7a2631478084e1485189e3b6659c132", + "category": "suitcase", + "source": "objaverse-xl", + "gt_dims_mm": [ + 495, + 486, + 370 + ], + "zone": "C", + "glb": "suitcase_large.glb" + }, + "printer_office": { + "uid": "102023952b9e4eaa8de936f6e72ea21c", + "category": "printer", + "source": "objaverse-xl", + "gt_dims_mm": [ + 478, + 444, + 167 + ], + "zone": "C", + "glb": "printer_office.glb" + }, + "toaster_oven": { + "uid": "0a36fc7335524892884a73c3ca1c37d6", + "category": "toaster_oven", + "source": "objaverse-xl", + "gt_dims_mm": [ + 466, + 466, + 411 + ], + "zone": "C", + "glb": "toaster_oven.glb" + }, + "toaster_compact": { + "uid": "29f60fa077de45df92f84711cebef978", + "category": "toaster_oven", + "source": "objaverse-xl", + "gt_dims_mm": [ + 470, + 406, + 351 + ], + "zone": "C", + "glb": "toaster_compact.glb" + }, + "air_conditioner": { + "uid": "e8d19ae2e41645dc9eae55c9f948b34b", + "category": "air_conditioner", + "source": "objaverse-xl", + "gt_dims_mm": [ + 492, + 380, + 203 + ], + "zone": "C", + "glb": "air_conditioner.glb" + }, + "briefcase_hard": { + "uid": "cb5c52c48f6c4540b2eaae6852267cc5", + "category": "briefcase", + "source": "objaverse-xl", + "gt_dims_mm": [ + 472, + 391, + 174 + ], + "zone": "C", + "glb": "briefcase_hard.glb" + }, + "duffel_bag": { + "uid": "0c4ab7620efe4d3fb3d298661673f39a", + "category": "duffel_bag", + "source": "objaverse-xl", + "gt_dims_mm": [ + 498, + 437, + 200 + ], + "zone": "C", + "glb": "duffel_bag.glb" + }, + "duffel_round": { + "uid": "14d4adabb2564307ae69838eb4cac51a", + "category": "duffel_bag", + "source": "objaverse-xl", + "gt_dims_mm": [ + 461, + 419, + 419 + ], + "zone": "C", + "glb": "duffel_round.glb" + }, + "cooler_box": { + "uid": "ae52f5f6241b4759b9cbe48226a9db48", + "category": "cooler_(for_food)", + "source": "objaverse-xl", + "gt_dims_mm": [ + 488, + 409, + 322 + ], + "zone": "C", + "glb": "cooler_box.glb" + }, + "cooler_cube": { + "uid": "9d9557c5f3af4639bcacf468e4e5182a", + "category": "cooler_(for_food)", + "source": "objaverse-xl", + "gt_dims_mm": [ + 474, + 437, + 436 + ], + "zone": "C", + "glb": "cooler_cube.glb" + }, + "printer_compact": { + "uid": "42a0335c77c34ff3bf08618a168f73ec", + "category": "printer", + "source": "objaverse-xl", + "gt_dims_mm": [ + 480, + 353, + 262 + ], + "zone": "C", + "glb": "printer_compact.glb" + }, + "carton_large": { + "uid": "78440f26dbb64b9ea0edf1a218ab8090", + "category": "carton", + "source": "objaverse-xl", + "gt_dims_mm": [ + 496, + 338, + 161 + ], + "zone": "C", + "glb": "carton_large.glb" + } +} \ No newline at end of file diff --git a/assets/objaverse_c/printer_compact.glb b/assets/objaverse_c/printer_compact.glb new file mode 100644 index 0000000..2e50d10 Binary files /dev/null and b/assets/objaverse_c/printer_compact.glb differ diff --git a/assets/objaverse_c/printer_office.glb b/assets/objaverse_c/printer_office.glb new file mode 100644 index 0000000..bdffc01 Binary files /dev/null and b/assets/objaverse_c/printer_office.glb differ diff --git a/assets/objaverse_c/suitcase_large.glb b/assets/objaverse_c/suitcase_large.glb new file mode 100644 index 0000000..74c9aaa Binary files /dev/null and b/assets/objaverse_c/suitcase_large.glb differ diff --git a/assets/objaverse_c/toaster_compact.glb b/assets/objaverse_c/toaster_compact.glb new file mode 100644 index 0000000..afbf6d7 Binary files /dev/null and b/assets/objaverse_c/toaster_compact.glb differ diff --git a/assets/objaverse_c/toaster_oven.glb b/assets/objaverse_c/toaster_oven.glb new file mode 100644 index 0000000..00a19fe Binary files /dev/null and b/assets/objaverse_c/toaster_oven.glb differ diff --git a/assets/objaverse_tiny/cleat_small.glb b/assets/objaverse_tiny/cleat_small.glb new file mode 100644 index 0000000..b1731eb Binary files /dev/null and b/assets/objaverse_tiny/cleat_small.glb differ diff --git a/assets/objaverse_tiny/clothespin_flat.glb b/assets/objaverse_tiny/clothespin_flat.glb new file mode 100644 index 0000000..4eb6b25 Binary files /dev/null and b/assets/objaverse_tiny/clothespin_flat.glb differ diff --git a/assets/objaverse_tiny/manifest_tiny.json b/assets/objaverse_tiny/manifest_tiny.json new file mode 100644 index 0000000..7944d21 --- /dev/null +++ b/assets/objaverse_tiny/manifest_tiny.json @@ -0,0 +1,54 @@ +{ + "safety_pin": { + "uid": "fefe61636e7e4670b83f38943cdac85d", + "category": "safety_pin", + "source": "objaverse-xl", + "gt_dims_mm": [ + 9.5, + 6.22, + 3.31 + ], + "zone": "C", + "glb": "safety_pin.glb", + "note": "меньше 10x10x10 мм: класс C по MIN_DIM" + }, + "nailfile_mini": { + "uid": "d3c99262d8654599b30f5ad6ae1815c7", + "category": "nailfile", + "source": "objaverse-xl", + "gt_dims_mm": [ + 6.5, + 4.24, + 3.15 + ], + "zone": "C", + "glb": "nailfile_mini.glb", + "note": "меньше 10x10x10 мм: класс C по MIN_DIM" + }, + "clothespin_flat": { + "uid": "63c774fbf93341e4804c124c1ef90f0b", + "category": "clothespin", + "source": "objaverse-xl", + "gt_dims_mm": [ + 7.8, + 5.51, + 1.46 + ], + "zone": "C", + "glb": "clothespin_flat.glb", + "note": "меньше 10x10x10 мм: класс C по MIN_DIM" + }, + "cleat_small": { + "uid": "759e480d707944ee87b1ce61875a320e", + "category": "cleat_(for_securing_rope)", + "source": "objaverse-xl", + "gt_dims_mm": [ + 6.8, + 2.85, + 2.58 + ], + "zone": "C", + "glb": "cleat_small.glb", + "note": "меньше 10x10x10 мм: класс C по MIN_DIM" + } +} \ No newline at end of file diff --git a/assets/objaverse_tiny/nailfile_mini.glb b/assets/objaverse_tiny/nailfile_mini.glb new file mode 100644 index 0000000..9f59da9 Binary files /dev/null and b/assets/objaverse_tiny/nailfile_mini.glb differ diff --git a/assets/objaverse_tiny/safety_pin.glb b/assets/objaverse_tiny/safety_pin.glb new file mode 100644 index 0000000..4546b0b Binary files /dev/null and b/assets/objaverse_tiny/safety_pin.glb differ diff --git a/assets/plow/plow_arm.usd b/assets/plow/plow_arm.usd new file mode 100644 index 0000000..74a0122 Binary files /dev/null and b/assets/plow/plow_arm.usd differ diff --git a/assets/plow/plow_base.usd b/assets/plow/plow_base.usd new file mode 100644 index 0000000..32111f2 Binary files /dev/null and b/assets/plow/plow_base.usd differ diff --git a/config/calib.json b/config/calib.json new file mode 100644 index 0000000..3a6ee3a --- /dev/null +++ b/config/calib.json @@ -0,0 +1,206 @@ +{ + "center": [ + -0.75, + 0.0, + 1.781 + ], + "belt_top": 1.781, + "rectified": true, + "cameras": { + "RealSense_D435": { + "baseline_m": 0.07352941176470584, + "intrinsics": { + "fx": 674.4192801798159, + "fy": 674.4192801798159, + "cx": 640.0, + "cy": 360.0, + "width": 1280, + "height": 720 + }, + "left_path": "/RigRS/RealSense_D435_Left", + "right_path": "/RigRS/RealSense_D435_Right", + "left_world": [ + [ + -0.999999612839731, + 0.0008799547647936078, + 0.0, + 0.0 + ], + [ + -0.0005857536015939647, + -0.6656630525216037, + 0.7462522076351525, + 0.0 + ], + [ + 0.0006566681858463012, + 0.7462519187159471, + 0.6656633102399899, + 0.0 + ], + [ + -0.7128235436455979, + 0.4679059598012155, + 2.1984051112352905, + 1.0 + ] + ], + "right_world": [ + [ + -0.999999612839731, + 0.0008799547647936078, + 0.0, + 0.0 + ], + [ + -0.0005857536015939647, + -0.6656630525216037, + 0.7462522076351525, + 0.0 + ], + [ + 0.0006566681858463012, + 0.7462519187159471, + 0.6656633102399899, + 0.0 + ], + [ + -0.7863529269426368, + 0.46797066235745033, + 2.1984051112352905, + 1.0 + ] + ] + }, + "Orbbec_Gemini305": { + "baseline_m": 0.026470588235294006, + "intrinsics": { + "fx": 662.7394008259646, + "fy": 662.7394008259646, + "cx": 640.0, + "cy": 400.0, + "width": 1280, + "height": 800 + }, + "left_path": "/RigRS/Orbbec_Gemini305_Left", + "right_path": "/RigRS/Orbbec_Gemini305_Right", + "left_world": [ + [ + 0.5003812738237227, + -0.8658051633055491, + 0.0, + 0.0 + ], + [ + 0.5765794758052561, + 0.33322690230045576, + 0.7459999595599041, + 0.0 + ], + [ + -0.6458906168126959, + -0.37328441003703045, + 0.665945989053633, + 0.0 + ], + [ + -1.1614573934228245, + -0.22250996955474017, + 2.1984051112352905, + 1.0 + ] + ], + "right_world": [ + [ + 0.5003812738237227, + -0.8658051633055491, + 0.0, + 0.0 + ], + [ + 0.5765794758052561, + 0.33322690230045576, + 0.7459999595599041, + 0.0 + ], + [ + -0.6458906168126959, + -0.37328441003703045, + 0.665945989053633, + 0.0 + ], + [ + -1.148212006762785, + -0.24542834152459286, + 2.1984051112352905, + 1.0 + ] + ] + }, + "Orbbec_Gemini345": { + "baseline_m": 0.12941176470588234, + "intrinsics": { + "fx": 628.9262483940416, + "fy": 628.9262483940416, + "cx": 640.0, + "cy": 400.0, + "width": 1280, + "height": 800 + }, + "left_path": "/RigRS/Orbbec_Gemini345_Left", + "right_path": "/RigRS/Orbbec_Gemini345_Right", + "left_world": [ + [ + 0.49961921012690946, + 0.866245141326728, + -0.0, + 0.0 + ], + [ + -0.5763830506655224, + 0.33243712520337265, + 0.7465039428509558, + 0.0 + ], + [ + 0.6466554134758858, + -0.37296771028381803, + 0.6653809910930556, + 0.0 + ], + [ + -0.3766700723271712, + -0.29002031174316084, + 2.1984051112352905, + 1.0 + ] + ], + "right_world": [ + [ + 0.49961921012690946, + 0.866245141326728, + -0.0, + 0.0 + ], + [ + -0.5763830506655224, + 0.33243712520337265, + 0.7465039428509558, + 0.0 + ], + [ + 0.6466554134758858, + -0.37296771028381803, + 0.6653809910930556, + 0.0 + ], + [ + -0.31201346866368884, + -0.17791799933617253, + 2.1984051112352905, + 1.0 + ] + ] + } + } +} \ No newline at end of file diff --git a/config/categories.json b/config/categories.json new file mode 100644 index 0000000..4b64cf9 --- /dev/null +++ b/config/categories.json @@ -0,0 +1,374 @@ +{ + "backpack": { + "name": "backpack", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.4547, + 0.3703, + 0.3009 + ], + "k_round": 0.82, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "bag": { + "name": "bag", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.2017, + 0.1753, + 0.1703 + ], + "k_round": 0.896, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "banana": { + "name": "banana", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.1826, + 0.0706, + 0.033 + ], + "k_round": 0.94, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "barrel": { + "name": "barrel", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.88, + 0.5418, + 0.5418 + ], + "k_round": 0.995, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "bolts_cluster": { + "name": "bolts_cluster", + "category": "sortable", + "zone": "B", + "obb_extents_m": [ + 0.1936, + 0.1356, + 0.0526 + ], + "k_round": 0.718, + "label_ru": "Подходит для сортировки" + }, + "bottle": { + "name": "bottle", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.3048, + 0.091, + 0.091 + ], + "k_round": 0.995, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "box_300x200x200": { + "name": "box_300x200x200", + "category": "sortable", + "zone": "B", + "obb_extents_m": [ + 0.301, + 0.2005, + 0.2 + ], + "k_round": 0.72, + "label_ru": "Подходит для сортировки" + }, + "box_400x400x300": { + "name": "box_400x400x300", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.401, + 0.4, + 0.3005 + ], + "k_round": 0.716, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "broom": { + "name": "broom", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 1.1996, + 0.3898, + 0.1033 + ], + "k_round": 0.966, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "bucket": { + "name": "bucket", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.2874, + 0.2874, + 0.2723 + ], + "k_round": 0.995, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "chip_bag": { + "name": "chip_bag", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.25, + 0.1622, + 0.0691 + ], + "k_round": 0.811, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "cone": { + "name": "cone", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.5, + 0.3505, + 0.3505 + ], + "k_round": 0.991, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "cylinder": { + "name": "cylinder", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.4349, + 0.05, + 0.043 + ], + "k_round": 0.867, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "detergent": { + "name": "detergent", + "category": "sortable", + "zone": "B", + "obb_extents_m": [ + 0.2782, + 0.2599, + 0.1798 + ], + "k_round": 0.742, + "label_ru": "Подходит для сортировки" + }, + "headphones": { + "name": "headphones", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.1984, + 0.1949, + 0.0933 + ], + "k_round": 0.807, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "helmet": { + "name": "helmet", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.3535, + 0.2971, + 0.2799 + ], + "k_round": 0.895, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "lunchbox": { + "name": "lunchbox", + "category": "sortable", + "zone": "B", + "obb_extents_m": [ + 0.2011, + 0.1524, + 0.0623 + ], + "k_round": 0.646, + "label_ru": "Подходит для сортировки" + }, + "mug": { + "name": "mug", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.1129, + 0.099, + 0.0832 + ], + "k_round": 0.985, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "office_chair": { + "name": "office_chair", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 1.0953, + 0.8599, + 0.8254 + ], + "k_round": 0.996, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "pallet": { + "name": "pallet", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 1.2, + 1.196, + 0.1555 + ], + "k_round": 0.711, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "parcel_box": { + "name": "parcel_box", + "category": "sortable", + "zone": "B", + "obb_extents_m": [ + 0.3444, + 0.1551, + 0.1437 + ], + "k_round": 0.699, + "label_ru": "Подходит для сортировки" + }, + "pen": { + "name": "pen", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.1485, + 0.0131, + 0.009 + ], + "k_round": 0.842, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "perfume": { + "name": "perfume", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.12, + 0.0531, + 0.0531 + ], + "k_round": 0.924, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "pillow": { + "name": "pillow", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.4551, + 0.4306, + 0.2127 + ], + "k_round": 0.905, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "plate": { + "name": "plate", + "category": "round", + "zone": "D", + "obb_extents_m": [ + 0.2094, + 0.2094, + 0.0266 + ], + "k_round": 0.998, + "label_ru": "Не подходит для сортировки без доупаковки" + }, + "pouf": { + "name": "pouf", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.4889, + 0.4889, + 0.264 + ], + "k_round": 0.994, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "sneaker": { + "name": "sneaker", + "category": "sortable", + "zone": "B", + "obb_extents_m": [ + 0.2704, + 0.208, + 0.1254 + ], + "k_round": 0.706, + "label_ru": "Подходит для сортировки" + }, + "tire": { + "name": "tire", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.6459, + 0.6459, + 0.2112 + ], + "k_round": 0.994, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "tool_case": { + "name": "tool_case", + "category": "sortable", + "zone": "B", + "obb_extents_m": [ + 0.3, + 0.1435, + 0.06 + ], + "k_round": 0.454, + "label_ru": "Подходит для сортировки" + }, + "umbrella": { + "name": "umbrella", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.8944, + 0.734, + 0.7249 + ], + "k_round": 0.628, + "label_ru": "Не подходит для сортировки по габаритам" + }, + "watch": { + "name": "watch", + "category": "oversize", + "zone": "C", + "obb_extents_m": [ + 0.23, + 0.23, + 0.0046 + ], + "k_round": 0.995, + "label_ru": "Не подходит для сортировки по габаритам" + } +} \ No newline at end of file diff --git a/control_test/.memory.md b/control_test/.memory.md new file mode 100644 index 0000000..6ab08ab --- /dev/null +++ b/control_test/.memory.md @@ -0,0 +1,283 @@ +# .memory — состояние пайплайна control_test + +Живая справка по замкнутому контуру «поток → CV → механика». Обновлять при изменении +конфигурации или при появлении нового замеренного факта. Здесь только то, что **измерено**; +предположения помечены отдельно. + +Последнее обновление: 2026-08-01. + +--- + +## 1. Что сейчас работает + +**Полный прогон с кинематикой и CV — `run_sorting_cv.py` в паре с `cv_worker.py`.** + +Класс товара приходит от стереопайплайна **во время движения**, а не из разметки. Пушер и +плуг реагируют физически на предсказанный класс. Разметка используется только для подсчёта +ошибки в конце. + +Прежний `run_pipeline.py` (классы из `labels.json`, без CV) остаётся рабочим и нужен как +контроль: он разделяет ошибки механики и ошибки распознавания. + +### Почему два процесса + +torch внутри Isaac роняет процесс. Поэтому CV живёт отдельно, обмен через каталог: + +``` +run_sorting_cv.py (в Isaac, без torch) cv_worker.py (отдельный процесс, torch+GPU) + поток 700 мм при 1 м/с + ворота x = -0.750 -> 6 кадров ──заявка──> runtime/req/<товар>.json + DEFOM vitl / вход 480 / iters 24 + облако -> габариты -> k -> класс + класс <──ответ── runtime/res/<товар>.json + пушер: класс D -> Cell.stroke() + плуг: класс B/C -> Plow.target(-16 / +16) +``` + +### Запуск + +```bash +# 1. работник CV (прогрев ~90 с: грузится vitl-энкодер) +cd /home/dasha/robozon-sorter/control_test +nohup /home/whatevenif/isaacsim/python.sh cv_worker.py > /tmp/cvworker.log 2>&1 & + +# 2. УБЕДИТЬСЯ ПО PID, а не по файлу runtime/worker_ready +pgrep -af "python.*cv_worker.py" + +# 3. открыть сцену заново (cell.prepare меняет её состояние), затем прогон +cd /home/dasha/robozon-sorter +python3 isaacsim_send.py --context cvsort --timeout 2580 --execution-timeout 2560 \ + --file control_test/run_sorting_cv.py +``` + +--- + +## 2. Зафиксированный бейзлайн CV + +В `measure_plane.py` значения по умолчанию: + +| параметр | значение | +|---|---| +| стереодвижок | **DEFOM-Stereo vitl** (`STEREO=defom`) | +| вход сети | **480** px по ширине (`SW=480`) | +| итерации | **24** + scale_iters 8 | +| окно | **кроп зоны осмотра** (`CROP=1`) | +| сегментация | **не используется** | + +Товар отделяется от полотна превышением над плоскостью (порог 20 мм) плюс отсев по +плотности; сегментация не участвует вовсе. + +**Замер на статичных кадрах потока 700 мм (9 товаров):** классы 8/9 = 89 %, габариты MAE +медиана 32.8 мм, 469 мс на товар при такте 700 мс. + +Веса: `/home/dasha/isaac_assets/cv/defom-stereo/checkpoints/` — +`defomstereo_vitl_sceneflow.pth` (1.53 ГБ), `defomstereo_vits_sceneflow.pth` (173 МБ), +энкодер `depth_anything_v2_vitl.pth` (1.34 ГБ). + +### Сравнение движков на том же потоке + +| конфигурация | MAE медиана | классы | время | в такт 700 мс | +|---|---|---|---|---| +| **DEFOM vitl, вход 480, iters 24** | 32.8 мм | **8/9 = 89 %** | **469 мс** | да, +231 мс | +| DEFOM vits, вход 640, iters 24 | 33.9 мм | 7/9 | 502 мс | да | +| DEFOM vitl, вход 640, iters 12 | 37.5 мм | 6/9 | 627 мс | да | +| DEFOM vitl, вход 640, iters 24 | 29.9 мм | 7/9 | 735 мс | нет, −35 мс | +| CRE, кроп, вход 640 | **23.5 мм** | 7/9 | 551 мс | да | +| CRE, кроп, исходный вход | 25.6 мм | 7/9 | 1366 мс | нет | +| CRE, вся лента, исходный вход | 32.4 мм | 7/9 | 1701 мс | нет | +| FastSAM + CRE | 40.0 мм | 5/9 | 753 мс | нет | +| yolo26n-seg + CRE | 41.2 мм | 2/6 | 238 мс | да | +| FoundationStereo (только CPU) | 152.8 мм | 4/9 | 7159 мс | нет | + +**CRE точнее по габаритам (23.5 против 32.8), DEFOM лучше по классам (8/9 против 7/9).** + +--- + +## 3. Кинематика + +### Ленты — 1.0 м/с + +Семь дорожек, привод через `PhysxSurfaceVelocityAPI`. Скорость задаётся **в локальной +системе тела**, а дорожки уложены по-разному: у `_04` и `_06` локальный +X смотрит в +мировой −X. Направление выводится из мировой цели, величина делится на то, сколько +мирового стоит одна локальная единица (у `_06` масштаб 0.5). + +`ConveyorTrack_06` — **криволинейный** угол на 90°, четверть кольца с центром +(−8.005, +1.042), радиусы 0.517…1.018. Линейный привод уводил товар в пустую середину +кольца; направление задаётся хордой, сохраняющей радиус, и в мировых координатах. + +Проверено: товар 160 мм проходит **7.93 м за 7.9 с ровно на 1.00 м/с** без замедлений; +поток из 9 товаров проходит всю линию 10.9 м. + +### Пушер — класс D + +Нож двигается **записью трансформа** (`Cell.blade_to`), его призматический сустав +**выключен**: иначе сустав тянет нож к своей цели, пока скрипт пишет его в другое место, +и нож дрожит весь прогон. Ход 720 мм, срабатывание у `PUSH_X = -3.900`. + +### Плуг — классы B и C + +Лезвие **кинематическое**, шарнир **выключен** (`physics:jointEnabled=False`), угол +пишется напрямую (`Plow.target`). Силовой привод перенастраивали трижды и он не держал: +звенел на ±21.4° быстрее, чем его успевала вести команда. + +Углы: `PLOW_PRESET = {"B": -16.0, "C": +16.0, "D": 0.0}`. Положительный поворот отклоняет +в −Y. Лезвие ставится **заранее**, до подхода товара. Скольжение товара вдоль кромки +измерено: 315–350 мм, то есть товар ведётся, а не отбрасывается ударом. + +### Камеры — E60 + +Шесть камер, три стереопары, все на 700 мм над лентой, возвышение 60°, дистанция 808 мм, +азимуты 90° / 208.3° / 330°. Точка осмотра (−0.750, 0.0, 1.781). + +Базы: D435 73.5 мм, Gemini305 26.5 мм, Gemini345 129.4 мм. Цена глубины соответственно +13.2 / 37.2 / 8.1 мм на пиксель диспаратности. + +**Расхождение с реальным стендом:** у настоящего D435 база **50.0 мм** (прочитано из +прошивки), а в симуляции 73.5 — в 1.47 раза больше. Значит результаты симуляции для этого +рига оптимистичнее реальности примерно в полтора раза. Не исправлено сознательно. + +--- + +## 4. Замер последнего прогона замкнутого контура + +**Классов получено 7 из 9, верно 5 из 7.** Верно: `bag` (D), `backpack` (C), `lunchbox` (B), +`detergent` (B), `box_400x400x300` (C). + +**Задержка от ворот до класса — медиана 0.70 с** при 3.15 с до пушера и 7.10 с до плуга. +Инференс 631–1284 мс. Запас четырёх- и десятикратный. + +Пушер сработал по классу D от камер. Плуг предпозиционировался шесть раз. + +**Сквозная доставка: в контейнеры попало 5 из 9, в СВОЙ контейнер - 3 из 9.** + +| причина потери | сколько | что именно | +|---|---|---| +| ошибка CV | 2 | bucket (D->C), box_300x200x200 (B->C) | +| класс верный, механика не довела | 1 | bag - пушер сработал, товар остался в лотке B вместо BinD | +| столкновение на входе | 2 | helmet и pillow, выпущены подряд | +| бросок пушера | 1 | detergent улетел на (+262, +2085) | +| плуг сдвинул недостаточно | 1 | box_400x400x300 на y = -1.49, за краем лотка C | + +Из четырёх потерь по механике ни одна не связана с распознаванием. Подробная таблица с +координатами и зонами лотков - в README, раздел 10.6. + +Сквозную доставку осмысленно мерить на шаге 1.4 м (там прежний прогон давал 9/9), а шаг +0.7 м использовать для замера классификации и габаритов. + +--- + +## 5. Проблемы, которые сейчас есть + +### Не решены + +1. **Класс D берётся неустойчиво.** `bucket` (истинный k = 0.995) не определяется ни одной + конфигурацией. На **эталонной геометрии меша** та же функция даёт 0.934, на нашем облаке + 0.66 — разрыв целиком в качестве облака, не в метрике. Габарит ведра выходит + вытянутым (333 × 239 при истинных 287 × 287), а вытянутое сечение высокого k дать не может. + +2. **Габариты в движении хуже статичных.** `box_300x200x200` в потоке дал 513 × 452 против + 311 × 218 на статичных кадрах и из-за этого ушёл в C вместо B. Причина видна в логе: + `detergent` попал на ворота уже на x = −1.708, то есть **мимо точки осмотра**, а кроп + привязан к неподвижной точке (−0.750). Товар в кадре смещён, в кроп попадает соседний. + +3. **Два товара не доехали до ворот.** `helmet` встал на x = +4.02, `pillow` на +1.15. Оба + выпускались подряд (2.15 и 2.85 с); при их габаритах (354 и 455 мм) шаг 700 мм оставляет + мало зазора, и они, судя по позициям, столкнулись у входа. + +4. **Пушер выбрасывает товар.** `detergent` закончил на (+262, +2085) — улетел на километры. + Скорость ножа на пределе: `PUSHER_MAX_SAFE = 2.5` м/с с пометкой «выше ~2.5 м/с + кинематический нож сбрасывает товар с линии». + +5. **Шаг 700 мм механически не даёт B/C.** Лезвие плуга 0.63 м, на смену угла остаётся + 0.07 м (10 % шага). Замерено: при 1.4 м — 9/9, при 0.7 м — 5/9. От скорости ленты не + зависит: доля занятости лезвия = 0.63/0.70 = 90 %. Нужен шаг > ~0.95 м либо другой + отводящий орган. + +6. **Очень тонкие товары проходят под лезвием плуга.** `watch` (4.2 мм) класса C проехал + мимо: класс определяется верно, механика — нет. + +### Проверено и НЕ помогло + +Каждый пункт — отдельный замер, все ухудшили результат: + +| попытка | результат | +|---|---| +| выбор маски по плоскости ленты вместо воротного пикселя | MAE 39.9 (было 40.0), классы 4/9 (было 5/9) | +| k подгонкой окружности P10/P90 | подняло k без разбора формы: коробки пошли в D | +| k по трём **мировым** сечениям | классы 6/9 (было 7/9) | +| сглаживание контура по угловым секторам | k макс 0.67 (было 0.85), классы 6/9 | +| проверка лево-право 0.5 / 1.0 / 1.5 px | отсеивает 28–52 % пикселей, MAE 34.6 (было 32.4), время ×2 | +| подгонка цилиндра RANSAC | не сработала ни на одном товаре: доля точек в допуске < 60 % | +| отбраковка ракурса по центроиду, порог 60 мм | MAE 65.4 (было 49.5) — откидывала два вида из трёх | +| ICP/RANSAC-совмещение облаков | 11.9 → 40.5 мм, три ракурса видят разные поверхности | + +**Общий вывод из этой серии:** у нас не выбросы, а **дырки в диспаратности**. Любая правка, +которая *вычитает* точки (сглаживание, лево-право, отбраковка), делает хуже. Помогает то, +что *повышает плотность* или *уменьшает область поиска*: кроп зоны осмотра (MAE 32.4 → 25.6) +и понижение входа сети (25.6 → 23.5). + +Недоделанная половина рецепта лево-право: заполнение мелких внутренних дырок и edge-aware +фильтр с запретом интерполяции через границу. Сейчас реализовано только удаление. + +### Установлено, что НЕ виновато + +- **Калибровка камер.** Восстановленное полотно садится на эталонную плоскость со смещением + 0.59 / 0.56 / 1.13 мм и наклоном 0.56° / 0.35° / 1.72° по трём ригам. Ни интринсики, ни + боковое расположение, ни положение виртуальных камер не при чём. +- **Стереодвижок как таковой.** CRE проверен против штатной глубины RealSense на физическом + стенде: отношение 0.998 и 1.000 на 249 тыс. пикселей. +- **Покрытие ракурсами.** Дуга сечения у ведра покрыта на 295–360°. + +--- + +## 6. Ловушки, на которых уже теряли время + +Каждая давала правдоподобный, но неверный результат. + +1. **Узлы OmniGraph надо УДАЛЯТЬ, а не деактивировать.** `SetActive(False)` убирает прем из + обхода, но собранный граф продолжает работать: `ConveyorBeltGraph` обнулял + `surfaceVelocity` за 5 шагов после `play`, `DiverterAnimGraph` останавливал таймлайн. + +2. **Невидимость не убирает коллайдер.** `capture_roi.py` прятал снятый товар через + `MakeInvisible()`, и после двух прогонов захвата в точке осмотра стояло **18 невидимых, + но твёрдых предметов**. Поток вставал на них «посреди ConveyorTrack_02». Лечится + `cell.clear_capture_parks()`, вызывается в `prepare()`. + +3. **`BBoxCache` во время прогона врёт.** Он читает авторские трансформы из слоя USD, а + физика пишет в Fabric. Отчёт показывал, что все товары стоят в точках выпуска, хотя + таймлайн отработал 26 с. Положения читать через `RigidPrim.get_world_poses()`. + +4. **`play()` после `stop()` перематывает в начало** и сбрасывает физику. Обработчик, + «возобновляющий» остановившийся таймлайн, обнуляет весь опыт. + +5. **Заданная частота физики не применяется.** `timeStepsPerSecond=120` не подействовал, + фактический шаг 83.33 мс (60 Гц). Скорости выходили ровно вдвое завышенными. Время + брать из таймлайна. + +6. **Файл `runtime/worker_ready` остаётся от прошлого запуска** и даёт ложную готовность. + Проверять работника по PID. + +7. **`cloud_from_roi` возвращает ПАРУ** (облако товара, облако полотна). Складывание + кортежа целиком роняет `np.vstack` на разнородных формах. + +8. **Меши `items_flow/` уже в каталожном масштабе и уже посажены на z = 0**, коллайдеры в + них уже есть. Домасштабирование и свои коллайдеры ломают спавн — товары не едут. + +9. **ArUco-метки не видны в ИК** (на физическом стенде): типографская краска на 850 нм + в значительной мере прозрачна. Позу брать из цветного кадра с ЦВЕТНЫМИ интринсиками и + переводить в систему ИК заводскими экстринсиками. + +--- + +## 7. Что делать дальше — по приоритету + +1. **Привязать кроп к товару, а не к неподвижной точке осмотра.** Это лечит проблему 2 — + самую вредную из открытых: из-за неё габариты в движении вдвое хуже статичных. +2. **Снизить скорость ножа пушера** — проблема 4, товар улетает. +3. **Разнести выпуск товаров по времени** либо увеличить шаг — проблема 3. +4. Доделать вторую половину фильтрации лево-право (заполнение дырок, edge-aware). +5. FoundationStereo на GPU: заблокировано внешне — `onnxruntime-gpu` требует CUDA 13, на + сервере 12.8; колёс под CUDA 12 для python 3.12 нет; `onnx2torch` падает на динамическом + `Clip`; TensorRT не установлен. Нужен либо `.pth` через код репозитория, либо CUDA 13 + (установка требует прав root). diff --git a/control_test/README.md b/control_test/README.md new file mode 100644 index 0000000..9481f7e --- /dev/null +++ b/control_test/README.md @@ -0,0 +1,493 @@ +# control_test — сортировочная ячейка с папкой объектов + +Автономный стенд: конвейер + пушер + плуг из `plow_cell_90_45_test.usd`, где **набор +товаров берётся из папки `items/`**. Положили новый `.usd` — он попадает в следующий +прогон. Ничего не зашито под конкретный объект. + +``` +control_test/ +├── scene/plow_cell_90_45_test.usd сцена (ссылается на ../assets → симлинк на assets проекта) +├── items/ меши товаров: *.usd + textures/ + labels.json +├── classify.py чтение разметки + правила B/C/D (для проверки) +├── cell.py физика ячейки: ленты, плуг, пушер, стыки, свет, пол +├── run_pipeline.py прогон сортировки: спавн из items/ по очереди, отчёт +│ +│ ── стенд замера габаритов камерами (раздел 9) ── +├── cam_configs.py расстановки камер; DEFAULT = "E60" — рабочая +├── capture_roi.py ЭТАП 1 в Isaac: рендер L/R по всем ригам +├── measure_roi.py ЭТАП 2 отдельным процессом: FastSAM + CREStereo +├── captures// кадры, manifest.json, roi_compare.json +│ +│ ── замкнутый контур: поток -> CV -> механика (раздел 10) ── +├── run_sorting_cv.py прогон сцены: поток, ворота, пушер, плуг по классу от CV +├── cv_worker.py процесс CV: DEFOM -> габариты -> k -> класс +├── measure_plane.py сам замер; здесь зафиксирован бейзлайн +├── runtime/ обмен заявками и ответами, кадры, результат прогона +├── .memory.md СОСТОЯНИЕ ПАЙПЛАЙНА: замеры, проблемы, ловушки +│ +└── diag/ одноразовые диагностики, не часть пайплайна +``` + +**Устаревшие файлы** помечены заголовком `SUPERSEDED` и оставлены только как история: +`capture_cfg.py` (тихо снимал пустую ленту), `measure_cfg.py` (мерил ленту вместо +товара), `reposition.py` (зашитые 600 мм). Использовать их нельзя. + +## 1. Запуск Isaac Sim + +Isaac Sim 6.0.1 стоит под пользователем `whatevenif`, запускается со стримингом WebRTC +и включённым python-сервером (TCP 8226) — через него в симулятор шлётся код. + +```bash +cd /home/whatevenif/isaacsim +nohup ./kit/kit ./apps/isaacsim.exp.full.streaming.kit \ + --no-window --no-ros-env \ + --enable isaacsim.code_editor.python_server \ + --/exts/omni.kit.livestream.app/primaryStream.publicIp=46.39.224.77 \ + --/exts/omni.services.livestream.session/quitOnSessionEnded=false \ + > /tmp/isaac.log 2>&1 & +``` + +Готовность: + +```bash +grep -q "app ready" /tmp/isaac.log && ss -ltn | grep 8226 # порт должен слушать +``` + +С другой машины порт 8226 пробрасывается ssh-туннелем: + +```bash +ssh -N -L 8226:127.0.0.1:8226 dasha@46.39.224.77 & +``` + +## 2. Открыть сцену + +```bash +cd /home/dasha/robozon-sorter +python3 isaacsim_send.py --timeout 120 \ + --file ~/.claude/skills/isaac-sim-remote/scripts/open_stage.py \ + --arg action=open \ + --arg usd_path=/home/dasha/robozon-sorter/control_test/scene/plow_cell_90_45_test.usd +``` + +Должно ответить `Stage prims: 366`. Сцену надо открывать **заново перед каждым прогоном** — +`run_pipeline.py` меняет состояние сцены (удаляет графы, снимает коллизии, спавнит тела). + +## 3. Прогон + +```bash +cd /home/dasha/robozon-sorter +python3 isaacsim_send.py --context ct --timeout 400 --execution-timeout 390 \ + --file control_test/run_pipeline.py +``` + +Только часть объектов / другие параметры: + +```bash +python3 isaacsim_send.py --context ct --timeout 400 --execution-timeout 390 \ + --args-json '{"only": ["bag","lunchbox"], "pitch": 1.4, "plow_angle": 20}' \ + --file control_test/run_pipeline.py +``` + +| аргумент | по умолчанию | что делает | +|---|---|---| +| `speed` | 1.0 | скорость лент, м/с | +| `pitch` | 1.4 | расстояние между товарами, м | +| `plow_angle` | 20 | угол плуга, град (B = −угол, C = +угол) | +| `swing_margin` | 0.25 | доля T_pitch на поворот; меньше → быстрее плуг | +| `plow_hold_max` | 4.0 | сколько плуг держит угол, с | +| `only` | все | список имён объектов | +| `limit` | 0 | взять первые N | + +## 4. Как добавить свой товар + +1. Положить `<имя>.usd` в `items/` (текстуры — в `items/textures/`). +2. Дописать строку в `items/labels.json`: + +```json +"my_part": { "zone": "D", "dims_mm": [220, 180, 175], "k": 0.91 } +``` + +3. Прогнать — товар подхватится сам. + +**Классы берутся из разметки, а не измеряются.** Меши обнаруживаются в папке и +спавнятся из неё, но `zone`, `dims_mm` и `k` читаются из `labels.json` — это ground +truth, относительно которого проверяется механика. + +Почему не автозамер: он был реализован и отброшен. Габариты мерились точно (сверено со +всем каталогом: `pen` 148.5/13.2/9.0 против 148/13/9, `pouf` 488.9 против 489), но +геометрическая оценка круглости систематически занижала тела с ручкой или полостью — +`bucket` 0.737 против 0.995, `mug` 0.731 против 0.985, `cylinder` 0.749 против 0.867. +Класс D тихо превращался в B, и это выглядело как отказ механики. Совпадение с каталогом +было 20/25. Для стенда, где проверяется именно механика, надёжнее читать метку. + +**Товары без записи в `labels.json` пропускаются** — не угадываются. Прогон печатает их +списком; сейчас это 12 мешей (`air_conditioner`, `briefcase_hard`, `carton_large`, +`cleat_small`, `clothespin_flat`, `cooler_cube`, `duffel_round`, `nailfile_mini`, +`printer_compact`, `safety_pin`, `toaster_compact`, `toaster_oven`). + +**Разметка проверяется на согласованность.** `classify.verify_labels()` прогоняет каждую +запись через правила раздела 5, и прогон печатает `labels consistent with the documented +B/C/D rules` либо перечисляет расхождения: ошибка в метке иначе всплыла бы как +необъяснимый сбой механики. + +## 5. Правила классификации + +Порядок как в пайплайне — сначала D: + +* **D** «не подходит без доупаковки» → пушер → BinD. + Габариты в норме (как у B), но `k > 0.8` хотя бы в одном сечении. +* **C** «не подходит по габаритам» → плуг `+angle` → ConveyorTrack_01 → контейнер C. + Хотя бы один размер `< 10 мм` **или** не влезает в `450×320×320 мм`. Форма не важна. +* **B** «подходит для сортировки» → плуг `−angle` → ConveyorTrack_06 → контейнер B. + Все размеры в диапазоне `10×10×10 … 450×320×320 мм` и `k ≤ 0.8`. + +Влезаемость проверяется по отсортированным габаритам против отсортированной рамки — +товар можно положить любой гранью. + +## 6. Ограничения, которые стоит знать + +* **Шаг 0.7 м из ТЗ: класс D работает, B/C — нет.** Замерено на 9 товарах: + + | шаг | B | C | D | итого | + |---|---|---|---|---| + | 1.4 м | 3/3 | 3/3 | 3/3 | **9/9** | + | 0.7 м | 1/3 | 1/3 | 3/3 | 5/9 | + + Причина чисто геометрическая: **лезвие плуга длиной 0.63 м**, и товар «занимает» его + 0.63 м пути. При шаге 0.7 м на смену угла остаётся 0.7 − 0.63 = **0.07 м (10 % шага)** — + это ниже уровня шума физики (товары подрагивают, интервал плывёт), поэтому регулярно + два товара разных классов оказываются на лезвии одновременно и второй получает чужой + угол. + + **Скорость ленты тут не поможет:** доля занятости = длина_лезвия / шаг = 0.63/0.70 = 90 % + и от скорости не зависит — время сокращается пропорционально и у товара, и у лезвия. + Реально помогает только шаг: нужен **> ~1.5 × длины лезвия ≈ 0.95 м**. При 1.4 м — + 100 % по всем классам. + + Укоротить лезвие нельзя «просто так»: его длина и есть вылет, которым он перекладывает + товар через ленту шириной 0.9 м. Чтобы держать 0.7 м, нужен другой отводящий орган + (второй плуг в шахматном порядке или толкатель вместо плуга). + +* **Очень тонкие товары не отрабатывают на плуге.** `watch` (4.2 мм) класса C + проехал мимо и остался на линии (x −7.64): лезвие плуга приподнято над полотном, и + такой товар проходит под ним. Класс определяется верно, механика — нет. +* 12 мешей в `items/` не имеют записи в `labels.json` и потому пропускаются + (см. раздел 4). Часть из них ещё и обмерена в других единицах, так что + зашивать им класс наугад нельзя — нужна честная разметка. + +## 7. Что именно чинит `cell.py` + +Каждый пункт — отдельная найденная и замеренная проблема, все включены в `prepare()`: + +| функция | зачем | +|---|---| +| `_kill_stale_graphs` | авторские `ConveyorBeltGraph` / `DiverterAnimGraph` **удаляются**, а не деактивируются: `SetActive(False)` не останавливает уже собранный OmniGraph, и он обнуляет `surfaceVelocity` каждый тик | +| `configure_belts` | все 7 лент + 4 плиты стыка; `ConveyorTrack_05` (вход линии) и ветка пушера `Belt_01` отсутствовали в списке проекта | +| `drive_corner_belt` | `ConveyorTrack_06` — **криволинейный** угол (четверть кольца, центр (−8.005, +1.042), радиусы 0.517…1.018). Линейный привод уводил товар в пустую середину кольца, и он проваливался. Направление задано хордой, сохраняющей радиус, и **в мировых координатах** — у ленты неравномерный масштаб, и пересчёт в локальные искажает направление | +| `add_transfer_bridge` | между краем `_04` (y = +0.45) и началом `_06` (x = −8.00) не было опоры ровно там, где плуг сталкивает товар | +| `add_container_catchers` | лотки толщиной 40 мм пробивались товаром при падении с ленты (~3.4 м/с ≈ 57 мм за шаг) | +| `open_junction` | у конвейерной «обшивки» есть коллайдер вместе с бортами — стена ровно там, где товар должен уходить вбок | +| `regrip_decks` / `_ensure_grip_material` | `configure_plow` перебивает плиты скользким материалом; сам grip-материал проект создаёт только внутри своей `configure_belts`, которую этот модуль не вызывает | +| `resize_pusher_blade` / `grip_pusher_blade` / `seat_pusher_blade` | нож 1200 → 500 мм; свой цепкий материал вместо скользкого «плужного»; посадка на 1 мм над полотном (было 14 мм — тонкие товары проходили под ним) | +| `add_ground_and_light` / `add_side_rails` | пол и купольный свет; борта только на прямых участках — на стыках они блокируют штатный сход товара | + +## 8. Зависимости + +`cell.py` и `run_pipeline.py` импортируют `robozon_sorter` (константы `config.py`, класс +`Plow`, помощники `sim/scene.py` и `sim/plow_cell.py`), поэтому `/home/dasha/robozon-sorter` +должен быть в `sys.path` — `run_pipeline.py` добавляет его сам. + +`items/` и `scene/` — собственные копии, их правка проект не задевает. `assets` — +симлинк на `../assets`, потому что сцена ссылается на ленты и плуг относительным путём. + + +## 9. Стенд замера габаритов камерами + +Отдельная задача от сортировки: по стереопарам определить габариты **неизвестного** +товара. Классы тут не читаются из разметки — они и есть то, что надо предсказать. + +### 9.1 Рабочая расстановка — E60 + +| параметр | значение | +|---|---| +| высота над лентой | **700 мм** (все три рига) | +| возвышение | **60°** | +| рабочая дистанция | **808 мм** | +| азимуты | **90° / 208.3° / 330°** (RealSense D435 / Gemini305 / Gemini345) | +| точка осмотра | `(-0.750, 0.0, 1.781)` — поверхность `ConveyorTrack_04` | + +Зафиксирована в `cam_configs.DEFAULT` и записана в саму сцену. Применить заново: + +```bash +python3 isaacsim_send.py --file /tmp/save_e60.py # apply_config(stage, CC.DEFAULT) +``` + +Калибровка проверена: поворот левой и правой камеры в паре совпадает точно +(отклонение 0.0e+00), база равна паспортной с нулевой поперечной составляющей, +Δv между кадрами 0.000 px — то есть `depth = fx·B/disp` применим без ректификации. + +Цена глубины на 808 мм: Gemini345 8.0 мм/px, D435 13.2, Gemini305 37.2. + +### 9.2 Почему именно E60 + +Развёртка по возвышению при фиксированной высоте 700 мм и, отдельно, попытка дать +каждому ригу свою дистанцию под одинаковую цену глубины (EQ15/EQ20). Медиана MAE по +9 товарам, слияние трёх ригов: + +| конфиг | возвыш. | дистанция | MAE медиана | D435 | Gemini305 | Gemini345 | +|---|---|---|---|---|---|---| +| A_original | 42° | 1051 мм | 30.0 | 29.3 | **109.5 (1/9)** | 67.4 (5/9) | +| E45 | 45° | 991 мм | 25.5 | 29.9 | 50.8 (1/9) | 32.8 | +| **E60** | **60°** | **808 мм** | **22.3** | **25.8** | **27.1** | **28.0** | +| E75 | 75° | 726 мм | 23.6 | 27.8 | 26.3 | 29.4 | +| EQ15 | 45° | 518/863/1146 | 29.8 | 42.2 | 31.3 | 30.9 | +| EQ20 | 45° | 598/996/1323 | 24.6 | 33.4 | 28.2 | 32.7 | + +Два вывода, на которых всё держится: + +* **Gemini305 не нужна своя короткая дистанция.** На 1051 мм он давал облако один раз + из девяти, потому что диспаратность в точке осмотра была всего 17 px. На 808 мм она + 21.7 px и риг работает наравне с остальными. Попытка подобрать каждому ригу свою + дистанцию (EQ15/EQ20) починила Gemini305, но испортила D435 (25.8 → 42.2) и сделала + слияние хуже любой общей дистанции. +* **E60 — первая расстановка, где слияние трёх ригов выигрывает у лучшего одиночного** + (22.3 против 25.8). На 1051 мм слияние не давало ничего. + +E75 почти не хуже по точности, но его общая зона ленты на треть меньше +(5756 против 7681 см²) — меньше запас на смещение товара. + +### 9.3 Как устроен замер + +Два этапа, потому что torch внутри Isaac роняет процесс: + +```bash +# ЭТАП 1 — в Isaac, без torch: рендер L/R со всех шести камер +python3 isaacsim_send.py --timeout 880 --file control_test/capture_roi.py +python3 isaacsim_send.py --timeout 880 --file control_test/capture_roi.py --arg cfg=E75 + +# ЭТАП 2 — отдельным процессом: FastSAM + CREStereo + обратная проекция +/home/whatevenif/isaacsim/python.sh measure_roi.py # E60, все 4 варианта ROI +/home/whatevenif/isaacsim/python.sh measure_roi.py E60 objroi # только рабочая схема +``` + +Схема ROI (`objroi`), она же рабочая: + +``` +общая зона ленты, видимая всеми 6 камерами + ↓ ограничивает, где FastSAM ищет +FastSAM segment-everything → маска, содержащая «воротный» пиксель + ↓ bbox + 48 px запаса +ОДНО окно колонок на левый и правый кадр, левый край расширен на 1.35 × макс. диспаратности + ↓ +CREStereo → диспаратность → depth = fx·B/disp → обратная проекция по экстринсикам +``` + +Три правила, каждое подтверждено замером: + +* **RGB не маскируется до CREStereo.** Матчеру нужен фон вокруг предмета; маска + применяется только к глубине, с эрозией 2 × 3×3, иначе в облако попадает кайма фона. +* **Левое и правое окно обязаны совпадать.** Контрольный вариант `objroi_bad`, где + правый кроп центрируется по своему bbox, разрушил **8 облаков из 9** — сдвиг окна + подменяет диспаратность. +* **Слияние идёт только по калиброванным экстринсикам, без RANSAC/ICP.** Повторная + регистрация ухудшала результат в каждой проверке (11.9 → 40.5 мм): три ракурса видят + разные поверхности, и ICP совмещает несоответствующие участки. + +Общая зона ленты проецируется в 84–92 % кадра, поэтому сама по себе разрешения она не +экономит — её роль в том, чтобы ограничить область поиска сегментации. Выигрыш даёт +вторая ступень: `objroi` втрое быстрее полного кадра (519 против 1620 мс на товар) при +той же точности и не теряет мелкие предметы (`lunchbox` полный кадр терял вовсе). + +### 9.4 Эталон габаритов — bbox меша в сцене, не каталог + +Меши в `items/` **в 2.0–2.8 раза мельче** своих паспортных размеров, и коэффициент у +каждого свой (`bag` 2.05, `box_400x400x300` 2.41, `box_300x200x200` 2.79). Для сортировки +по меткам это безразлично, но оценивать предсказание масштаба против `labels.json` +нельзя. `capture_roi.py` пишет в манифест оба числа: `gt_scene_mm` (реальный bbox в +сцене — эталон замера) и `gt_catalogue` (паспорт из `labels.json` — эталон сортировки). + +### 9.5 Что не решено + +* **Систематическое занижение.** На E60: `pillow` 223 → 138, `bucket` 136 → 101, + `backpack` 170 → 158. Подушка худшая во всех шести расстановках (66–77 мм) — плоский + мягкий силуэт; у ведра, похоже, снимается кромка, а не корпус. От расстановки камер + это не зависит. +* **Сегментация иногда берёт ленту.** На EQ-расстановках `detergent` дал 157 мм вместо + 108. Напрашивается отбраковка ракурса по 3D-центроиду: вид, чей центр дальше 6 см от + медианы по трём ригам, выбрасывать до слияния (в прошлом пайплайне это помогало). +* Замерено на 9 товарах из 25 — полный набор ещё не прогонялся. + +### 9.6 Ловушки спавна, из-за которых стенд полгода мерил пустую ленту + +Обе тихие, обе дают правдоподобные кадры пустого конвейера: + +1. **`ClearXformOpOrder()` на приме, который несёт ссылку**, стирает собственное + размещение меша. Замер bbox до очистки и последующий перенос дают промах ровно на + этот сдвиг — товары уходили на 0.6 м под полотно. Ссылка должна жить на **дочернем** + приме, размещение — на родителе. +2. **Слои товаров прописывают `visibility = invisible` на своём корне**, а + `MakeVisible()` на родителе авторское значение потомка не снимает. Нужно пройти + `Usd.PrimRange` и выставить `inherited` всем Imageable. + +Плюс: `BBoxCache.ComputeWorldBound()` на только что созданном родителе возвращает пустой +диапазон даже при скомпонованном потомке — мерить надо сам прим со ссылкой; и ссылка не +компонуется в том же тике, нужен цикл `await app_utils.update_app_async(steps=2)`. + +Поэтому `capture_roi.py` печатает **контраст**: разницу средней яркости внутри ожидаемого +силуэта и в кольце вокруг него, по каждой камере. Меньше 3 — товар не отрендерился, +строка помечается `<-- НЕ ВИДЕН`. Без этой проверки четыре круга «настройки сегментации» +были потрачены на кадры, где предмета не было вовсе. + +--- + +## 10. Замкнутый контур: поток → CV → механика + +Полный прогон, где класс товара приходит **от стереопайплайна во время движения**, а не из +`labels.json`. Пушер и плуг реагируют физически на предсказанный класс. Разметка нужна +только для подсчёта ошибки в конце. + +Прежний `run_pipeline.py` (классы из разметки) остаётся рабочим и служит контролем: он +разделяет ошибки механики и ошибки распознавания. + +Подробное состояние пайплайна, все замеры и открытые проблемы — в `.memory.md`. + +### 10.1 Два процесса + +torch внутри Isaac роняет процесс, поэтому CV живёт отдельно, а обмен идёт через каталог +`runtime/`: + +``` +run_sorting_cv.py (в Isaac, без torch) cv_worker.py (отдельный процесс, torch+GPU) + поток 700 мм при 1 м/с + ворота x = -0.750 -> 6 кадров ──заявка──> runtime/req/<товар>.json + DEFOM vitl / вход 480 / iters 24 + облако -> габариты -> k -> класс + класс <──ответ── runtime/res/<товар>.json + пушер: класс D -> Cell.stroke() + плуг: класс B/C -> Plow.target(-16 / +16) +``` + +Времени хватает с запасом: от ворот до пушера товар едет 3.15 с, до плуга 7.10 с, а полная +задержка от ворот до класса замерена в **0.70 с** (медиана; инференс 631–1284 мс). + +### 10.2 Запуск + +```bash +# 1. работник CV. Прогрев ~90 с - грузится vitl-энкодер +cd /home/dasha/robozon-sorter/control_test +nohup /home/whatevenif/isaacsim/python.sh cv_worker.py > /tmp/cvworker.log 2>&1 & + +# 2. проверить ПО PID, а не по файлу runtime/worker_ready: +# файл остаётся от прошлого запуска и даёт ложную готовность +pgrep -af "python.*cv_worker.py" + +# 3. открыть сцену заново (cell.prepare меняет её состояние), затем прогон +cd /home/dasha/robozon-sorter +python3 isaacsim_send.py --context cvsort --timeout 2580 --execution-timeout 2560 \ + --file control_test/run_sorting_cv.py +``` + +Результат прогона: `runtime/sorting_cv.json`, кадры товаров `runtime/frames/`, +обзорный снимок `runtime/shots/overview.png`. + +### 10.3 Зафиксированный бейзлайн CV + +Значения по умолчанию в `measure_plane.py`: + +| параметр | значение | +|---|---| +| стереодвижок | **DEFOM-Stereo vitl** (`STEREO=defom`) | +| вход сети | **480** px по ширине (`SW=480`) | +| итерации | **24** + `scale_iters` 8 | +| окно | **кроп зоны осмотра** (`CROP=1`) | +| сегментация | **не используется** | + +Товар отделяется от полотна превышением над плоскостью (20 мм) плюс отсев по плотности: +товар даёт сплошную поверхность, полотно — редкие выбросы диспаратности. Сегментация не +участвует вовсе — она была источником и раздутых габаритов, и промахов по классу D. + +Веса: `/home/dasha/isaac_assets/cv/defom-stereo/checkpoints/`. + +**Замер на статичных кадрах потока 700 мм (9 товаров):** классы 8/9 = 89 %, габариты +MAE медиана 32.8 мм, 469 мс на товар при такте 700 мс. + +**Замер замкнутого контура:** классов получено 7 из 9, верно 5 из 7. + +Полная таблица сравнения движков и конфигураций — в `.memory.md`, раздел 2. + +### 10.4 Кинематика + +* **Ленты 1.0 м/с.** `surfaceVelocity` задаётся в ЛОКАЛЬНОЙ системе тела; у `_04` и `_06` + локальный +X смотрит в мировой −X. `ConveyorTrack_06` — криволинейный угол на 90°, + направление задаётся хордой в мировых координатах. Проверено: товар 160 мм идёт 7.93 м + ровно на 1.00 м/с. +* **Пушер** — нож двигается записью трансформа (`Cell.blade_to`), призматический сустав + выключен: иначе сустав и скрипт тянут нож в разные стороны и он дрожит весь прогон. +* **Плуг** — лезвие кинематическое, шарнир выключен, угол пишется напрямую + (`Plow.target`). Силовой привод не держал: звенел на ±21.4°. Углы + `{"B": -16, "C": +16, "D": 0}`, лезвие ставится ЗАРАНЕЕ. Скольжение товара вдоль кромки + замерено: 315–350 мм. + +### 10.5 Открытые проблемы + +1. **Габариты в движении вдвое хуже статичных.** Кроп привязан к неподвижной точке осмотра + (−0.750), а товар пересекает её не точно: `detergent` попал на ворота уже на x = −1.708. + Товар в кадре смещён, в кроп попадает соседний. Самая вредная из открытых. +2. **Класс D берётся неустойчиво.** `bucket` не определяется ни одной конфигурацией. На + эталонной геометрии меша та же функция даёт 0.934, на нашем облаке 0.66 — разрыв целиком + в качестве облака. +3. **Товары могут не доехать до ворот.** `helmet` и `pillow`, выпущенные подряд, столкнулись + у входа: при габаритах 354 и 455 мм шаг 700 мм оставляет мало зазора. +4. **Пушер выбрасывает товар с линии** при скорости ножа на пределе (`PUSHER_MAX_SAFE` 2.5 м/с). +5. **Шаг 700 мм механически не даёт B/C** — лезвие плуга 0.63 м, на смену угла остаётся + 0.07 м. При 1.4 м — 9/9, при 0.7 м — 5/9 (см. раздел 6). + +Что уже проверено и **не** помогло (сглаживание контура, проверка лево-право, цилиндр +RANSAC, отбраковка по центроиду и другое) — перечислено в `.memory.md`, раздел 5. + +### 10.6 Сколько товаров доходит до контейнеров + +Замер последнего прогона замкнутого контура. Зоны лотков берутся из самой сцены +(`B_Floor` x −9.26…−8.36 / y +1.07…+1.87; `C_Floor` x −10.90…−10.00 / y −0.63…+0.18; +`BinD_Floor` x −6.21…−4.95 / y +1.59…+2.85), допуск 0.35 м — падая с ленты, товар может +лечь у стенки, а не над серединой пола. + +**В контейнеры попало 5 из 9. В СВОЙ контейнер — 3 из 9.** + +| товар | эталон | CV | конец X, Y | куда попал | +|---|---|---|---|---| +| `backpack` | C | C | −10.44, −0.47 | контейнер C — **верно** | +| `lunchbox` | B | B | −8.95, +1.55 | контейнер B — **верно** | +| `bag` | D | D | −8.37, +1.03 | контейнер B — класс верный, доставка нет | +| `bucket` | D | C | −10.64, +0.14 | контейнер C — ошибка CV | +| `box_300x200x200` | B | C | −10.63, −0.28 | контейнер C — ошибка CV | +| `box_400x400x300` | C | C | −9.39, −1.49 | остался на линии, за краем лотка | +| `detergent` | B | B | +262.50, +2085.32 | улетел за пределы ячейки | +| `helmet` | D | — | +4.02, +0.78 | не доехал до ворот | +| `pillow` | C | — | +1.15, +0.91 | не доехал до ворот | + +### Разложение потерь по причинам + +Общая цифра здесь малополезна: причины разные и лечатся по-разному. + +| причина | сколько | что именно | +|---|---|---| +| ошибка CV | 2 | `bucket` (D→C), `box_300x200x200` (B→C) | +| класс верный, механика не довела | 1 | `bag` — пушер сработал (есть в логе), но товар остался в лотке B вместо BinD | +| столкновение на входе | 2 | `helmet` и `pillow` выпущены подряд; при габаритах 354 и 455 мм шаг 700 мм слишком тесен | +| бросок пушера | 1 | `detergent` — скорость ножа на пределе `PUSHER_MAX_SAFE` = 2.5 м/с | +| плуг сдвинул недостаточно | 1 | `box_400x400x300` закончил на y = −1.49, за краем лотка C | + +**Из четырёх потерь по механике ни одна не связана с распознаванием.** CV ошибся на двух +товарах из семи, кому вообще выдал класс. + +### Фоновое ограничение + +При шаге 700 мм классы B и C механически не отрабатывают в принципе: лезвие плуга 0.63 м, +на смену угла остаётся 0.07 м (10 % шага) — см. раздел 6. Прежний прогон с классами из +разметки давал при шаге 1.4 м результат **9/9**. То есть значительная часть этих потерь — +не про CV и не про настройку, а про геометрию плуга, и на шаге 0.7 м она не устраняется +ни скоростью ленты, ни точностью классификации. + +Проверять сквозную доставку осмысленно **на шаге 1.4 м**, а шаг 0.7 м использовать для +замера классификации и габаритов. diff --git a/control_test/assets b/control_test/assets new file mode 120000 index 0000000..94b2199 --- /dev/null +++ b/control_test/assets @@ -0,0 +1 @@ +/home/dasha/robozon-sorter/assets \ No newline at end of file diff --git a/control_test/bodies_and_shots.py b/control_test/bodies_and_shots.py new file mode 100644 index 0000000..588c00a --- /dev/null +++ b/control_test/bodies_and_shots.py @@ -0,0 +1,63 @@ +"""Rebuild the camera body markers at the new poses, then look through the D435 pair.""" +import omni.usd, omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics + +stage = omni.usd.get_context().get_stage() +xc = UsdGeom.XformCache() +ROOT = "/World/CameraBodiesSide" +NAMES = ["RealSense_D435_Left", "RealSense_D435_Right", + "Orbbec_Gemini305_Left", "Orbbec_Gemini305_Right", + "Orbbec_Gemini345_Left", "Orbbec_Gemini345_Right"] +COLOR = {"RealSense": (0.15, 0.5, 0.95), "Orbbec": (0.95, 0.45, 0.1)} + +def cam(name): + for p in stage.Traverse(): + if p.IsA(UsdGeom.Camera) and p.GetName() == name: + return p + raise KeyError(name) + +# the old bodies are still at the old poses and would now be both wrong and in the way +old = stage.GetPrimAtPath("/World/CameraBodies") +if old.IsValid(): + UsdGeom.Imageable(old).MakeInvisible() + for d in Usd.PrimRange(old): + a = d.GetAttribute("physics:collisionEnabled") + if a: + a.Set(False) + print("old /World/CameraBodies hidden (stale poses, and they occluded the new views)") + +UsdGeom.Xform.Define(stage, ROOT) +for n in NAMES: + c = cam(n) + M = xc.GetLocalToWorldTransform(c) + pos = Gf.Vec3d(M.ExtractTranslation()) + fwd = M.TransformDir(Gf.Vec3d(0, 0, -1)); fwd = fwd / (fwd.GetLength() or 1) + path = f"{ROOT}/{n}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + cube = UsdGeom.Cube.Define(stage, path) + cube.CreateSizeAttr().Set(1.0) + xf = UsdGeom.Xformable(cube.GetPrim()) + # BEHIND the lens plane: a housing centred on the camera looks into its own inside + # and the frame comes back black - that failure is documented in this project. + xf.AddTranslateOp().Set(pos - fwd * 0.045) + xf.AddScaleOp().Set(Gf.Vec3f(0.05, 0.05, 0.05)) + key = "RealSense" if n.startswith("RealSense") else "Orbbec" + UsdGeom.Gprim(cube.GetPrim()).CreateDisplayColorAttr().Set([Gf.Vec3f(*COLOR[key])]) +print(f"rebuilt {len(NAMES)} body markers under {ROOT}, each offset behind its lens") + +w = vp.get_active_viewport() +orig = w.camera_path +shots = [] +for n in ("RealSense_D435_Left", "RealSense_D435_Right"): + w.camera_path = cam(n).GetPath() + await app_utils.update_app_async(steps=45) + f = f"/tmp/cam_{n}.png" + vp.capture_viewport_to_file(w, file_path=f) + await app_utils.update_app_async(steps=20) + shots.append(f) + print("captured", f) +w.camera_path = orig +await app_utils.update_app_async(steps=10) +print("viewport camera restored:", orig) diff --git a/control_test/calib_check.py b/control_test/calib_check.py new file mode 100644 index 0000000..0ae1352 --- /dev/null +++ b/control_test/calib_check.py @@ -0,0 +1,74 @@ +"""Verify the rigs against each other: standoff, pair geometry, and a reprojection test. + +The reprojection test is the end-to-end one: take the inspection point, transform it into +each camera's frame with that camera's own extrinsics, project with its intrinsics, and +check where it lands. A correctly aimed camera puts it on the principal point. +""" +import math +import omni.usd +from pxr import Gf, Usd, UsdGeom + +stage = omni.usd.get_context().get_stage() +xc = UsdGeom.XformCache() +TARGET = Gf.Vec3d(-0.750, 0.0, 1.781) + +def cam(name): + for p in stage.Traverse(): + if p.IsA(UsdGeom.Camera) and p.GetName() == name: + return p + raise KeyError(name) + +NAMES = ["RealSense_D435_Left", "RealSense_D435_Right", + "Orbbec_Gemini305_Left", "Orbbec_Gemini305_Right", + "Orbbec_Gemini345_Left", "Orbbec_Gemini345_Right"] +RES = {"RealSense_D435": (1280, 720), "Orbbec_Gemini305": (1280, 800), + "Orbbec_Gemini345": (1280, 800)} + +print("=== standoff (requested 600 mm) ===") +P, F = {}, {} +for n in NAMES: + M = xc.GetLocalToWorldTransform(cam(n)) + p = Gf.Vec3d(M.ExtractTranslation()) + f = M.TransformDir(Gf.Vec3d(0, 0, -1)); f = f / (f.GetLength() or 1) + P[n], F[n] = p, f + print(f" {n:>24}: {(TARGET-p).GetLength()*1000:7.1f} mm") + +print("\n=== pair geometry ===") +for rig in ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"): + l, r = P[f"{rig}_Left"], P[f"{rig}_Right"] + fl, fr = F[f"{rig}_Left"], F[f"{rig}_Right"] + base = (r - l).GetLength() + ang = math.degrees(math.acos(max(-1, min(1, Gf.Dot(fl, fr))))) + kind = ("OPPOSING - not a stereo pair" if ang > 150 else + "rectified (parallel axes)" if ang < 0.5 else f"verged {ang:.2f} deg") + print(f" {rig:>18}: baseline {base*1000:8.1f} mm axes {ang:6.2f} deg {kind}") + +print("\n=== reprojection of the inspection point (should land on the principal point) ===") +for n in NAMES: + prim = cam(n) + c = UsdGeom.Camera(prim) + fl_mm = c.GetFocalLengthAttr().Get() + ha = c.GetHorizontalApertureAttr().Get() + va = c.GetVerticalApertureAttr().Get() + rig = n.rsplit("_", 1)[0] + W, H = RES[rig] + fx = fl_mm / ha * W + fy = fl_mm / va * H + cx, cy = W / 2.0, H / 2.0 + M = xc.GetLocalToWorldTransform(prim) + Pcam = M.GetInverse().Transform(TARGET) # world -> camera frame + z = -Pcam[2] # camera looks down -Z + if z <= 1e-6: + print(f" {n:>24}: BEHIND the camera"); continue + u = cx + fx * (Pcam[0] / z) + v = cy - fy * (Pcam[1] / z) + print(f" {n:>24}: depth {z*1000:6.1f} mm pixel ({u:7.1f},{v:7.1f}) " + f"offset from centre ({u-cx:+5.1f},{v-cy:+5.1f}) px") + +print("\n=== cross-rig consistency: does every camera see the same point in front of it? ===") +depths = [] +for n in NAMES: + M = xc.GetLocalToWorldTransform(cam(n)) + depths.append(-M.GetInverse().Transform(TARGET)[2]) +print(f" depth spread across all six: {min(depths)*1000:.1f} .. {max(depths)*1000:.1f} mm" + f" (max-min = {(max(depths)-min(depths))*1000:.1f} mm)") diff --git a/control_test/calib_rs_side.json b/control_test/calib_rs_side.json new file mode 100644 index 0000000..8ae13aa --- /dev/null +++ b/control_test/calib_rs_side.json @@ -0,0 +1,89 @@ +{ + "target": [ + -0.75, + 0.0, + 1.781 + ], + "standoff_m": 0.6, + "side_elevation_deg": 20.0, + "cameras": { + "RealSense_D435_Left": { + "pos": [ + -0.75, + 0.5638, + 1.9862 + ], + "fwd": [ + 0.0, + -0.9397, + -0.342 + ], + "dist_mm": 600.0 + }, + "RealSense_D435_Right": { + "pos": [ + -0.75, + -0.5638, + 1.9862 + ], + "fwd": [ + 0.0, + 0.9397, + -0.342 + ], + "dist_mm": 600.0 + }, + "Orbbec_Gemini305_Left": { + "pos": [ + -1.1442, + -0.2125, + 2.1806 + ], + "fwd": [ + 0.6459, + 0.3733, + -0.6659 + ], + "dist_mm": 600.1 + }, + "Orbbec_Gemini305_Right": { + "pos": [ + -1.1309, + -0.2354, + 2.1806 + ], + "fwd": [ + 0.6459, + 0.3733, + -0.6659 + ], + "dist_mm": 600.1 + }, + "Orbbec_Gemini345_Left": { + "pos": [ + -0.3943, + -0.2798, + 2.1802 + ], + "fwd": [ + -0.6467, + 0.373, + -0.6654 + ], + "dist_mm": 603.5 + }, + "Orbbec_Gemini345_Right": { + "pos": [ + -0.3297, + -0.1677, + 2.1802 + ], + "fwd": [ + -0.6467, + 0.373, + -0.6654 + ], + "dist_mm": 603.5 + } + } +} \ No newline at end of file diff --git a/control_test/cam_configs.py b/control_test/cam_configs.py new file mode 100644 index 0000000..28ee764 --- /dev/null +++ b/control_test/cam_configs.py @@ -0,0 +1,174 @@ +"""Named camera arrangements. The canonical one is E60 - see DEFAULT below. + +Each rig is described by (azimuth, elevation) of its CENTRE as seen from the inspection +point, plus its native baseline. Both eyes of a rig always share one orientation and the +right eye is offset along the camera's own +X - that is what keeps a pair rectified, which +CREStereo needs (depth = fx*B/disp assumes parallel axes). +""" +import json, math +import omni.usd +from pxr import Gf, Usd, UsdGeom + +TARGET = Gf.Vec3d(-0.750, 0.0, 1.781) + +# HEIGHT drives the layout: every rig sits this far ABOVE the belt surface, and its +# standoff follows from its elevation angle (standoff = HEIGHT / sin(elev)). At the old +# 600 mm standoff / 400 mm height the vertical field at the target was +-320 mm and an +# object taller than ~220 mm had its top outside four of the six frames - the height is +# what buys headroom for the 450x320x320 envelope. +HEIGHT = 0.70 # every rig this far above the belt surface +STANDOFF = 0.60 # kept only for the SPLIT rig, which is deliberately low+sideways +BASELINE = {"RealSense_D435": 0.0735, "Orbbec_Gemini305": 0.0265, "Orbbec_Gemini345": 0.1294} + +# azimuth measured in the belt plane (deg, 0 = +X downstream, 90 = +Y side), elevation +# above the belt plane. The ORIGINAL rig measured out at ~90/208/330 deg azimuth and ~42 +# deg elevation - i.e. three views already spread ~120 deg apart, which is a sane merge +# geometry; only the distance was 628 mm rather than 600. +CONFIGS = { + "A_original": { # previous layout, brought to 600 mm + "RealSense_D435": (90.0, 41.8), + "Orbbec_Gemini305": (208.3, 41.8), + "Orbbec_Gemini345": (330.0, 41.7), + }, + "B_side_opposed": { # D435 split to face itself across the belt, low and sideways + "RealSense_D435": ("SPLIT", 20.0), + "Orbbec_Gemini305": (208.3, 41.8), + "Orbbec_Gemini345": (330.0, 41.7), + }, + "C_low_triad": { # same 120 deg spread, but LOWER - more side/height coverage, + "RealSense_D435": (90.0, 25.0), # which is where the old pipeline lost accuracy + "Orbbec_Gemini305": (210.0, 25.0), + "Orbbec_Gemini345": (330.0, 25.0), + }, + "D_mixed_elev": { # one overhead for footprint + two low for height/silhouette + "RealSense_D435": (90.0, 65.0), + "Orbbec_Gemini305": (210.0, 22.0), + "Orbbec_Gemini345": (330.0, 22.0), + }, +} + +AZ = (90.0, 208.3, 330.0) # rig azimuths in the belt plane, ~120 deg apart + +# ============================ CANONICAL ARRANGEMENT ============================ +# E60: height 700 mm, elevation 60 deg -> working distance 808 mm, azimuths 90 / +# 208.3 / 330 deg. Chosen by measurement on 2026-08-01, not by preference: +# +# config elev distance MAE med D435 G305 G345 +# A_orig 42 1051 mm 30.0 29.3 109.5 (1/9) 67.4 (5/9) +# E45 45 991 mm 25.5 29.9 50.8 (1/9) 32.8 +# E60 60 808 mm 22.3 25.8 27.1 28.0 <-- all 9/9 +# E75 75 726 mm 23.6 27.8 26.3 29.4 +# EQ15/20 45 per-rig 29.8 42.2 31.3 30.9 +# +# Two findings are load-bearing: +# * Gemini305's 26.5 mm baseline does NOT need its own short distance. At 1051 mm it +# produced a cloud once in nine tries because the disparity at the target was only +# 17 px; at 808 mm it is 22 px and the rig works. Giving each rig its own +# "equal depth precision" distance (EQ15/EQ20) fixed G305 but wrecked D435 +# (25.8 -> 42.2) and made the merge worse than any common-distance layout. +# * E60 is the first layout where fusing three rigs beats the best single rig +# (22.3 vs 25.8). At 1051 mm fusion bought nothing. +# +# E75 is nearly as accurate but its shared belt region is a third smaller +# (5756 vs 7681 cm2), i.e. less room for the item to sit off-centre. +DEFAULT = "E60" + +CONFIGS["E60"] = { + "RealSense_D435": (AZ[0], 60.0), + "Orbbec_Gemini305": (AZ[1], 60.0), + "Orbbec_Gemini345": (AZ[2], 60.0), +} + +# ---- kept for reproducing the sweep above; not used by the pipeline ---- +for _el in (45.0, 75.0): + CONFIGS[f"E{int(_el)}"] = {rig: (az, _el) for rig, az in zip( + ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"), AZ)} + +_FX = 674.419 +_B = {"RealSense_D435": 0.0735, "Orbbec_Gemini305": 0.0265, "Orbbec_Gemini345": 0.1294} +for _r_mm in (15.0, 20.0): # per-rig distance for one shared mm-per-disp-px + CONFIGS[f"EQ{int(_r_mm)}"] = { + rig: (az, 45.0, math.sqrt(_r_mm / 1000.0 * _FX * _B[rig])) + for rig, az in zip(("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"), AZ)} + + +def _cam(stage, name): + for p in stage.Traverse(): + if p.IsA(UsdGeom.Camera) and p.GetName() == name: + return p + raise KeyError(name) + + +def _look_at(pos, target): + fwd = target - pos + fwd = fwd / (fwd.GetLength() or 1.0) + zax = -fwd + up = Gf.Vec3d(0, 0, 1) + if abs(Gf.Dot(up, zax)) > 0.999: + up = Gf.Vec3d(0, 1, 0) + xax = Gf.Cross(up, zax); xax = xax / (xax.GetLength() or 1.0) + yax = Gf.Cross(zax, xax) + M = Gf.Matrix4d(1.0) + M.SetRow3(0, xax); M.SetRow3(1, yax); M.SetRow3(2, zax) + M.SetTranslateOnly(pos) + return M, xax, fwd + + +def _place(stage, name, M): + xf = UsdGeom.Xformable(_cam(stage, name)) + xf.ClearXformOpOrder() + xf.AddTransformOp().Set(M) + + +def apply_config(stage, cfg_name, res=(1280, 720)): + """position all six cameras, and square up the apertures for the render resolution. + + The aperture aspect must match the image aspect or fx != fy and every back-projected + point is stretched - a silent scale error in exactly the dimension we are measuring. + """ + cfg = CONFIGS[cfg_name] + W, H = res + out = {} + for rig, spec in cfg.items(): + b = BASELINE[rig] + if spec[0] == "SPLIT": + th = math.radians(spec[1]) + standoff = HEIGHT / max(math.sin(th), 1e-6) + for side, sgn in (("Left", +1.0), ("Right", -1.0)): + pos = TARGET + Gf.Vec3d(0.0, sgn * standoff * math.cos(th), + standoff * math.sin(th)) + M, _, _ = _look_at(pos, TARGET) + _place(stage, f"{rig}_{side}", M) + else: + az, el = math.radians(spec[0]), math.radians(spec[1]) + d = Gf.Vec3d(math.cos(az) * math.cos(el), math.sin(az) * math.cos(el), math.sin(el)) + # a third element pins this rig's own distance: depth cost is Z^2/(fx*B), + # so rigs with different baselines need different distances to reach the + # same mm-per-disparity-pixel. One shared distance always starves the + # narrowest baseline. + standoff = (spec[2] if len(spec) > 2 else HEIGHT / max(math.sin(el), 1e-6)) + centre = TARGET + d * standoff + M, xax, _ = _look_at(centre, TARGET) + for side, off in (("Left", -b / 2.0), ("Right", +b / 2.0)): + Mi = Gf.Matrix4d(M) + Mi.SetTranslateOnly(centre + xax * off) + _place(stage, f"{rig}_{side}", Mi) + + xc = UsdGeom.XformCache() + for rig in cfg: + for side in ("Left", "Right"): + n = f"{rig}_{side}" + prim = _cam(stage, n) + c = UsdGeom.Camera(prim) + ha = c.GetHorizontalApertureAttr().Get() + c.CreateVerticalApertureAttr().Set(ha * H / W) # square pixels + fl = c.GetFocalLengthAttr().Get() + M = xc.GetLocalToWorldTransform(prim) + out[n] = dict( + fx=fl / ha * W, fy=fl / (ha * H / W) * H, cx=W / 2.0, cy=H / 2.0, + width=W, height=H, baseline=BASELINE[rig], rig=rig, side=side, + M=[[M[r][col] for col in range(4)] for r in range(4)], + pos=[M.ExtractTranslation()[i] for i in range(3)], + height_mm=round((M.ExtractTranslation()[2] - TARGET[2]) * 1000, 1), + standoff_mm=round((M.ExtractTranslation() - TARGET).GetLength() * 1000, 1)) + return out diff --git a/control_test/capture_bg.py b/control_test/capture_bg.py new file mode 100644 index 0000000..87fc631 --- /dev/null +++ b/control_test/capture_bg.py @@ -0,0 +1,60 @@ +"""Background plate: the same cameras, the same lighting, no object. + +In simulation this gives an EXACT object mask by image difference, which beats every +height-threshold heuristic - and the height threshold is precisely what failed: CREStereo +puts the belt a few mm above belt_z, so a z-crop returns a belt patch whose footprint is +the crop window (measured 1700x1550x64 mm for every item, i.e. the crop, not the object). +""" +import json, os, sys +REPO = "/home/dasha/robozon-sorter" +for e in (REPO, f"{REPO}/control_test"): + if e not in sys.path: + sys.path.insert(0, e) +for _m in [k for k in list(sys.modules) if k.startswith("cam_configs")]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import asyncio +import omni.usd, omni.timeline +import omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from pxr import UsdGeom + +import cam_configs as CC + +CFG = globals().get("cfg", CC.DEFAULT) +OUT = f"{REPO}/control_test/captures/{CFG}" +os.makedirs(OUT, exist_ok=True) + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +calib = CC.apply_config(stage, CFG) +root = stage.GetPrimAtPath("/World/CapItems") +if root.IsValid(): + for c in root.GetChildren(): + UsdGeom.Imageable(c).MakeInvisible() +await app_utils.update_app_async(steps=25) + +w = vp.get_active_viewport() +orig = w.camera_path +bg = {} +for cam_name in calib: + w.camera_path = CC._cam(stage, cam_name).GetPath() + await app_utils.update_app_async(steps=22) + await asyncio.sleep(0) + f = f"{OUT}/__background__{cam_name}.png" + vp.capture_viewport_to_file(w, file_path=f) + await app_utils.update_app_async(steps=12) + await asyncio.sleep(0) + bg[cam_name] = f +w.camera_path = orig +await app_utils.update_app_async(steps=10) + +man_path = f"{OUT}/manifest.json" +man = json.load(open(man_path)) +man["background"] = bg +json.dump(man, open(man_path, "w"), indent=1) +print(f"background plate: {len(bg)} views -> {man_path}") diff --git a/control_test/capture_cfg.py b/control_test/capture_cfg.py new file mode 100644 index 0000000..2f7189d --- /dev/null +++ b/control_test/capture_cfg.py @@ -0,0 +1,112 @@ +"""SUPERSEDED - kept for history only. + +SUPERSEDED by capture_roi.py. This version composes the item reference +onto the prim whose xformOpOrder it then clears, which destroys the mesh's own +placement and buries every item 0.6 m under the belt, and it never clears the +authored visibility=invisible the item layers carry. Both failures are silent: +the captures look like a normal empty belt. Do not use. +""" + +"""STAGE 1 (inside Isaac, no torch): render a rectified L/R pair from every rig for every +item, for one named camera configuration. + +Items are parked kinematic at the inspection point so the pair is perfectly consistent - +a moving object between the two eyes would fake a disparity that is not there. +""" +import json, os, sys +REPO = "/home/dasha/robozon-sorter" +for e in (REPO, f"{REPO}/control_test"): + if e not in sys.path: + sys.path.insert(0, e) +for _m in [k for k in list(sys.modules) if k.startswith(("cam_configs", "classify", "cell"))]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import asyncio +import omni.usd, omni.timeline +import omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdLux, UsdPhysics + +import cam_configs as CC +import classify as CL + +CFG = globals().get("cfg", "A_original") +ITEMS = globals().get("items", ["bag", "backpack", "lunchbox", "helmet", "pillow", + "detergent", "bucket", "box_400x400x300", "box_300x200x200"]) +OUT = f"/home/dasha/robozon-sorter/control_test/captures/{CFG}" +os.makedirs(OUT, exist_ok=True) + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +calib = CC.apply_config(stage, CFG) +print(f"config {CFG}: {len(calib)} cameras placed at {CC.STANDOFF*1000:.0f} mm") + +# side views look at shadowed faces, so light the cell from several directions - the +# earlier study found texture matters far more than brightness, but a black frame has +# neither +for i, (nm, pos) in enumerate((("K0", (-0.75, 1.6, 2.6)), ("K1", (-0.75, -1.6, 2.6)), + ("K2", (0.6, 0.0, 2.6)), ("K3", (-2.1, 0.0, 2.6)))): + p = f"/World/_CapLight_{nm}" + if not stage.GetPrimAtPath(p).IsValid(): + sl = UsdLux.SphereLight.Define(stage, p) + sl.CreateRadiusAttr().Set(0.25) + sl.CreateIntensityAttr().Set(90000.0) + UsdGeom.Xformable(sl.GetPrim()).AddTranslateOp().Set(Gf.Vec3d(*pos)) +dome = stage.GetPrimAtPath("/Environment/_BrightFill") +if dome.IsValid(): + dome.GetAttribute("inputs:intensity").Set(3500.0) + +lib = {r["name"]: r for r in CL.load_library(f"{REPO}/control_test/items") if "error" not in r} +ROOT = "/World/CapItems" +UsdGeom.Xform.Define(stage, ROOT) + +def spawn(name): + path = f"{ROOT}/{name}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + prim = UsdGeom.Xform.Define(stage, path).GetPrim() + prim.GetReferences().AddReference(lib[name]["path"]) + # sit it ON the belt at the inspection point + bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + r = bb.ComputeWorldBound(prim).ComputeAlignedRange() + drop = r.GetMin()[2] + xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set( + Gf.Vec3d(CC.TARGET[0], CC.TARGET[1], CC.TARGET[2] - drop + 0.001)) + UsdGeom.Imageable(prim).MakeVisible() + return prim + +w = vp.get_active_viewport() +orig_cam = w.camera_path +manifest = {"config": CFG, "target": [float(v) for v in CC.TARGET], + "standoff_m": CC.STANDOFF, "calib": calib, "items": {}} + +for name in ITEMS: + if name not in lib: + print(f" skip {name} (not in library)"); continue + prim = spawn(name) + await app_utils.update_app_async(steps=20) + files = {} + for cam_name in calib: + w.camera_path = f"/RigRS/{cam_name}" if stage.GetPrimAtPath(f"/RigRS/{cam_name}").IsValid() \ + else CC._cam(stage, cam_name).GetPath() + await app_utils.update_app_async(steps=22) + await asyncio.sleep(0) + f = f"{OUT}/{name}__{cam_name}.png" + vp.capture_viewport_to_file(w, file_path=f) + await app_utils.update_app_async(steps=12) + await asyncio.sleep(0) + files[cam_name] = f + manifest["items"][name] = dict(files=files, gt=dict( + cls=lib[name]["cls"], dims_mm=lib[name]["dims_mm"], k=lib[name]["k"])) + print(f" {name}: {len(files)} views") + UsdGeom.Imageable(prim).MakeInvisible() + +w.camera_path = orig_cam +await app_utils.update_app_async(steps=10) +json.dump(manifest, open(f"{OUT}/manifest.json", "w"), indent=1) +print(f"\nmanifest -> {OUT}/manifest.json") diff --git a/control_test/capture_roi.py b/control_test/capture_roi.py new file mode 100644 index 0000000..aded671 --- /dev/null +++ b/control_test/capture_roi.py @@ -0,0 +1,207 @@ +"""STAGE 1 (inside Isaac, no torch): render rectified L/R pairs + an EXACT object mask. + +Two fixes over capture_cfg.py: + * the reference used to be composed onto the same prim whose xformOpOrder we then cleared, + which destroyed the mesh's own placement and dropped every item 0.6 m under the belt. + The reference now lives on a child, so only our holder carries the placement. + * ground truth is the mesh's real bbox in the scene, not the product catalogue - the two + disagree by 2.0-2.8x per item, so catalogue dims cannot score a size prediction. + +The mask is analytic (mesh points projected through the camera), the same trick the earlier +ROI pipeline used for its GT channel - no segmentation error contaminates a geometry study. +""" +import json, os, sys +REPO = "/home/dasha/robozon-sorter" +for e in (REPO, f"{REPO}/control_test"): + if e not in sys.path: + sys.path.insert(0, e) +for _m in [k for k in list(sys.modules) if k.startswith(("cam_configs", "classify", "cell"))]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import asyncio, numpy as np, cv2 +import omni.usd, omni.timeline +import omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from pxr import UsdPhysics, Gf, Usd, UsdGeom, UsdLux + +import cam_configs as CC +import cell +import classify as CL + +try: # --file runs isolated, so an injected arg lands in + CFG = cfg # LOCALS, not globals() - the bare name catches both +except NameError: + CFG = CC.DEFAULT +ITEMS = globals().get("items", ["bag", "backpack", "lunchbox", "helmet", "pillow", + "detergent", "bucket", "box_400x400x300", "box_300x200x200"]) +OUT = f"{REPO}/control_test/captures/{CFG}" +os.makedirs(OUT, exist_ok=True) + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +calib = CC.apply_config(stage, CFG) +print(f"config {CFG}: {len(calib)} камер | " + ", ".join( + f"{n.replace('_Left','')} h={c['height_mm']:.0f} d={c['standoff_mm']:.0f}" + for n, c in calib.items() if n.endswith("_Left"))) + +# without this the cell has no floor and no dome: everything off the belt renders as +# void, which both looks wrong over WebRTC and starves the side views of bounce light +_gl = cell.add_ground_and_light(stage) +print(f"пол {_gl['ground']}, купол {_gl['light']}") + +for nm, pos in (("K0", (-0.75, 1.6, 2.6)), ("K1", (-0.75, -1.6, 2.6)), + ("K2", (0.6, 0.0, 2.6)), ("K3", (-2.1, 0.0, 2.6))): + p = f"/World/_CapLight_{nm}" + if not stage.GetPrimAtPath(p).IsValid(): + sl = UsdLux.SphereLight.Define(stage, p) + sl.CreateRadiusAttr().Set(0.25); sl.CreateIntensityAttr().Set(90000.0) + UsdGeom.Xformable(sl.GetPrim()).AddTranslateOp().Set(Gf.Vec3d(*pos)) +dome = stage.GetPrimAtPath("/Environment/_BrightFill") +if dome.IsValid(): + dome.GetAttribute("inputs:intensity").Set(3500.0) + +lib = {r["name"]: r for r in CL.load_library(f"{REPO}/control_test/items") if "error" not in r} +ROOT = "/World/CapItems2" +UsdGeom.Xform.Define(stage, ROOT) +BB = lambda: UsdGeom.BBoxCache(Usd.TimeCode.Default(), + [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +async def spawn(name): + """holder Xform carries the placement; the reference sits on a child so its own + transform survives.""" + path = f"{ROOT}/{name}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + holder = UsdGeom.Xform.Define(stage, path).GetPrim() + inner = UsdGeom.Xform.Define(stage, f"{path}/mesh").GetPrim() + inner.GetReferences().AddReference(lib[name]["path"]) + r = None + for _ in range(12): # a reference does not compose within the tick + await app_utils.update_app_async(steps=2) + r = BB().ComputeWorldBound(inner).ComputeAlignedRange() + if not r.IsEmpty(): + break + if r is None or r.IsEmpty(): + raise RuntimeError(f"{name}: пустой bbox - ссылка не разрешилась") + mn, mx = r.GetMin(), r.GetMax() + UsdGeom.Xformable(holder).AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set( + Gf.Vec3d(CC.TARGET[0] - (mn[0] + mx[0]) / 2.0, + CC.TARGET[1] - (mn[1] + mx[1]) / 2.0, + CC.TARGET[2] - mn[2] + 0.001)) + # the exported item layers author visibility=invisible on their own root, and + # MakeVisible on an ancestor does NOT clear a descendant's authored value - that is + # why every earlier capture showed bare belt + for d in Usd.PrimRange(holder): + if d.IsA(UsdGeom.Imageable): + UsdGeom.Imageable(d).GetVisibilityAttr().Set(UsdGeom.Tokens.inherited) + UsdGeom.Imageable(holder).MakeVisible() + r2 = BB().ComputeWorldBound(inner).ComputeAlignedRange() + ext = [(r2.GetMax()[i] - r2.GetMin()[i]) * 1000.0 for i in range(3)] + return holder, ext, [r2.GetMin()[i] for i in range(3)], [r2.GetMax()[i] for i in range(3)] + +def world_geom(prim): + """every mesh of the item in world space, as vertices + triangles. Vertices alone are + not enough: a box has eight of them, so a point-splat mask covers ~60 px and the ROI + collapses. Filling the projected triangles gives the true silhouette.""" + xc = UsdGeom.XformCache(Usd.TimeCode.Default()); V = []; T = []; base = 0 + for d in Usd.PrimRange(prim): + if not d.IsA(UsdGeom.Mesh): + continue + m = UsdGeom.Mesh(d) + pts = m.GetPointsAttr().Get() + if not pts: + continue + M = np.array(xc.GetLocalToWorldTransform(d), dtype=np.float64) + P = np.asarray(pts, dtype=np.float64) + V.append((np.c_[P, np.ones(len(P))] @ M)[:, :3]) + cnt = m.GetFaceVertexCountsAttr().Get() or [] + idx = m.GetFaceVertexIndicesAttr().Get() or [] + o = 0 + for c in cnt: # fan-triangulate each polygon + for k in range(1, c - 1): + T.append((base + idx[o], base + idx[o + k], base + idx[o + k + 1])) + o += c + base += len(P) + if not V: + return np.zeros((0, 3)), np.zeros((0, 3), int) + return np.concatenate(V, 0), np.asarray(T, dtype=np.int64).reshape(-1, 3) + + +def gt_mask(V, T, cam): + W, H = cam["width"], cam["height"] + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.c_[V, np.ones(len(V))] @ Minv)[:, :3] + z = -c[:, 2] + u = c[:, 0] / np.maximum(z, 1e-9) * cam["fx"] + cam["cx"] + v = -c[:, 1] / np.maximum(z, 1e-9) * cam["fy"] + cam["cy"] + uv = np.c_[u, v] + m = np.zeros((H, W), np.uint8) + if len(T): + good = (z[T] > 1e-3).all(1) + tri = uv[T[good]].astype(np.int32) + tri = np.clip(tri, [-4 * W, -4 * H], [4 * W, 4 * H]) + cv2.fillPoly(m, list(tri), 1) + else: + ok = (z > 1e-3) & (u >= 0) & (u < W) & (v >= 0) & (v < H) + m[v[ok].astype(int), u[ok].astype(int)] = 1 + m = cv2.morphologyEx(m, cv2.MORPH_CLOSE, np.ones((5, 5), np.uint8)) + n, lab, st, _ = cv2.connectedComponentsWithStats(m) + if n > 1: + m = (lab == (1 + np.argmax(st[1:, cv2.CC_STAT_AREA]))).astype(np.uint8) + return m.astype(bool) + + +w = vp.get_active_viewport(); orig_cam = w.camera_path +manifest = {"config": CFG, "target": [float(v) for v in CC.TARGET], + "standoff_m": CC.STANDOFF, "calib": calib, "items": {}} + +for name in ITEMS: + if name not in lib: + print(f" пропуск {name} (нет в библиотеке)"); continue + prim, ext, bmin, bmax = await spawn(name) + await app_utils.update_app_async(steps=20) + VV, TT = world_geom(prim) + files, masks, cover, seen = {}, {}, {}, {} + for cam_name, cam in calib.items(): + w.camera_path = f"/RigRS/{cam_name}" if stage.GetPrimAtPath(f"/RigRS/{cam_name}").IsValid() \ + else CC._cam(stage, cam_name).GetPath() + await app_utils.update_app_async(steps=22); await asyncio.sleep(0) + f = f"{OUT}/{name}__{cam_name}.png" + vp.capture_viewport_to_file(w, file_path=f) + await app_utils.update_app_async(steps=12); await asyncio.sleep(0) + files[cam_name] = f + mk = gt_mask(VV, TT, cam) + _img = cv2.imread(f) + if _img is not None and _img.shape[:2] == mk.shape: + _g = cv2.cvtColor(_img, cv2.COLOR_BGR2GRAY).astype(float) + _ring = cv2.dilate(mk.astype(np.uint8), np.ones((41, 41), np.uint8)).astype(bool) & ~mk + seen[cam_name] = round(float(abs(_g[mk].mean() - _g[_ring].mean())), 1) + np.savez_compressed(f"{OUT}/{name}__{cam_name}_mask.npz", m=mk) + masks[cam_name] = f"{OUT}/{name}__{cam_name}_mask.npz" + cover[cam_name] = int(mk.sum()) + manifest["items"][name] = dict( + files=files, masks=masks, mask_px=cover, nverts=int(len(VV)), ntris=int(len(TT)), + contrast=seen, gt_catalogue=lib[name]["dims_mm"], cls=lib[name]["cls"], + gt_scene_mm=[round(e, 1) for e in ext], bmin=bmin, bmax=bmax) + print(f" {name}: в сцене {[round(e) for e in sorted(ext, reverse=True)]} мм " + f"маска {min(cover.values())}-{max(cover.values())} px, контраст {min(seen.values()):.0f}-{max(seen.values()):.0f}" + + (" <-- НЕ ВИДЕН" if min(seen.values()) < 3 else "")) + # Спрятать МАЛО: невидимость не убирает коллайдер, и снятый товар остаётся твёрдой + # стеной ровно в точке осмотра, посреди рабочей линии. После двух прогонов там стояло + # 18 невидимых предметов, и поток вставал на них, не доезжая до плуга. + UsdGeom.Imageable(prim).MakeInvisible() + for _d in Usd.PrimRange(prim): + _a = _d.GetAttribute("physics:collisionEnabled") + if _a and _a.IsValid(): + _a.Set(False) + elif _d.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI(_d).CreateCollisionEnabledAttr().Set(False) + +w.camera_path = orig_cam +await app_utils.update_app_async(steps=10) +json.dump(manifest, open(f"{OUT}/manifest.json", "w"), indent=1) +print(f"\nmanifest -> {OUT}/manifest.json") diff --git a/control_test/cell.py b/control_test/cell.py new file mode 100644 index 0000000..87bdcc7 --- /dev/null +++ b/control_test/cell.py @@ -0,0 +1,766 @@ +"""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 + +import pathlib + +from pxr import Gf, PhysxSchema, Usd, UsdGeom, UsdLux, UsdPhysics, UsdShade + +# absolute imports: control_test/cell.py is loaded as a top-level module, not as part of +# the robozon_sorter package it was copied out of. robozon_sorter must be importable - +# see control_test/README.md ("Dependencies"). +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene +from robozon_sorter.sim.plow_cell import GRIP_MATERIAL, configure_plow, drive_belt + +SCENE = pathlib.Path(__file__).resolve().parent / "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 = 500.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 + + +CAPTURE_PARKS = ("/World/CapItems", "/World/CapItems2", "/World/_CapItems") + + +def clear_capture_parks(stage, parks=CAPTURE_PARKS): + """Снять коллизию с товаров, оставленных стендом захвата кадров в точке осмотра. + + capture_roi.py ставит очередной товар в точку осмотра (-0.750, 0.0, 1.781), снимает + его шестью камерами и в конце прячет вызовом MakeInvisible(). НЕВИДИМОСТЬ НЕ УБИРАЕТ + КОЛЛАЙДЕР: после двух прогонов захвата в сцене осталось 18 невидимых, но твёрдых + предметов (/World/CapItems и /World/CapItems2 по девять), все в одной точке на ленте. + + Симптом ровно тот, на который жалуются: товар идёт 1.00 м/с и встаёт "посреди + ConveyorTrack_02" - середина этой секции как раз x ~ -1.0, а стена стоит на -0.75. + Проба 60 мм в замере вставала на x = -0.667 и уползала вбок на y = -0.11, обтекая + невидимое препятствие. + + Коллизия снимается, а не удаляются премы: кадры в captures/ ссылаются на эти пути, + и стенд замера должен продолжать работать. + """ + off = [] + for root in parks: + r = stage.GetPrimAtPath(root) + if not r.IsValid(): + continue + for d in Usd.PrimRange(r): + a = d.GetAttribute("physics:collisionEnabled") + if a and a.IsValid(): + if a.Get() is not False: + a.Set(False); off.append(str(d.GetPath())) + elif d.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI(d).CreateCollisionEnabledAttr().Set(False) + off.append(str(d.GetPath())) + return off + + +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 + + + +WIDEN_PATH = "/World/_Widen" +LINE_CLEAR = 0.50 # required clear width between the guards, metres + + +def widen_line(stage, clear=LINE_CLEAR, speed=None): + """Widen the straight runs to `clear` between guards, without touching the belts. + + The conveyor asset's belt is 450 mm wide (rails ended up at y +-0.22), so a parcel + presented across an axis longer than that wedges between the guards and the whole + queue stops behind it - measured with catalogue-scale goods, where the first 455 mm + item jammed at x ~ -0.4 and the following eight piled up nose to tail. + + Rather than rescale the conveyor (its surface velocity is authored in LOCAL space and + a non-uniform Y scale would skew the drive direction - the same trap that made the + corner belt drop items), this bolts a driven strip along each edge at exactly the + belt's top height, bound to the SAME grip material and carrying the SAME world-space + velocity, then moves the guards out to the new edge. Friction and drive are unchanged + because they are literally the same material and the same velocity vector. + """ + v_belt = C.BELT_SPEED if speed is None else speed + grip = UsdShade.Material(_ensure_grip_material(stage)) + bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), + [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + if not stage.GetPrimAtPath(WIDEN_PATH).IsValid(): + UsdGeom.Xform.Define(stage, WIDEN_PATH) + xc = UsdGeom.XformCache() + made = [] + 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() + if (mx[0] - mn[0]) < (mx[1] - mn[1]): + continue # not an X-running straight segment + width = mx[1] - mn[1] + pad = (clear - width) / 2.0 + if pad <= 0.001: + continue + # the belt's drive direction in WORLD terms, whatever frame it was authored in + api = PhysxSchema.PhysxSurfaceVelocityAPI(prim) + vloc = api.GetSurfaceVelocityAttr().Get() if prim.HasAPI( + PhysxSchema.PhysxSurfaceVelocityAPI) else None + local = bool(api.GetSurfaceVelocityLocalSpaceAttr().Get()) if vloc else False + if vloc is None: + vw = Gf.Vec3f(-v_belt, 0.0, 0.0) + elif local: + M = xc.GetLocalToWorldTransform(prim) + d = M.TransformDir(Gf.Vec3d(vloc[0], vloc[1], vloc[2])) + n = d.GetLength() or 1.0 + vw = Gf.Vec3f(*[float(c) / n * v_belt for c in d]) + else: + vw = Gf.Vec3f(*[float(c) for c in vloc]) + safe = belt.replace("/", "_") + for side, y_edge, sgn in ((0, mn[1], -1.0), (1, mx[1], +1.0)): + path = f"{WIDEN_PATH}/{safe}_{side}" + if stage.GetPrimAtPath(path).IsValid(): + made.append(path) + continue + cube = UsdGeom.Cube.Define(stage, path) + cube.CreateSizeAttr().Set(1.0) + p = cube.GetPrim() + xf = UsdGeom.Xformable(p) + xf.AddTranslateOp().Set(Gf.Vec3d((mn[0] + mx[0]) / 2.0, + y_edge + sgn * pad / 2.0, + mx[2] - 0.02)) + xf.AddScaleOp().Set(Gf.Vec3f(mx[0] - mn[0], pad, 0.04)) + UsdPhysics.CollisionAPI.Apply(p) + UsdShade.MaterialBindingAPI.Apply(p).Bind( + grip, UsdShade.Tokens.weakerThanDescendants, "physics") + sv = PhysxSchema.PhysxSurfaceVelocityAPI.Apply(p) + sv.CreateSurfaceVelocityEnabledAttr().Set(True) + sv.CreateSurfaceVelocityLocalSpaceAttr().Set(False) + sv.CreateSurfaceAngularVelocityAttr().Set(Gf.Vec3f(0.0, 0.0, 0.0)) + sv.CreateSurfaceVelocityAttr().Set(vw) + UsdGeom.Imageable(p).MakeInvisible() + made.append(path) + # the guards were built off the old edge - rebuild them on the new one + rails = stage.GetPrimAtPath(RAIL_PATH) + if rails.IsValid(): + stage.RemovePrim(RAIL_PATH) + UsdGeom.Xform.Define(stage, RAIL_PATH) + 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() + if (mx[0] - mn[0]) < (mx[1] - mn[1]): + continue + cy = (mn[1] + mx[1]) / 2.0 + safe = belt.replace("/", "_") + for side, sgn in ((0, -1.0), (1, +1.0)): + path = f"{RAIL_PATH}/{safe}_{side}" + cube = UsdGeom.Cube.Define(stage, path) + cube.CreateSizeAttr().Set(1.0) + xf = UsdGeom.Xformable(cube.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d((mn[0] + mx[0]) / 2.0, + cy + sgn * clear / 2.0, + mx[2] + RAIL_HEIGHT / 2.0)) + xf.AddScaleOp().Set(Gf.Vec3f(mx[0] - mn[0] + 0.10, 0.02, RAIL_HEIGHT)) + UsdPhysics.CollisionAPI.Apply(cube.GetPrim()) + UsdGeom.Imageable(cube.GetPrim()).MakeInvisible() + return dict(strips=len(made), clear_mm=round(clear * 1000)) + + +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) + + +BRIDGE_PATH = "/World/_TransferBridge" +# The plow discharges class-B goods over ConveyorTrack_04's +Y edge (y = +0.45) while they +# are still at x -7.95..-7.32 (the blade's own span). ConveyorTrack_06 - the belt that +# takes them to container B - only starts at x = -8.00, and the authored transition plates +# sit UPSTREAM of the plow at x -7.39..-6.39 (they belong to the old layout). So between +# _04's edge and _06 there is simply no floor at the exact point the plow pushes goods +# across, and they drop through it. Measured: an item placed directly on _06 rides it and +# lands in container B at z=1.236, but the same item arriving via the plow ends up on the +# ground at z~0.00. +# +# This plate bridges that corner. Its top sits 3 mm BELOW the belt surface (1.778 vs +# 1.781) so it clears the plow arm, whose underside measured z=1.78 - a bridge flush with +# the belt would foul the blade. +BRIDGE_X0, BRIDGE_X1 = -8.06, -7.24 +BRIDGE_Y0, BRIDGE_Y1 = 0.40, 1.08 +BRIDGE_TOP_Z = 1.778 +BRIDGE_THICK = 0.03 + + +def add_transfer_bridge(stage, speed=None): + """floor the _04 -> _06 corner and drive it toward container B""" + speed = speed if speed is not None else C.BELT_SPEED + prim = stage.GetPrimAtPath(BRIDGE_PATH) + if not prim.IsValid(): + cube = UsdGeom.Cube.Define(stage, BRIDGE_PATH) + cube.CreateSizeAttr().Set(1.0) + xf = UsdGeom.Xformable(cube.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d((BRIDGE_X0 + BRIDGE_X1) / 2.0, + (BRIDGE_Y0 + BRIDGE_Y1) / 2.0, + BRIDGE_TOP_Z - BRIDGE_THICK / 2.0)) + xf.AddScaleOp().Set(Gf.Vec3f(BRIDGE_X1 - BRIDGE_X0, BRIDGE_Y1 - BRIDGE_Y0, BRIDGE_THICK)) + prim = cube.GetPrim() + UsdPhysics.CollisionAPI.Apply(prim) + UsdGeom.Imageable(prim).MakeInvisible() + # carry goods across it toward container B instead of letting them sit on a dead plate + drive_belt(stage, BRIDGE_PATH, (-0.846, 0.532, 0.0), speed) + grip = _ensure_grip_material(stage) + UsdShade.MaterialBindingAPI.Apply(prim).Bind( + UsdShade.Material(grip), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + return (BRIDGE_X0, BRIDGE_X1, BRIDGE_Y0, BRIDGE_Y1, BRIDGE_TOP_Z) + + +CATCHERS = { + # tray floor footprint -> its top z. Measured off the authored prims. + "/World/PlowContainers/B_Floor": None, + "/World/PlowContainers/C_Floor": None, + "/World/SortingRig/BinD_Floor": None, +} +CATCH_DEPTH = 0.30 + + +def add_container_catchers(stage): + """thicken the tray floors downward with an invisible slab. + + The authored floors are 40 mm thick. Goods arrive off the belt (z 1.781) and land on a + tray floor at z~1.18 - a 0.6 m drop, so ~3.4 m/s, which at the scene's step is ~57 mm + of travel per step against a 40 mm slab: the item can pass straight through between + two steps. Measured exactly that - class-B goods reached container B's footprint + (x -8.58..-9.05, y 1.07..1.40, all inside the tray) and then ended up on the ground at + z~0.00. A single item dropped gently onto the same floor in isolation was caught, which + is the signature of tunnelling rather than a missing collider. + + Deepening the collider (not the visible tray) means the item has several steps' worth + of solid to hit, so it cannot pass through. Purely additive: the slab sits BELOW each + existing floor, so nothing that already worked changes. + """ + bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), + [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + made = [] + for path in CATCHERS: + src = stage.GetPrimAtPath(path) + if not src.IsValid(): + continue + r = bbc.ComputeWorldBound(src).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + out = f"/World/_Catch{src.GetName()}" + if stage.GetPrimAtPath(out).IsValid(): + made.append(out) + continue + cube = UsdGeom.Cube.Define(stage, out) + cube.CreateSizeAttr().Set(1.0) + xf = UsdGeom.Xformable(cube.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d((mn[0] + mx[0]) / 2.0, (mn[1] + mx[1]) / 2.0, + mx[2] - CATCH_DEPTH / 2.0)) + xf.AddScaleOp().Set(Gf.Vec3f(mx[0] - mn[0], mx[1] - mn[1], CATCH_DEPTH)) + UsdPhysics.CollisionAPI.Apply(cube.GetPrim()) + UsdGeom.Imageable(cube.GetPrim()).MakeInvisible() + made.append(out) + return made + + +CORNER_BELT = "/World/ConveyorTrack_06/Belt" + + +def drive_corner_belt(stage, path=CORNER_BELT, speed=None): + """drive the 90-degree corner along the CHORD that stays on its arc. + + ConveyorBelt_A03 is curved: sampling the top surface gives a quarter-annulus centred on + (-8.005, 1.042) with radii 0.517..1.018 - not the rectangle its bounding box implies. + The original linear direction (-0.545, +0.839) was too +Y-heavy, so goods cut across + the hollow middle of the annulus and fell through: a traced class-B item dropped at + (-8.49, +0.88), which is r=0.511 from the centre - just inside r_in=0.517. + + PhysX's angular surface velocity would be the textbook answer, but it measured inert on + this body (goods crept at ~0.02 m/s in both local and world space), so the drive stays + linear and is instead AIMED so the straight chord never leaves the band. Goods enter at + (-8.05, 0.1), i.e. r=0.943; leaving at the same radius a quarter-turn round is + (-8.948, 1.042), giving direction (-0.69, 0.7238). That chord's midpoint sits at + r=0.683, comfortably inside 0.517..1.018 - the 0.5 m band is wide enough to + swallow the ~0.26 m a 90-degree chord deviates from its arc. + """ + speed = speed if speed is not None else C.BELT_SPEED + 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 space, bypassing drive_belt's world->local conversion. That conversion + # normalises the direction *in local space*, which does not preserve the world + # direction when the frame carries a non-uniform scale - and this belt does. The + # symptom was unmistakable: asking for (-0.69, +0.72) drove goods to y = -0.098, i.e. + # the wrong way across the line and into container C. + api = PhysxSchema.PhysxSurfaceVelocityAPI.Apply(prim) + api.CreateSurfaceVelocityEnabledAttr().Set(True) + api.CreateSurfaceVelocityLocalSpaceAttr().Set(False) + api.CreateSurfaceAngularVelocityAttr().Set(Gf.Vec3f(0.0, 0.0, 0.0)) + v = Gf.Vec3f(-0.69 * speed, 0.7238 * speed, 0.0) + api.CreateSurfaceVelocityAttr().Set(v) + grip = _ensure_grip_material(stage) + UsdShade.MaterialBindingAPI.Apply(prim).Bind( + UsdShade.Material(grip), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + return tuple(round(c, 3) for c in v) + + +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 + # ConveyorTrack_06 is a CURVED corner and is driven rotationally instead - see + # drive_corner_belt(). Driving it linearly walked goods off the arc. + # 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) + parked = await _retrying(clear_capture_parks, 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) + widened = await _retrying(widen_line, stage, LINE_CLEAR, belt_speed) + bridge = await _retrying(add_transfer_bridge, stage, belt_speed) + catchers = await _retrying(add_container_catchers, stage) + corner = await _retrying(drive_corner_belt, stage, CORNER_BELT, belt_speed) + opened = await _retrying(open_junction, stage) + return dict(script_control=script_control, plow_ready=plow, belts=belts, + graphs_removed=killed, parks_cleared=len(parked), env=env, pusher_dims=pusher_dims, rails=len(rails), + widened=widened, pusher_seat=seat, decks_regripped=regripped, bridge=bridge, catchers=len(catchers), corner_dir=corner, 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) diff --git a/control_test/classify.py b/control_test/classify.py new file mode 100644 index 0000000..4e5d397 --- /dev/null +++ b/control_test/classify.py @@ -0,0 +1,85 @@ +"""Item library for control_test: meshes are DISCOVERED in items/, classes are READ from +items/labels.json. + +No geometric auto-measurement. Dimensions and k are taken from the labelling, which is the +ground truth this cell is verified against - measuring them from the mesh was tried and +the roundness estimate under-read handled/hollow bodies (bucket 0.737 vs 0.995, mug 0.731 +vs 0.985), i.e. class D silently became B. Reading the label removes that whole class of +error from the mechanics test. + +The folder is still the source of items: drop a .usd in, add one line to labels.json, and +it joins the next run. Anything in the folder without a label is reported and skipped +rather than guessed at. + +The documented rules are kept in `classify()` so a labelling can be checked for internal +consistency (`verify_labels()`), not to derive it: + + D "не подходит без доупаковки" габариты как у B, но k > 0.8 хотя бы в одном сечении + C "не подходит по габаритам" любой размер < 10 мм ИЛИ не влезает в 450x320x320 мм. + Форма не важна. + B "подходит для сортировки" всё от 10x10x10 до 450x320x320 мм и k <= 0.8 + +Fit is tested with the item's extents and the envelope both sorted descending - a parcel +may be presented on any face. +""" +from __future__ import annotations + +import json +import pathlib + +ENVELOPE_MM = sorted((450.0, 320.0, 320.0), reverse=True) +MIN_DIM_MM = 10.0 +K_THRESHOLD = 0.8 + +LABELS_FILE = "labels.json" + + +def classify(dims_mm, k): + """the documented decision, D checked first""" + d = sorted(dims_mm, reverse=True) + undersize = min(d) < MIN_DIM_MM + fits = all(a <= b + 1e-9 for a, b in zip(d, ENVELOPE_MM)) + if undersize or not fits: + return "C" # shape irrelevant + return "D" if k > K_THRESHOLD else "B" + + +def load_library(items_dir): + """every .usd in items_dir, sorted, paired with its label. + + Returns dicts with name/path/cls/dims_mm/k, or name/path/error for meshes that have + no entry in labels.json - those are skipped by the runner, never guessed. + """ + items_dir = pathlib.Path(items_dir) + labels_path = items_dir / LABELS_FILE + if not labels_path.exists(): + raise FileNotFoundError(f"{labels_path} missing - the item classes live there") + labels = json.loads(labels_path.read_text()) + + out = [] + for f in sorted(items_dir.glob("*.usd")): + rec = labels.get(f.stem) + if rec is None: + out.append(dict(name=f.stem, path=str(f), + error="no entry in labels.json")) + continue + out.append(dict(name=f.stem, path=str(f), cls=rec["zone"], + dims_mm=rec.get("dims_mm"), k=rec.get("k"))) + return out + + +def verify_labels(items_dir): + """check each label against the documented rules; returns the rows that disagree. + + A label whose own dims/k imply a different class is a labelling bug, and it would + otherwise show up as a mysterious mechanical failure. + """ + bad = [] + for r in load_library(items_dir): + if "error" in r or r.get("dims_mm") is None or r.get("k") is None: + continue + implied = classify(r["dims_mm"], r["k"]) + if implied != r["cls"]: + bad.append(dict(name=r["name"], labelled=r["cls"], implied=implied, + dims_mm=r["dims_mm"], k=r["k"])) + return bad diff --git a/control_test/cv_worker.py b/control_test/cv_worker.py new file mode 100644 index 0000000..a2b4a41 --- /dev/null +++ b/control_test/cv_worker.py @@ -0,0 +1,119 @@ +"""Рабочий процесс замкнутого контура: слушает заявки от Isaac, отвечает классом товара. + +Зачем отдельный процесс. torch внутри Isaac роняет процесс, поэтому CV не может жить в +одном процессе со сценой. Обмен идёт через каталог: Isaac кладёт заявку с путями к шести +кадрам, работник отвечает файлом с габаритами, k и классом. + +Времени на это хватает с запасом. От ворот осмотра (x = -0.750) до пушера (x = -3.900) +товар при 1 м/с едет 3.15 с, до плуга (x = -7.85) - 7.1 с, а инференс на зафиксированном +бейзлайне занимает 469 мс. То есть класс успевает к обоим механизмам, и контур замыкается +по-настоящему, а не постфактум. + +БЕЙЗЛАЙН ЗАФИКСИРОВАН: DEFOM-Stereo vitl, вход сети 480, iters 24, кроп зоны осмотра, +без сегментации (товар отделяется превышением над плоскостью полотна + отсев по плотности). +На потоке 700 мм он дал классы 8/9 = 89 % и габариты MAE 32.8 мм. +""" +import os, sys, json, time, glob, traceback + +os.environ["IMPORT_ONLY"] = "1" # measure_plane импортируется как библиотека +CT = "/home/dasha/robozon-sorter/control_test" +sys.path.insert(0, CT) + +import numpy as np +import cv2 +import measure_plane as MP +import measure_flow as MF +import classify as CL + +RT = f"{CT}/runtime" +REQ, RES = f"{RT}/req", f"{RT}/res" +for d in (REQ, RES): + os.makedirs(d, exist_ok=True) + +calib = MF.calib +TARGET, BELT_Z, RIGS = MF.TARGET, MF.BELT_Z, MF.RIGS + + +def measure(files): + """шесть кадров -> габариты, k, класс. Тот же тракт, что в зафиксированном бейзлайне.""" + pairs, metas = [], [] + for rig in RIGS: + cam = calib[f"{rig}_Left"] + IL = cv2.imread(files[f"{rig}_Left"]) + IR = cv2.imread(files[f"{rig}_Right"]) + if IL is None or IR is None: + continue + x0, y0, x1, y1 = MP.gate_crop_px(cam) + maxd = int(np.ceil(MF.DPAD * cam["fx"] * cam["baseline"] / MF.ZMIN)) + x0 = max(0, x0 - maxd) + pairs.append((IL[y0:y1, x0:x1].astype(np.float32), IR[y0:y1, x0:x1].astype(np.float32))) + metas.append(((x0, y0, x1, y1), cam)) + if not pairs: + return None + disps = MP.cre_scaled(pairs, MP.SW) # движок выбирается внутри (бейзлайн: DEFOM) + clouds = [] + for d, (win, cam) in zip(disps, metas): + # cloud_from_roi отдаёт ПАРУ: облако товара и облако полотна. В первой версии я + # складывал кортеж целиком, и vstack падал на разнородных формах. + obj, _belt = MP.cloud_from_roi(d, win, cam) + if len(obj): + clouds.append(obj) + if not clouds: + return None + P = np.vstack(clouds) + sel = MP.dense_only(P) + if sel.sum() >= 60: + P = P[sel] + P = P[MP.biggest_blob_idx(P)] + out = MF.dims_and_k(P) + if out is None: + return None + dims, _ = out + k, is_round, sec = MP.circular_section_K(P) + cls = CL.classify(dims, 0.0 if not k else k) + return dict(dims=[round(v, 1) for v in dims], k=round(float(k), 3), + cls=cls, n=len(P)) + + +print(f"работник запущен: движок {MP.STEREO} {MP.DEFOM_CKPT}, вход {MP.SW}, " + f"iters {MP.DEFOM_ITERS}, кроп {MP.CROP}") +print(f"заявки: {REQ} ответы: {RES}") + +# прогрев, чтобы первый настоящий товар не ждал загрузку энкодера +try: + z = np.zeros((240, 480, 3), np.float32) + MP.cre_scaled([(z, z)], MP.SW) + print("прогрев выполнен") +except Exception as ex: + print("прогрев не удался:", ex) + +open(f"{RT}/worker_ready", "w").write(str(time.time())) +seen = set() +idle = 0.0 +while True: + reqs = sorted(glob.glob(f"{REQ}/*.json")) + if not reqs: + time.sleep(0.05); idle += 0.05 + if idle > 1800: + print("1800 с без заявок - выхожу"); break + continue + idle = 0.0 + for rq in reqs: + try: + j = json.load(open(rq)) + except Exception: + continue + name = j["name"] + t0 = time.time() + try: + r = measure(j["files"]) + except Exception: + traceback.print_exc(); r = None + dt = (time.time() - t0) * 1000 + ans = dict(name=name, ms=round(dt), ok=r is not None) + if r: + ans.update(r) + json.dump(ans, open(f"{RES}/{name}.json", "w"), ensure_ascii=False) + os.remove(rq) + print(f" {name}: {ans.get('cls','-')} dims={ans.get('dims')} k={ans.get('k')} " + f"за {dt:.0f} мс", flush=True) diff --git a/control_test/flow_capture.py b/control_test/flow_capture.py new file mode 100644 index 0000000..8da9730 --- /dev/null +++ b/control_test/flow_capture.py @@ -0,0 +1,194 @@ +"""STAGE 1 of the real-time flow bench (inside Isaac, no torch). + +Items ride the real belt at PITCH spacing, exactly like run_pipeline.py. When one +crosses the inspection gate the timeline is paused, all six cameras are rendered, and the +run continues. Nothing about the item is written into the capture except its frames - +class and size are what stage 2 has to predict. + +Meshes in items/ are 2.0-2.8x smaller than the catalogue dims they are labelled with, so +each is scaled up uniformly to catalogue scale first. Left small, every item fits the +450x320x320 envelope and class C becomes geometrically unreachable - the class metric +would be measuring nothing. Ground truth is the bbox actually in the scene after scaling. +""" +import asyncio, json, os, pathlib, sys, time +import numpy as np +import omni.timeline, omni.usd +import omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema +from isaacsim.core.experimental.prims import RigidPrim + +HERE = pathlib.Path("/home/dasha/robozon-sorter/control_test") +for extra in (str(HERE), "/home/dasha/robozon-sorter"): + if extra not in sys.path: + sys.path.insert(0, extra) +for _m in [k for k in list(sys.modules) + if k.startswith(("robozon_sorter", "cell", "classify", "cam_configs"))]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import cell +import cam_configs as CC +import classify as CL +from robozon_sorter import config as C + +try: PITCH = float(pitch) +except NameError: PITCH = 0.70 +try: SPEED = float(speed) +except NameError: SPEED = 1.0 +try: ITEMS = list(items) +except NameError: + ITEMS = ["bag", "backpack", "lunchbox", "helmet", "pillow", + "detergent", "bucket", "box_400x400x300", "box_300x200x200"] + +GATE_X = float(CC.TARGET[0]) # inspection point, items travel -X past it +OUT = str(HERE / "captures" / "flow") +os.makedirs(OUT, exist_ok=True) +ROOT = "/World/FlowItems" + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +info = await cell.prepare(stage, belt_speed=SPEED, script_control=True) +_gl = cell.add_ground_and_light(stage) +print(f"prepare: belts={len(info['belts'])} | пол {_gl['ground']}, купол {_gl['light']}") + +calib = CC.apply_config(stage, CC.DEFAULT) +print(f"камеры {CC.DEFAULT}: " + ", ".join( + f"{n.replace('_Left','')} h={c['height_mm']:.0f} d={c['standoff_mm']:.0f}" + for n, c in calib.items() if n.endswith("_Left"))) + +# extra fill so the side views do not sit in shadow; the belt cell itself is unlit metal +for nm, pos in (("K0", (-0.75, 1.6, 2.6)), ("K1", (-0.75, -1.6, 2.6)), + ("K2", (0.6, 0.0, 2.6)), ("K3", (-2.1, 0.0, 2.6))): + p = f"/World/_CapLight_{nm}" + if not stage.GetPrimAtPath(p).IsValid(): + from pxr import UsdLux + sl = UsdLux.SphereLight.Define(stage, p) + sl.CreateRadiusAttr().Set(0.25); sl.CreateIntensityAttr().Set(90000.0) + UsdGeom.Xformable(sl.GetPrim()).AddTranslateOp().Set(Gf.Vec3d(*pos)) + +lib = {r["name"]: r for r in CL.load_library(str(HERE / "items_flow")) if "error" not in r} +ITEMS = [n for n in ITEMS if n in lib] +BB = lambda: UsdGeom.BBoxCache(Usd.TimeCode.Default(), + [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +UsdGeom.Xform.Define(stage, ROOT) +GT, ORDER = {}, [] +for i, name in enumerate(ITEMS): + path = f"{ROOT}/{name}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + # items_flow/ meshes are already catalogue-scale and already seated on z=0 (see + # scale_items.py), so this is run_pipeline's proven load path verbatim: reference on + # the body prim, one translate op, nothing nested. + prim = UsdGeom.Xform.Define(stage, path).GetPrim() + prim.GetReferences().ClearReferences() + prim.GetReferences().AddReference(lib[name]["path"]) + xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set( + Gf.Vec3d(9.0 + 1.5 * i, 5.0, 0.4)) + UsdPhysics.RigidBodyAPI.Apply(prim) + 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.CreateSolverVelocityIterationCountAttr().Set(8) + px.CreateSleepThresholdAttr().Set(0.0) + px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) + for d in Usd.PrimRange(prim): + if d.HasAPI(UsdPhysics.CollisionAPI): + pc = PhysxSchema.PhysxCollisionAPI.Apply(d) + pc.CreateContactOffsetAttr().Set(0.004) + pc.CreateRestOffsetAttr().Set(0.001) + await app_utils.update_app_async(steps=4) + r = BB().ComputeWorldBound(prim).ComputeAlignedRange() + dims = sorted([(r.GetMax()[k] - r.GetMin()[k]) * 1000.0 for k in range(3)], reverse=True) + GT[name] = dict(dims_mm=[round(v, 1) for v in dims], k=lib[name]["k"], + zone_label=lib[name]["cls"], + zone_scene=CL.classify(dims, lib[name]["k"])) + UsdGeom.Imageable(prim).MakeInvisible() + ORDER.append(name) + print(f" {name:18s} {[round(v) for v in dims]} мм метка {lib[name]['cls']}, " + f"по геометрии сцены {GT[name]['zone_scene']}") + +view = RigidPrim(paths=[f"{ROOT}/{n}" for n in ORDER]) +plow_home = None +try: + from robozon_sorter.sim.plow import Plow + plow = Plow(stage, kinematic=True); plow.home() +except BaseException: + pass + +w = vp.get_active_viewport(); orig_cam = w.camera_path +CAMS = list(calib.keys()) +manifest = dict(config=CC.DEFAULT, target=[float(v) for v in CC.TARGET], + pitch_m=PITCH, speed_mps=SPEED, calib=calib, items={}) + +def activate(name): + prim = stage.GetPrimAtPath(f"{ROOT}/{name}") + for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + op.Set(Gf.Vec3d(cell.ENTRY_X, cell.ENTRY_Y, C.BELT_Z + 0.02)); break + UsdGeom.Imageable(prim).MakeVisible() + +async def shoot(name, x_at): + tl.pause() + for _ in range(3): + await app_utils.update_app_async(steps=2) + files = {} + for cam in CAMS: + w.camera_path = (f"/RigRS/{cam}" if stage.GetPrimAtPath(f"/RigRS/{cam}").IsValid() + else CC._cam(stage, cam).GetPath()) + await app_utils.update_app_async(steps=18); await asyncio.sleep(0) + f = f"{OUT}/{name}__{cam}.png" + vp.capture_viewport_to_file(w, file_path=f) + await app_utils.update_app_async(steps=10); await asyncio.sleep(0) + files[cam] = f + manifest["items"][name] = dict(files=files, x_at=round(float(x_at), 4), gt=GT[name]) + tl.play() + print(f" снят {name} на x={x_at:+.3f}") + +tl.play() +await app_utils.update_app_async(steps=10) +print(f"\n===== ПОТОК: {len(ORDER)} товаров, шаг {PITCH*1000:.0f} мм @ {SPEED} м/с " + f"(интервал {PITCH/SPEED:.2f} с) =====") + +released, shot, prev_x = [], set(), {} +t_next = float(tl.get_current_time()) +idx = 0 +t_end = t_next + (len(ORDER) + 1) * PITCH / SPEED + 25.0 +while float(tl.get_current_time()) < t_end and len(shot) < len(ORDER): + now = float(tl.get_current_time()) + if idx < len(ORDER) and now >= t_next: + activate(ORDER[idx]); released.append(ORDER[idx]) + print(f" {now - (t_end - (len(ORDER)+1)*PITCH/SPEED - 25.0):6.2f}с выпущен {ORDER[idx]}") + idx += 1; t_next = now + PITCH / SPEED + try: + pos = view.get_world_poses()[0].numpy() + except BaseException: + pos = None + if pos is not None: + for j, n in enumerate(ORDER): + if n in shot or n not in released: + continue + x = float(pos[j][0]) + if prev_x.get(n, 9e9) > GATE_X >= x and \ + abs(float(pos[j][2]) - C.BELT_Z) < 0.5: + shot.add(n) + await shoot(n, x) + prev_x[n] = x + await app_utils.update_app_async(steps=2) + await asyncio.sleep(0) + +tl.stop() +w.camera_path = orig_cam +await app_utils.update_app_async(steps=10) +json.dump(manifest, open(f"{OUT}/manifest.json", "w"), indent=1) +print(f"\nснято {len(manifest['items'])}/{len(ORDER)} -> {OUT}/manifest.json") +missed = [n for n in ORDER if n not in manifest["items"]] +if missed: + print("не прошли ворота:", ", ".join(missed)) diff --git a/control_test/items/air_conditioner.usd b/control_test/items/air_conditioner.usd new file mode 100644 index 0000000..ee85068 Binary files /dev/null and b/control_test/items/air_conditioner.usd differ diff --git a/control_test/items/backpack.usd b/control_test/items/backpack.usd new file mode 100644 index 0000000..0f7d68a Binary files /dev/null and b/control_test/items/backpack.usd differ diff --git a/control_test/items/bag.usd b/control_test/items/bag.usd new file mode 100644 index 0000000..1a6ffc8 Binary files /dev/null and b/control_test/items/bag.usd differ diff --git a/control_test/items/banana.usd b/control_test/items/banana.usd new file mode 100644 index 0000000..7aeffd8 Binary files /dev/null and b/control_test/items/banana.usd differ diff --git a/control_test/items/bolts_cluster.usd b/control_test/items/bolts_cluster.usd new file mode 100644 index 0000000..5533e38 Binary files /dev/null and b/control_test/items/bolts_cluster.usd differ diff --git a/control_test/items/bottle.usd b/control_test/items/bottle.usd new file mode 100644 index 0000000..ac10b1f Binary files /dev/null and b/control_test/items/bottle.usd differ diff --git a/control_test/items/box_300x200x200.usd b/control_test/items/box_300x200x200.usd new file mode 100644 index 0000000..df89acc Binary files /dev/null and b/control_test/items/box_300x200x200.usd differ diff --git a/control_test/items/box_400x400x300.usd b/control_test/items/box_400x400x300.usd new file mode 100644 index 0000000..173554e Binary files /dev/null and b/control_test/items/box_400x400x300.usd differ diff --git a/control_test/items/briefcase_hard.usd b/control_test/items/briefcase_hard.usd new file mode 100644 index 0000000..5a72d51 Binary files /dev/null and b/control_test/items/briefcase_hard.usd differ diff --git a/control_test/items/bucket.usd b/control_test/items/bucket.usd new file mode 100644 index 0000000..de9fd09 Binary files /dev/null and b/control_test/items/bucket.usd differ diff --git a/control_test/items/carton_large.usd b/control_test/items/carton_large.usd new file mode 100644 index 0000000..5533f35 Binary files /dev/null and b/control_test/items/carton_large.usd differ diff --git a/control_test/items/chip_bag.usd b/control_test/items/chip_bag.usd new file mode 100644 index 0000000..04a2e08 Binary files /dev/null and b/control_test/items/chip_bag.usd differ diff --git a/control_test/items/cleat_small.usd b/control_test/items/cleat_small.usd new file mode 100644 index 0000000..4ecdd1c Binary files /dev/null and b/control_test/items/cleat_small.usd differ diff --git a/control_test/items/clothespin_flat.usd b/control_test/items/clothespin_flat.usd new file mode 100644 index 0000000..8e21f5c Binary files /dev/null and b/control_test/items/clothespin_flat.usd differ diff --git a/control_test/items/cone.usd b/control_test/items/cone.usd new file mode 100644 index 0000000..613660a Binary files /dev/null and b/control_test/items/cone.usd differ diff --git a/control_test/items/cooler_cube.usd b/control_test/items/cooler_cube.usd new file mode 100644 index 0000000..9ef511a Binary files /dev/null and b/control_test/items/cooler_cube.usd differ diff --git a/control_test/items/cylinder.usd b/control_test/items/cylinder.usd new file mode 100644 index 0000000..f7128a3 Binary files /dev/null and b/control_test/items/cylinder.usd differ diff --git a/control_test/items/detergent.usd b/control_test/items/detergent.usd new file mode 100644 index 0000000..d76ecc1 Binary files /dev/null and b/control_test/items/detergent.usd differ diff --git a/control_test/items/duffel_round.usd b/control_test/items/duffel_round.usd new file mode 100644 index 0000000..4833831 Binary files /dev/null and b/control_test/items/duffel_round.usd differ diff --git a/control_test/items/headphones.usd b/control_test/items/headphones.usd new file mode 100644 index 0000000..a08bd0b Binary files /dev/null and b/control_test/items/headphones.usd differ diff --git a/control_test/items/helmet.usd b/control_test/items/helmet.usd new file mode 100644 index 0000000..f9da2f8 Binary files /dev/null and b/control_test/items/helmet.usd differ diff --git a/control_test/items/labels.json b/control_test/items/labels.json new file mode 100644 index 0000000..35e7d65 --- /dev/null +++ b/control_test/items/labels.json @@ -0,0 +1,227 @@ +{ + "backpack": { + "dims_mm": [ + 454.7, + 370.3, + 300.9 + ], + "k": 0.82, + "zone": "C" + }, + "bag": { + "dims_mm": [ + 201.7, + 175.3, + 170.3 + ], + "k": 0.896, + "zone": "D" + }, + "banana": { + "dims_mm": [ + 182.6, + 70.6, + 33.0 + ], + "k": 0.94, + "zone": "D" + }, + "bolts_cluster": { + "dims_mm": [ + 193.6, + 135.6, + 52.6 + ], + "k": 0.718, + "zone": "B" + }, + "bottle": { + "dims_mm": [ + 304.8, + 91.0, + 91.0 + ], + "k": 0.995, + "zone": "D" + }, + "box_300x200x200": { + "dims_mm": [ + 301.0, + 200.5, + 200.0 + ], + "k": 0.72, + "zone": "B" + }, + "box_400x400x300": { + "dims_mm": [ + 401.0, + 400.0, + 300.5 + ], + "k": 0.716, + "zone": "C" + }, + "bucket": { + "dims_mm": [ + 287.4, + 287.4, + 272.3 + ], + "k": 0.995, + "zone": "D" + }, + "chip_bag": { + "dims_mm": [ + 250.0, + 162.2, + 69.1 + ], + "k": 0.811, + "zone": "D" + }, + "cone": { + "dims_mm": [ + 500.0, + 350.5, + 350.5 + ], + "k": 0.991, + "zone": "C" + }, + "cylinder": { + "dims_mm": [ + 434.9, + 50.0, + 43.0 + ], + "k": 0.867, + "zone": "D" + }, + "detergent": { + "dims_mm": [ + 278.2, + 259.9, + 179.8 + ], + "k": 0.742, + "zone": "B" + }, + "headphones": { + "dims_mm": [ + 198.4, + 194.9, + 93.3 + ], + "k": 0.807, + "zone": "D" + }, + "helmet": { + "dims_mm": [ + 353.5, + 297.1, + 279.9 + ], + "k": 0.895, + "zone": "D" + }, + "lunchbox": { + "dims_mm": [ + 201.1, + 152.4, + 62.3 + ], + "k": 0.646, + "zone": "B" + }, + "mug": { + "dims_mm": [ + 112.9, + 99.0, + 83.2 + ], + "k": 0.985, + "zone": "D" + }, + "parcel_box": { + "dims_mm": [ + 344.4, + 155.1, + 143.7 + ], + "k": 0.699, + "zone": "B" + }, + "pen": { + "dims_mm": [ + 148.5, + 13.1, + 9.0 + ], + "k": 0.842, + "zone": "C" + }, + "perfume": { + "dims_mm": [ + 120.0, + 53.1, + 53.1 + ], + "k": 0.924, + "zone": "D" + }, + "pillow": { + "dims_mm": [ + 455.1, + 430.6, + 212.7 + ], + "k": 0.905, + "zone": "C" + }, + "plate": { + "dims_mm": [ + 209.4, + 209.4, + 26.6 + ], + "k": 0.998, + "zone": "D" + }, + "pouf": { + "dims_mm": [ + 488.9, + 488.9, + 264.0 + ], + "k": 0.994, + "zone": "C" + }, + "sneaker": { + "dims_mm": [ + 270.4, + 208.0, + 125.4 + ], + "k": 0.706, + "zone": "B" + }, + "tool_case": { + "dims_mm": [ + 300.0, + 143.5, + 60.0 + ], + "k": 0.454, + "zone": "B" + }, + "watch": { + "dims_mm": [ + 230.0, + 230.0, + 4.6 + ], + "k": 0.995, + "zone": "C" + } +} \ No newline at end of file diff --git a/control_test/items/lunchbox.usd b/control_test/items/lunchbox.usd new file mode 100644 index 0000000..a9bb60f Binary files /dev/null and b/control_test/items/lunchbox.usd differ diff --git a/control_test/items/mug.usd b/control_test/items/mug.usd new file mode 100644 index 0000000..699fcad Binary files /dev/null and b/control_test/items/mug.usd differ diff --git a/control_test/items/nailfile_mini.usd b/control_test/items/nailfile_mini.usd new file mode 100644 index 0000000..90e1778 Binary files /dev/null and b/control_test/items/nailfile_mini.usd differ diff --git a/control_test/items/parcel_box.usd b/control_test/items/parcel_box.usd new file mode 100644 index 0000000..4c60b97 Binary files /dev/null and b/control_test/items/parcel_box.usd differ diff --git a/control_test/items/pen.usd b/control_test/items/pen.usd new file mode 100644 index 0000000..4196138 Binary files /dev/null and b/control_test/items/pen.usd differ diff --git a/control_test/items/perfume.usd b/control_test/items/perfume.usd new file mode 100644 index 0000000..f3df47f Binary files /dev/null and b/control_test/items/perfume.usd differ diff --git a/control_test/items/pillow.usd b/control_test/items/pillow.usd new file mode 100644 index 0000000..61fea87 Binary files /dev/null and b/control_test/items/pillow.usd differ diff --git a/control_test/items/plate.usd b/control_test/items/plate.usd new file mode 100644 index 0000000..6e42972 Binary files /dev/null and b/control_test/items/plate.usd differ diff --git a/control_test/items/pouf.usd b/control_test/items/pouf.usd new file mode 100644 index 0000000..9cdb655 Binary files /dev/null and b/control_test/items/pouf.usd differ diff --git a/control_test/items/printer_compact.usd b/control_test/items/printer_compact.usd new file mode 100644 index 0000000..3bd3f8d Binary files /dev/null and b/control_test/items/printer_compact.usd differ diff --git a/control_test/items/safety_pin.usd b/control_test/items/safety_pin.usd new file mode 100644 index 0000000..e80d10b Binary files /dev/null and b/control_test/items/safety_pin.usd differ diff --git a/control_test/items/sneaker.usd b/control_test/items/sneaker.usd new file mode 100644 index 0000000..135d058 Binary files /dev/null and b/control_test/items/sneaker.usd differ diff --git a/control_test/items/textures/air_conditioner_texture0.jpg b/control_test/items/textures/air_conditioner_texture0.jpg new file mode 100644 index 0000000..e2f1607 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture0.jpg differ diff --git a/control_test/items/textures/air_conditioner_texture1.png b/control_test/items/textures/air_conditioner_texture1.png new file mode 100644 index 0000000..eb9d47d Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture1.png differ diff --git a/control_test/items/textures/air_conditioner_texture10.png b/control_test/items/textures/air_conditioner_texture10.png new file mode 100644 index 0000000..5d1e4ee Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture10.png differ diff --git a/control_test/items/textures/air_conditioner_texture11.png b/control_test/items/textures/air_conditioner_texture11.png new file mode 100644 index 0000000..9c33233 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture11.png differ diff --git a/control_test/items/textures/air_conditioner_texture2.png b/control_test/items/textures/air_conditioner_texture2.png new file mode 100644 index 0000000..a894e06 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture2.png differ diff --git a/control_test/items/textures/air_conditioner_texture3.jpg b/control_test/items/textures/air_conditioner_texture3.jpg new file mode 100644 index 0000000..2c47729 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture3.jpg differ diff --git a/control_test/items/textures/air_conditioner_texture4.png b/control_test/items/textures/air_conditioner_texture4.png new file mode 100644 index 0000000..c1969de Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture4.png differ diff --git a/control_test/items/textures/air_conditioner_texture5.png b/control_test/items/textures/air_conditioner_texture5.png new file mode 100644 index 0000000..ac3af00 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture5.png differ diff --git a/control_test/items/textures/air_conditioner_texture6.jpg b/control_test/items/textures/air_conditioner_texture6.jpg new file mode 100644 index 0000000..bd7ac45 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture6.jpg differ diff --git a/control_test/items/textures/air_conditioner_texture7.png b/control_test/items/textures/air_conditioner_texture7.png new file mode 100644 index 0000000..3527997 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture7.png differ diff --git a/control_test/items/textures/air_conditioner_texture8.png b/control_test/items/textures/air_conditioner_texture8.png new file mode 100644 index 0000000..415f12d Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture8.png differ diff --git a/control_test/items/textures/air_conditioner_texture9.jpg b/control_test/items/textures/air_conditioner_texture9.jpg new file mode 100644 index 0000000..16a1554 Binary files /dev/null and b/control_test/items/textures/air_conditioner_texture9.jpg differ diff --git a/control_test/items/textures/briefcase_hard_texture0.jpg b/control_test/items/textures/briefcase_hard_texture0.jpg new file mode 100644 index 0000000..0edced2 Binary files /dev/null and b/control_test/items/textures/briefcase_hard_texture0.jpg differ diff --git a/control_test/items/textures/briefcase_hard_texture1.png b/control_test/items/textures/briefcase_hard_texture1.png new file mode 100644 index 0000000..a109b68 Binary files /dev/null and b/control_test/items/textures/briefcase_hard_texture1.png differ diff --git a/control_test/items/textures/briefcase_hard_texture2.png b/control_test/items/textures/briefcase_hard_texture2.png new file mode 100644 index 0000000..da0cffb Binary files /dev/null and b/control_test/items/textures/briefcase_hard_texture2.png differ diff --git a/control_test/items/textures/carton_large_texture0.jpg b/control_test/items/textures/carton_large_texture0.jpg new file mode 100644 index 0000000..7c5da04 Binary files /dev/null and b/control_test/items/textures/carton_large_texture0.jpg differ diff --git a/control_test/items/textures/carton_large_texture1.png b/control_test/items/textures/carton_large_texture1.png new file mode 100644 index 0000000..f413e4d Binary files /dev/null and b/control_test/items/textures/carton_large_texture1.png differ diff --git a/control_test/items/textures/carton_large_texture2.png b/control_test/items/textures/carton_large_texture2.png new file mode 100644 index 0000000..d1f87c1 Binary files /dev/null and b/control_test/items/textures/carton_large_texture2.png differ diff --git a/control_test/items/textures/cleat_small_texture0.jpg b/control_test/items/textures/cleat_small_texture0.jpg new file mode 100644 index 0000000..ec5255a Binary files /dev/null and b/control_test/items/textures/cleat_small_texture0.jpg differ diff --git a/control_test/items/textures/clothespin_flat_texture0.jpg b/control_test/items/textures/clothespin_flat_texture0.jpg new file mode 100644 index 0000000..c23ec64 Binary files /dev/null and b/control_test/items/textures/clothespin_flat_texture0.jpg differ diff --git a/control_test/items/textures/cooler_box_texture0.jpg b/control_test/items/textures/cooler_box_texture0.jpg new file mode 100644 index 0000000..f763b9e Binary files /dev/null and b/control_test/items/textures/cooler_box_texture0.jpg differ diff --git a/control_test/items/textures/cooler_cube_texture0.jpg b/control_test/items/textures/cooler_cube_texture0.jpg new file mode 100644 index 0000000..e799116 Binary files /dev/null and b/control_test/items/textures/cooler_cube_texture0.jpg differ diff --git a/control_test/items/textures/cooler_cube_texture1.png b/control_test/items/textures/cooler_cube_texture1.png new file mode 100644 index 0000000..d7527be Binary files /dev/null and b/control_test/items/textures/cooler_cube_texture1.png differ diff --git a/control_test/items/textures/cooler_cube_texture2.png b/control_test/items/textures/cooler_cube_texture2.png new file mode 100644 index 0000000..93a7cd0 Binary files /dev/null and b/control_test/items/textures/cooler_cube_texture2.png differ diff --git a/control_test/items/textures/duffel_bag_texture0.jpg b/control_test/items/textures/duffel_bag_texture0.jpg new file mode 100644 index 0000000..92b34d2 Binary files /dev/null and b/control_test/items/textures/duffel_bag_texture0.jpg differ diff --git a/control_test/items/textures/duffel_bag_texture1.jpg b/control_test/items/textures/duffel_bag_texture1.jpg new file mode 100644 index 0000000..ef54f2f Binary files /dev/null and b/control_test/items/textures/duffel_bag_texture1.jpg differ diff --git a/control_test/items/textures/duffel_bag_texture2.png b/control_test/items/textures/duffel_bag_texture2.png new file mode 100644 index 0000000..a9f19d3 Binary files /dev/null and b/control_test/items/textures/duffel_bag_texture2.png differ diff --git a/control_test/items/textures/duffel_bag_texture3.png b/control_test/items/textures/duffel_bag_texture3.png new file mode 100644 index 0000000..e74a185 Binary files /dev/null and b/control_test/items/textures/duffel_bag_texture3.png differ diff --git a/control_test/items/textures/duffel_bag_texture4.png b/control_test/items/textures/duffel_bag_texture4.png new file mode 100644 index 0000000..f175e3b Binary files /dev/null and b/control_test/items/textures/duffel_bag_texture4.png differ diff --git a/control_test/items/textures/duffel_bag_texture5.jpg b/control_test/items/textures/duffel_bag_texture5.jpg new file mode 100644 index 0000000..5674858 Binary files /dev/null and b/control_test/items/textures/duffel_bag_texture5.jpg differ diff --git a/control_test/items/textures/duffel_bag_texture6.jpg b/control_test/items/textures/duffel_bag_texture6.jpg new file mode 100644 index 0000000..5674858 Binary files /dev/null and b/control_test/items/textures/duffel_bag_texture6.jpg differ diff --git a/control_test/items/textures/duffel_round_texture0.jpg b/control_test/items/textures/duffel_round_texture0.jpg new file mode 100644 index 0000000..354be4e Binary files /dev/null and b/control_test/items/textures/duffel_round_texture0.jpg differ diff --git a/control_test/items/textures/nailfile_mini_texture0.png b/control_test/items/textures/nailfile_mini_texture0.png new file mode 100644 index 0000000..f75f05c Binary files /dev/null and b/control_test/items/textures/nailfile_mini_texture0.png differ diff --git a/control_test/items/textures/nailfile_mini_texture1.png b/control_test/items/textures/nailfile_mini_texture1.png new file mode 100644 index 0000000..90f0a0e Binary files /dev/null and b/control_test/items/textures/nailfile_mini_texture1.png differ diff --git a/control_test/items/textures/printer_compact_texture0.png b/control_test/items/textures/printer_compact_texture0.png new file mode 100644 index 0000000..3b9f75c Binary files /dev/null and b/control_test/items/textures/printer_compact_texture0.png differ diff --git a/control_test/items/textures/printer_compact_texture1.jpg b/control_test/items/textures/printer_compact_texture1.jpg new file mode 100644 index 0000000..6634f0a Binary files /dev/null and b/control_test/items/textures/printer_compact_texture1.jpg differ diff --git a/control_test/items/textures/printer_office_texture0.jpg b/control_test/items/textures/printer_office_texture0.jpg new file mode 100644 index 0000000..0830500 Binary files /dev/null and b/control_test/items/textures/printer_office_texture0.jpg differ diff --git a/control_test/items/textures/printer_office_texture1.jpg b/control_test/items/textures/printer_office_texture1.jpg new file mode 100644 index 0000000..31ad44c Binary files /dev/null and b/control_test/items/textures/printer_office_texture1.jpg differ diff --git a/control_test/items/textures/printer_office_texture2.jpg b/control_test/items/textures/printer_office_texture2.jpg new file mode 100644 index 0000000..dc1e441 Binary files /dev/null and b/control_test/items/textures/printer_office_texture2.jpg differ diff --git a/control_test/items/textures/printer_office_texture3.jpg b/control_test/items/textures/printer_office_texture3.jpg new file mode 100644 index 0000000..7be7943 Binary files /dev/null and b/control_test/items/textures/printer_office_texture3.jpg differ diff --git a/control_test/items/textures/printer_office_texture4.jpg b/control_test/items/textures/printer_office_texture4.jpg new file mode 100644 index 0000000..67ac7f7 Binary files /dev/null and b/control_test/items/textures/printer_office_texture4.jpg differ diff --git a/control_test/items/textures/safety_pin_texture0.png b/control_test/items/textures/safety_pin_texture0.png new file mode 100644 index 0000000..7a25f38 Binary files /dev/null and b/control_test/items/textures/safety_pin_texture0.png differ diff --git a/control_test/items/textures/safety_pin_texture1.png b/control_test/items/textures/safety_pin_texture1.png new file mode 100644 index 0000000..9b19e5b Binary files /dev/null and b/control_test/items/textures/safety_pin_texture1.png differ diff --git a/control_test/items/textures/safety_pin_texture2.png b/control_test/items/textures/safety_pin_texture2.png new file mode 100644 index 0000000..cb55497 Binary files /dev/null and b/control_test/items/textures/safety_pin_texture2.png differ diff --git a/control_test/items/textures/suitcase_large_texture0.jpg b/control_test/items/textures/suitcase_large_texture0.jpg new file mode 100644 index 0000000..2e7f8ab Binary files /dev/null and b/control_test/items/textures/suitcase_large_texture0.jpg differ diff --git a/control_test/items/textures/toaster_compact_texture0.png b/control_test/items/textures/toaster_compact_texture0.png new file mode 100644 index 0000000..15e9a78 Binary files /dev/null and b/control_test/items/textures/toaster_compact_texture0.png differ diff --git a/control_test/items/textures/toaster_compact_texture1.png b/control_test/items/textures/toaster_compact_texture1.png new file mode 100644 index 0000000..798b59b Binary files /dev/null and b/control_test/items/textures/toaster_compact_texture1.png differ diff --git a/control_test/items/textures/toaster_oven_texture0.png b/control_test/items/textures/toaster_oven_texture0.png new file mode 100644 index 0000000..93e5dcd Binary files /dev/null and b/control_test/items/textures/toaster_oven_texture0.png differ diff --git a/control_test/items/textures/toaster_oven_texture1.png b/control_test/items/textures/toaster_oven_texture1.png new file mode 100644 index 0000000..bdfbe4a Binary files /dev/null and b/control_test/items/textures/toaster_oven_texture1.png differ diff --git a/control_test/items/textures/toaster_oven_texture2.png b/control_test/items/textures/toaster_oven_texture2.png new file mode 100644 index 0000000..5f9ec24 Binary files /dev/null and b/control_test/items/textures/toaster_oven_texture2.png differ diff --git a/control_test/items/textures/toaster_oven_texture3.png b/control_test/items/textures/toaster_oven_texture3.png new file mode 100644 index 0000000..f949a1f Binary files /dev/null and b/control_test/items/textures/toaster_oven_texture3.png differ diff --git a/control_test/items/textures/toaster_oven_texture4.png b/control_test/items/textures/toaster_oven_texture4.png new file mode 100644 index 0000000..30f9cce Binary files /dev/null and b/control_test/items/textures/toaster_oven_texture4.png differ diff --git a/control_test/items/textures/toaster_oven_texture5.png b/control_test/items/textures/toaster_oven_texture5.png new file mode 100644 index 0000000..2234d80 Binary files /dev/null and b/control_test/items/textures/toaster_oven_texture5.png differ diff --git a/control_test/items/toaster_compact.usd b/control_test/items/toaster_compact.usd new file mode 100644 index 0000000..b4aad83 Binary files /dev/null and b/control_test/items/toaster_compact.usd differ diff --git a/control_test/items/toaster_oven.usd b/control_test/items/toaster_oven.usd new file mode 100644 index 0000000..f56a548 Binary files /dev/null and b/control_test/items/toaster_oven.usd differ diff --git a/control_test/items/tool_case.usd b/control_test/items/tool_case.usd new file mode 100644 index 0000000..1c3abc3 Binary files /dev/null and b/control_test/items/tool_case.usd differ diff --git a/control_test/items/watch.usd b/control_test/items/watch.usd new file mode 100644 index 0000000..7fc881c Binary files /dev/null and b/control_test/items/watch.usd differ diff --git a/control_test/items_flow/backpack.usd b/control_test/items_flow/backpack.usd new file mode 100644 index 0000000..7da3699 Binary files /dev/null and b/control_test/items_flow/backpack.usd differ diff --git a/control_test/items_flow/bag.usd b/control_test/items_flow/bag.usd new file mode 100644 index 0000000..3c49a45 Binary files /dev/null and b/control_test/items_flow/bag.usd differ diff --git a/control_test/items_flow/box_300x200x200.usd b/control_test/items_flow/box_300x200x200.usd new file mode 100644 index 0000000..e6fd16d Binary files /dev/null and b/control_test/items_flow/box_300x200x200.usd differ diff --git a/control_test/items_flow/box_400x400x300.usd b/control_test/items_flow/box_400x400x300.usd new file mode 100644 index 0000000..9f795cb Binary files /dev/null and b/control_test/items_flow/box_400x400x300.usd differ diff --git a/control_test/items_flow/bucket.usd b/control_test/items_flow/bucket.usd new file mode 100644 index 0000000..f20807f Binary files /dev/null and b/control_test/items_flow/bucket.usd differ diff --git a/control_test/items_flow/detergent.usd b/control_test/items_flow/detergent.usd new file mode 100644 index 0000000..ac75794 Binary files /dev/null and b/control_test/items_flow/detergent.usd differ diff --git a/control_test/items_flow/helmet.usd b/control_test/items_flow/helmet.usd new file mode 100644 index 0000000..5893a37 Binary files /dev/null and b/control_test/items_flow/helmet.usd differ diff --git a/control_test/items_flow/labels.json b/control_test/items_flow/labels.json new file mode 100644 index 0000000..35e7d65 --- /dev/null +++ b/control_test/items_flow/labels.json @@ -0,0 +1,227 @@ +{ + "backpack": { + "dims_mm": [ + 454.7, + 370.3, + 300.9 + ], + "k": 0.82, + "zone": "C" + }, + "bag": { + "dims_mm": [ + 201.7, + 175.3, + 170.3 + ], + "k": 0.896, + "zone": "D" + }, + "banana": { + "dims_mm": [ + 182.6, + 70.6, + 33.0 + ], + "k": 0.94, + "zone": "D" + }, + "bolts_cluster": { + "dims_mm": [ + 193.6, + 135.6, + 52.6 + ], + "k": 0.718, + "zone": "B" + }, + "bottle": { + "dims_mm": [ + 304.8, + 91.0, + 91.0 + ], + "k": 0.995, + "zone": "D" + }, + "box_300x200x200": { + "dims_mm": [ + 301.0, + 200.5, + 200.0 + ], + "k": 0.72, + "zone": "B" + }, + "box_400x400x300": { + "dims_mm": [ + 401.0, + 400.0, + 300.5 + ], + "k": 0.716, + "zone": "C" + }, + "bucket": { + "dims_mm": [ + 287.4, + 287.4, + 272.3 + ], + "k": 0.995, + "zone": "D" + }, + "chip_bag": { + "dims_mm": [ + 250.0, + 162.2, + 69.1 + ], + "k": 0.811, + "zone": "D" + }, + "cone": { + "dims_mm": [ + 500.0, + 350.5, + 350.5 + ], + "k": 0.991, + "zone": "C" + }, + "cylinder": { + "dims_mm": [ + 434.9, + 50.0, + 43.0 + ], + "k": 0.867, + "zone": "D" + }, + "detergent": { + "dims_mm": [ + 278.2, + 259.9, + 179.8 + ], + "k": 0.742, + "zone": "B" + }, + "headphones": { + "dims_mm": [ + 198.4, + 194.9, + 93.3 + ], + "k": 0.807, + "zone": "D" + }, + "helmet": { + "dims_mm": [ + 353.5, + 297.1, + 279.9 + ], + "k": 0.895, + "zone": "D" + }, + "lunchbox": { + "dims_mm": [ + 201.1, + 152.4, + 62.3 + ], + "k": 0.646, + "zone": "B" + }, + "mug": { + "dims_mm": [ + 112.9, + 99.0, + 83.2 + ], + "k": 0.985, + "zone": "D" + }, + "parcel_box": { + "dims_mm": [ + 344.4, + 155.1, + 143.7 + ], + "k": 0.699, + "zone": "B" + }, + "pen": { + "dims_mm": [ + 148.5, + 13.1, + 9.0 + ], + "k": 0.842, + "zone": "C" + }, + "perfume": { + "dims_mm": [ + 120.0, + 53.1, + 53.1 + ], + "k": 0.924, + "zone": "D" + }, + "pillow": { + "dims_mm": [ + 455.1, + 430.6, + 212.7 + ], + "k": 0.905, + "zone": "C" + }, + "plate": { + "dims_mm": [ + 209.4, + 209.4, + 26.6 + ], + "k": 0.998, + "zone": "D" + }, + "pouf": { + "dims_mm": [ + 488.9, + 488.9, + 264.0 + ], + "k": 0.994, + "zone": "C" + }, + "sneaker": { + "dims_mm": [ + 270.4, + 208.0, + 125.4 + ], + "k": 0.706, + "zone": "B" + }, + "tool_case": { + "dims_mm": [ + 300.0, + 143.5, + 60.0 + ], + "k": 0.454, + "zone": "B" + }, + "watch": { + "dims_mm": [ + 230.0, + 230.0, + 4.6 + ], + "k": 0.995, + "zone": "C" + } +} \ No newline at end of file diff --git a/control_test/items_flow/lunchbox.usd b/control_test/items_flow/lunchbox.usd new file mode 100644 index 0000000..d36c434 Binary files /dev/null and b/control_test/items_flow/lunchbox.usd differ diff --git a/control_test/items_flow/pillow.usd b/control_test/items_flow/pillow.usd new file mode 100644 index 0000000..5c9de44 Binary files /dev/null and b/control_test/items_flow/pillow.usd differ diff --git a/control_test/items_flow/textures b/control_test/items_flow/textures new file mode 120000 index 0000000..46cdf27 --- /dev/null +++ b/control_test/items_flow/textures @@ -0,0 +1 @@ +/home/dasha/robozon-sorter/control_test/items/textures \ No newline at end of file diff --git a/control_test/measure_cfg.py b/control_test/measure_cfg.py new file mode 100644 index 0000000..74a460b --- /dev/null +++ b/control_test/measure_cfg.py @@ -0,0 +1,215 @@ +"""SUPERSEDED - kept for history only. + +SUPERSEDED by measure_roi.py. Isolated the object by 3D cropping and RGB +background subtraction; both were measuring the belt. Use the ROI pipeline. +""" + +#!/usr/bin/env python3 +"""STAGE 2 (standalone via python.sh - NOT inside the running app, so the renderer keeps +its GPU): CREStereo on each rig's pair -> metric depth -> world cloud -> merge the rigs -> +dimensions, against the labelled ground truth. + +Merging uses the CALIBRATED extrinsics and nothing else. A previous study on this cell +measured RANSAC/trimmed-ICP re-registration making it worse in every case (11.9 mm -> +40.5 mm): the three views see largely different surfaces, so ICP aligns non-corresponding +geometry and drags cameras off their correct poses. Accuracy comes from calibration, not +from re-registration - so RANSAC is deliberately not used here. + +Dimensions use voxel-resample + robust 2/98 percentile extents on PCA axes, which the same +study found best (11.9 mm) against raw (17.6), SOR (19.1) and convex-hull OBB (27.4). +""" +import glob, json, os, sys +import numpy as np, cv2, torch, torch.nn.functional as F + +sys.path.insert(0, "/home/dasha/isaac_assets/cv/crestereo") +from nets import Model + +CFG = sys.argv[1] if len(sys.argv) > 1 else "A_original" +BASE = f"/home/dasha/robozon-sorter/control_test/captures/{CFG}" +MAN = json.load(open(f"{BASE}/manifest.json")) +SCALE = 3.0 # scene is 1/3 of real size; labels are real mm +TARGET = np.array(MAN["target"], dtype=np.float64) +BELT_Z = TARGET[2] + +dev = "cuda" if torch.cuda.is_available() else "cpu" +model = Model(max_disp=256, mixed_precision=False, test_mode=True) +model.load_state_dict(torch.load("/home/dasha/isaac_assets/cv/crestereo/models/crestereo_eth3d.pth", + map_location="cpu"), strict=True) +model.to(dev).eval() +print(f"CREStereo on {dev} config={CFG}", flush=True) + + +def infer(L, R, n=20): + iL = torch.tensor(np.ascontiguousarray(L.transpose(2, 0, 1)[None]).astype("float32")).to(dev) + iR = torch.tensor(np.ascontiguousarray(R.transpose(2, 0, 1)[None]).astype("float32")).to(dev) + h, w = iL.shape[2], iL.shape[3] + iLd = F.interpolate(iL, size=(h // 2, w // 2), mode="bilinear", align_corners=True) + iRd = F.interpolate(iR, size=(h // 2, w // 2), mode="bilinear", align_corners=True) + with torch.inference_mode(): + f0 = model(iLd, iRd, iters=n, flow_init=None) + f = model(iL, iR, iters=n, flow_init=f0) + return torch.squeeze(f[:, 0, :, :]).cpu().numpy() + + +def object_mask(fL, fbg, thr=6): + """exact mask by background subtraction. + + Order matters and the first attempt got it backwards. A ray-traced render differs from + its background mostly on TEXTURED pixels, so the raw difference is a speckle field, not + a solid silhouette: 13199 differing pixels came out as 86 fragments whose largest was + 178 px, and picking the largest component then measured a speck instead of the object. + CLOSE first to bridge the speckle into one region, THEN open to drop stray noise, then + take the largest blob and fill its holes. + """ + a = cv2.imread(fL).astype(np.int16) + b = cv2.imread(fbg).astype(np.int16) + d = np.abs(a - b).max(axis=2).astype(np.uint8) + m = (d > thr).astype(np.uint8) + m = cv2.morphologyEx(m, cv2.MORPH_CLOSE, np.ones((15, 15), np.uint8)) + m = cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8)) + n, lab, stats, _ = cv2.connectedComponentsWithStats(m, 8) + if n <= 1: + return m.astype(bool), 0 + k = 1 + int(np.argmax(stats[1:, cv2.CC_STAT_AREA])) + blob = (lab == k).astype(np.uint8) + # fill interior holes so untextured patches inside the object are not lost + ff = blob.copy() + h, w = ff.shape + cv2.floodFill(ff, np.zeros((h + 2, w + 2), np.uint8), (0, 0), 1) + blob = blob | (1 - ff) + return blob.astype(bool), int(blob.sum()) + + +def rig_cloud(fL, fR, K, b, use_mask=False, fbg=None): + L = cv2.cvtColor(cv2.imread(fL), cv2.COLOR_BGR2RGB) + R = cv2.cvtColor(cv2.imread(fR), cv2.COLOR_BGR2RGB) + H, W = L.shape[:2] + sw = 640; sh = int(round(H * sw / W / 8)) * 8 + d = infer(cv2.resize(L, (sw, sh)), cv2.resize(R, (sw, sh))) + disp = cv2.resize(d, (W, H)) * (W / sw) + depth = np.where(disp > 0.5, K["fx"] * b / np.maximum(disp, 1e-6), np.nan) + fx, fy, cx, cy = K["fx"], K["fy"], K["cx"], K["cy"] + vs, us = np.mgrid[0:H, 0:W] + m = np.isfinite(depth) & (depth > 1e-3) & (depth < 3.0) + npix = 0 + if use_mask and fbg is not None and os.path.exists(fbg): + om, npix = object_mask(fL, fbg) + m &= om + u, v, z = us[m], vs[m], depth[m] + if len(z) == 0: + return np.zeros((0, 3)), npix, float("nan") + Pc = np.stack([(u - cx) * z / fx, -(v - cy) * z / fy, -z, np.ones_like(z)], 1) + return (Pc @ np.array(K["M"], dtype=np.float64))[:, :3], npix, float(np.median(z)) + + +RADIUS = 0.16 # scene metres around the inspection point (~0.48 m real) + + +def belt_reference(rig, K, b): + """reconstructed belt height for THIS rig, from its own background pair. + + A fixed z-threshold does not work: CREStereo places the belt a few mm above its true + height, and the offset differs per rig, so a global cut either keeps a belt patch (the + first attempt measured 1700x1550 mm for every item - the crop window, not the object) + or removes the object's base. Measuring the belt from the background pair calibrates + that bias away per rig. RGB background subtraction was tried instead and is worse: the + object's SHADOW differs from the background too, so the mask swallows the belt. + """ + fbg = MAN.get("background", {}).get(f"{rig}_Left") + fbgR = MAN.get("background", {}).get(f"{rig}_Right") + if not (fbg and fbgR and os.path.exists(fbg) and os.path.exists(fbgR)): + return None + P, _, _ = rig_cloud(fbg, fbgR, K, b) + m = ((np.abs(P[:, 0] - TARGET[0]) < RADIUS) & (np.abs(P[:, 1] - TARGET[1]) < RADIUS)) + if m.sum() < 200: + return None + return float(np.percentile(P[m][:, 2], 90)) # top of the reconstructed belt + + +def isolate(P, belt_z): + z0 = (belt_z if belt_z is not None else BELT_Z) + 0.004 + m = ((np.abs(P[:, 0] - TARGET[0]) < RADIUS) & (np.abs(P[:, 1] - TARGET[1]) < RADIUS) + & (P[:, 2] > z0) & (P[:, 2] < BELT_Z + 0.30)) + return P[m] + + +def voxel(P, s=0.002): + if len(P) == 0: + return P + keys = np.floor(P / s).astype(np.int64) + _, idx = np.unique(keys, axis=0, return_index=True) + return P[idx] + + +def dims_mm(P): + """robust extents on PCA axes, in real mm""" + if len(P) < 30: + return None + Q = voxel(P) + c = Q.mean(0) + X = Q - c + _, _, V = np.linalg.svd(X, full_matrices=False) + A = X @ V.T + lo = np.percentile(A, 2, axis=0) + hi = np.percentile(A, 98, axis=0) + return sorted(((hi - lo) * 1000.0 * SCALE), reverse=True) + + +BELTREF = {} +rows = [] +for name, rec in MAN["items"].items(): + gt = sorted(rec["gt"]["dims_mm"], reverse=True) + per_rig, merged = {}, [] + for rig in ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"): + kL = MAN["calib"][f"{rig}_Left"] + fL, fR = rec["files"][f"{rig}_Left"], rec["files"][f"{rig}_Right"] + if not (os.path.exists(fL) and os.path.exists(fR)): + continue + try: + if rig not in BELTREF: + BELTREF[rig] = belt_reference(rig, kL, kL["baseline"]) + P, npix, zmed = rig_cloud(fL, fR, kL, kL["baseline"]) + P = isolate(P, BELTREF[rig]) + except Exception as e: + print(f" {name}/{rig}: {e}"); continue + per_rig[rig] = dict(n=len(P), dims=dims_mm(P), mask_px=npix, z_med_mm= + round(zmed * 1000, 1) if zmed == zmed else None) + if len(P): + merged.append(P) + M = np.vstack(merged) if merged else np.zeros((0, 3)) + dm = dims_mm(M) + err = None + if dm: + err = float(np.mean([abs(a - b) for a, b in zip(dm, gt)])) + rows.append(dict(item=name, cls=rec["gt"]["cls"], gt=gt, + merged=[round(v, 1) for v in dm] if dm else None, + n_merged=int(len(M)), + mae_mm=round(err, 1) if err is not None else None, + per_rig={k: dict(n=v["n"], mask_px=v["mask_px"], + z_med_mm=v["z_med_mm"], + dims=[round(x, 1) for x in v["dims"]] if v["dims"] else None, + mae=round(float(np.mean([abs(a - b) for a, b in + zip(v["dims"], gt)])), 1) + if v["dims"] else None) + for k, v in per_rig.items()})) + print(f" {name:>18} {rec['gt']['cls']} gt={[round(g) for g in gt]} " + f"merged={[round(v) for v in dm] if dm else None} MAE={err:.1f} mm" + if dm else f" {name:>18}: no cloud", flush=True) + +out = f"{BASE}/measure.json" +json.dump(dict(config=CFG, rows=rows), open(out, "w"), indent=1) +ok = [r for r in rows if r["mae_mm"] is not None] +print(f"\n=== {CFG} ===") +print(f" items measured: {len(ok)}/{len(rows)}") +if ok: + print(f" merged MAE: {np.mean([r['mae_mm'] for r in ok]):.1f} mm") + for c in ("B", "C", "D"): + s = [r["mae_mm"] for r in ok if r["cls"] == c] + if s: + print(f" class {c}: {np.mean(s):6.1f} mm (n={len(s)})") + for rig in ("RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"): + s = [r["per_rig"][rig]["mae"] for r in ok + if rig in r["per_rig"] and r["per_rig"][rig]["mae"] is not None] + if s: + print(f" single rig {rig:>18}: {np.mean(s):6.1f} mm (n={len(s)})") +print(f" -> {out}") diff --git a/control_test/measure_ffs.py b/control_test/measure_ffs.py new file mode 100644 index 0000000..bdfceed --- /dev/null +++ b/control_test/measure_ffs.py @@ -0,0 +1,168 @@ +"""Fast FoundationStereo вместо CREStereo, на том же прогоне потока 700 мм. + +Модель взята ONNX-экспортом (23_36_37, iters 4, 320x736) и гоняется через onnxruntime-gpu: +это снимает зависимость от окружения репозитория (torch 2.6 + xformers), которое с +питоном Isaac не совпадает. Веса лежат в fast-foundationstereo/weights. + +ВХОД У МОДЕЛИ ФИКСИРОВАННЫЙ - 320x736. Кроп приводится к этому размеру, а полученная +диспаратность масштабируется ОБРАТНО по горизонтали: диспаратность измеряется в пикселях, +и при сжатии кадра в k раз по ширине она сжимается во столько же. Без этого множителя +глубина уехала бы ровно в k раз - самая частая ошибка при подмене стереосети. + +Сравнение честное: сегментация, отделение полотна, слияние и метрики те же, что в +варианте без сегментации (лучшем на сегодня). Меняется ТОЛЬКО стереодвижок. +""" +import os, sys, json, time +import numpy as np +import cv2 + +CV = "/home/dasha/isaac_assets/cv" +CT = "/home/dasha/robozon-sorter/control_test" +FFS = f"{CV}/fast-foundationstereo/weights/ffs_23_36_37_iters4_320x736.onnx" +OUT = f"{CT}/diag/ffs" +os.makedirs(OUT, exist_ok=True) +sys.path.insert(0, CT) +os.environ.setdefault("MASK_MODE", "gate") +import measure_flow as MF +import classify as CL + +import onnxruntime as ort +so = ort.SessionOptions() +so.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL +sess = ort.InferenceSession(FFS, so, providers=["CUDAExecutionProvider", "CPUExecutionProvider"]) +IN_H, IN_W = 320, 736 +print(f"Fast FoundationStereo: {os.path.basename(FFS)}, вход {IN_W}x{IN_H}, " + f"провайдер {sess.get_providers()[0]}") + +man, calib = MF.man, MF.calib +TARGET, BELT_Z, RIGS = MF.TARGET, MF.BELT_Z, MF.RIGS +FLOOR, GATE_R = 0.020, 0.28 +DENS_CELL, DENS_MIN = 0.010, 12 +PITCH_S = 0.70 + + +def ffs_disp(L, R): + """диспаратность одной пары. Кроп -> 320x736 -> обратный масштаб по горизонтали.""" + # cv2.imread даёт BGR, а модель обучена на RGB (run_demo.py читает через imageio). + # Нормализацию (ImageNet mean/std после деления на 255) модель делает ВНУТРИ forward, + # см. core/foundation_stereo.py:32 - значит она экспортирована в ONNX, и подавать надо + # именно СЫРЫЕ 0-255. Ошибкой был только порядок каналов. + h, w = L.shape[:2] + sx = IN_W / float(w) + lr = cv2.resize(cv2.cvtColor(L, cv2.COLOR_BGR2RGB), (IN_W, IN_H)).astype(np.float32) + rr = cv2.resize(cv2.cvtColor(R, cv2.COLOR_BGR2RGB), (IN_W, IN_H)).astype(np.float32) + a = lr.transpose(2, 0, 1)[None] + b = rr.transpose(2, 0, 1)[None] + d = sess.run(["disparity"], {"left_image": a, "right_image": b})[0][0, 0] + d = cv2.resize(d, (w, h), interpolation=cv2.INTER_LINEAR) + return np.abs(d) / sx # пиксели исходного кропа, а не приведённого + + +def dense_only(P, cell=DENS_CELL, need=DENS_MIN): + if len(P) < 60: + return np.ones(len(P), bool) + key = np.floor(P[:, :2] / cell).astype(np.int64) + uniq, inv, cnt = np.unique(key, axis=0, return_inverse=True, return_counts=True) + return cnt[inv] >= need + + +def biggest_blob_idx(P, grid=0.012): + if len(P) < 40: + return np.ones(len(P), bool) + cx = np.round(P[:, 0] / grid).astype(int); cy = np.round(P[:, 1] / grid).astype(int) + im = np.zeros((cy.max() - cy.min() + 3, cx.max() - cx.min() + 3), np.uint8) + im[cy - cy.min() + 1, cx - cx.min() + 1] = 255 + im = cv2.morphologyEx(im, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8)) + n, lab = cv2.connectedComponents(im) + li = lab[cy - cy.min() + 1, cx - cx.min() + 1] + best, bn = None, 0 + for k in range(1, n): + m = li == k + if m.sum() > bn: + best, bn = m, m.sum() + return np.ones(len(P), bool) if best is None else best + + +def cloud_from_roi(disp, win, cam): + x0, y0, x1, y1 = win + depth = np.where(disp > 0.5, cam["fx"] * cam["baseline"] / np.maximum(disp, 1e-6), np.nan) + vs, us = np.mgrid[y0:y0 + depth.shape[0], x0:x0 + depth.shape[1]] + ok = np.isfinite(depth) & (depth > 1e-3) + u, v, z = us[ok], vs[ok], depth[ok] + P = np.stack([(u - cam["cx"]) * z / cam["fx"], + -(v - cam["cy"]) * z / cam["fy"], -z, np.ones_like(z)], 1) + P = (P @ np.array(cam["M"]))[:, :3] + near = np.hypot(P[:, 0] - TARGET[0], P[:, 1] - TARGET[1]) < GATE_R + return P[near & (P[:, 2] > BELT_Z + FLOOR) & (P[:, 2] < BELT_Z + 0.60)] + + +for _ in range(2): # прогрев + ffs_disp(np.zeros((240, 480, 3), np.uint8), np.zeros((240, 480, 3), np.uint8)) +print("подача: RGB 0-255, нормализация внутри графа") + +print(f"\n {'товар':18s} {'эталон, мм':>18s} {'предсказано, мм':>20s} {'MAE':>6s} " + f"{'k':>6s} {'класс':>12s} {'точек':>7s} {'мс':>6s}") +print(" " + "-" * 100) +rows, times = [], [] +for name, e in man["items"].items(): + t0 = time.time() + clouds = [] + for rig in RIGS: + cam = calib[f"{rig}_Left"] + IL = cv2.imread(e["files"][f"{rig}_Left"]) + IR = cv2.imread(e["files"][f"{rig}_Right"]) + if IL is None or IR is None: + continue + x0, y0, x1, y1 = MF.belt_roi_px(cam, MF.POLY3) + maxd = int(np.ceil(MF.DPAD * cam["fx"] * cam["baseline"] / MF.ZMIN)) + x0 = max(0, x0 - maxd) + d = ffs_disp(IL[y0:y1, x0:x1], IR[y0:y1, x0:x1]) + P = cloud_from_roi(d, (x0, y0, x1, y1), cam) + if len(P): + clouds.append(P) + if name in ("bag", "bucket", "box_300x200x200"): + dn = cv2.normalize(d, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) + cv2.imwrite(f"{OUT}/{name}_{rig}_disp.png", cv2.applyColorMap(dn, cv2.COLORMAP_TURBO)) + dt = (time.time() - t0) * 1000 + times.append(dt) + gt = e["gt"]; gtd = sorted(gt["dims_mm"], reverse=True) + if not clouds: + print(f" {name:18s} {str([round(v) for v in gtd]):>18s} облако пустое"); continue + P = np.vstack(clouds) + s1 = dense_only(P) + if s1.sum() >= 60: + P = P[s1] + P = P[biggest_blob_idx(P)] + out = MF.dims_and_k(P) + if out is None: + print(f" {name:18s} габариты не взялись"); continue + dims, k = out + mae = float(np.mean(np.abs(np.array(dims) - np.array(gtd)))) + cls = CL.classify(dims, 0.0 if np.isnan(k) else k) + ok = "верно" if cls == gt["zone_scene"] else f"ОШ({gt['zone_scene']})" + print(f" {name:18s} {str([round(v) for v in gtd]):>18s} " + f"{str([round(v) for v in dims]):>20s} {mae:6.1f} {k:6.2f} {cls+' '+ok:>12s} " + f"{len(P):7d} {dt:6.0f}") + rows.append(dict(name=name, gt=gtd, gt_cls=gt["zone_scene"], pred=[round(v, 1) for v in dims], + pred_k=round(float(k), 3), pred_cls=cls, mae=round(mae, 1), + n=len(P), ms=round(dt))) + +print("\n === МЕТРИКИ (Fast FoundationStereo, без сегментации) ===") +if rows: + maes = [r["mae"] for r in rows] + acc = sum(1 for r in rows if r["pred_cls"] == r["gt_cls"]) + print(f" габариты: MAE медиана {np.median(maes):.1f} мм, среднее {np.mean(maes):.1f}, " + f"худший {max(maes):.1f} ({max(rows, key=lambda r: r['mae'])['name']})") + print(f" классы: {acc}/{len(rows)} = {100.0*acc/len(rows):.0f}%") + print(f" k макс {max(r['pred_k'] for r in rows):.2f}") + lab = ["B", "C", "D"] + print(" матрица (строки истина, столбцы предсказание):") + print(" " + "".join(f"{c:>5s}" for c in lab)) + for a in lab: + print(f" {a:3s} " + "".join( + f"{sum(1 for r in rows if r['gt_cls']==a and r['pred_cls']==b):5d}" for b in lab)) +print(f" время: медиана {np.median(times):.0f} мс, такт {PITCH_S*1000:.0f} мс -> " + f"{'УКЛАДЫВАЕТСЯ' if np.median(times) < PITCH_S*1000 else 'НЕ УКЛАДЫВАЕТСЯ'} " + f"(запас {PITCH_S*1000 - np.median(times):+.0f} мс)") +json.dump(rows, open(f"{OUT}/metrics.json", "w"), indent=1, ensure_ascii=False) +print(f"\n -> {OUT}/metrics.json") diff --git a/control_test/measure_flow.py b/control_test/measure_flow.py new file mode 100644 index 0000000..9c7625b --- /dev/null +++ b/control_test/measure_flow.py @@ -0,0 +1,431 @@ +"""ЭТАП 2 потока: батчевый инференс по кадрам real-time прогона, метрики габаритов и классов. + +Стенд отдельным процессом, потому что torch внутри Isaac роняет процесс - то же разделение, +что у measure_roi.py. + +ЧТО ПРЕДСКАЗЫВАЕТСЯ, А ЧТО ТОЛЬКО СВЕРЯЕТСЯ. Из захвата берутся ТОЛЬКО кадры. Габариты, +округлость k и класс вычисляются из облака точек; разметка из манифеста используется +исключительно для подсчёта ошибки. Окно поиска и "воротный" пиксель выводятся из +калибровки камер и точки осмотра, а не из положения товара. + +БАТЧИРОВАНИЕ. Три рига обрабатываются одним вызовом на каждой ступени: +FastSAM получает список из трёх левых кадров, CREStereo - тензор (3, 3, H, W) с кропами, +дополненными до общего размера. Так GPU загружается один раз вместо трёх, и это то, что +позволяет уложиться в такт потока: при шаге 700 мм и 1 м/с на товар отводится 0.70 с. + +СХЕМА ROI - objroi, единственная проверенная: общая зона ленты ограничивает область +сегментации, CRE считается по bbox маски с запасом, причём левое и правое окно колонок +ОДНО И ТО ЖЕ, с расширением влево на максимальную диспаратность. Контрольный вариант с +независимым центрированием правого кропа разрушал 8 облаков из 9. + +RGB не маскируется до CRE - матчеру нужен фон вокруг предмета; маска применяется к +глубине, с эрозией. + +ОКРУГЛОСТЬ k считается из облака, а не читается: берётся сечение на середине высоты, +k = вписанный радиус / описанный. У круга 1.0, у квадрата 0.707, порог класса D - 0.8. +""" +import os, sys, json, time +import numpy as np +import cv2 +import torch +import torch.nn.functional as F + +CV = "/home/dasha/isaac_assets/cv" +CT = "/home/dasha/robozon-sorter/control_test" +CAP = f"{CT}/captures/flow" +MASK_MODE = os.environ.get("MASK_MODE", "gate") # gate | plane +K_MODE = os.environ.get("K_MODE", "hull") # hull | fit +TAG = os.environ.get("TAG", f"{MASK_MODE}+{K_MODE}") + +PITCH_S = 0.70 # такт потока: 700 мм при 1 м/с +dev = "cuda" + +sys.path.insert(0, CT) +import classify as CL + +man = json.load(open(f"{CAP}/manifest.json")) +calib = man["calib"] +TARGET = np.array(man["target"]); BELT_Z = float(TARGET[2]) +RIGS = sorted({c["rig"] for c in calib.values()}) + +os.environ["YOLO_CONFIG_DIR"] = "/tmp/Ultralytics" +sys.path.insert(0, f"{CV}/crestereo") +from nets import Model +cre = Model(max_disp=256, mixed_precision=False, test_mode=True) +cre.load_state_dict(torch.load(f"{CV}/crestereo/models/crestereo_eth3d.pth", + map_location="cpu"), strict=True) +cre.to(dev).eval() +SEG_MODEL = os.environ.get("SEG_MODEL", "fastsam") # fastsam | yolo +if SEG_MODEL == "yolo": + # yolo26n-seg: экземплярная сегментация с классами COCO. На конвейере товары в COCO + # почти не представлены, поэтому классы игнорируются - берутся только маски, а выбор + # кандидата остаётся тем же, что у FastSAM. Локально есть только вариант n (nano); + # yolo26-s-seg по ссылке пришлось бы скачивать. + from ultralytics import YOLO + fsam = YOLO(f"{CV}/yolo26n-seg.pt") +else: + from ultralytics import FastSAM + fsam = FastSAM(f"{CV}/FastSAM-s.pt") + +ZMIN, DPAD, PAD, VOX = 0.60, 1.35, 48, 0.004 +# Порог отбраковки ракурса. 0.06 м оказался ВРЕДЕН: MAE вырос 49.5 -> 65.4, классы +# 5/9 -> 3/9, потому что правило откидывало два вида из трёх и оставляло одиночный, +# который систематически занижает габарит из-за самозатенения. Центроиды трёх ригов +# расходятся ЗАКОННО: каждый видит свою обращённую к нему поверхность, и у выпуклого тела +# центр видимой части смещён к наблюдателю примерно на 2r/pi - для предмета 300 мм это +# уже 60-95 мм. Поэтому порог грубый: ловим только явные выбросы, когда сегментация взяла +# ленту (у bag было 141 и 230 мм). +REJECT_M = 0.15 + + +def cre_batch(pairs, iters=20): + """CREStereo одним проходом по нескольким парам кропов. + + Кропы у ригов разного размера, поэтому все дополняются нулями до общего (кратного 8) + и складываются в один тензор. Дополнение не влияет на результат: диспаратность + читается только внутри исходных границ каждого кропа. + """ + if not pairs: + return [] + hs = [p[0].shape[0] for p in pairs]; ws = [p[0].shape[1] for p in pairs] + Hp = (max(hs) + 7) // 8 * 8; Wp = (max(ws) + 7) // 8 * 8 + n = len(pairs) + Lb = np.zeros((n, 3, Hp, Wp), np.float32); Rb = np.zeros((n, 3, Hp, Wp), np.float32) + for i, (L, R) in enumerate(pairs): + 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(dev); iR = torch.from_numpy(Rb).to(dev) + dL = F.interpolate(iL, (Hp // 2, Wp // 2), mode="bilinear", align_corners=True) + dR = F.interpolate(iR, (Hp // 2, Wp // 2), mode="bilinear", align_corners=True) + with torch.inference_mode(): + f0 = cre(dL, dR, iters=iters, flow_init=None) + f = cre(iL, iR, iters=iters, flow_init=f0) + out = np.abs(f[:, 0].detach().cpu().numpy()) + return [out[i][:hs[i], :ws[i]] for i in range(n)] + + +def segment_batch(imgs, wins, gates): + """FastSAM одним вызовом по нескольким кадрам; для каждого - самая мелкая маска, + содержащая воротный пиксель.""" + subs, offs = [], [] + for img, (x0, y0, x1, y1) in zip(imgs, wins): + subs.append(img[y0:y1, x0:x1]); offs.append((x0, y0)) + if SEG_MODEL == "yolo": + res = fsam(subs, imgsz=1024, conf=0.10, iou=0.70, retina_masks=True, + device=dev, verbose=False) # низкий conf: классы COCO тут не подходят + else: + res = fsam(subs, imgsz=1024, conf=0.40, iou=0.90, retina_masks=True, + device=dev, verbose=False) + out = [] + for r, img, (x0, y0), (gx, gy) in zip(res, imgs, offs, gates): + H, W = img.shape[:2] + if r.masks is None: + out.append(None); continue + raw = r.masks.data.cpu().numpy().astype(bool) + hs, ws = raw.shape[1], raw.shape[2] + gx_, gy_ = gx - x0, gy - y0 + best = None + if MASK_MODE == "gate": + for m in raw: + if not (0 <= gy_ < hs and 0 <= gx_ < ws) or not m[gy_, gx_]: + continue + n = int(m.sum()) + if n < 200 or n > 0.5 * hs * ws: + continue + if best is None or n < best[0]: + best = (n, m) + else: + cam = CAM_BY_IMG[id(img)] + for m in raw: + n = int(m.sum()) + if n < 200 or n > BELT_FRAC * hs * ws: + continue # такой площади бывает только лента + yy, xx = np.nonzero(m) + u = float(xx.mean()) + x0; v = float(yy.mean()) + y0 + q = ray_to_belt(cam, u, v) + if q is None: + continue + d = float(np.hypot(q[0] - TARGET[0], q[1] - TARGET[1])) + if d > 0.45: + continue # не у точки осмотра + if best is None or d < best[0]: + best = (d, m) + if best is None: + out.append(None); continue + full = np.zeros((H, W), bool) + full[y0:y0 + hs, x0:x0 + ws] = best[1] + out.append(full) + return out + + +BELT_FRAC = 0.28 # доля окна: больше - это полотно, а не товар +CAM_BY_IMG = {} + + +def ray_to_belt(cam, u, v): + """пиксель -> точка на ПЛОСКОСТИ ЛЕНТЫ по калибровке. Глубина не нужна: плоскость + известна (z = BELT_Z), поэтому луч пересекается с ней аналитически.""" + M = np.array(cam["M"]) + d_cam = np.array([(u - cam["cx"]) / cam["fx"], -(v - cam["cy"]) / cam["fy"], -1.0, 0.0]) + o = (np.array([0.0, 0.0, 0.0, 1.0]) @ M)[:3] + d = (d_cam @ M)[:3] + if abs(d[2]) < 1e-9: + return None + t = (BELT_Z - o[2]) / d[2] + if t <= 0: + return None + return o + t * d + + +def belt_roi_px(cam, poly3): + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.c_[poly3, np.ones(len(poly3))] @ Minv)[:, :3]; z = -c[:, 2] + u = c[:, 0] / np.maximum(z, 1e-9) * cam["fx"] + cam["cx"] + v = -c[:, 1] / np.maximum(z, 1e-9) * cam["fy"] + cam["cy"] + return (int(max(0, np.floor(u.min()))), int(max(0, np.floor(v.min()))), + int(min(cam["width"], np.ceil(u.max()))), int(min(cam["height"], np.ceil(v.max())))) + + +def gate_px(cam): + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.r_[TARGET, 1.0] @ Minv)[:3]; z = -c[2] + return (int(np.clip(c[0] / z * cam["fx"] + cam["cx"], 0, cam["width"] - 1)), + int(np.clip(-c[1] / z * cam["fy"] + cam["cy"], 0, cam["height"] - 1))) + + +def backproj(disp, win, mask, cam): + """disp задана над окном `win` ЛЕВОГО кадра. Пиксельные координаты берутся ПОЛНОГО + кадра, поэтому интринсики остаются в силе и сдвигать cx/cy не нужно. + + Матрица камеры умножается СПРАВА и БЕЗ транспонирования: `P @ M`. Транспонирование + даёт правдоподобные по виду, но неверные координаты - облако уезжает за пределы + отсечения, и на выходе получается ноль точек при исправной сегментации и диспаратности. + """ + x0, y0, x1, y1 = win + depth = np.where(disp > 0.5, cam["fx"] * cam["baseline"] / np.maximum(disp, 1e-6), np.nan) + mc = cv2.erode(mask[y0:y1, x0:x1].astype(np.uint8), + np.ones((3, 3), np.uint8), iterations=2).astype(bool) + mc = mc[:depth.shape[0], :depth.shape[1]] + vs, us = np.mgrid[y0:y0 + depth.shape[0], x0:x0 + depth.shape[1]] + sel = mc & np.isfinite(depth) & (depth > 1e-3) + if sel.sum() < 30: + return np.zeros((0, 3)) + u, v, z = us[sel], vs[sel], depth[sel] + P = np.stack([(u - cam["cx"]) * z / cam["fx"], + -(v - cam["cy"]) * z / cam["fy"], -z, np.ones_like(z)], 1) + P = (P @ np.array(cam["M"]))[:, :3] + return P[(np.abs(P[:, 0] - TARGET[0]) < 0.30) & (np.abs(P[:, 1] - TARGET[1]) < 0.30) + & (P[:, 2] > BELT_Z + 0.006) & (P[:, 2] < BELT_Z + 0.60)] + + +def voxel(P, v=VOX): + k = np.floor(P / v).astype(np.int64) + _, idx = np.unique(k, axis=0, return_index=True) + return P[idx] + + +def dims_and_k(P): + """габариты и округлость из облака. k = вписанный/описанный радиус сечения.""" + if len(P) < 60: + return None + P = voxel(P) + c = P.mean(0); r = np.linalg.norm(P - c, axis=1) + P = P[r < np.percentile(r, 94)] + if len(P) < 40: + return None + h = (np.percentile(P[:, 2], 98) - BELT_Z) * 1000.0 + rect = cv2.minAreaRect(np.ascontiguousarray(P[:, :2].astype(np.float32))) + w, l = sorted([rect[1][0] * 1000.0, rect[1][1] * 1000.0]) + dims = sorted([h, w, l], reverse=True) + + # округлость по сечению на середине высоты + zmid = BELT_Z + (np.percentile(P[:, 2], 98) - BELT_Z) * 0.5 + band = P[np.abs(P[:, 2] - zmid) < 0.02][:, :2] + k = float("nan") + if K_MODE == "fit" and len(band) >= 25: + pts = band * 1000.0 + x_, y_ = pts[:, 0], pts[:, 1] + A = np.stack([x_, y_, np.ones_like(x_)], 1) + try: + sol, *_ = np.linalg.lstsq(A, x_ ** 2 + y_ ** 2, rcond=None) + cx, cy = sol[0] / 2, sol[1] / 2 + for _ in range(40): # геометрическое уточнение центра + dx, dy = x_ - cx, y_ - cy + rr = np.maximum(np.hypot(dx, dy), 1e-9) + R = rr.mean() + J = np.stack([-dx / rr, -dy / rr], 1) + st, *_ = np.linalg.lstsq(J, -(rr - R), rcond=None) + cx += st[0]; cy += st[1] + if abs(st[0]) + abs(st[1]) < 1e-6: + break + r = np.hypot(x_ - cx, y_ - cy) + lo, hi = np.percentile(r, 10), np.percentile(r, 90) + k = float(min(1.0, lo / hi)) if hi > 1e-6 else float("nan") + except np.linalg.LinAlgError: + pass + elif len(band) >= 25: + pts = (band - band.mean(0)) * 1000.0 + hull = cv2.convexHull(np.ascontiguousarray(pts.astype(np.float32))) + (_, _), R_out = cv2.minEnclosingCircle(hull) + if R_out > 1e-6: + s = 2.0 # 0.5 мм на пиксель + sh = int(np.ceil((pts.max() - pts.min()) * s)) + 20 + img = np.zeros((sh, sh), np.uint8) + poly = np.int32((hull.reshape(-1, 2) - pts.min()) * s + 10) + cv2.fillConvexPoly(img, poly, 255) + dt = cv2.distanceTransform(img, cv2.DIST_L2, 5) + r_in = float(dt.max()) / s + k = min(1.0, r_in / R_out) + return dims, k + + +# ---- общая зона ленты, видимая всеми шестью камерами ---- +g = 0.005 +xs = np.arange(TARGET[0] - 1.6, TARGET[0] + 1.6, g) +ys = np.arange(TARGET[1] - 1.6, TARGET[1] + 1.6, g) +X, Y = np.meshgrid(xs, ys) +G = np.c_[X.ravel(), Y.ravel(), np.full(X.size, BELT_Z)] +vis = np.ones(len(G), bool) +for cam in calib.values(): + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.c_[G, np.ones(len(G))] @ Minv)[:, :3]; z = -c[:, 2] + u = c[:, 0] / np.maximum(z, 1e-9) * cam["fx"] + cam["cx"] + v = -c[:, 1] / np.maximum(z, 1e-9) * cam["fy"] + cam["cy"] + vis &= (z > 1e-3) & (u >= 0) & (u < cam["width"]) & (v >= 0) & (v < cam["height"]) +cn, _ = cv2.findContours(vis.reshape(X.shape).astype(np.uint8), + cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) +pol = max(cn, key=cv2.contourArea).reshape(-1, 2) +POLY3 = np.c_[xs[pol[:, 0]], ys[pol[:, 1]], np.full(len(pol), BELT_Z)] + +for _ in range(2): # прогрев GPU + cre_batch([(np.zeros((160, 224, 3), np.float32), np.zeros((160, 224, 3), np.float32))]) + +print(f"[{TAG}] поток: {len(man['items'])} товаров, такт {PITCH_S:.2f} с " + f"(шаг 700 мм при 1 м/с), риги {', '.join(RIGS)}\n") +print(f" {'товар':18s} {'эталон, мм':>18s} {'предсказано, мм':>20s} {'MAE':>6s} " + f"{'k пред':>7s} {'класс':>11s} {'мс':>6s} виды") +print(" " + "-" * 96) + +rows, times = [], [] +for name, e in man["items"].items(): + t0 = time.time() + imgs, wins, gates, cams, rights = [], [], [], [], [] + for rig in RIGS: + camL = calib[f"{rig}_Left"] + IL = cv2.imread(e["files"][f"{rig}_Left"]) + IR = cv2.imread(e["files"][f"{rig}_Right"]) + if IL is None or IR is None: + continue + imgs.append(IL); rights.append(IR); cams.append(camL) + wins.append(belt_roi_px(camL, POLY3)); gates.append(gate_px(camL)) + CAM_BY_IMG[id(IL)] = camL + if not imgs: + continue + t_seg0 = time.time() + masks = segment_batch(imgs, wins, gates) + t_seg = (time.time() - t_seg0) * 1000 + + pairs, metas = [], [] + for IL, IR, cam, mask in zip(imgs, rights, cams, masks): + if mask is None: + continue + H, W = IL.shape[:2] + ys_, xs_ = np.where(mask) + maxd = int(np.ceil(DPAD * cam["fx"] * cam["baseline"] / ZMIN)) + y0 = max(0, ys_.min() - PAD); y1 = min(H, ys_.max() + PAD) + x1 = min(W, xs_.max() + PAD); x0 = max(0, xs_.min() - PAD - maxd) + pairs.append((IL[y0:y1, x0:x1].astype(np.float32), + IR[y0:y1, x0:x1].astype(np.float32))) + metas.append(((x0, y0, x1, y1), mask, cam)) + t_cre0 = time.time() + disps = cre_batch(pairs) + t_cre = (time.time() - t_cre0) * 1000 + + clouds, cnames = [], [] + for disp, (win, mask, cam) in zip(disps, metas): + P = backproj(disp, win, mask, cam) + if P is not None and len(P): + clouds.append(P); cnames.append(cam["rig"]) + + # ОТБРАКОВКА РАКУРСА ПО 3D-ЦЕНТРОИДУ. + # Слияние идёт по калиброванным экстринсикам, поэтому три облака одного предмета + # обязаны лежать в одном месте. Если сегментация одного рига прихватила ленту, его + # центроид уезжает, и после слияния габарит раздувается: у bag вышло 530 мм при + # истинных 202, у bucket 603 при 287 - и раздутое сечение перестаёт быть круглым, + # из-за чего оба класса D ушли в C. + # Берётся ПОКООРДИНАТНАЯ МЕДИАНА центроидов (устойчива при трёх видах: один выброс + # её не двигает) и выбрасываются виды дальше REJECT_M от неё. + dropped = [] + if len(clouds) >= 2: + cent = np.array([c.mean(0) for c in clouds]) + med = np.median(cent, axis=0) + off = np.linalg.norm(cent - med, axis=1) + keep = off <= REJECT_M + # никогда не опускаться ниже двух видов: слияние двух ракурсов лучше одиночного + if keep.sum() < 2 and len(clouds) >= 2: + keep = np.zeros(len(clouds), bool) + keep[np.argsort(off)[:2]] = True + if 2 <= keep.sum() < len(clouds): + dropped = [(cnames[i], round(float(off[i]) * 1000)) for i in range(len(clouds)) + if not keep[i]] + clouds = [c for c, k in zip(clouds, keep) if k] + cnames = [n for n, k in zip(cnames, keep) if k] + dt = (time.time() - t0) * 1000 + times.append(dt) + + gt = e["gt"] + gt_dims = sorted(gt["dims_mm"], reverse=True) + if not clouds: + nm = sum(1 for m in masks if m is not None) + print(f" {name:18s} {str([round(v) for v in gt_dims]):>18s} " + f" масок {nm}/{len(imgs)}, пар в CRE {len(pairs)}, облаков 0") + rows.append(dict(name=name, gt=gt_dims, gt_cls=gt["zone_scene"], + gt_label=gt["zone_label"], pred=None)) + continue + out = dims_and_k(np.vstack(clouds)) + if out is None: + print(f" {name:18s} {'габариты не взялись':>40s}"); continue + dims, k = out + mae = float(np.mean(np.abs(np.array(dims) - np.array(gt_dims)))) + pred_cls = CL.classify(dims, 0.0 if np.isnan(k) else k) + ok = "верно" if pred_cls == gt["zone_scene"] else f"ОШИБКА({gt['zone_scene']})" + drp = ("" if not dropped else + " откинут " + ", ".join(f"{n} ({d} мм)" for n, d in dropped)) + print(f" {name:18s} {str([round(v) for v in gt_dims]):>18s} " + f"{str([round(v) for v in dims]):>20s} {mae:6.1f} {k:7.2f} " + f"{pred_cls + ' ' + ok:>11s} {dt:6.0f} {len(clouds)}в{drp}") + rows.append(dict(name=name, gt=gt_dims, gt_cls=gt["zone_scene"], gt_label=gt["zone_label"], gt_k=gt.get("k"), + pred=[round(v, 1) for v in dims], pred_k=round(float(k), 3), + pred_cls=pred_cls, mae=round(mae, 1), ms=round(dt), + views=len(clouds), dropped=dropped, + t_seg=round(t_seg), t_cre=round(t_cre))) + +got = [r for r in rows if r.get("pred")] +print("\n === МЕТРИКИ ===") +if got: + maes = [r["mae"] for r in got] + print(f" габариты: MAE медиана {np.median(maes):.1f} мм, среднее {np.mean(maes):.1f}, " + f"худший {max(maes):.1f} ({max(got, key=lambda r: r['mae'])['name']})") + acc = sum(1 for r in got if r["pred_cls"] == r["gt_cls"]) + accl = sum(1 for r in got if r["pred_cls"] == r.get("gt_label")) + print(f" классы против геометрии сцены: {acc}/{len(got)} = {100.0*acc/len(got):.0f}%") + print(f" классы против паспортной метки: {accl}/{len(got)} = {100.0*accl/len(got):.0f}%" + f" (расходятся там, где метка не соответствует мешу)") + lab = ["B", "C", "D"] + print(" матрица (строки - истина, столбцы - предсказание):") + print(" " + "".join(f"{c:>5s}" for c in lab)) + for a in lab: + row = [sum(1 for r in got if r["gt_cls"] == a and r["pred_cls"] == b) for b in lab] + print(f" {a:3s} " + "".join(f"{v:5d}" for v in row)) + print(f" облако собрано: {len(got)}/{len(rows)}") +if times: + print(f"\n время на товар: медиана {np.median(times):.0f} мс, худшее {max(times):.0f} мс") + print(f" такт потока {PITCH_S*1000:.0f} мс -> " + f"{'УКЛАДЫВАЕТСЯ' if np.median(times) < PITCH_S*1000 else 'НЕ УКЛАДЫВАЕТСЯ'} " + f"(запас {PITCH_S*1000 - np.median(times):+.0f} мс)") + if got: + print(f" из них сегментация {np.median([r['t_seg'] for r in got]):.0f} мс, " + f"CREStereo {np.median([r['t_cre'] for r in got]):.0f} мс") +json.dump(rows, open(f"{CAP}/flow_metrics_{TAG}.json", "w"), indent=1, ensure_ascii=False) +print(f"\n -> {CAP}/flow_metrics.json") diff --git a/control_test/measure_plane.py b/control_test/measure_plane.py new file mode 100644 index 0000000..8c9cc53 --- /dev/null +++ b/control_test/measure_plane.py @@ -0,0 +1,561 @@ +"""ВАРИАНТ БЕЗ СЕГМЕНТАЦИИ: CRE-Stereo по всему ROI, товар отделяется от полотна по высоте. + +Зачем. Разбор показал, что и раздутые габариты, и непойманный класс D идут от сегментации: +у bag, bucket и detergent FastSAM выделял ПОЛОТНО, а не товар (маска 174-267 тыс. px против +17-143 тыс. у нормальных, выход точек падал с 96 % до 0.5-6.6 %). На кадре видно, что ведро +не выделено ни одной маской - залито всё полотно. + +Здесь сегментации нет вовсе. CRE считается по общей зоне ленты, диспаратность переводится +в 3D, и товаром считается то, что ВЫСТУПАЕТ над плоскостью полотна выше порога. Плоскость +известна из калибровки (z = BELT_Z), поэтому её не надо ни искать, ни подгонять. + +Это тот же приём, что дал рабочий результат на физическом стенде: там отказ от сегментации +снял две ошибки сразу - выбор маски и проекцию силуэта. + +Порог над полотном 12 мм: измеренный разброс самого полотна в облаке меньше, а товары +здесь от 122 мм высотой. Дополнительно точки чистятся по связности в плане, чтобы соседний +товар потока (при шаге 700 мм он попадает в кадр) не приклеился к целевому. +""" +import os, sys, json, time +import numpy as np +import cv2 +import torch + +CV = "/home/dasha/isaac_assets/cv" +CT = "/home/dasha/robozon-sorter/control_test" +CAP = f"{CT}/captures/flow" +OUT = f"{CT}/diag/plane" +os.makedirs(OUT, exist_ok=True) +sys.path.insert(0, CT) +os.environ.setdefault("MASK_MODE", "gate") +import measure_flow as MF # берём cre_batch, backproj-математику, POLY3 +import classify as CL + +man = MF.man; calib = MF.calib +TARGET, BELT_Z, RIGS = MF.TARGET, MF.BELT_Z, MF.RIGS +FLOOR = 0.020 # порог над полотном, м +DENS_CELL = 0.010 # ячейка сетки плотности, м +DENS_MIN = 12 # точек в ячейке, чтобы считать её товаром +GATE_R = 0.28 # радиус вокруг точки осмотра, м - отсекает соседа по потоку +PITCH_S = 0.70 + +RIGCOL = {RIGS[0]: (0, 140, 255), RIGS[1]: (60, 230, 90), RIGS[2]: (240, 120, 240)} + + +def cloud_from_roi(disp, win, cam): + """всё окно -> 3D, без маски. Товар выделяется превышением над полотном.""" + x0, y0, x1, y1 = win + depth = np.where(disp > 0.5, cam["fx"] * cam["baseline"] / np.maximum(disp, 1e-6), np.nan) + vs, us = np.mgrid[y0:y0 + depth.shape[0], x0:x0 + depth.shape[1]] + ok = np.isfinite(depth) & (depth > 1e-3) + u, v, z = us[ok], vs[ok], depth[ok] + P = np.stack([(u - cam["cx"]) * z / cam["fx"], + -(v - cam["cy"]) * z / cam["fy"], -z, np.ones_like(z)], 1) + P = (P @ np.array(cam["M"]))[:, :3] + near = (np.hypot(P[:, 0] - TARGET[0], P[:, 1] - TARGET[1]) < GATE_R) + above = (P[:, 2] > BELT_Z + FLOOR) & (P[:, 2] < BELT_Z + 0.60) + return P[near & above], P[near & (P[:, 2] > BELT_Z - 0.03) & (P[:, 2] <= BELT_Z + FLOOR)] + + +def dense_only(P, cell=DENS_CELL, need=DENS_MIN): + """оставить только ПЛОТНЫЕ ячейки. + + Снимок облака показал, из чего состоит ошибка: сам товар - плотное пятно верного + размера, а вокруг него разреженный ореол на все 556 x 517 мм. Это точки полотна, + пережившие порог по высоте, и борта ленты. Связность их склеивает с товаром в один + сгусток, и габарит раздувается до размера зоны осмотра. + + Полотно даёт РЕДКИЕ выбросы (шум диспаратности на однородной поверхности), товар - + сплошную поверхность. Поэтому отбор идёт по числу точек в ячейке, а не по связности. + """ + if len(P) < 60: + return np.ones(len(P), bool) + key = np.floor(P[:, :3] / cell).astype(np.int64) + key2 = key[:, :2] + uniq, inv, cnt = np.unique(key2, axis=0, return_inverse=True, return_counts=True) + return cnt[inv] >= need + + +def biggest_blob_idx(P, grid=0.012): + """самый крупный связный сгусток в плане - целевой товар, а не сосед потока""" + if len(P) < 40: + return np.ones(len(P), bool) + cx = np.round(P[:, 0] / grid).astype(int); cy = np.round(P[:, 1] / grid).astype(int) + im = np.zeros((cy.max() - cy.min() + 3, cx.max() - cx.min() + 3), np.uint8) + im[cy - cy.min() + 1, cx - cx.min() + 1] = 255 + im = cv2.morphologyEx(im, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8)) + n, lab = cv2.connectedComponents(im) + li = lab[cy - cy.min() + 1, cx - cx.min() + 1] + best, bn = None, 0 + for k in range(1, n): + m = li == k + if m.sum() > bn: + best, bn = m, m.sum() + return np.ones(len(P), bool) if best is None else best + + +def scatter(P, cols, ax=(0, 1), w=460, h=460, title=""): + a, b = ax + x, y = P[:, a], P[:, b] + pad = 26 + sx = (w - 2 * pad) / max(1e-6, x.max() - x.min()) + sy = (h - 2 * pad) / max(1e-6, y.max() - y.min()) + s = min(sx, sy) + img = np.zeros((h, w, 3), np.uint8) + for (px, py), c in zip(np.stack([x, y], 1), cols): + u = int((px - x.min()) * s) + pad + v = h - (int((py - y.min()) * s) + pad) + if 0 <= u < w and 0 <= v < h: + cv2.circle(img, (u, v), 1, (int(c[0]), int(c[1]), int(c[2])), -1) + cv2.putText(img, title, (8, 18), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (210, 210, 210), 1) + cv2.putText(img, f"{(x.max()-x.min())*1000:.0f} x {(y.max()-y.min())*1000:.0f} mm", + (8, h - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (150, 150, 150), 1) + return img + + + +def k_three_sections(P): + """k по ТРЁМ взаимно перпендикулярным сечениям, берётся максимум. + + Правило класса D в README: "k > 0.8 ХОТЯ БЫ В ОДНОМ СЕЧЕНИИ". До сих пор считалось + только горизонтальное сечение на середине высоты, и этого достаточно лишь для тел, + стоящих вертикально. Ведро на прогоне ЛЕЖИТ НА БОКУ: его горизонтальный срез - это + прямоугольник "длина x хорда", и k = 0.43 для него верен, просто отвечает не на тот + вопрос. Круглое сечение лежащего цилиндра - вертикальное, поперёк оси. + + Замерено, что дело не в камерах: восстановленное полотно садится на эталонную + плоскость со смещением 0.6-1.1 мм и наклоном 0.35-1.72 градуса по всем трём ригам, + а дуга у ведра покрыта на 295-360 градусов. То есть ни интринсики, ни расположение + ригов, ни слияние тут ни при чём - не хватало именно второго и третьего сечения. + """ + if len(P) < 60: + return float("nan"), -1 + best, bax = float("nan"), -1 + for ax in range(3): # секущая плоскость перпендикулярна оси ax + keep = [i for i in range(3) if i != ax] + lo, hi = np.percentile(P[:, ax], 2), np.percentile(P[:, ax], 98) + mid = (lo + hi) / 2.0 + half = max(0.012, (hi - lo) * 0.08) + band = P[np.abs(P[:, ax] - mid) < half][:, keep] + if len(band) < 25: + continue + pts = (band - band.mean(0)) * 1000.0 + hull = cv2.convexHull(np.ascontiguousarray(pts.astype(np.float32))) + (_, _), R_out = cv2.minEnclosingCircle(hull) + if R_out < 1e-6: + continue + sc = 2.0 + sh = int(np.ceil((pts.max() - pts.min()) * sc)) + 20 + if sh < 8 or sh > 4000: + continue + img = np.zeros((sh, sh), np.uint8) + cv2.fillConvexPoly(img, np.int32((hull.reshape(-1, 2) - pts.min()) * sc + 10), 255) + k = min(1.0, float(cv2.distanceTransform(img, cv2.DIST_L2, 5).max()) / sc / R_out) + if np.isnan(best) or k > best: + best, bax = k, ax + return best, bax + + + +K_SECTORS = 48 # угловых секторов, 7.5 градуса каждый +K_MIN_PER = 4 # точек в секторе, иначе сектор не в счёт + + +def k_smooth(P, mode="h"): + """k по СГЛАЖЕННОМУ контуру сечения: радиус как функция угла, в секторе - медиана. + + Прежний k брался как вписанный радиус к описанному по выпуклой ОБОЛОЧКЕ. Оболочка + строится по крайним точкам, а край товара размазан шумом диспаратности: разброс самого + полотна замерен в 7-10 мм, и такой же разброс сидит на границе предмета. Показатель + страдает дважды - выброс наружу увеличивает R_out, выброс внутрь уменьшает r_in, + поэтому k занижался у всего: у коробки с истинным 0.72 читался 0.59, у ведра с 0.99 - 0.43. + + Здесь контур сглаживается по углу: точки сечения разбиваются на сектора вокруг центра, + в каждом берётся МЕДИАНА радиуса, и k считается по этим медианам. Единичный выброс в + секторе из десятков точек не проходит. Смысл сохраняется: у круга профиль радиуса + плоский -> k ~ 1, у квадрата меняется от a до a*sqrt2 -> k ~ 0.707. + + Центр берётся медианой координат, а не средним: среднее тянется в сторону той дуги, + где точек больше. + """ + if len(P) < 60: + return float("nan") + top = np.percentile(P[:, 2], 98) + zmid = BELT_Z + (top - BELT_Z) * 0.5 + band = P[np.abs(P[:, 2] - zmid) < 0.02][:, :2] + if len(band) < 40: + return float("nan") + c = np.median(band, axis=0) + d = (band - c) * 1000.0 + r = np.hypot(d[:, 0], d[:, 1]) + a = np.arctan2(d[:, 1], d[:, 0]) + idx = ((a + np.pi) / (2 * np.pi) * K_SECTORS).astype(int) % K_SECTORS + prof = [] + for i in range(K_SECTORS): + m = idx == i + if m.sum() >= K_MIN_PER: + prof.append(np.median(r[m])) + if len(prof) < K_SECTORS // 2: # дуга меньше половины круга - судить нельзя + return float("nan") + prof = np.array(prof) + lo, hi = np.percentile(prof, 10), np.percentile(prof, 90) + return float(min(1.0, lo / hi)) if hi > 1e-6 else float("nan") + + + +# --------------------------------------------------------------------------------------- +# ПРОВЕРЕННЫЙ ПОКАЗАТЕЛЬ КРУГОВОГО СЕЧЕНИЯ, перенесён из isaac_assets/cv/circular_section.py +# +# Мои три попытки поднять k провалились подряд (выпуклая оболочка по одному горизонтальному +# сечению 0.85, три мировых сечения 0.83, сглаживание по секторам 0.67), и все три отличались +# от этой реализации одним и тем же: они резали облако по МИРОВЫМ осям и на ОДНОЙ высоте. +# +# Здесь облако сначала выравнивается по СОБСТВЕННЫМ главным осям (SVD), и только потом +# режется - на пяти высотах вдоль каждой из трёх осей, толщиной 7 % размаха. Для лежащего +# на боку ведра главная ось это и есть ось цилиндра, поэтому перпендикулярное сечение +# оказывается тем самым кругом; при резке по мировым осям такого сечения не существует. +# +# Плюс защита от вытянутых пятен: невязка подгонки окружности (Kasa) должна быть < 0.15, +# иначе скруглённый овал прошёл бы как круг. +# +# На эталонной геометрии мешей эта версия давала bucket 0.934 при истинных 0.995, bag 0.889 +# при 0.896, и корректно отвергала коробки (0.685-0.688 при пороге 0.8). +# --------------------------------------------------------------------------------------- +K_ROUND = 0.80 + +STEREO = os.environ.get("STEREO", "defom") # cre | defom (бейзлайн: defom) +DEFOM_CKPT = os.environ.get("DEFOM_CKPT", "vitl") # vitl | vits +DEFOM_ITERS = int(os.environ.get("DEFOM_ITERS", "24")) +DEFOM_SITERS = int(os.environ.get("DEFOM_SITERS", "8")) +_defom = None + + +def _defom_model(): + global _defom + if _defom is None: + import types, torch + DR = "/home/dasha/isaac_assets/cv/defom-stereo" + if DR not in sys.path: + sys.path.insert(0, DR); sys.path.insert(0, DR + "/core") + from core.defom_stereo import DEFOMStereo + a = types.SimpleNamespace( + dinov2_encoder=DEFOM_CKPT, idepth_scale=0.5, hidden_dims=[128] * 3, + corr_implementation="reg", shared_backbone=False, corr_levels=2, corr_radius=4, + scale_list=[0.125, 0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 2.0], scale_corr_radius=2, + n_downsample=2, context_norm="batch", n_gru_layers=3, mixed_precision=False) + m = DEFOMStereo(a) + ck = torch.load(f"{DR}/checkpoints/defomstereo_{DEFOM_CKPT}_sceneflow.pth", + map_location="cpu", weights_only=False) + m.load_state_dict({k.replace("module.", ""): v for k, v in ck.items()}, strict=False) + _defom = m.to("cuda").eval() + return _defom + + +def defom_batch(pairs, iters=None, scale_iters=None): + """DEFOM одним проходом по нескольким парам. Кропы дополняются нулями до общего размера, + кратного 32; диспаратность читается только внутри исходных границ каждого кропа.""" + import torch + if not pairs: + return [] + m = _defom_model() + iters = DEFOM_ITERS if iters is None else iters + scale_iters = DEFOM_SITERS if scale_iters is None else scale_iters + hs = [q[0].shape[0] for q in pairs]; ws = [q[0].shape[1] for q in pairs] + Hp = (max(hs) + 31) // 32 * 32; Wp = (max(ws) + 31) // 32 * 32 + n = len(pairs) + A = np.zeros((n, 3, Hp, Wp), np.float32); B = np.zeros((n, 3, Hp, Wp), np.float32) + for i, (L, R) in enumerate(pairs): + h, w = L.shape[:2] + A[i, :, :h, :w] = L.transpose(2, 0, 1) + B[i, :, :h, :w] = R.transpose(2, 0, 1) + iL = torch.from_numpy(A).cuda(); iR = torch.from_numpy(B).cuda() + with torch.inference_mode(): + d = m(iL, iR, iters=iters, scale_iters=scale_iters, test_mode=True) + d = np.abs(d.squeeze(1).detach().cpu().numpy()) + return [d[i][:hs[i], :ws[i]] for i in range(n)] + + + +CROP = os.environ.get("CROP", "1") == "1" # считать CRE по КРОПУ зоны осмотра, а не всей ленты +CROP_H = 0.45 # запас по высоте товара, м +SW = int(os.environ.get("SW", "480")) # ширина входа сети (0 - без понижения) + + +def cre_scaled(pairs, sw): + """CRE на пониженном разрешении с обратным масштабом диспаратности. + + Приём взят из прежнего DEFOM-скрипта проекта (flow_defom_cache.py): кадр сжимается до + ширины sw, сеть считает по нему, диспаратность возвращается к исходному размеру и + УМНОЖАЕТСЯ на W/sw. Диспаратность измеряется в пикселях, поэтому при сжатии кадра в k + раз она сжимается во столько же - без этого множителя глубина уехала бы ровно в k раз. + Пропорции сохраняются: масштаб по обеим осям один, иначе ломается эпиполярная геометрия. + """ + engine = defom_batch if STEREO == "defom" else MF.cre_batch + if sw <= 0: + return engine(pairs) + small, meta = [], [] + for (L, R) in pairs: + h, w = L.shape[:2] + sh = max(8, int(round(h * sw / w / 8)) * 8) + small.append((cv2.resize(L, (sw, sh)), cv2.resize(R, (sw, sh)))) + meta.append((w, h, w / float(sw))) + ds = engine(small) + out = [] + for d, (w, h, f) in zip(ds, meta): + out.append(cv2.resize(d, (w, h), interpolation=cv2.INTER_LINEAR) * f) + return out + + +def gate_crop_px(cam, r=GATE_R, hmax=CROP_H, pad=24): + """окно кадра, куда проецируется зона осмотра - цилиндр радиусом r над лентой. + + Зона ленты, по которой сейчас считается CRE, занимает 84-92 % кадра, хотя товар всегда + внутри круга радиусом 280 мм вокруг точки осмотра. Проекция этого круга (с запасом по + высоте на сам товар) даёт окно в разы меньше, и стереосети достаётся во столько же раз + меньше пикселей. Локализация тут ГЕОМЕТРИЧЕСКАЯ - ни сегментации, ни грубого прохода + не нужно, поэтому лишнего вызова сети не появляется. + """ + th = np.linspace(0, 2 * np.pi, 24, endpoint=False) + ring = np.c_[TARGET[0] + r * np.cos(th), TARGET[1] + r * np.sin(th)] + pts = np.vstack([np.c_[ring, np.full(len(ring), BELT_Z)], + np.c_[ring, np.full(len(ring), BELT_Z + hmax)]]) + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.c_[pts, np.ones(len(pts))] @ Minv)[:, :3] + z = -c[:, 2] + u = c[:, 0] / np.maximum(z, 1e-9) * cam["fx"] + cam["cx"] + v = -c[:, 1] / np.maximum(z, 1e-9) * cam["fy"] + cam["cy"] + x0 = int(max(0, np.floor(u.min()) - pad)); x1 = int(min(cam["width"], np.ceil(u.max()) + pad)) + y0 = int(max(0, np.floor(v.min()) - pad)); y1 = int(min(cam["height"], np.ceil(v.max()) + pad)) + return x0, y0, x1, y1 + + + +LR_THR = float(os.environ.get("LR_THR", "0")) # порог проверки лево-право, px (0 = выкл) +CYL = os.environ.get("CYL", "0") == "1" # подгонка цилиндра как второй признак +CYL_TOL = 0.008 # допуск на радиус, м +CYL_MIN_INLIER = 0.60 # доля точек в допуске, чтобы счесть цилиндром + + +def disp_lr(L, R): + """диспаратность в обе стороны одним батчем: прямая пара и зеркально отражённая. + + Отражение по горизонтали превращает задачу "справа налево" в обычную "слева направо", + поэтому вторую карту даёт та же сеть без правок: отражаем оба кадра, меняем их местами, + считаем, отражаем результат обратно. + """ + Lf = np.ascontiguousarray(L[:, ::-1]) + Rf = np.ascontiguousarray(R[:, ::-1]) + dL, dRf = MF.cre_batch([(L, R), (Rf, Lf)]) + dR = np.ascontiguousarray(dRf[:, ::-1]) + return dL, dR + + +def lr_valid(dL, dR, thr): + """маска согласованных точек: d_L(x) должна совпасть с d_R в точке x - d_L(x).""" + h, w = dL.shape + xs = np.arange(w)[None, :].repeat(h, 0) + xr = np.rint(xs - dL).astype(int) + ok = (xr >= 0) & (xr < w) + xr = np.clip(xr, 0, w - 1) + dRs = np.take_along_axis(dR, xr, axis=1) + return ok & (np.abs(dL - dRs) <= thr) + + +def cylinder_ransac(P, tol=CYL_TOL, n_axis=64, seed=0): + """RANSAC по МОДЕЛИ цилиндра (не по совмещению облаков). + + Ось ищется перебором направлений: главные оси облака плюс случайные. Точки проецируются + на плоскость, перпендикулярную оси, туда подгоняется окружность, и считается доля точек, + чей радиус попал в допуск. Нормали не нужны - они на рыхлом облаке сами шумят. + """ + if len(P) < 200: + return 0.0, float("nan"), float("nan") + rng = np.random.default_rng(seed) + Q = P - P.mean(0) + _, _, V = np.linalg.svd(Q, full_matrices=False) + axes = [V[0], V[1], V[2]] + for _ in range(n_axis): + v = rng.normal(size=3); axes.append(v / (np.linalg.norm(v) + 1e-12)) + best = (0.0, float("nan"), float("nan")) + for d in axes: + d = d / (np.linalg.norm(d) + 1e-12) + a = np.array([1.0, 0.0, 0.0]) + if abs(d @ a) > 0.9: + a = np.array([0.0, 1.0, 0.0]) + e1 = np.cross(d, a); e1 /= np.linalg.norm(e1) + e2 = np.cross(d, e1) + xy = np.c_[Q @ e1, Q @ e2] + x, y = xy[:, 0], xy[:, 1] + A = np.c_[2 * x, 2 * y, np.ones(len(x))] + try: + sol, *_ = np.linalg.lstsq(A, x * x + y * y, rcond=None) + except np.linalg.LinAlgError: + continue + cx, cy, cc = sol + R = np.sqrt(max(cc + cx * cx + cy * cy, 1e-12)) + r = np.hypot(x - cx, y - cy) + inl = float(np.mean(np.abs(r - R) <= tol)) + if inl > best[0]: + best = (inl, float(R), float(np.median(np.abs(r - R)))) + return best + + + +def _kasa(P): + x,y=P[:,0],P[:,1]; A=np.c_[2*x,2*y,np.ones(len(x))]; b=x*x+y*y + s,*_=np.linalg.lstsq(A,b,rcond=None); cx,cy,cc=s; r=np.sqrt(max(cc+cx*cx+cy*cy,1e-12)) + return cx,cy,r,np.abs(np.hypot(x-cx,y-cy)-r).mean() + + +def section_K(xy): + """True r_inscribed/R_circumscribed of a full cross-section outline via the convex-hull + incenter (largest inscribed circle) and the circumscribed radius from that centre. + K=1 for a circle, b/a for an ellipse, 0.707 for a square, short/long for a rectangle.""" + from scipy.spatial import ConvexHull + if len(xy)<20: return None,0.0,1.0 + try: h=ConvexHull(xy) + except Exception: return None,0.0,1.0 + V=xy[h.vertices] # CCW hull vertices + A=V; B=np.roll(V,-1,axis=0); E=B-A; L=np.linalg.norm(E,axis=1)+1e-12 + mn=xy.min(0); mx=xy.max(0) + G=np.stack(np.meshgrid(np.linspace(mn[0],mx[0],40),np.linspace(mn[1],mx[1],40)),-1).reshape(-1,2) + # signed distance from each grid point to each hull edge (CCW -> interior side positive) + d=(E[:,0][None,:]*(G[:,1][:,None]-A[:,1][None,:]) - E[:,1][None,:]*(G[:,0][:,None]-A[:,0][None,:]))/L[None,:] + inside=(d>0).all(1) + if inside.sum()<3: return 0.0,1.0,1.0 + rin=float(d[inside].min(1).max()) # max inscribed circle radius (its own centre) + # min enclosing circle radius (its own centre): grid centre minimising max distance to hull vertices + Gd=np.stack(np.meshgrid(np.linspace(mn[0],mx[0],48),np.linspace(mn[1],mx[1],48)),-1).reshape(-1,2) + Rout=float(np.linalg.norm(Gd[:,None,:]-V[None,:,:],axis=2).max(1).min()) + return rin/max(Rout,1e-9),1.0,0.0 + + +def circular_section_K(points): + """Max K over cross-sections sampled along each principal axis (a circle in ANY section + -> round). Returns (max_K, is_round, best_section).""" + if len(points)<60: return 0.0,False,None + c=points.mean(0); Q=points-c; _,_,V=np.linalg.svd(Q,full_matrices=False); proj=Q@V.T + best=0.0; best_sec=None + for a in range(3): + o=[i for i in range(3) if i!=a]; ca=proj[:,a]; sp=np.ptp(ca)+1e-9 + for frac in (0.25,0.375,0.5,0.625,0.75): # sample slices along the axis + lvl=np.percentile(ca,frac*100) + sl=proj[np.abs(ca-lvl)<0.07*sp][:,o] + K,cov,rr=section_K(sl) + if K is not None and rr<0.15 and K>best: + best=K; best_sec=(a,round(frac,2),round(cov,2),rr) + return round(best,3), (best>K_ROUND), best_sec + + + +# При импорте из рабочего процесса замкнутого контура прогон по девяти товарам не нужен - +# нужны только функции (defom_batch, gate_crop_px, cloud_from_roi, circular_section_K...). +if os.environ.get("IMPORT_ONLY") == "1": + import sys as _s + _s.modules[__name__].__dict__.setdefault("_ready", True) +else: + print(f"БЕЗ СЕГМЕНТАЦИИ: CRE по зоне ленты, товар выше полотна на {FLOOR*1000:.0f} мм | " + f"проверка лево-право {LR_THR if LR_THR>0 else 'выкл'} px | цилиндр {'вкл' if CYL else 'выкл'}" + f" | окно {'КРОП зоны осмотра' if CROP else 'вся лента'}" + f" | вход сети {SW if SW>0 else 'исходный'} | движок {STEREO.upper()}\n") + print(f" {'товар':18s} {'эталон, мм':>18s} {'предсказано, мм':>20s} {'MAE':>6s} " + f"{'k':>6s} {'ось':>3s} {'класс':>12s} {'точек':>7s} {'мс':>6s}") + print(" " + "-" * 100) + + rows, times = [], [] + FOCUS = {"bag", "bucket", "box_300x200x200"} + for name, e in man["items"].items(): + t0 = time.time() + pairs, metas = [], [] + for rig in RIGS: + camL = calib[f"{rig}_Left"] + IL = cv2.imread(e["files"][f"{rig}_Left"]) + IR = cv2.imread(e["files"][f"{rig}_Right"]) + if IL is None or IR is None: + continue + x0, y0, x1, y1 = (gate_crop_px(camL) if CROP else MF.belt_roi_px(camL, MF.POLY3)) + maxd = int(np.ceil(MF.DPAD * camL["fx"] * camL["baseline"] / MF.ZMIN)) + x0 = max(0, x0 - maxd) # запас влево на максимальную диспаратность: правый + # двойник обязан попасть в ТО ЖЕ окно колонок + pairs.append((IL[y0:y1, x0:x1].astype(np.float32), IR[y0:y1, x0:x1].astype(np.float32))) + metas.append(((x0, y0, x1, y1), camL, rig, IL)) + if LR_THR > 0: + # обе карты считаются ДО проекции: несогласованные пиксели вообще не становятся + # точками, а не удаляются потом из облака + disps, rejected = [], [] + for (L, R) in pairs: + dL, dR = disp_lr(L, R) + good = lr_valid(dL, dR, LR_THR) + rejected.append(1.0 - float(good.mean())) + dd = dL.copy(); dd[~good] = 0.0 # 0 -> пиксель не пройдёт порог disp > 0.5 + disps.append(dd) + else: + disps = cre_scaled(pairs, SW); rejected = [] + clouds, cols, belts = [], [], [] + for disp, (win, cam, rig, IL) in zip(disps, metas): + obj, belt = cloud_from_roi(disp, win, cam) + if len(obj): + clouds.append(obj); cols += [RIGCOL[rig]] * len(obj) + belts.append(len(belt)) + if name in FOCUS: + dn = cv2.normalize(disp, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) + hm = cv2.applyColorMap(dn, cv2.COLORMAP_TURBO) + cv2.imwrite(f"{OUT}/{name}_{rig}_disp.png", hm) + dt = (time.time() - t0) * 1000 + times.append(dt) + gt = e["gt"]; gtd = sorted(gt["dims_mm"], reverse=True) + if not clouds: + print(f" {name:18s} {str([round(v) for v in gtd]):>18s} облако пустое"); continue + P = np.vstack(clouds); C = list(cols) + # самый крупный сгусток в плане: при шаге 700 мм сосед потока попадает в кадр + sel = dense_only(P) + if sel.sum() >= 60: + P = P[sel]; C = [C[i] for i in np.nonzero(sel)[0]] + sel = biggest_blob_idx(P) + P = P[sel]; C = [C[i] for i in np.nonzero(sel)[0]] + out = MF.dims_and_k(P) + if out is None: + print(f" {name:18s} габариты не взялись"); continue + dims, _k_hull = out + k, _is_round, _sec = circular_section_K(P) + kax = _sec[0] if _sec else -1 + if not k: + k = _k_hull + cyl_inl = cyl_R = float("nan") + if CYL: + cyl_inl, cyl_R, cyl_res = cylinder_ransac(P) + if cyl_inl >= CYL_MIN_INLIER: + k = max(k, 0.81) # цилиндр подтверждён - признак D сработал + mae = float(np.mean(np.abs(np.array(dims) - np.array(gtd)))) + cls = CL.classify(dims, 0.0 if np.isnan(k) else k) + ok = "верно" if cls == gt["zone_scene"] else f"ОШ({gt['zone_scene']})" + print(f" {name:18s} {str([round(v) for v in gtd]):>18s} " + f"{str([round(v) for v in dims]):>20s} {mae:6.1f} {k:6.2f} {'xyz'[kax] if kax>=0 else '-':>3s} " + f"{cls+' '+ok:>12s} {len(P):7d} {dt:6.0f}" + + (f" цил {cyl_inl*100:3.0f}% R={cyl_R*1000:5.0f}мм" if CYL else "") + + (f" отсев ЛП {np.mean(rejected)*100:3.0f}%" if LR_THR > 0 and rejected else "")) + rows.append(dict(name=name, gt=gtd, gt_cls=gt["zone_scene"], gt_k=gt["k"], + pred=[round(v, 1) for v in dims], pred_k=round(float(k), 3), + pred_cls=cls, mae=round(mae, 1), n=len(P), ms=round(dt))) + if name in FOCUS: + cv2.imwrite(f"{OUT}/{name}_cloud_top.png", scatter(P, C, (0, 1), title=f"{name} top")) + cv2.imwrite(f"{OUT}/{name}_cloud_side.png", scatter(P, C, (0, 2), title=f"{name} side")) + + print("\n === МЕТРИКИ (без сегментации) ===") + if rows: + maes = [r["mae"] for r in rows] + acc = sum(1 for r in rows if r["pred_cls"] == r["gt_cls"]) + print(f" габариты: MAE медиана {np.median(maes):.1f} мм, среднее {np.mean(maes):.1f}, " + f"худший {max(maes):.1f} ({max(rows, key=lambda r: r['mae'])['name']})") + print(f" классы: {acc}/{len(rows)} = {100.0*acc/len(rows):.0f}%") + print(f" k макс {max(r['pred_k'] for r in rows):.2f}") + lab = ["B", "C", "D"] + print(" матрица (строки истина, столбцы предсказание):") + print(" " + "".join(f"{c:>5s}" for c in lab)) + for a in lab: + print(f" {a:3s} " + "".join( + f"{sum(1 for r in rows if r['gt_cls']==a and r['pred_cls']==b):5d}" for b in lab)) + print(f" время: медиана {np.median(times):.0f} мс, такт {PITCH_S*1000:.0f} мс -> " + f"{'УКЛАДЫВАЕТСЯ' if np.median(times) < PITCH_S*1000 else 'НЕ УКЛАДЫВАЕТСЯ'}") + json.dump(rows, open(f"{OUT}/metrics.json", "w"), indent=1, ensure_ascii=False) + print(f"\n картинки -> {OUT}") diff --git a/control_test/measure_roi.py b/control_test/measure_roi.py new file mode 100644 index 0000000..0b2f430 --- /dev/null +++ b/control_test/measure_roi.py @@ -0,0 +1,255 @@ +"""STAGE 2 (standalone, torch): compare ROI strategies for CREStereo dimensioning. + + full CRE on the whole frame, FastSAM on the whole frame + beltroi FastSAM + CRE restricted to the belt region all 3 rigs share + objroi belt ROI bounds the segmentation, CRE runs on the object bbox + pad, + L and R sharing ONE column window (left-extended by max disparity) + objroi_bad same, but the right crop is re-centred on the right image's own bbox - + the control that shows what independent centring costs + +RGB is never masked before CRE: the matcher needs the surround. The mask is applied to +the depth, eroded, only when back-projecting. +""" +import os, sys, json, time, numpy as np, cv2, torch, torch.nn.functional as F + +CV = "/home/dasha/isaac_assets/cv" +CT = "/home/dasha/robozon-sorter/control_test" +CFG = sys.argv[1] if len(sys.argv) > 1 else "E60" # cam_configs.DEFAULT; +# not imported here because cam_configs needs omni.usd and this stage runs standalone +CAP = f"{CT}/captures/{CFG}" +VARIANTS = (sys.argv[2].split(",") if len(sys.argv) > 2 + else ["full", "beltroi", "objroi", "objroi_bad"]) + +man = json.load(open(f"{CAP}/manifest.json")) +calib = man["calib"] # the calibration this capture was actually shot with +TARGET = np.array(man["target"]); BELT_Z = TARGET[2] +RIGS = sorted({c["rig"] for c in calib.values()}) +dev = "cuda" + +os.environ["YOLO_CONFIG_DIR"] = "/tmp/Ultralytics" +sys.path.insert(0, f"{CV}/crestereo") +from nets import Model +cre = Model(max_disp=256, mixed_precision=False, test_mode=True) +cre.load_state_dict(torch.load(f"{CV}/crestereo/models/crestereo_eth3d.pth", + map_location="cpu"), strict=True) +cre.to(dev).eval() +from ultralytics import FastSAM +fsam = FastSAM(f"{CV}/FastSAM-s.pt") + +ZMIN, DPAD, PAD = 0.60, 1.35, 48 +VOX = 0.004 + + +def cre_infer(L, R, iters=20): + """two-stage CRE on one already-corresponding pair of crops""" + h, w = L.shape[:2] + Hp, Wp = (h + 7) // 8 * 8, (w + 7) // 8 * 8 + Lb = np.zeros((1, 3, Hp, Wp), np.float32); Rb = np.zeros((1, 3, Hp, Wp), np.float32) + Lb[0, :, :h, :w] = L.transpose(2, 0, 1); Rb[0, :, :h, :w] = R.transpose(2, 0, 1) + iL = torch.from_numpy(Lb).to(dev); iR = torch.from_numpy(Rb).to(dev) + dL = F.interpolate(iL, (Hp // 2, Wp // 2), mode="bilinear", align_corners=True) + dR = F.interpolate(iR, (Hp // 2, Wp // 2), mode="bilinear", align_corners=True) + with torch.inference_mode(): + f0 = cre(dL, dR, iters=iters, flow_init=None) + f = cre(iL, iR, iters=iters, flow_init=f0) + return np.abs(f[0, 0].detach().cpu().numpy())[:h, :w] + + +def belt_roi_px(cam, poly3): + """the shared belt region, projected into this camera, as an axis-aligned window""" + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.c_[poly3, np.ones(len(poly3))] @ Minv)[:, :3]; z = -c[:, 2] + u = c[:, 0] / np.maximum(z, 1e-9) * cam["fx"] + cam["cx"] + v = -c[:, 1] / np.maximum(z, 1e-9) * cam["fy"] + cam["cy"] + x0 = int(max(0, np.floor(u.min()))); x1 = int(min(cam["width"], np.ceil(u.max()))) + y0 = int(max(0, np.floor(v.min()))); y1 = int(min(cam["height"], np.ceil(v.max()))) + return x0, y0, x1, y1 + + +def gate_px(cam): + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.r_[TARGET, 1.0] @ Minv)[:3]; z = -c[2] + return (int(np.clip(c[0] / z * cam["fx"] + cam["cx"], 0, cam["width"] - 1)), + int(np.clip(-c[1] / z * cam["fy"] + cam["cy"], 0, cam["height"] - 1))) + + +def segment(img, win, gate): + """FastSAM inside `win`; keep the smallest candidate containing the gate pixel. + Returns a FULL-FRAME boolean mask.""" + H, W = img.shape[:2] + x0, y0, x1, y1 = win + sub = img[y0:y1, x0:x1] + hs, ws = sub.shape[:2]; A = hs * ws + res = fsam(sub, imgsz=1024, conf=0.40, iou=0.90, retina_masks=True, + max_det=60, verbose=False, device=dev) + raw = (res[0].masks.data.cpu().numpy().astype(bool) + if res[0].masks is not None else np.zeros((0, hs, ws), bool)) + cands = [] + for m in raw: + if m.shape != (hs, ws): + m = cv2.resize(m.astype(np.uint8), (ws, hs)).astype(bool) + a = int(m.sum()) + if a < 0.0015 * A or a > 0.35 * A: + continue + ys, xs = np.where(m) + bw = xs.max() - xs.min() + 1; bh = ys.max() - ys.min() + 1 + if a / (bw * bh) < 0.12: + continue + cands.append(m) + if not cands: + return None + gx, gy = gate[0] - x0, gate[1] - y0 + inside = [m for m in cands + if 0 <= gy < hs and 0 <= gx < ws and m[gy, gx]] + sel = (min(inside, key=lambda m: int(m.sum())) if inside else + min(cands, key=lambda m: (np.where(m)[1].mean() - gx) ** 2 + + (np.where(m)[0].mean() - gy) ** 2)) + out = np.zeros((H, W), bool); out[y0:y1, x0:x1] = sel + return out + + +def backproj(disp, win, mask, cam, xoff_r=0): + """disp is defined over `win` of the LEFT image; full-frame pixel coords keep the + intrinsics valid, so no cx/cy shift is needed.""" + x0, y0, x1, y1 = win + d = disp + xoff_r + depth = np.where(d > 0.5, cam["fx"] * cam["baseline"] / np.maximum(d, 1e-6), np.nan) + mc = 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 = mc & np.isfinite(depth) & (depth > 1e-3) + if sel.sum() < 30: + return np.zeros((0, 3)) + u, v, z = us[sel], vs[sel], depth[sel] + P = np.stack([(u - cam["cx"]) * z / cam["fx"], + -(v - cam["cy"]) * z / cam["fy"], -z, np.ones_like(z)], 1) + return (P @ np.array(cam["M"]))[:, :3] + + +crop3d = lambda P: P[(np.abs(P[:, 0] - TARGET[0]) < 0.30) + & (np.abs(P[:, 1] - TARGET[1]) < 0.30) + & (P[:, 2] > BELT_Z + 0.006) & (P[:, 2] < BELT_Z + 0.60)] + + +def voxel(P, v=VOX): + k = np.floor(P / v).astype(np.int64) + _, i = np.unique(k, axis=0, return_index=True) + return P[i] + + +def dims_of(P): + if len(P) < 60: + return None + P = voxel(P) + c = P.mean(0); r = np.linalg.norm(P - c, axis=1) + P = P[r < np.percentile(r, 94)] + if len(P) < 40: + return None + h = (np.percentile(P[:, 2], 98) - BELT_Z) * 1000.0 + rect = cv2.minAreaRect(np.ascontiguousarray(P[:, :2].astype(np.float32))) + w, l = sorted([rect[1][0] * 1000.0, rect[1][1] * 1000.0]) + return sorted([h, w, l], reverse=True) + + +# ---- the belt region all six cameras see ---- +g = 0.005 +xs = np.arange(TARGET[0] - 1.6, TARGET[0] + 1.6, g) +ys = np.arange(TARGET[1] - 1.6, TARGET[1] + 1.6, g) +X, Y = np.meshgrid(xs, ys) +G = np.c_[X.ravel(), Y.ravel(), np.full(X.size, BELT_Z)] +vis = np.ones(len(G), bool) +for cam in calib.values(): + Minv = np.linalg.inv(np.array(cam["M"])) + c = (np.c_[G, np.ones(len(G))] @ Minv)[:, :3]; z = -c[:, 2] + u = c[:, 0] / np.maximum(z, 1e-9) * cam["fx"] + cam["cx"] + v = -c[:, 1] / np.maximum(z, 1e-9) * cam["fy"] + cam["cy"] + vis &= (z > 1e-3) & (u >= 0) & (u < cam["width"]) & (v >= 0) & (v < cam["height"]) +cn, _ = cv2.findContours(vis.reshape(X.shape).astype(np.uint8), + cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) +pol = max(cn, key=cv2.contourArea).reshape(-1, 2) +POLY3 = np.c_[xs[pol[:, 0]], ys[pol[:, 1]], np.full(len(pol), BELT_Z)] +print(f"общая зона ленты: {vis.sum()*g*g*1e4:.0f} см2") +for rig in RIGS: + cam = calib[f"{rig}_Left"] + x0, y0, x1, y1 = belt_roi_px(cam, POLY3) + print(f" {rig:18s} ROI ленты {x1-x0}x{y1-y0} px " + f"= {100.0*(x1-x0)*(y1-y0)/(cam['width']*cam['height']):.0f}% кадра") + +for _ in range(2): + cre_infer(np.zeros((160, 224, 3), np.float32), np.zeros((160, 224, 3), np.float32)) + +results = {v: {} for v in VARIANTS} +timing = {v: [] for v in VARIANTS} + +for name, e in man["items"].items(): + gt = sorted(e["gt_scene_mm"], reverse=True) + for var in VARIANTS: + clouds, per_rig, by_rig = [], {}, {} + t0 = time.time() + for rig in RIGS: + camL = calib[f"{rig}_Left"] + IL = cv2.imread(e["files"][f"{rig}_Left"]).astype(np.float32) + IR = cv2.imread(e["files"][f"{rig}_Right"]).astype(np.float32) + if IL is None or IR is None: + continue + H, W = IL.shape[:2] + full = (0, 0, W, H) + broi = belt_roi_px(camL, POLY3) + gate = gate_px(camL) + segwin = full if var == "full" else broi + mask = segment(IL.astype(np.uint8), segwin, gate) + if mask is None: + continue + maxd = int(np.ceil(DPAD * camL["fx"] * camL["baseline"] / ZMIN)) + if var in ("full", "beltroi"): + x0, y0, x1, y1 = (full if var == "full" else broi) + x0 = max(0, x0 - maxd) + cL = IL[y0:y1, x0:x1]; cR = IR[y0:y1, x0:x1] + disp = cre_infer(cL, cR) + P = backproj(disp, (x0, y0, x1, y1), mask, camL) + else: + ys_, xs_ = np.where(mask) + bx0, bx1 = xs_.min(), xs_.max(); by0, by1 = ys_.min(), ys_.max() + y0 = max(0, by0 - PAD); y1 = min(H, by1 + PAD) + x1 = min(W, bx1 + PAD) + x0 = max(0, bx0 - PAD - maxd) # room for the right-image match + cL = IL[y0:y1, x0:x1] + if var == "objroi": + cR = IR[y0:y1, x0:x1] # ONE window, both eyes + shift = 0 + else: # independently re-centred right crop + d0 = int(round(camL["fx"] * camL["baseline"] + / max(np.linalg.norm(np.array(camL["pos"]) - TARGET), 1e-6))) + rx0 = max(0, x0 - d0); rx1 = min(W, x1 - d0) + cR = IR[y0:y1, rx0:rx1] + cR = cv2.resize(cR, (cL.shape[1], cL.shape[0])) if cR.shape != cL.shape else cR + shift = -d0 + disp = cre_infer(cL, cR) + P = backproj(disp, (x0, y0, x1, y1), mask, camL, xoff_r=shift) + P = crop3d(P) + if len(P): + clouds.append(P); per_rig[rig] = dims_of(P); by_rig[rig] = P + dt = (time.time() - t0) * 1000 + timing[var].append(dt) + merged = dims_of(np.concatenate(clouds, 0)) if clouds else None + wide = [P for r, P in by_rig.items() if r != "Orbbec_Gemini305"] + merged2 = dims_of(np.concatenate(wide, 0)) if wide else None + results[var][name] = dict(gt=gt, merged=merged, merged_no305=merged2, + per_rig=per_rig, ms=round(dt)) + mm = (f"{merged[0]:6.0f}{merged[1]:7.0f}{merged[2]:7.0f}" if merged else " нет облака ") + mae = (np.mean(np.abs(np.array(merged) - np.array(gt))) if merged else float("nan")) + print(f"{name:18s} {var:11s} эталон {gt[0]:5.0f}{gt[1]:6.0f}{gt[2]:6.0f} -> {mm} MAE {mae:6.1f} мм {dt:5.0f} мс") + +print("\n===== итог =====") +print(f"{'вариант':12s} {'MAE, мм':>9s} {'облаков':>9s} {'мс/объект':>11s}") +summ = {} +for var in VARIANTS: + maes = [np.mean(np.abs(np.array(r["merged"]) - np.array(r["gt"]))) + for r in results[var].values() if r["merged"]] + summ[var] = dict(mae=round(float(np.mean(maes)), 1) if maes else None, + n=len(maes), ms=round(float(np.mean(timing[var])))) + print(f"{var:12s} {summ[var]['mae'] if maes else '-':>9} " + f"{len(maes)}/{len(results[var]):>7} {summ[var]['ms']:>11}") +json.dump(dict(summary=summ, results=results), + open(f"{CAP}/roi_compare.json", "w"), indent=1) +print(f"\n-> {CAP}/roi_compare.json") diff --git a/control_test/reposition.py b/control_test/reposition.py new file mode 100644 index 0000000..25fe238 --- /dev/null +++ b/control_test/reposition.py @@ -0,0 +1,98 @@ +"""SUPERSEDED - kept for history only. + +SUPERSEDED by cam_configs.apply_config(stage, cam_configs.DEFAULT). +Hard-codes the old 600 mm standoff. +""" + +"""Reposition the camera rigs: + * RealSense D435: Left and Right on OPPOSITE sides of the belt, facing each other. + * All three rigs: working distance exactly 600 mm to the inspection point. +Gemini 305/345 keep their azimuth and stay rectified (shared orientation, right eye +offset along the camera's local +X by the baseline). +""" +import json, math +import omni.usd +from pxr import Gf, Usd, UsdGeom + +stage = omni.usd.get_context().get_stage() +TARGET = Gf.Vec3d(-0.750, 0.0, 1.781) # inspection point on the belt +STANDOFF = 0.60 # requested working distance, metres +SIDE_ELEV_DEG = 20.0 # D435 sits low, at the side +UP = Gf.Vec3d(0, 0, 1) + +def find(name): + for p in stage.Traverse(): + if p.IsA(UsdGeom.Camera) and p.GetName() == name: + return p + raise KeyError(name) + +def look_at_matrix(pos, target): + """USD camera convention: looks down local -Z, local +Y is up.""" + fwd = (target - pos) + fwd = fwd / (fwd.GetLength() or 1.0) + zax = -fwd + up = UP + if abs(Gf.Dot(up, zax)) > 0.999: + up = Gf.Vec3d(0, 1, 0) + xax = Gf.Cross(up, zax); xax = xax / (xax.GetLength() or 1.0) + yax = Gf.Cross(zax, xax) + M = Gf.Matrix4d(1.0) + M.SetRow3(0, xax); M.SetRow3(1, yax); M.SetRow3(2, zax) + M.SetTranslateOnly(pos) + return M, xax, fwd + +def set_pose(prim, M): + xf = UsdGeom.Xformable(prim) + xf.ClearXformOpOrder() + xf.AddTransformOp().Set(M) + +def current_dir_from_target(prim): + p = UsdGeom.XformCache().GetLocalToWorldTransform(prim).ExtractTranslation() + d = Gf.Vec3d(p) - TARGET + return d / (d.GetLength() or 1.0) + +report = {} + +# ---- D435: opposing pair, one each side, both aimed at the belt -------------------- +th = math.radians(SIDE_ELEV_DEG) +for side_name, sgn in (("Left", +1.0), ("Right", -1.0)): + prim = find(f"RealSense_D435_{side_name}") + pos = TARGET + Gf.Vec3d(0.0, sgn * STANDOFF * math.cos(th), STANDOFF * math.sin(th)) + M, _, fwd = look_at_matrix(pos, TARGET) + set_pose(prim, M) + report[f"RealSense_D435_{side_name}"] = dict( + pos=[round(v, 4) for v in pos], fwd=[round(v, 4) for v in fwd], + dist_mm=round((TARGET - pos).GetLength() * 1000, 1)) + +# ---- Gemini pairs: same azimuth, distance forced to 600 mm, stay RECTIFIED --------- +for rig, baseline_m in (("Orbbec_Gemini305", 0.0265), ("Orbbec_Gemini345", 0.1294)): + left, right = find(f"{rig}_Left"), find(f"{rig}_Right") + # keep the direction the pair currently views from, measured at its midpoint + xc = UsdGeom.XformCache() + pl = Gf.Vec3d(xc.GetLocalToWorldTransform(left).ExtractTranslation()) + pr = Gf.Vec3d(xc.GetLocalToWorldTransform(right).ExtractTranslation()) + mid = (pl + pr) * 0.5 + d = mid - TARGET + d = d / (d.GetLength() or 1.0) + centre = TARGET + d * STANDOFF + M, xax, fwd = look_at_matrix(centre, TARGET) + # both eyes share ONE orientation - that is what keeps the pair rectified; the right + # eye is displaced along the camera's own +X by the baseline + for prim, off in ((left, -baseline_m / 2.0), (right, +baseline_m / 2.0)): + Mi = Gf.Matrix4d(M) + Mi.SetTranslateOnly(centre + xax * off) + set_pose(prim, Mi) + report[prim.GetName()] = dict( + pos=[round(v, 4) for v in (centre + xax * off)], + fwd=[round(v, 4) for v in fwd], + dist_mm=round((TARGET - (centre + xax * off)).GetLength() * 1000, 1)) + +print(f"{'camera':>26} {'position':>26} {'dist to belt':>13}") +for k, v in report.items(): + print(f"{k:>26} ({v['pos'][0]:+6.3f},{v['pos'][1]:+6.3f},{v['pos'][2]:+6.3f}) {v['dist_mm']:>10.1f} mm") + +out = "/home/dasha/robozon-sorter/control_test/calib_rs_side.json" +json.dump(dict(target=[round(v, 4) for v in TARGET], standoff_m=STANDOFF, + side_elevation_deg=SIDE_ELEV_DEG, cameras=report), + open(out, "w"), indent=2) +print(f"\nextrinsics -> {out}") diff --git a/control_test/run_pipeline.py b/control_test/run_pipeline.py new file mode 100644 index 0000000..a8ccef5 --- /dev/null +++ b/control_test/run_pipeline.py @@ -0,0 +1,527 @@ +"""control_test - run the sorting cell against whatever meshes are sitting in items/. + + isaacsim_send.py --context ct --file control_test/run_pipeline.py + isaacsim_send.py --context ct --file control_test/run_pipeline.py \ + --args-json '{"only": ["bag","backpack","lunchbox"], "pitch": 1.4}' + +Every .usd in items/ is measured (control_test/classify.py) and classified by the +documented rules, then fed onto the line in sorted order at PITCH spacing. Nothing is +hard-coded per object: drop a mesh in the folder and it joins the next run. + + D round, dimensions in envelope -> laser at the pusher -> blade -> Belt_01 -> BinD + C undersize or oversize -> plow +PLOW_ANGLE -> ConveyorTrack_01 -> container C + B in envelope, not round -> plow -PLOW_ANGLE -> ConveyorTrack_06 -> container B + +Injectable args: pitch, speed, plow_angle, swing_margin, plow_hold_max, only, limit. +""" +import asyncio +import pathlib +import sys +import time + +import numpy as np +import omni.timeline +import omni.usd +import omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from omni.physx import get_physx_interface, get_physx_scene_query_interface +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema +from isaacsim.core.experimental.prims import RigidPrim + +# the python_server exec's this file as a string, so __file__ does not exist here; +# `control_test_dir` can be injected to run the folder from somewhere else. +HERE = pathlib.Path(globals().get("control_test_dir", + "/home/dasha/robozon-sorter/control_test")).resolve() +for extra in (str(HERE), "/home/dasha/robozon-sorter"): + if extra not in sys.path: + sys.path.insert(0, extra) +for _m in [k for k in list(sys.modules) if k.startswith(("robozon_sorter", "cell", "classify"))]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import cell +import classify as CL +from robozon_sorter import config as C +from robozon_sorter.sim.plow import Plow + +# ---------------------------------------------------------------- knobs +SPEED = float(globals().get("speed", 1.0)) # belt m/s +# 700 mm, the spec figure. Workable because the blade is only OWNED while an item is +# physically on it (0.63 s), leaving a 0.07 s gap to change angle - see T_GAP below. +PITCH = float(globals().get("pitch", 0.70)) # the spec figure +PLOW_ANGLE = float(globals().get("plow_angle", 20.0)) +SWING_MARGIN = float(globals().get("swing_margin", 0.25)) +ONLY = globals().get("only") # optional list of item names +LIMIT = int(globals().get("limit", 0)) # optional cap on how many to run + +PLOW_SENSE_X = -6.30 # ~1.0 m of lead on the blade body at -7.32 +PUSH_SENSE_X = C.PUSH_X + cell.PUSHER_X_MM / 2000.0 +# The blade must be back home before the NEXT item reaches it, i.e. the whole +# out-hold-return cycle has to fit inside T_pitch. At 0.70 m pitch that is 0.70 s, and the +# old 1.3 m/s / 0.15 s dwell cycle took 0.63 + 0.15 + 0.33 = 1.11 s - the blade was still +# in the lane when the following B/C item arrived, which knocked detergent onto the floor +# and stopped lunchbox dead against it. +# +# Moving the blade fast no longer costs transfer, because the carry-assist below drives +# the item rather than the blade face doing it: the assist velocity is set independently +# (PUSH_ASSIST_V) so the blade can clear the lane quickly while the item still gets a +# controlled 1.6 m/s across. +# cycle = 0.77/2.5 + 0.03 + 0.77/2.5 = 0.31 + 0.03 + 0.31 = 0.65 s < 0.70 s +PUSH_SPEED, PUSH_RETURN_SPEED, PUSH_OUT_Y, PUSH_HOLD_S = 2.5, 2.5, 0.52, 0.03 +PUSH_HOME_Y = -0.25 # was config's -0.30; the blade face still clears a 400 mm item +# +Y velocity handed to the item while the blade advances. The out phase is only +# (0.52+0.25)/2.5 = 0.31 s, and belt friction eats part of it: 1.6 m/s measured just +# dy=+0.40 m, short of the branch belt's near edge at y=0.443, so class-D items stayed on +# the main line and were caught by the plow instead. Scaled up to clear it with margin. +# Calibrated on this cell, not guessed: 1.6 m/s -> dy +0.40 m (short of the branch belt's +# near edge at y=0.443, item stays on the line); 2.4 m/s -> dy +0.69 m (shoots clean over +# Belt_01, which ends at y=2.169, and lands on the floor). The response is close to linear +# in between, so ~1.95 m/s puts the item at dy~0.52 - just past the near edge, nowhere near +# the far one, and the branch belt takes it from there. +PUSH_ASSIST_V = 2.1 +SENSE_Y0, SENSE_Y1, SENSE_RAYS, GATE_WINDOW = -0.45, 0.45, 121, 0.15 +BLADE_LEADING_X, BLADE_TRAILING_X = -7.32, -7.95 +BLADE_GUARD = 0.08 # keep the slow rate while an item is this close to the blade + +T_PITCH = PITCH / SPEED +# How long an item ACTUALLY owns the blade: the blade body is 0.63 m long, so at 1 m/s an +# item is against it for 0.63 s. (config's PLOW_SWEEP_X0..PLOW_RELEASE_X spans 0.95 m and +# was used as "T_zone" before - that is the wider sweep WINDOW, not the blade, and taking +# it as the occupancy is what made a 0.70 m pitch look geometrically impossible.) +T_BLADE = (BLADE_LEADING_X - BLADE_TRAILING_X) / SPEED +# Item N leaves the blade T_BLADE after reaching it; item N+1 reaches it T_PITCH after N. +# The blade is therefore EMPTY for this long between two consecutive items, and that is +# the whole budget for a class change. +T_GAP = T_PITCH - T_BLADE +# Rate needed for the worst case (a full B<->C reversal) inside that gap, x1.5 margin. +# Swinging this fast is safe because it only happens while the blade is empty - there is +# nothing against it to bat. While an item IS on the blade the slower PLOW_RATE is used. +PLOW_REPOSITION_RATE = min(2000.0, (2.0 * PLOW_ANGLE) / max(T_GAP, 0.02) * 1.5) +PLOW_RATE = (2.0 * PLOW_ANGLE) / (SWING_MARGIN * T_PITCH) +PLOW_ANGLES = {"B": -PLOW_ANGLE, "C": PLOW_ANGLE, "D": 0.0} + +CONTAINER_B, CONTAINER_C, CONTAINER_R = (-8.81, 1.47), (-10.45, -0.225), 0.55 +# An item counts as delivered only if it is RESTING IN the tray, not merely above its +# x/y footprint: the tray floors sit at z 1.14..1.18 and stand on legs, so a `z < 1.30` +# test alone also accepts an item lying on the ground under the container. That is exactly +# what hid this bug - B items were reported OK at z=+0.00 while sitting on the floor. +CONTAINER_Z_MIN, CONTAINER_Z_MAX = 1.10, 1.72 +BIN_Z_MIN = 1.15 +BIN_X0, BIN_X1, BIN_Y0, BIN_Y1, BIN_LIP_Z = -6.21, -4.95, 1.57, 2.86, 1.72 +EXPECT = {"D": "bin_D", "B": "container_B", "C": "container_C"} + +# ---------------------------------------------------------------- item library +raw = CL.load_library(HERE / "items") +lib = [r for r in raw if "error" not in r] +skipped = [r for r in raw if "error" in r] +if ONLY: + lib = [r for r in lib if r["name"] in set(ONLY)] +if LIMIT: + lib = lib[:LIMIT] +if not lib: + raise SystemExit("no usable meshes in items/") + +print(f"===== ITEM LIBRARY ({HERE / 'items'}, classes from labels.json) =====") +print(f"{'item':>20} {'dims mm (label)':>24} {'k':>6} class") +for r in lib: + print(f"{r['name']:>20} {str(r['dims_mm']):>24} {r['k']:>6.3f} {r['cls']}") +if skipped: + print(f" skipped, no entry in labels.json: {[r['name'] for r in skipped]}") +# a label whose own dims/k imply another class would surface as a baffling mechanical +# failure, so it is caught here instead +bad = CL.verify_labels(HERE / "items") +if bad: + print(" WARNING - labels inconsistent with the documented rules:") + for b in bad: + print(f" {b['name']}: labelled {b['labelled']}, rules imply {b['implied']} " + f"(dims={b['dims_mm']} k={b['k']})") +else: + print(" labels consistent with the documented B/C/D rules") +from collections import Counter +print(" totals:", dict(Counter(r["cls"] for r in lib))) + +ORDER = [r["name"] for r in lib] +CLASSES = {r["name"]: r["cls"] for r in lib} +PATHS = {r["name"]: r["path"] for r in lib} + +print(f"\n===== TIMING =====") +print(f" T_pitch (item spacing) = {T_PITCH:.3f} s") +print(f" T_blade (item on the blade) = {T_BLADE:.3f} s") +print(f" T_gap (blade empty between) = {T_GAP:.3f} s") +print(f" reposition rate needed = {PLOW_REPOSITION_RATE:.0f} deg/s " + f"(hold rate {PLOW_RATE:.0f} deg/s)") +PUSH_CYCLE = (PUSH_OUT_Y - PUSH_HOME_Y) / PUSH_SPEED + PUSH_HOLD_S + \ + (PUSH_OUT_Y - PUSH_HOME_Y) / PUSH_RETURN_SPEED +print(f" pusher cycle (out+hold+back) = {PUSH_CYCLE:.3f} s" + + (" OK - clears the lane before the next item" + if PUSH_CYCLE < T_PITCH else + f" TOO SLOW - blade still in the lane when the next item arrives")) +if T_GAP <= 0: + print(f" IMPOSSIBLE: an item still owns the blade when the next arrives. " + f"Need pitch > {T_BLADE*SPEED:.2f} m at {SPEED} m/s.") + +# ---------------------------------------------------------------- scene +stage = omni.usd.get_context().get_stage() +if not stage or "plow_cell_90_45" not in (stage.GetRootLayer().identifier or ""): + raise SystemExit(f"open {cell.SCENE} first - see control_test/README.md") +info = await cell.prepare(stage, belt_speed=SPEED, script_control=True) +print(f"\nprepare: belts={len(info['belts'])} graphs_removed={len(info['graphs_removed'])} " + f"junction_opened={info['junction_opened']} rails={info['rails']} " + f"pusher={info['pusher_dims'][0]:.2f} m") + +ITEMS_ROOT = "/World/CtrlItems" + + +def _load(names): + """define every item physics-ready BEFORE play; never re-tag a body afterwards""" + UsdGeom.Xform.Define(stage, ITEMS_ROOT) + ok = [] + for i, name in enumerate(names): + try: + prim = UsdGeom.Xform.Define(stage, f"{ITEMS_ROOT}/{name}").GetPrim() + prim.GetReferences().ClearReferences() + prim.GetReferences().AddReference(PATHS[name]) + xf = UsdGeom.Xformable(prim) + xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set( + Gf.Vec3d(9.0 + 1.2 * i, 5.0, 0.4)) + UsdPhysics.RigidBodyAPI.Apply(prim) + # exported meshes arrive kinematic; force dynamic once, before play + 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.CreateSolverVelocityIterationCountAttr().Set(8) + px.CreateSleepThresholdAttr().Set(0.0) + px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) + for d in Usd.PrimRange(prim): + if d.HasAPI(UsdPhysics.CollisionAPI): + pc = PhysxSchema.PhysxCollisionAPI.Apply(d) + pc.CreateContactOffsetAttr().Set(0.004) + pc.CreateRestOffsetAttr().Set(0.001) + UsdGeom.Imageable(prim).MakeInvisible() + ok.append(name) + except BaseException as exc: + print(f" WARNING: {name} failed to load: {type(exc).__name__}") + return ok + + +ORDER = _load(ORDER) +rp = {n: RigidPrim(paths=[f"{ITEMS_ROOT}/{n}"]) for n in ORDER} +# ONE view over every item: get_world_poses() then costs a single backend round-trip +# instead of nine. Benchmarked on this scene: 0.746 ms for nine separate reads vs +# 0.081 ms batched. The physics callback used to do ~36 separate reads per step (trace +# loop + two curtains + the plow schedule), i.e. ~3 ms of a 16.7 ms budget spent almost +# entirely on GPU round-trips that also stall the render thread feeding WebRTC. +items_view = RigidPrim(paths=[f"{ITEMS_ROOT}/{n}" for n in ORDER]) +_pose_cache = {} + + +def refresh_poses(): + try: + arr = items_view.get_world_poses()[0].numpy() + for i, n in enumerate(ORDER): + _pose_cache[n] = arr[i] + _last[n] = arr[i] + except BaseException: + pass +await app_utils.update_app_async(steps=20) + +plow = Plow(stage, kinematic=True) +plow.home() +query = get_physx_scene_query_interface() +_last = {} + + +def item_pose(name): + """cached: refresh_poses() fills the whole cache in one call per step""" + if name in _pose_cache: + return _pose_cache[name] + refresh_poses() + if name in _pose_cache: + return _pose_cache[name] + if name in _last: + return _last[name] + raise KeyError(name) + + +def activate(name): + prim = stage.GetPrimAtPath(f"{ITEMS_ROOT}/{name}") + for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + op.Set(Gf.Vec3d(cell.ENTRY_X, cell.ENTRY_Y, C.BELT_Z + 0.05)) + break + UsdGeom.Imageable(prim).MakeVisible() + + +# ---------------------------------------------------------------- pusher blade +blade_prim = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher") +blade_op = next(o for o in UsdGeom.Xformable(blade_prim).GetOrderedXformOps() + if o.GetOpType() == UsdGeom.XformOp.TypeTranslate) +blade_base = blade_op.Get() +BLADE_PARENT_Y = -0.35 + + +def blade_to(y): + blade_op.Set(Gf.Vec3d(blade_base[0], y - BLADE_PARENT_Y, blade_base[2])) + + +blade_to(PUSH_HOME_Y) + +gate_log, push_log = [], [] +push_swept, plow_swept, push_queue = set(), set(), [] +plow_done = set() +sim_t = [0.0] +push_state = {"phase": "idle", "item": None, "y": PUSH_HOME_Y, "t": 0.0, "y0": 0.0} + + +def _curtain(x, exclude): + if not any(abs(float(item_pose(n)[0]) - x) < GATE_WINDOW + for n in ORDER if n not in exclude and n in rp): + return None + z0, reach = C.BELT_Z + 0.40, 0.399 + for i in range(SENSE_RAYS): + y = SENSE_Y0 + (SENSE_Y1 - SENSE_Y0) * i / (SENSE_RAYS - 1) + hit = query.raycast_closest([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 n in ORDER: + if n not in exclude and f"{ITEMS_ROOT}/{n}" in path: + return n + return None + + +def _push_begin(name): + push_state.update(phase="out", item=name, t=0.0, y0=float(item_pose(name)[1])) + + +def _push_step(dt): + """the pusher, advanced from the PHYSICS step - never from an async task. Driving it + from a coroutine that pumps the app concurrently with the feed loop made the blade + jump further per sim-step than the stroke maths assumed, and the transfer collapsed + from ~0.8 m to ~0.2 m.""" + st = push_state + if st["phase"] == "idle": + return + name = st["item"] + if st["phase"] == "out": + st["y"] = min(PUSH_OUT_Y, st["y"] + PUSH_SPEED * dt) + blade_to(st["y"]) + # carry assist: a transform-driven kinematic blade imparts no momentum (PhysX sees + # a teleport), so the item's +Y velocity is matched to the blade's each step - a + # carry, not a single kick. Measured dy 0.21 m -> 0.80 m. + if name in rp: + try: + lin = rp[name].get_velocities()[0].numpy()[0] + rp[name].set_velocities(np.array([[float(lin[0]), PUSH_ASSIST_V, float(lin[2])]]), + np.array([[0.0, 0.0, 0.0]])) + except BaseException: + pass + if st["y"] >= PUSH_OUT_Y - 1e-6: + st["phase"], st["t"] = "hold", 0.0 + p = item_pose(name) + push_log.append(dict(item=name, dy=round(float(p[1]) - st["y0"], 3), + x=round(float(p[0]), 3), y=round(float(p[1]), 3))) + elif st["phase"] == "hold": + st["t"] += dt + if st["t"] >= PUSH_HOLD_S: + st["phase"] = "back" + else: + st["y"] = max(PUSH_HOME_Y, st["y"] - PUSH_RETURN_SPEED * dt) + blade_to(st["y"]) + if st["y"] <= PUSH_HOME_Y + 1e-6: + st.update(phase="idle", item=None) + if push_queue: + _push_begin(push_queue.pop(0)) + + +RUN_ACTIVE = [True] + + +def _step(dt): + # If the script dies mid-run the subscription can outlive it, and a callback still + # touching dead RigidPrims while the timeline keeps playing is what left the app + # pinned at full load and unreachable - which reads exactly like a crash. The finally + # block below clears this flag no matter how the run ends. + if not RUN_ACTIVE[0]: + return + sim_t[0] += dt + refresh_poses() # one batched read; everything below uses the cache + try: + seen = _curtain(PUSH_SENSE_X, push_swept) + if seen is not None: + push_swept.add(seen) + gate_log.append(("push", seen, CLASSES[seen])) + if CLASSES[seen] == "D": + (push_queue.append if push_state["phase"] != "idle" else _push_begin)(seen) + + seen = _curtain(PLOW_SENSE_X, plow_swept) + if seen is not None: + plow_swept.add(seen) + gate_log.append(("plow", seen, CLASSES[seen], + float(PLOW_ANGLES.get(CLASSES[seen], 0.0)))) + + # Purely POSITION-DRIVEN, no commit / hold / timeout - so nothing can be starved. + # Whoever is physically on the blade owns the angle; the instant they clear its + # trailing edge the blade is free and snaps to the next arrival's angle. The old + # scheme committed at the sensor (x -6.30) and held to x -7.95, occupying the + # blade for 1.65 s - 2.4 items' worth at a 0.70 m pitch, which is what starved + # everyone behind and forced the pitch up to 1.4 m. + on_blade = nearest = None + for n in ORDER: + if n not in rp or n in plow_done: + continue + x = float(item_pose(n)[0]) + if x < BLADE_TRAILING_X: + plow_done.add(n) + elif x <= BLADE_LEADING_X + BLADE_GUARD: # on the blade, or close enough + if on_blade is None or x < on_blade[0]: # that a fast snap would bat it + on_blade = (x, n) # deepest in = furthest along + elif n in plow_swept: # sensed, still approaching + if nearest is None or x < nearest[0]: + nearest = (x, n) # smallest x = closest to the blade + target = on_blade or nearest + ang = float(PLOW_ANGLES.get(CLASSES[target[1]], 0.0)) if target else 0.0 + plow.step_toward(ang, dt, + rate=PLOW_RATE if on_blade else PLOW_REPOSITION_RATE) + _push_step(dt) + except BaseException as exc: + gate_log.append(("step-error", "", repr(exc))) + + +sub = get_physx_interface().subscribe_physics_step_events(_step) +tl = omni.timeline.get_timeline_interface() + +# Everything from Play onward runs inside try/finally. Without it, any error in the run - +# and several happened while building this - left the physics callback subscribed AND the +# timeline playing, so the app kept simulating at full tilt with stale references. The +# python_server then could not answer, WebRTC dropped with NVST_R_BUSY, and the whole +# thing looked like a crash when it was really a leaked run. +try: + tl.play() + await app_utils.update_app_async(steps=20) + + w = vp.get_active_viewport() + + + async def _wait(sec): + target = float(tl.get_current_time()) + sec + while float(tl.get_current_time()) < target: + await app_utils.update_app_async(steps=5) + await asyncio.sleep(0) # hand the event loop back to the + # WebRTC streamer and the python server. Pumping update_app_async back-to-back for + # the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client + # disconnected from WebRTC server' on every run, and a second client cannot connect + # at all - from outside that is indistinguishable from Isaac Sim having crashed. + + + print(f"\n===== FEED ({len(ORDER)} items, {PITCH*1000:.0f} mm pitch @ {SPEED} m/s) =====") + sim_t0 = float(tl.get_current_time()) + for i, name in enumerate(ORDER): + for _ in range(8): + try: + activate(name) + break + except BaseException: + await app_utils.update_app_async(steps=2) + print(f" {i*PITCH/SPEED:6.2f}s {name} ({CLASSES[name]})") + await _wait(PITCH / SPEED) + + settled = {} + + + def outcome(name): + if name not in rp: + return None + p = item_pose(name) + x, y, z = float(p[0]), float(p[1]), float(p[2]) + if BIN_X0 < x < BIN_X1 and BIN_Y0 < y < BIN_Y1 and BIN_Z_MIN < z < BIN_LIP_Z: + return "bin_D" + 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 CONTAINER_Z_MIN < z < CONTAINER_Z_MAX): + return tag + if z < 0.25: # ended up on the ground, wherever that was + return "floor" + return None + + + wall0 = time.time() + while (float(tl.get_current_time()) - sim_t0 < 60.0 + len(ORDER) * PITCH / SPEED + and time.time() - wall0 < 90.0): + await app_utils.update_app_async(steps=10) + await asyncio.sleep(0) # hand the event loop back to the + # WebRTC streamer and the python server. Pumping update_app_async back-to-back for + # the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client + # disconnected from WebRTC server' on every run, and a second client cannot connect + # at all - from outside that is indistinguishable from Isaac Sim having crashed. + for n in ORDER: + if n not in settled and (w_ := outcome(n)) is not None: + settled[n] = w_ + if len(settled) >= len(ORDER): + break + + await app_utils.update_app_async(steps=5) + await asyncio.sleep(0) # hand the event loop back to the + # WebRTC streamer and the python server. Pumping update_app_async back-to-back for + # the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client + # disconnected from WebRTC server' on every run, and a second client cannot connect + # at all - from outside that is indistinguishable from Isaac Sim having crashed. + vp.capture_viewport_to_file(w, file_path="/tmp/control_test_final.png") + await app_utils.update_app_async(steps=5) + await asyncio.sleep(0) # hand the event loop back to the + # WebRTC streamer and the python server. Pumping update_app_async back-to-back for + # the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client + # disconnected from WebRTC server' on every run, and a second client cannot connect + # at all - from outside that is indistinguishable from Isaac Sim having crashed. + final = {n: item_pose(n).copy() for n in ORDER} + sub = None + tl.stop() + await app_utils.update_app_async(steps=10) + await asyncio.sleep(0) # hand the event loop back to the + # WebRTC streamer and the python server. Pumping update_app_async back-to-back for + # the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client + # disconnected from WebRTC server' on every run, and a second client cannot connect + # at all - from outside that is indistinguishable from Isaac Sim having crashed. + + +finally: + RUN_ACTIVE[0] = False + sub = None + try: + tl.stop() + except BaseException: + pass + await app_utils.update_app_async(steps=10) + await asyncio.sleep(0) # hand the event loop back to the + # WebRTC streamer and the python server. Pumping update_app_async back-to-back for + # the length of a run starves them: the kit log shows NVST_R_BUSY then 'Client + # disconnected from WebRTC server' on every run, and a second client cannot connect + # at all - from outside that is indistinguishable from Isaac Sim having crashed. + print("cleanup: physics callback released, timeline stopped") + +print("\n===== DELIVERY =====") +ok_n = 0 +by_class = {"B": [0, 0], "C": [0, 0], "D": [0, 0]} +for name in ORDER: + cls = CLASSES[name] + got, want = settled.get(name, "line/unresolved"), EXPECT[cls] + ok = got == want + ok_n += ok + by_class[cls][1] += 1 + by_class[cls][0] += int(ok) + p = final[name] + print(f" {name:20s} {cls} -> {got:16s} want={want:14s} {'OK' if ok else 'FAIL'}" + f" ({float(p[0]):+.2f},{float(p[1]):+.2f},{float(p[2]):+.2f})") +print(f"\ndelivered {ok_n}/{len(ORDER)}") +for c in ("B", "C", "D"): + h, t = by_class[c] + print(f" {c}: {h}/{t}" + (f" ({100*h/t:.0f}%)" if t else "")) +if push_log: + print("\n===== PUSHER =====") + for e in push_log: + print(f" {e['item']:20s} dy={e['dy']:+.3f} -> ({e['x']:+.2f},{e['y']:+.2f})") +print("\nscreenshot: /tmp/control_test_final.png") diff --git a/control_test/run_sorting_cv.py b/control_test/run_sorting_cv.py new file mode 100644 index 0000000..868cb36 --- /dev/null +++ b/control_test/run_sorting_cv.py @@ -0,0 +1,270 @@ +"""ЗАМКНУТЫЙ КОНТУР: товары едут, CV определяет класс на лету, пушер и плуг реагируют. + +Отличие от прежнего прогона с заранее заданными классами: класс здесь НЕ читается из +разметки, а приходит от стереопайплайна во время движения. Разметка используется только +для подсчёта ошибки в конце. + +Как замыкается контур. torch внутри Isaac роняет процесс, поэтому CV живёт отдельным +процессом (cv_worker.py), а обмен идёт через каталог runtime/: при пересечении ворот +осмотра сцена рендерит шесть кадров и кладёт заявку, работник отвечает файлом с классом. +Времени хватает с большим запасом: от ворот (x = -0.750) до пушера (-3.900) товар едет +3.15 с, до плуга (-7.85) - 7.1 с, инференс - 469 мс. + +Механика та же, что проверена раньше: +* пушер берёт класс D - нож двигается записью трансформа (Cell.blade_to), его призматический + сустав выключен, иначе сустав и скрипт тянут нож в разные стороны; +* плуг ставится ЗАРАНЕЕ по классу (B - в одну сторону, C - в другую), лезвие кинематическое, + шарнир выключен: силовой привод в этой сборке не держал угол и звенел. +""" +import sys, os, math, json, time, glob +CT = "/home/dasha/robozon-sorter/control_test" +REPO = "/home/dasha/robozon-sorter" +for p in (CT, REPO): + if p not in sys.path: + sys.path.insert(0, p) + +import numpy as np +import omni.usd, omni.timeline +import omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim +import importlib +import cell as cellmod +importlib.reload(cellmod) +import classify as CL +import cam_configs as CC +from robozon_sorter import config as C +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.plow import Plow + +SPEED, PITCH = 1.0, 0.70 +GATE_X = float(CC.TARGET[0]) +RT = f"{CT}/runtime" +REQ, RES, SHOTS = f"{RT}/req", f"{RT}/res", f"{RT}/shots" +for d in (RT, REQ, RES, SHOTS): + os.makedirs(d, exist_ok=True) +for f in glob.glob(f"{REQ}/*") + glob.glob(f"{RES}/*"): + os.remove(f) + +ITEMS = ["bag", "backpack", "lunchbox", "helmet", "pillow", + "detergent", "bucket", "box_400x400x300", "box_300x200x200"] +RIGS = ["RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"] + +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) +omni.usd.get_context().open_stage(str(cellmod.SCENE)) +await app_utils.update_app_async(steps=60) +stage = omni.usd.get_context().get_stage() +info = await cellmod.prepare(stage, belt_speed=SPEED, script_control=True) +print(f"ячейка готова: парковок обезврежено {info.get('parks_cleared')}, лент {len(info['belts'])}") + +fps = stage.GetTimeCodesPerSecond() or 60.0 +stage.SetEndTimeCode(stage.GetStartTimeCode() + fps * 180.0) +tl.set_end_time(float(stage.GetEndTimeCode()) / fps) +tl.set_looping(False) + +lib = {i["name"]: i for i in CL.load_library(f"{CT}/items_flow") if "cls" in i} +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +w = vp.get_active_viewport(); orig_cam = w.camera_path + +ROOT = "/World/_CV" +if stage.GetPrimAtPath(ROOT).IsValid(): + stage.RemovePrim(ROOT) +stage.DefinePrim(ROOT, "Xform") + + +def spawn(name, i): + """товар из items_flow по ПРОВЕРЕННОМУ пути загрузки (как в flow_capture/run_pipeline). + + Три вещи, которые я сделал неверно в первой версии и из-за которых поток не поехал: + меши items_flow УЖЕ в каталожном масштабе и УЖЕ посажены на z=0 (scale_items.py), так + что домасштабирование лишнее; коллайдеры в них УЖЕ есть, свои накладывать поверх нельзя; + и рождаться товар должен на парковке, а к ленте переноситься в момент выпуска. + """ + path = f"{ROOT}/{name}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + prim = UsdGeom.Xform.Define(stage, path).GetPrim() + prim.GetReferences().ClearReferences() + prim.GetReferences().AddReference(lib[name]["path"]) + xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() + op = xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble) + op.Set(Gf.Vec3d(9.0 + 1.5 * i, 5.0, 0.4)) # парковка вне линии + UsdPhysics.RigidBodyAPI.Apply(prim) + 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.CreateSolverVelocityIterationCountAttr().Set(8) + px.CreateSleepThresholdAttr().Set(0.0) + px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) + for d in Usd.PrimRange(prim): + if d.HasAPI(UsdPhysics.CollisionAPI): + pc = PhysxSchema.PhysxCollisionAPI.Apply(d) + pc.CreateContactOffsetAttr().Set(0.004) + pc.CreateRestOffsetAttr().Set(0.001) + im = UsdGeom.Imageable(d) + if im: + im.MakeVisible() + return prim, op + + +async def compose(prim): + for _ in range(4): + await app_utils.update_app_async(steps=2) + for d in Usd.PrimRange(prim): + im = UsdGeom.Imageable(d) + if im: + im.MakeVisible() + + +async def shoot(name): + """шесть кадров в кроп-разрешении сцены; таймлайн на это время ставится на паузу""" + files = {} + was = tl.is_playing() + if was: + tl.pause() + for rig in RIGS: + for eye in ("Left", "Right"): + cam = f"/RigRS/{rig}_{eye}" + if not stage.GetPrimAtPath(cam).IsValid(): + continue + w.camera_path = cam + await app_utils.update_app_async(steps=6) + f = f"{RT}/frames/{name}__{rig}_{eye}.png" + os.makedirs(os.path.dirname(f), exist_ok=True) + vp.capture_viewport_to_file(w, file_path=f) + await app_utils.update_app_async(steps=4) + files[f"{rig}_{eye}"] = f + w.camera_path = orig_cam + await app_utils.update_app_async(steps=3) + if was: + tl.play() + return files + + +print("ожидаю работника CV...") +for _ in range(600): + if os.path.exists(f"{RT}/worker_ready"): + break + await app_utils.update_app_async(steps=6) +print("работник готов:", os.path.exists(f"{RT}/worker_ready")) + +cell = Cell(stage, items={}) +plow = Plow(stage); plow.target(C.PLOW_PRESET["D"]) +tl.play(); await app_utils.update_app_async(steps=20) +t0 = float(tl.get_current_time()) + +state = {} # name -> dict(prim, rp, shot, cls, pushed, plowed, gt) +order, nxt = [], 0.0 +pusher_busy = -1.0 +print(f"\nпоток: шаг {PITCH*1000:.0f} мм при {SPEED} м/с, ворота x={GATE_X:+.3f}, " + f"пушер x={C.PUSH_X:+.3f}, плуг x={C.PLOW_X:+.3f}\n") + +while float(tl.get_current_time()) - t0 < 90.0: + t = float(tl.get_current_time()) - t0 + if len(order) < len(ITEMS) and t >= nxt: + nm = ITEMS[len(order)] + prim, op = spawn(nm, len(order)) + for _ in range(3): + await app_utils.update_app_async(steps=2) # ссылка компонуется не в тот же тик + op.Set(Gf.Vec3d(cellmod.ENTRY_X, cellmod.ENTRY_Y, C.BELT_Z + 0.02)) + rp = RigidPrim(paths=[str(prim.GetPath())]) + state[nm] = dict(prim=prim, rp=rp, shot=False, cls=None, pushed=False, + plowed=False, gt=lib[nm]["cls"], t_gate=None, t_cls=None) + order.append(nm); nxt += PITCH / SPEED + print(f" {t:5.2f}s выпущен {nm}") + + # ворота осмотра: снять кадры и отправить заявку + for nm in order: + st = state[nm] + if st["shot"]: + continue + x = float(st["rp"].get_world_poses()[0].numpy()[0][0]) + if x <= GATE_X + 0.02: + st["shot"] = True; st["t_gate"] = t + files = await shoot(nm) + json.dump(dict(name=nm, files=files), open(f"{REQ}/{nm}.json", "w")) + print(f" {t:5.2f}s {nm} на воротах x={x:+.3f} - заявка отправлена") + + # ответы от работника + for f in glob.glob(f"{RES}/*.json"): + try: + a = json.load(open(f)) + except Exception: + continue + nm = a["name"] + if nm in state and state[nm]["cls"] is None: + state[nm]["cls"] = a.get("cls") + state[nm]["dims"] = a.get("dims"); state[nm]["k"] = a.get("k") + state[nm]["t_cls"] = t + lat = (t - (state[nm]["t_gate"] or t)) + print(f" {t:5.2f}s {nm} -> класс {a.get('cls')} (эталон {state[nm]['gt']}), " + f"dims={a.get('dims')}, задержка {lat:.2f}с, инференс {a.get('ms')}мс") + os.remove(f) + + # пушер берёт класс D + if t > pusher_busy: + for nm in order: + st = state[nm] + if st["cls"] != "D" or st["pushed"]: + continue + x = float(st["rp"].get_world_poses()[0].numpy()[0][0]) + if x <= C.PUSH_X + 0.06: + print(f" {t:5.2f}s ПУШЕР берёт {nm} (класс D) на x={x:+.3f}") + await cell.stroke(app_utils, out=True, speed=1.2) + await cell.stroke(app_utils, out=False, speed=1.5) + st["pushed"] = True + pusher_busy = float(tl.get_current_time()) - t0 + 0.2 + break + + # плуг ставится заранее по классу B/C + for nm in order: + st = state[nm] + if st["cls"] not in ("B", "C") or st["plowed"]: + continue + x = float(st["rp"].get_world_poses()[0].numpy()[0][0]) + if x <= C.PLOW_X + 1.2: + ang = C.PLOW_PRESET[st["cls"]] + plow.target(ang) + st["plowed"] = True + print(f" {t:5.2f}s ПЛУГ на {ang:+.0f}° под {nm} (класс {st['cls']}) x={x:+.3f}") + + await app_utils.update_app_async(steps=3) + +# итог +print(f"\n {'товар':18s} {'эталон':>7s} {'предсказано':>12s} {'габариты, мм':>20s} " + f"{'пушер':>6s} {'плуг':>5s} {'конец X,Y':>16s}") +print(" " + "-" * 96) +rows = [] +for nm in order: + st = state[nm] + q = st["rp"].get_world_poses()[0].numpy()[0] + ok = "верно" if st["cls"] == st["gt"] else "ОШИБКА" + print(f" {nm:18s} {st['gt']:>7s} {str(st['cls'])+' '+ok:>12s} " + f"{str(st.get('dims')):>20s} {'да' if st['pushed'] else '-':>6s} " + f"{'да' if st['plowed'] else '-':>5s} ({float(q[0]):+6.2f},{float(q[1]):+6.2f})") + rows.append(dict(name=nm, gt=st["gt"], pred=st["cls"], dims=st.get("dims"), + k=st.get("k"), pushed=st["pushed"], plowed=st["plowed"], + end=[round(float(q[0]), 2), round(float(q[1]), 2)], + lat=None if st["t_cls"] is None or st["t_gate"] is None + else round(st["t_cls"] - st["t_gate"], 2))) +got = [r for r in rows if r["pred"]] +acc = sum(1 for r in got if r["pred"] == r["gt"]) +print(f"\n классов получено {len(got)}/{len(rows)}, верно {acc}/{len(got)}") +lats = [r["lat"] for r in got if r["lat"] is not None] +if lats: + print(f" задержка от ворот до класса: медиана {np.median(lats):.2f} с " + f"(до пушера {abs(C.PUSH_X-GATE_X)/SPEED:.2f} с, до плуга {abs(C.PLOW_X-GATE_X)/SPEED:.2f} с)") +json.dump(rows, open(f"{RT}/sorting_cv.json", "w"), indent=1, ensure_ascii=False) + +# обзорный кадр +w.camera_path = orig_cam +await app_utils.update_app_async(steps=30) +vp.capture_viewport_to_file(w, file_path=f"{SHOTS}/overview.png") +await app_utils.update_app_async(steps=30) +tl.stop(); await app_utils.update_app_async(steps=5) +print(f"\n -> {RT}/sorting_cv.json, кадры в {SHOTS}") diff --git a/control_test/scale_items.py b/control_test/scale_items.py new file mode 100644 index 0000000..e5e1768 --- /dev/null +++ b/control_test/scale_items.py @@ -0,0 +1,75 @@ +"""Bake catalogue scale and a clean origin into copies of the item meshes. + +The originals are 2.0-2.8x smaller than the dims they are labelled with and sit ~0.6 m +above their own origin. Referencing them and fixing that with xform ops nests a scale +under a translate (which scales the translation) or a rigid body under a rigid body +(which hands the collider to the inner one and free-falls the outer). Baking the +transform into the points removes both traps: the result seats on z=0, is centred in +x/y, and loads through exactly the path run_pipeline.py already proves. +""" +import json, pathlib, shutil, sys +import numpy as np +from pxr import Usd, UsdGeom, Gf + +SRC = pathlib.Path("/home/dasha/robozon-sorter/control_test/items") +DST = pathlib.Path("/home/dasha/robozon-sorter/control_test/items_flow") +DST.mkdir(exist_ok=True) +labels = json.loads((SRC / "labels.json").read_text()) +shutil.copy(SRC / "labels.json", DST / "labels.json") +for t in ("textures",): + if (SRC / t).exists() and not (DST / t).exists(): + try: (DST / t).symlink_to(SRC / t) + except FileExistsError: pass + +names = sys.argv[1:] or sorted(n for n in labels) +print(f"{'товар':20s} {'исходник, мм':22s} {'масштаб':>8s} {'после, мм':22s}") +for name in names: + src = SRC / f"{name}.usd" + if not src.exists(): + continue + dst = DST / f"{name}.usd" + shutil.copy(src, dst) + st = Usd.Stage.Open(str(dst)) + root = st.GetDefaultPrim() + xc = UsdGeom.XformCache(Usd.TimeCode.Default()) + meshes = [d for d in Usd.PrimRange(root) if d.IsA(UsdGeom.Mesh)] + if not meshes: + print(f"{name:20s} нет мешей, пропуск"); continue + # world-space points under the item's own transforms + allpts, per = [], [] + for m in meshes: + P = np.asarray(UsdGeom.Mesh(m).GetPointsAttr().Get(), dtype=np.float64) + M = np.array(xc.GetLocalToWorldTransform(m), dtype=np.float64) + W = (np.c_[P, np.ones(len(P))] @ M)[:, :3] + per.append((m, W)); allpts.append(W) + A = np.concatenate(allpts, 0) + lo, hi = A.min(0), A.max(0) + ext = hi - lo + s = max(labels[name]["dims_mm"]) / 1000.0 / float(max(ext)) + cx, cy = (lo[0] + hi[0]) / 2.0, (lo[1] + hi[1]) / 2.0 + BELT_CLEAR_MM = 430.0 + # Singulated pose: rest on the largest face and send the longest axis down the belt. + # The line's belt is 450 mm between rails (measured: /World/_Rails at y +-0.22), so an + # item presented across its long axis jams and the whole queue stops behind it. + order = list(np.argsort(-ext)) # largest -> X, middle -> Y, least -> Z + for m, W in per: + Q = (W - np.array([cx, cy, lo[2]]))[:, order] * s + UsdGeom.Mesh(m).GetPointsAttr().Set([Gf.Vec3f(float(a), float(b), float(c)) for a, b, c in Q]) + e = UsdGeom.Mesh(m).GetExtentAttr() + if e: + e.Set([Gf.Vec3f(*[float(v) for v in Q.min(0)]), Gf.Vec3f(*[float(v) for v in Q.max(0)])]) + UsdGeom.Xformable(m).ClearXformOpOrder() # transform is now in the points + for d in Usd.PrimRange(root): + if d.IsA(UsdGeom.Xformable): + UsdGeom.Xformable(d).ClearXformOpOrder() + if d.IsA(UsdGeom.Imageable): + UsdGeom.Imageable(d).GetVisibilityAttr().Set(UsdGeom.Tokens.inherited) + st.GetRootLayer().Save() + chk = Usd.Stage.Open(str(dst)) + r = UsdGeom.BBoxCache(Usd.TimeCode.Default(), + [UsdGeom.Tokens.default_, UsdGeom.Tokens.render] + ).ComputeWorldBound(chk.GetDefaultPrim()).ComputeAlignedRange() + out = [(r.GetMax()[k] - r.GetMin()[k]) * 1000 for k in range(3)] + warn = " ШИРЕ ПОЛОТНА" if out[1] > BELT_CLEAR_MM else "" + print(f"{name:20s} {str([round(v*1000) for v in ext]):22s} {s:8.2f} " + f"{str([round(v) for v in out]):22s} поперёк {out[1]:5.0f} мм{warn}") diff --git a/control_test/scene/plow_cell_90_45_test.usd b/control_test/scene/plow_cell_90_45_test.usd new file mode 100644 index 0000000..214bd12 Binary files /dev/null and b/control_test/scene/plow_cell_90_45_test.usd differ diff --git a/control_test/scene/plow_cell_90_45_test.usd.before_side_cams b/control_test/scene/plow_cell_90_45_test.usd.before_side_cams new file mode 100644 index 0000000..17fa760 Binary files /dev/null and b/control_test/scene/plow_cell_90_45_test.usd.before_side_cams differ diff --git a/isaacsim_send.py b/isaacsim_send.py new file mode 100644 index 0000000..71aa183 --- /dev/null +++ b/isaacsim_send.py @@ -0,0 +1,316 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Send Python code to a running Isaac Sim instance via the python_server TCP socket. + +Usage: + # Inline code + python isaacsim_send.py 'print("hello")' + + # From stdin (pipe or heredoc) + echo 'print("hello")' | python isaacsim_send.py + + # Send a .py file + python isaacsim_send.py --file path/to/script.py + + # Send a file with injected variables + python isaacsim_send.py --file script.py --arg output_path=/tmp/shot.png --arg width=1920 + + # Custom host/port/timeout + python isaacsim_send.py --host 127.0.0.1 --port 8226 --timeout 120 'print("hello")' + + # Raw JSON output (default is formatted human-readable) + python isaacsim_send.py --raw 'print("hello")' + + # --file scripts are isolated by default (wrapped in a function scope). + # Use --context for persistent named namespaces: + python isaacsim_send.py --context recording --file setup.py + python isaacsim_send.py --context recording 'print(my_var)' + + # JSON envelope mode: named context, injected args, per-request timeout + python isaacsim_send.py --context my_session --execution-timeout 30 'x = 1' + python isaacsim_send.py --args-json '{"x": 42}' 'print(x)' + + # Fire-and-forget: get immediate ACK and task_id + python isaacsim_send.py --fire-and-forget 'import time; time.sleep(5)' + + # Introspection queries + python isaacsim_send.py --introspect status + python isaacsim_send.py --introspect contexts + python isaacsim_send.py --introspect tasks + +Exit codes: + 0 - Execution succeeded (status: "ok") + 1 - Execution failed (status: "error") or connection error +""" + +import argparse +import asyncio +import json +import sys + + +async def send_and_receive(host: str, port: int, source: str, timeout: float = 60.0) -> dict: + """Send Python source code or a JSON envelope and return the parsed JSON response.""" + reader, writer = await asyncio.wait_for(asyncio.open_connection(host, port), timeout=timeout) + writer.write(source.encode()) + writer.write_eof() + data = await asyncio.wait_for(reader.read(), timeout=timeout) + writer.close() + return json.loads(data.decode()) + + +def _inject_args(source: str, args: list[str]) -> str: + """Prepend variable assignments for --arg key=value pairs.""" + if not args: + return source + lines = [] + for arg in args: + key, _, value = arg.partition("=") + key = key.strip() + value = value.strip() + try: + parsed = eval(value, {"__builtins__": {}}) # noqa: S307 + lines.append(f"{key} = {repr(parsed)}") + except Exception: + lines.append(f'{key} = "{value}"') + return "\n".join(lines) + "\n" + source + + +def _wrap_isolated(source: str, args: list[str]) -> str: + """Wrap script in an async function scope to isolate from executor state. + + The python_server executor shares global state between connections. + Wrapping in a function prevents variable leakage between calls. + Top-level `await` expressions are supported inside the wrapper. + """ + # Build argument assignments + arg_lines = [] + for arg in args: + key, _, value = arg.partition("=") + key = key.strip() + value = value.strip() + try: + parsed = eval(value, {"__builtins__": {}}) # noqa: S307 + arg_lines.append(f" {key} = {repr(parsed)}") + except Exception: + arg_lines.append(f' {key} = "{value}"') + + # Indent the source + indented = "\n".join(" " + line for line in source.splitlines()) + + parts = ["async def _isolated_script():"] + if arg_lines: + parts.extend(arg_lines) + parts.append(indented) + parts.append("") + parts.append("await _isolated_script()") + + return "\n".join(parts) + + +def _parse_args_kv(arg_list: list[str]) -> dict: + """Convert a list of ``key=value`` strings to a dict with type inference.""" + result = {} + for arg in arg_list: + key, _, value = arg.partition("=") + key = key.strip() + value = value.strip() + try: + result[key] = eval(value, {"__builtins__": {}}) # noqa: S307 + except Exception: + result[key] = value + return result + + +def _needs_envelope(args: argparse.Namespace) -> bool: + """Return whether any flag requires the JSON envelope format.""" + return bool( + args.json_envelope + or args.context + or args.fire_and_forget + or args.execution_timeout is not None + or args.args_json + or args.introspect + ) + + +async def main() -> None: + parser = argparse.ArgumentParser(description="Send Python code to Isaac Sim python_server") + parser.add_argument("code", nargs="?", help="Python code to execute (reads stdin if omitted)") + parser.add_argument("--file", "-f", help="Path to a Python file to send instead of inline code") + parser.add_argument( + "--arg", "-a", action="append", default=[], help="Inject key=value as a global variable (use with --file)" + ) + parser.add_argument("--host", default="127.0.0.1", help="Server host (default: 127.0.0.1)") + parser.add_argument("--port", type=int, default=8226, help="Server port (default: 8226)") + parser.add_argument("--timeout", type=float, default=60.0, help="Client TCP timeout in seconds (default: 60)") + parser.add_argument("--raw", action="store_true", help="Print raw JSON instead of formatted output") + parser.add_argument( + "--no-isolate", + action="store_true", + help="Don't wrap --file scripts in isolated scope (use for state persistence)", + ) + + # JSON envelope options + envelope_group = parser.add_argument_group("JSON envelope options") + envelope_group.add_argument("--json-envelope", "-j", action="store_true", help="Force JSON envelope mode") + envelope_group.add_argument( + "--context", "-c", help="Named execution context (creates or reuses a persistent namespace)" + ) + envelope_group.add_argument( + "--fire-and-forget", + "--ff", + action="store_true", + dest="fire_and_forget", + help="Fire-and-forget mode: receive immediate ACK with task_id, code runs in background", + ) + envelope_group.add_argument( + "--execution-timeout", + "-t", + type=float, + metavar="SECONDS", + dest="execution_timeout", + help="Per-request server-side execution timeout (0 = no limit; different from --timeout)", + ) + envelope_group.add_argument( + "--args-json", + metavar="JSON", + help="Inject variables as a JSON object string, e.g. '{\"x\": 42}'", + ) + envelope_group.add_argument( + "--introspect", + metavar="COMMAND", + help=( + "Run an introspection query instead of executing code. " + "Commands: status, contexts, context, tasks, task, delete_context" + ), + ) + args = parser.parse_args() + + # Handle introspection queries + if args.introspect: + envelope: dict = {"introspect": args.introspect} + if args.context: + envelope["context"] = args.context + if args.code: + # Allow passing a task_id as a positional argument for 'task' queries + envelope["task_id"] = args.code + source = json.dumps(envelope) + try: + result = await send_and_receive(args.host, args.port, source, args.timeout) + except ConnectionRefusedError: + print(f"Error: Cannot connect to Isaac Sim at {args.host}:{args.port}", file=sys.stderr) + sys.exit(1) + except asyncio.TimeoutError: + print(f"Error: Timeout after {args.timeout}s waiting for response", file=sys.stderr) + sys.exit(1) + if args.raw: + print(json.dumps(result, indent=2)) + else: + if result.get("status") == "ok": + print(json.dumps(result.get("result"), indent=2)) + else: + print(f"ERROR: {result.get('result', result)}", file=sys.stderr) + sys.exit(1) + return + + # Build code string + if args.file: + with open(args.file) as f: + source_code = f.read() + if args.no_isolate or _needs_envelope(args): + source_code = _inject_args(source_code, args.arg) + else: + source_code = _wrap_isolated(source_code, args.arg) + elif args.code: + source_code = args.code + if not _needs_envelope(args): + source_code = _inject_args(source_code, args.arg) + else: + source_code = sys.stdin.read() + + if not source_code.strip(): + print("Error: No code provided", file=sys.stderr) + sys.exit(1) + + # Build the final request: JSON envelope or raw Python + if _needs_envelope(args): + envelope = {"code": source_code} + + if args.context: + envelope["context"] = args.context + if args.fire_and_forget: + envelope["fire_and_forget"] = True + if args.execution_timeout is not None: + envelope["timeout"] = args.execution_timeout + + # Merge args from --arg key=value pairs and --args-json + merged_args: dict = _parse_args_kv(args.arg) + if args.args_json: + try: + json_args = json.loads(args.args_json) + if isinstance(json_args, dict): + merged_args.update(json_args) + else: + print("Error: --args-json must be a JSON object", file=sys.stderr) + sys.exit(1) + except json.JSONDecodeError as exc: + print(f"Error: Invalid --args-json: {exc}", file=sys.stderr) + sys.exit(1) + if merged_args: + envelope["args"] = merged_args + + source = json.dumps(envelope) + else: + source = source_code + + try: + result = await send_and_receive(args.host, args.port, source, args.timeout) + except ConnectionRefusedError: + print(f"Error: Cannot connect to Isaac Sim at {args.host}:{args.port}", file=sys.stderr) + print("Make sure Isaac Sim is running with isaacsim.code_editor.python_server enabled.", file=sys.stderr) + sys.exit(1) + except asyncio.TimeoutError: + print(f"Error: Timeout after {args.timeout}s waiting for response", file=sys.stderr) + sys.exit(1) + + if args.raw: + print(json.dumps(result, indent=2)) + else: + # Fire-and-forget: show task_id prominently + if result.get("fire_and_forget"): + print(f"Task submitted. task_id: {result.get('task_id')}") + sys.exit(0) + + output = result.get("output", "") + if output: + print(output) + if "result" in result and result["result"] is not None: + print(f"=> {result['result']}") + if result.get("status") == "error": + print(f"\nERROR [{result.get('ename', '?')}]: {result.get('evalue', '?')}", file=sys.stderr) + for tb in result.get("traceback", []): + print(tb, file=sys.stderr) + + if result.get("elapsed_seconds") is not None: + print(f"(elapsed: {result['elapsed_seconds']:.2f}s)", file=sys.stderr) + + sys.exit(0 if result.get("status") == "ok" else 1) + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..2af74a0 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +# Isaac Sim 6.0 already ships torch, numpy and opencv; these are the extras. +ultralytics>=8.3 # FastSAM +# CRE-Stereo network code is cloned by scripts/fetch_models.py, not pip-installable diff --git a/robozon_sorter/__init__.py b/robozon_sorter/__init__.py new file mode 100644 index 0000000..68b8292 --- /dev/null +++ b/robozon_sorter/__init__.py @@ -0,0 +1,2 @@ +"""Robozon conveyor sorting cell for Isaac Sim: procedural scene + CRE-ROI v2b vision.""" +__version__ = "0.1.0" diff --git a/robozon_sorter/config.py b/robozon_sorter/config.py new file mode 100644 index 0000000..7555e8a --- /dev/null +++ b/robozon_sorter/config.py @@ -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"} diff --git a/robozon_sorter/cv/__init__.py b/robozon_sorter/cv/__init__.py new file mode 100644 index 0000000..76493c2 --- /dev/null +++ b/robozon_sorter/cv/__init__.py @@ -0,0 +1 @@ +from .pipeline import CreRoiV2b, classify, roundness # noqa: F401 diff --git a/robozon_sorter/cv/pipeline.py b/robozon_sorter/cv/pipeline.py new file mode 100644 index 0000000..d51bd4c --- /dev/null +++ b/robozon_sorter/cv/pipeline.py @@ -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)) diff --git a/robozon_sorter/sim/__init__.py b/robozon_sorter/sim/__init__.py new file mode 100644 index 0000000..89900aa --- /dev/null +++ b/robozon_sorter/sim/__init__.py @@ -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. +""" diff --git a/robozon_sorter/sim/lane_beams.py b/robozon_sorter/sim/lane_beams.py new file mode 100644 index 0000000..b536b4f --- /dev/null +++ b/robozon_sorter/sim/lane_beams.py @@ -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()) diff --git a/robozon_sorter/sim/mechanics.py b/robozon_sorter/sim/mechanics.py new file mode 100644 index 0000000..81a74f7 --- /dev/null +++ b/robozon_sorter/sim/mechanics.py @@ -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" diff --git a/robozon_sorter/sim/plow.py b/robozon_sorter/sim/plow.py new file mode 100644 index 0000000..0761076 --- /dev/null +++ b/robozon_sorter/sim/plow.py @@ -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 diff --git a/robozon_sorter/sim/plow_cell.py b/robozon_sorter/sim/plow_cell.py new file mode 100644 index 0000000..ce7717f --- /dev/null +++ b/robozon_sorter/sim/plow_cell.py @@ -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) diff --git a/robozon_sorter/sim/plow_cell_9045.py b/robozon_sorter/sim/plow_cell_9045.py new file mode 100644 index 0000000..a4f55e5 --- /dev/null +++ b/robozon_sorter/sim/plow_cell_9045.py @@ -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) diff --git a/robozon_sorter/sim/plow_contact.py b/robozon_sorter/sim/plow_contact.py new file mode 100644 index 0000000..3309c29 --- /dev/null +++ b/robozon_sorter/sim/plow_contact.py @@ -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) diff --git a/robozon_sorter/sim/plow_sort.py b/robozon_sorter/sim/plow_sort.py new file mode 100644 index 0000000..92971ba --- /dev/null +++ b/robozon_sorter/sim/plow_sort.py @@ -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"} diff --git a/robozon_sorter/sim/plow_vision.py b/robozon_sorter/sim/plow_vision.py new file mode 100644 index 0000000..b0c27b5 --- /dev/null +++ b/robozon_sorter/sim/plow_vision.py @@ -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) diff --git a/robozon_sorter/sim/run.py b/robozon_sorter/sim/run.py new file mode 100644 index 0000000..f68de7c --- /dev/null +++ b/robozon_sorter/sim/run.py @@ -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) diff --git a/robozon_sorter/sim/scene.py b/robozon_sorter/sim/scene.py new file mode 100644 index 0000000..1e395ff --- /dev/null +++ b/robozon_sorter/sim/scene.py @@ -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) diff --git a/robozon_sorter/sim/spawner.py b/robozon_sorter/sim/spawner.py new file mode 100644 index 0000000..cd2d68c --- /dev/null +++ b/robozon_sorter/sim/spawner.py @@ -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}) diff --git a/robozon_sorter/sim/staging.py b/robozon_sorter/sim/staging.py new file mode 100644 index 0000000..604c818 --- /dev/null +++ b/robozon_sorter/sim/staging.py @@ -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 diff --git a/scene/90_degree.usd b/scene/90_degree.usd new file mode 100644 index 0000000..6bea6b0 Binary files /dev/null and b/scene/90_degree.usd differ diff --git a/scene/_plow_cell_preview.png b/scene/_plow_cell_preview.png new file mode 100644 index 0000000..7aebe0b Binary files /dev/null and b/scene/_plow_cell_preview.png differ diff --git a/scene/demo.usd b/scene/demo.usd new file mode 100644 index 0000000..4607eea Binary files /dev/null and b/scene/demo.usd differ diff --git a/scene/plow_cell.usd b/scene/plow_cell.usd new file mode 100644 index 0000000..072df53 Binary files /dev/null and b/scene/plow_cell.usd differ diff --git a/scene/plow_cell_90_45_test.usd b/scene/plow_cell_90_45_test.usd new file mode 100644 index 0000000..17fa760 Binary files /dev/null and b/scene/plow_cell_90_45_test.usd differ diff --git a/scene/sorter.usd b/scene/sorter.usd new file mode 100644 index 0000000..f8d7ad9 Binary files /dev/null and b/scene/sorter.usd differ diff --git a/scripts/add_vision_to_plow_cell.py b/scripts/add_vision_to_plow_cell.py new file mode 100644 index 0000000..3a2720b --- /dev/null +++ b/scripts/add_vision_to_plow_cell.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python3 +"""Give scene/plow_cell.usd an infeed belt, the camera portal and the laser gate. + + python scripts/add_vision_to_plow_cell.py + +Purely additive: the conveyors, the Y-split pusher and the plow are left exactly as +authored, including their drives and the DiverterAnimGraph. Nothing existing is edited, +so the cell's kinematics are untouched. + +What gets added + ConveyorTrack_05 a fourth infeed belt, upstream of ConveyorTrack_02. It references the + SAME ConveyorBelt_A06.usd with the same scale as the existing tracks, + so it is the same conveyor with the same textures, not a lookalike. + Goods run -X, so "behind" ConveyorTrack_02 (which spans x -2..0) means + x 0..+2. + CameraMountFrame the portal, camera bodies and the three rectified stereo pairs, copied + CameraBodies verbatim from sorter.usd. They already sit over x=-0.75, which is + /RigRS inside ConveyorTrack_02's belt, so no repositioning is needed. + SortingRig/LaserGate the through-beam gate at the Y-split pusher (x=-3.74). + +Re-running is safe: existing prims are replaced rather than duplicated. +""" +from __future__ import annotations + +import shutil +import sys +from pathlib import Path + +from pxr import Gf, Sdf, Usd, UsdGeom + +ROOT = Path(__file__).resolve().parent.parent +CELL = ROOT / "scene" / "plow_cell.usd" +SORTER = ROOT / "scene" / "sorter.usd" + +# copied from sorter.usd unchanged - they are already aligned with this belt +FROM_SORTER = [ + "/World/CameraMountFrame", + "/World/CameraBodies", + "/RigRS", + "/World/SortingRig", # carries LaserGate (and its materials) +] + +# sorter.usd's SortingRig carries a plain box SpawnBelt at x 0..2.6. The real conveyor +# added below occupies the same lane, so the box and its rails/legs are dropped after the +# copy - otherwise two belts sit inside each other. LaserGate, the bin and the materials stay. +DROP_AFTER_COPY = [ + "/World/SortingRig/SpawnBelt", + "/World/SortingRig/SpawnRail_p", "/World/SortingRig/SpawnRail_n", + "/World/SortingRig/Spawn_Leg0", "/World/SortingRig/Spawn_Leg1", + "/World/SortingRig/Spawn_Leg2", "/World/SortingRig/Spawn_Leg3", +] + +INFEED_PRIM = "/World/ConveyorTrack_05" +INFEED_ASSET = "../assets/conveyors/ConveyorBelt_A06.usd" +# ConveyorTrack_02 sits at translate x=-2 and spans x -2..0, so the asset occupies +# [tx, tx+2]. Upstream of it is therefore tx=0 -> x 0..+2. +INFEED_TRANSLATE = Gf.Vec3d(0.0, 0.0, 0.0) +INFEED_SCALE = Gf.Vec3d(1.0, 0.5, 1.0) # identical to the other tracks + + +def _drop(layer: Sdf.Layer, path: str): + """remove a prim spec if present, so the script is idempotent""" + spec = layer.GetPrimAtPath(path) + if not spec: + return False + parent = layer.GetPrimAtPath(str(Sdf.Path(path).GetParentPath())) or layer.pseudoRoot + name = Sdf.Path(path).name + if name in parent.nameChildren: + del parent.nameChildren[name] + return True + return False + + +def add_infeed(layer: Sdf.Layer): + """a fourth conveyor upstream of ConveyorTrack_02, same asset and scale""" + _drop(layer, INFEED_PRIM) + spec = Sdf.CreatePrimInLayer(layer, INFEED_PRIM) + spec.specifier = Sdf.SpecifierDef + spec.typeName = "Xform" + spec.referenceList.prependedItems.append(Sdf.Reference(INFEED_ASSET)) + for name, value, vtype in ( + ("xformOp:translate", INFEED_TRANSLATE, Sdf.ValueTypeNames.Double3), + ("xformOp:scale", INFEED_SCALE, Sdf.ValueTypeNames.Double3)): + attr = Sdf.AttributeSpec(spec, name, vtype) + attr.default = value + order = Sdf.AttributeSpec(spec, "xformOpOrder", Sdf.ValueTypeNames.TokenArray) + order.default = ["xformOp:translate", "xformOp:scale"] + return INFEED_PRIM + + +def copy_from_sorter(layer: Sdf.Layer, src: Sdf.Layer): + copied = [] + for path in FROM_SORTER: + if not src.GetPrimAtPath(path): + print(f" skip {path} - not in sorter.usd") + continue + _drop(layer, path) + if Sdf.CopySpec(src, Sdf.Path(path), layer, Sdf.Path(path)): + copied.append(path) + return copied + + +def main(): + if not CELL.exists(): + print(f"{CELL} not found - build it with scripts/build_plow_cell.py") + return 1 + if not SORTER.exists(): + print(f"{SORTER} not found - the camera stand is copied from it") + return 1 + + backup = CELL.with_suffix(".usd.bak") + if backup.exists(): + print(f"backup {backup.name} already exists - keeping the pre-vision copy") + else: + shutil.copy(CELL, backup) + print(f"backup -> {backup.name}") + + layer = Sdf.Layer.FindOrOpen(str(CELL)) + src = Sdf.Layer.FindOrOpen(str(SORTER)) + + infeed = add_infeed(layer) + print(f"added {infeed} (references {INFEED_ASSET}, translate {tuple(INFEED_TRANSLATE)})") + + for path in copy_from_sorter(layer, src): + print(f"copied {path}") + + for path in DROP_AFTER_COPY: + if _drop(layer, path): + print(f"dropped {path} (superseded by the real conveyor)") + + layer.Save() + print(f"saved {CELL}") + + # verify by composing the result + stage = Usd.Stage.Open(str(CELL)) + cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True) + print("\nverification:") + ok = True + for path in [f"{INFEED_PRIM}/Belt", "/World/ConveyorTrack_02/Belt", + "/World/CameraMountFrame", "/World/SortingRig/LaserGate"]: + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + print(f" MISSING {path}") + ok = False + continue + r = cache.ComputeWorldBound(prim).ComputeAlignedRange() + if r.IsEmpty(): + print(f" EMPTY {path}") + ok = False + continue + mn, mx = r.GetMin(), r.GetMax() + print(f" ok {path}: x[{mn[0]:7.3f}..{mx[0]:7.3f}] y[{mn[1]:6.3f}..{mx[1]:6.3f}] " + f"top_z={mx[2]:.3f}") + cams = stage.GetPrimAtPath("/RigRS") + n = len(cams.GetChildren()) if cams.IsValid() else 0 + print(f" {'ok ' if n == 6 else 'PROBLEM'} /RigRS: {n} cameras") + return 0 if ok and n == 6 else 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scripts/bin_scan.py b/scripts/bin_scan.py new file mode 100644 index 0000000..2326770 --- /dev/null +++ b/scripts/bin_scan.py @@ -0,0 +1,40 @@ +"""Find the real D-item collection bin/container geometry near the pusher branch.""" +import omni.usd +from pxr import Usd, UsdGeom + +stage = omni.usd.get_context().get_stage() +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +print("=== config bin constants (old sorter.usd) ===") +import sys +sys.path.insert(0, "/home/dasha/robozon-sorter") +from robozon_sorter import config as C +print(f"BIN_X0={C.BIN_X0} BIN_X1={C.BIN_X1} BIN_Y0={C.BIN_Y0} BIN_Y1={C.BIN_Y1} BIN_LIP_Z={C.BIN_LIP_Z}") + +print("\n=== SortingRig subtree (candidate bin location) ===") +p = stage.GetPrimAtPath("/World/SortingRig") +if p.IsValid(): + for c in p.GetChildren(): + r = bbc.ComputeWorldBound(c).ComputeAlignedRange() + if not r.IsEmpty(): + mn, mx = r.GetMin(), r.GetMax() + print(f" {c.GetPath()} x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]") +else: + print(" MISSING") + +print("\n=== ConveyorTrack_03 subtree (pusher branch) ===") +p = stage.GetPrimAtPath("/World/ConveyorTrack_03") +for c in p.GetChildren(): + r = bbc.ComputeWorldBound(c).ComputeAlignedRange() + if not r.IsEmpty(): + mn, mx = r.GetMin(), r.GetMax() + print(f" {c.GetPath()} x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]") + +print("\n=== search whole stage for Bin-like names ===") +for pr in stage.Traverse(): + nm = pr.GetName().lower() + if "bin" in nm: + r = bbc.ComputeWorldBound(pr).ComputeAlignedRange() + if not r.IsEmpty(): + mn, mx = r.GetMin(), r.GetMax() + print(f" {pr.GetPath()} x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]") diff --git a/scripts/bottle_collision_check.py b/scripts/bottle_collision_check.py new file mode 100644 index 0000000..b1112e4 --- /dev/null +++ b/scripts/bottle_collision_check.py @@ -0,0 +1,23 @@ +"""Does bottle's referenced mesh actually carry collision, compared to a working item?""" +import omni.usd +from pxr import Usd, UsdGeom, UsdPhysics +import sys +sys.path.insert(0, "/home/dasha/robozon-sorter") +from robozon_sorter import config as C + +for name in ["bottle", "box_300x200x200", "bag", "helmet"]: + tmp_stage = Usd.Stage.CreateInMemory() + prim = tmp_stage.DefinePrim("/probe", "Xform") + prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{name}.usd")) + print(f"=== {name} ===") + n_col = 0 + n_mesh = 0 + for p in Usd.PrimRange(prim): + if p.IsA(UsdGeom.Mesh): + n_mesh += 1 + has_col = p.HasAPI(UsdPhysics.CollisionAPI) or p.HasAPI(UsdPhysics.MeshCollisionAPI) + if has_col: + n_col += 1 + approx = p.GetAttribute("physics:approximation") + print(f" {p.GetPath()} type={p.GetTypeName()} approx={approx.Get() if approx else None}") + print(f" meshes={n_mesh} prims_with_collision={n_col}") diff --git a/scripts/branch_check.py b/scripts/branch_check.py new file mode 100644 index 0000000..7b7f818 --- /dev/null +++ b/scripts/branch_check.py @@ -0,0 +1,97 @@ +"""1) Does Belt_01 actually CARRY an item, and does it carry it to BinD? + 2) How long does the pusher take to stroke out and return home?""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene, plow_cell_9045 + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) +await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True) + +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +b01 = stage.GetPrimAtPath(_scene.BRANCH) +r = bbc.ComputeWorldBound(b01).ComputeAlignedRange() +print(f"Belt_01 bbox x[{r.GetMin()[0]:+.3f}..{r.GetMax()[0]:+.3f}] " + f"y[{r.GetMin()[1]:+.3f}..{r.GetMax()[1]:+.3f}] top_z={r.GetMax()[2]:+.3f}") +sv = b01.GetAttribute("physxSurfaceVelocity:surfaceVelocity").Get() +en = b01.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled") +xc = UsdGeom.XformCache() +wv = xc.GetLocalToWorldTransform(b01).TransformDir(Gf.Vec3d(*sv)) +print(f" local surfaceVelocity={sv} enabled={en.Get() if en else None}") +print(f" WORLD drive = ({wv[0]:+.3f},{wv[1]:+.3f},{wv[2]:+.3f}) |v|={wv.GetLength():.3f}") +api = UsdShade.MaterialBindingAPI(b01) +mat, _ = api.ComputeBoundMaterial(materialPurpose="physics") +if mat: + m = UsdPhysics.MaterialAPI(mat.GetPrim()) + print(f" friction static/dynamic = {m.GetStaticFrictionAttr().Get()}/{m.GetDynamicFrictionAttr().Get()}") +bd = bbc.ComputeWorldBound(stage.GetPrimAtPath("/World/SortingRig/BinD_Floor")).ComputeAlignedRange() +print(f"BinD floor x[{bd.GetMin()[0]:+.2f}..{bd.GetMax()[0]:+.2f}] y[{bd.GetMin()[1]:+.2f}..{bd.GetMax()[1]:+.2f}] " + f"centre=({(bd.GetMin()[0]+bd.GetMax()[0])/2:+.2f},{(bd.GetMin()[1]+bd.GetMax()[1])/2:+.2f})") + +ipath = "/World/Items/_branchprobe" +def spawn(x, y): + if stage.GetPrimAtPath(ipath).IsValid(): + stage.RemovePrim(ipath) + prim = UsdGeom.Xform.Define(stage, ipath).GetPrim() + prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / "bag.usd")) + xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(x, y, C.BELT_Z + 0.06)) + UsdPhysics.RigidBodyAPI.Apply(prim) + 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) + UsdGeom.Imageable(prim).MakeVisible() + return RigidPrim(paths=[ipath]) + +print("\n--- placing an item DIRECTLY on Belt_01 at (-4.10, +0.70): does it reach BinD? ---") +rp = spawn(-4.10, 0.70) +tl.play(); await app_utils.update_app_async(steps=10) +p0 = rp.get_world_poses()[0].numpy()[0].copy() +for i in range(9): + await app_utils.update_app_async(steps=40) + p = rp.get_world_poses()[0].numpy()[0] + print(f" t~{(i+1)*40/60:4.1f}s x={float(p[0]):+.2f} y={float(p[1]):+.2f} z={float(p[2]):+.2f}") +pf = rp.get_world_poses()[0].numpy()[0] +in_bin = (-6.21 < float(pf[0]) < -4.95) and (1.57 < float(pf[1]) < 2.86) +print(f" travelled ({float(pf[0])-float(p0[0]):+.2f},{float(pf[1])-float(p0[1]):+.2f}) IN BIN_D: {in_bin}") +tl.stop(); await app_utils.update_app_async(steps=6) + +print("\n--- pusher stroke-out / return timing ---") +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + bop = op; break +bbase = bop.Get() +def blade_to(y): + bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2])) +blade_to(C.BLADE_HOME_Y) +tl.play(); await app_utils.update_app_async(steps=5) +for label, a, b, spd in (("out ", C.BLADE_HOME_Y, 0.55, 1.3), ("back", 0.55, C.BLADE_HOME_Y, 1.3), + ("back", 0.55, C.BLADE_HOME_Y, 2.5)): + blade_to(a); await app_utils.update_app_async(steps=3) + dur = abs(b - a) / spd + t0 = float(tl.get_current_time()) + while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / dur) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + if u >= 1.0: + break + print(f" {label} @ {spd} m/s : {float(tl.get_current_time())-t0:.3f} s " + f"(stroke {abs(b-a):.2f} m)") +tl.stop() diff --git a/scripts/build_fork_v2.py b/scripts/build_fork_v2.py new file mode 100644 index 0000000..c39fbe8 --- /dev/null +++ b/scripts/build_fork_v2.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +"""Lay the discharge out as a fork: C carries straight on, B branches, plow in the corner. + + /home/whatevenif/isaacsim/python.sh scripts/build_fork_v2.py + +Design, from the sketch: + + main run ──────┬─────────────▶ lane C (straight on, same line as the run) + │ + └───▶ lane B (branches away at 45 deg) + plow sits in this corner + +Class C needs no action - it runs straight through, and the blade at rest closes the B +mouth, so C is the default route and the blade only leans on it if it wanders. Class B is +the only case that actuates: the blade swings over, the B mouth opens, and the item drives +into its branch instead of being shoved across a belt. + +**How the tracks are actually built** - this is what the first attempt got wrong. Each +ConveyorTrack carries `translate + orient(quaternion) + scale`; there is no rotateZ to +write, so clearing the op order and adding one silently produced a different transform. The +`Belt` child then sits at a fixed local offset of +1.0 along the track's local X, scaled by +the track's own X scale. So: + + belt centre = track origin + (local +X in world) * 1.0 * scale_x + +Placing a branch therefore means: point the track's local X down the branch, and put the +track origin at the fork apex, which lands the belt centre one length-half down the branch. + +Verification uses a **fresh** BBoxCache after every write. Reusing one is what made the +first attempt report "nothing moved" while the geometry underneath had in fact been +scattered. +""" +from __future__ import annotations + +import math +import sys +from pathlib import Path + +from pxr import Gf, Usd, UsdGeom + +SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd" +APEX = Gf.Vec3d(-7.00, 0.0, 0.0) # downstream end of the main run +TRAYS = "/World/PlowContainers" +PLOW = "/World/Diverters/DiverterEnd" + +LANE_C = "/World/ConveyorTrack_01" # straight on, 0 deg off the run +LANE_B = "/ConveyorTrack_01" # branches 45 deg toward -Y +C_DEG, B_DEG = 0.0, -45.0 +TRAY_AT = 2.60 # how far down each branch its tray sits + + +def quat_z(deg): + h = math.radians(deg) / 2.0 + return Gf.Quatd(math.cos(h), Gf.Vec3d(0, 0, math.sin(h))) + + +def world_dir(deg): + """travel direction of a branch `deg` off the -X run""" + a = math.radians(180.0 + deg) + return Gf.Vec3d(math.cos(a), math.sin(a), 0.0) + + +def place_track(stage, path, deg): + """point the track down its branch and hang its origin on the apex""" + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + return False + xf = UsdGeom.Xformable(prim) + for op in xf.GetOrderedXformOps(): + n = op.GetOpName() + if n.endswith("translate"): + op.Set(APEX) + elif n.endswith("orient"): + op.Set(quat_z(180.0 + deg)) # local +X onto the branch direction + return True + + +def move_group(stage, prefix, to_xy): + """shift a tray so its centre lands on `to_xy`, keeping its parts together""" + cache = UsdGeom.BBoxCache(0, ["default"]) + parts = [c for c in stage.GetPrimAtPath(TRAYS).GetChildren() + if c.GetName().startswith(prefix)] + if not parts: + return 0 + xs, ys = [], [] + for c in parts: + r = cache.ComputeWorldBound(c).ComputeAlignedRange() + xs += [r.GetMin()[0], r.GetMax()[0]] + ys += [r.GetMin()[1], r.GetMax()[1]] + dx = to_xy[0] - (min(xs) + max(xs)) / 2.0 + dy = to_xy[1] - (min(ys) + max(ys)) / 2.0 + n = 0 + for c in parts: + for op in UsdGeom.Xformable(c).GetOrderedXformOps(): + if op.GetOpName().endswith("translate"): + t = op.Get() + op.Set(type(t)(t[0] + dx, t[1] + dy, t[2])) + n += 1 + break + return n + + +def report(stage, path, label): + """measure with a FRESH cache - a reused one reports the state before the write""" + r = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound( + stage.GetPrimAtPath(path)).ComputeAlignedRange() + if r.IsEmpty(): + print(f" {label:22s} (empty)") + return + print(f" {label:22s} x[{r.GetMin()[0]:+.2f},{r.GetMax()[0]:+.2f}] " + f"y[{r.GetMin()[1]:+.2f},{r.GetMax()[1]:+.2f}] top z={r.GetMax()[2]:.3f}") + + +def main(): + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + stage = Usd.Stage.Open(str(SCENE)) + + for path, deg, tag in ((LANE_C, C_DEG, "C"), (LANE_B, B_DEG, "B")): + if not place_track(stage, path, deg): + print(f" {path} missing"); continue + d = world_dir(deg) + tray = (APEX[0] + d[0] * TRAY_AT, APEX[1] + d[1] * TRAY_AT) + moved = move_group(stage, f"{tag}_", tray) + print(f"branch {tag}: {deg:+.0f} deg, dir ({d[0]:+.3f},{d[1]:+.3f}), " + f"tray -> ({tray[0]:+.2f},{tray[1]:+.2f}) [{moved} parts]") + + # the plow sits in the corner between the two branches + pxf = UsdGeom.Xformable(stage.GetPrimAtPath(PLOW)) + for op in pxf.GetOrderedXformOps(): + if op.GetOpName().endswith("translate"): + t = op.Get() + op.Set(Gf.Vec3d(APEX[0], APEX[1], t[2])) + break + + stage.GetRootLayer().Save() + print("\n--- measured after the write (fresh cache each time) ---") + report(stage, "/World/ConveyorTrack_04/Belt", "main run") + report(stage, f"{LANE_C}/Belt", "lane C (straight)") + report(stage, f"{LANE_B}/Belt", "lane B (45 deg)") + report(stage, f"{PLOW}/Arm", "plow arm") + for tag in ("B", "C"): + cache = UsdGeom.BBoxCache(0, ["default"]) + xs, ys = [], [] + for c in stage.GetPrimAtPath(TRAYS).GetChildren(): + if c.GetName().startswith(f"{tag}_"): + r = cache.ComputeWorldBound(c).ComputeAlignedRange() + xs += [r.GetMin()[0], r.GetMax()[0]]; ys += [r.GetMin()[1], r.GetMax()[1]] + if xs: + print(f" tray {tag} centre " + f"({(min(xs)+max(xs))/2:+.2f},{(min(ys)+max(ys))/2:+.2f})") + print(f"\nsaved {SCENE}") + + +if __name__ == "__main__": + main() diff --git a/scripts/build_plow_cell.py b/scripts/build_plow_cell.py new file mode 100644 index 0000000..097685c --- /dev/null +++ b/scripts/build_plow_cell.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +"""Build scene/plow_cell.usd from the authored 90_degree.usd. + + python scripts/build_plow_cell.py [path/to/90_degree.usd] + +The source is the original authored cell - conveyor art, the Y-split pusher, the plow +(``DiverterEnd``) and its OmniGraph drive script exactly as built. This script does not +regenerate geometry; it only makes the file self-contained inside this repo: + +1. **References re-pointed.** The source pulls conveyor art straight off the Omniverse S3 + bucket and the plow meshes from its own folder. Both are re-pointed at the repo's + ``assets/`` tree so the scene composes offline (``scripts/fetch_assets.py`` fills + ``assets/conveyors/``). + +2. **Baked drive animation stripped.** The pusher's ``PusherSlide`` carries a long + ``targetPosition.timeSamples`` track. Time samples outrank the attribute default, so + anything that tries to *control* that drive - the authored script node or Python - is + overwritten every frame while the timeline runs. The track is dropped; the drive keeps + its authored gains and limits. (The plow's own ``ArmHinge`` is already clean in + 90_degree.usd; the earlier fixed.usd bakes it too, hence the pattern matches both.) + +The authored ``DiverterAnimGraph`` script node is deliberately kept: open the scene, press +Play, and the cell demonstrates itself the way it was built. ``sim/plow_cell.py`` switches +that graph off when you want to drive the plow from code instead. +""" +from __future__ import annotations + +import re +import shutil +import subprocess +import sys +import tempfile +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent +OUT = ROOT / "scene" / "plow_cell.usd" +DEFAULT_SRC = (Path.home() / "Desktop" / "isaac_sim_project" / "test_isassc" / "90_degree.usd") + +S3 = ("https://omniverse-content-production.s3-us-west-2.amazonaws.com/" + "Assets/Isaac/6.0/Isaac/Props/Conveyors/") + +# asset path in the source -> path relative to scene/ +REFS = { + f"{S3}ConveyorBelt_A06.usd": "../assets/conveyors/ConveyorBelt_A06.usd", + f"{S3}ConveyorBelt_A24.usd": "../assets/conveyors/ConveyorBelt_A24.usd", + "./plow_base.usd": "../assets/plow/plow_base.usd", + "./plow_arm.usd": "../assets/plow/plow_arm.usd", +} + +# Baked drive tracks fight every attempt to control a diverter. In 90_degree.usd only the +# pusher's linear drive carries one (the plow's angular drive is already clean), but the +# earlier fixed.usd bakes the plow too - match both so either source builds the same way. +BAKED = re.compile(r"drive:(linear|angular):physics:targetPosition\.timeSamples") + + +def usdcat(src: Path, dst: Path) -> None: + if not shutil.which("usdcat"): + sys.exit("usdcat not found - it ships with USD / Isaac Sim and is needed to " + "convert the binary .usd to text and back") + subprocess.run(["usdcat", str(src), "-o", str(dst)], check=True) + + +def strip_baked_track(text: str) -> tuple[str, int]: + """drop `.timeSamples = { ... }` blocks""" + out, skipping, dropped = [], False, 0 + for line in text.splitlines(keepends=True): + if not skipping and BAKED.search(line) and line.rstrip().endswith("{"): + skipping, dropped = True, dropped + 1 + continue + if skipping: + if line.strip() == "}": + skipping = False + continue + out.append(line) + return "".join(out), dropped + + +def repoint_refs(text: str) -> tuple[str, dict[str, int]]: + counts = {} + for old, new in REFS.items(): + n = text.count(f"@{old}@") + if n: + text = text.replace(f"@{old}@", f"@{new}@") + counts[old] = n + return text, counts + + +def build(src: Path) -> Path: + if not src.exists(): + sys.exit(f"source scene not found: {src}") + OUT.parent.mkdir(parents=True, exist_ok=True) + + with tempfile.TemporaryDirectory() as tmp: + flat = Path(tmp) / "src.usda" + usdcat(src, flat) + text = flat.read_text() + + text, refs = repoint_refs(text) + text, dropped = strip_baked_track(text) + + missing = [k for k, v in refs.items() if v == 0] + if missing: + print("warning: reference not found in source (layout may have changed):") + for m in missing: + print(f" {m}") + + edited = Path(tmp) / "edited.usda" + edited.write_text(text) + usdcat(edited, OUT) + + print(f"built {OUT.relative_to(ROOT)} from {src}") + for old, new in REFS.items(): + print(f" ref {refs[old]}x {Path(old).name:24s} -> {new}") + print(f" drop {dropped}x baked drive targetPosition.timeSamples") + left = re.findall(r"@(https?://[^@]+)@", OUT.read_text(errors="ignore")) if OUT.suffix == ".usda" else [] + if left: + print(f" warning: {len(left)} remote reference(s) still present") + return OUT + + +if __name__ == "__main__": + build(Path(sys.argv[1]) if len(sys.argv) > 1 else DEFAULT_SRC) diff --git a/scripts/build_y_fork.py b/scripts/build_y_fork.py new file mode 100644 index 0000000..65e9813 --- /dev/null +++ b/scripts/build_y_fork.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +"""Rebuild the discharge as a Y fork with the plow at its apex. + + /home/whatevenif/isaacsim/python.sh scripts/build_y_fork.py + +The layout so far was a T: lane B perpendicular, lane C at 45 deg, meeting the main run at +different x, with the blade out in the middle of the belt trying to shove goods 400 mm +sideways onto them. Every failure this session came from that - the dead zone, the wedges, +goods stalling on the lip - because a push was being asked to do the job of a route. + +A fork does not need the push. Both branches leave one apex, the blade sits in it as a +railway point, and goods **drive** into their branch: + + rest (class C) blade closes the B mouth -> everything runs straight on to C + B arrives blade swings over -> the B mouth opens and takes it + +Geometry, all from the apex at the downstream end of the main run: + + apex x -7.00, y 0 + branch C 15 deg up from the run (travel -0.966, +0.259) + branch B 35 deg down from the run (travel -0.819, -0.574) + +Each track is placed by measurement, not by assumption: the script reads where the belt slab +currently sits relative to its own prim origin, then sets the transform so the slab's near +end lands on the apex pointing along its branch. The trays follow their branches. +""" +from __future__ import annotations + +import math +import sys +from pathlib import Path + +from pxr import Gf, Usd, UsdGeom + +SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd" +APEX = (-7.00, 0.0) +TRAYS = "/World/PlowContainers" + +# track prim -> (branch angle from the -X run, tray prefix, tray distance along the branch) +BRANCHES = { + "/World/ConveyorTrack_01": dict(deg=+15.0, tray="C_", tray_at=2.30, name="C"), + "/ConveyorTrack_01": dict(deg=-35.0, tray="B_", tray_at=2.30, name="B"), +} + + +def _dir(deg): + """world travel direction of a branch `deg` off the -X run""" + a = math.radians(180.0 - deg) # -X is 180 deg; +deg swings toward +Y + return math.cos(a), math.sin(a) + + +def _belt_of(stage, track): + for p in (f"{track}/Belt", f"{track}/Belt_01"): + if stage.GetPrimAtPath(p).IsValid(): + return p + return None + + +def _set_xform(stage, path, tx, ty, tz, rot_deg): + xf = UsdGeom.Xformable(stage.GetPrimAtPath(path)) + xf.ClearXformOpOrder() + xf.AddTranslateOp().Set(Gf.Vec3d(tx, ty, tz)) + xf.AddRotateZOp().Set(float(rot_deg)) + + +def main(): + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + stage = Usd.Stage.Open(str(SCENE)) + bb = UsdGeom.BBoxCache(0, ["default"]) + xc = UsdGeom.XformCache() + + for track, b in BRANCHES.items(): + prim = stage.GetPrimAtPath(track) + if not prim.IsValid(): + print(f" {track} missing"); continue + belt = _belt_of(stage, track) + if belt is None: + print(f" {track} has no Belt"); continue + + # where the slab sits now, relative to this track's own origin + org = xc.GetLocalToWorldTransform(prim).ExtractTranslation() + r = bb.ComputeWorldBound(stage.GetPrimAtPath(belt)).ComputeAlignedRange() + span_x, span_y = r.GetMax()[0] - r.GetMin()[0], r.GetMax()[1] - r.GetMin()[1] + length = max(span_x, span_y) + top_z = r.GetMax()[2] + # the slab's centre offset from the origin, in the track's own frame + cx = (r.GetMin()[0] + r.GetMax()[0]) / 2.0 - org[0] + cy = (r.GetMin()[1] + r.GetMax()[1]) / 2.0 - org[1] + off = math.hypot(cx, cy) + + dx, dy = _dir(b["deg"]) + # put the slab centre half a length down the branch from the apex + tx = APEX[0] + dx * (length / 2.0) - (dx * off - dx * off) + ty = APEX[1] + dy * (length / 2.0) + _set_xform(stage, track, tx - cx, ty - cy, org[2], 180.0 - b["deg"]) + + r2 = bb.ComputeWorldBound(stage.GetPrimAtPath(belt)).ComputeAlignedRange() + print(f" branch {b['name']} {b['deg']:+.0f} deg dir ({dx:+.3f},{dy:+.3f}) " + f"length {length:.2f} m") + print(f" slab now x[{r2.GetMin()[0]:+.2f},{r2.GetMax()[0]:+.2f}] " + f"y[{r2.GetMin()[1]:+.2f},{r2.GetMax()[1]:+.2f}] top z={r2.GetMax()[2]:.3f}") + + # the tray rides to the end of its branch + tex, tey = APEX[0] + dx * b["tray_at"], APEX[1] + dy * b["tray_at"] + moved = 0 + for c in stage.GetPrimAtPath(TRAYS).GetChildren(): + if not c.GetName().startswith(b["tray"]): + continue + cr = bb.ComputeWorldBound(c).ComputeAlignedRange() + ccx = (cr.GetMin()[0] + cr.GetMax()[0]) / 2.0 + ccy = (cr.GetMin()[1] + cr.GetMax()[1]) / 2.0 + cxf = UsdGeom.Xformable(c) + for op in cxf.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + t = op.Get() + op.Set(type(t)(t[0] + (tex - ccx), t[1] + (tey - ccy), t[2])) + moved += 1 + break + print(f" tray {b['name']} -> ({tex:+.2f},{tey:+.2f}) {moved} parts moved") + + stage.GetRootLayer().Save() + print(f"saved {SCENE}") + print("NOTE: the blade's rest position is now 'B closed', not 0 - re-measure which sign") + print(" closes B before running a sort.") + + +if __name__ == "__main__": + main() diff --git a/scripts/cam_check.py b/scripts/cam_check.py new file mode 100644 index 0000000..88beb6d --- /dev/null +++ b/scripts/cam_check.py @@ -0,0 +1,71 @@ +"""Какая камера сдвинута: сверка живой сцены с тем, что записано в файле. + +В сцене несколько камер разного назначения - шесть стереокамер стенда в /RigRS, корпуса +камер в /World/CameraBodies и служебная перспектива вьюпорта. Сдвиг любой из них +выглядит одинаково, а чинятся они по-разному, поэтому сначала находится ТА САМАЯ. + +Живая сцена читается из памяти, эталон - из слоя на диске: несовпадение и есть правка, +сделанная мышью во вьюпорте. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd +from pxr import Usd, UsdGeom, Gf + +live = omni.usd.get_context().get_stage() +path = live.GetRootLayer().identifier +print(f"живая сцена: {path}\n") + +disk = Usd.Stage.Open(path) +cache_l = UsdGeom.XformCache() +cache_d = UsdGeom.XformCache() + +cams = [p for p in live.Traverse() if p.IsA(UsdGeom.Camera)] +print(f"камер в сцене: {len(cams)}\n") +print(f" {'камера':44s} {'положение сейчас':>34s} расхождение с файлом") +print(" " + "-" * 104) +moved = [] +for c in cams: + p = str(c.GetPath()) + Ml = cache_l.GetLocalToWorldTransform(c) + tl_ = Ml.ExtractTranslation() + d = disk.GetPrimAtPath(p) + if not d or not d.IsValid(): + print(f" {p:44s} ({tl_[0]:+7.3f},{tl_[1]:+7.3f},{tl_[2]:+7.3f}) нет в файле") + continue + Md = cache_d.GetLocalToWorldTransform(d) + td = Md.ExtractTranslation() + dt = (tl_ - td).GetLength() + # угловое расхождение по направлению взгляда камеры (-Z в её системе) + vl = Ml.TransformDir(Gf.Vec3d(0, 0, -1)); vd = Md.TransformDir(Gf.Vec3d(0, 0, -1)) + vl = vl / (vl.GetLength() or 1); vd = vd / (vd.GetLength() or 1) + import math + ang = math.degrees(math.acos(max(-1.0, min(1.0, vl[0]*vd[0] + vl[1]*vd[1] + vl[2]*vd[2])))) + flag = "СДВИНУТА" if (dt > 0.001 or ang > 0.1) else "совпадает" + print(f" {p:44s} ({tl_[0]:+7.3f},{tl_[1]:+7.3f},{tl_[2]:+7.3f}) " + f"{dt*1000:7.1f} мм / {ang:5.2f}° {flag}") + if flag == "СДВИНУТА": + moved.append((p, td, tl_, dt, ang)) + +# служебная перспектива вьюпорта - её в файле обычно нет, она хранится в сессии +persp = live.GetPrimAtPath("/OmniverseKit_Persp") +if persp.IsValid(): + M = cache_l.GetLocalToWorldTransform(persp) + t = M.ExtractTranslation() + print(f"\n перспектива вьюпорта /OmniverseKit_Persp: " + f"({t[0]:+.2f}, {t[1]:+.2f}, {t[2]:+.2f})") + +print() +if moved: + print(f"СДВИНУТО КАМЕР: {len(moved)}") + for p, td, tlv, dt, ang in moved: + print(f" {p}") + print(f" было в файле: ({td[0]:+7.3f},{td[1]:+7.3f},{td[2]:+7.3f})") + print(f" стало сейчас: ({tlv[0]:+7.3f},{tlv[1]:+7.3f},{tlv[2]:+7.3f})") + print(f" расхождение {dt*1000:.1f} мм, поворот {ang:.2f}°") +else: + print("Ни одна камера-прем не сдвинута относительно файла.") + print("Значит двигали перспективу вьюпорта - она в файл не пишется и на прогоны не влияет.") diff --git a/scripts/cam_reset.py b/scripts/cam_reset.py new file mode 100644 index 0000000..3e49333 --- /dev/null +++ b/scripts/cam_reset.py @@ -0,0 +1,52 @@ +"""Вернуть перспективу вьюпорта на обзор ячейки. + +Сдвинута оказалась только служебная камера /OmniverseKit_Persp - она уехала в +(-8.96, -5.49, -6.70), то есть ПОД пол, и смотрела снизу. Шесть стереокамер стенда в +/RigRS не тронуты (0.0 мм, 0.00°), их править нечего - а именно они участвуют в прогонах. + +Перспектива вьюпорта в файл не пишется и на физику с замерами не влияет: это только то, +что видит человек. Поэтому здесь она ставится по фактическому габариту ячейки, а не по +запомненному числу - сцена менялась, и старая точка могла бы снова смотреть мимо. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.rendering_manager import ViewportManager +from pxr import Usd, UsdGeom, Gf + +stage = omni.usd.get_context().get_stage() +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +# габарит по дорожкам и плугу - то, на что осмысленно смотреть +lo = Gf.Vec3d(1e9, 1e9, 1e9); hi = Gf.Vec3d(-1e9, -1e9, -1e9) +for p in stage.Traverse(): + n = p.GetName() + if not (n.startswith("ConveyorTrack") or n == "Diverters"): + continue + r = bb.ComputeWorldBound(p).ComputeAlignedRange() + a, b = r.GetMin(), r.GetMax() + for i in range(3): + lo[i] = min(lo[i], a[i]); hi[i] = max(hi[i], b[i]) +ctr = Gf.Vec3d((lo[0]+hi[0])/2, (lo[1]+hi[1])/2, (lo[2]+hi[2])/2) +span = max(hi[0]-lo[0], hi[1]-lo[1]) +print(f"габарит ячейки: x {lo[0]:+.2f}..{hi[0]:+.2f} y {lo[1]:+.2f}..{hi[1]:+.2f} " + f"z {lo[2]:+.2f}..{hi[2]:+.2f}") +print(f"центр ({ctr[0]:+.2f}, {ctr[1]:+.2f}, {ctr[2]:+.2f}), протяжённость {span:.1f} м") + +# три четверти сверху-сбоку: вся линия в кадре, плуг и пушер видны не с торца +eye = Gf.Vec3d(ctr[0] + span * 0.45, ctr[1] - span * 0.65, ctr[2] + span * 0.55) +tgt = Gf.Vec3d(ctr[0], ctr[1], 1.60) +ViewportManager.set_camera_view("/OmniverseKit_Persp", + eye=[eye[0], eye[1], eye[2]], + target=[tgt[0], tgt[1], tgt[2]]) +await app_utils.update_app_async(steps=30) + +cache = UsdGeom.XformCache() +t = cache.GetLocalToWorldTransform(stage.GetPrimAtPath("/OmniverseKit_Persp")).ExtractTranslation() +print(f"\nперспектива возвращена: ({t[0]:+.2f}, {t[1]:+.2f}, {t[2]:+.2f}) " + f"-> смотрит на ({tgt[0]:+.2f}, {tgt[1]:+.2f}, {tgt[2]:+.2f})") +print("камеры стенда /RigRS не трогались") diff --git a/scripts/contact_and_decks.py b/scripts/contact_and_decks.py new file mode 100644 index 0000000..a36f740 --- /dev/null +++ b/scripts/contact_and_decks.py @@ -0,0 +1,56 @@ +"""1) Blade vs item contact height at the pusher. 2) Actual authored deck surfaceVelocity +magnitudes vs the commanded 1.0 m/s - the log showed items crawling 8-50s across ~1-2m +of deck (expected ~1-2s), which smells like the drive_belt() scale-correction being wrong +for these particular Cube prims (same class of bug as ConveyorTrack_04's documented +0.5-scale issue).""" +import omni.usd +from pxr import Usd, UsdGeom, PhysxSchema, Gf + +stage = omni.usd.get_context().get_stage() +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +print("=== pusher blade vs belt height ===") +blade = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom") +r = bbc.ComputeWorldBound(blade).ComputeAlignedRange() +print(f" blade z[{r.GetMin()[2]:+.3f}..{r.GetMax()[2]:+.3f}]") +belt = stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt") +r2 = bbc.ComputeWorldBound(belt).ComputeAlignedRange() +print(f" ConveyorTrack_03/Belt top z={r2.GetMax()[2]:+.3f}") + +print("\n=== item rest heights (bottom of bbox) for the 11 named items, unposed ===") +import sys +sys.path.insert(0, "/home/dasha/robozon-sorter") +from robozon_sorter import config as C +items_dir = C.ROOT / "assets" / "items" +for name in ["bottle","box_300x200x200","box_400x400x300","lunchbox","bag","detergent", + "pouf","pen","plate","cylinder","helmet"]: + tmp_stage = Usd.Stage.CreateInMemory() + prim = tmp_stage.DefinePrim("/probe", "Xform") + prim.GetReferences().AddReference(str(items_dir / f"{name}.usd")) + bb2 = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + rr = bb2.ComputeWorldBound(prim).ComputeAlignedRange() + if rr.IsEmpty(): + print(f" {name:18s} EMPTY BBOX"); continue + mn, mx = rr.GetMin(), rr.GetMax() + print(f" {name:18s} local z[{mn[2]:+.3f}..{mx[2]:+.3f}] height={mx[2]-mn[2]:.3f}") + +print("\n=== deck actual surfaceVelocity (authored) vs commanded speed=1.0 ===") +for path in ["/World/PlowTransition_B", "/World/PlowCornerDeck_B", + "/World/PlowTransition_C", "/World/PlowCornerDeck_C"]: + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + print(f" {path} MISSING"); continue + sv = prim.GetAttribute("physxSurfaceVelocity:surfaceVelocity") + v = sv.Get() if sv else None + mag = (v[0]**2+v[1]**2+v[2]**2)**0.5 if v else 0.0 + # world-space check: transform local surfaceVelocity to world using current xform + xc = UsdGeom.XformCache() + M = xc.GetLocalToWorldTransform(prim) + world_v = M.TransformDir(Gf.Vec3d(*v)) if v else Gf.Vec3d(0,0,0) + world_mag = world_v.GetLength() + scale_op = None + for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeScale: + scale_op = op.Get() + print(f" {path}") + print(f" local surfaceVelocity={v} |local|={mag:.3f} |world|={world_mag:.3f} scale={scale_op}") diff --git a/scripts/diag_belts.py b/scripts/diag_belts.py new file mode 100644 index 0000000..6b2733b --- /dev/null +++ b/scripts/diag_belts.py @@ -0,0 +1,71 @@ +"""Почему пробы не поехали: есть ли коллизия на лентах и куда падают тела. + +Ни одна проба не сдвинулась, две улетели. Это картина не буксования, а отсутствия опоры: +если у према Belt нет коллайдера, тело проваливается сквозь ленту и дальше поведение +случайно. Поэтому проверяется опора, а не сила трения. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema +from isaacsim.core.experimental.prims import RigidPrim + +stage = omni.usd.get_context().get_stage() +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +BELTS = ["/World/ConveyorTrack_05/Belt", "/World/ConveyorTrack/Belt", + "/World/ConveyorTrack_03/Belt", "/World/ConveyorTrack_04/Belt", + "/World/ConveyorTrack_01/Belt", "/World/ConveyorTrack_06/Belt", + "/World/ConveyorTrack_03/Belt_01"] + +print("ОПОРА ПОД ТОВАРОМ:\n") +for path in BELTS: + pr = stage.GetPrimAtPath(path) + if not pr.IsValid(): + print(f" {path}: НЕТ ПРЕМА"); continue + r = bb.ComputeWorldBound(pr).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + own = pr.HasAPI(UsdPhysics.CollisionAPI) + rb = pr.HasAPI(UsdPhysics.RigidBodyAPI) + kin = UsdPhysics.RigidBodyAPI(pr).GetKinematicEnabledAttr().Get() if rb else None + # коллайдеры среди потомков + kids, meshes = [], [] + for d in Usd.PrimRange(pr): + if d == pr: + continue + if d.HasAPI(UsdPhysics.CollisionAPI): + kids.append(d.GetName()) + if d.IsA(UsdGeom.Mesh): + meshes.append(d.GetName()) + print(f" {path}") + print(f" тип={pr.GetTypeName()} коллизия_на_себе={own} rigid={rb} кинематик={kin}") + print(f" габарит z {mn[2]:.3f}..{mx[2]:.3f} xy {mn[0]:.2f}..{mx[0]:.2f} / {mn[1]:.2f}..{mx[1]:.2f}") + print(f" потомков с коллизией: {len(kids)} {kids[:4]} мешей: {len(meshes)} {meshes[:4]}") + +# что вообще есть под точкой (-1.0, 0.0): все коллайдеры, чей габарит её накрывает +print("\nЧТО НАКРЫВАЕТ ТОЧКУ (-1.0, 0.0) сверху вниз:") +hits = [] +for p in stage.Traverse(): + if not p.HasAPI(UsdPhysics.CollisionAPI): + continue + r = bb.ComputeWorldBound(p).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + if mn[0] <= -1.0 <= mx[0] and mn[1] <= 0.0 <= mx[1]: + hits.append((mx[2], str(p.GetPath()), mn[2])) +for top, path, bot in sorted(hits, reverse=True)[:10]: + print(f" z {bot:6.3f}..{top:6.3f} {path}") +if not hits: + print(" НИЧЕГО - под товаром нет ни одного коллайдера") + +# где сейчас лежат пробы +probe = stage.GetPrimAtPath("/World/_BeltProbe") +if probe.IsValid(): + print("\nГДЕ ОКАЗАЛИСЬ ПРОБЫ:") + for c in probe.GetChildren(): + r = bb.ComputeWorldBound(c).ComputeAlignedRange() + m = r.GetMidpoint() + print(f" {c.GetName():10s} ({m[0]:+7.2f}, {m[1]:+7.2f}, {m[2]:+7.2f})") diff --git a/scripts/diag_drive.py b/scripts/diag_drive.py new file mode 100644 index 0000000..897764b --- /dev/null +++ b/scripts/diag_drive.py @@ -0,0 +1,79 @@ +"""Почему лента не тянет: сырой прогон одной дорожки с печатью каждого шага. + +Предыдущий вывод "стоит/упал" был ложным - я снимал положения ПОСЛЕ stop(), который +возвращает сцену в исходное состояние, поэтому все тела оказались в точках рождения +независимо от того, ехали они или нет. Здесь положение печатается ВО ВРЕМЯ прогона. + +Первый подозреваемый - surfaceVelocityEnabled: PhysxSurfaceVelocityAPI можно применить +и задать вектор, но без включённого флага он не действует, и внешне это неотличимо от +нехватки трения. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +BELT = "/World/ConveyorTrack/Belt" +PROBE = "/World/_BeltProbe/main_00" + +pr = stage.GetPrimAtPath(BELT) +print("=== состояние привода ленты ===") +for a in pr.GetAttributes(): + n = a.GetName() + if "urfaceVelocity" in n or "kinematic" in n.lower(): + print(f" {n} = {a.Get()}") +print(" применённые схемы:", [s for s in pr.GetAppliedSchemas()]) + +# включить флаг явно +api = PhysxSchema.PhysxSurfaceVelocityAPI.Apply(pr) +en = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled") +if not en or not en.IsValid(): + en = api.CreateSurfaceVelocityEnabledAttr() +en.Set(True) +print(" surfaceVelocityEnabled выставлен в True") + +# трение: без материала на ленте тянуть нечем +MAT = "/World/_TestGrip" +m = stage.GetPrimAtPath(MAT) +if not m.IsValid(): + m = stage.DefinePrim(MAT, "Material") +pm = UsdPhysics.MaterialAPI.Apply(m) +pm.CreateStaticFrictionAttr().Set(1.1) +pm.CreateDynamicFrictionAttr().Set(0.95) +pm.CreateRestitutionAttr().Set(0.0) +for target in (BELT, PROBE): + t = stage.GetPrimAtPath(target) + if t.IsValid(): + UsdShade.MaterialBindingAPI.Apply(t).Bind( + UsdShade.Material(m), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") +print(f" трение 1.1/0.95 привязано к ленте и к пробе") + +probe = stage.GetPrimAtPath(PROBE) +print(f" проба существует: {probe.IsValid()}") + +tl.play() +await app_utils.update_app_async(steps=20) +rp = RigidPrim(paths=[PROBE]) +print("\n=== ВО ВРЕМЯ прогона ===") +print(" шаг x y z dx за шаг") +prev = None +for i in range(12): + pos, _ = rp.get_world_poses() + p = pos.numpy()[0] + d = "-" if prev is None else f"{(p[0]-prev)*1000:+7.1f} мм" + print(f" {i*10:4d} {p[0]:+7.3f} {p[1]:+7.3f} {p[2]:+7.3f} {d}") + prev = p[0] + await app_utils.update_app_async(steps=10) +tl.stop() +await app_utils.update_app_async(steps=5) diff --git a/scripts/diag_junction.py b/scripts/diag_junction.py new file mode 100644 index 0000000..046779d --- /dev/null +++ b/scripts/diag_junction.py @@ -0,0 +1,72 @@ +"""Why goods stop at x=-6.0: place one item either side of the belt junction and watch. + +Run A parks an item at x=-5.5, upstream of the transfer, and lets it drive at it. +Run B starts one already at x=-6.3, past the transfer, on the wide belt through the plow. + +If A stalls and B runs, the transfer is blocked by structure, not by a dead belt. The +suspect is the downstream end frame of ConveyorTrack_03: `open_junction()` clears the shell +collider on the plow track and both lanes, but not on the track goods arrive *on*, so its +end plate stands across the path at exactly x=-6.0. +""" +import sys, time +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +# The live Isaac process keeps every module it has ever imported, so an edited +# robozon_sorter/ on disk is invisible to a second run in the same session. Drop the +# package from sys.modules first or you spend the evening re-testing the old code. +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() # a *new* module file is invisible until the finder is reset + +import omni.timeline, isaacsim.core.experimental.utils.app as app_utils +from pxr import UsdPhysics +from robozon_sorter import config as C +from robozon_sorter.sim import plow_sort, plow_vision +from robozon_sorter.sim.mechanics import Cell + +OPEN_03 = bool(globals().get("open_03", False)) # also clear ConveyorTrack_03's shell +START_X = float(globals().get("start_x", -5.5)) +ITEM = globals().get("item", "barrel") +SECONDS = float(globals().get("seconds", 6.0)) + +stage, info = plow_vision.load(belt_speed=1.0, script_control=True) +plow_sort.keep_lanes_active(stage) +plow_sort.configure_lanes(stage, 1.0) +opened = plow_sort.open_junction(stage) +extra = None +if OPEN_03: + p = stage.GetPrimAtPath("/World/ConveyorTrack_03/SM_ConveyorBelt_A24_02") + a = p.GetAttribute("physics:collisionEnabled") or \ + UsdPhysics.CollisionAPI.Apply(p).CreateCollisionEnabledAttr() + a.Set(False) + extra = str(p.GetPath()) +print(f"opened {len(opened)} shells, extra={extra}") + +items = {k: v["zone"] for k, v in info["items"].items()} +await app_utils.update_app_async(steps=30) +cell = Cell(stage, items.keys()) +cell.park_all() +await app_utils.update_app_async(steps=10) + +cell.place(ITEM, (START_X, 0.0, C.BELT_Z + 0.10)) +app_utils.play(commit=True) +await app_utils.update_app_async(steps=20) + +print(f"\n{ITEM} from x={START_X} (plow at x={C.PLOW_POS[0]})") +t0, last = time.time(), None +while time.time() - t0 < SECONDS: + await app_utils.update_app_async(steps=12) + p = cell.pose(ITEM) + x, y, z = (float(v) for v in p[:3]) + moved = "" if last is None else f" dx={x - last:+.3f}" + print(f" t={time.time() - t0:4.1f} x={x:+.3f} y={y:+.3f} z={z:+.3f}{moved}") + last = x + if z < C.BELT_Z - 0.5: + print(" -> fell off"); break + +app_utils.stop() +await app_utils.update_app_async(steps=10) +print(f"verdict: {'REACHED PLOW' if last is not None and last < -6.6 else 'STALLED at x=%.2f' % (last or 0)}") diff --git a/scripts/diag_stall.py b/scripts/diag_stall.py new file mode 100644 index 0000000..f26522d --- /dev/null +++ b/scripts/diag_stall.py @@ -0,0 +1,89 @@ +"""Что держит товар на x = -3.15: список коллайдеров в этой полосе + трасса остановки. + +Пушер проверить не вышло - товар не доехал до точки срабатывания (PUSH_X = -3.9) и встал +раньше. Прежде чем править пушер, надо понять, упирается товар в препятствие или теряет +привод: это разные неисправности, и внешне они неотличимы. +""" +import sys, math +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +print("КОЛЛАЙДЕРЫ, накрывающие полосу x -3.4..-3.0 на высоте ленты (z 1.75..2.10):") +for p in stage.Traverse(): + if not p.HasAPI(UsdPhysics.CollisionAPI): + continue + en = p.GetAttribute("physics:collisionEnabled") + if en and en.Get() is False: + continue + r = bb.ComputeWorldBound(p).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + if mx[0] < -3.4 or mn[0] > -3.0: + continue + if mx[2] < 1.75 or mn[2] > 2.10: + continue + if abs(mn[1]) > 0.6 and abs(mx[1]) > 0.6 and mn[1] * mx[1] > 0: + continue + print(f" x {mn[0]:+7.3f}..{mx[0]:+7.3f} y {mn[1]:+6.2f}..{mx[1]:+6.2f} " + f"z {mn[2]:+6.3f}..{mx[2]:+6.3f} {p.GetPath()}") + +# какая лента под этой точкой и что у неё со скоростью +print("\nЛЕНТЫ ПОД x=-3.15:") +for path in list(plow_cell.BELTS) + [plow_cell.BRANCH]: + pr = stage.GetPrimAtPath(path) + if not pr.IsValid(): + continue + r = bb.ComputeWorldBound(pr).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + if mn[0] <= -3.15 <= mx[0]: + v = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocity").Get() + e = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled") + print(f" {path}: v={v} включено={e.Get() if e else None} " + f"y {mn[1]:+.2f}..{mx[1]:+.2f}") + +# трасса: пустить товар и печатать x, пока не встанет +TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt") + ).ComputeAlignedRange().GetMax()[2] +if stage.GetPrimAtPath("/World/_Stall").IsValid(): + stage.RemovePrim("/World/_Stall") +stage.DefinePrim("/World/_Stall", "Xform") +c = UsdGeom.Cube.Define(stage, "/World/_Stall/item"); c.CreateSizeAttr().Set(2.0) +xf = UsdGeom.Xformable(c.GetPrim()) +xf.AddTranslateOp().Set(Gf.Vec3d(-2.40, 0.0, TOP + 0.055)) +xf.AddScaleOp().Set(Gf.Vec3f(0.05, 0.05, 0.05)) +p = c.GetPrim() +UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p) +UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(0.5) +PhysxSchema.PhysxRigidBodyAPI.Apply(p).CreateSolverPositionIterationCountAttr().Set(32) +g = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL) +if g.IsValid(): + UsdShade.MaterialBindingAPI.Apply(p).Bind( + UsdShade.Material(g), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + +tl.play(); await app_utils.update_app_async(steps=25) +rp = RigidPrim(paths=["/World/_Stall/item"]) +print("\nТРАССА (старт x=-2.40):") +prev, t0 = None, float(tl.get_current_time()) +for i in range(28): + q = rp.get_world_poses()[0].numpy()[0] + t = float(tl.get_current_time()) + d = "-" if prev is None else f"{(float(q[0])-prev)*1000:+7.1f} мм" + print(f" t={t-t0:5.2f}s x={float(q[0]):+7.3f} y={float(q[1]):+6.3f} " + f"z={float(q[2]):+6.3f} {d}") + prev = float(q[0]) + await app_utils.update_app_async(steps=6) +tl.stop(); await app_utils.update_app_async(steps=5) diff --git a/scripts/diag_zero.py b/scripts/diag_zero.py new file mode 100644 index 0000000..892321b --- /dev/null +++ b/scripts/diag_zero.py @@ -0,0 +1,54 @@ +"""Кто обнуляет surfaceVelocity: значение читается до play и НЕСКОЛЬКО РАЗ во время. + +drive_belt вернул (-1.0, 0, 0), а в атрибуте лежит (-0, 0, 0). Два разных объяснения: +запись не дошла до према, либо её перетирают во время прогона. Отличить их можно только +чтением атрибута в обоих состояниях - что и делается здесь. + +Подозреваемый - авторские узлы ConveyorBeltGraph: по документации сборки собственной +скорости они не несут и на каждом тике пишут свою (нулевую), забивая явную установку. +Их деактивация в предыдущем прогоне могла не подействовать на уже созданный граф. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, PhysxSchema +from isaacsim.core.experimental.prims import RigidPrim + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +BELT = "/World/ConveyorTrack/Belt" +PROBE = "/World/_BeltProbe/main_00" +pr = stage.GetPrimAtPath(BELT) +attr = pr.GetAttribute("physxSurfaceVelocity:surfaceVelocity") + +# состояние графов конвейера +graphs = [p for p in stage.Traverse() if "ConveyorBeltGraph" in p.GetName()] +print("графы ConveyorBeltGraph:") +for g in graphs: + print(f" {g.GetPath()} активен={g.IsActive()}") + +attr.Set(Gf.Vec3f(-1.0, 0.0, 0.0)) +print(f"\nзаписал -1.0 -> читается ДО play: {attr.Get()}") + +tl.play() +await app_utils.update_app_async(steps=5) +print(f"после play, 5 шагов: {attr.Get()}") +await app_utils.update_app_async(steps=20) +print(f"после play, 25 шагов: {attr.Get()}") + +rp = RigidPrim(paths=[PROBE]) +x0 = rp.get_world_poses()[0].numpy()[0][0] +for i in range(6): + await app_utils.update_app_async(steps=20) + x = rp.get_world_poses()[0].numpy()[0][0] + print(f" шаг {25+(i+1)*20:4d}: v={attr.Get()} проба x={x:+.3f} прошла {(x-x0)*1000:+7.1f} мм") +tl.stop() +await app_utils.update_app_async(steps=5) +print(f"\nпосле stop: {attr.Get()}") diff --git a/scripts/env_scan.py b/scripts/env_scan.py new file mode 100644 index 0000000..dea2e88 --- /dev/null +++ b/scripts/env_scan.py @@ -0,0 +1,43 @@ +"""Check ConveyorTrack_05 geometry, existing floor/ground, and lighting in the fresh stage.""" +import omni.usd +from pxr import Usd, UsdGeom, UsdLux + +stage = omni.usd.get_context().get_stage() +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +xc = UsdGeom.XformCache() + +def report(path): + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + print(f"{path} MISSING"); return + r = bbc.ComputeWorldBound(prim).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + M = xc.GetLocalToWorldTransform(prim) + lx = M.TransformDir((1,0,0)); lx = lx/(lx.GetLength() or 1) + print(f"{path}") + print(f" bbox x[{mn[0]:+.3f}..{mx[0]:+.3f}] y[{mn[1]:+.3f}..{mx[1]:+.3f}] z[{mn[2]:+.3f}..{mx[2]:+.3f}]") + print(f" local+X in world = ({lx[0]:+.3f},{lx[1]:+.3f},{lx[2]:+.3f})") + +for p in ["/World/ConveyorTrack_05", "/World/ConveyorTrack_05/Belt"]: + report(p) + +print("\n=== whole /World bbox ===") +w = stage.GetPrimAtPath("/World") +r = bbc.ComputeWorldBound(w).ComputeAlignedRange() +print(f" x[{r.GetMin()[0]:+.2f}..{r.GetMax()[0]:+.2f}] y[{r.GetMin()[1]:+.2f}..{r.GetMax()[1]:+.2f}] z[{r.GetMin()[2]:+.2f}..{r.GetMax()[2]:+.2f}]") + +print("\n=== lights ===") +n = 0 +for p in stage.Traverse(): + if p.IsA(UsdLux.BoundableLightBase) or p.IsA(UsdLux.NonboundableLightBase): + n += 1 + inten = p.GetAttribute("inputs:intensity") + vis = UsdGeom.Imageable(p).ComputeVisibility() + print(f" {p.GetPath()} type={p.GetTypeName()} intensity={inten.Get() if inten else '?'} vis={vis}") +print(f" total lights: {n}") + +print("\n=== any ground/floor plane already present? ===") +for p in stage.Traverse(): + name = p.GetName().lower() + if "floor" in name or "ground" in name or "plane" in name: + print(" candidate:", p.GetPath(), p.GetTypeName()) diff --git a/scripts/exc_test.py b/scripts/exc_test.py new file mode 100644 index 0000000..f542ee3 --- /dev/null +++ b/scripts/exc_test.py @@ -0,0 +1,18 @@ +"""Minimal repro: can Python catch the Tf threading-violation error at all?""" +import omni.usd +from pxr import UsdPhysics +import isaacsim.core.experimental.utils.app as app_utils + +omni.usd.get_context().open_stage("/home/dasha/robozon-sorter/scene/plow_cell_90_45_test.usd") +await app_utils.update_app_async(steps=30) +stage = omni.usd.get_context().get_stage() + +print("attempting Scene.Define with bare except:") +try: + scene = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim() + print("SUCCESS, no exception") +except: + import traceback + print("CAUGHT something:") + traceback.print_exc() +print("after try/except block, script continues") diff --git a/scripts/exc_test2.py b/scripts/exc_test2.py new file mode 100644 index 0000000..a9c81aa --- /dev/null +++ b/scripts/exc_test2.py @@ -0,0 +1,27 @@ +"""Does the threading violation clear if we just retry Scene.Define a few times? +Never let an exception escape this script so stdout survives regardless of outcome.""" +import asyncio +import omni.usd +from pxr import UsdPhysics +import isaacsim.core.experimental.utils.app as app_utils + +omni.usd.get_context().open_stage("/home/dasha/robozon-sorter/scene/plow_cell_90_45_test.usd") +print("opened, waiting...") +await app_utils.update_app_async(steps=120) +await asyncio.sleep(3.0) +await app_utils.update_app_async(steps=60) +stage = omni.usd.get_context().get_stage() +print("stage settled, prim count:", len(list(stage.Traverse()))) + +for attempt in range(8): + try: + scene = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim() + print(f"attempt {attempt}: SUCCESS, prim valid={scene.IsValid()}") + break + except BaseException as exc: + print(f"attempt {attempt}: FAILED, type={type(exc).__name__}") + await app_utils.update_app_async(steps=60) + await asyncio.sleep(1.0) +else: + print("all attempts failed") +print("done") diff --git a/scripts/export_item_library.py b/scripts/export_item_library.py new file mode 100644 index 0000000..eb583f7 --- /dev/null +++ b/scripts/export_item_library.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +"""Flatten the classified objects out of the working scene into assets/items/. + + /home/whatevenif/isaacsim/python.sh scripts/export_item_library.py + +The six meshes bundled in `assets/meshes/` are a smoke-test set, two per class. A sorting +run wants the whole catalogue, and the classified objects live inside +`robozon_conveyor_scaled.usd` under `/World/CVObjects`, referencing `.glb` files that are +not in this repo. Each one is therefore *flattened* on export so the result carries its own +geometry and composes anywhere - the recipe in CLAUDE.md, applied in bulk. + +Ground truth comes from `categories.json`: `zone` (B/C/D) and `obb_extents_m`, which the +run harness reports predicted dimensions against. Objects larger than the 500 mm incoming +envelope are skipped - they could never reach this conveyor. +""" +from __future__ import annotations + +import json +import sys +from pathlib import Path + +from pxr import Usd, UsdGeom + +ASSETS = Path("/home/dasha/isaac_assets") +SCENE = ASSETS / "robozon_conveyor_scaled.usd" +CATS = ASSETS / "categories.json" +OUT = Path(__file__).resolve().parent.parent / "assets" / "items" +ENVELOPE_MM = 500.0 + + +def main(): + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + cats = json.loads(CATS.read_text()) + OUT.mkdir(parents=True, exist_ok=True) + + src = Usd.Stage.Open(str(SCENE)) + root = src.GetPrimAtPath("/World/CVObjects") + if not root.IsValid(): + sys.exit("/World/CVObjects missing") + present = {p.GetName() for p in root.GetChildren()} + + manifest, skipped = {}, [] + for name, meta in sorted(cats.items()): + if name not in present: + skipped.append((name, "not in scene")) + continue + ext_mm = [e * 1000.0 for e in meta["obb_extents_m"]] + if max(ext_mm) > ENVELOPE_MM: + skipped.append((name, f"oversize {max(ext_mm):.0f} mm")) + continue + + ns = Usd.Stage.CreateInMemory() + item = UsdGeom.Xform.Define(ns, "/Item") + item.GetPrim().GetReferences().AddReference(str(SCENE), f"/World/CVObjects/{name}") + ns.SetDefaultPrim(item.GetPrim()) + dst = OUT / f"{name}.usd" + ns.Flatten().Export(str(dst)) + + manifest[name] = dict(zone=meta["zone"], + gt_dims_mm=[round(v) for v in sorted(ext_mm, reverse=True)], + k_round=round(meta.get("k_round", 0.0), 3), + label_ru=meta.get("label_ru", "")) + print(f" {name:22s} {meta['zone']} {manifest[name]['gt_dims_mm']} " + f"{dst.stat().st_size / 1024:.0f} KB") + + (OUT / "manifest.json").write_text(json.dumps(manifest, indent=2, ensure_ascii=False)) + from collections import Counter + print(f"\nexported {len(manifest)} -> {OUT} " + f"{dict(Counter(v['zone'] for v in manifest.values()))}") + if skipped: + print(f"skipped {len(skipped)}: " + ", ".join(f"{n} ({w})" for n, w in skipped[:8])) + + +if __name__ == "__main__": + main() diff --git a/scripts/extend_transition_decks.py b/scripts/extend_transition_decks.py new file mode 100644 index 0000000..95a723f --- /dev/null +++ b/scripts/extend_transition_decks.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +"""Carry the driven surface inboard, to where the plow can actually deliver. + + /home/whatevenif/isaacsim/python.sh scripts/extend_transition_decks.py + +Measured with one item and a full 42 deg sweep: the blade imparts a real push (item picks up +0.59 m/s and travels 139 mm sideways) but leaves it at **y = 0.261**, and every driven +surface past the plow starts at **|y| = 0.380**: + + belt ConveyorTrack_04 x -7.00..-6.00 y -0.450..+0.450 + deck PlowTransition_C x -7.00..-6.00 y +0.380..+0.698 + lane C x -8.12..-6.39 y +0.380..+2.112 + lane B x -7.03..-6.58 y -2.379..-0.380 (no transition deck at all) + +So there is a 119 mm band where a swept item sits on the very lip of the main belt with +nothing driving it toward its lane. That is the gap the goods die in - not a hole they fall +through, a strip with no traction, right where the blade lets go of them. + +This closes it from the inside: each transition plate is brought in to |y| = 0.20, well +short of where the blade releases, and B gets the plate it never had. `plow_sort.DECK_DIR` +already drives both toward their lanes, so an item landing here is carried on instead of +stopping. + +Plates are static Cubes with collision, coplanar with the belt at z 1.7805, and are made +kinematic + surface-driven at load time like every other deck. +""" +from __future__ import annotations + +import sys +from pathlib import Path + +from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics + +SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd" +TOP_Z = 1.7805 +THICK = 0.01 +INBOARD = 0.20 # how far in the driven surface now reaches + +# Lane B is perpendicular, so a straight strip meets it flush. +PLATES_STRAIGHT = {"PlowTransition_B": (-7.03, -6.45, -0.380)} + +# Lane C is laid at 45 deg. Its bounding box, x -8.123..-6.391 by y 0.380..2.112, is the +# AABB of a rotated rectangle and describes a footprint the belt does not have: the near +# side is a single CORNER at (-7.257, 0.380), and the edge runs away from it at 45 deg, +# +# y = x + 7.637 +# +# so at x -7.03 the lane really starts at y 0.607, and at x -6.39 at y 1.247 - not at 0.380 +# anywhere except that one corner. A straight plate ending at y 0.380 therefore leaves a +# widening wedge of open air, which is the dark triangle in the viewport and where +# `bolts_cluster` fell through after the blade had successfully pushed it to y +0.425. +# +# The C plate is a trapezoid instead: inboard edge at |y| = INBOARD, outer edge ON the +# lane's diagonal. +LANE_C_EDGE = lambda x: x + 7.637 + + +def _plate(stage, name, x0, x1, y_in, y_out): + path = f"/World/{name}" + prim = stage.GetPrimAtPath(path) + if prim.IsValid(): + stage.RemovePrim(path) + cube = UsdGeom.Cube.Define(stage, path) + cube.CreateSizeAttr().Set(2.0) # size 2 so the scale op IS the half-extent + cx, cy = (x0 + x1) / 2.0, (y_in + y_out) / 2.0 + hx, hy = abs(x1 - x0) / 2.0, abs(y_out - y_in) / 2.0 + xf = UsdGeom.Xformable(cube.GetPrim()) + xf.ClearXformOpOrder() + xf.AddTranslateOp().Set(Gf.Vec3d(cx, cy, TOP_Z - THICK)) + xf.AddScaleOp().Set(Gf.Vec3f(hx, hy, THICK)) + cube.CreateDisplayColorAttr().Set([Gf.Vec3f(0.30, 0.31, 0.33)]) + UsdPhysics.CollisionAPI.Apply(cube.GetPrim()) + return path, (round(cx - hx, 3), round(cx + hx, 3), round(cy - hy, 3), round(cy + hy, 3)) + + +def _trapezoid(stage, name, quad): + """a thin prism whose top face is the given 4 corners, coplanar with the belt. + + A Cube cannot do this - the plate has to follow a 45 deg edge, so it is authored as an + explicit mesh. Given thickness rather than left as a zero-height quad: a flat sheet is a + poor collider and goods catch on its rim. + """ + path = f"/World/{name}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + mesh = UsdGeom.Mesh.Define(stage, path) + top = [Gf.Vec3f(x, y, TOP_Z) for x, y in quad] + bot = [Gf.Vec3f(x, y, TOP_Z - THICK) for x, y in quad] + pts = top + bot + mesh.CreatePointsAttr().Set(pts) + faces, counts = [], [] + faces += [0, 1, 2, 3]; counts.append(4) # top + faces += [7, 6, 5, 4]; counts.append(4) # bottom + for i in range(4): # sides + j = (i + 1) % 4 + faces += [i, 4 + i, 4 + j, j]; counts.append(4) + mesh.CreateFaceVertexIndicesAttr().Set(faces) + mesh.CreateFaceVertexCountsAttr().Set(counts) + xs = [p[0] for p in pts]; ys = [p[1] for p in pts]; zs = [p[2] for p in pts] + mesh.CreateExtentAttr().Set([Gf.Vec3f(min(xs), min(ys), min(zs)), + Gf.Vec3f(max(xs), max(ys), max(zs))]) + mesh.CreateDisplayColorAttr().Set([Gf.Vec3f(0.30, 0.31, 0.33)]) + mesh.CreateSubdivisionSchemeAttr().Set("none") + UsdPhysics.CollisionAPI.Apply(mesh.GetPrim()) + UsdPhysics.MeshCollisionAPI.Apply(mesh.GetPrim()).CreateApproximationAttr().Set("convexHull") + return path + + +def main(): + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + stage = Usd.Stage.Open(str(SCENE)) + for name, (x0, x1, y_out) in PLATES_STRAIGHT.items(): + y_in = INBOARD if y_out > 0 else -INBOARD + path, span = _plate(stage, name, x0, x1, y_in, y_out) + print(f" {name:20s} straight x[{span[0]:+.3f},{span[1]:+.3f}] " + f"y[{span[2]:+.3f},{span[3]:+.3f}]") + + x0, x1 = -7.03, -6.39 + quad = [(x0, INBOARD), (x1, INBOARD), (x1, LANE_C_EDGE(x1)), (x0, LANE_C_EDGE(x0))] + _trapezoid(stage, "PlowTransition_C", quad) + print(f" PlowTransition_C trapezoid corners " + + " ".join(f"({a:+.2f},{b:+.2f})" for a, b in quad)) + print(f" outer edge follows the lane diagonal y = x + 7.637 " + f"({LANE_C_EDGE(x0):+.3f} at x={x0}, {LANE_C_EDGE(x1):+.3f} at x={x1})") + stage.GetRootLayer().Save() + print(f"saved {SCENE}") + print(f"driven surface now reaches |y| = {INBOARD}; the blade releases goods at ~0.26") + + +if __name__ == "__main__": + main() diff --git a/scripts/fetch_assets.py b/scripts/fetch_assets.py new file mode 100755 index 0000000..4d8242b --- /dev/null +++ b/scripts/fetch_assets.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +"""Fetch the conveyor art that scene/sorter.usd references. + + python scripts/fetch_assets.py + +~270 MB, too big for git. Downloads in resumable Range chunks: the Omniverse bucket is +slow enough that a plain GET truncates, and a half-written USD fails to compose with a +confusing "could not open asset" rather than an obvious size error. +""" +from __future__ import annotations +import os, sys, time, urllib.request +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent +DEST = ROOT / "assets" / "conveyors" +BASE = ("https://omniverse-content-production.s3-us-west-2.amazonaws.com/" + "Assets/Isaac/6.0/Isaac/Props/Conveyors/") +CHUNK, TRIES = 1 << 21, 12 + +# the scene composes these; the Textures/ and Material Library/ folders come with them +FILES = ["ConveyorBelt_A06.usd", "ConveyorBelt_A24.usd"] +TEXTURES = [ + "Textures/M_ConveyorBelt_A01_Belt.usd", "Textures/M_ConveyorBelt_A01_Decal.usd", + "Textures/MetalPainted_Blue_Glossy_A.usd", "Textures/Plastic_Orange_A.usd", + "Textures/Plastic_Rough_Black_A.usd", "Textures/Steel_A.usd", + "Textures/T_ConveyorBelt_A01_Belt_Albedo.png", "Textures/T_ConveyorBelt_A01_Belt_Normal.png", + "Textures/T_ConveyorBelt_A01_Belt_ORM.png", + "Textures/T_ConveyorsBelt_A01_Decal_Albedo.png", "Textures/T_ConveyorsBelt_A01_Decal_Alpha.png", + "Textures/T_ConveyorsBelt_A01_Decal_ORM.png", + "Material%20Library/physics_material.usd", +] + +def size_of(url): + try: + req = urllib.request.Request(url, method="HEAD") + with urllib.request.urlopen(req, timeout=60) as r: + return int(r.headers.get("Content-Length", 0)) + except Exception: + return 0 + +def grab(url, out: Path): + total = size_of(url) + have = out.stat().st_size if out.exists() else 0 + if total and have == total: + print(f" {out.name}: present"); return True + out.parent.mkdir(parents=True, exist_ok=True) + if not total: + urllib.request.urlretrieve(url, out); return out.exists() + pos = have if have < total else 0 + with open(out, "r+b" if pos else "wb") as fh: + fh.seek(pos); t0 = time.time() + while pos < total: + end = min(pos + CHUNK - 1, total - 1) + for a in range(TRIES): + try: + req = urllib.request.Request(url) + req.add_header("Range", f"bytes={pos}-{end}") + with urllib.request.urlopen(req, timeout=120) as r: + data = r.read() + if len(data) != end - pos + 1: + raise IOError("short chunk") + fh.write(data); fh.flush(); pos += len(data) + print(f"\r {out.name}: {100.0*pos/total:5.1f}%" + f" {(pos-have)/max(time.time()-t0,1e-6)/1024:6.0f} KB/s", end="", flush=True) + break + except Exception as exc: + if a == TRIES - 1: + print(f"\n {out.name}: FAILED at {pos}: {exc}"); return False + time.sleep(min(2 ** a, 20)) + print() + return out.stat().st_size == total + +def main(): + print(f"fetching conveyor art into {DEST}") + ok = True + for rel in FILES + TEXTURES: + local = DEST / rel.replace("%20", " ") + ok &= grab(BASE + rel, local) + print("\nall present" if ok else "\nincomplete - re-run, downloads resume") + return 0 if ok else 1 + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scripts/fetch_models.py b/scripts/fetch_models.py new file mode 100644 index 0000000..44b6cfd --- /dev/null +++ b/scripts/fetch_models.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +"""Fetch the model weights the pipeline needs. They are far too big for git. + + python scripts/fetch_models.py + +Downloads in small Range chunks with per-chunk retries: the CRE weights come off a slow +mirror and a plain single-stream GET truncates silently, leaving an unloadable file. +""" +from __future__ import annotations + +import os +import sys +import time +import urllib.request +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent +MODELS = ROOT / "assets" / "models" +CHUNK = 1 << 21 +TRIES = 12 + +SOURCES = { + "FastSAM-s.pt": + "https://huggingface.co/Ultralytics/FastSAM/resolve/main/FastSAM-s.pt?download=true", + # CRE-Stereo ETH3D weights, from the megvii research release mirror + "crestereo_eth3d.pth": + "https://github.com/ibaiGorordo/CREStereo-Pytorch/releases/download/0.0.1/crestereo_eth3d.pth", +} + +CRE_REPO = "https://github.com/ibaiGorordo/CREStereo-Pytorch" + + +def content_length(url): + req = urllib.request.Request(url, method="HEAD") + with urllib.request.urlopen(req, timeout=60) as r: + if r.status in (301, 302) and r.headers.get("Location"): + return content_length(r.headers["Location"]) + return int(r.headers.get("Content-Length", 0)) + + +def get_range(url, a, b): + req = urllib.request.Request(url) + req.add_header("Range", f"bytes={a}-{b}") + with urllib.request.urlopen(req, timeout=120) as r: + return r.read() + + +def fetch(name, url, out_dir): + out = out_dir / name + total = content_length(url) + have = out.stat().st_size if out.exists() else 0 + if total and have == total: + print(f" {name}: already complete ({total/1e6:.1f} MB)") + return True + if not total: # server refuses HEAD: plain download + print(f" {name}: streaming (no content-length)") + urllib.request.urlretrieve(url, out) + return out.exists() + if have > total: + have = 0 + mode = "r+b" if have else "wb" + pos = have + with open(out, mode) as fh: + fh.seek(pos) + t0 = time.time() + while pos < total: + end = min(pos + CHUNK - 1, total - 1) + for attempt in range(TRIES): + try: + data = get_range(url, pos, end) + if len(data) != end - pos + 1: + raise IOError("short chunk") + fh.write(data) + fh.flush() + pos += len(data) + pct = 100.0 * pos / total + rate = (pos - have) / max(time.time() - t0, 1e-6) / 1024 + print(f"\r {name}: {pct:5.1f}% {rate:6.0f} KB/s", end="", flush=True) + break + except Exception as exc: + if attempt == TRIES - 1: + print(f"\n {name}: FAILED at byte {pos}: {exc}") + return False + time.sleep(min(2 ** attempt, 20)) + print() + ok = out.stat().st_size == total + print(f" {name}: {'ok' if ok else 'SIZE MISMATCH'} ({out.stat().st_size/1e6:.1f} MB)") + return ok + + +def main(): + MODELS.mkdir(parents=True, exist_ok=True) + print(f"fetching model weights into {MODELS}") + results = {n: fetch(n, u, MODELS) for n, u in SOURCES.items()} + + cre_dir = MODELS / "crestereo" + if not (cre_dir / "nets").exists(): + print(f"\nCRE-Stereo network code is not vendored here. Clone it next to the weights:") + print(f" git clone {CRE_REPO} {cre_dir}") + print(" (only the `nets` package is imported)") + + missing = [n for n, ok in results.items() if not ok] + if missing: + print(f"\nincomplete: {missing} - re-run, downloads resume where they stopped") + return 1 + print("\nall weights present") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scripts/fix_plow_reach_and_trays.py b/scripts/fix_plow_reach_and_trays.py new file mode 100644 index 0000000..0744334 --- /dev/null +++ b/scripts/fix_plow_reach_and_trays.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 +"""Close the two geometry faults that stop goods reaching a tray. + + /home/whatevenif/isaacsim/python.sh scripts/fix_plow_reach_and_trays.py + +**1. Tray B is walled shut on the side goods arrive from.** Measured: + + B_W0 (near, y -2.55) z 1.16 .. 2.00 <- 220 mm ABOVE the lane belt (1.7805) + B_W1 (far, y -3.35) z 1.16 .. 1.70 + tray C, correctly built: + C_W1 (near, y +1.88) z 1.16 .. 1.70 <- 80 mm BELOW the belt, goods slide over + C_W0 (far, y +2.68) z 1.16 .. 2.00 + + B has its tall backboard on the near face instead of the far one, so the lane runs + goods straight into a wall. The two heights are swapped to match C. + +**2. The plow cannot reach the lane.** The arm is 600 mm on a hinge at the belt centre, so + its tip reaches ``0.6 * sin(limit)``. At the authored +-35 deg that is 344 mm, while + the lanes start at |y| = 450 mm: a 106 mm dead zone no command can cross. Goods are + nudged to about y 0.15 and left on the line, which is exactly what every trace shows. + + Raising the joint limit to +-45 deg gives 424 mm of tip travel. That is still short of + 450 mm *at the centre of the item*, but an item is not a point: a 150 mm-wide box is + carried by the lane once its near edge crosses, i.e. at a centre of about 375 mm, so + 42 deg (402 mm) delivers it with margin. Moving the lanes inboard instead was rejected - + they would overlap the main belt, and two coincident belt colliders at the same height + is its own failure. + +Both edits are written back into scene/plow_cell.usd. +""" +from __future__ import annotations + +import sys +from pathlib import Path + +from pxr import Usd, UsdGeom, UsdPhysics + +SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd" +HINGE = "/World/Diverters/DiverterEnd/ArmHinge" +TRAYS = "/World/PlowContainers" + +NEW_LIMIT = 45.0 # joint hard limit, degrees either side +NEAR_WALL_TOP = 1.70 # must sit below the lane belt at 1.7805 +FAR_WALL_TOP = 2.00 + + +def _range(stage, path): + return UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound( + stage.GetPrimAtPath(path)).ComputeAlignedRange() + + +def _set_top(stage, path, top): + """scale a wall Cube about its base so its top lands at `top`""" + prim = stage.GetPrimAtPath(path) + r = _range(stage, path) + z0, z1 = r.GetMin()[2], r.GetMax()[2] + if abs(z1 - top) < 1e-4: + return None + want = max(top - z0, 0.02) + xf = UsdGeom.Xformable(prim) + for op in xf.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeScale: + s = op.Get() + op.Set(type(s)(s[0], s[1], s[2] * (want / max(z1 - z0, 1e-6)))) + break + else: + return None + # keep the base where it was: scaling a centred cube moves it by half the delta + for op in xf.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + t = op.Get() + op.Set(type(t)(t[0], t[1], t[2] + (want - (z1 - z0)) / 2.0)) + break + return (round(z1, 3), round(_range(stage, path).GetMax()[2], 3)) + + +def main(): + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + stage = Usd.Stage.Open(str(SCENE)) + + print("tray B - swap the tall backboard to the far face") + for wall, top, tag in ((f"{TRAYS}/B_W0", NEAR_WALL_TOP, "near (goods arrive)"), + (f"{TRAYS}/B_W1", FAR_WALL_TOP, "far (backboard)")): + if not stage.GetPrimAtPath(wall).IsValid(): + print(f" {wall} missing"); continue + changed = _set_top(stage, wall, top) + print(f" {wall.rsplit('/', 1)[1]:6s} {tag:22s} " + + (f"top {changed[0]} -> {changed[1]}" if changed else "already correct")) + + print(f"plow hinge - raise the limit so the arm can reach the lane") + hinge = stage.GetPrimAtPath(HINGE) + if hinge.IsValid(): + j = UsdPhysics.RevoluteJoint(hinge) + lo, hi = j.GetLowerLimitAttr().Get(), j.GetUpperLimitAttr().Get() + j.GetLowerLimitAttr().Set(-NEW_LIMIT) + j.GetUpperLimitAttr().Set(NEW_LIMIT) + import math + print(f" limits {lo:+.0f}/{hi:+.0f} -> {-NEW_LIMIT:+.0f}/{NEW_LIMIT:+.0f} " + f"(tip reach {0.6 * math.sin(math.radians(abs(hi))):.3f} -> " + f"{0.6 * math.sin(math.radians(NEW_LIMIT)):.3f} m, lane edge at 0.450)") + else: + print(f" {HINGE} missing") + + stage.GetRootLayer().Save() + print(f"saved {SCENE}") + + +if __name__ == "__main__": + main() diff --git a/scripts/geom_scan.py b/scripts/geom_scan.py new file mode 100644 index 0000000..cc66193 --- /dev/null +++ b/scripts/geom_scan.py @@ -0,0 +1,47 @@ +"""Scan the new 90/45 scene: exact belt geometry, plow decks, and containers.""" +import omni.usd +from pxr import Usd, UsdGeom, UsdPhysics + +stage = omni.usd.get_context().get_stage() +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +xc = UsdGeom.XformCache() + +def report(path): + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + print(f"{path} MISSING") + return + r = bbc.ComputeWorldBound(prim).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + M = xc.GetLocalToWorldTransform(prim) + localx = M.TransformDir((1,0,0)) + localx = localx / (localx.GetLength() or 1) + active = prim.IsActive() + print(f"{path} active={active}") + print(f" bbox x[{mn[0]:+.3f}..{mx[0]:+.3f}] y[{mn[1]:+.3f}..{mx[1]:+.3f}] z[{mn[2]:+.3f}..{mx[2]:+.3f}]") + print(f" local+X in world = ({localx[0]:+.3f},{localx[1]:+.3f},{localx[2]:+.3f})") + kids = [c.GetName() for c in prim.GetChildren()] + print(f" children: {kids}") + +print("=== root-level /ConveyorTrack_01 duplicate check ===") +p = stage.GetPrimAtPath("/ConveyorTrack_01") +print("exists:", p.IsValid()) + +for path in ["/World/ConveyorTrack_04", "/World/ConveyorTrack_04/Belt", + "/World/ConveyorTrack_06", "/World/ConveyorTrack_06/Belt", + "/World/ConveyorTrack_01", "/World/ConveyorTrack_01/Belt", + "/World/PlowCornerDeck_B", "/World/PlowCornerDeck_C", + "/World/PlowTransition_B", "/World/PlowTransition_C", + "/World/PlowContainers", "/World/Diverters/DiverterEnd"]: + report(path) + print() + +print("=== PlowContainers full subtree ===") +pc = stage.GetPrimAtPath("/World/PlowContainers") +if pc.IsValid(): + for c in Usd.PrimRange(pc): + r = bbc.ComputeWorldBound(c).ComputeAlignedRange() + if not r.IsEmpty(): + mn, mx = r.GetMin(), r.GetMax() + print(f" {c.GetPath()} type={c.GetTypeName()} " + f"x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]") diff --git a/scripts/gravity_check.py b/scripts/gravity_check.py new file mode 100644 index 0000000..492edef --- /dev/null +++ b/scripts/gravity_check.py @@ -0,0 +1,43 @@ +"""Is gravity actually simulated at all right now? Check PhysicsScene + one item's live state.""" +import omni.usd, omni.timeline +from pxr import UsdPhysics, PhysxSchema, UsdGeom, Usd +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +print("timeline playing:", tl.is_playing()) + +sc = stage.GetPrimAtPath("/World/PhysicsScene") +print("PhysicsScene valid:", sc.IsValid()) +if sc.IsValid(): + physxScene = PhysxSchema.PhysxSceneAPI(sc) + print(" gravityMagnitude:", sc.GetAttribute("physics:gravityMagnitude").Get()) + print(" gravityDirection:", sc.GetAttribute("physics:gravityDirection").Get()) + print(" timeStepsPerSecond:", sc.GetAttribute("physxScene:timeStepsPerSecond").Get()) + print(" enableCCD:", sc.GetAttribute("physxScene:enableCCD").Get()) + +name = "bottle" +prim = stage.GetPrimAtPath(f"/World/Items/{name}") +print(f"\n{name} prim valid:", prim.IsValid()) +print(" applied schemas:", list(prim.GetAppliedSchemas())) +print(" kinematicEnabled:", prim.GetAttribute("physics:kinematicEnabled").Get()) +attr = prim.GetAttribute("xformOp:translate") +print(" authored translate:", attr.Get()) +rp = RigidPrim(paths=[f"/World/Items/{name}"]) +print(" RigidPrim world pose (fabric):", rp.get_world_poses()[0].numpy()[0]) + +# check mass / collision on descendant meshes +n_col = 0 +for p in Usd.PrimRange(prim): + if p.HasAPI(UsdPhysics.CollisionAPI) or p.HasAPI(UsdPhysics.MeshCollisionAPI): + n_col += 1 +print(" descendant prims with CollisionAPI:", n_col) + +print("\n=== stepping physics 60 more times, watching bottle's Z ===") +if not tl.is_playing(): + tl.play() +for i in range(6): + await app_utils.update_app_async(steps=10) + p = rp.get_world_poses()[0].numpy()[0] + print(f" step batch {i}: pos={p} playing={tl.is_playing()} time={tl.get_current_time():.3f}") diff --git a/scripts/impulse_probe.py b/scripts/impulse_probe.py new file mode 100644 index 0000000..3a032ed --- /dev/null +++ b/scripts/impulse_probe.py @@ -0,0 +1,105 @@ +"""1) find the working RigidPrim velocity-set signature + 2) does Belt_01 (now aimed diagonally) actually carry an item into BinD? + 3) does a per-step 'carry assist' (matching the item's +Y velocity to the blade's) + get a pushed item across, where the bare blade tops out at ~0.21 m?""" +import sys, inspect +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import numpy as np +import omni.usd, omni.timeline +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene, plow_cell_9045 + +print("set_velocities signature:", inspect.signature(RigidPrim.set_velocities)) +print("get_velocities signature:", inspect.signature(RigidPrim.get_velocities)) + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) +info = await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True) +print("blade width:", info["pusher_dims"][0], "m") + +ipath = "/World/Items/_imp" +def spawn(x, y, mesh="bag"): + if stage.GetPrimAtPath(ipath).IsValid(): + stage.RemovePrim(ipath) + prim = UsdGeom.Xform.Define(stage, ipath).GetPrim() + prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{mesh}.usd")) + xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(x, y, C.BELT_Z + 0.06)) + UsdPhysics.RigidBodyAPI.Apply(prim) + 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) + UsdGeom.Imageable(prim).MakeVisible() + return RigidPrim(paths=[ipath]) + +# ---------- 2) Belt_01 -> BinD ---------- +print("\n--- item placed on Belt_01 at (-4.10,+0.70), does it reach BinD? ---") +rp = spawn(-4.10, 0.70) +tl.play(); await app_utils.update_app_async(steps=10) +for i in range(8): + await app_utils.update_app_async(steps=45) + p = rp.get_world_poses()[0].numpy()[0] + print(f" t~{(i+1)*45/60:4.1f}s x={float(p[0]):+.2f} y={float(p[1]):+.2f} z={float(p[2]):+.2f}") +p = rp.get_world_poses()[0].numpy()[0] +print(" IN BIN_D:", (-6.21 < float(p[0]) < -4.95) and (1.57 < float(p[1]) < 2.86)) +tl.stop(); await app_utils.update_app_async(steps=6) + +# ---------- 1)+3) velocity API and carry assist ---------- +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + bop = op; break +bbase = bop.Get() +def blade_to(y): + bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2])) + +SENSE_X = C.PUSH_X + plow_cell_9045.PUSHER_X_MM / 2000.0 +for mode in ("bare blade", "blade + carry assist"): + blade_to(C.BLADE_HOME_Y) + rp = spawn(-3.05, 0.0) + tl.play(); await app_utils.update_app_async(steps=8) + for _ in range(400): + if float(rp.get_world_poses()[0].numpy()[0][0]) <= SENSE_X: + break + await app_utils.update_app_async(steps=1) + p0 = rp.get_world_poses()[0].numpy()[0].copy() + a, b, spd = C.BLADE_HOME_Y, 0.52, 1.3 + dur = abs(b - a) / spd + t0 = float(tl.get_current_time()); err = None + while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / dur) + blade_to(a + (b - a) * u) + if mode.endswith("assist") and u < 1.0: + try: + lin = rp.get_velocities()[0].numpy()[0] + rp.set_velocities(np.array([[float(lin[0]), spd, float(lin[2])]]), + np.array([[0.0, 0.0, 0.0]])) + except BaseException as exc: + err = f"{type(exc).__name__}: {exc}" + break + await app_utils.update_app_async(steps=1) + if u >= 1.0: + break + if err: + print(f"\n {mode}: set_velocities FAILED -> {err}") + else: + for _ in range(90): + await app_utils.update_app_async(steps=1) + p = rp.get_world_poses()[0].numpy()[0] + onbranch = float(p[1]) > 0.45 + print(f"\n {mode}: dy={float(p[1])-float(p0[1]):+.3f} final=({float(p[0]):+.2f}," + f"{float(p[1]):+.2f},{float(p[2]):+.2f}) reached_branch={onbranch}") + tl.stop(); await app_utils.update_app_async(steps=6) diff --git a/scripts/live_demo_webrtc.py b/scripts/live_demo_webrtc.py new file mode 100644 index 0000000..cef2e3e --- /dev/null +++ b/scripts/live_demo_webrtc.py @@ -0,0 +1,86 @@ +"""Self-running plow demo for watching over WebRTC. + +Sent into the live streaming Kit. Unlike the test harness this does **not** block in a +loop: it hooks the feeder and the plow onto the physics step, aims the viewport at the plow, +presses Play and returns. The cell then runs on its own for as long as the session lives, +which is what makes it watchable in the browser - a blocking script would hold the +interpreter and the stream would show a frozen frame. + +The subscriptions are stashed in the module namespace on purpose. A PhysX step +subscription dies the moment its Python handle is garbage-collected, so a demo that forgets +to keep a reference stops after the call returns and looks like the scene simply ignoring +the Play button. +""" +import sys + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +import importlib +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +importlib.invalidate_caches() + +import omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.rendering_manager import ViewportManager + +from robozon_sorter import config as C +from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.spawner import AutoFeeder + +SPEED = float(globals().get("speed", 0.8)) +PITCH = float(globals().get("pitch", 3.0)) +RATE = float(globals().get("rate", 300.0)) +N = int(globals().get("n", 8)) + +C.BELT_SPEED = SPEED +C.PLOW_SWEEP_RATE = RATE + +stage, info = plow_vision.load(belt_speed=SPEED, script_control=True, + meshes_dir=f"{REPO}/assets/items") +staging.stage_cell(stage, preset="bright", floor=True) +plow_sort.keep_lanes_active(stage) +lanes = plow_sort.configure_lanes(stage, SPEED) +plow_sort.open_junction(stage) +items = {k: v["zone"] for k, v in info["items"].items()} + +await app_utils.update_app_async(steps=40) +cell = Cell(stage, items.keys()) +cell.park_all() +await app_utils.update_app_async(steps=15) + +order = ([n for n in sorted(items) if items[n] == "B"][:3] + + [n for n in sorted(items) if items[n] == "C"][:3] + + [n for n in sorted(items) if items[n] == "D"][:2])[:N] +sorter = plow_sort.PlowSorter(stage, cell, items, plow_sort.calibrate_mapping()) +beams = lane_beams.LaneBeams(stage, cell, plow=sorter.plow) + + +def _step(dt): + try: + sorter.update(dt) + beams.tick(dt) + beams.poll(rate=RATE) + except Exception: + pass + + +from omni.physx import get_physx_interface +# keep the handles alive in the namespace or the callbacks are collected and the cell stops +STEP_SUB = get_physx_interface().subscribe_physics_step_events(_step) +FEEDER = AutoFeeder(cell, order=order, pitch=PITCH, route=dict(items), loop=True).install() + +# look at the plow from the discharge side so the sweep and both lanes are in frame +ViewportManager.set_camera_view("/OmniverseKit_Persp", + eye=[-5.2, -3.4, 3.2], target=[-7.0, 0.0, 1.9]) +await app_utils.update_app_async(steps=20) +app_utils.play(commit=True) +await app_utils.update_app_async(steps=20) + +print(f"LIVE: {len(order)} items looping, pitch {PITCH} m @ {SPEED} m/s, " + f"sweep {RATE} deg/s (tip {C.PLOW_ARM_LEN * RATE * 3.14159 / 180:.2f} m/s)") +print(f"order: {order}") +print(f"lanes/decks driven: {len(lanes)} | mapping {sorter.mapping}") +print("running on the physics step - the stream stays live, nothing is blocking") diff --git a/scripts/lower_plow_to_belt.py b/scripts/lower_plow_to_belt.py new file mode 100644 index 0000000..ff050b2 --- /dev/null +++ b/scripts/lower_plow_to_belt.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +"""Drop the plow so its blade sweeps at belt level instead of over the goods. + + /home/whatevenif/isaacsim/python.sh scripts/lower_plow_to_belt.py + +Measured on the built scene: + + blade z 1.810 .. 1.890 + belt top z 1.781 + gap under the blade = 29 mm + +29 mm is enough for flat goods to pass straight under the blade, and tall ones get caught +near their top edge and tipped rather than led across. It is the reason items reached only +y ~ 0.24 while the arm was correctly holding 42 deg with 402 mm of reach: the blade was not +touching them at all, so no amount of angle or lane geometry could have helped. + +The whole ``DiverterEnd`` is moved, not just the arm: base and arm keep their relative +placement, so the hinge stays consistent whether the arm is driven kinematically or by its +joint. The pedestal sinks the same 27 mm, which is invisible - it stands on the floor. + +Target clearance is 2 mm: enough that the blade is not grinding on the belt collider, +little enough that nothing rides under it. +""" +from __future__ import annotations + +import sys +from pathlib import Path + +from pxr import Gf, Usd, UsdGeom + +SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd" +DIVERTER = "/World/Diverters/DiverterEnd" +ARM = DIVERTER + "/Arm" +BELT = "/World/ConveyorTrack_04/Belt" +CLEARANCE = 0.002 + + +def _range(stage, path): + return UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound( + stage.GetPrimAtPath(path)).ComputeAlignedRange() + + +def main(): + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + stage = Usd.Stage.Open(str(SCENE)) + + arm = _range(stage, ARM) + belt = _range(stage, BELT) + if arm.IsEmpty() or belt.IsEmpty(): + sys.exit("arm or belt has no bounds - wrong scene?") + + blade_bottom, belt_top = arm.GetMin()[2], belt.GetMax()[2] + gap = blade_bottom - belt_top + drop = gap - CLEARANCE + print(f"blade bottom z {blade_bottom:.4f} | belt top z {belt_top:.4f}") + print(f"gap {gap * 1000:.0f} mm -> target {CLEARANCE * 1000:.0f} mm, dropping {drop * 1000:.0f} mm") + + if abs(drop) < 1e-4: + print("already at height, nothing to do") + return + + prim = stage.GetPrimAtPath(DIVERTER) + xf = UsdGeom.Xformable(prim) + for op in xf.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + t = op.Get() + op.Set(Gf.Vec3d(t[0], t[1], t[2] - drop)) + break + else: + sys.exit(f"{DIVERTER} has no translate op to move") + + stage.GetRootLayer().Save() + after = _range(stage, ARM) + print(f"blade now z[{after.GetMin()[2]:.4f}, {after.GetMax()[2]:.4f}] " + f"-> clearance {(after.GetMin()[2] - belt_top) * 1000:.0f} mm") + print(f"saved {SCENE}") + + +if __name__ == "__main__": + main() diff --git a/scripts/move_lanes_inboard.py b/scripts/move_lanes_inboard.py new file mode 100644 index 0000000..742e09f --- /dev/null +++ b/scripts/move_lanes_inboard.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +"""Bring both plow lanes inboard so the blade can actually reach them. + + /home/whatevenif/isaacsim/python.sh scripts/move_lanes_inboard.py [--shift 0.07] + +Measured over the 25-object run: **no item ever crossed y = 0.39**, while the lanes start +at |y| = 0.45. 16 of 25 came to rest against the blade still on the belt. 0.39 is not a +coincidence - it is where the blade tip is: a 600 mm arm at 42 deg reaches 0.6*sin42 = +0.402 m, and the item is pushed to the tip and no further, because that is where the blade +ends. Raising the angle cannot close it either: at the joint's 45 deg limit the tip reaches +0.424, still short. + +So the lane comes to the blade. Each lane moves 70 mm toward the centreline, putting its +near edge at |y| = 0.38 - inside the tip's reach with ~20 mm to spare. + +**Its tray moves with it.** Moving the lane alone would widen the gap between the lane end +and the tray wall from 100 mm to 170 mm, and goods would fall short onto the floor instead +of into the tray. Lane and tray are one assembly and are shifted by the same vector. + +Known cost: the lane now overlaps the carrying belt by 70 mm (the belt is +-0.45 wide). +Two belt colliders share that strip at the same height, with different surface velocities. +That strip is exactly the hand-over region, so goods there being pulled by both is the +intended behaviour rather than a defect - but it is the thing to look at first if items +start jittering at the lane entry. +""" +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +from pxr import Gf, Usd, UsdGeom + +SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd" + +# prim -> how far to move it in +y (toward the centreline from its own side) +GROUPS = { + "B": dict(shift=+1.0, prims=["/ConveyorTrack_01", "/World/PlowCornerDeck_B"], + tray_prefix="B_"), + "C": dict(shift=-1.0, prims=["/World/ConveyorTrack_01", "/World/PlowCornerDeck_C", + "/World/PlowTransition_C"], + tray_prefix="C_"), +} +TRAYS = "/World/PlowContainers" + + +def _shift_y(stage, path, dy): + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + return False + xf = UsdGeom.Xformable(prim) + for op in xf.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + t = op.Get() + op.Set(type(t)(t[0], t[1] + dy, t[2])) + return True + if op.GetOpType() == UsdGeom.XformOp.TypeTransform: + M = Gf.Matrix4d(op.Get()) + tr = M.ExtractTranslation() + M.SetTranslateOnly(Gf.Vec3d(tr[0], tr[1] + dy, tr[2])) + op.Set(M) + return True + xf.AddTranslateOp().Set(Gf.Vec3d(0.0, dy, 0.0)) + return True + + +def _edge(stage, path): + r = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound( + stage.GetPrimAtPath(path)).ComputeAlignedRange() + return None if r.IsEmpty() else (r.GetMin()[1], r.GetMax()[1]) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--shift", type=float, default=0.07, help="metres toward the centreline") + args = ap.parse_args() + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + stage = Usd.Stage.Open(str(SCENE)) + + for tag, g in GROUPS.items(): + dy = g["shift"] * args.shift + before = _edge(stage, g["prims"][0]) + moved = [p for p in g["prims"] if _shift_y(stage, p, dy)] + trays = [c.GetPath().pathString + for c in stage.GetPrimAtPath(TRAYS).GetChildren() + if c.GetName().startswith(g["tray_prefix"])] + moved += [p for p in trays if _shift_y(stage, p, dy)] + after = _edge(stage, g["prims"][0]) + near_before = min(abs(v) for v in before) if before else float("nan") + near_after = min(abs(v) for v in after) if after else float("nan") + print(f"lane {tag}: dy {dy:+.3f} m, {len(moved)} prims " + f"({len(trays)} of them tray parts)") + print(f" near edge |y| {near_before:.3f} -> {near_after:.3f} " + f"(blade tip reaches 0.402)") + + stage.GetRootLayer().Save() + print(f"saved {SCENE}") + + +if __name__ == "__main__": + main() diff --git a/scripts/narrow_plow.py b/scripts/narrow_plow.py new file mode 100644 index 0000000..4c061f7 --- /dev/null +++ b/scripts/narrow_plow.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +"""Bring the plow arm to a 600 mm sweep width. + + python scripts/narrow_plow.py [--width 0.60] + +The authored arm is 730 mm along its own long axis. That is wider than the 450 mm belt by +enough that it overhangs both rails: it clips goods it should have passed and shoulders +others off the lane instead of steering them. 600 mm still spans the belt with margin while +leaving the lane edges clear. + +Only the scale on `DiverterEnd/Arm/Geom` changes. The hinge, its drive, the limits and the +arm's rigid body are untouched, so the kinematics are exactly as authored - the arm is +simply shorter. + +Which local axis carries the length is not obvious: Geom is rotated -90 deg about Z and its +parent another 180 deg, so the mesh's own X and Y do not map to the world axes you would +guess. The script measures instead of assuming. +""" +from __future__ import annotations + +import argparse +import shutil +import sys +from pathlib import Path + +import numpy as np +from pxr import Gf, Usd, UsdGeom + +ROOT = Path(__file__).resolve().parent.parent +CELL = ROOT / "scene" / "plow_cell.usd" +ARM_GEOM = "/World/Diverters/DiverterEnd/Arm/Geom" +ARM = "/World/Diverters/DiverterEnd/Arm" + + +def arm_length(stage): + """longest principal extent of the arm's mesh points, in world metres""" + cache = UsdGeom.XformCache() + pts = [] + for prim in Usd.PrimRange(stage.GetPrimAtPath(ARM)): + mesh = UsdGeom.Mesh(prim) + if not mesh: + continue + p = mesh.GetPointsAttr().Get() + if not p: + continue + M = cache.GetLocalToWorldTransform(prim) + pts.append(np.array([M.Transform(Gf.Vec3d(*q)) for q in p])) + if not pts: + return None + P = np.vstack(pts) + Q = P - P.mean(0) + _, _, vt = np.linalg.svd(Q, full_matrices=False) + return float(np.ptp(Q @ vt[0])) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--width", type=float, default=0.60, help="target sweep width, metres") + args = ap.parse_args() + + if not CELL.exists(): + print(f"{CELL} not found") + return 1 + + stage = Usd.Stage.Open(str(CELL)) + geom = stage.GetPrimAtPath(ARM_GEOM) + if not geom.IsValid(): + print(f"{ARM_GEOM} missing - is this plow_cell.usd?") + return 1 + + before = arm_length(stage) + if not before: + print("arm carries no mesh points - is assets/plow/ populated?") + return 1 + print(f"arm length now {before*1000:.0f} mm, target {args.width*1000:.0f} mm") + + xf = UsdGeom.Xformable(geom) + scale_op = None + for op in xf.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeScale: + scale_op = op + if scale_op is None: + scale_op = xf.AddScaleOp() + scale_op.Set(Gf.Vec3f(1, 1, 1)) + base = Gf.Vec3f(scale_op.Get() or Gf.Vec3f(1, 1, 1)) + + # find which local axis the length rides on, by testing rather than reasoning about + # the two stacked rotations + factor = args.width / before + best = None + for axis in (0, 1, 2): + trial = Gf.Vec3f(base) + trial[axis] = base[axis] * factor + scale_op.Set(trial) + got = arm_length(stage) + print(f" scale on local {'XYZ'[axis]} -> {got*1000:.0f} mm") + if best is None or abs(got - args.width) < abs(best[1] - args.width): + best = (axis, got, trial) + axis, got, trial = best + scale_op.Set(trial) + + if abs(got - args.width) > 0.005: + print(f"closest achievable was {got*1000:.0f} mm on local {'XYZ'[axis]} - " + "the arm's length may not lie on a single local axis") + return 1 + + backup = CELL.with_suffix(".usd.prewidth") + if not backup.exists(): + shutil.copy(CELL, backup) + print(f"backup -> {backup.name}") + stage.GetRootLayer().Save() + + check = Usd.Stage.Open(str(CELL)) + final = arm_length(check) + cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True) + r = cache.ComputeWorldBound(check.GetPrimAtPath(ARM)).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + print(f"\nsaved. arm is now {final*1000:.0f} mm " + f"(scale {tuple(round(v,4) for v in trial)} on local {'XYZ'[axis]})") + print(f" world AABB x[{mn[0]:.3f}..{mx[0]:.3f}] y[{mn[1]:.3f}..{mx[1]:.3f}] " + f"z[{mn[2]:.3f}..{mx[2]:.3f}]") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scripts/place_plow_lanes.py b/scripts/place_plow_lanes.py new file mode 100644 index 0000000..3ddcadb --- /dev/null +++ b/scripts/place_plow_lanes.py @@ -0,0 +1,317 @@ +#!/usr/bin/env python3 +"""Place the two plow take-away lanes clear of the blade, and put a container at each end. + + python scripts/place_plow_lanes.py + +The plow (x = -7.05, 600 mm arm, +-35 deg about Z) sweeps goods off the side of the main +run. A take-away lane therefore has to start OUTSIDE the main belt, not on it: the first +attempt put both near edges at y = +-0.05, which is inside the 450 mm belt, so the lanes +sat under the blade and fouled its swing. + +Final layout, near edges at y = +-0.25 (just past the belt edge at +-0.225): + + lane C +Y side, rotated 45 deg near end (-7.05, +0.45) runs toward (-X, +Y) + lane B -Y side, perpendicular near end (-7.25, -0.45) runs -Y + container at the far end of each + +Both sit at belt height (top z = 1.781) and start flush with ConveyorTrack_04's belt edge +at y = +-0.45, so they join the run the same way the D branch joins it upstream. + +C is angled because a plow deflection carries goods sideways *and* downstream - they leave +the belt on a diagonal, and a 45 deg lane meets that trajectory instead of fighting it. +B stays square because the -Y throw is the shorter one. + +Only these two tracks move. The plow, its hinge, its drive and the main run are untouched. +""" +from __future__ import annotations + +import math +import shutil +import sys +from pathlib import Path + +from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade + +ROOT = Path(__file__).resolve().parent.parent +CELL = ROOT / "scene" / "plow_cell.usd" + +LANE_B = "/ConveyorTrack_01" # -Y side, perpendicular +LANE_C = "/World/ConveyorTrack_01" # +Y side, 45 deg + +BELT_Z = 1.781 +LANE_SCALE = Gf.Vec3d(1.0, 0.5, 1.0) # same as every other track +# Height matters and was wrong the first time. The D branch (ConveyorTrack_03/Belt_01) +# sits at z 1.74..1.78, flush with the main belt, which is what makes it read as part of +# the conveyor. These tracks carried an authored -0.1 drop, putting them 100 mm low so they +# looked like separate furniture parked nearby. 0.0 puts their belt tops at 1.781 too. +LANE_DROP = 0.0 + +# Near ends must clear the ARM's swept envelope, not just the belt edge. The 600 mm arm +# pivots at (-7.05, 0) and reaches +-35 deg, so its tip traces out to +# 0.60*sin(35) = 0.344 m either side. Starting the lanes at +-0.45 leaves ~100 mm. +ARM_SWEEP_Y = 0.60 * math.sin(math.radians(35.0)) # 0.344 m +# A plow sweeps goods sideways while they are still ON the belt, so a take-away lane has to +# run ALONGSIDE it, its near edge touching the belt's side rail - not past the belt's end. +# The first placement put lane B at x -7.48..-7.03, entirely downstream of where the main +# belt stops (x = -7.00): goods would have had to leave the belt and cross a gap to reach +# it, which is why nothing ever arrived. Both lanes now sit inside the arm's working span +# (x -7.12..-6.52) with their near edges on the belt edge at y = +-0.45. +B_TRANSLATE = Gf.Vec3d(-6.80, -0.45, LANE_DROP) +B_YAW = -90.0 # travel -Y +# A 45 deg lane meeting a straight belt edge does not touch at its centreline: the near +# corner runs ahead of it. Measured overlap at y=0.45 was 159 mm into the belt, so the lane +# is offset by that much and its nearest corner then lands on the edge instead of inside it. +# x=-6.55 puts the lane's near corner at the arm's tip (-6.52) and inside the belt span +# (-7.00..-6.00), i.e. on the junction itself. At -6.90 the corner sat behind the plow, so +# a +Y deflection had nowhere to land and goods rode on past. +C_TRANSLATE = Gf.Vec3d(-6.55, 0.45 + 0.159, LANE_DROP) +C_YAW = 135.0 # travel (-X, +Y): the diagonal a plow throw makes + + +def yaw_quat(deg): + r = math.radians(deg) / 2.0 + return Gf.Quatd(math.cos(r), Gf.Vec3d(0.0, 0.0, math.sin(r))) + + +def set_xform(layer, path, translate, yaw, scale=LANE_SCALE): + spec = layer.GetPrimAtPath(path) + if not spec: + return False + for name, value, vtype in ( + ("xformOp:translate", translate, Sdf.ValueTypeNames.Double3), + ("xformOp:orient", yaw_quat(yaw), Sdf.ValueTypeNames.Quatd), + ("xformOp:scale", scale, Sdf.ValueTypeNames.Double3)): + attr = spec.attributes.get(name) or Sdf.AttributeSpec(spec, name, vtype) + attr.default = value + order = spec.attributes.get("xformOpOrder") or Sdf.AttributeSpec( + spec, "xformOpOrder", Sdf.ValueTypeNames.TokenArray) + order.default = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + return True + + +# ---------------------------------------------------------------- containers +CONTAINERS = "/World/PlowContainers" +BIN_HALF = Gf.Vec3f(0.45, 0.40, 0.26) # inner half-extents +BIN_FLOOR_Z = 1.16 +BIN_WALL_TOP = 1.70 # under the lane surface, so goods tip in +TH = 0.024 + + +def _material(stage, path, rgb): + mat = UsdShade.Material.Define(stage, path) + sh = UsdShade.Shader.Define(stage, path + "/Shader") + sh.CreateIdAttr("UsdPreviewSurface") + sh.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*rgb)) + sh.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.55) + mat.CreateSurfaceOutput().ConnectToSource(sh.ConnectableAPI(), "surface") + return mat + + +def _box(stage, path, centre, half, mat, collider=True): + cube = UsdGeom.Cube.Define(stage, path) + cube.GetSizeAttr().Set(2.0) # scale == half-extent + xf = UsdGeom.Xformable(cube.GetPrim()) + xf.ClearXformOpOrder() + xf.AddTranslateOp().Set(Gf.Vec3d(*centre)) + xf.AddScaleOp().Set(Gf.Vec3f(*half)) + UsdShade.MaterialBindingAPI.Apply(cube.GetPrim()) + UsdShade.MaterialBindingAPI(cube.GetPrim()).Bind(mat) + if collider: + UsdPhysics.CollisionAPI.Apply(cube.GetPrim()) + return cube.GetPrim() + + +def add_container(stage, tag, centre, mat): + """open-top box; the far wall rises above belt height so a moving item cannot skim over""" + cx, cy = centre + hx, hy, _ = BIN_HALF + wh = (BIN_WALL_TOP - BIN_FLOOR_Z) / 2 + wz = BIN_FLOOR_Z + wh + back_top = BELT_Z + 0.22 + back_h = (back_top - BIN_FLOOR_Z) / 2 + base = f"{CONTAINERS}/{tag}" + _box(stage, f"{base}_Floor", (cx, cy, BIN_FLOOR_Z), (hx, hy, TH), mat) + for name, c, h in ( + ("W0", (cx, cy + hy, BIN_FLOOR_Z + back_h), (hx, TH, back_h)), + ("W1", (cx, cy - hy, wz), (hx, TH, wh)), + ("W2", (cx - hx, cy, wz), (TH, hy, wh)), + ("W3", (cx + hx, cy, wz), (TH, hy, wh))): + _box(stage, f"{base}_{name}", c, h, mat) + for i, (lx, ly) in enumerate([(cx - hx + 0.06, cy - hy + 0.06), (cx + hx - 0.06, cy - hy + 0.06), + (cx - hx + 0.06, cy + hy - 0.06), (cx + hx - 0.06, cy + hy - 0.06)]): + _box(stage, f"{base}_Leg{i}", (lx, ly, BIN_FLOOR_Z / 2), + (0.024, 0.024, BIN_FLOOR_Z / 2), mat, collider=False) + + +# ---------------------------------------------------------------- transition plate +def add_transition(stage, lane_path, edge_y, sign, mat, edge_x0, edge_x1): + """Deck the WHOLE discharge corner, not just the touching triangle. + + An angled lane leaves gaps on BOTH sides of its end face: one between its near corner + and the belt edge, another beyond its far corner. Goods do not cross at a single point - + the plow can put them anywhere across the discharge width - so the plate has to span the + entire region between the belt edge and the lane's end face, from one side of the zone + to the other. A plate covering only the first triangle still drops anything pushed wide. + + Built as the convex hull of the belt-edge segment and both end-face corners, extruded + down 20 mm, coplanar with both belt surfaces. + """ + cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True) + xf = UsdGeom.XformCache() + belt = stage.GetPrimAtPath(f"{lane_path}/Belt") + M = xf.GetLocalToWorldTransform(belt) + r = cache.ComputeWorldBound(belt).ComputeAlignedRange() + top = r.GetMax()[2] + + travel = M.TransformDir(Gf.Vec3d(1, 0, 0)).GetNormalized() + across = Gf.Vec3d(-travel[1], travel[0], 0.0) + half_w = 0.225 + centre = Gf.Vec3d((r.GetMin()[0] + r.GetMax()[0]) / 2, + (r.GetMin()[1] + r.GetMax()[1]) / 2, 0.0) + near = centre - travel * 1.0 + c0 = near + across * half_w + c1 = near - across * half_w + + # everything the plate must reach: the belt edge across the discharge zone, and both + # corners of the lane's end face + xs = [edge_x0, edge_x1, float(c0[0]), float(c1[0])] + ys = [edge_y, edge_y, float(c0[1]), float(c1[1])] + far_y = max(ys) if sign > 0 else min(ys) + quad = [(min(xs), edge_y), (max(xs), edge_y), (max(xs), far_y), (min(xs), far_y)] + + path = f"/World/PlowTransition_{'C' if sign > 0 else 'B'}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + if abs(far_y - edge_y) < 0.002: + # a square lane meets the edge flush along its whole face - no gap to deck, and a + # zero-area mesh would be a degenerate collider + return None, [] + mesh = UsdGeom.Mesh.Define(stage, path) + pts = [Gf.Vec3f(x, y, top) for x, y in quad] + [Gf.Vec3f(x, y, top - 0.02) for x, y in quad] + mesh.GetPointsAttr().Set(pts) + faces = [(0, 3, 2, 1), (4, 5, 6, 7), (0, 1, 5, 4), (1, 2, 6, 5), (2, 3, 7, 6), (3, 0, 4, 7)] + mesh.GetFaceVertexCountsAttr().Set([4] * len(faces)) + mesh.GetFaceVertexIndicesAttr().Set([i for f in faces for i in f]) + mesh.GetSubdivisionSchemeAttr().Set("none") + mesh.GetExtentAttr().Set([Gf.Vec3f(min(xs), min(edge_y, far_y), top - 0.02), + Gf.Vec3f(max(xs), max(edge_y, far_y), top)]) + UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()) + UsdShade.MaterialBindingAPI(mesh.GetPrim()).Bind(mat) + UsdPhysics.CollisionAPI.Apply(mesh.GetPrim()) + return path, [(round(x, 3), round(y, 3)) for x, y in quad] + + +def add_corner_deck(stage, lane_path, edge_y, sign, mat, edge_far_x): + """Close the right angle a SQUARE lane leaves against a wider belt. + + Lane B meets the belt flush along its own face, but the belt is wider than the lane: + the run reaches x=-6.00 while the lane stops at x=-6.58. Anything the plow pushes + sideways in that leftover span has open air under it. A triangular fillet spanning the + belt edge out to the run's end and back down the lane's side turns that right angle + into a chute, so goods slide into the lane instead of dropping through. + """ + cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True) + belt = stage.GetPrimAtPath(f"{lane_path}/Belt") + r = cache.ComputeWorldBound(belt).ComputeAlignedRange() + top = r.GetMax()[2] + lane_far_x = r.GetMax()[0] # the lane's edge nearest the open span + span = abs(edge_far_x - lane_far_x) + if span < 0.02: + return None, [] + corner = (lane_far_x, edge_y) + along_belt = (edge_far_x, edge_y) + down_lane = (lane_far_x, edge_y + sign * span) + + path = f"/World/PlowCornerDeck_{'C' if sign > 0 else 'B'}" + if stage.GetPrimAtPath(path).IsValid(): + stage.RemovePrim(path) + tri = [corner, along_belt, down_lane] + mesh = UsdGeom.Mesh.Define(stage, path) + pts = [Gf.Vec3f(x, y, top) for x, y in tri] + [Gf.Vec3f(x, y, top - 0.02) for x, y in tri] + mesh.GetPointsAttr().Set(pts) + faces = [(0, 2, 1), (3, 4, 5), (0, 1, 4, 3), (1, 2, 5, 4), (2, 0, 3, 5)] + mesh.GetFaceVertexCountsAttr().Set([len(f) for f in faces]) + mesh.GetFaceVertexIndicesAttr().Set([i for f in faces for i in f]) + mesh.GetSubdivisionSchemeAttr().Set("none") + xs = [p[0] for p in tri]; ys = [p[1] for p in tri] + mesh.GetExtentAttr().Set([Gf.Vec3f(min(xs), min(ys), top - 0.02), + Gf.Vec3f(max(xs), max(ys), top)]) + UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()) + UsdShade.MaterialBindingAPI(mesh.GetPrim()).Bind(mat) + UsdPhysics.CollisionAPI.Apply(mesh.GetPrim()) + return path, [(round(x, 3), round(y, 3)) for x, y in tri] + + +def main(): + if not CELL.exists(): + print(f"{CELL} not found") + return 1 + backup = CELL.with_suffix(".usd.prelanes") + if not backup.exists(): + shutil.copy(CELL, backup) + print(f"backup -> {backup.name}") + + layer = Sdf.Layer.FindOrOpen(str(CELL)) + set_xform(layer, LANE_B, B_TRANSLATE, B_YAW) + set_xform(layer, LANE_C, C_TRANSLATE, C_YAW) + layer.Save() + print(f"arm sweeps to y=+-{ARM_SWEEP_Y:.3f}; lanes start at +-0.45") + print(f"lane B (-Y, square) near end {tuple(B_TRANSLATE)[:2]}") + print(f"lane C (+Y, 45 deg) near end {tuple(C_TRANSLATE)[:2]}") + + stage = Usd.Stage.Open(str(CELL)) + cache = UsdGeom.BBoxCache(0, ["default", "render"], useExtentsHint=True) + xf = UsdGeom.XformCache() + + if stage.GetPrimAtPath(CONTAINERS).IsValid(): + stage.RemovePrim(CONTAINERS) + UsdGeom.Xform.Define(stage, CONTAINERS) + m_b = _material(stage, f"{CONTAINERS}/M_B", (0.85, 0.22, 0.20)) + m_c = _material(stage, f"{CONTAINERS}/M_C", (0.25, 0.72, 0.32)) + + ends = {} + for tag, path in (("B", LANE_B), ("C", LANE_C)): + belt = stage.GetPrimAtPath(f"{path}/Belt") + r = cache.ComputeWorldBound(belt).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + d = xf.GetLocalToWorldTransform(belt).TransformDir(Gf.Vec3d(1, 0, 0)).GetNormalized() + centre = Gf.Vec3d((mn[0] + mx[0]) / 2, (mn[1] + mx[1]) / 2, 0) + # container just past the discharge end, along the lane's own travel direction + far = centre + d * ((max(mx[0] - mn[0], mx[1] - mn[1]) / 2) + BIN_HALF[1] + 0.10) + ends[tag] = (float(far[0]), float(far[1])) + print(f" lane {tag}: x[{mn[0]:6.2f}..{mx[0]:6.2f}] y[{mn[1]:6.2f}..{mx[1]:6.2f}] " + f"top={mx[2]:.3f} travel({d[0]:+.2f},{d[1]:+.2f})") + + add_container(stage, "B", ends["B"], m_b) + add_container(stage, "C", ends["C"], m_c) + + m_t = _material(stage, f"{CONTAINERS}/M_transition", (0.30, 0.31, 0.34)) + for old in ("/World/PlowTransition_C", "/World/PlowTransition_B"): + if stage.GetPrimAtPath(old).IsValid(): + stage.RemovePrim(old) + # deck both discharge corners across the full width of the junction (belt x -7.00..-6.00) + for lane, edge, sign in ((LANE_C, 0.45, +1), (LANE_B, -0.45, -1)): + path, corners = add_transition(stage, lane, edge, sign, m_t, -7.00, -6.00) + print(f" transition {path or 'not needed (lane meets flush)'}: {corners}") + # a flush lane still leaves the right angle where the belt runs on past it + cpath, ctri = add_corner_deck(stage, lane, edge, sign, m_t, -6.00) + print(f" corner deck {cpath or 'not needed'}: {ctri}") + stage.GetRootLayer().Save() + + print("\ncontainers:") + for tag in ("B", "C"): + r = cache.ComputeWorldBound( + stage.GetPrimAtPath(f"{CONTAINERS}/{tag}_Floor")).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + print(f" {tag}: x[{mn[0]:6.2f}..{mx[0]:6.2f}] y[{mn[1]:6.2f}..{mx[1]:6.2f}] " + f"floor z={mx[2]:.2f}") + + arm = cache.ComputeWorldBound( + stage.GetPrimAtPath("/World/Diverters/DiverterEnd/Arm")).ComputeAlignedRange() + print(f"\nplow arm x[{arm.GetMin()[0]:.2f}..{arm.GetMax()[0]:.2f}] " + f"y[{arm.GetMin()[1]:.2f}..{arm.GetMax()[1]:.2f}]; swept envelope +-{ARM_SWEEP_Y:.2f}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scripts/plow_barrier_scan.py b/scripts/plow_barrier_scan.py new file mode 100644 index 0000000..d3ccc13 --- /dev/null +++ b/scripts/plow_barrier_scan.py @@ -0,0 +1,43 @@ +"""Find what's colliding right at the plow pile-up point, and check Track_06's drive.""" +import omni.usd +from pxr import Usd, UsdGeom, UsdPhysics, PhysxSchema + +stage = omni.usd.get_context().get_stage() +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +print("=== everything with a collider near the plow (x -8.3..-7.0, y -0.6..0.9, z 1.7..2.6) ===") +for p in stage.Traverse(): + has_col = (p.HasAPI(UsdPhysics.CollisionAPI) or p.HasAPI(UsdPhysics.MeshCollisionAPI)) + if not has_col: + continue + attr = p.GetAttribute("physics:collisionEnabled") + enabled = attr.Get() if attr and attr.HasAuthoredValue() else True + if not enabled: + continue + r = bbc.ComputeWorldBound(p).ComputeAlignedRange() + if r.IsEmpty(): + continue + mn, mx = r.GetMin(), r.GetMax() + if mx[0] < -8.3 or mn[0] > -7.0 or mx[1] < -0.6 or mn[1] > 0.9 or mx[2] < 1.7: + continue + approx = None + if p.HasAPI(UsdPhysics.MeshCollisionAPI): + a = p.GetAttribute("physics:approximation") + approx = a.Get() if a else None + print(f" {p.GetPath()} type={p.GetTypeName()} approx={approx}") + print(f" x[{mn[0]:+.2f}..{mx[0]:+.2f}] y[{mn[1]:+.2f}..{mx[1]:+.2f}] z[{mn[2]:+.2f}..{mx[2]:+.2f}]") + +print("\n=== ConveyorTrack_06/Belt drive + friction ===") +belt = stage.GetPrimAtPath("/World/ConveyorTrack_06/Belt") +print(" applied schemas:", list(belt.GetAppliedSchemas())) +sv = belt.GetAttribute("physxSurfaceVelocity:surfaceVelocity") +print(" surfaceVelocity:", sv.Get() if sv else None) +en = belt.GetAttribute("physxSurfaceVelocity:surfaceVelocityEnabled") +print(" surfaceVelocityEnabled:", en.Get() if en else None) +from pxr import UsdShade +api = UsdShade.MaterialBindingAPI(belt) +mat, rel = api.ComputeBoundMaterial(materialPurpose="physics") +print(" physics material:", mat.GetPath() if mat else None) +if mat: + m = UsdPhysics.MaterialAPI(mat.GetPrim()) + print(" static/dynamic friction:", m.GetStaticFrictionAttr().Get(), m.GetDynamicFrictionAttr().Get()) diff --git a/scripts/prep_inplace.py b/scripts/prep_inplace.py new file mode 100644 index 0000000..0fdbc35 --- /dev/null +++ b/scripts/prep_inplace.py @@ -0,0 +1,20 @@ +"""Skip reopening the stage - it's already loaded (367 prims). Just prepare() it in place +to test whether the threading violation is specific to the reopen, not to editing per se.""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import omni.usd +import isaacsim.core.experimental.utils.app as app_utils +from robozon_sorter.sim import plow_cell_9045 + +stage = omni.usd.get_context().get_stage() +print("current stage:", stage.GetRootLayer().identifier, "prims:", len(list(stage.Traverse()))) + +info = await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True) +print("prepare OK:", info) diff --git a/scripts/probe_push_physics.py b/scripts/probe_push_physics.py new file mode 100644 index 0000000..ede16ba --- /dev/null +++ b/scripts/probe_push_physics.py @@ -0,0 +1,110 @@ +"""Does the kinematic blade actually PUSH, or does it pass through and let PhysX untangle? + +The question the whole plow rests on and which nothing so far has answered. A kinematic body +moved with `setKinematicTarget` sweeps: PhysX derives a velocity from the pose delta and +transfers momentum to whatever it meets. A body moved by writing its pose is a **teleport**: +it reappears somewhere else, and anything it now overlaps is pushed apart by depenetration +only - a shove with no momentum behind it, roughly proportional to how deep the overlap is +rather than to how fast the blade was going. + +The two look identical in a viewport and identical in a contact sensor. They differ in one +measurable: the item's velocity while the blade is on it. + + push item picks up lateral speed close to the blade's tangential speed + teleport item barely moves, gets a small separation nudge, and stops + +This puts one item against a stationary blade, sweeps the blade through it, and records the +item's velocity every step. It also confirms the arm's simulated pose actually changes - if +PhysX never sees the rotation, the blade is a ghost and no amount of rate tuning matters. +""" +import sys +import time + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +import importlib +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +importlib.invalidate_caches() + +import omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import UsdPhysics + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell, plow_vision, staging +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.plow import Plow + +ITEM = globals().get("item", "box_300x200x200") +RATE = float(globals().get("rate", 300.0)) +BELT = bool(globals().get("belt", True)) # is the belt driving the item at the time + +stage, info = plow_vision.load(belt_speed=0.8 if BELT else 0.0, script_control=True, + meshes_dir=f"{REPO}/assets/items") +staging.stage_cell(stage, preset="bright", floor=True) +items = {k: v["zone"] for k, v in info["items"].items()} +await app_utils.update_app_async(steps=30) +cell = Cell(stage, items.keys()) +cell.park_all() +await app_utils.update_app_async(steps=10) + +arm = stage.GetPrimAtPath(C.PLOW_ARM) +print("--- what the arm IS ---") +print(" kinematic :", UsdPhysics.RigidBodyAPI(arm).GetKinematicEnabledAttr().Get()) +print(" hinge on :", stage.GetPrimAtPath(C.PLOW_HINGE).GetAttribute( + "physics:jointEnabled").Get()) + +plow = Plow(stage, kinematic=True) +plow.target(0.0) +# put the item just in front of the blade, offset to the side the blade sweeps toward +cell.place(ITEM, (-6.75, 0.10, C.BELT_Z + 0.06)) +tl = omni.timeline.get_timeline_interface() +tl.play() +await app_utils.update_app_async(steps=40) + + +def vel(): + v = cell._rp[ITEM].get_velocities()[0].numpy()[0] + return float(v[0]), float(v[1]), float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5) + + +p0 = cell.pose(ITEM) +print(f"\n--- before: item at x={float(p0[0]):+.3f} y={float(p0[1]):+.3f}, " + f"arm measured {plow.angle:+.1f} deg ---") + +print(f"\n--- sweeping to +42 deg at {RATE} deg/s ---") +print(f"{'step':>4} {'cmd':>7} {'arm':>7} {'item y':>8} {'vy':>7} {'|v|':>6}") +dt = 1.0 / 120.0 +rows = [] +for i in range(220): + done = plow.step_toward(42.0, dt, rate=RATE) + await app_utils.update_app_async(steps=1) + p = cell.pose(ITEM) + vx, vy, sp = vel() + rows.append((plow.commanded, plow.angle, float(p[1]), vy, sp)) + if i % 12 == 0 or (done and i % 4 == 0): + print(f"{i:4d} {plow.commanded:7.1f} {plow.angle:7.1f} {float(p[1]):8.3f} " + f"{vy:7.2f} {sp:6.2f}") + if done and i > 60: + break + +arm_moved = max(abs(r[1]) for r in rows) +peak_vy = max(abs(r[3]) for r in rows) +y_gain = max(r[2] for r in rows) - float(p0[1]) +tip_speed = C.PLOW_ARM_LEN * RATE * 3.14159 / 180.0 + +print(f"\n--- verdict ---") +print(f" arm reached {arm_moved:.1f} deg (commanded 42)") +print(f" blade tip speed {tip_speed:.2f} m/s") +print(f" item peak |vy| {peak_vy:.2f} m/s") +print(f" item lateral travel {y_gain:+.3f} m") +if arm_moved < 5: + print(" => PhysX never saw the rotation: the blade is a ghost") +elif peak_vy < 0.15 * tip_speed: + print(" => TELEPORT, not a sweep: the arm arrives without momentum and the item only") + print(" gets a depenetration nudge. Rate tuning cannot fix this.") +else: + print(" => real push: the item takes up a fair share of the blade's tip speed") +tl.stop() diff --git a/scripts/push_method.py b/scripts/push_method.py new file mode 100644 index 0000000..46fa8a5 --- /dev/null +++ b/scripts/push_method.py @@ -0,0 +1,133 @@ +"""Blade drive METHOD comparison - the speed sweep proved speed is not the variable. + +A: write xformOp:translate (what the pipeline does now) = a TELEPORT. PhysX sees no + velocity; the item gets only a depenetration shove, so a FASTER blade pushes LESS - + exactly what the sweep measured (1.3->99mm, 2.6->17mm). This is probe_push_physics.py's + documented teleport signature. +B: RigidPrim.set_world_poses() -> sets the KINEMATIC TARGET on the physics backend, so + PhysX derives velocity = delta/dt and transfers real momentum. +C: dynamic blade + set_velocities() -> a genuine moving mass carrying momentum. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import numpy as np +import omni.usd, omni.timeline +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene, plow_cell_9045 + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) +await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True) + +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +blade_rp = RigidPrim(paths=[_scene.BLADE]) +def _bop(): + for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op +bop = _bop(); bbase = bop.Get() +def blade_xform_to(y): + bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2])) + +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +r0 = bbc.ComputeWorldBound(blade_prim).ComputeAlignedRange() +BLADE_WORLD_X = (r0.GetMin()[0] + r0.GetMax()[0]) / 2.0 +BLADE_WORLD_Z = (r0.GetMin()[2] + r0.GetMax()[2]) / 2.0 +SENSE_X = r0.GetMax()[0] +print(f"blade centre x={BLADE_WORLD_X:+.3f} z={BLADE_WORLD_Z:+.3f}, sense {SENSE_X:+.3f}") + +ipath = "/World/Items/_pushprobe2" +def spawn(): + if stage.GetPrimAtPath(ipath).IsValid(): + stage.RemovePrim(ipath) + prim = UsdGeom.Xform.Define(stage, ipath).GetPrim() + prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / "box_300x200x200.usd")) + xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(-3.05, 0.0, C.BELT_Z + 0.05)) + UsdPhysics.RigidBodyAPI.Apply(prim) + 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.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) + UsdGeom.Imageable(prim).MakeVisible() + return RigidPrim(paths=[ipath]) + +SPEED = 1.3 +A, B = C.BLADE_HOME_Y, 0.55 + +async def ride_to_blade(rp): + for _ in range(400): + if float(rp.get_world_poses()[0].numpy()[0][0]) <= SENSE_X: + return True + await app_utils.update_app_async(steps=1) + return False + +print(f"\n{'method':>34} {'y_gain':>8} {'final_y':>8} {'final_z':>8} verdict") +print("-" * 76) + +for method in ("A: usd xform write (current)", "B: RigidPrim.set_world_poses", + "C: dynamic + set_velocities"): + # reset blade to kinematic home + UsdPhysics.RigidBodyAPI(blade_prim).CreateKinematicEnabledAttr().Set(True) + blade_xform_to(A) + rp = spawn() + tl.play(); await app_utils.update_app_async(steps=8) + await ride_to_blade(rp) + p0 = rp.get_world_poses()[0].numpy()[0].copy() + + dur = abs(B - A) / SPEED + t0 = float(tl.get_current_time()) + if method.startswith("C"): + UsdPhysics.RigidBodyAPI(blade_prim).CreateKinematicEnabledAttr().Set(False) + UsdPhysics.MassAPI.Apply(blade_prim).CreateMassAttr().Set(200.0) + PhysxSchema.PhysxRigidBodyAPI.Apply(blade_prim).CreateDisableGravityAttr().Set(True) + await app_utils.update_app_async(steps=2) + while True: + t = float(tl.get_current_time()) - t0 + u = min(1.0, t / dur) + y = A + (B - A) * u + try: + if method.startswith("A"): + blade_xform_to(y) + elif method.startswith("B"): + blade_rp.set_world_poses(positions=np.array([[BLADE_WORLD_X, y, BLADE_WORLD_Z]])) + else: + vy = 0.0 if u >= 1.0 else SPEED + blade_rp.set_velocities(np.array([[0.0, vy, 0.0, 0.0, 0.0, 0.0]])) + except BaseException as exc: + print(f" {method}: drive call failed: {type(exc).__name__}") + break + await app_utils.update_app_async(steps=1) + if u >= 1.0: + break + for _ in range(60): + if method.startswith("C"): + try: + blade_rp.set_velocities(np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]])) + except BaseException: + pass + await app_utils.update_app_async(steps=1) + + p = rp.get_world_poses()[0].numpy()[0] + gain = float(p[1]) - float(p0[1]) + verdict = ("FELL" if float(p[2]) < 1.2 else + "EJECTED" if abs(float(p[1])) > 3.0 else + "DELIVERED" if float(p[1]) > 0.45 else "short") + print(f"{method:>34} {gain:8.3f} {float(p[1]):8.3f} {float(p[2]):8.3f} {verdict}") + + tl.stop(); await app_utils.update_app_async(steps=6) + UsdPhysics.RigidBodyAPI(blade_prim).CreateKinematicEnabledAttr().Set(True) + blade_xform_to(A) diff --git a/scripts/push_sweep.py b/scripts/push_sweep.py new file mode 100644 index 0000000..ca40db0 --- /dev/null +++ b/scripts/push_sweep.py @@ -0,0 +1,119 @@ +"""Pusher speed sweep with the CURRENT grip material + 500 mm blade. + +Earlier speed tuning (pusher_diag*.py -> PUSH_SPEED=1.3) was measured while the blade was +still bound to the SLIPPERY DiverterMaterial (0.12/0.08); the grip fix (1.1/0.95) makes +those numbers stale. Also tests firing IMMEDIATELY at detection vs waiting to PUSH_X+0.08: +geometry says the item has only 0.5 m of blade (0.5 s at 1 m/s) and the wait burns 0.17 s +of it, so the wait may be why the stroke never completes on the item. + +Outcome per trial: y_gain (needs > ~0.45 to reach the branch belt) and whether the item +survived at belt height or was ejected / fell through. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib; importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene, plow_cell_9045 + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +info = await plow_cell_9045.prepare(stage, belt_speed=1.0, script_control=True) +print("blade dims:", info["pusher_dims"], " seat:", info["pusher_seat"]) + +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +def _bop(): + for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op +bop = _bop(); bbase = bop.Get() +def blade_to(y): + bop.Set(Gf.Vec3d(bbase[0], y - _scene.BLADE_PARENT_Y, bbase[2])) + +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +r = bbc.ComputeWorldBound(stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom")).ComputeAlignedRange() +BLADE_X0, BLADE_X1 = r.GetMin()[0], r.GetMax()[0] +SENSE_X = BLADE_X1 # leading (downstream-facing) edge +print(f"blade x[{BLADE_X0:+.3f}..{BLADE_X1:+.3f}] sense at {SENSE_X:+.3f} " + f"contact window = {(BLADE_X1-BLADE_X0)/1.0:.3f} s at 1 m/s") + +ITEM = "box_300x200x200" +ipath = "/World/Items/_pushprobe" + +def spawn(): + if stage.GetPrimAtPath(ipath).IsValid(): + stage.RemovePrim(ipath) + prim = UsdGeom.Xform.Define(stage, ipath).GetPrim() + prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{ITEM}.usd")) + xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(-3.05, 0.0, C.BELT_Z + 0.05)) + UsdPhysics.RigidBodyAPI.Apply(prim) + 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.CreateSolverVelocityIterationCountAttr().Set(8) + px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) + UsdGeom.Imageable(prim).MakeVisible() + return RigidPrim(paths=[ipath]) + +print(f"\n{'speed':>6} {'wait':>6} {'y_gain':>8} {'final_x':>8} {'final_y':>8} {'final_z':>8} verdict") +print("-" * 78) +results = [] +for speed in (1.3, 1.7, 2.1, 2.6): + for wait_to_center in (False, True): + rp = spawn() + blade_to(C.BLADE_HOME_Y) + tl.play(); await app_utils.update_app_async(steps=8) + # ride until the leading edge sees it + for _ in range(400): + if float(rp.get_world_poses()[0].numpy()[0][0]) <= SENSE_X: + break + await app_utils.update_app_async(steps=1) + if wait_to_center: + for _ in range(60): + if float(rp.get_world_poses()[0].numpy()[0][0]) <= C.PUSH_X + 0.08: + break + await app_utils.update_app_async(steps=1) + p0 = rp.get_world_poses()[0].numpy()[0].copy() + a, b = C.BLADE_HOME_Y, 0.55 + dur = abs(b - a) / speed + t0 = float(tl.get_current_time()) + while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / dur) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + if u >= 1.0: + break + for _ in range(60): # let it settle / travel on + await app_utils.update_app_async(steps=1) + p = rp.get_world_poses()[0].numpy()[0] + gain = float(p[1]) - float(p0[1]) + if float(p[2]) < 1.2: + verdict = "FELL/LOST" + elif abs(float(p[1])) > 3.0: + verdict = "EJECTED" + elif float(p[1]) > 0.45: + verdict = "DELIVERED" + else: + verdict = "short - stayed on main belt" + print(f"{speed:6.1f} {str(wait_to_center):>6} {gain:8.3f} {float(p[0]):8.3f} " + f"{float(p[1]):8.3f} {float(p[2]):8.3f} {verdict}") + results.append((speed, wait_to_center, gain, verdict)) + tl.stop(); await app_utils.update_app_async(steps=6) + blade_to(C.BLADE_HOME_Y) + +good = [r for r in results if r[3] == "DELIVERED"] +print(f"\nDELIVERED configs: {[(s, w) for s, w, g, v in good]}") diff --git a/scripts/pusher_diag.py b/scripts/pusher_diag.py new file mode 100644 index 0000000..e2852e4 --- /dev/null +++ b/scripts/pusher_diag.py @@ -0,0 +1,101 @@ +"""Isolated pusher diagnostic: place one item right at the blade, sweep it, log velocity +every step. Determines push (item picks up tangential speed) vs teleport (item barely +moves, gets a depenetration nudge, stops) - probe_push_physics.py's own distinction. +Reuses the CURRENTLY prepared stage (belts/plow/pusher/rails already configured by the +last full run) rather than reopening. +""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop() + await app_utils.update_app_async(steps=10) + +print("=== blade geometry now ===") +geom = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom") +bbc = UsdGeom.BBoxCache(0, ["default", "render"]) +r = bbc.ComputeWorldBound(geom).ComputeAlignedRange() +print(f" blade bbox x[{r.GetMin()[0]:+.3f}..{r.GetMax()[0]:+.3f}] " + f"y[{r.GetMin()[1]:+.3f}..{r.GetMax()[1]:+.3f}] z[{r.GetMin()[2]:+.3f}..{r.GetMax()[2]:+.3f}]") +print(f" BLADE_HOME_Y={C.BLADE_HOME_Y} BLADE_OUT_Y={C.BLADE_OUT_Y} PUSH_X={C.PUSH_X} BELT_Z={C.BELT_Z}") + +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +print(" blade kinematic:", UsdPhysics.RigidBodyAPI(blade_prim).GetKinematicEnabledAttr().Get()) +print(" blade collision enabled (self):", blade_prim.GetAttribute("physics:collisionEnabled").Get()) +for c in blade_prim.GetChildren(): + print(" child", c.GetPath(), "collisionEnabled:", c.GetAttribute("physics:collisionEnabled").Get()) + +# reset blade to home +def blade_op(): + for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op +blade_base = blade_op().Get() +def blade_to(y): + b = blade_base + blade_op().Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2])) +blade_to(C.BLADE_HOME_Y) + +# place a fresh item just upstream of the blade, in its path +name = "probe_item" +items_dir = C.ROOT / "assets" / "items" +prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() +prim.GetReferences().ClearReferences() +prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd")) +xf = UsdGeom.Xformable(prim) +xf.ClearXformOpOrder() +xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.05, 0.0, C.BELT_Z + 0.10)) +UsdPhysics.RigidBodyAPI.Apply(prim) +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) +UsdGeom.Imageable(prim).MakeVisible() +rp = RigidPrim(paths=[f"/World/Items/{name}"]) + +tl.play() +await app_utils.update_app_async(steps=40) +p0 = rp.get_world_poses()[0].numpy()[0] +print(f"\n--- settled at x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f} ---") + +print("\n--- sweeping blade out at PUSHER_MAX_SAFE, logging item state ---") +speed = C.PUSHER_MAX_SAFE +a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y +duration = abs(b - a) / speed +t0 = float(tl.get_current_time()) +i = 0 +print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'item_z':>8} {'vx':>7} {'vy':>7}") +while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / duration) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + p = rp.get_world_poses()[0].numpy()[0] + v = rp.get_velocities()[0].numpy()[0] + if i % 3 == 0 or u >= 1.0: + print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} " + f"{p[0]:8.3f} {p[1]:8.3f} {p[2]:8.3f} {v[0]:7.3f} {v[1]:7.3f}") + i += 1 + if u >= 1.0: + break + +p_final = rp.get_world_poses()[0].numpy()[0] +print(f"\n--- after stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (started y={p0[1]:+.3f}, moved {p_final[1]-p0[1]:+.3f}) ---") +tl.stop() diff --git a/scripts/pusher_diag2.py b/scripts/pusher_diag2.py new file mode 100644 index 0000000..69b4338 --- /dev/null +++ b/scripts/pusher_diag2.py @@ -0,0 +1,82 @@ +"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide +clean past the blade's x-window before the sweep even started - fire the sweep the +instant the item is placed, matching the real pipeline's timing exactly.""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop() + await app_utils.update_app_async(steps=10) + +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +def blade_op(): + for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op +blade_base = blade_op().Get() +def blade_to(y): + b = blade_base + blade_op().Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2])) +blade_to(C.BLADE_HOME_Y) + +name = "probe_item2" +items_dir = C.ROOT / "assets" / "items" +prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() +prim.GetReferences().ClearReferences() +prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd")) +xf = UsdGeom.Xformable(prim) +xf.ClearXformOpOrder() +xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10)) +UsdPhysics.RigidBodyAPI.Apply(prim) +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) +UsdGeom.Imageable(prim).MakeVisible() +rp = RigidPrim(paths=[f"/World/Items/{name}"]) + +tl.play() +await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget +p0 = rp.get_world_poses()[0].numpy()[0] +print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}") +print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}") + +speed = C.PUSHER_MAX_SAFE +a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y +duration = abs(b - a) / speed +t0 = float(tl.get_current_time()) +i = 0 +print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}") +while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / duration) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + p = rp.get_world_poses()[0].numpy()[0] + v = rp.get_velocities()[0].numpy()[0] + print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} " + f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}") + i += 1 + if u >= 1.0: + break + +p_final = rp.get_world_poses()[0].numpy()[0] +print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})") +tl.stop() diff --git a/scripts/pusher_diag3.py b/scripts/pusher_diag3.py new file mode 100644 index 0000000..99bad54 --- /dev/null +++ b/scripts/pusher_diag3.py @@ -0,0 +1,82 @@ +"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide +clean past the blade's x-window before the sweep even started - fire the sweep the +instant the item is placed, matching the real pipeline's timing exactly.""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop() + await app_utils.update_app_async(steps=10) + +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +def blade_op(): + for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op +blade_base = blade_op().Get() +def blade_to(y): + b = blade_base + blade_op().Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2])) +blade_to(C.BLADE_HOME_Y) + +name = "probe_item2" +items_dir = C.ROOT / "assets" / "items" +prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() +prim.GetReferences().ClearReferences() +prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd")) +xf = UsdGeom.Xformable(prim) +xf.ClearXformOpOrder() +xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10)) +UsdPhysics.RigidBodyAPI.Apply(prim) +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) +UsdGeom.Imageable(prim).MakeVisible() +rp = RigidPrim(paths=[f"/World/Items/{name}"]) + +tl.play() +await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget +p0 = rp.get_world_poses()[0].numpy()[0] +print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}") +print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}") + +speed = 1.0 +a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y +duration = abs(b - a) / speed +t0 = float(tl.get_current_time()) +i = 0 +print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}") +while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / duration) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + p = rp.get_world_poses()[0].numpy()[0] + v = rp.get_velocities()[0].numpy()[0] + print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} " + f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}") + i += 1 + if u >= 1.0: + break + +p_final = rp.get_world_poses()[0].numpy()[0] +print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})") +tl.stop() diff --git a/scripts/pusher_diag4.py b/scripts/pusher_diag4.py new file mode 100644 index 0000000..587bbcf --- /dev/null +++ b/scripts/pusher_diag4.py @@ -0,0 +1,82 @@ +"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide +clean past the blade's x-window before the sweep even started - fire the sweep the +instant the item is placed, matching the real pipeline's timing exactly.""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop() + await app_utils.update_app_async(steps=10) + +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +def blade_op(): + for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op +blade_base = blade_op().Get() +def blade_to(y): + b = blade_base + blade_op().Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2])) +blade_to(C.BLADE_HOME_Y) + +name = "probe_item2" +items_dir = C.ROOT / "assets" / "items" +prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() +prim.GetReferences().ClearReferences() +prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd")) +xf = UsdGeom.Xformable(prim) +xf.ClearXformOpOrder() +xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10)) +UsdPhysics.RigidBodyAPI.Apply(prim) +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) +UsdGeom.Imageable(prim).MakeVisible() +rp = RigidPrim(paths=[f"/World/Items/{name}"]) + +tl.play() +await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget +p0 = rp.get_world_poses()[0].numpy()[0] +print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}") +print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}") + +speed = 0.6 +a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y +duration = abs(b - a) / speed +t0 = float(tl.get_current_time()) +i = 0 +print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}") +while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / duration) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + p = rp.get_world_poses()[0].numpy()[0] + v = rp.get_velocities()[0].numpy()[0] + print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} " + f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}") + i += 1 + if u >= 1.0: + break + +p_final = rp.get_world_poses()[0].numpy()[0] +print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})") +tl.stop() diff --git a/scripts/pusher_diag5.py b/scripts/pusher_diag5.py new file mode 100644 index 0000000..bcaffb9 --- /dev/null +++ b/scripts/pusher_diag5.py @@ -0,0 +1,82 @@ +"""Same as pusher_diag.py but WITHOUT the pre-sweep settle wait that let the item slide +clean past the blade's x-window before the sweep even started - fire the sweep the +instant the item is placed, matching the real pipeline's timing exactly.""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import scene as _scene + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop() + await app_utils.update_app_async(steps=10) + +blade_prim = stage.GetPrimAtPath(_scene.BLADE) +def blade_op(): + for op in UsdGeom.Xformable(blade_prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op +blade_base = blade_op().Get() +def blade_to(y): + b = blade_base + blade_op().Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2])) +blade_to(C.BLADE_HOME_Y) + +name = "probe_item2" +items_dir = C.ROOT / "assets" / "items" +prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() +prim.GetReferences().ClearReferences() +prim.GetReferences().AddReference(str(items_dir / "box_300x200x200.usd")) +xf = UsdGeom.Xformable(prim) +xf.ClearXformOpOrder() +xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(C.PUSH_X + 0.08, 0.0, C.BELT_Z + 0.10)) +UsdPhysics.RigidBodyAPI.Apply(prim) +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) +UsdGeom.Imageable(prim).MakeVisible() +rp = RigidPrim(paths=[f"/World/Items/{name}"]) + +tl.play() +await app_utils.update_app_async(steps=3) # bare minimum so PhysX registers the body, no drift budget +p0 = rp.get_world_poses()[0].numpy()[0] +print(f"start of sweep: x={p0[0]:+.3f} y={p0[1]:+.3f} z={p0[2]:+.3f}") +print(f"blade x-window at this Y: computed from Geom scale, PUSH_X={C.PUSH_X}") + +speed = 1.3 +a, b = C.BLADE_HOME_Y, C.BLADE_OUT_Y +duration = abs(b - a) / speed +t0 = float(tl.get_current_time()) +i = 0 +print(f"{'i':>3} {'t':>6} {'blade_y':>8} {'item_x':>8} {'item_y':>8} {'vx':>7} {'vy':>7}") +while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / duration) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + p = rp.get_world_poses()[0].numpy()[0] + v = rp.get_velocities()[0].numpy()[0] + print(f"{i:3d} {float(tl.get_current_time())-t0:6.3f} {a+(b-a)*u:8.3f} " + f"{p[0]:8.3f} {p[1]:8.3f} {v[0]:7.3f} {v[1]:7.3f}") + i += 1 + if u >= 1.0: + break + +p_final = rp.get_world_poses()[0].numpy()[0] +print(f"\nafter stroke: x={p_final[0]:+.3f} y={p_final[1]:+.3f} (moved y by {p_final[1]-p0[1]:+.3f})") +tl.stop() diff --git a/scripts/pusher_geom.py b/scripts/pusher_geom.py new file mode 100644 index 0000000..913d639 --- /dev/null +++ b/scripts/pusher_geom.py @@ -0,0 +1,19 @@ +"""Inspect the pusher blade's actual geometry and any existing D-push helpers.""" +import omni.usd +from pxr import Usd, UsdGeom + +stage = omni.usd.get_context().get_stage() +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +for path in ["/World/Diverters/DiverterY_Split", "/World/Diverters/DiverterY_Split/Pusher", + "/World/Diverters/DiverterY_Split/Pusher/Geom"]: + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + print(path, "MISSING"); continue + r = bbc.ComputeWorldBound(prim).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + print(f"{path}") + print(f" bbox x[{mn[0]:+.3f}..{mx[0]:+.3f}] ({(mx[0]-mn[0])*1000:.0f}mm) " + f"y[{mn[1]:+.3f}..{mx[1]:+.3f}] ({(mx[1]-mn[1])*1000:.0f}mm) " + f"z[{mn[2]:+.3f}..{mx[2]:+.3f}]") + print(f" children: {[c.GetName() for c in prim.GetChildren()]}") diff --git a/scripts/pusher_mat.py b/scripts/pusher_mat.py new file mode 100644 index 0000000..e27375c --- /dev/null +++ b/scripts/pusher_mat.py @@ -0,0 +1,12 @@ +import omni.usd +from pxr import UsdShade, UsdPhysics +stage = omni.usd.get_context().get_stage() +prim = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher/Geom") +api = UsdShade.MaterialBindingAPI(prim) +mat, rel = api.ComputeBoundMaterial(materialPurpose="physics") +print("physics material bound:", mat.GetPath() if mat else None) +if mat: + m = UsdPhysics.MaterialAPI(mat.GetPrim()) + print(" static friction:", m.GetStaticFrictionAttr().Get()) + print(" dynamic friction:", m.GetDynamicFrictionAttr().Get()) + print(" restitution:", m.GetRestitutionAttr().Get()) diff --git a/scripts/pusher_ops.py b/scripts/pusher_ops.py new file mode 100644 index 0000000..1e109cc --- /dev/null +++ b/scripts/pusher_ops.py @@ -0,0 +1,9 @@ +import omni.usd +from pxr import UsdGeom +stage = omni.usd.get_context().get_stage() +for path in ["/World/Diverters/DiverterY_Split/Pusher", "/World/Diverters/DiverterY_Split/Pusher/Geom"]: + prim = stage.GetPrimAtPath(path) + print(path, prim.GetTypeName()) + for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): + print(" ", op.GetOpName(), op.GetOpType(), op.Get()) + print(" refs:", [str(r) for r in prim.GetPrimStack()][:2]) diff --git a/scripts/reverse_plow_mount.py b/scripts/reverse_plow_mount.py new file mode 100644 index 0000000..2ceef68 --- /dev/null +++ b/scripts/reverse_plow_mount.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 +"""Turn the plow round: pivot upstream, free end downstream at the discharge edge. + + /home/whatevenif/isaacsim/python.sh scripts/reverse_plow_mount.py + +Measured on the built scene, with goods travelling in **-X**: + + pivot x = -7.050 <- DOWNSTREAM end + free tip x = -6.658 (at 42 deg) <- UPSTREAM end, y +0.353 + +That is a plough mounted backwards. A plough is an inclined plane: the belt drives the item +along the blade toward the blade's **downstream** end, and the item leaves there. With the +downstream end sitting at the pivot on the belt centreline (y = 0), goods are funnelled +*inward*, slip past the pivot and carry on down the line. They can never discharge. + +It explains every symptom: the 0.39 m ceiling is the brief shove from the sweep, after +which the item slides back toward the centre; moving the lanes inboard changed nothing +because the lane edge was never what goods were failing to reach; and goods pile in the +wedge between the blade and the belt centre, which is what the viewport shows. + +The fix is the mounting, not the length: + + pivot x -7.050 -> -6.522 (upstream end of the same physical span) + arm extends -X instead of +X (rotateZ 180 -> 0) + +so at 42 deg the free end lands near x -6.92, y +-0.353 - downstream of the pivot and out +at the discharge side. Goods now slide *outward and forward* along the blade. + +**The swing sign flips with the mount.** `plow_sort.calibrate_mapping()` says to measure it +rather than reason about it; re-measure after running this. +""" +from __future__ import annotations + +import math +import sys +from pathlib import Path + +from pxr import Gf, Usd, UsdGeom + +SCENE = Path(__file__).resolve().parent.parent / "scene" / "plow_cell.usd" +DIVERTER = "/World/Diverters/DiverterEnd" +ARM = DIVERTER + "/Arm" + + +def main(): + if not SCENE.exists(): + sys.exit(f"{SCENE} not found") + stage = Usd.Stage.Open(str(SCENE)) + prim = stage.GetPrimAtPath(DIVERTER) + if not prim.IsValid(): + sys.exit(f"{DIVERTER} missing") + + xc = UsdGeom.XformCache() + piv = xc.GetLocalToWorldTransform(stage.GetPrimAtPath(ARM)).ExtractTranslation() + r = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound( + stage.GetPrimAtPath(ARM)).ComputeAlignedRange() + reach = r.GetMax()[0] - piv[0] # +0.528: arm points upstream today + print(f"before: pivot x={piv[0]:.3f}, arm reaches {reach:+.3f} m in X " + f"({'UPSTREAM - wrong way' if reach > 0 else 'downstream'})") + + xf = UsdGeom.Xformable(prim) + moved = flipped = False + for op in xf.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate and not moved: + t = op.Get() + op.Set(Gf.Vec3d(t[0] + reach, t[1], t[2])) # pivot to the upstream end + moved = True + elif op.GetOpType() == UsdGeom.XformOp.TypeRotateZ and not flipped: + op.Set(float((op.Get() or 0.0) + 180.0) % 360.0) # arm now points downstream + flipped = True + if not (moved and flipped): + sys.exit(f"{DIVERTER} needs both a translate and a rotateZ op " + f"(moved={moved}, flipped={flipped})") + + stage.GetRootLayer().Save() + + xc2 = UsdGeom.XformCache() + piv2 = xc2.GetLocalToWorldTransform(stage.GetPrimAtPath(ARM)).ExtractTranslation() + r2 = UsdGeom.BBoxCache(0, ["default"]).ComputeWorldBound( + stage.GetPrimAtPath(ARM)).ComputeAlignedRange() + L = abs(reach) + print(f"after : pivot x={piv2[0]:.3f}, arm spans x[{r2.GetMin()[0]:.3f}, " + f"{r2.GetMax()[0]:.3f}]") + for a in (0, 20, 42): + print(f" {a:2d} deg: free end x={piv2[0] - L * math.cos(math.radians(a)):+.3f} " + f"y={L * math.sin(math.radians(a)):+.3f} (downstream of pivot = correct)") + print(f"saved {SCENE}") + print("NOTE: the swing sign flips with the mount - re-measure calibrate_mapping()") + + +if __name__ == "__main__": + main() diff --git a/scripts/run_demo.py b/scripts/run_demo.py new file mode 100644 index 0000000..4bb7746 --- /dev/null +++ b/scripts/run_demo.py @@ -0,0 +1,302 @@ +#!/usr/bin/env python3 +"""One-command demonstration of the whole cell: staging, conveyor, vision, pusher, plow. + + ./python.sh scripts/run_demo.py # 50 dispatches, lit, windowed + ./python.sh scripts/run_demo.py --headless --repeats 2 + ./python.sh scripts/run_demo.py --preset harsh --no-vision + ./python.sh scripts/run_demo.py --items bucket,barrel,box_300x200x200 + +This is the entry point for someone who has not built the scenario before: it stages the +cell, dispatches the item library, and writes one JSON with everything needed to judge the +result. Nothing has to be assembled by hand first. + +It reports two things that are easy to conflate and must be kept apart: + +* **classification** - did the vision stack name the class correctly? Measured against + ground truth from the manifest, with a confusion matrix. +* **delivery** - did the item physically reach the tray its class routes to? A correct + class that ends up on the floor is a delivery failure, not a vision failure, and the + reverse happens too - a misread item can still land somewhere by luck. + +Per dispatch it also records the plow's state *at the moment the item is level with it*: +the commanded angle, the angle the arm actually reached, and its angular rate. Those three +are different numbers because the plow is a compliant force drive, and the difference is +usually what explains a miss. +""" +from __future__ import annotations + +import argparse +import json +import math +import sys +import time +from datetime import datetime +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent +if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) +USER_SITE = "/home/dasha/.local/lib/python3.12/site-packages" # ultralytics lives here +if Path(USER_SITE).exists() and USER_SITE not in sys.path: + sys.path.append(USER_SITE) + + +def parse_args(argv=None): + p = argparse.ArgumentParser(description="Robozon sorting cell - full demonstration run") + p.add_argument("--headless", action="store_true") + p.add_argument("--preset", default="bright", choices=["bright", "dim", "harsh"], + help="lighting preset the run is staged under") + p.add_argument("--no-floor", action="store_true", help="skip the catch floor") + p.add_argument("--repeats", type=int, default=2, + help="passes over the library; 2 x 25 items = 50 dispatches") + p.add_argument("--items", default=None, help="comma-separated subset, for a quick check") + p.add_argument("--no-vision", action="store_true", + help="route on ground truth instead of running CRE-ROI v2b") + p.add_argument("--pitch", type=float, default=2.5, help="metres between dispatches") + p.add_argument("--speed", type=float, default=None, help="belt speed override, m/s") + p.add_argument("--settle", type=float, default=6.0, + help="seconds to let the last item come to rest before scoring") + p.add_argument("--out", default=None, help="where to write the run log") + return p.parse_args(argv) + + +# --------------------------------------------------------------------------- scoring +def confusion(records): + labels = ["B", "C", "D"] + m = {g: {p: 0 for p in labels + ["?"]} for g in labels} + for r in records: + if r["gt"] in m: + m[r["gt"]][r["pred"] if r["pred"] in m[r["gt"]] else "?"] += 1 + return m + + +def summarise(records, expect): + graded = [r for r in records if r.get("pred") not in (None, "?")] + hits = sum(1 for r in graded if r["pred"] == r["gt"]) + delivered = [r for r in records if r.get("delivered")] + by_class = {} + for cls in ("B", "C", "D"): + same = [r for r in records if r["gt"] == cls] + if same: + by_class[cls] = dict( + dispatched=len(same), + delivered=sum(1 for r in same if r.get("delivered")), + classified=sum(1 for r in same if r.get("pred") == cls), + target=expect.get(cls)) + where = {} + for r in records: + where[r["outcome"]] = where.get(r["outcome"], 0) + 1 + return dict( + dispatched=len(records), + classification=dict(graded=len(graded), correct=hits, + accuracy=round(hits / len(graded), 3) if graded else None, + confusion=confusion(records)), + delivery=dict(delivered=len(delivered), + rate=round(len(delivered) / len(records), 3) if records else None, + by_class=by_class, resting_places=where), + ) + + +async def _run(app_utils, args): + from robozon_sorter import config as C + from robozon_sorter.sim import plow_sort as PS + from robozon_sorter.sim import plow_vision as PV + from robozon_sorter.sim import staging + from robozon_sorter.sim.mechanics import Cell + from robozon_sorter.sim.spawner import AutoFeeder + + if args.speed: + C.BELT_SPEED = args.speed + + print("=" * 74) + print(f" Robozon sorting cell - {datetime.now():%Y-%m-%d %H:%M}") + print(f" lighting {args.preset}, belt {C.BELT_SPEED} m/s, pitch {args.pitch} m, " + f"vision {'off' if args.no_vision else 'on'}") + print("=" * 74) + + # ---- 1. scene ---------------------------------------------------------- + stage, info = PV.load(script_control=True) + await app_utils.update_app_async(steps=50) + staged = staging.stage_cell(stage, preset=args.preset, floor=not args.no_floor) + PS.keep_lanes_active(stage) + lanes = PS.configure_lanes(stage) + opened = PS.open_junction(stage) + print(f"[scene ] lighting={staged['lighting']['preset']} " + f"floor={'yes' if 'floor' in staged else 'no'} lanes={len(lanes)} " + f"junction shells opened={len(opened)}") + + # ---- 2. item library --------------------------------------------------- + lib = ROOT / "assets" / "items" + if not (lib / "manifest.json").exists(): + lib = C.MESHES + print(f"[items ] assets/items missing - falling back to {lib.name} " + "(run scripts/export_item_library.py for the full catalogue)") + items_meta = PV.load_items(stage, meshes_dir=lib) + classes = {k: v["zone"] for k, v in items_meta.items()} + await app_utils.update_app_async(steps=30) + from collections import Counter + print(f"[items ] {len(classes)} loaded from {lib.name}: {dict(Counter(classes.values()))}") + + cell = Cell(stage, classes.keys()) + cell.park_all() + await app_utils.update_app_async(steps=15) + + # ---- 3. vision --------------------------------------------------------- + vision = None + if not args.no_vision: + from robozon_sorter.cv.pipeline import CreRoiV2b + vision = CreRoiV2b() + vision.attach_cameras() + print(f"[vision] CRE-ROI v2b ready, gate pixels {vision.gate_px}") + + # ---- 4. dispatch order ------------------------------------------------- + if args.items: + order = [n.strip() for n in args.items.split(",") if n.strip() in classes] + else: + order = [n for _ in range(args.repeats) for n in sorted(classes)] + mapping = PS.calibrate_mapping() + expect = {"D": "bin", "B": "container_B", "C": "container_C"} + sorter = PS.PlowSorter(stage, cell, classes, mapping) + print(f"[plan ] {len(order)} dispatches, plow mapping {mapping}, expect {expect}") + + # ---- 5. run ------------------------------------------------------------ + route, records, seen = {}, {}, set() + def rec(name): + return records.setdefault(name + f"#{len([k for k in records if k.startswith(name)])}" + if False else name, dict(item=name, gt=classes[name])) + + log_events = [] + def on_event(kind, name, payload): + log_events.append(dict(kind=kind, item=name, **payload)) + if kind in ("release", "divert", "done"): + print(f" {kind:8s} {name:20s} {payload if payload else ''}") + + feeder = AutoFeeder(cell, order=order, pitch=args.pitch, route=route, + on_event=on_event).install() + + import omni.timeline + timeline = omni.timeline.get_timeline_interface() + app_utils.play(commit=True) + await app_utils.update_app_async(steps=20) + + dt_block, blocks = 15, 0 + prev_angle, prev_t = sorter.plow.angle, time.time() + while blocks < 900 and len(feeder.finished) < len(order): + await app_utils.update_app_async(steps=dt_block) + blocks += 1 + sorter.update(dt_block / 60.0) + + for name in list(feeder.active): + r = records.setdefault(name, dict(item=name, gt=classes[name], pred=None, + dims=None, k=None, cre_ms=None, views=None, + commanded=None, reached=None, rate=None, + outcome=None, delivered=False)) + x = float(cell.pose(name)[0]) + + # vision, once, while the item is under the portal + if name not in seen and abs(x - C.CAM_X) < 0.08: + if vision is not None: + playing = timeline.is_playing() + res = vision.measure() + if playing and not timeline.is_playing(): + timeline.play() # Replicator stops the timeline + await app_utils.update_app_async(steps=2) + r.update(pred=res["cls"], dims=res["dims"], k=res["k"], + views=res["views"], cre_ms=res["cre_ms"]) + else: + r["pred"] = classes[name] + route[name] = r["pred"] + seen.add(name) + mark = "ok " if r["pred"] == r["gt"] else "MISS" + print(f" vision {name:20s} pred={r['pred']} gt={r['gt']} {mark} " + f"dims={r['dims']} K={r['k']}") + + # plow state at the moment the item is level with the blade + if r["commanded"] is None and abs(x - C.PLOW_POS[0]) < 0.25: + now = time.time() + ang = sorter.plow.angle + r["commanded"] = sorter.decided.get(name) + r["reached"] = round(ang, 2) + r["rate"] = round((ang - prev_angle) / max(now - prev_t, 1e-6), 1) + print(f" plow {name:20s} cmd={r['commanded']} reached={r['reached']} " + f"rate={r['rate']} deg/s") + prev_angle, prev_t = sorter.plow.angle, time.time() + + # let the stragglers come to rest before scoring + for _ in range(int(args.settle * 60 / dt_block)): + await app_utils.update_app_async(steps=dt_block) + sorter.update(dt_block / 60.0) + + for name in order: + r = records.setdefault(name, dict(item=name, gt=classes[name], pred=None, + outcome=None, delivered=False)) + p = cell.pose(name) + r["outcome"] = sorter.lane_of(name) + r["final"] = [round(float(v), 3) for v in p] + r["expected"] = expect.get(r["gt"]) + r["delivered"] = (r["outcome"] == r["expected"]) + + app_utils.stop() + await app_utils.update_app_async(steps=15) + feeder.remove() + cell.blade_to(C.BLADE_HOME_Y) + sorter.plow.home() + + # ---- 6. report --------------------------------------------------------- + recs = list(records.values()) + summary = summarise(recs, expect) + print("\n" + "=" * 74) + print(f" dispatched {summary['dispatched']}") + cls_s = summary["classification"] + if cls_s["graded"]: + print(f" classification {cls_s['correct']}/{cls_s['graded']} " + f"= {cls_s['accuracy']:.0%}") + print(f" {'gt\\pred':>8} " + " ".join(f"{p:>5}" for p in ["B", "C", "D", "?"])) + for g, row in cls_s["confusion"].items(): + print(f" {g:>8} " + " ".join(f"{row[p]:>5}" for p in ["B", "C", "D", "?"])) + d = summary["delivery"] + print(f" delivery {d['delivered']}/{summary['dispatched']} = {d['rate']:.0%}") + for cls, row in d["by_class"].items(): + print(f" class {cls} -> {row['target']:<12} " + f"delivered {row['delivered']}/{row['dispatched']}, " + f"classified {row['classified']}/{row['dispatched']}") + print(f" came to rest {d['resting_places']}") + print("=" * 74) + + out = Path(args.out) if args.out else ROOT / "runs" / f"demo_{datetime.now():%Y%m%d_%H%M%S}.json" + out.parent.mkdir(parents=True, exist_ok=True) + out.write_text(json.dumps(dict( + config=dict(preset=args.preset, floor=not args.no_floor, speed=C.BELT_SPEED, + pitch=args.pitch, vision=not args.no_vision, repeats=args.repeats, + mapping=mapping, expect=expect, library=lib.name), + summary=summary, items=recs, events=log_events[-400:]), indent=2)) + print(f" log -> {out}") + return summary + + +def main(argv=None): + args = parse_args(argv) + try: + import omni.usd + inside = omni.usd.get_context().get_stage() is not None + except Exception: + inside = False + + app = None + if not inside: + from isaacsim import SimulationApp + app = SimulationApp({"headless": args.headless, "width": 1600, "height": 900}) + + 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, args)) + finally: + if app is not None: + app.close() + + +if __name__ == "__main__": + sys.exit(0 if main() else 1) diff --git a/scripts/run_flow.py b/scripts/run_flow.py new file mode 100644 index 0000000..5f74ea4 --- /dev/null +++ b/scripts/run_flow.py @@ -0,0 +1,260 @@ +"""Поток товаров с шагом 700 мм: лазерная завеса определяет класс, пушер берёт класс D. + +Луч теперь ДАТЧИК, а не преграда - коллизия снята прямо в файле. Раньше он перекрывал всю +ширину полотна на 19 мм над лентой, и товар вставал на x = -3.044, не доходя до пушера. + +КАК ДАТЧИК ОПРЕДЕЛЯЕТ КРУГОВОЕ СЕЧЕНИЕ. Одиночный луч даёт только факт прохода. Завеса +из 181 луча поперёк ленты меряет ШИРИНУ товара, а по мере его проезда набирается профиль +ширины вдоль хода. У коробки он прямоугольный - ширина постоянна почти до конца; у тела +кругового сечения он дугообразный, ширина плавно нарастает и спадает. + +Различаются они отношением средней ширины к наибольшей. Для прямоугольника оно стремится +к 1.0, для круга даёт площадь полукруга к описанному прямоугольнику, то есть pi/4 = 0.785. +Порог 0.90 разделяет их с запасом и не требует ни камеры, ни обучения - только геометрия. + +Спавн идёт по времени, а не расстановкой заранее: при 1 м/с шаг 700 мм это ровно 0.70 с +между выпусками. Точка выпуска x = 1.90, а не C.SPAWN_X = 2.30 - в этой сборке подающая +секция ConveyorTrack_05 кончается на x = 2.001, и 2.30 висит в воздухе. +""" +import sys, math +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim +from omni.physx import get_physx_scene_query_interface + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.plow import Plow + +SPEED = 1.0 +PITCH = 0.70 # м между товарами +RELEASE_X = 1.90 +SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd" +GATE_X = -3.20 # где стоит луч +RAYS, Y0, Y1 = 31, -0.45, 0.45 +CURTAIN_H = 0.40 +ROUND_T = 0.90 # средняя/наибольшая ширина: ниже - круглое сечение + +# что пускаем: цилиндры - класс D (круговое сечение), коробки - не D +PLAN = [("D_cyl_1", "cyl"), ("box_1", "box"), ("D_cyl_2", "cyl"), ("box_2", "box"), + ("D_cyl_3", "cyl"), ("box_3", "box"), ("D_cyl_4", "cyl"), ("box_4", "box")] + +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) +omni.usd.get_context().open_stage(SCENE) +await app_utils.update_app_async(steps=60) +stage = omni.usd.get_context().get_stage() + +killed = [p.GetPath() for p in stage.Traverse() + if "ConveyorBeltGraph" in p.GetName() or "DiverterAnimGraph" in p.GetName()] +for path in killed: + stage.RemovePrim(path) +print(f"удалено узлов графов: {len(killed)} {[str(k).split(chr(47))[-1] for k in killed]}") +await app_utils.update_app_async(steps=10) +info = plow_cell.prepare(stage, belt_speed=SPEED, script_control=True, kinematic_arm=True) +for path, intent in (("/World/ConveyorTrack_05/Belt", (-1, 0, 0)), + ("/World/ConveyorTrack_06/Belt", (0, 1, 0))): + pr = stage.GetPrimAtPath(path) + if pr.IsValid(): + plow_cell.drive_belt(stage, path, intent, SPEED) +for path in list(plow_cell.BELTS) + [plow_cell.BRANCH, + "/World/ConveyorTrack_05/Belt", "/World/ConveyorTrack_06/Belt"]: + pr = stage.GetPrimAtPath(path) + if pr.IsValid(): + PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True) + +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_05/Belt") + ).ComputeAlignedRange().GetMax()[2] +GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL) +print(f"сцена готова: лент {len(info['belts'])}+2, скорость {SPEED} м/с, " + f"верх ленты z={TOP:.3f}, луч на x={GATE_X}") + +ROOT = "/World/_Flow" +if stage.GetPrimAtPath(ROOT).IsValid(): + stage.RemovePrim(ROOT) +stage.DefinePrim(ROOT, "Xform") + +def make(name, kind): + path = f"{ROOT}/{name}" + if kind == "cyl": + g = UsdGeom.Cylinder.Define(stage, path) + g.CreateRadiusAttr().Set(0.045); g.CreateHeightAttr().Set(0.10) + g.CreateAxisAttr().Set("Z") + half = 0.05 + else: + g = UsdGeom.Cube.Define(stage, path); g.CreateSizeAttr().Set(2.0) + half = 0.045 + xf = UsdGeom.Xformable(g.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d(RELEASE_X, 0.0, TOP + half + 0.006)) + if kind == "box": + xf.AddScaleOp().Set(Gf.Vec3f(0.045, 0.045, 0.045)) + p = g.GetPrim() + UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p) + UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(0.4) + rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p) + rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32) + if GRIP.IsValid(): + UsdShade.MaterialBindingAPI.Apply(p).Bind( + UsdShade.Material(GRIP), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + return path + +query = get_physx_scene_query_interface() + +# Положения тел ЧИТАЮТСЯ ЧЕРЕЗ RigidPrim, а не через BBoxCache. BBoxCache берёт авторские +# трансформы из слоя USD, а физика пишет состояние в Fabric - во время прогона эти два +# источника расходятся, и замер по BBoxCache показывает позы, которых на экране нет. +# Именно из-за этого предыдущий прогон отчитался, что все восемь товаров стоят ровно в +# точках выпуска, хотя таймлайн отработал полные 26 секунд. +_views = {} +def pos_of(path): + v = _views.get(path) + if v is None: + v = RigidPrim(paths=[path]); _views[path] = v + q = v.get_world_poses()[0].numpy()[0] + return float(q[0]), float(q[1]), float(q[2]) + +def curtain_width(): + """ширина того, что сейчас под завесой, в мм. Луч, попавший выше полотна, - товар.""" + z0 = TOP + CURTAIN_H + hit_y = [] + for i in range(RAYS): + y = Y0 + (Y1 - Y0) * i / (RAYS - 1) + h = query.raycast_closest((GATE_X, y, z0), (0.0, 0.0, -1.0), CURTAIN_H - 0.001) + if h and h.get("hit"): + zh = z0 - h["distance"] + if zh > TOP + 0.006: # выше полотна на 6 мм - значит товар + hit_y.append(y) + if not hit_y: + return 0.0 + return (max(hit_y) - min(hit_y)) * 1000.0 + (Y1 - Y0) / (RAYS - 1) * 1000.0 + +# Таймлайн сцены кончается на своём endTimeCode и останавливается сам. В прошлом прогоне +# из-за этого прошло только 5.1 с вместо 26: головной товар едва дошёл до створа, и +# завеса не успела ничего измерить. Продлеваем ленту времени под длительность опыта. +fps = stage.GetTimeCodesPerSecond() or 24.0 +print(f"таймлайн: {stage.GetStartTimeCode()}..{stage.GetEndTimeCode()} кадров при {fps} к/с " + f"= {(stage.GetEndTimeCode()-stage.GetStartTimeCode())/fps:.1f} с - продлеваем") +stage.SetEndTimeCode(stage.GetStartTimeCode() + fps * 90.0) +tl.set_end_time(float(stage.GetEndTimeCode()) / fps) +tl.set_looping(False) + +cell = Cell(stage, items={}) +plow = Plow(stage); plow.target(C.PLOW_PRESET["D"]) + +tl.play() +await app_utils.update_app_async(steps=20) +t0 = float(tl.get_current_time()) +released, profiles, done, order = [], {}, {}, [] +next_release = 0.0 +pusher_busy_until = -1.0 +print(f"\nвыпуск каждые {PITCH/SPEED:.2f} с (шаг {PITCH*1000:.0f} мм при {SPEED} м/с)") +print("\n событие") +print(" " + "-" * 76) + +T_END = 26.0 +import time as _wall +_it, _w0 = 0, _wall.time() +while float(tl.get_current_time()) - t0 < T_END: + t = float(tl.get_current_time()) - t0 + _it += 1 + if _it % 40 == 0: + print(f" [цикл] итерация {_it}: сим t={t:5.2f}s, стена {_wall.time()-_w0:5.1f}s, " + f"играет={tl.is_playing()}, товаров={len(released)}") + if not tl.is_playing(): + # play() после stop() перематывает в начало и сбрасывает физику: в прошлом прогоне + # это возвращало все товары в точки выпуска и обесценивало весь замер. + print(f" [цикл] ТАЙМЛАЙН ОСТАНОВИЛСЯ САМ на t={t:.2f}s - прерываю, " + f"перезапуск обнулил бы опыт") + break + # выпуск потока + if len(released) < len(PLAN) and t >= next_release: + name, kind = PLAN[len(released)] + path = make(name, kind) + released.append((name, kind, path)) + order.append(name) + next_release += PITCH / SPEED + print(f" {t:5.2f}s выпущен {name} ({'цилиндр' if kind=='cyl' else 'коробка'})") + # завеса: набрать профиль ширины для того, кто сейчас в створе + w = curtain_width() + if w > 5.0: + # чей это профиль - ближайший по x к воротам + best, bd = None, 1e9 + for name, kind, path in released: + if not stage.GetPrimAtPath(path).IsValid(): + continue + d = abs(pos_of(path)[0] - GATE_X) + if d < bd: + best, bd = name, d + if best is not None and bd < 0.20: + profiles.setdefault(best, []).append(w) + # решение по классу, когда товар вышел из створа + for name, kind, path in released: + if name in done or name not in profiles: + continue + if not stage.GetPrimAtPath(path).IsValid(): + continue + x = pos_of(path)[0] + if x < GATE_X - 0.09 and len(profiles[name]) >= 3: + prof = profiles[name] + ratio = (sum(prof) / len(prof)) / max(prof) + cls = "D" if ratio < ROUND_T else "B/C" + done[name] = dict(gt=("D" if kind == "cyl" else "B/C"), pred=cls, + ratio=round(ratio, 3), wmax=round(max(prof)), + n=len(prof), pushed=False) + print(f" {t:5.2f}s завеса: {name} ширина макс {max(prof):.0f} мм, " + f"проб {len(prof)}, ср/макс {ratio:.3f} -> класс {cls}") + # пушер: взять класс D, когда он дошёл до ножа + if t > pusher_busy_until: + for name, kind, path in released: + d = done.get(name) + if not d or d["pred"] != "D" or d["pushed"]: + continue + if not stage.GetPrimAtPath(path).IsValid(): + continue + x = pos_of(path)[0] + if x <= C.PUSH_X + 0.06: + print(f" {t:5.2f}s ПУШЕР берёт {name} на x={x:+.2f}") + await cell.stroke(app_utils, out=True, speed=1.2) + await cell.stroke(app_utils, out=False, speed=1.5) + d["pushed"] = True + pusher_busy_until = float(tl.get_current_time()) - t0 + 0.15 + break + await app_utils.update_app_async(steps=2) + +# итог +print("\n ИТОГ") +print(f" {'товар':10s} {'истина':7s} {'датчик':7s} {'ср/макс':>8s} {'шир,мм':>7s} " + f"{'пушер':>6s} {'конец X,Y':>16s} где") +print(" " + "-" * 84) +okc = okp = 0 +for name, kind, path in released: + d = done.get(name, dict(gt=("D" if kind == "cyl" else "B/C"), pred="-", ratio=0, + wmax=0, pushed=False)) + if stage.GetPrimAtPath(path).IsValid(): + x, y, z = pos_of(path) + else: + x = y = z = float("nan") + where = ("ВЕТКА пушера" if y > 0.50 else + "упал" if z < TOP - 0.20 else + "+Y (угол)" if y > 0.10 else + "-Y" if y < -0.10 else "прямо") + if d["pred"] == d["gt"]: + okc += 1 + if (d["gt"] == "D") == bool(d["pushed"]): + okp += 1 + print(f" {name:10s} {d['gt']:7s} {d['pred']:7s} {d['ratio']:8.3f} {d['wmax']:7.0f} " + f"{'да' if d['pushed'] else 'нет':>6s} ({x:+6.2f},{y:+6.2f}) {where}") +print(f"\n цикл: {_it} итераций, сим {float(tl.get_current_time())-t0:.2f}s, " + f"стена {_wall.time()-_w0:.0f}s, играет={tl.is_playing()}") +tl.stop(); await app_utils.update_app_async(steps=5) +print(f"\n класс определён верно: {okc}/{len(released)} " + f"пушер сработал по назначению: {okp}/{len(released)}") diff --git a/scripts/run_plow_9045_known_classes.py b/scripts/run_plow_9045_known_classes.py new file mode 100644 index 0000000..af7a102 --- /dev/null +++ b/scripts/run_plow_9045_known_classes.py @@ -0,0 +1,732 @@ +"""Full-line test of scene/plow_cell_90_45_test.usd with known (pre-assigned) classes: +a laser curtain on ConveyorTrack_04 reads each item's pre-known class and shifts the plow +right (-16 deg) for B - so it slides along the blade onto ConveyorTrack_06 into container +B - and left (+16 deg) for C - so it nudges onto ConveyorTrack_01 into container C. A +second curtain further upstream (x=-3.2, same spot the pusher already uses) intercepts +class D for the pusher's own bin, unchanged from the already-verified pipeline. + +Class ground truth is NOT taken from the catalogue's `zone` fields - categories.json and +manifest.json disagree with each other and with their own roundness numbers in several +places (pouf is zone C in both yet k_round=0.994, i.e. round => class D; pen is C in one +file and D in the other). Classes are asserted explicitly in ITEMS below, with the reason. + +Run inside the live Isaac Sim through the code editor's python server: + python isaacsim_send.py --context plow9045 --file scripts/run_plow_9045_known_classes.py +""" +import asyncio +import sys +import time + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import numpy as np +import omni.usd +import omni.timeline +import omni.kit.viewport.utility as vp +import isaacsim.core.experimental.utils.app as app_utils +from omni.physx import get_physx_interface, get_physx_scene_query_interface + +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell_9045, scene as _scene +from robozon_sorter.sim.plow import Plow + +# Strict B/C/B/C alternation - the worst case for the blade, a full reversal every 0.7 s. +# +# The catalogue's own `zone` fields are NOT trustworthy and are not used to pick these: +# * pouf is zone C in BOTH categories.json and manifest.json, but k_round = 0.994 - +# it is round, so it is class **D** and belongs to the pusher, not the plow. Removed. +# * pen is zone C in categories.json and zone D in manifest.json, and k_round = 0.842 +# is over the 0.82 roundness threshold - genuinely ambiguous, so it is not used to +# measure the plow either. (It is also 13x9 mm, thin enough to slip under a blade.) +# The C slots below are items that are oversize by DIMENSION and clearly not round: +# backpack 455x370x301 (k 0.82) and pillow 455x431x213 (k 0.905 but flat, not a solid of +# revolution). B slots are unambiguous: lunchbox k 0.646, detergent k 0.742. +# +# box_300x200x200 / box_400x400x300 are also left out: the kinematics log measured them +# dwelling 5.97 s and 54.61 s in the plow zone (vs ~1.4 s for everything else), and while +# the blade is held by one stuck item every item behind it is starved of its own angle - +# that measures the stall, not the swing. +# FULL D/C/B run: 9 items, three of each class, repeating D -> C -> B so every consecutive +# pair is a different class (the hardest ordering for a single blade + single pusher). +# Classes asserted from the physical criteria, not the catalogue's `zone` fields: +# D = round (k_round above the 0.82 operating threshold) -> pusher -> BinD +# C = oversize by dimension, not round -> plow +20 -> container_C +# B = fits the envelope, not round -> plow -20 -> container_B +ITEMS = [ + ("bag", "D"), # 202x175x170 k 0.896 round + ("backpack", "C"), # 455x370x301 k 0.82 oversize + ("lunchbox", "B"), # 201x152x62 k 0.646 + ("helmet", "D"), # 354x297x280 k 0.895 round + ("pillow", "C"), # 455x431x213 k 0.905 oversize (flat, not a solid of rev.) + ("detergent", "B"), # 278x260x180 k 0.742 + ("bucket", "D"), # 287x287x272 k 0.995 round + ("box_400x400x300", "C"), # 401x400x301 k 0.716 oversize + ("box_300x200x200", "B"), # 301x200x200 k 0.72 +] +CLASSES = dict(ITEMS) +ORDER = [n for n, _ in ITEMS] +# 700 mm is the spec. It is also SHORTER than the deflection zone an item occupies +# (T_zone*speed = 0.95 m), so two opposite-class items are inside the plow at once and +# one blade cannot give both their own angle - injectable here to test that directly. +PITCH = float(globals().get("pitch", 0.70)) # metres between items at SPEED +SPEED = 1.0 # m/s + +# The blade itself occupies x -7.95..-7.32 (measured). The sensor has to sit far enough +# UPSTREAM (+X) of -7.32 that a full B<->C reversal completes before the item touches it. +# -7.20 (tried last round) was a mistake born of reading C.PLOW_SWEEP_X0=-7.15 as "the +# blade": that constant is the upstream sweep WINDOW, not the blade body, so the sensor +# ended up 0.12 m = 0.12 s ahead of the blade while a reversal needs ~0.175 s. The blade +# provably could not arrive in time - the kinematics log showed served=NO / held +0 for +# every single item that run. Keep >= ~1 m of lead. +PLOW_SENSE_X = float(globals().get("plow_sense_x", -6.30)) # ~1.02 m / 1.02 s of lead +PUSH_SENSE_X = C.PUSH_X + plow_cell_9045.PUSHER_X_MM / 2000.0 # the blade's own upstream +# edge (half its 500 mm width ahead of centre), not a separate gate 700 mm further back - +# detection and the stroke firing are now the same event, no lag for the belt to eat. + +# ---- derive PLOW_RATE / PLOW_ANGLE from the 1 m/s + 700 mm spec, instead of guessing ---- +# T_pitch: time between two items at any fixed point. +# T_lead : sensor-to-pivot warning time (plenty - the blade only needs a fraction of it). +# T_zone : how long ONE item spends inside the active deflection zone (SWEEP_X0 to +# RELEASE_X) - the real constraint, because a second item enters this zone +# before the first clears it whenever T_zone > T_pitch: with a single blade, +# two back-to-back opposite-class items then CANNOT both get a clean, +# uninterrupted deflection window - there is an unavoidable overlap, independent +# of how fast the blade turns. Sizing the blade speed only controls how much of +# that overlap is wasted on the swing itself. +# Injectable so the angle/rate can be swept without editing the file: +# isaacsim_send.py --args-json '{"plow_angle": 28, "swing_margin": 0.25}' +PLOW_ANGLE = float(globals().get("plow_angle", 20.0)) # inside PLOW_LIMIT=45 +T_PITCH = PITCH / SPEED +BLADE_LEADING_X = -7.32 # measured upstream face of the plow blade body +BLADE_TRAILING_X = -7.95 # measured downstream face +T_LEAD = abs(PLOW_SENSE_X - BLADE_LEADING_X) / SPEED # to the BLADE, not the pivot +T_ZONE = abs(C.PLOW_RELEASE_X - C.PLOW_SWEEP_X0) / SPEED +SWING_MARGIN = float(globals().get("swing_margin", 0.25)) # fraction of T_pitch allotted + # to the swing itself; smaller => faster commanded blade +PLOW_RATE = (2.0 * PLOW_ANGLE) / (SWING_MARGIN * T_PITCH) # worst case: full reversal +# The return-to-centre leg had been sharing PLOW_RATE with the deflection swing - fine for +# steering an item (where too fast caused overshoot: RATE=600 measured 0/3 on class C), +# but a SLOW return with nothing to steer just leaves a residual angle live when the next +# item arrives - measured misrouting B->C traffic that should have seen a clean 0. There is +# no overshoot risk on an empty return (nothing is being deflected), so it can run flat out: +# 3x PLOW_RATE reaches home well inside the same 0.5*T_pitch budget with margin to spare. +PLOW_RETURN_RATE = 3.0 * PLOW_RATE +PLOW_ANGLES = {"B": -PLOW_ANGLE, "C": PLOW_ANGLE, "D": 0.0} +# Force-release timeout. 3*T_zone (2.85 s) measured TOO SHORT: items dwell 4.5-53 s in +# the zone, so the blade released its angle long before the item actually reached the +# blade body, and the item passed a neutral (0 deg) blade - which sends it +Y by +# default, because ConveyorTrack_06 (y 0.025..1.048, driving +Y) claims anything at +# y>0 at the end of Track_04. Every class-C miss this run is that: served=NO, held +0. +PLOW_HOLD_MAX = float(globals().get("plow_hold_max", 3.0 * T_ZONE)) +PLOW_X_LOG_HI = PLOW_SENSE_X + 0.20 # log window: a little before the sensor... +PLOW_X_LOG_LO = C.PLOW_RELEASE_X - 0.20 # ...to a little past release + +print(f"\n===== PLOW TIMING (1 m/s, {PITCH*1000:.0f} mm pitch) =====") +print(f" T_pitch (item spacing) = {T_PITCH:.3f} s") +print(f" T_lead (sensor -> blade) = {T_LEAD:.3f} s") +T_SWING_FULL = (2.0 * PLOW_ANGLE) / PLOW_RATE if PLOW_RATE else 0.0 +print(f" T_swing (full B<->C reversal)= {T_SWING_FULL:.3f} s" + + (" OK - blade arrives in time" if T_SWING_FULL < T_LEAD + else " TOO SLOW - blade cannot arrive before the item does")) +print(f" T_zone (in deflection zone)= {T_ZONE:.3f} s") +if T_ZONE > T_PITCH: + print(f" T_zone > T_pitch by {T_ZONE - T_PITCH:.3f} s: back-to-back opposite-class " + f"items WILL overlap in the zone - this is geometry, not a rate problem.") +print(f" PLOW_RATE = 2*{PLOW_ANGLE:.0f} / ({SWING_MARGIN}*{T_PITCH:.3f}) = {PLOW_RATE:.0f} deg/s " + f"(config default {C.PLOW_SWEEP_RATE:.0f})") +print(f" PLOW_RETURN_RATE = 3x PLOW_RATE = {PLOW_RETURN_RATE:.0f} deg/s (no overshoot risk " + f"on an empty return, so it does not need the deflection swing's slower budget)") +print(f" PUSH_SENSE_X = PUSH_X + blade_halfwidth = {PUSH_SENSE_X:.3f} (blade's own edge)") + +# C.PUSHER_MAX_SAFE (2.5 m/s) is a ceiling against throwing goods off the line, not a +# measured-good speed - isolated single-item tests (pusher_diag*.py) found it FLICKS the +# item (a brief velocity spike, then the blade outruns it: item ends up only 0.01-0.05 m +# over against a 0.42 m commanded stroke). 0.6 m/s is too slow the other way - the item's +# own belt-driven X motion carries it clean out of the blade's X window before the stroke +# finishes. 1.3 m/s hit 0.407/0.42 m (97%) in the same isolated test - a real carry. +# Contact-window arithmetic, measured not guessed. The blade spans 500 mm of belt, so at +# 1 m/s an item is in front of it for only 0.50 s. The old 1.3 m/s over a 0.85 m stroke +# takes 0.654 s: the pusher log showed the item entering at x=-3.83 and leaving at x=-4.42, +# i.e. off the blade's trailing edge (-4.15) after ~0.33 s - barely half the stroke, giving +# dy of only +0.17..+0.22 m against the ~0.5 m needed to reach the branch belt. Waiting for +# the item to reach PUSH_X+0.08 first burned another 0.18 m of that window, so the stroke +# now fires the instant the curtain sees the item. +# stroke = BLADE_HOME_Y..PUSH_OUT_Y = 0.30 + 0.52 = 0.82 m +# at 1.8 m/s that is 0.456 s < the 0.50 s window, with ~0.04 s of margin. +PUSH_SPEED = 1.3 # best measured momentum transfer; the blade is sized for it above +# The return leg carries nothing, so it does not need the carry speed: measured +# 0.683 s at 1.3 m/s vs 0.367 s at 2.5 m/s over the same 0.85 m stroke. Getting the +# blade home sooner is what lets a following D item be served at all. +PUSH_RETURN_SPEED = C.PUSHER_MAX_SAFE # 2.5 m/s +PUSH_OUT_Y = 0.52 # C.BLADE_OUT_Y (0.42) stops short of the branch belt's own start + # (y=0.443, measured); 0.52 clears it with margin while keeping the + # stroke short enough to finish inside the contact window above. +SENSE_Y0, SENSE_Y1 = -0.45, 0.45 +SENSE_RAYS = 121 +GATE_WINDOW = 0.15 + +CONTAINER_B = (-8.81, 1.47) +CONTAINER_C = (-10.45, -0.225) +CONTAINER_R = 0.55 +CONTAINER_Z = 1.30 # floor 1.14-1.18; anything below this is resting in the tray + +# real BinD geometry (/World/SortingRig/BinD_*), measured directly on this scene - +# config.BIN_X0/X1/Y0/Y1 are the OLD sorter.usd's bin and do not apply here, same mistake +# as SPAWN_X/BELTS earlier: every "shared" config constant needs re-verifying per scene. +BIN_X0, BIN_X1 = -6.21, -4.95 +BIN_Y0, BIN_Y1 = 1.57, 2.86 +BIN_LIP_Z = 1.72 + +# ---------------------------------------------------------------- scene +# NOT plow_cell_9045.load(): reopening this stage while the WebRTC stream is attached +# races the background Hydra-populate thread and reliably throws 'Detected usd threading +# violation' (measured over ~10 attempts here). The stage is already the right one +# (confirmed via health_check) - prepare it in place instead. +stage = omni.usd.get_context().get_stage() +print("current stage:", stage.GetRootLayer().identifier) +info = await plow_cell_9045.prepare(stage, belt_speed=SPEED, script_control=True) +print(f"prepare: {info}") + +def _load_items(stage, names): + """define each item, fully physics-ready (RigidBodyAPI, mass, CCD, collision), + BEFORE the timeline ever plays - and NEVER change that API afterward. + + Two things measured broken in THIS session when tried during an already-playing + simulation: (1) flipping kinematicEnabled True->False mid-play - the item's authored + translate op and kinematic flag both "write" successfully (no exception) but the body + never actually moves, forever kinematic in the solver's own copy of the actor; (2) + applying UsdPhysics.RigidBodyAPI.Apply() fresh mid-play - same silent no-op. A plain + xformOp:translate WRITE on a body that already has its RigidBodyAPI from before Play + started, in contrast, is the pattern used successfully everywhere else in this + project (mechanics.Cell.place/blade_to, the plow's own kinematic rotateZ) - so items + get their physics now, sit on the new ground plane at their park slot, and are only + ever teleported (never re-tagged) at release time. + + Each spawn is its own try/except: this Kit session raises even benign Tf warnings as + exceptions (e.g. 'sneaker' fails on a float3-vs-double xformOp precision mismatch that + Tf itself says it is proceeding past), so one bad mesh must not take the other ten down. + """ + items_dir = C.ROOT / "assets" / "items" + UsdGeom.Xform.Define(stage, "/World/Items") + ok = [] + for i, name in enumerate(names): + usd = items_dir / f"{name}.usd" + try: + prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() + prim.GetReferences().ClearReferences() + prim.GetReferences().AddReference(str(usd)) + xf = UsdGeom.Xformable(prim) + xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set( + Gf.Vec3d(9.0 + 1.2 * i, 5.0, 0.4)) + UsdPhysics.RigidBodyAPI.Apply(prim) + # meshes exported from a streaming scene arrive kinematic (scene.py's own + # load_test_items() docstring says so) - the referenced .usd itself authors + # kinematicEnabled=True, so it must be forced False here explicitly, ONCE, + # before Play. This is what was actually silently pinning every item in place + # this whole time - not a mid-play toggle race, a stale authored default this + # code never overrode. + 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.CreateSolverVelocityIterationCountAttr().Set(8) + px.CreateSleepThresholdAttr().Set(0.0) # a settled item must still be draggable + # bottle (tall, narrow, round) has been measured disappearing into the belt - + # a contact-resolution/CCD tunnel, same failure mode plow_cell.py caps on the + # plow arm with C.MAX_DEPENETRATION: an uncapped deep-penetration event lets + # PhysX separate the overlap at whatever speed it likes, which can eject a + # thin body clean through a thin collider in a single step. + px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) + for desc in Usd.PrimRange(prim): + if desc.HasAPI(UsdPhysics.CollisionAPI): + pxcol = PhysxSchema.PhysxCollisionAPI.Apply(desc) + pxcol.CreateContactOffsetAttr().Set(0.004) # tighter than the ~5cm + pxcol.CreateRestOffsetAttr().Set(0.001) # PhysX default for small items + UsdGeom.Imageable(prim).MakeInvisible() # shown at release, not before + ok.append(name) + except BaseException as exc: + print(f" WARNING: failed to load item {name!r}: {type(exc).__name__}") + return ok + + +loaded = _load_items(stage, ORDER) +print(f"items loaded: {loaded}") +ORDER = loaded # downstream code (spawn loop, report) only sees what actually loaded +rp = {n: RigidPrim(paths=[f"/World/Items/{n}"]) for n in ORDER} # built now, physics is already live +await app_utils.update_app_async(steps=20) + +plow = Plow(stage, kinematic=True) +plow.home() + +query = get_physx_scene_query_interface() + + +def _activate_item(name, x, y): + """teleport + reveal an already-physics-ready item - see _load_items for why nothing + else may change here once the timeline is playing.""" + prim = stage.GetPrimAtPath(f"/World/Items/{name}") + for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + op.Set(Gf.Vec3d(x, y, C.BELT_Z + 0.05)) + break + UsdGeom.Imageable(prim).MakeVisible() + + +_last_pose = {} # name -> last successfully read pose, for when the tensor backend hiccups + + +def item_pose(name): + """`get_world_poses()` can raise 'Failed to get rigid body transforms from backend' + if PhysX's tensor view for this actor is momentarily invalid (measured after a hard + contact from the plow/pusher) - fall back to the last good read rather than crash the + whole run over one body's one bad tick.""" + try: + p = rp[name].get_world_poses()[0].numpy()[0] + _last_pose[name] = p + return p + except BaseException: + if name in _last_pose: + return _last_pose[name] + raise + + +# -- the pusher blade, driven directly (mechanics.Cell.blade_to/stroke, inlined - the +# rest of Cell assumes the park/thaw item pattern this script deliberately does not use) +def _blade_op(stage): + prim = stage.GetPrimAtPath(_scene.BLADE) + for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + return op + raise RuntimeError(f"{_scene.BLADE} has no translate op") + + +blade_op = _blade_op(stage) +blade_base = blade_op.Get() + + +def blade_to(y): + b = blade_base + blade_op.Set(Gf.Vec3d(b[0], y - _scene.BLADE_PARENT_Y, b[2])) + + +blade_to(C.BLADE_HOME_Y) + + +async def stroke(out=True, speed=None): + """pace the blade by REAL elapsed sim time (tl.get_current_time()), not an assumed + dt=1/60 - this scene's actual physics step has measured well under 60 Hz elsewhere in + this project (verify_belts2.py found 83.33 ms, not 16.67 ms). Assuming 60 Hz here made + each `update_app_async(steps=1)` cover several times the intended distance, so the + blade arrived in a handful of big jumps instead of a smooth sweep - PROBE_PUSH_PHYSICS's + own distinction between a genuine push (item picks up the blade's tangential speed) and + a teleport (item gets a small depenetration nudge and stops dead): the user's own + 'item stops in place on contact' report is exactly the teleport symptom.""" + speed = min(speed or C.PUSHER_SPEED, C.PUSHER_MAX_SAFE) + # C.BLADE_OUT_Y (0.42) lands 20 mm SHORT of where the branch belt (Belt_01) actually + # starts (y=0.44, measured) - close enough that a pushed item straddles the boundary + # and the main belt's -X drive keeps winning over the branch's +Y pull. PUSH_OUT_Y + # gives real margin onto the branch instead of leaving it to a coin flip. + a, b = (C.BLADE_HOME_Y, PUSH_OUT_Y) if out else (PUSH_OUT_Y, C.BLADE_HOME_Y) + duration = abs(b - a) / max(speed, 1e-6) + t0 = float(tl.get_current_time()) + while True: + u = min(1.0, (float(tl.get_current_time()) - t0) / max(duration, 1e-6)) + blade_to(a + (b - a) * u) + await app_utils.update_app_async(steps=1) + if u >= 1.0: + break + + +def _curtain(x, exclude): + near = any(abs(float(item_pose(n)[0]) - x) < GATE_WINDOW for n in ORDER + if n not in exclude and n in rp) + if not near: + return None + z0 = C.BELT_Z + 0.40 + reach = 0.40 - 0.001 + for i in range(SENSE_RAYS): + y = SENSE_Y0 + (SENSE_Y1 - SENSE_Y0) * i / (SENSE_RAYS - 1) + hit = query.raycast_closest([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 n in ORDER: + if n in exclude: + continue + if f"/World/Items/{n}" in path: + return n + return None + + +gate_log = [] +push_swept = set() +plow_swept = set() +plow_pending = {} +plow_active = None # (name, angle, commit_sim_t) the blade is currently committed to +pushing = set() +push_queue = [] # D items waiting for the blade to finish the item ahead of them +push_log = [] # what the pusher actually did to each D item +plow_trace = {n: [] for n in ORDER} # per-item kinematics while inside the sense-to-release +sim_t = [0.0] # boxed so _step (no `global` needed) can advance it + + +# ---------------------------------------------------------------- pusher state machine +# Driven from the PHYSICS CALLBACK, exactly like the plow - not from an async coroutine. +# That was the whole problem: `_do_push` used to `await update_app_async()` inside a task +# fired by asyncio.ensure_future, while the main feed loop pumped the app too. With two +# tasks pumping, the sim advanced further between consecutive blade_to() writes than the +# stroke maths assumed, so the blade jumped in bigger steps - the teleport regime again. +# Isolated (single pumper) the same blade+speed reached dy=+1.62; inside the full run it +# managed +0.21. Advancing the blade by PUSH_SPEED*dt once per physics step removes the +# dependency on who else is pumping. +PUSH_HOLD_S = 0.15 # dwell at full extension before returning + +push_state = {"phase": "idle", "item": None, "y": C.BLADE_HOME_Y, "t": 0.0, + "y0": 0.0, "x0": 0.0} + + +def _push_begin(name): + push_state.update(phase="out", item=name, t=0.0, + y0=float(item_pose(name)[1]), x0=float(item_pose(name)[0])) + pushing.add(name) + + +def _push_step(dt): + """advance the blade one physics step; returns nothing""" + st = push_state + if st["phase"] == "idle": + return + name = st["item"] + + if st["phase"] == "out": + st["y"] = min(PUSH_OUT_Y, st["y"] + PUSH_SPEED * dt) + blade_to(st["y"]) + # Carry assist - the "impulse". A transform-driven kinematic blade transfers no + # momentum of its own (PhysX sees a teleport, so the item gets only a + # depenetration shove), which is why the bare blade plateaued at ~0.21 m. Rather + # than one violent kick, the item's +Y velocity is matched to the blade's every + # step while the blade is advancing: that is what a real carrying push does, and + # it measured dy 1.62 -> 1.92 in isolation. X and Z are left alone so the belt + # keeps driving it down the line normally. + if name in rp: + try: + lin = rp[name].get_velocities()[0].numpy()[0] + rp[name].set_velocities( + np.array([[float(lin[0]), PUSH_SPEED, float(lin[2])]]), + np.array([[0.0, 0.0, 0.0]])) + except BaseException: + pass + if st["y"] >= PUSH_OUT_Y - 1e-6: + st["phase"], st["t"] = "hold", 0.0 + if name in rp: + p = item_pose(name) + push_log.append(dict(item=name, start_x=round(st["x0"], 3), + start_y=round(st["y0"], 3), + after_x=round(float(p[0]), 3), + after_y=round(float(p[1]), 3), + after_z=round(float(p[2]), 3), + dy=round(float(p[1]) - st["y0"], 3))) + + elif st["phase"] == "hold": + st["t"] += dt + if st["t"] >= PUSH_HOLD_S: + st["phase"] = "back" + + elif st["phase"] == "back": + st["y"] = max(C.BLADE_HOME_Y, st["y"] - PUSH_RETURN_SPEED * dt) + blade_to(st["y"]) + if st["y"] <= C.BLADE_HOME_Y + 1e-6: + pushing.discard(name) + st.update(phase="idle", item=None) + if push_queue: + _push_begin(push_queue.pop(0)) + + +def _step(dt): + global plow_active + sim_t[0] += dt + try: + # kinematics trace: every item still between the plow sensor and the release + # point, every tick - what the plow tuning needs to actually be corrected from, + # rather than re-guessed. Cheap: only items in this ~1.7 m window are sampled. + for n in ORDER: + if n not in rp: + continue + p = item_pose(n) + x = float(p[0]) + if PLOW_X_LOG_HI >= x >= PLOW_X_LOG_LO: + plow_trace[n].append((round(sim_t[0], 4), round(x, 4), round(float(p[1]), 4), + round(plow.commanded, 2), round(plow.angle, 2), + n == (plow_active[0] if plow_active else None))) + + seen = _curtain(PUSH_SENSE_X, push_swept) + if seen is not None: + push_swept.add(seen) + gate_log.append(("push", seen, CLASSES[seen])) + if CLASSES[seen] == "D": + if push_state["phase"] != "idle": + push_queue.append(seen) + else: + _push_begin(seen) + + seen = _curtain(PLOW_SENSE_X, plow_swept) + if seen is not None: + plow_swept.add(seen) + ang = float(PLOW_ANGLES.get(CLASSES[seen], 0.0)) + gate_log.append(("plow", seen, CLASSES[seen], ang)) + if abs(ang) > 1e-6: + plow_pending[seen] = ang + + # Once the blade commits to an item, hold that angle until the item clears the + # release point - a newer arrival with the opposite angle must NOT reassign the + # target while the current item is still physically sliding along the blade, or + # the blade reverses mid-deflection and both items end up misrouted (measured: + # box_400x400x300 wanted +20, got dragged to container_B instead of C - a B item + # 0.7 s ahead of it in the queue). + # + # PLOW_HOLD_MAX is a force-release timeout on top of the position check. The + # kinematics log showed items occasionally taking 8-50 s to clear the zone + # (expected ~T_zone=0.95s) - a deck-contact stick/jitter issue, not a plow one - + # and while that is unresolved a position-only release leaves the blade locked + # to one stalled item and unable to return to centre or serve anyone else for the + # rest of the run. Releasing on a timeout keeps the blade responsive even when an + # individual item is still slowly working itself loose behind it. + if plow_active is not None: + name, _, commit_t = plow_active + x = float(item_pose(name)[0]) + # release once the item is past the blade BODY (its downstream face), not the + # further-downstream PLOW_RELEASE_X - by the blade's own trailing edge the + # deflection has already happened and holding longer only starves the queue. + if x < BLADE_TRAILING_X or (sim_t[0] - commit_t) > PLOW_HOLD_MAX: + plow_active = None + + for n in list(plow_pending): + if float(item_pose(n)[0]) < C.PLOW_RELEASE_X: + # starved: it crossed release without ever being served its own angle - + # picked up whatever the blade happened to be doing instead. Logged, not + # silently dropped, because "nearest to PLOW_X" (the old rule below) could + # cause exactly this: a stalled item still gets judged "far" while a NEWER + # item that entered later but is moving normally overtakes it in raw + # distance and keeps winning the slot - the case measured on lunchbox and + # detergent, both starved behind an adjacent, slower-clearing C item. + gate_log.append(("plow-starved", n, CLASSES[n], plow_pending[n])) + plow_pending.pop(n, None) + + if plow_active is None and plow_pending: + # FIFO, not nearest-to-PLOW_X: whichever item was DETECTED first is served + # first. Nearest-distance let a normally-moving newer arrival leapfrog an + # older one that had merely stalled a little, starving it (see above) - FIFO + # cannot starve anyone, every pending item's turn always eventually comes. + oldest = next(iter(plow_pending)) + plow_active = (oldest, plow_pending.pop(oldest), sim_t[0]) + + if plow_active is not None: + plow.step_toward(plow_active[1], dt, rate=PLOW_RATE) + else: + plow.step_toward(0.0, dt, rate=PLOW_RETURN_RATE) + + _push_step(dt) + except BaseException as exc: + # pxr.Tf.ErrorException (the stage-vs-Fabric sync race seen throughout this run) + # derives from BaseException, not Exception - `except Exception` never sees it, and + # missing one 1/60s physics tick of plow/sensor update is harmless; the next tick + # retries on its own. + gate_log.append(("step-error", "", repr(exc))) + + +sub = get_physx_interface().subscribe_physics_step_events(_step) + +# tl.play()/tl.stop(), not app_utils.play()/stop(): pacing everything downstream off an +# assumed 60 fps (steps=int(round(PITCH*60))) measured wrong on this scene before - the +# timeline's actual step can run well under 60 Hz, so a "0.7 s" wait was really much +# shorter and every item piled up at the entry belt instead of spreading out at 700 mm. +# Pace off tl.get_current_time() instead, which is what verify_belts2.py/verify_plow2.py +# (the only scripts that measured correct 1 m/s transport on this scene) actually do. +tl = omni.timeline.get_timeline_interface() +tl.play() +await app_utils.update_app_async(steps=20) +print("timeline playing:", tl.is_playing()) + +# ---------------------------------------------------------------- feed + shoot screenshots +w = vp.get_active_viewport() +shots = [] + + +async def _shot(tag): + await app_utils.update_app_async(steps=5) + path = f"/tmp/plow9045_{tag}.png" + vp.capture_viewport_to_file(w, file_path=path) + await app_utils.update_app_async(steps=3) + shots.append(path) + + +async def _release(name): + """_activate_item() during Play can still race the physics-Fabric sync thread the + same way setup did, but a short retry is enough here - unlike the one-time setup race, + this one resolves in a tick or two, and the loop's overall 0.7 s pitch tolerates jitter.""" + for attempt in range(8): + try: + return _activate_item(name, plow_cell_9045.ENTRY_X, plow_cell_9045.ENTRY_Y) + except BaseException: + await app_utils.update_app_async(steps=2) + return _activate_item(name, plow_cell_9045.ENTRY_X, plow_cell_9045.ENTRY_Y) # let it raise for real + + +async def _wait_sim_seconds(seconds): + """advance by SIM time, not an assumed frame count - this scene's actual physics step + has measured well under 60 Hz before, and a fixed steps=N wait ran short as a result.""" + target = float(tl.get_current_time()) + seconds + while float(tl.get_current_time()) < target: + await app_utils.update_app_async(steps=5) + + +sim_t0 = float(tl.get_current_time()) +for i, name in enumerate(ORDER): + await _release(name) + print(f" {i * PITCH:5.2f}s released {name} ({CLASSES[name]})") + await _wait_sim_seconds(PITCH) + if i == 0: + await app_utils.update_app_async(steps=10) + print(f" {name} position 0.1s+ after release: {item_pose(name)}" + f" (spawned at {plow_cell_9045.ENTRY_X:.2f},{plow_cell_9045.ENTRY_Y:.2f}) " + f"- should have moved if belts + gravity are live") + if i % 3 == 0: + await _shot(f"feed_{i:02d}_{name}") + +# ---------------------------------------------------------------- wait for everything to settle +MAX_SECONDS = 60.0 +settled = {} + + +def _outcome(name): + if name not in rp: + return None + p = item_pose(name) + x, y, z = float(p[0]), float(p[1]), float(p[2]) + if BIN_X0 < x < BIN_X1 and BIN_Y0 < y < BIN_Y1 and z < BIN_LIP_Z: + return "bin_D" + if abs(x - CONTAINER_B[0]) < CONTAINER_R and abs(y - CONTAINER_B[1]) < CONTAINER_R and z < CONTAINER_Z: + return "container_B" + if abs(x - CONTAINER_C[0]) < CONTAINER_R and abs(y - CONTAINER_C[1]) < CONTAINER_R and z < CONTAINER_Z: + return "container_C" + if z < C.BELT_Z - 0.5 and x > -8.3: + return "floor" + return None + + +wall_t0 = time.time() +wall_budget = 240.0 # backstop in case the timeline stalls entirely - don't hang forever +while (float(tl.get_current_time()) - sim_t0 < MAX_SECONDS + len(ORDER) * PITCH + and time.time() - wall_t0 < wall_budget): + await app_utils.update_app_async(steps=30) + for n in ORDER: + if n in settled: + continue + w_ = _outcome(n) + if w_ is not None: + settled[n] = w_ + if len(settled) >= len(ORDER): + break + +await _shot("final") + +# tl.stop() resets every rigid body to its authored (pre-Play) transform - mechanics.py's +# own docstring warns of exactly this ("capture renders while playing"). Read final poses +# NOW, while still playing, or the report shows everyone back at their park slot. +final_pos = {n: item_pose(n).copy() for n in ORDER} + +sub = None +tl.stop() +await app_utils.update_app_async(steps=10) + +# ---------------------------------------------------------------- report +EXPECT = {"D": "bin_D", "B": "container_B", "C": "container_C"} +print("\n===== GATE LOG (first item at each gate) =====") +seen_gates = set() +for entry in gate_log: + key = (entry[0], entry[1]) + if key in seen_gates: + continue + seen_gates.add(key) + print(" ", entry) + +print("\n===== DELIVERY =====") +ok_n = 0 +by_class = {"B": [0, 0], "C": [0, 0], "D": [0, 0]} # class -> [correct, total] +for name, cls in ITEMS: + if name not in loaded: + print(f" {name:18s} class={cls} -> SKIPPED (failed to load)") + continue + outcome = settled.get(name, "line/unresolved") + want = EXPECT[cls] + ok = outcome == want + ok_n += ok + by_class[cls][1] += 1 + by_class[cls][0] += int(ok) + p = final_pos[name] + print(f" {name:18s} class={cls} -> {outcome:14s} want={want:14s} " + f"{'OK' if ok else 'FAIL'} final=({float(p[0]):+.2f},{float(p[1]):+.2f},{float(p[2]):+.2f})") +print(f"\ndelivered {ok_n}/{len(ITEMS)}") +print("\n===== ACCURACY BY CLASS (plow: B/C, pusher: D) =====") +for cls in ("B", "C", "D"): + hit, total = by_class[cls] + rate = hit / total if total else 0.0 + print(f" {cls}: {hit}/{total} ({rate*100:.0f}%)") +print(f"\nPLOW_RATE used this run: {PLOW_RATE:.0f} deg/s (config default {C.PLOW_SWEEP_RATE:.0f})") +print(f"PLOW_RETURN_RATE used this run: {PLOW_RETURN_RATE:.0f} deg/s") +print(f"PLOW_HOLD_MAX used this run: {PLOW_HOLD_MAX:.2f} s") +print(f"PUSH_SENSE_X used this run: {PUSH_SENSE_X:.3f} (blade's own edge)") +print(f"PLOW_ANGLE used this run: +-{PLOW_ANGLE:.0f} deg (config default 16)") +print("\nscreenshots:", shots) + +# ---------------------------------------------------------------- kinematics log +import json + +KIN_LOG = "/tmp/plow9045_kinematics.json" +json.dump(dict( + timing=dict(T_pitch=T_PITCH, T_lead=T_LEAD, T_zone=T_ZONE, plow_rate=PLOW_RATE, + plow_angle=PLOW_ANGLE, push_speed=PUSH_SPEED), + items=[dict(name=n, cls=CLASSES[n], target_angle=PLOW_ANGLES.get(CLASSES[n], 0.0), + outcome=settled.get(n, "unresolved"), samples=plow_trace[n]) + for n in ORDER], +), open(KIN_LOG, "w"), indent=1) +print(f"\nkinematics log -> {KIN_LOG} ({sum(len(plow_trace[n]) for n in ORDER)} samples)") + +print("\n===== PUSHER LOG (class D) =====") +if not push_log: + print(" the pusher never fired - no D item was detected at the curtain") +for e in push_log: + print(f" {e['item']:18s} stroke start x={e['start_x']:+.3f} y={e['start_y']:+.3f}" + f" ==> after stroke x={e['after_x']:+.3f} y={e['after_y']:+.3f} " + f"z={e['after_z']:+.3f} dy={e['dy']:+.3f}") + +print("\n===== KINEMATICS SUMMARY (plow zone only) =====") +for n in ORDER: + tr = plow_trace[n] + if not tr: + print(f" {n:18s} never entered the logged zone") + continue + t0, x0, y0, cmd0, ang0, active0 = tr[0] + t1, x1, y1, cmd1, ang1, active1 = tr[-1] + lag = max(abs(c - a) for _, _, _, c, a, _ in tr) + active_frac = sum(1 for row in tr if row[5]) / len(tr) + want = PLOW_ANGLES.get(CLASSES[n], 0.0) + # lateral deflection actually achieved across the zone, and whether the blade was + # holding this item's OWN angle when it mattered - the two numbers the angle/rate + # tuning has to be read from. + served = "yes" if abs(ang1 - want) < 5.0 else f"NO (held {ang1:+.0f})" + print(f" {n:18s} cls={CLASSES[n]} want={want:+.0f} dy={y1-y0:+.3f}m " + f"served={served:14s} active={active_frac*100:3.0f}% " + f"dwell={t1-t0:.2f}s (T_pitch={T_PITCH:.2f}s)") diff --git a/scripts/run_plow_cell_vision.py b/scripts/run_plow_cell_vision.py new file mode 100644 index 0000000..6e394e7 --- /dev/null +++ b/scripts/run_plow_cell_vision.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 +"""Run the plow cell with the full vision stack. + + ./python.sh scripts/run_plow_cell_vision.py # windowed + ./python.sh scripts/run_plow_cell_vision.py --headless + ./python.sh scripts/run_plow_cell_vision.py --no-vision # route on ground truth + +Items are released on the added infeed belt, measured by CRE-ROI v2b under the camera +portal, and the ones that come back class D are diverted by the Y-split pusher when they +break the laser beam. The plow and the authored kinematics are not touched. +""" +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent +if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) + + +def parse_args(argv=None): + p = argparse.ArgumentParser(description="plow cell + CRE-ROI v2b") + p.add_argument("--headless", action="store_true") + p.add_argument("--no-vision", action="store_true", + help="skip inference and route on ground truth") + p.add_argument("--speed", type=float, default=None, help="belt speed, m/s") + p.add_argument("--pitch", type=float, default=None, help="metres between items") + p.add_argument("--items", default=None, help="comma-separated release order") + p.add_argument("--log", default=None) + return p.parse_args(argv) + + +async def _run(app_utils, args): + from robozon_sorter import config as C + from robozon_sorter.sim import plow_vision + from robozon_sorter.sim.mechanics import Cell + from robozon_sorter.sim.spawner import AutoFeeder + + if args.speed: + C.BELT_SPEED = args.speed + + stage, info = plow_vision.load(belt_speed=args.speed, script_control=True) + items = {k: v["zone"] for k, v in info["items"].items()} + print(f"plow cell ready: {len(items)} items {sorted(set(items.values()))}, " + f"infeed release at x={info['spawn_x']}") + + vision = None + if not args.no_vision: + from robozon_sorter.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) + + order = [n.strip() for n in args.items.split(",")] if args.items else sorted(items) + order = [n for n in order if n in items] + pitch = args.pitch if args.pitch is not None else C.RELEASE_GAP + + log, seen = [], set() + route = dict(items) if args.no_vision else {} + + def on_event(kind, name, payload): + print(f" {kind:8s} {name:18s} {payload if payload else ''}") + if kind == "done": + for rec in log: + if rec["item"] == name and "outcome" not in rec: + rec["outcome"] = payload.get("where") + + feeder = AutoFeeder(cell, order=order, pitch=pitch, route=route, + on_event=on_event).install() + + import omni.timeline + timeline = omni.timeline.get_timeline_interface() + app_utils.play(commit=True) + await app_utils.update_app_async(steps=20) + + print(f"\npitch {pitch} m at {C.BELT_SPEED} m/s\n{'kind':>10} detail") + for _ in range(400): + await app_utils.update_app_async(steps=15) + + # classify each item once, while it sits under the portal + if vision is not None: + for name in list(feeder.active): + if name in seen: + continue + x = float(cell.pose(name)[0]) + if abs(x - C.CAM_X) < 0.08: + 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) + gt = items[name] + route[name] = res["cls"] + seen.add(name) + print(f" vision {name:18s} pred={res['cls']} gt={gt} " + f"{'ok' if res['cls'] == 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)) + + if len(feeder.finished) >= len(order): + break + + app_utils.stop() + await app_utils.update_app_async(steps=15) + feeder.remove() + cell.blade_to(C.BLADE_HOME_Y) + + print(f"\n outcomes: {feeder.finished}") + expected = {n: ("bin" if items[n] == "D" else "line-end") for n in order} + wrong = [n for n in expected if feeder.finished.get(n) != expected[n]] + print(f" expected: {expected}") + print(" routing matches ground truth" if not wrong else f" differs on: {wrong}") + graded = [r for r in log if r.get("cls") not in (None, "?")] + if graded: + hits = sum(1 for r in graded if r["cls"] == r["gt"]) + cre = [r["cre_ms"] for r in graded if r.get("cre_ms")] + print(f" vision agreed with ground truth on {hits}/{len(graded)}" + + (f", CRE {sum(cre)/len(cre):.0f} ms/item" if cre else "")) + if args.log: + Path(args.log).write_text(json.dumps(log, indent=2)) + print(f" log -> {args.log}") + return log + + +def main(argv=None): + args = parse_args(argv) + try: + import omni.usd + inside = omni.usd.get_context().get_stage() is not None + except Exception: + inside = False + + app = None + if not inside: + from isaacsim import SimulationApp + app = SimulationApp({"headless": args.headless, "width": 1600, "height": 900}) + + 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, args)) + finally: + if app is not None: + app.close() + + +if __name__ == "__main__": + sys.exit(0 if main() is not None else 1) diff --git a/scripts/run_plow_sorting.py b/scripts/run_plow_sorting.py new file mode 100644 index 0000000..b269e69 --- /dev/null +++ b/scripts/run_plow_sorting.py @@ -0,0 +1,266 @@ +"""Full cell run: goods ride the line, vision classifies them, the pusher takes D and the +plow splits B and C into their trays. Sent into a live Isaac Sim: + + isaacsim_send.py --context sort --file scripts/run_plow_sorting.py \ + --args-json '{"vision": true, "pitch": 1.2}' + +`scripts/run_plow_cell_vision.py` exercises the pusher only - it never constructs a +PlowSorter, so B and C simply ran off the end of the line. This one wires the plow in and, +more importantly, records *why* an item ended up where it did: + +* **detection** - predicted vs ground-truth class, dims, roundness, view count, CRE time. +* **delivery** - the tray the item actually came to rest in, against the tray its class + maps to. +* **kinematics** - for every item, a trace sampled while it crosses the plow: its pose and + speed, the angle the plow was commanded to and the angle the arm actually reached. When + an item does not arrive, that trace is what says whether the sensor missed it, the blade + was still moving, or it was deflected and then stopped short. + +The plow is a compliant force drive, so commanded and measured angle are different numbers +and both are logged; treating them as one is what hides a blade that never took up its +angle in time. +""" +import json +import sys +import time + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +# The live Isaac process keeps every module it has ever imported, so an edited +# robozon_sorter/ on disk is invisible to a second run in the same session. Drop the +# package from sys.modules first or you spend the evening re-testing the old code. +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() # a *new* module file is invisible until the finder is reset + +import omni.timeline +import isaacsim.core.experimental.utils.app as app_utils + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_sort, plow_vision +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.spawner import AutoFeeder + +USE_VISION = bool(globals().get("vision", True)) +PITCH = float(globals().get("pitch", 1.20)) +SPEED = float(globals().get("speed", C.BELT_SPEED)) +MAX_SECONDS = float(globals().get("max_seconds", 90.0)) +OUT = globals().get("out", "/home/dasha/robozon-sorter/runs/plow_sorting.json") + +# where each class is supposed to end up +EXPECT = {"D": "bin", "B": "container_B", "C": "container_C"} + +# ---------------------------------------------------------------- scene +C.BELT_SPEED = SPEED +stage, info = plow_vision.load(belt_speed=SPEED, script_control=True) +items = {k: v["zone"] for k, v in info["items"].items()} + +# plow_cell.prepare() deactivates /ConveyorTrack_01 as a stray duplicate. In this scene it +# is not a duplicate, it is the -Y sorting lane, so it has to come back on before the lanes +# are driven - otherwise everything the plow deflects toward -Y falls through the gap. +relit = plow_sort.keep_lanes_active(stage) +lanes = plow_sort.configure_lanes(stage, SPEED) +opened = plow_sort.open_junction(stage) +print(f"scene: {len(items)} items {sorted(set(items.values()))} | lane restored={relit} " + f"| lanes driven={len(lanes)} | junction shells opened={len(opened)}") + +vision = None +if USE_VISION: + from robozon_sorter.cv.pipeline import CreRoiV2b + vision = CreRoiV2b() + vision.attach_cameras() + print("CRE-ROI v2b ready") + +await app_utils.update_app_async(steps=40) +cell = Cell(stage, items.keys()) +cell.park_all() +await app_utils.update_app_async(steps=15) + +order = sorted(items) +route = {} if USE_VISION else dict(items) # what the pusher acts on (class D) +classes = {} if USE_VISION else dict(items) # what the plow acts on (B / C) + +sorter = plow_sort.PlowSorter(stage, cell, classes, plow_sort.calibrate_mapping()) +print(f"plow mapping {sorter.mapping} | sensor x={sorter.sense_x} | swing {sorter.swing} deg") + +# ---------------------------------------------------------------- logging +rec = {n: dict(item=n, gt=items[n], pred=None, dims=None, k=None, views=None, + cre_ms=None, sensed=False, commanded=None, angle_at_plow=None, + trace=[], max_speed=0.0, blowup=None, + outcome=None, expected=EXPECT.get(items[n]), ok=None) + for n in order} +events = [] + + +def on_event(kind, name, payload): + events.append((round(t_sim, 2), kind, name, payload)) + if kind in ("release", "gate", "divert", "error"): + print(f" {kind:8s} {name:18s} {payload if payload else ''}") + + +feeder = AutoFeeder(cell, order=order, pitch=PITCH, route=route, on_event=on_event) + +# ---------------------------------------------------------------- physics hook +t_sim = 0.0 + + +DECIMATE = 8 # 120 Hz / 8 = 15 samples/s: 60 of them span 4 s, the whole discharge +BLOWUP_MS = 5.0 # a belt runs at 1 m/s; anything past this is the solver, not the belt +_tick = 0 + + +def _speed(name): + try: + v = cell._rp[name].get_velocities()[0].numpy()[0] + return float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5) + except Exception: + return 0.0 + + +def _step(dt): + """the plow has to be serviced from the physics step, like the pusher: the sensor is a + raycast and the blade target is ramped per-step. + + The trace is decimated: at full rate 60 samples cover 0.5 m and run out before the item + even reaches the plow, which is how the first pass missed where goods were being thrown. + """ + global t_sim, _tick + t_sim += dt + _tick += 1 + try: + sorter.update(dt) + for n in list(feeder.active): + p = cell.pose(n) + x, y, z = float(p[0]), float(p[1]), float(p[2]) + r = rec[n] + if x < -5.6: # from the plow approach onward + spd = _speed(n) + if spd > r.get("max_speed", 0.0): + r["max_speed"] = round(spd, 2) + if spd > BLOWUP_MS and r.get("blowup") is None: + r["blowup"] = dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3), + z=round(z, 3), speed=round(spd, 1), + cmd=round(sorter.plow.commanded, 1), + arm=round(sorter.plow.angle, 1)) + if _tick % DECIMATE == 0 and len(r["trace"]) < 60: + r["trace"].append(dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3), + z=round(z, 3), v=round(spd, 2), + cmd=round(sorter.plow.commanded, 1), + arm=round(sorter.plow.angle, 1))) + if n in sorter.decided and not r["sensed"]: + r["sensed"] = True + r["commanded"] = round(sorter.decided[n], 1) + if abs(x - C.PLOW_POS[0]) < 0.25 and r["angle_at_plow"] is None: + r["angle_at_plow"] = round(sorter.plow.angle, 1) + except Exception as exc: + events.append((round(t_sim, 2), "step-error", "", repr(exc))) + + +from omni.physx import get_physx_interface +sub = get_physx_interface().subscribe_physics_step_events(_step) +feeder.install() + +timeline = omni.timeline.get_timeline_interface() +app_utils.play(commit=True) +await app_utils.update_app_async(steps=20) + +# ---------------------------------------------------------------- run +print(f"\nrunning: pitch {PITCH} m at {SPEED} m/s, vision={USE_VISION}") +seen = set() +t0 = time.time() +settled = {} +while time.time() - t0 < MAX_SECONDS: + await app_utils.update_app_async(steps=15) + + if vision is not None: + for name in list(feeder.active): + if name in seen: + continue + if abs(float(cell.pose(name)[0]) - C.CAM_X) < 0.10: + was = timeline.is_playing() + res = vision.measure() + if was and not timeline.is_playing(): # Replicator's step stops the timeline + timeline.play() + await app_utils.update_app_async(steps=2) + seen.add(name) + r = rec[name] + r.update(pred=res["cls"], dims=res["dims"], k=round(res.get("k", 0.0), 3), + views=res.get("views"), cre_ms=res.get("cre_ms")) + route[name] = res["cls"] + classes[name] = res["cls"] + sorter.classes[name] = res["cls"] + print(f" vision {name:18s} pred={res['cls']} gt={items[name]} " + f"{'ok' if res['cls'] == items[name] else 'MISS'} " + f"dims={res['dims']} K={res.get('k', 0):.2f}") + + for n in order: # freeze the outcome once it stops + if n in settled: + continue + p = cell.pose(n) + where = sorter.lane_of(n) + if where.startswith("container") or where == "floor": + settled[n] = where + elif where == "line" and float(p[0]) < C.MAIN_X0 + 0.35: + settled[n] = "line-end" + if len(settled) >= len(order): + break + +# outcomes: the D bin is the pusher's, read through mechanics; the trays are the plow's +for n in order: + w = sorter.lane_of(n) + if w == "line" and cell.where(n) == "bin": + w = "bin" + rec[n]["outcome"] = settled.get(n, w) + rec[n]["ok"] = (rec[n]["outcome"] == rec[n]["expected"]) + p = cell.pose(n) + rec[n]["final"] = [round(float(v), 3) for v in p[:3]] + +app_utils.stop() +await app_utils.update_app_async(steps=10) +sub = None +feeder.remove() + +# ---------------------------------------------------------------- report +print("\n===== DETECTION =====") +graded = [r for r in rec.values() if r["pred"] not in (None, "?")] +if graded: + hit = sum(1 for r in graded if r["pred"] == r["gt"]) + cre = [r["cre_ms"] for r in graded if r["cre_ms"]] + print(f" class agreement {hit}/{len(graded)}" + + (f" | CRE {sum(cre)/len(cre):.0f} ms/item" if cre else "")) + for r in sorted(graded, key=lambda r: r["item"]): + print(f" {r['item']:18s} gt={r['gt']} pred={r['pred']} " + f"{'ok' if r['pred'] == r['gt'] else 'MISS':4s} dims={r['dims']} K={r['k']}") +else: + print(" (no vision this run)") + +print("\n===== DELIVERY =====") +for r in sorted(rec.values(), key=lambda r: r["item"]): + print(f" {r['item']:18s} gt={r['gt']} -> {str(r['outcome']):12s} " + f"want={str(r['expected']):12s} {'OK' if r['ok'] else 'FAIL'} " + f"final={r['final']}") +good = [r for r in rec.values() if r["ok"]] +print(f" delivered {len(good)}/{len(order)}") + +bad = [r for r in rec.values() if not r["ok"]] +if bad: + print("\n===== KINEMATICS ON FAILURES =====") + for r in bad: + print(f" {r['item']} ({r['gt']}) -> {r['outcome']}") + print(f" sensed={r['sensed']} commanded={r['commanded']} " + f"arm_at_plow={r['angle_at_plow']}") + for s in r["trace"][:12]: + print(f" t={s['t']:6.2f} x={s['x']:+.2f} y={s['y']:+.2f} z={s['z']:+.2f} " + f"cmd={s['cmd']:+.1f} arm={s['arm']:+.1f}") + +import os +os.makedirs(os.path.dirname(OUT), exist_ok=True) +json.dump(dict(config=dict(pitch=PITCH, speed=SPEED, vision=USE_VISION, + mapping=sorter.mapping, expect=EXPECT), + items=list(rec.values()), + events=[dict(t=t, kind=k, item=n, payload=str(p)) for t, k, n, p in events]), + open(OUT, "w"), indent=2) +print(f"\nlog -> {OUT}") diff --git a/scripts/seam_check.py b/scripts/seam_check.py new file mode 100644 index 0000000..7187346 --- /dev/null +++ b/scripts/seam_check.py @@ -0,0 +1,45 @@ +"""Check for gaps/height-mismatches at every belt-to-belt handoff seam, and compare +plow_cell.usd's own reference grip-material setup against what we're using.""" +import omni.usd +from pxr import Usd, UsdGeom, UsdShade, UsdPhysics + +stage = omni.usd.get_context().get_stage() +bbc = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) + +def report(path): + prim = stage.GetPrimAtPath(path) + if not prim.IsValid(): + print(f" {path} MISSING"); return None + r = bbc.ComputeWorldBound(prim).ComputeAlignedRange() + mn, mx = r.GetMin(), r.GetMax() + api = UsdShade.MaterialBindingAPI(prim) + mat, _ = api.ComputeBoundMaterial(materialPurpose="physics") + fric = None + if mat: + m = UsdPhysics.MaterialAPI(mat.GetPrim()) + fric = (round(m.GetStaticFrictionAttr().Get(),2), round(m.GetDynamicFrictionAttr().Get(),2)) + print(f" {path}") + print(f" x[{mn[0]:+.3f}..{mx[0]:+.3f}] y[{mn[1]:+.3f}..{mx[1]:+.3f}] z[{mn[2]:+.3f}..{mx[2]:+.3f}] friction={fric} mat={mat.GetPath() if mat else None}") + return mn, mx + +print("=== pusher handoff: ConveyorTrack_03/Belt -> Belt_01 ===") +b1 = report("/World/ConveyorTrack_03/Belt") +b2 = report("/World/ConveyorTrack_03/Belt_01") +if b1 and b2: + print(f" Y GAP (belt max_y to branch min_y): {b2[0][1]-b1[1][1]:+.3f} m Z step: {b2[0][2]-b1[1][2]:+.3f} m") + +print("\n=== plow handoff: ConveyorTrack_04 -> decks -> ConveyorTrack_06 / _01 ===") +for path in ["/World/ConveyorTrack_04/Belt", "/World/PlowTransition_B", "/World/PlowCornerDeck_B", + "/World/ConveyorTrack_01/Belt", "/World/PlowTransition_C", "/World/PlowCornerDeck_C", + "/World/ConveyorTrack_06/Belt"]: + report(path) + +print("\n=== reference: plow_cell.py's own GRIP_MATERIAL values ===") +import sys +sys.path.insert(0, "/home/dasha/robozon-sorter") +from robozon_sorter.sim import plow_cell as _pc +grip = stage.GetPrimAtPath(_pc.GRIP_MATERIAL) +print(" GRIP_MATERIAL path:", _pc.GRIP_MATERIAL, " valid:", grip.IsValid()) +if grip.IsValid(): + m = UsdPhysics.MaterialAPI(grip) + print(" static/dynamic:", m.GetStaticFrictionAttr().Get(), m.GetDynamicFrictionAttr().Get()) diff --git a/scripts/smoke_plow_cell.py b/scripts/smoke_plow_cell.py new file mode 100644 index 0000000..2e13779 --- /dev/null +++ b/scripts/smoke_plow_cell.py @@ -0,0 +1,68 @@ +"""Smoke test for scene/plow_cell.usd: load the cell, run the belts, swing the plow under +script control and confirm the arm physically follows. + +Run it inside a live Isaac Sim through the code editor's python server, e.g. + + python isaacsim_send.py --context plow --file scripts/smoke_plow_cell.py + +It asserts the things that were silent failures during the transfer: that the referenced +plow geometry actually composed (a stub resolves to 8 points, the real base to 72k), that +the belts got a surface velocity, and that commanding the drive moves the arm rather than +just setting an attribute nothing reads. +""" +import sys + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import PhysxSchema, UsdGeom + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell +from robozon_sorter.sim.plow import Plow + +stage, info = plow_cell.load(script_control=True) +print("loaded:", info) + +# --- geometry actually composed? -------------------------------------------------------- +for path, label, floor in ((C.PLOW_BASE + "/Geom/Mesh", "plow base", 1000), + (C.PLOW_ARM + "/Geom/Mesh", "plow arm", 1000)): + pts = UsdGeom.Mesh(stage.GetPrimAtPath(path)).GetPointsAttr().Get() + n = len(pts) if pts else 0 + print(f" {label:10s} {n:>7} points {'OK' if n > floor else 'FAIL (stub or missing)'}") + +# --- belts driven? ---------------------------------------------------------------------- +driven = 0 +for b in plow_cell.BELTS + [plow_cell.BRANCH]: + p = stage.GetPrimAtPath(b) + if p.IsValid() and p.HasAPI(PhysxSchema.PhysxSurfaceVelocityAPI): + v = PhysxSchema.PhysxSurfaceVelocityAPI(p).GetSurfaceVelocityAttr().Get() + if v and any(abs(c) > 1e-6 for c in v): + driven += 1 +print(f" belts driven: {driven}/{len(plow_cell.BELTS) + 1}") + +# --- plow moves? ------------------------------------------------------------------------ +tl = omni.timeline.get_timeline_interface() +tl.play() +await app_utils.update_app_async(steps=30) + +plow = Plow(stage) +rest = plow.angle +print(f" rest angle {rest:+.2f} deg") + +await plow.swing(app_utils, C.PLOW_SWING) +out = plow.angle +print(f" swung to {out:+.2f} deg (commanded {C.PLOW_SWING:+.1f})") + +await plow.swing(app_utils, 0.0) +back = plow.angle +print(f" returned {back:+.2f} deg") + +tl.stop() +moved = abs(out - rest) > 0.5 * C.PLOW_SWING +homed = abs(back) < 5.0 +print(f"RESULT: arm moved={moved} returned_home={homed} " + f"{'PASS' if moved and homed else 'FAIL'}") diff --git a/scripts/spawn_debug.py b/scripts/spawn_debug.py new file mode 100644 index 0000000..27a8919 --- /dev/null +++ b/scripts/spawn_debug.py @@ -0,0 +1,62 @@ +"""Isolated: does _activate_item's write actually stick, and is physics playing?""" +import sys +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +import importlib +importlib.invalidate_caches() + +import omni.usd, omni.timeline +from pxr import Gf, UsdGeom, UsdPhysics, PhysxSchema +import isaacsim.core.experimental.utils.app as app_utils +from isaacsim.core.experimental.prims import RigidPrim +from robozon_sorter import config as C + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +print("timeline playing (before):", tl.is_playing()) + +name = "bottle" +prim = stage.GetPrimAtPath(f"/World/Items/{name}") +print("prim valid:", prim.IsValid()) +if not prim.IsValid(): + UsdGeom.Xform.Define(stage, "/World/Items") + prim = UsdGeom.Xform.Define(stage, f"/World/Items/{name}").GetPrim() + prim.GetReferences().AddReference(str(C.ROOT / "assets" / "items" / f"{name}.usd")) + xf = UsdGeom.Xformable(prim) + xf.ClearXformOpOrder() + xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(9.0, 5.0, 0.4)) + UsdGeom.Imageable(prim).MakeInvisible() + +print("xform ops before activate:", [str(op.GetOpType()) for op in UsdGeom.Xformable(prim).GetOrderedXformOps()]) + +try: + for op in UsdGeom.Xformable(prim).GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + op.Set(Gf.Vec3d(C.SPAWN_X, 0.0, C.BELT_Z + 0.05)) + print("translate write: OK") + break + UsdPhysics.RigidBodyAPI.Apply(prim) + UsdPhysics.MassAPI.Apply(prim).CreateMassAttr().Set(0.6) + px = PhysxSchema.PhysxRigidBodyAPI.Apply(prim) + px.CreateEnableCCDAttr().Set(True) + UsdGeom.Imageable(prim).MakeVisible() + print("physics API apply: OK") +except BaseException as exc: + print("EXCEPTION during activate:", type(exc).__name__, exc) + +# read back the AUTHORED attribute directly (not RigidPrim/Fabric) +attr = prim.GetAttribute("xformOp:translate") +print("authored translate now:", attr.Get() if attr else "NO ATTR") + +rp = RigidPrim(paths=[f"/World/Items/{name}"]) +print("RigidPrim world pose now:", rp.get_world_poses()[0].numpy()[0]) + +print("timeline playing (still):", tl.is_playing()) +app_utils.play(commit=True) +print("timeline playing (after play() call):", tl.is_playing()) +await app_utils.update_app_async(steps=30) +print("RigidPrim world pose after 30 steps of play:", rp.get_world_poses()[0].numpy()[0]) +app_utils.stop() diff --git a/scripts/test_sorting_run.py b/scripts/test_sorting_run.py new file mode 100644 index 0000000..7a7fc0a --- /dev/null +++ b/scripts/test_sorting_run.py @@ -0,0 +1,438 @@ +"""Controlled sorting test over the whole item library, with per-item kinematics. + + isaacsim_send.py --context test --file scripts/test_sorting_run.py \ + --args-json '{"vision": true, "preset": "bright", "repeats": 2}' + +What it records, per dispatched item: + +* **dispatch** when it was released and with what ground-truth class +* **detection** predicted class, dimensions, roundness, views, CRE time +* **kinematics** at the moment the item is level with the plow: the commanded angle, the + angle the arm had actually reached, and the arm's **angular rate** in deg/s. Commanded + and reached are different numbers - the drive is compliant - and a blade that is still + travelling when the item arrives deflects it differently from one that has settled. +* **outcome** where it came to rest: tray B, tray C, the D bin, a lane, the line, or the + floor, plus the resting pose and whether it matches the tray its class maps to. + +Two metric blocks are reported separately, because they fail independently: classification +(what the vision stack decided) and delivery (where the mechanics actually put it). An item +can be classified perfectly and still be left on the line, and the run is only useful if +those two are not conflated. + +Lighting preset and the floor come from `sim/staging`, so a run can be repeated under +`bright` / `dim` / `harsh` to see how much of the classification error is illumination. +""" +import json +import os +import sys +import time +from collections import Counter, defaultdict + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +import importlib + +# The live Isaac process keeps every module it has ever imported, so an edited +# robozon_sorter/ on disk is invisible to a second run. Dropping the package is not enough +# on its own: a module file that did not exist when the directory was first scanned stays +# invisible until the import finder's cached listing is thrown away too. +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +importlib.invalidate_caches() + +import omni.timeline +import isaacsim.core.experimental.utils.app as app_utils + +from robozon_sorter import config as C +from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.spawner import AutoFeeder + +USE_VISION = bool(globals().get("vision", True)) +PRESET = globals().get("preset", "bright") +REPEATS = int(globals().get("repeats", 1)) +PITCH = float(globals().get("pitch", 2.5)) +SPEED = float(globals().get("speed", 1.0)) +LIMIT = int(globals().get("limit", 0)) # 0 = whole library +CLASSES_ONLY = set(str(globals().get("classes", "")).upper()) or None +# Explicit dispatch list, in order, repeats allowed. `limit`/`classes` cannot express +# "these exact items, plus an even 10/10/10 of the rest" when a class has fewer than 10 +# unique members - the only way to balance is to send some of them twice. +ONLY = [n for n in str(globals().get("only", "")).split(",") if n.strip()] +# Items to score SEPARATELY as well as in the overall figures. +FOCUS = [n for n in str(globals().get("focus", "")).split(",") if n.strip()] +BUDGET = float(globals().get("max_seconds", 240.0)) +TRACK_END_X = float(globals().get("track_end_x", -11.5)) # past both trays +ITEMS_DIR = globals().get("items_dir", f"{REPO}/assets/items") +OUT = globals().get("out", f"{REPO}/runs/test_sorting_{PRESET}.json") + +EXPECT = {"D": "bin", "B": "container_B", "C": "container_C"} +CLASSES = ("B", "C", "D") + +# ---------------------------------------------------------------- scene +C.BELT_SPEED = SPEED +stage, info = plow_vision.load(belt_speed=SPEED, script_control=True, + meshes_dir=ITEMS_DIR) +staged = staging.stage_cell(stage, preset=PRESET, floor=True) +plow_sort.keep_lanes_active(stage) +lanes = plow_sort.configure_lanes(stage, SPEED) +opened = plow_sort.open_junction(stage) + +items = {k: v["zone"] for k, v in info["items"].items()} +gt_dims = {k: v.get("gt_dims_mm") for k, v in info["items"].items()} +print(f"library {len(items)} items {dict(Counter(items.values()))} | light={PRESET} " + f"| floor={'yes' if staged.get('floor') else 'no'} | lanes={len(lanes)} " + f"| junction opened={len(opened)}") + +vision = None +if USE_VISION: + # The streaming launcher starts Kit WITHOUT the user site-packages, so ultralytics and + # torch installed under ~/.local are invisible to the running app even though + # `python.sh` imports them fine - it is the same interpreter (3.12.13), just a + # different sys.path. Appending (not prepending) leaves Kit's own bundled copies first. + for _sp in ("/home/dasha/.local/lib/python3.12/site-packages",): + if os.path.isdir(_sp) and _sp not in sys.path: + sys.path.append(_sp) + from robozon_sorter.cv.pipeline import CreRoiV2b + vision = CreRoiV2b() + vision.attach_cameras() + _w = await vision.warmup() + print(f"CRE-ROI v2b attached | прогрев камер: {'ок' if _w['ok'] else 'НЕ УДАЛСЯ'} " + f"за {_w['attempts']} подход(а), самый тёмный глаз max={_w['darkest_eye_max']}") + if not _w["ok"]: + print(" ВНИМАНИЕ: камеры всё ещё отдают чёрное - классификация будет пустой") + +await app_utils.update_app_async(steps=40) +# Aim the viewport at the plow before anything else. The default Persp framing tries to +# fit the WHOLE stage, and the stage contains the parked queue off at x +37 - so the cell +# ends up a few pixels wide and the stream looks black with only the emissive laser stripe +# in it. That is what "renders wrong" was: aim, not lighting. +try: + from isaacsim.core.rendering_manager import ViewportManager + ViewportManager.set_camera_view("/OmniverseKit_Persp", eye=[-4.5, -5.0, 5.0], + target=[-6.5, 0.0, 1.8]) +except Exception as _e: + print(" (камеру навести не удалось:", _e, ")") + +cell = Cell(stage, items.keys()) +cell.park_all() +await app_utils.update_app_async(steps=15) + +base_order = sorted(items) +# Optional class filter, e.g. classes="BC" runs only the B and C items. Without it a small +# `limit` just takes the first N alphabetically, which can miss a whole class: limit=8 gave +# B=4 D=4 and not one C, so the C route went untested. +if ONLY: + missing = [n for n in ONLY if n not in items] + if missing: + print(f" ВНИМАНИЕ: нет в библиотеке: {missing}") + base_order = [n.strip() for n in ONLY if n.strip() in items] +elif CLASSES_ONLY: + base_order = [n for n in base_order if items[n] in CLASSES_ONLY] +if LIMIT: + base_order = base_order[:LIMIT] +order = base_order * max(1, REPEATS) # repeat the library to reach a dispatch count +route, classes = ({}, {}) if USE_VISION else (dict(items), dict(items)) +sorter = plow_sort.PlowSorter(stage, cell, classes, plow_sort.calibrate_mapping()) +BEAMS = lane_beams.LaneBeams(stage, cell, plow=sorter.plow) +print(f"dispatching {len(order)} ({len(base_order)} unique x{max(1, REPEATS)}) | " + f"mapping {sorter.mapping} | pitch {PITCH} m @ {SPEED} m/s") + +# ---------------------------------------------------------------- logging +def blank(name, pas): + return dict(item=name, pass_no=pas, gt=items[name], gt_dims=gt_dims.get(name), + released_t=None, pred=None, dims=None, k=None, views=None, cre_ms=None, + sensed=False, commanded=None, + arm_at_plow=None, rate_at_plow=None, arm_max_rate=0.0, + max_speed=0.0, blowup=None, trace=[], + contact=[], contact_first=None, contact_last=None, + outcome=None, expected=EXPECT.get(items[name]), delivered=None, + final=None) + +pas = defaultdict(int) +rec = {} # name -> record for the pass currently on the line +done_records = [] +events = [] +t_sim = 0.0 + +DECIMATE = 8 +BLOWUP_MS = 5.0 +_tick = 0 +_prev_angle = 0.0 + + +def on_event(kind, name, payload): + events.append(dict(t=round(t_sim, 2), kind=kind, item=name, payload=str(payload))) + if kind in ("release", "divert", "error"): + print(f" {kind:8s} {name:20s} {payload if payload else ''}") + + +feeder = AutoFeeder(cell, order=order, pitch=PITCH, route=route, on_event=on_event) + + +def _speed(name): + try: + v = cell._rp[name].get_velocities()[0].numpy()[0] + return float((v[0] ** 2 + v[1] ** 2 + v[2] ** 2) ** 0.5) + except Exception: + return 0.0 + + +def _step(dt): + """service the plow and sample kinematics as goods cross it""" + global t_sim, _tick, _prev_angle + t_sim += dt + _tick += 1 + try: + sorter.update(dt) + BEAMS.tick(dt) + BEAMS.poll(rate=C.PLOW_SWEEP_RATE) + arm = sorter.plow.angle + rate = (arm - _prev_angle) / dt if dt > 0 else 0.0 # deg/s, measured not commanded + _prev_angle = arm + for n in list(feeder.active): + r = rec.get(n) + if r is None: + continue + p = cell.pose(n) + x, y, z = float(p[0]), float(p[1]), float(p[2]) + if x >= -5.6: + continue + spd = _speed(n) + r["max_speed"] = max(r["max_speed"], round(spd, 2)) + r["arm_max_rate"] = max(r["arm_max_rate"], round(abs(rate), 1)) + if spd > BLOWUP_MS and r["blowup"] is None: + r["blowup"] = dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3), + z=round(z, 3), speed=round(spd, 1), + arm=round(arm, 1), rate=round(rate, 1)) + if _tick % DECIMATE == 0 and len(r["trace"]) < 50: + r["trace"].append(dict(t=round(t_sim, 2), x=round(x, 3), y=round(y, 3), + z=round(z, 3), v=round(spd, 2), + cmd=round(sorter.plow.commanded, 1), + arm=round(arm, 1), rate=round(rate, 1))) + if n in sorter.decided and not r["sensed"]: + r["sensed"] = True + r["commanded"] = round(sorter.decided[n], 1) + # the instant the item is level with the plow: this is the state that decides + if abs(x - C.PLOW_POS[0]) < 0.25 and r["arm_at_plow"] is None: + r["arm_at_plow"] = round(arm, 1) + r["rate_at_plow"] = round(rate, 1) + # CONTACT WINDOW: while the item is inside the arm's sweep radius, record how + # the blade is actually turning. This is what says whether it leaned the item + # over at tip speed or arrived as a hit - a single sample at the plow centre + # cannot tell those apart. + reach = (x - C.PLOW_POS[0]) ** 2 + (y - C.PLOW_POS[1]) ** 2 + if reach < (C.PLOW_ARM_LEN + 0.10) ** 2: + if r["contact_first"] is None: + r["contact_first"] = dict(t=round(t_sim, 2), x=round(x, 3), + y=round(y, 3), arm=round(arm, 1), + rate=round(rate, 1), v=round(spd, 2)) + if len(r["contact"]) < 40: + r["contact"].append(dict(t=round(t_sim, 2), y=round(y, 3), + arm=round(arm, 1), rate=round(rate, 1), + v=round(spd, 2))) + r["contact_last"] = dict(t=round(t_sim, 2), y=round(y, 3), + arm=round(arm, 1), v=round(spd, 2)) + except Exception as exc: + events.append(dict(t=round(t_sim, 2), kind="step-error", item="", payload=repr(exc))) + + +from omni.physx import get_physx_interface +sub = get_physx_interface().subscribe_physics_step_events(_step) +feeder.install() + +timeline = omni.timeline.get_timeline_interface() +app_utils.play(commit=True) +await app_utils.update_app_async(steps=20) + +# ---------------------------------------------------------------- run +seen, settled = set(), {} +t0 = time.time() +while time.time() - t0 < BUDGET: + await app_utils.update_app_async(steps=15) + + for n in feeder.active: # open a record when an item is released + if n not in rec: + pas[n] += 1 + rec[n] = blank(n, pas[n]) + rec[n]["released_t"] = round(t_sim, 2) + + if vision is not None: + for name in list(feeder.active): + if name in seen or name not in rec: + continue + if abs(float(cell.pose(name)[0]) - C.CAM_X) < 0.10: + was = timeline.is_playing() + res = vision.measure() + if was and not timeline.is_playing(): + timeline.play() + await app_utils.update_app_async(steps=2) + seen.add(name) + r = rec[name] + r.update(pred=res.get("cls"), dims=res.get("dims"), + k=round(res.get("k", 0.0), 3), views=res.get("views"), + cre_ms=res.get("cre_ms")) + route[name] = res.get("cls") + sorter.classes[name] = res.get("cls") + + for n in list(rec): # freeze an outcome once the item stops + if n in settled: + continue + where = sorter.lane_of(n) + if where == "line" and cell.where(n) == "bin": + where = "bin" + p = cell.pose(n) + resting = where.startswith("container") or where in ("bin", "floor") + # Freeze only once the item is genuinely done. The old cutoff was MAIN_X0 + 0.35 = + # -7.65, which is the fork apex - every item was declared "line-end" at full 0.80 m/s + # the instant it entered its branch, so no B or C delivery could ever be observed. + if resting or (where == "line" and float(p[0]) < TRACK_END_X): + settled[n] = where if resting else "line-end" + r = rec.pop(n) + r["outcome"] = settled[n] + r["final"] = [round(float(v), 3) for v in p[:3]] + r["delivered"] = (r["outcome"] == r["expected"]) + done_records.append(r) + seen.discard(n) + settled.pop(n, None) + if len(done_records) >= len(order): + break + +for n, r in list(rec.items()): # whatever is still on the line at the end + p = cell.pose(n) + r["outcome"] = sorter.lane_of(n) + r["final"] = [round(float(v), 3) for v in p[:3]] + r["delivered"] = (r["outcome"] == r["expected"]) + done_records.append(r) + +app_utils.stop() +await app_utils.update_app_async(steps=10) +sub = None +feeder.remove() + +# ---------------------------------------------------------------- metrics +print(f"\n===== DISPATCHED {len(done_records)} =====") +print(f"{'item':22s} {'gt':2s} {'pred':4s} {'outcome':13s} {'want':13s} " + f"{'arm':>6s} {'rate':>8s} {'vmax':>6s}") +for r in done_records: + print(f"{r['item']:22s} {r['gt']:2s} {str(r['pred'] or '-'):4s} " + f"{str(r['outcome']):13s} {str(r['expected']):13s} " + f"{str(r['arm_at_plow']):>6s} {str(r['rate_at_plow']):>8s} " + f"{r['max_speed']:>6.1f} {'OK' if r['delivered'] else ''}") + +# --- classification ------------------------------------------------------- +graded = [r for r in done_records if r["pred"] in CLASSES] +print("\n===== CLASSIFICATION (CV) =====") +if graded: + conf = {a: Counter() for a in CLASSES} + for r in graded: + conf[r["gt"]][r["pred"]] += 1 + hits = sum(conf[a][a] for a in CLASSES) + print(f" accuracy {hits}/{len(graded)} = {hits / len(graded):.2f}") + print(" confusion (rows GT, cols pred): " + " ".join(CLASSES)) + for a in CLASSES: + print(f" {a}: " + " ".join(f"{conf[a][b]:3d}" for b in CLASSES)) + for a in CLASSES: + tp = conf[a][a] + fp = sum(conf[g][a] for g in CLASSES) - tp + fn = sum(conf[a].values()) - tp + pr = tp / (tp + fp) if tp + fp else 0.0 + rc = tp / (tp + fn) if tp + fn else 0.0 + f1 = 2 * pr * rc / (pr + rc) if pr + rc else 0.0 + print(f" {a}: precision {pr:.2f} recall {rc:.2f} F1 {f1:.2f} (n={tp + fn})") + cre = [r["cre_ms"] for r in graded if r.get("cre_ms")] + if cre: + print(f" CRE {sum(cre) / len(cre):.0f} ms/item over {len(cre)}") + + if FOCUS: + fset = {n.strip() for n in FOCUS} + fg = [r for r in graded if r["item"] in fset] + print(f"\n ----- ОТДЕЛЬНО ПО НАЗВАННЫМ ТОВАРАМ ({len(fg)} из {len(fset)}) -----") + print(f" {'товар':<20} {'GT':<3} {'пред':<5} {'дim пред, мм':<20} {'GT дим, мм':<20} {'k':<6} верно") + okn = 0 + for r in sorted(fg, key=lambda r: r["item"]): + good = r["pred"] == r["gt"] + okn += bool(good) + dp = "x".join(str(int(x)) for x in (r.get("dims") or [])) or "-" + dg = "x".join(str(int(x)) for x in (r.get("gt_dims") or [])) or "-" + print(f" {r['item']:<20} {r['gt']:<3} {str(r['pred']):<5} {dp:<20} {dg:<20} " + f"{(r.get('k') or 0):<6.3f} {'да' if good else 'НЕТ'}") + if fg: + print(f" точность по названным: {okn}/{len(fg)} = {okn / len(fg):.2f}") + miss = sorted(fset - {r["item"] for r in fg}) + if miss: + print(f" не получили предсказания: {miss}") +else: + print(" no vision this run") + +# --- delivery ------------------------------------------------------------- +if sorter.contact is not None: + rep = sorter.contact.report() + print("\n===== PLOW CONTACT SENSOR =====") + print(f" {len(rep['touches'])} items touched the blade") + print(f" {'item':22s} {'cls':4s} {'angle@touch':>12s} {'range':>14s} {'dur s':>7s}") + for t in rep["touches"]: + print(f" {t['item']:22s} {str(t['cls']):4s} {str(t['angle_at_touch']):>12s} " + f"{str(t['angle_min']) + '..' + str(t['angle_max']):>14s} " + f"{str(t['duration']):>7s}" + + ("" if t["classified"] else " UNCLASSIFIED - not steered")) + +rep = BEAMS.report() +cf = sorted(getattr(sorter, "conflicts", set())) +print("\n===== КОНФЛИКТЫ ОЧЕРЕДИ ПЛУГА =====") +if not cf: + print(" нет: в зоне лезвия ни разу не оказалось двух классов одновременно") +else: + print(f" {len(cf)} товар(ов) делили зону лезвия с товаром ДРУГОГО класса.") + print(" Один нож не может держать два угла сразу - это предел подачи, не сбой:") + print(" " + ", ".join(cf)) + +print("\n===== ЛАЗЕР ПЕРЕД ПЛУГОМ (предустановка угла) =====") +gl = getattr(sorter, "gate_log", []) +if not gl: + print(" створ не сработал ни разу") +else: + print(f" сработал {len(gl)} раз | створ x={plow_sort.SENSE_X}, лезвие с x=-7.32") + print(f" {'товар':<20} {'класс':<6} {'угол':>7} {'x на срабатывании':>18}") + for g in gl: + print(f" {g['item']:<20} {str(g['cls']):<6} {g['angle']:>+7.1f} {g['x']:>18.2f}") + +print("\n===== ЛАЗЕРНЫЕ ДАТЧИКИ НА ЛЕНТАХ B/C =====") +print(f" доехали до ленты: {len(rep)} из {len(done_records)} отправленных") +for c in rep: + print(f" {c['item']:20s} -> {c['lane']:7s} t={c['t']:6.2f}s угол ножа={c['angle']} v={c['speed']}") +if not rep: + print(" ни один товар не доехал ни до одной ленты") + +print("\n===== DELIVERY (mechanics) =====") +ok = [r for r in done_records if r["delivered"]] +print(f" delivered {len(ok)}/{len(done_records)} = {len(ok) / max(len(done_records), 1):.2f}") +per_class = defaultdict(lambda: [0, 0]) +for r in done_records: + per_class[r["gt"]][1] += 1 + per_class[r["gt"]][0] += bool(r["delivered"]) +for a in CLASSES: + got, tot = per_class[a] + if tot: + print(f" {a}: {got}/{tot} into {EXPECT[a]}") +print(" where everything ended up: " + + str(dict(Counter(r["outcome"] for r in done_records)))) +thrown = [r for r in done_records if r["blowup"]] +stalled = [r for r in done_records if r["outcome"] in ("line", "lane_B", "lane_C")] +print(f" thrown by the mechanics: {len(thrown)} | stalled short of a tray: {len(stalled)}") +if thrown: + r = thrown[0] + print(f" e.g. {r['item']}: {r['blowup']}") +if stalled: + r = stalled[0] + print(f" e.g. {r['item']}: stopped at {r['final']} arm={r['arm_at_plow']}") + +os.makedirs(os.path.dirname(OUT), exist_ok=True) +contact_report = sorter.contact.report() if sorter.contact else None +json.dump(dict(contact=contact_report, config=dict(preset=PRESET, vision=USE_VISION, pitch=PITCH, speed=SPEED, + repeats=REPEATS, mapping=sorter.mapping, expect=EXPECT, + staged=staged, items_dir=ITEMS_DIR), + records=done_records, events=events[-400:]), + open(OUT, "w"), indent=2, ensure_ascii=False) +print(f"\nlog -> {OUT}") diff --git a/scripts/tune_sweep_rate.py b/scripts/tune_sweep_rate.py new file mode 100644 index 0000000..d50ac3f --- /dev/null +++ b/scripts/tune_sweep_rate.py @@ -0,0 +1,117 @@ +"""Find the plow sweep rate that actually lands goods on their lane. + + isaacsim_send.py --context tune --file scripts/tune_sweep_rate.py \ + --args-json '{"rates": [120, 200, 300, 450], "n": 4}' + +Delivery alone cannot tune this: an item on the floor and an item still on the belt both +score zero and need opposite corrections. So each rate is judged on the lane-entry beams +(`sim/lane_beams`), which separate the two: + + crossed the push reached the lane <- too slow if this is low + speed how fast it was going when it did <- throwing it if this is high + +A usable rate crosses most items at a modest crossing speed. The sweep is a **push**, so +the blade returns to centre after each item and waits there - `PlowSorter` does that, and +the run reports how many times it completed a return, so a blade that stops homing shows up +as a number rather than as a mystery later. +""" +import json +import sys +import time + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) +import importlib +for _m in [k for k in list(sys.modules) if k.startswith("robozon_sorter")]: + del sys.modules[_m] +importlib.invalidate_caches() + +import omni.timeline +import isaacsim.core.experimental.utils.app as app_utils + +from robozon_sorter import config as C +from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.spawner import AutoFeeder + +RATES = globals().get("rates", [120.0, 200.0, 300.0, 450.0]) +N = int(globals().get("n", 4)) +SPEED = float(globals().get("speed", 0.8)) +PITCH = float(globals().get("pitch", 3.5)) +BUDGET = float(globals().get("per_rate_seconds", 70.0)) +OUT = globals().get("out", f"{REPO}/runs/sweep_tuning.json") + +stage, info = plow_vision.load(belt_speed=SPEED, script_control=True, + meshes_dir=f"{REPO}/assets/items") +staging.stage_cell(stage, preset="bright", floor=True) +plow_sort.keep_lanes_active(stage) +plow_sort.configure_lanes(stage, SPEED) +plow_sort.open_junction(stage) +items = {k: v["zone"] for k, v in info["items"].items()} +await app_utils.update_app_async(steps=30) +cell = Cell(stage, items.keys()) + +order = [n for n in sorted(items) if items[n] in ("B", "C")][:N] +print(f"tuning on {len(order)} B/C items: {order}") +timeline = omni.timeline.get_timeline_interface() +results = [] + +for rate in RATES: + C.PLOW_SWEEP_RATE = float(rate) + cell.park_all() + await app_utils.update_app_async(steps=15) + sorter = plow_sort.PlowSorter(stage, cell, items, plow_sort.calibrate_mapping()) + beams = lane_beams.LaneBeams(stage, cell, plow=sorter.plow) + + def _step(dt, _s=sorter, _b=beams, _r=rate): + _s.update(dt) + _b.tick(dt) + _b.poll(rate=_r) + + from omni.physx import get_physx_interface + sub = get_physx_interface().subscribe_physics_step_events(_step) + feeder = AutoFeeder(cell, order=order, pitch=PITCH, route={}).install() + app_utils.play(commit=True) + await app_utils.update_app_async(steps=20) + + t0 = time.time() + while time.time() - t0 < BUDGET: + await app_utils.update_app_async(steps=15) + if len(beams.crossings) >= len(order): + break + app_utils.stop() + await app_utils.update_app_async(steps=10) + feeder.remove() + sub = None + + cr = beams.report() + speeds = [c["speed"] for c in cr] + where = {n: sorter.lane_of(n) for n in order} + delivered = sum(1 for n in order + if where[n] == {"B": "container_B", "C": "container_C"}[items[n]]) + row = dict(rate=rate, crossed=len(cr), of=len(order), + mean_cross_speed=round(sum(speeds) / len(speeds), 2) if speeds else None, + max_cross_speed=round(max(speeds), 2) if speeds else None, + delivered=delivered, homed=sorter.homed, where=where, + crossings=cr) + results.append(row) + print(f" rate {rate:6.0f} deg/s -> crossed {len(cr)}/{len(order)} " + f"cross speed mean {row['mean_cross_speed']} max {row['max_cross_speed']} " + f"delivered {delivered} homed {sorter.homed}") + +print("\n===== SWEEP RATE TUNING =====") +print(f"{'rate':>7} {'crossed':>9} {'mean v':>8} {'max v':>7} {'delivered':>10} {'homed':>6}") +for r in results: + print(f"{r['rate']:7.0f} {r['crossed']:4d}/{r['of']:<4d} " + f"{str(r['mean_cross_speed']):>8} {str(r['max_cross_speed']):>7} " + f"{r['delivered']:10d} {r['homed']:6d}") +best = max(results, key=lambda r: (r["delivered"], r["crossed"], -(r["max_cross_speed"] or 9))) +print(f"\nbest so far: {best['rate']:.0f} deg/s " + f"(tip {C.PLOW_ARM_LEN * best['rate'] * 3.14159 / 180:.2f} m/s)") + +import os +os.makedirs(os.path.dirname(OUT), exist_ok=True) +json.dump(dict(rates=RATES, belt=SPEED, pitch=PITCH, results=results), + open(OUT, "w"), indent=2) +print(f"log -> {OUT}") diff --git a/scripts/tune_sweep_standalone.py b/scripts/tune_sweep_standalone.py new file mode 100644 index 0000000..769f646 --- /dev/null +++ b/scripts/tune_sweep_standalone.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 +"""Sweep-rate tuning as a standalone Isaac Sim process. + + /home/whatevenif/isaacsim/python.sh scripts/tune_sweep_standalone.py \ + --rates 120,200,300,450 --items 4 --headless + +Same experiment as `tune_sweep_rate.py`, but it brings up its own `SimulationApp` instead +of being sent into a live Kit through the code-editor socket. That socket stopped returning +results on anything longer than a minute or two - the code kept running (one run wrote its +log in full) but the reply never arrived, so six runs in a row looked like failures. A +standalone process writes its log itself and can be read afterwards, which removes the +connection from the experiment entirely. + +Each rate is judged on the lane-entry beams, not on delivery alone: an item left on the belt +and an item thrown to the floor both score zero and need opposite corrections. + + crossed the push reached the lane -> low means the sweep is too slow + speed how fast it crossed -> high means it is throwing them + homed completed returns to centre -> the blade must park in the middle between items +""" +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent + + +def parse_args(argv=None): + p = argparse.ArgumentParser(description="tune the plow sweep rate") + p.add_argument("--rates", default="120,200,300,450", help="deg/s, comma separated") + p.add_argument("--items", type=int, default=4, help="how many B/C items per rate") + p.add_argument("--speed", type=float, default=0.8, help="belt m/s") + p.add_argument("--pitch", type=float, default=3.5, help="metres between items") + p.add_argument("--seconds", type=float, default=60.0, help="budget per rate") + p.add_argument("--headless", action="store_true") + p.add_argument("--out", default=str(ROOT / "runs" / "sweep_tuning.json")) + return p.parse_args(argv) + + +async def _run(app_utils, args): + import omni.timeline + from omni.physx import get_physx_interface + + from robozon_sorter import config as C + from robozon_sorter.sim import lane_beams, plow_sort, plow_vision, staging + from robozon_sorter.sim.mechanics import Cell + from robozon_sorter.sim.spawner import AutoFeeder + + rates = [float(r) for r in args.rates.split(",") if r.strip()] + stage, info = plow_vision.load(belt_speed=args.speed, script_control=True, + meshes_dir=str(ROOT / "assets" / "items")) + staging.stage_cell(stage, preset="bright", floor=True) + plow_sort.keep_lanes_active(stage) + lanes = plow_sort.configure_lanes(stage, args.speed) + plow_sort.open_junction(stage) + items = {k: v["zone"] for k, v in info["items"].items()} + print(f"scene ready: {len(items)} items, {len(lanes)} lanes/decks driven") + + await app_utils.update_app_async(steps=40) + cell = Cell(stage, items.keys()) + order = [n for n in sorted(items) if items[n] in ("B", "C")][:args.items] + want = {"B": "container_B", "C": "container_C"} + print(f"tuning on {len(order)} B/C items: {order}") + + timeline = omni.timeline.get_timeline_interface() + results = [] + for rate in rates: + C.PLOW_SWEEP_RATE = rate + cell.park_all() + await app_utils.update_app_async(steps=20) + + sorter = plow_sort.PlowSorter(stage, cell, items, plow_sort.calibrate_mapping()) + beams = lane_beams.LaneBeams(stage, cell, plow=sorter.plow) + + def _step(dt, _s=sorter, _b=beams, _r=rate): + try: + _s.update(dt) + _b.tick(dt) + _b.poll(rate=_r) + except Exception: + pass + + sub = get_physx_interface().subscribe_physics_step_events(_step) + feeder = AutoFeeder(cell, order=order, pitch=args.pitch, route={}).install() + app_utils.play(commit=True) + await app_utils.update_app_async(steps=20) + + for _ in range(int(args.seconds * 4)): + await app_utils.update_app_async(steps=15) + if len(beams.crossings) >= len(order): + break + app_utils.stop() + await app_utils.update_app_async(steps=15) + feeder.remove() + sub = None + + cr = beams.report() + sp = [c["speed"] for c in cr] + where = {n: sorter.lane_of(n) for n in order} + delivered = sum(1 for n in order if where[n] == want[items[n]]) + row = dict(rate=rate, crossed=len(cr), of=len(order), delivered=delivered, + homed=sorter.homed, + mean_cross_speed=round(sum(sp) / len(sp), 2) if sp else None, + max_cross_speed=round(max(sp), 2) if sp else None, + where=where, crossings=cr) + results.append(row) + print(f" rate {rate:6.0f} deg/s (tip {C.PLOW_ARM_LEN * rate * 3.14159 / 180:.2f} m/s)" + f" -> crossed {len(cr)}/{len(order)} delivered {delivered} " + f"homed {sorter.homed} cross v mean {row['mean_cross_speed']} " + f"max {row['max_cross_speed']}") + + print("\n===== SWEEP RATE TUNING =====") + print(f"{'rate':>7} {'tip m/s':>8} {'crossed':>9} {'delivered':>10} {'homed':>6} " + f"{'mean v':>8} {'max v':>7}") + for r in results: + print(f"{r['rate']:7.0f} {0.6 * r['rate'] * 3.14159 / 180:8.2f} " + f"{r['crossed']:4d}/{r['of']:<4d} {r['delivered']:10d} {r['homed']:6d} " + f"{str(r['mean_cross_speed']):>8} {str(r['max_cross_speed']):>7}") + if results: + best = max(results, key=lambda r: (r["delivered"], r["crossed"], + -(r["max_cross_speed"] or 99))) + print(f"\nbest: {best['rate']:.0f} deg/s " + f"crossed {best['crossed']}/{best['of']} delivered {best['delivered']}") + + out = Path(args.out) + out.parent.mkdir(parents=True, exist_ok=True) + out.write_text(json.dumps(dict(rates=rates, belt=args.speed, pitch=args.pitch, + items=order, results=results), indent=2)) + print(f"log -> {out}") + return results + + +def main(argv=None): + args = parse_args(argv) + if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) + from isaacsim import SimulationApp + app = SimulationApp({"headless": args.headless, "width": 1280, "height": 800}) + try: + import asyncio + import isaacsim.core.experimental.utils.app as app_utils + return asyncio.get_event_loop().run_until_complete(_run(app_utils, args)) + finally: + app.close() + + +if __name__ == "__main__": + sys.exit(0 if main() else 1) diff --git a/scripts/verify_belts.py b/scripts/verify_belts.py new file mode 100644 index 0000000..e23f0df --- /dev/null +++ b/scripts/verify_belts.py @@ -0,0 +1,190 @@ +"""Проверка переноса товара трением на 1 м/с по ВСЕМ дорожкам новой сцены 90/45. + +Почему это не просто "запустить существующий код". Список лент в scene.py жёстко +заканчивается на ConveyorTrack_05, а в новой сборке есть седьмая дорожка - +ConveyorTrack_06, угловая секция 1.04 x 1.05 м в точке (-8.0, +0.25). Кроме того из +сцены пропала корневая /ConveyorTrack_01 - 45-градусная дорожка плуга, которую этот +угол заменил. Поэтому ленты перечисляются здесь заново, по факту сцены. + +surfaceVelocity задаётся в ЛОКАЛЬНОЙ системе тела, и дорожки уложены по-разному: +у ConveyorTrack_04 и _06 локальный +X смотрит в мировой -X. Направление выводится из +мировой цели, а величина делится на то, сколько мирового стоит одна локальная единица - +у _06 масштаб 0.5, и без этого деления скорость вышла бы вдвое меньше. + +Скорость измеряется по фактическому перемещению тел, а не по заданному атрибуту: +атрибут можно выставить и не заметить, что трения не хватает и товар проскальзывает. +""" +import sys, math + +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell + +SPEED = 1.0 +SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd" + +# дорожка -> мировое направление, куда она должна везти. Выведено из раскладки: +# главный ход идёт в -X, ветка пушера в +Y, угловая секция _06 уводит в +Y. +INTENT = { + "/World/ConveyorTrack_05/Belt": (-1, 0, 0), + "/World/ConveyorTrack/Belt": (-1, 0, 0), + "/World/ConveyorTrack_02/Belt": (-1, 0, 0), + "/World/ConveyorTrack_03/Belt": (-1, 0, 0), + "/World/ConveyorTrack_04/Belt": (-1, 0, 0), + "/World/ConveyorTrack_01/Belt": (-1, 0, 0), + "/World/ConveyorTrack_06/Belt": (0, 1, 0), # угол на 90 градусов + "/World/ConveyorTrack_03/Belt_01": (0, 1, 0), # ветка пушера +} + +stage = omni.usd.get_context().get_stage() +if stage is None or SCENE not in stage.GetRootLayer().identifier: + omni.usd.get_context().open_stage(SCENE) + await app_utils.update_app_async(steps=60) + stage = omni.usd.get_context().get_stage() +print("сцена:", stage.GetRootLayer().identifier) + +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop() + await app_utils.update_app_async(steps=10) + +# --- физическая сцена ----------------------------------------------------------------- +ps = None +for p in stage.Traverse(): + if p.IsA(UsdPhysics.Scene): + ps = p; break +if ps is None: + ps = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim() + print("физическая сцена создана") +px = PhysxSchema.PhysxSceneAPI.Apply(ps) +hz = px.GetTimeStepsPerSecondAttr().Get() or 60 +if hz < 120: + px.CreateTimeStepsPerSecondAttr().Set(120); hz = 120 +px.CreateEnableCCDAttr().Set(True) +print(f"физика: {ps.GetPath()}, {hz} Гц, CCD включён") + +# --- поверхности лент: измерить, а не угадать ------------------------------------------ +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +surf = {} +for path in INTENT: + pr = stage.GetPrimAtPath(path) + if not pr.IsValid(): + print(f" НЕТ ПРЕМА {path}"); continue + r = bb.ComputeWorldBound(pr).ComputeAlignedRange() + surf[path] = (r.GetMin(), r.GetMax()) + +# --- привод лент ---------------------------------------------------------------------- +print(f"\nПРИВОД ЛЕНТ на {SPEED} м/с:") +driven = {} +for path, intent in INTENT.items(): + if path not in surf: + continue + v = plow_cell.drive_belt(stage, path, intent, SPEED) + driven[path] = v + mn, mx = surf[path] + print(f" {path:36s} цель {intent} локальная v={v} верх z={mx[2]:.3f}") + +# графы конвейера гасим на ВСЕХ семи дорожках - код проекта знает только шесть +off = 0 +for p in stage.Traverse(): + if "ConveyorBeltGraph" in p.GetName(): + p.SetActive(False); off += 1 +print(f" отключено графов ConveyorBeltGraph: {off}") + +# --- пробные тела ---------------------------------------------------------------------- +TESTS = [ + ("main_05", "/World/ConveyorTrack_05/Belt", (+1.20, 0.00)), + ("main_00", "/World/ConveyorTrack/Belt", (-1.00, 0.00)), + ("fork_03", "/World/ConveyorTrack_03/Belt", (-3.00, 0.00)), + ("plow_04", "/World/ConveyorTrack_04/Belt", (-6.60, 0.00)), + ("lane_01", "/World/ConveyorTrack_01/Belt", (-8.60, -0.22)), + ("corner_06","/World/ConveyorTrack_06/Belt", (-8.45, 0.35)), + ("branch", "/World/ConveyorTrack_03/Belt_01", (-3.90, 0.60)), +] +ROOT = "/World/_BeltProbe" +if stage.GetPrimAtPath(ROOT).IsValid(): + stage.RemovePrim(ROOT) +stage.DefinePrim(ROOT, "Xform") + +made = [] +for name, belt, (x, y) in TESTS: + if belt not in surf: + print(f" проба {name}: нет ленты {belt}"); continue + top = surf[belt][1][2] + path = f"{ROOT}/{name}" + cube = UsdGeom.Cube.Define(stage, path) + cube.CreateSizeAttr().Set(2.0) # half-extent = 1, масштабом задаём 6 см + xf = UsdGeom.Xformable(cube.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d(x, y, top + 0.045)) + xf.AddScaleOp().Set(Gf.Vec3f(0.03, 0.03, 0.03)) + pr = cube.GetPrim() + UsdPhysics.RigidBodyAPI.Apply(pr) + UsdPhysics.CollisionAPI.Apply(pr) + UsdPhysics.MassAPI.Apply(pr).CreateMassAttr().Set(0.5) + rb = PhysxSchema.PhysxRigidBodyAPI.Apply(pr) + rb.CreateEnableCCDAttr().Set(True) + rb.CreateSolverPositionIterationCountAttr().Set(32) + rb.CreateSolverVelocityIterationCountAttr().Set(8) + made.append((name, belt, path, x, y, top)) +print(f"\nпроб создано: {len(made)}") + +# --- прогон ---------------------------------------------------------------------------- +paths = [m[2] for m in made] +tl.play() +await app_utils.update_app_async(steps=30) # осадка на ленте +rp = RigidPrim(paths=paths) + +SAMPLES, EVERY = 26, 8 +import time +traj = [] +for i in range(SAMPLES): + pos, _ = rp.get_world_poses() + traj.append(pos.numpy().copy()) + await app_utils.update_app_async(steps=EVERY) +tl.stop() +await app_utils.update_app_async(steps=5) + +dt = EVERY / float(hz) +print(f"\nШАГ ВЫБОРКИ {dt*1000:.1f} мс, всего {SAMPLES} выборок ({SAMPLES*dt:.2f} с)\n") +print(f" {'проба':10s} {'дорожка':22s} {'путь,мм':>9s} {'|v|,м/с':>8s} {'напр.':>16s} {'оценка':>12s}") +print(" " + "-" * 88) +res = {} +for k, (name, belt, path, x0, y0, top) in enumerate(made): + P = [t[k] for t in traj] + # установившаяся скорость: по второй половине выборки, чтобы отбросить осадку + h = len(P) // 2 + d = P[-1] - P[h] + span = (len(P) - 1 - h) * dt + v = d / span + sp = float((v[0]**2 + v[1]**2) ** 0.5) + total = float(((P[-1][0]-P[0][0])**2 + (P[-1][1]-P[0][1])**2) ** 0.5) * 1000 + dz = float(P[-1][2] - P[0][2]) + want = INTENT[belt] + wn = math.sqrt(want[0]**2 + want[1]**2) or 1 + cosang = (v[0]*want[0] + v[1]*want[1]) / (sp * wn) if sp > 1e-4 else 0.0 + if dz < -0.05: + verdict = "УПАЛ" + elif sp < 0.15: + verdict = "СТОИТ" + elif cosang < 0.7: + verdict = "НЕ ТУДА" + elif sp < 0.80 * SPEED: + verdict = "буксует" + else: + verdict = "ок" + print(f" {name:10s} {belt.split('/World/')[-1]:22s} {total:9.0f} {sp:8.2f} " + f"({v[0]:+.2f},{v[1]:+.2f}) {verdict:>12s}") + res[name] = dict(speed=round(sp, 3), dir=[round(float(v[0]), 3), round(float(v[1]), 3)], + dz=round(dz, 3), verdict=verdict) + +ok = sum(1 for v in res.values() if v["verdict"] == "ок") +print(f"\n ИТОГ: {ok} из {len(res)} дорожек везут товар на {SPEED} м/с в нужную сторону") +globals()["BELT_RESULT"] = res diff --git a/scripts/verify_belts2.py b/scripts/verify_belts2.py new file mode 100644 index 0000000..91cf0cc --- /dev/null +++ b/scripts/verify_belts2.py @@ -0,0 +1,208 @@ +"""Перенос трением на 1 м/с по всем семи дорожкам. Чистая последовательность. + +Что выяснилось предыдущими прогонами и почему порядок именно такой: + +* Значение surfaceVelocity записывается верно, но ОБНУЛЯЕТСЯ в течение 5 шагов после + play. Пишет ноль авторский узел ConveyorBeltGraph: собственной скорости он не несёт и + на каждом тике кладёт свою. SetActive(False) на преме графа этого не останавливает - + прем перестаёт обходиться, но уже собранный граф OmniGraph продолжает работать. + Поэтому узлы УДАЛЯЮТСЯ, а сцена переоткрывается, чтобы граф не пережил правку. + +* Список лент в scene.py заканчивается на ConveyorTrack_05, а в этой сборке семь дорожек: + добавлена угловая ConveyorTrack_06 (поворот на 90 градусов), и исчезла корневая + /ConveyorTrack_01 - прежняя 45-градусная дорожка плуга, которую этот угол заменил. + +* surfaceVelocity задаётся в ЛОКАЛЬНОЙ системе тела, а дорожки уложены по-разному: + у _04 и _06 локальный +X смотрит в мировой -X. Направление выводится из мировой цели, + величина делится на то, сколько мирового стоит одна локальная единица - у _06 масштаб + 0.5, и без деления скорость вышла бы вдвое меньше. + +Скорость меряется по фактическому перемещению тел ВО ВРЕМЯ прогона: stop() возвращает +сцену в исходное состояние, и замер после него показывает точки рождения независимо от +того, ехал товар или нет. +""" +import sys, math +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell + +SPEED = 1.0 +SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd" +INTENT = { + "/World/ConveyorTrack_05/Belt": (-1, 0, 0), + "/World/ConveyorTrack/Belt": (-1, 0, 0), + "/World/ConveyorTrack_02/Belt": (-1, 0, 0), + "/World/ConveyorTrack_03/Belt": (-1, 0, 0), + "/World/ConveyorTrack_04/Belt": (-1, 0, 0), + "/World/ConveyorTrack_01/Belt": (-1, 0, 0), + "/World/ConveyorTrack_06/Belt": (0, 1, 0), + "/World/ConveyorTrack_03/Belt_01": (0, 1, 0), +} +# Пробы ставятся в НАЧАЛО своей секции по ходу движения: в прошлом прогоне они +# проезжали секцию за 1.2 с и упирались, а скорость я считал по второй половине окна - +# то есть уже по стоящему телу. Отсюда были ложные "стоит" при пройденных 1475 мм. +TESTS = [ + ("main_05", "/World/ConveyorTrack_05/Belt", (+1.85, 0.00)), + ("main_00", "/World/ConveyorTrack/Belt", (-0.15, 0.00)), + ("fork_03", "/World/ConveyorTrack_03/Belt", (-2.15, 0.00)), + ("plow_04", "/World/ConveyorTrack_04/Belt", (-6.15, 0.00)), + ("lane_01", "/World/ConveyorTrack_01/Belt", (-8.15, -0.22)), + ("corner_06", "/World/ConveyorTrack_06/Belt", (-8.45, 0.12)), + ("branch", "/World/ConveyorTrack_03/Belt_01", (-4.20, 0.55)), +] + +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +# 1. переоткрыть - чтобы не остался собранный граф от прошлой правки +omni.usd.get_context().open_stage(SCENE) +await app_utils.update_app_async(steps=60) +stage = omni.usd.get_context().get_stage() +print("сцена переоткрыта:", stage.GetRootLayer().identifier) + +# 2. УДАЛИТЬ узлы конвейерного графа (гасить недостаточно) +doomed = [p.GetPath() for p in stage.Traverse() if "ConveyorBeltGraph" in p.GetName()] +for path in doomed: + stage.RemovePrim(path) +print(f"удалено узлов ConveyorBeltGraph: {len(doomed)}") +anim = stage.GetPrimAtPath(plow_cell.ANIM_GRAPH) +if anim.IsValid(): + stage.RemovePrim(anim.GetPath()); print("удалён DiverterAnimGraph (иначе перетирает приводы)") +await app_utils.update_app_async(steps=20) + +# 3. физика +ps = next((p for p in stage.Traverse() if p.IsA(UsdPhysics.Scene)), None) +if ps is None: + ps = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim() +px = PhysxSchema.PhysxSceneAPI.Apply(ps) +hz = px.GetTimeStepsPerSecondAttr().Get() or 60 +if hz < 120: + px.CreateTimeStepsPerSecondAttr().Set(120); hz = 120 +px.CreateEnableCCDAttr().Set(True) +px.CreateEnableStabilizationAttr().Set(True) +print(f"физика: {ps.GetPath()}, {hz} Гц") + +# 4. привод лент +GRIP = "/World/_BeltGrip" +g = stage.GetPrimAtPath(GRIP) +if not g.IsValid(): + g = stage.DefinePrim(GRIP, "Material") +pm = UsdPhysics.MaterialAPI.Apply(g) +pm.CreateStaticFrictionAttr().Set(1.1) +pm.CreateDynamicFrictionAttr().Set(0.95) +pm.CreateRestitutionAttr().Set(0.02) + +print(f"\nПРИВОД на {SPEED} м/с:") +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +surf = {} +for path, intent in INTENT.items(): + pr = stage.GetPrimAtPath(path) + if not pr.IsValid(): + print(f" {path}: НЕТ ПРЕМА"); continue + surf[path] = bb.ComputeWorldBound(pr).ComputeAlignedRange() + v = plow_cell.drive_belt(stage, path, intent, SPEED, grip_path=GRIP) + en = PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr() + en.Set(True) + M = UsdGeom.XformCache().GetLocalToWorldTransform(pr) + per = M.TransformDir(Gf.Vec3d(1, 0, 0)).GetLength() + print(f" {path.split('/World/')[-1]:26s} цель{str(intent):12s} локальная v={v}" + f" мир/локаль по X = {per:.3f}") + +# 5. пробы +ROOT = "/World/_BeltProbe" +if stage.GetPrimAtPath(ROOT).IsValid(): + stage.RemovePrim(ROOT) +stage.DefinePrim(ROOT, "Xform") +made = [] +for name, belt, (x, y) in TESTS: + if belt not in surf: + continue + top = surf[belt].GetMax()[2] + path = f"{ROOT}/{name}" + cube = UsdGeom.Cube.Define(stage, path) + cube.CreateSizeAttr().Set(2.0) + xf = UsdGeom.Xformable(cube.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d(x, y, top + 0.035)) + xf.AddScaleOp().Set(Gf.Vec3f(0.03, 0.03, 0.03)) + pp = cube.GetPrim() + UsdPhysics.RigidBodyAPI.Apply(pp) + UsdPhysics.CollisionAPI.Apply(pp) + UsdPhysics.MassAPI.Apply(pp).CreateMassAttr().Set(0.5) + rb = PhysxSchema.PhysxRigidBodyAPI.Apply(pp) + rb.CreateEnableCCDAttr().Set(True) + rb.CreateSolverPositionIterationCountAttr().Set(32) + UsdShade.MaterialBindingAPI.Apply(pp).Bind( + UsdShade.Material(g), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + made.append((name, belt, path)) +print(f"проб: {len(made)}") + +# 6. прогон +tl.play() +await app_utils.update_app_async(steps=40) +belt0 = stage.GetPrimAtPath("/World/ConveyorTrack/Belt") +print("контроль после play: surfaceVelocity =", + belt0.GetAttribute("physxSurfaceVelocity:surfaceVelocity").Get()) + +rp = RigidPrim(paths=[m[2] for m in made]) +EVERY, N = 5, 26 +# Время берётся ИЗ ТАЙМЛАЙНА. Заданная timeStepsPerSecond и фактический шаг физики +# могут расходиться, а от этого напрямую зависит вычисленная скорость: при ошибке +# вдвое лента "поедет" вдвое быстрее, ничего на самом деле не изменив. +traj, tstamp = [], [] +for i in range(N): + traj.append(rp.get_world_poses()[0].numpy().copy()) + tstamp.append(float(tl.get_current_time())) + await app_utils.update_app_async(steps=EVERY) +spans = [b - a for a, b in zip(tstamp[:-1], tstamp[1:])] +dt = sum(spans) / len(spans) if spans else EVERY / float(hz) +print(f"фактический шаг таймлайна {dt*1000:.2f} мс против {EVERY/float(hz)*1000:.2f} мс " + f"по заданным {hz} Гц") +print(f"\nвыборка каждые {dt*1000:.0f} мс, {N} точек ({N*dt:.2f} с)\n") +print(f" {'проба':10s} {'дорожка':24s} {'путь,мм':>8s} {'v уст.,м/с':>10s} {'направление':>18s} оценка") +print(" " + "-" * 90) +res = {} +for k, (name, belt, path) in enumerate(made): + P = [t[k] for t in traj] + # мгновенные скорости между соседними выборками + inst = [] + for j, (a, b) in enumerate(zip(P[:-1], P[1:])): + h = spans[j] if j < len(spans) and spans[j] > 1e-6 else dt + dx, dy = float(b[0]-a[0]), float(b[1]-a[1]) + inst.append((math.hypot(dx, dy)/h, dx/h, dy/h)) + moving = [t for t in inst if t[0] > 0.15] + total = float(math.hypot(P[-1][0]-P[0][0], P[-1][1]-P[0][1])) * 1000 + dz = float(min(p[2] for p in P) - P[0][2]) + if not moving: + sp, vx, vy = 0.0, 0.0, 0.0 + else: + # установившаяся: медиана верхней половины, чтобы отбросить разгон и упор + moving.sort(key=lambda t: t[0]) + top = moving[len(moving)//2:] + sp = sorted(t[0] for t in top)[len(top)//2] + vx = sum(t[1] for t in top)/len(top) + vy = sum(t[2] for t in top)/len(top) + w = INTENT[belt]; wn = math.hypot(w[0], w[1]) or 1 + cos = (vx*w[0] + vy*w[1]) / (sp*wn) if sp > 1e-3 else 0.0 + if dz < -0.10: verdict = "УПАЛ" + elif sp < 0.15: verdict = "СТОИТ" + elif cos < 0.7: verdict = "НЕ ТУДА" + elif sp < 0.80 * SPEED: verdict = f"буксует {sp/SPEED*100:.0f}%" + else: verdict = f"ок ({sp/SPEED*100:.0f}%)" + print(f" {name:10s} {belt.split('/World/')[-1]:24s} {total:8.0f} {sp:10.2f} " + f"({vx:+.2f},{vy:+.2f}) {verdict}") + res[name] = dict(v=round(sp, 3), path_mm=round(total), verdict=verdict) +tl.stop() +await app_utils.update_app_async(steps=5) +ok = sum(1 for r in res.values() if r["verdict"].startswith("ок")) +print(f"\n ИТОГ: {ok} из {len(res)} дорожек везут на {SPEED} м/с в нужную сторону") +globals()["BELTS_OK"] = res diff --git a/scripts/verify_plow.py b/scripts/verify_plow.py new file mode 100644 index 0000000..9b29332 --- /dev/null +++ b/scripts/verify_plow.py @@ -0,0 +1,187 @@ +"""Отработка пушера и плуга на новой сцене: поворот по классу и скольжение товара по лезвию. + +Опирается на выясненное прогонами лент: +* узлы ConveyorBeltGraph нужно УДАЛЯТЬ - деактивация не мешает уже собранному графу + обнулять surfaceVelocity на каждом тике; +* DiverterAnimGraph тоже удаляется: он переписывает цели приводов каждый тик и затирает + всё, что задаёт Python; +* время берётся из таймлайна - заданная частота физики не применяется, фактический шаг + 83.33 мс (60 Гц), и от этого напрямую зависит вычисленная скорость. + +Плуг ставится в положение ДО подхода товара - это и есть предпозиционирование по классу. +Скольжение по лезвию меряется как путь товара ВДОЛЬ кромки за время контакта: если товар +только отбрасывается, поперечная составляющая есть, а продольной нет. +""" +import sys, math +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell + +SPEED = 1.0 +SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd" +INTENT = { + "/World/ConveyorTrack_05/Belt": (-1, 0, 0), "/World/ConveyorTrack/Belt": (-1, 0, 0), + "/World/ConveyorTrack_02/Belt": (-1, 0, 0), "/World/ConveyorTrack_03/Belt": (-1, 0, 0), + "/World/ConveyorTrack_04/Belt": (-1, 0, 0), "/World/ConveyorTrack_01/Belt": (-1, 0, 0), + "/World/ConveyorTrack_06/Belt": (0, 1, 0), "/World/ConveyorTrack_03/Belt_01": (0, 1, 0), +} + +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) +omni.usd.get_context().open_stage(SCENE) +await app_utils.update_app_async(steps=60) +stage = omni.usd.get_context().get_stage() + +for path in [p.GetPath() for p in stage.Traverse() + if "ConveyorBeltGraph" in p.GetName() or "DiverterAnimGraph" in p.GetName()]: + stage.RemovePrim(path) +await app_utils.update_app_async(steps=20) + +ps = next((p for p in stage.Traverse() if p.IsA(UsdPhysics.Scene)), None) +if ps is None: + ps = UsdPhysics.Scene.Define(stage, "/World/PhysicsScene").GetPrim() +PhysxSchema.PhysxSceneAPI.Apply(ps).CreateEnableCCDAttr().Set(True) + +GRIP = "/World/_BeltGrip" +g = stage.GetPrimAtPath(GRIP) +if not g.IsValid(): + g = stage.DefinePrim(GRIP, "Material") +pm = UsdPhysics.MaterialAPI.Apply(g) +pm.CreateStaticFrictionAttr().Set(1.1); pm.CreateDynamicFrictionAttr().Set(0.95) +pm.CreateRestitutionAttr().Set(0.02) +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +for path, intent in INTENT.items(): + pr = stage.GetPrimAtPath(path) + if pr.IsValid(): + plow_cell.drive_belt(stage, path, intent, SPEED, grip_path=GRIP) + PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True) +TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_04/Belt") + ).ComputeAlignedRange().GetMax()[2] +print(f"сцена готова, ленты на {SPEED} м/с, верх ленты z={TOP:.3f}") + +# --- механизмы ------------------------------------------------------------------------ +hinge = stage.GetPrimAtPath(C.PLOW_HINGE) +slide = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/PusherSlide") +print(f"плуг {C.PLOW_HINGE}: {hinge.IsValid()} пушер PusherSlide: {slide.IsValid()}") +hdrive = UsdPhysics.DriveAPI(hinge, "angular") +pdrive = UsdPhysics.DriveAPI(slide, "linear") +arm = stage.GetPrimAtPath(C.PLOW_ARM) + +def yaw_of(prim): + M = UsdGeom.XformCache().GetLocalToWorldTransform(prim) + d = M.TransformDir(Gf.Vec3d(1, 0, 0)) + return math.degrees(math.atan2(d[1], d[0])) + +def spawn(name, x, y, size=0.05, mass=0.5): + path = f"/World/_Goods/{name}" + c = UsdGeom.Cube.Define(stage, path) + c.CreateSizeAttr().Set(2.0) + xf = UsdGeom.Xformable(c.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d(x, y, TOP + size + 0.005)) + xf.AddScaleOp().Set(Gf.Vec3f(size, size, size)) + p = c.GetPrim() + UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p) + UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(mass) + rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p) + rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32) + UsdShade.MaterialBindingAPI.Apply(p).Bind( + UsdShade.Material(g), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + return path + +async def run_case(label, plow_deg, start_x=-6.30, start_y=0.0, seconds=4.0): + """поставить плуг ЗАРАНЕЕ, пустить товар, проследить его через плуг""" + if stage.GetPrimAtPath("/World/_Goods").IsValid(): + stage.RemovePrim("/World/_Goods") + stage.DefinePrim("/World/_Goods", "Xform") + path = spawn(label, start_x, start_y) + if hdrive: + hdrive.GetTargetPositionAttr().Set(float(plow_deg)) + hdrive.CreateStiffnessAttr().Set(C.PLOW_STIFFNESS) + hdrive.CreateDampingAttr().Set(C.PLOW_DAMPING) + hdrive.CreateMaxForceAttr().Set(C.PLOW_MAX_FORCE) + tl.play() + await app_utils.update_app_async(steps=45) # дать лезвию встать ДО подхода + reached = yaw_of(arm) if arm.IsValid() else None + rp = RigidPrim(paths=[path]) + T, P = [], [] + t_end = float(tl.get_current_time()) + seconds + while float(tl.get_current_time()) < t_end: + P.append(rp.get_world_poses()[0].numpy()[0].copy()) + T.append(float(tl.get_current_time())) + await app_utils.update_app_async(steps=3) + tl.stop(); await app_utils.update_app_async(steps=5) + return label, plow_deg, reached, T, P + +CASES = [("D_прямо", C.PLOW_PRESET["D"]), ("B_влево", C.PLOW_PRESET["B"]), + ("C_вправо", C.PLOW_PRESET["C"]), ("B_широкий", C.PLOW_B_ANGLE)] +print(f"\nуглы из конфига: preset={C.PLOW_PRESET} PLOW_B_ANGLE={C.PLOW_B_ANGLE} " + f"PLOW_X={C.PLOW_X}\n") +print(f" {'случай':11s} {'цель°':>6s} {'факт°':>6s} {'смещ.Y,мм':>10s} {'скольж.,мм':>11s} " + f"{'конец X,Y':>16s} сторона") +print(" " + "-" * 84) +out = {} +for label, deg in CASES: + label, deg, reached, T, P = await run_case(label, deg) + if len(P) < 5: + print(f" {label:11s} нет данных"); continue + y0 = float(P[0][1]); dy = float(P[-1][1]) - y0 + # скольжение по лезвию: путь ВДОЛЬ кромки за время контакта с зоной плуга + slide_mm, prev = 0.0, None + for p in P: + if C.PLOW_SWEEP_X1 >= float(p[0]) >= C.PLOW_SWEEP_X0: + if prev is not None: + a = math.radians(reached if reached is not None else deg) + ex, ey = math.cos(a), math.sin(a) # направление кромки + slide_mm += abs((float(p[0])-prev[0])*ex + (float(p[1])-prev[1])*ey) * 1000 + prev = (float(p[0]), float(p[1])) + side = "+Y (B)" if dy > 0.05 else ("-Y (C)" if dy < -0.05 else "прямо") + print(f" {label:11s} {deg:6.1f} {(reached if reached is not None else float('nan')):6.1f} " + f"{dy*1000:10.0f} {slide_mm:11.0f} ({P[-1][0]:+6.2f},{P[-1][1]:+6.2f}) {side}") + out[label] = dict(target=deg, reached=reached, dy_mm=round(dy*1000), + slide_mm=round(slide_mm), end=[round(float(P[-1][0]), 2), + round(float(P[-1][1]), 2)], side=side) + +# --- пушер ------------------------------------------------------------------------------ +print("\nПУШЕР:") +if stage.GetPrimAtPath("/World/_Goods").IsValid(): + stage.RemovePrim("/World/_Goods") +stage.DefinePrim("/World/_Goods", "Xform") +gpath = spawn("push_D", -3.20, 0.0) +blade = stage.GetPrimAtPath("/World/Diverters/DiverterY_Split/Pusher") +tl.play(); await app_utils.update_app_async(steps=30) +rp = RigidPrim(paths=[gpath]) +b0 = bb.ComputeWorldBound(blade).ComputeAlignedRange().GetMidpoint() if blade.IsValid() else None +fired = False +T, P, BY = [], [], [] +t_end = float(tl.get_current_time()) + 5.0 +while float(tl.get_current_time()) < t_end: + p = rp.get_world_poses()[0].numpy()[0] + P.append(p.copy()); T.append(float(tl.get_current_time())) + if not fired and float(p[0]) <= C.PUSH_X + 0.10: + if pdrive: + pdrive.GetTargetPositionAttr().Set(0.45) # выдвинуть + fired = True + print(f" команда пушеру при x={float(p[0]):+.2f} (PUSH_X={C.PUSH_X})") + if blade.IsValid(): + BY.append(float(bb.ComputeWorldBound(blade).ComputeAlignedRange().GetMidpoint()[1])) + await app_utils.update_app_async(steps=3) +tl.stop(); await app_utils.update_app_async(steps=5) +if P: + dy = float(P[-1][1]) - float(P[0][1]) + stroke = (max(BY) - min(BY)) * 1000 if BY else 0.0 + print(f" ход лезвия пушера {stroke:.0f} мм") + print(f" товар: старт ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> " + f"конец ({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f}), смещение по Y {dy*1000:+.0f} мм") + print(f" {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.15 else 'товар НЕ уведён'}") + out["pusher"] = dict(stroke_mm=round(stroke), dy_mm=round(dy*1000)) +globals()["PLOW_RESULT"] = out diff --git a/scripts/verify_plow2.py b/scripts/verify_plow2.py new file mode 100644 index 0000000..3d7aa99 --- /dev/null +++ b/scripts/verify_plow2.py @@ -0,0 +1,159 @@ +"""Пушер и плуг новой сцены ШТАТНЫМ механизмом проекта. + +Прошлый прогон был поставлен неверно: я командовал силовым приводом шарнира, а проект от +него отказался. configure_plow(kinematic_arm=True) делает лезвие КИНЕМАТИЧЕСКИМ, выключает +сам шарнир (physics:jointEnabled=False) и пишет угол напрямую - в комментарии сказано, что +привод перенастраивали трижды и он не держал, звеня на +-21.4 градуса быстрее, чем его +успевала вести команда. Нож пушера так же не ездит по своему призматическому суставу: +сустав выключен, а нож переносится записью трансформа (mechanics.Cell.blade_to). +Поэтому здесь всё идёт через plow_cell.prepare() + Plow + Cell. + +Сверх штатного добавлено то, чего код проекта про эту сборку не знает: +* узлы ConveyorBeltGraph УДАЛЯЮТСЯ - deactivate недостаточно, уже собранный граф + продолжает обнулять surfaceVelocity на каждом тике; +* приводятся ConveyorTrack_05 и новая угловая ConveyorTrack_06: список лент в scene.py + заканчивается на _05 и седьмой дорожки не содержит. + +Время берётся из таймлайна: заданная частота физики не применяется, фактический шаг +83.33 мс, и на предположении о 120 Гц скорости выходили ровно вдвое завышенными. +""" +import sys, math +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim + +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell +from robozon_sorter.sim.plow import Plow + +SPEED = 1.0 +SCENE = f"{REPO}/scene/plow_cell_90_45_test.usd" + +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) +omni.usd.get_context().open_stage(SCENE) +await app_utils.update_app_async(steps=60) +stage = omni.usd.get_context().get_stage() + +killed = [p.GetPath() for p in stage.Traverse() if "ConveyorBeltGraph" in p.GetName()] +for path in killed: + stage.RemovePrim(path) +await app_utils.update_app_async(steps=10) + +info = plow_cell.prepare(stage, belt_speed=SPEED, script_control=True, kinematic_arm=True) +print(f"prepare: плуг готов={info['plow_ready']}, лент приведено={len(info['belts'])}, " + f"скорость={info['belt_speed']} (узлов графа удалено {len(killed)})") + +# дорожки, которых нет в списке проекта +for path, intent in (("/World/ConveyorTrack_05/Belt", (-1, 0, 0)), + ("/World/ConveyorTrack_06/Belt", (0, 1, 0))): + pr = stage.GetPrimAtPath(path) + if pr.IsValid(): + v = plow_cell.drive_belt(stage, path, intent, SPEED) + PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True) + print(f" дополнительно приведена {path.split('/World/')[-1]}: v={v}") +for path in plow_cell.BELTS + [plow_cell.BRANCH]: + pr = stage.GetPrimAtPath(path) + if pr.IsValid(): + PhysxSchema.PhysxSurfaceVelocityAPI(pr).CreateSurfaceVelocityEnabledAttr().Set(True) + +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_04/Belt") + ).ComputeAlignedRange().GetMax()[2] +GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL) + +def spawn(name, x, y, size=0.05, mass=0.5): + path = f"/World/_Goods/{name}" + c = UsdGeom.Cube.Define(stage, path); c.CreateSizeAttr().Set(2.0) + xf = UsdGeom.Xformable(c.GetPrim()) + xf.AddTranslateOp().Set(Gf.Vec3d(x, y, TOP + size + 0.005)) + xf.AddScaleOp().Set(Gf.Vec3f(size, size, size)) + p = c.GetPrim() + UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p) + UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(mass) + rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p) + rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32) + if GRIP.IsValid(): + UsdShade.MaterialBindingAPI.Apply(p).Bind( + UsdShade.Material(GRIP), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + return path + +# ---------- ПЛУГ: угол по классу, заранее ---------------------------------------------- +plow = Plow(stage) +print(f"\nПЛУГ. углы по классам {C.PLOW_PRESET}, широкий B={C.PLOW_B_ANGLE}, " + f"ось плуга X={C.PLOW_X}, кинематическое лезвие") +print(f" {'класс':10s} {'цель°':>6s} {'угол лезвия°':>13s} {'смещ.Y,мм':>10s} " + f"{'скольж.,мм':>11s} {'конец X,Y':>16s} вывод") +print(" " + "-" * 92) +out = {} +for label, deg in (("D", C.PLOW_PRESET["D"]), ("B", C.PLOW_PRESET["B"]), + ("C", C.PLOW_PRESET["C"]), ("B широкий", C.PLOW_B_ANGLE)): + if stage.GetPrimAtPath("/World/_Goods").IsValid(): + stage.RemovePrim("/World/_Goods") + stage.DefinePrim("/World/_Goods", "Xform") + gp = spawn("item", -6.30, 0.0) + plow.target(deg) # ЗАРАНЕЕ, до подхода товара + tl.play() + await app_utils.update_app_async(steps=30) + reached = plow.angle + rp = RigidPrim(paths=[gp]) + P, T = [], [] + t_end = float(tl.get_current_time()) + 4.0 + while float(tl.get_current_time()) < t_end: + P.append(rp.get_world_poses()[0].numpy()[0].copy()) + T.append(float(tl.get_current_time())) + await app_utils.update_app_async(steps=3) + tl.stop(); await app_utils.update_app_async(steps=5) + y0, dy = float(P[0][1]), float(P[-1][1]) - float(P[0][1]) + a = math.radians(reached) + ex, ey = math.cos(a), math.sin(a) + slide, prev = 0.0, None + for p in P: + if C.PLOW_SWEEP_X1 >= float(p[0]) >= C.PLOW_SWEEP_X0: + if prev is not None: + slide += abs((float(p[0])-prev[0])*ex + (float(p[1])-prev[1])*ey) * 1000 + prev = (float(p[0]), float(p[1])) + side = "ушёл в +Y" if dy > 0.05 else ("ушёл в -Y" if dy < -0.05 else "прошёл прямо") + print(f" {label:10s} {deg:6.1f} {reached:13.1f} {dy*1000:10.0f} {slide:11.0f} " + f"({float(P[-1][0]):+6.2f},{float(P[-1][1]):+6.2f}) {side}") + out[label] = dict(target=deg, reached=round(reached, 1), dy_mm=round(dy*1000), + slide_mm=round(slide), end=[round(float(P[-1][0]), 2), + round(float(P[-1][1]), 2)]) + +# ---------- ПУШЕР ------------------------------------------------------------------------ +print(f"\nПУШЕР. ход {C.BLADE_HOME_Y} -> {C.BLADE_OUT_Y} ({C.BLADE_STROKE*1000:.0f} мм), " + f"срабатывание у PUSH_X={C.PUSH_X}") +from robozon_sorter.sim.mechanics import Cell +if stage.GetPrimAtPath("/World/_Goods").IsValid(): + stage.RemovePrim("/World/_Goods") +stage.DefinePrim("/World/_Goods", "Xform") +gp = spawn("push_D", -2.80, 0.0) +cell = Cell(stage, items={}) +plow.target(C.PLOW_PRESET["D"]) +tl.play(); await app_utils.update_app_async(steps=25) +rp = RigidPrim(paths=[gp]) +P, fired, stroke_s = [], False, None +t_end = float(tl.get_current_time()) + 6.0 +while float(tl.get_current_time()) < t_end: + p = rp.get_world_poses()[0].numpy()[0] + P.append(p.copy()) + if not fired and float(p[0]) <= C.PUSH_X + 0.08: + print(f" товар дошёл до x={float(p[0]):+.2f} - ход ножа") + stroke_s = await cell.stroke(app_utils, out=True) + fired = True + await app_utils.update_app_async(steps=3) +tl.stop(); await app_utils.update_app_async(steps=5) +dy = float(P[-1][1]) - float(P[0][1]) +print(f" ход ножа занял {stroke_s if stroke_s else 0:.2f} с") +print(f" товар: ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> " + f"({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f}), по Y {dy*1000:+.0f} мм") +print(f" {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.15 else 'товар НЕ уведён на ветку'}") +out["pusher"] = dict(dy_mm=round(dy*1000), fired=fired) +globals()["PLOW2"] = out diff --git a/scripts/verify_pusher.py b/scripts/verify_pusher.py new file mode 100644 index 0000000..84c409d --- /dev/null +++ b/scripts/verify_pusher.py @@ -0,0 +1,96 @@ +"""Пушер после снятия коллизии с луча лазера. + +Товар вставал на x = -3.044 и не доходил до точки срабатывания PUSH_X = -3.9. Причина +найдена в списке коллайдеров: /World/SortingRig/LaserGate/Beam - визуализация луча +датчика - имеет ВКЛЮЧЁННУЮ коллизию и перекрывает всю ширину полотна (y -0.42..+0.42 +при ленте -0.45..+0.45) на высоте 19 мм над лентой. Датчик должен смотреть, а не +преграждать; коллизия с него снимается, после чего проверяется сам пушер. +""" +import sys, math +REPO = "/home/dasha/robozon-sorter" +if REPO not in sys.path: + sys.path.insert(0, REPO) + +import omni.usd, omni.timeline +import isaacsim.core.experimental.utils.app as app_utils +from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade +from isaacsim.core.experimental.prims import RigidPrim +from robozon_sorter import config as C +from robozon_sorter.sim import plow_cell +from robozon_sorter.sim.mechanics import Cell +from robozon_sorter.sim.plow import Plow + +stage = omni.usd.get_context().get_stage() +tl = omni.timeline.get_timeline_interface() +if tl.is_playing(): + tl.stop(); await app_utils.update_app_async(steps=10) + +# снять коллизию со всей визуализации датчиков +off = [] +for p in stage.Traverse(): + path = str(p.GetPath()) + if "LaserGate" in path or "AimRay" in path or "TriggerRay" in path: + a = p.GetAttribute("physics:collisionEnabled") + if a: + a.Set(False); off.append(path) + elif p.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI(p).CreateCollisionEnabledAttr().Set(False); off.append(path) +print(f"коллизия снята с {len(off)} премов визуализации датчиков:") +for o in off: + print(f" {o}") + +bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) +TOP = bb.ComputeWorldBound(stage.GetPrimAtPath("/World/ConveyorTrack_03/Belt") + ).ComputeAlignedRange().GetMax()[2] +GRIP = stage.GetPrimAtPath(plow_cell.GRIP_MATERIAL) + +if stage.GetPrimAtPath("/World/_Goods").IsValid(): + stage.RemovePrim("/World/_Goods") +stage.DefinePrim("/World/_Goods", "Xform") +c = UsdGeom.Cube.Define(stage, "/World/_Goods/push_D"); c.CreateSizeAttr().Set(2.0) +xf = UsdGeom.Xformable(c.GetPrim()) +xf.AddTranslateOp().Set(Gf.Vec3d(-2.40, 0.0, TOP + 0.055)) +xf.AddScaleOp().Set(Gf.Vec3f(0.05, 0.05, 0.05)) +p = c.GetPrim() +UsdPhysics.RigidBodyAPI.Apply(p); UsdPhysics.CollisionAPI.Apply(p) +UsdPhysics.MassAPI.Apply(p).CreateMassAttr().Set(0.5) +rb = PhysxSchema.PhysxRigidBodyAPI.Apply(p) +rb.CreateEnableCCDAttr().Set(True); rb.CreateSolverPositionIterationCountAttr().Set(32) +if GRIP.IsValid(): + UsdShade.MaterialBindingAPI.Apply(p).Bind( + UsdShade.Material(GRIP), bindingStrength=UsdShade.Tokens.strongerThanDescendants, + materialPurpose="physics") + +cell = Cell(stage, items={}) +plow = Plow(stage); plow.target(C.PLOW_PRESET["D"]) +print(f"\nход ножа {C.BLADE_HOME_Y} -> {C.BLADE_OUT_Y} ({C.BLADE_STROKE*1000:.0f} мм), " + f"срабатывание у PUSH_X={C.PUSH_X}, скорость ножа {C.PUSHER_SPEED} м/с") + +tl.play(); await app_utils.update_app_async(steps=25) +rp = RigidPrim(paths=["/World/_Goods/push_D"]) +P, fired, t_str = [], False, None +t0 = float(tl.get_current_time()); t_end = t0 + 7.0 +print("\n t,с x y событие") +while float(tl.get_current_time()) < t_end: + q = rp.get_world_poses()[0].numpy()[0] + P.append(q.copy()) + t = float(tl.get_current_time()) + ev = "" + if not fired and float(q[0]) <= C.PUSH_X + 0.08: + ev = "КОМАНДА ножу" + print(f" {t-t0:5.2f} {float(q[0]):+7.3f} {float(q[1]):+6.3f} {ev}") + t_str = await cell.stroke(app_utils, out=True) + fired = True + ev = "" + if len(P) % 8 == 1: + print(f" {t-t0:5.2f} {float(q[0]):+7.3f} {float(q[1]):+6.3f} {ev}") + await app_utils.update_app_async(steps=4) +tl.stop(); await app_utils.update_app_async(steps=5) + +dy = float(P[-1][1]) - float(P[0][1]) +dx = float(P[-1][0]) - float(P[0][0]) +print(f"\n нож сработал: {fired}, ход занял {t_str if t_str else 0:.2f} с") +print(f" товар: ({float(P[0][0]):+.2f},{float(P[0][1]):+.2f}) -> " + f"({float(P[-1][0]):+.2f},{float(P[-1][1]):+.2f})") +print(f" смещение: по X {dx*1000:+.0f} мм, по Y {dy*1000:+.0f} мм") +print(f" ВЫВОД: {'ТОВАР УВЕДЁН НА ВЕТКУ' if dy > 0.20 else 'товар НЕ уведён на ветку'}")