コード例 #1
0
def pose_tracker_change_base(state, point=Point(0, 0, 0), src=ROOT, dst=ROOT):
    """
    Transforms point from source coordinate system to destination.
    Point(0, 0, 0) means the origin of the source.
    """
    def fold(objects):
        return functools.reduce(lambda a, b: a.dot(b),
                                [state[key].transform for key in objects],
                                np.identity(4))

    up, down = ascend(state, src), list(reversed(ascend(state, dst)))

    # Find common prefix. Last item is common root
    root = [n1 for n1, n2 in zip(reversed(up), down) if n1 is n2].pop()

    # Nodes up to root, EXCLUDING root
    up = list(itertools.takewhile(lambda node: node is not root, up))

    # Nodes down from root, EXCLUDING root
    down = list(itertools.dropwhile(lambda node: node is not root, down))[1:]

    # Point in root's coordinate system
    if src is ROOT:
        # folded = fold(up)  # will be identity matrix; thus so will inverse
        inv_folded = np.identity(4)
    else:
        inv_folded = inv4x4(fold(up))
    point_in_root = inv_folded.dot((*point, 1))

    # Return point in destination's coordinate system
    return fold(down).dot(point_in_root)[:-1]
コード例 #2
0
def calibrate_container_with_delta(pose_tree,
                                   container,
                                   delta_x,
                                   delta_y,
                                   delta_z,
                                   save,
                                   new_container_name=None):

    delta = Point(delta_x, delta_y, delta_z)

    new_coordinates = change_base(
        pose_tree, src=container, dst=container.parent) + delta

    pose_tree = update(pose_tree, container, new_coordinates)

    if ff.split_labware_definitions():
        for well in container.wells():
            well._coordinates = well._coordinates + delta
    else:
        container._coordinates = container._coordinates + delta

    if save and new_container_name:
        database.save_new_container(container, new_container_name)
    elif save:
        database.overwrite_container(container)
    return pose_tree
コード例 #3
0
def mover_move(self,
               pose_tree,
               x=None,
               y=None,
               z=None,
               home_flagged_axes=True):
    """
    Dispatch move command to the driver changing base of
    x, y and z from source coordinate system to destination.

    Value must be set for each axis that is mapped.

    home_flagged_axes: (default=True)
        This kwarg is passed to the driver. This ensures that any axes
        within this Mover's axis_mapping is homed before moving, if it has
        not yet done so. See driver docstring for details
    """
    def defaults(_x, _y, _z):
        _x = _x if x is not None else 0
        _y = _y if y is not None else 0
        _z = _z if z is not None else 0
        return _x, _y, _z

    dst_x, dst_y, dst_z = pose_tracker_change_base(
        pose_tree,
        src=self._src,
        dst=self._dst,
        point=Point(*defaults(x, y, z)))
    driver_target = {}

    if 'x' in self._axis_mapping:
        assert x is not None, "Value must be set for each axis mapped"
        driver_target[self._axis_mapping['x']] = dst_x

    if 'y' in self._axis_mapping:
        assert y is not None, "Value must be set for each axis mapped"
        driver_target[self._axis_mapping['y']] = dst_y

    if 'z' in self._axis_mapping:
        assert z is not None, "Value must be set for each axis mapped"
        driver_target[self._axis_mapping['z']] = dst_z
    self._driver.move(driver_target, home_flagged_axes=home_flagged_axes)

    # Update pose with the new value. Since stepper motors are open loop
    # there is no need to to query diver for position
    return pose_tracker_update(pose_tree, self,
                               Point(*defaults(dst_x, dst_y, dst_z)))
コード例 #4
0
ファイル: mover.py プロジェクト: wheresaddie/opentrons
 def update_pose_from_driver(self, pose_tree):
     # map from driver axis names to xyz and expand position
     # into point object
     point = Point(
         x=self._driver.position.get(self._axis_mapping.get('x', ''), 0.0),
         y=self._driver.position.get(self._axis_mapping.get('y', ''), 0.0),
         z=self._driver.position.get(self._axis_mapping.get('z', ''), 0.0)
     )
     log.debug(f'Point in update pose from driver {point}')
     return update(pose_tree, self, point)
