feat(env): tilburg hand controller integration

python/examples/env_cartesian_control_tilburg.py

@@ -0,0 +1,110 @@
+import logging
+from typing import Union
+
+from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager
+from rcsss.control.utils import load_creds_fr3_desk
+from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance
+from rcsss.envs.factories import fr3_hw_env, fr3_sim_env
+from rcsss.envs.utils import (
+    default_fr3_hw_robot_cfg,
+    default_fr3_sim_gripper_cfg,
+    default_fr3_sim_robot_cfg,
+    default_mujoco_cameraset_cfg,
+    default_tilburg_hw_hand_cfg,
+)
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+ROBOT_IP = "192.168.101.1"
+ROBOT_INSTANCE = RobotInstance.SIMULATION
+
+
+"""
+Create a .env file in the same directory as this file with the following content:
+FR3_USERNAME=<username on franka desk>
+FR3_PASSWORD=<password on franka desk>
+
+When you use a real FR3 you first need to unlock its joints using the following cli script:
+
+python -m rcsss fr3 unlock <ip>
+
+or put it into guiding mode using:
+
+python -m rcsss fr3 guiding-mode <ip>
+
+When you are done you lock it again using:
+
+python -m rcsss fr3 lock <ip>
+
+or even shut it down using:
+
+python -m rcsss fr3 shutdown <ip>
+"""
+
+
+def main():
+    resource_manger: FCI | DummyResourceManager
+    if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        user, pw = load_creds_fr3_desk()
+        resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
+    else:
+        resource_manger = DummyResourceManager()
+    with resource_manger:
+        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+            env_rel = fr3_hw_env(
+                ip=ROBOT_IP,
+                control_mode=ControlMode.CARTESIAN_TQuart,
+                robot_cfg=default_fr3_hw_robot_cfg(),
+                collision_guard="lab",
+                gripper_cfg=default_tilburg_hw_hand_cfg(),
+                max_relative_movement=0.5,
+                relative_to=RelativeTo.LAST_STEP,
+            )
+        else:
+            env_rel = fr3_sim_env(
+                control_mode=ControlMode.CARTESIAN_TQuart,
+                robot_cfg=default_fr3_sim_robot_cfg(),
+                collision_guard=False,
+                gripper_cfg=default_fr3_sim_gripper_cfg(),
+                camera_set_cfg=default_mujoco_cameraset_cfg(),
+                max_relative_movement=0.5,
+                relative_to=RelativeTo.LAST_STEP,
+            )
+            env_rel.get_wrapper_attr("sim").open_gui()
+
+        env_rel.reset()
+        print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
+        close_action: Union[int, list[float]]
+        open_action: Union[int, list[float]]
+        if default_tilburg_hw_hand_cfg().binary_action:
+            close_action = 0
+            open_action = 1
+        else:
+            template = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0]
+            value = 0.5
+            close_action = [value * val for val in template]
+            open_action = [0.0] * len(template)
+
+        for _ in range(10):
+            for _ in range(10):
+                # move 1cm in x direction (forward) and close gripper
+                act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "hand": close_action}
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+            from time import sleep
+
+            sleep(5)
+            for _ in range(10):
+                # move 1cm in negative x direction (backward) and open gripper
+                act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "hand": open_action}
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+
+
+if __name__ == "__main__":
+    main()

python/rcsss/envs/base.py

@@ -14,9 +14,11 @@
     RCSpaceType,
     Vec6Type,
     Vec7Type,
+    Vec18Type,
     get_space,
     get_space_keys,
 )
+from rcsss.hand.hand import Hand
 
 _logger = logging.getLogger()
 
@@ -97,6 +99,22 @@ class GripperDictType(RCSpaceType):
     gripper: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)]
 
 
+class HandBinDictType(RCSpaceType):
+    # 0 for closed, 1 for open (>=0.5 for open)
+    hand: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)]
+
+
+class HandVecDictType(RCSpaceType):
+    hand: Annotated[
+        Vec18Type,
+        gym.spaces.Box(
+            low=np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
+            high=np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]),
+            dtype=np.float32,
+        ),
+    ]
+
+
 class CameraDictType(RCSpaceType):
     frames: dict[
         Annotated[str, "camera_names"],
@@ -577,3 +595,74 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         self._last_gripper_cmd = gripper_action
         del action[self.gripper_key]
         return action
+
+
+class HandWrapper(ActObsInfoWrapper):
+    """
+    This wrapper allows for controlling the hand of the robot
+    using either binary or continuous actions.
+    The binary action space allows for opening and closing the hand,
+    while the continuous action space allows for setting the hand
+    to a specific pose.
+    The wrapper also provides an observation space that includes
+    the hand state.
+    The hand state is represented as a binary value (0 for closed,
+    1 for open) or as a continuous value (normalized joint positions).
+    """
+
+    BINARY_HAND_CLOSED = 0
+    BINARY_HAND_OPEN = 1
+
+    def __init__(self, env, hand: Hand, binary: bool = True):
+        super().__init__(env)
+        self.unwrapped: FR3Env
+        self.observation_space: gym.spaces.Dict
+        self.action_space: gym.spaces.Dict
+        self.binary = binary
+        if self.binary:
+            self.observation_space.spaces.update(get_space(HandBinDictType).spaces)
+            self.action_space.spaces.update(get_space(HandBinDictType).spaces)
+            self.hand_key = get_space_keys(HandBinDictType)[0]
+        else:
+            self.observation_space.spaces.update(get_space(HandVecDictType).spaces)
+            self.action_space.spaces.update(get_space(HandVecDictType).spaces)
+            self.hand_key = get_space_keys(HandVecDictType)[0]
+        self._hand = hand
+        self._last_hand_cmd = None
+
+    def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]:
+        self._hand.reset()
+        self._last_hand_cmd = None
+        return super().reset(**kwargs)
+
+    def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]:
+        observation = copy.deepcopy(observation)
+        if self.binary:
+            observation[self.hand_key] = (
+                self._last_hand_cmd if self._last_hand_cmd is not None else self.BINARY_HAND_OPEN
+            )
+        else:
+            observation[self.hand_key] = self._hand.get_normalized_joints_poses()
+
+        info = {}
+        return observation, info
+
+    def action(self, action: dict[str, Any]) -> dict[str, Any]:
+
+        action = copy.deepcopy(action)
+        assert self.hand_key in action, "hand action not found."
+
+        hand_action = np.round(action[self.hand_key]) if self.binary else action[self.hand_key]
+        hand_action = np.clip(hand_action, 0.0, 1.0)
+
+        if self.binary:
+            if self._last_hand_cmd is None or self._last_hand_cmd != hand_action:
+                if hand_action == self.BINARY_HAND_CLOSED:
+                    self._hand.grasp()
+                else:
+                    self._hand.open()
+        else:
+            self._hand.set_normalized_joints_poses(hand_action)
+        self._last_hand_cmd = hand_action
+        del action[self.hand_key]
+        return action

python/rcsss/envs/factories.py

@@ -15,6 +15,7 @@
     ControlMode,
     FR3Env,
     GripperWrapper,
+    HandWrapper,
     RelativeActionSpace,
     RelativeTo,
 )
@@ -35,6 +36,8 @@
     default_realsense,
     get_urdf_path,
 )
