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