コード例 #5
0
def state():
    return init() \
        .add('1', point=Point(1, 2, 3)) \
        .add('2', point=Point(-1, -2, -3)) \
        .add('1-1', parent='1', point=Point(11, 12, 13)) \
        .add('1-1-1', parent='1-1', point=Point(0, 0, 0)) \
        .add('1-2', parent='1', point=Point(21, 22, 23)) \
        .add('2-1', parent='2', point=Point(-11, -12, -13)) \
        .add('2-2', parent='2', point=Point(-21, -22, -23))
コード例 #6
0
def test_relative(state):
    from math import pi
    assert (change_base(state, src='1-1', dst='2-1') == (24., 28., 32.)).all()
    state = \
        init() \
        .add('1', transform=rotate(pi / 2.0)) \
        .add('1-1', parent='1', point=Point(1, 0, 0)) \
        .add('2', point=Point(1, 0, 0))

    assert isclose(change_base(state, src='1-1', dst='2'),
                   (-1.0, -1.0, 0)).all()
    assert isclose(change_base(state, src='2', dst='1-1'), (0.0, 0.0, 0)).all()

    state = init() \
        .add('1', transform=scale(2, 1, 1)) \
        .add('1-1', parent='1', point=Point(1, 1, 1)) \

    assert isclose(change_base(state, src=ROOT, dst='1-1'),
                   (-2.0, -1.0, -1.0)).all()
    assert isclose(change_base(state, src='1-1', dst=ROOT),
                   (0.5, 1.0, 1.0)).all()
コード例 #7
0
def mover_update_pose_from_driver(self, pose_tree):
    from opentrons.legacy_api.robot.mover import log
    # map from driver axis names to xyz and expand position
    # into point object
    point = Point(x=self._driver.position.get(self._axis_mapping.get('x', ''),
                                              0.0),
                  y=self._driver.position.get(self._axis_mapping.get('y', ''),
                                              0.0),
                  z=self._driver.position.get(self._axis_mapping.get('z', ''),
                                              0.0))
    log.debug(f'Point in update pose from driver {point}')
    return pose_tracker_update(pose_tree, self, point)
コード例 #8
0
def test_add():
    state = init()
    assert state == {
        ROOT: Node(parent=None,
                   children=[],
                   transform=translate(Point(0, 0, 0)))
    }
    state = add(state, obj='child', point=Point(1, 2, 3))
    assert state == {
        ROOT:
        Node(parent=None,
             children=['child'],
             transform=translate(Point(0, 0, 0))),
        'child':
        Node(parent=ROOT, children=[], transform=translate(Point(-1, -2, -3)))
    }

    with pytest.raises(AssertionError):
        add(state, obj='child', point=Point(1, 2, 3))

    with pytest.raises(KeyError):
        add(state,
            obj='new-child',
            parent='another-root',
            point=Point(1, 2, 3))
コード例 #9
0
def update_instrument_config(instrument,
                             measured_center) -> Tuple[Point, float]:
    """
    Update config and pose tree with instrument's x and y offsets
    and tip length based on delta between probe center and measured_center,
    persist updated config and return it
    """
    from copy import deepcopy
    from opentrons.trackers.pose_tracker import update

    robot = instrument.robot
    config = robot.config
    instrument_offset = deepcopy(config.instrument_offset)

    dx, dy, dz = array(measured_center) - config.tip_probe.center

    log.debug("This is measured probe center dx {}".format(Point(dx, dy, dz)))
    # any Z offset will adjust the tip length, so instruments have Z=0 offset
    old_x, old_y, _ = instrument_offset[instrument.mount][instrument.type]
    instrument_offset[instrument.mount][instrument.type] = \
        (old_x - dx, old_y - dy, 0.0)
    tip_length = deepcopy(config.tip_length)
    tip_length[instrument.model] = tip_length[instrument.model] + dz

    config = config \
        ._replace(instrument_offset=instrument_offset) \
        ._replace(tip_length=tip_length)
    robot.config = config
    log.debug("Updating config for {} instrument".format(instrument.mount))
    robot_configs.save_robot_settings(config)

    new_coordinates = change_base(
        robot.poses, src=instrument, dst=instrument.instrument_mover) - Point(
            dx, dy, 0.0)
    robot.poses = update(robot.poses, instrument, new_coordinates)

    return robot.config
