diff --git a/pylabrobot/resources/__init__.py b/pylabrobot/resources/__init__.py index ca92f0edc5b..3da3d237ee1 100644 --- a/pylabrobot/resources/__init__.py +++ b/pylabrobot/resources/__init__.py @@ -23,6 +23,7 @@ from .corning import * from .deck import Deck from .diy import * +from .end_effector import MechanicalGripper from .eppendorf import * from .errors import ResourceNotFoundError from .greiner import * @@ -30,6 +31,7 @@ from .itemized_resource import ItemizedResource from .lid import Lid, Liddable from .liquid import Liquid +from .manipulator import Link from .nest import * from .opentrons import * from .perkin_elmer import * diff --git a/pylabrobot/resources/end_effector.py b/pylabrobot/resources/end_effector.py new file mode 100644 index 00000000000..c758333d537 --- /dev/null +++ b/pylabrobot/resources/end_effector.py @@ -0,0 +1,112 @@ +"""End-effectors: what is fitted at an arm's mechanical interface, and the parts they are made of. + +An end-effector - equally a tool, or end-of-arm tooling - is what an arm carries at its wrist +flange so that it can do its task. Its tool centre point is the point a move is programmed +against, stated as an offset from that flange, and it belongs to the tool rather than to the arm: +fit a different one and the point moves with it. + +`MechanicalGripper` spans that offset, flange to grip centre, which is why it is a `Link`. +""" + +from typing import Optional, Sequence, Tuple, cast + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.manipulator import Link +from pylabrobot.resources.resource import Resource + + +class MechanicalGripper(Link): + """A gripper that holds by closing two fingers on what it takes. + + A link: it spans the joint it turns on to the point it grips at, which is `tool_center_point`. + Its body, its two fingers and a pad on each are material bolted to that span; a gripper whose + fingers meet the resource themselves carries no pads. How far apart the fingers stand is state + rather than shape, so `jaw_width` moves them. + """ + + def __init__( + self, + name: str, + length: float, + body: Resource, + body_location: Coordinate, + fingers: Sequence[Resource], + finger_location: Coordinate, + jaw_range: Tuple[float, float], + pads: Optional[Sequence[Resource]] = None, + pad_location: Optional[Coordinate] = None, + jaw_width: Optional[float] = None, + category: str = "mechanical_gripper", + model: Optional[str] = None, + ): + """ + Args: + name: what to call this one. + length: the joint it turns on to the grip centre, in mm. + body: the material around the span. + body_location: where it sits, from the joint this gripper turns on. + fingers: the two jaws, either side of the span. + finger_location: where a finger sits along and above the span. Its Y is `jaw_width`'s. + jaw_range: how far apart the fingers stand, closed and open, in mm. + pads: what each finger meets the resource with, in the same order as `fingers`. A gripper + whose fingers meet it themselves has none. + pad_location: where a pad sits, from the finger it is fixed to. Given with `pads`. + jaw_width: how far apart they stand to begin with, in mm. Where a gripper is known to come + up at a particular width - the one it homes at, say - that is what to build it at, so the + model does not start out claiming a width nothing has read. Open, when not given. + """ + super().__init__(name=name, length=length, category=category, model=model) + if len(fingers) != 2: + raise ValueError(f"a gripper has two fingers, not {len(fingers)}") + if (pads is None) != (pad_location is None): + raise ValueError("pads and pad_location go together: give both, or neither") + # Zipping a short list against a long one would drop material without saying so. + if pads is not None and len(pads) != len(fingers): + raise ValueError(f"a gripper has a pad on each finger, not {len(pads)} on {len(fingers)}") + self.jaw_range = jaw_range + + self.body = body + self.assign_child_resource(body, location=body_location) + self.fingers = list(fingers) + for jaw in self.fingers: + self.assign_child_resource(jaw, location=finger_location) + + self.pads = list(pads) if pads is not None else [] + for jaw, face in zip(self.fingers, self.pads): + jaw.assign_child_resource(face, location=cast(Coordinate, pad_location)) + + # Through the setter, which is where a width is checked and the fingers are stood apart. + self.jaw_width = jaw_range[1] if jaw_width is None else jaw_width + + @property + def tool_center_point(self) -> Coordinate: + """Where this tool is programmed against, as an offset from where it is mounted. + + Returns: + The grip centre, which the fingers reach past. + """ + return Coordinate(self.get_size_x(), 0.0, 0.0) + + @property + def jaw_width(self) -> float: + """How far apart the fingers stand, in mm.""" + return self._jaw_width + + @jaw_width.setter + def jaw_width(self, width: float) -> None: + low, high = self.jaw_range + if not low <= width <= high: + raise ValueError(f"the jaws open {low} to {high} mm, not {width}") + self._jaw_width = width + self._place_the_fingers() + + def _place_the_fingers(self) -> None: + """Stand the fingers either side of the span, as far apart as the jaws are open.""" + for finger, side in zip(self.fingers, (1.0, -1.0)): + here = cast(Coordinate, finger.location) + finger.location = Coordinate( + here.x, side * self._jaw_width / 2.0 - finger.get_size_y() / 2.0, here.z + ) + + def serialize(self) -> dict: + return {**super().serialize(), "jaw_range": list(self.jaw_range)} diff --git a/pylabrobot/resources/end_effector_tests.py b/pylabrobot/resources/end_effector_tests.py new file mode 100644 index 00000000000..503095fa555 --- /dev/null +++ b/pylabrobot/resources/end_effector_tests.py @@ -0,0 +1,101 @@ +import unittest +from typing import cast + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.end_effector import MechanicalGripper +from pylabrobot.resources.resource import Resource + +# Measured off a Hamilton iSWAP. +LENGTH = 137.7 +BODY_LOCATION = Coordinate(-13.0, -45.0, -1.3) +FINGER_LOCATION = Coordinate(6.5, 0.0, 4.0) +PAD_LOCATION = Coordinate(109.0, 1.5, -17.0) +JAW_RANGE = (70.844, 133.706) + + +def gripper(**overrides) -> MechanicalGripper: + body = Resource(name="demo_body", size_x=59.0, size_y=90.0, size_z=20.3, category="body") + + fingers = [ + Resource( + name=f"demo_finger_{side}", + size_x=135.0, + size_y=7.0, + size_z=8.0, + category="finger", + ) + for side in ("left", "right") + ] + pads = [ + Resource(name=f"demo_finger_{side}_pad", size_x=37.0, size_y=4.0, size_z=17.0, category="pad") + for side in ("left", "right") + ] + + arguments = dict( + name="demo_gripper", + length=LENGTH, + body=body, + body_location=BODY_LOCATION, + fingers=fingers, + finger_location=FINGER_LOCATION, + jaw_range=JAW_RANGE, + pads=pads, + pad_location=PAD_LOCATION, + ) + return MechanicalGripper(**{**arguments, **overrides}) + + +class TestTheSpan(unittest.TestCase): + def test_the_grip_centre_sits_at_the_end_of_the_span(self): + g = gripper() + self.assertEqual(g.tool_center_point, Coordinate(LENGTH, 0.0, 0.0)) + + +class TestJaws(unittest.TestCase): + def test_a_width_stands_the_fingers_that_far_apart(self): + g = gripper() + for width in (133.706, 100.0, 70.844): + g.jaw_width = width + left, right = g.fingers + centres = [ + cast(Coordinate, finger.location).y + finger.get_size_y() / 2 for finger in (left, right) + ] + self.assertAlmostEqual(centres[0] - centres[1], width) + self.assertAlmostEqual(centres[0] + centres[1], 0.0) + + def test_the_jaws_refuse_a_width_they_do_not_reach(self): + with self.assertRaises(ValueError): + gripper(jaw_width=200.0) + g = gripper() + before = g.jaw_width + with self.assertRaises(ValueError): + g.jaw_width = 5.0 + self.assertEqual(g.jaw_width, before) + + def test_a_gripper_starts_open_unless_told_otherwise(self): + self.assertEqual(gripper().jaw_width, JAW_RANGE[1]) + self.assertEqual(gripper(jaw_width=100.0).jaw_width, 100.0) + + +class TestPads(unittest.TestCase): + def test_a_gripper_can_have_bare_fingers(self): + g = gripper(pads=None, pad_location=None) + self.assertEqual(g.pads, []) + self.assertEqual([jaw.children for jaw in g.fingers], [[], []]) + + def test_pads_and_their_location_go_together(self): + with self.assertRaises(ValueError): + gripper(pad_location=None) + with self.assertRaises(ValueError): + gripper(pads=None) + + def test_a_pad_sits_inside_its_finger(self): + g = gripper() + for jaw, face in zip(g.fingers, g.pads): + sits_at = cast(Coordinate, face.location).y + self.assertGreaterEqual(sits_at, 0.0) + self.assertLessEqual(sits_at + face.get_size_y(), jaw.get_size_y()) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/resources/manipulator.py b/pylabrobot/resources/manipulator.py new file mode 100644 index 00000000000..a0b45f967e1 --- /dev/null +++ b/pylabrobot/resources/manipulator.py @@ -0,0 +1,59 @@ +"""The moving mechanism of an arm: the links its joints turn between. + +A manipulator is a chain of links and powered joints. A link is one rigid member of that chain and +nothing else: geometry is attached as children with their own origins - the separation a robot +description draws between a link's frame and its visual geometry - so material may extend past +either joint without entering the kinematics. +""" + +from typing import Optional + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.resource import Resource + + +class Link(Resource): + """One of the rigid pieces an arm is built from, joined to its neighbours by joints. + + What sets the reach is the distance between a link's joints, not the shape of the piece, so the + material is attached as children with their own origins and may overhang a joint at either end. + + `joint` is where the joint it turns on sits within the link, which `turn_to` pivots about. It + needs no particular place: a link is not obliged to put its own origin there. + + Unrotated it lies along +X. + """ + + def __init__( + self, + name: str, + length: float, + joint: Optional[Coordinate] = None, + category: str = "link", + model: Optional[str] = None, + ): + """ + Args: + name: what to call this one. + length: joint to joint, in mm. + joint: where the joint this link turns on sits within it. Its own origin when None. + category: what kind of resource this is. + model: which link this is. + """ + super().__init__( + name=name, size_x=length, size_y=0.0, size_z=0.0, category=category, model=model + ) + self.joint = joint if joint is not None else Coordinate.zero() + + def turn_to(self, angle: float) -> None: + """Point the link along `angle`, pivoting on `joint`. + + Args: + angle: the angle to point along, in its parent's frame, in degrees. + + Raises: + RuntimeError: If the link has not been placed, so there is nothing for it to turn in. + """ + if self.location is None: + raise RuntimeError(f"{self.name} is not placed, so there is nothing for it to turn in") + self.rotate_to(z=angle, reference=self.joint) diff --git a/pylabrobot/resources/manipulator_tests.py b/pylabrobot/resources/manipulator_tests.py new file mode 100644 index 00000000000..ec290f028ff --- /dev/null +++ b/pylabrobot/resources/manipulator_tests.py @@ -0,0 +1,54 @@ +import unittest + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.manipulator import Link +from pylabrobot.resources.resource import Resource +from pylabrobot.utils.linalg import matrix_vector_multiply_3x3 + + +class TestLink(unittest.TestCase): + def test_a_child_link_turns_on_top_of_its_parent(self): + base = Resource(name="base", size_x=500, size_y=500, size_z=0) + first = Link(name="first", length=100.0) + second = Link(name="second", length=50.0) + base.assign_child_resource(first, location=Coordinate(0, 0, 0)) + first.assign_child_resource(second, location=Coordinate(first.get_size_x(), 0, 0)) + + def far_end() -> Coordinate: + carried = matrix_vector_multiply_3x3( + second.get_absolute_rotation().get_rotation_matrix(), + Coordinate(second.get_size_x(), 0, 0).vector(), + ) + return second.get_absolute_location() + Coordinate(*carried) + + self.assertEqual(far_end(), Coordinate(150, 0, 0)) + + second.turn_to(90) + self.assertEqual(far_end(), Coordinate(100, 50, 0)) + + first.turn_to(90) + self.assertEqual(far_end(), Coordinate(-50, 100, 0)) + + def test_turning_to_an_angle_is_absolute(self): + base = Resource(name="base", size_x=500, size_y=500, size_z=0) + link = Link(name="link", length=100.0) + base.assign_child_resource(link, location=Coordinate(0, 0, 0)) + + link.turn_to(30) + link.turn_to(30) + self.assertEqual(link.rotation.z, 30) + + link.rotate(z=30) + self.assertEqual(link.rotation.z, 60) + + def test_an_unplaced_link_has_nothing_to_turn_in(self): + base = Resource(name="base", size_x=500, size_y=500, size_z=0) + link = Link(name="link", length=100.0) + base.assign_child_resource(link, location=Coordinate(0, 0, 0)) + + with self.assertRaises(RuntimeError): + Link(name="loose", length=100.0).turn_to(0) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/resources/resource.py b/pylabrobot/resources/resource.py index 4c603a9c65a..399f7c534bf 100644 --- a/pylabrobot/resources/resource.py +++ b/pylabrobot/resources/resource.py @@ -900,6 +900,46 @@ def rotate(self, x: float = 0, y: float = 0, z: float = 0): # Visualizer) so they can re-render. self._state_updated() + def rotate_to( + self, + x: Optional[float] = None, + y: Optional[float] = None, + z: Optional[float] = None, + reference: Optional[Coordinate] = None, + ): + """Rotate counter-clockwise to the given degrees, where `rotate` turns by them. + + Args: + x: degrees to point along about X. Left where it is when None. + y: degrees to point along about Y. Left where it is when None. + z: degrees to point along about Z. Left where it is when None. + reference: the point to turn about. This resource's own corner when None. + """ + pivot = reference if self.location is not None else None + before = self.get_absolute_rotation().get_rotation_matrix() if pivot is not None else None + + self.rotation.x = self.rotation.x if x is None else x % 360 + self.rotation.y = self.rotation.y if y is None else y % 360 + self.rotation.z = self.rotation.z if z is None else z % 360 + + if pivot is not None and before is not None: + after = self.get_absolute_rotation().get_rotation_matrix() + was = matrix_vector_multiply_3x3(before, pivot.vector()) + now = matrix_vector_multiply_3x3(after, pivot.vector()) + carried = Coordinate(was[0] - now[0], was[1] - now[1], was[2] - now[2]) + # `carried` is in this resource's frame, `location` in the parent's. + parent = self.parent + if parent is not None: + turned = parent.get_absolute_rotation().get_rotation_matrix() + carried = Coordinate( + *matrix_vector_multiply_3x3( + [[turned[j][i] for j in range(3)] for i in range(3)], carried.vector() + ) + ) + self.location = cast(Coordinate, self.location) + carried + + self._state_updated() + def copy(self) -> Self: resource_copy = self.__class__.deserialize(self.serialize(), allow_marshal=True) resource_copy.load_all_state(self.serialize_all_state()) diff --git a/pylabrobot/resources/resource_tests.py b/pylabrobot/resources/resource_tests.py index 0a46efc7fb4..77277072612 100644 --- a/pylabrobot/resources/resource_tests.py +++ b/pylabrobot/resources/resource_tests.py @@ -332,6 +332,98 @@ def test_rotation90(self): self.assertAlmostEqual(c.get_absolute_size_x(), 20) self.assertAlmostEqual(c.get_absolute_size_y(), 10) + def test_rotating_about_a_reference_point_leaves_that_point_where_it_was(self): + """A resource turns about its own left front bottom corner. `reference` names another point to + turn on - a hinge, a joint - and the resource is carried so that point does not move, which is + what a joint is. Checked on the point itself rather than on the resource's location, since the + location moving is the mechanism and the point standing still is the promise.""" + parent = Resource("parent", size_x=500, size_y=500, size_z=10) + parent.location = Coordinate.zero() + bar = Resource("bar", size_x=100, size_y=10, size_z=10) + parent.assign_child_resource(bar, location=Coordinate(0, 0, 0)) + far_end = Coordinate(100, 0, 0) + + before = bar.get_absolute_location() + far_end + bar.rotate_to(z=90, reference=far_end) + carried = matrix_vector_multiply_3x3( + bar.get_absolute_rotation().get_rotation_matrix(), far_end.vector() + ) + + self.assertEqual(bar.get_absolute_location() + Coordinate(*carried), before) + self.assertEqual(bar.location, Coordinate(100, -100, 0)) + + def test_rotate_to_goes_to_an_angle_where_rotate_moves_by_one(self): + parent = Resource("parent", size_x=500, size_y=500, size_z=10) + parent.location = Coordinate.zero() + bar = Resource("bar", size_x=100, size_y=10, size_z=10) + parent.assign_child_resource(bar, location=Coordinate.zero()) + + bar.rotate_to(z=30) + bar.rotate_to(z=30) + self.assertEqual(bar.rotation.z, 30) + + bar.rotate(z=30) + self.assertEqual(bar.rotation.z, 60) + + def test_rotate_to_sets_one_axis_normalized_and_leaves_the_others(self): + for axis in ("x", "y", "z"): + for start in ((0, 0, 15), (90, 0, 90), (15, 90, 200)): + parent = Resource("parent", size_x=500, size_y=500, size_z=10) + parent.location = Coordinate.zero() + bar = Resource("bar", size_x=100, size_y=10, size_z=10) + parent.assign_child_resource(bar, location=Coordinate.zero()) + bar.rotate(x=start[0], y=start[1], z=start[2]) + was = {name: getattr(bar.rotation, name) for name in ("x", "y", "z")} + + bar.rotate_to( + x=390.0 if axis == "x" else None, + y=390.0 if axis == "y" else None, + z=390.0 if axis == "z" else None, + ) + + for name in ("x", "y", "z"): + expected = 30.0 if name == axis else was[name] + self.assertAlmostEqual(getattr(bar.rotation, name), expected, places=9) + + def test_rotate_keeps_every_axis_normalized(self): + resource = Resource("resource", size_x=10, size_y=10, size_z=10) + resource.rotate(x=350, y=350, z=350) + resource.rotate(x=20, y=20, z=20) + for axis in (resource.rotation.x, resource.rotation.y, resource.rotation.z): + self.assertGreaterEqual(axis, 0) + self.assertLess(axis, 360) + + def test_a_pivot_inside_a_turned_parent_still_holds(self): + parent = Resource("parent", size_x=500, size_y=500, size_z=10) + parent.location = Coordinate.zero() + bar = Resource("bar", size_x=100, size_y=10, size_z=10) + parent.assign_child_resource(bar, location=Coordinate(30, 40, 0)) + parent.rotate(z=90) + far_end = Coordinate(100, 0, 0) + + def where() -> Coordinate: + carried = matrix_vector_multiply_3x3( + bar.get_absolute_rotation().get_rotation_matrix(), far_end.vector() + ) + return bar.get_absolute_location() + Coordinate(*carried) + + before = where() + bar.rotate_to(z=90, reference=far_end) + self.assertEqual(where(), before) + + def test_rotate_to_turns_about_a_reference_point(self): + parent = Resource("parent", size_x=500, size_y=500, size_z=10) + parent.location = Coordinate.zero() + bar = Resource("bar", size_x=100, size_y=10, size_z=10) + parent.assign_child_resource(bar, location=Coordinate.zero()) + far_end = Coordinate(100, 0, 0) + + bar.rotate_to(z=90, reference=far_end) + carried = matrix_vector_multiply_3x3( + bar.get_absolute_rotation().get_rotation_matrix(), far_end.vector() + ) + self.assertEqual(bar.get_absolute_location() + Coordinate(*carried), far_end) + def test_rotation180(self): r = Resource("parent", size_x=200, size_y=100, size_z=100) r.location = Coordinate.zero()