diff --git a/pyproject.toml b/pyproject.toml index be9c8681..da64feac 100644 --- a/pyproject.toml +++ b/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 = [ diff --git a/python/examples/env_cartesian_control_tilburg.py b/python/examples/env_cartesian_control_tilburg.py new file mode 100644 index 00000000..052920a7 --- /dev/null +++ b/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= +FR3_PASSWORD= + +When you use a real FR3 you first need to unlock its joints using the following cli script: + +python -m rcs fr3 unlock + +or put it into guiding mode using: + +python -m rcs fr3 guiding-mode + +When you are done you lock it again using: + +python -m rcs fr3 lock + +or even shut it down using: + +python -m rcs fr3 shutdown +""" + + +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() diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index c33e6bb0..9a2e42fb 100644 --- a/python/rcs/__init__.py +++ b/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( diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 24554f3f..85558a91 100644 --- a/python/rcs/envs/base.py +++ b/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 diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 43d3e93e..84339ccb 100644 --- a/python/rcs/envs/creators.py +++ b/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) diff --git a/python/rcs/envs/space_utils.py b/python/rcs/envs/space_utils.py index 62909c53..86eb39ab 100644 --- a/python/rcs/envs/space_utils.py +++ b/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): ... diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 2701efca..47782146 100644 --- a/python/rcs/envs/utils.py +++ b/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 diff --git a/python/rcs/hand/__init__.py b/python/rcs/hand/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/rcs/hand/interface.py b/python/rcs/hand/interface.py new file mode 100644 index 00000000..2ce5b700 --- /dev/null +++ b/python/rcs/hand/interface.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 diff --git a/python/rcs/hand/tilburg_hand.py b/python/rcs/hand/tilburg_hand.py new file mode 100644 index 00000000..9af06279 --- /dev/null +++ b/python/rcs/hand/tilburg_hand.py @@ -0,0 +1,164 @@ +import copy +import logging +from time import sleep + +import numpy as np +from pydantic import BaseModel +from rcs.envs.space_utils import Vec18Type +from rcs.hand.interface import BaseHand +from tilburg_hand import Finger, TilburgHandMotorInterface, Unit + +# Setup logger +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) +logger.disabled = False + + +class THConfig(BaseModel): + """Config for the Tilburg hand""" + + calibration_file: str | None = None + grasp_percentage: float = 1.0 + control_unit: Unit = Unit.NORMALIZED + hand_orientation: str = "right" + + +class TilburgHand(BaseHand): + """ + Tilburg Hand Class + This class provides an interface for controlling the Tilburg Hand. + It allows for grasping, resetting, and disconnecting from the hand. + """ + + MAX_GRASP_JOINTS_VALS = np.array( + [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] + ) + + def __init__(self, cfg: THConfig, verbose: bool = False): + """ + Initializes the Tilburg Hand interface. + """ + self._cfg = cfg + + self._motors = TilburgHandMotorInterface( + calibration_file=self._cfg.calibration_file, hand_orientation=self._cfg.hand_orientation, verbose=verbose + ) + + re = self._motors.connect() + assert re >= 0, "Failed to connect to the motors' board." + + logger.info("Connected to the motors' board.") + + @property + def config(self): + """ + Returns the configuration of the Tilburg Hand Control. + """ + return copy.deepcopy(self._cfg) + + @config.setter + def config(self, cfg: THConfig): + """ + Sets the configuration of the Tilburg Hand Control. + """ + self._cfg = cfg + + def set_pos_vector(self, pos_vector: Vec18Type): + """ + Sets the position vector for the motors. + """ + assert len(pos_vector) == len( + self._motors.n_motors + ), f"Invalid position vector length: {len(pos_vector)}. Expected: {len(self._motors.n_motors)}" + self._motors.set_pos_vector(copy.deepcopy(pos_vector), unit=self._cfg.control_unit) + logger.info(f"Set pose vector: {pos_vector}") + + def set_zero_pos(self): + """ + Sets all finger joint positions to zero. + """ + self._motors.goto_zero_position() + logger.info("All joints reset to zero position.") + + def set_joint_pos(self, finger_joint: Finger, pos_value: float): + """ + Sets a single joint to a specific normalized position. + """ + self._motors.set_pos_single(finger_joint, pos_value, unit=self._cfg.control_unit) + + def reset_joint_pos(self, finger_joint: Finger): + """ + Resets a specific joint to zero. + """ + self._motors.set_pos_single(finger_joint, 0, unit=self._cfg.control_unit) + logger.info(f"Reset joint {finger_joint.name} to 0") + + def disconnect(self): + """ + Gracefully disconnects from the motor interface. + """ + self._motors.disconnect() + logger.info("Disconnected from the motors' board") + + def get_pos_vector(self) -> Vec18Type: + """ + Returns the current position vector of the motors. + """ + return np.array(self._motors.get_encoder_vector(self._cfg.control_unit)) + + def get_pos_single(self, finger_joint: Finger) -> float: + """ + Returns the current position of a single joint. + """ + return self._motors.get_encoder_single(finger_joint, self._cfg.control_unit) + + def _grasp(self): + pos_normalized = self._cfg.grasp_percentage * self.MAX_GRASP_JOINTS_VALS + self._motors.set_pos_vector(pos_normalized, unit=self._cfg.control_unit) + logger.info(f"Grasp command sent with value: {self._cfg.grasp_percentage:.2f}") + + def auto_recovery(self): + if not np.array(self._motors.check_enabled_motors()).all(): + logger.warning("Some motors are not enabled. Attempting to enable them.") + self._motors.disconnect() + sleep(1) + re = self._motors.connect() + assert re >= 0, "Failed to reconnect to the motors' board." + + #### BaseHandControl Interface methods #### + + def grasp(self): + """ + Performs a grasp with a specified intensity (0.0 to 1.0). + """ + self._grasp() + + def open(self): + self.set_zero_pos() + + def reset(self): + """ + Resets the hand to its initial state. + """ + self.auto_recovery() + self.set_zero_pos() + logger.info("Hand reset to initial state.") + + def get_state(self) -> np.ndarray: + """ + Returns the current state of the hand. + """ + return self.get_pos_vector() + + def close(self): + """ + Closes the hand control interface. + """ + self.disconnect() + logger.info("Hand control interface closed.") + + def get_normalized_joints_poses(self) -> np.ndarray: + return self.get_pos_vector() + + def set_normalized_joints_poses(self, values: np.ndarray): + self.set_pos_vector(values)