From e6234df0badb79dd0f7dd0d2026f9a42710634b9 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Fri, 13 Nov 2026 19:51:55 +0000 Subject: [PATCH] v1 `Mantis`: move the arm in the deck frame, guarded, with zone routes and approach paths The arm's configuration on mantis.arm.configuration; moves in the deck frame through the unit's stage transform, each move across guarded by a rise to the traverse height; speeds in mm/s at the nozzle; park and recovery along each zone's route; move_to_waste and move_to_wash from wherever the head stands; request_pose for every joint in the deck frame. Co-Authored-By: Claude Opus 5.5 --- docs/api/pylabrobot.formulatrix.rst | 2 + .../formulatrix/mantis/hello-world.ipynb | 122 +- pylabrobot/formulatrix/mantis/design.md | 220 ++- pylabrobot/formulatrix/mantis/device.py | 46 +- pylabrobot/formulatrix/mantis/device_tests.py | 25 +- .../mantis/driver/arm_kinematics.py | 17 +- .../mantis/driver/arm_kinematics_tests.py | 5 +- .../mantis/driver/configuration.py | 12 +- .../formulatrix/mantis/driver/features/arm.py | 1555 +++++++++++++++-- .../mantis/driver/features/arm_tests.py | 982 ++++++++++- .../formulatrix/mantis/driver/master.py | 135 +- .../formulatrix/mantis/driver/master_tests.py | 7 +- .../formulatrix/mantis/driver/oem_config.py | 310 +++- .../mantis/driver/oem_config_tests.py | 341 +++- .../mantis/driver/recordings/README.md | 6 +- .../mantis/driver/sequence_files.py | 85 + .../mantis/driver/sequence_files_tests.py | 62 + .../formulatrix/mantis/driver/simulator.py | 58 +- .../mantis/driver/simulator_tests.py | 192 +- .../mantis/driver/stage_transform.py | 54 + .../mantis/driver/stage_transform_tests.py | 74 + .../mantis/resource_model/__init__.py | 26 +- 22 files changed, 3963 insertions(+), 373 deletions(-) create mode 100644 pylabrobot/formulatrix/mantis/driver/sequence_files.py create mode 100644 pylabrobot/formulatrix/mantis/driver/sequence_files_tests.py create mode 100644 pylabrobot/formulatrix/mantis/driver/stage_transform.py create mode 100644 pylabrobot/formulatrix/mantis/driver/stage_transform_tests.py diff --git a/docs/api/pylabrobot.formulatrix.rst b/docs/api/pylabrobot.formulatrix.rst index 5c237c97898..05cc281a13a 100644 --- a/docs/api/pylabrobot.formulatrix.rst +++ b/docs/api/pylabrobot.formulatrix.rst @@ -22,6 +22,8 @@ Mantis driver.simulator.MantisSimulationLedBar driver.features.arm.Arm driver.arm_kinematics + driver.stage_transform + driver.sequence_files driver.features.plate_clamp.PlateClamp driver.features.pump.Pump driver.features.rfid.Rfid diff --git a/docs/user_guide/formulatrix/mantis/hello-world.ipynb b/docs/user_guide/formulatrix/mantis/hello-world.ipynb index 9f0d1825603..f76fb19e177 100644 --- a/docs/user_guide/formulatrix/mantis/hello-world.ipynb +++ b/docs/user_guide/formulatrix/mantis/hello-world.ipynb @@ -7,7 +7,7 @@ "source": [ "# Formulatrix Mantis\n", "\n", - "The Mantis is a liquid dispenser from Formulatrix that dispenses through exchangeable chips. PyLabRobot connects to it, identifies it, reads its firmware versions, name, motors and pressure sensors, brings the arm and Z up and parks them at stop (v4), locks and unlocks the plate clamp, reads which chip sits in each input slot, and shows colours on a v4's LED bar. Nothing else in this driver moves the device, switches the pump on or opens a valve." + "The Mantis is a liquid dispenser from Formulatrix that dispenses through exchangeable chips. PyLabRobot connects to it, identifies it, reads its firmware versions, name, motors and pressure sensors, brings the arm and Z up, moves the nozzle in the deck frame, parks it from any zone and moves the head into the waste and either wash position (v4), locks and unlocks the plate clamp, reads which chip sits in each input slot, and shows colours on a v4's LED bar. Nothing in this driver switches the pump on or opens a valve." ] }, { @@ -25,7 +25,7 @@ "metadata": {}, "source": [ "```{warning}\n", - "This driver has run on one Mantis v4, on macOS and on Windows; no v3 has run it yet. The arm's bring-up and the stop that parks it have not yet run on a device through this driver; run them with the head clear and someone at the power switch. `setup()` logs a warning on a firmware set not seen on a Mantis yet, which includes every v3. If you run this guide on a Mantis, please [report the result](../../needs-testing.md).\n", + "This driver has run on one Mantis v4, on macOS and on Windows; no v3 has run it yet. The arm's bring-up, its moves, the waste, the washes and the stop that parks it have not yet run on a device through this driver; run them with the head clear and someone at the power switch. `setup()` logs a warning on a firmware set not seen on a Mantis yet, which includes every v3. If you run this guide on a Mantis, please [report the result](../../needs-testing.md).\n", "```" ] }, @@ -42,12 +42,12 @@ "| Reads | firmware versions, device name, motor limits and state, the arm's absolute encoders (v4), line pressures and the pressure board's state |\n", "| Interface | USB, an FTDI bridge (VID 0x0403 / PID 0x6010): interface B for commands, interface A for the RFID reader |\n", "| Sequencer | queues PPI writes and moves of the three motors, and waits until each is played; internal, for the features that act |\n", - "| Arm (v4) | brought up at setup from where its absolute encoders read; Z homed; parked at idle at stop |\n", + "| Arm (v4) | brought up at setup from where its absolute encoders read; Z homed; the nozzle moved in the deck frame, across or up and down, through the unit's stage transform; parked along the unit's routes from any zone; moved into the waste and either wash position along the unit's approach paths, from wherever it stands |\n", "| Plate clamp | lock at one of the unit's levels, waiting for clamping done; unlock |\n", "| RFID | the chip in each input slot: serial, chip type, date of manufacture |\n", "| LED bar (v4) | one colour, one per LED, or an animation; shows setup, with the arm's bring-up; off at stop |\n", - "| Unit configuration | the clamp's lock levels, each slot's RFID antenna and the arm's calibration, from the unit's configuration folder or a saved file; the first two else from a recording of its generation |\n", - "| Not supported yet | moving the arm to a position, switching the pump or a pressure controller, chips, dispensing |" + "| Unit configuration | the clamp's lock levels, each slot's RFID antenna, the arm's calibration, its routes and the stage transform, from the unit's configuration folder or a saved file; the first two else from a recording of its generation |\n", + "| Not supported yet | switching the pump or a pressure controller, chips, dispensing |" ] }, { @@ -81,9 +81,9 @@ "source": [ "## Unit configuration\n", "\n", - "Some of a Mantis's values are set per unit and cannot be read off the device; they sit in the unit's `.config` files, in `Data/Device/Configs`. Give setup that folder once, then save the configuration: the saved file carries the unit's values from then on, and the folder is no longer needed. So far these are the plate clamp's lock levels, each input slot's RFID antenna and, on a v4, the arm's calibration: each arm motor's offset, absolute encoder reference and encoder correction.\n", + "Some of a Mantis's values are set per unit and cannot be read off the device; they sit in the unit's `.config` files, in `Data/Device/Configs`. Give setup that folder once, then save the configuration: the saved file carries the unit's values from then on, and the folder is no longer needed. So far these are the plate clamp's lock levels, each input slot's RFID antenna and, on a v4, the arm's calibration: each arm motor's offset, absolute encoder reference and encoder correction, and the routes the head takes: each zone's route to idle and the approach paths to the waste and the two wash positions; and the stage transform that places the nozzle: its transformation matrix and tilt, and the plate holder's origin.\n", "\n", - "Without the clamp's levels or the antennas, a recording of a unit of the same generation supplies them, with a warning: the Mantis runs, less accurately. The arm's calibration never comes from a recording: without it the arm is not brought up and setup refuses. Where the folder and a saved file both hold a value, they must agree, or setup refuses. A file saved by an earlier version still loads: a value it lacks comes from the folder, else the recording, with one warning naming it." + "Without the clamp's levels or the antennas, a recording of a unit of the same generation supplies them, with a warning: the Mantis runs, less accurately. The arm's calibration, its routes and the stage transform never come from a recording: without the calibration the arm is not brought up and setup refuses; without the routes the head is moved to idle only from near it, and the waste and the washes refuse; without the stage transform the nozzle's moves refuse. Where the folder and a saved file both hold a value, they must agree, or setup refuses. A file saved by an earlier version still loads: a value it lacks comes from the folder, else the recording, with one warning naming it." ] }, { @@ -105,7 +105,7 @@ "source": [ "## Setup\n", "\n", - "`setup()` opens both links, reads the firmware versions, the motor limits and the pressure sensors' calibration, takes the unit's values and sets up the RFID reader. On a v4 it then brings the arm and Z up, as in the recorded v4 startup: it energises the arm motors, declares them where their absolute encoders read, moves them to idle, homes Z, and moves to idle again. It does so only if the head stands near idle, in front of or behind it; from anywhere else it refuses, the arm unpowered: park the arm at idle first. `setup(skip_arm=True)` reads only and moves nothing. On a v4 the LED bar shows the setup, the arm's bring-up with it, green for a second at its end, then the idle animation." + "`setup()` opens both links, reads the firmware versions, the motor limits and the pressure sensors' calibration, takes the unit's values and sets up the RFID reader. On a v4 it then brings the arm and Z up, as in the recorded v4 startup: it energises the arm motors, declares them where their absolute encoders read, moves them to idle, homes Z, and moves to idle again. From wherever the head stands it follows its zone's routes. Without the unit's routes it does so only if the head stands near idle, in front of or behind it; from anywhere else it refuses, the arm unpowered: park the arm at idle first. `setup(skip_arm=True)` reads only and moves nothing. On a v4 the LED bar shows the setup, the arm's bring-up with it, green for a second at its end, then the idle animation." ] }, { @@ -186,7 +186,7 @@ "source": [ "## Visualizer\n", "\n", - "Show the Mantis in the 3D visualizer. `start()` serves it and opens it in a browser." + "Show the Mantis in the 3D visualizer. `start()` serves it and opens it in a browser. After each arm move the arm's links and the Z column take where the encoders read." ] }, { @@ -259,7 +259,7 @@ "metadata": {}, "outputs": [], "source": [ - "mantis.driver.configuration.motor_limits" + "mantis.arm.configuration.motor_limits" ] }, { @@ -385,7 +385,7 @@ "metadata": {}, "outputs": [], "source": [ - "await mantis.arm.request_joint_position()" + "await mantis.arm.request_joint_state()" ] }, { @@ -403,7 +403,7 @@ "metadata": {}, "outputs": [], "source": [ - "await mantis.arm.request_absolute_joint_position()" + "await mantis.arm.request_absolute_joint_state()" ] }, { @@ -413,7 +413,7 @@ "metadata": {}, "outputs": [], "source": [ - "await mantis.arm.request_head_position()" + "await mantis.arm.request_pose()" ] }, { @@ -441,7 +441,69 @@ "metadata": {}, "outputs": [], "source": [ - "mantis.arm.default_arm_speed, mantis.arm.default_z_speed" + "mantis.arm.default_xy_speed, mantis.arm.default_z_speed" + ] + }, + { + "cell_type": "markdown", + "id": "arm-moves", + "metadata": {}, + "source": [ + "### Moving the arm (v4)\n", + "\n", + "The nozzle moves in the deck frame, mm: `move_to_xy_position` moves it across, `move_to_z_position` up or down with the arms held. Every move across is guarded, as on the STAR: Z first rises to `minimum_traverse_height_start` (`default_minimum_traverse_height` when None, else the safe height) if the nozzle is below it. Over the plate holder the unit's stage transform places it; elsewhere the arm frame fitted to that stage. `move_to_location` also lowers Z to the location's height after. Speeds are the nozzle's, in mm/s; without one the arms move at the route profile. Every target is checked against the motor limits, and Z's speed and acceleration against the range of the recorded moves (1 to 55 mm/s, 200 to 500 mm/s²), before anything is sent. Nothing models the chip racks: move only with the path clear." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "arm-move-to-location", + "metadata": {}, + "outputs": [], + "source": [ + "pose = await mantis.arm.request_pose()\n", + "await mantis.arm.move_to_xy_position(x=pose.nozzle_location.x, y=pose.nozzle_location.y - 20.0)\n", + "await mantis.arm.park()" + ] + }, + { + "cell_type": "markdown", + "id": "arm-park", + "metadata": {}, + "source": [ + "`park()` moves the head to idle along its zone's route, rising first if it is low; `request_is_parked()` reads whether it stands there. `recover_from_fault()` recovers after a motor error as setup does." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "arm-park-code", + "metadata": {}, + "outputs": [], + "source": [ + "await mantis.arm.park()\n", + "await mantis.arm.request_is_parked()" + ] + }, + { + "cell_type": "markdown", + "id": "arm-approach-paths", + "metadata": {}, + "source": [ + "`move_to_waste()` and `move_to_wash(position=...)` follow the unit's approach paths from wherever the head stands: from one wash position to the other it stays in the wash. `park()` takes the head back out along the path it is on, then to idle." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "arm-approach-paths-code", + "metadata": {}, + "outputs": [], + "source": [ + "await mantis.arm.move_to_waste()\n", + "await mantis.arm.park()\n", + "await mantis.arm.move_to_wash(position=1)\n", + "await mantis.arm.park()" ] }, { @@ -572,6 +634,36 @@ "await mantis.plate_clamp.request_state()" ] }, + { + "cell_type": "markdown", + "id": "arm-wellplate", + "metadata": {}, + "source": [ + "### Moving over a wellplate (v4)\n", + "\n", + "A well's centre in the deck frame comes from the resource model; the wellplate is taken to be clamped with A1 at the back left. Heights are those of a chip's nozzle, from the unit's calibration; where the head stands without a chip is not measured, so stay high, here 12.5 mm above the wellplate. Start from idle with the path clear." + ] + }, + { + "cell_type": "code", + "id": "arm-wellplate-code", + "metadata": {}, + "source": [ + "from pylabrobot.resources.corning import cor_96_wellplate_360uL_Fb\n", + "\n", + "wellplate = cor_96_wellplate_360uL_Fb(name=\"wellplate\")\n", + "mantis.plate_holder.assign_child_resource(wellplate)\n", + "top = wellplate.get_location_wrt(mantis.deck, z=\"t\").z\n", + "await mantis.arm.park()\n", + "for name in (\"A1\", \"H12\"):\n", + " center = wellplate.get_item(name).get_location_wrt(mantis.deck, x=\"c\", y=\"c\")\n", + " await mantis.arm.move_to_xy_position(x=center.x, y=center.y)\n", + " await mantis.arm.move_to_z_position(z=top + 12.5)\n", + "await mantis.arm.park()" + ], + "execution_count": null, + "outputs": [] + }, { "cell_type": "markdown", "id": "mantis-clamp-unlock-text", @@ -826,7 +918,7 @@ "source": [ "## Stop\n", "\n", - "Stop the visualizer and close the link. On a v4, `stop()` first aborts the sequencer and stops every motor; an arm brought up is then parked at idle and Z lowered to 20 mm, as in the recorded close, unless the head is away from idle. Then the plate clamp is released." + "Stop the visualizer and close the link. On a v4, `stop()` first aborts the sequencer and stops every motor; an arm brought up is then parked at idle and Z lowered to 20 mm, as in the recorded close; from away from idle the head first follows its zone's route, and without the unit's routes it is not parked. Then the plate clamp is released." ] }, { diff --git a/pylabrobot/formulatrix/mantis/design.md b/pylabrobot/formulatrix/mantis/design.md index 5bbc0b6a0e1..4e537e6dda8 100644 --- a/pylabrobot/formulatrix/mantis/design.md +++ b/pylabrobot/formulatrix/mantis/design.md @@ -15,6 +15,8 @@ driver/master.py MantisDriver. Owns the link, the reader, reply matc bar. driver/arm_kinematics.py The arm's geometry and units: device units, inverse and forward kinematics, the absolute encoder's correction. Pure maths. +driver/stage_transform.py The stage transform: where a point on the wellplate is in the arm + frame. Pure maths. driver/events.py The events the device sends unasked: their codes, what each carries and how it is logged. driver/simulator.py MantisSimulationDriver, MantisSimulationArm, @@ -25,7 +27,9 @@ driver/framing.py The frame: header, check word, splitting, payload s driver/errors.py MantisError and one error per kind; what the device's codes mean. driver/configuration.py DeviceConfiguration: what the device answered about itself. driver/oem_config.py The unit's own values, read from its vendor - .config files, into a declared configuration. + .config and sequence files, into a declared configuration. +driver/sequence_files.py The unit's sequence files: the waypoints of a route that moves the + arm. driver/confirmed_firmware_versions.py The firmware sets confirmed on a device, per generation. driver/features/ One class per thing the device is fitted with: Arm, PlateClamp, Pump, @@ -59,8 +63,8 @@ no LED count. A difference between v3 and v4 is expressed there, by which featur by a branch on the generation inside a command. **M4. A configuration holds facts only, and is data.** (STAR P3, P13.) `DeviceConfiguration` holds -what the device answered: generation, firmware versions, motor limits, and the pressure board's -sensor calibration (`feedback_sensor_parameters`), read at setup and never written. +what the device answered: generation, firmware versions and the pressure board's sensor +calibration (`feedback_sensor_parameters`), read at setup and never written. `save_configuration` writes it; `declared_configuration_json` reads one back and setup refuses if the device disagrees on a fitted field (`FITTED_FIELDS`). The calibration is neither saved nor declared: the bottle and chip sensors' offsets are re-zeroed on the device between sessions, so @@ -68,8 +72,10 @@ they are read at every setup. A declared file that holds one, saved by an earlie it is ignored. A simulated pump answers the generation's recording's. Each feature gets its own configuration when it is added, holding its own facts; the device configuration does not grow per feature, the calibration excepted, as discovery reads it with the -firmware versions. Motor limits are compared as a build fact: every v4 is taken to share them until -a second device shows otherwise. +firmware versions. The arm's is `MantisArmConfiguration` on `arm.configuration`, as the PF400's +`PreciseFlexArmConfiguration`: the motor limits, read for it at setup, and the unit's `arm`, `zones`, +`approach_paths` and `stage` blocks. The motor limits are read at every setup and checked against every +target, so they are not compared with a declared file; only the generation is fitted. **M4a. A unit's own values come from its folder or its declared file, else its generation's recording.** What is set per unit and cannot be read off the device sits in the unit's vendor @@ -84,13 +90,20 @@ folder the saved file alone carries them. A block is loaded only where its sourc `plate_clamp.levels` for a clamp driven as a DC motor, `rfid.slot_to_antenna` where `RFID.config` exists, `led_bar.number_of_leds` where `Device.config` sets `NumberOfLed`, `arm` where the unit declares the arms' absolute encoder references (a v4), its encoder correction from the `Data` -folder. +folder; `zones` where the unit declares its zones (a v4), with each zone's routes, and +`approach_paths`, the unit's paths from idle to the waste (`"waste"`) and to each wash position +(`"wash_1"`, `"wash_2"`), from `System/Sequences/Common`; `stage` where the unit +declares its stage transform, with its tilt and arm type and the plate holder's origin from the +`Data` folder. **M4d. Motion calibration never comes from a recording.** A block that calibrates motion -(`MOTION_CALIBRATION_BLOCKS`: `arm`) comes from the unit's folder or its declared file only; no -recording holds one. Without it, nothing that needs it is sent: `Arm.initialize` refuses, and so -does `setup` unless `skip_arm=True`. Factory files (encoder corrections, taught sequences) are -never committed; tests use synthetic tables. +(`MOTION_CALIBRATION_BLOCKS`: `arm`, `zones`, `approach_paths`, `stage`) comes from the unit's folder or +its declared file only; no recording holds one. Without it, nothing that needs it is sent: without +`arm` `Arm.initialize` refuses, and so does `setup` unless `skip_arm=True`; without `zones` the head +is moved to idle from near idle only; without `approach_paths` a move to the waste or a wash refuses; without +`stage` a move to a wellplate refuses. An arm type other than `Standard` is refused. A route file is +read whole and refused on any line it does not understand, never skipped. Factory files (encoder +corrections, taught sequences) are never committed; tests use synthetic tables. Which vendor values are the unit's own, from a v3 and a v4 install side by side: @@ -157,7 +170,17 @@ the device sent, decoded but not converted, until the feature that owns it knows A feature then converts in one place, on its configuration, and speaks PyLabRobot's units. No unit suffix on names in default units. The arm converts in `arm_kinematics`: 200 device units per motor revolution, degrees on the arms and mm on Z (down from its flag), each rate rounded up half a step -as on the wire. +as on the wire. A joint state keeps the motor's sign (Z down from its flag). The routes keep the +frame their files write: the arm frame, Z down from its flag. Every public position and height is +the nozzle's in the deck frame, every public speed the nozzle's in mm/s and mm/s²; degrees/s stay +inside, on the wire. Over the plate holder a point goes through the stage transform +(`stage_transform`) in full: y flipped to run from the holder's back edge, the height taken below +the unit's `ReferenceHeight` (`StageTuning.config`), the arm type, the holder's origin, the +projective matrix on x and y and the tilt on z; a point read back is the matrix inverted, so it +reads where it was sent. Elsewhere x and y go through the arm frame fitted once to the stage over +the holder, its edges included (`_get_deck_to_arm_transform`): one turn and shift, as the resource +model places the arm by the manufacturer's model and each unit's differs by millimetres. A height is +always the stage's, its tilt extended past the holder. **M7. A range is checked before anything is sent.** (STAR P5.) A read accepts only the indices seen on a device (`MOTORS`, `ABSOLUTE_ENCODER_MOTORS`, the boards, `PRESSURE_CONTROLLERS`); @@ -168,16 +191,58 @@ P9.) The raw call is `_unchecked_fw_*` and sends once; the public method checks confirms what the device reports. `lock` checks the level against the declared levels, calls `_unchecked_fw_do_plate_clamping` and reads the clamp state until it reports clamping done. `Arm.initialize` matches the recorded v4 startup command for command: energise the arms, declare -them three times where their absolute encoders read, set their following error, clear Z, declare, -move to idle, home Z (waiting for its home-done event), twice declare and move to idle, home Z. A -move reads each motor's status until none moves; a fault bit raises `MantisMotionError`. Idle is -`POST_HOMING_POSITION` (the same on every unit seen) less the unit's offsets. +them three times where their absolute encoders read, set their following error, clear Z, then +recover to idle. Recovering declares the arms, moves them along the head's zone's route to its safe +XY, homes Z (waiting for its home-done event), twice declares them and moves along the zone's route +to idle, and homes Z; the zone is read again before each route, from the controller's position +just declared. `recover_from_fault` stops every motor, clears Z's faults then the arms', and +recovers the same way, as in a recorded recovery after a motor error. A route moves each motor its +line names on its own (command 22), as in the recorded recoveries; every target is checked first. A +command 22 answers a motor error code, and one not 0 raises `MantisMotionError` before the next +move is sent. A move reads each motor's status until none moves; a fault bit raises +`MantisMotionError`. Routes move at the `ROUTE_*` profile, the only one queued moves were recorded +at; the other moves take the arms at it too unless an xy speed is given. A move that fails stops +each motor at that move's own acceleration. Idle is +`POST_HOMING_POSITION` (the same on every unit seen) less the unit's offsets; a route line's +(x, y) goes through the same inverse kinematics, so a line at (15, 31.180) is sent as such. + +The moves are built bottom up. `move_to_joint_state` moves the motors a joint state names, queued +(command 68), the sequencer cleared and started before each item and the item waited for. A move of +Z alone is one item. A move of an arm motor is guarded, as v1 STAR's moves across the deck +(`move_to_xy_positions`): Z rises first to `minimum_traverse_height_start` (the nozzle's height above +the deck over where it starts and ends; `default_minimum_traverse_height` when None, and with that +None `SAFE_HEIGHT` above Z's flag) if its encoder reads it below, by command 22, and is +read back by its encoder (command 84, within `Z_TRAVERSE_TOLERANCE`) or it raises; then the arms move +with Z held; then Z, if named. `move_to_xy_position` takes the nozzle to a deck-frame (x, y) through +the inverse kinematics on it; `move_to_location` adds Z's move to the location's height; +`move_to_z_position` moves Z alone where the nozzle stands. Every public move that moves the arms is +guarded; a dispense, which moves between wells at well height, queues its moves +privately. Every move checks every target against the motor limits read, and Z's speed and +acceleration against `Z_SPEED_RANGE` and `Z_ACCELERATION_RANGE`, before anything is sent. Nothing +models the chip racks. + +An xy speed in mm/s turns into each arm motor's by its turn over the nozzle's straight-line +distance, acceleration likewise: the motors start and end together, the move takes as long as a +straight one would, and the nozzle's path is not straight. Without one, given or +`default_xy_speed`/`default_xy_acceleration`, the arms take the route profile. `park` rises and reads Z back the same way, then follows the zone's +route to idle. + +`move_to_waste` and `move_to_wash(position=...)` follow the unit's approach paths, queued line for +line as in the recorded move to the waste, from wherever the head stands, which is read from the +motors, never remembered. A head on a path's descended part (after its last line at the safe +height, within `APPROACH_PATH_TOLERANCE` of a line) is on that path. On the target's path it goes +on from there. On a neighbouring path, one sharing the target's descent (the two washes), it goes +back to that path's safe-Z line (its last line but one), then to the target's and down. On another it goes back to the last line the two share, then +forward. In the open it is parked first. Going back, a path's lines play backwards, one queued +item each; a line naming the arms and Z moves both +in one item. Every other move, and `park`, from a pose on a path first goes back along it to its +start. **M9. Names say what kind of thing they are.** (STAR P10.) | what it touches | prefix | here | |---|---|---| -| the device, doing work | `move_` | | +| the device, doing work | `move_` | `arm.move_to_xy_position`, `arm.move_to_z_position` | | the device, a process left running | `start_`, `stop_` | | | the device's sensors | `measure_`, `sense_` | `pump.measure_pressure` | | the device's memory, reading | `request_` | `request_motor_limits`, `request_motor_status` | @@ -191,14 +256,15 @@ Commands, boards and fields are named for what they do; no vendor class, method (pump, pressure controllers, clamp, a chip) turns it off or releases it in a `finally`, each step in its own `try`, and the first fault is the one raised. `stop` fails what waits on the link; on a v4 it aborts the sequencer, stops each motor at its default deceleration and, if every motor stopped, -parks an arm brought up: Z up on its own, read back by its encoder (command 84), never the demand -position (command 27); then queued, as in the recorded close, the arms, both, then Z down to -`SHUTDOWN_Z` at idle. Then it releases each built feature, turns the LED bar off and closes the -link. A move that fails part way stops the motors it sent a move and logs where their encoders -read. A `lock` that does not report clamping done releases the clamp before raising, and `stop` -releases a clamp that does not read released. A failed `Arm.initialize` unpowers the arm motors -again, and a failed setup's `stop` does not park them. Failure behaviour is designed before -coverage. +parks an arm brought up: from away from idle first along its zone's route; then Z up on its own, +read back by its encoder (command 84), never the demand position (command 27); then queued, as in +the recorded close, the arms, both, then Z down to `SHUTDOWN_Z` at idle. Then it releases each +built feature, turns the LED bar off and closes the link. A move that fails part way stops the +motors it sent a move and logs where their encoders read. A `lock` that does not report clamping +done releases the clamp before raising, and `stop` releases a clamp that does not read released. A +failed `Arm.initialize` or `recover_from_fault` unpowers the arm motors again, and a failed setup's +`stop` does not park them. Queued moves not reported played abort the sequencer, so nothing more +plays, and log where the motors stopped. Failure behaviour is designed before coverage. **M11. Firmware is confirmed, never gated.** A version set outside `CONFIRMED_FIRMWARE_VERSIONS` warns once at setup; nothing refuses. A set goes in once it has been seen on a device. @@ -226,11 +292,12 @@ above the wire: `MantisSimulationPlateClamp` reads 0 until a command sets clampi unclamping done; `MantisSimulationRfid` reads every slot empty; `MantisSimulationPump` reads off, every line at 0, with the recorded calibration; `MantisSimulationLedBar` takes every command and waits no hold. `MantisSimulationArm` stands at idle with its absolute encoders reading there, so it -needs no encoder correction; its offsets are the declared ones, else 0. Each command moves it at -once, and the motor reads answer from it; without it the motors read idle at 0, as after power-on. -A motor read the configuration does not hold is not simulated. The sequencer, on the driver, holds -`SEQUENCER_QUEUE_SIZE` items, stopped and empty after setup, and once started plays each item at -once, a queued move moving the simulated arm; stop and abort keep what is queued. +needs no encoder correction; its offsets are the declared ones, else 0, and its zones, approach paths and +stage the declared ones, else none. Each command moves it at once, and the motor reads answer from +it; without it the motors read idle at 0, as after power-on. A motor read the configuration does +not hold is not simulated. The sequencer, on the driver, holds `SEQUENCER_QUEUE_SIZE` items, +stopped and empty after setup, and once started plays each item at once, a queued move moving the +simulated arm; stop and abort keep what is queued. **M16. Locking is by subsystem.** (STAR P11.) One lock serializes the link today. When features add commands that the device runs together, each subsystem gets its own lock, taken in declaration @@ -243,7 +310,11 @@ master's links. Interface B carries the command frames; interface A, the RFID re **M18. The resource model mirrors the device; it never decides for it.** (STAR P12.) `MantisDevice` builds the parts and holds the driver; a feature reaches the parts through the device, and moves keep them in step. A driver with no resource tree still drives. The Z column carries the arm's -four links (`LinkBody`), turned to the joint angles setup reads after the bring-up. +four links (`LinkBody`). The arm holds no resource: setup registers the device's +`_update_arm_model` as the arm's `_pose_observer`, and after each move, park, approach path, recovery +and the stop's park the arm reads its three encoders (command 84) and passes the links' angles and +Z's height to it; the column rises and falls with Z. A failed move passes where it stopped from the +reads it logs. With no observer nothing more is read; a failed update only warns (STAR P8). **M19. The LED bar shows what the device does, never at the cost of it.** An action runs inside `_show_action`: the bar shows its colour, the enclosing action's when it ends, the idle animation @@ -268,8 +339,8 @@ where a colour took a release from 0.21 s to 0.34 s on the recorded v4. (10 ms) and the reply timeout (5 s). A frame whose reserved word is not zero is logged, not refused. 4. **The arm model is partial.** The links are drawn as boxes: the manufacturer's link models are - compressed in a form not converted yet. The Z column is drawn with Z at 0: how far it travels - with Z is not measured. Which side of the device motor 0's links sit on is taken from the name + compressed in a form not converted yet. The Z column is drawn where it stands with Z at 0 and + moved by Z's height; that the drawn pose is Z at 0 is not measured. Which side of the device motor 0's links sit on is taken from the name of a link length in the unit's files, not seen on a device. The motor reads stay on the driver in the device's units; the arm converts them. 5. **Whether motor limits differ between devices of one generation** is unknown; one v4 has been @@ -332,18 +403,81 @@ where a colour took a release from 0.21 s to 0.34 s on the recorded v4. within `Z_TRAVERSE_TOLERANCE` (0.1 mm) of its flag by its encoder, twice the largest gap read between Z's encoder and its demand position. Z's encoder has been read at rest only, never right after a move. -28. **The arm is moved to idle only from near idle.** Without the unit's zones, setup and stop move - to idle only with the head within `FRONT_ZONE_X_RANGE` (the Front zone's x bounds, the same on - every unit seen); elsewhere they refuse. That admits the strip below the Front zone (y under - 31 mm), where a recorded startup began (y about 28 mm): that startup moved X, Y and Z at once - from there; this driver raises Z first, as from the Front zone. The zones' routes lift this. -29. **The absolute encoder's wrap and the forward kinematics' choice are not observed.** A motor's - position is taken one span lower above 100 (motor 0) or 120 (motor 1) device units, and of the - two solutions the head's is the one of greater y; no reading near a wrap has been seen. +28. **Without the unit's zones, the arm is moved to idle only from near idle.** Setup, `park`, + `recover_from_fault` and stop then move to idle only with the head within `FRONT_ZONE_X_RANGE` + (the Front zone's x bounds), along the Front zone's routes, the same on every unit seen; + elsewhere they refuse. With the zones, the head's zone names its routes. +29. **The absolute encoder's wrap is not observed.** A motor's position is taken one span lower + above 100 (motor 0) or 120 (motor 1) device units; no reading near a wrap has been seen. Of the + forward kinematics' two solutions the head's is the one left of the line between the elbows: + the recorded recoveries from four start poses each followed the zone it gives, and from one + (a left zone) the solution of greater y would have chosen a right zone. No recovery from a + back zone, where the two differ most, has been recorded. 30. **A slip before Z homes refuses.** If an arm reads a following error just before Z homes, setup raises instead of homing; no recording shows what should follow. 31. **Unpowering the arm (command 19, open loop) is unrecorded.** A failed bring-up sends it; its reply has not been seen. -32. **The encoder correction files are found two levels above `oem_config_directory`.** That - argument still names `Data/Device/Configs`; the corrections sit in `Data/System`. - +32. **The encoder correction files and the sequences are found two levels above + `oem_config_directory`, the plate holder's origin one level above.** That argument still names + `Data/Device/Configs`; they sit in `Data/System` and `Data/Device/Adapters`. +33. **Zones that share an edge are tried in `ZONES` order.** A head on an edge goes to the first + zone listed that holds it, in the order the unit lists its zones; the tie-break is not + known. +34. **None of the moves, routes or approach paths has run through this driver.** Their wire form matches + recorded exchanges: a recovery's routes as command 22 per motor, the waste's path as queued + moves line for line, each matched against a recording; a `park` from any zone also uses + command 22, as the recovery does, since no park at a pause or a stop has been recorded. +35. **`park` rises to the traverse height first, and an approach path is entered from idle, or + from the line it shares with the path the head is on.** Neither is recorded as a rule: the + recorded routes move X and Y at the height the head is at, and the recorded approach paths + start with the arms' move to the ready position. Both are this driver's guards, as STAR's park + lifts before it travels. +36. **Z's speed and acceleration ranges are those of the recorded moves, not the device's.** The + jog's 1 to 20 mm/s at 500 mm/s² and the routes' 55 mm/s at 200 mm/s² were accepted; the + firmware refuses a speed or acceleration it will not take in the move's reply (5 or 6). Queued + moves have been recorded at 55 mm/s and 200 mm/s² only. +37. **`recover_from_fault` stops each motor at the route acceleration.** In a recorded recovery + each was stopped at the acceleration of its last move, the route's there; the order of + stops, clears and declarations is the recorded one. +38. **The waste's approach path is the generation's own file.** Every unit seen has both + `MoveToWasteStation` and `MoveToWasteStation_Adapter`, two different routes: a unit with zones + (a v4) reads the first, which a v4 played on the wire; one without them (a v3) reads the + second, not seen played, and a v3 builds no arm. The wash paths are + `MoveToWashStation1` and `2`; their `Lc3` variants are not read. +39. **The resource model draws the arm where the manufacturer's model places it,** not where the + fit puts it: on the unit the fit was checked on, about 2 mm off. +40. **`request_is_parked`'s tolerance, 0.5 degrees and 0.5 mm, is ours.** In the recordings the + declared arms read up to about 0.1 degrees from their idle target after a move there. +41. **A command 22 answering a motor error code is untested.** Every reply seen is 0; no test + plays another. The check reads the code only; commands 19, 21 and 24 answer one too, unchecked. +42. **The read-back after a rise is Z's encoder (command 84), not the demand position.** It + catches a rise the device refused, dropped or stalled, within `Z_TRAVERSE_TOLERANCE`; Z's + encoder has been read at rest only, never right after a move. +43. **The stage transform has not commanded a move through this driver.** The same chain + reproduces, to 1 µm, the recorded head positions over the 96 wells of recorded dispenses on a + v4; that check needs the unit's own matrix and origin, so it is kept outside this repository + and the tests here are synthetic. Whether the nozzle stands over each well is for a device run + to show. +44. **The holder's back-left corner is the wellplate frame's origin,** as the clamp pushes a + wellplate there, with A1 at the back left: y runs from the holder's back edge. +45. **The stage maps the nozzle of a fitted chip.** It is the unit's calibration, made with a chip; + whether every chip's nozzle stands alike, and where the head stands without one, is not + measured. A height starts the unit's `ReferenceHeight` above the plate holder, at the stage + origin's z; the gap it gives has not been gauged with a chip seated, so absolute heights are + unverified. The tilt is validated over the holder only. +46. **Off the plate holder, x and y are the fit's.** On the unit it was checked on, the fit leaves + under 0.2 mm over the holder and predicts its chip pick-ups to about 0.3 mm; another unit's + stage left 0.5 mm. At the holder's edge the two can differ by that much. +47. **The stage's x and y offsets keep their x and y, unused.** Only their z tilts the stage; + the recorded x and y were reproduced without them. +48. **The deck-frame moves are joint moves:** nothing models the chip racks, so they run only + with the path clear. With an xy speed the nozzle's path bows away from the straight line, by + tens of mm on long moves near the edge of reach. +49. **No arm speed or acceleration limit is known.** The firmware refuses an illegal one in the + move's reply; it took a far faster arm command than the route profile's, which then failed on + its following error. Speeds in mm/s are scaled from the route profile only when given, untested + on a device. +50. **The way out along an approach path is the path backwards, line for line,** as recorded + leaving a wash, and the move between the washes goes safe Z to safe Z, as recorded. A head + stopped between two lines of a path is not placed on it: it is in the open, and rises straight + up. diff --git a/pylabrobot/formulatrix/mantis/device.py b/pylabrobot/formulatrix/mantis/device.py index 42d0777c5d0..4601fe57e30 100644 --- a/pylabrobot/formulatrix/mantis/device.py +++ b/pylabrobot/formulatrix/mantis/device.py @@ -23,17 +23,21 @@ from .driver.features.rfid import Rfid from .driver.master import MantisDriver from .driver.simulator import MantisSimulationDriver -from .resource_model import MANIFEST, arm_link, chip_rack, z_column - -# The main box's size, in mm, from the manufacturer's model: up to the top plate's surface. -SIZE_X, SIZE_Y, SIZE_Z = MANIFEST["size"] -# The plate holder on the deck: an SBS footprint pushed into the back-left of the pin nest, in mm. -PLATE_HOLDER_LOCATION = Coordinate(23.62, 15.02, 0.0) -PLATE_HOLDER_SIZE_X, PLATE_HOLDER_SIZE_Y = 127.76, 85.48 -# Where the right motor turns, x and y in the device frame, and the arm's height, in mm; the arm -# frame is the device frame moved there and mirrored in y. -ARM_BASE_X, ARM_BASE_Y = MANIFEST["joints"]["base_right"] -ARM_Z = MANIFEST["joints"]["arm_z"] +from .resource_model import ( + ARM_BASE_X, + ARM_BASE_Y, + ARM_Z, + MANIFEST, + PLATE_HOLDER_LOCATION, + PLATE_HOLDER_SIZE_X, + PLATE_HOLDER_SIZE_Y, + SIZE_X, + SIZE_Y, + SIZE_Z, + arm_link, + chip_rack, + z_column, +) class MantisDevice(Resource): @@ -43,13 +47,17 @@ class MantisDevice(Resource): arm. The generation is read at setup (``driver.configuration.generation``). """ - def _update_arm_model(self, *, theta_1: float, theta_2: float) -> None: - """Turn the arm's links to two link angles, the geometry's own, in degrees. + def _update_arm_model(self, *, theta_1: float, theta_2: float, z: float) -> None: + """Turn the arm's links to two link angles, the geometry's own, in degrees, and raise or lower + the Z column with the arm on it to a height. Args: theta_1: motor 0's link angle; with its offset, the angle the motor reads. theta_2: motor 1's link angle; with its offset, the angle the motor reads. + z: Z's height, in mm above its home flag, up positive. """ + column_x, column_y, column_z = MANIFEST["modules"]["z_column"]["measured_at"] + self.z_column.location = Coordinate(x=column_x, y=column_y, z=column_z + z) right_elbow, left_elbow = compute_elbow_positions(theta_1=theta_1, theta_2=theta_2) head = compute_head_position(theta_1=theta_1, theta_2=theta_2) for side, base, elbow in ( @@ -164,7 +172,7 @@ def __init__( self.assign_child_resource(self.right_chip_rack, location=measured_at["right_chip_rack"]) # The Z column and the arm it carries, drawn at idle until setup reads where the arm is. The - # column is drawn where it stands with Z at 0: how far it travels with Z is not measured. + # column is drawn where it stands with Z at 0 and moves with Z, the arm on it. self.z_column = z_column(name=f"{name}_z_column") self.assign_child_resource(self.z_column, location=measured_at["z_column"]) self.arm_links: Dict[str, LinkBody] = {} @@ -184,7 +192,7 @@ def __init__( location=short.distal_joint - long.proximal_joint + Coordinate(0.0, 0.0, above), ) self.arm_links[f"{side}_short_arm"], self.arm_links[f"{side}_long_arm"] = short, long - self._update_arm_model(theta_1=0.0, theta_2=0.0) + self._update_arm_model(theta_1=0.0, theta_2=0.0, z=0.0) @property def plate_clamp(self) -> Optional[PlateClamp]: @@ -219,9 +227,11 @@ async def setup(self, *, skip_arm: bool = False) -> None: """ await self.driver.setup(skip_arm=skip_arm) arm = self.driver.arm - if arm is not None and arm.is_initialized: - theta_1, theta_2 = await arm.request_link_angles() - self._update_arm_model(theta_1=theta_1, theta_2=theta_2) + if arm is not None: + # Every move from here on passes where it ended to the model; the bring-up's is read here. + arm._pose_observer = self._update_arm_model + if arm.is_initialized: + await arm._report_pose() async def stop(self) -> None: """Stop the motors, park an arm brought up, release the clamp and disconnect.""" diff --git a/pylabrobot/formulatrix/mantis/device_tests.py b/pylabrobot/formulatrix/mantis/device_tests.py index 3061ddd7f48..cc1370d76fd 100644 --- a/pylabrobot/formulatrix/mantis/device_tests.py +++ b/pylabrobot/formulatrix/mantis/device_tests.py @@ -1,10 +1,12 @@ -"""Mantis device tests: the parts tree.""" +"""Mantis device tests: the parts tree, and the arm model following the moves.""" import os import unittest from unittest import mock from pylabrobot.formulatrix.mantis import Mantis, MantisDevice +from pylabrobot.formulatrix.mantis.driver.arm_kinematics import compute_joint_angles +from pylabrobot.formulatrix.mantis.driver.features.led_bar import LedBar from pylabrobot.io.ftdi import FTDI RESOURCE_MODEL = os.path.join(os.path.dirname(__file__), "resource_model") @@ -25,5 +27,26 @@ def test_simulation_refuses_a_link(self): Mantis(simulation=True, io=mock.create_autospec(FTDI, instance=True)) +class ArmModelTests(unittest.IsolatedAsyncioTestCase): + async def test_a_move_poses_the_links_and_moves_the_z_column(self): + device = Mantis(simulation=True) + with mock.patch.object(LedBar, "_sleep", autospec=True): + await device.setup() + self.addAsyncCleanup(device.stop) + assert device.arm is not None + column = device.z_column.location + assert column is not None + # The simulated arm has no offsets: its joint state is the links' own angles. + theta_1, theta_2 = compute_joint_angles(x=15.0, y=60.0) + await device.arm.move_to_joint_state(joint_state={0: theta_1, 1: theta_2, 2: 10.0}) + self.assertAlmostEqual(device.z_column.get_absolute_location().z, column.z - 10.0) + idle, posed = Mantis(simulation=True), Mantis(simulation=True) + posed._update_arm_model(theta_1=theta_1, theta_2=theta_2, z=-10.0) + for name, link in device.arm_links.items(): + with self.subTest(link=name): + self.assertAlmostEqual(link.rotation.z, posed.arm_links[name].rotation.z) + self.assertNotAlmostEqual(link.rotation.z, idle.arm_links[name].rotation.z) + + if __name__ == "__main__": unittest.main() diff --git a/pylabrobot/formulatrix/mantis/driver/arm_kinematics.py b/pylabrobot/formulatrix/mantis/driver/arm_kinematics.py index 244b282a6cc..4dc23d8cf1b 100644 --- a/pylabrobot/formulatrix/mantis/driver/arm_kinematics.py +++ b/pylabrobot/formulatrix/mantis/driver/arm_kinematics.py @@ -143,7 +143,10 @@ def compute_elbow_positions( def compute_head_position(*, theta_1: float, theta_2: float) -> Tuple[float, float]: - """Where the head is for two link angles: forward kinematics, the solution of greater y. + """Where the head is for two link angles: forward kinematics. + + Of the two solutions, the head's is the one left of the line from motor 0's elbow to motor 1's: + the recorded recoveries route from it in every zone recorded, the back zones included. Args: theta_1: motor 0's link angle, in degrees, the geometry's own. @@ -168,14 +171,10 @@ def compute_head_position(*, theta_1: float, theta_2: float) -> Tuple[float, flo across = math.sqrt(max(0.0, RIGHT_LONG_ARM_LENGTH**2 - along**2)) middle_x = first_x + along * (second_x - first_x) / distance middle_y = first_y + along * (second_y - first_y) / distance - solutions = [ - ( - middle_x + sign * across * (second_y - first_y) / distance, - middle_y - sign * across * (second_x - first_x) / distance, - ) - for sign in (1.0, -1.0) - ] - return max(solutions, key=lambda solution: solution[1]) + return ( + middle_x - across * (second_y - first_y) / distance, + middle_y + across * (second_x - first_x) / distance, + ) def compute_corrected_encoder_reading( diff --git a/pylabrobot/formulatrix/mantis/driver/arm_kinematics_tests.py b/pylabrobot/formulatrix/mantis/driver/arm_kinematics_tests.py index c3e8205e9c3..5b42baa5b03 100644 --- a/pylabrobot/formulatrix/mantis/driver/arm_kinematics_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/arm_kinematics_tests.py @@ -63,7 +63,10 @@ def test_both_links_at_0_put_the_head_at_idle(self): self.assertAlmostEqual(y, 31.177, places=3) def test_inverse_and_forward_kinematics_agree(self): - for x, y in ((15.0, 31.177), (-57.0, 0.0), (60.0, 80.0), (0.0, 100.0)): + # In front of the arm and in the back zones, where the other solution lies elsewhere. + points = [(15.0, 31.177), (-57.0, 0.0), (60.0, 80.0), (0.0, 100.0)] + points += [(-85.0, -45.0), (75.0, -50.0), (-65.0, -4.0), (90.0, -50.0)] + for x, y in points: with self.subTest(x=x, y=y): theta_1, theta_2 = compute_joint_angles(x=x, y=y) head = compute_head_position(theta_1=theta_1, theta_2=theta_2) diff --git a/pylabrobot/formulatrix/mantis/driver/configuration.py b/pylabrobot/formulatrix/mantis/driver/configuration.py index d47f5f4bb86..da990464973 100644 --- a/pylabrobot/formulatrix/mantis/driver/configuration.py +++ b/pylabrobot/formulatrix/mantis/driver/configuration.py @@ -18,7 +18,6 @@ class DeviceConfiguration: generation: "v4" if board 11 answered the version read, else "v3"; None until read. firmware_versions: each board's firmware version, by board; None for a board that did not answer. - motor_limits: each motor's (lower, upper) limit, by motor, in the device's units. feedback_sensor_parameters: each pressure controller's feedback sensor (scale, offset), by controller name (``PRESSURE_CONTROLLERS``), not by its index as the motors are; as the pressure board holds them, empty without one. Read at every setup, never saved. @@ -26,7 +25,6 @@ class DeviceConfiguration: generation: Optional[Literal["v3", "v4"]] = None firmware_versions: Dict[int, Optional[str]] = dataclasses.field(default_factory=dict) - motor_limits: Dict[int, Tuple[float, float]] = dataclasses.field(default_factory=dict) feedback_sensor_parameters: Dict[PressureController, Tuple[float, float]] = dataclasses.field( default_factory=dict ) @@ -36,13 +34,13 @@ class DeviceConfiguration: RECORDING_MANTIS_V3 = os.path.join(os.path.dirname(__file__), "recordings", "mantis_v3.json") RECORDING_MANTIS_V4 = os.path.join(os.path.dirname(__file__), "recordings", "mantis_v4.json") # The blocks a unit's own values fill: from its vendor folder, its declared file or a recording. -UNIT_BLOCKS = ("plate_clamp", "rfid", "led_bar", "arm") +UNIT_BLOCKS = ("plate_clamp", "rfid", "led_bar", "arm", "zones", "approach_paths", "stage") # The blocks that calibrate motion: never taken from a recording; without them motion refuses. -MOTION_CALIBRATION_BLOCKS = ("arm",) +MOTION_CALIBRATION_BLOCKS = ("arm", "zones", "approach_paths", "stage") # What is fitted: compared between a declared configuration and the device at setup. Firmware is # not, so a configuration taken off one device still describes another of its build. -FITTED_FIELDS = ("generation", "motor_limits") +FITTED_FIELDS = ("generation",) def compute_generation(*, firmware_versions: Dict[int, Optional[str]]) -> Literal["v3", "v4"]: @@ -72,10 +70,6 @@ def configuration_from_json(fields: Dict[str, Any]) -> DeviceConfiguration: firmware_versions={ int(board): version for board, version in fields.get("firmware_versions", {}).items() }, - motor_limits={ - int(motor): (float(lower), float(upper)) - for motor, (lower, upper) in fields.get("motor_limits", {}).items() - }, ) diff --git a/pylabrobot/formulatrix/mantis/driver/features/arm.py b/pylabrobot/formulatrix/mantis/driver/features/arm.py index 418760f4754..26127c86861 100644 --- a/pylabrobot/formulatrix/mantis/driver/features/arm.py +++ b/pylabrobot/formulatrix/mantis/driver/features/arm.py @@ -3,25 +3,43 @@ On a v4, board 11 reads each arm motor's absolute encoder, so where the arm stands is known after power-on: the arms are declared there, never homed. Z homes on its flag. Every bring-up step and its order matches the recorded v4 startup. + +The head goes to idle along the unit's routes: the zone it stands in names a route to a safe XY +and one to idle. Without them, only from near idle. The nozzle moves in the deck frame, through +the unit's stage transform over the plate holder and the arm frame fitted to it elsewhere. """ import asyncio +import dataclasses import logging +import math import struct -from typing import TYPE_CHECKING, Dict, List, Optional, Tuple +from typing import TYPE_CHECKING, Callable, Dict, List, Optional, Sequence, Tuple from pylabrobot.resources import Coordinate +from ...resource_model import ( + ARM_BASE_X, + ARM_BASE_Y, + ARM_Z, + PLATE_HOLDER_LOCATION, + PLATE_HOLDER_SIZE_X, + PLATE_HOLDER_SIZE_Y, + SIZE_Z, +) from ..arm_kinematics import ( compute_absolute_device_position, compute_device_position, compute_device_rate, + compute_elbow_positions, compute_head_position, compute_joint_angles, compute_position, ) from ..errors import ( + MOTOR_ERROR_CODE_DESCRIPTIONS, MOTOR_STATUS_ERROR_BITS, + FrameError, MantisConfigurationError, MantisKinematicsError, MantisMotionError, @@ -29,6 +47,15 @@ ) from ..events import HOME_DONE_EVENT, decode_home_done from ..framing import MOTION_BOARD +from ..oem_config import ( + UNKNOWN_ZONE, + WASH_APPROACH_PATH, + WASH_POSITIONS, + WASTE_APPROACH_PATH, + ZONES, +) +from ..sequence_files import RouteLine +from ..stage_transform import compute_arm_frame_position if TYPE_CHECKING: from ..master import MantisDriver @@ -47,6 +74,14 @@ # The idle position: where every move to idle sends the head, (x, y) in the arm frame, mm; the # same on a v3 and a v4. Each unit's offsets turn it into targets. POST_HOMING_POSITION = (15.0, 31.177) +# The Front zone's routes to safe XY and to idle, the same on every unit seen: the head is moved +# along them from near idle when the unit's zones are not given. +FRONT_SAFE_XY_ROUTE: List[RouteLine] = [(*POST_HOMING_POSITION, None)] +FRONT_IDLE_ROUTE: List[RouteLine] = [ + (None, None, 0.0), + (*POST_HOMING_POSITION, None), + (*POST_HOMING_POSITION, 0.0), +] # The arms' largest following error, in device units, set at bring-up. ARM_MAXIMUM_FOLLOWING_ERROR = 4.0 # How often each declaration of the arms' absolute position reads and writes them, as recorded. @@ -60,6 +95,16 @@ # rising edge in the negative direction. Z_HOMING_SPEED = 15.0 Z_HOMING_ACCELERATION = 400.0 +# PyLabRobot's allow-list for Z, mm/s and mm/s²: the recorded moves' range (the jog's 1 to 20 mm/s +# at 500 mm/s², the routes' 55 mm/s at 200 mm/s²); other values have not run on a device. +Z_SPEED_RANGE = (1.0, 55.0) +Z_ACCELERATION_RANGE = (200.0, 500.0) +# The route profile, in every recorded recovery, close, route and queued move: the arms in +# degrees/s and degrees/s², Z in mm/s and mm/s². Routes keep it; the arms take it by default. +ROUTE_ARM_SPEED = 10000.0 +ROUTE_ARM_ACCELERATION = 1500.0 +ROUTE_Z_SPEED = 55.0 +ROUTE_Z_ACCELERATION = 200.0 # How far below a height Z's encoder may read after rising to it, mm: twice the largest gap read # between Z's encoder and its demand position (2 device units, 0.05 mm), rounded up. Z_TRAVERSE_TOLERANCE = 0.1 @@ -67,6 +112,81 @@ MOTOR_STATUS_POLL_INTERVAL = 0.02 MOVE_TIMEOUT = 30.0 HOME_TIMEOUT = 30.0 +# The safe height, mm above Z's flag: the routes' Z -1.570. +SAFE_HEIGHT = 1.57 +# PyLabRobot design choice: the plate holder is sampled every 5 mm to fit the deck frame to the arm +# frame. +DECK_FIT_SPACING = 5.0 +# How far outside the plate holder, mm, a point read back through the inverted stage transform may +# land and still count as on it: rounding only. +HOLDER_EDGE_TOLERANCE = 1e-6 +# PyLabRobot design choice: how close, mm, the head must stand to a route line below the safe height on +# an approach path, in x, y and Z, to count as on that path. +APPROACH_PATH_TOLERANCE = 0.5 + +JointState = Dict[int, float] +"""Where each motor is, by motor: 0 and 1 in degrees, 2 (Z) in mm down from its flag.""" + + +@dataclasses.dataclass(frozen=True) +class MantisArmConfiguration: + """The arm's facts, read at setup, and the unit's calibration, from its folder or declared file. + + Each calibration value is None without its block (``arm``, ``zones``, ``approach_paths``, + ``stage``). + """ + + motor_limits: Dict[int, Tuple[float, float]] = dataclasses.field(default_factory=dict) + """Each motor's (lower, upper) position limit, by motor, in device units, as read at setup.""" + offsets: Optional[Dict[int, float]] = None + """Each arm motor's offset, degrees, added to its angle for the geometry's own.""" + homing_position_a: Optional[Dict[int, float]] = None + """Each arm motor's encoder reference, in the absolute encoder's units.""" + encoder_correction: Optional[Dict[int, List[Tuple[float, float]]]] = None + """Each arm motor's (ideal, measured) correction rows, measured ascending.""" + zone_rectangles: Optional[Dict[str, Tuple[float, float, float, float]]] = None + """Each zone's (minimum x, maximum x, minimum y, maximum y) in the arm frame, mm, by name.""" + safe_xy_routes: Optional[Dict[str, List[RouteLine]]] = None + """Each zone's route to its safe XY, the arms only, by name; Unknown included.""" + idle_routes: Optional[Dict[str, List[RouteLine]]] = None + """Each zone's route to idle, by name; Unknown included.""" + approach_paths: Optional[Dict[str, List[RouteLine]]] = None + """The unit's path from idle to each place it serves, by name: ``"waste"``, ``"wash_1"``, + ``"wash_2"``; arm frame, Z mm down from its flag, an axis left out held.""" + stage_origin: Optional[Tuple[float, float, float]] = None + """The plate holder's origin, (x, y, z), mm.""" + stage_transformation_matrix: Optional[Sequence[Sequence[float]]] = None + """The stage's projective matrix, 3 by 3.""" + stage_offset_x: Optional[Tuple[float, float, float]] = None + """The stage's x offset, (x, y, z); its z tilts the stage.""" + stage_offset_y: Optional[Tuple[float, float, float]] = None + """The stage's y offset, (x, y, z); its z tilts the stage.""" + arm_type_rotation_degree: Optional[float] = None + """The arm type's rotation, degrees.""" + arm_type_translation: Optional[Tuple[float, float, float]] = None + """The arm type's translation, (x, y, z), mm.""" + stage_reference_height: Optional[float] = None + """The wellplate frame's z origin above the plate holder, mm (``StageTuning.config``).""" + + +@dataclasses.dataclass(frozen=True) +class MantisPose: + """Where every joint of the arm is and where the nozzle is, in the deck frame, mm. + + One answer for the whole arm rather than for its end: whether the arm clears something depends + on its elbows as much as on the nozzle. + """ + + right_elbow_joint_location: Coordinate + """Where motor 0's short link ends: the joint the right long link turns about.""" + left_elbow_joint_location: Coordinate + """Where motor 1's short link ends: the joint the left long link turns about.""" + head_joint_location: Coordinate + """Where the two long links meet, at the links' height.""" + nozzle_location: Coordinate + """The nozzle's tip, the tool center point: under the head joint.""" + joints: JointState + """What each motor reported, as ``request_joint_state`` returns it.""" class Arm: @@ -74,35 +194,37 @@ class Arm: Bringing it up needs the unit's ``arm`` block: each arm motor's offset, encoder reference and encoder correction. Without it nothing is sent and ``initialize`` refuses; it never falls back to - another unit's values. + another unit's values. The ``zones`` and ``approach_paths`` blocks give the routes; without them + the head is moved to idle only from near it, and the waste and the washes refuse. Without the ``stage`` block the + deck-frame moves and ``request_pose`` refuse. Routes (bring-up, park, the approach paths, stop) move at the + ``ROUTE_*`` profile; the other moves take the arms at it unless an xy speed is given. + + The public moves take the nozzle in the deck frame, mm, and its speed in mm/s; a joint state + holds each motor as it reads, Z in mm down from its flag. + + After each move, and where a failed one stopped, the measured pose goes to ``_pose_observer`` if + one is set: the two links' angles and Z's height, as keywords ``theta_1``, ``theta_2`` and ``z``. """ - # Where the ``default_*`` properties start, in degrees/s, degrees/s², mm/s and mm/s². - _default_arm_speed: float = 10000.0 - _default_arm_acceleration: float = 1500.0 - _default_z_speed: float = 55.0 - _default_z_acceleration: float = 200.0 + # Where the ``default_*`` properties start: the arms at the route profile (None), Z at the + # route profile, and the safe height (None). + _default_xy_speed: Optional[float] = None + _default_xy_acceleration: Optional[float] = None + _default_z_speed: float = ROUTE_Z_SPEED + _default_z_acceleration: float = ROUTE_Z_ACCELERATION + _default_minimum_traverse_height: Optional[float] = None - def __init__( - self, - *, - driver: "MantisDriver", - offsets: Optional[Dict[int, float]], - homing_position_a: Optional[Dict[int, float]], - encoder_correction: Optional[Dict[int, List[Tuple[float, float]]]], - ): + def __init__(self, *, driver: "MantisDriver", configuration: MantisArmConfiguration): """ Args: driver: the driver it sends through. - offsets: each arm motor's offset, degrees, added to its angle for the geometry's own. - homing_position_a: each arm motor's encoder reference, in the absolute encoder's units. - encoder_correction: each arm motor's (ideal, measured) correction rows, measured ascending. + configuration: the arm's facts and the unit's calibration, as discovery reads them. """ self._driver = driver - self.offsets = offsets - self.homing_position_a = homing_position_a - self.encoder_correction = encoder_correction + self.configuration = configuration self._initialized = False + self._pose_observer: Optional[Callable[..., None]] = None + self._deck_to_arm_transform: Optional[Tuple[float, float, float]] = None @property def is_initialized(self) -> bool: @@ -111,70 +233,171 @@ def is_initialized(self) -> bool: @staticmethod def _check_positive(*, name: str, value: float) -> None: - """Raise ``ValueError`` unless ``value`` is above 0: the device reports no range of its own.""" - if not value > 0: - raise ValueError(f"{name} must be above 0, got {value}") + """Raise ``ValueError`` unless ``value`` is finite and above 0: the device reports no range of + its own.""" + if not (math.isfinite(value) and value > 0): + raise ValueError(f"{name} must be finite and above 0, got {value}") + + @staticmethod + def _check_within(*, name: str, value: float, allowed: Tuple[float, float]) -> None: + """Raise ``ValueError`` unless ``value`` lies in ``allowed``, ends included.""" + low, high = allowed + if not low <= value <= high: + raise ValueError(f"{name} must be from {low} to {high}, got {value}") @property - def default_arm_speed(self) -> float: - """Each arm motor's speed for a move, in degrees/s.""" - return self._default_arm_speed + def default_xy_speed(self) -> Optional[float]: + """The nozzle's speed across the deck for a move, mm/s; None takes the route profile.""" + return self._default_xy_speed - @default_arm_speed.setter - def default_arm_speed(self, value: float) -> None: - self._check_positive(name="default_arm_speed", value=value) - self._default_arm_speed = value + @default_xy_speed.setter + def default_xy_speed(self, value: Optional[float]) -> None: + if value is not None: + self._check_positive(name="default_xy_speed", value=value) + self._default_xy_speed = value @property - def default_arm_acceleration(self) -> float: - """Each arm motor's acceleration for a move, and its deceleration at a stop, in degrees/s².""" - return self._default_arm_acceleration + def default_xy_acceleration(self) -> Optional[float]: + """The nozzle's acceleration across the deck for a move, mm/s²; None takes the route + profile.""" + return self._default_xy_acceleration - @default_arm_acceleration.setter - def default_arm_acceleration(self, value: float) -> None: - self._check_positive(name="default_arm_acceleration", value=value) - self._default_arm_acceleration = value + @default_xy_acceleration.setter + def default_xy_acceleration(self, value: Optional[float]) -> None: + if value is not None: + self._check_positive(name="default_xy_acceleration", value=value) + self._default_xy_acceleration = value @property def default_z_speed(self) -> float: - """Z's speed for a move, in mm/s.""" + """Z's speed for a move, in mm/s, within ``Z_SPEED_RANGE``.""" return self._default_z_speed @default_z_speed.setter def default_z_speed(self, value: float) -> None: - self._check_positive(name="default_z_speed", value=value) + self._check_within(name="default_z_speed", value=value, allowed=Z_SPEED_RANGE) self._default_z_speed = value @property def default_z_acceleration(self) -> float: - """Z's acceleration for a move, and its deceleration at a stop, in mm/s².""" + """Z's acceleration for a move, and its deceleration at a stop, in mm/s², within + ``Z_ACCELERATION_RANGE``.""" return self._default_z_acceleration @default_z_acceleration.setter def default_z_acceleration(self, value: float) -> None: - self._check_positive(name="default_z_acceleration", value=value) + self._check_within(name="default_z_acceleration", value=value, allowed=Z_ACCELERATION_RANGE) self._default_z_acceleration = value - def _get_device_speed(self, *, motor: int) -> float: - """A motor's default speed, in device units.""" - speed = self.default_z_speed if motor == Z_MOTOR else self.default_arm_speed - return compute_device_rate(motor=motor, rate=speed) + @staticmethod + def _get_z_device_position(*, z: float) -> float: + """A height, mm above Z's home flag, as Z's position in device units (down positive).""" + return compute_device_position(motor=Z_MOTOR, position=0.0 - z) # 0.0, never -0.0 - def _get_device_acceleration(self, *, motor: int) -> float: - """A motor's default acceleration, in device units; a stop decelerates at it.""" - acceleration = ( - self.default_z_acceleration if motor == Z_MOTOR else self.default_arm_acceleration + def _check_reachable(self, *, motor: int, position: float) -> None: + """Raise ``ValueError`` unless a motor's target, in device units, is within its limits read. + + Raises: + MantisConfigurationError: if the motor's limits were not read. + """ + limits = self.configuration.motor_limits.get(motor) + if limits is None: + raise MantisConfigurationError(f"{self._driver.name}: motor {motor}'s limits are not read") + lower, upper = limits + if not lower <= position <= upper: + raise ValueError( + f"motor {motor}'s target {position:.3f} is outside its limits {lower} to {upper} " + "(device units)" + ) + + @property + def default_minimum_traverse_height(self) -> Optional[float]: + """The nozzle's height above the deck it rises to before it moves across, mm, where a move's + ``minimum_traverse_height_start`` is None; None takes the safe height, ``SAFE_HEIGHT`` + above Z's flag.""" + return self._default_minimum_traverse_height + + @default_minimum_traverse_height.setter + def default_minimum_traverse_height(self, value: Optional[float]) -> None: + if value is not None and not math.isfinite(value): + raise ValueError(f"default_minimum_traverse_height must be finite, got {value}") + self._default_minimum_traverse_height = value + + def _get_profile( + self, *, motor: int, speed: Optional[float] = None, acceleration: Optional[float] = None + ) -> Tuple[float, float]: + """A motor's (speed, acceleration) in device units: an arm motor's in degrees/s and degrees/s², + each None the route profile's; Z's in mm/s and mm/s², each None its default. + + Raises: + ValueError: if an arm's is not above 0, or Z's is outside ``Z_SPEED_RANGE`` or + ``Z_ACCELERATION_RANGE``. + """ + if motor == Z_MOTOR: + speed = self.default_z_speed if speed is None else speed + acceleration = self.default_z_acceleration if acceleration is None else acceleration + self._check_within(name="Z speed", value=speed, allowed=Z_SPEED_RANGE) + self._check_within(name="Z acceleration", value=acceleration, allowed=Z_ACCELERATION_RANGE) + else: + speed = ROUTE_ARM_SPEED if speed is None else speed + acceleration = ROUTE_ARM_ACCELERATION if acceleration is None else acceleration + self._check_positive(name="arm speed", value=speed) + self._check_positive(name="arm acceleration", value=acceleration) + return ( + compute_device_rate(motor=motor, rate=speed), + compute_device_rate(motor=motor, rate=acceleration), ) + + def _get_profiles( + self, + *, + arm_speed: Optional[float] = None, + arm_acceleration: Optional[float] = None, + z_speed: Optional[float] = None, + z_acceleration: Optional[float] = None, + ) -> Dict[int, Tuple[float, float]]: + """Each motor's (speed, acceleration) in device units, by motor, as ``_get_profile``.""" + return { + **{ + motor: self._get_profile(motor=motor, speed=arm_speed, acceleration=arm_acceleration) + for motor in ARM_MOTORS + }, + Z_MOTOR: self._get_profile(motor=Z_MOTOR, speed=z_speed, acceleration=z_acceleration), + } + + def _get_route_profiles(self) -> Dict[int, Tuple[float, float]]: + """Each motor's (speed, acceleration) in device units along a route: the ``ROUTE_*`` + profile, whatever the defaults.""" + return self._get_profiles( + arm_speed=ROUTE_ARM_SPEED, + arm_acceleration=ROUTE_ARM_ACCELERATION, + z_speed=ROUTE_Z_SPEED, + z_acceleration=ROUTE_Z_ACCELERATION, + ) + + def _get_device_acceleration(self, *, motor: int) -> float: + """A motor's acceleration where no move gives one, in device units: the route profile's on the arms, + ``default_z_acceleration`` on Z; a stop outside a move decelerates at it.""" + acceleration = self.default_z_acceleration if motor == Z_MOTOR else ROUTE_ARM_ACCELERATION return compute_device_rate(motor=motor, rate=acceleration) def _check_calibrated(self) -> None: """Raise ``MantisConfigurationError`` unless the unit's ``arm`` block was given.""" - if self.offsets is None or self.homing_position_a is None or self.encoder_correction is None: + if ( + self.configuration.offsets is None + or self.configuration.homing_position_a is None + or self.configuration.encoder_correction is None + ): raise MantisConfigurationError( f"{self._driver.name}: no arm calibration for this unit; give its configuration folder " "(oem_config_directory) or a configuration saved with it, or set up with skip_arm=True" ) + def _check_initialized(self) -> None: + """Raise ``MantisMotionError`` unless the arm was brought up: where it is is not known.""" + if not self._initialized: + raise MantisMotionError(f"{self._driver.name}: the arm is not brought up; call setup()") + # -- raw firmware calls ---------------------------------------------------- async def _unchecked_fw_clear_motor_faults(self, *, motor: int) -> None: @@ -215,10 +438,22 @@ async def _unchecked_fw_home( async def _unchecked_fw_move_absolute( self, *, motor: int, position: float, speed: float, acceleration: float - ) -> None: - """Move one motor (command 22), sent once, in device units; it answers as the move starts.""" + ) -> int: + """Move one motor (command 22), sent once, in device units; it answers as the move starts. + + Returns the reply's motor error code (``MOTOR_ERROR_CODE_DESCRIPTIONS``), 0 for none. + + Raises: + FrameError: if the reply is shorter than 2 bytes. + """ payload = struct.pack(" None: """Stop a motor (command 24), sent once; deceleration in device units.""" @@ -258,19 +493,22 @@ async def _request_absolute_device_position(self, *, motor: int) -> float: MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. """ self._check_calibrated() - assert self.encoder_correction is not None and self.homing_position_a is not None + assert ( + self.configuration.encoder_correction is not None + and self.configuration.homing_position_a is not None + ) reading = await self._driver.request_absolute_encoder_position(motor=motor) return compute_absolute_device_position( motor=motor, reading=reading, - correction=self.encoder_correction[motor], - homing_position_a=self.homing_position_a[motor], + correction=self.configuration.encoder_correction[motor], + homing_position_a=self.configuration.homing_position_a[motor], ) - async def request_absolute_joint_position(self) -> Dict[int, float]: + async def request_absolute_joint_state(self) -> JointState: """Each arm motor's angle from its absolute encoder, in degrees, by motor; nothing moves. - Kept across power-off, unlike ``request_joint_position`` before the arms are declared. + Kept across power-off, unlike ``request_joint_state`` before the arms are declared. Raises: MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. @@ -282,7 +520,7 @@ async def request_absolute_joint_position(self) -> Dict[int, float]: for motor in ARM_MOTORS } - async def request_joint_position(self) -> Dict[int, float]: + async def request_joint_state(self) -> JointState: """Where the controller holds each motor: 0 and 1 in degrees, 2 (Z) in mm down from its flag.""" return { motor: compute_position( @@ -311,67 +549,330 @@ def is_xy_reachable(self, *, x: float, y: float) -> bool: return False return True - def _get_link_angles(self, *, joint_position: Dict[int, float]) -> Tuple[float, float]: + def _get_link_angles(self, *, joint_state: JointState) -> Tuple[float, float]: """The two links' angles, the geometry's own, degrees: each arm motor's angle plus its offset. Raises: MantisConfigurationError: without the unit's ``arm`` block. """ self._check_calibrated() - assert self.offsets is not None - return (joint_position[0] + self.offsets[0], joint_position[1] + self.offsets[1]) - - async def request_link_angles(self) -> Tuple[float, float]: - """The two links' angles where the controller holds the arm motors, the geometry's own, degrees. - - Raises: - MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. - """ - self._check_calibrated() - return self._get_link_angles(joint_position=await self.request_joint_position()) + assert self.configuration.offsets is not None + return ( + joint_state[0] + self.configuration.offsets[0], + joint_state[1] + self.configuration.offsets[1], + ) - def _get_head_position(self, *, joint_position: Dict[int, float]) -> Tuple[float, float]: + def _get_head_position(self, *, joint_state: JointState) -> Tuple[float, float]: """Where the head is in the arm frame, mm, for the arm motors' angles in degrees. Raises: MantisConfigurationError: without the unit's ``arm`` block. MantisKinematicsError: if the links cannot meet at those angles. """ - theta_1, theta_2 = self._get_link_angles(joint_position=joint_position) + theta_1, theta_2 = self._get_link_angles(joint_state=joint_state) return compute_head_position(theta_1=theta_1, theta_2=theta_2) - def _get_idle_targets(self) -> Dict[int, float]: - """Each arm motor's target at idle, in device units: ``POST_HOMING_POSITION`` less its offset. + def _get_arm_targets(self, *, x: float, y: float) -> Dict[int, float]: + """Each arm motor's target, in device units, for the head at (x, y): the links' angles less + each motor's offset. Raises: MantisConfigurationError: without the unit's ``arm`` block. + MantisKinematicsError: if (x, y) is out of the arm's reach. """ self._check_calibrated() - assert self.offsets is not None - x, y = POST_HOMING_POSITION + assert self.configuration.offsets is not None angles = compute_joint_angles(x=x, y=y) return { - motor: compute_device_position(motor=motor, position=angles[motor] - self.offsets[motor]) + motor: compute_device_position( + motor=motor, position=angles[motor] - self.configuration.offsets[motor] + ) for motor in ARM_MOTORS } - async def request_head_position(self) -> Coordinate: - """Where the head is: x and y in the arm frame, mm; z where Z is, mm down from its flag. + def _get_idle_targets(self) -> Dict[int, float]: + """Each arm motor's target at idle, in device units: ``POST_HOMING_POSITION`` less its offset. Raises: - MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. + MantisConfigurationError: without the unit's ``arm`` block. + """ + x, y = POST_HOMING_POSITION + return self._get_arm_targets(x=x, y=y) + + def _check_stage(self) -> None: + """Raise ``MantisConfigurationError`` unless the unit's ``stage`` block was given.""" + if any( + value is None + for value in ( + self.configuration.stage_origin, + self.configuration.stage_transformation_matrix, + self.configuration.stage_offset_x, + self.configuration.stage_offset_y, + self.configuration.arm_type_rotation_degree, + self.configuration.arm_type_translation, + self.configuration.stage_reference_height, + ) + ): + raise MantisConfigurationError( + f"{self._driver.name}: no stage transform for this unit; give its configuration folder " + "(oem_config_directory) or a configuration saved with it" + ) + + def _get_stage_position(self, *, x: float, y: float, z: float) -> Tuple[float, float, float]: + """A point of the wellplate frame through the unit's stage transform: x and y in the arm frame, + mm; z in mm down from Z's flag. + + Args: + x: right of the plate holder's back-left corner, mm. + y: in front of the plate holder's back-left corner, mm. + z: down from the stage's reference height above the plate holder, mm. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + MantisKinematicsError: if the stage transform maps the point to infinity. + """ + self._check_stage() + assert ( + self.configuration.stage_origin is not None + and self.configuration.stage_transformation_matrix is not None + ) + assert ( + self.configuration.stage_offset_x is not None + and self.configuration.stage_offset_y is not None + ) + assert ( + self.configuration.arm_type_rotation_degree is not None + and self.configuration.arm_type_translation is not None + ) + return compute_arm_frame_position( + position=(x, y, z), + origin=self.configuration.stage_origin, + transformation_matrix=self.configuration.stage_transformation_matrix, + offset_x=self.configuration.stage_offset_x, + offset_y=self.configuration.stage_offset_y, + arm_type_rotation_degree=self.configuration.arm_type_rotation_degree, + arm_type_translation=self.configuration.arm_type_translation, + ) + + def _get_deck_to_arm_transform(self) -> Tuple[float, float, float]: + """The turn, in radians, and the shift (x, y), in mm, that best put the resource model's arm + frame on the unit's, fitted over the plate holder through the stage transform. + + The resource model places the arm by its base from the manufacturer's model; units differ from + it by millimetres, so each unit's placement is fitted from its own calibration. + + Fitted once: the configuration does not change. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + """ + if self._deck_to_arm_transform is not None: + return self._deck_to_arm_transform + + def get_samples(size: float) -> List[float]: + # Every DECK_FIT_SPACING from 0, and the far edge. + count = math.ceil(size / DECK_FIT_SPACING) + return [min(index * DECK_FIT_SPACING, size) for index in range(count + 1)] + + pairs = [] + for x in get_samples(PLATE_HOLDER_SIZE_X): + for y in get_samples(PLATE_HOLDER_SIZE_Y): + model = ( + PLATE_HOLDER_LOCATION.x + x - ARM_BASE_X, + ARM_BASE_Y - (PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - y), + ) + unit_x, unit_y, _ = self._get_stage_position(x=x, y=y, z=0.0) + pairs.append((model, (unit_x, unit_y))) + count = len(pairs) + model_x = sum(model[0] for model, _ in pairs) / count + model_y = sum(model[1] for model, _ in pairs) / count + unit_x = sum(unit[0] for _, unit in pairs) / count + unit_y = sum(unit[1] for _, unit in pairs) / count + cross = dot = 0.0 + for (mx, my), (ux, uy) in pairs: + mx, my, ux, uy = mx - model_x, my - model_y, ux - unit_x, uy - unit_y + cross += mx * uy - my * ux + dot += mx * ux + my * uy + turn = math.atan2(cross, dot) + self._deck_to_arm_transform = ( + turn, + unit_x - (math.cos(turn) * model_x - math.sin(turn) * model_y), + unit_y - (math.sin(turn) * model_x + math.cos(turn) * model_y), + ) + return self._deck_to_arm_transform + + def _get_arm_frame_xy(self, *, x: float, y: float) -> Tuple[float, float]: + """A deck-frame (x, y), mm, in the arm frame: by the stage transform over the plate holder, by + the fitted transform elsewhere. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + """ + holder_x = x - PLATE_HOLDER_LOCATION.x + holder_y = PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - y + if 0 <= holder_x <= PLATE_HOLDER_SIZE_X and 0 <= holder_y <= PLATE_HOLDER_SIZE_Y: + arm_x, arm_y, _ = self._get_stage_position(x=holder_x, y=holder_y, z=0.0) + return arm_x, arm_y + turn, shift_x, shift_y = self._get_deck_to_arm_transform() + model_x, model_y = x - ARM_BASE_X, ARM_BASE_Y - y + return ( + math.cos(turn) * model_x - math.sin(turn) * model_y + shift_x, + math.sin(turn) * model_x + math.cos(turn) * model_y + shift_y, + ) + + def _get_deck_frame_xy(self, *, x: float, y: float) -> Tuple[float, float]: + """An arm-frame (x, y), mm, in the deck frame: the stage transform inverted where it lands on + the plate holder, so a move there reads back where it was sent; the fitted transform elsewhere. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + """ + self._check_stage() + assert self.configuration.stage_origin is not None + assert self.configuration.stage_transformation_matrix is not None + assert self.configuration.arm_type_rotation_degree is not None + assert self.configuration.arm_type_translation is not None + (a, b, c), (d, e, f), (g, h, i) = self.configuration.stage_transformation_matrix + determinant = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g) + if determinant != 0: + # The matrix's inverse, up to scale (its adjugate), maps the arm frame back onto the stage. + stage_x = (e * i - f * h) * x + (c * h - b * i) * y + (b * f - c * e) + stage_y = (f * g - d * i) * x + (a * i - c * g) * y + (c * d - a * f) + scale = (d * h - e * g) * x + (b * g - a * h) * y + (a * e - b * d) + if scale != 0: + framed_x = stage_x / scale - self.configuration.stage_origin[0] + framed_y = stage_y / scale - self.configuration.stage_origin[1] + framed_x -= self.configuration.arm_type_translation[0] + framed_y -= self.configuration.arm_type_translation[1] + angle = math.radians(self.configuration.arm_type_rotation_degree) + holder_x = math.cos(angle) * framed_x + math.sin(angle) * framed_y + holder_y = -math.sin(angle) * framed_x + math.cos(angle) * framed_y + # Within rounding of the holder, as the stage transform was sent there. + if ( + -HOLDER_EDGE_TOLERANCE <= holder_x <= PLATE_HOLDER_SIZE_X + HOLDER_EDGE_TOLERANCE + and -HOLDER_EDGE_TOLERANCE <= holder_y <= PLATE_HOLDER_SIZE_Y + HOLDER_EDGE_TOLERANCE + ): + return ( + PLATE_HOLDER_LOCATION.x + holder_x, + PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - holder_y, + ) + turn, shift_x, shift_y = self._get_deck_to_arm_transform() + x, y = x - shift_x, y - shift_y + model_x = math.cos(turn) * x + math.sin(turn) * y + model_y = -math.sin(turn) * x + math.cos(turn) * y + return ARM_BASE_X + model_x, ARM_BASE_Y - model_y + + def _get_z_joint_position(self, *, x: float, y: float, z: float) -> float: + """Z's position, mm down from its flag, for the nozzle at a deck-frame height over (x, y), the + stage's tilt there taken in. + + Args: + x: the nozzle's x in the deck frame, mm. + y: the nozzle's y in the deck frame, mm. + z: the nozzle's height above the deck, mm. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + """ + self._check_stage() + assert self.configuration.stage_reference_height is not None + _, _, z_joint_position = self._get_stage_position( + x=x - PLATE_HOLDER_LOCATION.x, + y=PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - y, + z=self.configuration.stage_reference_height - (z - PLATE_HOLDER_LOCATION.z), + ) + return z_joint_position + + async def request_pose(self) -> MantisPose: + """Where every joint of the arm is and where the nozzle is, in the deck frame, worked out from + the joint state; nothing moves. + + The nozzle is that of a fitted chip, where the stage transform puts it; the links' heights are + the resource model's, for drawing. The elbows are placed by the fitted transform, approximate. + + Raises: + MantisConfigurationError: without the unit's ``arm`` or ``stage`` block; nothing is sent. MantisKinematicsError: if the links cannot meet at the angles read. """ self._check_calibrated() - joint_position = await self.request_joint_position() - x, y = self._get_head_position(joint_position=joint_position) - return Coordinate(x=x, y=y, z=joint_position[Z_MOTOR]) + self._check_stage() + joints = await self.request_joint_state() + theta_1, theta_2 = self._get_link_angles(joint_state=joints) + # The links' height for drawing only, as the resource model places them: ARM_Z at Z's flag is + # assumed, not measured; no clearance is judged from it. + link_z = ARM_Z - SIZE_Z - joints[Z_MOTOR] + right_elbow, left_elbow = compute_elbow_positions(theta_1=theta_1, theta_2=theta_2) + head = compute_head_position(theta_1=theta_1, theta_2=theta_2) + head_x, head_y = self._get_deck_frame_xy(x=head[0], y=head[1]) + # The nozzle's height: Z's position with the nozzle on the deck, less Z's position read. + nozzle_z = self._get_z_joint_position(x=head_x, y=head_y, z=0.0) - joints[Z_MOTOR] + elbows = [ + self._get_deck_frame_xy(x=elbow[0], y=elbow[1]) for elbow in (right_elbow, left_elbow) + ] + return MantisPose( + right_elbow_joint_location=Coordinate(x=elbows[0][0], y=elbows[0][1], z=link_z), + left_elbow_joint_location=Coordinate(x=elbows[1][0], y=elbows[1][1], z=link_z), + head_joint_location=Coordinate(x=head_x, y=head_y, z=link_z), + nozzle_location=Coordinate(x=head_x, y=head_y, z=nozzle_z), + joints=joints, + ) + + async def request_is_parked(self, *, tolerance: float = 0.5) -> bool: + """Whether the head is at idle: each arm motor at its idle target by its absolute encoder, and + Z at or above its home flag by its encoder (command 84). + + Args: + tolerance: how far each arm motor may be from its target, in degrees, and Z below its flag, + in mm. + + Raises: + MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. + """ + self._check_calibrated() + idle_targets = self._get_idle_targets() + absolute = await self.request_absolute_joint_state() + z = await self._request_measured_position(motor=Z_MOTOR) + return z <= tolerance and all( + abs(absolute[motor] - compute_position(motor=motor, device_position=idle_targets[motor])) + <= tolerance + for motor in ARM_MOTORS + ) # -- moves ------------------------------------------------------------------ + def _notify_pose(self, *, joint_state: JointState) -> None: + """Pass a measured pose to ``_pose_observer``, if one is set; a failure only warns. + + Args: + joint_state: each motor's position by its encoder, by motor, as + ``_request_measured_position`` reads it. + """ + if self._pose_observer is None: + return + try: + theta_1, theta_2 = self._get_link_angles(joint_state=joint_state) + self._pose_observer(theta_1=theta_1, theta_2=theta_2, z=0.0 - joint_state[Z_MOTOR]) + except Exception: + logger.warning("[%s] the arm's pose was not passed on", self._driver.name, exc_info=True) + + async def _report_pose(self) -> None: + """Read each motor's encoder (command 84) and pass the pose on, only if ``_pose_observer`` is + set; a failed read only warns.""" + if self._pose_observer is None: + return + try: + measured = { + motor: await self._request_measured_position(motor=motor) + for motor in (*ARM_MOTORS, Z_MOTOR) + } + except Exception: + logger.warning("[%s] the arm's pose was not read", self._driver.name, exc_info=True) + return + self._notify_pose(joint_state=measured) + async def _log_where_it_stopped(self) -> None: - """Log where each motor is by its encoder after a failure; a failed read is logged and - dropped.""" + """Log where each motor is by its encoder after a failure, and pass it on; a failed read is + logged and dropped.""" try: measured = { motor: await self._request_measured_position(motor=motor) @@ -380,15 +881,29 @@ async def _log_where_it_stopped(self) -> None: logger.warning("[%s] the motors stopped at %s by their encoders", self._driver.name, measured) except Exception: logger.warning("[%s] where the motors stopped is not known", self._driver.name) + return + self._notify_pose(joint_state=measured) + + async def _stop_motors( + self, *, motors: Tuple[int, ...], decelerations: Optional[Dict[int, float]] = None + ) -> bool: + """Stop each motor (command 24), each in its own try; a failure is logged. Return whether every + stop was answered. - async def _stop_motors(self, *, motors: Tuple[int, ...]) -> bool: - """Stop each motor at its default deceleration (command 24), each in its own try; a failure is - logged. Return whether every stop was answered.""" + Args: + motors: the motors to stop. + decelerations: each motor's, in device units, a move stopped at its own + acceleration; a motor without one takes ``_get_device_acceleration``. + """ stopped = True for motor in motors: + deceleration = (decelerations or {}).get(motor) try: await self._unchecked_fw_stop( - motor=motor, deceleration=self._get_device_acceleration(motor=motor) + motor=motor, + deceleration=( + self._get_device_acceleration(motor=motor) if deceleration is None else deceleration + ), ) except Exception: logger.warning("[%s] could not stop motor %d", self._driver.name, motor, exc_info=True) @@ -431,34 +946,125 @@ async def _wait_for_motors_idle(self, *, timeout: float) -> None: raise MantisTimeoutError(f"{self._driver.name}: motors {busy} still move after {timeout} s") await asyncio.sleep(MOTOR_STATUS_POLL_INTERVAL) - async def _move_motors(self, *, targets: Dict[int, float]) -> None: - """Move each motor to its target, in device units, at its default speed, then wait. + async def _move_motors( + self, *, targets: Dict[int, float], profiles: Dict[int, Tuple[float, float]] + ) -> None: + """Move each motor to its target, in device units, then wait. - Each move is read back once as it starts, as recorded, then every motor until all stop. On any - failure the motors sent a move are stopped and where they stopped is logged before it raises. + Each move's reply is checked and its motor read once as it starts, as recorded, then every + motor until all stop. On any failure the motors sent a move are stopped and where they stopped + is logged before it raises. Args: targets: device units, by motor, sent in motor order. + profiles: each motor's (speed, acceleration) in device units, by motor. + + Raises: + MantisMotionError: if a motor answers its move with an error code, or reports a fault. """ sent: List[int] = [] try: for motor in sorted(targets): + speed, acceleration = profiles[motor] sent.append(motor) # before the send: a send that raised may still have started it - await self._unchecked_fw_move_absolute( - motor=motor, - position=targets[motor], - speed=self._get_device_speed(motor=motor), - acceleration=self._get_device_acceleration(motor=motor), + code = await self._unchecked_fw_move_absolute( + motor=motor, position=targets[motor], speed=speed, acceleration=acceleration ) + if code != 0: + meaning = MOTOR_ERROR_CODE_DESCRIPTIONS.get(code, "a code not seen before") + raise MantisMotionError( + f"{self._driver.name}: motor {motor} answered its move with error {code}: {meaning}", + motor=motor, + code=code, + ) status = await self._driver.request_motor_status(motor=motor) self._check_status(motor=motor, status=status) await self._wait_for_motors_idle(timeout=MOVE_TIMEOUT) except BaseException: - await self._stop_motors(motors=tuple(sent)) + await self._stop_motors( + motors=tuple(sent), + decelerations={motor: acceleration for motor, (_, acceleration) in profiles.items()}, + ) + await self._log_where_it_stopped() + raise + + def _get_route_line_targets(self, *, route_line: RouteLine) -> Dict[int, float]: + """The motors a route line moves and their targets, in device units, each within its limits. + + Raises: + MantisKinematicsError: if its (x, y) is out of the arm's reach. + ValueError: if a target is outside its motor's limits. + """ + x, y, z = route_line + targets = self._get_arm_targets(x=x, y=y) if x is not None and y is not None else {} + if z is not None: + targets[Z_MOTOR] = compute_device_position(motor=Z_MOTOR, position=z) + for motor, position in targets.items(): + self._check_reachable(motor=motor, position=position) + return targets + + def _get_route_targets(self, *, route: List[RouteLine]) -> List[Dict[int, float]]: + """Each route line's targets, as ``_get_route_line_targets``, all checked before anything moves. + + Raises: + MantisKinematicsError, ValueError: as ``_get_route_line_targets``. + """ + return [self._get_route_line_targets(route_line=route_line) for route_line in route] + + async def _move_along_route(self, *, steps: List[Dict[int, float]]) -> None: + """Move to each step's targets in turn at the route profile, each motor on its own (command + 22), as in the recorded recoveries. + + Args: + steps: each route line's targets, from ``_get_route_targets``. + """ + profiles = self._get_route_profiles() + for targets in steps: + if targets: + await self._move_motors(targets=targets, profiles=profiles) + + def _get_queued_move( + self, *, targets: Dict[int, float], profiles: Dict[int, Tuple[float, float]] + ) -> List[Tuple[float, float, float]]: + """A queued move's (position, speed, acceleration) per motor, device units, motors 0 to 2; a + motor without a target is held, sent all zeros. + + Args: + targets: device units, by motor. + profiles: each motor's (speed, acceleration) in device units, by motor. + """ + return [ + (targets[motor], *profiles[motor]) if motor in targets else (0.0, 0.0, 0.0) + for motor in (*ARM_MOTORS, Z_MOTOR) + ] + + async def _play_queued_moves(self, *, moves: List[List[Tuple[float, float, float]]]) -> None: + """Clear and start the sequencer, queue each move, then wait until the last is played. + + If the wait fails, the sequencer is aborted so nothing more plays, and where the motors + stopped is logged. + + Raises: + MantisMotionError: if a motor reports an error first. + MantisTimeoutError: if the last move is not reported played within ``MOVE_TIMEOUT`` each. + """ + if not moves: + return + await self._driver._unchecked_fw_clear_sequencer() + await self._driver._unchecked_fw_start_sequencer() + try: + for targets in moves: + item = await self._driver._queue_move_item(targets=targets) + await self._driver._wait_for_queue_item(item=item, timeout=MOVE_TIMEOUT * len(moves)) + except BaseException: + try: + await self._driver._unchecked_fw_abort_sequencer() + except Exception: + logger.warning("[%s] could not abort the sequencer", self._driver.name, exc_info=True) await self._log_where_it_stopped() raise - async def _declare_absolute_joint_position(self) -> None: + async def _declare_absolute_joint_state(self) -> None: """Declare each arm motor where its absolute encoder reads, ``DECLARE_PASSES`` times. A pass clears both arm motors' faults, reads both encoders, then sets each motor's position @@ -475,19 +1081,19 @@ async def _declare_absolute_joint_position(self) -> None: await self._unchecked_fw_set_encoder_position(motor=motor, position=positions[motor]) await asyncio.sleep(0.01) # recorded passes are 10 ms apart - def _check_near_idle(self, *, joint_position: Dict[int, float]) -> None: + def _check_near_idle(self, *, joint_state: JointState) -> None: """Raise unless the head stands within ``FRONT_ZONE_X_RANGE``, where it is moved to idle from. From anywhere else, a move to idle could cross a chip rack. Args: - joint_position: each arm motor's angle, in degrees, by motor. + joint_state: each arm motor's angle, in degrees, by motor. Raises: MantisMotionError: if the head is elsewhere, or its position is not known. """ try: - x, y = self._get_head_position(joint_position=joint_position) + x, y = self._get_head_position(joint_state=joint_state) except MantisKinematicsError as error: raise MantisMotionError(f"{self._driver.name}: where the head is is not known") from error low, high = FRONT_ZONE_X_RANGE @@ -497,6 +1103,48 @@ def _check_near_idle(self, *, joint_position: Dict[int, float]) -> None: f"from x {low} to {high} mm. Park the arm at idle, then set up again" ) + def _get_zone(self, *, x: float, y: float) -> str: + """The zone whose rectangle holds (x, y), edges included, in ``ZONES`` order; else Unknown. + + Raises: + MantisConfigurationError: without the unit's ``zones`` block. + """ + if self.configuration.zone_rectangles is None: + raise MantisConfigurationError(f"{self._driver.name}: no zones for this unit") + for zone in ZONES: + minimum_x, maximum_x, minimum_y, maximum_y = self.configuration.zone_rectangles[zone] + if minimum_x <= x <= maximum_x and minimum_y <= y <= maximum_y: + return zone + return UNKNOWN_ZONE + + def _get_routes(self, *, joint_state: JointState) -> Tuple[List[RouteLine], List[RouteLine]]: + """The routes to safe XY and to idle from where the arm motors' angles put the head. + + The zone's own; without the unit's zones, the Front zone's, from near idle only. + + Args: + joint_state: each arm motor's angle, in degrees, by motor. + + Raises: + MantisMotionError: if where the head is is not known, or without zones it is not near idle. + """ + if ( + self.configuration.zone_rectangles is None + or self.configuration.safe_xy_routes is None + or self.configuration.idle_routes is None + ): + self._check_near_idle(joint_state=joint_state) + return FRONT_SAFE_XY_ROUTE, FRONT_IDLE_ROUTE + try: + x, y = self._get_head_position(joint_state=joint_state) + except MantisKinematicsError as error: + raise MantisMotionError(f"{self._driver.name}: where the head is is not known") from error + zone = self._get_zone(x=x, y=y) + logger.info( + "[%s] the head is at (%.3f, %.3f) mm, in the %s zone", self._driver.name, x, y, zone + ) + return self.configuration.safe_xy_routes[zone], self.configuration.idle_routes[zone] + async def _home_z(self) -> None: """Home Z on its flag, as recorded: check no motor slipped, clear Z's faults, search, wait. @@ -544,24 +1192,40 @@ async def _home_z(self) -> None: status=status, ) - async def _move_to_idle(self) -> None: - """Move to idle by the recorded route from the Front zone: Z up, the arms, then both.""" - arms = self._get_idle_targets() - await self._move_motors(targets={Z_MOTOR: 0.0}) - await self._move_motors(targets=arms) - await self._move_motors(targets={**arms, Z_MOTOR: 0.0}) + async def _recover_to_idle(self) -> None: + """Bring the head to idle from where the arms are declared, as in a recorded recovery. + + Declare the arms, move them along the zone's route to safe XY, home Z; twice declare them and + move along the zone's route to idle; home Z. The zone is read again before each route. + """ + await self._declare_absolute_joint_state() + safe_xy_route, _ = self._get_routes(joint_state=await self.request_joint_state()) + await self._move_along_route(steps=self._get_route_targets(route=safe_xy_route)) + await self._home_z() + for _ in range(2): + await self._declare_absolute_joint_state() + _, idle_route = self._get_routes(joint_state=await self.request_joint_state()) + await self._move_along_route(steps=self._get_route_targets(route=idle_route)) + await self._home_z() + + async def _unpower_arm_motors(self) -> None: + """Run both arm motors open loop, unpowered; a failure is logged, each motor in its own try.""" + for motor in ARM_MOTORS: + try: + await self._unchecked_fw_set_motor_open_loop(motor=motor, open_loop=True) + except Exception: + logger.warning("[%s] could not unpower motor %d", self._driver.name, motor, exc_info=True) async def initialize(self) -> None: """Bring the arm and Z up from power-on, as in the recorded v4 startup. Energise the arm motors, declare them where their absolute encoders read, set their following - error, move them to idle, home Z, then twice declare and move to idle again, and home Z. A - failure from energising on leaves the arm motors unpowered again. + error, then recover to idle: along the zone's routes, or without the unit's zones from near + idle only. A failure from energising on leaves the arm motors unpowered again. Raises: MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. - MantisMotionError: if the head is not where it is moved straight to idle from, or a motor - faults. + MantisMotionError: if the head is not where it is moved to idle from, or a motor faults. MantisTimeoutError: if a move or the home does not end in time. """ self._check_calibrated() @@ -569,72 +1233,629 @@ async def initialize(self) -> None: try: for motor in ARM_MOTORS: await self._unchecked_fw_set_motor_open_loop(motor=motor, open_loop=False) - await self._declare_absolute_joint_position() + await self._declare_absolute_joint_state() for motor in ARM_MOTORS: await self._unchecked_fw_set_following_error_configuration( motor=motor, enabled=True, maximum=ARM_MAXIMUM_FOLLOWING_ERROR ) await self._unchecked_fw_clear_motor_faults(motor=Z_MOTOR) - await self._declare_absolute_joint_position() - self._check_near_idle(joint_position=await self.request_joint_position()) - await self._move_motors(targets=self._get_idle_targets()) - await self._home_z() - for _ in range(2): - await self._declare_absolute_joint_position() - self._check_near_idle(joint_position=await self.request_joint_position()) - await self._move_to_idle() - await self._home_z() + await self._recover_to_idle() except BaseException: - for motor in ARM_MOTORS: - try: - await self._unchecked_fw_set_motor_open_loop(motor=motor, open_loop=True) - except Exception: - logger.warning("[%s] could not unpower motor %d", self._driver.name, motor, exc_info=True) + await self._unpower_arm_motors() raise self._initialized = True - def _get_queued_target( - self, *, motor: int, position: Optional[float] - ) -> Tuple[float, float, float]: - """A motor's (position, speed, acceleration) in a queued move, device units. + async def recover_from_fault(self) -> None: + """Recover from a motor fault as recorded: stop every motor, clear Z's faults then the arms', + and recover to idle as ``initialize`` does. This moves the arm. + + A failure leaves the arm motors unpowered; set up again. + + Raises: + MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. + MantisMotionError: if the arm is not brought up, the head is not where it is moved to idle + from, or a motor faults. + MantisTimeoutError: if a move or the home does not end in time. + """ + self._check_calibrated() + self._check_initialized() + self._initialized = False + try: + for motor in (*ARM_MOTORS, Z_MOTOR): + await self._unchecked_fw_stop( + motor=motor, deceleration=self._get_device_acceleration(motor=motor) + ) + for motor in (Z_MOTOR, *ARM_MOTORS): + await self._unchecked_fw_clear_motor_faults(motor=motor) + await self._recover_to_idle() + except BaseException: + await self._unpower_arm_motors() + raise + self._initialized = True + await self._report_pose() + + async def _rise_to_traverse_height( + self, *, z_joint_position: float, profiles: Dict[int, Tuple[float, float]] + ) -> None: + """Rise Z to the traverse height if its encoder reads it below it (command 22), then read Z + back at or above it by its encoder (command 84), never the demand position. Args: - motor: 0, 1 (arm) or 2 (Z). - position: the target in device units, or None to hold the motor: it is sent all zeros. + z_joint_position: the traverse height as Z's position, mm down from its flag, within limits. + profiles: each motor's (speed, acceleration) in device units, by motor. + + Raises: + MantisMotionError: if Z answers the move with an error code, faults, or reads more than + ``Z_TRAVERSE_TOLERANCE`` below the height after it. """ - if position is None: - return (0.0, 0.0, 0.0) - return ( - position, - self._get_device_speed(motor=motor), - self._get_device_acceleration(motor=motor), + traverse = compute_device_position(motor=Z_MOTOR, position=z_joint_position) + measured = await self._request_measured_position(motor=Z_MOTOR) + if compute_device_position(motor=Z_MOTOR, position=measured) <= traverse: + return + await self._move_motors(targets={Z_MOTOR: traverse}, profiles=profiles) + measured = await self._request_measured_position(motor=Z_MOTOR) + if measured > z_joint_position + Z_TRAVERSE_TOLERANCE: + raise MantisMotionError( + f"{self._driver.name}: Z reads {measured:.3f} mm down by its encoder after rising to " + f"{z_joint_position:.3f} mm down; the arms are not moved", + motor=Z_MOTOR, + ) + + def _get_traverse_z_joint_position( + self, *, minimum_traverse_height: Optional[float], points: Sequence[Tuple[float, float]] + ) -> float: + """Z's position, mm down from its flag, for the traverse height over every deck-frame point, + the highest the stage's tilt asks for; checked against Z's limits. + + Args: + minimum_traverse_height: the nozzle's height above the deck, mm; None takes + ``default_minimum_traverse_height``, and with that None ``SAFE_HEIGHT``. + points: the deck-frame (x, y) the head travels between. + + Raises: + MantisConfigurationError: for a height in the deck frame without the unit's ``stage`` block. + ValueError: if the height is outside Z's limits. + """ + if minimum_traverse_height is None: + minimum_traverse_height = self.default_minimum_traverse_height + if minimum_traverse_height is None: + z_joint_position = 0.0 - SAFE_HEIGHT + else: + z_joint_position = min( + self._get_z_joint_position(x=x, y=y, z=minimum_traverse_height) for x, y in points + ) + self._check_reachable( + motor=Z_MOTOR, position=compute_device_position(motor=Z_MOTOR, position=z_joint_position) + ) + return z_joint_position + + async def _get_park_traverse_z_joint_position( + self, *, minimum_traverse_height: Optional[float] = None + ) -> float: + """Z's position, mm down from its flag, for the traverse height where the head stands, as + ``_get_traverse_z_joint_position``; the head read only for a height in the deck frame.""" + if minimum_traverse_height is None: + minimum_traverse_height = self.default_minimum_traverse_height + points: List[Tuple[float, float]] = [] + if minimum_traverse_height is not None: + head_x, head_y = self._get_head_position(joint_state=await self.request_joint_state()) + points = [self._get_deck_frame_xy(x=head_x, y=head_y)] + return self._get_traverse_z_joint_position( + minimum_traverse_height=minimum_traverse_height, points=points + ) + + def _get_approach_path_states(self, *, path: List[RouteLine]) -> List[Tuple[float, float, float]]: + """Where each line of an approach path leaves the head: arm-frame x and y, mm, and Z, mm down + from its flag; an axis a line leaves out kept from the line before, from idle with Z at its + flag.""" + x, y, z = POST_HOMING_POSITION[0], POST_HOMING_POSITION[1], 0.0 + states = [] + for line_x, line_y, line_z in path: + if line_x is not None and line_y is not None: + x, y = line_x, line_y + if line_z is not None: + z = line_z + states.append((x, y, z)) + return states + + def _get_place(self, *, joint_state: JointState) -> Optional[Tuple[str, int]]: + """The approach path, by name, and its line the head stands at, if it stands at a line of one's + descended part (after the path's last line at the safe height), within + ``APPROACH_PATH_TOLERANCE``; None in the open. Of lines the head matches on several paths, the + deepest along its path is taken. + + Args: + joint_state: where the motors stand, as ``request_joint_state`` reads them. + """ + x, y = self._get_head_position(joint_state=joint_state) + z = joint_state[Z_MOTOR] + place: Optional[Tuple[str, int]] = None + for name, path in (self.configuration.approach_paths or {}).items(): + states = self._get_approach_path_states(path=path) + for index in range(self._get_descent_start(path=path), len(states)): + line_x, line_y, line_z = states[index] + near = ( + math.hypot(x - line_x, y - line_y) <= APPROACH_PATH_TOLERANCE + and abs(z - line_z) <= APPROACH_PATH_TOLERANCE + ) + if near and (place is None or index > place[1]): + place = (name, index) + return place + + def _get_descent_start(self, *, path: List[RouteLine]) -> int: + """The first line of an approach path's descended part: after its last line at the safe + height; the path's length if it never reaches the safe height.""" + states = self._get_approach_path_states(path=path) + at_safe_height = [ + index + for index, (_, _, line_z) in enumerate(states) + if line_z <= APPROACH_PATH_TOLERANCE - SAFE_HEIGHT + ] + return at_safe_height[-1] + 1 if at_safe_height else len(states) + + @staticmethod + def _get_shared_length(*, first: List[RouteLine], second: List[RouteLine]) -> int: + """How many lines two approach paths share from their start.""" + count = 0 + for line, other in zip(first, second): + if line != other: + break + count += 1 + return count + + async def _leave_approach_path(self, *, name: str, index: int, until: int) -> None: + """Move the head back along an approach path from the line it stands at to an earlier line: + the path's lines played backwards, one queued move each at the route profile; a line naming the arms and Z moves both in one item. + + Args: + name: the approach path's name. + index: the line the head stands at. + until: the line to stop at, 0 the path's start. + """ + assert self.configuration.approach_paths is not None + path = self.configuration.approach_paths[name] + steps = [line for line in reversed(path[until:index]) if line != (None, None, None)] + profiles = self._get_route_profiles() + await self._play_queued_moves( + moves=[ + self._get_queued_move(targets=targets, profiles=profiles) + for targets in self._get_route_targets(route=steps) + ] + ) + + async def _leave_approach_path_if_on_one(self, *, joint_state: JointState) -> JointState: + """Move the head out to the start of the approach path it stands on, if it stands on one, and + return where the motors stand then; else return ``joint_state`` as it is. + + Args: + joint_state: where the motors stand, as ``request_joint_state`` reads them. + """ + place = self._get_place(joint_state=joint_state) + if place is None: + return joint_state + name, index = place + logger.info( + "[%s] the head stands on the %s approach path; it leaves along it", self._driver.name, name ) + await self._leave_approach_path(name=name, index=index, until=0) + return await self.request_joint_state() + + def _get_arm_profiles( + self, + *, + start: Optional[JointState], + target: JointState, + xy_speed: Optional[float], + xy_acceleration: Optional[float], + ) -> Dict[int, Tuple[float, float]]: + """Each arm motor's (speed, acceleration) in device units for a move of the nozzle. + + Without an xy speed or acceleration, given or default, the route profile. Given, each + motor's is scaled by its turn over the nozzle's straight-line distance, so the motors start and + end together and the move takes as long as a straight one would; the nozzle's path is not + straight. + + Args: + start: where the arms stand; None only without an xy speed or acceleration. + target: the arms' targets, degrees. + xy_speed: mm/s; None takes ``default_xy_speed``. + xy_acceleration: mm/s²; None takes ``default_xy_acceleration``. + """ + speed = self.default_xy_speed if xy_speed is None else xy_speed + acceleration = self.default_xy_acceleration if xy_acceleration is None else xy_acceleration + rates = {motor: (ROUTE_ARM_SPEED, ROUTE_ARM_ACCELERATION) for motor in ARM_MOTORS} + if (speed is not None or acceleration is not None) and start is not None: + start_x, start_y = self._get_head_position(joint_state=start) + target_x, target_y = self._get_head_position(joint_state={**start, **target}) + distance = math.hypot(target_x - start_x, target_y - start_y) + for motor in ARM_MOTORS: + turn = abs(target.get(motor, start[motor]) - start[motor]) + if distance > 0 and turn > 0: + rates[motor] = ( + ROUTE_ARM_SPEED if speed is None else speed * turn / distance, + ROUTE_ARM_ACCELERATION if acceleration is None else acceleration * turn / distance, + ) + return { + motor: self._get_profile(motor=motor, speed=motor_speed, acceleration=motor_acceleration) + for motor, (motor_speed, motor_acceleration) in rates.items() + } + + async def move_to_joint_state( + self, + *, + joint_state: JointState, + minimum_traverse_height_start: Optional[float] = None, + xy_speed: Optional[float] = None, + xy_acceleration: Optional[float] = None, + z_speed: Optional[float] = None, + z_acceleration: Optional[float] = None, + ) -> None: + """Move the motors named to a joint state, queued (command 68); the others hold. + + A move of an arm motor is guarded, as v1 STAR's moves across the deck: Z rises first to the + traverse height if its encoder reads it below it, and is read back there, then the arms move + with Z held, then Z moves if named. A move of Z alone is one queued move. The arms move in one + joint move, not along a route: nothing here avoids the chip racks. + + Args: + joint_state: each motor's target, by motor, as ``request_joint_state`` reads them. + minimum_traverse_height_start: the nozzle's height above the deck to rise to before the arms + move, mm, over where it starts and ends; ``default_minimum_traverse_height`` when None. + xy_speed: the nozzle's, mm/s; None takes ``default_xy_speed``. + xy_acceleration: the nozzle's, mm/s²; None takes ``default_xy_acceleration``. + z_speed: mm/s; None takes ``default_z_speed``. + z_acceleration: mm/s²; None takes ``default_z_acceleration``. + + Raises: + MantisConfigurationError: for a traverse height in the deck frame without the unit's + ``stage`` block; nothing is sent. + MantisMotionError: if the arm is not brought up, a motor faults, or Z does not read at the traverse height after rising to it. + MantisKinematicsError: if the links cannot meet at the arm motors' targets. + ValueError: if a motor is unknown, a target outside its limits, or a speed or acceleration + out of range. + """ + self._check_initialized() + unknown = set(joint_state) - {*ARM_MOTORS, Z_MOTOR} + if unknown: + raise ValueError(f"the motors are {(*ARM_MOTORS, Z_MOTOR)}, got {sorted(unknown)}") + z_profile = self._get_profile(motor=Z_MOTOR, speed=z_speed, acceleration=z_acceleration) + for name, value in (("xy_speed", xy_speed), ("xy_acceleration", xy_acceleration)): + if value is not None: + self._check_positive(name=name, value=value) + targets = { + motor: compute_device_position(motor=motor, position=value) + for motor, value in joint_state.items() + } + for motor, target in targets.items(): + self._check_reachable(motor=motor, position=target) + arm_targets = {motor: target for motor, target in targets.items() if motor in ARM_MOTORS} + start = await self._leave_approach_path_if_on_one(joint_state=await self.request_joint_state()) + if not arm_targets: + await self._play_queued_moves( + moves=[self._get_queued_move(targets=targets, profiles={Z_MOTOR: z_profile})] + ) + await self._report_pose() + return + end = {**start, **joint_state} + self._get_head_position(joint_state=end) + if minimum_traverse_height_start is None: + minimum_traverse_height_start = self.default_minimum_traverse_height + points: List[Tuple[float, float]] = [] + if minimum_traverse_height_start is not None: + for pose in (start, end): + head_x, head_y = self._get_head_position(joint_state=pose) + points.append(self._get_deck_frame_xy(x=head_x, y=head_y)) + traverse = self._get_traverse_z_joint_position( + minimum_traverse_height=minimum_traverse_height_start, points=points + ) + arm_profiles = self._get_arm_profiles( + start=start, target=joint_state, xy_speed=xy_speed, xy_acceleration=xy_acceleration + ) + await self._rise_to_traverse_height(z_joint_position=traverse, profiles={Z_MOTOR: z_profile}) + moves = [self._get_queued_move(targets=arm_targets, profiles=arm_profiles)] + if Z_MOTOR in targets: + moves.append( + self._get_queued_move(targets={Z_MOTOR: targets[Z_MOTOR]}, profiles={Z_MOTOR: z_profile}) + ) + for move in moves: + await self._play_queued_moves(moves=[move]) + await self._report_pose() + + async def move_to_xy_position( + self, + *, + x: float, + y: float, + minimum_traverse_height_start: Optional[float] = None, + speed: Optional[float] = None, + acceleration: Optional[float] = None, + ) -> None: + """Move the nozzle across to (x, y) in the deck frame, guarded: Z rises first to the traverse + height if it is below it, then the arms move with Z held. Nothing avoids the chip racks. + + Over the plate holder the unit's stage transform places it; elsewhere the arm frame fitted to it. + + Args: + x: right of the deck's front-left corner, mm. + y: behind the deck's front-left corner, mm. + minimum_traverse_height_start: the nozzle's height above the deck to rise to first, mm; + ``default_minimum_traverse_height`` when None. + speed: the nozzle's, mm/s; None takes ``default_xy_speed``. + acceleration: the nozzle's, mm/s²; None takes ``default_xy_acceleration``. + + Raises: + MantisConfigurationError: without the unit's ``arm`` or ``stage`` block; nothing is sent. + MantisMotionError: if the arm is not brought up, a motor faults, or Z does not read at the traverse height after rising to it. + MantisKinematicsError: if (x, y) is out of the arm's reach. + ValueError: if a target is outside its motor's limits, or a speed or acceleration out of + range. + """ + self._check_initialized() + arm_x, arm_y = self._get_arm_frame_xy(x=x, y=y) + targets = self._get_arm_targets(x=arm_x, y=arm_y) + await self.move_to_joint_state( + joint_state={ + motor: compute_position(motor=motor, device_position=target) + for motor, target in targets.items() + }, + minimum_traverse_height_start=minimum_traverse_height_start, + xy_speed=speed, + xy_acceleration=acceleration, + ) + + async def move_to_z_position( + self, *, z: float, speed: Optional[float] = None, acceleration: Optional[float] = None + ) -> None: + """Move the nozzle to a height in the deck frame where it stands; the arms hold. The stage's + tilt under the nozzle is taken in. + + Args: + z: above the deck, mm. + speed: in mm/s; None takes ``default_z_speed``. + acceleration: in mm/s²; None takes ``default_z_acceleration``. + + Raises: + MantisConfigurationError: without the unit's ``arm`` or ``stage`` block; nothing is sent. + MantisMotionError: if the arm is not brought up, or Z faults. + MantisKinematicsError: if the links cannot meet at the angles read. + ValueError: if the target is outside Z's limits, or a speed or acceleration out of range. + """ + self._check_initialized() + self._check_stage() + z_profile = self._get_profile(motor=Z_MOTOR, speed=speed, acceleration=acceleration) + joint_state = await self._leave_approach_path_if_on_one( + joint_state=await self.request_joint_state() + ) + head_x, head_y = self._get_head_position(joint_state=joint_state) + x, y = self._get_deck_frame_xy(x=head_x, y=head_y) + target = compute_device_position( + motor=Z_MOTOR, position=self._get_z_joint_position(x=x, y=y, z=z) + ) + self._check_reachable(motor=Z_MOTOR, position=target) + await self._play_queued_moves( + moves=[self._get_queued_move(targets={Z_MOTOR: target}, profiles={Z_MOTOR: z_profile})] + ) + await self._report_pose() + + async def move_to_location( + self, + *, + location: Coordinate, + minimum_traverse_height_start: Optional[float] = None, + xy_speed: Optional[float] = None, + xy_acceleration: Optional[float] = None, + z_speed: Optional[float] = None, + z_acceleration: Optional[float] = None, + ) -> None: + """Move the nozzle to a deck-frame location: rise to the traverse height if below it, move + across with Z held, then move Z to the location's height. + + Every target is checked before anything moves, and Z is read back at the traverse height by + its encoder before the arms move. The arms move in one joint move: nothing here avoids the + chip racks. + + Args: + location: the nozzle's x, y and height above the deck, mm. + minimum_traverse_height_start: the nozzle's height above the deck to rise to first, mm, over + both ends; ``default_minimum_traverse_height`` when None. + xy_speed: the nozzle's, mm/s; None takes ``default_xy_speed``. + xy_acceleration: the nozzle's, mm/s²; None takes ``default_xy_acceleration``. + z_speed: mm/s; None takes ``default_z_speed``. + z_acceleration: mm/s²; None takes ``default_z_acceleration``. + + Raises: + MantisConfigurationError: without the unit's ``arm`` or ``stage`` block; nothing is sent. + MantisMotionError: if the arm is not brought up, a motor faults, or Z does not read at the traverse height after rising to it. + MantisKinematicsError: if the location is out of the arm's reach. + ValueError: if a target is outside its motor's limits, or a speed or acceleration out of + range. + """ + self._check_initialized() + self._check_stage() + z_profile = self._get_profile(motor=Z_MOTOR, speed=z_speed, acceleration=z_acceleration) + for name, value in (("xy_speed", xy_speed), ("xy_acceleration", xy_acceleration)): + if value is not None: + self._check_positive(name=name, value=value) + arm_x, arm_y = self._get_arm_frame_xy(x=location.x, y=location.y) + arm_targets = self._get_arm_targets(x=arm_x, y=arm_y) + z_target = self._get_z_joint_position(x=location.x, y=location.y, z=location.z) + for motor, target in ( + *arm_targets.items(), + (Z_MOTOR, compute_device_position(motor=Z_MOTOR, position=z_target)), + ): + self._check_reachable(motor=motor, position=target) + start = await self._leave_approach_path_if_on_one(joint_state=await self.request_joint_state()) + head_x, head_y = self._get_head_position(joint_state=start) + traverse = self._get_traverse_z_joint_position( + minimum_traverse_height=minimum_traverse_height_start, + points=[self._get_deck_frame_xy(x=head_x, y=head_y), (location.x, location.y)], + ) + await self._rise_to_traverse_height(z_joint_position=traverse, profiles={Z_MOTOR: z_profile}) + arm_joint_state = { + motor: compute_position(motor=motor, device_position=target) + for motor, target in arm_targets.items() + } + arm_profiles = self._get_arm_profiles( + start=start, target=arm_joint_state, xy_speed=xy_speed, xy_acceleration=xy_acceleration + ) + z_device_target = compute_device_position(motor=Z_MOTOR, position=z_target) + await self._play_queued_moves( + moves=[self._get_queued_move(targets=arm_targets, profiles=arm_profiles)] + ) + await self._play_queued_moves( + moves=[ + self._get_queued_move(targets={Z_MOTOR: z_device_target}, profiles={Z_MOTOR: z_profile}) + ] + ) + await self._report_pose() + + async def _route_to_idle(self, *, z_joint_position: float) -> None: + """Rise to the traverse height if below it, then move along the route to idle from where the + absolute encoders put the head, at the route profile. + + Args: + z_joint_position: the traverse height as Z's position, mm down from its flag. + + Raises: + MantisMotionError: if a motor faults, Z does not read at the traverse height after rising, + where the head is is not known, or without the unit's zones it is not near idle. + """ + await self._wait_for_motors_idle(timeout=MOVE_TIMEOUT) + _, idle_route = self._get_routes(joint_state=await self.request_absolute_joint_state()) + steps = self._get_route_targets(route=idle_route) + await self._rise_to_traverse_height( + z_joint_position=z_joint_position, profiles=self._get_route_profiles() + ) + await self._move_along_route(steps=steps) + + async def park(self, *, minimum_traverse_height_start: Optional[float] = None) -> None: + """Move the head to idle along its zone's route, from any zone; from an approach path, first out + along it. + + The head first rises to the traverse height if it is below it. Without the unit's zones it is + parked from near idle only. + + Args: + minimum_traverse_height_start: the nozzle's height above the deck to rise to first, mm; + ``default_minimum_traverse_height`` when None. + + Raises: + MantisConfigurationError: for a height in the deck frame without the unit's ``stage`` block. + MantisMotionError: if the arm is not brought up, a motor faults, Z does not read at the + traverse height after rising, or without the unit's zones the head is not near idle. + ValueError: if the traverse height is outside Z's limits. + """ + self._check_initialized() + await self._leave_approach_path_if_on_one(joint_state=await self.request_joint_state()) + traverse = await self._get_park_traverse_z_joint_position( + minimum_traverse_height=minimum_traverse_height_start + ) + await self._route_to_idle(z_joint_position=traverse) + await self._report_pose() + + async def _move_along_approach_path(self, *, name: str) -> None: + """Move the head to the end of an approach path, from wherever it stands. + + On that path, onward from where it stands. On a neighbouring path, one that shares this one's + descent (the two washes), back to its safe-Z line (its last line but one), then across to this + one's and down. On another path, back along it to the + last line the two share, then along this one from there. In the open, to idle first as ``park`` + takes it, then along the path from its start. Where the head stands is read from the motors, + never remembered. + + Raises: + MantisConfigurationError: without the path; nothing moves. + MantisMotionError: if the arm is not brought up, a motor faults, or Z does not read at the + traverse height after rising. + """ + self._check_initialized() + paths = self.configuration.approach_paths or {} + path = paths.get(name) + if path is None: + raise MantisConfigurationError( + f"{self._driver.name}: no approach path to the {name} for this unit; give its " + "configuration folder (oem_config_directory) or a configuration saved with it" + ) + profiles = self._get_route_profiles() + self._get_route_targets(route=path) # every line checked before anything moves + place = self._get_place(joint_state=await self.request_joint_state()) + if place is not None and place[0] == name: + start = place[1] + 1 + elif place is not None: + other_name, index = place + other = paths[other_name] + shared = self._get_shared_length(first=other, second=path) + safe_z_line = len(other) - 2 + if shared >= self._get_descent_start(path=other) and index >= safe_z_line >= 0: + await self._leave_approach_path(name=other_name, index=index, until=safe_z_line) + start = max(len(path) - 2, 0) + else: + await self._leave_approach_path(name=other_name, index=index, until=max(shared - 1, 0)) + start = shared + else: + if not await self.request_is_parked(): + await self.park() + start = 0 + moves = [ + self._get_queued_move(targets=targets, profiles=profiles) + for targets in self._get_route_targets(route=path[start:]) + ] + await self._play_queued_moves(moves=moves) + await self._report_pose() + + async def move_to_waste(self) -> None: + """Move the head into the waste along the unit's approach path, from wherever it stands. + + Raises: + MantisConfigurationError: without the unit's approach path; nothing moves. + MantisMotionError: if the arm is not brought up, or a motor faults. + """ + await self._move_along_approach_path(name=WASTE_APPROACH_PATH) + + async def move_to_wash(self, *, position: int) -> None: + """Move the head into a wash position along the unit's approach path, from wherever it stands; + from the other wash position without leaving the wash. + + Args: + position: 1 or 2. + + Raises: + MantisConfigurationError: without the unit's approach path; nothing moves. + MantisMotionError: if the arm is not brought up, or a motor faults. + ValueError: if ``position`` is not 1 or 2. + """ + if position not in WASH_POSITIONS: + raise ValueError(f"position must be one of {WASH_POSITIONS}, got {position}") + await self._move_along_approach_path(name=WASH_APPROACH_PATH.format(position=position)) async def _park(self) -> None: """Park at idle and lower Z to ``SHUTDOWN_Z``, as in the recorded close. - Z up on its own (command 22), read back by its encoder; then, queued, the arms with Z held, - both; then Z down at idle. It waits for the motors to stop, and checks where the head is from - the absolute encoders, not the controller. + From away from idle the head first moves to idle along its zone's route; without the unit's + zones it is parked from near idle only. Then Z up on its own (command 22), read back by its + encoder; then, queued, the arms with Z held, both; then Z down at idle. Where the head is comes + from the absolute encoders. Every target is checked against its motor's limits first. Raises: - MantisMotionError: if a motor reports a fault, or the head is not where it is moved to idle - from (nothing moves), or Z does not read up within ``Z_TRAVERSE_TOLERANCE`` after rising - (the arms are not moved). + MantisMotionError: if a motor reports a fault, or without zones the head is not near idle + (nothing moves), or Z does not read up within ``Z_TRAVERSE_TOLERANCE`` after rising (the + arms are not moved). + ValueError: if a target is outside its motor's limits; nothing moves. MantisTimeoutError: if a motor still moves, or a queued move is not reported played, in time. """ + idle_targets = self._get_route_line_targets(route_line=(*POST_HOMING_POSITION, None)) + z_up = self._get_route_line_targets(route_line=(None, None, 0.0)) + z_down = self._get_route_line_targets(route_line=(None, None, SHUTDOWN_Z)) await self._wait_for_motors_idle(timeout=MOVE_TIMEOUT) - self._check_near_idle(joint_position=await self.request_absolute_joint_position()) - idle_targets = self._get_idle_targets() - arms = [ - self._get_queued_target(motor=motor, position=idle_targets[motor]) for motor in ARM_MOTORS - ] - z_up = self._get_queued_target(motor=Z_MOTOR, position=0.0) - z_down = self._get_queued_target( - motor=Z_MOTOR, position=compute_device_position(motor=Z_MOTOR, position=SHUTDOWN_Z) - ) - z_held = self._get_queued_target(motor=Z_MOTOR, position=None) - await self._move_motors(targets={Z_MOTOR: 0.0}) + await self._leave_approach_path_if_on_one(joint_state=await self.request_joint_state()) + try: + self._check_near_idle(joint_state=await self.request_absolute_joint_state()) + except MantisMotionError: + if self.configuration.zone_rectangles is None: + raise + await self._route_to_idle(z_joint_position=await self._get_park_traverse_z_joint_position()) + profiles = self._get_route_profiles() + await self._move_motors(targets=z_up, profiles=profiles) z = await self._request_measured_position(motor=Z_MOTOR) if z > Z_TRAVERSE_TOLERANCE: raise MantisMotionError( @@ -642,12 +1863,16 @@ async def _park(self) -> None: "it; the arms are not moved", motor=Z_MOTOR, ) - for batch in ([arms + [z_held], arms + [z_up]], [arms + [z_down]]): - await self._driver._unchecked_fw_clear_sequencer() - await self._driver._unchecked_fw_start_sequencer() - for targets in batch: - item = await self._driver._queue_move_item(targets=targets) - await self._driver._wait_for_queue_item(item=item, timeout=MOVE_TIMEOUT) + await self._play_queued_moves( + moves=[ + self._get_queued_move(targets=idle_targets, profiles=profiles), + self._get_queued_move(targets={**idle_targets, **z_up}, profiles=profiles), + ] + ) + await self._play_queued_moves( + moves=[self._get_queued_move(targets={**idle_targets, **z_down}, profiles=profiles)] + ) + await self._report_pose() async def _halt(self) -> None: """Stop every motor at its default deceleration, then park the arm if it was brought up and diff --git a/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py b/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py index 8fb5f75710c..bb47e84561a 100644 --- a/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py @@ -6,11 +6,12 @@ """ import json +import math import os import struct import tempfile import unittest -from typing import Dict, List, Tuple +from typing import Dict, List, Mapping, Optional, Tuple, Union from unittest import mock from pylabrobot.formulatrix.mantis.device import RECORDING_MANTIS_V4 @@ -18,15 +19,22 @@ from pylabrobot.formulatrix.mantis.driver.arm_kinematics import ( compute_absolute_device_position, compute_device_position, + compute_device_rate, compute_joint_angles, compute_position, ) from pylabrobot.formulatrix.mantis.driver.errors import ( MantisConfigurationError, + MantisKinematicsError, MantisMotionError, MantisTimeoutError, ) -from pylabrobot.formulatrix.mantis.driver.features.arm import POST_HOMING_POSITION, Arm +from pylabrobot.formulatrix.mantis.driver.features import arm as arm_module +from pylabrobot.formulatrix.mantis.driver.features.arm import ( + POST_HOMING_POSITION, + Arm, + MantisArmConfiguration, +) from pylabrobot.formulatrix.mantis.driver.features.led_bar_tests import ( BLUE, GREEN, @@ -46,6 +54,20 @@ FakeLink, make_answers, ) +from pylabrobot.formulatrix.mantis.driver.oem_config_tests import ( + SYNTHETIC_APPROACH_PATHS_BLOCK, + SYNTHETIC_IDLE_ROUTES, + SYNTHETIC_SAFE_XY, + SYNTHETIC_STAGE_BLOCK, + SYNTHETIC_WASH_ROUTE, + SYNTHETIC_WASTE_ROUTE, + SYNTHETIC_ZONES_BLOCK, +) +from pylabrobot.formulatrix.mantis.resource_model import ( + PLATE_HOLDER_LOCATION, + PLATE_HOLDER_SIZE_Y, +) +from pylabrobot.resources import Coordinate with open(RECORDING_MANTIS_V4, encoding="utf-8") as f: RECORDING = json.load(f) @@ -223,6 +245,10 @@ def get_expected_startup() -> List[str]: return expected +# The synthetic stage's reference height, mm. +REFERENCE_HEIGHT = SYNTHETIC_STAGE_BLOCK["reference_height"] + + class ArmLink(FakeLink): """Answers the startup's commands as recorded; each motor reads moving once after its move starts, then idle, and Z homed once it homed.""" @@ -265,17 +291,19 @@ def setUp(self): self.addCleanup(directory.cleanup) self.directory = directory.name - def declare(self, *, arm_block) -> str: + def declare(self, *, arm_block, **blocks) -> str: path = os.path.join(self.directory, f"declared_{len(os.listdir(self.directory))}.json") with open(path, "w", encoding="utf-8") as f: - json.dump({**RECORDING, **({"arm": arm_block} if arm_block is not None else {})}, f) + json.dump({**RECORDING, **({"arm": arm_block} if arm_block is not None else {}), **blocks}, f) return path - async def set_up_arm(self, *, link: FakeLink, arm_block=SYNTHETIC_ARM_BLOCK): + async def set_up_arm(self, *, link: FakeLink, arm_block=SYNTHETIC_ARM_BLOCK, **blocks): driver = await self.set_up_driver( - link=link, declared_configuration_json=self.declare(arm_block=arm_block) + link=link, declared_configuration_json=self.declare(arm_block=arm_block, **blocks) ) assert driver.arm is not None + # Runs before the driver's stop: an arm a test brought up is not parked when it ends. + self.addCleanup(setattr, driver.arm, "_initialized", False) self.sent_before = len(link.written) return driver @@ -377,11 +405,11 @@ async def test_a_motor_that_slipped_stops_z_homing_before_it_starts(self): async def test_reads_convert_to_degrees_and_mm(self): link = ArmLink(answers=make_answers()) driver = await self.set_up_arm(link=link) - joint_position = await driver.arm.request_joint_position() - self.assertAlmostEqual(joint_position[0], -0.585047 * 1.8, places=5) - self.assertAlmostEqual(joint_position[1], 1.76374 * 1.8, places=4) - self.assertEqual(joint_position[2], 0.0) - absolute = await driver.arm.request_absolute_joint_position() + joint_state = await driver.arm.request_joint_state() + self.assertAlmostEqual(joint_state[0], -0.585047 * 1.8, places=5) + self.assertAlmostEqual(joint_state[1], 1.76374 * 1.8, places=4) + self.assertEqual(joint_state[2], 0.0) + absolute = await driver.arm.request_absolute_joint_state() self.assertEqual(sorted(absolute), [0, 1]) @@ -454,9 +482,11 @@ def test_idle_targets_follow_the_units_offsets(self): with self.subTest(offsets=offsets): arm = Arm( driver=mock.Mock(), - offsets=offsets, - homing_position_a=SYNTHETIC_HOMING_POSITION_A, - encoder_correction={0: [], 1: []}, + configuration=MantisArmConfiguration( + offsets=offsets, + homing_position_a=SYNTHETIC_HOMING_POSITION_A, + encoder_correction={0: [], 1: []}, + ), ) for motor, target in arm._get_idle_targets().items(): expected = IDLE_ANGLES[motor] - offsets[motor] @@ -465,43 +495,57 @@ def test_idle_targets_follow_the_units_offsets(self): def test_the_implied_offsets_give_the_recorded_idle_targets(self): arm = Arm( driver=mock.Mock(), - offsets=SYNTHETIC_OFFSETS, - homing_position_a=SYNTHETIC_HOMING_POSITION_A, - encoder_correction={0: [], 1: []}, + configuration=MantisArmConfiguration( + offsets=SYNTHETIC_OFFSETS, + homing_position_a=SYNTHETIC_HOMING_POSITION_A, + encoder_correction={0: [], 1: []}, + ), ) targets = arm._get_idle_targets() self.assertAlmostEqual(targets[0], RECORDED_IDLE_TARGETS[0], places=12) self.assertEqual(targets[1], RECORDED_IDLE_TARGETS[1]) def test_without_the_arm_block_there_are_no_idle_targets(self): - arm = Arm(driver=mock.Mock(), offsets=None, homing_position_a=None, encoder_correction=None) + arm = Arm(driver=mock.Mock(), configuration=MantisArmConfiguration()) with self.assertRaises(MantisConfigurationError): arm._get_idle_targets() class DefaultTests(unittest.TestCase): def test_defaults_are_checked_when_assigned(self): - from pylabrobot.formulatrix.mantis.driver.features.arm import Arm - - arm = Arm(driver=mock.Mock(), offsets=None, homing_position_a=None, encoder_correction=None) - for name in ("default_arm_speed", "default_arm_acceleration", "default_z_speed"): + arm = Arm(driver=mock.Mock(), configuration=MantisArmConfiguration()) + for name in ("default_xy_speed", "default_xy_acceleration", "default_z_speed"): with self.subTest(name=name): with self.assertRaises(ValueError): setattr(arm, name, 0.0) - arm.default_z_acceleration = 100.0 - self.assertEqual(arm.default_z_acceleration, 100.0) + # The xy defaults are mm/s and mm/s², None the route profile. + arm.default_xy_speed = 100.0 + arm.default_xy_speed = None + self.assertIsNone(arm.default_xy_speed) + # Z's speed and acceleration stay within the recorded moves'. + for name, value in (("default_z_speed", 56.0), ("default_z_acceleration", 100.0)): + with self.subTest(name=name): + with self.assertRaises(ValueError): + setattr(arm, name, value) + arm.default_z_acceleration = 300.0 + self.assertEqual(arm.default_z_acceleration, 300.0) -def get_away_arm_block() -> Dict: - """The synthetic arm block, its references putting the last recorded readings at (-57, 0) mm.""" +def get_arm_block_at(*, x: float, y: float) -> Dict: + """The synthetic arm block, its references putting the last recorded readings at (x, y) mm.""" homing_position_a = {} - for motor, angle in enumerate(compute_joint_angles(x=-57.0, y=0.0)): + for motor, angle in enumerate(compute_joint_angles(x=x, y=y)): _, reading = struct.unpack(" Dict: + """The synthetic arm block, its references putting the last recorded readings at (-57, 0) mm.""" + return get_arm_block_at(x=-57.0, y=0.0) + + class StopTests(ArmTestCase): def close_link(self) -> FakeLink: """Answers the recorded close; each motor reads idle, each encoder as last in the startup.""" @@ -515,6 +559,8 @@ def close_link(self) -> FakeLink: for motor in (0, 1, 2): key = struct.pack(" bytes: + """A move's payload (command 22) at the recorded profile.""" + profile = Z_PROFILE if motor == 2 else ARM_PROFILE + return struct.pack(" Dict[int, float]: + """Each arm motor's target for the head at (x, y), device units, with the synthetic offsets.""" + angles = compute_joint_angles(x=x, y=y) + return { + motor: compute_device_position(motor=motor, position=angles[motor] - SYNTHETIC_OFFSETS[motor]) + for motor in (0, 1) + } + + +def get_route_moves(*, route) -> List[bytes]: + """The moves (command 22) along a route, each motor a line names in motor order.""" + moves = [] + for x, y, z in route: + if x is not None: + moves += [ + get_expected_move(motor=motor, position=position) + for motor, position in get_arm_positions(x=x, y=y).items() + ] + if z is not None: + moves.append( + get_expected_move(motor=2, position=compute_device_position(motor=2, position=z)) + ) + return moves + + +def get_queued_move(*, item: int, targets: Dict[int, float]) -> bytes: + """A queued move's payload (command 68) at the recorded profiles; a motor not named is held.""" + payload = struct.pack(" None: + """Answer every absolute encoder read with the startup's reading at ``index``.""" + for motor in (0, 1): + self.answers[(11, 30, struct.pack(" int: + _, board, command, payload = parse_frame(frame=data) + if (board, command) == (0, 22): + self.answers.setdefault((0, 22, payload), [bytes.fromhex(RECORDED_MOVE_REPLY)]) + if (board, command) == (0, 68): + self.answers.setdefault((0, 68, payload), [bytes.fromhex(CLOSE_QUEUED_MOVE_REPLY)]) + return await super().write(data) + + +def tokens(commands: List[Tuple[int, int, bytes]], *, without=(20,)) -> List[str]: + return [token(command) for command in commands if command[1] not in without] + + +class ZoneTests(ArmTestCase): + async def set_up_routes(self, *, link: FakeLink, arm_block=SYNTHETIC_ARM_BLOCK, **blocks): + blocks = { + "zones": SYNTHETIC_ZONES_BLOCK, + "approach_paths": SYNTHETIC_APPROACH_PATHS_BLOCK, + **blocks, + } + return await self.set_up_arm(link=link, arm_block=arm_block, **blocks) + + async def test_from_the_front_zone_the_recorded_startup_is_sent(self): + home = bytes.fromhex(RECORDED_STARTUP[-1].split(":")[2]) + link = ArmLink( + answers=make_answers(), events={(0, 21, home): [bytes.fromhex(STARTUP_HOME_DONE)]} + ) + driver = await self.set_up_routes(link=link) + await driver.arm.initialize() + self.assertEqual(tokens(self.sent(link)), get_expected_startup()) + + async def test_from_another_zone_the_arms_follow_its_routes(self): + home = bytes.fromhex(RECORDED_STARTUP[-1].split(":")[2]) + link = RouteLink( + answers=make_answers(), + events={(0, 21, home): [bytes.fromhex(STARTUP_HOME_DONE)]}, + positions=AWAY_POSITION_REPLIES, + ) + driver = await self.set_up_routes(link=link) + with self.assertLogs("pylabrobot.formulatrix.mantis.driver.features.arm", level="INFO") as logs: + await driver.arm.initialize() + self.assertTrue(any("in the LeftMiddle zone" in line for line in logs.output)) + sent = self.sent(link) + moves = [payload for _, command, payload in sent if command == 22] + safe_xy = [[*SYNTHETIC_SAFE_XY["LeftMiddle"], None]] + idle = SYNTHETIC_IDLE_ROUTES["LeftMiddle"] + self.assertEqual(moves, get_route_moves(route=safe_xy + idle + idle)) + # The route to safe XY, Z homed, the routes to idle, Z homed again. + self.assertEqual([command for _, command, _ in sent if command in (21, 22)][2:3], [21]) + self.assertEqual([command for _, command, _ in sent][-1], 20) + self.assertTrue(driver.arm.is_initialized) + + async def test_without_zones_a_head_away_from_idle_still_refuses(self): + link = RouteLink(answers=make_answers(), positions=AWAY_POSITION_REPLIES) + driver = await self.set_up_arm(link=link) + driver.default_read_timeout = 0.25 # unpowering the arms is never answered + with self.assertRaisesRegex(MantisMotionError, "moved to idle only from x"): + await driver.arm.initialize() + self.assertNotIn(22, [command for _, command, _ in self.sent(link)]) + + async def test_recover_from_fault_stops_clears_and_recovers_as_recorded(self): + """The stops and the clears (Z, then the arms) in the order recorded after a motor error, + each stop at the default deceleration; the recovery after them as the startup's.""" + home = bytes.fromhex(RECORDED_STARTUP[-1].split(":")[2]) + link = RouteLink( + answers=make_answers(), events={(0, 21, home): [bytes.fromhex(STARTUP_HOME_DONE)]} + ) + for motor in (0, 1): # the startup's readings after its first declaration + link.answers[(11, 30, struct.pack(" None: + """The traverse height at Z's flag, where its encoder is recorded; the deck frame's conversion + is tested on its own.""" + driver.arm._get_traverse_z_joint_position = ( # type: ignore[method-assign] + lambda **_: 0.0 + ) + + async def test_move_to_xy_position_moves_the_arms_in_one_queued_move_z_held(self): + queued = get_queued_move(item=15, targets=DECK_POINT_ARM_TARGETS) + link = RouteLink( + answers=make_answers(), + encoders={2: Z_UP_ENCODER_REPLY}, + events={(0, 68, queued): [bytes.fromhex(CLOSE_ITEM_PLAYED[15])]}, + ) + driver = await self.set_up_moves(link=link) + driver._next_item = 15 + self.set_traverse_at_flag(driver=driver) + await driver.arm.move_to_xy_position(x=DECK_POINT[0], y=DECK_POINT[1]) + # Where it stands, then Z by its encoder at the traverse height already: no rise. + self.assertEqual( + tokens(self.sent(link)), + [ + "0:27:0000", + "0:27:0100", + "0:27:0200", + "0:84:0200", + "0:61:", + "0:62:", + f"0:68:{queued.hex()}", + ], + ) + + async def test_a_move_across_from_below_the_traverse_height_rises_first(self): + queued = get_queued_move(item=15, targets=DECK_POINT_ARM_TARGETS) + link = RouteLink( + answers=make_answers(), + encoders={2: Z_RISING_ENCODER_REPLIES}, + events={(0, 68, queued): [bytes.fromhex(CLOSE_ITEM_PLAYED[15])]}, + ) + driver = await self.set_up_moves(link=link) + driver._next_item = 15 + self.set_traverse_at_flag(driver=driver) + await driver.arm.move_to_xy_position(x=DECK_POINT[0], y=DECK_POINT[1]) + # Z read down, up alone to the traverse height and read back there before the arms move. + self.assertEqual( + tokens(self.sent(link)), + ["0:27:0000", "0:27:0100", "0:27:0200", "0:84:0200", f"0:22:{Z_UP_MOVE.hex()}"] + + ["0:84:0200", "0:61:", "0:62:", f"0:68:{queued.hex()}"], + ) + + async def test_move_to_z_position_moves_z_alone_to_the_height_above_the_deck(self): + target = compute_device_position(motor=2, position=REFERENCE_HEIGHT_Z) + queued = get_queued_move(item=15, targets={2: target}) + link = RouteLink( + answers=make_answers(), events={(0, 68, queued): [bytes.fromhex(CLOSE_ITEM_PLAYED[15])]} + ) + driver = await self.set_up_moves(link=link) + driver._next_item = 15 + await driver.arm.move_to_z_position(z=REFERENCE_HEIGHT) + self.assertEqual( + tokens(self.sent(link)), + ["0:27:0000", "0:27:0100", "0:27:0200", "0:61:", "0:62:", f"0:68:{queued.hex()}"], + ) + + async def test_moves_out_of_range_are_refused_before_anything_is_sent(self): + link = RouteLink(answers=make_answers()) + driver = await self.set_up_moves(link=link) + arm = driver.arm + with self.assertRaisesRegex(ValueError, "Z speed must be from 1.0 to 55.0"): + await arm.move_to_z_position(z=REFERENCE_HEIGHT, speed=60.0) + with self.assertRaisesRegex(ValueError, "Z acceleration must be from 200.0 to 500.0"): + await arm.move_to_z_position(z=REFERENCE_HEIGHT, acceleration=1000.0) + with self.assertRaisesRegex(ValueError, "acceleration must be finite and above 0"): + await arm.move_to_xy_position(x=DECK_POINT[0], y=DECK_POINT[1], acceleration=0.0) + with self.assertRaises(MantisKinematicsError): + await arm.move_to_xy_position(x=500.0, y=500.0) + with self.assertRaisesRegex(ValueError, "the motors are"): + await arm.move_to_joint_state(joint_state={3: 0.0}) + with self.assertRaisesRegex(ValueError, "outside its limits"): + await arm.move_to_joint_state(joint_state={0: 8000.0}) + self.assertEqual(self.sent(link), []) + + async def test_a_height_outside_zs_limits_moves_nothing(self): + link = RouteLink(answers=make_answers()) + driver = await self.set_up_moves(link=link) + with self.assertRaisesRegex(ValueError, "outside its limits"): + await driver.arm.move_to_z_position(z=-100.0) + self.assertEqual(tokens(self.sent(link)), ["0:27:0000", "0:27:0100", "0:27:0200"]) + + async def test_moves_refuse_an_arm_not_brought_up(self): + link = RouteLink(answers=make_answers()) + driver = await self.set_up_arm(link=link, stage=TRANSLATED_STAGE_BLOCK) + with self.assertRaisesRegex(MantisMotionError, "not brought up"): + await driver.arm.move_to_xy_position(x=DECK_POINT[0], y=DECK_POINT[1]) + with self.assertRaisesRegex(MantisMotionError, "not brought up"): + await driver.arm.move_to_z_position(z=REFERENCE_HEIGHT) + with self.assertRaisesRegex(MantisMotionError, "not brought up"): + await driver.arm.park() + self.assertEqual(self.sent(link), []) + + async def test_without_the_stage_block_the_deck_frame_moves_send_nothing(self): + link = RouteLink(answers=make_answers()) + driver = await self.set_up_arm(link=link) + driver.arm._initialized = True + with self.assertRaisesRegex(MantisConfigurationError, "no stage transform"): + await driver.arm.move_to_xy_position(x=DECK_POINT[0], y=DECK_POINT[1]) + with self.assertRaisesRegex(MantisConfigurationError, "no stage transform"): + await driver.arm.move_to_z_position(z=REFERENCE_HEIGHT) + with self.assertRaisesRegex(MantisConfigurationError, "no stage transform"): + await driver.arm.request_pose() + self.assertEqual(self.sent(link), []) + + async def test_a_joint_move_holds_the_motors_it_does_not_name(self): + held_z_up = bytes.fromhex(CLOSE_QUEUED_MOVES[13]) # the arms held, Z to 0 + queued = struct.pack(" Arm: + """An arm with a mocked driver and a stage block, as a driver builds it.""" + return Arm( + driver=mock.Mock(), + configuration=MantisArmConfiguration( + offsets=kwargs.get("offsets"), + homing_position_a=kwargs.get("homing_position_a"), + encoder_correction=kwargs.get("encoder_correction"), + stage_origin=tuple(stage["origin"]), + stage_transformation_matrix=stage["transformation_matrix"], + stage_offset_x=tuple(stage["offset_x"]), + stage_offset_y=tuple(stage["offset_y"]), + arm_type_rotation_degree=stage["arm_type_rotation_degree"], + arm_type_translation=tuple(stage["arm_type_translation"]), + stage_reference_height=stage["reference_height"], + ), + ) + + +class DeckFrameTests(unittest.IsolatedAsyncioTestCase): + def test_off_the_plate_holder_a_rigid_stage_is_fitted_exactly(self): + # A rigid stage turns and moves the wellplate frame; the resource model only moves it, so the + # fit is that turn, and a point anywhere on the deck lands where the stage would put it. + stage = { + **TRANSLATED_STAGE_BLOCK, + "arm_type_rotation_degree": 1.5, + "arm_type_translation": [2.0, -3.0, 0.0], + } + arm = make_staged_arm(stage=stage) + turn, _, _ = arm._get_deck_to_arm_transform() + self.assertAlmostEqual(math.degrees(turn), 1.5, places=9) + angle = math.radians(1.5) + for x, y in ((0.0, 0.0), (180.0, 150.0), (-40.0, 120.0)): + with self.subTest(x=x, y=y): + # The point in the wellplate frame, then turned and moved as the stage does. + holder_x = x - PLATE_HOLDER_LOCATION.x + holder_y = PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - y + expected = ( + holder_x * math.cos(angle) - holder_y * math.sin(angle) + 2.0 - 50.0, + holder_x * math.sin(angle) + holder_y * math.cos(angle) - 3.0 + 40.0, + ) + arm_x, arm_y = arm._get_arm_frame_xy(x=x, y=y) + self.assertAlmostEqual(arm_x, expected[0], places=9) + self.assertAlmostEqual(arm_y, expected[1], places=9) + deck_x, deck_y = arm._get_deck_frame_xy(x=arm_x, y=arm_y) + self.assertAlmostEqual(deck_x, x, places=9) + self.assertAlmostEqual(deck_y, y, places=9) + + def test_over_the_plate_holder_a_point_reads_back_where_it_was_sent(self): + # The projective stage is inverted exactly there, not through the fit. + arm = make_staged_arm(stage=SYNTHETIC_STAGE_BLOCK) + for holder_x, holder_y in ((0.0, 0.0), (14.3, 11.28), (127.0, 85.0), (64.0, 40.0)): + x = PLATE_HOLDER_LOCATION.x + holder_x + y = PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - holder_y + with self.subTest(x=x, y=y): + arm_x, arm_y = arm._get_arm_frame_xy(x=x, y=y) + deck_x, deck_y = arm._get_deck_frame_xy(x=arm_x, y=arm_y) + self.assertAlmostEqual(deck_x, x, places=9) + self.assertAlmostEqual(deck_y, y, places=9) + + def test_an_xy_speed_scales_each_motor_by_its_turn(self): + # 100 mm/s and 500 mm/s² over the straight line: each motor at its turn over the distance, so + # both end together; the motor that does not turn keeps the route profile. + arm = Arm( + driver=mock.Mock(), + configuration=MantisArmConfiguration( + offsets={0: 0.0, 1: 0.0}, + homing_position_a=SYNTHETIC_HOMING_POSITION_A, + encoder_correction={0: [], 1: []}, + ), + ) + start_angles = compute_joint_angles(x=15.0, y=31.177) + target_angles = compute_joint_angles(x=-20.0, y=70.0) + start = {0: start_angles[0], 1: start_angles[1], 2: 0.0} + target = {0: target_angles[0], 1: target_angles[1]} + distance = math.hypot(-20.0 - 15.0, 70.0 - 31.177) + profiles = arm._get_arm_profiles( + start=start, target=target, xy_speed=100.0, xy_acceleration=500.0 + ) + for motor in (0, 1): + turn = abs(target[motor] - start[motor]) + with self.subTest(motor=motor): + expected = ( + compute_device_rate(motor=motor, rate=100.0 * turn / distance), + compute_device_rate(motor=motor, rate=500.0 * turn / distance), + ) + for value, expected_value in zip(profiles[motor], expected): + self.assertAlmostEqual(value, expected_value, places=9) + route = arm._get_arm_profiles(start=None, target=target, xy_speed=None, xy_acceleration=None) + self.assertEqual(route[0], (ARM_PROFILE_SPEED, ARM_PROFILE_ACCELERATION)) + + def test_over_the_plate_holder_the_stage_transform_places_the_nozzle(self): + # The holder's back-left corner is the wellplate frame's origin. + arm = make_staged_arm(stage=SYNTHETIC_STAGE_BLOCK) + corner = arm._get_arm_frame_xy( + x=PLATE_HOLDER_LOCATION.x, y=PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y + ) + self.assertAlmostEqual( + corner[0], (-50.0 * 1.001 + 40.0 * 0.001 - 0.08) / (-50.0 * -2e-05 + 40.0 * -3e-05 + 1.004) + ) + self.assertAlmostEqual( + corner[1], (-50.0 * -0.004 + 40.0 * 0.9995 + 0.09) / (-50.0 * -2e-05 + 40.0 * -3e-05 + 1.004) + ) + + def test_a_height_above_the_deck_takes_the_reference_height_and_the_tilt(self): + # 10 mm right of and 20 mm in front of the holder's back-left corner, 5 mm above the deck: + # 51 + (9.665 - 5) - 0.0015 * 10 + 0.0025 * 20 = 55.7 mm down. + arm = make_staged_arm(stage=SYNTHETIC_STAGE_BLOCK) + z = arm._get_z_joint_position( + x=PLATE_HOLDER_LOCATION.x + 10.0, + y=PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - 20.0, + z=5.0, + ) + self.assertAlmostEqual(z, 55.7) + + async def test_request_pose_places_the_nozzle_in_the_deck_frame(self): + arm = make_staged_arm( + stage=TRANSLATED_STAGE_BLOCK, + offsets=SYNTHETIC_OFFSETS, + homing_position_a=SYNTHETIC_HOMING_POSITION_A, + encoder_correction={motor: SYNTHETIC_CORRECTION for motor in (0, 1)}, + ) + joints = { + **{motor: IDLE_ANGLES[motor] - SYNTHETIC_OFFSETS[motor] for motor in (0, 1)}, + 2: 10.0, + } + with mock.patch.object(arm, "request_joint_state", mock.AsyncMock(return_value=joints)): + pose = await arm.request_pose() + # The translated stage puts the wellplate frame's origin at (-50, 40) in the arm frame. + x, y = POST_HOMING_POSITION + self.assertAlmostEqual(pose.nozzle_location.x, PLATE_HOLDER_LOCATION.x + x + 50.0) + self.assertAlmostEqual( + pose.nozzle_location.y, PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - (y - 40.0) + ) + # Z 10 mm down from its flag: 51 + 9.665 - 10 above the deck. + self.assertAlmostEqual(pose.nozzle_location.z, 50.665) + self.assertEqual( + (pose.head_joint_location.x, pose.head_joint_location.y), + (pose.nozzle_location.x, pose.nozzle_location.y), + ) + self.assertEqual(pose.joints, joints) + + +# Each arm motor's encoder (command 84) as it stands at the recorded idle targets, in device units. +IDLE_ENCODER_REPLIES = { + motor: struct.pack(" RouteLink: + """A link that plays Z's queued move to its flag, the arms held, as item 15.""" + queued = struct.pack(" List[bytes]: + moves = [] + for item, (x, y, z) in enumerate(SYNTHETIC_WASTE_ROUTE, start=first_item): + targets: Dict[int, float] = {} + if x is not None and y is not None: + targets = get_arm_positions(x=x, y=y) + if z is not None: + targets[2] = compute_device_position(motor=2, position=z) + moves.append(get_queued_move(item=item, targets=targets)) + return moves + + async def test_from_idle_the_waste_path_is_queued_line_by_line(self): + first_item = 16 - len(SYNTHETIC_WASTE_ROUTE) + 1 + expected = self.get_expected_waste_moves(first_item=first_item) + link = RouteLink( + answers=make_answers(), events={(0, 68, expected[-1]): [bytes.fromhex(CLOSE_ITEM_PLAYED[16])]} + ) + driver = await self.set_up_approach_paths(link=link) + driver._next_item = first_item + await driver.arm.move_to_waste() + queued = [payload for _, command, payload in self.sent(link) if command == 68] + self.assertEqual(queued, expected) + # The line naming no axis and Z alone at the safe height, queued as recorded. + self.assertEqual(queued[0][2:], bytes.fromhex(RECORDED_WASTE_HOLD)[2:]) + self.assertEqual(queued[2][2:], bytes.fromhex(RECORDED_WASTE_SAFE_HEIGHT)[2:]) + # The ready position, within what the synthetic offsets give. + recorded = struct.unpack(" Dict[int, float]: + angles = compute_joint_angles(x=x, y=y) + return {0: angles[0], 1: angles[1], 2: z} + + # At the waste's bottom, at its last line; at idle, or level with a route line above the safe + # height, in the open. + self.assertEqual(arm._get_place(joint_state=joints_at(-60.0, 10.0, 35.0)), ("waste", 8)) + self.assertIsNone(arm._get_place(joint_state=joints_at(15.0, 31.177, 0.0))) + self.assertIsNone(arm._get_place(joint_state=joints_at(-50.0, 20.0, -1.57))) + # Off a route line by more than the tolerance, in the open too. + self.assertIsNone(arm._get_place(joint_state=joints_at(-60.0, 11.0, 35.0))) + + +class StageDiscoveryTests(ArmTestCase): + async def test_the_stage_block_is_never_taken_from_a_recording(self): + path = os.path.join(self.directory, "recording.json") + with open(path, "w", encoding="utf-8") as f: + json.dump({**RECORDING, "stage": SYNTHETIC_STAGE_BLOCK}, f) + with mock.patch.object(master_module, "RECORDING_MANTIS_V4", path): + driver = await self.set_up_driver(link=FakeLink(answers=make_answers())) + assert driver.arm is not None + self.assertIsNone(driver.arm.configuration.stage_origin) + + async def test_a_declared_stage_block_is_read_and_saved(self): + driver = await self.set_up_arm( + link=FakeLink(answers=make_answers()), stage=SYNTHETIC_STAGE_BLOCK + ) + self.assertEqual(driver.arm.configuration.stage_origin, (-50.0, 40.0, 51.0)) + self.assertEqual( + driver.arm.configuration.stage_transformation_matrix[2], (-2e-05, -3e-05, 1.004) + ) + path = os.path.join(self.directory, "saved.json") + driver.save_configuration(path=path) + with open(path, encoding="utf-8") as f: + self.assertEqual(json.load(f)["stage"], SYNTHETIC_STAGE_BLOCK) + + async def test_a_malformed_stage_block_refuses_setup(self): + for block in ( + {**SYNTHETIC_STAGE_BLOCK, "origin": [1.0, 2.0]}, + {**SYNTHETIC_STAGE_BLOCK, "transformation_matrix": [[1.0, 0.0, 0.0]]}, + {key: value for key, value in SYNTHETIC_STAGE_BLOCK.items() if key != "offset_y"}, + ): + with self.subTest(block=sorted(block)): + driver = self.make_driver( + link=FakeLink(answers=make_answers()), + declared_configuration_json=self.declare(arm_block=SYNTHETIC_ARM_BLOCK, stage=block), + ) + with self.assertRaises(MantisConfigurationError): + await driver.setup(skip_arm=True) + + +class RouteDiscoveryTests(ArmTestCase): + async def test_routes_are_never_taken_from_a_recording(self): + path = os.path.join(self.directory, "recording.json") + with open(path, "w", encoding="utf-8") as f: + json.dump({**RECORDING, "zones": SYNTHETIC_ZONES_BLOCK}, f) + with mock.patch.object(master_module, "RECORDING_MANTIS_V4", path): + driver = await self.set_up_driver(link=FakeLink(answers=make_answers())) + assert driver.arm is not None + self.assertIsNone(driver.arm.configuration.zone_rectangles) + + async def test_declared_routes_are_read_and_saved(self): + driver = await self.set_up_arm( + link=FakeLink(answers=make_answers()), + zones=SYNTHETIC_ZONES_BLOCK, + approach_paths=SYNTHETIC_APPROACH_PATHS_BLOCK, + ) + self.assertEqual(driver.arm.configuration.approach_paths["wash_1"][0], (None, None, -1.57)) + path = os.path.join(self.directory, "saved.json") + driver.save_configuration(path=path) + with open(path, encoding="utf-8") as f: + saved = json.load(f) + self.assertEqual(saved["zones"], SYNTHETIC_ZONES_BLOCK) + self.assertEqual(saved["approach_paths"], SYNTHETIC_APPROACH_PATHS_BLOCK) + + async def test_a_malformed_route_block_refuses_setup(self): + for blocks in ( + {"zones": {**SYNTHETIC_ZONES_BLOCK, "rectangles": {"Front": [0, 1, 0, 1]}}}, + {"zones": {**SYNTHETIC_ZONES_BLOCK, "safe_xy_routes": SYNTHETIC_IDLE_ROUTES}}, + {"approach_paths": {"wash_3": SYNTHETIC_WASTE_ROUTE}}, + ): + with self.subTest(blocks=list(blocks)): + driver = self.make_driver( + link=FakeLink(answers=make_answers()), + declared_configuration_json=self.declare(arm_block=SYNTHETIC_ARM_BLOCK, **blocks), + ) + with self.assertRaises(MantisConfigurationError): + await driver.setup(skip_arm=True) + + if __name__ == "__main__": unittest.main() diff --git a/pylabrobot/formulatrix/mantis/driver/master.py b/pylabrobot/formulatrix/mantis/driver/master.py index f42a74691c3..ced690f75bb 100644 --- a/pylabrobot/formulatrix/mantis/driver/master.py +++ b/pylabrobot/formulatrix/mantis/driver/master.py @@ -49,7 +49,7 @@ decode_sequence_progress, describe_event, ) -from .features.arm import Arm +from .features.arm import Arm, MantisArmConfiguration from .features.led_bar import LedBar from .features.plate_clamp import UNCLAMPING_DONE, PlateClamp from .features.pump import Pump @@ -65,7 +65,15 @@ parse_frame, split_frames, ) -from .oem_config import _load_unit_configuration +from .oem_config import ( + UNKNOWN_ZONE, + WASH_APPROACH_PATH, + WASH_POSITIONS, + WASTE_APPROACH_PATH, + ZONES, + _load_unit_configuration, +) +from .sequence_files import RouteLine logger = logging.getLogger(__name__) @@ -810,7 +818,7 @@ async def _wait_for_queue_item(self, *, item: int, timeout: float) -> None: # -- discovery and setup -------------------------------------------------- async def _request_configuration(self) -> DeviceConfiguration: - """Read the firmware versions and the motor limits off the device. + """Read the firmware versions off the device. Only the motion board must answer. Each other board gets one attempt; one that times out, refuses or answers a malformed version is absent, not a fault. @@ -828,7 +836,6 @@ async def _request_configuration(self) -> DeviceConfiguration: return DeviceConfiguration( generation=compute_generation(firmware_versions=firmware_versions), firmware_versions=firmware_versions, - motor_limits={motor: await self.request_motor_limits(motor=motor) for motor in MOTORS}, ) def _get_arm_calibration(self) -> Dict[str, Any]: @@ -864,13 +871,111 @@ def _get_arm_calibration(self) -> Dict[str, Any]: ) return calibration - def _discover_features(self) -> None: + def _get_route_blocks(self) -> Dict[str, Any]: + """The ``zones`` and ``approach_paths`` blocks as the arm takes them, by argument; each None + without its block. + + Raises: + MantisConfigurationError: if a block is malformed: a zone missing, a rectangle not four + numbers, a route line not three numbers or None, or a route to safe XY that moves Z. + """ + + def to_route(lines: Any) -> List[RouteLine]: + route = [] + for line in lines: + x, y, z = (None if value is None else float(value) for value in line) + route.append((x, y, z)) + return route + + routes: Dict[str, Any] = { + "zone_rectangles": None, + "safe_xy_routes": None, + "idle_routes": None, + "approach_paths": None, + } + zones = self._unit_blocks.get("zones") + approach_paths = self._unit_blocks.get("approach_paths") + try: + if zones is not None: + rectangles = {} + for zone in ZONES: + minimum_x, maximum_x, minimum_y, maximum_y = (float(v) for v in zones["rectangles"][zone]) + rectangles[zone] = (minimum_x, maximum_x, minimum_y, maximum_y) + routes["zone_rectangles"] = rectangles + for field in ("safe_xy_routes", "idle_routes"): + routes[field] = {zone: to_route(zones[field][zone]) for zone in (*ZONES, UNKNOWN_ZONE)} + if approach_paths is not None: + routes["approach_paths"] = { + str(name): to_route(lines) for name, lines in approach_paths.items() + } + except (KeyError, TypeError, ValueError, AttributeError) as error: + raise MantisConfigurationError( + f"malformed zones or approach_paths block: {error!r}" + ) from error + if routes["safe_xy_routes"] is not None and any( + x is None or z is not None for route in routes["safe_xy_routes"].values() for x, _, z in route + ): + raise MantisConfigurationError("zones block: a route to safe XY moves the arms only") + known = {WASTE_APPROACH_PATH, *(WASH_APPROACH_PATH.format(position=p) for p in WASH_POSITIONS)} + if routes["approach_paths"] is not None and not set(routes["approach_paths"]) <= known: + raise MantisConfigurationError( + f"approach_paths block: the paths are {sorted(known)}, got {sorted(routes['approach_paths'])}" + ) + return routes + + def _get_stage_calibration(self) -> Dict[str, Any]: + """The ``stage`` block as the arm takes it, by argument; each None without the block. + + Raises: + MantisConfigurationError: if the block is malformed: a vector not three numbers or the + matrix not three rows of three. + """ + block = self._unit_blocks.get("stage") + if block is None: + return dict.fromkeys( + ( + "stage_origin", + "stage_transformation_matrix", + "stage_offset_x", + "stage_offset_y", + "arm_type_rotation_degree", + "arm_type_translation", + "stage_reference_height", + ) + ) + vector_fields = ("origin", "offset_x", "offset_y", "arm_type_translation") + try: + vectors = {field: tuple(float(value) for value in block[field]) for field in vector_fields} + matrix = tuple(tuple(float(value) for value in row) for row in block["transformation_matrix"]) + rotation = float(block["arm_type_rotation_degree"]) + reference_height = float(block["reference_height"]) + except (KeyError, TypeError, ValueError) as error: + raise MantisConfigurationError(f"malformed stage block: {error!r}") from error + if any(len(vector) != 3 for vector in vectors.values()): + raise MantisConfigurationError(f"stage block: {', '.join(vector_fields)} are x, y and z") + if len(matrix) != 3 or any(len(row) != 3 for row in matrix): + raise MantisConfigurationError("stage block: the transformation matrix is 3 by 3") + return { + "stage_origin": vectors["origin"], + "stage_transformation_matrix": matrix, + "stage_offset_x": vectors["offset_x"], + "stage_offset_y": vectors["offset_y"], + "arm_type_rotation_degree": rotation, + "arm_type_translation": vectors["arm_type_translation"], + "stage_reference_height": reference_height, + } + + def _discover_features(self, *, motor_limits: Dict[int, Tuple[float, float]]) -> None: """Build the features the configuration says are fitted, with the unit's values. A feature already built is kept. + Args: + motor_limits: each motor's (lower, upper) limit, by motor, read for the arm. + Raises: - MantisConfigurationError: if the ``plate_clamp``, ``led_bar`` or ``arm`` block is malformed. + MantisConfigurationError: if the ``plate_clamp``, ``led_bar``, ``arm``, ``zones``, + ``approach_paths`` or ``stage`` block is malformed. """ if self.configuration.firmware_versions.get(LC3_BOARD) is not None and self.plate_clamp is None: levels: Optional[Dict[int, Tuple[float, int]]] = None @@ -906,7 +1011,15 @@ def _discover_features(self) -> None: ) sensor_board = self.configuration.firmware_versions.get(ABSOLUTE_POSITION_SENSOR_BOARD) if sensor_board is not None and self.arm is None: - self.arm = Arm(driver=self, **self._get_arm_calibration()) + self.arm = Arm( + driver=self, + configuration=MantisArmConfiguration( + motor_limits=motor_limits, + **self._get_arm_calibration(), + **self._get_route_blocks(), + **self._get_stage_calibration(), + ), + ) def _show_action( self, *, action: str, show_inner_actions: bool = True, only_when_idle: bool = False @@ -1028,7 +1141,13 @@ async def discover(self) -> None: self._unit_blocks_from_recording = { block for block in UNIT_BLOCKS if taken.get(block) in recordings } - self._discover_features() + motor_limits: Dict[int, Tuple[float, float]] = {} + if ( + self.configuration.firmware_versions.get(ABSOLUTE_POSITION_SENSOR_BOARD) is not None + and self.arm is None + ): + motor_limits = {motor: await self.request_motor_limits(motor=motor) for motor in MOTORS} + self._discover_features(motor_limits=motor_limits) if self.pump is not None: # Nothing uses the calibration yet: a board 10 that does not answer it does not fail setup. try: diff --git a/pylabrobot/formulatrix/mantis/driver/master_tests.py b/pylabrobot/formulatrix/mantis/driver/master_tests.py index 2a68cce890d..3a1446c68ac 100644 --- a/pylabrobot/formulatrix/mantis/driver/master_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/master_tests.py @@ -293,10 +293,15 @@ async def test_v4_discovery(self): DeviceConfiguration( generation="v4", firmware_versions={0: "4.20.6.2", 10: "1.0.8", 11: "0.1.7 DBG", 12: "0.1.8 DBG"}, - motor_limits={0: (-4000.0, 4000.0), 1: (-4000.0, 4000.0), 2: (-160.0, 2300.0)}, feedback_sensor_parameters=RECORDED_CALIBRATION, ), ) + # The motor limits are the arm's, read for it. + assert driver.arm is not None + self.assertEqual( + driver.arm.configuration.motor_limits, + {0: (-4000.0, 4000.0), 1: (-4000.0, 4000.0), 2: (-160.0, 2300.0)}, + ) async def test_only_board_0_must_answer_and_each_other_board_is_asked_once(self): self.default_read_timeout = 0.25 # boards 10 to 12 never answer diff --git a/pylabrobot/formulatrix/mantis/driver/oem_config.py b/pylabrobot/formulatrix/mantis/driver/oem_config.py index 4ee166aca98..c6527beccdc 100644 --- a/pylabrobot/formulatrix/mantis/driver/oem_config.py +++ b/pylabrobot/formulatrix/mantis/driver/oem_config.py @@ -1,18 +1,40 @@ -"""A Mantis unit's own values, read from the ``.config`` files of its vendor install. +"""A Mantis unit's own values, read from the unit's ``.config`` and sequence files. Only the values set per unit are read: a block is loaded only where its source applies to the unit. """ +import json import os import re import xml.etree.ElementTree as ElementTree -from typing import Any, Dict, List +from typing import Any, Dict, List, Optional from .errors import MantisConfigurationError +from .sequence_files import RouteLine, parse_move_sequence # Where a v4 install keeps each arm motor's absolute encoder correction, from its ``Data`` folder. ENCODER_CORRECTION_DIRECTORY = os.path.join("System", "Encoder Correction", "Sampling File") ENCODER_CORRECTION_FILE = "OneRevolutionSamplingData_Motor{motor}.csv" +# Where the unit's sequences that move the arm sit, below its ``Data`` folder. +COMMON_SEQUENCE_DIRECTORY = os.path.join("System", "Sequences", "Common") +# The unit's zones, in the order it lists them; a head in none of them is in the Unknown zone. +ZONES = ("Front", "RightBack", "RightMiddle", "RightFront", "LeftBack", "LeftMiddle", "LeftFront") +UNKNOWN_ZONE = "Unknown" +# The unit's approach path to the waste on a v4, and on a v3: every unit seen has both files. +WASTE_APPROACH_PATH_FILE = "MoveToWasteStation.seq.txt" +WASTE_APPROACH_PATH_ADAPTER_FILE = "MoveToWasteStation_Adapter.seq.txt" +# The unit's approach path to each wash position, by number. +WASH_APPROACH_PATH_FILE = "MoveToWashStation{position}.seq.txt" +WASH_POSITIONS = (1, 2) +# The approach paths' names: the waste, and each wash position. +WASTE_APPROACH_PATH = "waste" +WASH_APPROACH_PATH = "wash_{position}" +# Where an install keeps the plate holder's origin, from its ``Data/Device`` folder. +SBS_ADAPTER_FILE = os.path.join("Adapters", "SBS Adapter.ad.txt") +# The stage tuning file in the unit's configuration folder: its ReferenceHeight. +STAGE_TUNING_FILE = "StageTuning.config" +# The only arm type on every unit seen; how the stage is framed for another is unknown. +STANDARD_ARM_TYPE = "Standard" def _parse_config_entries(*, path: str) -> Dict[str, str]: @@ -35,6 +57,137 @@ def _parse_config_entries(*, path: str) -> Dict[str, str]: } +def _get_number(*, path: str, key: str, text: str) -> float: + """A setting's value as a number. + + Raises: + MantisConfigurationError: if it is not one. + """ + try: + return float(text) + except ValueError as error: + raise MantisConfigurationError(f"{path}: {key} is not a number: {text!r}") from error + + +def _get_setting(*, path: str, settings: Dict[str, str], key: str) -> str: + """A setting's text. + + Raises: + MantisConfigurationError: if the file has no such key. + """ + if key not in settings: + raise MantisConfigurationError(f"{path} has no {key}") + return settings[key] + + +def _parse_vector(*, path: str, key: str, text: str) -> List[float]: + """A vector written ``X Y Z ``, as the unit's files write one. + + Raises: + MantisConfigurationError: if the text is not one, or a value is not a number. + """ + match = re.fullmatch(r"\s*X\s+(\S+)\s+Y\s+(\S+)\s+Z\s+(\S+)\s*", text) + if match is None: + raise MantisConfigurationError(f"{path}: {key} is not 'X Y Z ': {text!r}") + return [_get_number(path=path, key=key, text=value) for value in match.groups()] + + +def _parse_adapter_origin(*, path: str) -> List[float]: + """The plate holder's origin, (x, y, z) in mm, from an adapter file read whole. + + Raises: + MantisConfigurationError: if the file is missing, of a version other than 1, a line is not + understood, or it does not hold one origin. + """ + if not os.path.isfile(path): + raise MantisConfigurationError(f"the plate holder's origin is missing: {path}") + origins = [] + with open(path, encoding="utf-8-sig") as f: + for number, line in enumerate(f, start=1): + text = line.strip() + if not text: + continue + version = re.fullmatch(r"\[\s*Version:\s*(\S+)\s*\]", text) + if version is not None: + if version.group(1) != "1": + raise MantisConfigurationError( + f"{path}: adapter file version {version.group(1)}; only version 1 is known" + ) + continue + origins.append(_parse_vector(path=path, key=f"line {number}", text=text)) + if len(origins) != 1: + raise MantisConfigurationError(f"{path}: one origin expected, found {len(origins)}") + return origins[0] + + +def _parse_transformation_matrix(*, path: str, text: str) -> List[List[float]]: + """``MicroplateTransformationMatrix``: three rows of three numbers, written as JSON. + + Raises: + MantisConfigurationError: if the text is not one. + """ + try: + matrix = json.loads(text) + except ValueError as error: + raise MantisConfigurationError(f"{path}: MicroplateTransformationMatrix is not JSON") from error + if ( + not isinstance(matrix, list) + or len(matrix) != 3 + or any(not isinstance(row, list) or len(row) != 3 for row in matrix) + or any( + isinstance(value, bool) or not isinstance(value, (int, float)) + for row in matrix + for value in row + ) + ): + raise MantisConfigurationError(f"{path}: MicroplateTransformationMatrix is not 3 by 3: {text}") + return [[float(value) for value in row] for row in matrix] + + +def _load_stage( + *, path: str, values: Dict[str, str], adapter_path: str, stage_tuning_path: str +) -> Dict[str, Any]: + """The ``stage`` block: the plate holder's origin, ``Device.config``'s stage transform and + ``StageTuning.config``'s reference height. + + Raises: + MantisConfigurationError: if a key, the adapter file or the stage tuning file is missing or + malformed, or the arm type is not ``STANDARD_ARM_TYPE``. + """ + if not os.path.isfile(stage_tuning_path): + raise MantisConfigurationError(f"{stage_tuning_path} is missing: the stage's reference height") + stage_tuning = _parse_config_entries(path=stage_tuning_path) + arm_type = _get_setting(path=path, settings=values, key="ArmType") + if arm_type != STANDARD_ARM_TYPE: + raise MantisConfigurationError( + f"{path}: ArmType {arm_type!r}; only {STANDARD_ARM_TYPE!r} is supported" + ) + + def get_vector(key: str) -> List[float]: + return _parse_vector(path=path, key=key, text=_get_setting(path=path, settings=values, key=key)) + + return { + "origin": _parse_adapter_origin(path=adapter_path), + "transformation_matrix": _parse_transformation_matrix( + path=path, + text=_get_setting(path=path, settings=values, key="MicroplateTransformationMatrix"), + ), + "offset_x": get_vector(key="MicroplateOffsetX"), + "offset_y": get_vector(key="MicroplateOffsetY"), + "arm_type_rotation_degree": _get_number( + path=path, + key="ArmTypeRotationDegree", + text=_get_setting(path=path, settings=values, key="ArmTypeRotationDegree"), + ), + "arm_type_translation": get_vector(key="ArmTypeTranslation"), + "reference_height": _get_number( + path=stage_tuning_path, + key="ReferenceHeight", + text=_get_setting(path=stage_tuning_path, settings=stage_tuning, key="ReferenceHeight"), + ), + } + + def _parse_correction_table(*, path: str) -> List[List[float]]: """The (ideal, measured) rows of one encoder correction file, its third column dropped. @@ -56,6 +209,81 @@ def _parse_correction_table(*, path: str) -> List[List[float]]: return rows +def _parse_route(*, path: str) -> List[List[Optional[float]]]: + """A route's lines, as a declared configuration holds them. + + Raises: + MantisConfigurationError: if the file is missing, malformed or moves nothing. + """ + if not os.path.isfile(path): + raise MantisConfigurationError(f"a route is missing: {path}") + route: List[RouteLine] = parse_move_sequence(path=path) + if all(route_line == (None, None, None) for route_line in route): + raise MantisConfigurationError(f"{path} moves nothing") + return [list(route_line) for route_line in route] + + +def _load_zones(*, path: str, sequence_directory: str) -> Dict[str, Any]: + """The ``zones`` block: each zone's rectangle in the arm frame, and its routes to safe XY and + to idle; the Unknown zone has routes and no rectangle. + + Raises: + MantisConfigurationError: if a rectangle or a route is missing or malformed, or a route to + safe XY moves anything but the arms. + """ + values = _parse_config_entries(path=path) + rectangles: Dict[str, List[float]] = {} + for zone in ZONES: + rectangle = [ + _get_number( + path=path, + key=f"{zone}Zone{bound}", + text=_get_setting(path=path, settings=values, key=f"{zone}Zone{bound}"), + ) + for bound in ("MinimumX", "MaximumX", "MinimumY", "MaximumY") + ] + if not (rectangle[0] < rectangle[1] and rectangle[2] < rectangle[3]): + raise MantisConfigurationError( + f"{path}: the {zone} zone's minimums are not below its maximums" + ) + rectangles[zone] = rectangle + routes: Dict[str, Dict[str, List[List[Optional[float]]]]] = {} + for field, key in ( + ("safe_xy_routes", "SafeMoveToSafeXYPositionFrom{zone}ZoneSequenceFile"), + ("idle_routes", "SafeMoveToIdleFrom{zone}ZoneSequenceFile"), + ): + routes[field] = {} + for zone in (*ZONES, UNKNOWN_ZONE): + file_name = _get_setting(path=path, settings=values, key=key.format(zone=zone)) + route = _parse_route(path=os.path.join(sequence_directory, file_name)) + if field == "safe_xy_routes" and any(x is None or z is not None for x, _, z in route): + raise MantisConfigurationError(f"{file_name}: a route to safe XY moves the arms only") + routes[field][zone] = route + return {"rectangles": rectangles, **routes} + + +def _load_approach_paths(*, sequence_directory: str, waste_file: str) -> Dict[str, Any]: + """The ``approach_paths`` block: the unit's path from idle to the waste and to each wash + position, by name (``WASTE_APPROACH_PATH``, ``WASH_APPROACH_PATH``); only those whose file exists. + + Args: + sequence_directory: the unit's ``System/Sequences/Common``. + waste_file: ``WASTE_APPROACH_PATH_FILE`` on a v4, ``WASTE_APPROACH_PATH_ADAPTER_FILE`` on a v3. + + Raises: + MantisConfigurationError: if a path is malformed or moves nothing. + """ + paths: Dict[str, Any] = {} + path = os.path.join(sequence_directory, waste_file) + if os.path.isfile(path): + paths[WASTE_APPROACH_PATH] = _parse_route(path=path) + for position in WASH_POSITIONS: + path = os.path.join(sequence_directory, WASH_APPROACH_PATH_FILE.format(position=position)) + if os.path.isfile(path): + paths[WASH_APPROACH_PATH.format(position=position)] = _parse_route(path=path) + return paths + + def _load_unit_configuration(*, oem_config_directory: str) -> Dict[str, Any]: """The unit's blocks, as declared configurations hold them, from its vendor ``.config`` files. @@ -67,41 +295,41 @@ def _load_unit_configuration(*, oem_config_directory: str) -> Dict[str, Any]: ``arm``: each arm motor's ``Offset`` and ``HomingPositionA`` from ``RobotArm.config`` and its encoder correction file from the ``Data`` folder two levels up, only where ``HomingPositionA`` is set (a v4). Motor 0 is ``Motor.1`` there. + ``zones``: ``DeviceZone.config``'s rectangles and each zone's routes to safe XY and to idle, + from the unit's ``System/Sequences/Common``, only where that file exists (a v4). + ``approach_paths``: the unit's paths from idle to the waste and to wash positions 1 and 2, from + the same folder, those that exist; the waste's ``WASTE_APPROACH_PATH_FILE`` where + ``DeviceZone.config`` exists (a v4), else ``WASTE_APPROACH_PATH_ADAPTER_FILE`` (a v3). + ``stage``: ``Device.config``'s ``MicroplateTransformationMatrix``, ``MicroplateOffsetX/Y`` and + ``ArmType*``, and the plate holder's origin from ``Data/Device/Adapters/SBS Adapter.ad.txt``, + only where ``Device.config`` sets the matrix. Args: oem_config_directory: the unit's ``Data/Device/Configs`` folder. Raises: MantisConfigurationError: if ``Device.config`` is missing, a block's key is missing or not a - number, or an arm motor's encoder correction file is missing or malformed. + number, or an arm motor's encoder correction file, a route or the adapter file is missing + or malformed. """ device_config = os.path.join(oem_config_directory, "Device.config") if not os.path.isfile(device_config): raise MantisConfigurationError(f"{oem_config_directory} has no Device.config") values = _parse_config_entries(path=device_config) - def number(*, path: str, key: str, text: str) -> float: - try: - return float(text) - except ValueError as error: - raise MantisConfigurationError(f"{path}: {key} is not a number: {text!r}") from error - - def setting(*, path: str, settings: Dict[str, str], key: str) -> str: - if key not in settings: - raise MantisConfigurationError(f"{path} has no {key}") - return settings[key] - blocks: Dict[str, Any] = {} if values.get("PlateClampController") == "MotorDC": pwms = [ - number(path=device_config, key="PlateClampInisiatePWM", text=text) - for text in setting(path=device_config, settings=values, key="PlateClampInisiatePWM").split( - "," - ) + _get_number(path=device_config, key="PlateClampInisiatePWM", text=text) + for text in _get_setting( + path=device_config, settings=values, key="PlateClampInisiatePWM" + ).split(",") ] timeouts_ms = [ - int(number(path=device_config, key="PlateClampTimer", text=text)) - for text in setting(path=device_config, settings=values, key="PlateClampTimer").split(",") + int(_get_number(path=device_config, key="PlateClampTimer", text=text)) + for text in _get_setting(path=device_config, settings=values, key="PlateClampTimer").split( + "," + ) ] if len(pwms) != len(timeouts_ms): raise MantisConfigurationError( @@ -117,7 +345,7 @@ def setting(*, path: str, settings: Dict[str, str], key: str) -> str: if "NumberOfLed" in values: blocks["led_bar"] = { "number_of_leds": int( - number(path=device_config, key="NumberOfLed", text=values["NumberOfLed"]) + _get_number(path=device_config, key="NumberOfLed", text=values["NumberOfLed"]) ) } @@ -129,18 +357,30 @@ def setting(*, path: str, settings: Dict[str, str], key: str) -> str: match = re.fullmatch(r"RFIDAntennaId\.ACC(\d+)", key) if match is not None: slot = int(match.group(1)) - slot_to_antenna[str(slot)] = int(number(path=rfid_config, key=key, text=rfid_values[key])) + slot_to_antenna[str(slot)] = int( + _get_number(path=rfid_config, key=key, text=rfid_values[key]) + ) if not slot_to_antenna: raise MantisConfigurationError(f"{rfid_config} has no RFIDAntennaId.ACC") blocks["rfid"] = { "slot_to_antenna": dict(sorted(slot_to_antenna.items(), key=lambda item: int(item[0]))), - "multiplexer_delay_raw": number( + "multiplexer_delay_raw": _get_number( path=device_config, key="RfidMultiplexerDelay", - text=setting(path=device_config, settings=values, key="RfidMultiplexerDelay"), + text=_get_setting(path=device_config, settings=values, key="RfidMultiplexerDelay"), ), } + if "MicroplateTransformationMatrix" in values: + blocks["stage"] = _load_stage( + path=device_config, + values=values, + adapter_path=os.path.join( + os.path.dirname(os.path.normpath(oem_config_directory)), SBS_ADAPTER_FILE + ), + stage_tuning_path=os.path.join(oem_config_directory, STAGE_TUNING_FILE), + ) + robot_arm_config = os.path.join(oem_config_directory, "RobotArm.config") arm_values = ( _parse_config_entries(path=robot_arm_config) if os.path.isfile(robot_arm_config) else {} @@ -158,14 +398,32 @@ def setting(*, path: str, settings: Dict[str, str], key: str) -> str: correction[str(motor)] = _parse_correction_table(path=path) blocks["arm"] = { field: { - str(motor): number( + str(motor): _get_number( path=robot_arm_config, key=f"Motor.{motor + 1}.{key}", - text=setting(path=robot_arm_config, settings=arm_values, key=f"Motor.{motor + 1}.{key}"), + text=_get_setting( + path=robot_arm_config, settings=arm_values, key=f"Motor.{motor + 1}.{key}" + ), ) for motor in (0, 1) } for field, key in (("offsets", "Offset"), ("homing_position_a", "HomingPositionA")) } blocks["arm"]["encoder_correction"] = correction + + # The routes, from the unit's sequences: the zones only where DeviceZone.config exists (a v4). + sequence_directory = os.path.join( + os.path.dirname(os.path.dirname(os.path.normpath(oem_config_directory))), + COMMON_SEQUENCE_DIRECTORY, + ) + zone_config = os.path.join(oem_config_directory, "DeviceZone.config") + is_v4 = os.path.isfile(zone_config) + if is_v4: + blocks["zones"] = _load_zones(path=zone_config, sequence_directory=sequence_directory) + approach_paths = _load_approach_paths( + sequence_directory=sequence_directory, + waste_file=WASTE_APPROACH_PATH_FILE if is_v4 else WASTE_APPROACH_PATH_ADAPTER_FILE, + ) + if approach_paths: + blocks["approach_paths"] = approach_paths return blocks diff --git a/pylabrobot/formulatrix/mantis/driver/oem_config_tests.py b/pylabrobot/formulatrix/mantis/driver/oem_config_tests.py index 9d062af282d..e6a5bfd9975 100644 --- a/pylabrobot/formulatrix/mantis/driver/oem_config_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/oem_config_tests.py @@ -5,14 +5,20 @@ import os import tempfile import unittest -from typing import Dict +from typing import Any, Dict, List, Optional, Tuple from pylabrobot.formulatrix.mantis.driver.configuration import RECORDING_MANTIS_V4 from pylabrobot.formulatrix.mantis.driver.errors import MantisConfigurationError from pylabrobot.formulatrix.mantis.driver.master_tests import DriverTestCase, FakeLink, make_answers from pylabrobot.formulatrix.mantis.driver.oem_config import ( + COMMON_SEQUENCE_DIRECTORY, ENCODER_CORRECTION_DIRECTORY, ENCODER_CORRECTION_FILE, + SBS_ADAPTER_FILE, + UNKNOWN_ZONE, + WASTE_APPROACH_PATH_ADAPTER_FILE, + WASTE_APPROACH_PATH_FILE, + ZONES, _load_unit_configuration, _parse_config_entries, ) @@ -64,6 +70,29 @@ ) +# A synthetic stage: the plate holder's origin, a projective matrix and a tilt. +SYNTHETIC_STAGE_BLOCK: Dict[str, Any] = { + "origin": [-50.0, 40.0, 51.0], + "transformation_matrix": [[1.001, 0.001, -0.08], [-0.004, 0.9995, 0.09], [-2e-05, -3e-05, 1.004]], + "offset_x": [0.0, 0.0, -0.0015], + "offset_y": [0.0, 0.0, 0.0025], + "arm_type_rotation_degree": 0.0, + "arm_type_translation": [0.0, 0.0, 0.0], + "reference_height": 9.665, +} +# The synthetic stage in the layout of a v4 install's Device.config and SBS Adapter.ad.txt. +SYNTHETIC_STAGE_DEVICE_CONFIG_LINES = ( + ' \r\n' + ' \r\n' + ' \r\n' + ' \r\n' + ' \r\n' + ' \r\n' +) +SYNTHETIC_SBS_ADAPTER = "[ Version: 1 ]\r\nX -50.000 Y 40.000 Z 51.000\r\n" + + # RobotArm.config lines in a v4 install's layout, with synthetic offsets and references. SYNTHETIC_V4_ROBOT_ARM_CONFIG = ( HEAD @@ -96,6 +125,127 @@ ) +# Synthetic zones in DeviceZone.config's layout: (minimum x, maximum x, minimum y, maximum y), mm. +SYNTHETIC_ZONE_RECTANGLES = { + "Front": [-10.0, 40.0, 30.0, 150.0], + "RightBack": [40.0, 190.0, -90.0, -30.0], + "RightMiddle": [40.0, 190.0, -30.0, 60.0], + "RightFront": [40.0, 190.0, 60.0, 150.0], + "LeftBack": [-160.0, -10.0, -90.0, -30.0], + "LeftMiddle": [-160.0, -10.0, -30.0, 60.0], + "LeftFront": [-160.0, -10.0, 60.0, 150.0], +} +SYNTHETIC_IDLE = [15.0, 31.177] +# Synthetic routes: each zone's safe XY, then to idle through it; the Front zone's and the Unknown +# zone's in the shape every install has, the Unknown zone's straight to idle. +SYNTHETIC_SAFE_XY = { + "Front": SYNTHETIC_IDLE, + "RightBack": [90.0, -50.0], + "RightMiddle": [80.0, 0.0], + "RightFront": [60.0, 70.0], + "LeftBack": [-80.0, -50.0], + "LeftMiddle": [-60.0, -10.0], + "LeftFront": [-35.0, 70.0], + UNKNOWN_ZONE: SYNTHETIC_IDLE, +} +SYNTHETIC_SAFE_XY_ROUTES = {zone: [[*xy, None]] for zone, xy in SYNTHETIC_SAFE_XY.items()} +SYNTHETIC_IDLE_ROUTES: Dict[str, List[List[Optional[float]]]] = { + zone: [[*xy, None], [*xy, 0.0], [*SYNTHETIC_IDLE, 0.0]] for zone, xy in SYNTHETIC_SAFE_XY.items() +} +SYNTHETIC_IDLE_ROUTES["Front"] = [ + [None, None, 0.0], + [*SYNTHETIC_IDLE, None], + [*SYNTHETIC_IDLE, 0.0], +] +SYNTHETIC_IDLE_ROUTES[UNKNOWN_ZONE] = [[*SYNTHETIC_IDLE, 0.0]] +SYNTHETIC_ZONES_BLOCK = { + "rectangles": SYNTHETIC_ZONE_RECTANGLES, + "safe_xy_routes": SYNTHETIC_SAFE_XY_ROUTES, + "idle_routes": SYNTHETIC_IDLE_ROUTES, +} +# Synthetic approach paths from idle, in the shape of a v4 install's: the waste's starts with a line +# naming no axis, the ready position and the safe height, Z -1.570; the washes share all but the end. +SYNTHETIC_WASTE_ROUTE: List[List[Optional[float]]] = [ + [None, None, None], + [15.0, 30.0, None], + [None, None, -1.57], + [15.0, 30.0, -1.57], + [None, None, -1.57], + [-50.0, 20.0, -1.57], + [None, None, 30.0], + [-60.0, 10.0, 30.0], + [-60.0, 10.0, 35.0], +] +SYNTHETIC_WASH_ROUTE: List[List[Optional[float]]] = [ + [None, None, -1.57], + [60.0, 40.0, -1.57], + [85.0, 0.0, -1.57], + [None, None, 30.0], + [None, None, 40.0], + [75.0, -50.0, 40.0], + [75.0, -50.0, 45.0], +] +# The other wash: the same path to 30 mm down, then its own safe Z, safe-Z line and bottom. +SYNTHETIC_WASH_2_ROUTE: List[List[Optional[float]]] = [ + *SYNTHETIC_WASH_ROUTE[:4], + [None, None, 40.5], + [90.0, -50.0, 40.5], + [90.0, -50.0, 45.5], +] +SYNTHETIC_APPROACH_PATHS_BLOCK = { + "waste": SYNTHETIC_WASTE_ROUTE, + "wash_1": SYNTHETIC_WASH_ROUTE, + "wash_2": SYNTHETIC_WASH_2_ROUTE, +} + + +def get_route_text(*, route: List[List[Optional[float]]]) -> str: + """A route as a sequence file writes it.""" + lines = [] + for route_line in route: + axes = "".join( + f"{axis} {value:.3f} " for axis, value in zip("XYZ", route_line) if value is not None + ) + lines.append(f"MoveSequenceItem %00 True {axes} 0\r\n") + return "".join(lines) + + +def get_zone_config(*, rectangles: Dict[str, List[float]]) -> str: + """A DeviceZone.config naming each zone's route files.""" + entries = [ + f'\t\r\n' + for zone, rectangle in rectangles.items() + for bound, value in zip(("MinimumX", "MaximumX", "MinimumY", "MaximumY"), rectangle) + ] + for family in ("SafeMoveToIdleFrom", "SafeMoveToSafeXYPositionFrom"): + for zone in (*ZONES, UNKNOWN_ZONE): + entries.append( + f'\t' + "\r\n" + ) + return HEAD + "".join(entries) + TAIL + + +def get_route_files(*, zones: Dict[str, Any], approach_paths: Dict[str, Any]) -> Dict[str, str]: + """The route files of an install's System/Sequences/Common folder, by file name.""" + files = {} + for family, field in ( + ("SafeMoveToIdleFrom", "idle_routes"), + ("SafeMoveToSafeXYPositionFrom", "safe_xy_routes"), + ): + for zone, route in zones[field].items(): + files[f"{family}{zone}ZoneSequence.seq.txt"] = get_route_text(route=route) + for name, route in approach_paths.items(): + if name == "waste": + files[WASTE_APPROACH_PATH_FILE] = ( + "RemarkSequenceItem Move%32To%32Waste%32Station \r\n" + get_route_text(route=route) + ) + else: + position = name.split("_")[1] + files[f"MoveToWashStation{position}.seq.txt"] = get_route_text(route=route) + return files + + def write_files(*, directory: str, files: Dict[str, str]) -> str: for file_name, text in files.items(): with open(os.path.join(directory, file_name), "w", encoding="utf-8", newline="") as f: @@ -207,6 +357,187 @@ def test_a_correction_that_does_not_ascend_refuses(self): ) +def write_zone_install( + *, + directory: str, + zones: Dict[str, Any], + approach_paths: Dict[str, Any], + zone_config: bool = True, +) -> str: + """Lay out a v4 install's Data folder with zones and approach paths; return its Device/Configs.""" + configs = {"Device.config": V4_DEVICE_CONFIG} + if zone_config: + configs["DeviceZone.config"] = get_zone_config(rectangles=zones["rectangles"]) + configs_directory = write_install(directory=directory, configs=configs, corrections={}) + sequence_directory = os.path.join(directory, "Data", COMMON_SEQUENCE_DIRECTORY) + os.makedirs(sequence_directory) + write_files( + directory=sequence_directory, + files=get_route_files(zones=zones, approach_paths=approach_paths), + ) + return configs_directory + + +class RouteBlockTests(unittest.TestCase): + def load( + self, *, zones=SYNTHETIC_ZONES_BLOCK, approach_paths=SYNTHETIC_APPROACH_PATHS_BLOCK, **kwargs + ): + with tempfile.TemporaryDirectory() as directory: + return _load_unit_configuration( + oem_config_directory=write_zone_install( + directory=directory, zones=zones, approach_paths=approach_paths, **kwargs + ) + ) + + def test_a_v4_install_gives_its_zones_and_approach_paths(self): + blocks = self.load() + self.assertEqual(blocks["zones"], SYNTHETIC_ZONES_BLOCK) + self.assertEqual(blocks["approach_paths"], SYNTHETIC_APPROACH_PATHS_BLOCK) + + def test_without_device_zone_config_there_are_no_zones(self): + blocks = self.load(zone_config=False) + self.assertNotIn("zones", blocks) + # A v3 does not read the v4's waste approach path. + self.assertEqual( + blocks["approach_paths"], + {"wash_1": SYNTHETIC_WASH_ROUTE, "wash_2": SYNTHETIC_WASH_2_ROUTE}, + ) + + def test_each_generation_reads_its_own_waste_approach_path(self): + # Every unit seen has both files: a v4 (with DeviceZone.config) reads one, a v3 the other. + for zone_config, expected in ((True, SYNTHETIC_WASTE_ROUTE), (False, SYNTHETIC_WASH_ROUTE)): + with self.subTest(zone_config=zone_config): + with tempfile.TemporaryDirectory() as directory: + configs_directory = write_zone_install( + directory=directory, + zones=SYNTHETIC_ZONES_BLOCK, + approach_paths={"waste": SYNTHETIC_WASTE_ROUTE}, + zone_config=zone_config, + ) + write_files( + directory=os.path.join(directory, "Data", COMMON_SEQUENCE_DIRECTORY), + files={WASTE_APPROACH_PATH_ADAPTER_FILE: get_route_text(route=SYNTHETIC_WASH_ROUTE)}, + ) + blocks = _load_unit_configuration(oem_config_directory=configs_directory) + self.assertEqual(blocks["approach_paths"], {"waste": expected}) + + def test_a_missing_route_refuses(self): + zones = { + **SYNTHETIC_ZONES_BLOCK, + "idle_routes": { + zone: route for zone, route in SYNTHETIC_IDLE_ROUTES.items() if zone != "LeftBack" + }, + } + with self.assertRaisesRegex(MantisConfigurationError, "a route is missing"): + self.load(zones=zones) + + def test_a_route_to_safe_xy_that_moves_z_refuses(self): + zones = { + **SYNTHETIC_ZONES_BLOCK, + "safe_xy_routes": {**SYNTHETIC_SAFE_XY_ROUTES, "LeftBack": [[-80.0, -50.0, 0.0]]}, + } + with self.assertRaisesRegex(MantisConfigurationError, "moves the arms only"): + self.load(zones=zones) + + def test_a_rectangle_that_is_missing_or_inverted_refuses(self): + rectangles = {**SYNTHETIC_ZONE_RECTANGLES, "Front": [40.0, -10.0, 30.0, 150.0]} + with self.assertRaisesRegex(MantisConfigurationError, "minimums are not below"): + self.load(zones={**SYNTHETIC_ZONES_BLOCK, "rectangles": rectangles}) + rectangles = {zone: values for zone, values in SYNTHETIC_ZONE_RECTANGLES.items()} + del rectangles["LeftFront"] + with self.assertRaisesRegex(MantisConfigurationError, "has no LeftFrontZoneMinimumX"): + self.load(zones={**SYNTHETIC_ZONES_BLOCK, "rectangles": rectangles}) + + +# A unit's StageTuning.config, in the layout of a v4 install. +SYNTHETIC_STAGE_TUNING = ( + '\r\n\r\n \r\n' + ' \r\n \r\n' + " \r\n\r\n" +) + + +def write_stage_install( + *, + directory: str, + device_lines: str, + adapter: Optional[str], + stage_tuning: Optional[str] = SYNTHETIC_STAGE_TUNING, +) -> str: + """Lay out an install's Device.config, StageTuning.config and SBS Adapter.ad.txt; return its + Device/Configs.""" + configs = {"Device.config": V4_DEVICE_CONFIG.replace(TAIL, device_lines + TAIL)} + if stage_tuning is not None: + configs["StageTuning.config"] = stage_tuning + configs_directory = write_install(directory=directory, configs=configs, corrections={}) + if adapter is not None: + adapter_path = os.path.join(os.path.dirname(configs_directory), SBS_ADAPTER_FILE) + os.makedirs(os.path.dirname(adapter_path)) + with open(adapter_path, "w", encoding="utf-8", newline="") as f: + f.write(adapter) + return configs_directory + + +class StageBlockTests(unittest.TestCase): + def load( + self, + *, + device_lines=SYNTHETIC_STAGE_DEVICE_CONFIG_LINES, + adapter=SYNTHETIC_SBS_ADAPTER, + stage_tuning=SYNTHETIC_STAGE_TUNING, + ): + with tempfile.TemporaryDirectory() as directory: + return _load_unit_configuration( + oem_config_directory=write_stage_install( + directory=directory, device_lines=device_lines, adapter=adapter, stage_tuning=stage_tuning + ) + ) + + def test_an_install_gives_the_holders_origin_and_the_stage_transform(self): + self.assertEqual(self.load()["stage"], SYNTHETIC_STAGE_BLOCK) + + def test_without_the_matrix_there_is_no_stage(self): + lines = SYNTHETIC_STAGE_DEVICE_CONFIG_LINES.split("\r\n") + self.assertNotIn("stage", self.load(device_lines="\r\n".join(lines[:5]) + "\r\n")) + + def test_a_missing_or_malformed_part_refuses(self): + cases: List[Tuple[Dict[str, Any], str]] = [ + ({"adapter": None}, "the plate holder's origin is missing"), + ({"stage_tuning": None}, "StageTuning.config is missing: the stage's reference height"), + ( + {"stage_tuning": SYNTHETIC_STAGE_TUNING.replace("ReferenceHeight", "Other")}, + "has no ReferenceHeight", + ), + ({"adapter": SYNTHETIC_SBS_ADAPTER + "X 1 Y 2 Z 3\r\n"}, "one origin expected, found 2"), + ({"adapter": "[ Version: 2 ]\r\nX 1 Y 2 Z 3\r\n"}, "version 2; only version 1 is known"), + ({"adapter": "Origin 1 2 3\r\n"}, "is not 'X Y Z '"), + ( + {"device_lines": SYNTHETIC_STAGE_DEVICE_CONFIG_LINES.replace("Standard", "Other")}, + "ArmType 'Other'; only 'Standard'", + ), + ( + {"device_lines": SYNTHETIC_STAGE_DEVICE_CONFIG_LINES.replace(",1.004]", "]")}, + "is not 3 by 3", + ), + ( + {"device_lines": SYNTHETIC_STAGE_DEVICE_CONFIG_LINES.replace("Z -0.0015", "-0.0015")}, + "MicroplateOffsetX is not 'X Y Z '", + ), + ( + { + "device_lines": SYNTHETIC_STAGE_DEVICE_CONFIG_LINES.replace( + ' \r\n', "" + ) + }, + "has no ArmTypeRotationDegree", + ), + ] + for kwargs, message in cases: + with self.subTest(message=message): + with self.assertRaisesRegex(MantisConfigurationError, message): + self.load(**kwargs) + + class UnitBlockSourceTests(DriverTestCase): async def set_up_with(self, **kwargs): return await self.set_up_driver(link=FakeLink(answers=make_answers()), **kwargs) @@ -276,9 +607,11 @@ async def test_an_older_declared_file_takes_what_it_lacks_from_the_folder(self): driver = await self.set_up_with( oem_config_directory=folder, declared_configuration_json=path ) - self.assertEqual(len(logs.output), 1) - self.assertIn(f"not in {path}: led_bar from {folder}", logs.output[0]) - self.assertNotIn("less accurate", logs.output[0]) + # Only the source warning counts: an unanswered LED write may warn too on a slow runner. + sources = [line for line in logs.output if f"not in {path}" in line] + self.assertEqual(len(sources), 1) + self.assertIn(f"not in {path}: led_bar from {folder}", sources[0]) + self.assertNotIn("less accurate", sources[0]) assert driver.led_bar is not None self.assertEqual(driver.led_bar.number_of_leds, 24) diff --git a/pylabrobot/formulatrix/mantis/driver/recordings/README.md b/pylabrobot/formulatrix/mantis/driver/recordings/README.md index be9b0da8986..2dfed8417b6 100644 --- a/pylabrobot/formulatrix/mantis/driver/recordings/README.md +++ b/pylabrobot/formulatrix/mantis/driver/recordings/README.md @@ -24,6 +24,7 @@ needed. | `rfid` | `RFID.config`: every `RFIDAntennaId.ACC`; `Device.config`: `RfidMultiplexerDelay`, as written (`multiplexer_delay_raw`) | takes its slots from it; every slot reads empty | takes each slot's antenna; a delay other than 0 refuses the reads, as its unit is unconfirmed | | `led_bar` | `Device.config`: `NumberOfLed`; set on a v4 only | takes its LED count from it | takes its LED count from it: each colour write sets that many LEDs | | `arm` | `RobotArm.config`: `Motor.1/2.Offset` and `HomingPositionA`; the `Data` folder's `System/Encoder Correction/Sampling File/OneRevolutionSamplingData_Motor0/1.csv`; only where `HomingPositionA` is set (a v4) | takes its offsets from it, else 0 | needs it to bring the arm up; never taken from a recording | +| `stage` | `Device.config`: `MicroplateTransformationMatrix`, `MicroplateOffsetX/Y`, `ArmType`, `ArmTypeTranslation`, `ArmTypeRotationDegree`; the `Data/Device/Adapters/SBS Adapter.ad.txt` origin; only where the matrix is set | takes it from it, else none: a move to a wellplate refuses | needs it to move to a wellplate; never taken from a recording | For each unit block, setup takes: @@ -32,8 +33,9 @@ For each unit block, setup takes: 2. the one source's, when only one has it; 3. the generation's recording's, when neither has it, with a warning: another unit's values, less accurate on this one. `save_configuration` does not save a block taken from a recording. - A block that calibrates motion (`MOTION_CALIBRATION_BLOCKS`: `arm`) is never taken from a - recording, and no recording holds one: without it, the motion it calibrates refuses. + A block that calibrates motion (`MOTION_CALIBRATION_BLOCKS`: `arm`, `zones`, `approach_paths`, + `stage`) is never taken from a recording, and no recording holds one: without it, the motion it + calibrates refuses. A declared file saved before a block or a non-fitted `device` field existed still loads: each value it lacks comes from the folder, else the recording, on a device and in simulation, with one diff --git a/pylabrobot/formulatrix/mantis/driver/sequence_files.py b/pylabrobot/formulatrix/mantis/driver/sequence_files.py new file mode 100644 index 00000000000..be2002bbfe7 --- /dev/null +++ b/pylabrobot/formulatrix/mantis/driver/sequence_files.py @@ -0,0 +1,85 @@ +"""A unit's sequence files (``.seq.txt``): one item per line, its class name first. + +Only what moves the arm is read here: each ``MoveSequenceItem`` line becomes a route line, the axes +it names in mm in the files' frame (the arm frame; Z down from its home flag), the others None. +""" + +from typing import List, Optional, Tuple + +from .errors import MantisConfigurationError + +# One line of a route file: x, y and z, each None where the line does not name it, so that axis +# holds; not a pose, which names every axis. +RouteLine = Tuple[Optional[float], Optional[float], Optional[float]] +MOVE_ITEM = "MoveSequenceItem" +# A line that only shows a message; it moves nothing. +REMARK_ITEM = "RemarkSequenceItem" +AXES = ("x", "y", "z") + + +def _parse_move_line(*, path: str, number: int, tokens: List[str]) -> RouteLine: + """One move line's axes, from the tokens after its class name. + + A line holds its message, its wait flag, then the axes it moves, each with its value, and a + delay; it may end in a ``TargetPosition`` and the position's name. + + Raises: + MantisConfigurationError: if the line is malformed, names one of X and Y alone, moves + relative to where the head is, does not wait or has a delay other than 0. + """ + where = f"{path}, line {number}" + if len(tokens) < 2 or tokens[1].lower() != "true": + raise MantisConfigurationError(f"{where}: a move that does not wait is not supported") + axes = {} + delays = [] + index = 2 + while index < len(tokens): + token = tokens[index].lower() + if token in AXES and index + 1 < len(tokens): + try: + axes[token] = float(tokens[index + 1]) + except ValueError as error: + raise MantisConfigurationError(f"{where}: {tokens[index + 1]!r} is no number") from error + index += 2 + elif token in ("dx", "dy", "dz"): + raise MantisConfigurationError(f"{where}: a relative move is not supported") + elif token == "targetposition": + index += 2 # names the position; it moves nothing + else: + try: + delays.append(int(token)) + except ValueError as error: + raise MantisConfigurationError(f"{where}: {tokens[index]!r} is not understood") from error + index += 1 + if any(delay != 0 for delay in delays): + raise MantisConfigurationError(f"{where}: a delay other than 0 is not supported") + if ("x" in axes) != ("y" in axes): + raise MantisConfigurationError(f"{where}: a move names one of X and Y alone") + return (axes.get("x"), axes.get("y"), axes.get("z")) + + +def parse_move_sequence(*, path: str) -> List[RouteLine]: + """The route lines of a sequence file that moves the arm, one per move line, in order. + + Remark lines and blank lines are skipped; a line that names no axis moves nothing and is kept. + + Args: + path: the ``.seq.txt`` file. + + Raises: + MantisConfigurationError: if a line is neither a move nor a remark, or a move line is not + supported (``_parse_move_line``). + """ + route_lines: List[RouteLine] = [] + # utf-8-sig drops a byte-order mark at the start; one within the file is dropped per line. + with open(path, encoding="utf-8-sig") as f: + for number, line in enumerate(f, start=1): + tokens = line.replace("\ufeff", "").split() + if not tokens or tokens[0] == REMARK_ITEM: + continue + if tokens[0] != MOVE_ITEM: + raise MantisConfigurationError( + f"{path}, line {number}: {tokens[0]} does not belong in a sequence that moves the arm" + ) + route_lines.append(_parse_move_line(path=path, number=number, tokens=tokens[1:])) + return route_lines diff --git a/pylabrobot/formulatrix/mantis/driver/sequence_files_tests.py b/pylabrobot/formulatrix/mantis/driver/sequence_files_tests.py new file mode 100644 index 00000000000..066cc51eca6 --- /dev/null +++ b/pylabrobot/formulatrix/mantis/driver/sequence_files_tests.py @@ -0,0 +1,62 @@ +"""Sequence file tests: the move lines of a sequence, in its files' layout.""" + +import os +import tempfile +import unittest + +from pylabrobot.formulatrix.mantis.driver.errors import MantisConfigurationError +from pylabrobot.formulatrix.mantis.driver.sequence_files import parse_move_sequence + +# A route in the layout of a v4 install's approach path files, with synthetic positions: a remark, a +# line naming no axis, X and Y, Z alone, all three, and a named target position. +SYNTHETIC_ROUTE = ( + "RemarkSequenceItem Move%32To%32Somewhere \r\n" + "MoveSequenceItem %00 True 0\r\n" + "MoveSequenceItem %00 True X 15.000 Y 30.000 0\r\n" + "MoveSequenceItem %00 True Z -1.570 0\r\n" + "MoveSequenceItem %00 True X -50.000 Y 20.000 Z -1.570 0\r\n" + "MoveSequenceItem %00 True X -60.000 Y 10.000 Z 30.000 0 TargetPosition SomeLocation\r\n" +) + + +class ParseMoveSequenceTests(unittest.TestCase): + def parse(self, text: str): + with tempfile.TemporaryDirectory() as directory: + path = os.path.join(directory, "route.seq.txt") + with open(path, "w", encoding="utf-8-sig", newline="") as f: + f.write(text) + return parse_move_sequence(path=path) + + def test_each_move_line_gives_the_axes_it_names(self): + self.assertEqual( + self.parse(SYNTHETIC_ROUTE), + [ + (None, None, None), + (15.0, 30.0, None), + (None, None, -1.57), + (-50.0, 20.0, -1.57), + (-60.0, 10.0, 30.0), + ], + ) + + def test_a_line_without_a_delay_and_a_byte_order_mark_are_read(self): + text = "MoveSequenceItem %00 True Z -1.570 0\r\nMoveSequenceItem %00 True X 15 Y 30 Z 1" + self.assertEqual(self.parse(text), [(None, None, -1.57), (15.0, 30.0, 1.0)]) + + def test_lines_that_are_not_supported_refuse(self): + cases = { + "MoveSequenceItem %00 True X 15.000 0": "one of X and Y alone", + "MoveSequenceItem %00 True dX 1.000 dY 1.000 0": "relative move", + "MoveSequenceItem %00 False X 15.000 Y 30.000 0": "does not wait", + "MoveSequenceItem %00 True X 15.000 Y 30.000 500": "delay other than 0", + "MoveSequenceItem %00 True X fifteen Y 30.000 0": "is no number", + "ValveStateSequenceItem %00 10 00000000000000000": "does not belong", + } + for line, message in cases.items(): + with self.subTest(line=line): + with self.assertRaisesRegex(MantisConfigurationError, message): + self.parse(line + "\r\n") + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/formulatrix/mantis/driver/simulator.py b/pylabrobot/formulatrix/mantis/driver/simulator.py index 534276c7b1d..8e45a41c552 100644 --- a/pylabrobot/formulatrix/mantis/driver/simulator.py +++ b/pylabrobot/formulatrix/mantis/driver/simulator.py @@ -1,5 +1,6 @@ """A simulated Mantis: the driver with no device behind it, answering from its configuration.""" +import dataclasses import logging from typing import Dict, List, Optional, Tuple @@ -14,7 +15,7 @@ read_configuration, ) from .errors import MantisConfigurationError, MantisTimeoutError -from .features.arm import ARM_MOTORS, MOTOR_HOMED, Z_MOTOR, Arm +from .features.arm import ARM_MOTORS, MOTOR_HOMED, Z_MOTOR, Arm, MantisArmConfiguration from .features.led_bar import LedBar from .features.plate_clamp import CLAMPING_DONE, UNCLAMPING_DONE, PlateClamp from .features.pump import PressureController, Pump @@ -139,27 +140,17 @@ class MantisSimulationArm(Arm): power-on. Without a declared ``arm`` block its offsets are 0. """ - def __init__( - self, - *, - driver: "MantisSimulationDriver", - offsets: Optional[Dict[int, float]], - homing_position_a: Optional[Dict[int, float]], - encoder_correction: Optional[Dict[int, List[Tuple[float, float]]]], - ): + def __init__(self, *, driver: "MantisSimulationDriver", configuration: MantisArmConfiguration): """ Args: driver: the simulated driver. - offsets: each arm motor's offset, as for ``Arm``; None takes 0. - homing_position_a: as for ``Arm``; unused. - encoder_correction: as for ``Arm``; unused. + configuration: as for ``Arm``; without offsets each takes 0. """ - super().__init__( - driver=driver, - offsets=offsets if offsets is not None else {motor: 0.0 for motor in ARM_MOTORS}, - homing_position_a=homing_position_a, - encoder_correction=encoder_correction, - ) + if configuration.offsets is None: + configuration = dataclasses.replace( + configuration, offsets={motor: 0.0 for motor in ARM_MOTORS} + ) + super().__init__(driver=driver, configuration=configuration) self._positions: Dict[int, float] = {0: 0.0, 1: 0.0, Z_MOTOR: 0.0} self._absolute_positions: Dict[int, float] = self._get_idle_targets() self._z_homed = False @@ -202,9 +193,10 @@ async def _unchecked_fw_home( async def _unchecked_fw_move_absolute( self, *, motor: int, position: float, speed: float, acceleration: float - ) -> None: - """Move the motor there at once.""" + ) -> int: + """Move the motor there at once; answer no error.""" self._set_position(motor=motor, position=position) + return 0 async def _unchecked_fw_stop(self, *, motor: int, deceleration: float) -> None: """Stop nothing: a simulated motor is never moving.""" @@ -275,6 +267,7 @@ def __init__( show_actions=show_actions, ) self._queued_items: List[int] = [] + self._simulated_motor_limits: Dict[int, Tuple[float, float]] = {} self._queued_moves: Dict[int, List[Tuple[float, float, float]]] = {} self._sequencer_started = False @@ -321,9 +314,9 @@ async def request_name(self) -> str: raise NotImplementedError(f"{self.name}: the device name is not simulated") async def request_motor_limits(self, *, motor: int) -> Tuple[float, float]: - """A motor's declared (lower, upper) limit.""" + """A motor's (lower, upper) limit as its generation's recording holds it.""" self._check_index(name="motor", value=motor, allowed=MOTORS) - return self.configuration.motor_limits[motor] + return self._simulated_motor_limits[motor] async def request_motor_currents(self, *, motor: int) -> Tuple[float, float, float]: """Raise: the currents are not part of the configuration.""" @@ -439,15 +432,25 @@ async def _request_configuration(self) -> DeviceConfiguration: values, _ = self._load_unit_values(generation=generation) configuration = configuration_from_json(fields=values["device"]) configuration.generation = generation + recording = RECORDING_MANTIS_V4 if generation == "v4" else RECORDING_MANTIS_V3 + self._simulated_motor_limits = { + int(motor): (float(lower), float(upper)) + for motor, (lower, upper) in read_configuration(path=recording)["device"] + .get("motor_limits", {}) + .items() + } return configuration - def _discover_features(self) -> None: + def _discover_features(self, *, motor_limits: Dict[int, Tuple[float, float]]) -> None: """Build the features as a device would, simulated, and the RFID reader. + Args: + motor_limits: as for ``MantisDriver``. + Raises: MantisConfigurationError: if a declared block is malformed. """ - super()._discover_features() + super()._discover_features(motor_limits=motor_limits) if self.plate_clamp is not None and not isinstance( self.plate_clamp, MantisSimulationPlateClamp ): @@ -461,12 +464,7 @@ def _discover_features(self) -> None: show_actions=self.led_bar.show_actions, ) if self.arm is not None and not isinstance(self.arm, MantisSimulationArm): - self.arm = MantisSimulationArm( - driver=self, - offsets=self.arm.offsets, - homing_position_a=self.arm.homing_position_a, - encoder_correction=self.arm.encoder_correction, - ) + self.arm = MantisSimulationArm(driver=self, configuration=self.arm.configuration) if self.rfid is None: rfid_block = self._unit_blocks.get("rfid") slot_to_antenna: Optional[Dict[int, int]] = None diff --git a/pylabrobot/formulatrix/mantis/driver/simulator_tests.py b/pylabrobot/formulatrix/mantis/driver/simulator_tests.py index d2f9c168c89..2da28432cef 100644 --- a/pylabrobot/formulatrix/mantis/driver/simulator_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/simulator_tests.py @@ -4,6 +4,8 @@ import os import tempfile import unittest +from typing import Tuple +from unittest import mock from pylabrobot.formulatrix.mantis import Mantis from pylabrobot.formulatrix.mantis.device import RECORDING_MANTIS_V4 @@ -17,10 +19,33 @@ ) from pylabrobot.formulatrix.mantis.driver.features.arm import SHUTDOWN_Z from pylabrobot.formulatrix.mantis.driver.master import SEQUENCER_QUEUE_SIZE +from pylabrobot.formulatrix.mantis.driver.oem_config_tests import ( + SYNTHETIC_APPROACH_PATHS_BLOCK, + SYNTHETIC_STAGE_BLOCK, + SYNTHETIC_WASH_2_ROUTE, + SYNTHETIC_WASH_ROUTE, + SYNTHETIC_WASTE_ROUTE, + SYNTHETIC_ZONES_BLOCK, +) from pylabrobot.formulatrix.mantis.driver.simulator import ( MantisSimulationArm, MantisSimulationLedBar, ) +from pylabrobot.formulatrix.mantis.driver.stage_transform import ( + compute_arm_frame_position, +) +from pylabrobot.resources.corning import cor_96_wellplate_360uL_Fb + + +async def request_head_position(*, arm) -> Tuple[float, float, float]: + """Where the head is: x and y in the arm frame, mm, and Z's height above its flag, mm.""" + joint_state = await arm.request_joint_state() + x, y = arm._get_head_position(joint_state=joint_state) + return x, y, 0.0 - joint_state[2] + + +# The synthetic stage's reference height, mm. +REFERENCE_HEIGHT = SYNTHETIC_STAGE_BLOCK["reference_height"] class SimulationTests(unittest.IsolatedAsyncioTestCase): @@ -96,12 +121,12 @@ async def test_setup_brings_the_arm_to_idle_and_homes_z(self): async def test_the_head_stands_at_idle_without_offsets(self): arm = self.mantis.arm assert arm is not None - head = await arm.request_head_position() - self.assertAlmostEqual(head.x, 15.0, places=9) - self.assertAlmostEqual(head.y, 31.177, places=9) - joint_position = await arm.request_joint_position() + head_x, head_y, _ = await request_head_position(arm=arm) + self.assertAlmostEqual(head_x, 15.0, places=9) + self.assertAlmostEqual(head_y, 31.177, places=9) + joint_state = await arm.request_joint_state() self.assertEqual( - await arm.request_absolute_joint_position(), {0: joint_position[0], 1: joint_position[1]} + await arm.request_absolute_joint_state(), {0: joint_state[0], 1: joint_state[1]} ) async def test_stop_parks_and_lowers_z(self): @@ -128,7 +153,7 @@ async def test_an_arm_away_from_idle_is_refused(self): async def test_the_long_links_meet_at_the_head(self): assert self.mantis.arm is not None - head = await self.mantis.arm.request_head_position() + head_x, head_y, _ = await request_head_position(arm=self.mantis.arm) for name in ("right_long_arm", "left_long_arm"): with self.subTest(name=name): link = self.mantis.arm_links[name] @@ -139,8 +164,8 @@ async def test_the_long_links_meet_at_the_head(self): end_x = corner.x + sum(matrix[0][k] * joint[k] for k in range(3)) end_y = corner.y + sum(matrix[1][k] * joint[k] for k in range(3)) # The arm frame is the device frame moved to the right motor's axis and mirrored in y. - self.assertAlmostEqual(end_x, 72.549 + head.x, places=2) - self.assertAlmostEqual(end_y, 142.594 - head.y, places=2) + self.assertAlmostEqual(end_x, 72.549 + head_x, places=2) + self.assertAlmostEqual(end_y, 142.594 - head_y, places=2) async def test_pump_reads_off_with_the_recorded_calibration(self): pump = self.mantis.pump @@ -235,5 +260,156 @@ async def test_simulated_mantis_sends_nothing(self): await self.mantis.driver._send_command(board=0, command=3) +class SimulatedRouteTests(unittest.IsolatedAsyncioTestCase): + """A simulated Mantis declaring the unit's zones and approach paths, synthetic.""" + + async def asyncSetUp(self): + with open(RECORDING_MANTIS_V4, encoding="utf-8") as f: + recording = json.load(f) + directory = tempfile.TemporaryDirectory() + self.addCleanup(directory.cleanup) + path = os.path.join(directory.name, "declared.json") + with open(path, "w", encoding="utf-8") as f: + json.dump( + { + **recording, + "zones": SYNTHETIC_ZONES_BLOCK, + "approach_paths": SYNTHETIC_APPROACH_PATHS_BLOCK, + "stage": SYNTHETIC_STAGE_BLOCK, + }, + f, + ) + self.mantis = Mantis(simulation=True, declared_configuration_json=path) + await self.mantis.setup() + self.addAsyncCleanup(self.mantis.stop) + arm = self.mantis.arm + assert isinstance(arm, MantisSimulationArm) + self.arm = arm + + async def assert_head_at(self, *, x: float, y: float, z: float): + head = await request_head_position(arm=self.arm) + for name, value, expected in zip("xyz", head, (x, y, z)): + with self.subTest(axis=name): + self.assertAlmostEqual(value, expected, places=6) + + async def move_head_to(self, *, x: float, y: float) -> None: + """Move the head to (x, y) in the arm frame, off the plate holder, through the deck frame.""" + deck_x, deck_y = self.arm._get_deck_frame_xy(x=x, y=y) + await self.arm.move_to_xy_position(x=deck_x, y=deck_y) + + async def test_a_move_off_the_plate_holder_and_back_to_idle(self): + await self.move_head_to(x=-57.0, y=0.0) + await self.arm.move_to_joint_state(joint_state={2: 10.0}) + await self.assert_head_at(x=-57.0, y=0.0, z=-10.0) + self.assertFalse(await self.arm.request_is_parked()) + await self.arm.park() + await self.assert_head_at(x=15.0, y=31.177, z=0.0) + self.assertTrue(await self.arm.request_is_parked()) + + async def test_z_and_joint_moves(self): + await self.arm.move_to_z_position(z=50.0) + self.assertAlmostEqual((await self.arm.request_pose()).nozzle_location.z, 50.0, places=6) + await self.arm.move_to_joint_state(joint_state={2: 5.0}) + self.assertEqual((await self.arm.request_joint_state())[2], 5.0) + + async def assert_at_end_of(self, *, path) -> None: + x, y, z = path[-1] + assert x is not None and y is not None and z is not None + await self.assert_head_at(x=x, y=y, z=-z) + + async def test_the_waste_and_each_wash_are_reached_from_wherever_the_head_stands(self): + await self.arm.move_to_waste() + await self.assert_at_end_of(path=SYNTHETIC_WASTE_ROUTE) + # From the waste: out along its path to its start, then in along the wash's. + await self.arm.move_to_wash(position=1) + await self.assert_at_end_of(path=SYNTHETIC_WASH_ROUTE) + # From one wash to the other: up to wash 1's safe Z (its last line but one), across to wash 2's + # safe-Z line, then down; the head stays in the wash. + with mock.patch.object( + self.arm, "_leave_approach_path", wraps=self.arm._leave_approach_path + ) as leave: + await self.arm.move_to_wash(position=2) + last = len(SYNTHETIC_WASH_ROUTE) - 1 + leave.assert_awaited_once_with(name="wash_1", index=last, until=last - 1) + await self.assert_at_end_of(path=SYNTHETIC_WASH_2_ROUTE) + await self.arm.park() + self.assertTrue(await self.arm.request_is_parked()) + with self.assertRaisesRegex(ValueError, "position must be one of"): + await self.arm.move_to_wash(position=3) + + async def test_a_move_from_the_waste_leaves_along_its_path_first(self): + # Where the head stands is read from the motors: nothing is remembered between moves. + await self.arm.move_to_waste() + with mock.patch.object( + self.arm, "_leave_approach_path", wraps=self.arm._leave_approach_path + ) as leave: + await self.move_head_to(x=-57.0, y=0.0) + leave.assert_awaited_once_with(name="waste", index=len(SYNTHETIC_WASTE_ROUTE) - 1, until=0) + await self.assert_head_at(x=-57.0, y=0.0, z=1.57) + + async def test_stop_from_a_back_zone_parks_along_its_route_and_lowers_z(self): + await self.arm.move_to_waste() + await self.mantis.stop() + self.assertAlmostEqual( + self.arm._positions[2], compute_device_position(motor=2, position=SHUTDOWN_Z) + ) + idle = compute_joint_angles(x=15.0, y=31.177) + for motor in (0, 1): + self.assertAlmostEqual( + self.arm._positions[motor], compute_device_position(motor=motor, position=idle[motor]) + ) + await self.mantis.setup() + + async def test_recover_from_fault_brings_the_head_back_to_idle(self): + await self.move_head_to(x=80.0, y=-40.0) + await self.arm.move_to_joint_state(joint_state={2: 5.0}) + await self.arm.recover_from_fault() + self.assertTrue(self.arm.is_initialized) + await self.assert_head_at(x=15.0, y=31.177, z=0.0) + + +class SimulatedWellplateTests(unittest.IsolatedAsyncioTestCase): + """A simulated Mantis declaring synthetic zones and stage, a wellplate in its plate holder.""" + + async def asyncSetUp(self): + with open(RECORDING_MANTIS_V4, encoding="utf-8") as f: + recording = json.load(f) + directory = tempfile.TemporaryDirectory() + self.addCleanup(directory.cleanup) + path = os.path.join(directory.name, "declared.json") + with open(path, "w", encoding="utf-8") as f: + json.dump({**recording, "zones": SYNTHETIC_ZONES_BLOCK, "stage": SYNTHETIC_STAGE_BLOCK}, f) + self.mantis = Mantis(simulation=True, declared_configuration_json=path) + await self.mantis.setup() + self.addAsyncCleanup(self.mantis.stop) + self.wellplate = cor_96_wellplate_360uL_Fb(name="wellplate") + self.mantis.plate_holder.assign_child_resource(self.wellplate) + + async def test_the_nozzle_goes_over_each_corner_well(self): + arm = self.mantis.arm + assert arm is not None + # Each well's centre from the wellplate's back-left corner, mm; 12.5 mm above its 14.2 mm frame. + for name, x, y in (("A1", 14.3, 11.28), ("A12", 113.3, 11.28), ("H1", 14.3, 74.28)): + with self.subTest(well=name): + center = self.wellplate.get_item(name).get_location_wrt(self.mantis.deck, x="c", y="c") + await arm.move_to_xy_position(x=center.x, y=center.y) + await arm.move_to_z_position(z=26.7) + arm_x, arm_y, z = compute_arm_frame_position( + position=(x, y, REFERENCE_HEIGHT - 26.7), + origin=(-50.0, 40.0, 51.0), + transformation_matrix=SYNTHETIC_STAGE_BLOCK["transformation_matrix"], + offset_x=(0.0, 0.0, -0.0015), + offset_y=(0.0, 0.0, 0.0025), + arm_type_rotation_degree=0.0, + arm_type_translation=(0.0, 0.0, 0.0), + ) + head_x, head_y, head_z = await request_head_position(arm=arm) + self.assertAlmostEqual(head_x, arm_x, places=3) + self.assertAlmostEqual(head_y, arm_y, places=3) + self.assertAlmostEqual(head_z, -z, places=3) + await arm.park() + self.assertTrue(await arm.request_is_parked()) + + if __name__ == "__main__": unittest.main() diff --git a/pylabrobot/formulatrix/mantis/driver/stage_transform.py b/pylabrobot/formulatrix/mantis/driver/stage_transform.py new file mode 100644 index 00000000000..2283f7336c4 --- /dev/null +++ b/pylabrobot/formulatrix/mantis/driver/stage_transform.py @@ -0,0 +1,54 @@ +"""The stage transform: where a point on the wellplate is in the arm frame. Pure maths. + +The wellplate frame's origin is the wellplate's back-left corner, the plate holder's datum, with +x to the right, y to the front and z down from the unit's reference height above the wellplate's +base, in mm. A point is turned and moved by the arm type, moved by the stage origin, its x and y mapped by +the projective transformation matrix and its z tilted; never through a fitted homography. +""" + +import math +from typing import Sequence, Tuple + +from .errors import MantisKinematicsError + + +def compute_arm_frame_position( + *, + position: Tuple[float, float, float], + origin: Tuple[float, float, float], + transformation_matrix: Sequence[Sequence[float]], + offset_x: Tuple[float, float, float], + offset_y: Tuple[float, float, float], + arm_type_rotation_degree: float, + arm_type_translation: Tuple[float, float, float], +) -> Tuple[float, float, float]: + """A point of the wellplate frame in the arm frame: x and y in mm, z in mm down from Z's flag. + + Args: + position: (x, y, z) in the wellplate frame, mm. + origin: the stage origin, the plate holder's (x, y, z), mm. + transformation_matrix: the stage's matrix, 3 by 3, projective on x and y. + offset_x: the stage's x offset; its z is the tilt per mm of x in the wellplate frame. + offset_y: the stage's y offset; its z is the tilt per mm of y in the wellplate frame. + arm_type_rotation_degree: the arm type's rotation, degrees, about the wellplate frame's origin. + arm_type_translation: the arm type's translation, (x, y, z), mm. + + Raises: + MantisKinematicsError: if the matrix maps the point to infinity. + """ + x, y, z = position + angle = math.radians(arm_type_rotation_degree) + framed_x = x * math.cos(angle) - y * math.sin(angle) + arm_type_translation[0] + framed_y = x * math.sin(angle) + y * math.cos(angle) + arm_type_translation[1] + framed_z = z + arm_type_translation[2] + stage_x, stage_y = framed_x + origin[0], framed_y + origin[1] + mapped = [row[0] * stage_x + row[1] * stage_y + row[2] for row in transformation_matrix] + if mapped[2] == 0: + raise MantisKinematicsError( + f"the stage transform maps ({x:.3f}, {y:.3f}) mm on the wellplate to infinity" + ) + return ( + mapped[0] / mapped[2], + mapped[1] / mapped[2], + origin[2] + framed_z + framed_x * offset_x[2] + framed_y * offset_y[2], + ) diff --git a/pylabrobot/formulatrix/mantis/driver/stage_transform_tests.py b/pylabrobot/formulatrix/mantis/driver/stage_transform_tests.py new file mode 100644 index 00000000000..f04a32c5888 --- /dev/null +++ b/pylabrobot/formulatrix/mantis/driver/stage_transform_tests.py @@ -0,0 +1,74 @@ +"""Stage transform tests: each step of the chain, worked by hand on synthetic values.""" + +import unittest + +from pylabrobot.formulatrix.mantis.driver.errors import MantisKinematicsError +from pylabrobot.formulatrix.mantis.driver.stage_transform import compute_arm_frame_position + +IDENTITY = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)) +FLAT = (0.0, 0.0, 0.0) + + +def compute(*, position, **stage): + arguments = { + "origin": FLAT, + "transformation_matrix": IDENTITY, + "offset_x": FLAT, + "offset_y": FLAT, + "arm_type_rotation_degree": 0.0, + "arm_type_translation": FLAT, + **stage, + } + return compute_arm_frame_position(position=position, **arguments) + + +class StageTransformTests(unittest.TestCase): + def assert_close(self, actual, expected): + for axis, value, wanted in zip("xyz", actual, expected): + with self.subTest(axis=axis): + self.assertAlmostEqual(value, wanted, places=12) + + def test_the_identity_keeps_the_point(self): + self.assert_close(compute(position=(14.3, 11.28, -7.035)), (14.3, 11.28, -7.035)) + + def test_the_origin_moves_the_point(self): + position = compute(position=(14.3, 11.28, -7.035), origin=(-50.0, 40.0, 51.0)) + self.assert_close(position, (-35.7, 51.28, 43.965)) + + def test_x_and_y_are_mapped_projectively_after_the_origin(self): + # (5, 5) + (10, 20) = (15, 25); weight 0.01 * 15 + 1 = 1.15. + matrix = ((2.0, 0.0, 1.0), (0.0, 3.0, -1.0), (0.01, 0.0, 1.0)) + position = compute( + position=(5.0, 5.0, 0.0), origin=(10.0, 20.0, 30.0), transformation_matrix=matrix + ) + self.assert_close(position, (31.0 / 1.15, 74.0 / 1.15, 30.0)) + + def test_z_tilts_with_the_wellplate_frame_not_the_stage(self): + # The offsets' x and y are not used; their z per mm of the wellplate frame's x and y. + position = compute( + position=(100.0, 50.0, -2.0), + origin=(-50.0, 40.0, 51.0), + offset_x=(5.0, 5.0, -0.001), + offset_y=(5.0, 5.0, 0.002), + ) + self.assert_close(position, (50.0, 90.0, 51.0 - 2.0 - 0.1 + 0.1)) + + def test_the_arm_type_turns_then_moves_the_point_before_the_origin(self): + position = compute( + position=(10.0, 0.0, 1.0), + origin=(100.0, 200.0, 0.0), + offset_x=(0.0, 0.0, 0.01), + arm_type_rotation_degree=90.0, + arm_type_translation=(1.0, 2.0, 3.0), + ) + # Turned to (0, 10), moved to (1, 12, 4); the tilt takes the turned and moved x, 1. + self.assert_close(position, (101.0, 212.0, 4.0 + 0.01)) + + def test_a_point_mapped_to_infinity_refuses(self): + matrix = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.1, 0.0, 1.0)) + with self.assertRaises(MantisKinematicsError): + compute(position=(-10.0, 0.0, 0.0), transformation_matrix=matrix) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/formulatrix/mantis/resource_model/__init__.py b/pylabrobot/formulatrix/mantis/resource_model/__init__.py index a3ea790eb92..b5c2eed4a83 100644 --- a/pylabrobot/formulatrix/mantis/resource_model/__init__.py +++ b/pylabrobot/formulatrix/mantis/resource_model/__init__.py @@ -17,6 +17,16 @@ # The device frame of ``manifest.json``: its size, base file and modules. MANIFEST: Dict[str, Any] = dict(json.load(f)["frames"]["Mantis"]) +# The main box's size, in mm, from the manufacturer's model: up to the top plate's surface. +SIZE_X, SIZE_Y, SIZE_Z = MANIFEST["size"] +# The plate holder on the deck: an SBS footprint pushed into the back-left of the pin nest, in mm. +PLATE_HOLDER_LOCATION = Coordinate(23.62, 15.02, 0.0) +PLATE_HOLDER_SIZE_X, PLATE_HOLDER_SIZE_Y = 127.76, 85.48 +# Where the right motor turns, x and y in the device frame, and the arm's height, in mm; the arm +# frame is the device frame moved there and mirrored in y. +ARM_BASE_X, ARM_BASE_Y = MANIFEST["joints"]["base_right"] +ARM_Z = MANIFEST["joints"]["arm_z"] + def _mesh(*, file: str) -> Dict[str, str]: """The ``mesh`` declaration for a model file in this folder.""" @@ -86,4 +96,18 @@ def arm_link(name: str, *, link: str) -> LinkBody: ) -__all__ = ["MANIFEST", "arm_link", "chip_rack", "z_column"] +__all__ = [ + "ARM_BASE_X", + "ARM_BASE_Y", + "ARM_Z", + "MANIFEST", + "PLATE_HOLDER_LOCATION", + "PLATE_HOLDER_SIZE_X", + "PLATE_HOLDER_SIZE_Y", + "SIZE_X", + "SIZE_Y", + "SIZE_Z", + "arm_link", + "chip_rack", + "z_column", +]