コード例 #10
0
ファイル: mover.py プロジェクト: wheresaddie/opentrons
    def axis_maximum(self, pose_tree, axis):
        assert axis in 'xyz', "axis value should be x, y or z"
        assert axis in self._axis_mapping, "mapping is not set for " + axis

        d_axis_max = self._driver.homed_position[self._axis_mapping[axis]]
        d_point = {'x': 0, 'y': 0, 'z': 0}
        d_point[axis] = d_axis_max
        x, y, z = change_base(
            pose_tree,
            src=self._dst,
            dst=self._src,
            point=Point(**d_point))
        point = {'x': x, 'y': y, 'z': z}
        self._axis_maximum[axis] = point[axis]
        return self._axis_maximum[axis]
コード例 #11
0
def _calculate_safeheight(robot, z_crossover_clearance):
    center = Point(*robot.config.tip_probe.center)

    return center.z + z_crossover_clearance
コード例 #12
0
def probe_instrument(instrument, robot, tip_length=None) -> Point:
    robot.home()
    tp = robot.config.tip_probe
    if tip_length is None:
        tip_length = robot.config.tip_length[instrument.model]
    instrument._add_tip(tip_length)

    # probe_center is the point at the center of the switch pcb
    center = Point(*tp.center)

    hot_spots = robot_configs.calculate_tip_probe_hotspots(tip_length, tp)

    # The saved axis positions from limit switch response
    axis_pos = []

    safe_height = _calculate_safeheight(robot, tp.z_clearance.crossover)

    log.info("Moving to safe z: {}".format(safe_height))
    robot.poses = instrument._move(robot.poses, z=safe_height)

    for hs in hot_spots:
        x0 = tp.center[0] + hs.x_start_offs
        y0 = tp.center[1] + hs.y_start_offs
        z0 = hs.z_start_abs

        log.info("Moving to {}".format((x0, y0, z0)))
        robot.poses = instrument._move(robot.poses, x=x0, y=y0)
        robot.poses = instrument._move(robot.poses, z=z0)

        axis_index = 'xyz'.index(hs.axis)
        robot.poses = instrument._probe(robot.poses, hs.axis,
                                        hs.probe_distance)

        # Tip position is stored in accumulator and averaged for each axis
        # to be used for more accurate positioning for the next axis
        value = absolute(robot.poses, instrument)[axis_index]
        axis_pos.append(value)

        # after probing two points along the same axis
        # average them out, update center and clear accumulator
        # except Z, we're only probing that once
        if hs.axis == 'z':
            center = center._replace(**{hs.axis: axis_pos[0]})
            axis_pos.clear()
        elif len(axis_pos) == 2:
            center = center._replace(
                **{hs.axis: (axis_pos[0] + axis_pos[1]) / 2.0})

            axis_pos.clear()

        log.debug("Current axis positions for {}: {}".format(
            hs.axis, axis_pos))

        # Bounce back to release end stop
        sgn = hs.probe_distance / abs(hs.probe_distance)
        bounce = value + (tp.bounce_distance * -sgn)

        robot.poses = instrument._move(robot.poses, **{hs.axis: bounce})
        robot.poses = instrument._move(robot.poses, z=safe_height)

        log.debug("Updated center point tip probe {}".format(center))

    instrument._remove_tip(tip_length)

    return center
