Fix Robotiq2F140 finger_length and make clearance configurable (#42)

Two changes:

1. Robotiq2F140.FINGER_LENGTH: 55mm → 114mm

   finger_length is the distance from the MuJoCo grasp_site to the
   fingertip along the approach axis. The previous value (55mm) was the
   pad length from the Robotiq datasheet, but the grasp_site sits at
   the base_mount origin — 100mm behind the pad base. Measured from
   the actual MuJoCo model: the pad tip corner is 114mm from the
   grasp_site.

   With 55mm, the TSR placed the gripper 55mm above the top face for
   the shallowest grasp. The real fingertips were actually at 114mm,
   so the gripper housing was 59mm lower than intended — colliding
   with the table for any object shorter than ~60mm. Sugar box (48mm)
   and gelatin box (28mm) were unpickable.

   With 114mm, the shallowest grasp correctly places the fingertips
   at the object's top edge, keeping the housing clear of the table.

2. ParallelJawGripper.clearance_fraction: hardcoded 0.3 → configurable

   The clearance (safety margin at grasp depth limits) was hardcoded
   as 30% of graspable_depth. Now a constructor parameter defaulting
   to 0.1 (10%). The old 30% was too conservative with the corrected
   finger_length — it rejected valid grasps on objects shorter than
   ~70mm.

Author: siddhss5

src/tsr/hands/parallel_jaw.py

@@ -37,23 +37,28 @@ class ParallelJawGripper(GripperBase):
         max_aperture:  Maximum jaw opening [m].
     """
 
-    def __init__(self, finger_length: float, max_aperture: float):
+    def __init__(
+        self,
+        finger_length: float,
+        max_aperture: float,
+        clearance_fraction: float = 0.1,
+    ):
         self.finger_length = finger_length
         self.max_aperture  = max_aperture
+        self.clearance_fraction = clearance_fraction
 
     def _default_clearance(self, graspable_depth: float) -> float:
         """Compute default clearance from graspable depth.
 
-        Clearance is 30% of the graspable depth — the distance fingers
-        can wrap around the object. This ensures the shallowest grasp
-        has enough contact area for friction to hold.
+        Clearance is ``clearance_fraction`` of the graspable depth — the
+        distance fingers can wrap around the object.
 
         Args:
             graspable_depth: max penetration depth for this grasp geometry,
                 typically min(finger_length, object_radius) for side grasps
                 or finger_length for top/bottom grasps.
         """
-        return 0.3 * graspable_depth
+        return self.clearance_fraction * graspable_depth
 
     def _validate(self, cylinder_radius: float, preshape: float) -> None:
         if cylinder_radius <= 0:
@@ -941,7 +946,11 @@ def renderer(self):
 class Robotiq2F140(ParallelJawGripper):
     """Robotiq 2F-140 parallel gripper.
 
-    Fixed hardware parameters: finger_length=55 mm, max_aperture=140 mm.
+    Fixed hardware parameters: finger_length=82 mm, max_aperture=140 mm.
+
+    finger_length is the distance from the MuJoCo grasp_site to the finger
+    pad tip (measured along the approach axis). The grasp_site is at the
+    base_mount origin; the pad tip is 114 mm along the approach direction.
 
     Outputs poses in the canonical TSR EE frame (z=approach, y=finger-opening,
     x=palm normal). The corresponding MuJoCo model (geodude_assets 2f140.xml)
@@ -950,7 +959,7 @@ class Robotiq2F140(ParallelJawGripper):
     directly.
     """
 
-    FINGER_LENGTH = 0.055
+    FINGER_LENGTH = 0.114
     MAX_APERTURE  = 0.140
 
     def __init__(self):

tests/tsr/hands/test_box_grasp.py

@@ -63,7 +63,7 @@ def test_palm_offset_is_h_palm_outside_face(self):
 
     def test_preshape_matches_span_dim(self):
         """Auto-computed preshape = span_dim + clearance for each orientation."""
-        clearance = 0.3 * self.gripper.finger_length
+        clearance = self.gripper.clearance_fraction * self.gripper.finger_length
         for t in self.gripper.grasp_box(SX, SY, SZ):
             ps = t.preshape[0]
             # preshape should be close to one of the box face dimensions + clearance
@@ -121,7 +121,7 @@ def test_names_contain_span_labels(self):
         self.assertTrue(any("span-y" in n for n in names))
 
     def test_span_x_slides_in_y(self):
-        clearance = 0.3 * self.gripper.finger_length
+        clearance = self.gripper.clearance_fraction * self.gripper.finger_length
         span_x_templates = [t for t in self.gripper.grasp_box_top(SX, SY, SZ)
                              if "span-x" in t.name]
         for t in span_x_templates:
@@ -130,7 +130,7 @@ def test_span_x_slides_in_y(self):
             self.assertAlmostEqual(t.Bw[1, 1],  (SY / 2 - clearance))
 
     def test_span_y_slides_in_x(self):
-        clearance = 0.3 * self.gripper.finger_length
+        clearance = self.gripper.clearance_fraction * self.gripper.finger_length
         span_y_templates = [t for t in self.gripper.grasp_box_top(SX, SY, SZ)
                              if "span-y" in t.name]
         for t in span_y_templates:
@@ -212,7 +212,7 @@ def test_tw_e_valid_se3(self):
             _check_se3(self, t.Tw_e[:3, :3])
 
     def test_span_y_slides_in_z_only(self):
-        clearance = 0.3 * self.gripper.finger_length
+        clearance = self.gripper.clearance_fraction * self.gripper.finger_length
         span_y = [t for t in self.gripper.grasp_box_face_x(SX, SY, SZ)
                   if "span-y" in t.name]
         for t in span_y:
@@ -222,7 +222,7 @@ def test_span_y_slides_in_z_only(self):
             self.assertAlmostEqual(t.Bw[2, 1],  (SZ / 2 - clearance))
 
     def test_span_z_slides_in_y_only(self):
-        clearance = 0.3 * self.gripper.finger_length
+        clearance = self.gripper.clearance_fraction * self.gripper.finger_length
         span_z = [t for t in self.gripper.grasp_box_face_x(SX, SY, SZ)
                   if "span-z" in t.name]
         for t in span_z:
@@ -270,7 +270,7 @@ def test_tw_e_valid_se3(self):
             _check_se3(self, t.Tw_e[:3, :3])
 
     def test_span_x_slides_in_z_only(self):
-        clearance = 0.3 * self.gripper.finger_length
+        clearance = self.gripper.clearance_fraction * self.gripper.finger_length
         span_x = [t for t in self.gripper.grasp_box_face_y(SX, SY, SZ)
                   if "span-x" in t.name]
         for t in span_x:

tests/tsr/hands/test_parallel_jaw.py

@@ -42,7 +42,7 @@ def test_narrow_height_raises(self):
             self.gripper.grasp_cylinder_side(R, 0.001)
 
     def test_default_preshape_is_2r_plus_clearance(self):
-        clearance = 0.3 * min(self.gripper.finger_length, R)
+        clearance = self.gripper.clearance_fraction * min(self.gripper.finger_length, R)
         expected  = 2 * R + clearance
         templates = self.gripper.grasp_cylinder_side(R, H)
         np.testing.assert_allclose(templates[0].preshape[0], expected)

tests/tsr/hands/test_robotiq.py

@@ -11,7 +11,7 @@ def setUp(self):
         self.gripper = Robotiq2F140()
 
     def test_fixed_params(self):
-        self.assertAlmostEqual(self.gripper.finger_length, 0.055)
+        self.assertAlmostEqual(self.gripper.finger_length, 0.114)
         self.assertAlmostEqual(self.gripper.max_aperture,  0.140)
 
     def test_is_subclass_of_parallel_jaw(self):
@@ -21,7 +21,7 @@ def test_outputs_canonical_frames(self):
         # Robotiq2F140 now outputs canonical TSR EE poses (no frame correction).
         # It should produce identical Tw_e as a plain ParallelJawGripper with
         # the same hardware parameters.
-        base      = ParallelJawGripper(finger_length=0.055, max_aperture=0.140)
+        base      = ParallelJawGripper(finger_length=0.114, max_aperture=0.140)
         t_base    = base.grasp_cylinder_side(0.040, 0.10)
         t_robotiq = self.gripper.grasp_cylinder_side(0.040, 0.10)
 

tests/tsr/hands/test_sphere_grasp.py

@@ -47,7 +47,7 @@ def test_z_ee_points_inward(self):
                                        err_msg=f"z_EE not inward in {t.name}")
 
     def test_standoff_within_expected_range(self):
-        clearance = 0.3 * min(FL, RADIUS)
+        clearance = self.gripper.clearance_fraction * min(FL, RADIUS)
         depth_min = RADIUS  # fingertips at center
         depth_max = min(FL, 2 * RADIUS) - clearance
         ro_max = RADIUS + FL - depth_min  # = FL
@@ -84,7 +84,7 @@ def test_bw_full_yaw_freedom(self):
     # ── Preshape ──────────────────────────────────────────────────────────
 
     def test_default_preshape_is_diameter_plus_clearance(self):
-        clearance = 0.3 * min(FL, RADIUS)
+        clearance = self.gripper.clearance_fraction * min(FL, RADIUS)
         expected = 2 * RADIUS + clearance
         for t in self.templates:
             self.assertAlmostEqual(t.preshape[0], expected)

tests/tsr/hands/test_torus_grasp.py

@@ -116,7 +116,7 @@ def test_bw_no_roll_pitch_freedom(self):
     # ── Preshape ──────────────────────────────────────────────────────────
 
     def test_default_preshape_is_tube_diameter_plus_clearance(self):
-        clearance = 0.3 * min(FL, Sr)
+        clearance = self.gripper.clearance_fraction * min(FL, Sr)
         expected = 2 * Sr + clearance
         for t in self.templates:
             self.assertAlmostEqual(t.preshape[0], expected)
@@ -229,7 +229,7 @@ def test_bw_no_roll_pitch_freedom(self):
             self.assertEqual(t.Bw[4, 0], t.Bw[4, 1])
 
     def test_preshape_spans_outer_diameter(self):
-        clearance = 0.3 * FL
+        clearance = self.gripper.clearance_fraction * FL
         expected = 2 * (SR + Sr) + clearance
         for t in self.gripper.grasp_torus_span(SR, Sr):
             self.assertAlmostEqual(t.preshape[0], expected)