diff --git a/pylabrobot/resources/end_effector.py b/pylabrobot/resources/end_effector.py index dce7691c015..70985c1c4cb 100644 --- a/pylabrobot/resources/end_effector.py +++ b/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 ) diff --git a/pylabrobot/resources/end_effector_tests.py b/pylabrobot/resources/end_effector_tests.py index c521bb54ec7..1d1e17341d9 100644 --- a/pylabrobot/resources/end_effector_tests.py +++ b/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): diff --git a/pylabrobot/resources/manipulator_tests.py b/pylabrobot/resources/manipulator_tests.py index 036359d7d3f..27233a73f8d 100644 --- a/pylabrobot/resources/manipulator_tests.py +++ b/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()