+from rcsss.hand.hand import Hand
+from rcsss.hand.tilburg_hand_control import TilburgHandControl
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -45,7 +48,7 @@ def fr3_hw_env(
     control_mode: ControlMode,
     robot_cfg: rcsss.hw.FR3Config,
     collision_guard: str | PathLike | None = None,
-    gripper_cfg: rcsss.hw.FHConfig | None = None,
+    gripper_cfg: rcsss.hw.FHConfig | rcsss.hand.interface.THConfig | None = None,
     camera_set: BaseHardwareCameraSet | None = None,
     max_relative_movement: float | tuple[float, float] | None = None,
     relative_to: RelativeTo = RelativeTo.LAST_STEP,
@@ -81,7 +84,7 @@ def fr3_hw_env(
     env: gym.Env = FR3Env(robot, ControlMode.JOINTS if collision_guard is not None else control_mode)
 
     env = FR3HW(env)
-    if gripper_cfg is not None:
+    if isinstance(gripper_cfg, rcsss.sim.FHConfig):
         gripper = rcsss.hw.FrankaHand(ip, gripper_cfg)
         env = GripperWrapper(env, gripper, binary=True)
 
@@ -161,10 +164,14 @@ def fr3_sim_env(
         camera_set = SimCameraSet(simulation, camera_set_cfg)
         env = CameraSetWrapper(env, camera_set, include_depth=True)
 
-    if gripper_cfg is not None:
+    if isinstance(gripper_cfg, rcsss.sim.FHConfig):
         gripper = sim.FrankaHand(simulation, "0", gripper_cfg)
         env = GripperWrapper(env, gripper, binary=True)
 
+    elif isinstance(gripper_cfg, rcsss.hand.interface.THConfig):
+        hand = Hand(TilburgHandControl(gripper_cfg))
+        env = HandWrapper(env, hand, binary=gripper_cfg.binary_action)
+
     if collision_guard:
         env = CollisionGuard.env_from_xml_paths(
             env,

python/rcsss/envs/space_utils.py

@@ -17,6 +17,7 @@
 Vec7Type: TypeAlias = np.ndarray[Literal[7], np.dtype[np.float64]]
 Vec3Type: TypeAlias = np.ndarray[Literal[3], np.dtype[np.float64]]
 Vec6Type: TypeAlias = np.ndarray[Literal[6], np.dtype[np.float64]]
+Vec18Type: TypeAlias = np.ndarray[Literal[18], np.dtype[np.float64]]
 
 
 class RCSpaceType(TypedDict): ...

python/rcsss/envs/utils.py

@@ -39,6 +39,10 @@ def default_fr3_hw_gripper_cfg():
     return gripper_cfg
 
 
+def default_tilburg_hw_hand_cfg():
+    return rcsss.hand.interface.THConfig()
+
+
 def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None:
     if name2id is None:
         return None

python/rcsss/hand/hand.py

@@ -0,0 +1,68 @@
+from typing import Optional
+
+
+class HandControl:
+    """
+    Base class for hand control.
+    This class provides an interface for hand control, but does not implement any functionality.
+    """
+
+    def __init__(self):
+        pass
+
+    def grasp(self, value: Optional[float] = None):
+        self.value = value  # to pass pylint
+        message = "This method should be overridden by subclasses."
+        raise NotImplementedError(message)
+
+    def set_zero_pos(self):
+        message = "This method should be overridden by subclasses."
+        raise NotImplementedError(message)
+
+    def disconnect(self):
+        message = "This method should be overridden by subclasses."
+        raise NotImplementedError(message)
+
+    def get_pos_vector(self):
+        message = "This method should be overridden by subclasses."
+        raise NotImplementedError(message)
+
+    def set_pos_vector(self, values: list[float]):
+        self.values = values  # to pass pylint
+        message = "This method should be overridden by subclasses."
+        raise NotImplementedError(message)
+
+
+class Hand:
+    """
+    Hand Class
+    This class provides an interface for hand control.
+    It allows for grasping, resetting, and disconnecting from the hand.
+    """
+
+    def __init__(self, hand: HandControl):
+        self._hand = hand
+
+    def grasp(self, value: Optional[float] = None):
+        self._hand.grasp(value)
+
+    def reset(self):
+        self._hand.set_zero_pos()
+
+    def get_state(self):
+        pass
+
+    def disconnect(self):
+        self._hand.disconnect()
+
+    def get_normalized_joints_poses(self):
+        self._hand.get_pos_vector()
+
+    def set_normalized_joints_poses(self, values: list):
+        self._hand.set_pos_vector(values)
+
+    def open(self):
+        self.reset()
+
+    def __del__(self):
+        self._hand.disconnect()

python/rcsss/hand/interface.py

@@ -0,0 +1,11 @@
+import os
+from pathlib import Path
+
+from pydantic import BaseModel
+
+
+class THConfig(BaseModel):
+    """Config for the Tilburg hand"""
+
+    binary_action: bool = True
+    calibration_file_path: str = os.path.join(Path.home(), "tilburg_hand/calibration.json")