Resource: turn about a joint, and the links and end-effectors that need it

pylabrobot/resources/__init__.py

@@ -23,13 +23,15 @@
 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 *
 from .hamilton import *
 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 *

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)}

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()

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)

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()

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())