diff --git a/CHANGELOG.md b/CHANGELOG.md index 870ab59605e..499b9b93464 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/). - User guide notebook for the MicroSpin (`docs/user_guide/01_material-handling/centrifuge/highres_microspin.ipynb`). - `Plate`: optional `stacking_z_height` parameter -- the per-plate vertical pitch when plates are stacked directly on top of each other (`size_z` minus the nesting overlap), mirroring `NestedTipRack.stacking_z_height`. Because it is a physical dimension, plates that differ in it no longer compare equal; `Plate` also now serializes `stacking_z_height` and the pre-existing `plate_type` so both round-trip through `deserialize`/`copy`. (#1110) - `ResourceStack`: bare plates stacked in the z direction now nest into one another by their `stacking_z_height` (a stack of `N` identical plates is `size_z + (N - 1) * stacking_z_height` tall, for both `get_size_z()` and child placement). Plates without a `stacking_z_height`, and plates wearing a lid, do not nest, so existing behaviour is unchanged. (#1112) +- `Resource.rotate_to(x=, y=, z=)`: set the rotation about each axis, where `rotate` turns by an amount. Axes left as `None` keep the angle they had, and each is normalised to `[0, 360)`. (#1249) +- `Resource.rotate`, `rotate_to` and `rotated` take an optional `pivot_coordinate`: a point in the resource's own frame that stays where it is, so a resource can turn about its centre, an edge, or any other point rather than only about its origin. `location` carries by however far the turn moved that point. Raises `NoLocationError` when the resource has no location, since there is nothing to carry. (#1249) +- `LinkBody` (`pylabrobot.resources.LinkBody`): one rigid member of a manipulator, an ordinary resource whose origin is a corner and which carries its `proximal_joint` and `distal_joint` as coordinates within it. The link is the line between the two joints and `length` is the distance, `None` on a member that ends the chain. A member turns about its proximal joint rather than its origin. (#1249) +- `MechanicalGripper` (`pylabrobot.resources.MechanicalGripper`): a `LinkBody` that ends the chain, holding what it takes between two fingers. Its far end is a `tool_center_point` rather than a joint, it is sized to its body because `jaw_width` moves the fingers, and the jaws straddle the grip centre. (#1249) ### Fixed diff --git a/docs/api/pylabrobot.resources.rst b/docs/api/pylabrobot.resources.rst index ccd5d0a3ee0..6ba5916a850 100644 --- a/docs/api/pylabrobot.resources.rst +++ b/docs/api/pylabrobot.resources.rst @@ -18,7 +18,9 @@ Resources represent on-deck liquid handling equipment, including tip racks, plat ItemizedResource utils.create_equally_spaced_2d Lid + LinkBody Liquid + MechanicalGripper PetriDish Plate PlateCarrier diff --git a/pylabrobot/resources/__init__.py b/pylabrobot/resources/__init__.py index ca92f0edc5b..288f7e2a995 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 LinkBody 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..74ff74d0c88 --- /dev/null +++ b/pylabrobot/resources/end_effector.py @@ -0,0 +1,201 @@ +"""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 `LinkBody`. +""" + +import math +from typing import Any, Dict, Optional, Sequence, Tuple, cast + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.manipulator import LinkBody +from pylabrobot.resources.resource import Resource +from pylabrobot.resources.rotation import Rotation +from pylabrobot.serializer import deserialize + + +class MechanicalGripper(LinkBody): + """A gripper that holds by closing two fingers on what it takes. + + A member that ends the chain: nothing attaches past a tool, so it has no distal joint. What sits + at the far end of its span is `tool_center_point`, the point it grips at. Its body, its two + fingers and a pad on each are material bolted to it. + + The gap between the fingers is state rather than shape, so `jaw_width` moves them. That is why + the member is sized to its body alone: a box drawn around the fingers would change size every + time the jaws did. The fingers reach past it. + """ + + def __init__( + self, + name: str, + proximal_joint: Coordinate, + tool_center_point: Coordinate, + 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. + proximal_joint: where the joint this gripper turns on sits within it. + tool_center_point: the point it grips at, from this gripper's own origin. + body: the material around the span, which is also what sizes this member. + body_location: where it sits, from this gripper's own origin. + fingers: the two jaws, either side of the span. + finger_location: where a finger sits along and above the span. Its Y is not used: the + jaws straddle `tool_center_point`, and `jaw_width` sets how far apart. + jaw_range: the gap between the fingers, 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: the gap to begin with, in mm. Open, when not given. + """ + super().__init__( + name=name, + # A tool is sized to its body, since the fingers move and a box around them would resize + # with the jaws. The body states that box, so it is not asked for a second time. + size_x=body.get_size_x(), + size_y=body.get_size_y(), + size_z=body.get_size_z(), + proximal_joint=proximal_joint, + distal_joint=None, + 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._tool_center_point = tool_center_point + + 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 self._tool_center_point + + @property + def length(self) -> float: + """The joint this gripper turns on to the point it grips at, in mm.""" + return math.dist(self._tool_center_point.vector(), self.proximal_joint.vector()) + + @property + def jaw_width(self) -> float: + """The gap between the fingers' facing surfaces, in mm: what fits between them.""" + 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 grip centre, leaving `jaw_width` of gap between them.""" + for finger, side in zip(self.fingers, (1.0, -1.0)): + here = cast(Coordinate, finger.location) + # A resource sits at its lowest-y corner: the facing surface on the +Y side, the back of the + # finger on the -Y side. They close on what is at the grip centre, so they straddle the tool + # centre point rather than the joint or the member's own middle. + facing = self._tool_center_point.y + side * self._jaw_width / 2.0 + finger.location = Coordinate( + here.x, facing if side > 0 else facing - finger.get_size_y(), here.z + ) + + def serialize(self) -> dict: + serialized = super().serialize() + # Nothing attaches past a tool, so the key its base emits has nothing to say and + # `__init__` has nowhere to put it. + serialized.pop("distal_joint", None) + return { + **serialized, + "jaw_range": list(self.jaw_range), + "tool_center_point": self.tool_center_point.serialize(), + } + + @classmethod + def deserialize(cls, data: dict, allow_marshal: bool = False) -> "MechanicalGripper": + """Rebuild a gripper, taking its own parts back out of its children. + + Its body and its two fingers are constructor arguments rather than children assigned after the + fact, so they are read off the front of `children`, in the order `__init__` put them there. + Anything after them is what the gripper was holding. + """ + children = data["children"] + body, *fingers = ( + Resource.deserialize(child, allow_marshal=allow_marshal) for child in children[:3] + ) + pads = [pad for finger in fingers for pad in list(finger.children)] + for pad in pads: + pad.unassign() + + def where(child: dict) -> Coordinate: + return cast(Coordinate, deserialize(child["location"], allow_marshal=allow_marshal)) + + gripper = cls( + name=data["name"], + proximal_joint=cast( + Coordinate, deserialize(data["proximal_joint"], allow_marshal=allow_marshal) + ), + tool_center_point=cast( + Coordinate, deserialize(data["tool_center_point"], allow_marshal=allow_marshal) + ), + body=body, + body_location=where(children[0]), + fingers=fingers, + finger_location=where(children[1]), + jaw_range=(data["jaw_range"][0], data["jaw_range"][1]), + pads=pads or None, + pad_location=where(children[1]["children"][0]) if pads else None, + category=data.get("category", "mechanical_gripper"), + model=data.get("model"), + ) + rotation = data.get("rotation") + if rotation is not None: + gripper.rotation = cast(Rotation, deserialize(rotation, allow_marshal=allow_marshal)) + for child in children[3:]: + gripper.assign_child_resource( + Resource.deserialize(child, allow_marshal=allow_marshal), location=where(child) + ) + return gripper + + def serialize_state(self) -> Dict[str, Any]: + return {**super().serialize_state(), "jaw_width": self.jaw_width} + + def load_state(self, state: Dict[str, Any]) -> None: + super().load_state(state) + self.jaw_width = state["jaw_width"] diff --git a/pylabrobot/resources/end_effector_tests.py b/pylabrobot/resources/end_effector_tests.py new file mode 100644 index 00000000000..acc65014441 --- /dev/null +++ b/pylabrobot/resources/end_effector_tests.py @@ -0,0 +1,185 @@ +import math +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. The origin is the body's corner, and the joint sits inside it. +LENGTH = 137.7 +BODY_SIZE = (59.0, 90.0, 20.3) +# The joint deliberately sits off the body's own middle, 45.0, so that a test cannot pass by +# reading one when it means the other. +PROXIMAL_JOINT = Coordinate(13.0, 38.0, 1.3) +BODY_LOCATION = Coordinate(0.0, 0.0, 0.0) +FINGER_LOCATION = Coordinate(19.5, 38.0, 5.3) +PAD_LOCATION = Coordinate(109.0, 1.5, -17.0) +JAW_RANGE = (70.844, 133.706) + + +def tcp(z: float = 0.0, y: float = 0.0) -> Coordinate: + """The grip centre `LENGTH` along the span from the joint, and `y`/`z` off it.""" + return Coordinate(PROXIMAL_JOINT.x + LENGTH, PROXIMAL_JOINT.y + y, PROXIMAL_JOINT.z + z) + + +def gripper(**overrides) -> MechanicalGripper: + body = Resource( + name="demo_body", + size_x=BODY_SIZE[0], + size_y=BODY_SIZE[1], + size_z=BODY_SIZE[2], + 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", + proximal_joint=PROXIMAL_JOINT, + tool_center_point=tcp(), + 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, tcp()) + self.assertAlmostEqual(g.length, LENGTH) + + def test_a_tool_can_grip_below_where_it_is_mounted(self): + g = gripper(tool_center_point=tcp(z=-13.0)) + self.assertEqual(g.tool_center_point, tcp(z=-13.0)) + # The span now runs diagonally, so the link is longer than its reach along X. + self.assertAlmostEqual(g.length, math.dist((LENGTH, -13.0), (0.0, 0.0))) + + def test_nothing_attaches_past_a_tool(self): + self.assertIsNone(gripper().distal_joint) + + def test_the_member_is_sized_to_its_body_not_to_its_fingers(self): + g = gripper() + self.assertEqual((g.get_size_x(), g.get_size_y(), g.get_size_z()), BODY_SIZE) + # The body states that box once: the member is not told its own size a second time. + self.assertEqual( + (g.body.get_size_x(), g.body.get_size_y(), g.body.get_size_z()), + (g.get_size_x(), g.get_size_y(), g.get_size_z()), + ) + # The fingers are longer than the body they hang from, and reach past it. + self.assertGreater(g.fingers[0].get_size_x(), g.get_size_x()) + + def test_nothing_past_a_tool_means_no_joint_in_its_payload(self): + self.assertNotIn("distal_joint", gripper().serialize()) + + +class TestJaws(unittest.TestCase): + def test_a_width_is_the_gap_the_fingers_leave_between_them(self): + g = gripper() + for width in (133.706, 100.0, 70.844): + g.jaw_width = width + left, right = g.fingers + faces = [ + cast(Coordinate, left.location).y, + cast(Coordinate, right.location).y + right.get_size_y(), + ] + self.assertAlmostEqual(faces[0] - faces[1], width) + # They straddle the grip centre, which is neither the member's middle nor its origin. + self.assertAlmostEqual(faces[0] + faces[1], 2 * tcp().y) + self.assertNotAlmostEqual(tcp().y, g.get_size_y() / 2.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_the_jaws_close_on_the_grip_centre_not_on_the_joint(self): + """A tool that grips off to one side stands its fingers there, not over its own mounting.""" + g = gripper(tool_center_point=tcp(y=-18.0)) + left, right = g.fingers + faces = [ + cast(Coordinate, left.location).y, + cast(Coordinate, right.location).y + right.get_size_y(), + ] + self.assertAlmostEqual(faces[0] + faces[1], 2 * (PROXIMAL_JOINT.y - 18.0)) + self.assertAlmostEqual(faces[0] - faces[1], g.jaw_width) + + def test_the_jaws_keep_the_reach_and_height_they_were_given(self): + """`jaw_width` moves the fingers across the span and must leave X and Z alone.""" + g = gripper() + for width in (133.706, 100.0, 70.844): + g.jaw_width = width + for finger in g.fingers: + here = cast(Coordinate, finger.location) + self.assertAlmostEqual(here.x, FINGER_LOCATION.x) + self.assertAlmostEqual(here.z, FINGER_LOCATION.z) + + 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): + bare = gripper(pads=None, pad_location=None) + self.assertEqual(bare.pads, []) + self.assertEqual([jaw.children for jaw in bare.fingers], [[], []]) + + padded = gripper() + self.assertEqual(len(padded.pads), 2) + self.assertEqual([jaw.children for jaw in padded.fingers], [[pad] for pad in padded.pads]) + + 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_is_fixed_to_its_own_finger_where_it_was_put(self): + g = gripper() + for jaw, face in zip(g.fingers, g.pads): + self.assertIs(face.parent, jaw) + self.assertEqual(face.location, PAD_LOCATION) + + +class TestRoundTrip(unittest.TestCase): + def test_a_gripper_comes_back_with_its_parts_and_its_width(self): + g = gripper(jaw_width=100.0, tool_center_point=tcp(z=-13.0)) + back = MechanicalGripper.deserialize(g.serialize()) + back.load_all_state(g.serialize_all_state()) + + self.assertEqual(back.tool_center_point, g.tool_center_point) + self.assertEqual(back.proximal_joint, PROXIMAL_JOINT) + self.assertEqual((back.get_size_x(), back.get_size_y(), back.get_size_z()), BODY_SIZE) + self.assertEqual(back.jaw_range, g.jaw_range) + self.assertEqual(back.jaw_width, 100.0) + self.assertEqual(cast(Coordinate, back.body.location), BODY_LOCATION) + self.assertEqual([pad.location for pad in back.pads], [PAD_LOCATION] * 2) + self.assertEqual([pad.parent for pad in back.pads], back.fingers) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/resources/manipulator.py b/pylabrobot/resources/manipulator.py new file mode 100644 index 00000000000..45a1175a43a --- /dev/null +++ b/pylabrobot/resources/manipulator.py @@ -0,0 +1,71 @@ +"""The moving mechanism of an arm: the bodies its joints turn between. + +A manipulator is a chain of rigid members and powered joints. A `LinkBody` is one of those +members, an ordinary resource with its origin at a corner, carrying both of its joints as +coordinates within it. Geometry hangs off it as children with their own origins - the separation a +robot description draws between a member's frame and its visual geometry - so material may extend +past either joint without entering the kinematics. + +The link is the line between the two joints. Nothing stores it: `length` is its only measure. +""" + +import math +from typing import Optional + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.resource import Resource + + +class LinkBody(Resource): + """One of the rigid members an arm is built from, joined to its neighbours by joints. + + Its origin is a corner, as any resource's is, and neither joint is obliged to sit there. It + turns about `proximal_joint`, so a caller moves one with + `rotate(z=angle, pivot_coordinate=body.proximal_joint)`. + + A member that ends the chain has no `distal_joint`, because nothing attaches past it. What sits + at the far end of its span is that subclass's own business, and so is its `length`. + """ + + def __init__( + self, + name: str, + size_x: float, + size_y: float, + size_z: float, + proximal_joint: Coordinate, + distal_joint: Optional[Coordinate] = None, + category: str = "link_body", + model: Optional[str] = None, + ): + """ + Args: + name: what to call this one. + size_x: how far the member reaches along X, in mm. + size_y: how far it reaches along Y, in mm. + size_z: how far it reaches along Z, in mm. + proximal_joint: where the joint this member turns on sits within it. + distal_joint: where the joint the next member turns on sits within it. None on a member that + ends the chain. + category: what kind of resource this is. + model: which member this is. + """ + super().__init__( + name=name, size_x=size_x, size_y=size_y, size_z=size_z, category=category, model=model + ) + self.proximal_joint = proximal_joint + self.distal_joint = distal_joint + + @property + def length(self) -> Optional[float]: + """How long the link is, joint to joint, in mm. None on a member that ends the chain.""" + if self.distal_joint is None: + return None + return math.dist(self.distal_joint.vector(), self.proximal_joint.vector()) + + def serialize(self) -> dict: + return { + **super().serialize(), + "proximal_joint": self.proximal_joint.serialize(), + "distal_joint": self.distal_joint.serialize() if self.distal_joint is not None else None, + } diff --git a/pylabrobot/resources/manipulator_tests.py b/pylabrobot/resources/manipulator_tests.py new file mode 100644 index 00000000000..036359d7d3f --- /dev/null +++ b/pylabrobot/resources/manipulator_tests.py @@ -0,0 +1,193 @@ +import unittest +from typing import Tuple, cast + +from pylabrobot.resources.coordinate import Coordinate +from pylabrobot.resources.end_effector_tests import gripper as demo_gripper +from pylabrobot.resources.manipulator import LinkBody +from pylabrobot.resources.resource import Resource +from pylabrobot.utils.linalg import matrix_vector_multiply_3x3 + + +def absolute(resource: Resource, point: Coordinate) -> Coordinate: + """Where `point` in `resource`'s own frame sits on the deck, in mm.""" + carried = matrix_vector_multiply_3x3( + resource.get_absolute_rotation().get_rotation_matrix(), point.vector() + ) + return resource.get_absolute_location() + Coordinate(*carried) + + +def straight_member(name: str, length: float, inset: Coordinate) -> LinkBody: + """A member whose joints sit `inset` in from its own origin, `length` apart along X.""" + return LinkBody( + name=name, + size_x=length + 2 * inset.x, + size_y=2 * inset.y, + size_z=2 * inset.z if inset.z else 1.0, + proximal_joint=inset, + distal_joint=Coordinate(inset.x + length, inset.y, inset.z), + ) + + +class TestLinkBody(unittest.TestCase): + def test_the_link_is_the_distance_between_the_joints(self): + member = straight_member("member", length=100.0, inset=Coordinate(12.7, 12.75, 0.0)) + self.assertAlmostEqual(cast(float, member.length), 100.0) + # The member is longer than the link it carries: material overhangs both joints. + self.assertEqual(member.get_size_x(), 125.4) + + def test_a_link_runs_to_wherever_the_distal_joint_is(self): + member = LinkBody( + name="bent", + size_x=100.0, + size_y=100.0, + size_z=10.0, + proximal_joint=Coordinate(0.0, 0.0, 0.0), + distal_joint=Coordinate(30.0, 40.0, 0.0), + ) + self.assertAlmostEqual(cast(float, member.length), 50.0) + + def test_a_member_that_ends_the_chain_has_no_link(self): + member = LinkBody( + name="last", + size_x=10.0, + size_y=10.0, + size_z=10.0, + proximal_joint=Coordinate(5.0, 5.0, 0.0), + ) + self.assertIsNone(member.distal_joint) + self.assertIsNone(member.length) + + def test_a_child_link_turns_on_top_of_its_parent(self): + inset = Coordinate(10.0, 5.0, 0.0) + base = Resource(name="base", size_x=500, size_y=500, size_z=0) + first = straight_member("first", length=100.0, inset=inset) + second = straight_member("second", length=50.0, inset=inset) + base.assign_child_resource(first, location=Coordinate(-inset.x, -inset.y, 0)) + # The second member's joint lands on the first member's far joint. + first.assign_child_resource( + second, location=Coordinate(cast(Coordinate, first.distal_joint).x - inset.x, 0, 0) + ) + + def far_end() -> Coordinate: + carried = matrix_vector_multiply_3x3( + second.get_absolute_rotation().get_rotation_matrix(), + cast(Coordinate, second.distal_joint).vector(), + ) + return second.get_absolute_location() + Coordinate(*carried) + + self.assertEqual(far_end(), Coordinate(150, 0, 0)) + + # A member turns on its own joint, which is not its origin, so the pivot is not optional. + second.rotate_to(z=90, pivot_coordinate=second.proximal_joint) + self.assertEqual(far_end(), Coordinate(100, 50, 0)) + + first.rotate_to(z=90, pivot_coordinate=first.proximal_joint) + self.assertEqual(far_end(), Coordinate(-50, 100, 0)) + + def test_a_member_comes_back_with_its_shape_and_both_joints(self): + member = LinkBody( + name="member", + size_x=163.4, + size_y=25.5, + size_z=15.3, + proximal_joint=Coordinate(12.7, 12.75, -20.3), + distal_joint=Coordinate(150.4, 12.75, -20.3), + model="demo", + ) + back = LinkBody.deserialize(member.serialize()) + + self.assertEqual((back.get_size_x(), back.get_size_y(), back.get_size_z()), (163.4, 25.5, 15.3)) + self.assertEqual(back.proximal_joint, member.proximal_joint) + self.assertEqual(back.distal_joint, member.distal_joint) + self.assertAlmostEqual(cast(float, back.length), cast(float, member.length)) + self.assertEqual(back.model, "demo") + + def test_a_member_that_ends_the_chain_comes_back_without_one(self): + member = LinkBody( + name="last", + size_x=10.0, + size_y=10.0, + size_z=10.0, + proximal_joint=Coordinate(5.0, 5.0, 0.0), + ) + back = LinkBody.deserialize(member.serialize()) + self.assertIsNone(back.distal_joint) + + +class TestAnAssembledArm(unittest.TestCase): + """A member and the tool it carries, placed on a deck and turned joint by joint. + + The two are built in separate modules and every other test exercises them apart, so this is + the only place the whole chain's absolute geometry is pinned. + """ + + def arm(self): + deck = Resource("deck", size_x=1000, size_y=1000, size_z=10) + deck.location = Coordinate.zero() + forearm = LinkBody( + name="forearm", + size_x=220.0, + size_y=40.0, + size_z=20.0, + proximal_joint=Coordinate(10.0, 20.0, 10.0), + distal_joint=Coordinate(210.0, 20.0, 10.0), + ) + deck.assign_child_resource(forearm, location=Coordinate(100.0, 100.0, 0.0)) + + hand = demo_gripper() + # The tool's own joint lands on the member's far joint. That is what mounting means. + forearm.assign_child_resource( + hand, location=cast(Coordinate, forearm.distal_joint) - hand.proximal_joint + ) + return forearm, hand + + def wrist(self, forearm: LinkBody, hand) -> Tuple[Coordinate, Coordinate]: + """Where the wrist is, read off each side of the joint independently.""" + return ( + absolute(forearm, cast(Coordinate, forearm.distal_joint)), + absolute(hand, hand.proximal_joint), + ) + + def test_the_tool_hangs_where_the_member_ends(self): + forearm, hand = self.arm() + from_member, from_tool = self.wrist(forearm, hand) + self.assertEqual(from_member, Coordinate(310, 120, 10)) + self.assertEqual(from_member, from_tool) + self.assertEqual(absolute(hand, hand.tool_center_point), Coordinate(447.7, 120, 10)) + + def test_the_tool_rides_the_member_it_is_mounted_on(self): + forearm, hand = self.arm() + forearm.rotate_to(z=90, pivot_coordinate=forearm.proximal_joint) + + from_member, from_tool = self.wrist(forearm, hand) + self.assertEqual(from_member, Coordinate(110, 320, 10)) + self.assertEqual(from_member, from_tool) + # The tool did not turn on its own joint, so it swung round with the member carrying it. + self.assertEqual(absolute(hand, hand.tool_center_point), Coordinate(110, 457.7, 10)) + + def test_the_tool_also_turns_on_its_own_joint(self): + forearm, hand = self.arm() + forearm.rotate_to(z=90, pivot_coordinate=forearm.proximal_joint) + hand.rotate_to(z=90, pivot_coordinate=hand.proximal_joint) + + from_member, from_tool = self.wrist(forearm, hand) + # The wrist is the fixed point of the tool's own turn, so it has not moved. + self.assertEqual(from_member, Coordinate(110, 320, 10)) + self.assertEqual(from_member, from_tool) + # Two right angles, so the grip centre now points back the way the member came. + self.assertEqual(absolute(hand, hand.tool_center_point), Coordinate(-27.7, 320, 10)) + + def test_the_fingers_travel_with_the_tool(self): + forearm, hand = self.arm() + at_rest = [absolute(finger, Coordinate.zero()) for finger in hand.fingers] + self.assertEqual(at_rest[0], Coordinate(316.5, 186.853, 14)) + self.assertEqual(at_rest[1], Coordinate(316.5, 46.147, 14)) + + forearm.rotate_to(z=90, pivot_coordinate=forearm.proximal_joint) + turned = [absolute(finger, Coordinate.zero()) for finger in hand.fingers] + self.assertEqual(turned[0], Coordinate(43.147, 326.5, 14)) + self.assertEqual(turned[1], Coordinate(183.853, 326.5, 14)) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/resources/resource.py b/pylabrobot/resources/resource.py index 4c603a9c65a..e10f967461e 100644 --- a/pylabrobot/resources/resource.py +++ b/pylabrobot/resources/resource.py @@ -892,24 +892,136 @@ def location(self, location: Optional[Coordinate]) -> None: if changed and self.parent is not None: self._state_updated() - def rotate(self, x: float = 0, y: float = 0, z: float = 0): - """Rotate counter-clockwise around the parent-coordinate axes by the given degrees.""" + def _apply_pivot_shift(self, before: List[List[float]], pivot_coordinate: Coordinate) -> None: + """Shift `location` so `pivot_coordinate` ends where it was before this resource turned. + + Args: + before: this resource's absolute rotation matrix, from before the turn. + pivot_coordinate: what to hold still, in this resource's own frame. + """ + after = self.get_absolute_rotation().get_rotation_matrix() + was = matrix_vector_multiply_3x3(before, pivot_coordinate.vector()) + now = matrix_vector_multiply_3x3(after, pivot_coordinate.vector()) + shift = Coordinate(was[0] - now[0], was[1] - now[1], was[2] - now[2]) + # `shift` is in this resource's frame, `location` in the parent's. A rotation matrix + # inverts by transposing. A parent with no location of its own is where + # `get_absolute_location` stops walking, so `location` is read in absolute axes from there + # and needs no conversion. + parent = self.parent + if parent is not None and parent.location is not None: + turned = parent.get_absolute_rotation().get_rotation_matrix() + shift = Coordinate( + *matrix_vector_multiply_3x3( + [[turned[j][i] for j in range(3)] for i in range(3)], shift.vector() + ) + ) + # Straight onto the field: the caller fires one `_state_updated` for the whole turn, and + # going through the setter would fire a second carrying the same final state. + self._location = cast(Coordinate, self.location) + shift + + def _pivot_reference(self, pivot_coordinate: Optional[Coordinate]) -> Optional[List[List[float]]]: + """The rotation to measure a pivoted turn against, or None when no pivot was asked for. + + Args: + pivot_coordinate: what the caller wants held still, if anything. + + Returns: + This resource's absolute rotation matrix, to hand back after the turn. + + Raises: + NoLocationError: If a pivot is given for a resource with no location. A pivot is held by + moving `location`, so there is nothing to hold it with. + """ + if pivot_coordinate is None: + return None + if self.location is None: + raise NoLocationError(f"Resource '{self.name}' has no location, so a pivot cannot be held.") + return self.get_absolute_rotation().get_rotation_matrix() + + def rotate( + self, + x: float = 0, + y: float = 0, + z: float = 0, + pivot_coordinate: Optional[Coordinate] = None, + ): + """Rotate counter-clockwise around the parent-coordinate axes by the given degrees. + + Args: + x: degrees to turn about X. + y: degrees to turn about Y. + z: degrees to turn about Z. + pivot_coordinate: what to turn about, in this resource's own frame. Its own origin when + None, which is what a resource turns about when nothing is said. Given one, `location` + carries by however far the turn moved it, so it ends where it began. + + Raises: + NoLocationError: If a pivot is given for a resource with no location. A pivot is held by + moving `location`, so there is nothing to hold it with. + """ + before = self._pivot_reference(pivot_coordinate) self.rotation._prepend(Rotation(x=x, y=y, z=z)) + + if before is not None: + self._apply_pivot_shift(before, cast(Coordinate, pivot_coordinate)) + # Rotation is part of the resource's state; notify subscribers (e.g. the # 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, + pivot_coordinate: Optional[Coordinate] = None, + ): + """Set the rotation about each axis, where `rotate` turns by an amount instead. + + Args: + x: the angle about X to sit at, in degrees. Left where it is when None. + y: the angle about Y to sit at, in degrees. Left where it is when None. + z: the angle about Z to sit at, in degrees. Left where it is when None. + pivot_coordinate: what to turn about, as `rotate` takes it. + + Raises: + NoLocationError: If a pivot is given for a resource with no location, as `rotate` raises. + """ + before = self._pivot_reference(pivot_coordinate) + + 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 before is not None: + self._apply_pivot_shift(before, cast(Coordinate, pivot_coordinate)) + + 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()) return resource_copy - def rotated(self, x: float = 0, y: float = 0, z: float = 0) -> Self: - """Return a copy of this resource rotated by the given number of degrees.""" + def rotated( + self, + x: float = 0, + y: float = 0, + z: float = 0, + pivot_coordinate: Optional[Coordinate] = None, + ) -> Self: + """Return a copy of this resource rotated by the given number of degrees. + + Args: + x: degrees to turn about X. + y: degrees to turn about Y. + z: degrees to turn about Z. + pivot_coordinate: what to turn about, as `rotate` takes it. + """ new_resource = self.copy() - new_resource.rotate(x=x, y=y, z=z) + new_resource.rotate(x=x, y=y, z=z, pivot_coordinate=pivot_coordinate) return new_resource def at(self, location: Coordinate) -> Self: diff --git a/pylabrobot/resources/resource_tests.py b/pylabrobot/resources/resource_tests.py index 0a46efc7fb4..6caaefe1d0d 100644 --- a/pylabrobot/resources/resource_tests.py +++ b/pylabrobot/resources/resource_tests.py @@ -19,7 +19,7 @@ from pylabrobot.resources.barcode import Barcode from pylabrobot.resources.coordinate import Coordinate from pylabrobot.resources.deck import Deck -from pylabrobot.resources.errors import ResourceNotFoundError +from pylabrobot.resources.errors import NoLocationError, ResourceNotFoundError from pylabrobot.resources.plate_adapter import PlateAdapter from pylabrobot.resources.resource import Resource from pylabrobot.resources.rotation import Rotation @@ -332,6 +332,159 @@ def test_rotation90(self): self.assertAlmostEqual(c.get_absolute_size_x(), 20) self.assertAlmostEqual(c.get_absolute_size_y(), 10) + 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_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, pivot_coordinate=far_end) + self.assertEqual(where(), before) + + def test_a_pivot_holds_about_every_axis(self): + for turn in ("rotate", "rotate_to"): + for axis in ("x", "y", "z"): + for angle in (30.0, 90.0, 200.0): + parent = Resource("parent", size_x=500, size_y=500, size_z=500) + parent.location = Coordinate.zero() + bar = Resource("bar", size_x=100, size_y=10, size_z=10) + parent.assign_child_resource(bar, location=Coordinate(7, 11, 13)) + joint = Coordinate(100, 5, 5) + + def where(bar: Resource = bar, joint: Coordinate = joint) -> Coordinate: + carried = matrix_vector_multiply_3x3( + bar.get_absolute_rotation().get_rotation_matrix(), joint.vector() + ) + return bar.get_absolute_location() + Coordinate(*carried) + + before = where() + x = angle if axis == "x" else None + y = angle if axis == "y" else None + z = angle if axis == "z" else None + if turn == "rotate": + bar.rotate(x=x or 0.0, y=y or 0.0, z=z or 0.0, pivot_coordinate=joint) + else: + bar.rotate_to(x=x, y=y, z=z, pivot_coordinate=joint) + + after = where() + for was, now in zip((before.x, before.y, before.z), (after.x, after.y, after.z)): + self.assertAlmostEqual(was, now, places=9) + # Holding the pivot still is not enough: the turn has to have happened. + reached = (bar.rotation.x, bar.rotation.y, bar.rotation.z) + self.assertAlmostEqual(reached["xyz".index(axis)], angle % 360, places=9) + self.assertEqual(sum(1 for turned in reached if turned != 0), 1) + + def test_a_pivot_turns_by_an_amount_and_goes_to_an_angle(self): + """`rotate` carries the pivot on each turn; `rotate_to` holds it where a repeat changes nothing.""" + parent = Resource("parent", size_x=500, size_y=500, size_z=10) + parent.location = Coordinate.zero() + plate = Resource("plate", size_x=100, size_y=50, size_z=10) + parent.assign_child_resource(plate, location=Coordinate(200, 300, 0)) + centre = Coordinate(50, 25, 0) + + plate.rotate(z=90, pivot_coordinate=centre) + self.assertEqual(plate.rotation.z, 90) + self.assertEqual(plate.location, Coordinate(275, 275, 0)) + + plate.rotate(z=90, pivot_coordinate=centre) + self.assertEqual(plate.rotation.z, 180) + # The centre is still at (250, 325, 0), so the corner the location names has swung again. + self.assertEqual(plate.location, Coordinate(300, 350, 0)) + + where = plate.location + plate.rotate_to(z=180, pivot_coordinate=centre) + self.assertEqual((plate.rotation.z, plate.location), (180, where)) + + def test_a_pivot_needs_a_location_to_be_held_by(self): + """A pivot is held by moving `location`, so one with none cannot honour the request.""" + joint = Coordinate(50, 5, 5) + with self.assertRaises(NoLocationError): + Resource("loose", size_x=100, size_y=10, size_z=10).rotate(z=90, pivot_coordinate=joint) + with self.assertRaises(NoLocationError): + Resource("loose", size_x=100, size_y=10, size_z=10).rotate_to(z=90, pivot_coordinate=joint) + + # Without a pivot an unplaced resource turns about its origin, as it always has. + loose = Resource("loose", size_x=100, size_y=10, size_z=10) + loose.rotate(z=90) + self.assertEqual(loose.rotation.z, 90) + + def test_a_pivot_holds_under_an_unlocated_parent_that_a_rotated_ancestor_carries(self): + """`location` is read in absolute axes from where the location chain stops, not in a parent's.""" + grandparent = Resource("grandparent", size_x=500, size_y=500, size_z=10) + grandparent.location = Coordinate.zero() + grandparent.rotate(z=30) + parent = Resource("parent", size_x=300, size_y=300, size_z=10) + grandparent.assign_child_resource(parent, location=None) + plate = Resource("plate", size_x=100, size_y=50, size_z=10) + parent.assign_child_resource(plate, location=Coordinate(200, 300, 0)) + centre = Coordinate(50, 25, 0) + + def where() -> Coordinate: + carried = matrix_vector_multiply_3x3( + plate.get_absolute_rotation().get_rotation_matrix(), centre.vector() + ) + return plate.get_absolute_location() + Coordinate(*carried) + + before = where() + plate.rotate(z=90, pivot_coordinate=centre) + after = where() + for was, now in zip((before.x, before.y, before.z), (after.x, after.y, after.z)): + self.assertAlmostEqual(was, now, places=3) + + def test_rotated_carries_the_pivot_and_leaves_the_original(self): + parent = Resource("parent", size_x=500, size_y=500, size_z=10) + parent.location = Coordinate.zero() + plate = Resource("plate", size_x=100, size_y=50, size_z=10) + parent.assign_child_resource(plate, location=Coordinate(200, 300, 0)) + + turned = plate.rotated(z=90, pivot_coordinate=Coordinate(50, 25, 0)) + + self.assertEqual(turned.location, Coordinate(275, 275, 0)) + self.assertEqual(plate.location, Coordinate(200, 300, 0)) + self.assertEqual(plate.rotation.z, 0) + def test_rotation180(self): r = Resource("parent", size_x=200, size_y=100, size_z=100) r.location = Coordinate.zero()