Add touch sensors at each fingertip.

robopianist/models/hands/shadow_hand.py

@@ -183,6 +183,10 @@ def _parse_mjcf_elements(self) -> None:
         self._joints = tuple(joints)
         self._actuators = tuple(actuators)
 
+        # Remove "grasp_site".
+        if self._hand_side == base.HandSide.RIGHT:
+            mjcf_utils.safe_find(self._mjcf_root, "site", "grasp_site").remove()
+
     def _add_mjcf_elements(self) -> None:
         # Add sites to the tips of the fingers.
         fingertip_sites = []
@@ -241,6 +245,30 @@ def _add_mjcf_elements(self) -> None:
         self._actuator_velocity_sensors = tuple(actuator_velocity_sensors)
         self._actuator_force_sensors = tuple(actuator_force_sensors)
 
+        # Add touch sensors to the fingertips.
+        fingertip_touch_sensors = []
+        for tip_name in consts.FINGERTIP_BODIES:
+            tip_elem = mjcf_utils.safe_find(
+                self._mjcf_root, "body", self._prefix + tip_name
+            )
+            offset = _THUMBTIP_OFFSET if tip_name == "thdistal" else _FINGERTIP_OFFSET
+            touch_site = tip_elem.add(
+                "site",
+                name=tip_name + "_touch_site",
+                pos=(0.0, 0.0, offset),
+                type="sphere",
+                size=(0.01,),
+                group=composer.SENSOR_SITES_GROUP,
+                rgba=(0, 1, 0, 0.6),
+            )
+            touch_sensor = self._mjcf_root.sensor.add(
+                "touch",
+                site=touch_site,
+                name=tip_name + "_touch",
+            )
+            fingertip_touch_sensors.append(touch_sensor)
+        self._fingertip_touch_sensors = tuple(fingertip_touch_sensors)
+
     def _add_dofs(self) -> None:
         """Add forearm degrees of freedom."""
 
@@ -335,6 +363,10 @@ def actuator_velocity_sensors(self) -> Sequence[types.MjcfElement]:
     def actuator_force_sensors(self) -> Sequence[types.MjcfElement]:
         return self._actuator_force_sensors
 
+    @property
+    def fingertip_touch_sensors(self) -> Sequence[types.MjcfElement]:
+        return self._fingertip_touch_sensors
+
     # Action specs.
 
     def action_spec(self, physics: mjcf.Physics) -> specs.BoundedArray:
@@ -391,3 +423,12 @@ def _get_fingertip_positions(physics: mjcf.Physics) -> np.ndarray:
             return physics.bind(self._entity.fingertip_sites).xpos.ravel()
 
         return observable.Generic(raw_observation_callable=_get_fingertip_positions)
+
+    @composer.observable
+    def fingertip_force(self):
+        """Returns for each finger, the sum of forces felt at the fingertip."""
+
+        def _get_fingertip_force(physics: mjcf.Physics) -> np.ndarray:
+            return physics.bind(self._entity.fingertip_touch_sensors).sensordata
+
+        return observable.Generic(raw_observation_callable=_get_fingertip_force)

robopianist/models/hands/shadow_hand_test.py

@@ -142,6 +142,7 @@ def test_get_element_property(self, name: str) -> None:
             "actuator_velocity_sensors",
             "actuator_force_sensors",
             "fingertip_sites",
+            "fingertip_touch_sensors",
         ]
     )
     def test_get_element_tuple_property(self, name: str) -> None:
@@ -160,6 +161,7 @@ def test_get_element_tuple_property(self, name: str) -> None:
             "actuators_velocity",
             "actuators_power",
             "position",
+            "fingertip_force",
         ]
     )
     def test_evaluate_observable(self, name: str) -> None: