diff --git a/pylabrobot/hamilton/star/MOTION_PROFILES.md b/pylabrobot/hamilton/star/MOTION_PROFILES.md new file mode 100644 index 00000000000..c38a53c4673 --- /dev/null +++ b/pylabrobot/hamilton/star/MOTION_PROFILES.md @@ -0,0 +1,459 @@ +# Motion profiles, by firmware command + +What the viewer plays for each Hamilton STAR firmware command, and where every number and choice +comes from. Each command the v1 driver sends either has a motion profile (made of phases, played in +order) or has none. Each value is tagged with its source: + +| Tag | Meaning | +|---|---| +| **FW** | Carried by the command itself: the page plays what the device was told. | +| **PLR** | A value or model in PyLabRobot's code: a driver constant, a configuration, or the resource model. Cited by file and symbol. | +| **SIM** | How PLR's STAR simulator records the command (`hamilton/star/driver/simulator.py`). This is PLR's own model of the motion, not a documented firmware sequence. | +| **FIT** | Fitted to a real STAR's own command timings: every firmware request and reply, millisecond-stamped, from Venus HxUsbComm traces. Reproducible with `tools/hxusbcomm_timing.py`; data and statistics in section 8. | +| **DOC** | External documentation or a forum post, linked to the post. labautomation.io posts by Hamilton staff are treated as authoritative. | +| **HEUR** | Our choice, with no documented reference. The justification is given. | + +Files: the decoder is `pylabrobot/visualizer3D/motion.py` (Python, turns a command into targets); +the player is `static/motion_player.js` (the page, plays targets in phases); the profile is +`static/motion_profile.js`. + +--- + +## 0. Applies to every profile + +| Choice | Value | Tag | Source / justification | +|---|---|---|---| +| Speed profile of a single move | Symmetric trapezoid (a triangle if too short to cruise; constant speed with no acceleration), except the X-arm, which moves on a jerk-limited S-curve | SIM + FIT | Trapezoid: the simulator's timing model, `SimulatedPipettes._get_travel_time` (`simulator.py`). S-curve for X: the traces reject a trapezoid for the X-arm (section 8.1). Page: `motionProfile(distance, speed, acceleration, jerk)` in `static/motion_profile.js`. | +| Moves that happen at once | Drives in the same phase start together, and the phase ends with the slowest | SIM | `owe_motion_time`: "the drives move at once, so a command takes as long as its slowest axis" (`simulator.py`). | +| Smallest move played | 0.05 mm (`STILL`); anything smaller is skipped | HEUR | Below the 0.1 mm firmware resolution; avoids zero-length tweens. | +| Model arrivals | After each motion, the model's own update must find the page already there (0.11 mm / 0.11° tolerance in the harness) | PLR + HEUR | The model is the source of truth (PLR). The tolerance is ours: `ARRIVAL_MM` / `ARRIVAL_DEG` in `smoothness_page.mjs`, just above the firmware's 0.1 mm resolution. Enforced by `smoothness.py`'s `assert_smooth`. | +| Page hidden / background tab | Motions jump to their end | HEUR | Browsers throttle hidden tabs, and blocking the run on an unwatched page makes no sense. | +| Python waits for the page | Until every connected page reports `motion_done`, at most 120 s (`Viewer3D.MOTION_TIMEOUT_S`) | HEUR | Paces the simulated run to the drawing; the timeout keeps a stuck page from hanging a run. | +| Heights | Converted to where the channel's stop disc is; the lowest point of a mounted tip is the disc minus the tip's overhang | PLR + SIM | `_Frames.lowest_point_z`, with the overhang from `SimulatedPipettes._below_stop_disc`. The master reports the lowest point of what a channel carries (`simulator.py` `RZ`). For CO-RE grip tools this is the grip line (`HamiltonCoreGripperTool.grip_line_height`). **Gap:** `_below_stop_disc` exists only on the simulator. Driving a real STAR, the decoder takes the overhang as 0, so heights with a tip mounted would be off by the tip's length. It should come from the model (`TipMountingShaft.tip_bottom`) instead. | + +### Drive speeds used when the command does not state one + +| Drive | Speed | Acceleration | Tag | Source | +|---|---|---|---|---| +| X-arm | 600 mm/s | 1297 mm/s², jerk 3210 mm/s³ | FIT | `X_SPEED`, `X_ACCELERATION`, `X_JERK` (`motion.py`). The driver states no X rate; firmware X commands carry only an acceleration *level*. Section 8.1: S-curve within-group R² 0.976, RMS 23.4 ms, ΔAIC 72.7 over a trapezoid. Jerk is well determined; speed and acceleration less so. | +| Channel Y | 300 mm/s | 900 mm/s² | FIT | `CHANNEL_Y_SPEED`, `CHANNEL_Y_ACCELERATION` (`motion.py`), from single-channel jogs (section 8.2); PLR's `default_y_speed` is 250 mm/s. The firmware states Y acceleration only as a level (1–4, `default_y_acceleration_level = 3`). The channels set off 0.118 s apart (`CHANNEL_Y_STAGGER`, section 8.4). The simulator still times Y at constant speed. | +| Channel Z | 150 mm/s | 800 mm/s² | FIT + PLR | Speed: `CHANNEL_Z_SPEED`, fitted from dispense→aspirate pairs (section 8.4; loose, 150–200); PLR's `default_z_speed` is 125 mm/s. Acceleration: `Pipettes.default_z_acceleration`, which the traces agree with. | +| 96-head Y / Z | the head's defaults | the head's defaults | PLR | `HeadConfiguration.y_drive_speed_default` / `z_drive_speed_default` and their accelerations (`features/head.py`): the value the firmware reports when read, else the configured default increments. | +| iSWAP gripper, when a close states no speed | `gripper_close_speed_default_increments` (5000), converted | `gripper_acceleration_default_increments` (75), converted | PLR | `iSWAPConfiguration` (`features/iswap.py`). | + +--- + +## 1. Channel commands + +### `C0 TP` — tip pick-up (`Pipettes.pick_up_tips`) +Profile: **stroke with handover**. + +| Phase | Target | Tag | Source | +|---|---|---|---| +| 1. Rise | Any channel lower than `th` rises to it (by lowest point) | FW + SIM | `th`. The simulator records the stroke "across at the height it starts from" (`_record_tip_command`). | +| 2. Across | The arm to the X of the last column (`xp`); the channels to `yp`; the channels not named pushed along the rail by the spacing rule | FW + PLR + SIM | Positions are FW. Pushing the others follows `Pipettes._plan_y_positions(make_space=True)`, as the simulator does ("one rail", `_record_tip_command`). Ported as `_Frames.planned_ys`. | +| 0. Fixed | 0.065 s of command handling before anything moves | HEUR | Section 8.6. | +| 3. Down | The lowest point to `tp` at the Z drive's speed, setting off when 82% of the crossing's time has passed | FW + FIT | `tp`; the overlap, section 8.6. | +| 3b. Press | On to `tz` at 11.5 mm/s | FW + FIT | `tz`; the speed, section 8.6. | +| 4. Handover and hold | Each spot's tip goes to the channel's shaft, placed as `TipMountingShaft.mount_tip` places it, then seated at the Z drive's pace; the channels hold 4.356 s, the rest of the fixed time | PLR + FIT + HEUR | The placement is PLR (`_mounted_location`). Seating the tip at the Z-drive pace instead of snapping it is ours (HEUR): the model's placement and the stroke's bottom differ by the fitting geometry. | +| 5. Up | The lowest point to `th`, now with the tip's overhang | FW + PLR | The overhang is read off the tip in the spot and where the shaft seats it (as `TipMountingShaft.tip_bottom` reads it); the simulator's `defined_tip_lengths` only when the model has no tip there. | + +Also used: begin height `tp` = spot + collar height (PLR, legacy `STARBackend.pick_up_tips`, per `_pick_up_tips_in_one_move`). + +### `C0 TR` — tip drop / return (`drop_tips`, `return_tips`, `discard_tips`) +Profile: **stroke with handover**. + +| Phase | Target | Tag | Source | +|---|---|---|---| +| 1–2 | As `C0 TP` | FW + SIM | | +| 3. Down | `tz`. With `ti=1` (DROP) this is the stop disc's height; with PLACE_SHIFT it is where the tip's cone ends | PLR | `_unchecked_fw_drop_tips` docstring: "With `PLACE_SHIFT` the heights are where the tip's cone ends; with `DROP`, the stop disc's." | +| 4. Handover | The tip goes to the spot under the channel (placed by `resting_location`), or nowhere (waste) | PLR | `tip_rack.resting_location`. | +| 5. Up | To `te`, empty | FW | | + +Default heights: DROP begins at spot + collar and ends a fitting depth lower; PLACE_SHIFT uses 59.9 / 49.9 mm (PLR, legacy "Empirical, from legacy: https://github.com/PyLabRobot/pylabrobot/pull/63"). + +### `C0 AS` — aspirate (`Pipettes.aspirate`) +Profile: **stroke with dwell and leave**. + +| Phase | Target | Tag | Source | +|---|---|---|---| +| 1–2 | As `C0 TP` | FW + SIM | | +| 0. Fixed | 1.879 s + 6.761 × transport-air time, before anything moves | FIT + HEUR | Section 8.6; placement HEUR. | +| 3. Down | max(`zl` ∓ `ip`, `zx`): into the liquid by the immersion depth, or above it when `it` is 1; no lower than the minimum height | FW | Field meanings are in legacy's `STARBackend.aspirate_pip` docstring (after the firmware guide): `zl` "Liquid surface at function without LLD", `ip` "Immersion depth", `zx` "Minimum height (maximum immersion depth)". | +| 4. Dwell | Longest channel of (`av` / `as`) + `wt` + mixing; while the volume is drawn, each tip follows the surface down by `fp` at a steady pace (no lower than `zx`) | FW + FIT | Volume ÷ flow rate, settling time, mixing (section 8.6). `fp` is "the total distance the tip moves during aspiration, from the surface down to the new, reduced height" (PLR maintainer, [discuss.pylabrobot.org/t/304/6](https://discuss.pylabrobot.org/t/hamilton-surface-following-issues/304/6)). | +| 5. Leave | Up to the surface at the swap speed `de` | FW | `de` "Swap speed (on leaving liquid)" (legacy docstring; default 10 mm/s). | +| 5b. Pull-out | On up by `po` from the surface, at the swap speed, before the transport air is drawn | FW + FIT + HEUR | `po` "rise before drawing transport air" (PLR's `aspirate` docstring). The speed is FIT: Venus aspirations otherwise identical take 5.35 s (n 4 vs 802) and 4.85 s (n 12 vs 3,534) longer with a 10 mm pull-out at a 2 mm/s swap speed, i.e. 10 mm at 2 mm/s. Measured from the surface: HEUR. With PLR's defaults (`po` 10 mm, `de` 2 mm/s) every aspirate spends about 5 s on it. | +| 6. Up | To `te` | FW | | + +Known divergences from the device (to do): +- The conical second section (`zu`, `zr`) is not played: how the firmware changes the following pace there isn't documented. +- In the demo's aspirate (PLR's defaults), PLR sent the minimum height `zx` equal to the surface `zl`, so the tip went no deeper than the surface and could follow nowhere, whatever `ip` and `fp` said. Worth checking how PLR sets `zx` upstream. +- In v1, LLD is **never** done inside `C0AS`: capacitive/pressure searches run first as their own commands (see `Px ZL` / `Px ZE`, section 5), and `C0AS` goes with LLD off (PR [#1362](https://github.com/PyLabRobot/pylabrobot/pull/1362)). So `C0AS` itself needs no search phase. + +### `C0 JY` — channels to Y positions +Profile: **single axis (Y)**. Each channel to `yp` (FW), at the Y drive speed (section 0). + +### `C0 JZ` — channels to Z positions +Profile: **single axis (Z)**. Each channel's lowest point to `zp` (FW), at the Z drive speed and acceleration (section 0). + +### `C0 FY` — free the Y range (the iSWAP's `make_space`) +Profile: **single axis (Y), commanded target from the model**. The master packs the channels as far forward as they fit, each against the one in front. The target isn't in the command. The v1 driver reads the channels' Y back after the command, in a `finally` (PLR, `iSWAP.make_space`). The simulator writes the packed positions ahead, because its reads answer from the model (SIM, `_unchecked_fw_position_components_for_free_y_range`), and the page plays to them (`recorded_first`: state held back until played). This is the commanded outcome, not a confirmed one: a failure would be reconciled only by the read-back (planned). + +### `C0 ZA` — all channels to Z safety +Profile: **single axis (Z), commanded target from the model**. Every channel up to the top of its Z travel, `Pipettes.configuration.z_range[1]`, where `probe_z_max` leaves them (PLR). The simulator writes this ahead of the command, since its reads answer from the model (SIM, `SimulatedPipettes.probe_z_max`); the driver's read-back is what confirms it. + +### `C0 ZT` — pick up the CO-RE grip tools (`Pipettes.pick_up_core_gripper_tools`) +Profile: **stroke with handover**, on two adjacent channels. + +| Phase | Target | Tag | Source | +|---|---|---|---| +| 1–2 | Both channels to the tools' pick-up points: X `xs`, Y `ya` (back) / `yb` (front) | FW + PLR | Points come from the model: each tool's `pick_up_location` (`resources/hamilton/core_gripper_tools.py`), in the holder at `channel_x_center` and its front/back channel Y centres (`core_grippers.py`). They match legacy's wire exactly (`xs07975 ya1250 yb1070` on a STARlet, legacy `STAR_tests.py`). | +| 3. Down | `tz` = tool top − 10 mm, by the stop disc (the channel is bare) | PLR + HEUR | Heights are legacy's (`pick_up_core_gripper_tools`: begin 235, end 225, against tools whose tops stand at 235.0, probed per `hamilton_core_gripper_1000ul_5ml_on_waste`). Reading `tz` as a stop-disc height is ours (HEUR): the firmware's reference point for `ZT` heights is not documented publicly. | +| 4. Handover | Each tool onto its shaft, placed by `mount_tip` (pick-up location on the shaft's axis) | PLR | `TipMountingShaft.mount_tip`. | +| 5. Up | `th`, with the tool hanging by its grip line (pick-up z − fitting depth − grip-line height) | PLR + SIM | Matches what the simulator reports for a channel carrying a tool. | + +Travel height `th`: as high as a tool on a channel reaches (v1's convention for tip commands, `_tip_traverse_height`). Legacy sends the iSWAP traversal height, 280 mm (PLR, `_iswap_traversal_height`); `minimum_traverse_height=280` reproduces it. + +### `C0 ZS` — return the CO-RE grip tools (`return_core_gripper_tools`) +Profile: **stroke with handover**, the reverse of `C0 ZT`. The channels go to where the tools were parked (remembered at pick-up). Down to `tz` = tool top − 30 mm (legacy: begin 215, end 205; PLR). Here `tz` is read as the lowest point *with* the tool mounted (HEUR, as for `ZT`). Each tool goes back to its parked location and rotation (PLR), then the channels rise to `te`. + +--- + +## 2. X-arm + +### `X0 XP` / `X0 SP` — arm to an X position +Profile: **single axis (X)**. `XP` is the left arm and `SP` the right. The target is the arm's `…a` field, in increments converted by `XArmConfiguration.x_increments_to_mm`, less `reference_point_from_left` (FW + PLR), at 400 mm/s and 500 mm/s² (HEUR, section 0). Everything on the arm (channels, 96-head, iSWAP) rides with it (PLR: they're children of the arm resource). + +--- + +## 3. iSWAP + +Joint conventions (DOC, Camillo Moschner, [discuss.pylabrobot.org/t/517/1](https://discuss.pylabrobot.org/t/intro-to-epic-tame-the-iswap/517/1)): +- Rotation (elbow) −90° / 0° / +90° = left / front / right. +- Wrist −135° / −45° / +45° / +135° = right / straight / left / reverse. + +In PLR these are `iSWAPConfiguration`'s predefined increments, read at setup. + +### `R0 YA` / `R0 ZA` — the head along Y / Z +Profile: **single axis**. +- Y: to `ya` at `yv`, converted by the arm's configuration (FW + PLR). No acceleration rate, since `R0 YA` carries only a level, so it plays at constant speed (HEUR, as channel Y). +- Z: to `za` + `elbow_z_offset_above_finger` at `zv`, with acceleration `zr` × 1000 increments/s² (FW + PLR). + +### `R0 PA` — elbow and wrist together +Profile: **two joint turns, together**. Each joint to its own angle at its own speed and acceleration (`wa`/`wv`/`wr` for the elbow, `ta`/`tv`/`tr` for the wrist; FW), converted by `iSWAPConfiguration` (PLR). Each turns about the pivot the driver turns it about (`proximal_joint`; PLR). Both run on a straight line in joint space (HEUR; the controller's interpolation is not public). + +### `R0 GA` — jaws to a width +Profile: **jaws**. The fingers move to the width `ga` at `gv` / `gr` (FW). Each finger travels half the width change in the same time, so at half the drive's speed (derived from symmetric jaws). If opening, the page lets go of what it held *before* moving; if closing, it takes hold *after* (HEUR: the ordering that avoids dragging or dropping the plate mid-motion). + +### `C0 GC` — close onto an object +Profile: **jaws**. Closes to `gb` at the default close speed and acceleration (PLR, section 0), then takes hold (HEUR, as `R0 GA`). + +### Plate and lid moves (`iSWAPTransport`, Python, from the primitives above) +The compound commands (`C0 PP` / `PR` / `PM`) are not used. Their internal motion isn't public, and the firmware "chooses among multiple valid poses unpredictably" (DOC, [discuss.pylabrobot.org/t/517/1](https://discuss.pylabrobot.org/t/intro-to-epic-tame-the-iswap/517/1)). A move is planned as primitives (`hamilton/star/driver/features/iswap_transport.py`): + +| Choice | Value | Tag | Source / justification | +|---|---|---|---| +| Phases | open → rise → travel → descend → grip / release → rise | HEUR | Follows the phases `C0 PP`'s parameters describe (open width, traverse height, grip height, end height). | +| Travel height | `iswap.default_minimum_traverse_height` (284 mm), fixed, never computed from the deck | PLR | Legacy sends 280 mm. | +| Grip width, open margin, strength, tolerance, pick-up depth | width across the plate; +3 mm; 4; 2 mm; 5 mm below the top (or `preferred_pickup_location`) | PLR | Legacy `STARBackend.pick_up_resource`. | +| Grip below a lid | 2 mm below the lid's skirt (`nesting_z_height`) | HEUR | Jaws closing within the skirt would close on the lid. | +| Elbow configuration | Only the three elbow stops; the stop with the smallest joint turn from now, front on ties | HEUR | The stops are PLR/DOC; the ranking is ours. | +| Travel order | Y then turn, else turn then Y, else turn at a safe Y = max(y_min, min(now, target, y_max − link 1 − tool)) | HEUR | Chosen so the turn never sweeps the arm behind the rail. Checked by sampling each turn at 36 points against `_check_pose_reachable`. | +| X during travel | Runs concurrently with Y and the turn | HEUR | The X-arm is a separate drive. | + +### Not decoded +- `C0 PG` park: close the jaws, lift to the traverse height, retract ("Sending no height leaves that to the master, which does not raise the arm"; PLR, `iSWAP.park` docstring). A rotation-drive calibration lists a parking position of ≈29500 increments (≈+91°) (DOC, [discuss.pylabrobot.org/t/517/4](https://discuss.pylabrobot.org/t/intro-to-epic-tame-the-iswap/517/4)). The retract path itself is not public. +- `R0 GB` close to an object, `R0 GS` relative jaw move: jaws profiles, not yet mapped. +- `C0 FI`, `R0 GI`: initialisation; no public profile. +- Venus 6.2 teaches custom Get/Place paths as waypoint poses (Transport Paths Editor), documented only in the non-public Venus 6.2 Programmer's Manual (DOC, [labautomation.io/t/5759](https://labautomation.io/t/transport-paths-editor/5759)). + +--- + +## 4. 96-head + +### `C0 EP` / `C0 ER` — tip pick-up / drop +Profile: **stroke with handover**, all 96 shafts. The head goes to `xs` (sign from `xd`) and `yh`, down and up as the tip commands (FW). Channel A1 goes to spot A1 (PLR: `Head96` / `NChannelPipette` layout). The head's drive defaults are in section 0. The simulator places the head after the command (`SimulatedHead96._place_after_tip_command`: arm X, head Y, z = ze + overhang; SIM). + +### `H0 YA` / `H0 ZA` — the head along Y / Z +Profile: **single axis**, to the target at `yv`/`yr` or `zv`/`zr` (FW), converted by `HeadConfiguration` (PLR). + +### Not decoded +- `H0 PA` / `H0 PB` aspirate / dispense (a stroke with dwell, like `C0 AS`). +- `C0 EM` move to a coordinate. +- `H0 ZL` cLLD probe (see section 5). +- `H0 DQ` dispensing drive (piston only, no visible motion). +- Touch-off is not available on the multi-probe head: "the MPH is a significantly heavier tool … making this feature impractical" (DOC, Hamilton, [labautomation.io/t/1788/2](https://labautomation.io/t/touch-off-with-mph/1788/2)). + +--- + +## 5. Not decoded yet, with what is known + +### Detection searches — `Px ZL` (cLLD), `Px ZE` (pLLD), `Px ZH` (Z-touch), `C0 XL`, `Px YL` +Profile to build: **search**. Each channel on its own, all concurrently: +1. approach to the start; +2. a slow constant-speed descent (or traverse, for X/Y) at the search speed, until detection or the end; +3. a small post-detection move; +4. then stay, or go to Z safety. + +Channels stop at different heights at different moments. + +| Parameter | Firmware field | Tag | Source | +|---|---|---|---| +| Start, end (stop-disc heights) | `zc`, `zh` (ZL/ZE); `zb`, `za` (ZH) | FW | `_unchecked_fw_probe_z_using_clld` / `_plld` / `_ztouch` docstrings (`pipettes.py`). | +| Search speed, acceleration | `zl`, `zr` (ZL); `zu`, `zr` (ZH); approach `zv` (ZH) | FW | Same. | +| After detection | `zj` (0 down, 1 up) by `zi` | FW | Same. | +| Z-touch force | detection limiter PWM `cg`, push-down PWM `cf` | FW | Same. Touch-off "relies on force feedback to determine when the channel is contacting the bottom surface" (DOC, Hamilton, [labautomation.io/t/1788/2](https://labautomation.io/t/touch-off-with-mph/1788/2)). Needs channel firmware from March 2022 on (DOC, [discuss.pylabrobot.org/t/543/3](https://discuss.pylabrobot.org/t/reason-behind-z-touch-only-being-available-to-8-channels-made-in-2022-and-onwards/543/3)). | +| Where searches start and stop | 5 mm above a container's top (2 mm for wells); 1 mm below the modelled cavity bottom | PLR | `Pipettes.search_start_clearance`, `well_search_start_clearance`, `search_limit_below_cavity_bottom`. | +| Sequence in an LLD aspirate | `Px DC` (blow-out air, in air) → approach → searches → `C0 RL` → `C0 AS` with LLD off | PLR | PR [#1362](https://github.com/PyLabRobot/pylabrobot/pull/1362). | +| **Where it stops** | not in the command; known only from the answer (`C0 RL`, or the model the simulator moves) | — | Needs post-answer targets: play the descent to where the model reports (exact in the simulator), or an open-ended descent at search speed that stops when the answer lands (on hardware). | + +### Single-channel and arm moves +- `Px ZA`: one channel to a stop-disc Z. A single-axis Z move (FW). +- `C0 JE`: "the device spreads them itself, over the same band the initialization procedure uses" (PLR, `spread_channels`). The targets are the band spread evenly (PLR, `default_initialize_y_positions`). +- `C0 JP`: frees one channel as much as possible; "the device decides where the others go" (PLR, `make_max_space_for_channel`). Targets are known only after the answer. +- `C0 KX` / `KR`: "The master raises what the arm carries before it travels" to Z safety, then X (PLR, `_unchecked_fw_move_x_with_attached_components_at_z_safety`). Profile: Z-safety phase, then X. +- `Px DC`: blow-out air drawn in air. Piston only, no visible motion. + +### Side touch (dispense; v1 has no dispense yet) +"The channel will go down to the touch-off z-height specified, then moves over to the right, dispense, and then moves up" (DOC, Hamilton, [labautomation.io/t/2255/6](https://labautomation.io/t/post-dispense-tip-touch-on-starlet/2255/6)). "It is restricted to a positive x-movement so to the right. Max distance is 4.5mm" (DOC, Hamilton, [labautomation.io/t/2255/9](https://labautomation.io/t/post-dispense-tip-touch-on-starlet/2255/9)). Legacy's `C0 DS` carries a side-touch distance. + +### Autoload, initialisation +- Autoload (`I0 YA/ZA/YP/ZP`, `C0 IV`, the barcode scanner move, carrier load/unload): no public profile. Loading moves a carrier into the tree. +- Initialisation (`C0 DI`, `C0 FI`, `X0 XI`, the heads): no public profile. + +--- + +## 6. Commands with no motion profile +Reads (`R*`, `Q*`, `VW`), pressure-monitoring and sensor setup (`AC`, `AF`, `AN`, `AQ`, `BG`, `BH`), drive parameters (`AA`), brakes (`R0 BA/BO`), drive power (`X0 XO`, `R0 GO`), cover (`C0 CO/HO/CE/CD`), tip-type definition (`C0 TT`), master setup (`C0 UA`, `C0 VI`), and autoload sensing and barcode setup (`CQ`, `CS`, `CT`, `CB`, `CP`, `AR`, `AF`). + +--- + +## 8. Measured from firmware traces + +**Source:** Venus `HxUsbComm*.trc` logs from the Chory lab's STAR (266 files, 447 MB, Nov 2023 – Apr 2025; not in the repo). Every request (`<`) and reply (`>`) carries a millisecond timestamp. Pairing them by id gives 2,559,959 answered commands and how long each took. Arm-X reads (`C0 RX`, 744,966 of them) give where the arm was before most moves. + +**Reproduce:** `python tools/hxusbcomm_timing.py --max-mb 900 --out timing.json` on any Venus traces. It reads one file at a time, so memory stays flat. + +**How moves are isolated:** within a group of otherwise identical commands, time = overhead + move(distance), with one overhead per group. +- Pure X jogs: `C0 JX`, and `C0 EM` when Y and Z are unchanged. +- `C0 AS`/`DS` identical except for X, with the channels' Y unchanged from the previous command. +- Single-channel Y and Z jogs: `C0 KY`, `C0 KZ`. +- A move is measured only from a previous command that answered without error. + +### 8.1 The X-arm is jerk-limited +15 groups: + +| Model | Parameters | RMS | Within-group R² | AIC | AICc | BIC | +|---|---|---|---|---|---|---| +| **S-curve, overhead per group** | v 600 mm/s, a 1297 mm/s², j 3210 mm/s³ | **23.4 ms** | **0.976** | **−639.6** | **−630.0** | **−594.6** | +| Trapezoid, overhead per group | v 595 mm/s, a 605 mm/s² | 35.5 ms | 0.945 | −566.9 | −558.4 | −524.4 | +| S-curve, one shared delay | — | 3818 ms | −635 | 249.1 | 249.6 | 259.1 | +| Trapezoid, one shared delay | — | 3880 ms | −656 | 250.1 | 250.3 | 257.6 | + +- **Why a trapezoid can't fit:** in both pure jogs, going from 1 to 10 mm adds 0.30 s and from 10 to 100 mm adds 0.50 s (a ratio of 1.65). A trapezoid gives at least √10 ≈ 3.2 for any acceleration, even with a speed cap. A constant delay cancels in these differences, so it can't rescue the trapezoid. +- **Sensitivity:** jerk is well determined (halving or doubling it roughly doubles the RMS). Speed and acceleration are loosely determined above ≈450 mm/s and ≈1100 mm/s², because few recorded moves are long enough to cruise. +- **One shared delay is rejected:** the constant part of a command's time is specific to the command, not a single latency. + +### 8.2 Channel Y and Z are trapezoids + +| Axis | Jog data (distance: median time) | Fit | +|---|---|---| +| Channel Y (`C0 KY`) | 1 mm: 0.132 s (n20); 10 mm: 0.285 s (n63); 100 mm: 0.730 s (n1) | Trapezoid v ≈ 300 mm/s, a ≈ 900 mm/s², RMS 4.7 ms. Jerk improves it by < 1 ms (noise). | +| Channel Z (`C0 KZ`) | 1 mm: 0.185 s (n139); 10 mm: 0.338 s (n39) | PLR's `default_z_acceleration` 800 mm/s² reproduces the 1→10 mm step to 0.1 ms. | + +Only three Y distances (one sample at 100 mm) and two Z distances exist, so mild jerk on Y or Z can't be ruled out. + +### 8.3 Other factors (command overheads are now used by the page: 8.6) +- **Communication round trip:** read replies arrive in ≈9–21 ms (median `X0 RF` 9 ms, `H0 RH` 7 ms, `C0 RX` 21 ms). This is the only delay common to all commands. +- **Command overhead is command-specific.** For the same X move, the 96-head's `C0 EM` takes ≈0.36 s longer than a channel `C0 JX` (overheads ≈0.48 s vs ≈0.12 s after the fitted move time). +- **In tip commands, short X moves overlap the Z stroke.** A `C0 TP` after a `C0 TR` takes 6.331 s for 9 mm and 6.335 s for 18 mm, where the X-arm alone needs ≈0.1 s more for the longer move. Above ≈20 mm the X distance shows again. The firmware appears to move X while Z is still travelling. The player runs the phases in sequence, so tip strokes are drawn slightly long for short moves. +- **Reply times cluster on ≈50 ms steps** in some long commands (e.g. `C0 DS` at 4.10 / 4.15 / 4.20 s), which suggests the controller reports on a polling tick. It limits resolution for small effects. +- **Command durations** (medians over all traces; these depend on each command's parameters, so they describe, they don't model): + +| Command | n | Median | Command | n | Median | +|---|---|---|---|---|---| +| `C0 AS` aspirate | 229,489 | 6.41 s | `C0 PP` iSWAP get plate | 50,365 | 6.76 s | +| `C0 DS` dispense | 218,050 | 5.00 s | `C0 PR` iSWAP put plate | 47,743 | 6.05 s | +| `C0 TP` tip pick-up | 55,084 | 6.18 s | `C0 PG` iSWAP park | 11,839 | 7.62 s | +| `C0 TR` tip drop | 55,088 | 8.02 s | `C0 EA` / `C0 ED` 96-head aspirate / dispense | 49,616 / 48,950 | 7.75 / 6.11 s | +| `C0 EP` / `C0 ER` 96-head tips | 5,965 / 5,967 | 8.82 / 7.46 s | `C0 RX` read arm X | 744,966 | 0.021 s | + +### 8.4 Channel commands: a full model, and do the channels move in Y one after another? + +**Reproduce:** `python tools/hxusbcomm_channels.py `. + +**Method.** Each `C0 AS/DS/TP/TR` is paired with the channel command just before it, when nothing else moved in between, both answered without error, and the active channels share one X. The duration is fit as + +d = a[group] + b_v · volume time + b_m · mix time + b_z · Tz(stroke) + b_xy · Txy(dx, dy₁…dy₈) + +- A group is the previous command plus every categorical parameter; continuous parameters, and mixing parameters when there is no mixing, are removed. +- Volume time is volume ÷ flow for the slowest channel. The Z stroke runs from traverse height to the liquid surface or tip height. +- X uses the §8.1 S-curve and Y the §8.2 trapezoid. Nonlinear constants come from a grid search with the linear solve inside each grid point. +- A Y shift under 0.5 mm counts as no move (positions jitter by 0.1 mm between commands). + +**`C0 AS`** (228,260 pairs, 706 groups; Y moves: 63,073 × 8 channels, 5,532 × 7, 95 × 2–4): + +| Txy model | R² within | RMS | AIC | BIC | +|---|---|---|---|---| +| no XY | 0.9610 | 96.3 ms | −1,067,012 | −1,059,682 | +| X only | 0.9831 | 63.4 ms | −1,257,512 | −1,250,172 | +| Y together (max) | 0.9708 | 83.4 ms | −1,132,777 | −1,125,437 | +| Y sequential (sum) | 0.9708 | 83.2 ms | −1,133,447 | −1,126,107 | +| max(X, Y together) | 0.9885 | 52.4 ms | −1,345,185 | −1,337,845 | +| max(X, Y sequential) | 0.9756 | 76.2 ms | −1,173,739 | −1,166,398 | +| X then Y together | 0.9853 | 59.1 ms | −1,290,135 | −1,282,795 | +| X then Y sequential | 0.9740 | 78.6 ms | −1,159,627 | −1,152,287 | +| **max(X, Y together + 0.80 s when Y moves)** | **0.9950** | **34.5 ms** | **−1,536,170** | **−1,528,820** | +| max(X, Y together + 0.12 s per extra channel) | 0.9950 | 34.6 ms | −1,534,356 | −1,527,005 | +| max(X, Y together + 0.80 s + 0.00 s per extra channel) | 0.9950 | 34.5 ms | −1,536,168 | −1,528,807 | + +**`C0 DS`** (216,436 pairs, 357 groups; Y moves: 67,759 × 8, 5,533 × 7, 71 × 4): + +| Txy model | R² within | RMS | AIC | BIC | +|---|---|---|---|---| +| no XY | 0.8699 | 148.8 ms | −824,027 | −820,324 | +| X only | 0.8820 | 141.7 ms | −845,089 | −841,376 | +| Y together (max) | 0.9637 | 78.6 ms | −1,100,086 | −1,096,373 | +| Y sequential (sum) | 0.9639 | 78.4 ms | −1,101,521 | −1,097,808 | +| max(X, Y together) | 0.8941 | 134.3 ms | −868,499 | −864,786 | +| max(X, Y sequential) | 0.9730 | 67.8 ms | −1,164,167 | −1,160,454 | +| X then Y together | 0.9335 | 106.3 ms | −969,445 | −965,732 | +| X then Y sequential | 0.9632 | 79.1 ms | −1,097,515 | −1,093,802 | +| max(X, Y together + 0.75 s when Y moves) | 0.9885 | 44.3 ms | −1,348,629 | −1,344,906 | +| max(X, Y together + 0.12 s per extra channel) | 0.9884 | 44.4 ms | −1,347,722 | −1,343,999 | +| **max(X, Y together + 0.20 s + 0.08 s per extra channel)** | **0.9887** | **43.9 ms** | **−1,352,573** | **−1,348,840** | + +**`C0 TP` / `C0 TR`:** within groups, a tip pick-up never changes Y. The best models reach R² 0.05 (TP, RMS 26 ms) and 0.007 (TR, RMS 123 ms). Nothing about Y can be read from them. + +**What is established:** +- X and Y overlap. In an earlier pass with a coarser key, max(X, Y together + a fitted Y cost) beat X-then-Y-with-its-own-fitted-cost by ΔAIC ≈ 212,000 (AS) and ≈ 433,000 (DS). +- Any Y move costs ≈0.75–0.80 s on top of its travel time (FIT). +- Volume time enters with slope 0.99–1.02 (FIT). + +**What is not established: ripple, i.e. whether the Y cost is a stagger per channel.** A flat 0.80 s and 0.12 s per extra channel (0.84 s for 8 channels) predict the same for the 8- and 7-channel moves that make up 99.9% of the data, so their AIC difference is small. For DS, the joint model's per-channel part rests on the 7-vs-8 split, which is also a split between protocols. +- The one independent test uses held-out rows with 2–6 channels moving, fitted on the rest. +- On 53 four-channel dispenses from one protocol, the flat 0.80 s overpredicts by 0.50 s (RMS 498 ms), while 0.12 s per extra channel misses by only −0.07 s (RMS 86 ms). This points to a stagger, but it comes from a single protocol. +- The aspirate "few-channel" rows are 0.1 mm jitter, not moves. +- On this data alone the ripple is suggestive. A run that moves 1–4 channels in Y inside otherwise identical commands would test it independently. Single-channel `C0 KY` jogs (overhead ≈65–75 ms beyond travel) are a different command and aren't used as evidence. + +**The ripple's size, taken as given.** The ripple is known from the device (user; DOC to follow). Fitting one Y cost per count of channels moving (`C0 DS`, `ripple` in the tool's output; R² within 0.9883, RMS 42.5 ms, AIC −1,366,602, the best DS model): + +| Channels moving in Y | Y cost beyond travel (FIT) | 95% profile interval | Rows | Source of the rows | +|---|---|---|---|---| +| 4 (channels 1–4) | 0.300 s | 0.295–0.315 s | 71 | one protocol, 15 groups, 12 with still rows | +| 7 | 0.690 s | 0.685–0.695 s | 5,533 | channel 8 idle | +| 8 | 0.760 s | 0.755–0.760 s | 67,759 | — | + +- A line through the three gives **0.118 s per extra channel** with a fixed part of −0.05 s, i.e. about none. +- The page model: channel *i* (in the order they move) starts 0.12 s after the one before it, and the command waits for the last to arrive. +- The 7→8 step (0.07 s) is smaller than the line's slope. The order the firmware moves them in, and whether the stagger depends on distance, aren't resolved by these three counts. + +**Pure Z,** from DS→AS pairs with no X or Y change and no mixing (32,968 pairs): +- Strokes span 12.9–57.9 mm in two clusters, 13 mm and 53–56 mm. +- Z is identified only when inert parameters are left out of the group key. With the mixing speed in the key, groups split by protocol and absorb the stroke: R² gains just 0.0002. + +| Model (group key without inert parameters) | R² within | RMS | AIC | BIC | +|---|---|---|---|---| +| volume only | 0.7644 | 134.9 ms | −132,056 | −131,972 | +| volume + linear stroke (0.0142 s/mm) | 0.9858 | 33.2 ms | −224,555 | −224,463 | +| **volume + 2 × trapezoid (v 150 mm/s, a 800 mm/s²), slope fixed at 1** | **0.9858** | **33.1 ms** | **−224,741** | **−224,640** | +| volume + 2 × trapezoid (v 125, a 800) | 0.9812 | 38.1 ms | −215,344 | −215,260 | + +The acceleration agrees with PLR's `default_z_acceleration` (800 mm/s²). The speed is loosely determined (150–200 mm/s across group keys), because the strokes form two clusters. + +### 8.5 Next fits the same data supports +- A decisive Y-ripple run (above). +- iSWAP park (`C0 PG`), and the far-right iSWAP path (8.8). +- The X/Z overlap in tip commands, as a phase overlap in the player. + +### 8.6 Fixed time per command (what a command takes beyond the drawn motion) + +**Reproduce:** `python tools/hxusbcomm_fixed.py `. + +**Method.** Each command is timed exactly as the page draws it, using the constants in `motion.py`: +- X on the S-curve; +- channel Y at 300 mm/s and 900 mm/s², the channels setting off 0.118 s apart; +- Z at 150 mm/s and 800 mm/s²; +- the aspiration dwell (volume ÷ flow + settling time), and the swap-speed exit. + +The measured duration minus that drawn time is the command's fixed time. It's fitted as a linear model of the parameters that plausibly set it. The page plays it as a pause before the motion, except the tip commands', which hold at the bottom: a pick-up with the tips on, a drop while they are pushed off. Each hold is the measured duration less the drawn motion, so it assumes the tip commands' Z runs at the fitted speed. *Where* in a command the firmware spends it isn't measured (HEUR). + +| Command | Model (FIT) | n | R² of the fixed time | RMS | AIC | BIC | Constant only: RMS / AIC | Whole-duration R², median miss | +|---|---|---|---|---|---|---|---|---| +| `C0 AS` | 1.879 s + 0.979 × mix volume time + 0.557 s × mix cycles + 6.761 × transport-air time | 228,477 | 0.641 | 521 ms | −297,806 | −297,765 | 870 ms / −63,563 | 0.912, 65 ms | +| `C0 TP` | 4.421 s, played as a 4.356 s hold at the bottom after 0.065 s of command handling (placement HEUR), **plus the press drawn as motion** (0.0871 s/mm of `tp − tz`) and the descent setting off at 82% of the crossing (both below) | 48,304 | 0.830 | 38 ms | −316,189 | −316,172 | 94 ms / −228,685 | ≈0.84, — | +| `C0 TR` | 5.009 s (mean; channels add 0.21 s each, R² 0.15, but 7 against 8 channels is also one protocol against the others: left out), played as a 4.944 s hold at the bottom while the tips are pushed off, after 0.065 s of command handling (placement HEUR) | 54,290 | 0.000 | 176 ms | −188,805 | −188,797 | same | ≈0.61, — | +| `C0 ZA` | 0.14 s (median of 1,783; the channels were mostly already up) | 1,783 | — | — | — | — | — | +| `C0 EP` (96-head tips on) | 4.938 s (median, against PLR's head drive defaults; IQR 4.934–4.947), played as a 4.873 s hold at the bottom after 0.065 s of handling (placement HEUR) | 155 | — | — | — | — | — | +| `C0 ER` (96-head tips off) | 4.490 s (median; IQR 4.281–4.499), played as a 4.425 s hold at the bottom after 0.065 s of handling (placement HEUR) | 5,867 | — | — | — | — | — | +| `C0 EA` / `C0 ED` (96-head aspirate / dispense) | `EA` 5.80 s; `ED` by mode (`da`): jet (0, 1) 1.41 s, surface / empty (2, 3, 4) 5.85 s. Held at the bottom with the pumping (volume ÷ flow) and settling time, after 0.065 s of handling (placement HEUR). From four local traces (`tools/hxusbcomm_head96_liquid.py`: travel timed as 8.7, less pumping and settling): `EA` median of 46 (IQR 5.2–6.9 s; 5.4–6.9 by protocol), `ED` without mixing at ≥ 100 µL/s: mode 1 median of 12 (1.40–1.44 s), mode 2 median of 8 (5.83–5.90 s), mode 3 a single 4.90 s. Modes 0 and 4 are unrecorded, and a few mode-3 or 10 µL/s dispenses took 11–13 s that these parameters don't explain; the Chory lab set (≈49,000 of each) would settle both | 46 / 21 | — | — | — | — | — | +| `H0 YA/ZA` (96-head moves) | 0.11 s: an `H0 YP` that travels nothing (10th percentile of 31); HEUR by analogy | — | — | — | — | — | — | +| `C0 JY/JZ/FY`, `X0 XP`, iSWAP `R0` steps | 0.065 s: a 1 mm `C0 KY` jog less its travel; HEUR by analogy | — | — | — | — | — | — | +| `C0 ZT`/`ZS` (CO-RE tools) | as `C0 TP`/`C0 TR`, held at the bottom; HEUR, no recorded counterpart | — | — | — | — | — | — | +| `C0 ZP`/`ZR` (CO-RE plate grip / release) | as `C0 ZT`/`ZS`, held at the bottom (`CORE_PLATE_GRIP_FIXED`, `CORE_PLATE_RELEASE_FIXED`); HEUR and likely long: a grip closes two channels on a plate, with no tip to clamp or check. No recorded counterpart; a trace of a protocol that uses the CO-RE gripper (KAPA does) would settle it | — | — | — | — | — | — | + +- **Why these R² look modest:** they measure the fixed time, which is what's left after the drawn motion, and it spreads little (tip pick-up: 88 ms). Against the whole duration the page's timing reaches R² 0.91 (aspirate), ≈0.84 (tip pick-up) and ≈0.61 (tip drop; the whole-duration figures for the tip commands are 1 − RMS²/variance of the duration). +- **Channel count is left out.** Nearly every recorded command uses 7 or 8 channels, so its coefficient tracks protocols: +0.04 s in one extraction, −0.27 s in another differing by 0.1% of rows. +- **Aspirate:** the median |residual| is 65 ms; the RMS is inflated by a few protocols (liquid-level detection, 34 rows, which the page doesn't draw). Mixing happens at the bottom, so its two terms are added to the dwell, not the pause. +- **Tip press:** the fixed time of `C0 TP` grows 0.0871 s per mm of `tp − tz`. The page goes down to `tp` at the Z drive's speed and presses on to `tz` at 1 / 0.0871 = **11.5 mm/s**. Only two press lengths are recorded (8 and 10 mm). +- **Tip pick-up overlap:** the descent to `tp` sets off when 82% of the crossing's time has passed (R² of the fixed time 0.830 against 0.810 in sequence, ΔAIC 7,234). Going from 9 to 18 mm adds 116 ms of X travel but only 7 ms to the command; 27 mm adds 198 ms but 112 ms. Nearly every recorded pick-up moves 9 mm (48,024 of 48,172), so the share is loosely determined. Drops can't be checked: hardly any travel without a Y move. +- **Correction (2026-09-26):** a tip command sends `tp` and `tz` once for all channels. An earlier version of the tool read them as channel 0's only and timed the other channels' descents from Z = 0, which made the pick-up and drop fixed times about 3 s too small (1.46 and 3.06 s). The aspirate, whose heights are always per channel, was unaffected, as are the ripple and Z fits in 8.4. + +### 8.7 The 96-head + +`C0 EM/EP/ER` following another head command (7,182 pairs, 86 groups). Z heights are fixed within each group, so head Z can't be identified here. The head's X and Y overlap (ΔAIC ≈ 11,100 over X-then-Y at the same constants). + +| Model | R² within | RMS | AIC | BIC | +|---|---|---|---|---| +| no travel terms | 0 | 268.7 ms | −18,705 | — | +| X then head Y, + Z, PLR defaults | −0.001 | 268.8 ms | −18,698 | −18,107 | +| **max(X, head Y) + Z, PLR defaults (Y 390.6 mm/s, 546.9 mm/s²; Z 85 mm/s, 400 mm/s²), no free parameter** | **0.787** | **124.1 ms** | **−29,803** | **−29,211** | +| max(X, head Y 300, 1500) + Z PLR, head Y fitted | 0.855 | 102.2 ms | −32,594 | −32,002 | + +The fitted head constants slide along the grid edges (speed and acceleration trade off, and few moves reach cruise), so the page keeps **PLR's head defaults**. The data agrees with them (R² 0.79 with no free parameter); what it adds is the overlap of X and Y and the fixed times in 8.6. + +### 8.8 The iSWAP + +Venus sends whole moves (`C0 PP` get, `C0 PR` put); PLR sends the iSWAP's steps one by one (`R0` commands, each carrying its own speeds). So these fits describe the device, but don't map one-to-one onto PLR's steps. + +| Command | Model (fixed effect per target site and grip parameters) | n | Groups | R² within | RMS | AIC | BIC | +|---|---|---|---|---|---|---|---| +| `C0 PP` | max(X, Y), Y 400 mm/s, 1200 mm/s² | 33,727 | 158 | 0.650 | 229 ms | −99,149 | −97,818 | +| `C0 PP` | **X then Y, Y 300 mm/s, 800 mm/s²** | 33,727 | 158 | **0.923** | **107 ms** | **−150,299** | **−148,967** | +| `C0 PR` | max(X, Y), Y 300 mm/s, 200 mm/s² | 47,384 | 201 | 0.507 | 120 ms | −200,479 | −198,717 | +| `C0 PR` | **X then Y, Y 300 mm/s, 800 mm/s²** | 47,384 | 201 | **0.815** | **74 ms** | **−246,796** | **−245,034** | + +- **The iSWAP moves X first, then Y** (ΔAIC ≈ 51,000 for gets and ≈ 46,000 for puts). This is unlike the channels and the 96-head, which overlap X and Y. +- **iSWAP Y ≈ 300 mm/s, 800 mm/s² (FIT).** PLR sends 4,751 increments/s ≈ 220 mm/s, which it documents as 68% of the firmware's default (≈ 323 mm/s). Venus evidently runs near the documented default. PLR's plans carry their own speeds, so the page already plays PLR's slower Y; that is what PLR would do on the device. +- **Fixed times of the whole moves:** `C0 PP` 4.93 s and `C0 PR` 4.54 s for targets on the deck. +- **Far-right targets (X > 850 mm) take ≈ 8.5 s longer** (13.49 / 12.85 s fixed, 6,575 + 10,640 moves). They're all at X ≈ 890–910 mm, Y ≈ 244 or 421 mm, most likely off-deck or extended-reach positions where the firmware takes a longer arm path. Not modelled further. +- **Not yet known:** how a whole move's fixed time divides among PLR's `R0` steps (gripper open/close, wrist, Z). The page gives each step the generic 0.065 s. + +### 8.9 The whole timing model, at a glance + +| Factor | Value used by the page | Tag | Fit quality | PLR's value | Known for sure? | +|---|---|---|---|---|---| +| X-arm | S-curve 600 mm/s, 1297 mm/s², 3210 mm/s³ | FIT | R² 0.976, RMS 23 ms, AIC −640 (vs trapezoid −567) | none stated | Jerk yes; speed/acceleration loose above ≈450 / ≈1100 | +| Channel Y travel | trapezoid 300 mm/s, 900 mm/s² | FIT | RMS 4.7 ms on jogs; slope 1.01 in the full model | 250 mm/s | Yes for 1–10 mm; one 100 mm sample | +| Channel Y ripple | 0.118 s per channel after the first, channel order | FIT | best DS model: R² 0.9883, RMS 42.5 ms, AIC −1,366,602 | none (moves together) | Size yes (4/7/8 channels); order and distance dependence no | +| X and channel Y | overlap: max(X, Y) | FIT | AS, each with a fitted Y cost: R² 0.993 vs X-then-Y 0.983 (ΔAIC ≈ 212,000) | the simulator: at once | Yes | +| Channel Z | trapezoid 150 mm/s, 800 mm/s² | FIT (speed), PLR (acceleration) | R² 0.986, RMS 33 ms, AIC −224,741 | 125 mm/s, 800 mm/s² | Acceleration yes; speed loose, 150–200 | +| Aspiration dwell | volume ÷ flow + settle | FW | slope 0.99–1.02 | same | Yes | +| Mixing | 0.979 × 2·cycles·volume ÷ speed + 0.557 s per cycle | FIT | part of the AS model | not drawn before | Yes (20k mixes) | +| Tip press | 11.5 mm/s from `tp` to `tz` | FIT | TP fixed-time R² 0.83, RMS 38 ms | none (straight to `tz`) | Two press lengths only | +| Fixed times | per command, 8.6 | FIT / HEUR | 8.6 | none | Measured for AS, TP, TR, ZA, EP, ER; by analogy for the rest; when in the command it falls isn't known | +| 96-head X and Y | overlap: max(X, Y) | FIT | ΔAIC ≈ 11,100 over X-then-Y | the simulator: at once | Yes | +| 96-head Y, Z | PLR defaults (390.6 / 546.9; 85 / 400) | PLR | R² 0.79 with no free parameter | same | Not identified better by the traces | +| iSWAP X and Y | X then Y | FIT | ΔAIC ≈ 46,000–51,000 | PLR plans its own step order | Yes, for Venus's whole moves | +| iSWAP Y | PLR's own step speeds (≈220 mm/s) | PLR | Venus: 300 mm/s, 800 mm/s² (R² 0.92 / 0.82) | ≈220 mm/s | Yes for Venus; PLR deliberately slower | +| iSWAP step fixed time | 0.065 s per `R0` step | HEUR | — | none | No: whole moves measured (4.9 / 4.5 s), not their split + +## 7. Liquid, as drawn (not a firmware command) +A vessel's cavity is tinted from white to orange by volume ÷ capacity, stepping to 35% for any liquid at all (PR #1378 page, `live.js` `refreshOverlays`; not ours). It changes when the model's state update arrives, after the command. Tips show their contents only in the 2D channel panel. Moving liquid (a surface lowered over the dwell, a column rising in the tip) is not drawn yet. The data for it are PLR's (`Container.compute_height_from_volume`) plus the `C0 AS` fields above. + +--- + +## Sources +- PyLabRobot forum: [surface following, post 6](https://discuss.pylabrobot.org/t/hamilton-surface-following-issues/304/6); [z-touch firmware, posts 2–3](https://discuss.pylabrobot.org/t/reason-behind-z-touch-only-being-available-to-8-channels-made-in-2022-and-onwards/543/3); [Tame the iSWAP, post 1](https://discuss.pylabrobot.org/t/intro-to-epic-tame-the-iswap/517/1) and [post 4](https://discuss.pylabrobot.org/t/intro-to-epic-tame-the-iswap/517/4). +- labautomation.io: [side touch, post 6](https://labautomation.io/t/post-dispense-tip-touch-on-starlet/2255/6) and [post 9](https://labautomation.io/t/post-dispense-tip-touch-on-starlet/2255/9); [touch-off and the MPH, post 2](https://labautomation.io/t/touch-off-with-mph/1788/2); [Transport Paths Editor](https://labautomation.io/t/transport-paths-editor/5759). +- PyLabRobot PRs: [#1362 LLD aspirate](https://github.com/PyLabRobot/pylabrobot/pull/1362), [#1333 cLLD](https://github.com/PyLabRobot/pylabrobot/pull/1333), [#1334 pLLD](https://github.com/PyLabRobot/pylabrobot/pull/1334), [#1336 probing](https://github.com/PyLabRobot/pylabrobot/pull/1336), [#1352 96-head cLLD](https://github.com/PyLabRobot/pylabrobot/pull/1352), [#63 PLACE_SHIFT heights](https://github.com/PyLabRobot/pylabrobot/pull/63). diff --git a/pylabrobot/hamilton/star/driver/features/core_grippers.py b/pylabrobot/hamilton/star/driver/features/core_grippers.py index 9e19d35d0c4..e2d2ecca208 100644 --- a/pylabrobot/hamilton/star/driver/features/core_grippers.py +++ b/pylabrobot/hamilton/star/driver/features/core_grippers.py @@ -77,6 +77,10 @@ def __init__( self._holding_resource_width: Optional[float] = None self._held_resource: Optional[Resource] = None self._taken_from: Optional[Tuple[Resource, Optional[Coordinate]]] = None + # For whoever acts a command out (the viewer), set before it is sent: what a grip takes and + # where it will hang from the front tool, or what a release puts down and where. Cleared by + # nothing in particular: each grip or release overwrites it. + self._handover: Optional[Tuple[Resource, Optional[Resource], Optional[Coordinate]]] = None # -- what carries them --------------------------------------------------------------------------- @@ -168,19 +172,19 @@ def _front_tool(self) -> Optional[Resource]: shaft = self._pipettes.shaft(self._front_channel) return shaft.tip if shaft is not None and shaft.has_tip() else None - def _hang_held_resource_on_the_front_tool(self) -> None: - """Hang the held resource from the front tool where the jaws hold it, so it rides with them. + def _hang_location(self, held: Resource, from_top: Optional[float]) -> Optional[Coordinate]: + """Where `held` hangs from the front tool, as its child location, or None without the model. - Its centre at the jaws' centre, its top `pickup_distance_from_top` above the grip line - the - front channel's stop disc less the tool's overhang. Nothing happens while nothing models them. + Its centre at the jaws' centre, its top `from_top` below the grip line - the front channel's + stop disc less the tool's overhang. """ - held, from_top, tool = self._held_resource, self._pickup_distance_from_top, self._front_tool() + tool = self._front_tool() if held is None or from_top is None or not isinstance(tool, HamiltonCoreGripperTool): - return + return None back = self._pipettes.get_reference_point_location(cast(int, self._back_channel)) front = self._pipettes.get_reference_point_location(cast(int, self._front_channel)) if back is None or front is None: - return + return None grip_line = front.z - (tool.get_size_z() - tool.fitting_depth - tool.grip_line_height) center = held.center().rotated(held.get_absolute_rotation()) lfb = Coordinate( @@ -188,8 +192,20 @@ def _hang_held_resource_on_the_front_tool(self) -> None: (back.y + front.y) / 2 - center.y, grip_line + from_top - held.get_absolute_size_z(), ) + return lfb - tool.get_location_wrt(self._deck) + + def _hang_held_resource_on_the_front_tool(self) -> None: + """Hang the held resource from the front tool where the jaws hold it, so it rides with them. + + Nothing happens while nothing models them. + """ + held, from_top = self._held_resource, self._pickup_distance_from_top + location = self._hang_location(held, from_top) if held is not None else None + tool = self._front_tool() + if held is None or location is None or not isinstance(tool, HamiltonCoreGripperTool): + return held.unassign() - tool.assign_child_resource(held, location=lfb - tool.get_location_wrt(self._deck)) + tool.assign_child_resource(held, location=location) def _put_held_resource_on_the_deck(self) -> None: """Put a resource hanging from the front tool on the deck, where it is now.""" @@ -856,6 +872,9 @@ async def pick_up_resource( raise ValueError(f"the jaws would close to {closed} mm; squeeze_mm is too large") await pipettes._require_iswap_parked() + # For whoever acts the command out: what is taken, and where it will hang. + self._handover = (resource, self._front_tool(), self._hang_location(resource, from_top)) + source = (resource.parent, resource.location) try: async with pipettes._temporary_z_drive_profile( @@ -966,6 +985,9 @@ async def drop_resource( raise ValueError(f"y_clearance must be 0 or more, is {y_clearance}") await pipettes._require_iswap_parked() + # For whoever acts the command out: what is put down, and where it lands. + self._handover = (held, destination, child) + try: async with pipettes._temporary_z_drive_profile( acceleration=z_acceleration, channels=[back, front] diff --git a/pylabrobot/hamilton/star/driver/features/head.py b/pylabrobot/hamilton/star/driver/features/head.py index 1550a81bc56..39fe4266b56 100644 --- a/pylabrobot/hamilton/star/driver/features/head.py +++ b/pylabrobot/hamilton/star/driver/features/head.py @@ -753,6 +753,28 @@ def update_location_by_reference_point( here.z if z is None else z - on_the_arm.z - shaft.z, ) + def get_reference_point_location(self) -> Optional[Coordinate]: + """Where the model has the head's reference point, in mm on the deck. + + The inverse of `update_location_by_reference_point`: it converts a reported position into a + location, and this converts a location back into the position that would be reported. X is the + arm's, so it is carried through unread. + + Returns: + Where the model has it, or None when there is nothing modelling the head yet. + """ + deck = self._driver.deck + if self.resource is None or self.resource.location is None or deck is None: + return None + arm = self.resource.parent + if arm is None: + return None + # The drives report channel A1's axis: the shaft's centre, not the corner it is placed by. + shaft = self.resource.get_item(HEAD_REFERENCE_SHAFT).get_location_wrt( + self.resource, "c", "c", "b" + ) + return self.resource.location + arm.get_location_wrt(deck) + shaft + # ---------------------------------------- # Movement # ---------------------------------------- diff --git a/pylabrobot/hamilton/star/driver/features/head96.py b/pylabrobot/hamilton/star/driver/features/head96.py index 81d9babed1b..8d3c140bd61 100644 --- a/pylabrobot/hamilton/star/driver/features/head96.py +++ b/pylabrobot/hamilton/star/driver/features/head96.py @@ -751,6 +751,27 @@ async def pick_up_tips( await self._record_after_tip_command(command_error) await self.dispensing_drive_request_uL_position() + def _spots_under_shafts(self, offset: Optional[Coordinate]) -> List[Optional[int]]: + """For each shaft, in spot order (A1, B1, ..., H12), the index of the rack spot it stands over + when head channel A1 is sent to spot A1 plus `offset`, or None where it is past the rack. + + Raises: + ValueError: If the offset leaves the channels between spots rather than over them. + """ + pitch = self.configuration.channel_pitch + offset = offset or Coordinate.zero() + columns, rows = round(offset.x / pitch), round(-offset.y / pitch) + for residual in (offset.x - columns * pitch, -offset.y - rows * pitch): + if abs(residual) > 1.0: + raise ValueError( + f"an offset of ({offset.x}, {offset.y}) puts the channels between the rack's spots" + ) + under: List[Optional[int]] = [] + for shaft in range(96): + column, row = shaft // 8 + columns, shaft % 8 + rows + under.append(column * 8 + row if 0 <= column < 12 and 0 <= row < 8 else None) + return under + # -- tip drop ------------------------------------------------------------------------------------ async def drop_tips( diff --git a/pylabrobot/hamilton/star/driver/features/iswap_collisions.py b/pylabrobot/hamilton/star/driver/features/iswap_collisions.py new file mode 100644 index 00000000000..633978ca2b6 --- /dev/null +++ b/pylabrobot/hamilton/star/driver/features/iswap_collisions.py @@ -0,0 +1,399 @@ +"""What an iSWAP transport plan sweeps through, and what it would hit. + +`check_plan(transport, plan)` follows a plan's steps through the arm's joints and turns each into +the moves of every part the plan moves, rigidly: + +- the iSWAP's parts: the column (the iSWAP head) shifts with the elbow; link 1 turns about the + elbow and shifts with it; the gripper and anything held turn about the wrist and shift with it; + the fingers do too, slid along the jaws by however far they stand open; +- the X-arm's own body, and everything mounted on it that the plan does not drive - the channels, + the 96-head - shifts with X: the plan is what moves the carriage, so it is what moves them. + +Every group is swept and every pair of groups is compared over the times they share, so a gripper +swinging under lowered channels is caught the same way a plate carried into a rack is. What a plan +means to touch is never reported: what is picked up and what it stands on, what it is put down on +and what that stands on. Parts of one machine are let off each other, as is anything mounted +together on the carriage, which nothing in a plan moves relative to anything else in it. + +Turns are cut finely enough that no point strays more than `TURN_SLACK` from the hulls taken +(`Kinematics.arcs`): the hull of a piece at both ends of an arc holds every chord, and a point's way +strays from its chord by no more than its curvature allows. +""" + +from __future__ import annotations + +import dataclasses +import math +from typing import Dict, List, Optional, Tuple + +from pylabrobot.hamilton.star.driver.features.iswap_transport import ( + Grip, + Open, + Plan, + Release, + Rise, + Travel, + iSWAPTransport, +) +from pylabrobot.hamilton.star.driver.features.star_collisions import ( + ENCLOSURES, + Exemptions, + _allowed, + held_allowed, + iswap_names, + mounted_groups, + root_of, + scene, +) +from pylabrobot.resources.collision import ( + Collision, + Group, + Pose, + Segment, + check, +) +from pylabrobot.resources.resource import Resource + +# The most any point of the arm may stray from the hulls a turn is checked with, in mm. +TURN_SLACK = 0.5 +# How far past the grip centre anything the gripper carries reaches, at most, in mm: the fingers +# and a plate's half-diagonal. Only bounds how finely turns are cut. +CARRIED_REACH = 150.0 + + +@dataclasses.dataclass(frozen=True) +class Joints: + """Where the arm's drives are: the elbow's X and Y and the grip centre's Z, in mm on the deck, and + the two joints' drive angles, in degrees.""" + + x: float + y: float + z: float + elbow: float + wrist: float + + def but(self, **changes: float) -> "Joints": + """These joints with the named drives changed.""" + return dataclasses.replace(self, **changes) + + +@dataclasses.dataclass +class Parts: + """The arm's rigid parts, and the X-arm they ride.""" + + column: Resource + link: Resource + gripper: Resource + fingers: Tuple[Resource, Resource] + arm: Resource + + +def parts_of(transport: iSWAPTransport) -> Parts: + """The iSWAP's rigid parts as resources, from what the gripper hangs from.""" + gripper = transport.iswap.gripper + link = gripper.parent + column = link.parent + arm = column.parent + left, right = gripper.fingers + return Parts(column, link, gripper, (left, right), arm) + + +def joints_now(transport: iSWAPTransport) -> Joints: + """Where the transport's arm stands now, as `Joints`.""" + iswap = transport.iswap + drive = iswap.elbow_get_reference_point_location() + elbow, wrist = iswap.elbow_drive_get_angle(), iswap.wrist_drive_get_angle() + if drive is None or elbow is None or wrist is None: + raise RuntimeError("the iSWAP is not modelled, so what it sweeps is not known") + return Joints(drive.x, drive.y, transport._grip_z_now(), elbow, wrist) + + +class Kinematics: + """Where the parts go, as rigid moves from where they are at `now`, for any joints.""" + + def __init__(self, transport: iSWAPTransport, now: Joints): + self.link_1, self.tool, _ = transport._lengths() + self.now = now + self.origin = transport.deck.get_absolute_location() # the checks are made in absolute terms + turned = math.radians(transport.iswap.gripper.get_absolute_rotation().z) + self.jaw_axis = (-math.sin(turned), math.cos(turned)) # the gripper's own +Y, now + + def wrist(self, j: Joints) -> Tuple[float, float]: + """Where the wrist joint sits at `joints`, on the deck.""" + link = math.radians(j.elbow - 90.0) + return j.x + self.link_1 * math.cos(link), j.y + self.link_1 * math.sin(link) + + def _abs(self, x: float, y: float) -> Tuple[float, float, float]: + """(`x`, `y`) on the deck as absolute terms.""" + return (x + self.origin.x, y + self.origin.y, 0.0) + + def column(self, j: Joints) -> Pose: + """The column's move to `joints`: it slides with the carriage.""" + return Pose(shift=(j.x - self.now.x, j.y - self.now.y, j.z - self.now.z)) + + def link(self, j: Joints) -> Pose: + """Link 1's move to `joints`: it turns about the column as the elbow turns.""" + return Pose(j.elbow - self.now.elbow, self._abs(self.now.x, self.now.y), self.column(j).shift) + + def gripper(self, j: Joints) -> Pose: + """The gripper's move to `joints`: it turns as the elbow and wrist turn, and slides as the + wrist's end does.""" + w0, w = self.wrist(self.now), self.wrist(j) + turn = (j.elbow - self.now.elbow) + (j.wrist - self.now.wrist) + return Pose(turn, self._abs(*w0), (w[0] - w0[0], w[1] - w0[1], j.z - self.now.z)) + + def finger(self, j: Joints, side: float, opened: float) -> Pose: + """A finger, slid out by half of `opened` - how much wider the jaws are than now - on its side.""" + slide = side * opened / 2.0 + return self.gripper(j).after(self.jaw_axis[0] * slide, self.jaw_axis[1] * slide) + + def arcs(self, a: Joints, b: Joints) -> Tuple[int, float]: + """How many arcs a turn from `a` to `b` is cut into, and how far from the hulls a point strays. + + A point the gripper carries is at E + R(e) a + R(e + w) b - the elbow, link 1 turned by the + elbow, and the point's place from the wrist turned by both - so over an arc of turns de and dw + its second derivative is at most de^2 |a| + (de + dw)^2 |b|, and a point of link 1 at most + de^2 of its distance from the elbow. A curve strays from its chord by at most an eighth of its + second derivative's bound. + """ + de, dw = math.radians(abs(b.elbow - a.elbow)), math.radians(abs(b.wrist - a.wrist)) + carried = self.tool + CARRIED_REACH + bend = max( + de * de * (self.link_1 + CARRIED_REACH), de * de * self.link_1 + (de + dw) ** 2 * carried + ) + if bend == 0: + return 1, 0.0 + n = max(1, math.ceil(math.sqrt(bend / (8 * TURN_SLACK)))) + return n, bend / (n * n) / 8 + + +SIDES = (1.0, -1.0) # the fingers' sides of the jaws, as the gripper model orders them + + +class _Sweeps: + """The segments each part sweeps, a plan step taking one unit of time. + + The parts' poses are relative to the carriage they stand on: X is the carriage's own drive, and + its spans are the frame's (`carried`), not each part's. What the carriage carries is swept with + the frame applied against what stands still, and relative to it against what rides with it. + """ + + def __init__(self, kin: Kinematics): + self.kin = kin + self.opened = 0.0 + # While something is held: how it was put on the gripper - undoing the gripper's move up to + # the grip, so that it rides the gripper's moves from there on. + self.held_since: Optional[Pose] = None + self.parts: Dict[str, List[Segment]] = { + "held": [], + "column": [], + "link": [], + "gripper": [], + "finger 0": [], + "finger 1": [], + } + # How the carriage itself moves: X, over the steps that span it. + self.carried: List[Segment] = [] + + def _states(self, a: Joints, b: Joints, f0: float, f1: float) -> List[Joints]: + """The joints on the line from `a` to `b`, at fractions `f0` and `f1` of it - the x at the + plan's own, which the carriage's frame carries.""" + + def at(f: float) -> Joints: + return Joints( + self.kin.now.x, + a.y + (b.y - a.y) * f, + a.z + (b.z - a.z) * f, + a.elbow + (b.elbow - a.elbow) * f, + a.wrist + (b.wrist - a.wrist) * f, + ) + + return [at(f) for f in (f0, f1)] + + def _add(self, states: List[Joints], slack: float, s0: float, s1: float, opens=(0.0, 0.0)): + """Append a segment for every part: its poses at `states`, the fingers opened by `opens`.""" + kin = self.kin + for name, pose in (("column", kin.column), ("link", kin.link), ("gripper", kin.gripper)): + self.parts[name].append(Segment([pose(s) for s in states], slack, s0, s1)) + for k, side in enumerate(SIDES): + poses = [kin.finger(s, side, self.opened + o) for s in states for o in set(opens)] + self.parts[f"finger {k}"].append(Segment(poses, slack, s0, s1)) + if self.held_since is not None: + fix = self.held_since + poses = [fix.then(kin.gripper(s)) for s in states] + self.parts["held"].append(Segment(poses, slack, s0, s1)) + + def move(self, a: Joints, b: Joints, t0: float, t1: float) -> None: + """From `a` to `b` - the joints on a straight line in joint space. A move with no turn in it is + a box on independent axes: exact. The segment's poses end where the motion ends, at `b`.""" + n, slack = self.kin.arcs(a, b) + if slack == 0: + corners = [a.but(y=y, z=z) for y in (a.y, b.y) for z in (a.z, b.z)] + # Each corner once, at the plan's own x: the span of the carriage is the frame's to carry. + states = [self._states(c, c, 0.0, 0.0)[0] for c in corners] + self._add(states, 0.0, t0, t1) + return + for k in range(n): + states = self._states(a, b, k / n, (k + 1) / n) + self._add(states, slack, t0 + (t1 - t0) * k / n, t0 + (t1 - t0) * (k + 1) / n) + + def jaws(self, at: Joints, change: float, t0: float) -> None: + """The fingers sliding apart by `change`, at `at`.""" + self._add([self._states(at, at, 0.0, 0.0)[0]], 0.0, t0, t0 + 1, (0.0, change)) + self.opened += change + + +@dataclasses.dataclass +class PlanSweeps: + """What each part the plan moves sweeps over it, and what the plan means to touch.""" + + groups: List[Group] + touches: List[Resource] + held: Optional[Resource] = None + exemptions: Exemptions = dataclasses.field(default_factory=Exemptions) + + +def _standing_on(resource: Resource, stop: Resource) -> List[Resource]: + """`resource` and what it stands on, up to but not including `stop`.""" + out = [] + r: Optional[Resource] = resource + while r is not None and r is not stop: + out.append(r) + r = r.parent + return out + + +def sweeps(transport: iSWAPTransport, plan: Plan) -> PlanSweeps: + """Follow `plan` from where the arm is now, as `iSWAPTransport.execute` runs it.""" + parts = parts_of(transport) + now = joints_now(transport) + sw = _Sweeps(Kinematics(transport, now)) + width = float(transport.iswap.gripper.jaw_width) + held: Optional[Resource] = transport.holding + if held is not None: + sw.held_since = Pose() + deck = transport.deck + touches: List[Resource] = [] + j = now + for t, step in enumerate(plan.steps): + if isinstance(step, Rise): + b = j.but(z=step.z) + sw.move(j, b, t, t + 1) + j = b + elif isinstance(step, Travel): + target = Joints(step.elbow_x, step.elbow_y, j.z, step.elbow_angle, step.wrist_angle) + start, end = j, target.but(x=j.x) # X is spanned whole; the legs are Y and the turn + turned = start.but(elbow=end.elbow, wrist=end.wrist) + if step.order == "translate_first": + legs = [(start, start.but(y=end.y)), (start.but(y=end.y), end)] + elif step.order == "turn_first": + legs = [(start, turned), (turned, end)] + else: + at = start.but(y=step.turn_y if step.turn_y is not None else start.y) + there = at.but(elbow=end.elbow, wrist=end.wrist) + legs = [(start, at), (at, there), (there, end)] + for k, (a, b) in enumerate(legs): + sw.move(a, b, t + k / len(legs), t + (k + 1) / len(legs)) + # Where the carriage stands from the plan's start: still where it began, then at the travel's + # end - later stretches rest where this one leaves it. + sw.carried.append( + Segment( + [Pose(shift=(j.x - now.x, 0.0, 0.0)), Pose(shift=(step.elbow_x - now.x, 0.0, 0.0))], + 0.0, + t, + t + 1, + ) + ) + j = target + elif isinstance(step, (Open, Release)): + if step.width > width: + sw.jaws(j, step.width - width, t) + else: + sw.opened += step.width - width + width = step.width + if isinstance(step, Release): + sw.held_since = None + touches += [step.resource, *step.resource.get_all_children()] + if isinstance(step.destination, Resource): + touches += _standing_on(step.destination, deck) + elif isinstance(step, Grip): + sw.opened += step.width - width # closing on it: towards what they are meant to touch + width = step.width + held = step.resource + sw.held_since = sw.kin.gripper(j).inverse() + touches += [step.resource, *step.resource.get_all_children()] + if step.resource.parent is not None: + touches += _standing_on(step.resource.parent, deck) + + # What the plan moves without driving it: everything mounted on the X-arm that is not the iSWAP's + # own column. The plan is what moves the carriage, so it is what moves these; they keep their + # place on the carriage and ride the frame with it. + groups = mounted_groups( + parts.arm, + float(len(plan.steps)), + sw.parts, + carrying=transport.holding, + held=held, + frame=sw.carried, + ) + machine = iswap_names(groups) + carriage = {r.name for r in parts.arm.children if r is not parts.column} + return PlanSweeps( + groups, + touches, + held, + Exemptions( + machine=machine, + carriage=carriage, + body="X-arm", + held=held.name if held is not None else None, + ), + ) + + +def judge( + transport: iSWAPTransport, plan: Plan, clearance: float = 0.0, root: Optional[Resource] = None +) -> Tuple[List[Collision], Dict[str, Group]]: + """Everything `plan` would bring the arm, what it holds, or what rides the X-arm with it, within + `clearance` mm of, and the sweeps it was judged with. + + Args: + transport: the transport the plan is for, with the arm where the plan starts. + plan: from `plan_pick_up` or `plan_drop`. + clearance: how close is too close, in mm. 0 reports only what meets. + root: what stands around the arm; the whole tree the deck is in when None. + + Returns: + What the plan would hit, and each group that was judged by name, so what would have met can be + brought to where the meeting happened. + """ + parts = parts_of(transport) + if root is None: + root = root_of(transport.deck) + around = sweeps(transport, plan) + standing = scene(root, ENCLOSURES).obstacles([parts.arm]) + found = check( + root, + around.groups, + clearance, + _allowed(around.touches), + standing, + between_groups=around.exemptions.between, + allow_for=held_allowed(around.held, around.touches), + ) + return found, {g.name: g for g in around.groups} + + +def check_plan( + transport: iSWAPTransport, plan: Plan, clearance: float = 0.0, root: Optional[Resource] = None +) -> List[Collision]: + """Everything `plan` would bring the arm, what it holds, or what rides the X-arm with it, within + `clearance` mm of. + + Args: + transport: the transport the plan is for, with the arm where the plan starts. + plan: from `plan_pick_up` or `plan_drop`. + clearance: how close is too close, in mm. 0 reports only what meets. + root: what stands around the arm; the whole tree the deck is in when None. + """ + return judge(transport, plan, clearance, root)[0] diff --git a/pylabrobot/hamilton/star/driver/features/iswap_collisions_tests.py b/pylabrobot/hamilton/star/driver/features/iswap_collisions_tests.py new file mode 100644 index 00000000000..14268303154 --- /dev/null +++ b/pylabrobot/hamilton/star/driver/features/iswap_collisions_tests.py @@ -0,0 +1,222 @@ +"""What iSWAP transport plans sweep through, on a simulated STARlet's demo deck.""" + +import unittest +import unittest.mock + +from pylabrobot.hamilton.star.driver.features import star_collisions +from pylabrobot.hamilton.star.driver.features.iswap_collisions import ( + Joints, + Kinematics, + _allowed, + check_plan, + joints_now, + sweeps, +) +from pylabrobot.hamilton.star.driver.features.iswap_transport import ( + iSWAPCollisionError, + iSWAPTransport, +) +from pylabrobot.resources.collision import distance, solid_pieces +from pylabrobot.resources.resource import Resource +from pylabrobot.resources.tip_rack import TipRack +from pylabrobot.resources.tip_tracking import does_tip_tracking, set_tip_tracking +from pylabrobot.visualizer3D.demo import build_facility, star_of + + +class iSWAPCollisionTests(unittest.IsolatedAsyncioTestCase): + async def asyncSetUp(self) -> None: + was = does_tip_tracking() + set_tip_tracking(True) + self.addCleanup(set_tip_tracking, was) + self.facility = build_facility() + self.star = star_of(self.facility) + self.deck = self.star.deck + self.deck.get_resource("destination_1").unassign() + await self.star.setup() + iswap = self.star.iswap + assert iswap is not None + self.iswap = iswap + # Plans are checked here by hand, and some are run through what they hit to see where it ends. + self.transport = iSWAPTransport(iswap, check_collisions=False) + self.site = self.deck.get_resource("destination_carrier").children[1] + + def obstacles(self, plan) -> set: + return {c.obstacle.name for c in check_plan(self.transport, plan)} + + async def test_what_the_sweeps_end_on_is_where_the_model_puts_the_parts(self): + await self.iswap.make_space() + plate = self.deck.get_resource("source_1") + for plan_it in ( + lambda: self.transport.plan_pick_up(plate, direction="back"), + lambda: self.transport.plan_drop(self.site, direction="left"), + ): + plan = plan_it() + swept = sweeps(self.transport, plan) + predicted = [] + for group in swept.groups: + last = group.segments[-1].poses[-1] + # What the group rides - the carriage's X - is not in its own poses; the frame carries it. + carried = group.frame_poses(group.segments[-1])[-1] + world = last.then(carried) + predicted += [(p.resource, [world.apply(q) for q in p.points]) for p in group.pieces] + await self.transport.execute(plan) + for resource, points in predicted: + now = {tuple(round(v, 6) for v in p.points[0]): p for p in solid_pieces(resource)} + best = min( + max(abs(a - b) for q, r in zip(piece.points, points) for a, b in zip(q, r)) + for piece in now.values() + ) + # The finger tips sit ~1.5 mm from where the idealized wrist geometry of the kinematics + # puts them over a quarter turn - the model's recorded gripper geometry, not a bookkeeping + # error. The check is for the sweeps ending where the model puts the parts. + self.assertLess(best, 2.0, resource.name) + + async def test_no_point_of_a_turn_strays_further_from_its_hulls_than_their_slack(self): + await self.iswap.make_space() + plan = self.transport.plan_pick_up(self.deck.get_resource("source_1"), direction="back") + swept = sweeps(self.transport, plan) + kin = Kinematics(self.transport, joints_now(self.transport)) + gripper = next(g for g in swept.groups if g.name == "iSWAP gripper") + turning = [s for s in gripper.segments if s.slack > 0] + self.assertTrue(turning) + # The way is walked from the joints themselves: a big turn, cut as the sweeps cut it. + now = joints_now(self.transport) + target = Joints(now.x, now.y, now.z, now.elbow - 90.0, now.wrist + 60.0) + n, slack = kin.arcs(now, target) + for k in range(0, n, max(1, n // 10)): + ends = [ + now.but( + elbow=now.elbow + (target.elbow - now.elbow) * f, + wrist=now.wrist + (target.wrist - now.wrist) * f, + ) + for f in (k / n, (k + 1) / n) + ] + for piece in gripper.pieces: + hull = [kin.gripper(j).apply(q) for j in ends for q in piece.points] + for m in range(1, 10): + f = (k + m / 10) / n + j = now.but( + elbow=now.elbow + (target.elbow - now.elbow) * f, + wrist=now.wrist + (target.wrist - now.wrist) * f, + ) + for q in piece.points: + self.assertLessEqual(distance([kin.gripper(j).apply(q)], hull), slack + 1e-9) + + async def test_a_plate_moved_at_the_fixed_height_hits_nothing(self): + await self.iswap.make_space() + plate = self.deck.get_resource("source_1") + pick = self.transport.plan_pick_up(plate, direction="front") + self.assertEqual(self.obstacles(pick), set()) + await self.transport.execute(pick) + self.assertEqual(self.obstacles(self.transport.plan_drop(self.site, direction="front")), set()) + + async def test_a_tip_carrier_flush_against_the_source_carrier_stops_the_grip(self): + # The carrier's declared shape is what the checks see: its outer wall rises beside the sites, + # and the fingers pass below its top as they come down to grip. Butted against the source + # carrier there is no room for them; the demo deck keeps a track of clearance. + tip_carrier = self.deck.get_resource("tip_carrier") + self.deck.unassign_child_resource(tip_carrier) + self.deck.assign_child_resource(tip_carrier, track=2) + await self.iswap.make_space() + plate = self.deck.get_resource("source_1") + self.assertIn("tip_carrier", self.obstacles(self.transport.plan_pick_up(plate, "front"))) + gated = iSWAPTransport(self.iswap) + before = joints_now(gated) + with self.assertRaises(iSWAPCollisionError) as refused: + await gated.pick_up_resource(plate, direction="front") + self.assertIn("tip_carrier", {c.obstacle.name for c in refused.exception.collisions}) + self.assertEqual(joints_now(gated), before) + self.assertIsNone(gated.holding) + + async def test_a_plate_turned_a_quarter_onto_a_landscape_site_meets_its_neighbour(self): + # PyLabRobot places it; the plate is 127.8 mm deep turned, the sites 96 mm apart. + await self.iswap.make_space() + await self.transport.pick_up_resource(self.deck.get_resource("source_1"), direction="front") + drop = self.transport.plan_drop(self.site, direction="left") + self.assertIn("destination_2", self.obstacles(drop)) + + async def test_carried_too_low_it_sweeps_through_what_the_fixed_height_clears(self): + await self.iswap.make_space() + source_3 = self.deck.get_resource("source_3") + await self.transport.pick_up_resource(source_3, direction="front", end_height=205.0) + self.assertEqual(self.obstacles(self.transport.plan_drop(self.site)), set()) + low = self.transport.plan_drop(self.site, traverse_height=205.0) + self.assertIn("source_2", self.obstacles(low)) + + async def test_the_elbow_nearest_the_joints_can_bring_link_1_down_onto_a_tip_rack(self): + await self.iswap.make_space() + plate = self.deck.get_resource("source_3") + right = self.transport.plan_pick_up(plate, direction="front", elbow="right") + self.assertIn("tips_2", self.obstacles(right)) + front = self.transport.plan_pick_up(plate, direction="front", elbow="front") + self.assertNotIn("tips_2", self.obstacles(front)) + + async def test_tips_on_the_channels_are_in_the_way_and_the_housing_is_not(self): + rack = self.deck.get_resource("tips_0") + assert isinstance(rack, TipRack) and self.star.pipettes is not None + await self.star.pipettes.pick_up_tips([rack.get_item(f"{row}1") for row in "ABCDEFGH"]) + plan = self.transport.plan_pick_up(self.deck.get_resource("source_1"), direction="front") + hits = check_plan(self.transport, plan) + mounted = {c.obstacle.name for c in hits if c.obstacle.parent is not None} + self.assertTrue(any(name.startswith("tips_0_tipspot_") for name in mounted)) + self.assertFalse(any("housing" in c.obstacle.name for c in hits)) + + async def test_unparked_channels_are_in_the_way_and_parking_clears_them(self): + # The channels stand lowered over the deck, in the iSWAP's own way to the plate. The plan is + # refused for the channels themselves; clearing the arm - the channels raised to Z safety and + # moved aside, as the firmware's own plate commands do - lets it run. + assert self.star.pipettes is not None + await self.star.pipettes.move_stop_disc_to_z_positions({i: 200.0 for i in range(8)}) + plan = self.transport.plan_pick_up(self.deck.get_resource("source_1"), direction="front") + hits = check_plan(self.transport, plan) + self.assertTrue(hits) + self.assertTrue( + all("pipette_channel" in (c.other_group or "") for c in hits), + {(c.obstacle.name, c.other_group) for c in hits}, + ) + await self.iswap.make_space() + self.assertEqual(check_plan(self.transport, plan), []) + + async def test_a_plan_that_would_hit_something_is_refused_before_anything_moves(self): + await self.iswap.make_space() + plate = self.deck.get_resource("source_3") + home = plate.parent + gated = iSWAPTransport(self.iswap) + before = joints_now(gated) + with self.assertRaises(iSWAPCollisionError) as refused: + await gated.pick_up_resource(plate, direction="front", elbow="right") + self.assertIn("tips_2", {c.obstacle.name for c in refused.exception.collisions}) + self.assertEqual(joints_now(gated), before) + self.assertIs(plate.parent, home) + self.assertIsNone(gated.holding) + # The other elbow clears it, and runs. + await gated.pick_up_resource(plate, direction="front", elbow="front") + self.assertIs(gated.holding, plate) + + async def test_the_arm_passes_close_to_what_it_touches_but_not_into_a_shape(self): + # What the arm is meant to touch is let off only where it is a box - a plate, a site - and the + # box stands for the thing only roughly. A resource with declared hulls is its shape, and the + # arm's parts must clear it even on the way to put something down in it. + hulls = ((0.0, 0.0, 0.0), (10.0, 0.0, 0.0), (0.0, 10.0, 0.0), (0.0, 0.0, 10.0)) + boxed, shaped = Resource("boxed", 10, 10, 10), Resource("shaped", 10, 10, 10, model="shaped") + with unittest.mock.patch( + "pylabrobot.hamilton.star.driver.features.star_collisions.declared_hulls", + lambda r: (hulls,) if r.model == "shaped" else None, + ): + self.assertEqual(_allowed([boxed, shaped]), [boxed]) + + async def test_a_second_check_builds_no_new_scene(self): + await self.iswap.make_space() + plan = self.transport.plan_pick_up(self.deck.get_resource("source_1"), direction="back") + # Once the deck's pieces are worked out they are kept: a second check redoes only what changed, + # which is what makes it quick, and is said in what it builds rather than in seconds, which no + # two machines agree on. + check_plan(self.transport, plan) + scene = star_collisions.StaticScene + with unittest.mock.patch.object(star_collisions, "StaticScene", side_effect=scene) as built: + check_plan(self.transport, plan) + self.assertEqual(built.call_count, 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/hamilton/star/driver/features/iswap_transport.py b/pylabrobot/hamilton/star/driver/features/iswap_transport.py new file mode 100644 index 00000000000..795f5d3d17b --- /dev/null +++ b/pylabrobot/hamilton/star/driver/features/iswap_transport.py @@ -0,0 +1,764 @@ +"""Moving plates and lids with the iSWAP, planned in Python from its primitive moves. + +The firmware's own plate commands (`C0 PP`, `C0 PR`, `C0 PM`) take a plate, a grip direction and a +few heights, and decide everything else: which of the three ways the arm can reach a grip it folds +into, the order its drives move in, and how. What they do is known only from their parameters. This +module does the same job from the moves the driver already has - the X-arm, the elbow's Y and Z, +both joints at once, the jaws - so every step is known, can be read before it runs, and can be +changed. + +The plan follows the phases `C0 PP`'s parameters describe: the jaws open to the plate's width and a +margin (`open_gripper_position`), the arm rises to its traverse height +(`minimum_traverse_height_at_beginning_of_a_command`) and travels there, it comes down to the +gripping height (`z_position`), the jaws close on the plate with a force-sensed width window +(`plate_width`, `plate_width_tolerance`, `grip_strength`), and it rises to where the command ends +(`z_position_at_the_command_end`). Putting a plate down is the same travel and descent, the jaws +opening instead. The defaults are legacy's (`STARBackend.pick_up_resource`). + +Which way round the arm reaches is worked out, not left to the firmware: with the two links known +(`link_1_length`, the gripper's `tool_center_point`), a grip centre and the direction the gripper +faces fix the wrist and the elbow for each of the three elbow stops, and the plan takes the one that +the drives can reach, the driver's pose check passes, and is nearest where the joints are now. + +The resource tree follows the plate: once the jaws have closed on it, it hangs from the gripper +where it is; once they open, it is placed where PyLabRobot places anything on what it was put down +on - a site, a plate adapter, a stack, a plate for a lid. + +Before a plan runs, what it sweeps is checked against what stands around the arm and what rides the +X-arm with it (`iswap_collisions.check_plan`); a plan that would hit something is refused with an +`iSWAPCollisionError`, before anything moves. +""" + +import asyncio +import dataclasses +import math +from typing import ( + Any, + Awaitable, + Callable, + Dict, + List, + Literal, + Optional, + Sequence, + Tuple, + Union, + cast, +) + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.lid import Lid, Liddable +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.plate_adapter import PlateAdapter +from pylabrobot.resources.resource import Resource +from pylabrobot.resources.resource_holder import ResourceHolder +from pylabrobot.resources.resource_stack import ResourceStack +from pylabrobot.resources.rotation import Rotation +from pylabrobot.resources.trash import Trash + +GripDirection = Literal["front", "back", "left", "right"] + +# The side of the resource the arm grips from, as legacy's `GripDirection` means it and the firmware +# numbers it (`C0 PP gr`: 1 = -Y, 2 = +X, 3 = +Y, 4 = -X): the side the wrist is on. The gripper then +# faces the other way - a front grip faces +Y, deck degrees counter-clockwise from +X, as +# `GRIPPER_DECK_DIRECTIONS` states them. +GRIPPER_FACING: Dict[str, float] = {"front": 90.0, "right": 180.0, "back": -90.0, "left": 0.0} + +# The elbow stops, as the deck angle link 1 lies along: the drive's own angle less a quarter turn. +ELBOW_STOPS: Dict[str, float] = {"left": -180.0, "front": -90.0, "right": 0.0} + +# Legacy's defaults (`STARBackend.pick_up_resource`), in mm. +OPEN_MARGIN = 3.0 # the jaws open this much wider than the plate +WIDTH_TOLERANCE = 2.0 +GRIP_STRENGTH = 4 +PICKUP_DISTANCE_FROM_TOP = 5.0 # when the resource states no `preferred_pickup_location` +# How far below a lid's skirt a lidded plate is gripped, in mm. A lid comes down over the plate's +# sides by its `nesting_z_height`; jaws closing within that close on the lid, not the plate. +BELOW_LID = 2.0 + + +def _norm(degrees: float) -> float: + """An angle in (-180, 180].""" + return (degrees + 180.0) % 360.0 - 180.0 if (degrees + 180.0) % 360.0 else 180.0 + + +def placement( + deck: Resource, resource: Resource, destination: Union[Resource, Coordinate], turned_by: float +) -> Tuple[float, Coordinate]: + """Which way `resource` ends up turned against `destination`, and where its corner lands on the + deck - as PyLabRobot places it there (`place`). Shared by the iSWAP and the CO-RE gripper.""" + after = resource.get_absolute_rotation().z + turned_by + if isinstance(destination, Coordinate): + return after, destination + wrt_destination = after - destination.get_absolute_rotation().z + turned = resource.rotated(z=wrt_destination - resource.rotation.z) + base = destination.get_location_wrt(deck) + dest_rotation = destination.get_absolute_rotation() + if isinstance(destination, ResourceStack): + local = destination.get_new_child_location(turned) + elif isinstance(destination, ResourceHolder): + local = destination.get_default_child_location(turned) + elif isinstance(destination, PlateAdapter) and isinstance(resource, Plate): + local = destination.compute_plate_location(cast(Plate, turned)) + elif isinstance(destination, Plate) and isinstance(resource, Lid): + local = destination.get_lid_location(cast(Lid, turned)) + else: + local = Coordinate.zero() + return wrt_destination, base + local.rotated(dest_rotation) + + +def place( + deck: Resource, resource: Resource, destination: Union[Resource, Coordinate], rotation: float +) -> None: + """Put `resource` on what it was let go over, as PyLabRobot places it there.""" + resource.unassign() + resource.rotation = Rotation(z=rotation % 360) + if isinstance(destination, Coordinate): + deck.assign_child_resource( + resource, location=destination - (deck.location or Coordinate.zero()) + ) + elif isinstance(destination, (ResourceHolder, ResourceStack)): + destination.assign_child_resource(resource) + elif isinstance(destination, PlateAdapter) and isinstance(resource, Plate): + destination.assign_child_resource( + resource, location=destination.compute_plate_location(resource) + ) + elif isinstance(destination, Plate) and isinstance(resource, Lid): + destination.assign_child_resource(resource) + elif isinstance(destination, Trash): + pass + else: + destination.assign_child_resource(resource, location=Coordinate.zero()) + + +def _rotate(point: Coordinate, degrees: float) -> Coordinate: + """The point turned `degrees` degrees counter-clockwise about the origin.""" + a = math.radians(degrees) + return Coordinate( + point.x * math.cos(a) - point.y * math.sin(a), + point.x * math.sin(a) + point.y * math.cos(a), + point.z, + ) + + +# -- the plan ------------------------------------------------------------------------------------ + + +@dataclasses.dataclass +class Rise: + """The grip centre to a height, moving nothing else. Down is the same step: a lower height.""" + + z: float + """Where the grip centre goes, in mm on the deck.""" + speed: Optional[float] = None + acceleration: Optional[float] = None + + +@dataclasses.dataclass +class Travel: + """Across, at the height the arm is at: the X-arm, the elbow along Y, and both joints. + + The X-arm is a drive of its own and travels while the iSWAP moves; the elbow's Y and the joints + share one module, so one comes after the other, in `order`. + """ + + elbow_x: float + """Where the elbow goes along X, in mm on the deck.""" + elbow_y: float + """Where the elbow goes along Y, in mm on the deck.""" + elbow_angle: float + """The elbow drive's angle, in its own degrees: 0 at its front stop.""" + wrist_angle: float + """The wrist drive's angle, in its own degrees.""" + order: Literal["translate_first", "turn_first", "turn_at"] = "translate_first" + turn_y: Optional[float] = None + """For `turn_at`: the Y the elbow turns at, on the way from where it is to `elbow_y`.""" + x_acceleration_level: int = 3 + y_speed: Optional[float] = None + elbow_speed: Optional[float] = None + wrist_speed: Optional[float] = None + elbow_acceleration: Optional[float] = None + wrist_acceleration: Optional[float] = None + + +@dataclasses.dataclass +class Open: + """The jaws to a width.""" + + width: float + speed: Optional[float] = None + acceleration: Optional[float] = None + + +@dataclasses.dataclass +class Grip: + """The jaws closed on the resource, stopping on it, and the resource hung from the gripper.""" + + resource: Resource + width: float + """How wide the resource is between the jaws, in mm.""" + strength: int = GRIP_STRENGTH + tolerance: float = WIDTH_TOLERANCE + from_top: float = PICKUP_DISTANCE_FROM_TOP + """How far below its top the jaws hold it, in mm: kept, so it is put down held the same way.""" + offset: Coordinate = dataclasses.field(default_factory=Coordinate.zero) + + +@dataclasses.dataclass +class Release: + """The jaws opened, and the resource placed on what it was put down on.""" + + resource: Resource + destination: Union[Resource, Coordinate] + rotation_wrt_destination: float + """Which way the resource ends up turned against its destination, in degrees.""" + width: float + """How wide the jaws open, in mm.""" + + +Step = Union[Rise, Travel, Open, Grip, Release] + + +@dataclasses.dataclass +class Reach: + """One way the arm can put its grip centre somewhere, facing a direction.""" + + elbow: str + elbow_angle: float + wrist_angle: float + elbow_x: float + elbow_y: float + elbow_z: float + + +@dataclasses.dataclass +class Plan: + """What a pick-up or a put-down does, step by step. Read it, change it, then `execute` it.""" + + steps: List[Step] + reach: Reach + facing: float + """Which way the gripper faces, deck degrees.""" + + def describe(self) -> str: + """The plan's steps, as the driver would say them.""" + lines = [ + f"elbow {self.reach.elbow} ({self.reach.elbow_angle:.1f} deg), wrist " + f"{self.reach.wrist_angle:.1f} deg, gripper facing {self.facing:.0f} deg" + ] + for step in self.steps: + if isinstance(step, Rise): + lines.append(f" grip centre to z {step.z:.1f}") + elif isinstance(step, Travel): + lines.append( + f" travel: elbow to x {step.elbow_x:.1f}, y {step.elbow_y:.1f}; joints to " + f"{step.elbow_angle:.1f} / {step.wrist_angle:.1f} deg ({step.order}" + + (f" at y {step.turn_y:.1f})" if step.order == "turn_at" else ")") + ) + elif isinstance(step, Open): + lines.append(f" jaws open to {step.width:.1f} mm") + elif isinstance(step, Grip): + lines.append(f" jaws close on {step.resource.name} ({step.width:.1f} mm), hang it") + elif isinstance(step, Release): + where = ( + step.destination if isinstance(step.destination, Coordinate) else step.destination.name + ) + lines.append(f" jaws open to {step.width:.1f} mm, {step.resource.name} onto {where}") + return "\n".join(lines) + + +# -- the transport -------------------------------------------------------------------------------- + + +class iSWAPCollisionError(RuntimeError): + """A plan would bring the arm, or what it holds, into something: `collisions` says what.""" + + def __init__(self, plan: "Plan", collisions: Sequence[Any]): + self.plan = plan + self.collisions = list(collisions) + lines = "\n ".join(str(c) for c in self.collisions) + super().__init__(f"the iSWAP would hit something:\n {lines}") + + +@dataclasses.dataclass +class _Held: + resource: Resource + facing: float + """Which way the gripper faced when it took hold, deck degrees.""" + offset: Coordinate + pickup_distance_from_top: float + width: float + + +class iSWAPTransport: + """Plates and lids moved by an iSWAP, from its primitive moves, with the tree kept in step. + + Args: + iswap: the STAR's iSWAP feature, set up. + traverse_height: where the grip centre travels, in mm on the deck: one fixed height, high + enough for anything, not worked out from what is on the deck. The iSWAP's own + `default_minimum_traverse_height` when None (legacy sends 280 mm). + clear_the_arm: whether every plan first clears the deck volume for the iSWAP (`make_space`: the + channels to Z safety and moved aside in Y, the heads raised), as the firmware's own plate + commands do on every move. The channels ride the same X-arm; anything that moved them since + the last plan - a CO-RE move, a head's step - leaves them in the iSWAP's way otherwise. + check_collisions: whether a plan is checked for what it would hit before it runs. + clearance: how close to anything a plan may come, in mm, when checked. 0 refuses only what + meets. + """ + + def __init__( + self, + iswap: Any, + traverse_height: Optional[float] = None, + clear_the_arm: bool = True, + check_collisions: bool = True, + clearance: float = 0.0, + ): + self.iswap = iswap + self.clear_the_arm = clear_the_arm + self.check_collisions = check_collisions + self.clearance = clearance + self.traverse_height = ( + iswap.default_minimum_traverse_height if traverse_height is None else traverse_height + ) + self._held: Optional[_Held] = None + # Told what a refused plan would hit, before the refusal is raised. Set by whoever draws it. + self.collision_reporter: Optional[Callable[[Sequence[Any]], Awaitable[None]]] = None + + # -- what is where ---------------------------------------------------------------------------- + + @property + def deck(self) -> Resource: + deck = self.iswap._driver.deck + if deck is None: + raise RuntimeError("the iSWAP's driver was given no deck, so nothing is placed") + return cast(Resource, deck) + + @property + def holding(self) -> Optional[Resource]: + return None if self._held is None else self._held.resource + + def _lengths(self) -> Tuple[float, float, float]: + c = self.iswap.configuration + if c.link_1_length is None or c.wrist_drive_predefined_increments is None: + raise RuntimeError("the iSWAP's link length and wrist stops are read at setup") + straight = c.wrist_increments_to_deg(c.wrist_drive_predefined_increments.straight) + return c.link_1_length, self.iswap.gripper.tool_center_point.x, straight + + def reaches(self, grip: Coordinate, facing: float) -> List[Reach]: + """Every way the arm can put its grip centre at `grip`, facing `facing`, that it can reach. + + For each elbow stop, the wrist sits a tool's length back from the grip centre along the way the + gripper faces, and the elbow a link's length back from the wrist along the way link 1 lies. A + way is kept when both joints are inside their travel, the elbow's X, Y and Z inside the drives' + reach - Y no further forward than the channels can be packed out of its way - and the driver's + own pose check passes. + """ + c = self.iswap.configuration + link_1, tool, straight = self._lengths() + found = [] + for name, link_angle in ELBOW_STOPS.items(): + elbow_angle = link_angle + 90.0 + wrist_angle = _norm(facing - link_angle + straight) + lo, hi = c.wrist_range_increments + if not c.wrist_increments_to_deg(lo) <= wrist_angle <= c.wrist_increments_to_deg(hi): + continue + lo, hi = c.elbow_range_increments + if ( + not c.elbow_drive_increments_to_angle(lo) + <= elbow_angle + <= c.elbow_drive_increments_to_angle(hi) + ): + continue + wrist = Coordinate( + grip.x - tool * math.cos(math.radians(facing)), + grip.y - tool * math.sin(math.radians(facing)), + grip.z, + ) + elbow = Coordinate( + wrist.x - link_1 * math.cos(math.radians(link_angle)), + wrist.y - link_1 * math.sin(math.radians(link_angle)), + grip.z + c.elbow_z_offset_above_finger, + ) + # The channels stand in front of the iSWAP on the same Y: the elbow comes no further forward + # than they can be packed out of its way (`elbow_y_min`), whatever the drive's own travel. + if c.elbow_y_min is not None and elbow.y < c.elbow_y_min: + continue + try: + for axis, value in (("x", elbow.x), ("y", elbow.y), ("z", elbow.z)): + self.iswap._check_reachable(axis, round(value, 1)) + self.iswap._check_pose_reachable(elbow_angle, wrist_angle, y=elbow.y) + except ValueError: + continue + found.append(Reach(name, elbow_angle, wrist_angle, elbow.x, elbow.y, elbow.z)) + return found + + def _ranked(self, reaches: Sequence[Reach], elbow: Optional[str]) -> List[Reach]: + """The ways to reach, the one to try first first: nearest the joints as they are, so the arm + sweeps as little as it can, and the front stop first among equals, since it keeps link 1 over + the arm's own Y rather than out over the deck. Only `elbow`'s, when it is named.""" + if not reaches: + raise ValueError("the arm cannot reach that grip facing that way from any elbow stop") + if elbow is not None: + named = [r for r in reaches if r.elbow == elbow] + if not named: + raise ValueError( + f"the arm cannot reach that grip with the elbow {elbow}; it can with " + f"{', '.join(r.elbow for r in reaches)}" + ) + return named + now_elbow = self.iswap.elbow_drive_get_angle() or 0.0 + now_wrist = self.iswap.wrist_drive_get_angle() or 0.0 + + def cost(r: Reach) -> Tuple[float, int]: + turn = max(abs(r.elbow_angle - now_elbow), abs(r.wrist_angle - now_wrist)) + return (round(turn, 1), 0 if r.elbow == "front" else 1) + + return sorted(reaches, key=cost) + + # A turn is checked at this many points along its way, not only where it ends: both joints move + # at once, in a straight line in joint space, and the arm can sweep behind the rail part way. + SWEEP_SAMPLES = 36 + + def _holds(self, elbow: float, wrist: float, y: float) -> bool: + """Whether the pose is inside what the arm can reach at all.""" + try: + self.iswap._check_pose_reachable(elbow, wrist, y=y) + return True + except ValueError: + return False + + def _sweep_clear(self, elbow: float, wrist: float, reach: Reach, y: float) -> bool: + """Whether the elbow's sweep from where it is to `reach` stays over the deck at `y`.""" + n = self.SWEEP_SAMPLES + return all( + self._holds( + elbow + (reach.elbow_angle - elbow) * k / n, wrist + (reach.wrist_angle - wrist) * k / n, y + ) + for k in range(n + 1) + ) + + def _travel(self, reach: Reach) -> Optional[Travel]: + """How to get to `reach` at the height the arm is at, or None if no way clears. + + Along Y first and turned at the target, if the joints as they are can make that trip and the + turn clears there; turned first, where the drive is, if that clears; else along to a Y where + the arm clears the rail whichever way it points - no further back than both links' length in + front of the drive's back stop - turned there, and on to the target. + """ + elbow = self.iswap.elbow_drive_get_angle() + wrist = self.iswap.wrist_drive_get_angle() + drive = self.iswap.elbow_get_reference_point_location() + travel = Travel(reach.elbow_x, reach.elbow_y, reach.elbow_angle, reach.wrist_angle) + if elbow is None or wrist is None or drive is None: + travel.order = "turn_first" + return travel + if self._holds(elbow, wrist, reach.elbow_y) and self._sweep_clear( + elbow, wrist, reach, reach.elbow_y + ): + return travel + if self._sweep_clear(elbow, wrist, reach, drive.y): + travel.order = "turn_first" + return travel + c = self.iswap.configuration + link_1, tool, _ = self._lengths() + if c.elbow_y_max is not None: + safe = max(c.elbow_y_min, min(drive.y, reach.elbow_y, c.elbow_y_max - link_1 - tool)) + if self._holds(elbow, wrist, safe) and self._sweep_clear(elbow, wrist, reach, safe): + travel.order, travel.turn_y = "turn_at", round(safe, 1) + return travel + return None + + def _reach_and_travel( + self, grip: Coordinate, facing: float, elbow: Optional[str] + ) -> Tuple[Reach, Travel]: + """The reach for `grip`, gripped across `facing`, and the travel of the elbow to it: which + elbow is asked for (`left`, `front`, `right`) ranks the reaches, the nearest one standing in.""" + reaches = self._ranked(self.reaches(grip, facing), elbow) + for reach in reaches: + travel = self._travel(reach) + if travel is not None: + return reach, travel + raise ValueError( + "every way the arm reaches that grip sweeps it behind the X-arm's rail on the way: tried " + f"the elbow {', '.join(r.elbow for r in reaches)}. Move the arm forward first" + ) + + # -- planning --------------------------------------------------------------------------------- + + def _grip_point(self, resource: Resource, offset: Coordinate, from_top: float) -> Coordinate: + """Where the grip centre sits to grip `resource`, `from_top` mm below its top, at `offset`.""" + centre = resource.center().rotated(resource.get_absolute_rotation()) + top = resource.get_location_wrt(self.deck, "l", "f", "b") + centre + offset + return Coordinate(top.x, top.y, top.z + resource.get_absolute_size_z() - from_top) + + @staticmethod + def _width_across(resource: Resource, facing: float) -> float: + """How wide the resource is between jaws that close across the way the gripper faces.""" + along_x = abs(math.cos(math.radians(facing))) > 0.5 + return resource.get_absolute_size_y() if along_x else resource.get_absolute_size_x() + + @staticmethod + def _from_top(resource: Resource, pickup_distance_from_top: Optional[float]) -> float: + """How far below its top `resource` is gripped: as asked, its preferred pickup location, or 5 mm + - and for a plate with a lid on, below the lid's skirt, where the jaws meet the plate.""" + lid = resource.lid if isinstance(resource, Liddable) else None + skirt = lid.nesting_z_height if lid is not None else None + if pickup_distance_from_top is not None: + if lid is not None and skirt is not None and pickup_distance_from_top < skirt: + raise ValueError( + f"{resource.name} has {lid.name} on it, which comes {skirt} mm down its sides: jaws " + f"{pickup_distance_from_top} mm below its top close on the lid. Grip it more than " + f"{skirt} mm down, or take the lid off first" + ) + return pickup_distance_from_top + if resource.preferred_pickup_location is not None: + from_top = resource.get_size_z() - resource.preferred_pickup_location.z + else: + from_top = PICKUP_DISTANCE_FROM_TOP + if skirt is not None: + from_top = max(from_top, skirt + BELOW_LID) + return from_top + + def plan_pick_up( + self, + resource: Resource, + direction: Union[GripDirection, float] = "front", + pickup_distance_from_top: Optional[float] = None, + offset: Coordinate = Coordinate.zero(), + elbow: Optional[str] = None, + traverse_height: Optional[float] = None, + end_height: Optional[float] = None, + width: Optional[float] = None, + open_margin: float = OPEN_MARGIN, + grip_strength: int = GRIP_STRENGTH, + width_tolerance: float = WIDTH_TOLERANCE, + ) -> Plan: + """Plan picking `resource` up, as `C0 PP` would: open, rise, travel, descend, grip, rise. + + Args: + resource: what to pick up. Turned only about Z, by a quarter turn or several. + direction: the side it is gripped from - where the wrist is - or deck degrees the gripper + faces. + pickup_distance_from_top: how far below its top the jaws hold it, in mm. Its + `preferred_pickup_location` when None, else 5 mm. + offset: added to where the jaws hold it, in mm. + elbow: which elbow stop to reach from, `left`, `front` or `right`. The nearest the joints + are now when None. + traverse_height: where the grip centre travels, in mm. The transport's when None. + end_height: where the grip centre ends, in mm. The traverse height when None. + width: how wide it is between the jaws, in mm. Its size across the grip when None. + open_margin: how much wider than that the jaws open to take it, in mm. + grip_strength: 0 to 9. + width_tolerance: how far off `width` it may turn out to be, in mm. + """ + if self._held is not None: + raise RuntimeError(f"already holding {self._held.resource.name}") + rotation = resource.get_absolute_rotation() + if rotation.x or rotation.y or rotation.z % 90: + raise ValueError( + f"{resource.name} is turned {rotation}; only quarter turns about Z are gripped" + ) + facing = GRIPPER_FACING[direction] if isinstance(direction, str) else float(direction) + from_top = self._from_top(resource, pickup_distance_from_top) + grip = self._grip_point(resource, offset, from_top) + reach, travel = self._reach_and_travel(grip, facing, elbow) + across = self._width_across(resource, facing) if width is None else width + traverse = self.traverse_height if traverse_height is None else traverse_height + end = traverse if end_height is None else end_height + steps: List[Step] = [ + Open(across + open_margin), + Rise(max(traverse, self._grip_z_now())), + travel, + Rise(grip.z), + Grip(resource, across, grip_strength, width_tolerance, from_top, offset), + Rise(end), + ] + return Plan(steps, reach, facing) + + def plan_drop( + self, + destination: Union[Resource, Coordinate], + direction: Optional[Union[GripDirection, float]] = None, + offset: Optional[Coordinate] = None, + elbow: Optional[str] = None, + traverse_height: Optional[float] = None, + end_height: Optional[float] = None, + open_margin: float = OPEN_MARGIN, + ) -> Plan: + """Plan putting down what is held, as `C0 PR` would: rise, travel, descend, open, rise. + + Args: + destination: what to put it on - a site, a plate adapter, a stack, a plate for a lid, the + trash - or a place on the deck, its left front bottom corner. + direction: the side it is let go of from. The side it was gripped from when None: a different + side turns it by the difference. + offset: added to where the jaws let go of it. The pick-up's when None. + elbow, traverse_height, end_height, open_margin: as `plan_pick_up`. + """ + held = self._held + if held is None: + raise RuntimeError("nothing is held") + resource = held.resource + facing = ( + held.facing + if direction is None + else (GRIPPER_FACING[direction] if isinstance(direction, str) else float(direction)) + ) + turned_by = _norm(facing - held.facing) + if isinstance(destination, Resource): + destination.check_can_drop_resource_here(resource) + rotation_wrt_destination, corner = self._placement(resource, destination, turned_by) + centre = resource.center().rotated(Rotation(z=resource.get_absolute_rotation().z + turned_by)) + at = corner + centre + (held.offset if offset is None else offset) + grip = Coordinate( + at.x, at.y, at.z + resource.get_absolute_size_z() - held.pickup_distance_from_top + ) + reach, travel = self._reach_and_travel(grip, facing, elbow) + traverse = self.traverse_height if traverse_height is None else traverse_height + end = traverse if end_height is None else end_height + steps: List[Step] = [ + Rise(max(traverse, self._grip_z_now())), + travel, + Rise(grip.z), + Release(resource, destination, rotation_wrt_destination, held.width + open_margin), + Rise(end), + ] + return Plan(steps, reach, facing) + + def _grip_z_now(self) -> float: + """Where the grip centre stands now, in mm on the deck.""" + drive = self.iswap.elbow_get_reference_point_location() + if drive is None: + raise RuntimeError("the iSWAP is not modelled") + return float(drive.z - self.iswap.configuration.elbow_z_offset_above_finger) + + def _placement( + self, resource: Resource, destination: Union[Resource, Coordinate], turned_by: float + ) -> Tuple[float, Coordinate]: + """As `placement`, on this transport's deck.""" + return placement(self.deck, resource, destination, turned_by) + + # -- doing it --------------------------------------------------------------------------------- + + def collisions(self, plan: Plan) -> List[Any]: + """What `plan` would hit, from where the arm is now (`iswap_collisions.check_plan`).""" + from pylabrobot.hamilton.star.driver.features.iswap_collisions import check_plan + + return check_plan(self, plan, self.clearance) + + async def execute(self, plan: Plan) -> None: + """Carry a plan out, step by step: the arm cleared first, then refused, before the iSWAP moves, + if it would hit anything and collisions are checked - against the channels where clearing left + them.""" + if self.clear_the_arm: + await self.iswap.make_space() + if self.check_collisions: + found = self.collisions(plan) + if found: + if self.collision_reporter is not None: + await self.collision_reporter(found) + raise iSWAPCollisionError(plan, found) + for step in plan.steps: + await self._do(step, plan) + + async def _do(self, step: Step, plan: Plan) -> None: + """Drive one step of `plan` on the device, leaving the tree where the step leaves it.""" + iswap = self.iswap + c = iswap.configuration + if isinstance(step, Rise): + await iswap.elbow_move_to_z_position( + round(step.z + c.elbow_z_offset_above_finger, 1), + speed=step.speed, + acceleration=step.acceleration, + ) + elif isinstance(step, Travel): + + async def along_and_turn() -> None: + async def along() -> None: + await iswap.elbow_move_to_y_position(round(step.elbow_y, 1), speed=step.y_speed) + + async def turn() -> None: + await iswap.rotate_to_angles( + elbow_relative_angle=step.elbow_angle, + gripper_relative_angle=step.wrist_angle, + elbow_speed=step.elbow_speed, + wrist_speed=step.wrist_speed, + elbow_acceleration=step.elbow_acceleration, + wrist_acceleration=step.wrist_acceleration, + ) + + if step.order == "turn_at" and step.turn_y is not None: + await iswap.elbow_move_to_y_position(round(step.turn_y, 1), speed=step.y_speed) + await turn() + await along() + return + first, then = (along, turn) if step.order == "translate_first" else (turn, along) + await first() + await then() + + # The X-arm is a drive of its own: it travels while the iSWAP moves along and turns. + await asyncio.gather( + iswap.elbow_move_to_x_position( + round(step.elbow_x, 1), acceleration_level=step.x_acceleration_level + ), + along_and_turn(), + ) + elif isinstance(step, Open): + await iswap.gripper_move_to_jaw_position( + step.width, speed=step.speed, acceleration=step.acceleration + ) + elif isinstance(step, Grip): + await iswap.gripper_close_with_force_sensed_width_window( + step.width, grip_strength=step.strength, width_tolerance=step.tolerance + ) + self._hang(step.resource) + self._held = _Held(step.resource, plan.facing, step.offset, step.from_top, step.width) + elif isinstance(step, Release): + await iswap.gripper_move_to_jaw_position(step.width) + self._place(step.resource, step.destination, step.rotation_wrt_destination) + self._held = None + + def _hang(self, resource: Resource) -> None: + """Hang `resource` from the gripper where it is, so it rides with the arm.""" + gripper = self.iswap.gripper + deck = self.deck + turned = gripper.get_absolute_rotation().z + local = _rotate(resource.get_location_wrt(deck) - gripper.get_location_wrt(deck), -turned) + rotation = resource.get_absolute_rotation().z - turned + resource.unassign() + resource.rotation = Rotation(z=rotation % 360) + gripper.assign_child_resource(resource, location=local) + + def _place( + self, resource: Resource, destination: Union[Resource, Coordinate], rotation: float + ) -> None: + """As `place`, on this transport's deck.""" + place(self.deck, resource, destination, rotation) + + # -- the whole move --------------------------------------------------------------------------- + + async def pick_up_resource(self, resource: Resource, **kwargs: Any) -> Plan: + """Plan a pick-up with `plan_pick_up`'s arguments, and carry it out.""" + plan = self.plan_pick_up(resource, **kwargs) + await self.execute(plan) + return plan + + async def drop_resource(self, destination: Union[Resource, Coordinate], **kwargs: Any) -> Plan: + """Plan a put-down with `plan_drop`'s arguments, and carry it out.""" + plan = self.plan_drop(destination, **kwargs) + await self.execute(plan) + return plan + + async def move_resource( + self, + resource: Resource, + to: Union[Resource, Coordinate], + pickup_direction: Union[GripDirection, float] = "front", + drop_direction: Optional[Union[GripDirection, float]] = None, + **kwargs: Any, + ) -> None: + """Pick `resource` up and put it down on `to`, as `C0 PP` then `C0 PR`.""" + await self.pick_up_resource(resource, direction=pickup_direction, **kwargs) + await self.drop_resource(to, direction=drop_direction) diff --git a/pylabrobot/hamilton/star/driver/features/iswap_transport_tests.py b/pylabrobot/hamilton/star/driver/features/iswap_transport_tests.py new file mode 100644 index 00000000000..69c53a3f398 --- /dev/null +++ b/pylabrobot/hamilton/star/driver/features/iswap_transport_tests.py @@ -0,0 +1,164 @@ +"""Plates and lids moved by the iSWAP from its primitive moves, on a simulated STARlet.""" + +import unittest + +from pylabrobot.hamilton.star.driver.features.iswap_transport import ( + GRIPPER_FACING, + Grip, + Open, + Release, + Rise, + Travel, + iSWAPTransport, +) +from pylabrobot.resources.corning.plates import cor_96_wellplate_360uL_Fb_lid +from pylabrobot.resources.plate import Plate +from pylabrobot.visualizer3D.demo import build_facility, star_of + + +class iSWAPTransportTests(unittest.IsolatedAsyncioTestCase): + async def asyncSetUp(self) -> None: + facility = build_facility() + self.star = star_of(facility) + self.deck = self.star.deck + self.deck.get_resource("destination_1").unassign() + self.plate = self.deck.get_resource("source_1") + assert isinstance(self.plate, Plate) + assert self.plate.parent is not None + self.start = self.plate.parent + self.site = self.deck.get_resource("destination_carrier").children[1] + await self.star.setup() + iswap = self.star.iswap + assert iswap is not None + self.iswap = iswap + await iswap.make_space() + # Where things land, not what the arm passes on the way: that is `iswap_collisions_tests`. + self.transport = iSWAPTransport(iswap, check_collisions=False) + + def where_the_site_puts_it(self, site): + return site.get_absolute_location() + site.get_default_child_location(self.plate) + + async def test_a_plate_lands_where_its_site_places_it_whichever_side_it_is_gripped_from(self): + for direction in ("front", "back", "left", "right"): + with self.subTest(direction=direction): + before = self.plate.get_absolute_location() + await self.transport.move_resource(self.plate, self.site, pickup_direction=direction) + self.assertIs(self.plate.parent, self.site) + at, placed = self.plate.get_absolute_location(), self.where_the_site_puts_it(self.site) + self.assertLess(max(abs(at.x - placed.x), abs(at.y - placed.y), abs(at.z - placed.z)), 1e-6) + await self.transport.move_resource(self.plate, self.start, pickup_direction=direction) + back = self.plate.get_absolute_location() + self.assertLess(max(abs(back.x - before.x), abs(back.y - before.y)), 1e-6) + + async def test_a_held_plate_hangs_from_the_gripper_where_it_was_gripped(self): + before = self.plate.get_absolute_location() + await self.transport.pick_up_resource(self.plate, direction="front", end_height=None) + self.assertIs(self.plate.parent, self.iswap.gripper) + self.assertIs(self.transport.holding, self.plate) + # Lifted straight up: the same X and Y, higher. + now = self.plate.get_absolute_location() + self.assertAlmostEqual(now.x, before.x, places=6) + self.assertAlmostEqual(now.y, before.y, places=6) + self.assertGreater(now.z, before.z) + + async def test_the_plan_is_the_firmware_s_phases(self): + plan = self.transport.plan_pick_up(self.plate, direction="front") + kinds = [type(step) for step in plan.steps] + self.assertEqual(kinds[1:], [Rise, Travel, Rise, Grip, Rise]) + grip = plan.steps[4] + assert isinstance(grip, Grip) + # Legacy's widths: a front grip closes across the plate's X; the jaws open 3 mm wider. + self.assertAlmostEqual(grip.width, self.plate.get_absolute_size_x()) + opening = plan.steps[0] + assert isinstance(opening, Open) + self.assertAlmostEqual(opening.width, grip.width + 3.0) + self.assertEqual(plan.facing, GRIPPER_FACING["front"]) + self.assertIn("jaws close on source_1", plan.describe()) + + async def test_turning_the_grip_turns_the_plate(self): + await self.transport.pick_up_resource(self.plate, direction="back") + plan = self.transport.plan_drop(self.site, direction="front") + release = plan.steps[3] + assert isinstance(release, Release) + self.assertAlmostEqual(release.rotation_wrt_destination % 360, 180.0) + await self.transport.execute(plan) + self.assertAlmostEqual(self.plate.get_absolute_rotation().z % 360, 180.0) + + async def test_a_lidded_plate_is_gripped_below_its_lid(self): + lid = cor_96_wellplate_360uL_Fb_lid(name="source_1_lid") + self.plate.assign_child_resource(lid, location=None) + plan = self.transport.plan_pick_up(self.plate, direction="front") + rise = plan.steps[3] + assert isinstance(rise, Rise) + # The jaws close below where the lid's skirt comes down to. + self.assertLess(rise.z, lid.get_location_wrt(self.deck).z) + with self.assertRaises(ValueError): + self.transport.plan_pick_up(self.plate, direction="front", pickup_distance_from_top=5.0) + + async def test_a_lid_goes_on_a_plate_as_its_lid(self): + lid = cor_96_wellplate_360uL_Fb_lid(name="source_1_lid") + self.plate.assign_child_resource(lid, location=None) + await self.transport.pick_up_resource(lid, direction="front") + await self.transport.drop_resource(self.site) + self.assertIs(lid.parent, self.site) + await self.transport.pick_up_resource(lid, direction="front") + await self.transport.drop_resource(self.plate) + assert isinstance(self.plate, Plate) + self.assertIs(self.plate.lid, lid) + + async def test_a_grip_out_of_reach_is_refused_before_anything_moves(self): + front_most = self.deck.get_resource("source_0") + joints = (self.iswap.elbow_drive_get_angle(), self.iswap.wrist_drive_get_angle()) + with self.assertRaises(ValueError): + await self.transport.pick_up_resource(front_most, direction="front") + self.assertEqual( + (self.iswap.elbow_drive_get_angle(), self.iswap.wrist_drive_get_angle()), joints + ) + + async def test_every_plan_clears_the_channels_out_of_the_iswap_s_way_first(self): + pipettes = self.star.pipettes + assert pipettes is not None + cleared = await pipettes.request_y_positions() # where make_space left them, in setUp + # Something moves the channels back over the deck between two iSWAP moves. + await pipettes.move_to_y_positions({0: cleared[0] + 150.0}, make_space=True) + moved = await pipettes.request_y_positions() + self.assertGreater(moved[0], cleared[0] + 100.0) + commands = [] + listening = self.star.driver.motion_listener # type: ignore[attr-defined] + + async def listen(module, command, params): + commands.append(module + command) + if listening is not None: + await listening(module, command, params) + + self.star.driver.motion_listener = listen # type: ignore[attr-defined] + await self.transport.pick_up_resource(self.plate, direction="front") + # The Y range is freed before the iSWAP's first motion, and the channels stand aside again. + self.assertIn("C0FY", commands) + first_iswap = next(i for i, c in enumerate(commands) if c[:2] == "R0" and c[2] != "R") + self.assertLess(commands.index("C0FY"), first_iswap) + for now, before in zip(await pipettes.request_y_positions(), cleared): + self.assertAlmostEqual(now, before, delta=1.0) + + async def test_no_turn_sweeps_the_arm_behind_the_rail(self): + elbow, wrist = self.iswap.elbow_drive_get_angle(), self.iswap.wrist_drive_get_angle() + drive = self.iswap.elbow_get_reference_point_location() + assert elbow is not None and wrist is not None and drive is not None + for name in ("source_2", "source_4", "destination_3"): + for direction in ("front", "back", "left", "right"): + try: + plan = self.transport.plan_pick_up(self.deck.get_resource(name), direction=direction) + except ValueError: + continue + travel = plan.steps[2] + assert isinstance(travel, Travel) + # Where the turn is made: at the target, where the drive is, or part way. + y = {"translate_first": travel.elbow_y, "turn_at": travel.turn_y}.get(travel.order) + y = drive.y if y is None else y + self.assertTrue( + self.transport._sweep_clear(elbow, wrist, plan.reach, y), f"{name} {direction}" + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/hamilton/star/driver/features/pipettes.py b/pylabrobot/hamilton/star/driver/features/pipettes.py index 931221ef832..c06c81667d7 100644 --- a/pylabrobot/hamilton/star/driver/features/pipettes.py +++ b/pylabrobot/hamilton/star/driver/features/pipettes.py @@ -66,6 +66,7 @@ from pylabrobot.resources.container import Container from pylabrobot.resources.coordinate import Coordinate from pylabrobot.resources.errors import HasTipError, NoTipError +from pylabrobot.resources.hamilton.core_gripper_tools import HamiltonCoreGripperTool from pylabrobot.resources.hamilton.tip_creators import HamiltonTip, TipDropMethod, TipPickupMethod from pylabrobot.resources.n_channel_pipettes import NChannelPipette, TipMountingShaft from pylabrobot.resources.resource import Resource @@ -84,6 +85,20 @@ """An X tolerance wider than any deck: X alone never splits a tip command into batches, so spots spread across columns go out in one, as legacy sends them.""" + +def core_tool_face_distance(tool: HamiltonCoreGripperTool) -> float: + """How far a CO-RE grip tool's face stands from its channel's axis, in Y.""" + pick_up = tool.pick_up_location or tool.get_anchor("c", "c", "t") + return float(tool.get_size_y() - pick_up.y) + + +def core_tool_grip_line_overhang(tool: HamiltonCoreGripperTool) -> float: + """How far a mounted CO-RE grip tool's grip line hangs below its channel's stop disc, in mm: the + height the CO-RE plate commands are given in.""" + pick_up = tool.pick_up_location or tool.get_anchor("c", "c", "t") + return float(pick_up.z - tool.fitting_depth - tool.grip_line_height) + + T = TypeVar("T") ChannelType = Literal["ML_STAR", "ML_STAR_RPC"] diff --git a/pylabrobot/hamilton/star/driver/features/star_collisions.py b/pylabrobot/hamilton/star/driver/features/star_collisions.py new file mode 100644 index 00000000000..aa49194a198 --- /dev/null +++ b/pylabrobot/hamilton/star/driver/features/star_collisions.py @@ -0,0 +1,348 @@ +"""What the STAR arm's movers sweep through, and what a command would hit. + +The arm's checks share one scene of what stands still (`scene`), one shape for what is let off +what (`Exemptions`), one way to speak of what a command means to touch (`_allowed`, +`held_allowed`), and one account of what is mounted on the X-arm (`mounted_groups`). The iSWAP's +plan checks (`iswap_collisions`) and the pipette and head checks here are built on them. + +`check_pipette_move` and `check_head_move` sweep what a pipette or head command moves - the +channels, each on its own way in Y and Z, the head as one rigid body - against what stands around +them and against each other, so that anything in the way of a command is found as geometry rather +than assumed away. +""" + +from __future__ import annotations + +import dataclasses +from typing import Dict, List, Mapping, Optional, Sequence, Set, Tuple + +from pylabrobot.hamilton.star.driver.features.head import Head +from pylabrobot.hamilton.star.driver.features.pipettes import Pipettes +from pylabrobot.hamilton.star.resource_model import iSWAPHead +from pylabrobot.resources.collision import ( + Collision, + Group, + Piece, + Pose, + Segment, + StaticScene, + check, + declared_hulls, + on_axes, + solid_pieces, +) +from pylabrobot.resources.end_effector import MechanicalGripper +from pylabrobot.resources.manipulator import LinkBody +from pylabrobot.resources.resource import Resource + +# Resources that are enclosures, not solids: the X-arm travels into the left extension housing, and +# the model has the 96-head inside it whenever the arm is far enough left. +ENCLOSURES = ("left_extension_housing",) +# Two channels' moves within this much of each other, in mm, are one move: the row shifting together. +DELTA_TOLERANCE = 1e-6 + + +class CollisionError(RuntimeError): + """A command would bring what it moves into what stands in its way: `origin` says which command + it was, `collisions` what it would hit.""" + + def __init__(self, origin: str, collisions: Sequence[Collision]): + self.origin = origin + self.collisions = list(collisions) + lines = "\n ".join(str(c) for c in self.collisions) + super().__init__(f"{origin} would hit something:\n {lines}") + + +@dataclasses.dataclass +class Exemptions: + """Which pairs of a check's groups are let off each other. + + Nothing in a command moves the parts of one mechanism relative to each other, nor things mounted + together that the command does not drive apart; they would be reported against each other + wherever they stand close, which says nothing. + """ + + machine: Set[str] = dataclasses.field(default_factory=set) + """Groups that are one mechanism: allowed to touch each other.""" + carriage: Set[str] = dataclasses.field(default_factory=set) + """Groups mounted together on the X-arm that the command does not move relative to each other.""" + body: Optional[str] = None + """The X-arm's own body, if it has a group: too coarsely boxed to judge the parts against, it is + checked only against what stands still.""" + held: Optional[str] = None + """The name of what the gripper holds, which rides the gripper and so is let off the machine.""" + + def between(self, a: str, b: str) -> bool: + """Whether two of the groups are checked against each other.""" + if self.body is not None and self.body in (a, b): + return False + if a in self.machine and b in self.machine: + return False + if a in self.carriage and b in self.carriage: + return False + if self.held is not None and {a, b} & self.machine and self.held in (a, b): + return False + return True + + +def _allowed(touches: Sequence[Resource]) -> List[Resource]: + """Of what a command means to touch, what the moving parts may meet: all but what has a shape of + its own (declared hulls). A box stands for something only roughly - a plate holder the fingers + reach into - so meeting it means nothing; a shape is what is there, and the mover must keep clear + of it even on its way to touch what it is meant to.""" + return [r for r in touches if declared_hulls(r) is None] + + +def held_allowed( + held: Optional[Resource], touches: Sequence[Resource] +) -> Dict[str, List[Resource]]: + """What the gripper holds may meet all the command means to touch, shapes too: it is a box, so + its meeting the nest it goes into - a plate's wells in a thermocycler's block - means nothing.""" + if held is None or declared_hulls(held) is not None: + return {} + return {held.name: list(touches)} + + +_SCENES: Dict[int, StaticScene] = {} + + +def scene(root: Resource, hollow: Sequence[str] = ()) -> StaticScene: + """The kept solid pieces of `root`, so that a check works out only what changed since the last.""" + key = id(root) + kept = _SCENES.get(key) + if kept is None or kept.root is not root or tuple(kept.hollow) != tuple(hollow): + kept = _SCENES[key] = StaticScene(root, hollow) + return kept + + +def root_of(resource: Resource) -> Resource: + """The top of the tree `resource` stands in: what everything around it is under.""" + root = resource + while root.parent is not None: + root = root.parent + return root + + +def still(end: float) -> List[Segment]: + """Standing still for `end` units of time: one command, or a plan's steps.""" + return [Segment([Pose()], 0.0, 0.0, end)] + + +def mounted_groups( + arm: Resource, + end: float, + segments: Optional[Mapping[str, List[Segment]]] = None, + carrying: Optional[Resource] = None, + held: Optional[Resource] = None, + frame: Optional[List[Segment]] = None, +) -> List[Group]: + """Every group on the X-arm: the iSWAP's parts, what it holds, each mounted rider, and the arm's + own body. A group is given the segments named for it - a rider by its resource's name, a part of + the iSWAP by `"column"`, `"link"`, `"gripper"`, `"finger 0"`, `"finger 1"`, what is held by + `"held"` - and stands still where none are. + + Args: + arm: the X-arm's resource, which everything mounted hangs from. + end: how long the check's time runs, in units of one command or plan step. + segments: how each group moves, as poses relative to where it stands now. + carrying: what the gripper holds, left out of its pieces so they do not change shape with it. + held: what the gripper holds or is picking up, a group of its own. + frame: the carriage's own segments, which every group rides. + """ + moving = segments or {} + standing = still(end) + + def pieces(root: Resource, *leave_out: Resource) -> List[Piece]: + return solid_pieces(root, {id(r) for r in leave_out}) + + column = next((c for c in arm.children if isinstance(c, iSWAPHead)), None) + link = next((c for c in column.children if isinstance(c, LinkBody)), None) if column else None + gripper = ( + next((c for c in link.children if isinstance(c, MechanicalGripper)), None) if link else None + ) + fingers = tuple(gripper.fingers) if gripper is not None else () + + groups: List[Group] = [] + if column is not None: + groups.append( + Group( + "iSWAP column", + pieces(column, *([link] if link is not None else [])), + moving.get("column", standing), + frame, + ) + ) + if link is not None: + groups.append( + Group( + "iSWAP link 1", + pieces(link, *([gripper] if gripper is not None else [])), + moving.get("link", standing), + frame, + ) + ) + if gripper is not None: + groups.append( + Group( + "iSWAP gripper", + pieces(gripper, *fingers, *([carrying] if carrying is not None else [])), + moving.get("gripper", standing), + frame, + ) + ) + if held is not None: + groups.append(Group(held.name, pieces(held), moving.get("held", standing), frame)) + for k, finger in enumerate(fingers): + groups.append( + Group(f"iSWAP {finger.name}", pieces(finger), moving.get(f"finger {k}", standing), frame) + ) + groups.append(Group("X-arm", pieces(arm, *arm.children), moving.get("X-arm", standing), frame)) + for rider in arm.children: + if rider is column: + continue + groups.append(Group(rider.name, pieces(rider), moving.get(rider.name, standing), frame)) + return groups + + +def iswap_names(groups: Sequence[Group]) -> Set[str]: + """The groups that are the iSWAP's own parts: one mechanism, allowed to touch itself.""" + return {g.name for g in groups if g.name.startswith("iSWAP ")} + + +def _check_ride( + arm: Resource, + segments: Mapping[str, List[Segment]], + touch: Sequence[Resource], + clearance: float, + root: Resource, + carriage: Sequence[str] = (), +) -> List[Collision]: + """A pipette or head command's check: its movers swept against what stands around the arm and + against each other. + + The iSWAP's parts are let off each other - nothing in the command moves them relative to each + other - and the arm's own body is judged against nothing that rides it. The riders are checked + against each other, less those the command carries along together (`carriage`). + """ + groups = mounted_groups(arm, 1.0, segments) + exemptions = Exemptions(machine=iswap_names(groups), carriage=set(carriage), body="X-arm") + standing = scene(root, ENCLOSURES).obstacles([arm]) + return check( + root, groups, clearance, _allowed(touch), standing, between_groups=exemptions.between + ) + + +def check_pipette_move( + pipettes: Pipettes, + y: Optional[Mapping[int, float]] = None, + z: Optional[Mapping[int, float]] = None, + touch: Sequence[Resource] = (), + clearance: float = 0.0, + root: Optional[Resource] = None, +) -> List[Collision]: + """Where a pipette command's channels would come within `clearance` mm of what stands around + them, or of each other. + + The command is one unit of time, and every channel it moves sweeps its whole way in it: the check + is conservative about the order the drives settle in. Y and Z are where the drives report each + channel's stop disc, in mm on the deck, as `move_to_y_positions` and + `move_stop_disc_to_z_positions` take them. A tip mounted on a channel sweeps with it. Channels + commanded by the same amount move as one rigid body and are let off each other. + + Args: + pipettes: the channels' feature, with the arm where the command starts. + y: where each channel's reference point is going, by channel. + z: where each channel's stop disc is going, by channel. + touch: what the command means to touch - a rack's tip spots, a labware's wells: never reported, + unless it has a shape of its own. + clearance: how close is too close, in mm. 0 reports only what meets. + root: what stands around the arm; the whole tree the arm stands in when None. + + Raises: + RuntimeError: If no channels are modelled, or a channel to be moved is not, so its way is not + known. + """ + if not pipettes.resources: + raise RuntimeError("no channels are modelled, so what they sweep is not known") + targets: Dict[int, List[Optional[float]]] = {} + for channel, target in (y or {}).items(): + targets.setdefault(channel, [None, None])[0] = target + for channel, target in (z or {}).items(): + targets.setdefault(channel, [None, None])[1] = target + arm = pipettes.resources[0].parent + if arm is None: + raise RuntimeError("the channels are not on an arm, so what they sweep is not known") + deltas: Dict[str, Tuple[float, float]] = {} + for channel, (to_y, to_z) in targets.items(): + here = pipettes.get_reference_point_location(channel) + if here is None: + raise RuntimeError(f"channel {channel} is not modelled, so what it sweeps is not known") + dy = 0.0 if to_y is None else to_y - here.y + dz = 0.0 if to_z is None else to_z - here.z + deltas[pipettes.resources[channel].name] = (dy, dz) + # Channels commanded by the same amount move as one rigid body - the row shifting together - so + # nothing in the command moves them relative to each other, and they are let off each other. + shared: List[Tuple[Tuple[float, float], List[str]]] = [] + for name, delta in deltas.items(): + for common, names in shared: + if all(abs(delta[k] - common[k]) <= DELTA_TOLERANCE for k in (0, 1)): + names.append(name) + break + else: + shared.append((delta, [name])) + segments: Dict[str, List[Segment]] = {} + carriage: Set[str] = set() + for delta, names in shared: + segment = on_axes((0.0, delta[0], delta[1]), 0.0, 1.0) + for name in names: + segments[name] = segment + if len(names) > 1: + carriage.update(names) + return _check_ride( + arm, segments, touch, clearance, root if root is not None else root_of(arm), sorted(carriage) + ) + + +def check_head_move( + head: Head, + y: Optional[float] = None, + z: Optional[float] = None, + touch: Sequence[Resource] = (), + clearance: float = 0.0, + root: Optional[Resource] = None, +) -> List[Collision]: + """Where a head command would come within `clearance` mm of what stands around it, or of what + rides the arm with it. + + The command is one unit of time, and the head - one rigid body, its tips with it - sweeps its + whole way in it. Y is where channel A1 is going and Z where the head's lowest fixed feature is + going, in mm on the deck, as the head's drives report them. + + Args: + head: the head's feature, with the arm where the command starts. + y: where channel A1 is going, in mm on the deck. + z: where the head's lowest fixed feature is going, in mm on the deck. + touch: what the command means to touch - a rack's tip spots, a labware's wells: never + reported, unless it has a shape of its own. + clearance: how close is too close, in mm. 0 reports only what meets. + root: what stands around the arm; the whole tree the arm stands in when None. + + Raises: + RuntimeError: If the head is not modelled, so its way is not known. + """ + resource = head.resource + if resource is None or resource.location is None or resource.parent is None: + raise RuntimeError("the head is not modelled, so what it sweeps is not known") + here = head.get_reference_point_location() + if here is None: + raise RuntimeError("the head is not modelled, so what it sweeps is not known") + dy = 0.0 if y is None else y - here.y + dz = 0.0 if z is None else z - here.z + arm = resource.parent + return _check_ride( + arm, + {resource.name: on_axes((0.0, dy, dz), 0.0, 1.0)}, + touch, + clearance, + root if root is not None else root_of(arm), + ) diff --git a/pylabrobot/hamilton/star/driver/features/star_collisions_tests.py b/pylabrobot/hamilton/star/driver/features/star_collisions_tests.py new file mode 100644 index 00000000000..1b1ec50d6f3 --- /dev/null +++ b/pylabrobot/hamilton/star/driver/features/star_collisions_tests.py @@ -0,0 +1,265 @@ +"""What pipette and head commands sweep through, on a simulated STARlet's demo deck.""" + +import unittest +from typing import List + +from pylabrobot.hamilton.star.driver.features.iswap_transport import ( + iSWAPCollisionError, + iSWAPTransport, +) +from pylabrobot.hamilton.star.driver.features.star_collisions import ( + CollisionError, + check_head_move, + check_pipette_move, +) +from pylabrobot.hamilton.star.motion import attach_viewer_collisions +from pylabrobot.resources.lid import Lid +from pylabrobot.resources.n_channel_pipettes import TipMountingShaft +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.tip_rack import TipRack +from pylabrobot.resources.tip_tracking import does_tip_tracking, set_tip_tracking +from pylabrobot.visualizer3D.demo import build_facility, star_of + + +class _RecordingViewer: + """What the glue needs of a viewer, and a list of everything it was told to draw.""" + + def __init__(self, raise_on_collision: bool): + self.raise_on_collision = raise_on_collision + self.shown: list = [] + + async def show_collisions(self, collisions) -> None: + self.shown.append(collisions) + + +class StarCollisionTests(unittest.IsolatedAsyncioTestCase): + async def asyncSetUp(self) -> None: + was = does_tip_tracking() + set_tip_tracking(True) + self.addCleanup(set_tip_tracking, was) + self.facility = build_facility() + self.star = star_of(self.facility) + self.deck = self.star.deck + self.deck.get_resource("destination_1").unassign() + await self.star.setup() + assert self.star.pipettes is not None and self.star.head96 is not None + self.pipettes = self.star.pipettes + self.head = self.star.head96 + + async def test_a_row_of_channels_moved_across_the_deck_is_caught_when_lowered(self): + # The channels ride as one row when they are commanded the same way: let off each other, judged + # against the world. At Z safety the row moved towards the front is clear; lowered over the + # deck, the same move sweeps through the plate carrier and the racks; raised again, it is clear. + refs = [] + for channel in range(8): + here = self.pipettes.get_reference_point_location(channel) + assert here is not None + refs.append(here) + targets = {i: here.y - 150.0 for i, here in enumerate(refs)} + self.assertEqual(check_pipette_move(self.pipettes, y=targets), []) + await self.pipettes.move_stop_disc_to_z_positions({i: 150.0 for i in range(8)}) + hits = check_pipette_move(self.pipettes, y=targets) + self.assertTrue(hits) + self.assertIn("source_carrier", {c.obstacle.name for c in hits}) + await self.pipettes.move_stop_disc_to_z_positions({i: 334.7 for i in range(8)}) + self.assertEqual(check_pipette_move(self.pipettes, y=targets), []) + + async def test_a_channel_moved_across_its_neighbours_meets_them(self): + # The channels stand in a row along Y, each on its own drive: no channel may cross where another + # stands, whatever the height. Channel 0, moved towards the front, crosses two of its neighbours + # on the way, and nothing else stands in its way there. + hits = check_pipette_move(self.pipettes, y={0: 340.0}) + self.assertTrue(hits) + self.assertTrue( + all("pipette_channel" in (c.other_group or "") for c in hits), + {(c.obstacle.name, c.other_group) for c in hits}, + ) + + async def test_a_head_move_across_the_deck_at_working_height_is_caught_at_the_tip_racks(self): + # The head swept across the deck at a working height runs into the tip racks and the tips + # standing in them; at the height it rides above them, the same move is clear. + assert self.star.iswap is not None + await self.star.iswap.make_space() + hits = check_head_move(self.head, y=100.0, z=200.0) + self.assertTrue(hits) + self.assertIn("tips_2", {c.obstacle.name for c in hits}) + self.assertEqual(check_head_move(self.head, y=100.0, z=260.0), []) + + async def test_tips_lowered_onto_a_lidded_plate_meet_the_lid_and_not_the_plate(self): + # The command means to touch the plate, which is let off as the box it is. The lid seated on it + # is its own solid: tips carried on the channel meet it where they go into it, and it is + # reported. + source_0 = self.deck.get_resource("source_0") + assert isinstance(source_0, Plate) + lid = Lid( + name="source_0_lid", + size_x=source_0.get_size_x(), + size_y=source_0.get_size_y(), + size_z=10.0, + nesting_z_height=2.0, + ) + source_0.assign_child_resource(lid) + rack = self.deck.get_resource("tips_0") + assert isinstance(rack, TipRack) + await self.pipettes.pick_up_tips([rack.get_item(f"{row}1") for row in "ABCDEFGH"]) + # Over the plate: the arm carries the channels across in X, the channels spread along it in Y. + arm = self.star.driver.arms[0] + deck_at = self.deck.get_absolute_location() + plate_center_x = source_0.get_absolute_location().x + source_0.get_size_x() / 2 - deck_at.x + plate_center_y = source_0.get_absolute_location().y + source_0.get_size_y() / 2 - deck_at.y + ref_0 = self.pipettes.get_reference_point_location(0) + assert ref_0 is not None + await arm.move_to_x_position(await arm.request_position() + plate_center_x - ref_0.x) + await self.pipettes.move_to_y_positions({i: plate_center_y + (3.5 - i) * 9.5 for i in range(8)}) + # Where the stop disc stands when the tip's bottom just meets the lid's top. + tip = self.pipettes.get_mounted_tip(0) + assert tip is not None + to_the_lid = lid.get_absolute_location().z + lid.get_size_z() - tip.get_absolute_location().z + touching = ref_0.z + to_the_lid + self.assertEqual(check_pipette_move(self.pipettes, z={0: touching + 1.0}, touch=[source_0]), []) + hits = check_pipette_move(self.pipettes, z={0: touching - 2.0}, touch=[source_0]) + self.assertIn("source_0_lid", {c.obstacle.name for c in hits}) + + +class CollisionGateTests(unittest.IsolatedAsyncioTestCase): + """The viewer's flag turns the arm's checks into gates at the driver's dispatch.""" + + async def asyncSetUp(self) -> None: + was = does_tip_tracking() + set_tip_tracking(True) + self.addCleanup(set_tip_tracking, was) + self.facility = build_facility() + self.star = star_of(self.facility) + self.deck = self.star.deck + self.deck.get_resource("destination_1").unassign() + await self.star.setup() + assert self.star.pipettes is not None and self.star.head96 is not None + self.pipettes = self.star.pipettes + self.head = self.star.head96 + assert self.star.iswap is not None + self.iswap = self.star.iswap + self.viewer = _RecordingViewer(raise_on_collision=True) + self.transport = iSWAPTransport(self.iswap, check_collisions=True) + attach_viewer_collisions(self.star.driver, self.viewer, self.transport) + + async def test_a_channel_move_that_would_hit_something_is_refused_saying_which_command(self): + # Lowered with the gate off: the lowering itself would be refused from the row's spread, where + # two of the channels stand over plates. + self.viewer.raise_on_collision = False + await self.pipettes.move_stop_disc_to_z_positions({i: 150.0 for i in range(8)}) + self.viewer.raise_on_collision = True + before = self.pipettes.get_reference_point_location(0) + yp = " ".join(f"{round(300.0 * 10):04}" for _ in range(8)) + with self.assertRaises(CollisionError) as refused: + await self.star.driver.send_command(module="C0", command="JY", yp=yp) + self.assertEqual(refused.exception.origin, "the channels' Y move (C0 JY)") + self.assertTrue(self.viewer.shown) + # Refused before the device heard it: the channels are where they were. + self.assertEqual(self.pipettes.get_reference_point_location(0), before) + + async def test_one_channels_move_is_refused_naming_the_channel(self): + await self.iswap.make_space() + za = f"{self.pipettes.configuration.z_drive_mm_to_increments(150.0):05}" + with self.assertRaises(CollisionError) as refused: + await self.star.driver.send_command(module=self.pipettes.channel_id(0), command="ZA", za=za) + self.assertEqual( + refused.exception.origin, f"channel 0's Z move ({self.pipettes.channel_id(0)} ZA)" + ) + self.assertTrue(self.viewer.shown) + + async def test_a_tipped_channels_jz_is_judged_at_its_stop_disc(self): + # The command takes Z to be each channel's lowest point - the tip's bottom where one is + # mounted - and the check speaks of the stop disc: the gate lifts each target by the overhang + # of the tip the model has on the channel. A tip lowered toward a lid is refused where it + # meets it; a stop a millimetre above runs. + self.viewer.raise_on_collision = False + source_0 = self.deck.get_resource("source_0") + assert isinstance(source_0, Plate) + lid = Lid( + name="source_0_lid", + size_x=source_0.get_size_x(), + size_y=source_0.get_size_y(), + size_z=10.0, + nesting_z_height=2.0, + ) + source_0.assign_child_resource(lid) + rack = self.deck.get_resource("tips_0") + assert isinstance(rack, TipRack) + await self.pipettes.pick_up_tips([rack.get_item(f"{row}1") for row in "ABCDEFGH"]) + arm = self.star.driver.arms[0] + deck_at = self.deck.get_absolute_location() + plate_center_x = source_0.get_absolute_location().x + source_0.get_size_x() / 2 - deck_at.x + plate_center_y = source_0.get_absolute_location().y + source_0.get_size_y() / 2 - deck_at.y + ref_0 = self.pipettes.get_reference_point_location(0) + assert ref_0 is not None + await arm.move_to_x_position(await arm.request_position() + plate_center_x - ref_0.x) + await self.pipettes.move_to_y_positions({i: plate_center_y + (3.5 - i) * 9.5 for i in range(8)}) + self.viewer.raise_on_collision = True + assert self.pipettes.get_mounted_tip(0) is not None + shaft = next( + child for child in self.pipettes.resources[0].children if isinstance(child, TipMountingShaft) + ) + bottom = shaft.tip_bottom() + assert bottom is not None + tip = self.pipettes.get_mounted_tip(0) + assert tip is not None + # The command's Z is the tip's bottom: at the lid's top it touches, above it the way is clear. + to_the_lid = (lid.get_absolute_location().z + lid.get_size_z()) - tip.get_absolute_location().z + jz_touching = ref_0.z + to_the_lid + bottom.z + + def jz(offset: float) -> List[str]: + target = jz_touching + offset + return [f"{round(target * 10):04}" for _ in range(8)] + + await self.star.driver.send_command(module="C0", command="JZ", zp=jz(1.0)) + self.assertEqual(self.viewer.shown, []) + with self.assertRaises(CollisionError) as refused: + await self.star.driver.send_command(module="C0", command="JZ", zp=jz(-2.0)) + self.assertEqual(refused.exception.origin, "the channels' Z move (C0 JZ)") + self.assertTrue(self.viewer.shown) + + async def test_a_head_move_that_would_hit_something_is_refused_saying_which_command(self): + self.viewer.raise_on_collision = False + await self.head.move_stop_disc_to_z_position(200.0) + self.viewer.raise_on_collision = True + ya = f"{self.head.configuration.y_drive_mm_to_increments(100.0):05}" + with self.assertRaises(CollisionError) as refused: + await self.star.driver.send_command( + module=self.head.configuration.module, command="YA", ya=ya + ) + self.assertEqual( + refused.exception.origin, + f"the head's Y move ({self.head.configuration.module} YA)", + ) + self.assertTrue(self.viewer.shown) + + async def test_with_the_flag_off_the_same_command_runs(self): + self.viewer.raise_on_collision = False + await self.pipettes.move_stop_disc_to_z_positions({i: 150.0 for i in range(8)}) + yp = " ".join(f"{round(300.0 * 10):04}" for _ in range(8)) + await self.star.driver.send_command(module="C0", command="JY", yp=yp) + self.assertEqual(self.viewer.shown, []) + + async def test_reads_are_never_handed_to_the_gate(self): + handed: list = [] + + async def recorder(module: str, command: str, params) -> None: + handed.append((module, command)) + + self.star.driver.collision_listener = recorder + await self.pipettes.request_y_positions() + self.assertEqual(handed, []) + + async def test_a_refused_plan_is_drawn_before_it_is_refused(self): + plan = self.transport.plan_pick_up( + self.deck.get_resource("source_3"), direction="front", elbow="right" + ) + with self.assertRaises(iSWAPCollisionError): + await self.transport.execute(plan) + self.assertTrue(self.viewer.shown) + for shown in self.viewer.shown: + self.assertIn("tips_2", {c.obstacle.name for c in shown}) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/hamilton/star/driver/master.py b/pylabrobot/hamilton/star/driver/master.py index aa3cd92d4d7..8c1c7b09448 100644 --- a/pylabrobot/hamilton/star/driver/master.py +++ b/pylabrobot/hamilton/star/driver/master.py @@ -9,7 +9,20 @@ import datetime import json import logging -from typing import Any, Dict, FrozenSet, List, Literal, Optional, Tuple, Union, cast, overload +from typing import ( + Any, + Awaitable, + Callable, + Dict, + FrozenSet, + List, + Literal, + Optional, + Tuple, + Union, + cast, + overload, +) from pylabrobot.events import emit_event from pylabrobot.hamilton.protocol.text.framing import ( @@ -153,6 +166,11 @@ def __init__( # Coordinates commands on the shared link: one at a time per module, and a C0 master # command alone. Read-only requests are exempt, see `send_command`. self._lock = _FirmwareLock() + + # Handed every command that moves something, before it is sent, with its module, command and + # parameters. Set by whoever judges what a command's movers would sweep through; a raise here + # refuses the command before the device hears it. Reads are never handed over. + self.collision_listener: Optional[Callable[[str, str, Dict[str, Any]], Awaitable[None]]] = None self._replies = ReplyRouter( io=self.io, module_id_length=STAR_MODULE_ID_LENGTH, @@ -583,6 +601,10 @@ async def send_command( ) if command[0] in ("R", "Q"): return await self._send(module, command, **kwargs_) + # Judged where it stands, before any lock is taken: a refused command never reaches the device, + # and refusing does not hold a subsystem back. + if self.collision_listener is not None: + await self.collision_listener(module, command, kwargs) key = subsystem or (_FirmwareLock.EVERY_SUBSYSTEM if module == "C0" else module) async with self._lock.subsystem(key): return await self._send(module, command, **kwargs_) diff --git a/pylabrobot/hamilton/star/driver/simulator.py b/pylabrobot/hamilton/star/driver/simulator.py index 76f721542e6..740af048cae 100644 --- a/pylabrobot/hamilton/star/driver/simulator.py +++ b/pylabrobot/hamilton/star/driver/simulator.py @@ -15,7 +15,7 @@ import datetime import logging import math -from typing import Any, Dict, List, Literal, Optional, Tuple, cast +from typing import Any, Awaitable, Callable, Dict, List, Literal, Optional, Tuple, cast from pylabrobot.hamilton.protocol.text.framing import ( assemble_channel_command, @@ -1892,6 +1892,11 @@ def __init__( # What the drives would still be doing, in seconds: the longest move recorded since the last # command, waited out before the next one goes. self._motion_owed = 0.0 + # Who acts out a command's motion before the device answers it, or None: called with the + # module, the command and its parameters, and awaited. A viewer that animates what the drives + # do between the positions the model records sets this, so the command takes as long as the + # animation and the model moves only once it has played. + self.motion_listener: Optional[Callable[[str, str, Dict[str, Any]], Awaitable[None]]] = None # How far a tip of each defined type stands below the stop disc, by tip type index, as # `define_tip_needle` was told. A tip command names one of these, and what it collects hangs # that far down: the traverse height it ends at is the tip's, so the stop disc ends higher. @@ -2098,6 +2103,8 @@ async def _send( **kwargs, ) await self.pay_motion_time() + if self.motion_listener is not None: + await self.motion_listener(module, command, kwargs) answered = await self._answer(module, command, **kwargs) if answered is None: self._log_exchange(cmd, None) diff --git a/pylabrobot/hamilton/star/motion.py b/pylabrobot/hamilton/star/motion.py new file mode 100644 index 00000000000..46ab40b5777 --- /dev/null +++ b/pylabrobot/hamilton/star/motion.py @@ -0,0 +1,1347 @@ +"""What a STAR's firmware commands ask its drives to do, in the resource tree's frames. + +PyLabRobot records where the arm and the channels are once a command has run. How they got there is +the device's own business: its motion controller works it out from the command. The page acts that +out, and needs the command's targets to do it. + +`star_motion` reads one command and returns those targets as local positions of the resources that +model the drives, so the page can move them without knowing anything about firmware. A command that +moves nothing, or one this does not read, returns None. + +Everything here comes from the driver or from the command itself, except what the driver does not +state and a STAR's own command timings do (Venus HxUsbComm traces; MOTION_PROFILES.md section 8): +the X-arm's speed, acceleration and jerk (it moves on an S-curve); the channels' Y speed and +acceleration, and the ripple in which they start their Y moves one after another; the channels' Z +speed; the slow press of a tip pick-up; and each command's fixed time. + +- Channel Z acceleration is the channels' default (`Pipettes.default_z_acceleration`), which the + traces agree with. +- The stroke of a tip command is the one the simulator records (`_record_tip_command`): across, with + the arm and the channels moving at once, down onto the spots, and back up. A pick-up presses the + last stretch, from `tp` to `tz`, slowly. +- An aspiration dwells for as long as its own volume and flow rate say, plus its settling time and + its mixing, and leaves the liquid at its own swap speed. +- Every command also takes a fixed time, beyond the motion: what the traces measure less what the + page draws, as a function of the command's own parameters where they matter. +- The 96-head's Y and Z move at the head's own drive defaults (`HeadConfiguration`), or at what a + move of its own carries; its tip commands make the channels' stroke, channel A1 going to spot A1. +- An iSWAP command moves one drive or two, each at the speed and acceleration the command carries, + converted by the arm's own configuration. The elbow's Y is stated only as a level, so it too moves + at constant speed. A joint turns about the pivot the driver turns it about (`proximal_joint`). + +Heights: the firmware positions the lowest point of what a channel carries, while a channel's +resource is placed by its stop disc, which sits higher by the length of a mounted tip. Every Z here +is converted to the stop disc, with the overhang the channel has when the move is made. +""" + +from typing import Any, Dict, List, Optional, Sequence, cast + +from pylabrobot.hamilton.star.driver.features.head import Head +from pylabrobot.hamilton.star.driver.features.pipettes import ( + Pipettes, + core_tool_face_distance, + core_tool_grip_line_overhang, +) +from pylabrobot.hamilton.star.driver.features.star_collisions import ( + Collision, + CollisionError, + check_head_move, + check_pipette_move, +) +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.hamilton.core_grippers import ( + HamiltonCoreGrippers, + HamiltonCoreGripperTool, +) +from pylabrobot.resources.n_channel_pipettes import TipMountingShaft +from pylabrobot.resources.tip_rack import TipSpot, resting_location + +# The driver states no X speed or acceleration: these are fitted to a STAR's own timings, from 2.56 +# million command/reply pairs in Venus HxUsbComm traces (`tools/hxusbcomm_timing.py`). The X-arm is +# jerk-limited: an S-curve fits within-group R^2 0.976 against a trapezoid's 0.945 (delta AIC 72.7, 15 groups). +# Jerk is well determined; speed and acceleration less so (anything above ~450 mm/s and ~1100 mm/s^2 +# fits nearly as well), since few recorded moves are long enough to cruise. +X_SPEED = 600.0 # mm/s +X_ACCELERATION = 1297.0 # mm/s^2 +X_JERK = 3210.0 # mm/s^3 +# Channel Y is stated only as an acceleration level, not a rate. Single-channel Y jogs in the same +# traces (`C0 KY`, 1/10/100 mm) fit a trapezoid at 300 mm/s and this acceleration within 5 ms; jerk +# adds nothing. In the full model of `C0 AS/DS` (`tools/hxusbcomm_channels.py`) the Y travel at these +# values enters with slope 1.01. PLR's `default_y_speed` is 250 mm/s. +CHANNEL_Y_SPEED = 300.0 # mm/s +CHANNEL_Y_ACCELERATION = 900.0 # mm/s^2 +# The channels start their Y moves one after another (the ripple): the Y cost of a move grows by +# this much per channel moving after the first - 0.30 / 0.69 / 0.76 s for 4 / 7 / 8 channels in +# `C0 DS`, a line with slope 0.118 s and no fixed part (MOTION_PROFILES.md 8.4). The order they go +# in is not measured; the page takes them in channel order. +CHANNEL_Y_STAGGER = 0.118 # s +# Channel Z: dispense-then-aspirate pairs with no X or Y move fit a trapezoid at PLR's acceleration +# and this speed (R^2 within 0.986, delta AIC 186 over a straight line). The speed is loose, 150-200 +# mm/s, as the recorded strokes form two clusters. PLR's `default_z_speed` is 125 mm/s. +CHANNEL_Z_SPEED = 150.0 # mm/s +# A tip pick-up goes down to `tp` at the Z drive's speed and presses on to `tz` at this one: the +# fixed time of `C0 TP` grows 0.0871 s per mm of `tp - tz` (R^2 0.83 with TIP_PICKUP_DOWN_FROM; only +# 8 and 10 mm recorded). +TIP_PRESS_SPEED = 11.5 # mm/s +# A tip pick-up lowers its channels before the crossing has finished: from this share of the +# crossing's time (`tools/hxusbcomm_fixed.py`, delta AIC 7,234 over the two in sequence). Moving 18 +# rather than 9 mm adds 116 ms of X travel but 7 ms to the command, 27 mm adds 198 ms but 112 ms. +# Nearly every recorded pick-up moves 9 mm, so the share is loosely determined. Not measured for +# drops: hardly any recorded drop travels without a Y move. +TIP_PICKUP_DOWN_FROM = 0.82 + +# What a command takes beyond the motion the page draws: the measured duration less the drawn one, +# as a model of the command's own parameters (MOTION_PROFILES.md 8.6, `tools/hxusbcomm_fixed.py`). Played as a pause before +# the motion, since the traces time commands but not what the firmware does when. +# The channel count is left out: nearly every recorded command uses 7 or 8 channels, so its +# coefficient only tracks protocols (it swings from +0.04 to -0.27 s with 0.1% of the data). +ASPIRATE_FIXED = 1.879 # s +ASPIRATE_FIXED_PER_TRANSPORT_AIR_TIME = 6.761 # s per s of transport air at the flow rate +# Mixing happens at the bottom, so it adds to the dwell: its volume time, and a turnaround per cycle. +MIX_VOLUME_TIME_FACTOR = 0.979 +MIX_PER_CYCLE = 0.557 # s +TIP_PICKUP_FIXED = 4.421 # s: the tip clamped and checked, beyond the drawn motion +# Held at the bottom, the tip on the shaft, all but a simple command's handling (as a drop's, HEUR). +TIP_PICKUP_HOLD = TIP_PICKUP_FIXED - 0.065 # s +TIP_DROP_FIXED = ( + 5.009 # s, the mean. The channel count adds 0.21 s each (R^2 0.15), but 7 against 8 +) +# channels is also one protocol against the others, so it is left out. +# Where in a drop that time goes is not in the traces, which time whole commands. It is played at +# the bottom, as the hold while the tips are pushed off (HEUR, from watching the device), all but a +# simple command's handling before the motion. +TIP_EJECT_HOLD = TIP_DROP_FIXED - 0.065 # s +CHANNELS_UP_FIXED = 0.14 # s: `C0 ZA`, median over 1,783, the channels mostly already up +HEAD96_TIP_PICKUP_FIXED = 4.938 # s: `C0 EP`, median over 155, against PLR's head drive defaults +HEAD96_TIP_DROP_FIXED = 4.490 # s: `C0 ER`, median over 5,867 +HEAD96_MOVE_FIXED = 0.11 # s: a head command that travels nothing (`H0 YP`, 10th percentile) +# The 96-head's aspirate and dispense beyond their drawn travel, pumping (volume / flow) and settling +# time, from Venus traces (`tools/hxusbcomm_head96_liquid.py`, MOTION_PROFILES.md 8.6): +# `C0 EA` median of 46 (IQR 5.2-6.9 s, 5.4-6.9 by protocol). `C0 ED` by its mode (`da`): jet (0, +# 1) 1.41 s, median of 12 mode-1 dispenses (1.40-1.44 s); surface and empty (2, 3, 4) 5.85 s, median of 8 +# mode-2 dispenses at normal flow. Four traces: modes 0 and 4 are unrecorded, and a few mode-3 or +# 10 uL/s dispenses took 11-13 s the parameters here do not explain. +HEAD96_ASPIRATE_FIXED = 5.80 # s +HEAD96_DISPENSE_FIXED = {0: 1.41, 1: 1.41, 2: 5.85, 3: 5.85, 4: 5.85} # s, by `da` +# Simple single-drive commands with no recorded counterpart (`C0 JY/JZ/FY`, `X0 XP`, iSWAP `R0` +# primitives): what a single-channel 1 mm jog takes beyond its travel (`C0 KY`, 0.132 s less 0.067 s). +SIMPLE_MOVE_FIXED = 0.065 # s +# CO-RE grip tools have no recorded counterpart either; their strokes are tip strokes (HEUR). +CORE_TOOL_PICKUP_FIXED = TIP_PICKUP_FIXED +CORE_TOOL_RETURN_FIXED = TIP_DROP_FIXED +# Nor do their plate grip and release (`C0 ZP` / `ZR`): taken as the tool strokes' (HEUR, and likely +# long - a grip closes two channels on a plate, with no tip to clamp or check). Named apart so that a +# trace with them in it replaces these alone. +CORE_PLATE_GRIP_FIXED = CORE_TOOL_PICKUP_FIXED +CORE_PLATE_RELEASE_FIXED = CORE_TOOL_RETURN_FIXED + +# How close a command's position has to be to a tip spot's centre to be taken as that spot, in mm. +SPOT_TOLERANCE = 1.0 + +# `TipDropMethod.DROP`, as `C0 TR` sends it in `ti`. +TIP_DROP = 1 + + +def _pipetting_arm(driver: Any) -> Optional[Any]: + """The arm carrying the channels, or None when there is none or nothing models it yet.""" + arm = next((a for a in getattr(driver, "arms", []) if a.pipettes is not None), None) + if arm is None or arm.resource is None or not arm.pipettes.resources: + return None + return arm + + +def _xyz(coordinate: Any) -> Dict[str, float]: + return {"x": float(coordinate.x), "y": float(coordinate.y), "z": float(coordinate.z)} + + +def _tenths(value: Any) -> float: + return int(value) / 10 + + +def _as_list(value: Any) -> List[Any]: + """A per-channel parameter as a list; `JY` sends its values as one space-separated string.""" + if isinstance(value, str): + return value.split() + return list(value) + + +def _involved(pattern: Sequence[Any]) -> List[int]: + return [i for i, used in enumerate(pattern) if used in (True, 1, "1")] + + +class _Frames: + """Converts the positions the drives report into where the resources modelling them sit.""" + + def __init__(self, driver: Any, arm: Any): + self.driver = driver + self.arm = arm + self.pipettes = arm.pipettes + self.channels = arm.pipettes.resources + self.deck = driver.deck + self.on_arm = [c.parent.get_location_wrt(self.deck) for c in self.channels] + self.anchors = [self.pipettes._reference_anchor(c) for c in self.channels] + + def overhang(self, channel: int) -> float: + """How far what a channel carries hangs below its stop disc, in mm, read off the model. + + The firmware positions the lowest point of what the channel carries: a tip's bottom, a CO-RE + grip tool's grip line. From the resource tree, so it holds for any driver, not only the + simulator (which works it out the same way, `SimulatedPipettes._below_stop_disc`). + """ + shaft = self.shaft(channel) + if shaft is None: + return 0.0 + bottom = shaft.tip_bottom() + if bottom is None: + return 0.0 + if isinstance(shaft.tip, HamiltonCoreGripperTool): + return float(-bottom.z - shaft.tip.grip_line_height) + return float(-bottom.z) + + def arm_x(self, x: float) -> float: + return round(float(x - self.arm.configuration.reference_point_from_left), 2) + + def channel_y(self, channel: int, y: float) -> float: + return round(float(y - self.on_arm[channel].y - self.anchors[channel].y), 2) + + def channel_z(self, channel: int, stop_disc_z: float) -> float: + return round(float(stop_disc_z - self.on_arm[channel].z - self.anchors[channel].z), 2) + + def lowest_point_z(self, channel: int, z: float, overhang: Optional[float] = None) -> float: + """A firmware height, the lowest point, as the channel's local Z.""" + return self.channel_z(channel, z + (self.overhang(channel) if overhang is None else overhang)) + + def current_y(self, channel: int) -> float: + """Where the channel's reference point is along Y, in mm on the deck.""" + location = self.channels[channel].location + return float(location.y + self.on_arm[channel].y + self.anchors[channel].y) + + def planned_ys(self, targets: Dict[int, float]) -> Dict[int, float]: + """Every channel's Y once the named ones are at their targets, on one rail. + + As `Pipettes._plan_y_positions(make_space=True)` plans it - the plan the simulator records a + tip command with - from where the model has the channels, read as the device reports them, to a + tenth. Channel 0 is at the back, so Y falls as the channel number rises. Behind the backmost + named channel and in front of the frontmost, the others are pushed only as far as the spacing + asks; between two named channels, each unnamed one stands at the spacing in front of the one + behind it. + """ + if not targets: + return {} + n = len(self.channels) + + def gap(i: int, j: int) -> float: + return float(self.pipettes._min_spacing_between(i, j)) + + positions = [round(self.current_y(c), 1) for c in range(n)] + positions[-1] = max(positions[-1], self.driver.configuration.left_arm_min_y_position) + for c in range(n - 2, -1, -1): + if positions[c] - positions[c + 1] < gap(c, c + 1): + positions[c] = positions[c + 1] + gap(c, c + 1) + ys = dict(enumerate(positions)) + ys.update(targets) + back, front = min(targets), max(targets) + for c in range(back, 0, -1): + if ys[c - 1] - ys[c] < gap(c - 1, c): + ys[c - 1] = ys[c] + gap(c - 1, c) + for c in range(back + 1, front): + if c not in targets: + ys[c] = ys[c - 1] - gap(c - 1, c) + for c in range(front, n - 1): + if ys[c] - ys[c + 1] < gap(c, c + 1): + ys[c + 1] = ys[c] - gap(c, c + 1) + return {c: round(y, 2) for c, y in ys.items()} + + def shaft(self, channel: int) -> Optional[Any]: + return next( + ( + child for child in self.channels[channel].children if child.category == "tip_mounting_shaft" + ), + None, + ) + + def spot_at(self, x: float, y: float) -> Optional[TipSpot]: + """The tip spot centred at (x, y) on the deck, or None.""" + for resource in self.deck.get_all_children(): + if not isinstance(resource, TipSpot): + continue + centre = resource.get_location_wrt(self.deck, "c", "c", "b") + if abs(centre.x - x) <= SPOT_TOLERANCE and abs(centre.y - y) <= SPOT_TOLERANCE: + return resource + return None + + def drives(self) -> Dict[str, Dict[str, Optional[float]]]: + p = self.pipettes + return { + "x": {"speed": X_SPEED, "acceleration": X_ACCELERATION, "jerk": X_JERK}, + "y": { + "speed": CHANNEL_Y_SPEED, + "acceleration": CHANNEL_Y_ACCELERATION, + "stagger": CHANNEL_Y_STAGGER, + }, + "z": {"speed": CHANNEL_Z_SPEED, "acceleration": p.default_z_acceleration}, + } + + +def _request(frames: _Frames, kind: str, command: str) -> Dict[str, Any]: + return { + "kind": kind, + "command": command, + "arm": None, + "channels": [], + "traverse": [], + "attach": [], + "dwell": 0.0, + "fixed": 0.0, + "drives": frames.drives(), + "moves": [], + "turns": [], + "jaws": None, + } + + +def _channel(frames: _Frames, channel: int, **targets: Optional[float]) -> Dict[str, Any]: + return {"name": frames.channels[channel].name, "channel": channel, **targets} + + +def _stroke( + frames: _Frames, + kind: str, + command: str, + params: Dict[str, Any], + traverse: float, + down: Dict[int, float], + end: Dict[int, float], + extra: Optional[Dict[int, Dict[str, float]]] = None, +) -> Dict[str, Any]: + """A command that travels at a height, goes down onto its targets and comes back up. + + `traverse` is a firmware height; `down` and `end` are already local Z, keyed by channel. `extra` + adds fields to a channel's entry. + """ + pattern = _as_list(params["tm"]) + involved = _involved(pattern) + xs, ys = _as_list(params["xp"]), _as_list(params["yp"]) + request = _request(frames, kind, command) + if not involved: + return request + # The arm ends over the last column the command visited, as the simulator records it. + request["arm"] = {"name": frames.arm.resource.name, "x": frames.arm_x(_tenths(xs[involved[-1]]))} + planned = frames.planned_ys({c: _tenths(ys[c]) for c in involved}) + request["traverse"] = [ + _channel(frames, c, z=frames.lowest_point_z(c, traverse)) for c in range(len(frames.channels)) + ] + request["channels"] = [ + _channel( + frames, + c, + y=frames.channel_y(c, y), + down=down.get(c), + end=end.get(c), + **(extra or {}).get(c, {}), + ) + for c, y in planned.items() + ] + return request + + +def _mounted_location(shaft: Any, tip: Any) -> Dict[str, float]: + """Where a tip sits on a shaft once picked up, as `TipMountingShaft` places it: its pick-up + location `fitting_depth` up the shaft's axis.""" + grip = (tip.pick_up_location or tip.get_anchor("c", "c", "t")).rotated(tip.rotation) + return _xyz( + Coordinate( + shaft.get_size_x() / 2 - grip.x, + shaft.get_size_y() / 2 - grip.y, + tip.fitting_depth - grip.z, + ) + ) + + +def _handover(tip: Any, parent: Any, location: Optional[Dict[str, float]]) -> Dict[str, Any]: + """A tip changing hands at the bottom of a stroke, placed where the model will place it.""" + return { + "name": tip.name, + "parent": None if parent is None else parent.name, + "location": location, + "rotation": _xyz(tip.rotation), + } + + +def _positions(params: Dict[str, Any], channel: int) -> Any: + return _tenths(_as_list(params["xp"])[channel]), _tenths(_as_list(params["yp"])[channel]) + + +def _tip_pickup(frames: _Frames, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + involved = _involved(_as_list(params["tm"])) + # How far each tip will hang below its stop disc once on: read off the tip in the spot and where + # the shaft will seat it, as `TipMountingShaft.tip_bottom` then reads it. The simulator's table of + # defined tip lengths is only the fallback, for a spot the model has no tip in. + defined: float = getattr(frames.driver, "defined_tip_lengths", {}).get(int(params["tt"]), 0.0) + + def carried(c: int) -> float: + spot, shaft = frames.spot_at(*_positions(params, c)), frames.shaft(c) + if spot is None or spot.tip is None or shaft is None: + return defined + return float(-_mounted_location(shaft, spot.tip)["z"]) + + request = _stroke( + frames, + "tip_pickup", + command, + params, + traverse=_tenths(params["th"]), + # Down to where the tip begins, then pressed on to the end of the search, slowly. + down={c: frames.lowest_point_z(c, _tenths(params["tp"])) for c in involved}, + # It comes away carrying the tip, which then hangs below the stop disc. + end={c: frames.lowest_point_z(c, _tenths(params["th"]), carried(c)) for c in involved}, + extra={ + c: {"press": frames.lowest_point_z(c, _tenths(params["tz"])), "press_speed": TIP_PRESS_SPEED} + for c in involved + }, + ) + # The channels hold at the bottom once the tips are on: the pick-up's fixed time, bar the + # command's handling before it moves. + request["down_from"] = TIP_PICKUP_DOWN_FROM + request["dwell"] = TIP_PICKUP_HOLD + request["fixed"] = round(TIP_PICKUP_FIXED - TIP_PICKUP_HOLD, 3) + # At the bottom of the stroke each channel takes the tip in the spot under it onto its shaft. + for c in involved: + spot, shaft = frames.spot_at(*_positions(params, c)), frames.shaft(c) + if spot is not None and spot.tip is not None and shaft is not None: + request["attach"].append(_handover(spot.tip, shaft, _mounted_location(shaft, spot.tip))) + return request + + +def _tip_drop(frames: _Frames, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + involved = _involved(_as_list(params["tm"])) + # With `DROP` the heights are the stop disc's, not the lowest point's (`_unchecked_fw_drop_tips`): + # the tip still on it hangs below them. + drop = int(params.get("ti", 0)) == TIP_DROP + request = _stroke( + frames, + "tip_drop", + command, + params, + traverse=_tenths(params["th"]), + down={ + c: frames.lowest_point_z(c, _tenths(params["tz"]), 0.0 if drop else None) for c in involved + }, + # It comes away empty. + end={c: frames.lowest_point_z(c, _tenths(params["te"]), 0.0) for c in involved}, + ) + # At the bottom the channels hold while the tips are pushed off: the drop's fixed time, bar the + # command's handling before it moves. + request["dwell"] = TIP_EJECT_HOLD + request["fixed"] = round(TIP_DROP_FIXED - TIP_EJECT_HOLD, 3) + # At the bottom of the stroke each channel leaves its tip in the spot under it, or, over + # somewhere that is not a spot - the waste - where it is. + for c in involved: + shaft = frames.shaft(c) + if shaft is None or shaft.tip is None: + continue + spot = frames.spot_at(*_positions(params, c)) + if spot is not None and spot.tip is None: + request["attach"].append(_handover(shaft.tip, spot, _xyz(resting_location(spot, shaft.tip)))) + else: + request["attach"].append(_handover(shaft.tip, None, None)) + return request + + +def _as_tip_command(params: Dict[str, Any], back: int, channels: int) -> Dict[str, Any]: + """A CO-RE tool command's two channels in a tip command's lists: one X, a Y each.""" + xs, ys, pattern = ["0"] * channels, ["0"] * channels, [False] * channels + for channel, y in ((back, params["ya"]), (back + 1, params["yb"])): + xs[channel], ys[channel], pattern[channel] = params["xs"], y, True + return {**params, "xp": xs, "yp": ys, "tm": pattern} + + +def _below_grip_line(tool: Any) -> float: + """How far a mounted CO-RE grip tool's grip line hangs below the stop disc, in mm: what the + master reports a channel carrying one at.""" + pick_up = tool.pick_up_location or tool.get_anchor("c", "c", "t") + return float(pick_up.z - tool.fitting_depth - tool.grip_line_height) + + +def _core_grippers(frames: "_Frames") -> Optional[Any]: + """The driver's CO-RE grippers feature, or None when the driver has none.""" + return getattr(frames.pipettes._driver, "core_grippers", None) + + +def _core_channels(frames: "_Frames") -> List[int]: + """The channels carrying CO-RE grip tools, back to front, from the feature that picked them up; + from the model where the feature does not know.""" + grippers = _core_grippers(frames) + if grippers is not None and grippers._back_channel is not None: + return [grippers._back_channel, grippers._front_channel] + return [ + c + for c in range(len(frames.channels)) + if isinstance(frames.pipettes.get_mounted_tool(c), HamiltonCoreGripperTool) + ] + + +def _core_holder(frames: "_Frames") -> Any: + """The CO-RE gripper holder the deck carries.""" + deck = frames.pipettes._driver.deck + for resource in deck.get_all_children(): + if isinstance(resource, HamiltonCoreGrippers): + return resource + raise TypeError("the deck carries no CO-RE gripper holder") + + +def _hold_at_bottom(request: Dict[str, Any], fixed: float) -> Dict[str, Any]: + """Spend a command's fixed time at the bottom of its stroke - where tips are clamped or pushed + off, a tool seated, a plate gripped or let go - rather than as a still pause before it moves, + bar the command's handling (`SIMPLE_MOVE_FIXED`), as the channels' tip commands do. Where in a + command the firmware spends it is not measured (HEUR); played before the move, the head and the + grip tools stood for seconds before setting off.""" + request["dwell"] = round(fixed - SIMPLE_MOVE_FIXED, 3) + request["fixed"] = SIMPLE_MOVE_FIXED + return request + + +def _core_tool_pickup(frames: _Frames, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + """`C0 ZT`: down onto the parked tools, and up carrying them, as a tip pick-up.""" + back = int(params["pa"]) - 1 + holder = _core_holder(frames) + tools = {back: holder.back_tool, back + 1: holder.front_tool} + request = _stroke( + frames, + "core_tool_pickup", + command, + _as_tip_command(params, back, len(frames.channels)), + traverse=_tenths(params["th"]), + down={c: frames.lowest_point_z(c, _tenths(params["tz"]), 0.0) for c in tools}, + end={ + c: frames.lowest_point_z(c, _tenths(params["th"]), _below_grip_line(tool)) + for c, tool in tools.items() + }, + ) + for c, tool in tools.items(): + shaft = frames.shaft(c) + if shaft is not None: + request["attach"].append(_handover(tool, shaft, _mounted_location(shaft, tool))) + return _hold_at_bottom(request, CORE_TOOL_PICKUP_FIXED) + + +def _core_tool_return(frames: _Frames, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + """`C0 ZS`: down into the holder with the tools, leaving each where it was parked, and up.""" + channels = _core_channels(frames) + if len(channels) != 2: + raise ValueError("a tool return needs two channels carrying tools") + request = _stroke( + frames, + "core_tool_return", + command, + _as_tip_command(params, channels[0], len(frames.channels)), + traverse=_tenths(params["th"]), + down={c: frames.lowest_point_z(c, _tenths(params["tz"])) for c in channels}, + end={c: frames.lowest_point_z(c, _tenths(params["te"]), 0.0) for c in channels}, + ) + for c in channels: + shaft = frames.shaft(c) + if shaft is None or shaft.tip is None: + continue + grippers = _core_grippers(frames) + parked = ( + { + tool.name: (holder, location, tool.rotation) + for tool, holder, location in grippers._parked_tools + } + if grippers is not None + else {} + ) + holder, location, rotation = parked[shaft.tip.name] + handover = _handover(shaft.tip, holder, _xyz(location)) + handover["rotation"] = _xyz(rotation) + request["attach"].append(handover) + return _hold_at_bottom(request, CORE_TOOL_RETURN_FIXED) + + +def _core_plate(frames: _Frames, command: str, params: Dict[str, Any], kind: str) -> Dict[str, Any]: + """`C0 ZP`, `ZM`, `ZR`: the two tool channels travel, go down to the grip line at the plate's + centre, one on each side of it, and come up. At the bottom of a grip the plate passes to the + front tool channel's shaft; at the bottom of a release, to what it is put down on - as + `COREGripper` leaves it for the command (`Pipettes._core_handover`).""" + channels = _core_channels(frames) + if len(channels) != 2: + raise ValueError("a CO-RE plate command needs two channels carrying tools") + back, front = channels + shaft = frames.shaft(front) + tool = shaft.tip if shaft is not None else None + if not isinstance(tool, HamiltonCoreGripperTool): + raise ValueError("a CO-RE plate command needs its CO-RE grip tool on the front channel") + grippers = _core_grippers(frames) + handover = grippers._handover if grippers is not None else None + held = handover[0] if handover else None + width = held.get_absolute_size_y() if held is not None else _tenths(params.get("yo", 0)) - 3.0 + half = width / 2 + core_tool_face_distance(tool) + centre_y, grip = _tenths(params["yj"]), _tenths(params["zj"]) + along = {**params, "ya": round((centre_y + half) * 10), "yb": round((centre_y - half) * 10)} + end = _tenths(params["zj"] if kind == "core_move" else params.get("te", params["th"])) + below = core_tool_grip_line_overhang(tool) + request = _stroke( + frames, + kind, + command, + _as_tip_command(along, back, len(frames.channels)), + traverse=_tenths(params["th"]), + down={c: frames.lowest_point_z(c, grip, below) for c in channels}, + end={c: frames.lowest_point_z(c, end, below) for c in channels}, + ) + if handover and kind != "core_move": + resource, parent, location = handover + request["attach"].append( + { + "name": resource.name, + "parent": None if parent is None else parent.name, + "location": None if location is None else _xyz(location), + "rotation": _xyz(resource.rotation), + } + ) + if kind == "core_move": + return request + return _hold_at_bottom( + request, CORE_PLATE_GRIP_FIXED if kind == "core_grip" else CORE_PLATE_RELEASE_FIXED + ) + + +def _aspirate(frames: _Frames, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + involved = _involved(_as_list(params["tm"])) + + # Lists other than the positions run over the channels involved, in order. + def per_channel(field: str) -> Dict[int, float]: + values = _as_list(params[field]) + return {c: _tenths(values[i]) for i, c in enumerate(involved)} + + def raw(field: str, default: int = 0) -> Dict[int, int]: + values = _as_list(params.get(field, [default] * len(involved))) + return {c: int(values[i]) for i, c in enumerate(involved)} + + def optional(field: str) -> Dict[int, float]: + return per_channel(field) if field in params else {c: 0.0 for c in involved} + + surface = per_channel("zl") + floor = per_channel("zx") + swap_speed = per_channel("de") + # Into the liquid by the immersion depth, or above it when the direction says so (`it` 1); never + # below the lowest the command allows. + direction = raw("it") + immersion = optional("ip") + down = { + c: max(surface[c] + (immersion[c] if direction[c] == 1 else -immersion[c]), floor[c]) + for c in involved + } + # While it draws, the tip follows the sinking surface down by `fp`, at a steady pace over the + # time the volume takes. The narrower lower section `zu`/`zr` changes that pace on the device in + # a way the driver does not document, so it is not drawn. + following = optional("fp") + volumes, speeds = per_channel("av"), per_channel("as_") + draw_time = {c: volumes[c] / speeds[c] if speeds[c] else 0.0 for c in involved} + followed = {c: max(down[c] - following[c], floor[c]) for c in involved} + # Out of the liquid at the swap speed, then up by the pull-out distance before the transport air + # is drawn, at the swap speed too: Venus aspirations otherwise identical take 5.35 s (n 4 against + # 802) and 4.85 s (n 12 against 3,534) longer with a 10 mm pull-out at 2 mm/s - 10 mm at 2 mm/s. + # From the surface (HEUR - the driver says only "rise before drawing transport air"). + pull_out = optional("po") + + def extras(c: int) -> Dict[str, float]: + extra: Dict[str, float] = {} + if followed[c] < down[c] - 0.05 and draw_time[c] > 0: + extra["follow"] = frames.lowest_point_z(c, followed[c]) + extra["follow_speed"] = round((down[c] - followed[c]) / draw_time[c], 3) + if swap_speed[c] > 0: + extra["leave"] = frames.lowest_point_z(c, surface[c]) + extra["leave_speed"] = swap_speed[c] + if pull_out[c] > 0: + extra["pull_out"] = frames.lowest_point_z(c, surface[c] + pull_out[c]) + return extra + + request = _stroke( + frames, + "aspirate", + command, + params, + traverse=_tenths(params["th"]), + down={c: frames.lowest_point_z(c, down[c]) for c in involved}, + end={c: frames.lowest_point_z(c, _tenths(params["te"])) for c in involved}, + extra={c: extras(c) for c in involved}, + ) + # As long as the slowest channel takes to draw its volume, settle and mix. + settle = per_channel("wt") + cycles = {c: int(v) for c, v in zip(involved, _as_list(params.get("mc", [0] * len(involved))))} + mix_volumes = per_channel("mv") if "mv" in params else {c: 0.0 for c in involved} + mix_speeds = per_channel("ms") if "ms" in params else {c: 0.0 for c in involved} + + def mixing(c: int) -> float: + if not cycles.get(c) or not mix_speeds[c]: + return 0.0 + volume_time = 2 * cycles[c] * mix_volumes[c] / mix_speeds[c] + return MIX_VOLUME_TIME_FACTOR * volume_time + MIX_PER_CYCLE * cycles[c] + + request["dwell"] = round( + max((volumes[c] / speeds[c] if speeds[c] else 0.0) + settle[c] + mixing(c) for c in involved), + 2, + ) + transport_air = per_channel("ta") if "ta" in params else {c: 0.0 for c in involved} + request["fixed"] = round( + ASPIRATE_FIXED + + ASPIRATE_FIXED_PER_TRANSPORT_AIR_TIME + * max(transport_air[c] / speeds[c] if speeds[c] else 0.0 for c in involved), + 3, + ) + return request + + +def _move_y(frames: _Frames, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + request = _request(frames, "move_y", command) + ys = _as_list(params["yp"]) + request["channels"] = [ + _channel(frames, c, y=frames.channel_y(c, _tenths(y))) + for c, y in enumerate(ys[: len(frames.channels)]) + ] + return request + + +def _free_y_range(frames: _Frames, command: str) -> Dict[str, Any]: + """`C0 FY`: the master packs the channels as far forward as they fit, each against the ones in + front of it - where the simulator puts them, and writes them before the command is sent.""" + request = _request(frames, "move_y", command) + request["recorded_first"] = True + floor = frames.driver.configuration.left_arm_min_y_position + widths = [c.width for c in frames.pipettes.configuration.channels] + request["channels"] = [ + _channel(frames, c, y=frames.channel_y(c, floor + sum(widths[c + 1 :]))) + for c in range(len(frames.channels)) + ] + return request + + +def _channels_up(frames: _Frames, command: str) -> Dict[str, Any]: + """`C0 ZA`: every channel up to the top of its Z travel, where `probe_z_max` leaves them - and + where the simulator writes them before the command is sent.""" + request = _request(frames, "move_z", command) + request["recorded_first"] = True + ceiling = frames.pipettes.configuration.z_range[1] + request["channels"] = [ + _channel(frames, c, end=frames.channel_z(c, ceiling)) for c in range(len(frames.channels)) + ] + return request + + +def _move_z(frames: _Frames, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + request = _request(frames, "move_z", command) + zs = _as_list(params["zp"]) + request["channels"] = [ + _channel(frames, c, end=frames.lowest_point_z(c, _tenths(z))) + for c, z in enumerate(zs[: len(frames.channels)]) + ] + return request + + +def _move_x(frames: _Frames, driver: Any, command: str, params: Dict[str, Any]) -> Optional[Dict]: + side = "left" if command == "XP" else "right" + arm = next((a for a in driver.arms if a.side == side), None) + if arm is None or arm.resource is None: + return None + increments = params.get(f"{arm.parameter_prefix}a") + if increments is None: + return None + x = arm.configuration.x_increments_to_mm(int(increments)) + request = _request(frames, "move_x", command) + request["arm"] = { + "name": arm.resource.name, + "x": round(x - arm.configuration.reference_point_from_left, 2), + } + return request + + +# -- iSWAP --------------------------------------------------------------------- + + +def _iswap_of(driver: Any) -> Optional[Any]: + """The iSWAP, or None when there is none or nothing models it yet.""" + for arm in getattr(driver, "arms", []): + iswap = arm.iswap + if iswap is not None and None not in (iswap.resource, iswap.link_1, iswap.gripper): + return iswap + return None + + +def _iswap_request(kind: str, command: str) -> Dict[str, Any]: + # The iSWAP writes a move's target into the model as it sends it (`elbow_move_to_y_position`, + # `rotate_to_angles`, `gripper_move_to_jaw_position`): what the model says of these parts before + # the move is played is already its end. + return { + "kind": kind, + "command": command, + "recorded_first": True, + "arm": None, + "channels": [], + "traverse": [], + "attach": [], + "dwell": 0.0, + "drives": {}, + "moves": [], + "turns": [], + "jaws": None, + } + + +def _elbow_move(driver: Any, iswap: Any, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + """`R0 YA` or `R0 ZA`: the head the arm hangs from, along Y or Z.""" + c = iswap.configuration + head = iswap.resource + on_arm, anchor = head.parent.get_location_wrt(driver.deck), head.reference_point + request = _iswap_request("iswap_move", "R0" + command) + if command == "YA": + y = c.y_increments_to_mm(int(params["ya"])) + request["moves"].append( + { + "name": head.name, + "axis": 1, + "to": round(float(y - on_arm.y - anchor.y), 2), + "speed": c.y_increments_to_mm(int(params["yv"])), + "acceleration": None, + } + ) + else: + # The drive counts the finger plane; the head's bottom stands above it. + z = c.z_increments_to_mm(int(params["za"])) + c.elbow_z_offset_above_finger + request["moves"].append( + { + "name": head.name, + "axis": 2, + "to": round(float(z - on_arm.z - anchor.z), 2), + "speed": c.z_increments_to_mm(int(params["zv"])), + "acceleration": round(int(params["zr"]) * 1000 * c.z_mm_per_increment, 2), + } + ) + return request + + +def _joints(iswap: Any, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + """`R0 PA`: the elbow and the wrist together, each to its own angle at its own speed. + + A joint's angle is stated as its drive reports it; `base` is what the driver subtracts from it to + get the resource's rotation (`elbow_drive_update_angle`, `wrist_drive_update_angle`). + """ + c = iswap.configuration + request = _iswap_request("iswap_turn", "R0" + command) + link, gripper = iswap.link_1, iswap.gripper + request["turns"].append( + { + "name": link.name, + "drive": c.elbow_drive_increments_to_angle(int(params["wa"])), + "base": 90.0, + "pivot": _xyz(link.proximal_joint), + "speed": c.elbow_increments_to_deg_per_sec(int(params["wv"])), + "acceleration": c.elbow_increments_to_deg_per_sec2(int(params["wr"])), + } + ) + if c.wrist_drive_predefined_increments is not None: + request["turns"].append( + { + "name": gripper.name, + "drive": c.wrist_increments_to_deg(int(params["ta"])), + "base": c.wrist_increments_to_deg(c.wrist_drive_predefined_increments.straight), + "pivot": _xyz(gripper.proximal_joint), + "speed": c.wrist_increments_to_deg_per_sec(int(params["tv"])), + "acceleration": c.wrist_increments_to_deg_per_sec2(int(params["tr"])), + } + ) + return request + + +def _jaws( + iswap: Any, command: str, width: float, speed: float, acceleration: float +) -> Optional[Dict[str, Any]]: + """The fingers stood `width` apart, as `MechanicalGripper._place_the_fingers` stands them. + + Each finger travels half of what the width does, in the same time, so at half the drive's speed. + The page decides from which way they move whether this closes on something or lets it go. + """ + gripper = iswap.gripper + low, high = gripper.jaw_range + if not low <= width <= high: + return None # the model leaves the jaws where they are, and so does the page + centre = gripper.proximal_joint.y + gripper.tool_center_point.y + fingers = [] + for finger, side in zip(gripper.fingers, (1.0, -1.0)): + facing = centre + side * width / 2.0 + fingers.append( + {"name": finger.name, "y": round(facing if side > 0 else facing - finger.get_size_y(), 3)} + ) + request = _iswap_request("iswap_jaws", command) + request["jaws"] = { + "gripper": gripper.name, + "width": width, + "fingers": fingers, + "speed": speed / 2.0, + "acceleration": acceleration / 2.0, + # Where the fingers close, in the gripper's own frame: what they take hold of is there. + "grip_point": _xyz(gripper.proximal_joint + gripper.tool_center_point), + } + return request + + +def _iswap_motion( + driver: Any, iswap: Any, module: str, command: str, params: Dict[str, Any] +) -> Optional[Dict[str, Any]]: + c = iswap.configuration + if module == "R0" and command in ("YA", "ZA"): + return _elbow_move(driver, iswap, command, params) + if module == "R0" and command == "PA": + return _joints(iswap, command, params) + if module == "R0" and command == "GA": + return _jaws( + iswap, + "R0GA", + c.gripper_increments_to_mm(int(params["ga"])), + c.gripper_increments_to_mm_per_sec(int(params["gv"])), + c.gripper_increments_to_mm_per_sec2(int(params["gr"])), + ) + if module == "C0" and command == "GC": + # Closes onto what is there, at the drive's closing speed; the width is in tenths of a mm. + return _jaws( + iswap, + "C0GC", + _tenths(params["gb"]), + c.gripper_increments_to_mm_per_sec(c.gripper_close_speed_default_increments), + c.gripper_increments_to_mm_per_sec2(c.gripper_acceleration_default_increments), + ) + return None + + +# -- 96-head ------------------------------------------------------------------------------------- + + +def _head96_of(driver: Any) -> Optional[Any]: + """The 96-head, or None when there is none or nothing models it yet.""" + for arm in getattr(driver, "arms", []): + head = arm.head96 + if head is not None and head.resource is not None and head.resource.location is not None: + return head + return None + + +class _HeadFrames: + """Converts what the head's drives report - channel A1, on the deck - into where its resource + sits, as `Head.update_location_by_reference_point` records it.""" + + def __init__(self, driver: Any, head: Any): + self.driver = driver + self.head = head + self.resource = head.resource + self.a1 = head.resource.get_item("A1") + self.on_arm = head.resource.parent.get_location_wrt(driver.deck) + # The drives report channel A1's axis; a shaft, like any resource, is placed by its corner, set + # back from the axis by its radius. Read as the corner, the head was drawn half a shaft (3.5 mm) + # off in X and Y - the tips against the walls of a MIDI plate's 6.8 mm wells. + self.axis_in_head = self.a1.location + Coordinate( + self.a1.get_size_x() / 2, self.a1.get_size_y() / 2, 0 + ) + + def axis_on_deck(self) -> Coordinate: + """Where channel A1's axis is now, in deck mm (Z at the shaft's end).""" + return cast(Coordinate, self.a1.get_location_wrt(self.driver.deck, x="c", y="c", z="b")) + + def local_y(self, y: float) -> float: + return round(float(y - self.on_arm.y - self.axis_in_head.y), 2) + + def local_z(self, stop_disc_z: float) -> float: + return round(float(stop_disc_z - self.on_arm.z - self.a1.location.z), 2) + + def overhang(self) -> float: + """How far what channel A1 carries hangs below it, in mm.""" + bottom = self.a1.tip_bottom() + return -float(bottom.z) if bottom is not None else 0.0 + + def drives(self) -> Dict[str, Dict[str, Optional[float]]]: + c = self.head.configuration + return { + "x": {"speed": X_SPEED, "acceleration": X_ACCELERATION, "jerk": X_JERK}, + "y": {"speed": c.y_drive_speed_default, "acceleration": c.y_drive_acceleration_default}, + "z": {"speed": c.z_drive_speed_default, "acceleration": c.z_drive_acceleration_default}, + } + + def request(self, kind: str, command: str) -> Dict[str, Any]: + return { + "kind": kind, + "command": command, + "arm": None, + "channels": [], + "traverse": [], + "attach": [], + "dwell": 0.0, + "drives": self.drives(), + "moves": [], + "turns": [], + "jaws": None, + } + + +def _spots_under_head(deck: Any, head: Any, x: float, y: float) -> List[Optional[Any]]: + """Per shaft, the tip spot it stands over with head channel A1 at (x, y): the rack of whichever + spot is there, paired with the shafts as the head pairs them for an offset command + (`Head96._spots_under_shafts`). All None over no rack.""" + for resource in deck.get_all_children(): + if isinstance(resource, TipSpot) and resource.parent is not None: + centre = resource.get_location_wrt(deck, "c", "c", "b") + if abs(centre.x - x) <= SPOT_TOLERANCE and abs(centre.y - y) <= SPOT_TOLERANCE: + rack = resource.parent + if rack.num_items != 96: + break + a1 = rack.get_item("A1").get_location_wrt(deck, "c", "c", "b") + under = head._spots_under_shafts(Coordinate(x - a1.x, y - a1.y, 0)) + return [None if j is None else rack.get_item(j) for j in under] + return [None] * 96 + + +def _head96_tips(driver: Any, head: Any, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + """`C0 EP` or `C0 ER`: the head's stroke, channel A1 to spot A1. + + Up to traverse height, across - the arm in X, the head in Y - down with the stop discs to the + spot's height, the 96 tips changing hands, and up to where the command ends: the bottom of what + the head then carries, as the simulator records it (`SimulatedHead96._place_after_tip_command`). + """ + frames = _HeadFrames(driver, head) + pick_up = command == "EP" + request = frames.request("head96_tip_pickup" if pick_up else "head96_tip_drop", "C0" + command) + x = _tenths(params["xs"]) * (-1 if int(params["xd"]) else 1) + y = _tenths(params["yh"]) + arm = head.arm + if arm is not None and arm.resource is not None: + a1 = frames.axis_on_deck() + request["arm"] = { + "name": arm.resource.name, + "x": round(float(arm.resource.location.x + x - a1.x), 2), + } + + # Each shaft works the spot under it, whichever spot of the rack channel A1 stands over: a head + # sent over a rack shifted by whole columns (a partial pick-up off a tip support) pairs them + # shifted. + under = _spots_under_head(driver.deck, head, x, y) + shafts = frames.resource.get_all_items() + after = 0.0 + if pick_up: + for shaft, spot in zip(shafts, under): + if spot is not None and spot.tip is not None: + mounted = _mounted_location(shaft, spot.tip) + request["attach"].append(_handover(spot.tip, shaft, mounted)) + if after == 0.0: + after = -mounted["z"] + else: + for i, shaft in enumerate(shafts): + if shaft.tip is None: + continue + spot = under[i] + if spot is not None and spot.tracks_tips and spot.tip is None: + request["attach"].append( + _handover(shaft.tip, spot, _xyz(resting_location(spot, shaft.tip))) + ) + else: + request["attach"].append(_handover(shaft.tip, None, None)) + + name = frames.resource.name + request["traverse"] = [ + {"name": name, "z": frames.local_z(_tenths(params["zh"]) + frames.overhang())} + ] + request["channels"] = [ + { + "name": name, + "channel": 0, + "y": frames.local_y(y), + # The stop discs go to the spot's height: channel A1 to the centre of spot A1, at its Z. + "down": frames.local_z(_tenths(params["za"])), + "end": frames.local_z(_tenths(params["ze"]) + after), + } + ] + # The 96 tips are clamped on, or pushed off, at the bottom: the measured fixed time is spent there. + return _hold_at_bottom(request, HEAD96_TIP_PICKUP_FIXED if pick_up else HEAD96_TIP_DROP_FIXED) + + +def _head96_liquid(driver: Any, head: Any, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + """`C0 EA` or `C0 ED`: the head's stroke, channel A1 over well A1 (or centred over a container): + up to traverse height, across, down with the tips to the liquid surface the command gives, and + up to where it ends. The liquid itself is the model's, booked by the command.""" + frames = _HeadFrames(driver, head) + aspirate = command == "EA" + request = frames.request("head96_aspirate" if aspirate else "head96_dispense", "C0" + command) + x = _tenths(params["xs"]) * (-1 if int(params["xd"]) else 1) + y = _tenths(params["yh"]) + arm = head.arm + if arm is not None and arm.resource is not None: + a1 = frames.axis_on_deck() + request["arm"] = { + "name": arm.resource.name, + "x": round(float(arm.resource.location.x + x - a1.x), 2), + } + overhang = frames.overhang() + name = frames.resource.name + request["traverse"] = [{"name": name, "z": frames.local_z(_tenths(params["zh"]) + overhang)}] + request["channels"] = [ + { + "name": name, + "channel": 0, + "y": frames.local_y(y), + "down": frames.local_z(_tenths(params["zt"]) + overhang), + "end": frames.local_z(_tenths(params["ze"]) + overhang), + } + ] + # At the bottom: the volume at the command's flow rate, the settling time, and the command's + # measured fixed time (by dispense mode for a dispense). + volume, flow = (params["af"], params["ag"]) if aspirate else (params["df"], params["dg"]) + pumping = _tenths(volume) / max(_tenths(flow), 1e-6) + if aspirate: + fixed = HEAD96_ASPIRATE_FIXED + else: + fixed = HEAD96_DISPENSE_FIXED.get(int(params.get("da", 2)), HEAD96_DISPENSE_FIXED[2]) + _hold_at_bottom(request, fixed) + request["dwell"] = round(request["dwell"] + pumping + _tenths(params["wh"]), 3) + return request + + +def _head96_move(driver: Any, head: Any, command: str, params: Dict[str, Any]) -> Dict[str, Any]: + """`H0 YA` or `H0 ZA`: the head alone, along Y or Z, at the speed the command carries.""" + frames = _HeadFrames(driver, head) + c = head.configuration + request = frames.request("head96_move", head.configuration.module + command) + if command == "YA": + request["moves"].append( + { + "name": frames.resource.name, + "axis": 1, + "to": frames.local_y(c.y_drive_increments_to_mm(int(params["ya"]))), + "speed": c.y_drive_increments_to_mm(int(params["yv"])), + "acceleration": c.y_drive_acceleration_increments_to_mm(int(params["yr"])), + } + ) + else: + request["moves"].append( + { + "name": frames.resource.name, + "axis": 2, + "to": frames.local_z(c.z_drive_increments_to_mm(int(params["za"]))), + "speed": c.z_drive_increments_to_mm(int(params["zv"])), + "acceleration": c.z_drive_acceleration_increments_to_mm(int(params["zr"])), + } + ) + return request + + +def star_motion( + driver: Any, module: str, command: str, params: Dict[str, Any] +) -> Optional[Dict[str, Any]]: + """What one STAR firmware command asks the drives to do, as local targets, or None. + + Args: + driver: the STAR driver the command is going to, which knows the arm and the channels. + module: the module the command is addressed to. + command: the two-letter command code. + params: the command's parameters, as the driver passes them to be assembled. + + Returns: + A motion request for the page: the command's `kind`; the arm's target X; each channel's + targets - `y`, the heights `down` and `end`, and for an aspiration where it `leave`s the liquid + and at what `leave_speed`; the height every channel rises to first (`traverse`); the tips that + change hands at the bottom of the stroke (`attach`: a tip's name and the resource that takes + it, or None to leave it where it is); how long the stroke dwells at the bottom; the command's + `fixed` time beyond its motion; and the drives' speeds (the channels' Y with the `stagger` one + channel starts after another). A tip pick-up's channels also `press` to a height at a + `press_speed`. None for a command that moves nothing, or one this does not read. + """ + if command[0] in ("R", "Q"): + return None + request = _decode(driver, module, command, params) + if request is not None and not request.get("fixed"): + request["fixed"] = _FIXED.get(module + command, SIMPLE_MOVE_FIXED) + return request + + +# The fixed time of the commands whose time does not depend on their parameters; any other command +# the decoder reads is a simple single-drive move (`SIMPLE_MOVE_FIXED`). +_FIXED = { + "C0ZT": CORE_TOOL_PICKUP_FIXED, + "C0ZS": CORE_TOOL_RETURN_FIXED, + "C0ZP": CORE_TOOL_PICKUP_FIXED, + "C0ZR": CORE_TOOL_RETURN_FIXED, + "C0ZA": CHANNELS_UP_FIXED, + "C0EP": HEAD96_TIP_PICKUP_FIXED, + "C0ER": HEAD96_TIP_DROP_FIXED, + "C0EA": HEAD96_ASPIRATE_FIXED, + "C0ED": HEAD96_DISPENSE_FIXED[2], + "H0YA": HEAD96_MOVE_FIXED, + "H0ZA": HEAD96_MOVE_FIXED, +} + + +def _decode( + driver: Any, module: str, command: str, params: Dict[str, Any] +) -> Optional[Dict[str, Any]]: + key = module + command + try: + iswap = _iswap_of(driver) + if iswap is not None and (module == "R0" or key == "C0GC"): + return _iswap_motion(driver, iswap, module, command, params) + head = _head96_of(driver) + if head is not None and key in ("C0EP", "C0ER"): + return _head96_tips(driver, head, command, params) + if head is not None and key in ("C0EA", "C0ED"): + return _head96_liquid(driver, head, command, params) + if head is not None and module == head.configuration.module and command in ("YA", "ZA"): + return _head96_move(driver, head, command, params) + arm = _pipetting_arm(driver) + if arm is None: + return None + frames = _Frames(driver, arm) + if key == "C0TP": + return _tip_pickup(frames, key, params) + if key == "C0TR": + return _tip_drop(frames, key, params) + if key == "C0ZT": + return _core_tool_pickup(frames, key, params) + if key == "C0ZS": + return _core_tool_return(frames, key, params) + if key == "C0ZP": + return _core_plate(frames, key, params, "core_grip") + if key == "C0ZM": + return _core_plate(frames, key, params, "core_move") + if key == "C0ZR": + return _core_plate(frames, key, params, "core_release") + if key == "C0AS": + return _aspirate(frames, key, params) + if key == "C0JY": + return _move_y(frames, key, params) + if key == "C0JZ": + return _move_z(frames, key, params) + if key == "C0FY": + return _free_y_range(frames, key) + if key == "C0ZA": + return _channels_up(frames, key) + if module == "X0" and command in ("XP", "SP"): + return _move_x(frames, driver, command, params) + except (KeyError, IndexError, ValueError, TypeError, RuntimeError): + # A command shaped otherwise than this reads it is not acted out; the model still records + # where it ends. + return None + return None + + +def attach_viewer_motion(driver: Any, viewer: Any) -> None: + """Let a viewer act out what the driver's commands move. + + The driver hands each command to its `motion_listener` before the device answers it (a + `STARSimulationDriver` does); this reads the command for what it moves (`star_motion`) and hands + it to the viewer's `act_out`, which holds the command until the pages have played it. A read + moves nothing and is not worth a message. + + Args: + driver: a simulated STAR driver with a `motion_listener`. + viewer: a `Viewer3D` with an `act_out`. + """ + + async def listener(module: str, command: str, params: Dict[str, Any]) -> None: + motion = star_motion(driver, module, command, params) + if motion is None and command[0] in ("R", "Q"): + return + await viewer.act_out(module + command, motion) + + driver.motion_listener = listener + + +async def _refuse(viewer: Any, origin: str, found: List[Collision]) -> None: + """Draw what a refused command would hit, and refuse it, saying where the refusal came from.""" + if not found: + return + await viewer.show_collisions(found) + raise CollisionError(origin, found) + + +def attach_viewer_collisions(driver: Any, viewer: Any, transport: Any = None) -> None: + """Let a viewer turn the arm's collision checks into gates, and draw what they refuse. + + The driver hands every command that moves something to its `collision_listener` before it is + sent (`STARDriver.send_command` does). When the viewer's `raise_on_collision` is on, the + commands that move the channels or a head are swept where everything stands, and what they + would hit is drawn on the page and raised as a `CollisionError` naming the command: a command + that would hit something never reaches the device. When it is off, nothing is judged here and + the checks stay what they are where they are called. The iSWAP's plans are judged where they + are made: the transport refuses them before its first step, and `transport` is drawn what they + hit first. + + Args: + driver: a STAR driver with a `collision_listener`. + viewer: a `Viewer3D` with `raise_on_collision` and `show_collisions`. + transport: the iSWAP transport to tell what its refused plan would hit, when there is one. + """ + + pipettes = [f for f in driver.features if isinstance(f, Pipettes)] + heads = [f for f in driver.features if isinstance(f, Head)] + + def channel_of(module: str) -> Optional[int]: + return next( + ( + p.channel_from_module(module) for p in pipettes if p.channel_from_module(module) is not None + ), + None, + ) + + def head_of(module: str) -> Optional[Head]: + return next((h for h in heads if h.configuration.module == module), None) + + async def listener(module: str, command: str, params: Dict[str, Any]) -> None: + if not viewer.raise_on_collision: + return + if module == "C0" and command == "JY": + ys = {i: int(field) / 10 for i, field in enumerate(str(params["yp"]).split())} + for p in pipettes: + await _refuse(viewer, "the channels' Y move (C0 JY)", check_pipette_move(p, y=ys)) + elif module == "C0" and command == "JZ": + # What the command takes Z to be is each channel's lowest point - the tip's bottom where one + # is mounted - and the check speaks of the stop disc the drives report: each target is + # lifted by the overhang of the tip the model has on the channel. + zs: Dict[int, float] = {} + for lifted, field in enumerate(params["zp"]): + shaft = next( + ( + child + for child in pipettes[0].resources[lifted].children + if isinstance(child, TipMountingShaft) + ), + None, + ) + bottom = shaft.tip_bottom() if shaft is not None else None + z = int(field) / 10 + if bottom is not None: + z -= bottom.z + zs[lifted] = z + for p in pipettes: + await _refuse(viewer, "the channels' Z move (C0 JZ)", check_pipette_move(p, z=zs)) + else: + channel = channel_of(module) + head = head_of(module) + if command == "ZA" and channel is not None: + z = pipettes[0].configuration.z_drive_increments_to_mm(int(params["za"])) + await _refuse( + viewer, + f"channel {channel}'s Z move ({module} ZA)", + check_pipette_move(pipettes[0], z={channel: z}), + ) + elif head is not None and command == "YA": + y = head.configuration.y_drive_increments_to_mm(int(params["ya"])) + await _refuse(viewer, f"the head's Y move ({module} YA)", check_head_move(head, y=y)) + elif head is not None and command == "ZA": + z = head.configuration.z_drive_increments_to_mm(int(params["za"])) + await _refuse(viewer, f"the head's Z move ({module} ZA)", check_head_move(head, z=z)) + + driver.collision_listener = listener + + if transport is not None: + + async def report(found: List[Collision]) -> None: + if viewer.raise_on_collision: + await viewer.show_collisions(found) + + transport.collision_reporter = report diff --git a/pylabrobot/hamilton/star/motion_tests.py b/pylabrobot/hamilton/star/motion_tests.py new file mode 100644 index 00000000000..685914fdd42 --- /dev/null +++ b/pylabrobot/hamilton/star/motion_tests.py @@ -0,0 +1,555 @@ +"""The motion channel, checked without a browser. + +What a command is read as (`motion.py`), and how the server holds a command until the pages have +played it, against a simulated STAR and pages that are only websocket clients. The player itself is +checked in Node (`motion_player_tests.mjs`), run from here where Node is installed. +""" + +import asyncio +import inspect +import json +import pathlib +import shutil +import subprocess +import time +import unittest +from typing import Any, Dict, List, Optional, Tuple, cast +from unittest import mock + +import websockets + +from pylabrobot.hamilton.star import motion +from pylabrobot.hamilton.star.motion import ( + HEAD96_ASPIRATE_FIXED, + HEAD96_DISPENSE_FIXED, + HEAD96_TIP_DROP_FIXED, + HEAD96_TIP_PICKUP_FIXED, + SIMPLE_MOVE_FIXED, + attach_viewer_motion, + star_motion, +) +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.tip_rack import TipRack +from pylabrobot.resources.tip_tracking import does_tip_tracking, set_tip_tracking +from pylabrobot.visualizer3D.demo import build_facility, fill, star_of +from pylabrobot.visualizer3D.server import Viewer3D +from pylabrobot.visualizer3D.server_tests import free_ports, track_volumes + +HERE = pathlib.Path(__file__).parent +NODE = shutil.which("node") + + +async def simulated_star(test: unittest.TestCase) -> Any: + """The demo facility's STAR, set up, tracking volumes and tips.""" + track_volumes(test) + was_tracking = does_tip_tracking() + set_tip_tracking(True) + test.addCleanup(set_tip_tracking, was_tracking) + facility = build_facility() + star = star_of(facility) + await star.setup() + return facility, star + + +class DecoderTests(unittest.IsolatedAsyncioTestCase): + """A command is read for where it takes the drives, which is where the model then has them.""" + + async def asyncSetUp(self) -> None: + self.facility, self.star = await simulated_star(self) + self.requests: List[Dict[str, Any]] = [] + + async def listen(module: str, command: str, params: Dict[str, Any]) -> None: + request = star_motion(self.star.driver, module, command, params) + if request is not None: + self.requests.append(request) + + self.star.driver.motion_listener = listen + self.rack = self.star.deck.get_resource("tips_0") + self.source = self.star.deck.get_resource("source_0") + assert isinstance(self.rack, TipRack) and isinstance(self.source, Plate) + fill(self.source, 300.0) + + def assert_ends_where_the_model_is(self, request: Dict[str, Any]) -> None: + arm = self.star.x_arm.resource + if request["arm"] is not None: + self.assertAlmostEqual(request["arm"]["x"], arm.location.x, delta=0.1, msg="arm") + channels = {c.name: c for c in self.star.pipettes.resources} + for target in request["channels"]: + at = channels[target["name"]].location + if target.get("y") is not None: + self.assertAlmostEqual(target["y"], at.y, delta=0.1, msg=f"{target['name']} y") + if target.get("end") is not None: + self.assertAlmostEqual(target["end"], at.z, delta=0.1, msg=f"{target['name']} z") + + async def test_each_command_ends_where_the_model_records_it_ending(self): + spots = [self.rack.get_item(f"{row}1") for row in "ABCDEFGH"] + wells = [self.source.get_item(f"{row}1") for row in "ABCDEFGH"] + for operation, kind in ( + (lambda: self.star.pipettes.pick_up_tips(spots), "tip_pickup"), + (lambda: self.star.pipettes.aspirate(wells, [50.0] * 8), "aspirate"), + (lambda: self.star.pipettes.drop_tips(spots), "tip_drop"), + ): + self.requests.clear() + await operation() + stroke = [r for r in self.requests if r["kind"] == kind] + self.assertEqual(len(stroke), 1, f"{kind}: {[r['kind'] for r in self.requests]}") + self.assert_ends_where_the_model_is(stroke[0]) + + async def test_heights_with_a_tip_come_from_the_model_not_the_simulator(self): + """On a real STAR there is no simulator to ask how far a tip hangs below the stop disc. + Read from it, every height with a tip on was off by the tip's length.""" + pipettes = self.star.pipettes + simulators_own = type(pipettes)._below_stop_disc + + def not_from_the_decoder(this: Any, channel: int) -> float: + # The simulator keeps its own model with it; only the decoder must not lean on it. + if inspect.stack()[1].filename == motion.__file__: + raise AssertionError("the decoder asked the simulator how long the tip is") + return float(simulators_own(this, channel)) + + with mock.patch.object(type(pipettes), "_below_stop_disc", not_from_the_decoder): + with mock.patch.object(self.star.driver, "defined_tip_lengths", {}, create=True): + await self.test_each_command_ends_where_the_model_records_it_ending() + + async def test_the_tips_change_hands_where_the_model_then_has_them(self): + """Placed anywhere else, the tip jumps when the model's own move reaches the page.""" + spots = [self.rack.get_item(f"{row}2") for row in "AB"] + tips = [spot.tip for spot in spots] + await self.star.pipettes.pick_up_tips(spots) + pick_up = next(r for r in self.requests if r["kind"] == "tip_pickup") + shafts = [c.children[0].name for c in self.star.pipettes.resources[:2]] + self.assertEqual( + [(a["name"], a["parent"]) for a in pick_up["attach"]], + [(t.name, s) for t, s in zip(tips, shafts)], + ) + for handover, tip in zip(pick_up["attach"], tips): + self.assertEqual(tip.parent.name, handover["parent"]) + for axis in "xyz": + self.assertAlmostEqual(handover["location"][axis], getattr(tip.location, axis), 6) + + await self.star.pipettes.drop_tips(spots) + drop = next(r for r in self.requests if r["kind"] == "tip_drop") + self.assertEqual( + [(a["name"], a["parent"]) for a in drop["attach"]], + [(t.name, s.name) for t, s in zip(tips, spots)], + ) + for handover, tip in zip(drop["attach"], tips): + for axis in "xyz": + self.assertAlmostEqual(handover["location"][axis], getattr(tip.location, axis), 6) + + async def test_a_dropped_tip_is_let_go_of_down_in_its_spot(self): + """`DROP` heights are the stop disc's: taken as the tip's bottom, the channel stops a tip's + length short and lets go of it in the air above the rack.""" + spot = self.rack.get_item("A4") + tip = spot.tip + await self.star.pipettes.pick_up_tips([spot]) + channel, shaft = self.star.pipettes.resources[0], self.star.pipettes.resources[0].children[0] + carried = tip.get_absolute_location() - shaft.get_absolute_location() + await self.star.pipettes.drop_tips([spot]) + stroke = [r for r in self.requests if r["kind"] == "tip_drop"][-1]["channels"][0] + # Where the tip is at the bottom of the stroke, and where the model then rests it. + bottom = shaft.get_absolute_location().z - (stroke["end"] - stroke["down"]) + self.assertAlmostEqual(bottom + carried.z, tip.get_absolute_location().z, delta=0.5) + self.assertEqual(channel.location.z, stroke["end"]) + + async def test_an_aspiration_dwells_as_long_as_its_volume_takes(self): + await self.star.pipettes.pick_up_tips([self.rack.get_item("A3")]) + await self.star.pipettes.aspirate([self.source.get_item("A3")], [100.0]) + aspirate = next(r for r in self.requests if r["kind"] == "aspirate") + self.assertGreater(aspirate["dwell"], 0.0) + # The drives move as a STAR's own timings say, where PLR's defaults are slower. + self.assertEqual(aspirate["drives"]["z"]["speed"], motion.CHANNEL_Z_SPEED) + self.assertEqual(aspirate["drives"]["y"]["speed"], motion.CHANNEL_Y_SPEED) + self.assertEqual(aspirate["drives"]["y"]["stagger"], motion.CHANNEL_Y_STAGGER) + + async def test_every_command_takes_its_fixed_time_as_well(self): + spots = [self.rack.get_item(f"{row}5") for row in "ABCD"] + await self.star.pipettes.pick_up_tips(spots) + await self.star.pipettes.aspirate( + [self.source.get_item(f"{row}5") for row in "ABCD"], [50.0] * 4 + ) + await self.star.pipettes.drop_tips(spots) + by_kind = {r["kind"]: r for r in self.requests} + pickup = by_kind["tip_pickup"] + self.assertAlmostEqual(pickup["fixed"] + pickup["dwell"], motion.TIP_PICKUP_FIXED, places=3) + self.assertGreater(pickup["dwell"], 1.0) + self.assertGreaterEqual(by_kind["aspirate"]["fixed"] + 0.001, motion.ASPIRATE_FIXED) + # A drop spends its fixed time at the bottom, holding while the tips are pushed off. + drop = by_kind["tip_drop"] + self.assertAlmostEqual(drop["fixed"] + drop["dwell"], motion.TIP_DROP_FIXED, places=3) + self.assertGreater(drop["dwell"], 2.5) + for request in self.requests: + self.assertGreater(request["fixed"], 0.0, request["kind"]) + + async def test_a_tip_pick_up_presses_the_last_stretch_slowly(self): + await self.star.pipettes.pick_up_tips([self.rack.get_item("A6")]) + channel = next(r for r in self.requests if r["kind"] == "tip_pickup")["channels"][0] + self.assertLess(channel["press"], channel["down"]) + self.assertEqual(channel["press_speed"], motion.TIP_PRESS_SPEED) + + async def test_an_aspiration_follows_the_surface_and_pulls_out(self): + """Immersion, its direction, surface following and the pull-out, read off the command.""" + sent: List[Dict[str, Any]] = [] + listen = self.star.driver.motion_listener + + async def keep(module: str, command: str, params: Dict[str, Any]) -> None: + if module + command == "C0AS": + sent.append(dict(params)) + await listen(module, command, params) + + self.star.driver.motion_listener = keep + await self.star.pipettes.pick_up_tips([self.rack.get_item("A7")]) + await self.star.pipettes.aspirate([self.source.get_item("A7")], [100.0]) + params = sent[-1] + + # PLR sends the lowest allowed height as the surface itself; room below it, so immersion shows. + floor = [f"{int(z) - 200:04}" for z in params["zl"]] + + def decoded(**changed: Any) -> Dict[str, Any]: + request = star_motion(self.star.driver, "C0", "AS", {**params, "zx": floor, **changed}) + assert request is not None + return cast(Dict[str, Any], request["channels"][0]) + + plain = decoded(ip=["0020"], it=["0"], fp=["0000"], po=["0000"]) + deeper = decoded(ip=["0050"], it=["0"], fp=["0000"], po=["0000"]) + above = decoded(ip=["0050"], it=["1"], fp=["0000"], po=["0000"]) + self.assertAlmostEqual(plain["down"] - deeper["down"], 3.0, places=2) + self.assertAlmostEqual(above["down"] - deeper["down"], 10.0, places=2) + self.assertNotIn("follow", plain) + self.assertNotIn("pull_out", plain) + + following = decoded(ip=["0020"], it=["0"], fp=["0040"], po=["0100"]) + self.assertAlmostEqual(following["down"] - following["follow"], 4.0, places=2) + self.assertGreater(following["follow_speed"], 0.0) + self.assertAlmostEqual(following["pull_out"] - following["leave"], 10.0, places=2) + + async def test_a_read_moves_nothing(self): + self.assertIsNone(star_motion(self.star.driver, "C0", "RY", {})) + self.assertIsNone(star_motion(self.star.driver, "C0", "XX", {})) + + +class Head96DecoderTests(unittest.IsolatedAsyncioTestCase): + """A 96-head tip command is read for where it takes the arm and the head, and which tips move.""" + + async def test_a_rack_is_picked_up_and_put_back_where_the_model_then_has_it(self): + facility, star = await simulated_star(self) + head = star.driver.arms[0].head96 + requests: List[Dict[str, Any]] = [] + + async def listen(module: str, command: str, params: Dict[str, Any]) -> None: + request = star_motion(star.driver, module, command, params) + if request is not None: + requests.append(request) + + star.driver.motion_listener = listen + rack = star.deck.get_resource("tips_0") + tips = [spot.tip for spot in rack.get_all_items()] + for operation in (head.pick_up_tips, head.drop_tips): + requests.clear() + await operation(rack) + stroke = [r for r in requests if r["kind"].startswith("head96_tip")][-1] + self.assertAlmostEqual(stroke["arm"]["x"], star.x_arm.resource.location.x, delta=0.05) + target = stroke["channels"][0] + self.assertAlmostEqual(target["y"], head.resource.location.y, delta=0.05) + self.assertAlmostEqual(target["end"], head.resource.location.z, delta=0.05) + self.assertEqual(len(stroke["attach"]), 96) + for handover, tip in zip(stroke["attach"], tips): + self.assertEqual(handover["name"], tip.name) + self.assertEqual(handover["parent"], tip.parent.name) + for axis in "xyz": + self.assertAlmostEqual(handover["location"][axis], getattr(tip.location, axis), 6) + + async def test_the_tips_are_clamped_at_the_bottom_not_waited_for_before_the_head_moves(self): + facility, star = await simulated_star(self) + head = star.driver.arms[0].head96 + requests: List[Dict[str, Any]] = [] + + async def listen(module: str, command: str, params: Dict[str, Any]) -> None: + request = star_motion(star.driver, module, command, params) + if request is not None: + requests.append(request) + + star.driver.motion_listener = listen + rack = star.deck.get_resource("tips_0") + for operation, fixed in ( + (head.pick_up_tips, HEAD96_TIP_PICKUP_FIXED), + (head.drop_tips, HEAD96_TIP_DROP_FIXED), + ): + requests.clear() + await operation(rack) + stroke = [r for r in requests if r["kind"].startswith("head96_tip")][-1] + self.assertEqual(stroke["fixed"], SIMPLE_MOVE_FIXED) + self.assertAlmostEqual(stroke["dwell"] + stroke["fixed"], fixed, 3) + + async def test_an_aspirate_and_a_dispense_take_their_pumping_and_measured_time_at_the_bottom( + self, + ): + facility, star = await simulated_star(self) + head = star.driver.arms[0].head96 + requests: List[Tuple[str, Dict[str, Any], Dict[str, Any]]] = [] + + async def listen(module: str, command: str, params: Dict[str, Any]) -> None: + request = star_motion(star.driver, module, command, params) + if request is not None: + requests.append((command, params, request)) + + await head.pick_up_tips(star.deck.get_resource("tips_0")) + plate = star.deck.get_resource("source_1") + for well in plate.get_all_items(): + well.set_volume(100.0) + star.driver.motion_listener = listen + await head.aspirate(plate, 50.0) + await head.dispense(plate, 50.0) + self.assertEqual(sorted(c for c, _, _ in requests if c in ("EA", "ED")), ["EA", "ED"]) + for command, params, request in requests: + if command not in ("EA", "ED"): + continue + volume, flow = ( + (params["af"], params["ag"]) if command == "EA" else (params["df"], params["dg"]) + ) + pumping = int(volume) / int(flow) + measured = ( + HEAD96_ASPIRATE_FIXED if command == "EA" else HEAD96_DISPENSE_FIXED[int(params["da"])] + ) + settling = int(params["wh"]) / 10 + self.assertEqual(request["fixed"], SIMPLE_MOVE_FIXED) + self.assertAlmostEqual(request["dwell"] + request["fixed"], pumping + settling + measured, 2) + + +class FakePage: + """A page that is only a websocket: it plays each motion by waiting `delay` seconds.""" + + def __init__(self, viewer: Viewer3D, delay: Optional[float]): + self.viewer = viewer + self.delay = delay # None: never says it is done + self.events: List[str] = [] + self._socket: Any = None + self._task: Optional["asyncio.Task[None]"] = None + + async def open(self) -> "FakePage": + self._socket = await websockets.connect(self.viewer.ws_url, max_size=None) + await self._socket.send(json.dumps({"event": "hello", "data": {"backend": "none"}})) + self._task = asyncio.ensure_future(self._listen()) + while "scene" not in self.events: + await asyncio.sleep(0.01) + return self + + async def _listen(self) -> None: + try: + async for message in self._socket: + parsed = json.loads(message) + kind, data = parsed["event"], parsed["data"] + self.events.append(kind) + if kind == "state" and data.get("locations"): + self.events.append("locations") + self.events.extend(f"at:{name}" for name in data["locations"]) + if kind == "motion" and self.delay is not None: + asyncio.ensure_future(self._play(data["id"])) + except websockets.ConnectionClosed: + pass + + async def _play(self, motion_id: int) -> None: + await asyncio.sleep(self.delay or 0.0) + await self._socket.send(json.dumps({"event": "motion_done", "data": {"id": motion_id}})) + + async def close(self) -> None: + await self._socket.close() + if self._task is not None: + await self._task + + +class ServerTests(unittest.IsolatedAsyncioTestCase): + """A command waits until every page has played it, and for nothing when there is nobody.""" + + async def asyncSetUp(self) -> None: + self.facility, self.star = await simulated_star(self) + fs_port, ws_port = free_ports(2) + self.viewer = Viewer3D(self.facility, open_browser=False, fs_port=fs_port, ws_port=ws_port) + await self.viewer.start() + attach_viewer_motion(self.star.driver, self.viewer) + self.addAsyncCleanup(self.viewer.stop) + self.rack = self.star.deck.get_resource("tips_0") + assert isinstance(self.rack, TipRack) + + async def page(self, delay: Optional[float]) -> FakePage: + page = await FakePage(self.viewer, delay).open() + self.addAsyncCleanup(page.close) + return page + + async def timed_pick_up(self, well: str = "A1") -> float: + began = time.monotonic() + await self.star.pipettes.pick_up_tips([self.rack.get_item(well)]) + return time.monotonic() - began + + async def test_a_command_waits_for_the_page(self): + page = await self.page(0.5) + self.assertGreaterEqual(await self.timed_pick_up(), 0.5) + self.assertIn("motion", page.events) + + async def test_the_slowest_page_sets_the_pace(self): + await self.page(0.1) + await self.page(0.6) + self.assertGreaterEqual(await self.timed_pick_up(), 0.6) + + async def test_with_no_page_nothing_waits(self): + self.assertLess(await self.timed_pick_up(), 0.5) + + async def test_a_page_that_leaves_lets_the_command_go(self): + page = await self.page(None) # never answers + asyncio.get_running_loop().call_later(0.3, lambda: asyncio.ensure_future(page.close())) + self.assertLess(await self.timed_pick_up(), 5.0) + + async def test_a_page_that_leaves_while_a_motion_is_sent_lets_it_go_once(self): + """A reload closes the page while the motion is still being sent to it: letting the page go + releases the motion, and the send finding nobody left must not release it again.""" + await self.page(None) # never answers + send = self.viewer._broadcast + + async def send_then_leave(event: str, data: Any) -> None: + await send(event, data) + if event == "motion": + for websocket in list(self.viewer._clients): + self.viewer._clients.discard(websocket) + self.viewer._release_motions(websocket) + + self.viewer._broadcast = send_then_leave # type: ignore[method-assign] + self.assertLess(await self.timed_pick_up(), 5.0) + + async def test_a_motion_starts_from_where_the_last_command_left_things(self): + """The model moves when a command ends; the next motion is played from there, so what that + move changed has to reach the page first.""" + page = await self.page(0.05) + await self.timed_pick_up("A1") + await self.star.pipettes.drop_tips([self.rack.get_item("A1")]) + motions = [i for i, kind in enumerate(page.events) if kind == "motion"] + self.assertGreaterEqual(len(motions), 2) + between = page.events[motions[0] + 1 : motions[-1]] + # The pick-up took a tip onto a shaft, a change of shape, which carries the positions with it. + self.assertIn("moves", between, f"the pick-up reached the page too late: {page.events}") + + +class ISWAPDecoderTests(unittest.IsolatedAsyncioTestCase): + """An iSWAP command is read for the drives it moves, which is where the model then has them.""" + + async def asyncSetUp(self) -> None: + self.facility, self.star = await simulated_star(self) + self.iswap = self.star.iswap + self.requests: List[Dict[str, Any]] = [] + + async def listen(module: str, command: str, params: Dict[str, Any]) -> None: + request = star_motion(self.star.driver, module, command, params) + if request is not None: + self.requests.append(request) + + self.star.driver.motion_listener = listen + await self.iswap.make_space() + self.parked = await self.iswap.elbow_request_y_position() + + def last(self, kind: str) -> Dict[str, Any]: + return [r for r in self.requests if r["kind"] == kind][-1] + + async def test_the_head_moves_where_the_model_puts_it(self): + await self.iswap.elbow_move_to_y_position(self.parked - 150.0) + await self.iswap.elbow_move_to_z_position(250.0) + head = self.iswap.resource + moves = [r["moves"][0] for r in self.requests if r["kind"] == "iswap_move"] + along_y, along_z = moves[-2], moves[-1] + self.assertEqual((along_y["axis"], along_z["axis"]), (1, 2)) + self.assertAlmostEqual(along_y["to"], head.location.y, delta=0.05) + self.assertAlmostEqual(along_z["to"], head.location.z, delta=0.05) + self.assertGreater(along_y["speed"], 0) + + async def test_a_joint_turns_to_the_rotation_the_model_gives_it(self): + await self.iswap.elbow_move_to_y_position(self.parked - 200.0) + await self.iswap.rotate_to_angles(elbow_absolute_angle="front", gripper_absolute_angle="left") + turns = {t["name"]: t for t in self.last("iswap_turn")["turns"]} + for resource in (self.iswap.link_1, self.iswap.gripper): + turn = turns[resource.name] + self.assertAlmostEqual((turn["drive"] - turn["base"]) % 360, resource.rotation.z % 360, 3) + self.assertEqual(turn["pivot"]["x"], resource.proximal_joint.x) + self.assertGreater(turn["speed"], 0) + + async def test_the_fingers_stand_where_the_model_stands_them(self): + await self.iswap.gripper_move_to_jaw_position(90.0) + jaws = self.last("iswap_jaws")["jaws"] + for finger, target in zip(self.iswap.gripper.fingers, jaws["fingers"]): + self.assertEqual(target["name"], finger.name) + self.assertAlmostEqual(target["y"], finger.location.y, delta=0.01) + self.assertEqual(jaws["gripper"], self.iswap.gripper.name) + + +class Head96OffsetAndLiquidDecoderTests(unittest.IsolatedAsyncioTestCase): + """The 96-head's commands, read for what they move and which tips change hands.""" + + async def asyncSetUp(self) -> None: + self.facility, self.star = await simulated_star(self) + self.head = self.star.head96 + self.rack = self.star.deck.get_resource("tips_1") + self.requests: List[Dict[str, Any]] = [] + + async def listen(module: str, command: str, params: Dict[str, Any]) -> None: + request = star_motion(self.star.driver, module, command, params) + if request is not None: + self.requests.append(request) + + self.star.driver.motion_listener = listen + + async def test_an_aspiration_goes_down_to_the_liquid_surface(self): + # The 96-head has no liquid class for the demo rack's filtered 1000 uL tips, and the + # aspirate refuses without one; the decoder only needs the corrected volume, so the class + # the 300 uL head uses stands in. + from pylabrobot.hamilton.star.liquid_classes.mapping import star_mapping + from pylabrobot.resources.liquid import Liquid + + liquid_class = star_mapping[(300, True, True, False, Liquid.WATER, False, False)] + await self.head.pick_up_tips(self.rack) + plate = self.star.deck.get_resource("source_1") + fill(plate, 200.0) + await self.head.aspirate(plate, 50.0, liquid_height=2.0, hamilton_liquid_class=liquid_class) + request = [r for r in self.requests if r["kind"] == "head96_aspirate"][-1] + self.assertEqual(request["command"], "C0EA") + channel = request["channels"][0] + self.assertLess(channel["down"], channel["end"]) + + +class ISWAPServerTests(unittest.IsolatedAsyncioTestCase): + async def test_a_move_the_model_records_first_is_played_before_it_is_told(self): + """The iSWAP writes a move's target before sending it. Told first, the page would put the head + at the end of the move and have nothing left to play.""" + facility, star = await simulated_star(self) + fs_port, ws_port = free_ports(2) + viewer = Viewer3D(facility, open_browser=False, fs_port=fs_port, ws_port=ws_port) + await viewer.start() + self.addAsyncCleanup(viewer.stop) + attach_viewer_motion(star.driver, viewer) + await star.iswap.make_space() + y = await star.iswap.elbow_request_y_position() + page = await FakePage(viewer, 0.05).open() + self.addAsyncCleanup(page.close) + head = f"at:{star.iswap.resource.name}" + before = len(page.events) + await star.iswap.elbow_move_to_y_position(y - 100.0) + for _ in range(100): + if head in page.events[before:]: + break + await asyncio.sleep(0.02) + after = page.events[before:] + self.assertIn("motion", after) + self.assertIn(head, after) + self.assertLess(after.index("motion"), after.index(head), after) + + +@unittest.skipUnless(NODE, "no Node to run the player's tests") +class PlayerTests(unittest.TestCase): + def test_the_player(self): + result = subprocess.run( + [str(NODE), "--test", str(HERE.parent.parent / "visualizer3D" / "motion_player_tests.mjs")], + capture_output=True, + text=True, + timeout=120, + ) + self.assertEqual(result.returncode, 0, result.stdout[-3000:] + result.stderr[-2000:]) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/hamilton/star/resource_model/hamilton_legacy_star_dual_rail_arm.collision.json b/pylabrobot/hamilton/star/resource_model/hamilton_legacy_star_dual_rail_arm.collision.json new file mode 100644 index 00000000000..5d6b13dd278 --- /dev/null +++ b/pylabrobot/hamilton/star/resource_model/hamilton_legacy_star_dual_rail_arm.collision.json @@ -0,0 +1 @@ +{"model":"hamilton_legacy_star_dual_rail_arm","units":"mm","frame":"the model file's: the resource's front-left-bottom corner, Z up","source":{"file":"hamilton_legacy_star_dual_rail_arm.glb","sha256":"71f02e90d018d2333dad2e50e42c4889913b54810e2dba2cf8ebebc14ae34bdb"},"method":"CoACD, threshold 0.05, at most 8 hulls, rest pose","hulls":[[[48.555,707.427,-242.87],[48.555,711.331,-242.652],[105.445,707.427,-242.87],[339.997,707.422,-242.87],[48.555,671.891,-242.833],[44.457,707.427,-242.652],[44.658,711.212,-242.58],[48.555,713.286,-238.888],[396.847,711.331,-242.652],[396.847,707.422,-242.87],[48.555,668.067,-242.616],[44.698,668.156,-242.544],[44.538,671.891,-242.616],[396.847,671.891,-242.833],[339.997,668.062,-242.616],[41.805,707.427,-238.888],[43.453,712.132,-238.888],[48.555,714.892,-231.793],[396.847,713.286,-238.888],[400.825,711.212,-242.58],[401.066,707.422,-242.652],[48.555,666.44,-238.888],[43.573,667.39,-238.888],[41.926,671.891,-238.888],[400.986,671.891,-242.616],[400.785,668.156,-242.544],[396.847,668.062,-242.616],[39.636,707.427,-231.793],[42.448,712.933,-231.793],[-0.26,712.425,9.947],[5.928,713.892,45.459],[48.555,714.892,-224.661],[396.847,714.892,-231.793],[402.151,712.132,-238.888],[404.039,707.422,-238.888],[48.555,665.088,-231.793],[396.847,666.44,-238.888],[339.997,665.083,-231.793],[42.609,666.709,-231.793],[39.756,671.891,-231.793],[403.919,671.891,-238.888],[402.07,667.385,-238.888],[-3.715,676.372,9.947],[-3.715,693.213,9.947],[-3.675,700.31,9.947],[-3.313,707.417,9.947],[-3.353,707.417,17.042],[-0.26,712.425,116.555],[5.928,713.892,80.971],[41.484,713.892,116.555],[396.847,714.892,-224.661],[403.276,712.928,-231.793],[406.369,707.422,-231.793],[318.463,664.088,-210.435],[126.739,664.093,-210.435],[55.666,664.148,-210.435],[48.555,664.69,-210.435],[396.847,665.083,-231.793],[406.249,671.891,-231.793],[403.155,666.709,-231.793],[-3.755,676.372,95.197],[-3.835,693.213,38.4],[-3.715,700.31,17.042],[-3.353,707.417,116.555],[-3.795,700.31,38.4],[-3.835,700.31,88.102],[0.624,711.948,121.659],[5.928,712.425,122.455],[6.048,712.719,116.446],[41.484,712.425,123.107],[48.555,712.425,123.143],[297.249,712.43,123.179],[311.552,713.892,116.555],[389.696,713.892,73.876],[396.847,713.748,73.876],[401.749,712.306,73.876],[403.276,712.928,-224.661],[406.369,707.422,-224.661],[406.57,700.315,-224.661],[406.61,678.998,-224.661],[318.663,664.088,-210.435],[359.644,669.285,88.283],[55.666,664.307,-203.304],[48.555,664.824,-203.304],[-3.715,676.372,116.555],[396.847,664.451,-210.435],[389.736,664.138,-210.435],[406.249,671.891,-224.661],[403.155,666.709,-224.661],[-3.835,693.213,88.102],[-3.715,700.31,116.555],[-0.501,707.417,122.709],[5.928,707.417,125.279],[41.484,707.417,126.437],[48.555,707.417,126.546],[133.891,707.417,126.582],[347.109,712.43,123.179],[190.379,707.422,126.582],[396.847,713.713,80.971],[389.696,713.828,80.971],[401.709,712.276,80.971],[403.195,707.417,80.971],[403.396,700.305,73.876],[403.396,693.213,73.876],[403.316,693.213,80.971],[396.847,664.595,-203.304],[359.724,669.285,88.102],[391.624,676.581,81.622],[389.375,676.581,86.183],[353.898,676.576,123.433],[351.608,676.576,124.555],[347.109,676.576,126.727],[126.819,676.372,126.835],[98.334,676.372,126.799],[5.928,676.372,125.749],[-0.661,676.372,122.926],[402.874,666.624,-217.53],[403.316,686.12,38.4],[403.075,686.12,52.59],[402.713,686.12,66.781],[401.588,681.43,52.59],[-0.661,700.31,122.926],[5.928,700.31,125.713],[41.484,700.31,126.763],[48.555,700.31,126.872],[311.552,707.422,126.582],[347.109,707.422,126.51],[352.653,711.948,122.166],[401.267,711.833,85.459],[403.316,700.305,80.971],[401.749,707.417,85.966],[401.789,693.213,86.075],[401.187,686.12,85.423],[402.191,686.12,80.971],[402.472,686.12,73.876],[402.271,666.55,-210.435],[401.829,666.908,-189.113],[401.669,667.097,-182.018],[401.508,667.296,-174.887],[401.146,667.714,-160.661],[400.664,668.141,-146.434],[400.463,668.276,-141.946],[391.906,676.581,80.971],[400.544,682.45,84.699],[353.898,700.31,123.433],[347.109,700.31,126.727],[318.623,700.31,126.872],[318.623,676.576,126.835],[48.555,693.213,126.872],[41.484,693.213,126.799],[5.928,693.213,125.749],[401.428,681.629,59.685],[353.778,707.422,123.324],[401.789,700.305,86.075],[400.745,682.266,80.971],[401.187,674.786,-46.994],[400.504,675.363,-27.881]],[[12.964,103.173,131.224],[12.964,60.489,131.224],[34.284,676.364,125.831],[12.964,642.647,131.219],[6.004,642.647,129.319],[6.004,67.624,129.319],[6.008,60.489,129.319],[6.498,53.354,128.94],[12.964,53.354,130.538],[34.284,31.318,125.811],[39.073,676.364,124.32],[5.859,676.364,125.759],[12.969,657.611,130.246],[12.969,650.539,131.214],[6.013,650.539,129.319],[5.625,642.647,129.089],[2.451,67.624,127.122],[2.451,60.489,127.117],[2.946,53.354,126.917],[-0.737,31.318,122.968],[5.854,31.318,125.765],[40.511,31.318,123.019],[41.23,676.364,123.311],[3.422,676.364,124.73],[6.662,657.611,128.792],[-0.737,676.364,122.963],[2.456,650.539,127.117],[2.451,642.647,127.122],[-0.737,67.624,123.567],[-0.737,60.489,123.562],[-3.758,31.318,116.581],[42.799,31.318,117.278],[43.863,676.364,116.576],[-1.853,676.364,120.607],[-0.737,650.539,123.562],[-0.737,642.647,123.567],[-3.809,39.211,95.258],[-3.8,31.318,95.258],[-3.758,642.647,116.581],[43.065,31.318,116.581],[43.882,676.364,109.881],[-3.758,676.364,116.576],[-3.744,642.647,88.144],[-3.744,39.211,88.149],[-3.809,664.746,95.258],[-3.79,31.318,94.582],[43.069,31.318,109.467],[43.882,676.364,95.258],[-3.79,676.364,95.258],[-3.744,664.746,88.144],[-0.607,676.364,81.521],[-3.375,31.318,87.463],[-3.66,31.318,88.149],[43.069,31.318,95.258],[43.835,676.364,88.569],[-3.674,676.364,88.144],[5.859,676.364,79.099],[0.266,31.318,81.987],[-0.593,31.318,82.315],[-0.658,39.211,82.346],[42.752,31.318,88.149],[42.817,46.22,88.149],[43.83,676.364,88.144],[12.969,676.364,78.899],[5.854,31.318,80.01],[40.231,31.318,82.387],[40.282,46.22,82.346],[41.375,676.364,81.147],[41.907,676.364,82.377],[34.284,676.364,78.899],[12.964,31.318,79.862],[6.825,31.318,79.985],[35.185,31.318,80.389],[34.284,31.318,80.118],[28.14,31.318,79.882],[27.174,31.318,79.862]],[[119.603,31.445,127.582],[105.372,31.445,127.582],[84.283,31.318,79.77],[120.999,31.445,127.559],[126.701,676.301,126.853],[98.274,676.301,126.794],[98.266,31.445,127.465],[54.64,31.318,79.77],[80.85,31.318,16.455],[140.11,31.445,88.139],[140.795,31.445,116.565],[140.572,31.445,117.389],[139.168,31.445,122.05],[133.175,31.445,126.04],[126.701,31.445,127.37],[131.188,676.301,125.805],[85.345,676.364,78.828],[53.647,676.364,78.828],[55.685,676.301,110.032],[95.919,31.445,127.265],[54.64,31.318,24.165],[58.048,31.445,111.351],[56.892,31.445,110.021],[54.64,31.445,102.37],[53.535,676.364,17.056],[80.344,31.318,15.761],[138.492,31.445,83.501],[140.247,676.301,83.901],[141.009,676.301,87.88],[141.009,676.301,90.811],[140.547,676.301,116.577],[139.99,676.301,118.495],[138.954,676.301,121.815],[137.088,676.301,123.051],[133.808,676.301,125.181],[85.456,676.364,17.056],[53.544,676.364,38.384],[53.63,676.301,102.358],[55.385,676.301,109.044],[54.657,31.318,23.212],[55.548,676.364,9.817],[54.845,31.318,17.056],[77.467,31.318,12.1],[83.838,676.364,10.088],[139.682,676.301,82.301],[83.915,676.364,10.335],[55.839,676.364,9.711],[57.029,31.318,11.535],[55.102,31.318,16.244],[76.885,31.318,11.512],[76.954,676.364,7.686],[62.552,676.364,7.38],[62.731,31.318,9.758],[70.849,31.318,9.982],[69.838,676.364,7.333],[69.838,31.318,9.758],[69.658,676.364,7.333]],[[388.458,124.471,86.249],[384.354,124.471,86.249],[388.458,32.182,86.249],[389.418,676.501,86.194],[388.461,671.833,86.241],[384.329,32.182,86.249],[384.354,671.833,86.241],[369.189,642.626,86.186],[390.884,32.182,81.027],[391.963,676.501,81.019],[391.669,676.501,81.647],[387.935,676.501,86.194],[369.189,32.182,86.186],[368.415,676.501,86.162],[369.189,671.833,86.178],[391.076,32.182,73.92],[392.189,676.501,73.913],[365.848,32.182,81.027],[363.775,676.501,81.2],[391.192,32.182,24.16],[392.573,676.501,24.168],[365.743,32.182,74.046],[363.718,676.501,47.874],[391.023,32.182,17.054],[392.537,676.501,17.054],[366.023,32.182,24.16],[363.718,673.852,17.321],[363.718,673.852,17.054],[363.718,674.482,12.209],[363.718,674.987,10.481],[363.718,676.501,8.911],[388.481,32.182,11.738],[389.978,676.501,9.735],[366.237,32.182,17.054],[366.246,676.501,6.429],[369.302,32.182,11.738],[382.671,32.182,9.971],[382.668,676.501,6.037],[373.338,676.501,6.037],[375.564,32.182,9.971]],[[304.461,10.676,1.486],[304.461,17.762,1.882],[311.594,17.762,1.869],[361.957,17.772,1.857],[389.77,17.772,1.857],[396.879,10.669,1.56],[396.879,3.556,1.597],[304.461,3.563,1.572],[297.352,3.563,3.021],[297.352,10.676,2.996],[297.352,17.762,3.367],[297.352,22.16,5.076],[304.461,22.608,4.395],[375.552,22.9,4.37],[396.879,17.772,1.906],[382.661,22.9,4.37],[402.188,17.772,4.184],[402.396,10.669,3.986],[402.396,3.556,3.986],[401.726,-1.301,4.778],[396.879,-1.953,3.974],[361.957,-1.953,3.974],[304.461,-1.942,3.986],[297.352,-1.308,4.729],[168.581,-1.685,47.562],[168.581,3.559,46.089],[168.581,10.666,46.089],[168.581,11.463,46.114],[168.581,17.772,46.324],[293.59,21.574,5.967],[168.581,22.502,47.81],[369.297,32.119,11.748],[375.437,32.119,10.015],[375.552,32.119,10.003],[396.879,22.629,4.37],[401.472,22.184,5.051],[389.77,22.773,4.37],[382.661,32.119,10.003],[403.942,17.772,9.965],[404.358,10.669,9.965],[404.358,3.556,9.965],[396.879,-3.91,9.965],[402.396,-1.953,9.965],[361.957,-2.126,4.494],[168.581,-3.639,52.613],[168.581,-3.42,52.031],[304.461,-3.903,9.965],[168.581,23.793,52.613],[307.324,32.116,83.884],[369.228,32.119,11.835],[388.362,32.119,11.711],[388.454,32.119,11.748],[390.993,32.119,17.059],[402.003,22.629,9.965],[403.942,17.772,73.906],[402.003,22.646,17.059],[404.358,10.669,73.906],[404.381,3.556,73.906],[396.879,-3.91,73.906],[361.957,-3.91,9.965],[402.396,-1.953,73.906],[168.581,-3.91,59.719],[168.581,23.807,53.393],[306.008,32.116,88.105],[366.227,32.119,17.059],[391.178,32.119,24.177],[402.003,22.646,73.906],[401.957,22.622,81.011],[403.85,17.772,81.011],[404.219,10.669,81.011],[404.219,3.556,81.011],[402.326,-1.875,81.011],[396.879,-3.752,81.011],[311.594,-3.903,116.553],[288.951,-3.91,116.578],[168.581,-3.91,116.578],[168.581,23.905,59.719],[305.431,32.116,116.553],[365.996,32.119,24.177],[391.063,32.119,73.906],[390.878,32.119,81.011],[401.426,22.17,85.493],[402.096,17.772,86.087],[402.234,10.669,86.236],[402.234,3.556,86.236],[401.634,-1.195,85.679],[352.817,-1.294,122.297],[347.093,-1.95,123.399],[288.951,-1.953,123.399],[176.498,-1.953,123.399],[168.581,-1.674,124.241],[168.581,23.905,116.578],[305.431,32.116,116.627],[365.742,32.119,73.906],[388.431,32.119,86.236],[353.948,32.116,123.436],[353.948,10.669,123.436],[353.925,3.563,123.424],[347.093,3.563,126.692],[168.581,1.627,127.113],[168.581,-1.35,124.761],[168.581,23.782,117.494],[311.594,32.116,124.934],[306.723,32.116,121.579],[365.835,32.119,81.011],[365.742,32.119,74.029],[384.3,32.119,86.236],[318.703,32.116,126.853],[347.093,32.116,126.729],[347.093,10.669,126.729],[168.581,10.666,127.955],[168.581,3.559,127.435],[168.581,22.646,123.3],[369.158,32.119,86.174],[168.581,22.223,123.956],[168.581,18.114,127.435]],[[304.659,676.567,87.852],[305.36,676.567,116.572],[305.77,676.567,82.356],[306.023,32.119,88.153],[305.419,32.119,116.577],[306.698,676.567,121.594],[305.441,32.119,116.661],[306.719,32.119,121.594],[307.253,676.567,81.596],[307.329,32.119,83.916],[311.584,676.567,124.952],[306.806,32.119,121.655],[311.584,32.119,124.957],[311.584,676.567,80.041],[311.584,32.119,81.774],[311.509,32.119,81.807],[316.271,676.567,126.211],[311.703,32.119,124.99],[318.693,642.628,126.869],[318.687,669.249,126.864],[312.949,676.567,79.726],[318.693,32.119,80.177],[318.698,676.567,126.859],[318.693,32.119,126.869],[318.817,32.119,126.869],[347.116,32.119,126.742],[347.11,669.249,126.737],[318.698,676.567,78.946],[325.796,32.119,80.041],[347.11,676.567,126.733],[353.965,32.119,123.454],[353.965,642.628,123.454],[347.116,642.628,126.737],[351.63,676.567,124.572],[319.351,676.567,78.937],[334.269,32.119,80.041],[325.801,676.567,78.885],[357.244,32.119,116.577],[357.25,669.249,116.572],[357.25,676.567,116.572],[353.965,676.567,123.449],[347.116,32.119,80.045],[347.11,676.567,78.885],[359.752,32.119,88.148],[359.693,676.567,90.53],[354.106,32.119,81.624],[354.23,676.567,79.9],[359.752,32.119,88.078],[359.758,642.628,88.153],[359.758,669.249,88.153],[359.693,676.567,81.356],[354.224,32.119,81.657],[359.623,32.119,83.925]],[[140.922,10.666,1.459],[140.922,17.773,1.858],[148.043,17.773,2.861],[148.043,10.666,2.49],[148.043,3.555,2.531],[140.922,3.555,1.555],[41.404,10.666,1.486],[34.299,17.773,1.514],[34.299,24.884,1.555],[5.863,31.315,1.596],[34.299,31.315,1.596],[140.922,22.618,4.373],[148.043,22.311,4.813],[151.992,21.78,5.624],[152.262,17.773,5.116],[152.347,10.666,4.992],[152.347,3.555,4.992],[152.094,-0.693,5.459],[148.043,-1.497,4.483],[140.922,-1.955,3.961],[48.509,-1.955,3.961],[41.404,3.555,1.583],[34.299,10.666,1.5],[5.863,10.666,1.596],[-0.684,31.315,3.988],[70.853,31.315,9.969],[148.043,23.297,9.941],[76.894,31.315,11.509],[168.581,23.794,52.601],[168.581,22.504,47.803],[168.581,17.773,46.332],[168.581,11.463,46.112],[168.581,10.666,46.085],[168.581,3.555,46.085],[168.581,-1.1,47.391],[168.581,-1.686,47.569],[168.581,-3.424,52.038],[168.581,-3.641,52.601],[148.043,-2.993,9.941],[140.922,-3.91,9.941],[41.404,-3.91,9.941],[41.404,-1.955,3.988],[5.863,-1.955,4.153],[5.863,3.555,1.761],[-0.634,3.555,4.071],[-0.684,10.666,3.988],[-3.739,31.315,9.941],[138.508,31.315,83.493],[77.468,31.315,12.1],[168.581,23.808,53.399],[140.112,31.315,88.14],[168.581,-3.91,59.709],[5.863,-3.91,45.494],[6.201,-2.345,10.285],[-0.583,-1.955,9.941],[0.379,-1.303,4.896],[-3.604,3.555,9.941],[-3.722,10.666,9.941],[-3.739,17.773,9.941],[-3.756,31.315,16.375],[168.581,23.908,59.709],[140.787,31.315,116.57],[168.581,-3.91,116.57],[5.863,-3.91,81.032],[-0.583,-1.955,116.57],[-3.638,3.555,88.14],[-3.621,3.555,17.049],[-3.756,10.666,17.049],[-3.79,17.773,38.386],[-3.773,31.315,31.278],[168.581,23.908,116.57],[140.567,31.315,117.382],[168.581,-1.676,124.242],[159.654,-1.031,137.907],[155.148,-2.183,137.907],[155.148,-3.721,130.8],[148.043,-3.91,130.8],[6.1,-2.345,116.337],[41.404,-3.91,116.57],[0.379,-1.303,121.891],[-3.604,3.555,116.57],[5.863,-1.955,122.785],[-3.806,10.666,88.14],[-3.823,17.773,88.14],[-3.79,31.315,94.574],[168.581,23.78,117.505],[139.167,31.315,122.07],[168.581,-1.348,124.764],[158.878,-0.197,141.578],[155.148,-0.324,141.66],[148.043,-2.307,137.907],[69.84,-3.91,130.8],[-0.651,3.555,122.867],[66.043,-0.234,141.605],[5.863,3.555,125.589],[-3.756,10.666,116.57],[69.84,-2.307,137.907],[-3.79,31.315,95.247],[-3.756,31.315,116.57],[168.581,22.646,123.293],[133.193,31.315,126.043],[158.76,21.384,141.509],[168.581,1.628,127.115],[158.962,3.555,141.647],[155.148,3.555,141.867],[168.581,3.555,127.445],[168.581,10.666,127.954],[148.043,3.555,141.908],[148.043,-0.352,141.674],[-0.735,10.666,122.963],[5.863,10.666,125.768],[69.84,-0.352,141.674],[69.84,3.555,141.908],[65.925,3.555,141.674],[-1.022,31.315,122.358],[168.581,22.225,123.953],[126.712,31.315,127.376],[155.148,21.432,141.537],[155.148,17.773,141.743],[158.878,17.773,141.592],[168.581,18.118,127.445],[121.008,31.315,127.569],[158.962,10.666,141.647],[155.148,10.666,141.867],[148.043,10.666,141.908],[-0.735,31.315,122.963],[5.863,31.315,125.768],[69.84,10.666,141.908],[65.925,10.666,141.674],[148.043,21.456,141.564],[148.043,17.773,141.784],[119.607,31.315,127.583],[66.026,17.773,141.619],[66.178,21.415,141.523],[69.84,21.456,141.564],[98.276,31.315,127.473],[69.84,17.773,141.784],[105.381,31.315,127.583]],[[27.208,676.364,48.286],[34.286,676.364,48.241],[12.968,676.364,48.286],[27.175,31.318,47.613],[40.565,676.364,46.092],[34.286,31.318,47.551],[5.852,676.364,48.072],[5.852,31.318,47.454],[12.963,31.318,47.613],[41.5,676.364,45.663],[40.551,31.318,44.8],[5.544,676.364,47.944],[5.202,31.318,47.184],[43.894,676.364,38.369],[42.762,31.318,39.343],[-0.684,676.364,45.326],[-0.66,31.318,44.673],[-0.754,39.211,44.662],[43.936,676.364,9.942],[43.071,31.318,38.369],[-1.759,676.364,43.136],[-3.816,39.211,38.369],[-3.694,31.318,38.369],[41.996,676.364,4.843],[43.071,31.318,17.047],[43.034,31.318,10.968],[43.024,31.318,9.937],[-3.676,676.364,38.369],[-3.816,664.746,38.369],[-3.769,31.318,17.047],[-3.769,31.318,31.258],[-3.816,642.647,37.613],[41.173,676.364,2.918],[40.841,31.318,4.817],[-3.76,676.364,31.258],[-3.764,676.364,17.052],[-3.76,676.364,14.424],[-3.769,642.647,17.047],[-3.736,31.318,9.937],[12.968,650.539,-3.86],[34.286,676.364,1.586],[40.467,31.318,3.954],[35.002,31.318,1.785],[34.286,31.318,1.591],[12.963,60.489,-3.855],[-3.736,676.364,9.942],[-3.736,642.647,9.937],[-0.693,53.354,3.362],[-0.693,31.318,3.99],[-3.414,642.647,9.222],[-0.693,60.489,3.204],[6.381,643.404,-2.314],[6.395,650.539,-2.314],[7.348,657.611,-1.793],[12.968,657.611,-2.814],[6.381,67.624,-2.314],[8.475,676.364,1.591],[27.849,31.318,1.591],[12.963,53.354,-3.233],[6.881,53.354,-1.997],[6.385,60.489,-2.314],[-0.693,676.364,3.99],[-0.693,650.539,3.209],[-0.693,643.404,3.204],[2.827,60.489,-0.354],[3.323,53.354,-0.19],[5.852,31.318,1.596],[-0.319,642.647,2.821],[2.827,643.404,-0.354],[2.837,650.539,-0.349],[1.724,676.364,3.102],[5.852,676.364,1.596]]]} \ No newline at end of file diff --git a/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_body.collision.json b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_body.collision.json new file mode 100644 index 00000000000..2bb15829563 --- /dev/null +++ b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_body.collision.json @@ -0,0 +1 @@ +{"model":"hamilton_star_iswap_gripper_body","units":"mm","frame":"the model file's: the resource's front-left-bottom corner, Z up","source":{"file":"hamilton_star_iswap_gripper_body.glb","sha256":"713e575acbbe40bc3d3480f18262ed62dd478224f78f3b8aa5814fb2af695f2e"},"method":"CoACD, threshold 0.05, at most 8 hulls, rest pose","hulls":[[[0.006,7.647,15.0],[0.006,7.647,20.3],[0.006,8.008,14.483],[21.004,0.0,15.0],[21.004,0.0,20.3],[0.006,82.352,20.3],[58.993,8.008,14.483],[0.006,81.99,14.483],[58.993,0.0,15.0],[58.993,0.0,20.3],[21.004,89.998,20.3],[0.006,82.352,15.0],[58.993,81.99,14.483],[21.004,89.998,15.0],[58.993,89.998,20.3],[58.993,89.998,15.0]],[[0.006,8.0,5.015],[58.993,8.0,5.015],[0.006,8.0,14.483],[0.006,81.999,5.015],[58.993,8.0,14.483],[58.993,81.999,5.015],[0.006,81.999,14.483],[58.993,81.999,14.483]],[[0.006,7.647,0.0],[21.004,0.0,0.0],[0.006,7.647,4.999],[0.006,82.352,0.0],[21.004,0.0,4.999],[58.993,0.0,0.0],[0.006,8.008,5.015],[0.006,82.352,4.999],[21.004,89.998,0.0],[58.993,0.0,4.999],[58.993,89.998,0.0],[58.993,8.008,5.015],[0.006,81.99,5.015],[21.004,89.998,4.999],[58.993,89.998,4.999],[58.993,81.99,5.015]]]} \ No newline at end of file diff --git a/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_finger.collision.json b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_finger.collision.json new file mode 100644 index 00000000000..2c62015b67b --- /dev/null +++ b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_finger.collision.json @@ -0,0 +1 @@ +{"model":"hamilton_star_iswap_gripper_finger","units":"mm","frame":"the model file's: the resource's front-left-bottom corner, Z up","source":{"file":"hamilton_star_iswap_gripper_finger.glb","sha256":"7497772ba087545ee8d23acf64765dc1fc48bf8f67375e77360e0e68dbd6190a"},"method":"CoACD, threshold 0.05, at most 8 hulls, rest pose","hulls":[[[134.31,8.742,5.525],[134.31,8.53,6.874],[135.246,8.29,6.874],[135.353,8.396,5.525],[134.31,8.742,1.475],[0.687,8.742,5.525],[134.31,8.184,7.644],[135.085,8.14,7.613],[0.687,8.53,6.874],[135.499,7.375,6.874],[135.727,7.375,5.525],[135.353,8.396,1.475],[135.246,8.29,0.125],[134.31,8.53,0.125],[0.687,8.742,1.475],[-0.355,8.396,5.525],[-0.248,8.29,6.874],[134.31,7.375,7.741],[0.687,8.184,7.644],[135.139,7.375,7.645],[-0.088,8.14,7.613],[135.499,0.625,6.874],[135.139,6.025,7.646],[135.727,7.375,1.475],[135.727,0.625,5.525],[135.085,8.14,-0.613],[135.499,7.375,0.125],[134.31,8.184,-0.644],[0.687,8.53,0.125],[-0.248,8.29,0.125],[-0.355,8.396,1.475],[-0.729,7.375,5.525],[-0.502,7.375,6.874],[134.31,6.025,7.742],[0.687,7.375,7.741],[-0.141,7.375,7.645],[135.139,1.975,7.646],[135.139,0.625,7.645],[135.085,-0.14,7.613],[135.246,-0.29,6.874],[135.727,0.625,1.475],[135.353,-0.396,5.525],[134.31,7.375,-0.741],[135.139,7.375,-0.645],[135.499,0.625,0.125],[135.139,6.025,-0.646],[0.687,8.184,-0.644],[-0.088,8.14,-0.613],[-0.502,7.375,0.125],[-0.729,7.375,1.475],[-0.729,0.625,5.525],[-0.502,0.625,6.874],[-0.141,6.025,7.646],[134.31,1.975,7.742],[0.687,6.025,7.742],[134.31,0.625,7.741],[134.31,-0.185,7.644],[134.31,-0.531,6.874],[134.31,-0.742,5.525],[135.353,-0.396,1.475],[135.246,-0.29,0.125],[134.31,6.025,-0.742],[0.687,7.375,-0.741],[135.085,-0.14,-0.613],[135.139,0.625,-0.645],[135.139,1.975,-0.646],[-0.141,7.375,-0.645],[-0.502,0.625,0.125],[-0.141,6.025,-0.646],[-0.729,0.625,1.475],[-0.355,-0.396,5.525],[-0.248,-0.29,6.874],[-0.088,-0.14,7.613],[-0.141,0.625,7.645],[-0.141,1.975,7.646],[0.687,1.975,7.742],[0.687,0.625,7.741],[0.687,-0.185,7.644],[0.687,-0.531,6.874],[134.31,-0.742,1.475],[0.687,-0.742,5.525],[134.31,-0.531,0.125],[134.31,1.975,-0.742],[0.687,6.025,-0.742],[134.31,-0.185,-0.644],[134.31,0.625,-0.741],[-0.141,1.975,-0.646],[-0.141,0.625,-0.645],[-0.088,-0.14,-0.613],[-0.248,-0.29,0.125],[-0.355,-0.396,1.475],[0.687,-0.742,1.475],[0.687,-0.531,0.125],[0.687,1.975,-0.742],[0.687,-0.185,-0.644],[0.687,0.625,-0.741]]]} \ No newline at end of file diff --git a/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_finger_pad.collision.json b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_finger_pad.collision.json new file mode 100644 index 00000000000..535d323ea5f --- /dev/null +++ b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_gripper_finger_pad.collision.json @@ -0,0 +1 @@ +{"model":"hamilton_star_iswap_gripper_finger_pad","units":"mm","frame":"the model file's: the resource's front-left-bottom corner, Z up","source":{"file":"hamilton_star_iswap_gripper_finger_pad.glb","sha256":"00d7a17cfa55eec76a931b332c3d874f3f35ca82dba8e575e675f431fe3f7529"},"method":"CoACD, threshold 0.05, at most 8 hulls, rest pose","hulls":[[[31.747,0.626,9.346],[31.747,3.295,9.346],[31.75,0.335,9.346],[6.743,3.581,9.672],[31.75,3.664,9.346],[6.743,3.665,9.672],[31.754,0.301,9.346],[11.663,0.431,9.671],[1.018,3.581,9.798],[31.757,3.701,9.346],[1.018,3.665,9.798],[6.871,4.043,9.672],[30.147,0.301,9.806],[32.534,0.301,9.346],[11.663,0.431,9.795],[1.018,3.581,10.29],[0.93,3.764,15.369],[0.93,3.795,12.87],[31.782,3.912,9.346],[31.825,3.95,9.346],[31.999,4.087,9.346],[32.077,4.109,9.346],[32.368,4.167,9.346],[32.541,3.701,9.346],[0.93,4.133,12.87],[1.018,4.019,9.889],[7.219,4.182,9.672],[6.963,4.095,9.672],[30.147,0.301,10.489],[36.071,3.808,9.346],[36.067,3.795,9.346],[36.071,3.795,9.347],[37.167,3.778,12.662],[32.534,0.301,9.425],[11.667,0.431,9.837],[0.93,4.145,15.368],[11.716,0.431,10.115],[11.763,0.431,10.138],[12.029,0.431,10.232],[12.039,0.431,10.234],[19.892,3.618,17.103],[19.892,3.665,17.103],[36.071,4.133,9.346],[31.999,4.189,10.166],[1.018,4.186,10.166],[6.85,4.188,10.166],[12.025,4.189,10.166],[31.633,0.301,10.489],[12.397,0.431,10.27],[37.096,4.021,12.662],[37.167,3.778,15.714],[32.531,0.301,9.796],[19.921,3.962,17.07],[20.169,4.009,17.079],[20.169,4.177,16.824],[20.169,4.187,16.454],[12.454,4.173,15.714],[20.158,3.618,17.103],[20.169,3.677,17.103],[36.808,4.186,12.662],[36.883,4.143,12.662],[36.808,4.189,15.714],[23.865,4.19,16.084],[20.169,4.189,16.084],[32.002,0.301,10.454],[24.128,3.618,17.093],[23.872,3.618,17.097],[37.096,4.021,15.714],[37.06,3.964,15.992],[37.082,3.778,16.02],[32.474,0.301,10.144],[23.865,4.007,17.077],[23.865,4.173,16.824],[23.865,4.184,16.454],[23.858,3.672,17.098],[36.808,4.021,16.039],[32.353,0.301,10.29],[36.904,3.778,16.105],[36.808,3.778,16.132],[24.128,3.665,17.093],[24.099,3.957,17.066]],[[37.202,2.0,0.714],[36.71,4.177,9.343],[37.201,0.422,9.343],[37.202,0.336,0.714],[37.202,2.0,0.545],[36.71,4.188,0.176],[32.375,4.166,9.343],[36.524,0.39,15.504],[32.375,4.191,0.176],[37.201,0.336,9.343],[37.1,-0.021,0.714],[37.202,0.336,0.545],[37.2,2.0,0.176],[36.71,4.02,-0.107],[32.084,4.109,9.343],[30.151,0.303,15.504],[36.524,-0.171,15.504],[32.375,4.02,-0.105],[32.005,3.955,-0.067],[32.005,4.064,0.176],[32.005,4.067,0.545],[32.005,4.087,9.343],[37.099,-0.021,9.343],[37.038,-0.056,9.343],[36.813,-0.189,0.176],[37.1,-0.021,0.545],[37.099,-0.02,0.176],[37.2,0.336,0.176],[37.098,2.0,-0.107],[36.71,3.665,-0.203],[32.375,3.665,-0.203],[30.151,0.301,9.804],[30.151,-0.156,15.504],[31.831,3.949,9.343],[31.789,3.912,9.343],[30.151,-0.186,10.164],[32.005,-0.189,10.164],[36.812,-0.186,9.343],[32.005,3.665,-0.124],[31.812,3.877,-0.021],[31.808,3.891,0.176],[32.375,-0.191,0.176],[36.813,-0.02,-0.107],[37.064,0.037,-0.073],[37.098,0.336,-0.107],[36.813,2.0,-0.203],[32.375,0.336,-0.203],[30.151,-0.018,9.887],[31.808,3.665,-0.027],[31.808,0.336,-0.027],[31.812,0.123,-0.021],[32.005,-0.064,0.176],[32.005,0.336,-0.124],[32.005,0.045,-0.067],[32.375,-0.02,-0.105],[36.813,0.336,-0.203]],[[0.192,4.19,15.714],[0.192,4.186,15.368],[-0.096,4.021,15.368],[-0.096,4.021,15.714],[-0.063,3.964,15.992],[0.192,4.021,16.039],[19.883,3.618,17.103],[20.12,3.618,17.103],[20.149,3.618,17.103],[23.87,3.618,17.097],[12.448,4.173,15.714],[0.929,0.205,12.869],[12.448,4.145,15.368],[12.448,0.204,12.869],[-0.203,3.664,15.368],[-0.203,3.664,15.714],[0.192,3.664,16.175],[-0.096,3.664,16.039],[19.883,0.382,17.103],[20.149,0.382,17.103],[23.856,0.328,17.098],[23.895,0.382,17.097],[24.125,3.618,17.094],[36.068,3.776,16.118],[36.068,3.777,15.997],[36.068,3.777,15.327],[0.929,-0.132,12.869],[12.448,-0.132,12.869],[30.147,0.204,13.327],[-0.203,0.336,15.368],[-0.203,0.336,15.714],[0.192,0.336,16.175],[-0.096,0.336,16.039],[19.886,0.336,17.103],[20.163,0.323,17.103],[23.863,-0.007,17.077],[24.122,0.336,17.094],[24.125,0.382,17.094],[23.895,0.381,17.097],[36.525,0.336,16.165],[36.525,2.169,16.149],[36.068,3.664,16.153],[36.525,2.169,15.711],[36.525,0.391,15.503],[36.525,-0.171,15.503],[30.147,-0.133,13.327],[0.192,-0.186,15.368],[-0.078,-0.032,15.368],[-0.096,-0.02,15.368],[-0.103,0.003,15.368],[36.525,-0.182,15.715],[-0.096,-0.02,15.714],[-0.063,0.036,15.992],[0.192,-0.021,16.039],[20.163,-0.009,17.08],[23.863,-0.173,16.824],[36.525,-0.021,16.041],[36.525,0.057,16.07],[24.096,0.042,17.066],[20.163,-0.177,16.824],[0.192,-0.189,15.714],[23.863,-0.19,16.085],[23.863,-0.183,16.454],[20.163,-0.186,16.454]],[[36.814,-0.189,15.713],[37.101,-0.02,15.713],[37.067,0.036,15.991],[36.814,-0.021,16.039],[36.525,-0.021,16.039],[36.525,-0.181,15.713],[36.814,-0.186,9.345],[37.101,-0.021,9.345],[37.101,-0.021,12.659],[37.202,0.335,15.713],[37.101,0.335,16.037],[36.814,0.335,16.175],[36.525,0.057,16.069],[36.072,2.169,16.124],[35.853,-0.133,12.659],[35.853,-0.133,9.345],[36.703,0.422,0.717],[37.202,0.422,0.717],[37.202,0.335,9.345],[37.202,2.169,15.713],[37.101,3.664,16.037],[36.814,3.664,16.175],[36.072,3.662,16.153],[36.525,0.335,16.164],[35.853,0.205,12.659],[35.853,0.205,9.345],[36.71,2.0,-0.203],[37.202,2.0,0.717],[36.814,2.0,-0.203],[36.956,2.0,-0.154],[37.098,2.0,-0.104],[37.149,2.0,0.037],[37.2,2.0,0.178],[37.202,2.0,0.547],[37.202,3.664,15.713],[37.202,2.169,12.659],[37.088,3.778,16.021],[36.814,3.778,16.132],[36.074,3.778,16.117],[36.072,3.664,16.153],[36.909,3.778,16.105],[36.072,3.776,16.117],[36.077,4.133,12.659],[36.077,4.133,9.345],[36.71,4.02,-0.104],[36.71,3.664,-0.203],[37.202,3.664,0.547],[37.202,3.664,12.659],[36.814,3.664,-0.203],[37.098,3.664,-0.104],[37.2,3.664,0.178],[37.17,3.778,15.713],[36.814,4.186,12.659],[36.71,4.188,0.178],[36.814,4.02,-0.104],[36.814,4.189,0.178],[37.099,4.02,0.178],[37.101,4.021,0.547],[37.17,3.778,12.659],[37.065,3.963,-0.072],[37.101,4.021,12.659]],[[6.743,0.335,9.671],[0.186,0.335,9.797],[6.743,0.43,9.671],[6.746,0.318,9.671],[0.186,0.43,9.797],[6.868,-0.043,9.671],[7.216,-0.182,9.671],[0.186,-0.019,9.888],[-0.061,0.037,9.919],[-0.094,0.335,9.888],[12.451,0.432,9.701],[12.427,0.43,9.671],[-0.094,0.43,9.888],[11.654,-0.167,9.671],[30.15,-0.186,10.165],[12.026,-0.189,10.165],[0.186,-0.188,10.165],[-0.097,-0.02,10.165],[-0.198,0.335,10.165],[30.15,0.432,9.797],[-0.198,0.43,10.165],[-0.203,0.43,10.535],[12.448,0.432,10.101],[30.15,0.335,9.797],[12.424,0.335,9.671],[11.901,-0.152,9.671],[30.15,-0.015,9.887],[30.15,-0.019,9.888],[30.15,-0.133,13.326],[0.186,-0.186,12.868],[-0.1,-0.021,12.868],[-0.1,-0.021,10.535],[-0.203,0.335,10.535],[30.15,0.432,10.268],[-0.203,0.43,12.868],[12.448,0.432,10.269],[12.038,-0.105,9.671],[12.278,-0.014,9.671],[12.32,0.036,9.671],[30.15,0.203,13.326],[12.448,-0.133,13.326],[-0.132,0.088,12.868],[-0.203,0.335,12.868],[0.29,0.43,12.868],[12.448,0.203,13.326]],[[7.301,3.581,9.795],[7.212,0.43,10.106],[7.298,0.43,9.795],[7.301,3.581,9.671],[7.201,3.581,10.156],[7.198,0.43,10.153],[7.298,0.43,9.671],[7.202,2.17,9.671],[6.741,3.581,9.671],[7.165,3.581,10.171],[6.866,3.581,10.285],[6.844,0.43,10.269],[6.741,0.43,9.671],[6.741,2.17,9.671],[0.186,2.17,9.797],[1.012,3.581,9.796],[6.844,3.581,10.289],[0.317,0.431,10.5],[0.258,0.43,10.5],[0.186,0.43,9.797],[0.113,0.43,9.82],[-0.097,2.17,9.888],[-0.194,2.17,10.153],[-0.198,2.17,10.165],[-0.203,2.17,10.5],[1.012,3.581,10.288],[6.502,3.581,10.291],[0.224,2.17,10.5],[-0.203,0.43,10.5],[-0.097,0.43,9.888],[-0.123,0.43,9.959],[-0.198,0.43,10.165],[-0.199,0.43,10.26],[6.474,3.581,10.291]],[[12.428,1.998,9.671],[12.428,0.706,9.671],[12.428,3.294,9.669],[12.423,3.632,9.669],[12.422,3.664,9.669],[12.413,3.699,9.669],[12.318,3.963,9.669],[12.034,4.105,9.669],[7.585,4.153,9.67],[7.339,4.173,9.67],[7.215,4.182,9.67],[11.796,1.998,9.671],[12.428,1.998,9.669],[12.425,0.457,9.671],[12.196,0.336,0.175],[12.196,3.632,0.175],[12.196,3.664,0.176],[12.018,4.022,0.176],[11.653,4.166,9.669],[11.653,4.191,0.176],[6.96,4.095,9.67],[7.215,4.187,0.175],[6.868,4.043,9.67],[9.557,0.204,9.671],[12.422,0.336,9.671],[12.019,-0.022,0.175],[11.958,0.037,-0.074],[12.018,0.336,-0.106],[12.018,3.632,-0.106],[12.319,0.036,9.671],[12.017,3.664,-0.105],[11.958,3.963,-0.072],[11.653,4.02,-0.105],[9.555,4.02,-0.106],[7.215,4.02,-0.106],[6.958,3.963,-0.074],[6.924,4.019,0.175],[6.743,3.664,9.67],[9.557,-0.133,9.671],[11.654,-0.19,0.175],[12.278,-0.014,9.671],[11.654,-0.02,-0.106],[11.654,0.336,-0.203],[11.654,1.997,-0.203],[11.653,3.664,-0.203],[9.557,3.664,-0.203],[7.215,3.664,-0.203],[6.815,3.664,0.175],[6.922,3.664,-0.106],[6.742,0.692,9.669],[6.743,2.17,9.67],[6.812,3.294,0.175],[11.653,-0.166,9.671],[6.868,-0.043,9.669],[7.216,-0.182,9.669],[7.216,-0.188,0.176],[7.216,-0.02,-0.105],[9.556,-0.02,-0.106],[12.034,-0.105,9.671],[11.9,-0.151,9.671],[7.216,0.336,-0.203],[7.216,0.457,-0.203],[7.119,0.457,-0.172],[6.922,0.457,-0.106],[6.743,0.457,9.669],[6.812,0.706,0.175],[6.744,0.336,9.669],[6.748,0.319,9.669],[6.925,-0.019,0.176],[6.958,0.037,-0.072],[6.923,0.336,-0.105],[6.886,0.457,-0.014],[6.815,0.336,0.176]],[[0.185,-0.186,15.369],[-0.102,-0.021,12.869],[0.185,-0.186,12.869],[0.21,-0.186,15.369],[-0.083,-0.032,15.369],[-0.102,-0.021,15.369],[-0.203,0.336,12.869],[-0.203,0.431,10.536],[-0.203,0.431,10.501],[0.925,-0.133,12.869],[0.925,-0.144,15.369],[0.257,0.431,10.501],[0.316,0.431,10.501],[-0.203,0.336,15.369],[-0.203,2.169,10.535],[-0.198,2.169,10.165],[-0.194,2.169,10.154],[-0.097,2.169,9.889],[0.185,2.169,9.796],[1.012,2.169,9.796],[0.925,4.144,15.369],[1.012,2.169,10.186],[1.012,3.795,12.868],[1.012,4.132,12.868],[-0.203,3.665,15.369],[-0.203,3.665,10.535],[-0.198,3.665,10.165],[-0.097,3.665,9.889],[0.185,3.665,9.796],[1.012,3.665,9.796],[0.185,4.186,15.369],[1.012,4.185,10.165],[-0.101,4.021,15.369],[-0.101,4.021,10.535],[-0.099,4.02,10.165],[-0.101,4.021,12.869],[-0.064,3.962,9.919],[0.185,4.018,9.889],[1.012,3.935,9.867],[0.185,4.188,10.165],[1.012,4.146,10.1],[1.012,4.018,9.889]]]} \ No newline at end of file diff --git a/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_link_1_body.collision.json b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_link_1_body.collision.json new file mode 100644 index 00000000000..80c21078393 --- /dev/null +++ b/pylabrobot/hamilton/star/resource_model/hamilton_star_iswap_link_1_body.collision.json @@ -0,0 +1 @@ +{"model":"hamilton_star_iswap_link_1_body","units":"mm","frame":"the model file's: the resource's front-left-bottom corner, Z up","source":{"file":"hamilton_star_iswap_link_1_body.glb","sha256":"e4b32a1337b1702a2630dcb0958df3486f8219d362c00dcd616e1a91eb7d6c98"},"method":"CoACD, threshold 0.05, at most 8 hulls, rest pose","hulls":[[[149.463,26.294,14.768],[79.414,26.294,14.768],[149.463,26.275,15.001],[151.096,26.22,15.001],[151.096,26.236,14.768],[164.234,12.751,13.15],[23.696,21.735,14.778],[22.062,21.735,14.915],[20.642,21.735,15.352],[20.599,21.735,15.379],[20.599,21.735,15.972],[79.414,26.275,15.001],[163.751,12.751,13.15],[162.061,12.744,13.15],[149.463,25.984,15.969],[151.096,25.96,15.951],[152.715,25.898,15.897],[153.639,25.844,15.847],[152.715,26.077,15.001],[152.715,26.091,14.768],[164.234,12.751,13.368],[164.234,11.932,13.15],[153.852,25.91,14.768],[157.317,24.47,14.768],[158.141,23.927,14.768],[159.987,22.451,14.768],[161.493,20.688,14.768],[162.075,19.871,14.768],[162.927,18.231,14.768],[163.254,17.545,14.768],[163.481,16.964,14.768],[163.907,15.2,14.768],[79.428,1.974,13.675],[22.076,2.774,14.485],[20.443,2.774,14.565],[19.136,2.774,15.302],[22.062,21.735,16.144],[79.414,25.984,15.969],[19.136,2.774,15.494],[19.222,2.774,15.946],[19.548,2.774,16.006],[154.164,1.974,13.15],[149.463,24.999,16.159],[151.096,24.999,16.141],[79.414,25.099,16.14],[152.715,24.999,16.087],[155.996,24.496,15.991],[153.823,25.903,15.001],[157.062,24.32,15.922],[157.303,24.461,15.001],[164.078,13.57,14.768],[164.234,11.932,13.368],[164.035,10.299,13.15],[158.113,23.913,15.001],[159.135,23.097,15.001],[159.959,22.435,15.001],[161.479,20.671,15.001],[162.061,19.854,15.001],[162.885,18.305,14.84],[163.197,17.452,15.277],[163.496,16.879,14.84],[163.879,15.2,15.001],[79.428,1.974,16.194],[21.082,2.774,16.16],[23.696,21.735,16.198],[158.851,1.974,13.15],[154.349,23.366,16.198],[155.996,23.366,16.181],[79.414,24.999,16.159],[157.615,23.377,16.032],[158.837,22.842,15.944],[164.035,13.57,15.001],[164.078,12.753,14.768],[164.035,11.932,15.001],[163.879,10.299,15.001],[164.035,10.299,13.368],[163.666,8.883,13.15],[160.371,21.311,15.944],[161.876,19.593,15.942],[163.439,16.365,15.903],[163.538,15.2,15.977],[150.926,1.974,16.194],[79.428,2.134,16.198],[22.716,2.774,16.198],[79.414,23.366,16.198],[160.073,2.955,13.15],[158.851,1.974,13.368],[157.63,21.733,16.198],[159.334,21.806,16.046],[164.035,12.753,15.001],[163.595,13.57,16.007],[163.595,11.932,16.007],[163.538,10.299,15.977],[163.439,9.135,15.903],[163.496,8.621,14.84],[163.353,7.847,14.188],[163.353,7.847,13.15],[160.911,20.121,16.044],[162.146,18.467,16.006],[162.53,15.2,16.167],[152.559,1.974,16.192],[154.363,2.134,16.198],[160.073,2.955,13.368],[161.578,4.722,13.15],[158.61,1.974,15.001],[159.249,20.1,16.198],[160.882,18.467,16.196],[162.53,13.57,16.197],[162.53,11.932,16.197],[162.53,10.299,16.166],[162.132,7.035,16.005],[163.197,8.047,15.276],[161.891,5.907,15.941],[162.885,7.197,14.84],[162.061,5.645,15.001],[162.16,5.538,13.368],[162.97,7.033,13.368],[162.97,7.033,13.15],[154.207,1.974,16.188],[155.996,2.134,16.181],[157.615,3.767,16.198],[161.578,4.722,13.368],[161.465,4.829,15.001],[159.959,3.065,15.001],[162.16,5.538,13.15],[158.397,1.974,15.345],[160.882,16.834,16.198],[160.896,12.751,16.198],[160.896,8.666,16.198],[160.896,7.035,16.196],[160.911,5.379,16.044],[160.399,4.188,15.944],[155.84,1.974,16.163],[155.996,1.974,16.154],[157.417,1.974,16.027],[157.615,2.122,16.032],[159.32,3.693,16.046],[159.249,5.4,16.198],[158.851,2.66,15.944],[157.544,1.974,16.015]],[[81.712,2.131,-0.898],[81.712,3.579,-0.898],[152.652,2.131,-0.898],[81.712,0.498,-0.858],[12.267,-0.737,0.536],[12.267,3.726,0.536],[152.652,3.579,-0.898],[158.832,1.974,7.752],[152.652,0.498,-0.79],[151.099,0.498,-0.84],[149.46,0.498,-0.858],[81.712,-0.486,-0.668],[12.499,-0.747,0.536],[12.267,-0.835,1.933],[10.657,-0.681,7.752],[10.657,0.987,7.752],[10.657,1.132,15.0],[19.216,2.773,15.946],[19.549,2.773,16.006],[21.073,2.773,16.16],[22.712,2.773,16.198],[13.834,3.726,0.536],[152.652,3.579,0.599],[158.861,1.974,13.365],[81.698,3.724,0.536],[158.237,1.492,7.752],[152.652,-0.403,-0.599],[151.099,-0.462,-0.649],[149.46,-0.486,-0.668],[13.906,-0.794,0.536],[81.712,-0.776,0.3],[13.906,-0.897,1.933],[11.542,-0.766,7.752],[13.892,-0.898,7.752],[12.267,-0.835,13.365],[10.657,-0.681,13.365],[10.657,0.568,15.909],[20.434,2.131,16.171],[22.059,2.131,16.198],[79.391,2.773,16.198],[157.54,1.974,16.014],[158.382,1.974,15.343],[158.614,1.974,15.0],[158.121,1.583,15.0],[158.237,1.483,13.365],[157.773,1.197,7.752],[152.652,-0.587,0.3],[151.099,-0.721,0.3],[149.46,-0.776,0.3],[81.698,-0.794,0.536],[81.712,-0.898,1.935],[13.892,-0.898,13.365],[12.267,-0.721,15.0],[10.657,-0.583,15.0],[10.657,-0.401,15.897],[17.17,0.499,16.136],[18.795,0.499,16.156],[79.406,0.498,16.158],[79.391,2.131,16.198],[150.94,1.974,16.193],[152.565,1.974,16.192],[154.204,1.974,16.187],[155.829,1.974,16.161],[156.003,1.974,16.153],[156.003,1.001,15.989],[157.076,1.177,15.92],[157.308,1.037,15.0],[157.41,0.967,13.365],[157.41,0.967,7.752],[151.099,-0.835,1.935],[152.652,-0.686,1.935],[149.46,-0.898,1.935],[79.406,-0.776,15.0],[13.892,-0.776,15.0],[149.46,-0.898,13.365],[12.267,-0.463,15.951],[13.892,-0.487,15.967],[79.391,-0.388,15.987],[149.46,0.498,16.158],[151.099,0.498,16.14],[152.724,0.498,16.086],[152.724,-0.399,15.895],[153.638,-0.346,15.845],[153.827,-0.406,15.0],[156.003,0.365,13.365],[153.972,-0.451,7.752],[151.099,-0.835,7.752],[152.724,-0.677,7.752],[152.652,-0.686,7.75],[79.406,-0.747,15.097],[149.46,-0.776,15.0],[151.099,-0.835,13.365],[151.099,-0.721,15.0],[79.391,-0.487,15.967],[79.406,-0.388,15.987],[149.46,-0.486,15.967],[151.099,-0.462,15.949],[152.724,-0.579,15.0],[152.724,-0.677,13.365],[153.972,-0.451,13.365]],[[0.368,13.568,16.007],[-0.256,13.568,16.007],[0.368,12.75,16.007],[7.351,24.496,15.989],[-0.694,13.568,15.0],[-0.256,12.75,16.007],[1.217,18.468,15.989],[-0.196,15.202,15.976],[0.826,12.75,15.0],[12.246,24.768,14.995],[12.246,25.259,15.946],[12.246,25.961,15.95],[7.351,23.702,15.989],[10.616,25.898,15.897],[9.704,25.845,15.845],[6.272,24.321,15.92],[4.506,22.841,15.944],[-0.876,13.568,13.366],[-0.694,12.75,15.0],[-0.536,15.202,15.0],[1.455,19.594,15.941],[-0.092,16.366,15.904],[2.955,21.314,15.944],[0.871,12.75,14.186],[12.246,24.835,7.753],[0.915,12.75,13.366],[1.118,12.753,2.763],[12.246,26.219,15.0],[10.616,26.078,15.0],[9.518,25.904,15.0],[6.037,24.461,15.0],[5.222,23.902,15.0],[3.383,22.437,15.0],[-0.68,15.202,13.366],[-0.876,13.568,2.763],[-0.876,12.75,13.366],[-0.156,16.88,14.839],[1.276,19.858,15.0],[0.17,17.455,15.276],[1.884,20.674,15.0],[10.659,24.367,2.763],[12.246,24.845,7.753],[0.915,12.75,7.753],[-0.876,12.753,2.763],[12.246,26.333,13.366],[10.616,26.175,13.366],[9.379,25.949,13.366],[5.93,24.531,13.366],[5.116,24.004,13.366],[3.274,22.545,13.366],[0.001,17.653,14.188],[-0.253,16.835,13.366],[-0.68,15.202,2.763],[-0.876,12.75,7.753],[1.189,19.964,13.366],[0.372,18.469,13.366],[0.468,18.306,14.839],[1.784,20.782,13.366],[5.671,24.367,2.763],[12.246,26.333,7.757],[10.616,26.175,7.753],[9.379,25.949,7.753],[5.93,24.531,7.753],[5.671,24.367,3.043],[5.531,24.277,2.763],[5.114,24.01,2.763],[5.116,24.005,11.734],[3.272,22.545,7.753],[1.777,20.78,2.763],[1.782,20.782,11.734],[3.272,22.546,2.763],[0.182,18.054,2.763],[0.001,17.653,2.763],[0.371,18.471,7.753],[-0.253,16.835,2.763],[1.474,20.365,2.763],[1.189,19.964,2.763],[0.832,19.312,7.753],[11.331,26.244,7.753],[0.371,18.471,2.763]],[[12.262,-0.836,1.934],[10.617,-0.676,1.934],[10.617,-0.593,0.535],[10.852,-0.615,0.535],[12.262,-0.737,0.535],[12.262,-0.836,7.753],[10.656,-0.682,7.753],[10.617,-0.678,7.753],[9.38,-0.449,1.934],[9.498,-0.412,0.535],[9.677,-0.444,0.535],[12.262,3.727,0.535],[12.262,-0.812,7.753],[10.656,0.569,15.909],[10.656,-0.4,15.897],[10.656,-0.581,15.0],[10.656,-0.682,13.366],[10.617,-0.678,13.366],[9.38,-0.45,7.753],[7.35,0.365,1.934],[6.226,0.942,0.535],[4.036,12.751,0.535],[0.825,12.747,15.0],[10.656,1.134,15.0],[12.262,0.663,7.747],[10.653,0.567,15.91],[10.131,1.366,15.866],[7.349,1.005,15.989],[10.617,-0.398,15.897],[10.617,-0.578,15.0],[9.517,-0.405,15.0],[9.38,-0.45,13.366],[5.931,0.969,13.366],[5.931,0.969,7.753],[5.932,0.969,1.934],[6.022,1.029,0.535],[-0.719,12.751,0.535],[0.915,12.751,7.753],[0.369,12.747,16.007],[8.982,1.489,15.948],[7.349,1.799,15.989],[0.369,11.932,16.007],[-0.255,11.932,16.007],[1.217,7.031,15.989],[4.506,2.658,15.944],[6.273,1.178,15.921],[9.704,-0.345,15.845],[6.036,1.038,15.0],[5.115,1.495,13.366],[5.117,1.484,1.934],[5.905,1.102,0.535],[-0.719,11.932,0.535],[-0.875,12.751,1.934],[-0.875,12.751,7.753],[-0.255,12.747,16.007],[-0.195,10.298,15.975],[-0.475,12.747,15.501],[-0.692,11.932,15.0],[-0.092,9.135,15.903],[1.454,5.906,15.941],[2.955,4.187,15.944],[3.382,3.062,15.0],[5.222,1.595,15.0],[3.272,2.953,13.366],[4.089,2.3,1.934],[5.005,1.731,0.535],[5.208,1.57,0.535],[-0.584,10.531,0.535],[-0.875,11.932,1.934],[-0.876,12.747,7.753],[-0.876,12.747,13.366],[-0.692,12.747,15.0],[-0.536,10.298,15.0],[-0.876,11.932,13.366],[-0.156,8.619,14.84],[0.169,8.044,15.276],[1.276,5.644,15.0],[1.883,4.827,15.0],[1.786,4.719,13.366],[1.772,4.719,1.934],[3.272,2.953,7.753],[3.272,2.954,1.934],[4.182,2.387,0.535],[-0.556,10.297,0.535],[-0.68,10.297,1.934],[-0.876,11.932,7.753],[-0.68,10.298,13.366],[-0.68,10.297,7.753],[0.001,7.846,14.188],[0.469,7.194,14.84],[0.371,7.029,13.366],[1.188,5.536,13.366],[1.188,5.536,7.753],[1.189,5.536,1.934],[1.264,5.628,0.535],[1.347,5.511,0.535],[1.856,4.811,0.535],[3.365,3.048,0.535],[4.065,2.481,0.535],[-0.183,8.703,0.535],[0.002,7.845,1.113],[-0.253,8.664,1.934],[-0.493,9.583,7.753],[-0.253,8.664,13.366],[0.001,7.845,7.753],[0.209,7.386,7.753],[0.371,7.029,1.934],[0.729,6.375,7.753],[1.147,5.843,0.535],[-0.138,8.53,0.535],[0.403,7.268,0.535],[0.001,7.845,1.934],[-0.142,8.306,7.753]],[[81.719,22.588,-0.898],[23.711,21.735,14.776],[22.079,21.735,14.913],[5.67,24.367,2.763],[81.719,23.369,-0.898],[155.14,22.588,-0.898],[79.408,21.735,14.776],[20.644,21.735,15.351],[5.67,24.367,3.043],[5.942,24.533,2.763],[81.719,25.001,-0.858],[20.599,21.735,15.377],[154.354,23.369,-0.898],[156.772,22.588,-0.887],[79.408,21.735,16.198],[151.075,21.768,14.766],[5.685,24.367,7.75],[5.942,24.533,7.75],[20.599,21.735,15.971],[6.637,24.829,2.763],[9.387,25.95,2.763],[81.719,25.986,-0.668],[149.458,25.001,-0.858],[156.001,23.369,-0.88],[158.479,22.588,-0.738],[79.408,23.369,16.198],[23.711,21.735,16.198],[158.963,22.588,-0.668],[159.552,22.588,-0.047],[159.779,22.588,0.3],[160.036,22.588,1.935],[160.02,22.588,7.75],[151.075,26.237,14.766],[12.258,24.769,14.993],[12.258,25.26,15.946],[12.258,25.962,15.949],[9.387,25.95,7.75],[12.258,26.22,15.0],[22.064,21.735,16.141],[12.273,25.263,15.951],[10.626,26.177,2.763],[81.719,26.275,0.3],[149.458,25.986,-0.668],[12.258,26.335,2.763],[13.905,26.398,2.763],[151.09,25.001,-0.84],[156.001,24.498,-0.689],[157.618,23.381,-0.731],[152.722,25.001,-0.786],[79.408,25.001,16.156],[22.079,23.369,16.198],[20.432,23.369,16.171],[22.079,21.735,16.143],[158.842,22.844,-0.642],[157.074,24.323,-0.621],[158.132,23.917,0.3],[159.265,23.201,1.935],[158.237,24.008,7.75],[159.718,22.829,7.75],[151.075,26.335,13.367],[149.458,26.398,13.367],[149.458,26.293,14.766],[79.408,25.986,15.967],[79.408,25.1,16.14],[157.406,24.532,7.75],[153.961,25.95,7.75],[12.273,25.962,15.951],[13.905,26.275,15.0],[10.626,26.177,7.75],[12.258,26.334,13.365],[13.905,26.398,13.365],[17.169,25.001,16.136],[81.719,26.398,1.935],[149.458,26.275,0.3],[151.09,25.962,-0.649],[12.258,26.335,7.75],[13.905,26.398,7.75],[152.722,25.899,-0.596],[153.644,25.846,-0.545],[18.8,25.001,16.156],[157.301,24.462,0.3],[158.237,24.018,1.935],[153.266,26.078,7.75],[152.722,26.177,7.75],[151.09,26.335,7.75],[149.458,26.398,1.935],[79.408,26.398,13.367],[79.408,26.275,15.0],[13.905,25.986,15.967],[157.406,24.532,1.935],[156.001,25.135,1.935],[153.961,25.95,1.935],[151.09,26.221,0.3],[153.825,25.905,0.3],[152.722,26.079,0.3],[152.722,26.177,1.935],[151.09,26.335,1.935]],[[81.679,26.398,2.763],[159.854,22.587,0.843],[81.679,26.398,1.931],[13.912,26.398,2.763],[81.679,24.615,2.763],[164.234,12.746,2.643],[159.967,22.435,0.299],[159.773,22.587,0.299],[160.0,22.472,0.843],[81.679,26.27,0.299],[13.912,26.398,1.931],[159.547,22.587,-0.049],[158.965,22.587,-0.669],[12.263,26.329,2.763],[152.727,-0.674,2.643],[153.955,-0.448,2.643],[156.606,0.633,2.643],[157.414,0.971,2.643],[158.222,1.49,2.643],[160.08,2.959,2.643],[161.567,4.721,2.643],[162.165,5.538,2.643],[162.973,7.031,2.643],[163.345,7.849,2.643],[163.588,8.666,2.643],[164.024,10.3,2.643],[164.234,11.932,2.643],[1.144,12.754,2.763],[164.234,12.746,1.931],[163.927,15.2,0.843],[163.539,16.864,0.843],[163.41,17.332,0.843],[163.167,17.918,0.843],[162.101,19.89,0.843],[161.503,20.707,0.843],[160.468,21.929,0.843],[161.47,20.67,0.299],[161.891,19.592,-0.642],[160.388,21.312,-0.644],[81.679,26.127,-0.192],[13.912,26.27,0.299],[12.263,26.22,0.299],[12.263,26.329,1.931],[159.337,21.804,-0.746],[158.497,22.587,-0.739],[10.631,26.172,2.763],[152.646,-0.682,2.643],[152.727,-0.674,1.931],[153.955,-0.448,1.931],[155.991,0.369,1.931],[158.222,1.487,1.931],[157.414,0.971,1.931],[160.08,2.959,1.931],[161.567,4.721,1.931],[162.165,5.538,1.931],[162.973,7.031,1.931],[163.345,7.849,1.111],[163.588,8.666,1.931],[164.024,10.3,1.931],[164.234,11.932,1.931],[-0.876,12.754,2.763],[164.04,13.566,0.299],[164.105,13.566,0.843],[163.992,14.652,0.843],[163.879,15.2,0.299],[163.507,16.874,0.46],[163.2,17.452,0.023],[162.893,18.301,0.46],[162.068,19.856,0.299],[162.149,18.466,-0.706],[160.921,20.122,-0.744],[163.442,16.363,-0.604],[81.679,25.983,-0.669],[13.912,25.983,-0.669],[12.263,25.956,-0.651],[10.631,25.898,-0.597],[9.726,25.844,-0.546],[10.631,26.076,0.299],[10.631,26.172,1.931],[159.256,20.098,-0.898],[157.624,21.732,-0.898],[156.767,22.587,-0.889],[160.889,18.466,-0.896],[10.001,26.057,2.763],[81.679,-0.794,0.534],[152.646,-0.682,1.931],[12.279,-0.736,0.534],[12.506,-0.746,0.534],[13.896,-0.794,0.534],[152.727,-0.576,0.299],[153.826,-0.403,0.299],[157.317,1.042,0.299],[158.125,1.588,0.299],[161.487,4.83,0.299],[159.967,3.065,0.299],[162.068,5.645,0.299],[162.893,7.196,0.461],[163.2,8.045,0.023],[163.507,8.62,0.461],[163.879,10.3,0.299],[164.04,11.932,0.299],[164.04,12.749,0.299],[-0.876,13.563,2.763],[-0.876,12.754,1.931],[-0.537,10.3,0.534],[-0.117,8.533,0.534],[0.465,7.186,0.534],[1.16,5.844,0.534],[1.273,5.629,0.534],[1.871,4.811,0.534],[3.39,3.047,0.534],[4.344,2.272,0.534],[5.217,1.572,0.534],[6.041,1.029,0.534],[9.516,-0.411,0.534],[9.694,-0.443,0.534],[10.631,-0.592,0.534],[10.873,-0.616,0.534],[163.604,13.566,-0.707],[163.539,15.2,-0.676],[162.537,15.2,-0.867],[81.679,25.001,-0.859],[13.912,25.001,-0.859],[12.263,25.001,-0.841],[10.631,25.001,-0.788],[7.366,24.495,-0.691],[9.532,25.903,0.299],[6.283,24.322,-0.622],[6.057,24.46,0.299],[9.403,25.948,1.931],[160.889,16.832,-0.898],[160.905,12.746,-0.898],[155.135,22.587,-0.898],[162.537,13.566,-0.897],[9.403,25.948,2.763],[81.679,-0.775,0.299],[152.646,-0.581,0.299],[10.631,-0.579,0.299],[12.279,-0.717,0.299],[13.896,-0.775,0.299],[152.727,-0.395,-0.597],[153.632,-0.344,-0.546],[157.074,1.181,-0.621],[161.891,5.911,-0.641],[158.836,2.658,-0.644],[160.388,4.191,-0.644],[163.442,9.137,-0.603],[163.539,10.3,-0.676],[163.604,11.932,-0.707],[-0.876,13.563,1.931],[-0.65,15.198,2.763],[-0.699,11.934,0.534],[-0.569,10.534,0.534],[-0.521,10.3,0.299],[-0.133,8.62,0.46],[0.174,8.048,0.023],[0.465,7.196,0.46],[1.289,5.645,0.299],[1.887,4.827,0.299],[3.407,3.063,0.299],[4.215,2.403,0.299],[5.233,1.588,0.299],[6.057,1.04,0.299],[9.532,-0.406,0.299],[81.679,23.364,-0.898],[9.015,23.364,-0.898],[7.366,23.364,-0.881],[5.734,23.374,-0.733],[7.366,25.131,1.931],[4.522,22.839,-0.645],[3.407,22.435,0.299],[4.215,23.098,0.299],[5.233,23.915,0.299],[5.944,24.53,1.931],[5.136,24.013,1.931],[160.905,8.666,-0.898],[162.537,12.749,-0.897],[6.639,24.828,2.763],[81.679,-0.485,-0.669],[152.646,-0.398,-0.6],[9.726,-0.344,-0.547],[12.279,-0.459,-0.651],[10.631,-0.398,-0.597],[13.896,-0.485,-0.669],[152.727,0.502,-0.787],[155.991,1.003,-0.69],[157.624,2.123,-0.732],[160.921,5.378,-0.744],[162.149,7.034,-0.705],[159.337,3.694,-0.746],[162.521,10.3,-0.867],[162.521,11.932,-0.897],[-0.65,15.198,1.931],[-0.521,15.198,0.299],[-0.666,13.563,0.299],[-0.699,12.754,0.534],[-0.246,16.829,2.763],[-0.666,11.934,0.299],[-0.181,10.3,-0.676],[-0.068,9.137,-0.604],[1.467,5.908,-0.642],[2.97,4.189,-0.645],[4.522,2.658,-0.645],[6.3,1.178,-0.622],[5.734,21.729,-0.898],[4.021,21.804,-0.746],[2.97,21.309,-0.645],[1.467,19.589,-0.642],[1.887,20.67,0.299],[3.293,22.541,1.931],[4.102,23.196,1.931],[5.944,24.53,2.763],[5.136,24.008,2.763],[160.905,7.034,-0.896],[159.256,5.402,-0.898],[81.679,0.499,-0.859],[152.646,0.502,-0.79],[7.366,1.003,-0.691],[12.279,0.499,-0.841],[10.631,0.499,-0.788],[13.896,0.499,-0.859],[155.991,2.136,-0.881],[157.624,3.768,-0.898],[-0.246,16.829,1.931],[-0.149,16.874,0.461],[-0.084,16.361,-0.604],[-0.165,15.198,-0.676],[-0.246,13.563,-0.707],[-0.117,17.247,2.763],[-0.246,12.754,-0.707],[-0.23,11.934,-0.707],[0.837,10.3,-0.867],[1.209,7.034,-0.706],[2.437,5.378,-0.744],[4.037,3.694,-0.746],[5.734,2.123,-0.733],[4.102,20.095,-0.898],[2.469,18.463,-0.896],[2.453,20.119,-0.745],[1.289,19.853,0.299],[1.209,18.463,-0.706],[0.158,17.449,0.023],[1.79,20.777,1.931],[3.293,22.541,2.763],[81.679,2.134,-0.898],[154.359,2.136,-0.898],[7.366,2.134,-0.881],[8.999,2.134,-0.898],[0.013,17.646,1.111],[0.837,15.198,-0.867],[0.837,13.563,-0.897],[0.013,17.649,2.763],[0.013,17.649,1.931],[0.837,11.934,-0.897],[2.469,7.034,-0.896],[4.102,5.4,-0.898],[5.734,3.768,-0.898],[2.469,16.829,-0.898],[1.209,19.962,1.931],[0.465,18.301,0.461],[0.384,18.466,1.931],[1.79,20.777,2.763],[0.384,18.466,2.763],[2.469,8.666,-0.898],[1.209,19.962,2.763]],[[152.652,-0.685,2.643],[152.691,-0.682,2.643],[152.652,-0.685,7.753],[152.652,1.175,2.643],[152.74,-0.677,2.643],[152.74,-0.677,7.753],[152.652,0.979,7.753],[158.859,1.976,13.15],[154.167,1.976,13.15],[162.083,12.744,13.15],[162.209,12.747,2.643],[153.979,-0.451,2.643],[153.979,-0.451,7.753],[158.244,1.488,2.643],[159.134,2.194,13.15],[157.426,0.968,7.753],[157.783,1.197,7.753],[158.244,1.494,7.753],[163.766,12.751,13.15],[162.261,12.751,12.45],[162.444,12.751,7.753],[164.234,12.747,2.643],[156.626,0.628,2.643],[157.889,1.259,2.643],[160.085,2.954,2.643],[159.376,2.387,13.15],[157.426,0.968,2.643],[164.234,12.751,13.15],[164.234,12.751,7.753],[164.234,11.932,2.643],[161.581,4.719,2.643],[161.586,4.719,13.15],[160.085,2.954,13.15],[164.234,11.932,13.15],[164.037,10.298,2.643],[161.84,5.074,2.643],[162.17,5.536,13.15],[164.037,10.298,13.15],[163.612,8.663,2.643],[162.169,5.536,2.643],[162.17,5.536,7.753],[162.987,7.031,13.15],[163.667,8.882,13.15],[163.612,8.663,7.753],[163.469,8.203,7.753],[163.358,7.847,7.753],[163.357,7.847,2.643],[162.986,7.03,2.643],[162.987,7.031,7.753],[163.358,7.847,13.15],[163.611,8.665,13.15]],[[160.534,12.749,0.844],[153.138,22.587,0.844],[164.112,12.749,0.844],[162.443,12.749,7.753],[159.312,12.753,14.768],[151.079,21.766,14.768],[159.873,22.587,0.844],[151.079,24.837,7.753],[151.079,26.336,7.753],[164.112,13.568,0.844],[164.234,12.749,1.933],[164.234,12.749,7.753],[164.076,12.753,14.768],[151.079,26.238,14.768],[160.013,22.473,0.844],[151.107,26.335,7.753],[152.741,26.177,7.753],[153.281,26.078,7.753],[153.977,25.95,7.753],[157.427,24.531,7.753],[160.035,22.587,1.933],[151.079,26.336,13.367],[164.006,14.656,0.844],[164.234,13.568,1.933],[164.234,12.753,13.367],[164.076,13.568,14.768],[151.107,26.236,14.768],[160.085,22.545,1.933],[161.521,20.708,0.844],[151.107,26.335,13.367],[152.741,26.177,13.367],[153.977,25.95,13.367],[156.008,25.135,13.367],[158.242,24.006,7.753],[157.427,24.531,13.367],[158.242,24.015,13.367],[159.269,23.198,13.367],[160.085,22.545,13.367],[163.942,15.202,0.844],[164.037,15.202,1.933],[164.038,15.202,7.753],[164.234,13.568,13.367],[163.915,15.202,14.768],[152.741,26.093,14.768],[161.587,20.782,1.933],[161.916,20.164,0.844],[153.86,25.91,14.768],[157.337,24.471,14.768],[158.15,23.929,14.768],[158.296,23.813,14.768],[159.175,23.112,14.768],[159.991,22.452,14.768],[161.502,20.688,14.768],[161.587,20.78,13.367],[163.649,16.443,0.844],[163.611,16.835,1.933],[163.422,17.334,0.844],[163.357,17.653,1.114],[164.038,15.202,13.367],[163.518,16.878,14.768],[162.17,19.965,7.753],[162.17,19.965,1.933],[162.111,19.892,0.844],[162.095,19.872,14.768],[162.17,19.964,13.367],[163.5,17.193,7.753],[163.611,16.835,7.753],[163.17,17.92,0.844],[162.384,19.394,0.844],[162.986,18.468,1.933],[162.986,18.47,7.753],[163.357,17.653,14.187],[163.61,16.835,13.367],[163.496,16.965,14.768],[162.986,18.47,13.367],[162.954,18.233,14.768],[163.279,17.548,14.768]]]} \ No newline at end of file diff --git a/pylabrobot/resources/collision.py b/pylabrobot/resources/collision.py new file mode 100644 index 00000000000..ac688e1a898 --- /dev/null +++ b/pylabrobot/resources/collision.py @@ -0,0 +1,1124 @@ +"""Collision checks of motions against a resource tree: what moves, swept, against what stands. + +A motion is checked without checking everything against everything. What moves is known - the +groups a command moves, each rigid - so each group's solid pieces are swept along its way, and only +the pieces standing still that a sweep's bounding box meets, found in a bounding-volume tree, are +tested exactly. Two groups moving at once are tested against each other only over the same slice of +time. + +Solids. Every resource is a box - its size, where it is and how it is turned - and it is taken to +be solid or not by how it holds its children: + +- a resource with no children is solid, unless it is flat (a site, a trash's opening); +- a plate, a tip rack, any itemized resource, is solid as a whole: its items are inside it, but what + sits in its items - a tip in a tip spot - is looked at too, as is what sits on it - a lid seated + on a plate; +- a resource whose children lie outside its box - a channel with its tip mounting shaft below it, a + finger with its pad - is solid, and so are its children; +- a resource whose children lie inside its box - a deck, a carrier, an arm - is a frame: only its + base, from its bottom up to the lowest thing it holds, is solid (a carrier's body under its + sites, the deck's slab), and its children are looked at in turn. + +Sweeps. A group's way is cut into segments; each segment is the convex hull of the group's pieces +at a few poses, grown by a slack. The hull is exact for a straight move (its two ends) and for moves +on independent axes (the corners of the box they span); a turn is cut into short arcs, each grown +by the most any point strays from the chord - `turn_slack`. + +The exact test is GJK: the distance between two convex hulls, each grown by a radius. Things are in +collision when closer than the clearance asked for; things that touch - a plate resting on its site +- are not, by `CONTACT` of slack. +""" + +from __future__ import annotations + +import dataclasses +import functools +import json +import math +import os +from typing import Callable, Dict, Iterable, List, Mapping, Optional, Sequence, Set, Tuple, Union + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.itemized_resource import ItemizedResource +from pylabrobot.resources.lid import Lid +from pylabrobot.resources.resource import Resource + +Vec = Tuple[float, float, float] + +# How far two things may overlap and still only touch, in mm: a plate resting on a site, a finger on +# the side of what it grips. Every solid is drawn in by this much on each face, so things that touch +# stay a little apart and only things that go into each other meet. +CONTACT = 0.05 + + +# -- vectors ------------------------------------------------------------------------------------- + + +def _sub(a: Vec, b: Vec) -> Vec: + """`a` less `b`, by coordinate.""" + return (a[0] - b[0], a[1] - b[1], a[2] - b[2]) + + +def _add(a: Vec, b: Vec) -> Vec: + """`a` plus `b`, by coordinate.""" + return (a[0] + b[0], a[1] + b[1], a[2] + b[2]) + + +def _dot(a: Vec, b: Vec) -> float: + """The dot product of `a` and `b`.""" + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + + +def _scale(a: Vec, k: float) -> Vec: + """`a` with each coordinate multiplied by `k`.""" + return (a[0] * k, a[1] * k, a[2] * k) + + +def _vec(c: Coordinate) -> Vec: + """`c` as a plain tuple.""" + return (c.x, c.y, c.z) + + +# -- poses --------------------------------------------------------------------------------------- + + +@dataclasses.dataclass(frozen=True) +class Pose: + """A rigid move: a turn of `turn` degrees about the vertical through `pivot`, then a shift.""" + + turn: float = 0.0 + pivot: Vec = (0.0, 0.0, 0.0) + shift: Vec = (0.0, 0.0, 0.0) + + def apply(self, p: Vec) -> Vec: + """`p` carried by this pose: turned about its pivot, then shifted.""" + if self.turn: + c, s = math.cos(math.radians(self.turn)), math.sin(math.radians(self.turn)) + x, y = p[0] - self.pivot[0], p[1] - self.pivot[1] + p = (self.pivot[0] + c * x - s * y, self.pivot[1] + s * x + c * y, p[2]) + return _add(p, self.shift) + + def apply_all(self, points: Sequence[Vec]) -> List[Vec]: + """Every point of `points`, carried by this pose: `apply` for many at once, the turn's cosine + and sine taken once for all of them.""" + sx, sy, sz = self.shift + if not self.turn: + return [(p[0] + sx, p[1] + sy, p[2] + sz) for p in points] + c, s = math.cos(math.radians(self.turn)), math.sin(math.radians(self.turn)) + px, py = self.pivot[0], self.pivot[1] + return [ + ( + px + c * (p[0] - px) - s * (p[1] - py) + sx, + py + s * (p[0] - px) + c * (p[1] - py) + sy, + p[2] + sz, + ) + for p in points + ] + + def _linear(self) -> Tuple[float, Vec]: + """This pose as a turn about the origin and a shift: p -> R p + t.""" + moved = self.apply((0.0, 0.0, 0.0)) + return self.turn, moved + + def then(self, other: "Pose") -> "Pose": + """This pose, followed by `other`.""" + turn, t = self._linear() + return Pose(turn + other.turn, (0.0, 0.0, 0.0), other.apply(t)) + + def inverse(self) -> "Pose": + """The pose that undoes this one.""" + turn, t = self._linear() + return Pose(-turn, (0.0, 0.0, 0.0), _scale(Pose(-turn).apply(t), -1.0)) + + def after(self, x: float, y: float, z: float = 0.0) -> "Pose": + """This pose, taken after a shift by (`x`, `y`, `z`) where things are now.""" + d = (x, y, z) + return Pose(self.turn, _sub(self.pivot, d), _add(self.shift, d)) + + @staticmethod + def shifted(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> "Pose": + """A shift by (`x`, `y`, `z`), no turn.""" + return Pose(shift=(x, y, z)) + + +STILL = Pose() + + +# -- solid pieces -------------------------------------------------------------------------------- + + +@dataclasses.dataclass +class Piece: + """One convex solid: the corners of a box, and the resource it stands for.""" + + resource: Resource + points: List[Vec] + + def __post_init__(self) -> None: + self.lo, self.hi = _bounds(self.points) + + +def _bounds(points: Iterable[Vec]) -> Tuple[Vec, Vec]: + """The lowest and highest corner of `points`, by coordinate.""" + xs, ys, zs = zip(*points) + return (min(xs), min(ys), min(zs)), (max(xs), max(ys), max(zs)) + + +def _corners( + resource: Resource, z_from: float = 0.0, z_to: Optional[float] = None, inset: float = 0.0 +) -> List[Vec]: + """The eight corners of `resource`'s box - or of the slab of it from `z_from` to `z_to` above its + bottom - absolute, each face moved in by `inset`.""" + origin = _absolute(resource) + rotation = resource.get_absolute_rotation() + sx, sy, sz = resource.get_size_x(), resource.get_size_y(), resource.get_size_z() + top = sz if z_to is None else z_to + + def span(a: float, b: float) -> Tuple[float, float]: + return (a + inset, b - inset) if b - a > 2 * inset else ((a + b) / 2, (a + b) / 2) + + return [ + _vec(origin + Coordinate(x, y, z).rotated(rotation)) + for x in span(0.0, sx) + for y in span(0.0, sy) + for z in span(z_from, top) + ] + + +def _absolute(resource: Resource) -> Coordinate: + """Where `resource` is: absolute, or - in a tree whose root has not been put anywhere - in the + root's own terms.""" + top = resource + while top.parent is not None: + top = top.parent + if top.location is not None: + return resource.get_absolute_location() + return Coordinate.zero() if resource is top else resource.get_location_wrt(top) + + +def _flat(resource: Resource) -> bool: + """Whether `resource` is thin enough on every axis to be a surface rather than a solid.""" + return min(resource.get_size_x(), resource.get_size_y(), resource.get_size_z()) <= CONTACT + + +# -- declared hulls ------------------------------------------------------------------------------ +# A box is too coarse for a device with a cavity - a thermocycler whose plate sits in a recess, a +# body rising behind it - so a model may ship a convex decomposition beside its file: +# `.collision.json`, `{"units": "mm", "hulls": [[[x, y, z], ...], ...]}` in the resource's +# own frame (front-left-bottom corner, Z up). A resource whose model has one is those hulls. + +HULL_SUFFIX = ".collision.json" +_PACKAGE_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + + +@functools.lru_cache(maxsize=None) +def _hull_files() -> Dict[str, str]: + """Every hull file under the package, by the model it is for.""" + found: Dict[str, str] = {} + for directory, _, files in os.walk(_PACKAGE_ROOT): + for name in files: + if name.endswith(HULL_SUFFIX): + found[name[: -len(HULL_SUFFIX)]] = os.path.join(directory, name) + return found + + +@functools.lru_cache(maxsize=None) +def _hulls_for_model(model: str) -> Optional[Tuple[Tuple[Vec, ...], ...]]: + """The hulls of the model's file, scaled to mm, or None where the model ships none.""" + path = _hull_files().get(model) + if path is None: + return None + with open(path, encoding="utf-8") as f: + data = json.load(f) + units = data.get("units", "mm") + if units not in ("mm", "m"): + raise ValueError(f"{path}: hulls are in {units!r}, and only 'mm' or 'm' are known") + scale = {"mm": 1.0, "m": 1000.0}[units] + return tuple( + tuple((p[0] * scale, p[1] * scale, p[2] * scale) for p in hull) for hull in data["hulls"] + ) + + +def declared_hulls(resource: Resource) -> Optional[Tuple[Tuple[Vec, ...], ...]]: + """The convex hulls `resource`'s model declares, in its own frame in mm, or None.""" + model = resource.model + return _hulls_for_model(model) if model else None + + +def _hull_pieces(resource: Resource, hulls: Sequence[Sequence[Vec]]) -> List["Piece"]: + """Each declared hull as a piece of `resource`, at `resource`'s place and turned as it is.""" + origin = _absolute(resource) + rotation = resource.get_absolute_rotation() + return [ + Piece(resource, [_vec(origin + Coordinate(*p).rotated(rotation)) for p in hull]) + for hull in hulls + ] + + +def solid_pieces( + root: Resource, + leave_out: Optional[Set[int]] = None, + hollow: Iterable[str] = (), + reuse: Optional["_Reuse"] = None, +) -> List[Piece]: + """Every solid piece under `root` (itself included), as the module docstring sets out, less + anything in `leave_out` (resource ids) and what is under it. + + Args: + hollow: categories of resource that are enclosures - a housing something travels into - and + so not solid themselves, whatever their children. + reuse: asked for a child's pieces, and for the lowest thing it holds, before the walk goes into + it, with a way to work them out (`StaticScene` keeps them between checks). + """ + leave_out = leave_out or set() + enclosures = set(hollow) + boxes: Dict[int, Tuple[Vec, Vec]] = {} + lows: Dict[int, Optional[float]] = {} + + def box(resource: Resource) -> Tuple[Vec, Vec]: + """`resource`'s box, worked out once.""" + if id(resource) not in boxes: + boxes[id(resource)] = _bounds(_corners(resource)) + return boxes[id(resource)] + + def inside(child: Resource, parent: Resource) -> bool: + """Whether `child` is held within `parent`'s box - its centre is - rather than hung on it.""" + (c_lo, c_hi), (p_lo, p_hi) = box(child), box(parent) + return all(p_lo[k] - CONTACT <= (c_lo[k] + c_hi[k]) / 2 <= p_hi[k] + CONTACT for k in range(3)) + + def lowest(resource: Resource) -> Optional[float]: + """The lowest bottom of anything `resource` holds, left out things aside.""" + if id(resource) not in lows: + if reuse is not None and resource is not root: + lows[id(resource)] = reuse.lowest(resource, lambda: lowest_here(resource)) + else: + lows[id(resource)] = lowest_here(resource) + return lows[id(resource)] + + def lowest_here(resource: Resource) -> Optional[float]: + """The lowest bottom of anything `resource` holds, worked out here.""" + low: Optional[float] = None + for child in resource.children: + if id(child) in leave_out: + continue + if not _flat(child) or not child.children: + z = box(child)[0][2] + low = z if low is None else min(low, z) + below = lowest(child) + if below is not None: + low = below if low is None else min(low, below) + return low + + def visit(resource: Resource, into: List[Piece]) -> None: + """Walk `resource` for its pieces, through the kept-scene cache where there is one.""" + if id(resource) in leave_out: + return + if reuse is not None and resource is not root: + into.extend(reuse.pieces(resource, lambda: work_out(resource))) + else: + visit_here(resource, into) + + def work_out(resource: Resource) -> List[Piece]: + """`resource`'s pieces, walked here.""" + found: List[Piece] = [] + visit_here(resource, found) + return found + + def visit_here(resource: Resource, into: List[Piece]) -> None: + """Walk `resource` for its pieces by the module docstring's rules, its children after it.""" + children = [c for c in resource.children if id(c) not in leave_out] + if resource.category in enclosures: + for child in children: + visit(child, into) + return + hulls = declared_hulls(resource) + if hulls is not None: + into.extend(_hull_pieces(resource, hulls)) + for child in children: + visit(child, into) + return + if isinstance(resource, ItemizedResource): + if not _flat(resource): + into.append(Piece(resource, _corners(resource, inset=CONTACT))) + for item in children: + if isinstance(item, Lid): + # A lid sits on the resource, not in its grid, and above its box: its own box is what is + # there. + visit(item, into) + continue + for held in item.children: + visit(held, into) + return + if not children: + if not _flat(resource): + into.append(Piece(resource, _corners(resource, inset=CONTACT))) + return + if _flat(resource): + pass + elif any(inside(c, resource) for c in children): + low = lowest(resource) + base = (low - box(resource)[0][2]) if low is not None else resource.get_size_z() + if base > 2 * CONTACT: + top = min(base, resource.get_size_z()) + into.append(Piece(resource, _corners(resource, 0.0, top, CONTACT))) + else: + into.append(Piece(resource, _corners(resource, inset=CONTACT))) + for child in children: + visit(child, into) + + out: List[Piece] = [] + visit(root, out) + return out + + +class _Reuse: + """What `solid_pieces` asks before it walks into a resource: its pieces, and the lowest thing it + holds, either kept from before or worked out with the function given.""" + + def pieces(self, resource: Resource, work_out: Callable[[], List[Piece]]) -> List[Piece]: + return work_out() + + def lowest(self, resource: Resource, work_out: Callable[[], Optional[float]]) -> Optional[float]: + return work_out() + + +def _rotation(resource: Resource) -> Tuple[float, float, float]: + """`resource`'s rotation as a plain tuple.""" + r = resource.rotation + return (r.x, r.y, r.z) + + +class StaticScene(_Reuse): + """The solid pieces of a tree, kept between checks: a check works out only what has changed + since the last - a plate put down, a lid taken off - and takes the rest as it was. + + What a resource's pieces come from is its own box, how it stands (where it is, how it is turned, + in absolute terms) and the same of everything under it; that is its key. A resource whose key is + unchanged has the pieces it had. Anything with something left out of the check under it is worked + out afresh, its unchanged parts still taken as they were. + + Items of an itemized resource - a plate's wells, a rack's spots - are taken to stay where their + resource has them; only what they hold is looked at. + """ + + def __init__(self, root: Resource, hollow: Iterable[str] = ()): + self.root = root + self.hollow = tuple(hollow) + self._kept: Dict[int, Tuple[Resource, tuple, List[Piece]]] = {} + self._lows: Dict[int, Tuple[Resource, tuple, Optional[float]]] = {} + self._keys: Dict[int, Optional[tuple]] = {} + self._leave_out: Set[int] = set() + self.worked_out = 0 # resources whose pieces were not kept, over the scene's life + + def _shape(self, resource: Resource) -> Optional[tuple]: + """What `resource`'s pieces come from, under where it stands: None if anything under it is + left out.""" + if id(resource) in self._keys: + return self._keys[id(resource)] + key: Optional[tuple] + if id(resource) in self._leave_out: + key = None + else: + if isinstance(resource, ItemizedResource): + under = [h for item in resource.children for h in item.children] + else: + under = resource.children + shapes = [self._shape(c) for c in under] + if any(s is None for s in shapes): + key = None + else: + loc = resource.location + key = ( + id(resource), + resource.name, + (loc.x, loc.y, loc.z) if loc is not None else None, + _rotation(resource), + (resource.get_size_x(), resource.get_size_y(), resource.get_size_z()), + resource.category, + resource.model, + len(resource.children), + tuple(shapes), + ) + self._keys[id(resource)] = key + return key + + def _key(self, resource: Resource) -> Optional[tuple]: + shape = self._shape(resource) + if shape is None: + return None + where = _absolute(resource) + turn = resource.get_absolute_rotation() + return ((where.x, where.y, where.z), (turn.x, turn.y, turn.z), shape) + + def pieces(self, resource: Resource, work_out: Callable[[], List[Piece]]) -> List[Piece]: + """`resource`'s pieces, kept from the last asking where nothing about it has changed.""" + key = self._key(resource) + kept = self._kept.get(id(resource)) + if key is not None and kept is not None and kept[0] is resource and kept[1] == key: + return kept[2] + self.worked_out += 1 + found = work_out() + if key is not None: + self._kept[id(resource)] = (resource, key, found) + return found + + def lowest(self, resource: Resource, work_out: Callable[[], Optional[float]]) -> Optional[float]: + """The lowest thing `resource` holds, kept from the last asking where nothing has changed.""" + key = self._key(resource) + kept = self._lows.get(id(resource)) + if key is not None and kept is not None and kept[0] is resource and kept[1] == key: + return kept[2] + low = work_out() + if key is not None: + self._lows[id(resource)] = (resource, key, low) + return low + + def solid_pieces(self, leave_out: Iterable[Resource] = ()) -> List[Piece]: + """Every solid piece under the root, less `leave_out` and what is under it.""" + self._leave_out = {id(r) for r in leave_out} + self._keys = {} + try: + return solid_pieces(self.root, self._leave_out, self.hollow, reuse=self) + finally: + self._keys = {} + + def obstacles(self, leave_out: Iterable[Resource] = ()) -> "Obstacles": + """The root's solid pieces as an obstacle tree, less `leave_out` and what is under it.""" + return Obstacles(self.solid_pieces(leave_out)) + + +# -- the exact test: GJK distance ---------------------------------------------------------------- + + +def _support(points: Sequence[Vec], d: Vec) -> Vec: + """The point of `points` furthest along `d`.""" + best, best_dot = points[0], _dot(points[0], d) + for p in points[1:]: + k = _dot(p, d) + if k > best_dot: + best, best_dot = p, k + return best + + +def _solve(g: List[List[float]], b: List[float]) -> Optional[List[float]]: + """Solve the linear system `g` · x = `b` by Gaussian elimination, or None where it is singular.""" + n = len(b) + m = [row[:] + [b[i]] for i, row in enumerate(g)] + for col in range(n): + pivot = max(range(col, n), key=lambda r: abs(m[r][col])) + if abs(m[pivot][col]) < 1e-12: + return None + m[col], m[pivot] = m[pivot], m[col] + for r in range(n): + if r != col: + f = m[r][col] / m[col][col] + for c in range(col, n + 1): + m[r][c] -= f * m[col][c] + return [m[i][n] / m[i][i] for i in range(n)] + + +def _closest_on_simplex(simplex: List[Vec]) -> Tuple[Vec, List[Vec]]: + """The point of the simplex's hull nearest the origin, and the smallest face it lies on.""" + best: Optional[Tuple[float, Vec, List[Vec]]] = None + n = len(simplex) + for mask in range(1, 1 << n): + face = [simplex[i] for i in range(n) if mask >> i & 1] + p0 = face[0] + edges = [_sub(p, p0) for p in face[1:]] + if edges: + mu = _solve([[_dot(a, b) for b in edges] for a in edges], [-_dot(a, p0) for a in edges]) + if mu is None or any(m < -1e-12 for m in mu) or sum(mu) > 1 + 1e-12: + continue + point = p0 + for m, e in zip(mu, edges): + point = _add(point, _scale(e, m)) + else: + point = p0 + d2 = _dot(point, point) + if ( + best is None + or d2 < best[0] - 1e-15 + or (abs(d2 - best[0]) <= 1e-15 and len(face) < len(best[2])) + ): + best = (d2, point, face) + assert best is not None + return best[1], best[2] + + +def distance(a: Sequence[Vec], b: Sequence[Vec], stop_beyond: float = math.inf) -> float: + """The distance between the convex hulls of two point sets; 0 if they meet. Stops early, with a + lower bound, once the hulls are sure to be further apart than `stop_beyond`.""" + v = _sub(a[0], b[0]) + simplex: List[Vec] = [] + for _ in range(64): + vv = _dot(v, v) + if vv < 1e-18: + return 0.0 + w = _sub(_support(a, _scale(v, -1.0)), _support(b, v)) + vw = _dot(v, w) + if vw > 0 and vw * vw > stop_beyond * stop_beyond * vv: + return vw / math.sqrt(vv) # a separating plane further than asked for + if vv - vw <= 1e-9 * max(vv, 1.0): + return math.sqrt(vv) + simplex.append(w) + v, simplex = _closest_on_simplex(simplex) + if len(simplex) == 4: + return 0.0 + return math.sqrt(_dot(v, v)) + + +# -- the bounding-volume tree -------------------------------------------------------------------- + + +class _Node: + __slots__ = ("lo", "hi", "left", "right", "pieces") + + def __init__(self, pieces: List[Piece]): + self.lo, self.hi = _bounds([p.lo for p in pieces] + [p.hi for p in pieces]) + self.left: Optional[_Node] = None + self.right: Optional[_Node] = None + self.pieces: List[Piece] = [] + if len(pieces) <= 4: + self.pieces = pieces + return + axis = max(range(3), key=lambda k: self.hi[k] - self.lo[k]) + pieces = sorted(pieces, key=lambda p: p.lo[axis] + p.hi[axis]) + half = len(pieces) // 2 + self.left, self.right = _Node(pieces[:half]), _Node(pieces[half:]) + + +def _meets(lo: Vec, hi: Vec, lo2: Vec, hi2: Vec) -> bool: + """Whether the box from `lo` to `hi` meets the box from `lo2` to `hi2`, by coordinate.""" + return all(lo[k] <= hi2[k] and lo2[k] <= hi[k] for k in range(3)) + + +class Obstacles: + """The pieces that stand still, in a bounding-volume tree.""" + + def __init__(self, pieces: List[Piece]): + self.pieces = pieces + self._root = _Node(pieces) if pieces else None + self.visited = 0 # boxes looked at, to show the tree keeps queries small + + def near(self, lo: Vec, hi: Vec) -> List[Piece]: + """The pieces whose boxes meet the box from `lo` to `hi`.""" + found: List[Piece] = [] + stack = [self._root] if self._root is not None else [] + while stack: + node = stack.pop() + self.visited += 1 + if not _meets(lo, hi, node.lo, node.hi): + continue + if node.left is None: + found.extend(p for p in node.pieces if _meets(lo, hi, p.lo, p.hi)) + else: + stack.append(node.left) + if node.right is not None: + stack.append(node.right) + return found + + +# -- motions ------------------------------------------------------------------------------------- + + +@dataclasses.dataclass +class Segment: + """A stretch of a group's way: the hull of its pieces at `poses`, grown by `slack`, taken over + `start` to `end` in time.""" + + poses: List[Pose] + slack: float = 0.0 + start: float = 0.0 + end: float = 1.0 + + +def _through_poses(points: Sequence[Vec], poses: Sequence[Pose]) -> List[Vec]: + """`points` carried by every pose: the hull inputs along a segment's way.""" + return [carried for pose in poses for carried in pose.apply_all(points)] + + +def _carried(points: Sequence[Vec], poses: Sequence[Pose], frame: Sequence[Pose]) -> List[Vec]: + """`points` carried by every pose of the way, as the world sees them through every pose of the + frame: the way itself, then where the frame holds it.""" + own = [pose.apply_all(points) for pose in poses] + return [seen for at in own for where in frame for seen in where.apply_all(at)] + + +@dataclasses.dataclass +class Group: + """Things that move together, rigidly: their solid pieces as they are now, and their way, as + segments of poses relative to now. + + A group may also ride a frame that moves it without this group's own segments saying so - parts + mounted on one carriage, say, carried by its X while each keeps its own way besides. The frame is + the carriage's own segments. Where a group's frame is asked for, its poses are relative to the + frame, and anything the group is checked against sees the frame applied; two groups on one frame + are compared relative to it, which says exactly what one does to the other. + """ + + name: str + pieces: List[Piece] + segments: List[Segment] + frame: Optional[List[Segment]] = None + + def swept(self, k: int) -> List[Tuple[Piece, List[Vec], float]]: + """Each piece's hull over segment `k`, and its slack.""" + segment = self.segments[k] + return [ + (piece, _through_poses(piece.points, segment.poses), segment.slack) for piece in self.pieces + ] + + def frame_poses(self, segment: Segment) -> List[Pose]: + """The frame's poses over the stretch `segment` is taken over: its way while it is moving, and + where it last came to rest while it is not.""" + if not self.frame: + return [STILL] + poses: List[Pose] = [] + rest: Pose = STILL + for f in self.frame: + if min(f.end, segment.end) > max(f.start, segment.start): + poses += f.poses + elif f.end <= segment.start: + rest = f.poses[-1] + return poses or [rest] + + def swept_in_place(self, k: int) -> List[Tuple[Piece, List[Vec], float]]: + """Each piece's hull over segment `k` as the world sees it, the frame applied.""" + segment = self.segments[k] + frame = self.frame_poses(segment) + return [ + (piece, _carried(piece.points, segment.poses, frame), segment.slack) for piece in self.pieces + ] + + def _frame_at(self, t: float) -> Pose: + """The pose of the frame at time `t`: its way while it is moving, where it last came to rest + while it is not, and where it starts before its way begins.""" + if not self.frame: + return STILL + rest = STILL + for segment in self.frame: + if segment.start <= t <= segment.end and segment.end > segment.start: + f = (t - segment.start) / (segment.end - segment.start) + return _pose_between(segment.poses[0], segment.poses[-1], f) + if segment.end <= t: + rest = segment.poses[-1] + return rest + + def pose_at(self, t: float, relative: bool = False) -> Pose: + """The group's pose at time `t`, as the world sees it: its own way between the segment's poses, + and the frame it rides carrying it. `relative` leaves the frame off - its own way alone, for a + pair judged relative to the frame they share. Before its way, where it stands; after it, where + its way ends.""" + rest = STILL + for segment in self.segments: + if ( + segment.start <= t <= segment.end + and segment.end > segment.start + and math.isfinite(segment.end - segment.start) + ): + f = (t - segment.start) / (segment.end - segment.start) + own = ( + _pose_between(segment.poses[0], segment.poses[-1], f) + if len(segment.poses) == 2 + else segment.poses[round(f * (len(segment.poses) - 1))] + ) + return own if relative else own.then(self._frame_at(t)) + if segment.end <= t and math.isfinite(segment.end): + rest = segment.poses[-1] + return rest if relative else rest.then(self._frame_at(t)) + + +def moving( + name: str, + resources: Iterable[Resource], + segments: List[Segment], + leave_out: Iterable[Resource] = (), +) -> Group: + """A group of `resources`, each with everything under it but `leave_out`, moving along + `segments`.""" + out = {id(r) for r in leave_out} + return Group(name, [p for r in resources for p in solid_pieces(r, out)], segments) + + +def straight(shift: Vec, start: float = 0.0, end: float = 1.0) -> List[Segment]: + """A straight move by `shift`: exact.""" + return [Segment([STILL, Pose(shift=shift)], 0.0, start, end)] + + +def on_axes(shift: Vec, start: float = 0.0, end: float = 1.0) -> List[Segment]: + """A move by `shift` with each axis on its own profile, so along no known line: the box it spans.""" + corners = {(x, y, z) for x in (0.0, shift[0]) for y in (0.0, shift[1]) for z in (0.0, shift[2])} + return [Segment([Pose(shift=c) for c in sorted(corners)], 0.0, start, end)] + + +def turn_slack(radius: float, degrees: float) -> float: + """How far from the chord a point `radius` from a pivot strays, turning `degrees`.""" + return radius * (1.0 - math.cos(math.radians(abs(degrees)) / 2.0)) + + +def turning( + pieces: Sequence[Piece], + pivot: Vec, + degrees: float, + step: float = 5.0, + start: float = 0.0, + end: float = 1.0, + shift: Vec = (0.0, 0.0, 0.0), +) -> List[Segment]: + """A turn of `degrees` about the vertical through `pivot` - shifted by `shift` along the way, in + step - cut into arcs of at most `step` degrees, each grown by the most any piece strays.""" + n = max(1, math.ceil(abs(degrees) / step)) + radius = max( + (math.hypot(p[0] - pivot[0], p[1] - pivot[1]) for piece in pieces for p in piece.points), + default=0.0, + ) + slack = turn_slack(radius, degrees / n) + poses = [Pose(degrees * k / n, pivot, _scale(shift, k / n)) for k in range(n + 1)] + return [ + Segment( + [poses[k], poses[k + 1]], + slack, + start + (end - start) * k / n, + start + (end - start) * (k + 1) / n, + ) + for k in range(n) + ] + + +def trapezoid( + distance: float, speed: float, acceleration: Optional[float] +) -> Tuple[float, Callable[[float], float]]: + """A trapezoidal (or triangular) profile over `distance`: its duration, and the distance covered + by a time.""" + d = abs(distance) + if d == 0 or speed <= 0: + return 0.0, lambda t: 0.0 + if not acceleration: + return d / speed, lambda t: min(d, max(0.0, t) * speed) + ramp = speed / acceleration + if acceleration * ramp * ramp >= d: # never reaches speed + ramp = math.sqrt(d / acceleration) + speed = acceleration * ramp + cruise = 0.0 + else: + cruise = (d - acceleration * ramp * ramp) / speed + total = 2 * ramp + cruise + + def covered(t: float) -> float: + t = min(max(t, 0.0), total) + if t < ramp: + return 0.5 * acceleration * t * t + if t < ramp + cruise: + return 0.5 * acceleration * ramp * ramp + speed * (t - ramp) + left = total - t + return d - 0.5 * acceleration * left * left + + return total, covered + + +def profiled( + shift: Vec, + speed: float, + acceleration: Optional[float] = None, + start: float = 0.0, + slices: int = 8, +) -> List[Segment]: + """A straight move by `shift` on a trapezoidal profile starting at `start` s, cut into `slices` + of equal time, so that two things moving at once are compared only while both are there.""" + length = math.sqrt(_dot(shift, shift)) + total, covered = trapezoid(length, speed, acceleration) + if length == 0: + return [Segment([STILL], 0.0, start, start)] + unit = _scale(shift, 1.0 / length) + times = [total * k / slices for k in range(slices + 1)] + poses = [Pose(shift=_scale(unit, covered(t))) for t in times] + return [ + Segment([poses[k], poses[k + 1]], 0.0, start + times[k], start + times[k + 1]) + for k in range(slices) + ] + + +# -- the check ----------------------------------------------------------------------------------- + + +@dataclasses.dataclass +class Collision: + mover: Resource + obstacle: Resource + group: str + segment: int + gap: float # how far apart they come, in mm: 0 when they meet + other_group: Optional[str] = None + when: Optional[float] = None + """How far into the way's time the mover was when it first met, in the check's own units - one + command or plan step is one - worked out by walking the way finely. None where the sweep met + without a sampled pose doing so, and where both parties are on their way.""" + at: Optional[Pose] = None + """The mover's pose then, relative to where it stands: what brings it to the meeting.""" + + def __str__(self) -> str: + against = f" (moving with {self.other_group})" if self.other_group else "" + way = f", at {self.when:.2f} into the way" if self.when is not None else "" + return ( + f"{self.mover.name} ({self.group}, segment {self.segment}{way}) comes within {self.gap:.2f} " + f"mm of {self.obstacle.name}{against}" + ) + + +# How finely a reported sweep is walked to find where the meeting happens: the most shift or turn +# between one sampled pose and the next, and the most samples one stretch is walked in (a very long +# way is walked coarser than this, rather than in more steps). +SAMPLE_SHIFT = 1.0 +SAMPLE_TURN_DEG = 1.0 +SAMPLE_MOST = 512 + + +def _pose_between(p0: Pose, p1: Pose, f: float) -> Pose: + """The way between two poses, taken straight: a turn about the origin and a shift, each eased. + Exact where the move is a shift; where it turns, this strays from the true turn about the pivot + by as much as the pivot is far from the origin, which the walk that uses it only asks for where + the meeting is, not whether it is.""" + turn0, shift0 = p0.turn, p0.apply((0.0, 0.0, 0.0)) + turn1, shift1 = p1.turn, p1.apply((0.0, 0.0, 0.0)) + return Pose( + turn0 + (turn1 - turn0) * f, + (0.0, 0.0, 0.0), + ( + shift0[0] + (shift1[0] - shift0[0]) * f, + shift0[1] + (shift1[1] - shift0[1]) * f, + shift0[2] + (shift1[2] - shift0[2]) * f, + ), + ) + + +def _samples(segment: Segment) -> List[Tuple[float, Pose]]: + """A segment's way, finely: how far into its time, and the pose then. + + The way is walked at about a millimetre or a degree a step. A segment of two poses moves from one + to the other. A segment of more poses spans a box on independent axes, along no known line, so + its way is walked straight from pose to pose: where the drive actually goes is the box's to hide, + and the walk only says where along such a line the meeting would be. + """ + most = len(segment.poses) - 1 + out: List[Tuple[float, Pose]] = [] + for k in range(most): + a, b = segment.poses[k], segment.poses[k + 1] + span = max( + math.hypot( + b.shift[0] - a.shift[0], + b.shift[1] - a.shift[1], + b.shift[2] - a.shift[2], + ) + / SAMPLE_SHIFT, + abs(b.turn - a.turn) / SAMPLE_TURN_DEG, + 1.0, + ) + n = min(math.ceil(span), SAMPLE_MOST) + for j in range(n): + out.append(((k + j / n) / most, _pose_between(a, b, j / n))) + out.append((1.0, segment.poses[-1])) + return out + + +def check( + root: Resource, + groups: Sequence[Group], + clearance: float = 0.0, + allow: Iterable[Resource] = (), + obstacles: Optional[Obstacles] = None, + between_groups: Union[bool, Callable[[str, str], bool]] = True, + allow_for: Optional[Mapping[str, Iterable[Resource]]] = None, +) -> List[Collision]: + """Every place where something moving in `groups` comes within `clearance` of something standing + still under `root`, or of something in another group at the same time. + + Args: + root: what the moves happen in, a deck or everything around it. + groups: what moves, and how. + clearance: how close is too close, in mm. 0 reports only things that meet. + allow: things meant to be touched - what is picked up, where it is put down: never reported. + obstacles: the pieces standing still, if already worked out (`Obstacles(solid_pieces(...))`, + leaving out everything in `groups`). + between_groups: whether the groups are checked against each other too, over the times they + share; a callable of two group names asks per pair - parts of one machine that nothing here + moves relative to each other are let off. + allow_for: more things meant to be touched, by group name: by that group only. + + A meeting against what stands still is walked finely once it is found: the way is sampled, and + the collision carries where the mover first met (`when`, `at`), so what it ran into can be + brought to where the meeting happened and held there. Between groups, the meeting stands as the + sweep found it - except where one group stands still while the other passes, which is walked + finely as well. + """ + allowed = {id(r) for r in allow} + if obstacles is None: + leave_out = {id(piece.resource) for g in groups for piece in g.pieces} + obstacles = Obstacles(solid_pieces(root, leave_out)) + reach = max(clearance, 0.0) + found: List[Collision] = [] + reported: Dict[Tuple[int, int, str, Optional[str]], Collision] = {} + + def report( + mover: Piece, + obstacle: Resource, + group: str, + k: int, + gap: float, + other: Optional[str] = None, + when: Optional[float] = None, + at: Optional[Pose] = None, + ) -> None: + """Say one meeting of `mover` and `obstacle` in `group`, keeping the closest of a pair.""" + key = (id(mover.resource), id(obstacle), group, other) + earlier = reported.get(key) + if earlier is not None and earlier.gap <= gap: + return + if earlier is not None: + found.remove(earlier) + reported[key] = Collision(mover.resource, obstacle, group, k, max(gap, 0.0), other, when, at) + found.append(reported[key]) + + def contact( + piece: Piece, group: Group, k: int, other: Piece, relative: bool = False + ) -> Tuple[Optional[float], Optional[Pose]]: + """Where along the segment's way the piece first meets what stands against it, walked finely. + + The sweep that reported the meeting is the hull of the whole way; the way itself may clear what + its hull only leans on. Nothing found says so, and the meeting stands as the sweep said it. The + pose is the group's as the world sees it, the frame it rides included - or its own way alone, + where `relative` says the other's points are relative to the same frame. + """ + if k >= len(group.segments): + return None, None + segment = group.segments[k] + if not math.isfinite(segment.end - segment.start): + return None, None # standing before or after its way, not a stretch of it + span = segment.end - segment.start + for f, _ in _samples(segment): + t = segment.start + f * span + pose = group.pose_at(t, relative=relative) + if distance(pose.apply_all(piece.points), other.points) <= 0.0: + return t, pose + return None, None + + def swept_entries( + group: Group, grow_by: float, in_place: bool + ) -> List[Tuple[int, Piece, List[Vec], float, Vec, Vec]]: + """Each piece's hull over each segment, with its bounds grown by `grow_by`.""" + out = [] + for k in range(len(group.segments)): + for piece, hull, slack in group.swept_in_place(k) if in_place else group.swept(k): + lo, hi = _bounds(hull) + grow = slack + grow_by + out.append((k, piece, hull, slack, _sub(lo, (grow,) * 3), _add(hi, (grow,) * 3))) + return out + + # Each group's whole way is swept first, and the tree asked once, for what stands anywhere near + # it; each stretch of the way is then compared with only those. What a group's frame carries is + # part of its way as the world sees it. + for group in groups: + also = {id(r) for r in allow_for.get(group.name, ())} if allow_for else set() + swept = swept_entries(group, reach, in_place=True) + if not swept: + continue + lo = (min(w[4][0] for w in swept), min(w[4][1] for w in swept), min(w[4][2] for w in swept)) + hi = (max(w[5][0] for w in swept), max(w[5][1] for w in swept), max(w[5][2] for w in swept)) + skip = allowed | also + shortlist = [o for o in obstacles.near(lo, hi) if id(o.resource) not in skip] + for k, piece, hull, slack, lo, hi in swept: + for other in shortlist: + if not _meets(lo, hi, other.lo, other.hi): + continue + gap = distance(hull, other.points, stop_beyond=slack + reach + 1.0) - slack + if gap <= 0.0 or gap < reach: + when, at = contact(piece, group, k, other) + report(piece, other.resource, group.name, k, gap, when=when, at=at) + + # Groups against each other, only over the times both segments cover - each group standing where + # it starts before its first segment, and where it ends after its last. Two groups on one frame + # are compared relative to it, which is exact; a pair on different frames is compared as the + # world sees both. A pair the caller lets off is not swept at all; the rest are cut down by + # their bounds before the exact test. + if between_groups is False: + return found + ask = between_groups if callable(between_groups) else (lambda a, b: True) + + timelines: Dict[int, Group] = {} + timeline_sweeps: Dict[Tuple[int, bool], List[Tuple[int, Piece, List[Vec], float, Vec, Vec]]] = {} + + def timeline(g: Group) -> Group: + """`g` standing where it starts before its way and where it ends after it, so a pair is + compared over every time. One per group: every pair it takes part in is compared against the + same standing.""" + known = timelines.get(id(g)) + if known is not None: + return known + if not g.segments: + built = Group(g.name, g.pieces, [Segment([STILL], 0.0, -math.inf, math.inf)], g.frame) + else: + first, last = g.segments[0], g.segments[-1] + built = Group( + g.name, + g.pieces, + [Segment(first.poses[:1], 0.0, -math.inf, first.start)] + + g.segments + + [Segment(last.poses[-1:], 0.0, last.end, math.inf)], + g.frame, + ) + timelines[id(g)] = built + return built + + def swept_over_time( + g: Group, in_place: bool + ) -> List[Tuple[int, Piece, List[Vec], float, Vec, Vec]]: + """The swept hulls of `g`'s timeline, in place where `in_place` says the pair shares a frame. + Swept once per group and way: every pair the group takes part in compares against the same + hulls, which no pair's company changes.""" + key = (id(g), in_place) + entries = timeline_sweeps.get(key) + if entries is None: + entries = swept_entries(timeline(g), 0.0, in_place=in_place) + timeline_sweeps[key] = entries + return entries + + def pair(a: Group, b: Group) -> Optional[Tuple[Group, list, Group, list, bool]]: + """The two groups' timelines and swept hulls, or None the caller lets them off. The hulls are + the world's own unless the two ride one frame, where they are relative to it - `together` says + which.""" + if not ask(a.name, b.name): + return None + together = a.frame is not None and a.frame is b.frame + return ( + timeline(a), + swept_over_time(a, not together), + timeline(b), + swept_over_time(b, not together), + together, + ) + + pairs = [ + p + for i in range(len(groups)) + for j in range(i + 1, len(groups)) + for p in [pair(groups[i], groups[j])] + if p is not None + ] + for a, swept_a, b, swept_b, together in pairs: + for ka, pa, hull_a, slack_a, lo_a, hi_a in swept_a: + sa = a.segments[ka] + for kb, pb, hull_b, slack_b, lo_b, hi_b in swept_b: + sb = b.segments[kb] + if min(sa.end, sb.end) <= max(sa.start, sb.start): + continue + if not _meets(lo_a, hi_a, lo_b, hi_b): + continue + gap = distance(hull_a, hull_b) - slack_a - slack_b + if gap <= 0.0 or gap < reach: + # Where the other stands where it stood - still, or before or after its own way - the + # meeting is walked finely as well; both on their way, it stands as the sweep found it. + # A pair on one frame is walked relative to it, as their sweeps were. + when = at = None + if all(pose.turn == 0.0 and pose.shift == (0.0, 0.0, 0.0) for pose in sb.poses): + when, at = contact(pa, a, ka, pb, relative=together) + report(pa, pb.resource, a.name, ka, gap, b.name, when=when, at=at) + return found diff --git a/pylabrobot/resources/collision_tests.py b/pylabrobot/resources/collision_tests.py new file mode 100644 index 00000000000..09ad40ab25a --- /dev/null +++ b/pylabrobot/resources/collision_tests.py @@ -0,0 +1,463 @@ +"""Collision checks: the exact test against known answers, the tree against brute force, the solids a +tree is made of, and sweeps that catch what their ends miss.""" + +import itertools +import math +import random +import time +import unittest +from typing import List, Tuple + +from pylabrobot.resources.collision import ( + CONTACT, + Group, + Obstacles, + Piece, + Pose, + Segment, + StaticScene, + _bounds, + check, + declared_hulls, + distance, + moving, + on_axes, + profiled, + solid_pieces, + straight, + turning, +) +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.corning.plates import cor_96_wellplate_360uL_Fb +from pylabrobot.resources.hamilton.tip_carriers import hamilton_tip_carrier_L5 +from pylabrobot.resources.hamilton.tip_racks import hamilton_96_tiprack_1000uL +from pylabrobot.resources.lid import Lid +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.resource import Resource +from pylabrobot.resources.resource_holder import ResourceHolder +from pylabrobot.resources.rotation import Rotation + +Vec = Tuple[float, float, float] + + +def box(lo: Vec, size: Vec, turn: float = 0.0) -> List[Vec]: + """A box's corners, turned `turn` degrees about its own centre.""" + c = tuple(lo[k] + size[k] / 2 for k in range(3)) + pose = Pose(turn, c) # type: ignore[arg-type] + return [ + pose.apply((lo[0] + dx, lo[1] + dy, lo[2] + dz)) + for dx in (0, size[0]) + for dy in (0, size[1]) + for dz in (0, size[2]) + ] + + +def separated_by_an_axis(a: List[Vec], b: List[Vec]) -> bool: + """The separating axis test for two boxes: the oracle GJK is held to.""" + + def axes(points: List[Vec]) -> List[Vec]: + o = points[0] + return [ + tuple(points[i][k] - o[k] for k in range(3)) # type: ignore[misc] + for i in (4, 2, 1) # the x, y and z edges from the first corner, by `box`'s order + ] + + def cross(u: Vec, v: Vec) -> Vec: + return (u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2], u[0] * v[1] - u[1] * v[0]) + + candidates = axes(a) + axes(b) + [cross(u, v) for u in axes(a) for v in axes(b)] + for axis in candidates: + if sum(c * c for c in axis) < 1e-9: + continue + pa = [sum(p[k] * axis[k] for k in range(3)) for p in a] + pb = [sum(p[k] * axis[k] for k in range(3)) for p in b] + if max(pa) < min(pb) or max(pb) < min(pa): + return True + return False + + +def aabb_gap(a_lo: Vec, a_size: Vec, b_lo: Vec, b_size: Vec) -> float: + gaps = [ + max(0.0, b_lo[k] - (a_lo[k] + a_size[k]), a_lo[k] - (b_lo[k] + b_size[k])) for k in range(3) + ] + return math.sqrt(sum(g * g for g in gaps)) + + +def block(name: str, lo: Vec, size: Vec) -> Piece: + return Piece(Resource(name, *size), box(lo, size)) + + +class DistanceTests(unittest.TestCase): + def test_the_distance_between_boxes_is_the_one_their_faces_say(self): + rng = random.Random(1) + for _ in range(300): + a_lo = tuple(rng.uniform(-50, 50) for _ in range(3)) + b_lo = tuple(rng.uniform(-50, 50) for _ in range(3)) + a_size = tuple(rng.uniform(1, 40) for _ in range(3)) + b_size = tuple(rng.uniform(1, 40) for _ in range(3)) + expected = aabb_gap(a_lo, a_size, b_lo, b_size) # type: ignore[arg-type] + found = distance(box(a_lo, a_size), box(b_lo, b_size)) # type: ignore[arg-type] + self.assertAlmostEqual(found, expected, places=5) + + def test_turned_boxes_meet_exactly_when_no_axis_separates_them(self): + rng = random.Random(2) + met = 0 + for _ in range(400): + a = box( + tuple(rng.uniform(-30, 30) for _ in range(3)), # type: ignore[arg-type] + tuple(rng.uniform(5, 40) for _ in range(3)), # type: ignore[arg-type] + rng.uniform(0, 180), + ) + b = box( + tuple(rng.uniform(-30, 30) for _ in range(3)), # type: ignore[arg-type] + tuple(rng.uniform(5, 40) for _ in range(3)), # type: ignore[arg-type] + rng.uniform(0, 180), + ) + apart = distance(a, b) + if apart > 1e-6: + self.assertTrue(separated_by_an_axis(a, b)) + elif apart == 0.0: + met += 1 + self.assertFalse(separated_by_an_axis(a, b)) + self.assertGreater(met, 50) # both answers were tried + + def test_stopping_early_still_says_far_enough(self): + a, b = box((0, 0, 0), (10, 10, 10)), box((100, 0, 0), (10, 10, 10)) + self.assertGreater(distance(a, b, stop_beyond=5.0), 5.0) + + +class PoseTests(unittest.TestCase): + def test_poses_compose_and_undo(self): + a = Pose(30.0, (10.0, 5.0, 0.0), (1.0, 2.0, 3.0)) + b = Pose(-75.0, (-4.0, 8.0, 0.0), (0.5, -1.0, 0.0)) + p = (3.0, -7.0, 2.0) + both = a.then(b).apply(p) + self.assertTrue(all(abs(u - v) < 1e-9 for u, v in zip(both, b.apply(a.apply(p))))) + back = a.inverse().apply(a.apply(p)) + self.assertTrue(all(abs(u - v) < 1e-9 for u, v in zip(back, p))) + slid = a.after(2.0, -1.0).apply(p) + self.assertTrue(all(abs(u - v) < 1e-9 for u, v in zip(slid, a.apply((5.0, -8.0, 2.0))))) + + +class ObstacleTreeTests(unittest.TestCase): + def test_the_tree_finds_what_brute_force_finds_and_looks_at_little(self): + rng = random.Random(3) + pieces = [ + block(f"b{i}", (rng.uniform(0, 1000), rng.uniform(0, 600), rng.uniform(0, 200)), (20, 20, 20)) + for i in range(3000) + ] + tree = Obstacles(pieces) + for _ in range(50): + lo = (rng.uniform(0, 1000), rng.uniform(0, 600), rng.uniform(0, 200)) + hi = (lo[0] + 40, lo[1] + 40, lo[2] + 40) + tree.visited = 0 + found = {id(p) for p in tree.near(lo, hi)} + brute = { + id(p) for p in pieces if all(p.lo[k] <= hi[k] and lo[k] <= p.hi[k] for k in range(3)) + } + self.assertEqual(found, brute) + self.assertLess(tree.visited, 300) # of ~1500 nodes over 3000 pieces + + +class SolidTests(unittest.TestCase): + def carrier(self) -> Tuple[Resource, Plate]: + """A carrier 100 mm tall with a flat site 80 mm up, and a plate sitting 3 mm into the site.""" + carrier = Resource("carrier", 140, 100, 100) + site = ResourceHolder("site", 128, 86, 0) + carrier.assign_child_resource(site, location=Coordinate(5, 5, 80)) + plate = cor_96_wellplate_360uL_Fb("plate") + site.assign_child_resource(plate, location=Coordinate(0, 0, -3)) + return carrier, plate + + def test_a_carrier_is_solid_up_to_what_it_holds_and_a_plate_is_solid_whole(self): + carrier, plate = self.carrier() + by = {p.resource.name: p for p in solid_pieces(carrier)} + self.assertEqual(set(by), {"carrier", "plate"}) # no site, no wells + self.assertAlmostEqual(by["carrier"].hi[2], 77.0 - CONTACT) + self.assertAlmostEqual(by["plate"].lo[2], 77.0 + CONTACT) + + def test_an_empty_carrier_is_solid_up_to_its_site(self): + carrier, plate = self.carrier() + plate.unassign() + (piece,) = solid_pieces(carrier) + self.assertAlmostEqual(piece.hi[2], 80.0 - CONTACT) + + def test_a_carrier_whose_model_declares_hulls_is_its_shape(self): + # A tip carrier is walls and a roof around five pockets, not the slab its box suggests: what + # its model declares stands where the model does, pockets and all. + carrier = hamilton_tip_carrier_L5("carrier") + carrier[0] = hamilton_96_tiprack_1000uL("rack") + pieces = [p for p in solid_pieces(carrier) if p.resource is carrier] + hulls = declared_hulls(carrier) + self.assertIsNotNone(hulls) + assert hulls is not None + self.assertEqual(len(pieces), len(hulls)) + + def solid_at(point: Vec) -> bool: + return any(distance([point], piece.points) <= 0.0 for piece in pieces) + + self.assertTrue(solid_at((4.0, 200.0, 112.0))) # a side wall, above the slab's top + self.assertFalse(solid_at((67.0, 51.9, 64.0))) # in the first pocket, under the rack + + def test_something_hung_below_is_solid_and_so_is_what_it_hangs_from(self): + channel = Resource("channel", 9, 9, 140) + channel.assign_child_resource(Resource("shaft", 7, 7, 8), location=Coordinate(1, 1, -8)) + self.assertEqual({p.resource.name for p in solid_pieces(channel)}, {"channel", "shaft"}) + + def test_a_lid_seated_on_a_plate_is_solid_with_it(self): + carrier, plate = self.carrier() + lid = Lid("lid", plate.get_size_x(), plate.get_size_y(), 10.0, nesting_z_height=2.0) + plate.assign_child_resource(lid) + pieces = {p.resource.name: p for p in solid_pieces(carrier)} + self.assertEqual(set(pieces), {"carrier", "plate", "lid"}) + # The lid sits on the plate's top, sunk by its nesting height, above where the plate itself sits. + self.assertAlmostEqual(pieces["lid"].lo[2], 77.0 + 14.2 - 2.0 + CONTACT) + + def test_an_enclosure_is_not_solid(self): + housing = Resource("housing", 200, 200, 200, category="housing") + self.assertEqual(solid_pieces(housing, hollow=["housing"]), []) + self.assertEqual(len(solid_pieces(housing)), 1) + + def test_a_turned_resource_is_solid_where_it_is_turned_to(self): + plate = cor_96_wellplate_360uL_Fb("plate") + holder = Resource("holder", 300, 300, 1) + holder.assign_child_resource(plate, location=Coordinate(150, 0, 1)) + plate.rotation = Rotation(z=90) + piece = next(p for p in solid_pieces(holder) if p.resource is plate) + self.assertAlmostEqual(piece.hi[0] - piece.lo[0], plate.get_size_y() - 2 * CONTACT, places=6) + + +class SweepTests(unittest.TestCase): + def test_a_straight_move_catches_a_wall_both_its_ends_clear(self): + wall = Obstacles([block("wall", (50, -100, -100), (2, 200, 200))]) + piece = [block("m", (0, 0, 0), (10, 10, 10))] + root = Resource("r", 1, 1, 1) + # Looked at only where it starts and where it ends, it passes through unseen... + ends = Group("ends", piece, [Segment([Pose()]), Segment([Pose(shift=(100, 0, 0))])]) + self.assertEqual(check(root, [ends], obstacles=wall), []) + # ...swept, it does not. + swept = Group("swept", piece, straight((100, 0, 0))) + self.assertEqual([c.obstacle.name for c in check(root, [swept], obstacles=wall)], ["wall"]) + + def test_resting_on_something_and_leaving_it_is_not_a_collision(self): + # The floor's faces are drawn in by CONTACT, so a thing resting on it only touches. + floor = Piece( + Resource("floor", 100, 100, 10), + box((CONTACT, CONTACT, CONTACT), (100 - 2 * CONTACT, 100 - 2 * CONTACT, 10 - 2 * CONTACT)), + ) + thing = Piece( + Resource("thing", 10, 10, 10), + box((20 + CONTACT, 20 + CONTACT, 10 + CONTACT), (10 - 2 * CONTACT,) * 3), + ) + up = Group("up", [thing], straight((0, 0, 50))) + self.assertEqual(check(Resource("r", 1, 1, 1), [up], obstacles=Obstacles([floor])), []) + + def test_clearance_reports_what_passes_too_close(self): + post = block("post", (0, 13, 0), (10, 10, 10)) + past = Group("past", [block("m", (-50, 0, 0), (10, 10, 10))], straight((100, 0, 0))) + root = Resource("r", 1, 1, 1) + self.assertEqual(check(root, [past], obstacles=Obstacles([post])), []) + (near,) = check(root, [past], clearance=5.0, obstacles=Obstacles([post])) + self.assertAlmostEqual(near.gap, 3.0, places=6) + + def test_a_meeting_is_walked_to_where_it_happens(self): + wall = Obstacles([block("wall", (48, -100, -100), (2, 200, 200))]) + piece = [block("m", (0, 0, 0), (10, 10, 10))] + swept = Group("swept", piece, straight((100, 0, 0))) + root = Resource("r", 1, 1, 1) + (hit,) = check(root, [swept], obstacles=wall) + # The mover meets the wall face on, its front at x 48: 38 mm of the 100 mm way in. + at, when = hit.at, hit.when + assert at is not None and when is not None + self.assertAlmostEqual(when, 0.38, places=2) + self.assertAlmostEqual(at.shift[0], 38.0, places=1) + + def test_a_way_its_hull_only_leans_on_stands_as_the_sweep_said(self): + # The swept hull of the box spans corners the way itself never visits: a wall beside the way, + # met by the hull alone, is reported without a place where the mover was brought to it. + wall = Obstacles([block("wall", (55, -100, -100), (2, 200, 200))]) + piece = [block("m", (0, 0, 0), (10, 10, 10))] + moved = Group("moved", piece, on_axes((100, 100, 0))) + root = Resource("r", 1, 1, 1) + (hit,) = check(root, [moved], obstacles=wall) + self.assertIsNotNone(hit) + self.assertIsNone(hit.at) + self.assertIsNone(hit.when) + + def test_moves_on_their_own_axes_are_taken_as_the_box_they_span(self): + # Something going 100 along X and 100 along Y, each on its own profile, may pass the corner. + corner = block("corner", (101, 0, 0), (10, 10, 10)) + root = Resource("r", 1, 1, 1) + piece = [block("m", (0, 0, 0), (10, 10, 10))] + diagonal = Group("diagonal", piece, straight((100, 100, 0))) + self.assertEqual(check(root, [diagonal], obstacles=Obstacles([corner])), []) + either_first = Group("axes", piece, on_axes((100, 100, 0))) + self.assertEqual(len(check(root, [either_first], obstacles=Obstacles([corner]))), 1) + + def test_a_turn_catches_what_lies_on_its_arc_and_not_what_lies_beyond(self): + bar = [block("bar", (0, -2, 0), (100, 4, 4))] + root = Resource("r", 1, 1, 1) + at = 95 * math.cos(math.radians(45)) + on_arc = block("on arc", (at - 1, at - 1, 0), (2, 2, 4)) + beyond = block("beyond", (120 * 0.7071 - 1, 120 * 0.7071 - 1, 0), (2, 2, 4)) + for step in (5.0, 90.0): # finely cut, or one arc grown by all it strays + swing = Group("swing", bar, turning(bar, (0, 0, 0), 90.0, step=step)) + found = {c.obstacle.name for c in check(root, [swing], obstacles=Obstacles([on_arc, beyond]))} + self.assertEqual(found, {"on arc"}, f"step {step}") + # Ends only, not grown: the arc's middle is missed. Why the slack is there. + ends = Group("ends", bar, [turning(bar, (0, 0, 0), 90.0, step=90.0)[0]]) + ends.segments[0].slack = 0.0 + self.assertEqual(check(root, [ends], obstacles=Obstacles([on_arc])), []) + + def test_things_meant_to_be_touched_are_not_reported(self): + target = block("target", (50, 0, 0), (10, 10, 10)) + through = Group("through", [block("m", (0, 0, 0), (10, 10, 10))], straight((100, 0, 0))) + root = Resource("r", 1, 1, 1) + self.assertEqual( + check(root, [through], allow=[target.resource], obstacles=Obstacles([target])), [] + ) + + +class FrameTests(unittest.TestCase): + """What rides a moving frame: its own way, then the frame's, judged relative to the frame.""" + + @staticmethod + def frame() -> List[Segment]: + """The carriage's own way, shared by everything it carries.""" + return [Segment([Pose(), Pose(shift=(50, 0, 0))], 0.0, 0.0, 1.0)] + + def rider(self, name: str, at: Vec, way: Vec, frame: List[Segment]) -> Group: + return Group( + name, + [Piece(Resource(name, 10, 10, 10), box(at, (10, 10, 10)))], + [Segment([Pose(), Pose(shift=way)], 0.0, 0.0, 1.0)], # type: ignore[arg-type] + frame, + ) + + def test_a_rider_is_carried_by_its_own_way_then_the_frames(self): + group = self.rider("rider", (0, 0, 0), (0, 10, 0), self.frame()) + shifted = group.pose_at(0.5).apply((5.0, 5.0, 5.0)) + # Half of the frame's x after half of the rider's own y. + self.assertAlmostEqual(shifted[0], 30.0, places=9) + self.assertAlmostEqual(shifted[1], 10.0, places=9) + self.assertAlmostEqual(shifted[2], 5.0, places=9) + + def test_after_the_way_the_rider_stands_where_the_frame_left_it(self): + group = self.rider("rider", (0, 0, 0), (0, 10, 0), self.frame()) + shifted = group.pose_at(2.0).apply((5.0, 5.0, 5.0)) + self.assertAlmostEqual(shifted[0], 55.0, places=9) + self.assertAlmostEqual(shifted[1], 15.0, places=9) + self.assertAlmostEqual(shifted[2], 5.0, places=9) + + def test_the_sweep_spans_the_own_way_and_the_frames(self): + group = self.rider("rider", (0, 0, 0), (0, 10, 0), self.frame()) + hull = group.swept_in_place(0)[0][1] + lo, hi = _bounds(hull) + # The frame's x, the own way's y, over the piece's own extent. + self.assertAlmostEqual(lo[0], 0.0, places=9) + self.assertAlmostEqual(hi[0], 60.0, places=9) + self.assertAlmostEqual(lo[1], 0.0, places=9) + self.assertAlmostEqual(hi[1], 20.0, places=9) + + def test_two_riders_on_one_frame_are_judged_relative_to_it(self): + root = Resource("r", 1, 1, 1) + frame = self.frame() + crossing = self.rider("a", (0, 0, 0), (40, 0, 0), frame) + standing = self.rider("b", (30, 0, 0), (0, 0, 0), frame) + found = check(root, [crossing, standing], obstacles=Obstacles([])) + self.assertEqual([c.obstacle.name for c in found], ["b"]) + clear = self.rider("a", (0, 0, 0), (10, 0, 0), frame) + self.assertEqual(check(root, [clear, standing], obstacles=Obstacles([])), []) + + +class SceneTests(unittest.TestCase): + """The kept scene: what changed is worked out again, the rest taken as it was.""" + + def scene(self) -> Tuple[StaticScene, Resource, ResourceHolder]: + carrier = Resource("carrier", 100, 100, 100) + site = ResourceHolder("site", 80, 80, 0) + carrier.assign_child_resource(site, location=Coordinate(10, 10, 80)) + plate = cor_96_wellplate_360uL_Fb("plate") + site.assign_child_resource(plate) + return StaticScene(carrier), carrier, site + + def test_a_second_walk_keeps_the_pieces_of_what_did_not_change(self): + kept, _carrier, _site = self.scene() + first = {p.resource.name: p for p in kept.solid_pieces()} + self.assertGreater(kept.worked_out, 0) + worked = kept.worked_out + second = {p.resource.name: p for p in kept.solid_pieces()} + self.assertEqual(kept.worked_out, worked) + # The root is walked afresh every time; everything under it is kept. + for name, piece in first.items(): + if name != "carrier": + self.assertIs(second[name], piece) + + def test_something_moved_is_worked_out_again(self): + kept, _carrier, site = self.scene() + first = {p.resource.name: p for p in kept.solid_pieces()} + plate = site.children[0] + plate.unassign() + site.assign_child_resource(plate, location=Coordinate(0, 0, -3)) + second = {p.resource.name: p for p in kept.solid_pieces()} + self.assertGreater(kept.worked_out, 1) + self.assertIsNot(second["plate"], first["plate"]) + self.assertAlmostEqual(second["plate"].lo[2], first["plate"].lo[2] - 3.0, places=6) + + def test_an_enclosure_is_walked_through(self): + housing = Resource("housing", 200, 200, 200, category="housing") + inner = block("inner", (10, 10, 10), (20, 20, 20)) + housing.assign_child_resource(inner.resource, location=Coordinate(10, 10, 10)) + scene = StaticScene(housing, hollow=("housing",)) + pieces = scene.solid_pieces() + self.assertEqual([p.resource.name for p in pieces], ["inner"]) + + +class TimeTests(unittest.TestCase): + def channels(self, lead_start: float, follow_start: float): + lead = [block("lead", (0, 0, 0), (9, 9, 100))] + follow = [block("follow", (0, -20, 0), (9, 9, 100))] + return [ + Group("lead", lead, profiled((0, 40, 0), speed=200, acceleration=800, start=lead_start)), + Group( + "follow", follow, profiled((0, 40, 0), speed=200, acceleration=800, start=follow_start) + ), + ] + + def test_one_following_another_into_its_place_is_not_a_collision(self): + # The lead leaves before the follower arrives: their sweeps overlap, but never at once. + self.assertEqual( + check(Resource("r", 1, 1, 1), self.channels(0.0, 0.5), obstacles=Obstacles([])), [] + ) + + def test_arriving_before_the_other_has_left_is(self): + hits = check(Resource("r", 1, 1, 1), self.channels(0.4, 0.0), obstacles=Obstacles([])) + self.assertEqual({(c.mover.name, c.obstacle.name) for c in hits}, {("lead", "follow")}) + + +class ScaleTests(unittest.TestCase): + def test_a_sweep_among_thousands_is_quick_and_tests_few_exactly(self): + rng = random.Random(4) + root = Resource("deck", 2000, 2000, 10) + for i, (x, y) in enumerate(itertools.product(range(0, 1900, 30), range(0, 1900, 30))): + root.assign_child_resource( + Resource(f"post{i}", 10, 10, rng.uniform(20, 80)), location=Coordinate(x, y, 10) + ) + t = time.perf_counter() + pieces = solid_pieces(root) + tree = Obstacles(pieces) + built = time.perf_counter() - t + carried = Resource("carried", 120, 80, 20) + carried.location = Coordinate(100, 100, 100) + above = moving("above", [carried], straight((1500, 1000, 0))) + t = time.perf_counter() + tree.visited = 0 + self.assertEqual(check(root, [above], obstacles=tree), []) + checked = time.perf_counter() - t + self.assertGreater(len(pieces), 4000) + self.assertLess(checked, 1.0) + self.assertLess(built, 10.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/resources/hamilton/resource_model/hamilton_tip_carrier_L5.collision.json b/pylabrobot/resources/hamilton/resource_model/hamilton_tip_carrier_L5.collision.json new file mode 100644 index 00000000000..a10f7b8bd5d --- /dev/null +++ b/pylabrobot/resources/hamilton/resource_model/hamilton_tip_carrier_L5.collision.json @@ -0,0 +1 @@ +{"model":"hamilton_tip_carrier_L5","units":"mm","frame":"the model file's: the resource's front-left-bottom corner, Z up","source":{"file":"hamilton_tip_carrier_L5.glb","sha256":"6fa78d46ce93f881d8a7012c441a33309bb1efb6732d70f7676f95cef084a4ef"},"method":"CoACD, threshold 0.02, at most 40 hulls, rest pose","hulls":[[[57.543,243.814,0.0],[134.009,243.814,0.0],[57.543,243.814,13.508],[57.543,358.586,0.0],[134.009,243.814,16.48],[134.009,358.586,0.0],[124.496,243.814,16.48],[57.543,280.955,16.48],[57.543,358.586,13.508],[134.009,358.586,16.48],[57.543,302.84,16.48],[124.496,358.586,16.48]],[[0.006,243.814,0.0],[57.537,243.814,0.0],[0.006,243.814,16.48],[0.006,358.586,0.0],[57.537,243.814,13.508],[57.537,358.586,0.0],[9.502,243.814,16.48],[0.006,358.586,16.48],[57.537,280.955,16.48],[57.537,358.586,13.508],[9.502,358.586,16.48],[57.537,302.84,16.48]],[[66.994,110.872,0.0],[134.009,110.872,0.0],[66.994,110.872,13.508],[66.994,243.801,0.0],[134.009,110.872,16.48],[134.009,243.801,0.0],[124.497,110.872,16.48],[66.994,184.956,16.48],[66.994,243.801,13.508],[134.009,243.801,16.48],[66.994,206.857,16.48],[124.497,243.801,16.48]],[[66.994,426.817,0.0],[134.009,426.817,0.0],[66.994,426.817,13.508],[66.994,497.493,0.0],[134.009,426.817,16.48],[134.009,497.493,0.0],[124.497,426.817,16.48],[66.994,472.957,16.48],[66.994,497.493,16.48],[134.009,497.493,16.48]],[[66.994,358.586,0.0],[134.009,358.586,0.0],[66.994,358.586,13.509],[66.994,426.81,0.0],[134.009,358.586,13.569],[134.009,426.81,0.0],[124.497,358.586,13.569],[66.994,376.945,13.569],[66.994,426.81,13.509],[134.009,426.81,13.569],[66.994,398.836,13.569],[124.497,426.81,13.569]],[[66.987,497.498,114.696],[-0.0,497.498,114.696],[66.987,472.962,114.696],[66.987,497.498,0.0],[-0.0,472.938,114.696],[-0.0,497.498,0.0],[9.502,472.938,114.696],[66.987,472.938,13.517],[66.987,472.938,0.0],[-0.0,472.938,0.0]],[[-0.0,358.593,0.0],[66.987,358.593,0.0],[-0.0,358.593,16.48],[-0.0,426.78,0.0],[66.987,358.593,13.508],[66.987,426.78,0.0],[9.502,358.593,16.48],[-0.0,426.78,16.48],[66.987,376.943,16.48],[66.987,426.78,13.508],[9.502,426.78,16.48],[66.987,398.837,16.48]],[[134.009,472.951,114.696],[134.009,472.951,16.489],[124.5,472.951,114.696],[134.009,243.852,114.696],[134.008,426.805,13.569],[126.656,472.951,16.489],[134.009,243.852,16.489],[124.5,472.951,16.509],[124.446,398.805,114.696],[134.008,110.872,114.696],[124.502,426.805,13.569],[134.008,358.543,13.569],[134.008,110.872,16.489],[124.446,398.805,13.589],[124.442,302.792,114.696],[124.502,110.872,114.696],[124.446,384.274,13.569],[124.502,358.543,13.569],[124.502,110.872,16.489],[124.442,302.792,16.489],[124.442,280.96,114.696],[124.447,185.018,114.696],[124.446,376.902,13.569],[124.447,185.018,16.489],[124.442,280.96,16.489]],[[124.442,302.848,114.696],[57.543,302.848,114.696],[124.442,280.952,114.696],[124.442,302.848,16.48],[57.543,280.952,114.696],[57.543,302.848,16.48],[124.442,280.952,16.48],[57.543,280.952,16.48]],[[57.538,302.848,114.696],[9.547,302.848,114.648],[9.547,302.839,114.696],[57.538,280.952,114.696],[57.538,302.848,16.48],[9.547,302.848,16.48],[9.547,280.952,114.696],[57.538,280.952,16.48],[9.547,280.952,16.48]],[[9.499,426.78,114.696],[0.001,426.78,114.696],[9.523,398.822,114.696],[9.499,426.78,16.48],[0.001,426.78,16.48],[0.001,243.814,114.696],[9.523,398.822,16.48],[9.547,302.844,114.696],[0.001,243.814,16.48],[9.499,243.814,114.696],[9.547,302.844,16.48],[9.547,280.959,114.696],[9.499,243.814,16.48],[9.547,280.959,16.48]],[[9.499,110.861,114.696],[0.001,110.861,114.696],[9.523,88.939,114.696],[9.71,14.857,114.686],[9.523,110.829,114.666],[9.499,110.861,13.569],[0.001,110.861,13.569],[-0.0,51.909,114.686],[0.001,51.92,114.696],[9.499,51.92,114.696],[9.71,0.011,114.686],[9.71,14.857,13.598],[9.523,110.829,13.569],[0.001,51.92,13.569],[-0.0,51.909,13.598],[-0.0,0.011,114.686],[9.71,0.011,13.598],[9.523,88.939,13.569],[-0.0,0.011,13.598],[9.499,51.92,13.569]],[[124.444,206.86,114.696],[66.994,206.86,114.696],[124.444,184.962,114.696],[124.444,206.86,16.48],[66.994,184.962,114.696],[66.994,206.86,16.48],[124.444,184.962,16.48],[66.994,184.962,16.48]],[[134.009,110.872,13.569],[134.009,110.872,0.0],[124.497,110.872,13.569],[134.009,51.913,13.569],[134.009,51.913,0.0],[66.994,110.872,0.0],[66.994,110.872,13.509],[66.994,110.838,13.569],[124.497,51.913,13.569],[66.994,51.913,0.0],[66.994,88.939,13.569],[66.994,51.913,13.509]],[[134.009,51.902,13.569],[134.009,0.0,13.569],[134.009,51.902,0.0],[124.497,51.902,13.569],[134.009,0.0,0.0],[66.994,0.0,13.569],[66.994,51.902,0.0],[66.994,51.902,13.509],[66.994,14.852,13.569],[66.994,0.0,0.0]],[[66.994,206.86,114.696],[9.524,206.86,114.671],[9.524,184.962,114.696],[66.994,184.962,114.696],[66.994,206.86,27.403],[9.524,206.86,27.403],[9.524,184.962,27.403],[66.994,184.962,27.403]],[[9.499,243.801,114.696],[0.001,243.801,114.696],[9.524,184.948,114.696],[9.524,206.847,114.662],[9.524,243.801,13.52],[0.001,243.801,0.003],[0.001,110.872,114.696],[9.499,110.872,114.696],[9.524,110.872,13.475],[9.524,243.801,0.003],[0.001,110.872,0.003],[9.524,110.872,0.003]],[[9.524,110.872,0.0],[66.994,110.872,0.0],[9.524,110.872,13.471],[9.524,243.801,0.0],[66.994,110.872,13.51],[66.994,243.801,0.0],[9.552,110.872,13.51],[9.524,110.898,13.51],[9.524,243.801,13.51],[9.619,184.943,13.531],[66.994,184.956,13.531],[66.994,243.801,13.51],[9.524,184.943,13.531],[9.524,206.87,13.531],[66.994,206.857,13.531],[9.619,206.87,13.531]],[[66.987,110.872,13.509],[66.987,110.838,13.569],[66.987,110.872,0.0],[9.502,110.872,13.569],[66.987,88.939,13.569],[66.987,51.913,0.0],[-0.0,110.872,0.0],[-0.0,110.872,13.569],[66.981,51.913,13.509],[66.987,51.913,7.884],[9.502,51.913,13.569],[-0.0,51.913,0.0],[-0.0,51.913,13.569]],[[66.987,51.902,7.884],[66.987,51.902,0.0],[66.981,51.902,13.509],[66.987,51.897,13.509],[66.987,0.0,0.0],[-0.0,51.902,0.0],[66.987,14.852,13.586],[9.502,51.902,13.586],[66.987,0.0,13.586],[-0.0,0.0,0.0],[-0.0,51.902,13.586],[-0.0,0.0,13.586]],[[134.009,497.498,114.696],[134.009,472.951,114.696],[134.009,497.498,16.48],[66.994,497.498,114.696],[124.497,472.951,114.696],[134.009,472.951,16.48],[66.994,497.498,16.48],[66.994,472.96,114.696],[124.497,472.951,16.509],[126.649,472.951,16.48],[66.994,472.96,16.48]],[[134.009,14.872,114.696],[124.497,14.872,114.696],[134.009,0.0,114.696],[134.009,14.872,13.569],[66.994,14.85,114.696],[124.497,14.872,13.569],[66.994,0.0,114.696],[134.009,0.0,13.569],[66.994,14.85,13.569],[66.994,0.0,13.569]],[[66.994,110.84,114.696],[9.524,110.84,114.666],[9.524,88.942,114.696],[66.994,88.942,114.696],[66.994,110.84,13.569],[9.524,110.84,13.569],[9.524,88.942,13.569],[66.994,88.942,13.569]],[[124.444,398.838,114.696],[66.994,398.838,114.696],[124.444,376.94,114.696],[124.444,398.838,13.589],[66.994,376.94,114.696],[66.994,398.838,13.569],[124.444,376.94,13.569],[124.444,384.31,13.569],[124.433,398.838,13.569],[66.994,376.94,13.569]],[[66.994,472.938,13.509],[66.994,426.78,13.509],[66.994,472.938,0.0],[9.524,472.938,13.509],[52.834,426.78,13.509],[66.994,426.78,0.0],[9.524,472.938,0.0],[9.524,426.784,13.509],[9.524,426.78,13.476],[9.524,426.78,0.0]],[[0.001,426.78,0.0],[0.001,472.938,0.0],[9.524,426.78,0.0],[0.001,426.78,114.696],[9.524,472.938,0.0],[0.001,472.938,114.696],[9.524,426.78,13.484],[9.499,426.78,114.696],[9.524,472.938,13.517],[9.499,472.938,114.696],[9.524,426.784,13.517]],[[66.994,398.838,114.696],[9.524,398.834,114.696],[66.994,376.94,114.696],[66.994,398.838,16.48],[9.524,398.838,16.48],[9.524,376.94,114.696],[66.994,376.94,16.48],[9.524,376.94,16.48]],[[66.994,14.849,114.686],[9.71,14.849,114.686],[66.994,0.0,114.686],[66.994,14.849,13.598],[9.71,0.0,114.686],[9.71,14.849,13.598],[66.994,0.0,13.598],[9.71,0.0,13.598]],[[134.009,110.872,114.696],[124.497,110.872,114.696],[134.009,88.915,114.696],[134.009,110.872,13.569],[66.994,110.838,114.696],[124.497,110.872,13.569],[124.497,88.915,114.696],[134.009,88.915,13.569],[66.994,88.94,114.696],[66.994,110.838,13.569],[124.497,88.915,13.569],[66.994,88.94,13.569]],[[124.5,14.873,13.569],[134.009,14.873,13.569],[124.5,14.873,114.696],[124.5,88.904,13.569],[134.009,88.904,13.569],[134.009,14.873,114.696],[124.5,88.904,114.696],[134.009,88.904,114.696]],[[9.619,206.875,13.532],[66.994,206.858,27.402],[66.994,206.858,13.532],[9.524,206.875,13.532],[66.994,184.962,27.402],[9.524,206.858,27.402],[66.994,184.962,13.532],[9.524,184.953,13.532],[9.524,184.962,27.402],[9.619,184.947,13.532],[9.552,184.947,13.532]]]} \ No newline at end of file diff --git a/pylabrobot/visualizer3D/README.md b/pylabrobot/visualizer3D/README.md index d52986e0daa..f85fd9e8eb3 100644 --- a/pylabrobot/visualizer3D/README.md +++ b/pylabrobot/visualizer3D/README.md @@ -254,6 +254,8 @@ ports while the bind fails with `EADDRINUSE`; any other error is raised. | `scene` | the first message to a new client (the kept scene, not a rebuild); broadcast, with a full `state` after it, when a name appears in or disappears from the tree | `protocol`, `epoch`, `stats`, `models`, `instances` | | `state` | a snapshot after every `scene`; a delta whenever a batch of tracker updates holds something that looks different from what was last sent | `epoch`, `states`, `of`, `locations` | | `moves` | the tree changed shape but holds the same names: a reparent or a relocation, applied to the scene the page has | `epoch`, `moves` | +| `motion` | a command the device is carrying out, acted out by the page, which answers `motion_done` | the command's motion, or a `command` alone | +| `collisions` | what a refused command would hit, drawn over the scene until the next word about collisions or a rebuild | `collisions`: `{index, resource, group}` per resource hit | | `hello` | page to server, once per socket: what it draws with, or from `boot.js` why it could not; the first without an `error` ends `wait_for_browser` | `backend`, `renderer`, `software`, `quality`, `userAgent`, `error` | **`scene`.** `protocol` is `PROTOCOL` in `server.py`, currently 1, and the page holds its own diff --git a/pylabrobot/visualizer3D/browser_tests.py b/pylabrobot/visualizer3D/browser_tests.py index 04156acf6e1..62ddbb47805 100644 --- a/pylabrobot/visualizer3D/browser_tests.py +++ b/pylabrobot/visualizer3D/browser_tests.py @@ -40,7 +40,8 @@ def _find_chrome() -> str: - """Where a headless Chrome is, or empty: `PLR_CHROME`, then the path, then the macOS install.""" + """Where a headless Chrome is, or empty: `PLR_CHROME`, then the path, then the macOS and Windows + installs.""" named = os.environ.get("PLR_CHROME", "") if named: return named @@ -48,8 +49,14 @@ def _find_chrome() -> str: found = shutil.which(name) if found is not None: return found - installed = "/Applications/Google Chrome.app/Contents/MacOS/Google Chrome" - return installed if os.path.isfile(installed) else "" + for installed in ( + "/Applications/Google Chrome.app/Contents/MacOS/Google Chrome", + os.path.expandvars(r"%ProgramFiles%\Google\Chrome\Application\chrome.exe"), + os.path.expandvars(r"%ProgramFiles(x86)%\Google\Chrome\Application\chrome.exe"), + ): + if os.path.isfile(installed): + return installed + return "" CHROME = _find_chrome() @@ -273,6 +280,11 @@ def modelled(viewer: Viewer3D) -> int: return sum(1 for model in scene["instances"]["model"] if "mesh" in scene["models"][model]) +def listed(viewer: Viewer3D) -> int: + """How many resources the scene names: what `resources()` counts once all have arrived.""" + return len(viewer._scene_message()["instances"]["names"]) + + @unittest.skipUnless(CHROME, "no headless browser to drive") class BrowserTests(unittest.IsolatedAsyncioTestCase): async def asyncSetUp(self): @@ -727,7 +739,9 @@ async def test_after_a_run_every_resource_is_drawn_where_the_tree_has_it(self): try: async with Browser() as browser: await browser.open(viewer.url) - await browser.settle("window.plrViewer && window.plrViewer.resources().length > 3000", 60) + await browser.settle( + f"window.plrViewer && window.plrViewer.resources().length === {listed(viewer)}", 60 + ) await browser.settle(f"window.plrViewer.models().length === {modelled(viewer)}", 60) await browser.frames() self.assertGreater(await browser.drawn_fraction("#viewport"), 0.02, "the opening view") @@ -826,7 +840,9 @@ async def test_a_tip_is_listed_by_what_holds_it(self): try: async with Browser() as browser: await browser.open(viewer.url) - await browser.settle("window.plrViewer && window.plrViewer.resources().length > 3000", 60) + await browser.settle( + f"window.plrViewer && window.plrViewer.resources().length === {listed(viewer)}", 60 + ) rack = star.deck.get_resource("tips_0") assert isinstance(rack, TipRack) and star.pipettes is not None spot = rack.get_item("A1") diff --git a/pylabrobot/visualizer3D/collision_demo.py b/pylabrobot/visualizer3D/collision_demo.py new file mode 100644 index 00000000000..edb6919f3aa --- /dev/null +++ b/pylabrobot/visualizer3D/collision_demo.py @@ -0,0 +1,112 @@ +"""A grip with no room beside the plate: the whole method is judged, then played to the meeting. + +Run it: + + python -m pylabrobot.visualizer3D.collision_demo + +The tip carrier stands flush against the source carrier, as a packed deck puts them. The iSWAP's +pick-up is judged before anything moves - the whole method, one way from where the arm stands - and +the fingers meet the carrier's outer wall on the way down to the plate, at the opening width the +grip itself uses. The clean steps then play as they would, and the descent is driven part way, to +the instant of the meeting: the arm stands where the fingers would have hit, with what they met +drawn over the scene. +""" + +import asyncio +import logging + +from pylabrobot.hamilton.star.driver.features.iswap_collisions import judge +from pylabrobot.hamilton.star.driver.features.iswap_transport import ( + Rise, + iSWAPTransport, +) +from pylabrobot.hamilton.star.motion import attach_viewer_collisions, attach_viewer_motion + +from .demo import build_facility, star_of +from .server import Viewer3D + + +async def main() -> None: + logging.disable(logging.WARNING) + facility = build_facility() + star = star_of(facility) + deck = star.deck + deck.get_resource("destination_1").unassign() + # The carriers stand flush, as a packed deck puts them: the tip carrier's outer wall 1 mm from + # the source carrier, and no room for the fingers beside the plate they grip. + source_carrier = deck.get_resource("source_carrier") + deck.unassign_child_resource(source_carrier) + deck.assign_child_resource(source_carrier, track=7) + await star.setup() + + iswap = star.iswap + if iswap is None: + raise RuntimeError("the simulated STARlet has no iSWAP") + viewer = Viewer3D(facility, name="collision_demo.py", open_browser=False, raise_on_collision=True) + await viewer.start() + attach_viewer_motion(star.driver, viewer) + transport = iSWAPTransport(iswap, check_collisions=False) + attach_viewer_collisions(star.driver, viewer, transport) + print("waiting for a browser to draw the scene") + await viewer.wait_for_browser() + await viewer.wait_for_models() + print("the page is up; the run starts in fifteen seconds") + await asyncio.sleep(15.0) + + await iswap.make_space() + print("the arm is cleared; the pick-up is judged before anything moves") + + plate = deck.get_resource("source_1") + plan = transport.plan_pick_up(plate, direction="front") + found, _ = judge(transport, plan) + if not found: + print("the pick-up was clean after all; it plays as it normally would") + for step in plan.steps: + await transport._do(step, plan) + print("the run is paused where it ended; ctrl-c ends the demo") + await asyncio.Event().wait() + + # Where the way can be walked to the meeting, it is: the clean steps play as they would, and the + # step the meeting falls in is driven part way there, so the arm stands where the fingers would + # have met. A meeting the walk could not place inside the way - a hull that only leans on what it + # sweeps - is not driven to: the run stops where it stands and the boxes say what would have met. + met = [c for c in found if c.when is not None] + if not met: + await viewer.show_collisions(found) + hit = ", ".join(sorted({collision.obstacle.name for collision in found})) + print(f"the pick-up was refused before anything moved: the fingers met {hit}") + for collision in found: + print(f" {collision}") + print("the run is paused where it stands; ctrl-c ends the demo") + await asyncio.Event().wait() + first = min(met, key=lambda c: c.when if c.when is not None else float("inf")) + when = first.when + assert when is not None + step_index = int(when) + step = plan.steps[step_index] + moment = when - step_index + for earlier in plan.steps[:step_index]: + await transport._do(earlier, plan) + if not isinstance(step, Rise): + raise RuntimeError(f"acting out a meeting inside a {type(step).__name__} is not done here") + stood_at = max(s.z for s in plan.steps[:step_index] if isinstance(s, Rise)) + here = await iswap.elbow_request_z_position() + await iswap.elbow_move_to_z_position( + round(here + (step.z - stood_at) * moment, 1), + speed=step.speed, + acceleration=step.acceleration, + ) + await viewer.show_collisions(found) + hit = ", ".join(sorted({collision.obstacle.name for collision in found})) + print(f"the way is held {moment * 100:.0f}% into the descent: the fingers met {hit}") + for collision in found: + print(f" {collision}") + print("the run is paused where it would have met; ctrl-c ends the demo") + await asyncio.Event().wait() + + +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + pass diff --git a/pylabrobot/visualizer3D/core_gripper_demo.py b/pylabrobot/visualizer3D/core_gripper_demo.py new file mode 100644 index 00000000000..d7da55fae11 --- /dev/null +++ b/pylabrobot/visualizer3D/core_gripper_demo.py @@ -0,0 +1,78 @@ +"""The CO-RE gripper: tools on, a plate gripped off its site, carried across the deck, put down. + +The plate hangs from the front tool's jaws in the model, rides the X-arm, and passes to the site +when the jaws open. + +Run it: + + python -m pylabrobot.visualizer3D.core_gripper_demo +""" + +import asyncio +import logging + +from pylabrobot.resources.plate import Plate + +from ..hamilton.star.motion import attach_viewer_motion +from .demo import build_facility, star_of +from .server import Viewer3D + + +async def main() -> None: + logging.disable(logging.WARNING) + + facility = build_facility() + star = star_of(facility) + await star.setup() + grippers = star.core_grippers + if grippers is None: + raise RuntimeError("the simulated STARlet has no CO-RE grippers") + + deck = star.deck + source = deck.get_resource("source_0") + if not isinstance(source, Plate): + raise RuntimeError("the demo deck no longer holds the plate this run grips") + + # The site it goes to has to be free: the destination carrier's front-most site stands empty. + destination_carrier = deck.get_resource("destination_carrier") + site = min(destination_carrier.children, key=lambda s: s.get_absolute_location().y) + held = [child for child in site.children if isinstance(child, Plate)] + for plate in held: + plate.unassign() + + viewer = Viewer3D(facility, name="core_gripper_demo.py", open_browser=False) + await viewer.start() + attach_viewer_motion(star.driver, viewer) + print("waiting for a browser to draw the scene") + await viewer.wait_for_browser() + await viewer.wait_for_models() + print("the page is up; the run starts in ten seconds") + await asyncio.sleep(10.0) + + print("picking up the CO-RE grip tools") + await grippers.pick_up_tools(front_channel=7) + + print(f"gripping {source.name} where it stands") + await grippers.pick_up_resource(source) + + here = source.get_absolute_location().x + there = site.get_absolute_location().x + print(f"carrying it across the deck, {there - here:.0f} mm to the front-most destination site") + x = await star.x_arm.request_position() + await star.x_arm.move_to_x_position(x + (there - here)) + + print("putting it down on the site") + await grippers.drop_resource(site) + + print("returning the tools to their holder") + await grippers.drop_tools() + + print("run complete; the viewer stays up") + await asyncio.Event().wait() + + +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + pass diff --git a/pylabrobot/visualizer3D/demo.py b/pylabrobot/visualizer3D/demo.py index e771696d102..222c2259545 100644 --- a/pylabrobot/visualizer3D/demo.py +++ b/pylabrobot/visualizer3D/demo.py @@ -24,13 +24,9 @@ hamilton_96_tiprack_10uL_NTR, hamilton_96_tiprack_50uL_NTR, hamilton_96_tiprack_300uL_filter, - hamilton_96_tiprack_300uL_filter_slim, hamilton_96_tiprack_300uL_NTR, hamilton_96_tiprack_1000uL, hamilton_96_tiprack_1000uL_filter, - hamilton_mfx_carrier_L5_base, - hamilton_mfx_resourceholder_ntr, - hamilton_mfx_tiprackholder_standard, hamilton_tip_carrier_L5_ntr_a00, ) from pylabrobot.resources.plate import Plate @@ -70,7 +66,9 @@ def build_facility(*, bare: bool = False) -> Facility: facility.assign_child_resource(bench, location=Coordinate(0, 0, 0)) tip_carrier = TIP_CAR_480_A00(name="tip_carrier") - # Filtered and unfiltered side by side, so the filter in a tip can be told apart. + # Filtered and unfiltered side by side, so the filter in a tip can be told apart. Its declared + # shape stands where its model does, so the plate carriers stand a track clear of it: a wall + # flush against a gripped plate is in the fingers' way. tip_carrier[0] = hamilton_96_tiprack_1000uL(name="tips_0") tip_carrier[1] = hamilton_96_tiprack_1000uL_filter(name="tips_1") tip_carrier[2] = hamilton_96_tiprack_300uL_filter(name="tips_2") @@ -79,37 +77,18 @@ def build_facility(*, bare: bool = False) -> Facility: plate_carrier = PLT_CAR_L5AC_A00(name="source_carrier") for slot in range(5): plate_carrier[slot] = cor_96_wellplate_360uL_Fb(name=f"source_{slot}") - star.deck.assign_child_resource(plate_carrier, track=7) + star.deck.assign_child_resource(plate_carrier, track=8) destination_carrier = PLT_CAR_L5AC_A00(name="destination_carrier") for slot in range(5): destination_carrier[slot] = cor_96_wellplate_360uL_Fb(name=f"destination_{slot}") - star.deck.assign_child_resource(destination_carrier, track=13) + star.deck.assign_child_resource(destination_carrier, track=14) ntr_carrier = hamilton_tip_carrier_L5_ntr_a00(name="ntr_carrier") ntr_carrier[0] = hamilton_96_tiprack_10uL_NTR(name="ntr_10uL") ntr_carrier[1] = hamilton_96_tiprack_50uL_NTR(name="ntr_50uL") ntr_carrier[2] = hamilton_96_tiprack_300uL_NTR(name="ntr_300uL") - star.deck.assign_child_resource(ntr_carrier, track=19) - - # The same nested racks on MFX NTR4 modules, and two MFX tip modules holding framed racks: the - # last six tracks, up to the waste block. - modules = { - 0: hamilton_mfx_resourceholder_ntr(name="mfx_ntr4_0"), - 1: hamilton_mfx_resourceholder_ntr(name="mfx_ntr4_1"), - 2: hamilton_mfx_resourceholder_ntr(name="mfx_ntr4_2"), - 3: hamilton_mfx_tiprackholder_standard(name="mfx_tiprack_holder_3"), - 4: hamilton_mfx_tiprackholder_standard(name="mfx_tiprack_holder_4"), - } - mfx_carrier = hamilton_mfx_carrier_L5_base(name="mfx_carrier", modules=modules) - modules[0].assign_child_resource(hamilton_96_tiprack_10uL_NTR(name="mfx_ntr_10uL")) - modules[1].assign_child_resource(hamilton_96_tiprack_50uL_NTR(name="mfx_ntr_50uL")) - modules[2].assign_child_resource(hamilton_96_tiprack_300uL_NTR(name="mfx_ntr_300uL")) - modules[3].assign_child_resource(hamilton_96_tiprack_1000uL_filter(name="mfx_tips_1000uL_filter")) - modules[4].assign_child_resource( - hamilton_96_tiprack_300uL_filter_slim(name="mfx_tips_300uL_filter_slim") - ) - star.deck.assign_child_resource(mfx_carrier, track=25) + star.deck.assign_child_resource(ntr_carrier, track=20) # The deck's own `size_z` (900 mm) is the working envelope, not its extent. The top of the X-arm # riding above it is the honest height: channel travel plus the arm's own height. diff --git a/pylabrobot/visualizer3D/head96_demo.py b/pylabrobot/visualizer3D/head96_demo.py new file mode 100644 index 00000000000..a341babe367 --- /dev/null +++ b/pylabrobot/visualizer3D/head96_demo.py @@ -0,0 +1,71 @@ +"""The 96 head: a full plate of tips, a full plate of aspiration, a full plate of dispensing. + +The head moves as one: down to the liquid's surface, following it as it draws, then out and over. + +Run it: + + python -m pylabrobot.visualizer3D.head96_demo +""" + +import asyncio +import logging + +from pylabrobot.resources import set_volume_tracking +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.tip_rack import TipRack +from pylabrobot.resources.tip_tracking import set_tip_tracking + +from ..hamilton.star.motion import attach_viewer_motion +from .demo import build_facility, fill, star_of +from .server import Viewer3D + + +async def main() -> None: + logging.disable(logging.WARNING) + set_volume_tracking(True) + set_tip_tracking(True) + + facility = build_facility() + star = star_of(facility) + await star.setup() + head96 = star.head96 + if head96 is None: + raise RuntimeError("the simulated STARlet has no 96 head") + + deck = star.deck + rack = deck.get_resource("tips_0") + if not isinstance(rack, TipRack): + raise RuntimeError("the demo deck no longer holds the tips this run uses") + source = deck.get_resource("source_0") + destination = deck.get_resource("destination_0") + if not isinstance(source, Plate) or not isinstance(destination, Plate): + raise RuntimeError("the demo deck no longer holds the plates this run uses") + fill(source, 200.0) + + viewer = Viewer3D(facility, name="head96_demo.py", open_browser=False) + await viewer.start() + attach_viewer_motion(star.driver, viewer) + print("waiting for a browser to draw the scene") + await viewer.wait_for_browser() + await viewer.wait_for_models() + print("the page is up; the run starts in ten seconds") + await asyncio.sleep(10.0) + + print("picking up ninety-six tips") + await head96.pick_up_tips(rack) + + print("aspirating fifty uL from every well of the source plate") + await head96.aspirate(source, 50.0) + + print("dispensing into the destination plate") + await head96.dispense(destination, 50.0) + + print("run complete; the viewer stays up") + await asyncio.Event().wait() + + +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + pass diff --git a/pylabrobot/visualizer3D/iswap_demo.py b/pylabrobot/visualizer3D/iswap_demo.py new file mode 100644 index 00000000000..55d3ef7d01a --- /dev/null +++ b/pylabrobot/visualizer3D/iswap_demo.py @@ -0,0 +1,185 @@ +"""The iSWAP carrying a lidded plate between two carrier sites, and taking its lid off and on. + +Run it: + + python -m pylabrobot.visualizer3D.iswap_demo + +The arm is driven only by the PR's primitive moves - the X-arm, the head's Y and Z, the two joints +and the jaws - each of which the page plays at the speed the command carries. PyLabRobot does not +move what the iSWAP grips, so the page does: it hands the labware to the gripper when the jaws close +on it and to what is under it when they open - a site, or for a lid, a plate with no lid. + +Until stopped: the plate goes over with its lid, the lid comes off onto the empty site, goes back +on, and the plate comes back. +""" + +import asyncio +import logging +from typing import List + +from pylabrobot.hamilton.star.motion import attach_viewer_motion +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.corning.plates import ( + cor_96_wellplate_360uL_Fb, + cor_96_wellplate_360uL_Fb_lid, +) +from pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00 +from pylabrobot.resources.lid import Lid +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.resource import Resource + +from .demo import build_facility, star_of +from .server import Viewer3D + +# How far above a plate's bottom the jaws take hold of it, in mm: below where its lid comes down +# over it, or they would close on the lid. +PLATE_GRIP_HEIGHT = 4.0 +# How far above a lid's bottom the jaws take hold of it, in mm. +LID_GRIP_HEIGHT = 5.0 +# How much narrower than what they hold the jaws close to, in mm: they stop on it. +SQUEEZE = 3.0 + + +def grip_centre(iswap, deck: Resource) -> Coordinate: + """Where the model has the point the jaws close on, in mm on the deck.""" + gripper = iswap.gripper + local = (gripper.proximal_joint + gripper.tool_center_point).vector() + turn = gripper.get_absolute_rotation().get_rotation_matrix() + base = gripper.get_location_wrt(deck) + moved = [sum(turn[row][k] * local[k] for k in range(3)) for row in range(3)] + return Coordinate(base.x + moved[0], base.y + moved[1], base.z + moved[2]) + + +async def move_grip_centre(iswap, deck: Resource, target: Coordinate, axes: str) -> None: + """Bring the grip centre to `target` along the named axes, the joints held where they are.""" + offset = target - grip_centre(iswap, deck) + if "x" in axes: + x = await iswap.arm.request_position() + await iswap.arm.move_to_x_position(round(x + offset.x, 1)) + if "y" in axes: + y = await iswap.elbow_request_y_position() + await iswap.elbow_move_to_y_position(round(y + offset.y, 1)) + if "z" in axes: + z = await iswap.elbow_request_z_position() + await iswap.elbow_move_to_z_position(round(z + offset.z, 1)) + + +async def carry(iswap, deck: Resource, width: float, here: Coordinate, there: Coordinate, travel_z): + """Take what is `width` wide and gripped at `here` to `there`, travelling at `travel_z`.""" + await iswap.gripper_open() + await move_grip_centre(iswap, deck, Coordinate(here.x, here.y, travel_z), "xy") + await move_grip_centre(iswap, deck, here, "z") + await iswap.gripper_move_to_jaw_position(width - SQUEEZE) + await move_grip_centre(iswap, deck, Coordinate(here.x, here.y, travel_z), "z") + await move_grip_centre(iswap, deck, Coordinate(there.x, there.y, travel_z), "xy") + await move_grip_centre(iswap, deck, there, "z") + await iswap.gripper_open() + await move_grip_centre(iswap, deck, Coordinate(there.x, there.y, travel_z), "z") + + +def surface(site: Resource, deck: Resource) -> Coordinate: + """The middle of a site's surface, in mm on the deck.""" + return site.get_location_wrt(deck, "c", "c", "b") + + +def plate_grip(site: Resource, plate: Plate, deck: Resource) -> Coordinate: + """Where the jaws close on `plate` standing on `site`.""" + seated = plate.location.z if plate.location is not None else 0.0 + return surface(site, deck) + Coordinate(0, 0, seated + PLATE_GRIP_HEIGHT) + + +def lid_grip_on_plate(site: Resource, plate: Plate, lid: Lid, deck: Resource) -> Coordinate: + """Where the jaws close on `lid` covering `plate` on `site`: its bottom is the plate's top less + the height it nests over it.""" + seated = plate.location.z if plate.location is not None else 0.0 + bottom = seated + plate.get_size_z() - lid.nesting_z_height + return surface(site, deck) + Coordinate(0, 0, bottom + LID_GRIP_HEIGHT) + + +def lid_grip_on_site(site: Resource, deck: Resource) -> Coordinate: + """Where the jaws close on a lid lying on an empty `site`.""" + return surface(site, deck) + Coordinate(0, 0, LID_GRIP_HEIGHT) + + +async def main() -> None: + logging.disable(logging.WARNING) + facility = build_facility(bare=True) + star = star_of(facility) + deck = star.deck + + # A carrier of its own on an otherwise empty deck, its two sites as far apart as the carrier + # comes: the jaws open wider than what they grip, so everything around a move needs room for + # the open jaws, and out here there is nothing to clip. + carrier = PLT_CAR_L5AC_A00(name="plate_carrier") + deck.assign_child_resource(carrier, track=10) + + plate = cor_96_wellplate_360uL_Fb(name="plate") + carrier[0] = plate + lid = cor_96_wellplate_360uL_Fb_lid(name="plate_lid") + plate.assign_child_resource(lid) + await star.setup() + iswap = star.iswap + if iswap is None: + raise RuntimeError("the simulated STARlet has no iSWAP") + + # The turned gripper reaches less far than the elbow's own travel: with the wrist at -135 the + # grip centre hangs behind the elbow, so the farthest site the pose can bring the jaws to is the + # elbow's front stop less that hang - site three, not site four. + if plate.parent is None: + raise RuntimeError("the plate has no parent site to grip it at") + sites: List[Resource] = [plate.parent, carrier[3]] + + viewer = Viewer3D(facility, name="iswap_demo.py", open_browser=False) + await viewer.start() + attach_viewer_motion(star.driver, viewer) + print("waiting for a browser to draw the scene") + await viewer.wait_for_browser() + await viewer.wait_for_models() + print("the page is up; the run starts in ten seconds") + await asyncio.sleep(10.0) + + # Clear of the channels, then turned so the jaws close across the plate's short side. The elbow + # is brought forward by the first carry itself: its reach envelope depends on the turn, so an + # absolute jog from the folded park would guess wrong. + await iswap.make_space() + await iswap.rotate_to_angles(elbow_absolute_angle="front", gripper_absolute_angle="left") + travel_z = grip_centre(iswap, deck).z + + width = plate.get_size_y() + lid_width = lid.get_size_y() + while True: + start, other = sites + print("the plate goes over, with its lid") + await carry( + iswap, deck, width, plate_grip(start, plate, deck), plate_grip(other, plate, deck), travel_z + ) + print("its lid comes off, onto the site it left") + await carry( + iswap, + deck, + lid_width, + lid_grip_on_plate(other, plate, lid, deck), + lid_grip_on_site(start, deck), + travel_z, + ) + print("and goes back on") + await carry( + iswap, + deck, + lid_width, + lid_grip_on_site(start, deck), + lid_grip_on_plate(other, plate, lid, deck), + travel_z, + ) + print("the plate comes back") + await carry( + iswap, deck, width, plate_grip(other, plate, deck), plate_grip(start, plate, deck), travel_z + ) + await asyncio.sleep(1.0) + + +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + pass diff --git a/pylabrobot/visualizer3D/motion_demo.py b/pylabrobot/visualizer3D/motion_demo.py new file mode 100644 index 00000000000..d806c1e0622 --- /dev/null +++ b/pylabrobot/visualizer3D/motion_demo.py @@ -0,0 +1,72 @@ +"""A simulated STARlet pipetting, with the drives' motion acted out between the commands. + +Run it: + + python -m pylabrobot.visualizer3D.motion_demo + +The viewer reads each firmware command for its targets and the page plays how the drives get +there: across (the arm and the channels at once), down, tips taken or left at the bottom, back up. +Each command waits until the page has played it, so the run goes at the pace of the picture. Add +`?motion=2` to the address (before the `#`) for twice the speed, `?motion=0` to skip the motion. + +Column by column: pick up eight tips, aspirate from the source plate, put the tips back. +""" + +import asyncio +import logging + +from pylabrobot.hamilton.star.motion import attach_viewer_motion +from pylabrobot.resources import set_volume_tracking +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.tip_rack import TipRack +from pylabrobot.resources.tip_tracking import set_tip_tracking + +from .demo import build_facility, fill, star_of +from .server import Viewer3D + +ROWS = "ABCDEFGH" + + +async def main() -> None: + logging.disable(logging.WARNING) + set_volume_tracking(True) + # So a tip picked up is the one that was in the spot, carried to the channel and back. + set_tip_tracking(True) + + facility = build_facility() + star = star_of(facility) + await star.setup() + pipettes = star.pipettes + if pipettes is None: + raise RuntimeError("the simulated STARlet has no channels") + + rack = star.deck.get_resource("tips_0") + source = star.deck.get_resource("source_0") + if not isinstance(rack, TipRack) or not isinstance(source, Plate): + raise TypeError("the demo deck no longer holds the rack and plate this run uses") + fill(source, 300.0) + + viewer = Viewer3D(facility, name="motion_demo.py") + await viewer.start() + attach_viewer_motion(star.driver, viewer) + print("waiting for a browser to draw the scene") + await viewer.wait_for_browser() + await asyncio.sleep(1.0) + + for column in range(1, 13): + tips = [rack.get_item(f"{row}{column}") for row in ROWS] + wells = [source.get_item(f"{row}{column}") for row in ROWS] + print(f"column {column}: pick up, aspirate, put back") + await pipettes.pick_up_tips(tips) + await pipettes.aspirate(wells, [50.0] * len(wells)) + await pipettes.drop_tips(tips) + + print("run complete; the viewer stays up") + await asyncio.Event().wait() + + +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + pass diff --git a/pylabrobot/visualizer3D/motion_player_tests.mjs b/pylabrobot/visualizer3D/motion_player_tests.mjs new file mode 100644 index 00000000000..1a1cce5c338 --- /dev/null +++ b/pylabrobot/visualizer3D/motion_player_tests.mjs @@ -0,0 +1,522 @@ +// The motion player, outside a page: `node --test pylabrobot/visualizer3D/motion_player_tests.mjs`. +// Run from Python by `motion_tests.py`, which skips it where there is no Node. + +import assert from "node:assert/strict"; +import { test } from "node:test"; + +import { heldAt, siteUnder } from "./static/carry.js"; +import { createPlayer, turnedLocation } from "./static/motion_player.js"; +import { motionProfile } from "./static/motion_profile.js"; + +const DRIVES = { + x: { speed: 400, acceleration: 500 }, + y: { speed: 250, acceleration: null }, + z: { speed: 125, acceleration: 800 }, +}; + +// A world of named resources with positions, recording every change in order. +function fakeWorld(positions) { + const names = Object.keys(positions); + const at = names.map((n) => [...positions[n]]); + const log = []; + const parents = {}; + return { + log, + parents, + at: (name) => at[names.indexOf(name)], + deps: (skipping = () => false) => ({ + readAxis: (i, axis) => at[i][axis], + setAxis: (i, axis, value) => { + at[i][axis] = value; + log.push({ name: names[i], axis, value }); + }, + indexOf: (name) => (names.includes(name) ? names.indexOf(name) : undefined), + attach: (name, parent) => { + parents[name] = parent; + log.push({ attach: name, parent, z: at[names.indexOf("ch0")][2] }); + }, + skipping, + }), + }; +} + +// Play to the end in fixed frames, as the frame loop would, and say how long it took. +async function playOut(player, request, frame = 1 / 60) { + let done = false; + const playing = player.play(request).then(() => { + done = true; + }); + let seconds = 0; + for (let i = 0; i < 100000 && !done; i++) { + await new Promise((resolve) => setImmediate(resolve)); // let finished moves start the next + if (done) break; + player.step(frame); + seconds += frame; + } + await playing; + return seconds; +} + +const pickUp = { + kind: "tip_pickup", + arm: { name: "arm", x: 300 }, + traverse: [{ name: "ch0", z: 0 }], + channels: [{ name: "ch0", channel: 0, y: 200, down: -100, end: 10 }], + attach: [{ name: "tip", parent: "shaft0" }], + dwell: 0, + drives: DRIVES, +}; + +const start = () => ({ arm: [100, 0, 0], ch0: [0, 150, 10], tip: [0, 0, 0] }); + +test("a pick-up goes across, then down, takes the tip at the bottom, then comes up", async () => { + const world = fakeWorld(start()); + await playOut(createPlayer(world.deps()), pickUp); + + const firstZ = world.log.findIndex((e) => e.axis === 2); + const across = world.log.slice(0, firstZ); + assert.ok(across.some((e) => e.name === "arm") && across.some((e) => e.axis === 1)); + // The arm and the channel travel together: their changes interleave rather than follow. + const lastArm = across.findLastIndex((e) => e.name === "arm"); + const firstY = across.findIndex((e) => e.axis === 1); + assert.ok(firstY < lastArm, "the channel waited for the arm"); + + const handover = world.log.findIndex((e) => e.attach === "tip"); + assert.ok(handover > firstZ, "the tip was taken before the channel went down"); + assert.equal(world.log[handover].z, -100, "the tip was taken away from the bottom"); + assert.equal(world.parents.tip, "shaft0"); + const after = world.log.slice(handover + 1).filter((e) => e.axis === 2); + assert.ok(after.length > 1 && after.at(-1).value === 10, "the channel did not come back up"); + assert.deepEqual(world.at("arm"), [300, 0, 0]); + assert.deepEqual(world.at("ch0"), [0, 200, 10]); +}); + +test("a move passes through the positions between, forward only", async () => { + const world = fakeWorld(start()); + await playOut(createPlayer(world.deps()), pickUp); + const xs = world.log.filter((e) => e.name === "arm").map((e) => e.value); + assert.ok(xs.filter((x) => x > 101 && x < 299).length >= 10, "the arm jumped"); + for (let i = 1; i < xs.length; i++) assert.ok(xs[i] >= xs[i - 1], "the arm went backwards"); +}); + +test("a motion takes as long as its drives say", async () => { + const world = fakeWorld(start()); + const seconds = await playOut(createPlayer(world.deps()), pickUp); + // Across is the slower of X and Y; then down and back up in Z. + const across = Math.max( + motionProfile(200, 400, 500).duration, + motionProfile(50, 250, null).duration, + ); + const expected = + across + motionProfile(110, 125, 800).duration + motionProfile(110, 125, 800).duration; + assert.ok(Math.abs(seconds - expected) < 0.2, `took ${seconds}, expected ${expected}`); +}); + +test("an aspiration dwells at the bottom and leaves the liquid at its own speed", async () => { + const world = fakeWorld(start()); + const aspirate = { + ...pickUp, + kind: "aspirate", + attach: [], + dwell: 1.5, + channels: [{ name: "ch0", channel: 0, y: 150, down: -100, end: 10, leave: -90, leave_speed: 5 }], + }; + const quick = await playOut(createPlayer(fakeWorld(start()).deps()), { ...aspirate, dwell: 0 }); + const withDwell = await playOut(createPlayer(world.deps()), aspirate); + assert.ok(Math.abs(withDwell - quick - 1.5) < 0.05, "the dwell was not waited out"); + // 10 mm at 5 mm/s is at least 2 s, far slower than the drive's own 125 mm/s. + const leaving = world.log.filter((e) => e.axis === 2 && e.value > -100 && e.value <= -90); + assert.ok(leaving.length > 100, "it left the liquid at the drive's speed, not its own"); +}); + +test("a command's fixed time is spent before anything moves", async () => { + const world = fakeWorld(start()); + const quick = await playOut(createPlayer(fakeWorld(start()).deps()), pickUp); + const player = createPlayer(world.deps()); + let seconds = 0; + const playing = player.play({ ...pickUp, fixed: 1.2 }); + for (; seconds < 1.1; seconds += 1 / 60) { + await new Promise((resolve) => setImmediate(resolve)); + player.step(1 / 60); + } + assert.equal(world.log.length, 0, "something moved during the fixed time"); + let rest = 0; + let done = false; + playing.then(() => (done = true)); + for (let i = 0; i < 100000 && !done; i++) { + await new Promise((resolve) => setImmediate(resolve)); + if (done) break; + player.step(1 / 60); + rest += 1 / 60; + } + assert.ok(Math.abs(seconds + rest - quick - 1.2) < 0.05, `took ${seconds + rest}, expected ${quick + 1.2}`); +}); + +test("the channels set off in Y one after another, each a stagger after the last", async () => { + const positions = { arm: [100, 0, 0], ch0: [0, 150, 10], ch1: [0, 140, 10], ch2: [0, 130, 10] }; + const world = fakeWorld(positions); + const request = { + ...pickUp, + arm: null, + attach: [], + traverse: [], + channels: [ + { name: "ch2", channel: 2, y: 30 }, + { name: "ch0", channel: 0, y: 50 }, + { name: "ch1", channel: 1, y: 40 }, + ], + drives: { ...DRIVES, y: { speed: 250, acceleration: 800, stagger: 0.5 } }, + }; + const seconds = await playOut(createPlayer(world.deps()), request); + // All three move toward the front, so the front-most channel leaves first and the ripple runs + // back: ch2 first, ch0 last, two staggers after it. A channel that set off before the one in + // front of it had moved would run into the back of it. + const first = (name) => world.log.findIndex((e) => e.name === name); + assert.ok(first("ch2") < first("ch1") && first("ch1") < first("ch0"), "not leader-first"); + const expected = 2 * 0.5 + motionProfile(100, 250, 800).duration; + assert.ok(Math.abs(seconds - expected) < 0.05, `took ${seconds}, expected ${expected}`); + assert.deepEqual([world.at("ch0")[1], world.at("ch1")[1], world.at("ch2")[1]], [50, 40, 30]); +}); + +test("a tip pick-up presses the last stretch at its own slow speed", async () => { + const world = fakeWorld(start()); + const pressing = { + ...pickUp, + channels: [{ name: "ch0", channel: 0, y: 150, down: -90, press: -100, press_speed: 10, end: 10 }], + }; + const seconds = await playOut(createPlayer(world.deps()), pressing); + const handover = world.log.findIndex((e) => e.attach === "tip"); + assert.equal(world.log[handover].z, -100, "the tip was taken before the press ended"); + const slow = world.log.filter((e) => e.axis === 2 && e.value < -90 && e.value >= -100); + assert.ok(slow.length > 50, "the press ran at the drive's speed"); + assert.ok(seconds > 1.0, `10 mm at 10 mm/s took only ${seconds}`); +}); + +test("an aspiration follows the surface down while it draws, then pulls out", async () => { + const world = fakeWorld(start()); + const aspirate = { + ...pickUp, + kind: "aspirate", + arm: null, + attach: [], + dwell: 2, + channels: [ + { + name: "ch0", + channel: 0, + y: 150, + down: -100, + follow: -104, + follow_speed: 2, + leave: -100, + leave_speed: 50, + pull_out: -90, + end: 10, + }, + ], + }; + const seconds = await playOut(createPlayer(world.deps()), aspirate); + const zs = world.log.filter((e) => e.name === "ch0" && e.axis === 2).map((e) => e.value); + const lowest = Math.min(...zs); + assert.ok(Math.abs(lowest + 104) < 0.01, `it went down to ${lowest}, not the followed -104`); + // 4 mm at 2 mm/s fills the 2 s dwell: steadily, not in a jump. + const following = world.log.filter((e) => e.axis === 2 && e.value < -100 && e.value > -104); + assert.ok(following.length > 60, "it jumped rather than followed"); + // After the lowest point: out to the surface, then on up through the pull-out stretch. + const after = zs.slice(zs.indexOf(lowest)); + assert.ok(after.filter((z) => z > -95 && z < -90).length > 3, "it did not pull out steadily"); + assert.equal(world.at("ch0")[2], 10); + assert.ok(seconds > 2, `took only ${seconds}`); +}); + +test("a pick-up starts down before its crossing has finished", async () => { + const request = { ...pickUp, attach: [], down_from: 0.5 }; + const world = fakeWorld(start()); + const overlapped = await playOut(createPlayer(world.deps()), request); + const sequence = await playOut(createPlayer(fakeWorld(start()).deps()), { ...request, down_from: 0 }); + assert.ok(overlapped < sequence - 0.1, `overlapped ${overlapped}, in sequence ${sequence}`); + // The channel was going down while the arm still moved. + const firstDown = world.log.findIndex((e) => e.name === "ch0" && e.axis === 2); + const lastArm = world.log.findLastIndex((e) => e.name === "arm"); + assert.ok(firstDown < lastArm, "the descent waited for the arm"); + assert.deepEqual(world.at("arm"), [300, 0, 0]); +}); + +test("a page in the background jumps to the end and still hands the tips over", async () => { + const world = fakeWorld(start()); + const player = createPlayer(world.deps(() => true)); + await player.play(pickUp); // resolves with no frames at all + assert.deepEqual(world.at("ch0"), [0, 200, 10]); + assert.equal(world.parents.tip, "shaft0"); + assert.equal(player.step(0.016), false); +}); + +test("speed 0 jumps to the end", async () => { + const world = fakeWorld(start()); + const player = createPlayer(world.deps()); + player.setSpeed(0); + await player.play(pickUp); + assert.deepEqual(world.at("arm"), [300, 0, 0]); +}); + +test("a resource the page does not have is skipped, not waited for", async () => { + const world = fakeWorld(start()); + const request = { ...pickUp, arm: { name: "gone", x: 5 } }; + await playOut(createPlayer(world.deps()), request); + assert.deepEqual(world.at("ch0"), [0, 200, 10]); +}); + +test("the frame loop keeps going between one move ending and the next starting", async () => { + const world = fakeWorld(start()); + const player = createPlayer(world.deps()); + const playing = player.play(pickUp); + for (let i = 0; i < 2000; i++) { + await new Promise((resolve) => setImmediate(resolve)); + if (!player.step(1 / 60)) break; + } + await playing; + assert.deepEqual(world.at("ch0"), [0, 200, 10], "the loop stopped with the motion unfinished"); +}); + +test("the profile reaches the end exactly and never overshoots", () => { + for (const [d, v, a] of [ + [200, 400, 500], + [5, 400, 500], + [50, 250, null], + ]) { + const p = motionProfile(d, v, a); + assert.equal(p.progress(p.duration), 1); + for (let t = 0; t <= p.duration; t += p.duration / 50) { + assert.ok(p.progress(t) >= 0 && p.progress(t) <= 1); + } + } +}); + +// -- iSWAP ----------------------------------------------------------------------------------------- + +test("a turn keeps its pivot where PyLabRobot's rotate_to keeps it", () => { + // iSWAP link 1 in the demo, turned by `rotate_to(z=200, pivot_coordinate=proximal_joint)`. + const pivot = { x: 12.7, y: 12.75, z: -20.3 }; + const after = turnedLocation({ x: 2.8527, y: 2.2055, z: -15.3 }, 1.34477, 200, pivot); + assert.ok(Math.abs(after.x - 22.8233) < 1e-3 && Math.abs(after.y - 31.5748) < 1e-3); + assert.equal(after.z, -15.3); +}); + +// A joint as the page has it: its rotation and location, turned through `turnTo`. +function jointWorld(rotation, location) { + const state = { rotation, location: { ...location } }; + const turns = []; + const deps = { + readAxis: (_i, axis) => (axis === 5 ? state.rotation : 0), + setAxis: () => {}, + indexOf: (name) => (name === "link" ? 0 : undefined), + attach: () => {}, + skipping: () => false, + turnTo: (_i, degrees, pivot) => { + state.location = turnedLocation(state.location, state.rotation, degrees, pivot); + state.rotation = degrees; + turns.push(degrees); + }, + }; + return { state, turns, deps }; +} + +test("a joint turns the way its drive runs, from its angle to the one sent", async () => { + // Link 1 pointing right (drive 90, rotation 0) sent to the left (drive -90): through the front + // (rotation 270), not the short way through the back. + const pivot = { x: 12.7, y: 12.75, z: -20.3 }; + const { state, turns, deps } = jointWorld(0, { x: 0, y: 0, z: 0 }); + const start = turnedLocation(state.location, 0, 0, pivot); + const turn = { name: "link", drive: -90, base: 90, pivot, speed: 60, acceleration: 200 }; + await playOut(createPlayer(deps), { turns: [turn] }); + assert.equal(((state.rotation % 360) + 360) % 360, 180); + const unwrapped = turns.map((z) => z + 90); + assert.ok(unwrapped.some((d) => Math.abs(d) < 5), "it did not pass through the front"); + for (let i = 1; i < unwrapped.length; i++) assert.ok(unwrapped[i] <= unwrapped[i - 1] + 1e-9); + // The pivot has not moved. + const a = (state.rotation * Math.PI) / 180; + const px = state.location.x + Math.cos(a) * pivot.x - Math.sin(a) * pivot.y; + const py = state.location.y + Math.sin(a) * pivot.x + Math.cos(a) * pivot.y; + const p0x = start.x + pivot.x; + const p0y = start.y + pivot.y; + assert.ok(Math.abs(px - p0x) < 1e-6 && Math.abs(py - p0y) < 1e-6, "the pivot moved"); +}); + +test("a turn takes as long as its drive's speed and acceleration say", async () => { + const { deps } = jointWorld(0, { x: 0, y: 0, z: 0 }); + const turn = { name: "link", drive: -90, base: 90, pivot: { x: 0, y: 0, z: 0 }, speed: 60, acceleration: 200 }; + const seconds = await playOut(createPlayer(deps), { turns: [turn] }); + const expected = motionProfile(180, 60, 200).duration; + assert.ok(Math.abs(seconds - expected) < 0.1, `took ${seconds}, expected ${expected}`); +}); + +function jawsWorld(y0, y1) { + const at = { f0: y0, f1: y1 }; + const log = []; + const deps = { + readAxis: (i) => (i === 0 ? at.f0 : at.f1), + setAxis: (i, _axis, value) => { + at[i === 0 ? "f0" : "f1"] = value; + }, + indexOf: (name) => ({ f0: 0, f1: 1 })[name], + attach: () => {}, + skipping: () => false, + grip: (gripper) => log.push({ grip: gripper, f0: at.f0 }), + release: (gripper) => log.push({ release: gripper, f0: at.f0 }), + }; + return { at, log, deps }; +} + +const jaws = (f0, f1) => ({ + jaws: { + gripper: "g", + fingers: [ + { name: "f0", y: f0 }, + { name: "f1", y: f1 }, + ], + speed: 20, + acceleration: 100, + grip_point: { x: 0, y: 0, z: 0 }, + }, +}); + +test("jaws that close take hold once they have closed", async () => { + const { log, deps } = jawsWorld(110, -20); + await playOut(createPlayer(deps), jaws(90, 0)); + assert.deepEqual(log, [{ grip: "g", f0: 90 }]); +}); + +test("jaws that open let go before they move", async () => { + const { log, deps } = jawsWorld(90, 0); + await playOut(createPlayer(deps), jaws(110, -20)); + assert.deepEqual(log, [{ release: "g", f0: 90 }]); +}); + +const box = (x0, y0, z0, x1, y1, z1) => ({ + min: { x: x0, y: y0, z: z0 }, + max: { x: x1, y: y1, z: z1 }, +}); + +test("the jaws take the plate, not a well in it or the site under it", () => { + const candidates = [ + { index: 1, category: "plate_holder", box: box(0, 0, 100, 127, 86, 100) }, + { index: 2, category: "plate", box: box(0, 0, 97, 128, 85, 111) }, + { index: 3, category: "well", box: box(60, 40, 98, 67, 47, 111) }, + { index: 4, category: "plate_carrier", box: box(-5, -5, 0, 140, 500, 120) }, + ]; + assert.equal(heldAt({ x: 64, y: 43, z: 104 }, candidates), 2); + assert.equal(heldAt({ x: 300, y: 43, z: 104 }, candidates), undefined); +}); + +test("a plate let go of lands on the site under it", () => { + const plate = box(200, 0, 97, 328, 85, 111); + const sites = [ + { index: 1, category: "plate_holder", box: box(0, 0, 100, 127, 86, 100) }, + { index: 5, category: "plate_holder", box: box(200, 0, 100, 327, 86, 100) }, + { index: 6, category: "plate_holder", box: box(200, 0, 40, 327, 86, 40) }, + ]; + // The skirt sits 3 mm into the site: its surface stands above the plate's bottom. + assert.equal(siteUnder(plate, sites, { index: 9, category: "plate" }), 5); + // Carried away from any site, it has none. + assert.equal(siteUnder(box(600, 0, 97, 728, 85, 111), sites, { index: 9 }), undefined); +}); + +// A plate 14.2 mm tall on a site at z 100, seated 3 mm into it, and its lid, 8.9 mm tall, nesting +// 7.6 mm over it - the demo's Corning plate. +const site = { index: 1, category: "plate_holder", box: box(0, 0, 100, 127, 86, 100) }; +const plateOn = (index, covered = false) => ({ + index, + category: "plate", + covered, + box: box(0, 0, 97, 128, 85, 111.2), +}); +const lidAt = (z) => box(0, 0, z, 128, 85, z + 8.9); +const LID = { index: 7, category: "lid", nesting: 7.6 }; + +test("the jaws take the top lid of a nested stack, not the one under it", () => { + // Hamilton's ComfortLid: 8.5 tall, 6.7 apart in a stack. Gripped 5 below its top, the point is + // in the top lid and 1.7 above the one under it: within reach of both. + const stack = [0, 1, 2, 3, 4].map((i) => ({ + index: 10 + i, + category: "lid", + box: box(0, 0, 200 + 6.7 * i, 127.5, 85.3, 208.5 + 6.7 * i), + })); + const top = 200 + 6.7 * 4 + 8.5; + assert.equal(heldAt({ x: 64, y: 43, z: top - 5 }, stack), 14); + assert.equal(heldAt({ x: 64, y: 43, z: top - 5 }, [...stack].reverse()), 14); +}); + +test("the jaws take the lid at its height, and the plate below it", () => { + const candidates = [plateOn(2), { index: 7, category: "lid", box: lidAt(103.6) }]; + assert.equal(heldAt({ x: 64, y: 43, z: 109 }, candidates), 7); + assert.equal(heldAt({ x: 64, y: 43, z: 101 }, candidates), 2); +}); + +test("a lid let go of over a plate with none becomes that plate's lid", () => { + // Where it sits on the plate: the plate's top less the nesting. + assert.equal(siteUnder(lidAt(103.6), [site, plateOn(2)], LID), 2); +}); + +test("a lid is not put on a plate that has one, nor is a plate put on a plate", () => { + assert.equal(siteUnder(lidAt(103.6), [site, plateOn(2, true)], LID), 1); + assert.equal(siteUnder(box(0, 0, 111, 128, 85, 125), [plateOn(2)], { category: "plate" }), undefined); +}); + +test("a lid let go of on an empty site lands on the site", () => { + assert.equal(siteUnder(lidAt(100), [site], LID), 1); +}); + +test("a tip is handed over where it stands, then seated where the model gives it", async () => { + const world = fakeWorld(start()); + const deps = world.deps(); + const placed = []; + deps.attach = (name, parent, where) => placed.push({ name, parent, where }); + const location = { x: -0.6, y: -0.6, z: -2.05 }; + const request = { + ...pickUp, + attach: [{ name: "tip", parent: "shaft0", location, rotation: { x: 0, y: 0, z: 0 } }], + }; + await playOut(createPlayer(deps), request); + // No place given with the handover: it keeps where it is, and moves only as a seating. + assert.deepEqual(placed, [{ name: "tip", parent: "shaft0", where: undefined }]); + assert.deepEqual(world.at("tip"), [-0.6, -0.6, -2.05]); + const seating = world.log.filter((e) => e.name === "tip" && e.axis === 2).map((e) => e.value); + assert.ok(seating.length > 2, "the tip jumped to its seat instead of moving there"); +}); + +// -- the jerk-limited profile ----------------------------------------------------------------------- + +test("an S-curve takes as long as the fitted X-arm model says, short moves and long", () => { + // Reference durations from `tools/hxusbcomm_timing.py`'s `move_time` at the fitted X-arm limits. + const expected = { 1: 0.21522, 10: 0.463676, 100: 0.99896, 500: 1.709912 }; + for (const [d, t] of Object.entries(expected)) { + const { duration } = motionProfile(Number(d), 600, 1297, 3210); + assert.ok(Math.abs(duration - t) < 1e-4, `${d} mm: ${duration} s, expected ${t} s`); + } +}); + +test("an S-curve starts and ends at rest, climbs monotonically and is symmetric", () => { + for (const d of [1, 10, 100, 500]) { + const { duration, progress } = motionProfile(d, 600, 1297, 3210); + assert.equal(progress(0), 0); + assert.equal(progress(duration), 1); + let last = 0; + const n = 200; + for (let k = 1; k <= n; k++) { + const p = progress((duration * k) / n); + assert.ok(p >= last - 1e-12, `${d} mm goes backwards at step ${k}`); + last = p; + } + // Rest to rest, mirrored: as far in at a quarter as short of the end at three quarters. + assert.ok(Math.abs(progress(duration / 4) - (1 - progress((3 * duration) / 4))) < 1e-9); + // And it starts gently: jerk-limited, so the first hundredth covers far less than a hundredth. + assert.ok(progress(duration / 100) < 0.001); + } +}); + +test("without a jerk the profile is the simulator's trapezoid", () => { + const withNone = motionProfile(100, 250, 800); + const withInfinite = motionProfile(100, 250, 800, Number.POSITIVE_INFINITY); + assert.ok(Math.abs(withNone.duration - (100 / 250 + 250 / 800)) < 1e-9); + assert.equal(withInfinite.duration, withNone.duration); +}); diff --git a/pylabrobot/visualizer3D/ripple_demo.py b/pylabrobot/visualizer3D/ripple_demo.py new file mode 100644 index 00000000000..c91a4c339b0 --- /dev/null +++ b/pylabrobot/visualizer3D/ripple_demo.py @@ -0,0 +1,94 @@ +"""The Y ripple: one aspirate, five plates, the channels fanning out in Y. + +A1 of every plate on the source carrier is aspirated in one go - the channels start their Y moves +one after another (the ripple), each ending over a different plate. What was drawn is then +dispensed into the first column of the front-most plate on the destination carrier: the channels +close back up to the 9 mm pitch, dispensing down the column. + +Run it: + + python -m pylabrobot.visualizer3D.ripple_demo +""" + +import asyncio +import logging + +from pylabrobot.resources import set_volume_tracking +from pylabrobot.resources.plate import Plate +from pylabrobot.resources.tip_rack import TipRack +from pylabrobot.resources.tip_tracking import set_tip_tracking + +from ..hamilton.star.motion import attach_viewer_motion +from .demo import build_facility, fill, star_of +from .server import Viewer3D + +ROWS = "ABCDE" # the carrier holds five plates, so five channels carry the aspirate + + +async def main() -> None: + logging.disable(logging.WARNING) + set_volume_tracking(True) + set_tip_tracking(True) + + facility = build_facility() + star = star_of(facility) + await star.setup() + pipettes = star.pipettes + if pipettes is None: + raise RuntimeError("the simulated STARlet has no channels") + + deck = star.deck + source_carrier = deck.get_resource("source_carrier") + destination_carrier = deck.get_resource("destination_carrier") + rack = deck.get_resource("tips_0") + if not isinstance(rack, TipRack): + raise RuntimeError("the demo deck no longer holds the tips this run uses") + + plates = [ + child for site in source_carrier.children for child in site.children if isinstance(child, Plate) + ] + # Channel 0 is the back-most channel, and per-channel Y runs back to front: the wells go to the + # channels in the same order, so every plate is hit at once, in a single command. + plates.sort(key=lambda p: p.get_absolute_location().y, reverse=True) + if len(plates) < 2: + raise RuntimeError("expected several plates on the source carrier") + for plate in plates: + fill(plate, 200.0) + + destinations = [ + child + for site in destination_carrier.children + for child in site.children + if isinstance(child, Plate) + ] + front = min(destinations, key=lambda p: p.get_absolute_location().y) + + viewer = Viewer3D(facility, name="ripple_demo.py", open_browser=False) + await viewer.start() + attach_viewer_motion(star.driver, viewer) + print("waiting for a browser to draw the scene") + await viewer.wait_for_browser() + await viewer.wait_for_models() + print("the page is up; the run starts in ten seconds") + await asyncio.sleep(10.0) + + print(f"picking up {len(ROWS)} tips, one per channel that will aspirate") + await pipettes.pick_up_tips([rack.get_item(f"{row}1") for row in ROWS]) + + wells = [plate.get_item("A1") for plate in plates] + print(f"aspirating A1 of {len(wells)} plates at once - the channels ripple out in Y") + await pipettes.aspirate(wells, [50.0] * len(wells)) + + column = [front.get_item(f"{row}1") for row in ROWS] + print(f"dispensing into the first column of {front.name}, the front-most destination plate") + await pipettes.dispense(column, [50.0] * len(column)) + + print("run complete; the viewer stays up") + await asyncio.Event().wait() + + +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + pass diff --git a/pylabrobot/visualizer3D/server.py b/pylabrobot/visualizer3D/server.py index f4a23eca6c2..dc61a59c7e7 100644 --- a/pylabrobot/visualizer3D/server.py +++ b/pylabrobot/visualizer3D/server.py @@ -20,13 +20,25 @@ import sys import threading import webbrowser -from typing import Any, Callable, Dict, FrozenSet, Iterable, List, Optional, Set, Tuple +from typing import ( + Any, + Callable, + Dict, + FrozenSet, + Iterable, + List, + Optional, + Sequence, + Set, + Tuple, +) from urllib.parse import parse_qs, urlsplit import websockets from websockets.asyncio.server import Server, ServerConnection from websockets.http11 import Request, Response +from pylabrobot.resources.collision import Collision from pylabrobot.resources.coordinate import Coordinate from pylabrobot.resources.resource import Resource @@ -152,6 +164,19 @@ def _signature(cleaned: Dict[str, Any], key: str) -> str: return key + repr([round(v, STATE_DECIMALS) for v in _xyz(cleaned["location"])]) +def _moved_names(request: Dict[str, Any]) -> Set[str]: + """Every resource a motion moves.""" + names: Set[str] = set() + if request.get("arm"): + names.add(request["arm"]["name"]) + for key in ("channels", "traverse", "moves", "turns"): + names.update(entry["name"] for entry in request.get(key) or []) + jaws = request.get("jaws") + if jaws: + names.update(finger["name"] for finger in jaws["fingers"]) + return names + + class Viewer3D: """A visualizer that takes any resource as its world. @@ -166,6 +191,10 @@ class Viewer3D: the package is found without it. allowed_hosts: extra hostnames a browser may reach the viewer by. IP addresses, `localhost`, this machine's hostname and `.local` are always accepted. + raise_on_collision: whether a collision check that found something may refuse here. When a + device's collision gate is attached to this viewer (`attach_viewer_collisions`), the flag is + what turns the checks into gates: a command that would hit something is drawn on the page + and raises, rather than being reported to whoever called the check. Access: each run makes a token, hands it out only in the link it prints and opens (`url`, as its `#token=` fragment, which no request carries), and refuses a websocket without it. Both @@ -182,6 +211,7 @@ class Viewer3D: token: str allowed_hosts: Set[str] models_root: Optional[str] + raise_on_collision: bool rebuilds: int clients_seen: List[Dict[str, Optional[str]]] _clients: Set[ServerConnection] @@ -202,9 +232,12 @@ class Viewer3D: _scene: Optional[Scene] _refused_a_token: bool _browser_drawing: Optional[asyncio.Event] + _models_drawn: Optional[asyncio.Event] _moved: Set[str] _scene_timer: Optional[asyncio.TimerHandle] _subscribed: Dict[int, Tuple[Resource, Callable[[Dict[str, Any]], None]]] + _motions: Dict[int, Tuple["asyncio.Future[None]", Set[Any]]] + _motion_count: int # -- packing ----------------------------------------------------------------- @@ -429,11 +462,13 @@ def _scene_message(self, rebuild: bool = False) -> Dict[str, Any]: } return self._scene_payload - def _moves(self) -> Optional[List[Dict[str, Any]]]: + def _moves(self, hold: FrozenSet[str] = frozenset()) -> Optional[List[Dict[str, Any]]]: """Moves since the scene was built, applied to the kept scene too, or None if a name changed. A move is a resource whose parent or local transform differs from the kept scene, as `{name, - parent, location, rotation}`. A name appearing or disappearing needs a rebuild. + parent, location, rotation}`. A name appearing or disappearing needs a rebuild. A resource in + `hold` that has only moved, not changed parent, is left out and left unrecorded: a motion about + to be played takes it there, and the model has it there already. """ scene = self._scene if scene is None or frozenset(all_names(self.root)) != self._known_names: @@ -453,10 +488,10 @@ def walk(resource: Resource, parent: Optional[str]) -> None: float(rotation.z), ] parent_index = -1 if parent is None else self._index_of[parent] - if ( - parent_index != scene.parent_of_instance[index] - or local != scene.transforms[6 * index : 6 * index + 6] - ): + reparented = parent_index != scene.parent_of_instance[index] + if not reparented and resource.name in hold: + pass + elif reparented or local != scene.transforms[6 * index : 6 * index + 6]: scene.parent_of_instance[index] = parent_index scene.transforms[6 * index : 6 * index + 6] = local moves.append( @@ -484,7 +519,7 @@ async def _send_scene_to_all(self) -> None: self._published = {name: _signature(cleaned, key) for name, (cleaned, key) in states.items()} await self._broadcast("state", pack_state(states, self._epoch)) - async def _flush_scene(self) -> None: + async def _flush_scene(self, hold: FrozenSet[str] = frozenset()) -> None: self._scene_timer = None if not self._clients: # Nobody to tell. The kept scene is dropped, so the next client is greeted with one built @@ -492,7 +527,7 @@ async def _flush_scene(self) -> None: self._scene = None self._scene_payload = None return - moves = self._moves() + moves = self._moves(hold) if moves is None: self.rebuilds += 1 await self._send_scene_to_all() @@ -587,6 +622,18 @@ def _on_client_message(self, message: Any) -> bool: self._browser_drawing.set() return True + def _on_models_drawn(self, message: Any) -> bool: + """Whether `message` is a page saying every model file of its scene has loaded (or failed).""" + try: + parsed = json.loads(message) + except (TypeError, ValueError): + return False + if not isinstance(parsed, dict) or parsed.get("event") != "models_drawn": + return False + if self._models_drawn is not None: + self._models_drawn.set() + return True + async def _handler(self, websocket: ServerConnection) -> None: self._clients.add(websocket) await websocket.send(_encode("scene", self._scene_message())) @@ -594,6 +641,10 @@ async def _handler(self, websocket: ServerConnection) -> None: greeted = False try: async for message in websocket: + if self._on_motion_played(websocket, message): + continue + if self._on_models_drawn(message): + continue # A page says hello once. A repeat is still read, or keepalive stalls, but not kept. if not greeted: greeted = self._on_client_message(message) @@ -601,6 +652,136 @@ async def _handler(self, websocket: ServerConnection) -> None: pass finally: self._clients.discard(websocket) + # A page that has gone plays nothing more, so nothing waits for it. + self._release_motions(websocket) + + # -- motion ------------------------------------------------------------------ + + # How long a command waits for a page to play its motion before going on without it. + MOTION_TIMEOUT_S = 120.0 + + async def show_collisions(self, collisions: Sequence[Collision]) -> None: + """Draw on the pages what a refused command would hit. + + A box goes over each resource the command ran into - the ones met at the instant of the first + meeting, where the check walked its way finely; a meeting the hull only leans on, with no + instant of its own, stands as the sweep said it. The boxes stay until the scene is rebuilt or + other collisions are shown. With no page connected, or called from off the viewer's loop, + nothing is sent. + + Args: + collisions: what a check found, whose `obstacle` is the resource drawn over. + """ + if not self._clients or self._loop is None or asyncio.get_running_loop() is not self._loop: + return + items: List[Dict[str, Any]] = [] + seen: Set[int] = set() + met = [c.when for c in collisions if c.when is not None] + moment = min(met) if met else None + for collision in collisions: + index = self._index_of.get(collision.obstacle.name) + if index is None or index in seen: + continue + if moment is not None and collision.when is not None and collision.when != moment: + continue + seen.add(index) + items.append({"index": index, "resource": collision.obstacle.name, "group": collision.group}) + if items: + await self._broadcast("collisions", {"collisions": items}) + + async def act_out(self, command: str, motion: Optional[Dict[str, Any]]) -> None: + """Act out one command's motion, and hold the command until a page has played it. + + The motion is sent to the pages and waited on until every one of them says it has played it: + the slowest page sets the pace, and a page in a background tab, which gets no frames, jumps to + the end and answers at once. The model then records where the command ended, which is where + the pages have just brought everything. With no page connected, nothing is waited for. + + A motion of None is a command that moves nothing: it is said to the page, so whatever it moves + can be put down to it, and the page jumps. + + Args: + command: the command as the device names it, for a page that lists them. + motion: what the command moves, read out of it by the device's own motion model, or None. + """ + if not self._clients or self._loop is None or asyncio.get_running_loop() is not self._loop: + return + if motion is None: + await self._broadcast("command", {"command": command}) + return + request = motion + # A change is handed to the loop to be queued, so let what the last command changed reach the + # page first: a motion starts from where the page has everything. + await asyncio.sleep(0) + # Except where this very command has already been written: the iSWAP records a move's target as + # it sends it, and so does a channel packing (`recorded_first`). Sent now, that would put the + # part at the end of the move before it is played, so it is held until the pages have played the + # move there. Taken before anything else can flush. Only for those: most commands write the + # model once they have run, so what is pending is the last command's result, and holding that + # back would send it after this one's move - drawn back where it was, until the model's own + # move puts it right. + moving = _moved_names(request) if request.get("recorded_first") else set() + held = {name: self._pending.pop(name) for name in list(self._pending) if name in moving} + # A change of shape - a tip taken onto a shaft - waits out its debounce, and holds the + # positions back with it; the command that caused it is over, so it goes now. + if self._scene_timer is not None: + self._scene_timer.cancel() + # What this command already wrote is held back here too: a change of shape carries every + # position that differs from the kept scene, the move's own target among them. + await self._flush_scene(frozenset(moving)) + if self._pending: + await self._flush() + self._motion_count += 1 + motion_id = self._motion_count + played: "asyncio.Future[None]" = self._loop.create_future() + # Registered before sending: a page in the background answers as soon as it is told. + pages = set(self._clients) + self._motions[motion_id] = (played, pages) + try: + await self._broadcast("motion", {"id": motion_id, **request}) + # A page that could not be sent to has been dropped, and will not answer. One that closed while + # this was being sent - a reload - has already let the motion go. + pages.intersection_update(self._clients) + if not pages and not played.done(): + played.set_result(None) + await asyncio.wait_for(asyncio.shield(played), self.MOTION_TIMEOUT_S) + except asyncio.TimeoutError: + print(f"viewer: no page played {command} within {self.MOTION_TIMEOUT_S} s; going on") + finally: + self._motions.pop(motion_id, None) + if held: + # Anything newer that arrived meanwhile has the last word. + for name, state in held.items(): + self._pending.setdefault(name, state) + await self._flush() + + def _on_motion_played(self, websocket: Any, message: Any) -> bool: + """Take a page's word that it has played a motion; whether the message was that.""" + try: + parsed = json.loads(message) + except (TypeError, ValueError): + return False + if not isinstance(parsed, dict) or parsed.get("event") != "motion_done": + return False + data = parsed.get("data") + motion_id = data.get("id") if isinstance(data, dict) else None + waiting = self._motions.get(motion_id) if isinstance(motion_id, int) else None + if waiting is not None: + played, pages = waiting + pages.discard(websocket) + if not pages and not played.done(): + played.set_result(None) + return True + + def _release_motions(self, websocket: Any = None) -> None: + """Stop waiting on one page, or on every page when none is named.""" + for played, pages in self._motions.values(): + if websocket is None: + pages.clear() + else: + pages.discard(websocket) + if not pages and not played.done(): + played.set_result(None) # -- static files ------------------------------------------------------------ @@ -685,8 +866,9 @@ class Server(http.server.ThreadingHTTPServer): def handle_error(self, request: Any, client_address: Any) -> None: # A browser that leaves a page mid-download, or stalls past the timeout, is no error of - # ours, and a traceback on every reload buries anything that is. - if isinstance(sys.exc_info()[1], (BrokenPipeError, ConnectionResetError, socket.timeout)): + # ours, and a traceback on every reload buries anything that is. Every flavour of a + # vanished peer - broken pipe, reset, abort (Windows) - is a ConnectionError. + if isinstance(sys.exc_info()[1], (ConnectionError, socket.timeout)): return super().handle_error(request, client_address) @@ -718,6 +900,7 @@ def __init__( name: str = "facility", models_root: Optional[str] = None, allowed_hosts: Iterable[str] = (), + raise_on_collision: bool = False, ): self.root = root self.host = host @@ -725,6 +908,7 @@ def __init__( self.ws_port = ws_port self.open_browser = open_browser self.name = name + self.raise_on_collision = raise_on_collision self.token = secrets.token_urlsafe(32) machine = socket.gethostname().lower() # The interface bound to counts as a way in when it is a name rather than an address. @@ -773,6 +957,10 @@ def __init__( self.rebuilds = 0 # how many scene rebuilds a run actually cost self.clients_seen = [] # what each page said it draws with self._browser_drawing = None # made on the loop `start` runs on + self._models_drawn = None # likewise + # Motions sent to the pages and not yet played, by id, with the pages still playing each. + self._motions = {} + self._motion_count = 0 # Every resource this viewer listens to, with the callback it gave, so `stop` can take it back. # By identity: a tip compares by value and is not hashable. @@ -785,6 +973,7 @@ def __init__( async def start(self) -> None: self._loop = asyncio.get_running_loop() self._browser_drawing = asyncio.Event() + self._models_drawn = asyncio.Event() # Off the loop, before a client can connect: the package walk for model files, and the size of # the tree as one node per resource, which the stats panel compares against. self._legacy_bytes, _ = await asyncio.gather( @@ -826,11 +1015,32 @@ async def wait_for_browser(self, timeout: Optional[float] = None) -> None: raise RuntimeError("start the viewer before waiting for a browser") await asyncio.wait_for(self._browser_drawing.wait(), timeout) + async def wait_for_models(self, timeout: Optional[float] = None) -> None: + """Wait until a page has every model file of the scene on screen, or has given up on it. + + `wait_for_browser` returns as soon as a page is drawing - boxes, until the files land. Call this + too before a run whose first moves should be seen on the models themselves. A page says so once, + when it has loaded; each call takes one saying, so a later call waits for the next page - a + refresh - and a run can be played again for it. + + Args: + timeout: seconds to wait, or None to wait for as long as it takes. + + Raises: + RuntimeError: the viewer has not been started. + asyncio.TimeoutError: no page had its models drawn within `timeout`. + """ + if self._models_drawn is None: + raise RuntimeError("start the viewer before waiting for its models") + await asyncio.wait_for(self._models_drawn.wait(), timeout) + self._models_drawn.clear() + async def stop(self) -> None: """Close both servers and stop listening to the tree. The ports are free for the next viewer, and no change is handed to a loop that is gone. """ + self._release_motions() self._unsubscribe(self.root) self.root.deregister_did_assign_resource_callback(self._on_assign) self.root.deregister_did_unassign_resource_callback(self._on_unassign) diff --git a/pylabrobot/visualizer3D/server_tests.py b/pylabrobot/visualizer3D/server_tests.py index 7071c86da72..89ad636648c 100644 --- a/pylabrobot/visualizer3D/server_tests.py +++ b/pylabrobot/visualizer3D/server_tests.py @@ -20,6 +20,7 @@ from websockets.typing import Origin from pylabrobot.resources import does_volume_tracking, set_volume_tracking +from pylabrobot.resources.collision import Collision from pylabrobot.resources.coordinate import Coordinate from pylabrobot.resources.corning import cor_96_wellplate_360uL_Fb from pylabrobot.resources.hamilton import hamilton_96_tiprack_1000uL @@ -853,5 +854,65 @@ async def test_a_token_from_another_run_is_refused(self): await self.assert_refused(self.ws(other.token)) +class ShowCollisionsTests(unittest.IsolatedAsyncioTestCase): + """What a refused command would hit is said to the pages, one box per resource.""" + + async def asyncSetUp(self): + self.facility = empty_facility() + self.plate = cor_96_wellplate_360uL_Fb("plate") + self.facility.assign_child_resource(self.plate, location=Coordinate(100, 100, 0)) + self.mover = Resource("mover", 10, 10, 10) + self.facility.assign_child_resource(self.mover, location=Coordinate(0, 0, 0)) + self.viewer = Viewer3D( + self.facility, + open_browser=False, + fs_port=FS_PORT, + ws_port=WS_PORT, + name="tests", + raise_on_collision=True, + ) + await self.viewer.start() + + async def asyncTearDown(self): + await self.viewer.stop() + + def ws(self) -> str: + return f"ws://127.0.0.1:{self.viewer.ws_port}/?token={self.viewer.token}" + + @staticmethod + def collision(obstacle: Resource) -> Collision: + return Collision(mover=obstacle, obstacle=obstacle, group="test", segment=0, gap=0.0) + + async def test_a_page_is_told_what_was_hit_and_where_it_stands(self): + async with websockets.connect(self.ws(), max_size=None) as ws: + self.assertEqual(json.loads(await ws.recv())["event"], "scene") + hit = self.collision(self.plate) + await self.viewer.show_collisions([hit]) + # State may be published in between; what was hit is said when it is said. + while True: + said = json.loads(await ws.recv()) + if said["event"] == "collisions": + break + self.assertEqual( + said["data"]["collisions"], + [{"index": self.viewer._index_of["plate"], "resource": "plate", "group": "test"}], + ) + + async def test_nothing_is_sent_for_what_the_scene_does_not_name(self): + async with websockets.connect(self.ws(), max_size=None) as ws: + self.assertEqual(json.loads(await ws.recv())["event"], "scene") + elsewhere = Resource("elsewhere", 10, 10, 10) + await self.viewer.show_collisions([self.collision(elsewhere)]) + await ws.send(json.dumps({"event": "hello", "data": {"backend": "WebGL2"}})) + await (await ws.ping()) # answered once everything sent before it has been read + + async def test_with_no_page_connected_nothing_is_sent(self): + await self.viewer.show_collisions([self.collision(self.plate)]) + + async def test_the_flag_is_the_viewer_configuration_saying_whether_checks_may_refuse(self): + self.assertTrue(self.viewer.raise_on_collision) + self.assertFalse(Viewer3D(empty_facility(), open_browser=False).raise_on_collision) + + if __name__ == "__main__": unittest.main() diff --git a/pylabrobot/visualizer3D/static/app.js b/pylabrobot/visualizer3D/static/app.js index 1c9f45c2b0f..c708b70fbdb 100644 --- a/pylabrobot/visualizer3D/static/app.js +++ b/pylabrobot/visualizer3D/static/app.js @@ -1,6 +1,7 @@ import * as THREE from "three"; import { forgetDetail, updateDetail, updateEdgeMode } from "./appearance.js"; import { buildMeshes } from "./boxes.js"; +import { clearCollisions, showCollisions } from "./collisions.js"; import { PROTOCOL } from "./constants.js"; import { initDeviceTools } from "./device_tools.js"; import { hiddenNames, meshes, meshRoots, modelMeshes, stateOf, worldBox } from "./drawn.js"; @@ -44,6 +45,7 @@ import { updateOrigin, } from "./marks.js"; import { buildDeclaredMeshes, dracoLoader, gltfLoader } from "./models.js"; +import { dropMotions, playMotion, stepMotion } from "./motion.js"; import { clearSelection, hoverBox, @@ -84,7 +86,7 @@ import { updateBullseyes, updateDeltaLabels, } from "./tools.js"; -import { connect, initTransport } from "./transport.js"; +import { connect, initTransport, send } from "./transport.js"; import { buildTree, refreshTreeInfo, @@ -435,6 +437,10 @@ function rebuildScene(data) { const kept = rememberView(); setWorld(buildWorld(data)); glides.clear(); + // A motion under way moves resources by where they stood in the last scene. + dropMotions(); + // What the last scene's indices named is not what these do: the collision boxes go. + clearCollisions(); timings.decodeMs = performance.now() - _tScene; const _tBuild = performance.now(); forgetDetail(); @@ -511,6 +517,8 @@ whileMoving(updateArms); whileMoving(updateGlides); +whileMoving(stepMotion); + whileMoving(() => controls.update()); whileMoving(() => gif.isRecording()); @@ -582,6 +590,14 @@ initTransport({ moves: (moves) => { if (world && Array.isArray(moves)) applyMoves(moves); }, + // A command the device is carrying out: acted out, and the server told when it has been. + motion: (data) => { + playMotion(data, () => send("motion_done", { id: data.id })); + }, + // What a refused command would hit: a box over each resource named, held for looking at. + collisions: (data) => { + if (Array.isArray(data.collisions)) showCollisions(data.collisions); + }, }, }); diff --git a/pylabrobot/visualizer3D/static/carry.js b/pylabrobot/visualizer3D/static/carry.js new file mode 100644 index 00000000000..1c53b971061 --- /dev/null +++ b/pylabrobot/visualizer3D/static/carry.js @@ -0,0 +1,120 @@ +// What a gripper takes hold of when its jaws close, and what a plate it lets go of comes to rest on. +// +// PyLabRobot does not move a plate when the iSWAP grips it, so the page does: the plate is handed to +// the gripper when the jaws close on it, rides the arm, and is handed to the site under it when they +// open. A lid is labware like any other, except that what takes it can be a plate: set down on one +// with no lid, it becomes that plate's lid, as `Plate.assign_child_resource` makes it. Pure - it is +// given boxes, not the scene - so it can be checked outside a page. + +// What an arm picks up: labware, not what labware holds or what holds it. +export const MOVABLE = new Set([ + "plate", + "lid", + "tip_rack", + "tube_rack", + "plate_adapter", + "trough", +]); + +// What a plate is set down on. +export const SITES = new Set(["resource_holder", "plate_holder", "plate_adapter"]); + +// What a lid is set down on besides a site: a plate that has none, its bottom `nesting_z_height` +// below the plate's top. +export const LIDDABLE = new Set(["plate"]); + +// How far a point may lie outside a box and still be taken as in it, in mm. +const REACH = 2; +// How far above a site a plate may be let go of and still land on it, in mm. +const DROP = 30; +// How far a site's surface may stand above the plate's bottom: a plate's skirt sits down into its +// site, 3 mm on the demo's carriers. +const SUNK = 10; + +/** + * @typedef {object} Candidate + * @property {number} index + * @property {string} category + * @property {{min: {x: number, y: number, z: number}, max: {x: number, y: number, z: number}}} box + * @property {boolean} [covered] a plate that already has a lid + */ + +const inside = (p, box, pad) => + p.x >= box.min.x - pad && + p.x <= box.max.x + pad && + p.y >= box.min.y - pad && + p.y <= box.max.y + pad && + p.z >= box.min.z - pad && + p.z <= box.max.z + pad; + +const volume = (box) => (box.max.x - box.min.x) * (box.max.y - box.min.y) * (box.max.z - box.min.z); + +// How far a point lies outside a box, in mm: 0 inside it. +const outside = (p, box) => + Math.hypot( + Math.max(box.min.x - p.x, 0, p.x - box.max.x), + Math.max(box.min.y - p.y, 0, p.y - box.max.y), + Math.max(box.min.z - p.z, 0, p.z - box.max.z), + ); + +/** + * The labware at the point the jaws close on: of the movable things whose box holds it (within + * `REACH`), the one it is least outside of, then the smallest. A lid nested on another lid is + * gripped in its own box and within reach of the one below; the one it is in is the one taken. + * + * @param {{x: number, y: number, z: number}} point in world mm + * @param {Candidate[]} candidates + * @returns {number | undefined} + */ +export function heldAt(point, candidates) { + let best; + let rank = [Number.POSITIVE_INFINITY, Number.POSITIVE_INFINITY]; + for (const c of candidates) { + if (!MOVABLE.has(c.category) || !inside(point, c.box, REACH)) continue; + const r = [outside(point, c.box), volume(c.box)]; + if (r[0] < rank[0] - 1e-9 || (Math.abs(r[0] - rank[0]) <= 1e-9 && r[1] < rank[1])) { + rank = r; + best = c.index; + } + } + return best; +} + +/** + * What something let go of lands on: the highest seat under its middle that is near its bottom - a + * little above it, as a skirt sits down into a site, or a drop's height below it. A site's seat is + * its surface; for a lid, a plate with no lid is a seat too, `nesting` below the plate's top. + * + * @param {{min: {x: number, y: number, z: number}, max: {x: number, y: number, z: number}}} box + * what was let go of, in world mm + * @param {Candidate[]} candidates + * @param {{index?: number, category?: string, nesting?: number}} [released] what was let go of: + * not a seat for itself, and a lid when its category says so + * @returns {number | undefined} + */ +export function siteUnder(box, candidates, released = {}) { + const lid = released.category === "lid"; + const middle = { x: (box.min.x + box.max.x) / 2, y: (box.min.y + box.max.y) / 2 }; + let best; + let highest = Number.NEGATIVE_INFINITY; + for (const c of candidates) { + if (c.index === released.index) continue; + const b = c.box; + let seat; + if (SITES.has(c.category)) seat = b.max.z; + else if (lid && LIDDABLE.has(c.category) && !c.covered) + seat = b.max.z - (released.nesting ?? 0); + else continue; + const under = + middle.x >= b.min.x - REACH && + middle.x <= b.max.x + REACH && + middle.y >= b.min.y - REACH && + middle.y <= b.max.y + REACH; + if (!under || seat > box.min.z + SUNK || seat < box.min.z - DROP) continue; + if (seat > highest) { + highest = seat; + best = c.index; + } + } + return best; +} diff --git a/pylabrobot/visualizer3D/static/collisions.js b/pylabrobot/visualizer3D/static/collisions.js new file mode 100644 index 00000000000..8ef49140941 --- /dev/null +++ b/pylabrobot/visualizer3D/static/collisions.js @@ -0,0 +1,61 @@ +// What a refused command would hit: a red, transparent box over each resource the command ran +// into, drawn over everything, held until the next word about collisions or the scene is rebuilt. +// What brings the mover to the meeting is the mover's own motion, driven there by the device; the +// boxes mark where it stopped. + +import * as THREE from "three"; + +import { COLLISION, COLLISION_OPACITY } from "./constants.js"; +import { OVERLAY_ORDER, worldBox } from "./drawn.js"; +import { view } from "./renderer.js"; + +const boxes = new THREE.Group(); + +boxes.visible = false; + +view.add(boxes); + +export function clearCollisions() { + for (const child of [...boxes.children]) { + child.geometry.dispose(); + child.material.dispose(); + boxes.remove(child); + } + boxes.visible = false; +} + +/** One box over one resource's world extent, filled where it is and outlined where it ends. */ +function collisionBox(index) { + const box = worldBox(index); + const size = new THREE.Vector3(); + box.getSize(size); + const centre = new THREE.Vector3(); + box.getCenter(centre); + const mesh = new THREE.Mesh( + new THREE.BoxGeometry(size.x, size.y, size.z), + new THREE.MeshBasicMaterial({ + color: COLLISION, + transparent: true, + opacity: COLLISION_OPACITY, + depthTest: false, + }), + ); + mesh.position.copy(centre); + mesh.renderOrder = OVERLAY_ORDER + 30; + const edges = new THREE.LineSegments( + new THREE.EdgesGeometry(mesh.geometry), + new THREE.LineBasicMaterial({ color: COLLISION, transparent: true, depthTest: false }), + ); + mesh.add(edges); + return mesh; +} + +/** The server's word about what a refused command would hit, one item per resource. */ +export function showCollisions(items) { + clearCollisions(); + for (const item of items) { + if (typeof item.index !== "number") continue; + boxes.add(collisionBox(item.index)); + } + boxes.visible = boxes.children.length > 0; +} diff --git a/pylabrobot/visualizer3D/static/constants.js b/pylabrobot/visualizer3D/static/constants.js index ca112683bfd..5447d877ca8 100644 --- a/pylabrobot/visualizer3D/static/constants.js +++ b/pylabrobot/visualizer3D/static/constants.js @@ -116,6 +116,10 @@ export const FILTER_BELOW_COLLAR = 2; // been read for the bore at that height. export const FILTER_WIDTH_UNMEASURED = 0.6; export const SELECT = 0x1a4b8c; +// What a refused command hit: a red, transparent box over the resource it ran into, drawn over +// everything the way the selection box is, so it reads wherever the hit is. +export const COLLISION = 0xd0342c; +export const COLLISION_OPACITY = 0.25; // How long the selection box stays after a pick, as the existing visualizer's dashed rectangle // does: long enough to see what was chosen, gone before it gets in the way of looking at it. export const SELECTION_SHOWN_MS = 2000; diff --git a/pylabrobot/visualizer3D/static/live.js b/pylabrobot/visualizer3D/static/live.js index acd17aaa1ce..c4bacea49f4 100644 --- a/pylabrobot/visualizer3D/static/live.js +++ b/pylabrobot/visualizer3D/static/live.js @@ -4,7 +4,7 @@ import * as THREE from "three"; import { placeEdges, showEdges } from "./boxes.js"; -import { LIQUID, MOVING_PARTS, VESSEL_EMPTY } from "./constants.js"; +import { DEG, LIQUID, MOVING_PARTS, VESSEL_EMPTY } from "./constants.js"; import { hiddenNames, isVisible, @@ -18,6 +18,7 @@ import { } from "./drawn.js"; import { armPose, arms, gridMarks, referenceMarks, refreshHalos } from "./marks.js"; import { applyJoints } from "./models.js"; +import { turnedLocation } from "./motion_player.js"; import { mirrorPlacement, modelOf, @@ -110,29 +111,74 @@ export function updateArms(delta) { reduce || !glideSeconds ? arm.targetX : arm.currentX + (arm.targetX - arm.currentX) * Math.min(1, delta / glideSeconds); - const local = new THREE.Matrix4().makeTranslation(arm.currentX, arm.local[1], arm.local[2]); - arm.group.matrix.multiplyMatrices(arm.parentMatrix, local); - arm.group.matrixWorldNeedsUpdate = true; - - // Keep the scene model in step with what is drawn. Everything else reads position from here - - // the info panel, the selection box, the coordinate tool - so moving only the group would - // leave all of them quoting where the arm used to be. - mirrorPlacement(arm.index, arm.currentX, arm.group.matrix); - announce({ kind: "glide", index: arm.index }); - // What the arm itself is drawn from. Its frame and outline ride the group and have moved - // already, but everything else the arm owns is a separate object with a baked matrix - its - // declared model above all, which hangs off the view rather than off the group - and the line - // below deliberately skips the arm while it works out the subtree beneath it. Without this a - // part drawn from a file stays where it was loaded while the arm travels out from under it, - // which is what a model-drawn X-arm did: the reference line moved and the geometry did not. - redraw([arm.index]); - // Whatever rides the arm moves with it. Its own matrix is already set from the group, so only - // what is beneath it needs working out. - refreshSubtree(arm.index, true); + placeArm(arm); } return moved; } +// Draw an arm at its `currentX`, and everything it carries with it. +function placeArm(arm) { + const local = new THREE.Matrix4().makeTranslation(arm.currentX, arm.local[1], arm.local[2]); + arm.group.matrix.multiplyMatrices(arm.parentMatrix, local); + arm.group.matrixWorldNeedsUpdate = true; + + // Keep the scene model in step with what is drawn. Everything else reads position from here - + // the info panel, the selection box, the coordinate tool - so moving only the group would + // leave all of them quoting where the arm used to be. + mirrorPlacement(arm.index, arm.currentX, arm.group.matrix); + announce({ kind: "glide", index: arm.index }); + // What the arm itself is drawn from. Its frame and outline ride the group and have moved + // already, but everything else the arm owns is a separate object with a baked matrix - its + // declared model above all, which hangs off the view rather than off the group - and the line + // below deliberately skips the arm while it works out the subtree beneath it. Without this a + // part drawn from a file stays where it was loaded while the arm travels out from under it, + // which is what a model-drawn X-arm did: the reference line moved and the geometry did not. + redraw([arm.index]); + // Whatever rides the arm moves with it. Its own matrix is already set from the group, so only + // what is beneath it needs working out. + refreshSubtree(arm.index, true); +} + +const AXES = ["x", "y", "z"]; + +/** + * Where a resource is drawn along one axis, relative to its parent, in mm. + * + * @param {number} index + * @param {number} axis 0, 1 or 2 for x, y or z + */ +export function readAxis(index, axis) { + const arm = axis === 0 ? arms.find((a) => a.index === index) : undefined; + return arm ? arm.currentX : world.local[index * 6 + axis]; +} + +/** + * Draw a resource at `value` along one axis, and everything it carries with it. What a motion + * moves the drives through: an arm travels in its own group, anything else by its transform. + * + * @param {number} index + * @param {number} axis 0, 1 or 2 for x, y or z + * @param {number} value in mm, relative to its parent + */ +export function setAxis(index, axis, value) { + const arm = axis === 0 ? arms.find((a) => a.index === index) : undefined; + if (arm) { + arm.currentX = value; + arm.targetX = value; + placeArm(arm); + return; + } + // A motion is where this resource is drawn from now on; an ease toward an older target is over. + glides.delete(index); + const o = index * 6; + const location = { x: world.local[o], y: world.local[o + 1], z: world.local[o + 2] }; + location[AXES[axis]] = value; + if (setLocal(index, location)) { + refreshSubtree(index); + announce({ kind: "glide", index }); + } +} + // What each gliding resource is easing toward, by index. A second move replaces the first: the // model is already at the new target and only the drawing is behind. export const glides = new Map(); @@ -355,3 +401,66 @@ export function applyMoves(moves) { refreshHalos(); announce({ kind: "moves", rowsUnder }); } + +/** + * Hand a resource to a new holder where it stands: a tip taken onto a shaft at the bottom of a + * pick-up, or left in its spot at the bottom of a drop. Drawn now, as the device does it; the model + * records the same change once the command has run, and that `moves` message then finds it done. + * + * @param {string} name + * @param {string | null} parentName the new holder, or null to leave it standing where it is + * @param {{location: any, rotation?: any}} [placed] where the new holder has it, relative to itself, + * when the model says: a tip on a shaft sits at its fitting depth, not where the shaft met it + */ +export function reattach(name, parentName, placed) { + const index = world.indexOfName.get(name); + if (index === undefined) return; + const parent = parentName === null ? -1 : (world.indexOfName.get(parentName) ?? -1); + if (placed && parent >= 0) { + applyMoves([ + { + name, + parent: world.names[parent], + location: placed.location, + rotation: placed.rotation ?? { x: 0, y: 0, z: 0 }, + }, + ]); + return; + } + if (parent === world.parentOf[index]) return; + const local = parent >= 0 ? world.matrices[parent].clone().invert() : new THREE.Matrix4(); + local.multiply(world.matrices[index]); + const position = new THREE.Vector3(); + const turn = new THREE.Quaternion(); + local.decompose(position, turn, new THREE.Vector3()); + const euler = new THREE.Euler().setFromQuaternion(turn, "XYZ"); + applyMoves([ + { + name, + parent: parent >= 0 ? world.names[parent] : null, + location: { x: position.x, y: position.y, z: position.z }, + rotation: { x: euler.x / DEG, y: euler.y / DEG, z: euler.z / DEG }, + }, + ]); +} + +/** + * Turn a resource about Z to `degrees`, relative to its parent, keeping `pivot` - a point in its own + * frame - where it is. What `Resource.rotate_to(z=..., pivot_coordinate=...)` does to it, one frame + * of a turn at a time. A joint turns only about Z; the other angles are left as they are. + * + * @param {number} index + * @param {number} degrees + * @param {{x: number, y: number, z: number}} pivot + */ +export function turnTo(index, degrees, pivot) { + const o = index * 6; + const local = world.local; + const here = { x: local[o], y: local[o + 1], z: local[o + 2] }; + const moved = turnedLocation(here, local[o + 5], degrees, pivot); + setLocalRotation(index, { x: local[o + 3], y: local[o + 4], z: degrees }); + setLocal(index, moved); + glides.delete(index); + refreshSubtree(index); + announce({ kind: "glide", index }); +} diff --git a/pylabrobot/visualizer3D/static/models.js b/pylabrobot/visualizer3D/static/models.js index 85c5f77e657..8be649058f3 100644 --- a/pylabrobot/visualizer3D/static/models.js +++ b/pylabrobot/visualizer3D/static/models.js @@ -21,6 +21,7 @@ import { travels, } from "./drawn.js"; import { view } from "./renderer.js"; +import { send } from "./transport.js"; import { modelOf, world } from "./world.js"; // Geometry a resource declared for itself, drawn in place of its box. @@ -41,6 +42,19 @@ export const dracoLoader = new DRACOLoader(); // Counted up per rebuild, so a file that lands late is placed only by the scene that asked for it. let sceneGeneration = 0; +// The server is told once per connection, when the first scene it sends has every model file on +// screen: a run waiting for a page (a refresh, or a page whose socket dropped and came back) starts +// on that, and not on the rebuilds its own moves cause. +let toldDrawn = false; +function tellDrawn() { + if (toldDrawn) return; + toldDrawn = true; + send("models_drawn", {}); +} +window.addEventListener("plr:status", (event) => { + if (/** @type {CustomEvent} */ (event).detail?.connected) toldDrawn = false; +}); + // Every file that has been fetched and parsed, by url. A tree that changes shape sends a whole // scene, and an instanced mesh cannot be resized - so the meshes are built again, from this, // without going back to the network. @@ -225,6 +239,14 @@ export function buildDeclaredMeshes() { } for (const modelIndex of kept) modelIsDrawn(modelIndex); + // Files still being fetched for this scene. The server is told once none are, so a run can wait + // for the page to have every model on screen before it starts to move anything. + let loading = 0; + const loadingGeneration = sceneGeneration; + const settled = () => { + if (--loading === 0 && loadingGeneration === sceneGeneration) tellDrawn(); + }; + for (const [modelIndex, instances] of byModel) { const declared = world.models[modelIndex].mesh; const scale = MESH_UNITS[declared.units] ?? 1; @@ -310,10 +332,24 @@ export function buildDeclaredMeshes() { place(parsed); continue; } - gltfLoader.load(declared.url, place, undefined, (error) => - console.warn(`could not load the mesh declared by ${world.names[instances[0]]}`, error), + loading++; + gltfLoader.load( + declared.url, + (gltf) => { + try { + place(gltf); + } finally { + settled(); + } + }, + undefined, + (error) => { + console.warn(`could not load the mesh declared by ${world.names[instances[0]]}`, error); + settled(); + }, ); } + if (loading === 0) tellDrawn(); // Whatever is left was drawn for a set of resources this scene does not have. Let go here, so a // file that lands later finds nothing to reuse and builds its meshes afresh. for (const built of instanced.values()) { diff --git a/pylabrobot/visualizer3D/static/motion.js b/pylabrobot/visualizer3D/static/motion.js new file mode 100644 index 00000000000..bac1b729822 --- /dev/null +++ b/pylabrobot/visualizer3D/static/motion.js @@ -0,0 +1,115 @@ +// Acting out what a device's drives do between the positions PyLabRobot records. +// +// PyLabRobot sends a device a command and records where it ends up; the device's own motion +// controller decides how to get there. The server reads each command for its targets (see +// `motion.py`) and sends them here as a motion, which `motion_player.js` plays; the server is told +// when it is done. Python holds the command until then, so the model moves only once the picture +// has got there. +// +// One thing is the page's alone: a plate the iSWAP grips. PyLabRobot does not move it, so the page +// hands it to the gripper when the jaws close on it and to the site under it when they open +// (`carry.js`). A new scene puts it back where PyLabRobot has it. + +import * as THREE from "three"; + +import { heldAt, LIDDABLE, MOVABLE, SITES, siteUnder } from "./carry.js"; +import { worldBox } from "./drawn.js"; +import { readAxis, reattach, setAxis, turnTo } from "./live.js"; +import { createPlayer } from "./motion_player.js"; +import { modelOf, world } from "./world.js"; + +// What each gripper holds, by name, as the page has it. +const held = new Map(); + +// Every resource of the given categories, with the box it takes up in the world. +function candidates(categories) { + const found = []; + for (let index = 0; index < world.names.length; index++) { + const category = modelOf(index).category; + if (categories.has(category)) found.push({ index, category, box: worldBox(index) }); + } + return found; +} + +function grip(gripper, point) { + const index = world.indexOfName.get(gripper); + if (index === undefined || held.has(gripper)) return; + const at = new THREE.Vector3(point.x, point.y, point.z).applyMatrix4(world.matrices[index]); + const taken = heldAt(at, candidates(MOVABLE)); + if (taken === undefined) return; + reattach(world.names[taken], gripper); + held.set(gripper, world.names[taken]); +} + +function release(gripper) { + const name = held.get(gripper); + held.delete(gripper); + const index = name === undefined ? undefined : world.indexOfName.get(name); + if (index === undefined) return; + const model = modelOf(index); + const lid = model.category === "lid"; + const seats = candidates(lid ? new Set([...SITES, ...LIDDABLE]) : SITES).map((c) => + LIDDABLE.has(c.category) + ? { + ...c, + covered: world.childrenOf[c.index].some( + (child) => child !== index && modelOf(child).category === "lid", + ), + } + : c, + ); + const site = siteUnder(worldBox(index), seats, { + index, + category: model.category, + nesting: model.nesting_z_height, + }); + // Nothing under it: it stays where it was let go of. + reattach(name, site === undefined ? null : world.names[site]); +} + +const player = createPlayer({ + readAxis, + setAxis, + indexOf: (name) => world?.indexOfName.get(name), + attach: reattach, + turnTo, + grip, + release, + // A tab in the background gets no frames, so nothing it played would ever finish and the + // command would wait out its timeout. It jumps instead. + skipping: () => document.hidden, +}); + +// How fast motions play against the drives' own speeds: `?motion=2` twice as fast, `?motion=0` +// not at all - everything jumps to where it ends and the command goes straight on. +const asked = Number(new URLSearchParams(location.search).get("motion") ?? 1); +player.setSpeed(Number.isFinite(asked) && asked >= 0 ? asked : 1); + +export const stepMotion = (delta) => player.step(delta); + +/** A new scene: nothing moving carries on, and nothing is held that PyLabRobot does not have. */ +export function dropMotions() { + player.dropAll(); + held.clear(); +} + +document.addEventListener("visibilitychange", () => { + if (document.hidden) player.finishAll(); +}); + +/** + * Play a motion the server sent, then say so. The command waits for that, so it is said whatever + * happens: a motion that cannot be played is reported played at once. + * + * @param {any} request what `motion.py` read from the command + * @param {() => void} done tells the server + */ +export async function playMotion(request, done) { + try { + if (world) await player.play(request); + } catch (error) { + console.warn("a motion could not be played", error); + } finally { + done(); + } +} diff --git a/pylabrobot/visualizer3D/static/motion_player.js b/pylabrobot/visualizer3D/static/motion_player.js new file mode 100644 index 00000000000..a907f546e9c --- /dev/null +++ b/pylabrobot/visualizer3D/static/motion_player.js @@ -0,0 +1,373 @@ +// Plays a motion the server read from a device command: the drives' moves, in the order the device +// makes them, each following its drive's speed profile. Pure - it is handed how to read and set +// positions and how to move a tip - so it can be checked outside a page. +// +// First the command's fixed time: what the device takes beyond its motion, measured from its own +// command timings, spent before anything moves. Then the stroke the simulator records a tip +// command as: across, with the arm and the channels moving at once, the channels setting off one +// after another (the ripple); down onto the targets, a tip pick-up pressing the last stretch +// slowly; back up. Before it, anything below the traverse height rises to it; at the bottom, tips +// change hands and an aspiration dwells, then leaves the liquid at its own swap speed. A command +// that only moves one axis plays that one move. +// +// An iSWAP command moves its drives at once: the head along Y or Z, the two joints turning about +// their pivots, or the jaws. Jaws that close take hold of what is between them; jaws that open let +// go of it first. + +import { motionProfile } from "./motion_profile.js"; + +// A move shorter than this, in mm, is not made at all. +const STILL = 0.05; + +// Where the crossing is among the phases: after the fixed time and the rise to traverse height. +const ACROSS = 2; + +/** + * Where a resource sits once turned about Z from `from` to `to` degrees, keeping `pivot` - a point + * in its own frame - where it was: what `Resource.rotate_to(z=..., pivot_coordinate=...)` does. + * + * @param {{x: number, y: number, z: number}} location relative to its parent, before the turn + * @param {number} from degrees + * @param {number} to degrees + * @param {{x: number, y: number, z: number}} pivot + * @returns {{x: number, y: number, z: number}} + */ +export function turnedLocation(location, from, to, pivot) { + const a = (from * Math.PI) / 180; + const b = (to * Math.PI) / 180; + // Where the pivot is in the parent's frame, which the turn keeps. + const px = location.x + Math.cos(a) * pivot.x - Math.sin(a) * pivot.y; + const py = location.y + Math.sin(a) * pivot.x + Math.cos(a) * pivot.y; + return { + x: px - (Math.cos(b) * pivot.x - Math.sin(b) * pivot.y), + y: py - (Math.sin(b) * pivot.x + Math.cos(b) * pivot.y), + z: location.z, + }; +} + +/** + * @param {object} deps + * @param {(index: number, axis: number) => number} deps.readAxis where a resource is, per axis + * @param {(index: number, axis: number, value: number) => void} deps.setAxis put it somewhere + * @param {(name: string) => number | undefined} deps.indexOf a resource by name + * @param {(name: string, parent: string | null, placed?: any) => void} deps.attach hand a tip + * to a new holder, at `placed` ({location, rotation}) when given, else where it stands + * @param {() => boolean} deps.skipping whether to jump to the end rather than play + * @param {(index: number, degrees: number, pivot: any) => void} [deps.turnTo] turn about a pivot + * @param {(gripper: string, point: any) => void} [deps.grip] take hold of what is at `point` + * @param {(gripper: string) => void} [deps.release] let go of what the gripper holds + */ +export function createPlayer({ + readAxis, + setAxis, + indexOf, + attach, + skipping, + turnTo = () => {}, + grip = () => {}, + release = () => {}, +}) { + let speed = 1; + // What is moving now: one entry per axis of a resource or joint, or a pause (no `apply`). + const running = new Set(); + // Motions being played, which keep the frame loop going between one move ending and the next + // starting. + let playing = 0; + + function finish(entry) { + entry.apply?.(entry.to); + running.delete(entry); + entry.resolve(); + } + + // Take a value from `from` to `to` as `drive` would, handing each step to `apply`. + function tween(from, to, drive, apply) { + /** @type {Promise} */ + const moved = new Promise((resolve) => { + const profile = motionProfile(to - from, drive?.speed, drive?.acceleration, drive?.jerk); + const entry = { from, to, profile, t: 0, resolve, apply }; + if (skipping() || !(speed > 0) || profile.duration === 0) finish(entry); + else running.add(entry); + }); + return moved; + } + + // Move one axis of a resource to `to`, as `drive` would. + function move(index, axis, to, drive) { + return tween(readAxis(index, axis), to, drive, (v) => setAxis(index, axis, v)); + } + + // Turn a joint to the angle its drive is sent to. The resource's rotation is that angle less the + // joint's `base`, and the drive goes the way its own angles run, not the short way round. + function turn(index, joint) { + const from = ((((readAxis(index, 5) + joint.base + 180) % 360) + 360) % 360) - 180; + return tween(from, joint.drive, joint, (angle) => + turnTo(index, angle - joint.base, joint.pivot), + ); + } + + function pause(seconds) { + /** @type {Promise} */ + const paused = new Promise((resolve) => { + if (skipping() || !(speed > 0) || !(seconds > 0)) { + resolve(); + return; + } + running.add({ profile: { duration: seconds }, t: 0, resolve }); + }); + return paused; + } + + // The phases of a motion, in order. Each looks at where things are when its turn comes and + // returns the moves it starts together; an empty one is skipped. + function phases(request, timing = { across: 0 }) { + const drives = request.drives ?? {}; + const channels = (key) => + (request.channels ?? []) + .filter((c) => c[key] !== null && c[key] !== undefined) + .map((c) => ({ ...c, index: indexOf(c.name) })) + .filter((c) => c.index !== undefined); + + return [ + // What the command takes beyond its motion, before anything moves. + () => (request.fixed > 0 ? [() => pause(request.fixed)] : []), + + // Up to traverse height: whatever is lower rises, whatever is higher stays. + () => + (request.traverse ?? []) + .map((t) => ({ ...t, index: indexOf(t.name) })) + .filter((t) => t.index !== undefined && readAxis(t.index, 2) < t.z - STILL) + .map((t) => () => move(t.index, 2, t.z, drives.z)), + + // Across: the arm in X and the channels in Y, at once. + () => { + const moves = []; + const arm = request.arm; + const armIndex = arm ? indexOf(arm.name) : undefined; + // How long the crossing takes, for a descent that sets off before it has finished. + timing.across = 0; + if (armIndex !== undefined && Math.abs(readAxis(armIndex, 0) - arm.x) >= STILL) { + const dx = arm.x - readAxis(armIndex, 0); + timing.across = motionProfile( + dx, + drives.x?.speed, + drives.x?.acceleration, + drives.x?.jerk, + ).duration; + moves.push(() => move(armIndex, 0, arm.x, drives.x)); + } + // The channels set off one after another, each `stagger` after the last. Who goes first + // depends on where they are going: the channel the move puts ahead of the others leaves + // first, and the ripple runs back along the direction of travel - a channel starting + // before the one in front of it has moved would run into the back of it. + const stagger = drives.y?.stagger > 0 ? drives.y.stagger : 0; + const moving = channels("y").filter((c) => Math.abs(readAxis(c.index, 1) - c.y) >= STILL); + const rankOf = new Map(); + for (const towardBack of [false, true]) { + moving + .filter((c) => c.y - readAxis(c.index, 1) > 0 === towardBack) + .sort((a, b) => (towardBack ? b.y - a.y : a.y - b.y)) + .forEach((c, rank) => { + rankOf.set(c, rank); + }); + } + moving.forEach((c) => { + const rank = rankOf.get(c); + const dy = c.y - readAxis(c.index, 1); + const travel = motionProfile(dy, drives.y?.speed, drives.y?.acceleration).duration; + timing.across = Math.max(timing.across, rank * stagger + travel); + moves.push(async () => { + if (rank > 0 && stagger > 0) await pause(rank * stagger); + await move(c.index, 1, c.y, drives.y); + }); + }); + return moves; + }, + + // Down onto the targets. + () => + channels("down") + .filter((c) => Math.abs(readAxis(c.index, 2) - c.down) >= STILL) + .map((c) => () => move(c.index, 2, c.down, drives.z)), + + // The last stretch of a tip pick-up, pressed on slowly. + () => + channels("press") + .filter((c) => Math.abs(readAxis(c.index, 2) - c.press) >= STILL) + .map( + (c) => () => + move(c.index, 2, c.press, { + speed: c.press_speed, + acceleration: drives.z?.acceleration, + }), + ), + + // At the bottom: tips change hands, and whatever the command does there takes its time. + () => { + const handovers = request.attach ?? []; + if (!handovers.length && !(request.dwell > 0)) return []; + return [ + async () => { + // Handed over where it stands, so the handover itself moves nothing; then seated where + // the model will have it - a tip pressed onto its shaft, or settling in its spot - at the + // pace of the Z drive, while whatever the command does at the bottom takes its time. + const seating = []; + for (const { name, parent, location } of handovers) { + attach(name, parent ?? null); + const index = indexOf(name); + if (!location || index === undefined) continue; + ["x", "y", "z"].forEach((key, axis) => { + if (Math.abs(readAxis(index, axis) - location[key]) >= STILL / 10) { + seating.push(move(index, axis, location[key], drives.z)); + } + }); + } + // An aspiration's tips follow the sinking surface down, at a steady pace, while it draws. + const following = channels("follow") + .filter((c) => Math.abs(readAxis(c.index, 2) - c.follow) >= STILL) + .map((c) => move(c.index, 2, c.follow, { speed: c.follow_speed })); + await Promise.all([pause(request.dwell), ...seating, ...following]); + }, + ]; + }, + + // Out of the liquid, at the command's own speed. + () => + channels("leave") + .filter((c) => c.leave - readAxis(c.index, 2) >= STILL) + .map( + (c) => () => + move(c.index, 2, c.leave, { + speed: c.leave_speed, + acceleration: drives.z?.acceleration, + }), + ), + + // Up by the pull-out distance before the transport air is drawn, at the swap speed. + () => + channels("pull_out") + .filter((c) => c.pull_out - readAxis(c.index, 2) >= STILL) + .map( + (c) => () => + move(c.index, 2, c.pull_out, { + speed: c.leave_speed, + acceleration: drives.z?.acceleration, + }), + ), + + // Back up, to where the command ends. + () => + channels("end") + .filter((c) => Math.abs(readAxis(c.index, 2) - c.end) >= STILL) + .map((c) => () => move(c.index, 2, c.end, drives.z)), + + // A drive of the iSWAP's head, each at the command's own speed. + () => + (request.moves ?? []) + .map((m) => ({ ...m, index: indexOf(m.name) })) + .filter( + (m) => m.index !== undefined && Math.abs(readAxis(m.index, m.axis) - m.to) >= STILL, + ) + .map((m) => () => move(m.index, m.axis, m.to, m)), + + // The joints, turning together about their pivots. + () => + (request.turns ?? []) + .map((joint) => ({ ...joint, index: indexOf(joint.name) })) + .filter((joint) => joint.index !== undefined) + .map((joint) => () => turn(joint.index, joint)), + + // The jaws: letting go before they open, taking hold once they have closed. + () => { + const jaws = request.jaws; + if (!jaws) return []; + const fingers = jaws.fingers + .map((f) => ({ ...f, index: indexOf(f.name) })) + .filter((f) => f.index !== undefined); + if (!fingers.length) return []; + // The first finger faces the grip centre from +Y, so it closes toward -Y. + const closing = fingers[0].y < readAxis(fingers[0].index, 1) - STILL; + const opening = fingers[0].y > readAxis(fingers[0].index, 1) + STILL; + return [ + async () => { + if (opening) release(jaws.gripper); + await Promise.all(fingers.map((f) => move(f.index, 1, f.y, jaws))); + if (closing) grip(jaws.gripper, jaws.grip_point); + }, + ]; + }, + ]; + } + + return { + setSpeed(value) { + speed = Math.max(0, value); + }, + + /** + * Play a motion to its end. Resolves when it has, or at once when it cannot be played. + * + * @param {any} request what `motion.py` read from the command + */ + async play(request) { + playing++; + try { + const timing = { across: 0 }; + const list = phases(request, timing); + for (let i = 0; i < list.length; i++) { + const moves = list[i](); + if (!moves.length) continue; + const crossing = Promise.all(moves.map((start) => start())); + // A descent that sets off before the crossing has finished (`down_from`, a share of the + // crossing's time): the firmware lowers a tip pick-up's channels while the arm still + // travels. Otherwise each phase waits for the one before it. + if (i === ACROSS && request.down_from > 0 && request.down_from < 1 && timing.across > 0) { + await pause(request.down_from * timing.across); + const down = list[i + 1](); + await Promise.all([crossing, ...down.map((start) => start())]); + i++; + continue; + } + await crossing; + } + } finally { + playing--; + } + }, + + /** + * Advance everything that is moving. For the frame loop; says whether anything is still + * moving, or about to. + * + * @param {number} delta seconds since the last frame + */ + step(delta) { + const step = delta * speed; + for (const entry of running) { + entry.t += step; + if (entry.t >= entry.profile.duration) { + finish(entry); + continue; + } + if (entry.apply) { + const share = entry.profile.progress(entry.t); + entry.apply(entry.from + (entry.to - entry.from) * share); + } + } + return running.size > 0 || playing > 0; + }, + + /** Bring everything moving to where it was going, at once. */ + finishAll() { + for (const entry of [...running]) finish(entry); + }, + + /** Drop everything moving where it stands: the scene it was moving in is gone. */ + dropAll() { + for (const entry of [...running]) { + running.delete(entry); + entry.resolve(); + } + }, + }; +} diff --git a/pylabrobot/visualizer3D/static/motion_profile.js b/pylabrobot/visualizer3D/static/motion_profile.js new file mode 100644 index 00000000000..d5d90236a49 --- /dev/null +++ b/pylabrobot/visualizer3D/static/motion_profile.js @@ -0,0 +1,127 @@ +// How far along a move a drive is at a given moment: speed up, cruise, slow down. +// +// Without a jerk limit, the symmetric trapezoid the STAR simulator times moves by +// (`_get_travel_time`): a move too short to reach cruise speed speeds up for half its length and +// slows down for the other half. With one, an S-curve: acceleration itself ramps at the jerk, which +// is how the X-arm moves on a STAR (fitted from HxUsbComm traces; see MOTION_PROFILES.md). +// Pure: no three, no page, so it can be checked on its own. + +/** + * @typedef {object} Profile + * @property {number} duration seconds the move takes + * @property {(t: number) => number} progress share of the distance covered at `t` seconds, 0..1 + */ + +/** + * @param {number} distance mm, of either sign; only its size counts + * @param {number} speed mm/s the drive cruises at + * @param {number} acceleration mm/s^2 it speeds up and slows down at + * @param {number} [jerk] mm/s^3 its acceleration changes at; none for a trapezoid + * @returns {Profile} + */ +export function motionProfile(distance, speed, acceleration, jerk) { + const d = Math.abs(distance); + if (!(d > 0) || !(speed > 0)) return { duration: 0, progress: () => 1 }; + if (!(acceleration > 0)) { + const duration = d / speed; + return { duration, progress: (t) => clamp(t / duration) }; + } + if (jerk > 0 && Number.isFinite(jerk)) return sCurve(d, speed, acceleration, jerk); + + const tAccel = speed / acceleration; + const dAccel = 0.5 * acceleration * tAccel * tAccel; + + if (d >= 2 * dAccel) { + // Trapezoid: up to speed, cruise, and down again. + const dCruise = d - 2 * dAccel; + const tCruise = dCruise / speed; + const duration = 2 * tAccel + tCruise; + return { + duration, + progress: (t) => { + if (t >= duration) return 1; + if (t <= tAccel) return (0.5 * acceleration * t * t) / d; + if (t <= tAccel + tCruise) return (dAccel + speed * (t - tAccel)) / d; + const s = t - tAccel - tCruise; + return clamp((dAccel + dCruise + speed * s - 0.5 * acceleration * s * s) / d); + }, + }; + } + + // Triangle: never reaches cruise speed. + const vPeak = Math.sqrt(acceleration * d); + const tPeak = vPeak / acceleration; + const duration = 2 * tPeak; + return { + duration, + progress: (t) => { + if (t >= duration) return 1; + if (t <= tPeak) return (0.5 * acceleration * t * t) / d; + const s = t - tPeak; + return clamp((0.5 * d + vPeak * s - 0.5 * acceleration * s * s) / d); + }, + }; +} + +// The phases of one ramp from rest to `v`: jerk up, hold the acceleration, jerk down - the middle +// phase only when `v` is high enough for the acceleration to reach its limit. +function ramp(v, a, j) { + if (v >= (a * a) / j) return [a / j, v / a - a / j, a / j]; + const t1 = Math.sqrt(v / j); + return [t1, 0, t1]; +} + +const rampTime = (v, a, j) => ramp(v, a, j).reduce((s, t) => s + t, 0); + +/** Rest to rest over `d`, the speed, acceleration and jerk all limited: seven phases at most. */ +function sCurve(d, vmax, a, j) { + // The peak speed: the cruise speed if the move is long enough to reach it, else the speed at + // which a ramp up and a ramp down cover the distance between them (found by halving). + let v = vmax; + if (d < vmax * rampTime(vmax, a, j)) { + let lo = 0; + let hi = vmax; + for (let i = 0; i < 60; i++) { + const mid = (lo + hi) / 2; + if (mid * rampTime(mid, a, j) < d) lo = mid; + else hi = mid; + } + v = hi; + } + const [t1, t2, t3] = ramp(v, a, j); + const cruise = Math.max(0, (d - v * (t1 + t2 + t3)) / v); + // Each phase as [duration, jerk]; the accelerations and speeds follow by integration. + const phases = [ + [t1, j], + [t2, 0], + [t3, -j], + [cruise, 0], + [t3, -j], + [t2, 0], + [t1, j], + ]; + const duration = phases.reduce((s, [t]) => s + t, 0); + return { + duration, + progress: (t) => { + if (t >= duration) return 1; + let p = 0; + let vel = 0; + let acc = 0; + let left = Math.max(0, t); + for (const [dt, jk] of phases) { + const s = Math.min(dt, left); + p += vel * s + (acc * s * s) / 2 + (jk * s * s * s) / 6; + vel += acc * s + (jk * s * s) / 2; + acc += jk * s; + left -= s; + if (left <= 0) break; + } + return clamp(p / d); + }, + }; +} + +function clamp(p) { + return Math.max(0, Math.min(1, p)); +} diff --git a/pylabrobot/visualizer3D/static/transport.js b/pylabrobot/visualizer3D/static/transport.js index e0cbf3a38ee..931449db23d 100644 --- a/pylabrobot/visualizer3D/static/transport.js +++ b/pylabrobot/visualizer3D/static/transport.js @@ -53,6 +53,11 @@ function sayHello() { ); } +/** Say something to the server, when there is a socket to say it on. */ +export function send(event, data) { + if (socket?.readyState === WebSocket.OPEN) socket.send(JSON.stringify({ event, data })); +} + export function connect() { if ( socket && @@ -89,6 +94,8 @@ export function connect() { if (kind === "scene") handlers.scene(data); else if (kind === "state") handlers.state(data); else if (kind === "moves") handlers.moves(data.moves); + else if (kind === "motion") handlers.motion(data); + else if (kind === "collisions") handlers.collisions(data); }; } diff --git a/pylabrobot/visualizer3D/transport_demo.py b/pylabrobot/visualizer3D/transport_demo.py new file mode 100644 index 00000000000..a8bf2b6bb82 --- /dev/null +++ b/pylabrobot/visualizer3D/transport_demo.py @@ -0,0 +1,71 @@ +"""The iSWAP moving a lidded plate and its lid with `iSWAPTransport`, the model kept as it goes. + +Run it: + + python -m pylabrobot.visualizer3D.transport_demo + +Each move is planned from the resources - where they are, which side they are gripped from - and +made of the iSWAP's primitive moves, which the page plays. The model hands what is gripped to the +gripper when the jaws close and to where it goes when they open, so the page shows what PyLabRobot +holds. + +Until stopped: the plate goes over with its lid, gripped from the front; the lid comes off onto the +empty site and goes back on; the plate comes back gripped from the back, and so turned round, and +is turned back. +""" + +import asyncio +import logging + +from pylabrobot.hamilton.star.driver.features.iswap_transport import iSWAPTransport +from pylabrobot.hamilton.star.motion import attach_viewer_motion +from pylabrobot.resources.corning.plates import cor_96_wellplate_360uL_Fb_lid +from pylabrobot.resources.plate import Plate + +from .demo import build_facility, star_of +from .server import Viewer3D + + +async def main() -> None: + logging.disable(logging.WARNING) + facility = build_facility() + star = star_of(facility) + deck = star.deck + deck.get_resource("destination_1").unassign() + plate = deck.get_resource("source_1") + assert isinstance(plate, Plate) and plate.parent is not None + lid = cor_96_wellplate_360uL_Fb_lid(name="source_1_lid") + plate.assign_child_resource(lid) + await star.setup() + if star.iswap is None: + raise RuntimeError("the simulated STARlet has no iSWAP") + transport = iSWAPTransport(star.iswap) + start, other = plate.parent, deck.get_resource("destination_carrier").children[1] + + viewer = Viewer3D(facility, name="transport_demo.py") + await viewer.start() + attach_viewer_motion(star.driver, viewer) + print("waiting for a browser to draw the scene") + await viewer.wait_for_browser() + await asyncio.sleep(1.0) + + await star.iswap.make_space() + while True: + print("the plate goes over, with its lid") + await transport.move_resource(plate, other, pickup_direction="front") + print("its lid comes off, onto the site it left") + await transport.move_resource(lid, start, pickup_direction="front") + print("and goes back on") + await transport.move_resource(lid, plate, pickup_direction="front") + print("the plate comes back, gripped from the back: turned round") + await transport.move_resource(plate, start, pickup_direction="back", drop_direction="front") + print("and is turned back") + await transport.move_resource(plate, start, pickup_direction="front", drop_direction="back") + await asyncio.sleep(1.0) + + +if __name__ == "__main__": + try: + asyncio.run(main()) + except KeyboardInterrupt: + pass