MechanicalGripper: state tool_center_point from the flange

pylabrobot/resources/end_effector.py

@@ -51,7 +51,8 @@ def __init__(
     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.
+      tool_center_point: the point it grips at, from `proximal_joint` - the flange it is mounted
+        on - rather than 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.
@@ -110,7 +111,7 @@ def tool_center_point(self) -> Coordinate:
   @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())
+    return math.dist(self._tool_center_point.vector(), (0.0, 0.0, 0.0))
 
   @property
   def jaw_width(self) -> float:
@@ -132,7 +133,7 @@ def _place_the_fingers(self) -> None:
       # 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
+      facing = self.proximal_joint.y + 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
       )

pylabrobot/resources/end_effector_tests.py

@@ -19,8 +19,8 @@
 
 
 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)
+  """The grip centre `LENGTH` along the span from the joint, and `y`/`z` off it, from the joint."""
+  return Coordinate(LENGTH, y, z)
 
 
 def gripper(**overrides) -> MechanicalGripper:
@@ -74,6 +74,14 @@ def test_a_tool_can_grip_below_where_it_is_mounted(self):
     # 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_the_grip_centre_is_stated_from_where_the_tool_is_mounted(self):
+    """Moving the joint inside the member moves where the tool is mounted, not how far it reaches:
+    the grip centre is an offset from the flange, so it and the reach stay what they were."""
+    moved = PROXIMAL_JOINT + Coordinate(7.0, 0.0, 0.0)
+    g = gripper(proximal_joint=moved)
+    self.assertEqual(g.tool_center_point, tcp())
+    self.assertAlmostEqual(g.length, LENGTH)
+
   def test_nothing_attaches_past_a_tool(self):
     self.assertIsNone(gripper().distal_joint)
 
@@ -104,8 +112,8 @@ def test_a_width_is_the_gap_the_fingers_leave_between_them(self):
       ]
       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)
+      self.assertAlmostEqual(faces[0] + faces[1], 2 * (PROXIMAL_JOINT.y + tcp().y))
+      self.assertNotAlmostEqual(PROXIMAL_JOINT.y + tcp().y, g.get_size_y() / 2.0)
 
   def test_the_jaws_refuse_a_width_they_do_not_reach(self):
     with self.assertRaises(ValueError):

pylabrobot/resources/manipulator_tests.py

@@ -153,7 +153,9 @@ def test_the_tool_hangs_where_the_member_ends(self):
     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))
+    self.assertEqual(
+      absolute(hand, hand.proximal_joint + 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()
@@ -163,7 +165,9 @@ def test_the_tool_rides_the_member_it_is_mounted_on(self):
     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))
+    self.assertEqual(
+      absolute(hand, hand.proximal_joint + hand.tool_center_point), Coordinate(110, 457.7, 10)
+    )
 
   def test_the_tool_also_turns_on_its_own_joint(self):
     forearm, hand = self.arm()
@@ -175,7 +179,9 @@ def test_the_tool_also_turns_on_its_own_joint(self):
     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))
+    self.assertEqual(
+      absolute(hand, hand.proximal_joint + hand.tool_center_point), Coordinate(-27.7, 320, 10)
+    )
 
   def test_the_fingers_travel_with_the_tool(self):
     forearm, hand = self.arm()