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]
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
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)))
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)
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))
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()
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)
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))
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
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]
def _calculate_safeheight(robot, z_crossover_clearance): center = Point(*robot.config.tip_probe.center) return center.z + z_crossover_clearance
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
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()
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()
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()
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