From f53151fdbe2e67fc2f5b33b1d54156edaec779f2 Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Mon, 15 Oct 2018 10:54:41 -0400 Subject: [PATCH 1/3] refactor(api): Move robot_configs.py to config --- .../{legacy_api/robot => config}/robot_configs.py | 0 api/src/opentrons/deck_calibration/dc_main.py | 2 +- api/src/opentrons/deck_calibration/endpoints.py | 2 +- api/src/opentrons/hardware_control/controller.py | 2 +- api/src/opentrons/server/endpoints/settings.py | 3 +-- api/src/opentrons/util/calibration_functions.py | 3 +-- api/tests/opentrons/cli/test_cli.py | 6 +----- api/tests/opentrons/conftest.py | 2 +- .../{robot => hardware_control}/test_robots_config.py | 10 +++------- .../opentrons/server/test_calibration_endpoints.py | 2 +- api/tests/opentrons/server/test_settings_endpoints.py | 2 +- api/tests/opentrons/util/test_tip_probe.py | 3 +-- 12 files changed, 13 insertions(+), 24 deletions(-) rename api/src/opentrons/{legacy_api/robot => config}/robot_configs.py (100%) rename api/tests/opentrons/{robot => hardware_control}/test_robots_config.py (91%) diff --git a/api/src/opentrons/legacy_api/robot/robot_configs.py b/api/src/opentrons/config/robot_configs.py similarity index 100% rename from api/src/opentrons/legacy_api/robot/robot_configs.py rename to api/src/opentrons/config/robot_configs.py diff --git a/api/src/opentrons/deck_calibration/dc_main.py b/api/src/opentrons/deck_calibration/dc_main.py index 7dd351d97af..e25094fc907 100644 --- a/api/src/opentrons/deck_calibration/dc_main.py +++ b/api/src/opentrons/deck_calibration/dc_main.py @@ -11,8 +11,8 @@ from typing import Tuple from numpy.linalg import inv from numpy import dot, array -from opentrons.legacy_api.robot import robot_configs from opentrons import robot, instruments +from opentrons.config import robot_configs from opentrons.util.calibration_functions import probe_instrument from opentrons.deck_calibration.linal import solve, add_z, apply_transform from opentrons.deck_calibration import * diff --git a/api/src/opentrons/deck_calibration/endpoints.py b/api/src/opentrons/deck_calibration/endpoints.py index b39ba5c3d31..f20513501d1 100644 --- a/api/src/opentrons/deck_calibration/endpoints.py +++ b/api/src/opentrons/deck_calibration/endpoints.py @@ -2,7 +2,7 @@ from uuid import uuid1 from opentrons.legacy_api.instruments import pipette_config from opentrons import instruments, robot -from opentrons.legacy_api.robot import robot_configs +from opentrons.config import robot_configs from opentrons.deck_calibration import jog, position, dots_set, z_pos from opentrons.deck_calibration.linal import add_z, solve from typing import Dict, Tuple diff --git a/api/src/opentrons/hardware_control/controller.py b/api/src/opentrons/hardware_control/controller.py index 70e1a832e4d..c70a9927b5f 100644 --- a/api/src/opentrons/hardware_control/controller.py +++ b/api/src/opentrons/hardware_control/controller.py @@ -5,7 +5,7 @@ from typing import Dict, List, Optional, Tuple from opentrons.util import environment from opentrons.drivers.smoothie_drivers import driver_3_0 -from opentrons.legacy_api.robot import robot_configs +from opentrons.config import robot_configs from . import modules diff --git a/api/src/opentrons/server/endpoints/settings.py b/api/src/opentrons/server/endpoints/settings.py index 00bb0efd8f8..e32c5f5ada3 100644 --- a/api/src/opentrons/server/endpoints/settings.py +++ b/api/src/opentrons/server/endpoints/settings.py @@ -2,8 +2,7 @@ import shutil import os from aiohttp import web -from opentrons.config import advanced_settings as advs -from opentrons.legacy_api.robot import robot_configs as rc +from opentrons.config import advanced_settings as advs, robot_configs as rc from opentrons.data_storage import database as db log = logging.getLogger(__name__) diff --git a/api/src/opentrons/util/calibration_functions.py b/api/src/opentrons/util/calibration_functions.py index 56610021986..9a04e08613e 100644 --- a/api/src/opentrons/util/calibration_functions.py +++ b/api/src/opentrons/util/calibration_functions.py @@ -3,10 +3,9 @@ from opentrons.trackers.pose_tracker import ( update, Point, change_base ) -from opentrons.legacy_api.robot import robot_configs from opentrons.data_storage import database from opentrons.trackers.pose_tracker import absolute -from opentrons.config import feature_flags as ff +from opentrons.config import feature_flags as ff, robot_configs import logging diff --git a/api/tests/opentrons/cli/test_cli.py b/api/tests/opentrons/cli/test_cli.py index 29374a52418..a333b18fa15 100755 --- a/api/tests/opentrons/cli/test_cli.py +++ b/api/tests/opentrons/cli/test_cli.py @@ -1,12 +1,10 @@ import pytest -from opentrons.config import get_config_index +from opentrons.config import get_config_index, robot_configs from opentrons.config import advanced_settings as advs @pytest.fixture def mock_config(): - from opentrons.legacy_api.robot import robot_configs - test_config = robot_configs.load() test_config = test_config._replace(name='new-value1') robot_configs.save_robot_settings(test_config) @@ -21,7 +19,6 @@ def test_clear_config(mock_config): dc_main.clear_configuration_and_reload() from opentrons import robot - from opentrons.legacy_api.robot import robot_configs assert robot.config == robot_configs._build_config({}, {}) @@ -42,7 +39,6 @@ def test_save_and_clear_config(mock_config): dc_main.backup_configuration_and_reload(tag=tag) from opentrons import robot - from opentrons.legacy_api.robot import robot_configs assert robot.config == robot_configs._build_config({}, {}) diff --git a/api/tests/opentrons/conftest.py b/api/tests/opentrons/conftest.py index 08eb71ac49c..5ce5d8b1f03 100755 --- a/api/tests/opentrons/conftest.py +++ b/api/tests/opentrons/conftest.py @@ -388,7 +388,7 @@ def model_with_trough(robot): def smoothie(monkeypatch): from opentrons.drivers.smoothie_drivers.driver_3_0 import \ SmoothieDriver_3_0_0 as SmoothieDriver - from opentrons.legacy_api.robot import robot_configs + from opentrons.config import robot_configs monkeypatch.setenv('ENABLE_VIRTUAL_SMOOTHIE', 'true') driver = SmoothieDriver(robot_configs.load()) diff --git a/api/tests/opentrons/robot/test_robots_config.py b/api/tests/opentrons/hardware_control/test_robots_config.py similarity index 91% rename from api/tests/opentrons/robot/test_robots_config.py rename to api/tests/opentrons/hardware_control/test_robots_config.py index 45bdf7d1f38..0fe52760d55 100644 --- a/api/tests/opentrons/robot/test_robots_config.py +++ b/api/tests/opentrons/hardware_control/test_robots_config.py @@ -1,6 +1,7 @@ -def test_old_probe_height(short_trash_flag): - from opentrons.legacy_api.robot import robot_configs +from opentrons.config import robot_configs + +def test_old_probe_height(short_trash_flag): cfg = robot_configs.load() assert cfg.probe_center[2] == 55.0 @@ -8,8 +9,6 @@ def test_old_probe_height(short_trash_flag): def test_default_probe_height(): - from opentrons.legacy_api.robot import robot_configs - cfg = robot_configs.load() assert cfg.probe_center[2] == 77.0 assert cfg.probe_dimensions[2] == 82.0 @@ -17,7 +16,6 @@ def test_default_probe_height(): def test_load_corrupt_json(): import os - from opentrons.legacy_api.robot import robot_configs filename = os.path.join(os.path.dirname(__file__), 'bad_config.json') with open(filename, 'w') as file: file.write('') # empty config file @@ -27,8 +25,6 @@ def test_load_corrupt_json(): def test_build_config(): - from opentrons.legacy_api.robot import robot_configs - deck_cal = [ [1.23, 1.23, 1.23, 1.23], [1.23, 1.23, 1.23, 1.23], diff --git a/api/tests/opentrons/server/test_calibration_endpoints.py b/api/tests/opentrons/server/test_calibration_endpoints.py index 08fb3a9c02d..a5a44a31113 100644 --- a/api/tests/opentrons/server/test_calibration_endpoints.py +++ b/api/tests/opentrons/server/test_calibration_endpoints.py @@ -3,7 +3,7 @@ from opentrons import robot, instruments from opentrons import deck_calibration as dc from opentrons.deck_calibration import endpoints -from opentrons.legacy_api.robot import robot_configs +from opentrons.config import robot_configs # Note that several tests in this file have target/expected values that do not diff --git a/api/tests/opentrons/server/test_settings_endpoints.py b/api/tests/opentrons/server/test_settings_endpoints.py index d8b9eec5feb..38d08543bb7 100644 --- a/api/tests/opentrons/server/test_settings_endpoints.py +++ b/api/tests/opentrons/server/test_settings_endpoints.py @@ -6,7 +6,7 @@ from opentrons.server import init from opentrons.data_storage import database as db -from opentrons.legacy_api.robot import robot_configs as rc +from opentrons.config import robot_configs as rc def validate_response_body(body): diff --git a/api/tests/opentrons/util/test_tip_probe.py b/api/tests/opentrons/util/test_tip_probe.py index f08367aed14..0816ba8404e 100755 --- a/api/tests/opentrons/util/test_tip_probe.py +++ b/api/tests/opentrons/util/test_tip_probe.py @@ -1,6 +1,5 @@ import pytest -from opentrons.legacy_api.robot import robot_configs -from opentrons.config import get_config_index +from opentrons.config import get_config_index, robot_configs from opentrons.util.calibration_functions import update_instrument_config From cdc9b872ad485702b527f09bce6fc0f7983519f1 Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Mon, 15 Oct 2018 15:59:03 -0400 Subject: [PATCH 2/3] feat(api): hardware_control uses deck absolute coordinates Closes #2238 --- api/setup.py | 2 - api/src/opentrons/deck_calibration/dc_main.py | 4 +- .../opentrons/deck_calibration/endpoints.py | 4 +- .../opentrons/hardware_control/__init__.py | 233 ++++++++++++++---- .../opentrons/hardware_control/controller.py | 13 +- .../opentrons/hardware_control/simulator.py | 5 +- api/src/opentrons/hardware_control/types.py | 26 ++ api/src/opentrons/legacy_api/robot/robot.py | 5 +- .../{deck_calibration => util}/linal.py | 24 +- api/tests/opentrons/cli/test_linal.py | 5 +- .../opentrons/hardware_control/test_moves.py | 86 ++++++- 11 files changed, 335 insertions(+), 72 deletions(-) create mode 100644 api/src/opentrons/hardware_control/types.py rename api/src/opentrons/{deck_calibration => util}/linal.py (82%) diff --git a/api/setup.py b/api/setup.py index f6b0c0a4b0f..a3a344c54af 100755 --- a/api/setup.py +++ b/api/setup.py @@ -68,10 +68,8 @@ def _get_data_files(self): to_include = get_shared_data_files() destination = os.path.join(build_base, DEST_BASE_PATH) # And finally, tell the system about our files - print("FILES BEFORE {}".format(files)) files.append(('opentrons', SHARED_DATA_PATH, destination, to_include)) - print("FILES AFTER {}".format(files)) return files diff --git a/api/src/opentrons/deck_calibration/dc_main.py b/api/src/opentrons/deck_calibration/dc_main.py index e25094fc907..36efccbae51 100644 --- a/api/src/opentrons/deck_calibration/dc_main.py +++ b/api/src/opentrons/deck_calibration/dc_main.py @@ -14,7 +14,7 @@ from opentrons import robot, instruments from opentrons.config import robot_configs from opentrons.util.calibration_functions import probe_instrument -from opentrons.deck_calibration.linal import solve, add_z, apply_transform +from opentrons.util.linal import solve, add_z, apply_transform from opentrons.deck_calibration import * # TODO: add tests for methods, split out current point behavior per comment @@ -329,7 +329,7 @@ def _update_text_box(self, msg): points, # 'Smoothie: {}'.format(self.current_position), 'World: {}'.format(apply_transform( - self._calibration_matrix, self.current_position)), + inv(self._calibration_matrix), self.current_position)), 'Step: {}'.format(self.current_step()), 'Message: {}'.format(msg) ]) diff --git a/api/src/opentrons/deck_calibration/endpoints.py b/api/src/opentrons/deck_calibration/endpoints.py index f20513501d1..700c5e099eb 100644 --- a/api/src/opentrons/deck_calibration/endpoints.py +++ b/api/src/opentrons/deck_calibration/endpoints.py @@ -3,8 +3,8 @@ from opentrons.legacy_api.instruments import pipette_config from opentrons import instruments, robot from opentrons.config import robot_configs -from opentrons.deck_calibration import jog, position, dots_set, z_pos -from opentrons.deck_calibration.linal import add_z, solve +from . import jog, position, dots_set, z_pos +from opentrons.util.linal import add_z, solve from typing import Dict, Tuple import logging diff --git a/api/src/opentrons/hardware_control/__init__.py b/api/src/opentrons/hardware_control/__init__.py index aea7dd8ba3e..4f8cba18d70 100644 --- a/api/src/opentrons/hardware_control/__init__.py +++ b/api/src/opentrons/hardware_control/__init__.py @@ -11,18 +11,23 @@ """ import asyncio +from collections import OrderedDict import functools import logging -import enum from typing import Any, Dict, Union, List, Optional, Tuple -from opentrons import types + +from opentrons import types as top_types +from opentrons.util import linal from .simulator import Simulator +from opentrons.config import robot_configs + try: from .controller import Controller except ModuleNotFoundError: # implies windows Controller = None # type: ignore from . import modules +from .types import Axis mod_log = logging.getLogger(__name__) @@ -36,18 +41,6 @@ def _log_call_inner(*args, **kwargs): return _log_call_inner -class _Axis(enum.Enum): - X = enum.auto() - Y = enum.auto() - Z = enum.auto() - A = enum.auto() - - @classmethod - def by_mount(cls, mount): - bm = {types.Mount.LEFT: cls.Z, types.Mount.RIGHT: cls.A} - return bm[mount] - - class MustHomeError(RuntimeError): pass @@ -70,7 +63,7 @@ class API: def __init__(self, backend: _Backend, - config: dict = None, + config: robot_configs.robot_config = None, loop: asyncio.AbstractEventLoop = None) -> None: """ Initialize an API instance. @@ -79,21 +72,22 @@ def __init__(self, build_hardware_simulator should be used. """ self._log = self.CLS_LOG.getChild(str(id(self))) + self._config = config or robot_configs.load() self._backend = backend if None is loop: self._loop = asyncio.get_event_loop() else: self._loop = loop # {'X': 0.0, 'Y': 0.0, 'Z': 0.0, 'A': 0.0, 'B': 0.0, 'C': 0.0} - self._current_position: Dict[str, float] = {} + self._current_position: Dict[Axis, float] = {} - self._attached_instruments = {types.Mount.LEFT: None, - types.Mount.RIGHT: None} + self._attached_instruments = {top_types.Mount.LEFT: None, + top_types.Mount.RIGHT: None} self._attached_modules: Dict[str, Any] = {} @classmethod def build_hardware_controller( - cls, config: dict = None, + cls, config: robot_configs.robot_config = None, loop: asyncio.AbstractEventLoop = None) -> 'API': """ Build a hardware controller that will actually talk to hardware. @@ -110,9 +104,9 @@ def build_hardware_controller( @classmethod def build_hardware_simulator( cls, - attached_instruments: Dict[types.Mount, Optional[str]] = None, + attached_instruments: Dict[top_types.Mount, Optional[str]] = None, attached_modules: List[str] = None, - config: dict = None, + config: robot_configs.robot_config = None, loop: asyncio.AbstractEventLoop = None) -> 'API': """ Build a simulating hardware controller. @@ -120,8 +114,8 @@ def build_hardware_simulator( Multiple simulating hardware controllers may be active at one time. """ if None is attached_instruments: - attached_instruments = {types.Mount.LEFT: None, - types.Mount.RIGHT: None} + attached_instruments = {top_types.Mount.LEFT: None, + top_types.Mount.RIGHT: None} if None is attached_modules: attached_modules = [] return cls(Simulator(attached_instruments, @@ -160,7 +154,7 @@ async def identify(self, seconds): @_log_call async def cache_instrument_models(self): self._log.info("Updating instrument model cache") - for mount in types.Mount: + for mount in top_types.Mount: self._attached_instruments[mount] = \ self._backend.get_attached_instruments(mount) @@ -183,52 +177,199 @@ async def halt(self): # Gantry/frame (i.e. not pipette) action API @_log_call - async def home(self, *args, **kwargs): - # Initialize/update current_position - self._current_position = self._backend.home() + async def home_z(self, mount: top_types.Mount): + """ Home one mount's Z-axis """ + backend_pos = self._backend.home(Axis.by_mount(mount)) + self._current_position = self._deck_from_smoothie(backend_pos) @_log_call - async def home_z(self): - pass + async def home(self): + """ Home the entire robot and initialize current position. + """ + # Initialize/update current_position + smoothie_pos = self._backend.home() + self._current_position = self._deck_from_smoothie(smoothie_pos) + + def _deck_from_smoothie( + self, smoothie_pos: Dict[str, float]) -> Dict[Axis, float]: + """ Build a deck-abs position store from the smoothie's position + + This should take the smoothie style position {'X': float, etc} + and turn it into the position dict used here {Axis.X: float} in + deck-absolute coordinates. It runs the reverse deck transformation + for the axes that require it. + + One piece of complexity is that if the gantry transformation includes + a transition between non parallel planes, the z position of the left + mount would depend on its actual position in deck frame, so we have + to apply the mount offset. + + TODO: Figure out which frame the mount offset is measured in, because + if it's measured in the deck frame (e.g. by touching off points + on the deck) it has to go through the reverse transform to be + added to the smoothie coordinates here. + """ + with_enum = {Axis[k]: v for k, v in smoothie_pos.items()} + other = {k: v for k, v in with_enum.items() + if k not in Axis.gantry_axes()} + right = (with_enum[Axis.X], with_enum[Axis.Y], + with_enum[Axis.by_mount(top_types.Mount.RIGHT)]) + # Tell apply_transform to just do the change of base part of the + # transform rather than the full affine transform, because this is + # an offset + offset_in_smoothie = linal.apply_transform( + self.config.gantry_calibration, + self.config.mount_offset, + False) + left = (with_enum[Axis.X] + offset_in_smoothie[0], + with_enum[Axis.Y] + offset_in_smoothie[1], + with_enum[Axis.by_mount(top_types.Mount.LEFT)] + + offset_in_smoothie[2]) + right_deck = linal.apply_reverse(self.config.gantry_calibration, + right) + left_deck = linal.apply_reverse(self.config.gantry_calibration, + left) + deck_pos = {Axis.X: right_deck[0], + Axis.Y: right_deck[1], + Axis.by_mount(top_types.Mount.RIGHT): right_deck[2], + Axis.by_mount(top_types.Mount.LEFT): left_deck[2]} + deck_pos.update(other) + return deck_pos + + def current_position(self, mount: top_types.Mount) -> Dict[Axis, float]: + """ Return the postion (in deck coords) of the critical point of the + specified mount. + + This returns cached position to avoid hitting the smoothie driver + unless ``refresh`` is ``True``. + """ + if mount == mount.RIGHT: + offset = top_types.Point(0, 0, 0) + else: + offset = top_types.Point(*self.config.mount_offset) + z_ax = Axis.by_mount(mount) + return { + Axis.X: self._current_position[Axis.X] + offset[0], + Axis.Y: self._current_position[Axis.Y] + offset[1], + z_ax: self._current_position[z_ax] + offset[2] + } @_log_call async def move_to( - self, mount: types.Mount, abs_position: types.Point): + self, mount: top_types.Mount, abs_position: top_types.Point): + """ Move the critical point of the specified mount to a location + relative to the deck. + + The critical point of the mount depends on the current status of + the mount: + - If the mount does not have anything attached, its critical point is + the bottom of the mount attach bracket. + - If the mount has a pipette attached and it is not known to have a + pipette tip, the critical point is the end of the nozzle of a single + pipette or the end of the backmost nozzle of a multipipette + - If the mount has a pipette attached and it is known to have a + pipette tip, the critical point is the end of the pipette tip for + a single pipette or the end of the tip of the backmost nozzle of a + multipipette + """ if not self._current_position: raise MustHomeError - z_axis = _Axis.by_mount(mount) - try: - target_position = {_Axis.X.name: abs_position.x, - _Axis.Y.name: abs_position.y, - z_axis.name: abs_position.z} - except KeyError: - raise MustHomeError + z_axis = Axis.by_mount(mount) + if mount == top_types.Mount.LEFT: + offset = top_types.Point(*self.config.mount_offset) + else: + offset = top_types.Point(0, 0, 0) + target_position = OrderedDict( + ((Axis.X, abs_position.x - offset.x), + (Axis.Y, abs_position.y - offset.y), + (z_axis, abs_position.z - offset.z)) + ) await self._move(target_position) @_log_call - async def move_rel(self, mount: types.Mount, delta: types.Point): + async def move_rel(self, mount: top_types.Mount, delta: top_types.Point): + """ Move the critical point of the specified mount by a specified + displacement in a specified direction. + """ if not self._current_position: raise MustHomeError - z_axis = _Axis.by_mount(mount) + z_axis = Axis.by_mount(mount) try: - target_position = \ - {_Axis.X.name: self._current_position[_Axis.X.name] + delta.x, - _Axis.Y.name: self._current_position[_Axis.Y.name] + delta.y, - z_axis.name: self._current_position[z_axis.name] + delta.z} + target_position = OrderedDict( + ((Axis.X, + self._current_position[Axis.X] + delta.x), + (Axis.Y, + self._current_position[Axis.Y] + delta.y), + (z_axis, + self._current_position[z_axis] + delta.z)) + ) except KeyError: raise MustHomeError await self._move(target_position) - async def _move(self, target_position: Dict[str, float]): - self._current_position.update(target_position) + async def _move(self, target_position: 'OrderedDict[Axis, float]'): + """ Worker function to apply robot motion. + + Robot motion means the kind of motions that are relevant to the robot, + i.e. only one pipette plunger and mount move at the same time, and an + XYZ move in the coordinate frame of one of the pipettes. + + ``target_position`` should be an ordered dict (ordered by XYZABC) + containing any specified XY motion and at most one of a ZA or BC + components. The frame in which to move is identified by the presence of + (ZA) or (BC). + """ + # Transform only the x, y, and (z or a) axes specified since this could + # get the b or c axes as well + to_transform = tuple((tp + for ax, tp in target_position.items() + if ax in Axis.gantry_axes())) + + # Pre-fill the dict we’ll send to the backend with the axes we don’t + # need to transform + smoothie_pos = {ax.name: pos for ax, pos in target_position.items() + if ax not in Axis.gantry_axes()} + + # We’d better have all of (x, y, (z or a)) or none of them since the + # gantry transform requires them all + if len(to_transform) != 3: + self._log.error("Move derived {} axes to transform from {}" + .format(len(to_transform), target_position)) + raise ValueError("Moves must specify either exactly an x, y, and " + "(z or a) or none of them") + + # Type ignored below because linal.apply_transform (rightly) specifies + # Tuple[float, float, float] and the implied type from + # target_position.items() is (rightly) Tuple[float, ...] with unbounded + # size; unfortunately, mypy can’t quite figure out the length check + # above that makes this OK + transformed = linal.apply_transform( # type: ignore + self.config.gantry_calibration, to_transform) + + # Since target_position is an OrderedDict with the axes ordered by + # (x, y, z, a, b, c), and we’ll only have one of a or z (as checked + # by the len(to_transform) check above) we can use an enumerate to + # fuse the specified axes and the transformed values back together + for idx, ax in enumerate(target_position.keys()): + if ax in Axis.gantry_axes(): + smoothie_pos[ax.name] = transformed[idx] try: - self._backend.move(target_position) + self._backend.move(smoothie_pos) except Exception: self._log.exception('Move failed') self._current_position.clear() raise + else: + self._current_position.update(target_position) # Gantry/frame (i.e. not pipette) config API + @property + def config(self) -> robot_configs.robot_config: + return self._config + + async def update_deck_calibration(self, new_transform): + pass + @_log_call async def head_speed(self, combined_speed=None, x=None, y=None, z=None, a=None, b=None, c=None): diff --git a/api/src/opentrons/hardware_control/controller.py b/api/src/opentrons/hardware_control/controller.py index c70a9927b5f..fb2edc3d0ee 100644 --- a/api/src/opentrons/hardware_control/controller.py +++ b/api/src/opentrons/hardware_control/controller.py @@ -2,11 +2,12 @@ import os import fcntl import threading -from typing import Dict, List, Optional, Tuple +from typing import Any, Dict, List, Optional, Tuple from opentrons.util import environment from opentrons.drivers.smoothie_drivers import driver_3_0 from opentrons.config import robot_configs from . import modules +from .types import Axis _lock = threading.Lock() @@ -74,10 +75,14 @@ def move(self, target_position: Dict[str, float], home_flagged_axes=True): self._smoothie_driver.move( target_position, home_flagged_axes=home_flagged_axes) - def home(self): - return self._smoothie_driver.home() + def home(self, axes: List[Axis] = None) -> Dict[str, float]: + if axes: + args: Tuple[Any, ...] = (''.join([ax.name for ax in axes]),) + else: + args = tuple() + return self._smoothie_driver.home(*args) - def get_attached_instruments(self, mount): + def get_attached_instruments(self, mount) -> Optional[str]: return self._smoothie_driver.read_pipette_model(mount.name.lower()) def get_attached_modules(self) -> List[Tuple[str, str]]: diff --git a/api/src/opentrons/hardware_control/simulator.py b/api/src/opentrons/hardware_control/simulator.py index 78e810f211b..27a851b4605 100644 --- a/api/src/opentrons/hardware_control/simulator.py +++ b/api/src/opentrons/hardware_control/simulator.py @@ -3,6 +3,7 @@ from opentrons import types from . import modules +from .types import Axis class Simulator: @@ -24,11 +25,11 @@ def __init__(self, def move(self, target_position: Dict[str, float]): pass - def home(self): + def home(self, axes: List[Axis] = None) -> Dict[str, float]: # driver_3_0-> HOMED_POSITION return {'X': 418, 'Y': 353, 'Z': 218, 'A': 218, 'B': 19, 'C': 19} - def get_attached_instruments(self, mount): + def get_attached_instruments(self, mount) -> Optional[str]: return self._attached_instruments[mount] def get_attached_modules(self) -> List[Tuple[str, str]]: diff --git a/api/src/opentrons/hardware_control/types.py b/api/src/opentrons/hardware_control/types.py new file mode 100644 index 00000000000..eb51825fdf1 --- /dev/null +++ b/api/src/opentrons/hardware_control/types.py @@ -0,0 +1,26 @@ +import enum +from typing import Tuple + +import opentrons.types + + +class Axis(enum.Enum): + X = enum.auto() + Y = enum.auto() + Z = enum.auto() + A = enum.auto() + B = enum.auto() + C = enum.auto() + + @classmethod + def by_mount(cls, mount: opentrons.types.Mount): + bm = {opentrons.types.Mount.LEFT: cls.Z, + opentrons.types.Mount.RIGHT: cls.A} + return bm[mount] + + @classmethod + def gantry_axes(cls) -> Tuple['Axis', 'Axis', 'Axis', 'Axis']: + """ The axes which are tied to the gantry and require the deck + calibration transform + """ + return (cls.X, cls.Y, cls.Z, cls.A) diff --git a/api/src/opentrons/legacy_api/robot/robot.py b/api/src/opentrons/legacy_api/robot/robot.py index f51120e3ba8..0888f971979 100755 --- a/api/src/opentrons/legacy_api/robot/robot.py +++ b/api/src/opentrons/legacy_api/robot/robot.py @@ -13,12 +13,11 @@ from opentrons.drivers.smoothie_drivers import driver_3_0 from opentrons.trackers import pose_tracker from opentrons.config import feature_flags as fflags - +from opentrons.config.robot_configs import load from opentrons.legacy_api import containers -from opentrons.legacy_api.robot.mover import Mover -from opentrons.legacy_api.robot.robot_configs import load from opentrons.legacy_api.containers import Container from opentrons.legacy_api.instruments import pipette_config +from .mover import Mover log = logging.getLogger(__name__) diff --git a/api/src/opentrons/deck_calibration/linal.py b/api/src/opentrons/util/linal.py similarity index 82% rename from api/src/opentrons/deck_calibration/linal.py rename to api/src/opentrons/util/linal.py index 4dd0133c403..84b3b6249c9 100644 --- a/api/src/opentrons/deck_calibration/linal.py +++ b/api/src/opentrons/util/linal.py @@ -1,7 +1,7 @@ import numpy as np from numpy import insert, dot from numpy.linalg import inv -from typing import List, Tuple +from typing import List, Tuple, Union def solve(expected: List[Tuple[float, float]], @@ -115,14 +115,28 @@ def add_z(xy: np.ndarray, z: float) -> np.ndarray: def apply_transform( - t: List[List[float]], - pos: Tuple[float, float, float]) -> Tuple[float, ...]: + t: Union[List[List[float]], np.ndarray], + pos: Tuple[float, float, float], + with_offsets=True) -> Tuple[float, float, float]: """ Change of base using a transform matrix. Primarily used to render a point in space in a way that is more readable for the user. :param t: A transformation matrix from one 3D space [A] to another [B] :param pos: XYZ point in space A - :return: corresponding XYZ1 point in space B + :param with_offsets: Whether to apply the transform as an affine transform + or as a standard transform. You might use + with_offsets=False + :return: corresponding XYZ point in space B """ - return tuple(dot(inv(t), list(pos) + [1])[:-1]) + extended = 1 if with_offsets else 0 + return tuple(dot(t, list(pos) + [extended])[:3]) # type: ignore + + +def apply_reverse( + t: Union[List[List[float]], np.ndarray], + pos: Tuple[float, float, float], + with_offsets=True) -> Tuple[float, float, float]: + """ Like apply_transform but inverts the transform first + """ + return apply_transform(inv(t), pos) diff --git a/api/tests/opentrons/cli/test_linal.py b/api/tests/opentrons/cli/test_linal.py index a3cbe55077e..a04c7d5e7e1 100755 --- a/api/tests/opentrons/cli/test_linal.py +++ b/api/tests/opentrons/cli/test_linal.py @@ -1,5 +1,6 @@ from math import pi, sin, cos -from opentrons.deck_calibration.linal import solve, add_z, apply_transform +from opentrons.util.linal import solve, add_z, apply_transform +from numpy.linalg import inv import numpy as np @@ -65,5 +66,5 @@ def test_apply_transform(): round(y - y_delta, 2), round(z - z_delta, 2)) - result = apply_transform(transform, (x, y, z)) + result = apply_transform(inv(transform), (x, y, z)) assert result == expected diff --git a/api/tests/opentrons/hardware_control/test_moves.py b/api/tests/opentrons/hardware_control/test_moves.py index 474e6daf1e7..03689108a3a 100644 --- a/api/tests/opentrons/hardware_control/test_moves.py +++ b/api/tests/opentrons/hardware_control/test_moves.py @@ -1,6 +1,7 @@ import pytest from opentrons import types from opentrons import hardware_control as hc +from opentrons.hardware_control.types import Axis @pytest.fixture @@ -15,8 +16,30 @@ def mock_move(position): async def test_controller_home(loop): c = hc.API.build_hardware_simulator(loop=loop) await c.home() - assert c._current_position == {'X': 418, 'Y': 353, 'Z': 218, - 'A': 218, 'B': 19, 'C': 19} + assert c._current_position == {Axis.X: 418, + Axis.Y: 353, + Axis.Z: 218, + Axis.A: 218, + Axis.B: 19, + Axis.C: 19} + c._config = c._config._replace(gantry_calibration=[[1, 0, 0, 10], + [0, 1, 0, 20], + [0, 0, 1, 30], + [0, 0, 0, 1]], + mount_offset=[0, 0, 10]) + assert c.config.gantry_calibration == [[1, 0, 0, 10], + [0, 1, 0, 20], + [0, 0, 1, 30], + [0, 0, 0, 1]] + await c.home() + # Check that we correctly apply the inverse gantry calibration and + # mount offset + assert c._current_position == {Axis.X: 408, + Axis.Y: 333, + Axis.Z: 198, + Axis.A: 188, + Axis.B: 19, + Axis.C: 19} async def test_controller_musthome(hardware_api): @@ -29,13 +52,68 @@ async def test_controller_musthome(hardware_api): async def test_move(hardware_api): abs_position = types.Point(30, 20, 10) mount = types.Mount.RIGHT - target_position1 = {'X': 30, 'Y': 20, 'Z': 218, 'A': 10, 'B': 19, 'C': 19} + target_position1 = {Axis.X: 30, + Axis.Y: 20, + Axis.Z: 218, + Axis.A: 10, + Axis.B: 19, + Axis.C: 19} await hardware_api.home() await hardware_api.move_to(mount, abs_position) assert hardware_api._current_position == target_position1 + # This assert implicitly checks that the mount offset is not applied to + # relative moves; if you change this to move_to, the offset will be + # applied again rel_position = types.Point(30, 20, 10) mount2 = types.Mount.LEFT - target_position2 = {'X': 60, 'Y': 40, 'Z': 228, 'A': 10, 'B': 19, 'C': 19} + target_position2 = {Axis.X: 60, + Axis.Y: 40, + Axis.Z: 228, + Axis.A: 10, + Axis.B: 19, + Axis.C: 19} await hardware_api.move_rel(mount2, rel_position) assert hardware_api._current_position == target_position2 + + +async def test_mount_offset_applied(hardware_api): + await hardware_api.home() + abs_position = types.Point(30, 20, 10) + mount = types.Mount.LEFT + target_position = {Axis.X: 64, + Axis.Y: 20, + Axis.Z: 10, + Axis.A: 218, + Axis.B: 19, + Axis.C: 19} + await hardware_api.move_to(mount, abs_position) + assert hardware_api._current_position == target_position + + +async def test_deck_cal_applied(monkeypatch, loop): + new_gantry_cal = [[1, 0, 0, 10], + [0, 1, 0, 20], + [0, 0, 1, 30], + [0, 0, 0, 1]] + called_with = None + + def mock_move(position): + nonlocal called_with + called_with = position + + hardware_api = hc.API.build_hardware_simulator(loop=loop) + monkeypatch.setattr(hardware_api._backend, 'move', mock_move) + old_config = hardware_api.config + hardware_api._config = old_config._replace( + gantry_calibration=new_gantry_cal) + await hardware_api.home() + await hardware_api.move_to(types.Mount.RIGHT, types.Point(0, 0, 0)) + assert called_with['X'] == 10 + assert called_with['Y'] == 20 + assert called_with['A'] == 30 + # Check that mount offset is also applied + await hardware_api.move_to(types.Mount.LEFT, types.Point(0, 0, 0)) + assert called_with['X'] == 44 + assert called_with['Y'] == 20 + assert called_with['Z'] == 30 From e03d77df9e21797dd11119a50bf68cd238ce75a7 Mon Sep 17 00:00:00 2001 From: Seth Foster Date: Thu, 18 Oct 2018 11:51:12 -0400 Subject: [PATCH 3/3] fixup: Do not apply mount offset in reverse transform --- api/src/opentrons/hardware_control/__init__.py | 17 ++++++----------- .../opentrons/hardware_control/test_moves.py | 12 +++++++++--- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/api/src/opentrons/hardware_control/__init__.py b/api/src/opentrons/hardware_control/__init__.py index 4f8cba18d70..ca3beb979d6 100644 --- a/api/src/opentrons/hardware_control/__init__.py +++ b/api/src/opentrons/hardware_control/__init__.py @@ -210,21 +210,16 @@ def _deck_from_smoothie( added to the smoothie coordinates here. """ with_enum = {Axis[k]: v for k, v in smoothie_pos.items()} - other = {k: v for k, v in with_enum.items() - if k not in Axis.gantry_axes()} + plunger_axes = {k: v for k, v in with_enum.items() + if k not in Axis.gantry_axes()} right = (with_enum[Axis.X], with_enum[Axis.Y], with_enum[Axis.by_mount(top_types.Mount.RIGHT)]) # Tell apply_transform to just do the change of base part of the # transform rather than the full affine transform, because this is # an offset - offset_in_smoothie = linal.apply_transform( - self.config.gantry_calibration, - self.config.mount_offset, - False) - left = (with_enum[Axis.X] + offset_in_smoothie[0], - with_enum[Axis.Y] + offset_in_smoothie[1], - with_enum[Axis.by_mount(top_types.Mount.LEFT)] - + offset_in_smoothie[2]) + left = (with_enum[Axis.X], + with_enum[Axis.Y], + with_enum[Axis.by_mount(top_types.Mount.LEFT)]) right_deck = linal.apply_reverse(self.config.gantry_calibration, right) left_deck = linal.apply_reverse(self.config.gantry_calibration, @@ -233,7 +228,7 @@ def _deck_from_smoothie( Axis.Y: right_deck[1], Axis.by_mount(top_types.Mount.RIGHT): right_deck[2], Axis.by_mount(top_types.Mount.LEFT): left_deck[2]} - deck_pos.update(other) + deck_pos.update(plunger_axes) return deck_pos def current_position(self, mount: top_types.Mount) -> Dict[Axis, float]: diff --git a/api/tests/opentrons/hardware_control/test_moves.py b/api/tests/opentrons/hardware_control/test_moves.py index 03689108a3a..b98751ae2f5 100644 --- a/api/tests/opentrons/hardware_control/test_moves.py +++ b/api/tests/opentrons/hardware_control/test_moves.py @@ -32,14 +32,20 @@ async def test_controller_home(loop): [0, 0, 1, 30], [0, 0, 0, 1]] await c.home() - # Check that we correctly apply the inverse gantry calibration and - # mount offset + # Check that we correctly apply the inverse gantry calibration assert c._current_position == {Axis.X: 408, Axis.Y: 333, - Axis.Z: 198, + Axis.Z: 188, Axis.A: 188, Axis.B: 19, Axis.C: 19} + # Check that we subsequently apply mount offset + assert c.current_position(types.Mount.RIGHT) == {Axis.X: 408, + Axis.Y: 333, + Axis.A: 188} + assert c.current_position(types.Mount.LEFT) == {Axis.X: 408, + Axis.Y: 333, + Axis.Z: 198} async def test_controller_musthome(hardware_api):