diff --git a/docs/api/pylabrobot.formulatrix.rst b/docs/api/pylabrobot.formulatrix.rst index 7d614270fa2..19d7908f5aa 100644 --- a/docs/api/pylabrobot.formulatrix.rst +++ b/docs/api/pylabrobot.formulatrix.rst @@ -18,17 +18,17 @@ Mantis driver.master.MantisDriver driver.simulator.MantisSimulationDriver driver.simulator.MantisSimulationArm - driver.simulator.MantisSimulationV3Arm driver.simulator.MantisSimulationPlateClamp - driver.simulator.MantisSimulationPneumaticPlateClamp driver.simulator.MantisSimulationPump driver.simulator.MantisSimulationRfid driver.simulator.MantisSimulationLedBar driver.features.arm.Arm - driver.features.arm_v3.V3Arm + driver.features.arm.MantisPose + driver.features.arm.MantisArmConfiguration driver.arm_kinematics + driver.stage_transform + driver.sequence_files driver.features.plate_clamp.PlateClamp - driver.features.pneumatic_plate_clamp.PneumaticPlateClamp driver.features.pump.Pump driver.features.rfid.Rfid driver.features.led_bar.LedBar diff --git a/docs/user_guide/formulatrix/mantis/hello-world.ipynb b/docs/user_guide/formulatrix/mantis/hello-world.ipynb index 0836617debf..6c0537315e5 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, and initializes the arm and Z. A v3 homes against its flags and follows its calibrated route to idle; a v4 uses absolute encoders and parks at stop. The driver also locks and unlocks pneumatic and motor plate clamps, reads chip tags, and shows colours on a v4's LED bar. The separate LC3 carousel supports homing, absolute rotation and calibrated chip-slot selection. Moving the arm to arbitrary positions and dispensing are not supported." + "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 (a v3 against its flags and along its calibrated route to idle), 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 (pneumatic or motor), reads which chip sits in each input slot, and shows colours on a v4's LED bar. The separate LC3 carousel is homed, rotated to an absolute angle and turned to a calibrated chip slot. Nothing in this driver switches the pump on or opens a valve." ] }, { @@ -25,9 +25,9 @@ "metadata": {}, "source": [ "```{warning}\n", - "V3 arm initialization and stop have run on one Mantis on Linux. Two consecutive setups completed with all three motors homed and no final fault bits; the operator confirmed normal arm and Z movement and the idle position. The firmware set was motion 4.20.6 and pressure 1.0.5, with boards 11 and 12 absent. The [recorded replies and command sequence](https://github.com/PyLabRobot/pylabrobot/blob/16b62b1e409e566d37d5748405ae233cde9405c4/pylabrobot/formulatrix/mantis/driver/features/arm_v3_tests.py) are covered by regression tests.\n", + "V3 arm initialization and stop have run on one Mantis on Linux. Two consecutive setups completed with all three motors homed and no final fault bits; the operator confirmed normal arm and Z movement and the idle position. The firmware set was motion 4.20.6 and pressure 1.0.5, with boards 11 and 12 absent. The [recorded replies and command sequence](https://github.com/PyLabRobot/pylabrobot/blob/main/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py) are covered by regression tests.\n", "\n", - "The driver has also run on one v4 on macOS and Windows, but its arm initialization and parking remain unverified. Keep the head clear during setup. `setup()` warns on firmware sets not seen on a Mantis yet. Please [report results](../../needs-testing.md) from other units.\n", + "This driver has also run on one Mantis v4, on macOS and on Windows. 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. If you run this guide on a Mantis, please [report the result](../../needs-testing.md).\n", "```" ] }, @@ -45,13 +45,13 @@ "| 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 (v3) | homed at setup using the unit's calibration and route to idle; stopped in place at shutdown |\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 | v3 pneumatic lock/release with air-pump restoration; v4 motor clamp with adjustable levels |\n", "| LC3 carousel | separate USB controller; home, rotate to an angle, select a calibrated chip slot |\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 |" ] }, { @@ -85,9 +85,10 @@ "source": [ "## Unit configuration\n", "\n", - "Some values are set per unit and cannot be read off the device; they sit in its `.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. These include the plate clamp's lock levels or pneumatic output mapping, RFID antenna mapping and arm calibration. The pneumatic clamp requires `PlateClampController=\"PressureRegulator\"` and `PlateClampAuxId=\"2\"` in the unit's `Device.config`, or the corresponding saved configuration block. A v3 needs its offsets, home positions, flag-search settings and post-home route from `RobotArm.config`. A v4 needs its offsets, absolute encoder references and encoder correction files.\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 or, for a pneumatic clamp, its pressure-board output (`PlateClampController=\"PressureRegulator\"`, `PlateClampAuxId=\"2\"` in `Device.config`), 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", + "A v3's `arm` block holds its offsets, home positions, flag searches and post-home route, from `RobotArm.config`.\n", "\n", - "Without clamp levels or antennas, a recording of the same generation supplies them with a warning. **Arm calibration must come from this unit**: setup refuses without it unless `skip_arm=True`. Where the folder and a saved file both hold a value, they must agree. Older saved files can obtain missing arm calibration from the unit's folder." + "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." ] }, { @@ -109,10 +110,9 @@ "source": [ "## Setup\n", "\n", - "`setup()` opens both links, reads firmware versions, motor limits and pressure sensor calibration, loads the unit's configuration and sets up the RFID reader. It then initializes the arm and Z:\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. 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.", "\n", - "- **V3:** seats the arms against their stops, backs off relative to their live positions, and homes Z; repeats that sequence; homes both arm motors together, starting motor 1 before motor 0; declares the calibrated home frame and follows the unit's route to idle. Following errors during seating and homing are expected; all three motors must finish homed and fault-free. A failure or cancellation stops every motor.\n", - "- **V4:** energises the arm motors, declares them where their absolute encoders read, moves them to idle and homes Z. It requires the head to start near idle; otherwise it refuses with the arms unpowered. The LED bar shows setup, then green and the idle animation.\n", + "On a v3, setup seats the arms against their stops, backs off relative to their live positions, and homes Z; repeats that sequence; homes both arm motors together, starting motor 1 before motor 0; declares the calibrated home frame and follows the unit's route to idle. Following errors during seating and homing are expected; all three motors must finish homed and fault-free. A failure or cancellation stops every motor.\n", "\n", "`setup(skip_arm=True)` discovers the device without initializing or moving the arm. On a v3, the head pose is unavailable until initialization establishes its reference frame." ] @@ -159,7 +159,10 @@ "\n", "For a v3 simulation, pass a saved v3 configuration containing its `arm` block. It simulates seating, homing and the declared route without opening USB; no default v3 motion calibration is supplied.\n", "\n", - "A declared pneumatic clamp simulates its valve and air-pump states. Locking starts its simulated pump if needed; release restores a pump started by the clamp." + "A declared pneumatic clamp simulates its valve and air-pump states. Locking starts its simulated pump if needed; release restores a pump started by the clamp.", + "\n", + "\n", + "The recording holds no zones, approach paths or stage, so the arm's moves need a unit's. Here the synthetic ones the tests declare stand in for them: with this simulated Mantis as `mantis`, the arm cells of this guide run without a device or the unit's folder." ] }, { @@ -169,7 +172,31 @@ "metadata": {}, "outputs": [], "source": [ - "simulated = Mantis(name=\"simulated_mantis\", simulation=True)\n", + "import json\n", + "import os\n", + "import tempfile\n", + "\n", + "from pylabrobot.formulatrix.mantis import RECORDING_MANTIS_V4\n", + "from pylabrobot.formulatrix.mantis.driver.oem_config_tests import (\n", + " SYNTHETIC_APPROACH_PATHS_BLOCK,\n", + " SYNTHETIC_STAGE_BLOCK,\n", + " SYNTHETIC_ZONES_BLOCK,\n", + ")\n", + "\n", + "with open(RECORDING_MANTIS_V4, encoding=\"utf-8\") as f:\n", + " recording = json.load(f)\n", + "path = os.path.join(tempfile.mkdtemp(), \"synthetic_mantis.json\")\n", + "with open(path, \"w\", encoding=\"utf-8\") as f:\n", + " json.dump(\n", + " {\n", + " **recording,\n", + " \"zones\": SYNTHETIC_ZONES_BLOCK,\n", + " \"approach_paths\": SYNTHETIC_APPROACH_PATHS_BLOCK,\n", + " \"stage\": SYNTHETIC_STAGE_BLOCK,\n", + " },\n", + " f,\n", + " )\n", + "simulated = Mantis(name=\"simulated_mantis\", simulation=True, declared_configuration_json=path)\n", "await simulated.setup()\n", "simulated.driver.configuration" ] @@ -199,7 +226,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." ] }, { @@ -272,7 +299,7 @@ "metadata": {}, "outputs": [], "source": [ - "mantis.driver.configuration.motor_limits" + "mantis.arm.configuration.motor_limits" ] }, { @@ -399,7 +426,7 @@ "metadata": {}, "outputs": [], "source": [ - "await mantis.arm.request_joint_position()" + "await mantis.arm.request_joint_state()" ] }, { @@ -418,53 +445,108 @@ "outputs": [], "source": [ "if mantis.driver.configuration.generation == \"v4\":\n", - " await mantis.arm.request_absolute_joint_position()" + " await mantis.arm.request_absolute_joint_state()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "mantis-arm-5", + "metadata": {}, + "outputs": [], + "source": [ + "await mantis.arm.request_pose()" ] }, { "cell_type": "markdown", - "id": "v3-head-position", + "id": "mantis-arm-6", "metadata": {}, "source": [ - "The head position after initialization, on either generation:" + "Whether the nozzle can reach a point in the deck frame, here over the plate holder's centre; nothing is clamped. A v3 initializes using the rates in its unit configuration. The v4 initialization and parking use the arm's defaults, in degrees/s and mm/s:" ] }, { "cell_type": "code", "execution_count": null, - "id": "mantis-arm-5", + "id": "mantis-arm-7", "metadata": {}, "outputs": [], "source": [ - "await mantis.arm.request_head_position()" + "center = mantis.plate_holder.get_location_wrt(mantis.deck, x=\"c\", y=\"c\")\n", + "mantis.arm.is_xy_reachable(x=center.x, y=center.y)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "mantis-arm-8", + "metadata": {}, + "outputs": [], + "source": [ + "mantis.arm.default_xy_speed, mantis.arm.default_z_speed" ] }, { "cell_type": "markdown", - "id": "mantis-arm-6", + "id": "arm-moves", "metadata": {}, "source": [ - "Whether the head can reach a point in the arm frame; nothing is clamped. A v3 initializes using the rates in its unit configuration. The v4 initialization and parking use the arm's defaults, in degrees/s and mm/s:" + "### 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": "mantis-arm-7", + "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": [ - "mantis.arm.is_xy_reachable(x=15.0, y=60.0)" + "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": "mantis-arm-8", + "id": "arm-approach-paths-code", "metadata": {}, "outputs": [], "source": [ - "mantis.arm.default_arm_speed, mantis.arm.default_z_speed" + "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()" ] }, { @@ -585,6 +667,36 @@ "execution_count": null, "outputs": [] }, + { + "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": "plate-clamp-release", @@ -992,7 +1104,7 @@ "source": [ "## Stop\n", "\n", - "Stop the visualizer and close the links. `stop()` aborts the sequencer and stops every motor. A v3 stays where it stopped; it does not perform the v4 park move and must initialize again after reconnecting. On a v4, an initialized arm is parked at idle and Z lowered to 20 mm, unless the head is away from idle. The plate clamp is then released. A pneumatic clamp also restores a pump it started; a pump already running before clamping stays on." + "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. A v3 stays where it stopped: it is not parked, and is brought up again after reconnecting. Then the plate clamp is released." ] }, { diff --git a/pylabrobot/formulatrix/mantis/carousel.py b/pylabrobot/formulatrix/mantis/carousel.py index 97f8320a993..8fa18b0e544 100644 --- a/pylabrobot/formulatrix/mantis/carousel.py +++ b/pylabrobot/formulatrix/mantis/carousel.py @@ -277,7 +277,7 @@ async def move_to( except BaseException: await self._stop_after_failure() raise - logger.info("[%s] carousel at %.3f degrees", self.configuration.device_id, position % 360.0) + logger.info("[%s] carousel at %.2f degrees", self.configuration.device_id, position % 360.0) async def move_to_slot( self, slot: int, *, speed: Optional[float] = None, acceleration: Optional[float] = None diff --git a/pylabrobot/formulatrix/mantis/design.md b/pylabrobot/formulatrix/mantis/design.md index 268db192fbd..188c0995fd1 100644 --- a/pylabrobot/formulatrix/mantis/design.md +++ b/pylabrobot/formulatrix/mantis/design.md @@ -20,6 +20,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, @@ -30,7 +32,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, @@ -55,28 +59,29 @@ The carousel does not coordinate the arm or attach chips. and sending through it. No per-feature backend or frontend. **M2. The generation comes from discovery.** `_request_configuration` reads the firmware version -on boards 0, 10, 11 and 12. Only board 0 must answer. Each other board gets one attempt, and one -that times out, refuses or answers a malformed version is absent, not a fault: it is logged and -recorded as `None`. Board 11 answering means v4; absent means v3. Board 0's version string and the -serial number decide nothing. - -**M3. Nothing exists until the device says it is fitted.** (STAR P2.) `discover` reads the -configuration (`_request_configuration`), checks it against a declared one, then -`_discover_features` builds the features it lists; a refused setup builds none. The RFID reader -is the exception: `discover` builds it when it answers its ping on interface A, after every -opening of the link. -Each feature is an `Optional` attribute of the driver, `None` until `_discover_features` builds -it: a pneumatic `plate_clamp` requires a declared pressure-board output and board 10; -the motor clamp requires board 12. The controller choice is made during discovery; -`pump` only when board 10 answered; `arm` is `Arm` when board 11 answered (v4), otherwise -`V3Arm`, which homes against flags. `led_bar` is gated -by the generation: built only on a v4 whose board 12 answered, never on a v3, whose install sets -no LED count. A difference between v3 and v4 is expressed there, by which feature is built, never -by a branch on the generation inside a command. +on boards 0, 10, 11 and 12. Only board 0 must answer. Board 11 gets two attempts, each other board +one, and one that times out, refuses or answers a malformed version is absent, not a fault: it is +logged and recorded as `None`. Board 11 answering means v4; absent means v3. A unit whose arm block +has absolute-encoder references is refused as a v3, so a v4's arms are never seated against their +stops. Board 0's version string and the serial number decide nothing. + +**M3. Nothing exists until the device says it is fitted; one class per feature.** (STAR P2.) +`discover` reads the configuration (`_request_configuration`), checks it against a declared one, +then `_discover_features` builds the features it lists; a refused setup builds none. The RFID +reader is the exception: `discover` builds it when it answers its ping on interface A, after every +opening of the link. Each feature is an `Optional` attribute of the driver, `None` until built: +`plate_clamp` from the unit's `plate_clamp` block, its `controller` key matching the boards (a +pressure-board output and board 10, or levels and board 12); `pump` when board 10 answered; `arm` +on every unit; `led_bar` only on a v4 whose board 12 answered. Every feature is one class on both +generations, with the same public methods. The generation selects only private plumbing, in the +fork points listed in M6, each written `== "v3"` / `== "v4"` and refusing any other value. Where a +v3 lacks the hardware (absolute encoders, a motor clamp state) the method raises +`MantisConfigurationError`; where a v3 path has not run on a device it raises +`NotImplementedError`. **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 @@ -84,8 +89,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 @@ -100,14 +107,23 @@ 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, or its auxiliary output for a pneumatic clamp controlled by the pressure board, `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 (v4), its encoder correction from the `Data` -folder; otherwise the v3 home frame, flag searches, retract/backoff settings and route to idle. +declares the arms' absolute encoder references (a v4), its encoder correction from the `Data` +folder, otherwise the v3's home frame, flag searches, retract and backoff settings and route to +idle; `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`, on both generations. 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`, on both +generations; 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: @@ -174,7 +190,42 @@ 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, zones and +kinematics keep the frame their files write: the arm frame, Z down from its flag, typed +`MantisArmCoordinate` so it never passes for a deck-frame `Coordinate`. Every public position and +height (`is_xy_reachable` included) 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. `_get_arm_frame_location` and +`_get_deck_frame_location` convert between the two. 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. + +The arm's generation fork points (`Arm._get_generation`): + +- `Arm.__init__`: a v4 refuses a v3 `arm` block. +- `_check_calibrated`: a v4 needs `offsets`, `homing_position_a` and `encoder_correction`; a v3 + its v3 block (`_get_v3_arm_configuration`). +- `initialize`: `_initialize_v4` on a v4, `_initialize_v3` on a v3. +- `_request_absolute_device_position`: a v3 raises `MantisConfigurationError`, and so + `request_absolute_joint_state`: a v3 has no absolute encoders. +- `_request_arm_joint_state`, where the arms are for zones, routes, parking and the parked read: a + v4's absolute encoders, a v3's motor positions (command 27). +- `request_joint_state`: a v3 raises `MantisMotionError` before this connection's bring-up, so + does every read above on a v3. +- `_check_initialized`: a v3 raises `NotImplementedError` after its bring-up, until a v3 has run + its moves; so park, the waste, the washes and recovery too. +- `recover_from_fault`: a v4 recovers to idle from its absolute encoders, a v3 runs its bring-up + again. +- `_halt`: a v3 stops its motors and is not parked until a v3 has run its moves. + +**M6a. Numbers shown to a user are mm and degrees to 2 decimals; nothing else is rounded.** Logs, +exception messages and reprs print mm and degrees with 2 decimals (Z's encoder resolves about 6 µm; +0.01 degree is about 15 µm at the arm's reach). Returned values are not rounded beyond PLR's +`Coordinate`, and nothing sent is ever rounded. **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`); @@ -184,30 +235,82 @@ seen on a device (`MOTORS`, `ABSOLUTE_ENCODER_MOTORS`, the boards, `PRESSURE_CON P9.) The raw call is `_unchecked_fw_*` and sends once; the public method checks its arguments, then 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. +Clamping done (3) is the lock's timer running out, not a plate held: a device test read the same +states at the same times with and without a plate. `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. - -`V3Arm.initialize` seats and backs off the arms using live demand positions, homes Z, repeats -seating and Z homing, starts arm homing in order 1 then 0, declares the calibrated home frame and -follows the unit's route to idle. Expected following errors are tolerated during these phases; -other fault bits fail immediately. Each home must finish with its homed bit, and initialization -ends with a strict fault check. Failure or cancellation stops all three motors. A v3 stop sends -no park move. - -`PneumaticPlateClamp.lock` engages its declared auxiliary output and starts the air pump if -needed. Readbacks confirm both control states; they do not measure plate contact or pressure. -Unlock and shutdown release the valve and restore a pump started by the clamp. A pump already -running stays on. The clamp serializes its operations, owns a pump change before sending it, -and attempts valve release and pump restoration independently on failure or cancellation. +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. Of the two, one not given is the route +profile's on the motor turning most, scaled down on the other. Without either, 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 that moves the arms and raises Z is preceded by the arms alone, Z held, so they +go back at the depth the path took them across before Z rises. Every other move across, and +`park`, from a pose on a path first goes back along it to its start, every argument checked +before; a move of Z alone there is refused. + +On a v3, `Arm.initialize` (`_initialize_v3`) follows the captured cold start: it seats the arms +against their stops (`_seat_arm_motors`, at most `SEAT_PASSES` passes) and backs them off from +where command 27 reads them, homes Z with the unit's Z values (`_home_z`, its following-error +check and home-done wait as on a v4), repeats both, homes the arms in order 1 then 0 +(`_home_motors`, each waiting for its home-done event), declares the home frame +(`_declare_home_frame`, at `HOME_FRAME_DEVICE_*`) and follows the unit's route to idle at motor +0's homing speed. Following errors are tolerated while seating and homing; other fault bits fail +at once, and the bring-up ends with a strict status check. Failure or cancellation stops all three +motors and logs where they stopped. A v3 stop sends no park move. + +`PlateClamp` forks on `auxiliary_output is None` in `lock`, `unlock`, `request_locked`, +`request_state` and `_release`: board 12 without one, a pressure-board output with one. On an +output, `lock` takes no level, switches the valve on and starts the air pump if needed; readbacks +confirm both control states, never plate contact or pressure, and `request_state` raises +`MantisConfigurationError`. Unlock and shutdown release the valve and restore a pump started by the +clamp; a pump already running stays on. The clamp serializes these operations, owns a pump change +before sending it, and attempts valve release and pump restoration independently on failure or +cancellation. **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` | @@ -221,14 +324,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. @@ -255,16 +359,18 @@ feature has a simulation subclass that overrides its `_unchecked_fw_*` calls and above the wire: `MantisSimulationPlateClamp` reads 0 until a command sets clamping done or unclamping done; `MantisSimulationRfid` reads every slot empty; `MantisSimulationPump` starts off, reads every line at 0 with the recorded calibration, and follows clamp commands for its air-pump -state. `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. -`MantisSimulationPneumaticPlateClamp` holds the valve state and uses the simulated pump. A v3 uses -`MantisSimulationV3Arm`, which requires the unit's homing block and simulates seating against -stops, flags, and the route to idle. 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. +state; `MantisSimulationPlateClamp` also holds a valve's state; `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, and its zones, approach paths and +stage the declared ones, else none. On a v3 it runs the bring-up below the `_unchecked_fw_*` +calls: a retract seats an arm against its stop (status 0x100, 0x104 once homed), the unit's Z +values reach the home command, and every awaited event comes at once. 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 @@ -279,7 +385,11 @@ interface A, the RFID reader's commands. **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 @@ -303,9 +413,8 @@ where a colour took a release from 0.21 s to 0.34 s on the recorded v4. 3. **Three link values are ours, not the vendor's.** The latency timer (2 ms), the read poll (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 +4. **The arm model is partial.** 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 unit of each @@ -370,18 +479,82 @@ 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 v4 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; a v3's arm plays it once a v3 has run its moves. 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. Where the + recording moves the arms and raises Z in one item, the arms move first, Z held: in it the arms + end within the first 2 mm of the rise. 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 65077d5f7c0..d98669e120c 100644 --- a/pylabrobot/formulatrix/mantis/device.py +++ b/pylabrobot/formulatrix/mantis/device.py @@ -2,7 +2,7 @@ import math import os -from typing import Dict, Optional, Union +from typing import Dict, Optional, cast from pylabrobot.io.ftdi import FTDI from pylabrobot.resources import Coordinate, Deck, Resource @@ -19,22 +19,25 @@ from .driver.features.arm import Arm from .driver.features.led_bar import LedBar from .driver.features.plate_clamp import PlateClamp -from .driver.features.pneumatic_plate_clamp import PneumaticPlateClamp from .driver.features.pump import Pump 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): @@ -44,22 +47,30 @@ 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 ( ("right", RIGHT_BASE, right_elbow), ("left", LEFT_BASE, left_elbow), ): + base_x, base_y = cast(float, base.x), cast(float, base.y) + elbow_x, elbow_y = cast(float, elbow.x), cast(float, elbow.y) # The device frame mirrors the arm frame in y, so an angle there turns the other way. - short_angle = -math.degrees(math.atan2(elbow[1] - base[1], elbow[0] - base[0])) - long_angle = -math.degrees(math.atan2(head[1] - elbow[1], head[0] - elbow[0])) + short_angle = -math.degrees(math.atan2(elbow_y - base_y, elbow_x - base_x)) + long_angle = -math.degrees( + math.atan2(cast(float, head.y) - elbow_y, cast(float, head.x) - elbow_x) + ) short, long = self.arm_links[f"{side}_short_arm"], self.arm_links[f"{side}_long_arm"] short.rotate_to(z=short_angle, pivot_coordinate=short.proximal_joint) long.rotate_to(z=long_angle - short_angle, pivot_coordinate=long.proximal_joint) @@ -165,7 +176,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] = {} @@ -175,7 +186,9 @@ def __init__( assert short.distal_joint is not None self.z_column.assign_child_resource( short, - location=Coordinate(ARM_BASE_X + base[0], ARM_BASE_Y - base[1], ARM_Z) + location=Coordinate( + ARM_BASE_X + cast(float, base.x), ARM_BASE_Y - cast(float, base.y), ARM_Z + ) - measured_at["z_column"] - short.proximal_joint, ) @@ -185,11 +198,12 @@ 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[Union[PlateClamp, PneumaticPlateClamp]]: - """The discovered motor or pneumatic plate clamp; None before discovery or without one.""" + def plate_clamp(self) -> Optional[PlateClamp]: + """The plate clamp, on board 12 or on a pressure-board output; None before discovery or + without one.""" return self.driver.plate_clamp @property @@ -220,9 +234,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..d6d98f5d814 100644 --- a/pylabrobot/formulatrix/mantis/device_tests.py +++ b/pylabrobot/formulatrix/mantis/device_tests.py @@ -1,10 +1,15 @@ -"""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 ( + MantisArmCoordinate, + 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 +30,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(location=MantisArmCoordinate(x=15.0, y=60.0, z=None)) + 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..f5025bff836 100644 --- a/pylabrobot/formulatrix/mantis/driver/arm_kinematics.py +++ b/pylabrobot/formulatrix/mantis/driver/arm_kinematics.py @@ -10,10 +10,26 @@ import bisect import math -from typing import Dict, Sequence, Tuple +from dataclasses import dataclass +from typing import Dict, Optional, Sequence, Tuple, cast from .errors import MantisKinematicsError + +@dataclass(frozen=True) +class MantisArmCoordinate: + """A position in the arm frame, mm: the unit's files' frame, not PLR's deck frame. + + ``Coordinate`` has its origin at the deck's front-left-bottom with z up; here the origin is the + base of motor 0's link, and z is Z's position, down from its home flag. An axis is None where a + route line holds it, or where only x and y are meant. + """ + + x: Optional[float] + y: Optional[float] + z: Optional[float] + + # The link geometry, in mm and degrees; the same on a v3 and a v4. RIGHT_SHORT_ARM_LENGTH = 63.0 RIGHT_LONG_ARM_LENGTH = 84.0 @@ -23,11 +39,12 @@ GAMMA = 0.0 MIN_THETA_1 = -93.96 MIN_THETA_2 = -141.786 -# Where each link pair turns, (x, y) in the arm frame, mm: the right pair at the origin. -RIGHT_BASE = (0.0, 0.0) -LEFT_BASE = ( - ARM_DISTANCE * math.cos(math.radians(GAMMA)), - ARM_DISTANCE * math.sin(math.radians(GAMMA)), +# Where each link pair turns, in the arm frame: the right pair at the origin. +RIGHT_BASE = MantisArmCoordinate(x=0.0, y=0.0, z=None) +LEFT_BASE = MantisArmCoordinate( + x=ARM_DISTANCE * math.cos(math.radians(GAMMA)), + y=ARM_DISTANCE * math.sin(math.radians(GAMMA)), + z=None, ) # Each motor's travel per revolution: degrees on the arms, mm on Z. @@ -88,23 +105,27 @@ def compute_wrapped_angle(*, angle: float, minimum: float) -> float: return angle -def compute_joint_angles(*, x: float, y: float) -> Tuple[float, float]: - """The two links' angles that put the head at (x, y): inverse kinematics. +def compute_joint_angles(*, location: MantisArmCoordinate) -> Tuple[float, float]: + """The two links' angles that put the head at a location's x and y: inverse kinematics. The angles are the geometry's own, before a unit's motor offsets. Args: - x: the head's x in the arm frame, in mm. - y: the head's y in the arm frame, in mm. + location: the head's position in the arm frame; its z is not used. Raises: MantisKinematicsError: if (x, y) is out of either link pair's reach or on a base. + ValueError: if x or y is None. """ - base_x, base_y = LEFT_BASE + x, y = location.x, location.y + if x is None or y is None: + raise ValueError("inverse kinematics needs the head's x and y") + # The bases are always given in x and y. + base_x, base_y = cast(float, LEFT_BASE.x), cast(float, LEFT_BASE.y) distance = math.hypot(x, y) distance_to_other_base = math.hypot(x - base_x, y - base_y) if distance == 0 or distance_to_other_base == 0: - raise MantisKinematicsError(f"({x:.3f}, {y:.3f}) mm is on a motor's base") + raise MantisKinematicsError(f"({x:.2f}, {y:.2f}) mm is on a motor's base") first = (RIGHT_SHORT_ARM_LENGTH**2 - RIGHT_LONG_ARM_LENGTH**2 + distance**2) / ( 2 * RIGHT_SHORT_ARM_LENGTH * distance ) @@ -112,7 +133,7 @@ def compute_joint_angles(*, x: float, y: float) -> Tuple[float, float]: 2 * LEFT_SHORT_ARM_LENGTH * distance_to_other_base ) if not -1.0 <= first <= 1.0 or not -1.0 <= second <= 1.0: - raise MantisKinematicsError(f"({x:.3f}, {y:.3f}) mm is out of the arm's reach") + raise MantisKinematicsError(f"({x:.2f}, {y:.2f}) mm is out of the arm's reach") theta_1 = 90 + math.degrees(math.asin(first) - math.atan2(y, x)) theta_2 = 90 - math.degrees(math.pi - math.asin(second) - math.atan2(y - base_y, x - base_x)) return ( @@ -123,27 +144,36 @@ def compute_joint_angles(*, x: float, y: float) -> Tuple[float, float]: def compute_elbow_positions( *, theta_1: float, theta_2: float -) -> Tuple[Tuple[float, float], Tuple[float, float]]: - """Where each short link ends, (x, y) in the arm frame in mm. +) -> Tuple[MantisArmCoordinate, MantisArmCoordinate]: + """Where each short link ends in the arm frame, motor 0's first; z None. Args: theta_1: motor 0's link angle, in degrees, the geometry's own. theta_2: motor 1's link angle, in degrees, the geometry's own. """ - base_x, base_y = LEFT_BASE + # The bases are always given in x and y. + base_x, base_y = cast(float, LEFT_BASE.x), cast(float, LEFT_BASE.y) first = math.radians(180.0 - theta_1) second = math.radians(theta_2) return ( - (RIGHT_SHORT_ARM_LENGTH * math.cos(first), RIGHT_SHORT_ARM_LENGTH * math.sin(first)), - ( - base_x + LEFT_SHORT_ARM_LENGTH * math.cos(second), - base_y + LEFT_SHORT_ARM_LENGTH * math.sin(second), + MantisArmCoordinate( + x=RIGHT_SHORT_ARM_LENGTH * math.cos(first), + y=RIGHT_SHORT_ARM_LENGTH * math.sin(first), + z=None, + ), + MantisArmCoordinate( + x=base_x + LEFT_SHORT_ARM_LENGTH * math.cos(second), + y=base_y + LEFT_SHORT_ARM_LENGTH * math.sin(second), + z=None, ), ) -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. +def compute_head_position(*, theta_1: float, theta_2: float) -> MantisArmCoordinate: + """Where the head is in the arm frame for two link angles, z None: 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. @@ -152,9 +182,10 @@ def compute_head_position(*, theta_1: float, theta_2: float) -> Tuple[float, flo Raises: MantisKinematicsError: if the long links cannot meet at these angles. """ - (first_x, first_y), (second_x, second_y) = compute_elbow_positions( - theta_1=theta_1, theta_2=theta_2 - ) + first, second = compute_elbow_positions(theta_1=theta_1, theta_2=theta_2) + # The elbows are always given in x and y. + first_x, first_y = cast(float, first.x), cast(float, first.y) + second_x, second_y = cast(float, second.x), cast(float, second.y) distance = math.hypot(second_x - first_x, second_y - first_y) if ( distance == 0 @@ -162,20 +193,17 @@ def compute_head_position(*, theta_1: float, theta_2: float) -> Tuple[float, flo or distance < abs(RIGHT_LONG_ARM_LENGTH - LEFT_LONG_ARM_LENGTH) ): raise MantisKinematicsError( - f"the links cannot meet at angles ({theta_1:.3f}, {theta_2:.3f}) degrees" + f"the links cannot meet at angles ({theta_1:.2f}, {theta_2:.2f}) degrees" ) along = (RIGHT_LONG_ARM_LENGTH**2 - LEFT_LONG_ARM_LENGTH**2 + distance**2) / (2 * distance) 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 MantisArmCoordinate( + x=middle_x - across * (second_y - first_y) / distance, + y=middle_y + across * (second_x - first_x) / distance, + z=None, + ) 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..4a8c4279889 100644 --- a/pylabrobot/formulatrix/mantis/driver/arm_kinematics_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/arm_kinematics_tests.py @@ -4,6 +4,7 @@ import unittest from pylabrobot.formulatrix.mantis.driver.arm_kinematics import ( + MantisArmCoordinate, compute_absolute_device_position, compute_corrected_encoder_reading, compute_device_position, @@ -58,20 +59,28 @@ def test_200_device_units_are_one_revolution(self): class KinematicsTests(unittest.TestCase): def test_both_links_at_0_put_the_head_at_idle(self): # The idle position is (15.00, 31.177). - x, y = compute_head_position(theta_1=0.0, theta_2=0.0) - self.assertAlmostEqual(x, 15.0, places=9) - self.assertAlmostEqual(y, 31.177, places=3) + head = compute_head_position(theta_1=0.0, theta_2=0.0) + assert head.x is not None and head.y is not None + self.assertAlmostEqual(head.x, 15.0, places=9) + self.assertAlmostEqual(head.y, 31.177, places=3) + self.assertIsNone(head.z) 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) + theta_1, theta_2 = compute_joint_angles(location=MantisArmCoordinate(x=x, y=y, z=None)) head = compute_head_position(theta_1=theta_1, theta_2=theta_2) - self.assertAlmostEqual(head[0], x, places=6) - self.assertAlmostEqual(head[1], y, places=6) + assert head.x is not None and head.y is not None + self.assertAlmostEqual(head.x, x, places=6) + self.assertAlmostEqual(head.y, y, places=6) def test_idle_is_both_links_at_0(self): - theta_1, theta_2 = compute_joint_angles(x=15.0, y=31.17691453623979) + theta_1, theta_2 = compute_joint_angles( + location=MantisArmCoordinate(x=15.0, y=31.17691453623979, z=None) + ) self.assertAlmostEqual(theta_1, 0.0, places=9) self.assertAlmostEqual(theta_2, 0.0, places=9) @@ -79,7 +88,7 @@ def test_out_of_reach_raises(self): for x, y in ((0.0, 200.0), (0.0, 0.0), (30.0, 0.0)): with self.subTest(x=x, y=y): with self.assertRaises(MantisKinematicsError): - compute_joint_angles(x=x, y=y) + compute_joint_angles(location=MantisArmCoordinate(x=x, y=y, z=None)) class EncoderCorrectionTests(unittest.TestCase): 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..f792558d260 100644 --- a/pylabrobot/formulatrix/mantis/driver/features/arm.py +++ b/pylabrobot/formulatrix/mantis/driver/features/arm.py @@ -3,25 +3,48 @@ 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. + +A v3 has no absolute encoders: its bring-up seats the arms against their stops, homes every motor +on its flag and follows the unit's route to idle, as captured on a v3. + +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 functools import logging +import math import struct -from typing import TYPE_CHECKING, Dict, List, Optional, Tuple +from typing import TYPE_CHECKING, Callable, Dict, List, Literal, Optional, Sequence, Tuple, cast 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 ( + MantisArmCoordinate, 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 +52,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 ..stage_transform import compute_arm_frame_position +from ..v3_arm_configuration import V3ArmConfiguration if TYPE_CHECKING: from ..master import MantisDriver @@ -44,9 +76,17 @@ MOTOR_HOMED = 4 # The following-error bits: a motor that slipped, idle or moving. MOTOR_FOLLOWING_ERROR = 128 | 256 -# The idle position: where every move to idle sends the head, (x, y) in the arm frame, mm; the +# The idle position: where every move to idle sends the head, in the arm frame, Z held; the # same on a v3 and a v4. Each unit's offsets turn it into targets. -POST_HOMING_POSITION = (15.0, 31.177) +POST_HOMING_POSITION = MantisArmCoordinate(x=15.0, y=31.177, z=None) +# 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[MantisArmCoordinate] = [POST_HOMING_POSITION] +FRONT_IDLE_ROUTE: List[MantisArmCoordinate] = [ + MantisArmCoordinate(x=None, y=None, z=0.0), + POST_HOMING_POSITION, + dataclasses.replace(POST_HOMING_POSITION, z=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 +100,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,114 +117,339 @@ MOTOR_STATUS_POLL_INTERVAL = 0.02 MOVE_TIMEOUT = 30.0 HOME_TIMEOUT = 30.0 +# PyLabRobot design choice: how often a v3 bring-up drives the arms toward their stops, at most. +SEAT_PASSES = 8 +# A v3's move of each arm motor to 0 before its home frame is declared, in device units. +HOME_FRAME_DEVICE_SPEED = 500.0 +HOME_FRAME_DEVICE_ACCELERATION = 100.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[MantisArmCoordinate, MantisArmCoordinate]]] = None + """Each zone's (minimum, maximum) corner in the arm frame, z None, by name.""" + safe_xy_routes: Optional[Dict[str, List[MantisArmCoordinate]]] = None + """Each zone's route to its safe XY, the arms only, by name; Unknown included.""" + idle_routes: Optional[Dict[str, List[MantisArmCoordinate]]] = None + """Each zone's route to idle, by name; Unknown included.""" + approach_paths: Optional[Dict[str, List[MantisArmCoordinate]]] = 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``).""" + v3_arm_configuration: Optional[V3ArmConfiguration] = None + """A v3 unit's ``arm`` block: its homing settings, home frame and route to idle.""" + + +@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: - """The arm and Z, built when board 11 answers (a v4). - 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. +class Arm: + """The arm and Z, built on every unit; the generation selects only private plumbing. + + Bringing it up needs the unit's ``arm`` block: on a v4 each arm motor's offset, encoder reference + and encoder correction; on a v3 its homing settings, home frame and route to idle. Without it + nothing is sent and ``initialize`` refuses; it never falls back to another unit's values. A v3 + has no absolute encoders: what needs them raises ``MantisConfigurationError``, and its moves + after the bring-up raise ``NotImplementedError`` until one has run on a v3. + 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 _get_generation(self) -> Literal["v3", "v4"]: + """The device's generation, as discovery read it. + + Raises: + MantisConfigurationError: if it is neither "v3" nor "v4". + """ + generation = self._driver.configuration.generation + if generation == "v3": + return "v3" + if generation == "v4": + return "v4" + raise MantisConfigurationError(f"{self._driver.name}: unknown generation {generation!r}") + + 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 + if configuration.v3_arm_configuration is not None and self._get_generation() == "v4": + raise MantisConfigurationError(f"{driver.name}: a v4 takes no v3 arm block") @property def is_initialized(self) -> bool: """Whether ``initialize`` brought the arm up since the link opened.""" return self._initialized + def _get_v3_arm_configuration(self) -> V3ArmConfiguration: + """The v3 unit's ``arm`` block. + + Raises: + MantisConfigurationError: without it; nothing is sent. + """ + if self.configuration.v3_arm_configuration is None: + raise MantisConfigurationError( + f"{self._driver.name}: no v3 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" + ) + return self.configuration.v3_arm_configuration + @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._get_generation() == "v3": + self._get_v3_arm_configuration() + return + 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. + + Raises: + NotImplementedError: on a v3 after its bring-up: no move after it has run on a v3. + """ + if not self._initialized: + raise MantisMotionError(f"{self._driver.name}: the arm is not brought up; call setup()") + if self._get_generation() == "v3": + raise NotImplementedError(f"{self._driver.name}: a v3 arm moves only in its bring-up for now") + # -- raw firmware calls ---------------------------------------------------- async def _unchecked_fw_clear_motor_faults(self, *, motor: int) -> None: @@ -215,10 +490,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.""" @@ -255,25 +542,31 @@ async def _request_absolute_device_position(self, *, motor: int) -> float: """An arm motor's position from its absolute encoder, in device units. Raises: - MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. + MantisConfigurationError: on a v3, which has no absolute encoders, or without the unit's + ``arm`` block; nothing is sent. """ + if self._get_generation() == "v3": + raise MantisConfigurationError(f"{self._driver.name}: a v3 has no absolute arm encoders") 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. + MantisConfigurationError: on a v3, or without the unit's ``arm`` block; nothing is sent. """ return { motor: compute_position( @@ -282,8 +575,16 @@ async def request_absolute_joint_position(self) -> Dict[int, float]: for motor in ARM_MOTORS } - async def request_joint_position(self) -> Dict[int, float]: - """Where the controller holds each motor: 0 and 1 in degrees, 2 (Z) in mm down from its flag.""" + 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. + + Raises: + MantisMotionError: on a v3 before this connection's bring-up declared where it is. + """ + if self._get_generation() == "v3" and not self._initialized: + raise MantisMotionError( + f"{self._driver.name}: the v3 arm must be initialized before its pose is known" + ) return { motor: compute_position( motor=motor, device_position=await self._driver.request_motor_position(motor=motor) @@ -291,6 +592,20 @@ async def request_joint_position(self) -> Dict[int, float]: for motor in (*ARM_MOTORS, Z_MOTOR) } + async def _request_arm_joint_state(self) -> JointState: + """Where the arm motors are, by the device: a v4's absolute encoders, a v3's motor positions. + + A v3 knows where its arms are only after this connection's bring-up homed them. + + Raises: + MantisConfigurationError: on a v4 without the unit's ``arm`` block; nothing is sent. + MantisMotionError: on a v3 before this connection's bring-up; nothing is sent. + """ + if self._get_generation() == "v4": + return await self.request_absolute_joint_state() + joint_state = await self.request_joint_state() + return {motor: joint_state[motor] for motor in ARM_MOTORS} + async def _request_measured_position(self, *, motor: int) -> float: """Where a motor is by its servo encoder (command 84), measured: an arm motor in degrees, Z in mm down from its flag. The arms' encoders are declared at bring-up.""" @@ -298,47 +613,48 @@ async def _request_measured_position(self, *, motor: int) -> float: motor=motor, device_position=await self._driver.request_encoder_position(motor=motor) ) - def is_xy_reachable(self, *, x: float, y: float) -> bool: - """Whether the head can reach (x, y) in the arm frame, mm. Nothing is clamped. - - Args: - x: the head's x, in mm. - y: the head's y, in mm. - """ - try: - compute_joint_angles(x=x, y=y) - except MantisKinematicsError: - 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]) + assert self.configuration.offsets is not None + return ( + joint_state[0] + self.configuration.offsets[0], + joint_state[1] + self.configuration.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. + def _get_head_position(self, *, joint_state: JointState) -> MantisArmCoordinate: + """Where the head is in the arm frame, for the arm motors' angles in degrees; z is Z's + position where the joint state names it, else None. Raises: - MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. + MantisConfigurationError: without the unit's ``arm`` block. + MantisKinematicsError: if the links cannot meet at those angles. """ - self._check_calibrated() - return self._get_link_angles(joint_position=await self.request_joint_position()) + theta_1, theta_2 = self._get_link_angles(joint_state=joint_state) + head = compute_head_position(theta_1=theta_1, theta_2=theta_2) + return dataclasses.replace(head, z=joint_state.get(Z_MOTOR)) - def _get_head_position(self, *, joint_position: Dict[int, float]) -> Tuple[float, float]: - """Where the head is in the arm frame, mm, for the arm motors' angles in degrees. + def _get_arm_targets(self, *, location: MantisArmCoordinate) -> Dict[int, float]: + """Each arm motor's target, in device units, for the head at a location's x and y: the + links' angles less each motor's offset. Raises: MantisConfigurationError: without the unit's ``arm`` block. - MantisKinematicsError: if the links cannot meet at those angles. + MantisKinematicsError: if (x, y) is out of the arm's reach. """ - theta_1, theta_2 = self._get_link_angles(joint_position=joint_position) - return compute_head_position(theta_1=theta_1, theta_2=theta_2) + self._check_calibrated() + assert self.configuration.offsets is not None + angles = compute_joint_angles(location=location) + return { + motor: compute_device_position( + motor=motor, position=angles[motor] - self.configuration.offsets[motor] + ) + for motor in ARM_MOTORS + } def _get_idle_targets(self) -> Dict[int, float]: """Each arm motor's target at idle, in device units: ``POST_HOMING_POSITION`` less its offset. @@ -346,32 +662,333 @@ def _get_idle_targets(self) -> Dict[int, float]: Raises: MantisConfigurationError: without the unit's ``arm`` block. """ + return self._get_arm_targets(location=POST_HOMING_POSITION) + + 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) -> MantisArmCoordinate: + """A point of the wellplate frame through the unit's stage transform, in the arm frame. + + 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 = self._get_stage_position(x=x, y=y, z=0.0) + pairs.append((model, (cast(float, unit.x), cast(float, 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, unrounded: 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: + stage = self._get_stage_position(x=holder_x, y=holder_y, z=0.0) + return cast(float, stage.x), cast(float, stage.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, unrounded: 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 + deck: Optional[Tuple[float, float]] = 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 + ): + deck = ( + PLATE_HOLDER_LOCATION.x + holder_x, + PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - holder_y, + ) + if deck is None: + 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 + deck = (ARM_BASE_X + model_x, ARM_BASE_Y - model_y) + return deck + + 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; unrounded. + + 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 + stage = 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 cast(float, stage.z) + + def _get_arm_frame_location(self, *, location: Coordinate) -> MantisArmCoordinate: + """A deck-frame location in the arm frame: x and y as ``_get_arm_frame_xy``; z as Z's position + for the nozzle at the location's height above the deck, as ``_get_z_joint_position``. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + """ + x, y = self._get_arm_frame_xy(x=location.x, y=location.y) + z = self._get_z_joint_position(x=location.x, y=location.y, z=location.z) + return MantisArmCoordinate(x=x, y=y, z=z) + + def _get_deck_frame_location(self, *, arm_frame_location: MantisArmCoordinate) -> Coordinate: + """An arm-frame location in the deck frame: x and y as ``_get_deck_frame_xy``; z as the + nozzle's height above the deck for Z at the location's z. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + ValueError: if an axis is None. + """ + x, y, z = arm_frame_location.x, arm_frame_location.y, arm_frame_location.z + if x is None or y is None or z is None: + raise ValueError(f"{arm_frame_location} names no position in the deck frame") + deck_x, deck_y = self._get_deck_frame_xy(x=x, y=y) + # The nozzle's height: Z's position with the nozzle on the deck there, less Z's position. + return Coordinate( + x=deck_x, y=deck_y, z=self._get_z_joint_position(x=deck_x, y=deck_y, z=0.0) - z + ) + + def is_xy_reachable(self, *, x: float, y: float) -> bool: + """Whether the head can reach a deck-frame (x, y), mm, by the arm's inverse kinematics. + Nothing is clamped or sent. + + Args: + x: right of the deck's front-left corner, mm. + y: behind the deck's front-left corner, mm. + + Raises: + MantisConfigurationError: without the unit's ``stage`` block. + """ + try: + arm_x, arm_y = self._get_arm_frame_xy(x=x, y=y) + compute_joint_angles(location=MantisArmCoordinate(x=arm_x, y=arm_y, z=None)) + except MantisKinematicsError: + return False + return True + + 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() - assert self.offsets is not None - x, y = POST_HOMING_POSITION - angles = compute_joint_angles(x=x, y=y) - return { - motor: compute_device_position(motor=motor, position=angles[motor] - self.offsets[motor]) - for motor in ARM_MOTORS - } + 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) + # Z's position read, carried with each point; only the nozzle's height is kept. + nozzle, right, left = ( + self._get_deck_frame_location( + arm_frame_location=dataclasses.replace(point, z=joints[Z_MOTOR]) + ) + for point in (head, right_elbow, left_elbow) + ) + return MantisPose( + right_elbow_joint_location=Coordinate(x=right.x, y=right.y, z=link_z), + left_elbow_joint_location=Coordinate(x=left.x, y=left.y, z=link_z), + head_joint_location=Coordinate(x=nozzle.x, y=nozzle.y, z=link_z), + nozzle_location=nozzle, + joints=joints, + ) - 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. + 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. - MantisKinematicsError: if the links cannot meet at the angles read. + MantisMotionError: on a v3 before this connection's bring-up; nothing is sent. """ 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]) + idle_targets = self._get_idle_targets() + absolute = await self._request_arm_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 +997,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, ...]) -> 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.""" + 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. + + 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) @@ -405,8 +1036,19 @@ def _check_status(self, *, motor: int, status: int) -> None: status=status, ) - async def _wait_for_motors_idle(self, *, timeout: float) -> None: - """Read each motor's status until none moves or homes. + async def _wait_for_motors_idle( + self, + *, + timeout: float, + motors: Tuple[int, ...] = (*ARM_MOTORS, Z_MOTOR), + expected_following_error: bool = False, + ) -> Dict[int, int]: + """Read each motor's status until none moves or homes; return the last read, by motor. + + Args: + timeout: seconds. + motors: the motors read, in order. + expected_following_error: tolerate a following error, as a seat against a stop sets it. Raises: MantisMotionError: if a motor reports a fault. @@ -414,51 +1056,167 @@ async def _wait_for_motors_idle(self, *, timeout: float) -> None: """ deadline = asyncio.get_running_loop().time() + timeout while True: + statuses: Dict[int, int] = {} busy = [] - for motor in (*ARM_MOTORS, Z_MOTOR): + for motor in motors: status = await self._driver.request_motor_status(motor=motor) try: - self._check_status(motor=motor, status=status) + self._check_status( + motor=motor, + status=status & ~MOTOR_FOLLOWING_ERROR if expected_following_error else status, + ) except MantisMotionError: await self._log_where_it_stopped() raise + statuses[motor] = status if status & (MOTOR_MOVING | MOTOR_HOMING): busy.append(motor) if not busy: - return + return statuses if asyncio.get_running_loop().time() >= deadline: await self._log_where_it_stopped() 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]], + expected_following_error: bool = False, + ) -> Dict[int, int]: + """Move each motor to its target, in device units, then wait; return each motor's last + status, by motor. - 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. + expected_following_error: tolerate a following error, as a seat against a stop sets it. + + 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) + self._check_status( + motor=motor, + status=status & ~MOTOR_FOLLOWING_ERROR if expected_following_error else status, + ) + return await self._wait_for_motors_idle( + timeout=MOVE_TIMEOUT, expected_following_error=expected_following_error + ) + except BaseException: + 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: MantisArmCoordinate) -> 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. + """ + arms = route_line.x is not None and route_line.y is not None + targets = self._get_arm_targets(location=route_line) if arms else {} + if route_line.z is not None: + targets[Z_MOTOR] = compute_device_position(motor=Z_MOTOR, position=route_line.z) + for motor, position in targets.items(): + self._check_reachable(motor=motor, position=position) + return targets + + def _get_route_targets(self, *, route: List[MantisArmCoordinate]) -> 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]], + profiles: Optional[Dict[int, Tuple[float, float]]] = None, + ) -> None: + """Move to each step's targets in turn, each motor on its own (command 22), as in the + recorded recoveries. + + Args: + steps: each route line's targets, from ``_get_route_targets``. + profiles: each motor's (speed, acceleration) in device units, by motor; None the route + profile. + """ + if profiles is None: + 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 or is cancelled, the sequencer is aborted so nothing more plays, every + motor is stopped so the item playing ends there, 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: - await self._stop_motors(motors=tuple(sent)) + 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._stop_motors(motors=(*ARM_MOTORS, Z_MOTOR)) 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,31 +1233,85 @@ 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) + head = 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 - if not low <= x <= high: + if not low <= cast(float, head.x) <= high: raise MantisMotionError( - f"{self._driver.name}: the head is at ({x:.1f}, {y:.1f}) mm; it is moved to idle only " - f"from x {low} to {high} mm. Park the arm at idle, then set up again" + f"{self._driver.name}: the head is at ({head.x:.2f}, {head.y:.2f}) mm; it is moved to idle " + f"only from x {low} to {high} mm. Park the arm at idle, then set up again" ) - async def _home_z(self) -> None: + def _get_zone(self, *, location: MantisArmCoordinate) -> str: + """The zone whose rectangle holds a location's x and 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") + # The head and every rectangle's corners give x and y. + x, y = cast(float, location.x), cast(float, location.y) + for zone in ZONES: + minimum, maximum = self.configuration.zone_rectangles[zone] + within_x = cast(float, minimum.x) <= x <= cast(float, maximum.x) + if within_x and cast(float, minimum.y) <= y <= cast(float, maximum.y): + return zone + return UNKNOWN_ZONE + + def _get_routes( + self, *, joint_state: JointState + ) -> Tuple[List[MantisArmCoordinate], List[MantisArmCoordinate]]: + """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: + head = 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(location=head) + logger.info( + "[%s] the head is at (%.2f, %.2f) mm, in the %s zone", self._driver.name, head.x, head.y, zone + ) + return self.configuration.safe_xy_routes[zone], self.configuration.idle_routes[zone] + + async def _home_z( + self, *, speed: float = Z_HOMING_SPEED, acceleration: float = Z_HOMING_ACCELERATION + ) -> None: """Home Z on its flag, as recorded: check no motor slipped, clear Z's faults, search, wait. + Args: + speed: the fast search, mm/s; the slow search at a tenth of it. + acceleration: mm/s². + Raises: MantisMotionError: if a motor reports a following error first, or Z does not read homed. MantisTimeoutError: if Z does not report homing done within ``HOME_TIMEOUT``. @@ -520,7 +1332,7 @@ async def _home_z(self) -> None: code=HOME_DONE_EVENT, match=lambda payload: decode_home_done(payload=payload)[0] == Z_MOTOR, ) - fast_speed = compute_device_rate(motor=Z_MOTOR, rate=Z_HOMING_SPEED) + fast_speed = compute_device_rate(motor=Z_MOTOR, rate=speed) try: await self._unchecked_fw_home( motor=Z_MOTOR, @@ -529,7 +1341,7 @@ async def _home_z(self) -> None: positive_direction=False, slow_speed=fast_speed / 10, fast_speed=fast_speed, - acceleration=compute_device_rate(motor=Z_MOTOR, rate=Z_HOMING_ACCELERATION), + acceleration=compute_device_rate(motor=Z_MOTOR, rate=acceleration), ) except BaseException: waiter.cancel() @@ -544,119 +1356,953 @@ 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. - async def initialize(self) -> None: - """Bring the arm and Z up from power-on, as in the recorded v4 startup. + 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) - 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. + async def _initialize_v4(self) -> None: + """Bring a v4's arm and Z up from power-on, as in the recorded v4 startup. - 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. - MantisTimeoutError: if a move or the home does not end in time. + Energise the arm motors, declare them where their absolute encoders read, set their following + 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. """ - self._check_calibrated() self._initialized = False 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: + await self._unpower_arm_motors() + raise + self._initialized = True + + async def _seat_arm_motors(self) -> None: + """Drive the coupled arms toward their stops until they seat twice, then back them off from + where command 27 reads them and declare them at 0. + + Raises: + MantisMotionError: if they do not seat in ``SEAT_PASSES`` passes, or a motor faults. + ValueError: if a backoff target is outside its motor's limits. + """ + configuration = self._get_v3_arm_configuration() + profiles = { + motor: ( + compute_device_rate(motor=motor, rate=configuration.jog_speed), + compute_device_rate(motor=motor, rate=configuration.jog_acceleration), + ) + for motor in ARM_MOTORS + } + retract_targets = { + motor: compute_device_position(motor=motor, position=configuration.retract) + for motor in ARM_MOTORS + } + for motor in ARM_MOTORS: + await self._unchecked_fw_clear_motor_faults(motor=motor) + seated_once = False + for _ in range(SEAT_PASSES): 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._unchecked_fw_set_motor_position(motor=motor, position=0.0) + statuses = await self._move_motors( + targets=retract_targets, profiles=profiles, expected_following_error=True + ) + if any(statuses[motor] & MOTOR_FOLLOWING_ERROR for motor in ARM_MOTORS): + if seated_once: + break + seated_once = True + for motor in ARM_MOTORS: + await self._unchecked_fw_clear_motor_faults(motor=motor) + else: + raise MantisMotionError(f"{self._driver.name}: the arms did not seat in {SEAT_PASSES} passes") + for motor in ARM_MOTORS: + await self._unchecked_fw_clear_motor_faults(motor=motor) + backoff_targets = { + motor: await self._driver.request_motor_position(motor=motor) + + compute_device_position(motor=motor, position=configuration.backoff) + for motor in ARM_MOTORS + } + for motor, target in backoff_targets.items(): + self._check_reachable(motor=motor, position=target) + await self._move_motors(targets=backoff_targets, profiles=profiles) + for motor in ARM_MOTORS: + await self._unchecked_fw_set_motor_position(motor=motor, position=0.0) + + async def _home_motors(self, *, motors: Tuple[int, ...]) -> None: + """Start each motor's flag search in turn, as the v3 unit's ``arm`` block sets it, then wait + for each one's homing done and require its homed bit. + + An arm motor's search is followed by the block's ``motor_xy_homing_delay``. + + Args: + motors: the motors, in the order their searches start. + + Raises: + MantisMotionError: if a motor faults, or does not read homed. + MantisTimeoutError: if a motor does not report homing done within ``HOME_TIMEOUT``. + """ + configuration = self._get_v3_arm_configuration() + waiters: Dict[int, "asyncio.Future[bytes]"] = {} + try: + for motor in motors: + homing = configuration.motors[motor] + await self._unchecked_fw_clear_motor_faults(motor=motor) + # Bound now: each waiter matches its own motor's homing done. + waiters[motor] = self._driver._expect_event( + board=MOTION_BOARD, + code=HOME_DONE_EVENT, + match=functools.partial( + lambda payload, motor: decode_home_done(payload=payload)[0] == motor, motor=motor + ), + ) + fast_speed = compute_device_rate(motor=motor, rate=homing.speed) + await self._unchecked_fw_home( + motor=motor, + method=homing.method, + positive_edge=homing.positive_edge, + positive_direction=homing.positive_direction, + slow_speed=fast_speed / 10, + fast_speed=fast_speed, + acceleration=compute_device_rate(motor=motor, rate=homing.acceleration), + ) + if motor in ARM_MOTORS: + await asyncio.sleep(configuration.motor_xy_homing_delay) + for motor in motors: + await self._driver._wait_for_event(waiter=waiters[motor], timeout=HOME_TIMEOUT) + except BaseException: + for waiter in waiters.values(): + waiter.cancel() + raise + for motor in motors: + status = await self._driver.request_motor_status(motor=motor) + self._check_status(motor=motor, status=status & ~MOTOR_FOLLOWING_ERROR) + if not status & MOTOR_HOMED: + raise MantisMotionError( + f"{self._driver.name}: motor {motor} did not home (status 0x{status:04x})", + motor=motor, + status=status, + ) + + async def _declare_home_frame(self) -> None: + """Move each arm motor to 0 at the ``HOME_FRAME_DEVICE_*`` profile, then declare it at the v3 + unit's ``homing_position_n``.""" + configuration = self._get_v3_arm_configuration() + for motor in ARM_MOTORS: + await self._move_motors( + targets={motor: 0.0}, + profiles={motor: (HOME_FRAME_DEVICE_SPEED, HOME_FRAME_DEVICE_ACCELERATION)}, + expected_following_error=True, + ) + await self._unchecked_fw_set_motor_position( + motor=motor, + position=compute_device_position( + motor=motor, position=configuration.homing_position_n[motor] + ), + ) + + async def _initialize_v3(self) -> None: + """Bring a v3's arm and Z up, as captured: seat the arms and home Z twice, home the arms, + declare the home frame, then follow the unit's route to idle. + + Every target is checked against its motor's limits before anything is sent. A failure or + cancellation stops all three motors and logs where they stopped before it raises. + """ + configuration = self._get_v3_arm_configuration() + self._initialized = False + route: List[MantisArmCoordinate] = [] + for point in configuration.post_homing_positions: + route += [MantisArmCoordinate(x=None, y=None, z=0.0), point] + steps = self._get_route_targets(route=route) + for motor in ARM_MOTORS: + for position in (configuration.homing_position_n[motor], configuration.retract, 0.0): + self._check_reachable( + motor=motor, position=compute_device_position(motor=motor, position=position) + ) + homing = configuration.motors + # The route at motor 0's homing speed, the arms at its acceleration and Z at its own. + profiles = { + motor: ( + compute_device_rate(motor=motor, rate=homing[0].speed), + compute_device_rate( + motor=motor, rate=homing[Z_MOTOR if motor == Z_MOTOR else 0].acceleration + ), + ) + for motor in (*ARM_MOTORS, Z_MOTOR) + } + try: + await self._wait_for_motors_idle(timeout=MOVE_TIMEOUT, expected_following_error=True) + for pass_index in range(2): + logger.info("[%s] seating the arms, pass %d", self._driver.name, pass_index + 1) + await self._seat_arm_motors() + logger.info("[%s] homing Z", self._driver.name) + await self._home_z(speed=homing[Z_MOTOR].speed, acceleration=homing[Z_MOTOR].acceleration) + logger.info("[%s] homing arm motors together, motor 1 then motor 0", self._driver.name) + await self._home_motors(motors=(1, 0)) + logger.info("[%s] declaring the calibrated home frame", self._driver.name) + await self._declare_home_frame() + logger.info("[%s] following the calibrated route to idle", self._driver.name) + await self._move_along_route(steps=steps, profiles=profiles) + await self._wait_for_motors_idle(timeout=MOVE_TIMEOUT) + except BaseException: + await self._stop_motors(motors=(*ARM_MOTORS, Z_MOTOR)) + await self._log_where_it_stopped() + raise + self._initialized = True + + async def initialize(self) -> None: + """Bring the arm and Z up from power-on: a v4 from where its absolute encoders read, a v3 from + its stops and flags. + + Raises: + MantisConfigurationError: without the unit's ``arm`` block; nothing is sent. + MantisMotionError: if the head is not where it is moved to idle from, or a motor faults. + MantisTimeoutError: if a move or a home does not end in time. + ValueError: on a v3, if a target is outside its motor's limits; nothing is sent. + """ + self._check_calibrated() + if self._get_generation() == "v4": + await self._initialize_v4() + elif self._get_generation() == "v3": + await self._initialize_v3() + + 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: a v4 from its absolute encoders, a v3 by its whole + bring-up again. This moves the arm. + + A failure leaves a v4's arm motors unpowered, a v3's stopped; 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. + NotImplementedError: on a v3, until a v3 has run it; nothing is sent. + """ + 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) + if self._get_generation() == "v4": + await self._recover_to_idle() + elif self._get_generation() == "v3": + await self._initialize_v3() + except BaseException: + if self._get_generation() == "v4": + await self._unpower_arm_motors() raise self._initialized = True + await self._report_pose() - 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 _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:.2f} mm down by its encoder after rising to " + f"{z_joint_position:.2f} 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, unrounded. + + 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 = self._get_head_position(joint_state=await self.request_joint_state()) + points = [self._get_deck_frame_xy(x=cast(float, head.x), y=cast(float, 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[MantisArmCoordinate] + ) -> List[MantisArmCoordinate]: + """Where each line of an approach path leaves the head, in the arm frame, every axis given; an + axis a line leaves out kept from the line before, from idle with Z at its flag.""" + state = dataclasses.replace(POST_HOMING_POSITION, z=0.0) + states = [] + for line in path: + if line.x is not None and line.y is not None: + state = dataclasses.replace(state, x=line.x, y=line.y) + if line.z is not None: + state = dataclasses.replace(state, z=line.z) + states.append(state) + 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. + """ + head = self._get_head_position(joint_state=joint_state) + x, y, z = cast(float, head.x), cast(float, head.y), 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)): + state = states[index] + line_x, line_y = cast(float, state.x), cast(float, state.y) + near = ( + math.hypot(x - line_x, y - line_y) <= APPROACH_PATH_TOLERANCE + and abs(z - cast(float, state.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[MantisArmCoordinate]) -> 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, state in enumerate(states) + if cast(float, state.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[MantisArmCoordinate], second: List[MantisArmCoordinate] + ) -> 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 that moves the arms and raises Z is preceded by the arms' move alone, Z held: the arms + go back at the depth the path took them across, then Z rises, never both at once. + + 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] + empty = MantisArmCoordinate(x=None, y=None, z=None) + steps = [line for line in reversed(path[until:index]) if line != empty] + profiles = self._get_route_profiles() + here = self._get_approach_path_states(path=path)[index] + moves = [] + for line, targets in zip(steps, self._get_route_targets(route=steps)): + arms = line.x is not None and line.y is not None + arms_move = arms and (line.x, line.y) != (here.x, here.y) + if arms_move and line.z is not None and line.z < cast(float, here.z): + arm_targets = {motor: targets[motor] for motor in ARM_MOTORS} + moves.append(self._get_queued_move(targets=arm_targets, profiles=profiles)) + moves.append(self._get_queued_move(targets=targets, profiles=profiles)) + if arms: + here = dataclasses.replace(here, x=line.x, y=line.y) + if line.z is not None: + here = dataclasses.replace(here, z=line.z) + await self._play_queued_moves(moves=moves) + + 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. A speed, given or + the route's, is scaled per motor by its turn, so both motors run the move in the same time; the + nozzle's path is not straight. An acceleration is never scaled down: both motors take the + route's, or the given one as the motor that turns most needs it, as the device refuses a queued + move at too low an acceleration (seen at 18.2 device units/s²). They end within the short + acceleration phase of each other. + + 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_head = self._get_head_position(joint_state=start) + target_head = self._get_head_position(joint_state={**start, **target}) + distance = math.hypot( + cast(float, target_head.x) - cast(float, start_head.x), + cast(float, target_head.y) - cast(float, start_head.y), + ) + turns = {motor: abs(target.get(motor, start[motor]) - start[motor]) for motor in ARM_MOTORS} + largest = max(turns.values()) + if distance > 0 and largest > 0: + motor_acceleration = ( + ROUTE_ARM_ACCELERATION if acceleration is None else acceleration * largest / distance + ) + for motor, turn in turns.items(): + if turn > 0: + rates[motor] = ( + ROUTE_ARM_SPEED * turn / largest if speed is None else speed * turn / distance, + motor_acceleration, + ) + 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, refused on an + approach path. The arms move in one joint move, not along a route: nothing here avoids the + chip racks. From an approach path the arms first leave along it; every check runs before. + + 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 moves. + MantisMotionError: if the arm is not brought up, Z alone is to move on an approach path + (nothing moves), a motor faults, or Z does not read at the traverse height after rising. + 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.request_joint_state() + place = self._get_place(joint_state=start) + on_path = place is not None + if not arm_targets: + if place is not None: + raise MantisMotionError( + f"{self._driver.name}: the head stands on the {place[0]} approach path; Z alone is not " + "moved there. Leave it first: park(), or a move across" + ) + await self._play_queued_moves( + moves=[self._get_queued_move(targets=targets, profiles={Z_MOTOR: z_profile})] + ) + await self._report_pose() + return + if minimum_traverse_height_start is None: + minimum_traverse_height_start = self.default_minimum_traverse_height + # Checked where the head stands before anything moves; on a path, again where leaving it ends. + while True: + end = {**start, **joint_state} + self._get_head_position(joint_state=end) + points: List[Tuple[float, float]] = [] + if minimum_traverse_height_start is not None: + for pose in (start, end): + head = self._get_head_position(joint_state=pose) + points.append(self._get_deck_frame_xy(x=cast(float, head.x), y=cast(float, 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 + ) + if not on_path: + break + start = await self._leave_approach_path_if_on_one(joint_state=start) + on_path = False + 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. + From an approach path the head first leaves along 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(location=MantisArmCoordinate(x=arm_x, y=arm_y, z=None)) + 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. Refused on an approach path. + + 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, the head stands on an approach path + (nothing moves), 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.request_joint_state() + place = self._get_place(joint_state=joint_state) + if place is not None: + raise MantisMotionError( + f"{self._driver.name}: the head stands on the {place[0]} approach path; Z alone is not " + "moved there. Leave it first: park(), or a move across" + ) + head = self._get_head_position(joint_state=joint_state) + head_x, head_y = self._get_deck_frame_xy(x=cast(float, head.x), y=cast(float, head.y)) + z_joint_position = self._get_z_joint_position(x=head_x, y=head_y, z=z) + target = compute_device_position(motor=Z_MOTOR, position=z_joint_position) + 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. From an approach path the head first leaves along it. + + 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_frame_location = self._get_arm_frame_location(location=location) + arm_targets = self._get_arm_targets(location=arm_frame_location) + arm_joint_state = { + motor: compute_position(motor=motor, device_position=target) + for motor, target in arm_targets.items() + } + z_target = cast(float, arm_frame_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.request_joint_state() + on_path = self._get_place(joint_state=start) is not None + # Checked where the head stands before anything moves; on a path, again where leaving it ends. + while True: + head = 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=cast(float, head.x), y=cast(float, head.y)), + (location.x, location.y), + ], + ) + arm_profiles = self._get_arm_profiles( + start=start, target=arm_joint_state, xy_speed=xy_speed, xy_acceleration=xy_acceleration + ) + if not on_path: + break + start = await self._leave_approach_path_if_on_one(joint_state=start) + on_path = False + await self._rise_to_traverse_height(z_joint_position=traverse, profiles={Z_MOTOR: z_profile}) + 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_arm_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; + nothing is sent. + 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. + NotImplementedError: on a v3, until a v3 has run it; nothing is sent. + ValueError: if the traverse height is outside Z's limits; nothing moves. + """ + self._check_initialized() + # Checked where the head stands before anything moves; on a path, again where leaving it ends. + traverse = await self._get_park_traverse_z_joint_position( + minimum_traverse_height=minimum_traverse_height_start + ) + joint_state = await self.request_joint_state() + if self._get_place(joint_state=joint_state) is not None: + await self._leave_approach_path_if_on_one(joint_state=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. + NotImplementedError: on a v3, until a v3 has run it; nothing is sent. + """ + 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. """ - 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) + idle_targets = self._get_route_line_targets(route_line=POST_HOMING_POSITION) + z_up = self._get_route_line_targets(route_line=MantisArmCoordinate(x=None, y=None, z=0.0)) + z_down = self._get_route_line_targets( + route_line=MantisArmCoordinate(x=None, y=None, z=SHUTDOWN_Z) ) - z_held = self._get_queued_target(motor=Z_MOTOR, position=None) - await self._move_motors(targets={Z_MOTOR: 0.0}) + await self._wait_for_motors_idle(timeout=MOVE_TIMEOUT) + 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_arm_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( - f"{self._driver.name}: Z reads {z:.3f} mm below its flag by its encoder after rising to " + f"{self._driver.name}: Z reads {z:.2f} mm below its flag by its encoder after rising to " "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 - every motor stopped. + """Stop every motor at its default deceleration, then park a v4 arm if it was brought up and + every motor stopped; a v3 arm is not parked until a v3 has run its moves. Each stop runs whether or not the one before it succeeded, its failure logged; the arm is no longer brought up after it. """ initialized, self._initialized = self._initialized, False + if self._get_generation() == "v3": + await self._stop_motors(motors=(*ARM_MOTORS, Z_MOTOR)) + return if not await self._stop_motors(motors=(*ARM_MOTORS, Z_MOTOR)): if initialized: logger.warning("[%s] a motor did not stop; the arm is not parked", self._driver.name) diff --git a/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py b/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py index cc7e611d7cb..3bd9c801341 100644 --- a/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/features/arm_tests.py @@ -5,29 +5,38 @@ offsets declared are the ones the recorded idle targets imply, so the moves are sent as recorded. """ +import asyncio 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 from pylabrobot.formulatrix.mantis.driver import master as master_module from pylabrobot.formulatrix.mantis.driver.arm_kinematics import ( + MantisArmCoordinate, 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.arm_v3 import V3Arm +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, @@ -40,13 +49,29 @@ led_bar_frames, make_led_bar_answers, ) -from pylabrobot.formulatrix.mantis.driver.framing import parse_frame +from pylabrobot.formulatrix.mantis.driver.framing import build_frame, parse_frame from pylabrobot.formulatrix.mantis.driver.master_tests import ( RECORDED_MOTOR_ERROR_EVENTS, DriverTestCase, 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_V3_ARM_BLOCK, + SYNTHETIC_WASH_ROUTE, + SYNTHETIC_WASTE_ROUTE, + SYNTHETIC_ZONES_BLOCK, +) +from pylabrobot.formulatrix.mantis.driver.v3_arm_configuration import V3ArmConfiguration +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) @@ -57,7 +82,7 @@ } # A synthetic arm block: the offsets those targets imply, round references, each encoder reading # its own ideal value. -IDLE_ANGLES = compute_joint_angles(x=POST_HOMING_POSITION[0], y=POST_HOMING_POSITION[1]) +IDLE_ANGLES = compute_joint_angles(location=POST_HOMING_POSITION) SYNTHETIC_OFFSETS = { motor: IDLE_ANGLES[motor] - compute_position(motor=motor, device_position=RECORDED_IDLE_TARGETS[motor]) @@ -224,6 +249,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.""" @@ -266,17 +295,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 @@ -360,9 +391,18 @@ async def test_a_motor_error_while_z_homes_fails_at_once_and_unpowers_the_arms(s events={(0, 21, home): [bytes.fromhex(RECORDED_MOTOR_ERROR_EVENTS[0])]}, ) driver = await self.set_up_arm(link=link) - driver.default_read_timeout = 0.25 # unpowering the arms is never answered - with self.assertRaisesRegex(MantisMotionError, "reported error 9") as caught: - await driver.arm.initialize() + send_command = driver._send_command + + async def wait_briefly_for_unpower(**kwargs): + # Unpowering the arms (command 19) is never answered: only it waits briefly, so a slow + # runner cannot time out a command that is answered. + if kwargs["command"] == 19: + kwargs["timeout"] = 0.25 + return await send_command(**kwargs) + + with mock.patch.object(driver, "_send_command", side_effect=wait_briefly_for_unpower): + with self.assertRaisesRegex(MantisMotionError, "reported error 9") as caught: + await driver.arm.initialize() self.assertEqual((caught.exception.motor, caught.exception.code), (0, 9)) self.assertEqual(self.sent(link)[-2:], UNPOWER) @@ -378,11 +418,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,10 +494,12 @@ def test_idle_targets_follow_the_units_offsets(self): for offsets in ({0: 0.0, 1: 0.0}, {0: 1.0, 1: -2.0}): with self.subTest(offsets=offsets): arm = Arm( - driver=mock.Mock(), - offsets=offsets, - homing_position_a=SYNTHETIC_HOMING_POSITION_A, - encoder_correction={0: [], 1: []}, + driver=mock.Mock(**{"configuration.generation": "v4"}), + 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,44 +507,64 @@ 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: []}, + driver=mock.Mock(**{"configuration.generation": "v4"}), + 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.generation": "v4"}), 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.generation": "v4"}), 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(location=MantisArmCoordinate(x=x, y=y, z=None)) + ): _, 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.""" @@ -516,6 +578,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(location=MantisArmCoordinate(x=x, y=y, z=None)) + 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_is_xy_reachable_takes_the_deck_frame_and_sends_nothing(self): + link = RouteLink(answers=make_answers()) + driver = await self.set_up_moves(link=link) + self.assertTrue(driver.arm.is_xy_reachable(x=DECK_POINT[0], y=DECK_POINT[1])) + self.assertFalse(driver.arm.is_xy_reachable(x=500.0, y=500.0)) + self.assertEqual(self.sent(link), []) + + async def test_is_xy_reachable_without_the_stage_block_raises_and_sends_nothing(self): + link = RouteLink(answers=make_answers()) + driver = await self.set_up_arm(link=link) + with self.assertRaisesRegex(MantisConfigurationError, "no stage transform"): + driver.arm.is_xy_reachable(x=DECK_POINT[0], y=DECK_POINT[1]) + 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.generation": "v4"}), + 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) + arm_frame_location = arm._get_arm_frame_location(location=Coordinate(x=x, y=y, z=5.0)) + location = arm._get_deck_frame_location(arm_frame_location=arm_frame_location) + self.assertEqual(location, Coordinate(x=x, y=y, z=5.0)) + + 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_frame_location = arm._get_arm_frame_location(location=Coordinate(x=x, y=y, z=5.0)) + location = arm._get_deck_frame_location(arm_frame_location=arm_frame_location) + self.assertAlmostEqual(location.x, x, places=9) + self.assertAlmostEqual(location.y, y, places=9) + self.assertAlmostEqual(location.z, 5.0, places=9) + + def test_an_xy_speed_scales_each_motor_by_its_turn(self): + # 100 mm/s over the straight line: each motor's speed at its turn over the distance, so both + # run the move in the same time; 500 mm/s² on both as the motor that turns most needs it. + arm = Arm( + driver=mock.Mock(**{"configuration.generation": "v4"}), + 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(location=MantisArmCoordinate(x=15.0, y=31.177, z=None)) + target_angles = compute_joint_angles(location=MantisArmCoordinate(x=-20.0, y=70.0, z=None)) + 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) + turns = {motor: abs(target[motor] - start[motor]) for motor in (0, 1)} + largest = max(turns.values()) + profiles = arm._get_arm_profiles( + start=start, target=target, xy_speed=100.0, xy_acceleration=500.0 + ) + for motor in (0, 1): + with self.subTest(motor=motor): + expected = ( + compute_device_rate(motor=motor, rate=100.0 * turns[motor] / distance), + compute_device_rate(motor=motor, rate=500.0 * largest / 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)) + # Given one, a speed not given is the route's scaled by turn; an acceleration not given is the + # route's on both motors, never scaled down. + for given in ({"xy_speed": 100.0}, {"xy_acceleration": 500.0}): + profiles = arm._get_arm_profiles( + start=start, target=target, **{"xy_speed": None, "xy_acceleration": None, **given} + ) + for motor in (0, 1): + with self.subTest(given=given, motor=motor): + turn = turns[motor] + expected = ( + 100.0 * turn / distance if "xy_speed" in given else 10000.0 * turn / largest, + 500.0 * largest / distance if "xy_acceleration" in given else 1500.0, + ) + for value, rate in zip(profiles[motor], expected): + self.assertAlmostEqual(value, compute_device_rate(motor=motor, rate=rate), places=9) + + def test_a_motor_that_barely_turns_keeps_a_full_acceleration(self): + # On the device a queued move at 18.2 device units/s² was refused (error 6): a motor turning + # 0.25 du against the other's 11.66 never gets a scaled-down acceleration. + arm = Arm( + driver=mock.Mock(**{"configuration.generation": "v4"}), + 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(location=MantisArmCoordinate(x=15.0, y=31.177, z=None)) + start = {0: start_angles[0], 1: start_angles[1], 2: 0.0} + target = { + 0: start[0] + compute_position(motor=0, device_position=0.25), + 1: start[1] + compute_position(motor=1, device_position=11.66), + } + for given in ({"xy_speed": 50.0}, {"xy_speed": 50.0, "xy_acceleration": 200.0}, {}): + profiles = arm._get_arm_profiles( + start=start, target=target, **{"xy_speed": None, "xy_acceleration": None, **given} + ) + with self.subTest(given=given): + self.assertEqual(profiles[0][1], profiles[1][1]) + if "xy_acceleration" not in given: + self.assertEqual(profiles[0][1], 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_location( + location=Coordinate( + x=PLATE_HOLDER_LOCATION.x, y=PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y, z=0.0 + ) + ) + assert corner.x is not None and corner.y is not None + self.assertAlmostEqual( + corner.x, (-50.0 * 1.001 + 40.0 * 0.001 - 0.08) / (-50.0 * -2e-05 + 40.0 * -3e-05 + 1.004) + ) + self.assertAlmostEqual( + corner.y, (-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) + arm_frame_location = arm._get_arm_frame_location( + location=Coordinate( + x=PLATE_HOLDER_LOCATION.x + 10.0, + y=PLATE_HOLDER_LOCATION.y + PLATE_HOLDER_SIZE_Y - 20.0, + z=5.0, + ) + ) + assert arm_frame_location.z is not None + self.assertAlmostEqual(arm_frame_location.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.x, POST_HOMING_POSITION.y + assert x is not None and y is not None + 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(location=MantisArmCoordinate(x=x, y=y, z=None)) + 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], + MantisArmCoordinate(x=None, y=None, z=-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) + + +# Replies from a complete v3 setup and stop, by motor; a repeated status word as (word, count). The +# words are the captured cold start's, in its order, one per read of each phase. +CAPTURED_STATUS_RUNS = { + 0: [ + ("0000", 1), + ("0100", 2), + ("0001", 1), + ("0100", 2), + ("0001", 1), + ("0100", 2), + ("0000", 2), + ("0100", 2), + ("0001", 1), + ("0100", 2), + ("0001", 1), + ("0100", 2), + ("0000", 2), + ("0400", 1), + ("0500", 2), + ("0400", 4), + ("0500", 2), + ("0400", 2), + ("0500", 2), + ("0400", 2), + ("0500", 2), + ("0400", 2), + ], + 1: [ + ("0000", 1), + ("0100", 2), + ("0000", 1), + ("0100", 2), + ("0000", 1), + ("0100", 2), + ("0000", 2), + ("0100", 2), + ("0000", 1), + ("0100", 2), + ("0000", 1), + ("0100", 2), + ("0000", 2), + ("0400", 3), + ("0500", 2), + ("0400", 2), + ("0500", 2), + ("0400", 2), + ("0500", 1), + ("0400", 3), + ("0500", 2), + ("0400", 2), + ], + 2: [("0000", 8), ("0400", 13), ("0500", 1), ("0400", 12)], +} +# board, command, request payload, reply payload; "-" is an empty payload. Each following-error +# configuration read (command 82) answers as captured: enabled, at most 4.0. +CAPTURED_EXCHANGES = """ +0:1:-:34002e00320030002e0036000000 +10:1:-:31002e0030002e0035000000 +0:10:0000:000000000040afc0000000000040af40 +0:10:0100:000000000040afc0000000000040af40 +0:10:0200:00000000004055c000000000008aa140 +10:20:0000:00000040ed05873f000000a0aaa62ac0 +10:20:0100:00000040ed05873f000000a0aaa62ac0 +10:20:0200:00000000d432833f00000020af8741c0 +0:82:0000:01000000000000001040 +0:82:0100:01000000000000001040 +0:82:0200:01000000000000001040 +0:17:0000:- +0:17:0100:- +0:28:00000000000000000000:- +0:28:01000000000000000000:- +0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:17:0000:- +0:17:0100:- +0:28:00000000000000000000:- +0:28:01000000000000000000:- +0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:17:0000:- +0:17:0100:- +0:27:0000:0000000000c03940000000000000000000000000000000000000000000c039400000000000000000000000000088c340 +0:22:0000e4388ee3386e4240b4a291808fb3b540814e1be8b40a8a40:0000 +0:27:0100:000000a01cc73bc000000000000000000000000000000000000000a01cc73bc0000000808fb3b540000000e0b40a8a40 +0:22:0100398ee3d8aaaa30c0b4a291808fb3b540814e1be8b40a8a40:0000 +0:28:00000000000000000000:- +0:28:01000000000000000000:- +0:17:0200:- +0:21:0200000001000000d343867e1e874d4064ea130f737482407d2ae7ab04c2ce40:0000 +0:17:0000:- +0:17:0100:- +0:28:00000000000000000000:- +0:28:01000000000000000000:- +0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:17:0000:- +0:17:0100:- +0:28:00000000000000000000:- +0:28:01000000000000000000:- +0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 +0:17:0000:- +0:17:0100:- +0:27:0000:000000000000f83f00000000000000000000000000000000000000000000f83f0000000000000000000000000088c340 +0:22:00008ee3388ee3382940b4a291808fb3b540814e1be8b40a8a40:0000 +0:27:0100:000000000000f8bf00000000000000000000000000000000000000000000f8bf0000000000000000000000000088c340 +0:22:01008ee3388ee3382340b4a291808fb3b540814e1be8b40a8a40:0000 +0:28:00000000000000000000:- +0:28:01000000000000000000:- +0:17:0200:- +0:21:0200000001000000d343867e1e874d4064ea130f737482407d2ae7ab04c2ce40:0000 +0:17:0100:- +0:21:01000300010001001c7ba7a066391640e359d148c0c74b4025e09b5793b39540:0000 +0:17:0000:- +0:21:00000300010000001c7ba7a066391640e359d148c0c74b4025e09b5793b39540:0000 +0:22:000000000000000000000000000000407f400000000000005940:0000 +0:28:000000000000000049c0:- +0:22:010000000000000000000000000000407f400000000000005940:0000 +0:28:01000000000000005e40:- +0:22:0200000000000000000036496c9706c2ae407d2ae7ab04c2ce40:0000 +0:22:000040877b10d60349c0e359d148c0c74b4025e09b5793b39540:0000 +0:22:0100545f88b1d4794940e359d148c0c74b4025e09b5793b39540:0000 +0:22:0200000000000000000036496c9706c2ae407d2ae7ab04c2ce40:0000 +0:22:0000042e990e845733c0e359d148c0c74b4025e09b5793b39540:0000 +0:22:0100ff8a9491d13f4740e359d148c0c74b4025e09b5793b39540:0000 +0:22:0200000000000000000036496c9706c2ae407d2ae7ab04c2ce40:0000 +0:22:0000aaaaea83a1a3063fe359d148c0c74b4025e09b5793b39540:0000 +0:22:01008ee3d883a1a3063fe359d148c0c74b4025e09b5793b39540:0000 +0:27:0000:000000000000073f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 +0:27:0100:000000000000083f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 +0:27:0200:0000000000000000000000000000000000000000000000000000000000000000000000a006c2ae40000000a004c2ce40 +0:27:0000:000000000000073f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 +0:84:0000:000000008099a93f +0:27:0100:000000000000083f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 +0:84:0100:00000000a047d1bf +0:27:0200:0000000000000000000000000000000000000000000000000000000000000000000000a006c2ae40000000a004c2ce40 +0:84:0200:000000000000d0bf +0:64:-:0000 +0:24:0000814e1be8b40a8a40:0000 +0:24:0100814e1be8b40a8a40:0000 +0:24:0200bb34be4f05c2be40:0000 +""".split() + + +def make_v3_answers(): + """Play each captured reply list in order, repeating its final reply.""" + answers: Dict[Tuple[int, int, bytes], List[bytes]] = {} + for exchange in CAPTURED_EXCHANGES: + board, command, request, reply = exchange.split(":") + key = int(board), int(command), bytes.fromhex(request.replace("-", "")) + answers.setdefault(key, []).append(bytes.fromhex(reply.replace("-", ""))) + for motor, runs in CAPTURED_STATUS_RUNS.items(): + answers[0, 20, struct.pack(" List[Tuple[int, str]]: + """Commands that change a motor, leaving status polling out of comparisons.""" + return [ + (command, payload.hex()) + for _, board, command, payload in (parse_frame(frame=frame) for frame in link.written) + if board == 0 and command in (17, 21, 22, 24, 28) + ] + + +class V3Link(FakeLink): + """The v3 link; boards 11 and 12, which a v3 lacks, refuse at once instead of timing out, so the + tests keep the default read timeout. Each home answers with its captured homing done.""" + + def __init__(self, *, answers: Dict[Tuple[int, int, bytes], List[bytes]]): + """ + Args: + answers: as for ``FakeLink``. + """ + # Each home's request, and the homing done the captured cold start reported for its motor. + home_done = { + "0200000001000000d343867e1e874d4064ea130f737482407d2ae7ab04c2ce40": ( + 81, + "020000000000af90ebbf00000000f470c0bf", + ), + "01000300010001001c7ba7a066391640e359d148c0c74b4025e09b5793b39540": ( + 174, + "0100000000000040614000000000c01a0840", + ), + "00000300010000001c7ba7a066391640e359d148c0c74b4025e09b5793b39540": ( + 174, + "000000000000000060c00000000028170ac0", + ), + } + super().__init__( + answers=answers, + events={ + (0, 21, bytes.fromhex(request)): [ + build_frame( + packet_number=packet_number, board=0, command=0x0202, payload=bytes.fromhex(payload) + ) + ] + for request, (packet_number, payload) in home_done.items() + }, + ) + + async def write(self, data: bytes) -> int: + packet_number, board, _, _ = parse_frame(frame=data) + if board not in (11, 12): + return await super().write(data) + self.written.append(data) + self.incoming.extend(build_frame(packet_number=packet_number, board=board, command=0xFFFF)) + return len(data) + + +class V3ArmTests(DriverTestCase): + def setUp(self): + """Keep the status polls, without waiting between them.""" + super().setUp() + patcher = mock.patch.object(arm_module, "MOTOR_STATUS_POLL_INTERVAL", 0) + patcher.start() + self.addCleanup(patcher.stop) + self.directory = tempfile.TemporaryDirectory() + self.addCleanup(self.directory.cleanup) + self.path = os.path.join(self.directory.name, "unit.json") + with open(self.path, "w", encoding="utf-8") as file: + json.dump({"arm": SYNTHETIC_V3_ARM_BLOCK}, file) + + async def make_arm(self, *, answers=None, calibrated=True): + """Discover through the v3 link, leaving the bring-up to the test.""" + link = V3Link(answers=make_v3_answers() if answers is None else answers) + driver = await self.set_up_driver( + link=link, declared_configuration_json=self.path if calibrated else None + ) + assert driver.arm is not None + self.assertEqual(driver.arm._get_generation(), "v3") + return driver, link + + async def set_up_arm(self): + """Bring the v3 arm up through the v3 link.""" + link = V3Link(answers=make_v3_answers()) + driver = await self.set_up_driver( + link=link, skip_arm=False, declared_configuration_json=self.path + ) + assert driver.arm is not None + return driver, link + + async def test_setup_and_stop_follow_the_captured_v3_sequence(self): + """Seat twice, back off from live positions, home Z twice, then arms 1/0, and walk to idle.""" + driver, link = await self.set_up_arm() + self.assertTrue(driver.arm.is_initialized) + joint_state = await driver.arm.request_joint_state() + head = driver.arm._get_head_position(joint_state=joint_state) + self.assertAlmostEqual(head.x, 15.0, places=3) + self.assertAlmostEqual(head.y, 31.177, places=3) + self.assertAlmostEqual(joint_state[2], 0.0, places=3) + with self.assertRaisesRegex(MantisConfigurationError, "no absolute"): + await driver.arm.request_absolute_joint_state() + await driver.stop() + expected = [] + for exchange in CAPTURED_EXCHANGES: + board, command, request, _ = exchange.split(":") + if int(board) == 0 and int(command) in (17, 21, 22, 24, 28): + expected.append((int(command), request)) + self.assertEqual(get_motion_commands(link=link), expected) + self.assertFalse(driver.arm.is_initialized) + # Each Z home follows a following-error check of every motor, as captured. + homes = [ + (command, payload[:2].hex()) + for _, board, command, payload in (parse_frame(frame=frame) for frame in link.written) + if board == 0 and command in (82, 21) + ] + precheck = [(82, "0000"), (82, "0100"), (82, "0200")] + self.assertEqual( + homes, precheck + [(21, "0200")] + precheck + [(21, "0200"), (21, "0100"), (21, "0000")] + ) + + async def test_stop_sends_three_stops_and_no_park(self): + driver, link = await self.set_up_arm() + sent = len(link.written) + await driver.stop() + stops = [ + payload + for _, board, command, payload in (parse_frame(frame=frame) for frame in link.written[sent:]) + if (board, command) == (0, 24) + ] + self.assertEqual([payload[:2].hex() for payload in stops], ["0000", "0100", "0200"]) + self.assertNotIn( + 22, [command for _, _, command, _ in map(lambda f: parse_frame(frame=f), link.written[sent:])] + ) + + async def test_missing_calibration_refuses_motion_and_pose(self): + """Discovery remains usable without another unit's calibration being substituted.""" + driver, link = await self.make_arm(calibrated=False) + with self.assertRaisesRegex(MantisConfigurationError, "no v3 arm calibration"): + await driver.arm.initialize() + self.assertEqual(get_motion_commands(link=link), []) + with self.assertRaisesRegex(MantisMotionError, "must be initialized"): + await driver.arm.request_joint_state() + + async def test_invalid_route_refuses_before_any_motor_command(self): + """A calibrated route outside a discovered motor limit is rejected before retracting.""" + driver, link = await self.make_arm() + driver.arm.configuration.motor_limits[0] = (-1.0, 1.0) + with self.assertRaisesRegex(ValueError, "outside its limits"): + await driver.arm.initialize() + self.assertEqual(get_motion_commands(link=link), []) + + async def test_failure_to_seat_stops_every_motor(self): + """A retract that never meets a stop must not go on to home Z.""" + answers = make_v3_answers() + for motor in range(3): + answers[0, 20, struct.pack(" int: + if parse_frame(frame=data)[1:] == (0, 22, seat): + link.written.append(data) + raise OSError("device unplugged") + return int(await write(data)) + + link.io.write.side_effect = write_or_fail_the_seat + with self.assertRaisesRegex(OSError, "device unplugged"): + await driver.arm.initialize() + stopped = {payload[:4] for command, payload in get_motion_commands(link=link) if command == 24} + self.assertEqual(stopped, {"0000", "0100", "0200"}) + self.assertFalse(driver.arm.is_initialized) + + async def test_moves_after_the_bring_up_are_not_implemented(self): + """No v3 has run a move after its bring-up: each refuses before sending anything.""" + driver, link = await self.set_up_arm() + arm = driver.arm + moves = { + "move_to_joint_state": lambda: arm.move_to_joint_state(joint_state={2: 1.0}), + "move_to_z_position": lambda: arm.move_to_z_position(z=1.0), + "move_to_xy_position": lambda: arm.move_to_xy_position(x=15.0, y=40.0), + "move_to_location": lambda: arm.move_to_location(location=Coordinate(x=1.0, y=1.0, z=1.0)), + } + sent = len(link.written) + for name, move in moves.items(): + with self.subTest(move=name): + with self.assertRaisesRegex(NotImplementedError, "only in its bring-up"): + await move() + self.assertEqual(len(link.written), sent) + + async def test_park_and_recovery_wait_for_bring_up_then_for_a_v3_run(self): + """Before bring-up nothing is known; after it, every move refuses until a v3 has run it.""" + for set_up, error in ((False, MantisMotionError), (True, NotImplementedError)): + driver, link = await (self.set_up_arm() if set_up else self.make_arm()) + arm = driver.arm + calls = { + "park": lambda: arm.park(), + "move_to_waste": lambda: arm.move_to_waste(), + "recover_from_fault": lambda: arm.recover_from_fault(), + } + sent = len(link.written) + for name, call in calls.items(): + with self.subTest(call=name, set_up=set_up): + with self.assertRaises(error): + await call() + self.assertEqual(len(link.written), sent) + + async def test_parked_is_read_from_the_motors_after_bring_up(self): + """A v3 has no absolute encoders: before bring-up nothing is sent; after it, the motors.""" + driver, link = await self.make_arm() + sent = len(link.written) + with self.assertRaises(MantisMotionError): + await driver.arm.request_is_parked() + self.assertEqual(len(link.written), sent) + driver, _ = await self.set_up_arm() + self.assertTrue(await driver.arm.request_is_parked()) + with self.assertRaises(MantisConfigurationError): + await driver.arm.request_absolute_joint_state() + + async def test_a_v3_block_on_a_v4_is_refused(self): + driver = mock.Mock(**{"configuration.generation": "v4", "name": "Mantis"}) + with self.assertRaisesRegex(MantisConfigurationError, "takes no v3 arm block"): + Arm( + driver=driver, + configuration=MantisArmConfiguration( + v3_arm_configuration=V3ArmConfiguration.from_block(block=SYNTHETIC_V3_ARM_BLOCK) + ), + ) + + async def test_saved_configuration_keeps_the_v3_calibration(self): + """A saved configuration carries the complete home frame and route into another setup.""" + driver, _ = await self.make_arm() + saved = os.path.join(self.directory.name, "saved.json") + driver.save_configuration(path=saved) + with open(saved, encoding="utf-8") as file: + self.assertEqual(json.load(file)["arm"], SYNTHETIC_V3_ARM_BLOCK) + + if __name__ == "__main__": unittest.main() diff --git a/pylabrobot/formulatrix/mantis/driver/features/arm_v3.py b/pylabrobot/formulatrix/mantis/driver/features/arm_v3.py deleted file mode 100644 index 470a69092c5..00000000000 --- a/pylabrobot/formulatrix/mantis/driver/features/arm_v3.py +++ /dev/null @@ -1,293 +0,0 @@ -"""The v3 arm: seat against the stops, home on flags and follow the unit's route to idle.""" - -import asyncio -import logging -from typing import TYPE_CHECKING, Dict, Optional, Tuple - -from pylabrobot.resources import Coordinate - -from ..arm_kinematics import compute_device_position, compute_device_rate, compute_joint_angles -from ..errors import MantisConfigurationError, MantisMotionError, MantisTimeoutError -from ..v3_arm_configuration import V3ArmConfiguration -from .arm import ( - ARM_MOTORS, - HOME_TIMEOUT, - MOTOR_FOLLOWING_ERROR, - MOTOR_HOMED, - MOTOR_HOMING, - MOTOR_MOVING, - MOTOR_STATUS_POLL_INTERVAL, - MOVE_TIMEOUT, - Z_MOTOR, - Arm, -) - -if TYPE_CHECKING: - from ..master import MantisDriver - -logger = logging.getLogger(__name__) - - -class V3Arm(Arm): - """An arm without absolute encoders; homing requires this unit's ``arm`` block. - - Following errors are tolerated during homing and its positioning moves. Other faults fail - immediately. The final status check rejects remaining faults. A failed or cancelled - initialization stops every motor; stop does not park a v3. - """ - - default_prehome_passes: int = 8 - - def __init__(self, *, driver: "MantisDriver", configuration: Optional[V3ArmConfiguration]): - """Take the unit's calibration; None allows discovery but refuses initialization.""" - super().__init__( - driver=driver, - offsets=configuration.offsets if configuration is not None else None, - homing_position_a=None, - encoder_correction=None, - ) - self.configuration = configuration - - def _check_calibrated(self) -> None: - """Refuse motion without the unit's homing calibration.""" - if self.configuration is None: - raise MantisConfigurationError( - f"{self._driver.name}: no v3 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" - ) - - async def _request_absolute_device_position(self, *, motor: int) -> float: - """Refuse: this arm has no absolute encoders, even after homing.""" - raise NotImplementedError("a Mantis v3 has no absolute arm encoders") - - async def request_link_angles(self) -> Tuple[float, float]: - """Read the links only after this connection has established the home frame.""" - if not self.is_initialized: - raise MantisMotionError("the v3 arm must be initialized before its pose is known") - return await super().request_link_angles() - - async def request_head_position(self) -> Coordinate: - """Read the head's pose only after this connection has established the home frame.""" - if not self.is_initialized: - raise MantisMotionError("the v3 arm must be initialized before its pose is known") - return await super().request_head_position() - - def _check_target(self, *, motor: int, target: float) -> None: - """Check a device-unit target against the discovered motor limits.""" - lower, upper = self._driver.configuration.motor_limits[motor] - if not lower <= target <= upper: - raise MantisConfigurationError( - f"motor {motor}'s target {target} is outside its limits ({lower}, {upper})" - ) - - async def _wait_for_axes( - self, *, motors: Tuple[int, ...], timeout: float, expected_following_error: bool = False - ) -> Dict[int, int]: - """Wait for the selected axes, optionally tolerating a homing move's following errors.""" - deadline = asyncio.get_running_loop().time() + timeout - while True: - statuses = {} - for motor in motors: - status = await self._driver.request_motor_status(motor=motor) - checked = status & ~MOTOR_FOLLOWING_ERROR if expected_following_error else status - self._check_status(motor=motor, status=checked) - statuses[motor] = status - if not any(status & (MOTOR_MOVING | MOTOR_HOMING) for status in statuses.values()): - return statuses - if asyncio.get_running_loop().time() >= deadline: - raise MantisTimeoutError( - f"{self._driver.name}: motors {motors} still move after {timeout} s" - ) - await asyncio.sleep(MOTOR_STATUS_POLL_INTERVAL) - - async def _move_axis( - self, *, motor: int, target: float, speed: float, acceleration: float - ) -> None: - """Check and send one move; its caller waits for the coordinated axes.""" - self._check_target(motor=motor, target=target) - await self._unchecked_fw_move_absolute( - motor=motor, position=target, speed=speed, acceleration=acceleration - ) - - async def _prehome(self) -> None: - """Seat the coupled arms twice, then back off relative to each motor's live position.""" - assert self.configuration is not None - calibration = self.configuration - for motor in ARM_MOTORS: - await self._unchecked_fw_clear_motor_faults(motor=motor) - seated_once = False - for _ in range(self.default_prehome_passes): - for motor in ARM_MOTORS: - await self._unchecked_fw_set_motor_position(motor=motor, position=0.0) - for motor in ARM_MOTORS: - await self._move_axis( - motor=motor, - target=compute_device_position(motor=motor, position=calibration.retract), - speed=compute_device_rate(motor=motor, rate=calibration.jog_speed), - acceleration=compute_device_rate(motor=motor, rate=calibration.jog_acceleration), - ) - statuses = await self._wait_for_axes( - motors=ARM_MOTORS, timeout=MOVE_TIMEOUT, expected_following_error=True - ) - if any(status & MOTOR_FOLLOWING_ERROR for status in statuses.values()): - if seated_once: - break - seated_once = True - for motor in ARM_MOTORS: - await self._unchecked_fw_clear_motor_faults(motor=motor) - else: - raise MantisMotionError( - f"{self._driver.name}: the arms did not seat in {self.default_prehome_passes} passes" - ) - for motor in ARM_MOTORS: - await self._unchecked_fw_clear_motor_faults(motor=motor) - for motor in ARM_MOTORS: - position = await self._driver.request_motor_position(motor=motor) - await self._move_axis( - motor=motor, - target=position + compute_device_position(motor=motor, position=calibration.backoff), - speed=compute_device_rate(motor=motor, rate=calibration.jog_speed), - acceleration=compute_device_rate(motor=motor, rate=calibration.jog_acceleration), - ) - await self._wait_for_axes( - motors=ARM_MOTORS, timeout=MOVE_TIMEOUT, expected_following_error=True - ) - for motor in ARM_MOTORS: - await self._unchecked_fw_set_motor_position(motor=motor, position=0.0) - - async def _home_axes(self, *, motors: Tuple[int, ...]) -> None: - """Start flag searches in order, then wait for all axes and require every homed bit.""" - assert self.configuration is not None - for motor in motors: - home = self.configuration.motors[motor] - await self._unchecked_fw_clear_motor_faults(motor=motor) - fast = compute_device_rate(motor=motor, rate=home.speed) - await self._unchecked_fw_home( - motor=motor, - method=home.method, - positive_edge=home.positive_edge, - positive_direction=home.positive_direction, - slow_speed=fast / 10, - fast_speed=fast, - acceleration=compute_device_rate(motor=motor, rate=home.acceleration), - ) - if motor in ARM_MOTORS: - await asyncio.sleep(self.configuration.motor_delay) - await self._wait_for_axes(motors=motors, timeout=HOME_TIMEOUT, expected_following_error=True) - for motor in motors: - status = await self._driver.request_motor_status(motor=motor) - self._check_status(motor=motor, status=status & ~MOTOR_FOLLOWING_ERROR) - if not status & MOTOR_HOMED: - raise MantisMotionError( - f"{self._driver.name}: motor {motor} did not home (status 0x{status:04x})", - motor=motor, - status=status, - ) - - async def _set_home_frame(self) -> None: - """Settle each arm motor at zero, then declare its calibrated home position.""" - assert self.configuration is not None - for motor in ARM_MOTORS: - # The vendor override uses these device-unit rates, independent of the jog rates. - await self._move_axis(motor=motor, target=0.0, speed=500.0, acceleration=100.0) - await self._wait_for_axes( - motors=(motor,), timeout=MOVE_TIMEOUT, expected_following_error=True - ) - await self._unchecked_fw_set_motor_position( - motor=motor, - position=compute_device_position( - motor=motor, position=self.configuration.home_positions[motor] - ), - ) - - def _walk_targets(self) -> Tuple[Dict[int, float], ...]: - """Convert the unit's route to motor positions, subtracting its geometry offsets.""" - assert self.configuration is not None - return tuple( - { - motor: compute_device_position( - motor=motor, position=angle - self.configuration.offsets[motor] - ) - for motor, angle in enumerate(compute_joint_angles(x=x, y=y)) - } - for x, y in self.configuration.post_home_positions - ) - - async def _walk_to_idle(self) -> None: - """Raise Z before each waypoint, then move the two arm motors together.""" - assert self.configuration is not None - home = self.configuration.motors - for targets in self._walk_targets(): - await self._move_axis( - motor=Z_MOTOR, - target=0.0, - speed=compute_device_rate(motor=Z_MOTOR, rate=home[0].speed), - acceleration=compute_device_rate(motor=Z_MOTOR, rate=home[Z_MOTOR].acceleration), - ) - await self._wait_for_axes( - motors=(Z_MOTOR,), timeout=MOVE_TIMEOUT, expected_following_error=True - ) - for motor in ARM_MOTORS: - await self._move_axis( - motor=motor, - target=targets[motor], - speed=compute_device_rate(motor=motor, rate=home[0].speed), - acceleration=compute_device_rate(motor=motor, rate=home[0].acceleration), - ) - await self._wait_for_axes( - motors=ARM_MOTORS, timeout=MOVE_TIMEOUT, expected_following_error=True - ) - - async def initialize(self) -> None: - """Seat the arms and home Z twice, home the arms together, establish the frame and walk to idle. - - Missing calibration or an invalid route refuses before anything moves. A failure or - cancellation stops all three motors and never continues to the next phase. - """ - self._check_calibrated() - assert self.configuration is not None - self._initialized = False - if type(self.default_prehome_passes) is not int or self.default_prehome_passes < 2: - raise ValueError("default_prehome_passes must be an integer of at least 2") - for targets in self._walk_targets(): - for motor, target in targets.items(): - self._check_target(motor=motor, target=target) - for motor in ARM_MOTORS: - self._check_target( - motor=motor, - target=compute_device_position( - motor=motor, position=self.configuration.home_positions[motor] - ), - ) - self._check_target( - motor=motor, - target=compute_device_position(motor=motor, position=self.configuration.retract), - ) - self._check_target(motor=motor, target=0.0) - self._check_target(motor=Z_MOTOR, target=0.0) - try: - await self._wait_for_axes( - motors=(*ARM_MOTORS, Z_MOTOR), timeout=MOVE_TIMEOUT, expected_following_error=True - ) - for pass_index in range(2): - logger.info("[%s] seating the arms, pass %d", self._driver.name, pass_index + 1) - await self._prehome() - logger.info("[%s] homing Z", self._driver.name) - await self._home_axes(motors=(Z_MOTOR,)) - logger.info("[%s] homing arm motors together, motor 1 then motor 0", self._driver.name) - await self._home_axes(motors=(1, 0)) - logger.info("[%s] declaring the calibrated home frame", self._driver.name) - await self._set_home_frame() - logger.info("[%s] following the calibrated route to idle", self._driver.name) - await self._walk_to_idle() - await self._wait_for_axes(motors=(*ARM_MOTORS, Z_MOTOR), timeout=MOVE_TIMEOUT) - except BaseException: - await self._stop_motors(motors=(*ARM_MOTORS, Z_MOTOR)) - await self._log_where_it_stopped() - raise - self._initialized = True - - async def _halt(self) -> None: - """Stop all axes without a park move; a reconnect must establish the home frame again.""" - self._initialized = False - await self._stop_motors(motors=(*ARM_MOTORS, Z_MOTOR)) diff --git a/pylabrobot/formulatrix/mantis/driver/features/arm_v3_tests.py b/pylabrobot/formulatrix/mantis/driver/features/arm_v3_tests.py deleted file mode 100644 index 44946f0b1ae..00000000000 --- a/pylabrobot/formulatrix/mantis/driver/features/arm_v3_tests.py +++ /dev/null @@ -1,339 +0,0 @@ -"""V3 arm initialization against recorded device replies, with synthetic home references.""" - -import asyncio -import json -import os -import struct -import tempfile -from typing import Dict, List, Tuple -from unittest import mock - -from pylabrobot.formulatrix.mantis.driver.errors import MantisConfigurationError, MantisMotionError -from pylabrobot.formulatrix.mantis.driver.features import arm_v3 as arm_v3_module -from pylabrobot.formulatrix.mantis.driver.features.arm_v3 import V3Arm -from pylabrobot.formulatrix.mantis.driver.framing import build_frame, parse_frame -from pylabrobot.formulatrix.mantis.driver.master_tests import DriverTestCase, FakeLink - -SYNTHETIC_V3_ARM_BLOCK = { - "offsets": {"0": 0.0, "1": 0.0}, - "home_positions": {"0": -90.0, "1": 216.0}, - "motors": { - "0": { - "method": 3, - "positive_edge": True, - "positive_direction": False, - "speed": 100.0, - "acceleration": 2500.0, - }, - "1": { - "method": 3, - "positive_edge": True, - "positive_direction": True, - "speed": 100.0, - "acceleration": 2500.0, - }, - "2": { - "method": 0, - "positive_edge": True, - "positive_direction": False, - "speed": 15.0, - "acceleration": 400.0, - }, - }, - "retract": -50.0, - "backoff": 20.0, - "jog_speed": 10000.0, - "jog_acceleration": 1500.0, - "motor_delay": 0.2, - "post_home_positions": [[-38.403, 11.677], [-45.77, 47.82], [15.0, 31.177]], -} -# Replies from a complete v3 setup and stop. Repeated status words are stored as (word, count). -RECORDED_STATUS_RUNS = { - 0: [ - ("0000", 1), - ("0100", 3), - ("0001", 1), - ("0100", 2), - ("0001", 7), - ("0100", 5), - ("0000", 2), - ("0100", 8), - ("0000", 1), - ("0100", 1), - ("0101", 1), - ("0001", 2), - ("0100", 5), - ("0000", 1), - ("0300", 103), - ("0400", 28), - ("0500", 13), - ("0400", 1), - ("0500", 1), - ("0400", 28), - ("0500", 14), - ("0400", 1), - ("0500", 9), - ("0400", 14), - ], - 1: [ - ("0000", 1), - ("0100", 3), - ("0001", 1), - ("0100", 8), - ("0000", 1), - ("0100", 6), - ("0000", 1), - ("0100", 4), - ("0001", 5), - ("0100", 2), - ("0101", 1), - ("0001", 1), - ("0100", 5), - ("0000", 1), - ("0300", 129), - ("0400", 2), - ("0500", 13), - ("0400", 1), - ("0500", 28), - ("0400", 1), - ("0500", 3), - ("0400", 12), - ("0500", 20), - ("0400", 3), - ], - 2: [("0000", 1), ("0300", 143), ("0400", 2), ("0300", 3), ("0400", 7)], -} -# board, command, request payload, reply payload; "-" is an empty payload. -RECORDED_EXCHANGES = """ -0:1:-:34002e00320030002e0036000000 -10:1:-:31002e0030002e0035000000 -0:10:0000:000000000040afc0000000000040af40 -0:10:0100:000000000040afc0000000000040af40 -0:10:0200:00000000004055c000000000008aa140 -10:20:0000:00000040ed05873f000000a0aaa62ac0 -10:20:0100:00000040ed05873f000000a0aaa62ac0 -10:20:0200:00000000d432833f00000020af8741c0 -0:17:0000:- -0:17:0100:- -0:28:00000000000000000000:- -0:28:01000000000000000000:- -0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:17:0000:- -0:17:0100:- -0:28:00000000000000000000:- -0:28:01000000000000000000:- -0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:17:0000:- -0:17:0100:- -0:27:0000:0000000000c03940000000000000000000000000000000000000000000c039400000000000000000000000000088c340 -0:22:0000e4388ee3386e4240b4a291808fb3b540814e1be8b40a8a40:0000 -0:27:0100:000000a01cc73bc000000000000000000000000000000000000000a01cc73bc0000000808fb3b540000000e0b40a8a40 -0:22:0100398ee3d8aaaa30c0b4a291808fb3b540814e1be8b40a8a40:0000 -0:28:00000000000000000000:- -0:28:01000000000000000000:- -0:17:0200:- -0:21:0200000001000000d343867e1e874d4064ea130f737482407d2ae7ab04c2ce40:0000 -0:17:0000:- -0:17:0100:- -0:28:00000000000000000000:- -0:28:01000000000000000000:- -0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:17:0000:- -0:17:0100:- -0:28:00000000000000000000:- -0:28:01000000000000000000:- -0:22:0000721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:22:0100721cc7711cc73bc0b4a291808fb3b540814e1be8b40a8a40:0000 -0:17:0000:- -0:17:0100:- -0:27:0000:000000000000f83f00000000000000000000000000000000000000000000f83f0000000000000000000000000088c340 -0:22:00008ee3388ee3382940b4a291808fb3b540814e1be8b40a8a40:0000 -0:27:0100:000000000000f8bf00000000000000000000000000000000000000000000f8bf0000000000000000000000000088c340 -0:22:01008ee3388ee3382340b4a291808fb3b540814e1be8b40a8a40:0000 -0:28:00000000000000000000:- -0:28:01000000000000000000:- -0:17:0200:- -0:21:0200000001000000d343867e1e874d4064ea130f737482407d2ae7ab04c2ce40:0000 -0:17:0100:- -0:21:01000300010001001c7ba7a066391640e359d148c0c74b4025e09b5793b39540:0000 -0:17:0000:- -0:21:00000300010000001c7ba7a066391640e359d148c0c74b4025e09b5793b39540:0000 -0:22:000000000000000000000000000000407f400000000000005940:0000 -0:28:000000000000000049c0:- -0:22:010000000000000000000000000000407f400000000000005940:0000 -0:28:01000000000000005e40:- -0:22:0200000000000000000036496c9706c2ae407d2ae7ab04c2ce40:0000 -0:22:000040877b10d60349c0e359d148c0c74b4025e09b5793b39540:0000 -0:22:0100545f88b1d4794940e359d148c0c74b4025e09b5793b39540:0000 -0:22:0200000000000000000036496c9706c2ae407d2ae7ab04c2ce40:0000 -0:22:0000042e990e845733c0e359d148c0c74b4025e09b5793b39540:0000 -0:22:0100ff8a9491d13f4740e359d148c0c74b4025e09b5793b39540:0000 -0:22:0200000000000000000036496c9706c2ae407d2ae7ab04c2ce40:0000 -0:22:0000aaaaea83a1a3063fe359d148c0c74b4025e09b5793b39540:0000 -0:22:01008ee3d883a1a3063fe359d148c0c74b4025e09b5793b39540:0000 -0:27:0000:000000000000073f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 -0:27:0100:000000000000083f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 -0:27:0200:0000000000000000000000000000000000000000000000000000000000000000000000a006c2ae40000000a004c2ce40 -0:27:0000:000000000000073f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 -0:84:0000:000000008099a93f -0:27:0100:000000000000083f00000000000000000000000000000000000000000000073f00000040c0c74b400000006093b39540 -0:84:0100:00000000a047d1bf -0:27:0200:0000000000000000000000000000000000000000000000000000000000000000000000a006c2ae40000000a004c2ce40 -0:84:0200:000000000000d0bf -0:64:-:0000 -0:24:0000814e1be8b40a8a40:0000 -0:24:0100814e1be8b40a8a40:0000 -0:24:0200bb34be4f05c2be40:0000 -""".split() - - -def make_v3_answers(): - """Play each recorded reply list in order, repeating its final reply.""" - answers: Dict[Tuple[int, int, bytes], List[bytes]] = {} - for exchange in RECORDED_EXCHANGES: - board, command, request, reply = exchange.split(":") - key = int(board), int(command), bytes.fromhex(request.replace("-", "")) - answers.setdefault(key, []).append(bytes.fromhex(reply.replace("-", ""))) - for motor, runs in RECORDED_STATUS_RUNS.items(): - answers[0, 20, struct.pack(" int: - packet_number, board, _, _ = parse_frame(frame=data) - if board not in (11, 12): - return await super().write(data) - self.written.append(data) - self.incoming.extend(build_frame(packet_number=packet_number, board=board, command=0xFFFF)) - return len(data) - - -class V3ArmTests(DriverTestCase): - def setUp(self): - """Keep recorded status polls, without waiting between them.""" - super().setUp() - patcher = mock.patch.object(arm_v3_module, "MOTOR_STATUS_POLL_INTERVAL", 0) - patcher.start() - self.addCleanup(patcher.stop) - self.directory = tempfile.TemporaryDirectory() - self.addCleanup(self.directory.cleanup) - self.path = os.path.join(self.directory.name, "unit.json") - with open(self.path, "w", encoding="utf-8") as file: - json.dump({"arm": SYNTHETIC_V3_ARM_BLOCK}, file) - - async def make_arm(self, *, answers=None, calibrated=True): - """Discover through the recorded transport, deferring initialization for the test.""" - link = V3Link(answers=make_v3_answers() if answers is None else answers) - driver = await self.set_up_driver( - link=link, declared_configuration_json=self.path if calibrated else None - ) - self.assertIsInstance(driver.arm, V3Arm) - return driver, link - - async def test_setup_and_stop_follow_the_recorded_v3_sequence(self): - """Seat twice, use live backoffs, home Z twice, then arms 1/0, and walk to idle.""" - link = V3Link(answers=make_v3_answers()) - driver = await self.set_up_driver( - link=link, skip_arm=False, declared_configuration_json=self.path - ) - assert driver.arm is not None - self.assertTrue(driver.arm.is_initialized) - position = await driver.arm.request_head_position() - self.assertAlmostEqual(position.x, 15.0, places=3) - self.assertAlmostEqual(position.y, 31.177, places=3) - self.assertAlmostEqual(position.z, 0.0, places=3) - with self.assertRaisesRegex(NotImplementedError, "no absolute"): - await driver.arm.request_absolute_joint_position() - await driver.stop() - expected = [] - for exchange in RECORDED_EXCHANGES: - board, command, request, _ = exchange.split(":") - if int(board) == 0 and int(command) in (17, 21, 22, 24, 28): - expected.append((int(command), request)) - self.assertEqual(motion_commands(link), expected) - self.assertFalse(driver.arm.is_initialized) - - async def test_missing_calibration_refuses_motion_and_pose(self): - """Discovery remains usable without another unit's calibration being substituted.""" - driver, link = await self.make_arm(calibrated=False) - with self.assertRaisesRegex(MantisConfigurationError, "no v3 arm calibration"): - await driver.arm.initialize() - self.assertEqual(motion_commands(link), []) - with self.assertRaisesRegex(MantisMotionError, "must be initialized"): - await driver.arm.request_head_position() - - async def test_invalid_route_refuses_before_any_motor_command(self): - """A calibrated route outside a discovered motor limit is rejected before retracting.""" - driver, link = await self.make_arm() - driver.configuration.motor_limits[0] = (-1.0, 1.0) - with self.assertRaisesRegex(MantisConfigurationError, "outside its limits"): - await driver.arm.initialize() - self.assertEqual(motion_commands(link), []) - - async def test_failure_to_seat_stops_every_motor(self): - """A retract that never encounters a stop must not proceed to home Z.""" - answers = make_v3_answers() - for motor in range(3): - answers[0, 20, struct.pack(" (PWM duty, timeout_ms), as declared; None when - none are declared, and then ``lock`` refuses. + levels: a clamp on board 12: the unit's lock levels, level -> (PWM duty, timeout_ms), as + declared; None when none are declared, and then ``lock`` refuses. + auxiliary_output: a clamp on a pressure-board output: the output switching its valve. + pump: the pump supplying that valve's air; given with ``auxiliary_output`` only. + + Raises: + ValueError: unless given ``levels`` alone, nothing, or ``auxiliary_output`` with ``pump``; + or if ``auxiliary_output`` is not 2. """ + if (auxiliary_output is None) != (pump is None) or ( + auxiliary_output is not None and levels is not None + ): + raise ValueError("a plate clamp takes levels, or auxiliary_output together with pump") + if auxiliary_output is not None: + if type(auxiliary_output) is not int: + raise ValueError("auxiliary_output must be an integer") + driver._check_index(name="auxiliary_output", value=auxiliary_output, allowed=(2,)) self._driver = driver self.levels = levels + self._auxiliary_output = auxiliary_output + self._pump = pump + self._started_pump = False + self._operation_lock = asyncio.Lock() + + @property + def auxiliary_output(self) -> Optional[int]: + """The pressure-board output switching the clamp's valve; None for a clamp on board 12.""" + return self._auxiliary_output async def _unchecked_fw_do_plate_unclamping(self) -> None: """DoPlateUnclamping (board 12, command 31), sent once; its payload is fixed.""" @@ -71,30 +109,132 @@ async def _unchecked_fw_do_plate_clamping(self, *, pwm: float, timeout_ms: int) attempts=1, ) + async def _unchecked_fw_set_auxiliary_output(self, *, on: bool) -> None: + """Switch the clamp's valve (board 10, command 42), sent once. + + Args: + on: whether the valve holds the plate. + """ + await self._driver._send_command( + board=PRESSURE_BOARD, + command=42, + payload=struct.pack(" int: """The clamp's state (GetMotorClampState); ``CLAMP_STATE_DESCRIPTIONS`` says what each means. + Clamping done (3) means the lock's timer ran out, not that a plate is held: a device test read + the same states at the same times with and without a plate. + Args: attempts: how often the read is sent before giving up. + + Raises: + MantisConfigurationError: for a clamp on a pressure-board output, which has no state; + nothing is sent. """ + if self.auxiliary_output is not None: + raise MantisConfigurationError( + f"{self._driver.name}: a clamp on a pressure-board output has no clamp state; " + "request_locked reads its valve" + ) reply = await self._driver._send_command(board=LC3_BOARD, command=33, attempts=attempts) if len(reply) < 2: raise FrameError(f"{self._driver.name}: a clamp state reply is 2 bytes, got {reply.hex()}") (state,) = struct.unpack_from(" bool: + """Whether the clamp's valve is switched on (board 10, command 41); not plate contact. + + Args: + attempts: how often the read is sent before giving up. + + Raises: + FrameError: if the reply is shorter than 2 bytes or not 0 or 1. + """ + reply = await self._driver._send_command( + board=PRESSURE_BOARD, + command=41, + payload=struct.pack(" bool: - """Whether the motor controller reports clamping done.""" - return await self.request_state(attempts=attempts) == CLAMPING_DONE + """Whether the clamp holds: on board 12 whether it reports clamping done; on a pressure-board + output whether its valve reads switched on, which is not plate contact. + + Args: + attempts: how often the read is sent before giving up. + """ + if self.auxiliary_output is None: + return await self.request_state(attempts=attempts) == CLAMPING_DONE + return await self._request_auxiliary_output(attempts=attempts) + + async def _set_locked(self, *, locked: bool) -> None: + """Switch the valve only when needed, and require its readback to agree. + + Args: + locked: whether the valve holds the plate. + """ + if await self._request_auxiliary_output() == locked: + return + await self._unchecked_fw_set_auxiliary_output(on=locked) + actual = await self._request_auxiliary_output() + if actual != locked: + raise PlateClampError( + f"{self._driver.name}: clamp output did not become {locked}", state=int(actual) + ) + + async def _restore_pump(self) -> None: + """Turn off a pump this clamp started; it stays this clamp's if that fails.""" + if self._started_pump and self._pump is not None: + await self._pump._set_pump_on(on=False) + self._started_pump = False + + async def _release_after_failure(self) -> None: + """Try both the valve's release and the pump's restoration, keeping the operation's error.""" + try: + await self._unchecked_fw_set_auxiliary_output(on=False) + if await self._request_auxiliary_output(attempts=1): + raise PlateClampError( + f"{self._driver.name}: clamp output remains on after release", state=1 + ) + except Exception: + logger.warning("[%s] could not release the clamp valve", self._driver.name, exc_info=True) + try: + await self._restore_pump() + except Exception: + logger.warning("[%s] could not restore the clamp's pump", self._driver.name, exc_info=True) async def unlock(self) -> None: - """Release the clamp and wait until it reads unclamping done. + """Release the clamp: on board 12 wait until it reads unclamping done; on a pressure-board + output switch the valve off and restore a pump this clamp started. Raises: - PlateClampError: at once, if it reads a state in ``UNCLAMPING_FAILED``. + PlateClampError: at once, if it reads a state in ``UNCLAMPING_FAILED``, or the valve does not + read back off. MantisTimeoutError: if it does not read unclamping done within ``CLAMP_WAIT_TRIES`` times ``UNCLAMPING_TIMEOUT``, polled every ``CLAMP_STATE_POLL_INTERVAL``. """ + if self.auxiliary_output is not None: + async with self._operation_lock: + try: + await self._set_locked(locked=False) + except BaseException: + await self._release_after_failure() + raise + await self._restore_pump() + logger.info("[%s] clamp valve released", self._driver.name) + return await self._unchecked_fw_do_plate_unclamping() state = await self.request_state() for _ in range(CLAMP_WAIT_TRIES * round(UNCLAMPING_TIMEOUT / CLAMP_STATE_POLL_INTERVAL)): @@ -114,25 +254,53 @@ async def unlock(self) -> None: ) async def _release(self) -> None: - """Release at shutdown unless the motor already reports unclamping done.""" - if await self.request_state(attempts=1) != UNCLAMPING_DONE: - await self.unlock() + """Release at shutdown: on board 12 unless it already reads unclamping done; on a + pressure-board output always, with any pump restoration still outstanding.""" + if self.auxiliary_output is None: + if await self.request_state(attempts=1) != UNCLAMPING_DONE: + await self.unlock() + return + await self.unlock() async def lock(self, *, level: Optional[int] = None) -> None: - """Release the clamp, lock it at ``level`` and wait until it reads clamping done. + """On board 12, release the clamp, lock it at ``level`` and wait until it reads clamping done; + if it does not, it is released again before raising. On a pressure-board output, switch the + valve on and start the pump if it is off; a failure or cancellation releases the valve and + restores a pump this clamp started before raising. - If it does not, the clamp is released again before raising. + On board 12 clamping done is the lock's timer running out; it does not tell a plate is held. Args: - level: one of ``levels``, 1 the weakest. ``default_lock_level`` when None. + level: one of ``levels``, 1 the weakest. ``default_lock_level`` when None. None only on a + pressure-board output, which has no levels. Raises: MantisConfigurationError: if no levels are declared; nothing is sent. - ValueError: if ``level`` is not in ``levels``; nothing is sent. - PlateClampError: at once, after the release, if it reads a state in ``CLAMPING_FAILED``. + ValueError: if ``level`` is not in ``levels``, or is given on a pressure-board output; + nothing is sent. + PlateClampError: at once, after the release, if it reads a state in ``CLAMPING_FAILED``, or + the valve does not read back on. MantisTimeoutError: if it does not read clamping done within ``CLAMP_WAIT_TRIES`` times ``CLAMPING_TIMEOUT``, polled every ``CLAMP_STATE_POLL_INTERVAL``. """ + if self.auxiliary_output is not None: + if level is not None: + raise ValueError("a clamp on a pressure-board output has no lock levels; omit level") + if self._pump is None: + raise MantisConfigurationError(f"{self._driver.name}: no pump supplies the clamp's air") + async with self._operation_lock: + pump_on = await self._pump.request_pump_on() + try: + await self._set_locked(locked=True) + if not pump_on: + # Own the change before sending: a cancelled or unanswered write may still take effect. + self._started_pump = True + await self._pump._set_pump_on(on=True) + except BaseException: + await self._release_after_failure() + raise + logger.info("[%s] clamp valve engaged, air pump on", self._driver.name) + return if self.levels is None: raise MantisConfigurationError( f"{self._driver.name}: no clamp levels for this unit or its generation; give its " diff --git a/pylabrobot/formulatrix/mantis/driver/features/plate_clamp_tests.py b/pylabrobot/formulatrix/mantis/driver/features/plate_clamp_tests.py index 136b38887a9..298e323ad64 100644 --- a/pylabrobot/formulatrix/mantis/driver/features/plate_clamp_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/features/plate_clamp_tests.py @@ -1,17 +1,24 @@ -"""Plate clamp tests: the bytes sent, against clamp states recorded from a Mantis v4.""" +"""Plate clamp tests: the bytes sent, against clamp states recorded from a Mantis v4, and a clamp +on a pressure-board output against recorded pressure-board replies.""" +import asyncio +import json +import os +import tempfile import unittest from typing import List, Tuple from unittest import mock from pylabrobot.formulatrix.mantis.device import RECORDING_MANTIS_V4 +from pylabrobot.formulatrix.mantis.driver.configuration import RECORDING_MANTIS_V3 from pylabrobot.formulatrix.mantis.driver.errors import ( MantisConfigurationError, MantisTimeoutError, ) from pylabrobot.formulatrix.mantis.driver.features import plate_clamp +from pylabrobot.formulatrix.mantis.driver.features.arm import Arm from pylabrobot.formulatrix.mantis.driver.features.plate_clamp import PlateClamp -from pylabrobot.formulatrix.mantis.driver.framing import parse_frame +from pylabrobot.formulatrix.mantis.driver.framing import build_frame, parse_frame from pylabrobot.formulatrix.mantis.driver.master_tests import DriverTestCase, FakeLink, make_answers RELEASE = (12, 31, bytes.fromhex("000000000000e83f000000000000000088130000")) @@ -158,6 +165,255 @@ async def test_built_only_when_the_lc3_board_answers(self): ) self.assertEqual(driver.plate_clamp is not None, built) + async def test_declared_levels_where_board_12_does_not_answer_are_refused(self): + self.default_read_timeout = 0.25 # board 12 never answers + with tempfile.TemporaryDirectory() as directory: + path = os.path.join(directory, "unit.json") + with open(path, "w", encoding="utf-8") as file: + json.dump({"plate_clamp": {"levels": {"1": [0.119, 2000]}}}, file) + driver = self.make_driver( + link=FakeLink(answers=make_answers(absent_boards=(12,))), declared_configuration_json=path + ) + with self.assertRaisesRegex(MantisConfigurationError, "board 12 does not answer"): + await driver.setup(skip_arm=True) + + +PNEUMATIC_CLAMP_BLOCK = {"controller": "pressure_regulator", "auxiliary_output": 2} +# Board, command, payload and replies from the pneumatic clamp and its air pump. +VALVE_READ = (10, 41, bytes.fromhex("0200")) +VALVE_ON = (10, 42, bytes.fromhex("02000100")) +VALVE_OFF = (10, 42, bytes.fromhex("02000000")) +PUMP_READ = (10, 14, b"") +PUMP_ON = (10, 15, bytes.fromhex("0100")) +PUMP_OFF = (10, 15, bytes.fromhex("0000")) +REPLY_OFF, REPLY_ON, REPLY_SET = bytes.fromhex("0000"), bytes.fromhex("0100"), b"" + + +class ClampLink(FakeLink): + """Answer with recorded readbacks for the states selected by accepted commands; an absent + board refuses at once instead of timing out, so the tests keep the default read timeout.""" + + def __init__(self, *, absent_boards=(11, 12), pump_on=False): + """Start released, with the specified air-pump state.""" + super().__init__(answers=make_answers(absent_boards=absent_boards)) + self.absent_boards = absent_boards + self.answers.update( + {command: [REPLY_SET] for command in (VALVE_ON, VALVE_OFF, PUMP_ON, PUMP_OFF)} + ) + self.answers[VALVE_READ] = [REPLY_OFF] + self.answers[PUMP_READ] = [REPLY_ON if pump_on else REPLY_OFF] + + async def write(self, data: bytes) -> int: + """Apply the accepted write and expose its control state through the corresponding read.""" + packet_number, board, _, _ = parse_frame(frame=data) + if board in self.absent_boards: + self.written.append(data) + self.incoming.extend(build_frame(packet_number=packet_number, board=board, command=0xFFFF)) + return len(data) + command = parse_frame(frame=data)[1:] + for change, read, reply in ( + (VALVE_ON, VALVE_READ, REPLY_ON), + (VALVE_OFF, VALVE_READ, REPLY_OFF), + (PUMP_ON, PUMP_READ, REPLY_ON), + (PUMP_OFF, PUMP_READ, REPLY_OFF), + ): + if command == change: + self.answers[read] = [reply] + return await super().write(data) + + +def pressure_commands(link: ClampLink) -> List[Tuple[int, int, bytes]]: + """Clamp and pump commands, excluding discovery and unrelated reads.""" + return [ + command + for frame in link.written + for command in [parse_frame(frame=frame)[1:]] + if command in (VALVE_READ, VALVE_ON, VALVE_OFF, PUMP_READ, PUMP_ON, PUMP_OFF) + ] + + +class PneumaticPlateClampTests(DriverTestCase): + def setUp(self): + """Declare the unit's pneumatic output without substituting another unit's clamp levels.""" + super().setUp() + directory = tempfile.TemporaryDirectory() + self.addCleanup(directory.cleanup) + self.path = os.path.join(directory.name, "unit.json") + with open(self.path, "w", encoding="utf-8") as file: + json.dump({"plate_clamp": PNEUMATIC_CLAMP_BLOCK}, file) + + async def make_clamp(self, *, pump_on=False): + """Discover the pneumatic clamp through its pressure-board link.""" + link = ClampLink(pump_on=pump_on) + driver = await self.set_up_driver(link=link, declared_configuration_json=self.path) + assert isinstance(driver.plate_clamp, PlateClamp) + self.assertEqual(driver.plate_clamp.auxiliary_output, 2) + return driver, link, driver.plate_clamp + + async def test_lock_release_and_repeated_calls_match_the_wire(self): + """Repeated intents do not cycle the valve or lose ownership of the pump.""" + _, link, clamp = await self.make_clamp() + await clamp.lock() + await clamp.lock() + await clamp.unlock() + await clamp.unlock() + self.assertEqual( + pressure_commands(link), + [ + PUMP_READ, + VALVE_READ, + VALVE_ON, + VALVE_READ, + PUMP_READ, + PUMP_ON, + PUMP_READ, + PUMP_READ, + VALVE_READ, + VALVE_READ, + VALVE_OFF, + VALVE_READ, + PUMP_READ, + PUMP_OFF, + PUMP_READ, + VALVE_READ, + ], + ) + + async def test_an_existing_air_supply_is_preserved(self): + """A clamp must not shut down an air pump it did not start.""" + driver, link, clamp = await self.make_clamp(pump_on=True) + await clamp.lock() + await clamp.unlock() + await driver.stop() + commands = pressure_commands(link) + self.assertNotIn(PUMP_ON, commands) + self.assertNotIn(PUMP_OFF, commands) + self.assertEqual(link.answers[PUMP_READ], [REPLY_ON]) + self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) + + async def test_stop_releases_a_held_plate_and_restores_the_pump(self): + """Shutdown releases the valve before switching off an air supply owned by the clamp.""" + driver, link, clamp = await self.make_clamp() + await clamp.lock() + await driver.stop() + self.assertEqual( + pressure_commands(link)[-6:], + [VALVE_READ, VALVE_OFF, VALVE_READ, PUMP_READ, PUMP_OFF, PUMP_READ], + ) + self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) + self.assertEqual(link.answers[PUMP_READ], [REPLY_OFF]) + + async def test_cancellation_after_pump_write_releases_and_restores(self): + """A cancelled write can take effect, so cleanup must already own the pump change.""" + _, link, clamp = await self.make_clamp() + + async def write_then_cancel(data): + """Accept the pump-on write before cancelling the operation.""" + result = await link.write(data) + if parse_frame(frame=data)[1:] == PUMP_ON: + raise asyncio.CancelledError() + return result + + link.io.write.side_effect = write_then_cancel + with self.assertRaises(asyncio.CancelledError): + await clamp.lock() + self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) + self.assertEqual(link.answers[PUMP_READ], [REPLY_OFF]) + self.assertEqual(pressure_commands(link).count(PUMP_ON), 1) + + async def test_unanswered_pump_write_is_not_retried_and_is_restored(self): + """A missing acknowledgement does not mean the pump stayed off.""" + driver, link, clamp = await self.make_clamp() + driver.default_read_timeout = 0.25 # the pump-on write is never answered + del link.answers[PUMP_ON] + with self.assertRaises(MantisTimeoutError): + await clamp.lock() + self.assertEqual(pressure_commands(link).count(PUMP_ON), 1) + self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) + self.assertEqual(link.answers[PUMP_READ], [REPLY_OFF]) + + async def test_lock_levels_are_rejected_before_sending(self): + """The pneumatic actuator has no adjustable clamping levels.""" + _, link, clamp = await self.make_clamp() + before = len(link.written) + with self.assertRaisesRegex(ValueError, "no lock levels"): + await clamp.lock(level=1) + self.assertEqual(len(link.written), before) + + async def test_no_pressure_board_means_no_pneumatic_clamp(self): + """A configuration file alone cannot establish that a controller is fitted.""" + link = ClampLink(absent_boards=(10, 11, 12)) + driver = await self.set_up_driver(link=link, declared_configuration_json=self.path) + self.assertIsNone(driver.plate_clamp) + self.assertEqual(pressure_commands(link), []) + + async def test_no_unit_mapping_means_no_pneumatic_clamp(self): + """Do not infer an auxiliary output from the generation alone.""" + link = ClampLink() + driver = await self.set_up_driver(link=link) + self.assertIsNone(driver.plate_clamp) + + async def test_invalid_output_is_rejected_during_discovery(self): + """An auxiliary output other than the supported clamp output must not be actuated.""" + for output in (0, 1, 2.0, "2", True): + with self.subTest(output=output): + with open(self.path, "w", encoding="utf-8") as file: + json.dump({"plate_clamp": {**PNEUMATIC_CLAMP_BLOCK, "auxiliary_output": output}}, file) + link = ClampLink() + driver = self.make_driver(link=link, declared_configuration_json=self.path) + with self.assertRaises(MantisConfigurationError): + await driver.setup(skip_arm=True) + self.assertEqual(pressure_commands(link), []) + + async def test_request_state_refuses_with_nothing_sent(self): + """A valve on a pressure-board output has no clamp state; its readback is the valve.""" + _, link, clamp = await self.make_clamp() + before = len(link.written) + with self.assertRaisesRegex(MantisConfigurationError, "no clamp state"): + await clamp.request_state() + self.assertEqual(len(link.written), before) + self.assertFalse(await clamp.request_locked()) + self.assertEqual(pressure_commands(link)[-1:], [VALVE_READ]) + + async def test_discovery_builds_one_arm_and_one_clamp_class_from_both_recordings(self): + with open(RECORDING_MANTIS_V3, encoding="utf-8") as file: + v3_declared = {**json.load(file), "plate_clamp": PNEUMATIC_CLAMP_BLOCK} + with open(self.path, "w", encoding="utf-8") as file: + json.dump(v3_declared, file) + for link, path in ( + (FakeLink(answers=make_answers()), RECORDING_MANTIS_V4), + (ClampLink(), self.path), + ): + with self.subTest(path=path): + driver = await self.set_up_driver(link=link, declared_configuration_json=path) + self.assertIs(type(driver.arm), Arm) + self.assertIs(type(driver.plate_clamp), PlateClamp) + + async def test_a_pressure_board_output_where_board_12_answers_is_refused(self): + link = ClampLink(absent_boards=(11,)) + driver = self.make_driver(link=link, declared_configuration_json=self.path) + with self.assertRaisesRegex(MantisConfigurationError, "board 12 answers"): + await driver.setup(skip_arm=True) + + +class PlateClampInitTests(unittest.TestCase): + def test_levels_alone_or_an_output_with_its_pump(self): + driver, pump = mock.Mock(), mock.Mock() + self.assertIsNone(PlateClamp(driver=driver).auxiliary_output) + self.assertIsNone(PlateClamp(driver=driver, levels={1: (0.1, 1000)}).auxiliary_output) + self.assertEqual(PlateClamp(driver=driver, auxiliary_output=2, pump=pump).auxiliary_output, 2) + refused = { + "an output alone": lambda: PlateClamp(driver=driver, auxiliary_output=2), + "a pump alone": lambda: PlateClamp(driver=driver, pump=pump), + "levels with an output": lambda: PlateClamp( + driver=driver, levels={1: (0.1, 1000)}, auxiliary_output=2, pump=pump + ), + } + for name, build in refused.items(): + with self.subTest(arguments=name): + with self.assertRaises(ValueError): + build() + if __name__ == "__main__": unittest.main() diff --git a/pylabrobot/formulatrix/mantis/driver/features/pneumatic_plate_clamp.py b/pylabrobot/formulatrix/mantis/driver/features/pneumatic_plate_clamp.py deleted file mode 100644 index b13cb49adae..00000000000 --- a/pylabrobot/formulatrix/mantis/driver/features/pneumatic_plate_clamp.py +++ /dev/null @@ -1,131 +0,0 @@ -"""The pneumatic plate clamp, switched by an auxiliary output on the pressure board.""" - -import asyncio -import logging -import struct -from typing import TYPE_CHECKING, Optional - -from ..errors import FrameError, PlateClampError -from ..framing import PRESSURE_BOARD -from .pump import Pump - -if TYPE_CHECKING: - from ..master import MantisDriver - -logger = logging.getLogger(__name__) - - -class PneumaticPlateClamp: - """A fixed-force clamp valve and its air supply; readback reports the valve, not plate contact. - - Locking starts the pump when needed. Unlocking restores a pump started by this clamp, leaving - a pump that was already running alone. Failed locking and shutdown release the valve. - """ - - def __init__(self, *, driver: "MantisDriver", pump: Pump, auxiliary_output: int): - """Take the unit's configured clamp output and the discovered pressure-board pump.""" - if type(auxiliary_output) is not int: - raise ValueError("auxiliary_output must be an integer") - driver._check_index(name="auxiliary_output", value=auxiliary_output, allowed=(2,)) - self._driver = driver - self._pump = pump - self._auxiliary_output = auxiliary_output - self._started_pump = False - self._operation_lock = asyncio.Lock() - - @property - def auxiliary_output(self) -> int: - """The unit's configured clamp output.""" - return self._auxiliary_output - - async def _unchecked_fw_set_locked(self, *, locked: bool) -> None: - """Set the clamp valve (board 10, command 42), sent once; bools occupy a uint16.""" - await self._driver._send_command( - board=PRESSURE_BOARD, - command=42, - payload=struct.pack(" bool: - """Whether the valve is commanded to hold the plate; no plate-contact sensor is read.""" - reply = await self._driver._send_command( - board=PRESSURE_BOARD, - command=41, - payload=struct.pack(" None: - """Set the valve only when needed, and require its readback to agree.""" - if await self.request_locked() == locked: - return - await self._unchecked_fw_set_locked(locked=locked) - actual = await self.request_locked() - if actual != locked: - raise PlateClampError( - f"{self._driver.name}: clamp output did not become {locked}", state=int(actual) - ) - - async def _restore_pump(self) -> None: - """Restore a pump started by this clamp; retain ownership if restoring it fails.""" - if self._started_pump: - await self._pump._set_pump_on(on=False) - self._started_pump = False - - async def _release_after_failure(self) -> None: - """Try both valve release and pump restoration, preserving the operation's error.""" - try: - await self._unchecked_fw_set_locked(locked=False) - if await self.request_locked(attempts=1): - raise PlateClampError( - f"{self._driver.name}: clamp output remains on after release", state=1 - ) - except Exception: - logger.warning("[%s] could not release the clamp valve", self._driver.name, exc_info=True) - try: - await self._restore_pump() - except Exception: - logger.warning("[%s] could not restore the clamp's pump", self._driver.name, exc_info=True) - - async def lock(self, *, level: Optional[int] = None) -> None: - """Engage the valve and supply air, confirming both control states. - - There is no adjustable lock level or mechanical position feedback. A failure or cancellation - releases the valve and restores a pump started by this clamp before propagating the error. - """ - if level is not None: - raise ValueError("the pneumatic clamp has no lock levels; omit level") - async with self._operation_lock: - pump_on = await self._pump.request_pump_on() - try: - await self._set_locked(locked=True) - if not pump_on: - # Own the change before sending: a cancelled or unanswered write may still take effect. - self._started_pump = True - await self._pump._set_pump_on(on=True) - except BaseException: - await self._release_after_failure() - raise - logger.info("[%s] clamp valve engaged, air pump on", self._driver.name) - - async def unlock(self) -> None: - """Release the valve and restore a pump started by this clamp.""" - async with self._operation_lock: - try: - await self._set_locked(locked=False) - except BaseException: - await self._release_after_failure() - raise - await self._restore_pump() - logger.info("[%s] clamp valve released", self._driver.name) - - async def _release(self) -> None: - """Release at shutdown, including any pump restoration still outstanding.""" - await self.unlock() diff --git a/pylabrobot/formulatrix/mantis/driver/features/pneumatic_plate_clamp_tests.py b/pylabrobot/formulatrix/mantis/driver/features/pneumatic_plate_clamp_tests.py deleted file mode 100644 index 9110dcade65..00000000000 --- a/pylabrobot/formulatrix/mantis/driver/features/pneumatic_plate_clamp_tests.py +++ /dev/null @@ -1,198 +0,0 @@ -"""Pneumatic clamp commands and cleanup against recorded pressure-board replies.""" - -import asyncio -import json -import os -import tempfile -from typing import List, Tuple - -from pylabrobot.formulatrix.mantis.driver.errors import MantisConfigurationError, MantisTimeoutError -from pylabrobot.formulatrix.mantis.driver.features.pneumatic_plate_clamp import PneumaticPlateClamp -from pylabrobot.formulatrix.mantis.driver.framing import build_frame, parse_frame -from pylabrobot.formulatrix.mantis.driver.master_tests import DriverTestCase, FakeLink, make_answers - -PNEUMATIC_CLAMP_BLOCK = {"controller": "pressure_regulator", "auxiliary_output": 2} -# Board, command, payload and replies from the pneumatic clamp and its air pump. -VALVE_READ = (10, 41, bytes.fromhex("0200")) -VALVE_ON = (10, 42, bytes.fromhex("02000100")) -VALVE_OFF = (10, 42, bytes.fromhex("02000000")) -PUMP_READ = (10, 14, b"") -PUMP_ON = (10, 15, bytes.fromhex("0100")) -PUMP_OFF = (10, 15, bytes.fromhex("0000")) -REPLY_OFF, REPLY_ON, REPLY_SET = bytes.fromhex("0000"), bytes.fromhex("0100"), b"" - - -class ClampLink(FakeLink): - """Answer with recorded readbacks for the states selected by accepted commands; an absent - board refuses at once instead of timing out, so the tests keep the default read timeout.""" - - def __init__(self, *, absent_boards=(11, 12), pump_on=False): - """Start released, with the specified air-pump state.""" - super().__init__(answers=make_answers(absent_boards=absent_boards)) - self.absent_boards = absent_boards - self.answers.update( - {command: [REPLY_SET] for command in (VALVE_ON, VALVE_OFF, PUMP_ON, PUMP_OFF)} - ) - self.answers[VALVE_READ] = [REPLY_OFF] - self.answers[PUMP_READ] = [REPLY_ON if pump_on else REPLY_OFF] - - async def write(self, data: bytes) -> int: - """Apply the accepted write and expose its control state through the corresponding read.""" - packet_number, board, _, _ = parse_frame(frame=data) - if board in self.absent_boards: - self.written.append(data) - self.incoming.extend(build_frame(packet_number=packet_number, board=board, command=0xFFFF)) - return len(data) - command = parse_frame(frame=data)[1:] - for change, read, reply in ( - (VALVE_ON, VALVE_READ, REPLY_ON), - (VALVE_OFF, VALVE_READ, REPLY_OFF), - (PUMP_ON, PUMP_READ, REPLY_ON), - (PUMP_OFF, PUMP_READ, REPLY_OFF), - ): - if command == change: - self.answers[read] = [reply] - return await super().write(data) - - -def pressure_commands(link: ClampLink) -> List[Tuple[int, int, bytes]]: - """Clamp and pump commands, excluding discovery and unrelated reads.""" - return [ - command - for frame in link.written - for command in [parse_frame(frame=frame)[1:]] - if command in (VALVE_READ, VALVE_ON, VALVE_OFF, PUMP_READ, PUMP_ON, PUMP_OFF) - ] - - -class PneumaticPlateClampTests(DriverTestCase): - def setUp(self): - """Declare the unit's pneumatic output without substituting another unit's clamp levels.""" - super().setUp() - directory = tempfile.TemporaryDirectory() - self.addCleanup(directory.cleanup) - self.path = os.path.join(directory.name, "unit.json") - with open(self.path, "w", encoding="utf-8") as file: - json.dump({"plate_clamp": PNEUMATIC_CLAMP_BLOCK}, file) - - async def make_clamp(self, *, pump_on=False): - """Discover the pneumatic clamp through its pressure-board link.""" - link = ClampLink(pump_on=pump_on) - driver = await self.set_up_driver(link=link, declared_configuration_json=self.path) - assert isinstance(driver.plate_clamp, PneumaticPlateClamp) - return driver, link, driver.plate_clamp - - async def test_lock_release_and_repeated_calls_match_the_wire(self): - """Repeated intents do not cycle the valve or lose ownership of the pump.""" - _, link, clamp = await self.make_clamp() - await clamp.lock() - await clamp.lock() - await clamp.unlock() - await clamp.unlock() - self.assertEqual( - pressure_commands(link), - [ - PUMP_READ, - VALVE_READ, - VALVE_ON, - VALVE_READ, - PUMP_READ, - PUMP_ON, - PUMP_READ, - PUMP_READ, - VALVE_READ, - VALVE_READ, - VALVE_OFF, - VALVE_READ, - PUMP_READ, - PUMP_OFF, - PUMP_READ, - VALVE_READ, - ], - ) - - async def test_an_existing_air_supply_is_preserved(self): - """A clamp must not shut down an air pump it did not start.""" - driver, link, clamp = await self.make_clamp(pump_on=True) - await clamp.lock() - await clamp.unlock() - await driver.stop() - commands = pressure_commands(link) - self.assertNotIn(PUMP_ON, commands) - self.assertNotIn(PUMP_OFF, commands) - self.assertEqual(link.answers[PUMP_READ], [REPLY_ON]) - self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) - - async def test_stop_releases_a_held_plate_and_restores_the_pump(self): - """Shutdown releases the valve before switching off an air supply owned by the clamp.""" - driver, link, clamp = await self.make_clamp() - await clamp.lock() - await driver.stop() - self.assertEqual( - pressure_commands(link)[-6:], - [VALVE_READ, VALVE_OFF, VALVE_READ, PUMP_READ, PUMP_OFF, PUMP_READ], - ) - self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) - self.assertEqual(link.answers[PUMP_READ], [REPLY_OFF]) - - async def test_cancellation_after_pump_write_releases_and_restores(self): - """A cancelled write can take effect, so cleanup must already own the pump change.""" - _, link, clamp = await self.make_clamp() - - async def write_then_cancel(data): - """Accept the pump-on write before cancelling the operation.""" - result = await link.write(data) - if parse_frame(frame=data)[1:] == PUMP_ON: - raise asyncio.CancelledError() - return result - - link.io.write.side_effect = write_then_cancel - with self.assertRaises(asyncio.CancelledError): - await clamp.lock() - self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) - self.assertEqual(link.answers[PUMP_READ], [REPLY_OFF]) - self.assertEqual(pressure_commands(link).count(PUMP_ON), 1) - - async def test_unanswered_pump_write_is_not_retried_and_is_restored(self): - """A missing acknowledgement does not mean the pump stayed off.""" - driver, link, clamp = await self.make_clamp() - driver.default_read_timeout = 0.25 # the pump-on write is never answered - del link.answers[PUMP_ON] - with self.assertRaises(MantisTimeoutError): - await clamp.lock() - self.assertEqual(pressure_commands(link).count(PUMP_ON), 1) - self.assertEqual(link.answers[VALVE_READ], [REPLY_OFF]) - self.assertEqual(link.answers[PUMP_READ], [REPLY_OFF]) - - async def test_lock_levels_are_rejected_before_sending(self): - """The pneumatic actuator has no adjustable clamping levels.""" - _, link, clamp = await self.make_clamp() - before = len(link.written) - with self.assertRaisesRegex(ValueError, "no lock levels"): - await clamp.lock(level=1) - self.assertEqual(len(link.written), before) - - async def test_no_pressure_board_means_no_pneumatic_clamp(self): - """A configuration file alone cannot establish that a controller is fitted.""" - link = ClampLink(absent_boards=(10, 11, 12)) - driver = await self.set_up_driver(link=link, declared_configuration_json=self.path) - self.assertIsNone(driver.plate_clamp) - self.assertEqual(pressure_commands(link), []) - - async def test_no_unit_mapping_means_no_pneumatic_clamp(self): - """Do not infer an auxiliary output from the generation alone.""" - link = ClampLink() - driver = await self.set_up_driver(link=link) - self.assertIsNone(driver.plate_clamp) - - async def test_invalid_output_is_rejected_during_discovery(self): - """An auxiliary output other than the supported clamp output must not be actuated.""" - for output in (0, 1, 2.0, "2", True): - with self.subTest(output=output): - with open(self.path, "w", encoding="utf-8") as file: - json.dump({"plate_clamp": {**PNEUMATIC_CLAMP_BLOCK, "auxiliary_output": output}}, file) - link = ClampLink() - driver = self.make_driver(link=link, declared_configuration_json=self.path) - with self.assertRaises(MantisConfigurationError): - await driver.setup(skip_arm=True) - self.assertEqual(pressure_commands(link), []) diff --git a/pylabrobot/formulatrix/mantis/driver/master.py b/pylabrobot/formulatrix/mantis/driver/master.py index 663241b3687..608a0bccf3d 100644 --- a/pylabrobot/formulatrix/mantis/driver/master.py +++ b/pylabrobot/formulatrix/mantis/driver/master.py @@ -22,11 +22,11 @@ Optional, Set, Tuple, - Union, ) from pylabrobot.io.ftdi import FTDI +from .arm_kinematics import MantisArmCoordinate from .configuration import ( FITTED_FIELDS, MOTION_CALIBRATION_BLOCKS, @@ -60,11 +60,9 @@ decode_sequence_progress, describe_event, ) -from .features.arm import Arm -from .features.arm_v3 import V3Arm +from .features.arm import Arm, MantisArmConfiguration from .features.led_bar import LedBar from .features.plate_clamp import PlateClamp -from .features.pneumatic_plate_clamp import PneumaticPlateClamp from .features.pump import Pump from .features.rfid import RFID_BAUD_RATE, RFID_FTDI_INTERFACE, Rfid, _initialize_rfid_module from .framing import ( @@ -78,7 +76,14 @@ 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 .v3_arm_configuration import V3ArmConfiguration logger = logging.getLogger(__name__) @@ -189,7 +194,7 @@ def __init__( if rfid_io is not None or io is not None else self._build_rfid_io(device_id=device_id, ftdi_library=ftdi_library) ) - self.plate_clamp: Optional[Union[PlateClamp, PneumaticPlateClamp]] = None + self.plate_clamp: Optional[PlateClamp] = None self.pump: Optional[Pump] = None self.arm: Optional[Arm] = None self.rfid: Optional[Rfid] = None @@ -826,17 +831,19 @@ 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. + Only the motion board must answer. Board 11 gets a second attempt, as its silence makes a unit + a v3; each other board gets one. A board that times out, refuses or answers a malformed version + is absent, not a fault. """ firmware_versions: Dict[int, Optional[str]] = { MOTION_BOARD: await self.request_firmware_version(board=MOTION_BOARD) } for board in (PRESSURE_BOARD, ABSOLUTE_POSITION_SENSOR_BOARD, LC3_BOARD): + attempts = 2 if board == ABSOLUTE_POSITION_SENSOR_BOARD else 1 try: - reply = await self._send_command(board=board, command=1, attempts=1) + reply = await self._send_command(board=board, command=1, attempts=attempts) firmware_versions[board] = decode_string(payload=reply) except (TimeoutError, CommandRefusedError, FrameError, UnicodeDecodeError) as error: logger.info("[%s] board %d taken as absent: %r", self.name, board, error) @@ -844,7 +851,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]: @@ -880,33 +886,147 @@ 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[MantisArmCoordinate]: + route = [] + for line in lines: + x, y, z = (None if value is None else float(value) for value in line) + route.append(MantisArmCoordinate(x=x, y=y, z=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] = ( + MantisArmCoordinate(x=minimum_x, y=minimum_y, z=None), + MantisArmCoordinate(x=maximum_x, y=maximum_y, z=None), + ) + 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( + line.x is None or line.z is not None + for route in routes["safe_xy_routes"].values() + for line 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(PRESSURE_BOARD) is not None and self.pump is None: self.pump = Pump(driver=self) block = self._unit_blocks.get("plate_clamp") - if block is not None and block.get("controller") == "pressure_regulator": + lc3_answered = self.configuration.firmware_versions.get(LC3_BOARD) is not None + pneumatic = block is not None and block.get("controller") == "pressure_regulator" + # A clamp on a pressure-board output where board 12 answers, or with levels where it does not. + if pneumatic and lc3_answered: + raise MantisConfigurationError( + f"{self.name}: the plate_clamp block names a pressure-board output, but board 12 answers" + ) + if ( + block is not None + and not pneumatic + and not lc3_answered + and "plate_clamp" not in self._unit_blocks_from_recording + ): + raise MantisConfigurationError( + f"{self.name}: the plate_clamp block has lock levels, but board 12 does not answer" + ) + if pneumatic and block is not None: if self.pump is not None and self.plate_clamp is None: try: output = block["auxiliary_output"] - self.plate_clamp = PneumaticPlateClamp( - driver=self, pump=self.pump, auxiliary_output=output - ) + self.plate_clamp = PlateClamp(driver=self, auxiliary_output=output, pump=self.pump) except (KeyError, TypeError, ValueError) as error: raise MantisConfigurationError( f"malformed pneumatic plate_clamp block: {error}" ) from error - elif ( - self.configuration.firmware_versions.get(LC3_BOARD) is not None and self.plate_clamp is None - ): + elif lc3_answered and self.plate_clamp is None: levels: Optional[Dict[int, Tuple[float, int]]] = None - block = self._unit_blocks.get("plate_clamp") if block is not None: try: levels = { @@ -934,16 +1054,35 @@ def _discover_features(self) -> None: self.led_bar = LedBar( driver=self, number_of_leds=number_of_leds, show_actions=self._show_actions ) - sensor_board = self.configuration.firmware_versions.get(ABSOLUTE_POSITION_SENSOR_BOARD) - if self.arm is None: - if sensor_board is not None: - self.arm = Arm(driver=self, **self._get_arm_calibration()) - else: - block = self._unit_blocks.get("arm") - self.arm = V3Arm( - driver=self, - configuration=V3ArmConfiguration.from_block(block) if block is not None else None, + if self.arm is None and self.configuration.generation == "v4": + self.arm = Arm( + driver=self, + configuration=MantisArmConfiguration( + motor_limits=motor_limits, + **self._get_arm_calibration(), + **self._get_route_blocks(), + **self._get_stage_calibration(), + ), + ) + elif self.arm is None and self.configuration.generation == "v3": + block = self._unit_blocks.get("arm") + # Absolute-encoder references are a v4's: never seat a v4's arms against their stops. + if block is not None and ("homing_position_a" in block or "encoder_correction" in block): + raise MantisConfigurationError( + f"{self.name}: board 11 did not answer, but the arm block has absolute-encoder " + "references, as a v4's has; refusing to treat this unit as a v3" ) + v3_arm_configuration = ( + V3ArmConfiguration.from_block(block=block) if block is not None else None + ) + self.arm = Arm( + driver=self, + configuration=MantisArmConfiguration( + motor_limits=motor_limits, + offsets=v3_arm_configuration.offsets if v3_arm_configuration is not None else None, + v3_arm_configuration=v3_arm_configuration, + ), + ) def _show_action( self, *, action: str, show_inner_actions: bool = True, only_when_idle: bool = False @@ -1065,7 +1204,10 @@ 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.arm is None: # a v3's arm checks its targets against them too + 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: @@ -1221,7 +1363,7 @@ async def stop(self) -> None: async def setup(self, *, skip_arm: bool = False) -> None: """Open the link if it is not open, read the device's configuration and check it, then bring - the arm and Z up (``Arm.initialize`` or ``V3Arm.initialize``). This moves the device. + the arm and Z up (``Arm.initialize``). This moves the device. On a v4 the LED bar shows the setup, the arm's bring-up with it, then "setup_done" for ``SETUP_DONE_HOLD``, then idle. diff --git a/pylabrobot/formulatrix/mantis/driver/master_tests.py b/pylabrobot/formulatrix/mantis/driver/master_tests.py index 2a68cce890d..fde9b1b14d4 100644 --- a/pylabrobot/formulatrix/mantis/driver/master_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/master_tests.py @@ -293,12 +293,17 @@ 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): + async def test_only_board_0_must_answer_and_board_11_is_asked_twice(self): self.default_read_timeout = 0.25 # boards 10 to 12 never answer link = FakeLink(answers=make_answers(absent_boards=(10, 11, 12))) driver = await self.set_up_driver(link=link) @@ -307,9 +312,41 @@ async def test_only_board_0_must_answer_and_each_other_board_is_asked_once(self) driver.configuration.firmware_versions, {0: "4.20.6.2", 10: None, 11: None, 12: None} ) sent = [parse_frame(frame=frame)[1:] for frame in link.written] - for board in (10, 11, 12): + for board, count in ((10, 1), (11, 2), (12, 1)): with self.subTest(board=board): - self.assertEqual(sent.count((board, 1, b"")), 1) + self.assertEqual(sent.count((board, 1, b"")), count) + + async def test_board_11_answering_its_second_attempt_makes_a_v4(self): + self.default_read_timeout = 0.25 # board 11's first version request goes unanswered + link = FakeLink(answers=make_answers()) + write = link.write + dropped: List[bytes] = [] + + async def drop_first_board_11_request(data: bytes) -> int: + if parse_frame(frame=data)[1:3] == (11, 1) and not dropped: + dropped.append(data) + link.written.append(data) + return len(data) + return await write(data) + + link.io.write.side_effect = drop_first_board_11_request + driver = await self.set_up_driver(link=link) + self.assertEqual(driver.configuration.generation, "v4") + self.assertEqual(driver.configuration.firmware_versions[11], "0.1.7 DBG") + + async def test_a_v4_arm_block_on_a_silent_board_11_refuses_setup(self): + with tempfile.TemporaryDirectory() as directory: + path = os.path.join(directory, "declared.json") + with open(path, "w", encoding="utf-8") as f: + json.dump({"arm": {"homing_position_a": {"0": 0.0, "1": 0.0}}}, f) + link = FakeLink(answers=make_answers(absent_boards=(11,))) + driver = self.make_driver(link=link, declared_configuration_json=path) + driver.default_read_timeout = 0.25 # board 11 never answers + with self.assertRaisesRegex(MantisConfigurationError, "absolute-encoder references"): + await driver.setup() + sent = [parse_frame(frame=frame)[1:3] for frame in link.written] + self.assertNotIn((0, 22), sent) + self.assertIsNone(driver.arm) async def test_declared_configuration_that_disagrees_refuses_setup(self): with tempfile.TemporaryDirectory() as directory: diff --git a/pylabrobot/formulatrix/mantis/driver/oem_config.py b/pylabrobot/formulatrix/mantis/driver/oem_config.py index caf5fa654d3..8118ec26b58 100644 --- a/pylabrobot/formulatrix/mantis/driver/oem_config.py +++ b/pylabrobot/formulatrix/mantis/driver/oem_config.py @@ -1,20 +1,48 @@ -"""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 .arm_kinematics import MICROSTEPS, PITCH, STEPS_PER_REVOLUTION, compute_position +from .arm_kinematics import ( + MICROSTEPS, + PITCH, + STEPS_PER_REVOLUTION, + MantisArmCoordinate, + compute_position, +) from .errors import MantisConfigurationError +from .sequence_files import parse_move_sequence from .v3_arm_configuration import V3ArmConfiguration # 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]: @@ -37,6 +65,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. @@ -58,6 +217,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[MantisArmCoordinate] = parse_move_sequence(path=path) + if all(route_line == MantisArmCoordinate(x=None, y=None, z=None) for route_line in route): + raise MantisConfigurationError(f"{path} moves nothing") + return [[route_line.x, route_line.y, route_line.z] 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_v3_arm_configuration(*, values: Dict[str, str]) -> Dict[str, Any]: """Read the v3 home frame and search settings, converting vendor units at the boundary.""" try: @@ -91,7 +325,7 @@ def flag(key: str) -> bool: route.append([x, y]) block = { "offsets": {str(motor): number(f"Motor.{motor + 1}.Offset") for motor in (0, 1)}, - "home_positions": { + "homing_position_n": { str(motor): compute_position( motor=motor, device_position=number(f"Motor.{motor + 1}.HomingPositionN") ) @@ -111,10 +345,10 @@ def flag(key: str) -> bool: "backoff": number("Arm.PreHomingMovement.EnforceStep"), "jog_speed": number("Motor.1.DefaultVelocity"), "jog_acceleration": number("Motor.1.DefaultAcceleration"), - "motor_delay": number("MotorXYHomingDelay") / 1000.0, - "post_home_positions": route, + "motor_xy_homing_delay": number("MotorXYHomingDelay") / 1000.0, + "post_homing_positions": route, } - V3ArmConfiguration.from_block(block) + V3ArmConfiguration.from_block(block=block) return block except (KeyError, TypeError, ValueError) as error: raise MantisConfigurationError(f"invalid v3 RobotArm.config: {error}") from error @@ -131,37 +365,35 @@ def _load_unit_configuration(*, oem_config_directory: str) -> Dict[str, Any]: ``led_bar``: the LED count, from ``Device.config``'s ``NumberOfLed``, only where it is set. ``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). Otherwise, the v3's offsets, home positions, flag searches, retract/backoff + is set (a v4); otherwise the v3's offsets, home positions, flag searches, retract and backoff settings and post-home route. 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") == "PressureRegulator": key = "PlateClampAuxId" - output = number( - path=device_config, key=key, text=setting(path=device_config, settings=values, key=key) + output = _get_number( + path=device_config, key=key, text=_get_setting(path=device_config, settings=values, key=key) ) if output != 2: raise MantisConfigurationError( @@ -170,14 +402,16 @@ def setting(*, path: str, settings: Dict[str, str], key: str) -> str: blocks["plate_clamp"] = {"controller": "pressure_regulator", "auxiliary_output": int(output)} 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( @@ -193,7 +427,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"]) ) } @@ -205,18 +439,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 {} @@ -234,10 +480,12 @@ 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) } @@ -246,4 +494,20 @@ def setting(*, path: str, settings: Dict[str, str], key: str) -> str: blocks["arm"]["encoder_correction"] = correction elif arm_values: blocks["arm"] = _load_v3_arm_configuration(values=arm_values) + + # 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 ab38eb0f8d8..4279293e408 100644 --- a/pylabrobot/formulatrix/mantis/driver/oem_config_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/oem_config_tests.py @@ -5,15 +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.features.arm_v3_tests import SYNTHETIC_V3_ARM_BLOCK 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, ) @@ -65,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 @@ -113,6 +141,40 @@ ) + TAIL ) +# The arm block the synthetic v3 RobotArm.config gives. +SYNTHETIC_V3_ARM_BLOCK = { + "offsets": {"0": 0.0, "1": 0.0}, + "homing_position_n": {"0": -90.0, "1": 216.0}, + "motors": { + "0": { + "method": 3, + "positive_edge": True, + "positive_direction": False, + "speed": 100.0, + "acceleration": 2500.0, + }, + "1": { + "method": 3, + "positive_edge": True, + "positive_direction": True, + "speed": 100.0, + "acceleration": 2500.0, + }, + "2": { + "method": 0, + "positive_edge": True, + "positive_direction": False, + "speed": 15.0, + "acceleration": 400.0, + }, + }, + "retract": -50.0, + "backoff": 20.0, + "jog_speed": 10000.0, + "jog_acceleration": 1500.0, + "motor_xy_homing_delay": 0.2, + "post_homing_positions": [[-38.403, 11.677], [-45.77, 47.82], [15.0, 31.177]], +} # A synthetic encoder correction in the files' layout: ideal, measured, measured. SYNTHETIC_CORRECTION_ROWS = [(row, row - 0.25) for row in range(1, 201)] SYNTHETIC_CORRECTION_CSV = "\ufeff" + "".join( @@ -120,6 +182,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: @@ -262,6 +445,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) @@ -331,9 +695,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..d7dcdff49dc --- /dev/null +++ b/pylabrobot/formulatrix/mantis/driver/sequence_files.py @@ -0,0 +1,83 @@ +"""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 + +from .arm_kinematics import MantisArmCoordinate +from .errors import MantisConfigurationError + +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]) -> MantisArmCoordinate: + """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 MantisArmCoordinate(x=axes.get("x"), y=axes.get("y"), z=axes.get("z")) + + +def parse_move_sequence(*, path: str) -> List[MantisArmCoordinate]: + """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[MantisArmCoordinate] = [] + # 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..869b6ecc34e --- /dev/null +++ b/pylabrobot/formulatrix/mantis/driver/sequence_files_tests.py @@ -0,0 +1,69 @@ +"""Sequence file tests: the move lines of a sequence, in its files' layout.""" + +import os +import tempfile +import unittest + +from pylabrobot.formulatrix.mantis.driver.arm_kinematics import MantisArmCoordinate +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), + [ + MantisArmCoordinate(x=None, y=None, z=None), + MantisArmCoordinate(x=15.0, y=30.0, z=None), + MantisArmCoordinate(x=None, y=None, z=-1.57), + MantisArmCoordinate(x=-50.0, y=20.0, z=-1.57), + MantisArmCoordinate(x=-60.0, y=10.0, z=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), + [ + MantisArmCoordinate(x=None, y=None, z=-1.57), + MantisArmCoordinate(x=15.0, y=30.0, z=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 a51d0aa70e1..863800d27e3 100644 --- a/pylabrobot/formulatrix/mantis/driver/simulator.py +++ b/pylabrobot/formulatrix/mantis/driver/simulator.py @@ -1,10 +1,13 @@ """A simulated Mantis: the driver with no device behind it, answering from its configuration.""" +import asyncio +import dataclasses import logging from typing import Dict, List, Optional, Set, Tuple from pylabrobot.io.ftdi import FTDI +from .arm_kinematics import compute_device_position, compute_device_rate from .configuration import ( RECORDING_MANTIS_V3, RECORDING_MANTIS_V4, @@ -14,33 +17,47 @@ read_configuration, ) from .errors import MantisConfigurationError, MantisTimeoutError -from .features.arm import ARM_MOTORS, MOTOR_HOMED, Z_MOTOR, Arm -from .features.arm_v3 import V3Arm +from .features.arm import ( + ARM_MOTORS, + MOTOR_HOMED, + Z_HOMING_ACCELERATION, + Z_HOMING_SPEED, + Z_MOTOR, + Arm, + MantisArmConfiguration, +) from .features.led_bar import LedBar from .features.plate_clamp import CLAMPING_DONE, UNCLAMPING_DONE, PlateClamp -from .features.pneumatic_plate_clamp import PneumaticPlateClamp from .features.pump import PressureController, Pump from .features.rfid import Rfid from .framing import ABSOLUTE_POSITION_SENSOR_BOARD, LC3_BOARD, MOTION_BOARD, PRESSURE_BOARD from .master import ABSOLUTE_ENCODER_MOTORS, MOTORS, SEQUENCER_QUEUE_SIZE, MantisDriver -from .v3_arm_configuration import V3ArmConfiguration logger = logging.getLogger(__name__) class MantisSimulationPlateClamp(PlateClamp): - """A plate clamp with no device behind it: each command sets the state it ends in, at once.""" + """A plate clamp with no device behind it: each command sets the state it ends in, at once; a + valve on a pressure-board output switches at once.""" def __init__( - self, *, driver: "MantisSimulationDriver", levels: Optional[Dict[int, Tuple[float, int]]] + self, + *, + driver: "MantisSimulationDriver", + levels: Optional[Dict[int, Tuple[float, int]]] = None, + auxiliary_output: Optional[int] = None, + pump: Optional[Pump] = None, ): """ Args: driver: the simulated driver. levels: the declared lock levels, as for ``PlateClamp``. + auxiliary_output: the declared valve output, as for ``PlateClamp``. + pump: the simulated pump, with ``auxiliary_output`` only. """ - super().__init__(driver=driver, levels=levels) + super().__init__(driver=driver, levels=levels, auxiliary_output=auxiliary_output, pump=pump) self._state = 0 # idle, as after power-on + self._locked = False # the valve off async def _unchecked_fw_do_plate_unclamping(self) -> None: """Set the state to unclamping done.""" @@ -51,9 +68,19 @@ async def _unchecked_fw_do_plate_clamping(self, *, pwm: float, timeout_ms: int) self._state = CLAMPING_DONE async def request_state(self, *, attempts: int = 3) -> int: - """The simulated state.""" + """The simulated state; refused for a valve on a pressure-board output, as for ``PlateClamp``.""" + if self.auxiliary_output is not None: + return await super().request_state(attempts=attempts) return self._state + async def _unchecked_fw_set_auxiliary_output(self, *, on: bool) -> None: + """Switch the simulated valve.""" + self._locked = on + + async def _request_auxiliary_output(self, *, attempts: int = 3) -> bool: + """Whether the simulated valve is switched on.""" + return self._locked + class MantisSimulationPump(Pump): """A pump with no device behind it, answering from the declared configuration. @@ -116,23 +143,6 @@ async def _request_calibration(self) -> Dict[PressureController, Tuple[float, fl } -class MantisSimulationPneumaticPlateClamp(PneumaticPlateClamp): - """A pneumatic clamp whose valve changes state immediately, without reaching a wire.""" - - def __init__(self, *, driver: "MantisSimulationDriver", pump: Pump, auxiliary_output: int): - """Use the simulated pressure-board pump and the declared clamp output.""" - super().__init__(driver=driver, pump=pump, auxiliary_output=auxiliary_output) - self._locked = False - - async def _unchecked_fw_set_locked(self, *, locked: bool) -> None: - """Set the simulated valve's control state.""" - self._locked = locked - - async def request_locked(self, *, attempts: int = 3) -> bool: - """The simulated valve's control state.""" - return self._locked - - class MantisSimulationRfid(Rfid): """An RFID reader with no device behind it: every slot reads empty.""" @@ -165,33 +175,25 @@ class MantisSimulationArm(Arm): It stands at idle, as a parked arm does, and its absolute encoders read where it stands, so it needs no encoder correction. Until declared, the controller holds every motor at 0, as after - power-on. Without a declared ``arm`` block its offsets are 0. + power-on. Without a declared ``arm`` block its offsets are 0. A v3 arm's retract seats it + against its stop: it reads a following error until its faults are cleared. """ - 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 + self._statuses: Dict[int, int] = {0: 0, 1: 0, Z_MOTOR: 0} + self._homed: Set[int] = set() def _check_calibrated(self) -> None: """Pass: a simulated arm needs only its offsets, which it always has.""" @@ -209,7 +211,8 @@ def _play_move(self, *, targets: List[Tuple[float, float, float]]) -> None: self._set_position(motor=motor, position=position) async def _unchecked_fw_clear_motor_faults(self, *, motor: int) -> None: - """Clear nothing: a simulated motor has no faults.""" + """Clear the motor's following error, keeping its homed bit.""" + self._statuses[motor] = MOTOR_HOMED if motor in self._homed else 0 async def _unchecked_fw_set_motor_open_loop(self, *, motor: int, open_loop: bool) -> None: """Change nothing: a simulated motor holds either way.""" @@ -227,13 +230,23 @@ async def _unchecked_fw_home( ) -> None: """Put the motor on its flag, at 0, homed.""" self._set_position(motor=motor, position=0.0) - self._z_homed = self._z_homed or motor == Z_MOTOR + self._homed.add(motor) + self._statuses[motor] = MOTOR_HOMED 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. A v3 arm's retract seats it: a following + error while moving.""" self._set_position(motor=motor, position=position) + v3_arm_configuration = self.configuration.v3_arm_configuration + if ( + motor in ARM_MOTORS + and v3_arm_configuration is not None + and position == compute_device_position(motor=motor, position=v3_arm_configuration.retract) + ): + self._statuses[motor] |= 0x100 + return 0 async def _unchecked_fw_stop(self, *, motor: int, deceleration: float) -> None: """Stop nothing: a simulated motor is never moving.""" @@ -251,82 +264,39 @@ async def _unchecked_fw_set_encoder_position(self, *, motor: int, position: floa """Set nothing: the controller's position is the one simulated.""" async def _request_absolute_device_position(self, *, motor: int) -> float: - """Where the simulated arm motor stands, in device units.""" - return self._absolute_positions[motor] + """Where the simulated arm motor stands, in device units; refused on a v3, as for ``Arm``.""" + if self._get_generation() == "v4": + return self._absolute_positions[motor] + return await super()._request_absolute_device_position(motor=motor) + + async def _home_z( + self, *, speed: float = Z_HOMING_SPEED, acceleration: float = Z_HOMING_ACCELERATION + ) -> None: + """Home Z at once: no motor slips, and no event is waited for. - async def _home_z(self) -> None: - """Home Z at once: no motor slips, and no event is waited for.""" + Args: + speed: the fast search, mm/s, as for ``Arm``. + acceleration: mm/s², as for ``Arm``. + """ + fast_speed = compute_device_rate(motor=Z_MOTOR, rate=speed) await self._unchecked_fw_home( motor=Z_MOTOR, method=0, positive_edge=True, positive_direction=False, - slow_speed=0.0, - fast_speed=0.0, - acceleration=0.0, + slow_speed=fast_speed / 10, + fast_speed=fast_speed, + acceleration=compute_device_rate(motor=Z_MOTOR, rate=acceleration), ) -class MantisSimulationV3Arm(V3Arm): - """A v3 arm simulated above the wire, with flags and following errors at the stops.""" - - def __init__( - self, *, driver: "MantisSimulationDriver", configuration: Optional[V3ArmConfiguration] - ): - """Use the declared route; simulation also requires a v3 homing calibration.""" - super().__init__(driver=driver, configuration=configuration) - self._positions = {0: 0.0, 1: 0.0, 2: 0.0} - self._statuses = {0: 0, 1: 0, 2: 0} - self._homed: Set[int] = set() - - async def _unchecked_fw_clear_motor_faults(self, *, motor: int) -> None: - """Clear the simulated fault, keeping the home state.""" - self._statuses[motor] = MOTOR_HOMED if motor in self._homed else 0 - - async def _unchecked_fw_move_absolute( - self, *, motor: int, position: float, speed: float, acceleration: float - ) -> None: - """Move at once; a retract of an unhomed arm seats against its stop.""" - if motor in ARM_MOTORS and motor not in self._homed and position < self._positions[motor]: - self._statuses[motor] = 128 - self._positions[motor] = position - - async def _unchecked_fw_set_motor_position(self, *, motor: int, position: float) -> None: - """Declare the simulated controller's position.""" - self._positions[motor] = position - - async def _unchecked_fw_home( - self, - *, - motor: int, - method: int, - positive_edge: bool, - positive_direction: bool, - slow_speed: float, - fast_speed: float, - acceleration: float, - ) -> None: - """Find the flag immediately.""" - self._positions[motor] = 0.0 - self._homed.add(motor) - self._statuses[motor] = MOTOR_HOMED - - async def _unchecked_fw_stop(self, *, motor: int, deceleration: float) -> None: - """Do nothing: a simulated move has already finished.""" - - async def initialize(self) -> None: - """Establish the simulated home frame from an unhomed state.""" - self._homed.clear() - await super().initialize() - - class MantisSimulationDriver(MantisDriver): """A Mantis driver with no device behind it. Nothing reaches a wire. The configuration comes from ``declared_configuration_json``, and the reads answer from it; a board it leaves ``None`` does not answer. The plate clamp's state changes at once, every RFID slot reads empty, and the LED bar takes every command and holds nothing. The - motors read idle at 0, as after power-on. The device name and the motor reads the configuration + motors read idle at 0, as after power-on, and every awaited event comes at once. The device name and the motor reads the configuration does not hold are not simulated. The sequencer holds ``SEQUENCER_QUEUE_SIZE`` items, stopped and empty after setup; started, it @@ -357,6 +327,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 @@ -403,9 +374,11 @@ 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] + if motor not in self._simulated_motor_limits: + raise MantisTimeoutError(f"{self.name}: no motor {motor} limits are recorded to answer with") + 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.""" @@ -420,18 +393,16 @@ async def request_motor_configuration( raise NotImplementedError(f"{self.name}: the motor configuration is not simulated") async def request_motor_status(self, *, motor: int) -> int: - """Idle, no fault; Z homed once the simulated arm homed it.""" + """The simulated arm's status word for the motor; 0, idle, without one.""" self._check_index(name="motor", value=motor, allowed=MOTORS) - if isinstance(self.arm, MantisSimulationV3Arm): + if isinstance(self.arm, MantisSimulationArm): return self.arm._statuses[motor] - if isinstance(self.arm, MantisSimulationArm) and motor == Z_MOTOR and self.arm._z_homed: - return MOTOR_HOMED return 0 async def request_motor_position(self, *, motor: int) -> float: """Where the simulated arm's controller holds the motor; 0 without one, as after power-on.""" self._check_index(name="motor", value=motor, allowed=MOTORS) - if isinstance(self.arm, (MantisSimulationArm, MantisSimulationV3Arm)): + if isinstance(self.arm, MantisSimulationArm): return self.arm._positions[motor] return 0.0 @@ -450,6 +421,17 @@ async def request_absolute_encoder_position(self, *, motor: int) -> float: self._check_index(name="motor", value=motor, allowed=ABSOLUTE_ENCODER_MOTORS) raise NotImplementedError(f"{self.name}: the absolute encoders are not simulated") + async def _wait_for_event(self, *, waiter: "asyncio.Future[bytes]", timeout: float) -> bytes: + """Return at once, with no payload: a simulated command is done when it returns. + + Args: + waiter: as ``_expect_event`` registered it; it is dropped. + timeout: unused. + """ + self._event_waiters = [entry for entry in self._event_waiters if entry[3] is not waiter] + waiter.cancel() + return b"" + def _play_queued_items(self) -> None: """Play every queued item at once, if the sequencer is started; a move moves the arm.""" if self._sequencer_started and self._queued_items: @@ -523,43 +505,45 @@ 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 + # The declared file's limits, else the generation's recording's: the v3 recording holds none. + limits = values["device"].get("motor_limits") or read_configuration(path=recording)[ + "device" + ].get("motor_limits", {}) + self._simulated_motor_limits = { + int(motor): (float(lower), float(upper)) for motor, (lower, upper) in 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.pump is not None and not isinstance(self.pump, MantisSimulationPump): self.pump = MantisSimulationPump(driver=self) - if isinstance(self.plate_clamp, PneumaticPlateClamp): - if not isinstance(self.plate_clamp, MantisSimulationPneumaticPlateClamp): - assert self.pump is not None - self.plate_clamp = MantisSimulationPneumaticPlateClamp( - driver=self, pump=self.pump, auxiliary_output=self.plate_clamp.auxiliary_output - ) - elif self.plate_clamp is not None and not isinstance( + if self.plate_clamp is not None and not isinstance( self.plate_clamp, MantisSimulationPlateClamp ): - self.plate_clamp = MantisSimulationPlateClamp(driver=self, levels=self.plate_clamp.levels) + self.plate_clamp = MantisSimulationPlateClamp( + driver=self, + levels=self.plate_clamp.levels, + auxiliary_output=self.plate_clamp.auxiliary_output, + pump=self.pump if self.plate_clamp.auxiliary_output is not None else None, + ) if self.led_bar is not None and not isinstance(self.led_bar, MantisSimulationLedBar): self.led_bar = MantisSimulationLedBar( driver=self, number_of_leds=self.led_bar.number_of_leds, show_actions=self.led_bar.show_actions, ) - if isinstance(self.arm, V3Arm): - if not isinstance(self.arm, MantisSimulationV3Arm): - self.arm = MantisSimulationV3Arm(driver=self, configuration=self.arm.configuration) - elif 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, - ) + if self.arm is not None and not isinstance(self.arm, MantisSimulationArm): + 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 e65352eb60c..25a0d9304af 100644 --- a/pylabrobot/formulatrix/mantis/driver/simulator_tests.py +++ b/pylabrobot/formulatrix/mantis/driver/simulator_tests.py @@ -4,11 +4,15 @@ 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 from pylabrobot.formulatrix.mantis.driver.arm_kinematics import ( + MantisArmCoordinate, compute_device_position, + compute_device_rate, compute_joint_angles, ) from pylabrobot.formulatrix.mantis.driver.errors import ( @@ -16,14 +20,37 @@ MantisTimeoutError, ) from pylabrobot.formulatrix.mantis.driver.features.arm import SHUTDOWN_Z -from pylabrobot.formulatrix.mantis.driver.features.arm_v3_tests import SYNTHETIC_V3_ARM_BLOCK from pylabrobot.formulatrix.mantis.driver.features.plate_clamp import PlateClamp -from pylabrobot.formulatrix.mantis.driver.features.pneumatic_plate_clamp import PneumaticPlateClamp 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_V3_ARM_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 import Coordinate +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() + head = arm._get_head_position(joint_state=joint_state) + return head.x, head.y, 0.0 - joint_state[2] + + +# The synthetic stage's reference height, mm. +REFERENCE_HEIGHT = SYNTHETIC_STAGE_BLOCK["reference_height"] class SimulationTests(unittest.IsolatedAsyncioTestCase): @@ -57,7 +84,8 @@ async def test_pneumatic_clamp_uses_and_restores_the_simulated_pump(self): mantis = Mantis(simulation=True, declared_configuration_json=path) await mantis.setup(skip_arm=True) self.addAsyncCleanup(mantis.stop) - assert isinstance(mantis.plate_clamp, PneumaticPlateClamp) and mantis.pump is not None + assert isinstance(mantis.plate_clamp, PlateClamp) and mantis.pump is not None + self.assertEqual(mantis.plate_clamp.auxiliary_output, 2) await mantis.plate_clamp.lock() self.assertTrue(await mantis.plate_clamp.request_locked()) self.assertTrue(await mantis.pump.request_pump_on()) @@ -107,15 +135,47 @@ async def test_v3_initialization_uses_the_declared_route_and_can_repeat(self): await mantis.setup() assert mantis.arm is not None self.assertTrue(mantis.arm.is_initialized) - position = await mantis.arm.request_head_position() - self.assertAlmostEqual(position.x, 15.0) - self.assertAlmostEqual(position.y, 31.177) + head_x, head_y, _ = await request_head_position(arm=mantis.arm) + self.assertAlmostEqual(head_x, 15.0) + self.assertAlmostEqual(head_y, 31.177) for motor in (0, 1, 2): self.assertEqual(await mantis.driver.request_motor_status(motor=motor), 4) await mantis.stop() self.assertFalse(mantis.arm.is_initialized) self.assertEqual(await mantis.driver.request_motor_position(motor=2), 0.0) + async def test_v3_initialization_runs_below_the_firmware_calls(self): + """The unit's Z homing values reach the home command; a retract seats the arm against its + stop, read as a following error, with the homed bit once homed.""" + with open(RECORDING_MANTIS_V4, encoding="utf-8") as file: + recording = json.load(file) + recording["device"]["firmware_versions"]["11"] = None + recording["device"]["generation"] = "v3" + block = json.loads(json.dumps(SYNTHETIC_V3_ARM_BLOCK)) + block["motors"]["2"].update({"speed": 10.0, "acceleration": 300.0}) + recording["arm"] = block + with tempfile.TemporaryDirectory() as directory: + path = os.path.join(directory, "v3.json") + with open(path, "w", encoding="utf-8") as file: + json.dump(recording, file) + mantis = Mantis(simulation=True, declared_configuration_json=path) + self.addAsyncCleanup(mantis.stop) + await mantis.setup(skip_arm=True) + arm = mantis.arm + assert arm is not None + retract = compute_device_position(motor=0, position=block["retract"]) + await arm._unchecked_fw_move_absolute(motor=0, position=retract, speed=1.0, acceleration=1.0) + self.assertEqual(await mantis.driver.request_motor_status(motor=0), 0x100) + with mock.patch.object(arm, "_unchecked_fw_home", wraps=arm._unchecked_fw_home) as home: + await arm.initialize() + z_homes = [call.kwargs for call in home.call_args_list if call.kwargs["motor"] == 2] + self.assertEqual(len(z_homes), 2) + for z_home in z_homes: + self.assertEqual(z_home["fast_speed"], compute_device_rate(motor=2, rate=10.0)) + self.assertEqual(z_home["acceleration"], compute_device_rate(motor=2, rate=300.0)) + await arm._unchecked_fw_move_absolute(motor=0, position=retract, speed=1.0, acceleration=1.0) + self.assertEqual(await mantis.driver.request_motor_status(motor=0), 0x104) + async def test_rfid_reads_every_declared_slot_empty(self): rfid = self.mantis.rfid assert rfid is not None @@ -132,7 +192,9 @@ async def test_setup_brings_the_arm_to_idle_and_homes_z(self): # Without offsets, the arm motors' idle targets are the links' angles for the head at idle. idle = [ compute_device_position(motor=motor, position=angle) - for motor, angle in enumerate(compute_joint_angles(x=15.0, y=31.177)) + for motor, angle in enumerate( + compute_joint_angles(location=MantisArmCoordinate(x=15.0, y=31.177, z=None)) + ) ] for motor, status, position in ((0, 0, idle[0]), (1, 0, idle[1]), (2, 4, 0.0)): with self.subTest(motor=motor): @@ -147,12 +209,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): @@ -169,7 +231,7 @@ async def test_an_arm_away_from_idle_is_refused(self): self.addAsyncCleanup(mantis.stop) arm = mantis.driver.arm assert isinstance(arm, MantisSimulationArm) - theta_1, theta_2 = compute_joint_angles(x=-57.0, y=0.0) + theta_1, theta_2 = compute_joint_angles(location=MantisArmCoordinate(x=-57.0, y=0.0, z=None)) arm._absolute_positions = { 0: compute_device_position(motor=0, position=theta_1), 1: compute_device_position(motor=1, position=theta_2), @@ -179,7 +241,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] @@ -190,8 +252,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 @@ -286,5 +348,218 @@ 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_leaving_a_path_never_moves_the_arms_while_z_rises(self): + # Z down positive in device units: a target below where Z stands raises it. + driver = self.arm._driver + for name in ("waste", "wash_1"): + with self.subTest(path=name): + if name == "waste": + await self.arm.move_to_waste() + else: + await self.arm.move_to_wash(position=1) + positions = dict(self.arm._positions) + with mock.patch.object(driver, "_queue_move_item", wraps=driver._queue_move_item) as queue: + await self.arm.park() + self.assertTrue(queue.called) + for call in queue.call_args_list: + targets = call.kwargs["targets"] + named = {motor: target[0] for motor, target in enumerate(targets) if target[1] != 0} + arms_move = any(positions[motor] != named[motor] for motor in (0, 1) if motor in named) + z_rises = 2 in named and named[2] < positions[2] + self.assertFalse( + arms_move and z_rises, f"one item moves the arms and raises Z: {targets}" + ) + positions.update(named) + + async def test_on_a_path_z_alone_and_bad_arguments_move_nothing(self): + await self.arm.move_to_waste() + positions = dict(self.arm._positions) + with self.assertRaisesRegex(MantisMotionError, "on the waste approach path; Z alone"): + await self.arm.move_to_z_position(z=50.0) + with self.assertRaisesRegex(MantisMotionError, "on the waste approach path; Z alone"): + await self.arm.move_to_joint_state(joint_state={2: 5.0}) + # A traverse height that is no height is refused before the head leaves the path. + nan = float("nan") + deck_x, deck_y = self.arm._get_deck_frame_xy(x=-57.0, y=0.0) + with self.assertRaisesRegex(ValueError, "outside its limits"): + await self.arm.move_to_xy_position(x=deck_x, y=deck_y, minimum_traverse_height_start=nan) + with self.assertRaisesRegex(ValueError, "outside its limits"): + await self.arm.move_to_location( + location=Coordinate(x=deck_x, y=deck_y, z=60.0), minimum_traverse_height_start=nan + ) + with self.assertRaisesRegex(ValueError, "outside its limits"): + await self.arm.park(minimum_traverse_height_start=nan) + self.assertEqual(self.arm._positions, positions) + + async def test_move_to_xy_position_and_move_to_location_send_the_same_arm_targets(self): + # (-60, 14) mm in the arm frame: move_to_xy_position's targets go through the joint state and + # back, so they may differ in the last bit; move_to_location sends them as computed. + location = Coordinate(*self.arm._get_deck_frame_xy(x=-60.0, y=14.0), z=60.0) + driver = self.arm._driver + sent = [] + for move in ( + self.arm.move_to_xy_position(x=location.x, y=location.y), + self.arm.move_to_location(location=location), + ): + await self.arm.park() + with mock.patch.object(driver, "_queue_move_item", wraps=driver._queue_move_item) as queue: + await move + sent.append(queue.call_args_list[0].kwargs["targets"][:2]) + for xy_target, location_target in zip(sent[0], sent[1]): + self.assertAlmostEqual(xy_target[0], location_target[0], places=12) + + 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(location=MantisArmCoordinate(x=15.0, y=31.177, z=None)) + 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_frame_location = 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) + assert arm_frame_location.x is not None and arm_frame_location.y is not None + assert arm_frame_location.z is not None + self.assertAlmostEqual(head_x, arm_frame_location.x, places=3) + self.assertAlmostEqual(head_y, arm_frame_location.y, places=3) + self.assertAlmostEqual(head_z, -arm_frame_location.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..2c2e6ebab29 --- /dev/null +++ b/pylabrobot/formulatrix/mantis/driver/stage_transform.py @@ -0,0 +1,55 @@ +"""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 .arm_kinematics import MantisArmCoordinate +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], +) -> MantisArmCoordinate: + """A point of the wellplate frame in the arm frame, z as Z's position down from its 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:.2f}, {y:.2f}) mm on the wellplate to infinity" + ) + return MantisArmCoordinate( + x=mapped[0] / mapped[2], + y=mapped[1] / mapped[2], + z=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..801cff56e49 --- /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.x, actual.y, actual.z), 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/driver/v3_arm_configuration.py b/pylabrobot/formulatrix/mantis/driver/v3_arm_configuration.py index 64d1f89eac5..322014fd630 100644 --- a/pylabrobot/formulatrix/mantis/driver/v3_arm_configuration.py +++ b/pylabrobot/formulatrix/mantis/driver/v3_arm_configuration.py @@ -2,20 +2,22 @@ import math from dataclasses import dataclass -from typing import Any, Dict, List, Tuple +from typing import Any, Dict, List -from .arm_kinematics import compute_joint_angles +from .arm_kinematics import MantisArmCoordinate, compute_joint_angles from .errors import MantisConfigurationError -def _finite(value: Any) -> float: - """Read a finite calibration value, rejecting booleans and non-numbers.""" +def _check_finite(*, value: Any) -> None: + """Raise ``ValueError`` unless a calibration value is a finite number, not a boolean. + + Args: + value: the value as the block holds it. + """ if isinstance(value, bool): raise ValueError("a calibration value cannot be a boolean") - number = float(value) - if not math.isfinite(number): + if not math.isfinite(float(value)): raise ValueError("calibration values must be finite") - return number @dataclass(frozen=True) @@ -34,68 +36,82 @@ class V3ArmConfiguration: """The unit's offsets, homing frame and route to idle, in degrees, mm and seconds.""" offsets: Dict[int, float] - home_positions: Dict[int, float] + homing_position_n: Dict[int, float] motors: Dict[int, MotorHoming] retract: float backoff: float jog_speed: float jog_acceleration: float - motor_delay: float - post_home_positions: List[Tuple[float, float]] + motor_xy_homing_delay: float + post_homing_positions: List[MantisArmCoordinate] @classmethod - def from_block(cls, block: Dict[str, Any]) -> "V3ArmConfiguration": - """Validate a saved ``arm`` block before a motor command can be sent.""" + def from_block(cls, *, block: Dict[str, Any]) -> "V3ArmConfiguration": + """Validate a saved ``arm`` block before a motor command can be sent. + + Args: + block: the unit's ``arm`` block, as saved. + """ try: - maps = { - key: {int(motor): _finite(value) for motor, value in block[key].items()} - for key in ("offsets", "home_positions") - } + maps: Dict[str, Dict[int, float]] = {} + for key in ("offsets", "homing_position_n"): + maps[key] = {} + for motor, value in block[key].items(): + _check_finite(value=value) + maps[key][int(motor)] = float(value) if any(set(values) != {0, 1} for values in maps.values()): - raise ValueError("offsets and home_positions must name motors 0 and 1") + raise ValueError("offsets and homing_position_n must name motors 0 and 1") motors = {} for motor, values in block["motors"].items(): - method = _finite(values["method"]) + for key in ("method", "speed", "acceleration"): + _check_finite(value=values[key]) + method = float(values["method"]) if method != int(method) or not 0 <= method <= 32767: raise ValueError("homing method must be a nonnegative int16") if any(type(values[key]) is not bool for key in ("positive_edge", "positive_direction")): raise ValueError("homing edge and direction must be booleans") - speed, acceleration = _finite(values["speed"]), _finite(values["acceleration"]) + speed, acceleration = float(values["speed"]), float(values["acceleration"]) if min(speed, acceleration) <= 0: raise ValueError("homing speed and acceleration must be positive") motors[int(motor)] = MotorHoming( - int(method), values["positive_edge"], values["positive_direction"], speed, acceleration + method=int(method), + positive_edge=values["positive_edge"], + positive_direction=values["positive_direction"], + speed=speed, + acceleration=acceleration, ) if set(motors) != {0, 1, 2}: raise ValueError("homing settings must name motors 0, 1 and 2") - scalars = { - key: _finite(block[key]) - for key in ("retract", "backoff", "jog_speed", "jog_acceleration", "motor_delay") - } + scalars: Dict[str, float] = {} + for key in ("retract", "backoff", "jog_speed", "jog_acceleration", "motor_xy_homing_delay"): + _check_finite(value=block[key]) + scalars[key] = float(block[key]) if ( scalars["retract"] >= 0 or min(scalars["backoff"], scalars["jog_speed"], scalars["jog_acceleration"]) <= 0 - or scalars["motor_delay"] < 0 + or scalars["motor_xy_homing_delay"] < 0 ): raise ValueError("retract must be negative, rates and backoff positive, delay nonnegative") positions = [] - for point in block["post_home_positions"]: + for point in block["post_homing_positions"]: x, y = point - x, y = _finite(x), _finite(y) - compute_joint_angles(x=x, y=y) - positions.append((x, y)) + _check_finite(value=x) + _check_finite(value=y) + position = MantisArmCoordinate(x=float(x), y=float(y), z=None) + compute_joint_angles(location=position) + positions.append(position) if not positions: - raise ValueError("post_home_positions must contain a route to idle") + raise ValueError("post_homing_positions must contain a route to idle") return cls( offsets=maps["offsets"], - home_positions=maps["home_positions"], + homing_position_n=maps["homing_position_n"], motors=motors, retract=scalars["retract"], backoff=scalars["backoff"], jog_speed=scalars["jog_speed"], jog_acceleration=scalars["jog_acceleration"], - motor_delay=scalars["motor_delay"], - post_home_positions=positions, + motor_xy_homing_delay=scalars["motor_xy_homing_delay"], + post_homing_positions=positions, ) except (KeyError, TypeError, ValueError, AttributeError) as error: raise MantisConfigurationError(f"malformed v3 arm block: {error}") from error diff --git a/pylabrobot/formulatrix/mantis/resource_model/__init__.py b/pylabrobot/formulatrix/mantis/resource_model/__init__.py index a3ea790eb92..078b8fd8cf3 100644 --- a/pylabrobot/formulatrix/mantis/resource_model/__init__.py +++ b/pylabrobot/formulatrix/mantis/resource_model/__init__.py @@ -1,8 +1,7 @@ """The Mantis's parts, as resources, from the manufacturer's model. Sizes and positions come from ``manifest.json``. Each part's ``mesh`` is its ``.glb``: mm, Z up, -in the part's own frame; the base's is the main box's frame. An arm link has no model file and is -drawn as its box. +in the part's own frame; the base's is the main box's frame. """ import json @@ -17,6 +16,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.""" @@ -65,7 +74,7 @@ def z_column(name: str) -> Resource: def arm_link(name: str, *, link: str) -> LinkBody: - """One of the arm's four links, drawn as its box. Motor 0 turns the right pair, motor 1 the left. + """One of the arm's four links. Motor 0 turns the right pair, motor 1 the left. Args: name: the resource name. @@ -74,7 +83,7 @@ def arm_link(name: str, *, link: str) -> LinkBody: """ part = MANIFEST["arm_links"][link] size_x, size_y, size_z = part["size"] - return LinkBody( + body = LinkBody( name=name, size_x=size_x, size_y=size_y, @@ -82,8 +91,24 @@ def arm_link(name: str, *, link: str) -> LinkBody: proximal_joint=Coordinate(*part["proximal_joint"]), distal_joint=Coordinate(*part["distal_joint"]), category="mantis_arm_link", - model=f"formulatrix_mantis_{link}", + model=part["file"][: -len(".glb")], ) - - -__all__ = ["MANIFEST", "arm_link", "chip_rack", "z_column"] + body.mesh = _mesh(file=part["file"]) # type: ignore[attr-defined] + return body + + +__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", +] diff --git a/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_left_long_arm.glb b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_left_long_arm.glb new file mode 100644 index 00000000000..b454e9534e6 Binary files /dev/null and b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_left_long_arm.glb differ diff --git a/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_left_short_arm.glb b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_left_short_arm.glb new file mode 100644 index 00000000000..9feebee8afb Binary files /dev/null and b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_left_short_arm.glb differ diff --git a/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_right_long_arm.glb b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_right_long_arm.glb new file mode 100644 index 00000000000..0dcd6831d0a Binary files /dev/null and b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_right_long_arm.glb differ diff --git a/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_right_short_arm.glb b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_right_short_arm.glb new file mode 100644 index 00000000000..4ba847c2bf3 Binary files /dev/null and b/pylabrobot/formulatrix/mantis/resource_model/formulatrix_mantis_right_short_arm.glb differ diff --git a/pylabrobot/formulatrix/mantis/resource_model/manifest.json b/pylabrobot/formulatrix/mantis/resource_model/manifest.json index c9e6020760e..16da9133f46 100644 --- a/pylabrobot/formulatrix/mantis/resource_model/manifest.json +++ b/pylabrobot/formulatrix/mantis/resource_model/manifest.json @@ -1,5 +1,5 @@ { - "note": "Modular. The device frame is the main box's: origin at the body's front-left corner, at the bottom of the feet. The base file is authored in that frame and also draws the side lobes outside the box. Every other file is authored in its own resource's frame, its lowest corner at the origin, so declaring it on that resource needs no offset. `measured_at` records where a part sits in the device frame in the pose the manufacturer's model is drawn in, in millimetres. An arm link is drawn as its box (no model file): `proximal_joint` and `distal_joint` sit in its own frame, and `above` is how far an outer link sits above the short link it hangs from. `joints` record where, in the device frame and the pose drawn (the arm at idle), the right motor turns (`base_right`, at the height `arm_z`).", + "note": "Modular. The device frame is the main box's: origin at the body's front-left corner, at the bottom of the feet. The base file is authored in that frame and also draws the side lobes outside the box. Every other file is authored in its own resource's frame, its lowest corner at the origin, so declaring it on that resource needs no offset. `measured_at` records where a part sits in the device frame in the pose the manufacturer's model is drawn in, in millimetres. An arm link's file runs proximal to distal along +x in its own frame, where `proximal_joint` and `distal_joint` sit, and `above` is how far an outer link sits above the short link it hangs from. `joints` record where, in the device frame and the pose drawn (the arm at idle), the right motor turns (`base_right`, at the height `arm_z`).", "units": "mm", "up": "Z", "frames": { @@ -53,6 +53,7 @@ }, "arm_links": { "right_short_arm": { + "file": "formulatrix_mantis_right_short_arm.glb", "size": [ 77.0, 22.04, @@ -71,6 +72,7 @@ "above": 0.0 }, "right_long_arm": { + "file": "formulatrix_mantis_right_long_arm.glb", "size": [ 100.47, 17.0, @@ -89,6 +91,7 @@ "above": 7.56 }, "left_short_arm": { + "file": "formulatrix_mantis_left_short_arm.glb", "size": [ 73.97, 14.0, @@ -107,6 +110,7 @@ "above": 0.0 }, "left_long_arm": { + "file": "formulatrix_mantis_left_long_arm.glb", "size": [ 96.54, 13.94,