Merge pull request #172 from utn-mi/gamal/tilburg-hand

 feat(env): tilburg hand controller integration

Author: juelg

pyproject.toml

@@ -27,6 +27,7 @@ dependencies = ["websockets>=11.0",
     "mujoco==3.2.6",
     # NOTE: Same for pinocchio (pin)
     "pin==2.7.0",
+    "tilburg-hand",
 ]
 readme = "README.md"
 maintainers = [

python/examples/env_cartesian_control_tilburg.py

@@ -0,0 +1,101 @@
+import logging
+
+from rcs._core.common import RobotPlatform
+from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator
+from rcs.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 = RobotPlatform.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 rcs fr3 unlock <ip>
+
+or put it into guiding mode using:
+
+python -m rcs fr3 guiding-mode <ip>
+
+When you are done you lock it again using:
+
+python -m rcs fr3 lock <ip>
+
+or even shut it down using:
+
+python -m rcs fr3 shutdown <ip>
+"""
+
+
+def main():
+    resource_manger: FCI | ContextManager
+    if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
+        user, pw = load_creds_fr3_desk()
+        resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
+    else:
+        resource_manger = ContextManager()
+    with resource_manger:
+        if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
+            env_rel = RCSFR3EnvCreator()(
+                ip=ROBOT_IP,
+                control_mode=ControlMode.CARTESIAN_TQuat,
+                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 = RCSSimEnvCreator()(
+                control_mode=ControlMode.CARTESIAN_TQuat,
+                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 = 0
+        open_action = 1
+
+        for _ in range(10):
+            for _ in range(10):
+                # move 1cm in x direction (forward) and close gripper
+                act = {"tquat": [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 = {"tquat": [-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/rcs/__init__.py

@@ -4,7 +4,7 @@
 import site
 
 from gymnasium import register
-from rcs import camera, control, envs, sim
+from rcs import camera, control, envs, hand, sim
 from rcs._core import __version__, common, hw
 from rcs.envs.creators import (
     FR3LabPickUpSimDigitHandEnvCreator,
@@ -20,7 +20,7 @@
 }
 
 # make submodules available
-__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"]
+__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand"]
 
 # register gymnasium environments
 register(

python/rcs/envs/base.py

@@ -14,10 +14,12 @@
     RCSpaceType,
     Vec6Type,
     Vec7Type,
+    Vec18Type,
     VecType,
     get_space,
     get_space_keys,
 )
+from rcs.hand.interface import BaseHand
 
 _logger = logging.getLogger(__name__)
 
@@ -99,6 +101,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"],
@@ -627,3 +645,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: BaseHand, binary: bool = True):
+        super().__init__(env)
+        self.unwrapped: RobotEnv
+        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/rcs/envs/creators.py

@@ -5,6 +5,7 @@
 import gymnasium as gym
 import numpy as np
 import rcs
+import rcs.hand.tilburg_hand
 from gymnasium.envs.registration import EnvCreator
 from rcs import sim
 from rcs._core.sim import CameraType
@@ -14,6 +15,7 @@
     CameraSetWrapper,
     ControlMode,
     GripperWrapper,
+    HandWrapper,
     RelativeActionSpace,
     RelativeTo,
     RobotEnv,
@@ -37,6 +39,7 @@
     default_realsense,
     get_urdf_path,
 )
+from rcs.hand.tilburg_hand import TilburgHand
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -53,7 +56,7 @@ def __call__(  # type: ignore
         control_mode: ControlMode,
         robot_cfg: rcs.hw.FR3Config,
         collision_guard: str | PathLike | None = None,
-        gripper_cfg: rcs.hw.FHConfig | None = None,
+        gripper_cfg: rcs.hw.FHConfig | rcs.hand.tilburg_hand.THConfig | None = None,
         camera_set: BaseHardwareCameraSet | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
@@ -89,9 +92,12 @@ def __call__(  # type: ignore
         env: gym.Env = RobotEnv(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, rcs.hw.FHConfig):
             gripper = rcs.hw.FrankaHand(ip, gripper_cfg)
             env = GripperWrapper(env, gripper, binary=True)
+        elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig):
+            hand = TilburgHand(gripper_cfg)
+            env = HandWrapper(env, hand, binary=True)
 
         if camera_set is not None:
             camera_set.start()
@@ -193,7 +199,7 @@ def __call__(  # type: ignore
             camera_set = SimCameraSet(simulation, camera_set_cfg)
             env = CameraSetWrapper(env, camera_set, include_depth=True)
 
-        if gripper_cfg is not None:
+        if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig):
             gripper = sim.SimGripper(simulation, "0", gripper_cfg)
             env = GripperWrapper(env, gripper, binary=True)
             env = GripperWrapperSim(env, gripper)

python/rcs/envs/space_utils.py

@@ -20,6 +20,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/rcs/envs/utils.py

@@ -11,6 +11,7 @@
 from rcs.camera.interface import BaseCameraConfig
 from rcs.camera.realsense import RealSenseCameraSet, RealSenseSetConfig
 from rcs.camera.sim import SimCameraConfig, SimCameraSetConfig
+from rcs.hand.tilburg_hand import THConfig
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -41,6 +42,13 @@ def default_fr3_hw_gripper_cfg(async_control: bool = False):
     return gripper_cfg
 
 
+def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig:
+    hand_cfg = THConfig()
+    hand_cfg.grasp_percentage = 1.0
+    hand_cfg.calibration_file = str(file) if isinstance(file, PathLike) else file
+    return hand_cfg
+
+
 def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | None:
     if name2id is None:
         return None

python/rcs/hand/__init__.py

@@ -0,0 +1,31 @@
+from typing import Protocol
+
+import numpy as np
+
+
+class BaseHand(Protocol):
+    """
+    Hand Class
+    This class provides an interface for hand control.
+    """
+
+    def grasp(self):
+        pass
+
+    def open(self):
+        pass
+
+    def reset(self):
+        pass
+
+    def close(self):
+        pass
+
+    def get_state(self) -> np.ndarray:
+        pass
+
+    def get_normalized_joints_poses(self) -> np.ndarray:
+        pass
+
+    def set_normalized_joints_poses(self, values: np.ndarray):
+        pass