コード例 #13
0
def test_change_base():
    state = init() \
        .add('1', parent=ROOT, transform=scale(2, 2, 2)) \
        .add('1-1', parent='1', point=Point(1, 0, 0))

    assert isclose(change_base(state, src='1-1'), (0.5, 0, 0)).all()
コード例 #14
0
def test_update(state):
    state = update(state, '1-1', Point(0, 0, 0))
    assert (change_base(state, src='1-1-1') == (1, 2, 3)).all()
コード例 #15
0
ファイル: test_pose.py プロジェクト: wheresaddie/opentrons
import pytest

from opentrons.trackers.pose_tracker import Point, translate


@pytest.mark.parametrize("pose1, pose2, product", [
    (Point(0, 0, 0), Point(-1, -10, 10), Point(-1, -10, 10)),
    (Point(-1, -10, 10), Point(0, 0, 0), Point(-1, -10, 10)),
    (Point(1, 2, 3), Point(-1, -10, 10), Point(0, -8, 13)),
    (Point(90, 5, 20), Point(20, -100, 0), Point(110, -95, 20))
])
def test_mult(pose1, pose2, product):
    result = translate(pose1).dot(translate(pose2))
    assert (result == translate(product)).all()
コード例 #16
0
def probe_instrument(instrument, robot, tip_length=None) -> Point:
    robot.home()

    if tip_length is None:
        tip_length = robot.config.tip_length[instrument.name]
    instrument._add_tip(tip_length)

    # probe_center is the point at the center of the switch pcb
    center = Point(*robot.config.probe_center)

    hot_spots = _calculate_hotspots(robot, tip_length, SWITCH_CLEARANCE,
                                    X_SWITCH_OFFSET_MM, Y_SWITCH_OFFSET_MM,
                                    Z_SWITCH_OFFSET_MM, Z_DECK_CLEARANCE,
                                    Z_PROBE_CLEARANCE, Z_PROBE_START_CLEARANCE)

    # The saved axis positions from limit switch response
    axis_pos = []

    safe_height = _calculate_safeheight(robot, Z_CROSSOVER_CLEARANCE)

    log.info("Moving to safe z: {}".format(safe_height))
    robot.poses = instrument._move(robot.poses, z=safe_height)

    for axis, x, y, z, distance in hot_spots:
        if axis == 'z':

            x = x + center.x
            y = y + center.y
            z = z + center.z
        else:
            x = x + center.x
            y = y + center.y

        log.info("Moving to {}".format((x, y, z)))
        robot.poses = instrument._move(robot.poses, x=x, y=y)
        robot.poses = instrument._move(robot.poses, z=z)

        axis_index = 'xyz'.index(axis)
        robot.poses = instrument._probe(robot.poses, axis, distance)

        # Tip position is stored in accumulator and averaged for each axis
        # to be used for more accurate positioning for the next axis
        value = absolute(robot.poses, instrument)[axis_index]
        axis_pos.append(value)

        # after probing two points along the same axis
        # average them out, update center and clear accumulator
        # except Z, we're only probing that once
        if axis == 'z':
            center = center._replace(**{axis: axis_pos[0]})
            axis_pos.clear()
        elif len(axis_pos) == 2:
            center = center._replace(
                **{axis: (axis_pos[0] + axis_pos[1]) / 2.0})

            axis_pos.clear()

        log.debug("Current axis positions for {}: {}".format(axis, axis_pos))

        # Bounce back to release end stop
        bounce = value + (BOUNCE_DISTANCE_MM * (-distance / abs(distance)))

        robot.poses = instrument._move(robot.poses, **{axis: bounce})
        robot.poses = instrument._move(robot.poses, z=safe_height)

        log.debug("Updated center point tip probe {}".format(center))

    instrument._remove_tip(tip_length)

    return center