From de9970020e1f9fa9697d98779782d3cac86a67fc Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Tue, 13 May 2025 20:21:44 +0200 Subject: [PATCH 01/10] feat(examples digit_cam envs): working on digit integration --- python/examples/env_cartesian_control.py | 1 - .../examples/env_cartesian_control_digit.py | 100 ++++++++++++++++++ python/rcsss/digit_cam/__init__.py | 0 python/rcsss/digit_cam/digit_cam.py | 41 +++++++ python/rcsss/digit_cam/interface.py | 21 ++++ python/rcsss/envs/base.py | 69 ++++++++++++ python/rcsss/envs/factories.py | 13 ++- python/rcsss/envs/utils.py | 2 + requirements_dev.txt | 1 + 9 files changed, 245 insertions(+), 3 deletions(-) create mode 100644 python/examples/env_cartesian_control_digit.py create mode 100644 python/rcsss/digit_cam/__init__.py create mode 100644 python/rcsss/digit_cam/digit_cam.py create mode 100644 python/rcsss/digit_cam/interface.py diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index d8e9c5f7..0be49ab5 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -92,6 +92,5 @@ def main(): logger.info("Truncated or terminated!") return - if __name__ == "__main__": main() diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py new file mode 100644 index 00000000..19cfb5bb --- /dev/null +++ b/python/examples/env_cartesian_control_digit.py @@ -0,0 +1,100 @@ +import logging + +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_gripper_cfg, + default_fr3_hw_robot_cfg, + default_fr3_sim_gripper_cfg, + default_fr3_sim_robot_cfg, + default_mujoco_cameraset_cfg, + default_digit_cam_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= +FR3_PASSWORD= + +When you use a real FR3 you first need to unlock its joints using the following cli script: + +python -m rcsss fr3 unlock + +or put it into guiding mode using: + +python -m rcsss fr3 guiding-mode + +When you are done you lock it again using: + +python -m rcsss fr3 lock + +or even shut it down using: + +python -m rcsss fr3 shutdown +""" + + +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_fr3_hw_gripper_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(), + digit_set_cfg= default_digit_cam_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 + + 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], "gripper": 0} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + for _ in range(10): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + env_rel.close() + + +if __name__ == "__main__": + main() diff --git a/python/rcsss/digit_cam/__init__.py b/python/rcsss/digit_cam/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/rcsss/digit_cam/digit_cam.py b/python/rcsss/digit_cam/digit_cam.py new file mode 100644 index 00000000..87efa107 --- /dev/null +++ b/python/rcsss/digit_cam/digit_cam.py @@ -0,0 +1,41 @@ +from rcsss.camera.hw import BaseHardwareCameraSet, BaseCameraSetConfig, HWCameraSetConfig +from rcsss.camera.interface import Frame, DataFrame, CameraFrame +from rcsss.digit_cam.interface import DigitConfig +from digit_interface.digit import Digit + +class DigitCam(BaseHardwareCameraSet): + """ + This module provides an interface to interact with the DIGIT device. + It allows for connecting to the device, changing settings, and retrieving information. + """ + + def __init__(self, cfg: DigitConfig): + self.config = cfg + super().__init__() + self._cameras: dict[str, Digit] = {} + # Initialize the digit interface + self.initalize(self.config) + + def initalize(self, cfg: BaseCameraSetConfig): + """ + Initialize the digit interface with the given configuration. + :param cfg: Configuration for the DIGIT device. + """ + for name, serial in cfg.cameras.items(): + digit = Digit(serial, name) + digit.connect() + self._cameras[name] = digit + + def _poll_frame(self, camera_name: str) -> Frame: + """Polls the frame from the camera with the given name.""" + digit = self._cameras[camera_name] + frame = digit.get_frame() + color = DataFrame(data=frame) + # rgb to bgr as expected by opencv + # color = DataFrame(data=frame[:, :, ::-1]) + cf = CameraFrame(color=color) + + return Frame(camera=cf) + + def config(self) -> HWCameraSetConfig: + return self.config \ No newline at end of file diff --git a/python/rcsss/digit_cam/interface.py b/python/rcsss/digit_cam/interface.py new file mode 100644 index 00000000..c695be40 --- /dev/null +++ b/python/rcsss/digit_cam/interface.py @@ -0,0 +1,21 @@ +from rcsss.camera.hw import HWCameraSetConfig +from digit_interface.digit import Digit + +class DigitConfig(HWCameraSetConfig): + """ + Configuration for the DIGIT device. + This class is used to define the settings for the DIGIT device. + """ + cameras: dict[str, str] ={ + "camera1": "D21182", + } + stream: dict = Digit.STREAMS["QVGA"] # QVGA resolution or VGA resolution + resolution_width: int = stream["resolution"]["width"] + resolution_height: int = stream["resolution"]["height"] + # for QVGA resolution, 60 fps is the default or 30 fps + # for VGA resolution, 30 fps is the default or 15 fps + frame_rate: int = stream["fps"]["30fps"] # 30 fps + + @property + def name_to_identifier(self): + return {key: serial for key, serial in self.cameras.items()} \ No newline at end of file diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index a6b553ae..0094ca07 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -7,6 +7,7 @@ import gymnasium as gym import numpy as np +import rcsss from rcsss import common, sim from rcsss.camera.interface import BaseCameraSet from rcsss.envs.space_utils import ( @@ -134,6 +135,24 @@ class CameraDictType(RCSpaceType): ], ] +class DigitCameraDictType(RCSpaceType): + digit_frame: dict[ + Annotated[str, "camera_names"], + dict[ + Annotated[str, "camera_type"], # "rgb" or "depth" + Annotated[ + np.ndarray, + # needs to be filled with values downstream + lambda height, width, color_dim=3, dtype=np.uint8, low=0, high=255: gym.spaces.Box( + low=low, + high=high, + shape=(height, width, color_dim), + dtype=dtype, + ), + "digit_frame", + ], + ], + ] # joining works with inheritance but need to inherit from protocol again class ArmObsType(TQuartDictType, JointsDictType, TRPYDictType): ... @@ -532,12 +551,62 @@ def check_depth(depth): info["camera_available"] = True if frameset.avg_timestamp is not None: info["frame_timestamp"] = frameset.avg_timestamp + + if isinstance(self.camera_set, rcsss.digit_cam.digit_cam.DigitCam): #TODO reomve after debugging + pass return observation, info def close(self): self.camera_set.close() super().close() +class DigitCameraSetWrapper(CameraSetWrapper): + RGB_KEY = "rgb" + DEPTH_KEY = "depth" + + def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False): + super().__init__(env, camera_set, include_depth) + self.unwrapped: FR3Env + self.camera_set = camera_set + self.include_depth = include_depth + + self.observation_space: gym.spaces.Dict + # rgb is always included + params: dict = { + "digit_frame": + { + "height": camera_set.config.resolution_height, + "width": camera_set.config.resolution_width, + } + } + + if self.include_depth: + # depth is optional + params.update( + { + f"/{name}/{self.DEPTH_KEY}/frame": { + "height": camera_set.config.resolution_height, + "width": camera_set.config.resolution_width, + "color_dim": 1, + "dtype": np.float32, + "low": 0.0, + "high": 1.0, + } + for name in camera_set.camera_names + } + ) + self.observation_space.spaces.update( + get_space( + DigitCameraDictType, + child_dict_keys_to_unfold={ + "camera_names": camera_set.camera_names, + "camera_type": [self.RGB_KEY, self.DEPTH_KEY] if self.include_depth else [self.RGB_KEY], + }, + params=params, + ).spaces + ) + self.camera_key = get_space_keys(DigitCameraDictType)[0] + class GripperWrapper(ActObsInfoWrapper): # TODO: sticky gripper, like in aloha diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 3fc34093..0ee285b1 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -12,6 +12,7 @@ from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcsss.envs.base import ( CameraSetWrapper, + DigitCameraSetWrapper, ControlMode, FR3Env, GripperWrapper, @@ -38,7 +39,7 @@ ) from rcsss.hand.hand import Hand from rcsss.hand.tilburg_hand_control import TilburgHandControl - +from rcsss.digit_cam.digit_cam import DigitCam logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -119,6 +120,7 @@ def fr3_sim_env( collision_guard: bool = False, gripper_cfg: rcsss.sim.FHConfig | None = None, camera_set_cfg: SimCameraSetConfig | None = None, + digit_set_cfg: rcsss.digit_cam.interface.DigitConfig | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, urdf_path: str | PathLike | None = None, @@ -163,7 +165,14 @@ def fr3_sim_env( if camera_set_cfg is not None: camera_set = SimCameraSet(simulation, camera_set_cfg) env = CameraSetWrapper(env, camera_set, include_depth=True) - + + if digit_set_cfg is not None: + digit_cam = DigitCam(digit_set_cfg) + digit_cam.start() + digit_cam.wait_for_frames() + logger.info("CameraSet started") + env = DigitCameraSetWrapper(env, digit_cam) + if isinstance(gripper_cfg, rcsss.sim.FHConfig): gripper = sim.FrankaHand(simulation, "0", gripper_cfg) env = GripperWrapper(env, gripper, binary=True) diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index 43a17074..1358546a 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -62,6 +62,8 @@ def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | No def default_fr3_sim_gripper_cfg(): return sim.FHConfig() +def default_digit_cam_cfg(): + return rcsss.digit_cam.interface.DigitConfig() def default_mujoco_cameraset_cfg(): cameras = { diff --git a/requirements_dev.txt b/requirements_dev.txt index 7851d1de..1cc4354d 100644 --- a/requirements_dev.txt +++ b/requirements_dev.txt @@ -13,3 +13,4 @@ mujoco==3.2.6 pin==2.7.0 wheel tilburg-hand +digit-interface From b91874fcf89f4fc29e418419e72c3345a972aad9 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 22 May 2025 15:37:59 +0200 Subject: [PATCH 02/10] refacror(examples envs digit_cam): refactor the digit_cam feature to be consistent with the latest modifications in the master branch --- .../examples/env_cartesian_control_digit.py | 47 +++++++++---------- python/{rcsss => rcs}/digit_cam/__init__.py | 0 python/{rcsss => rcs}/digit_cam/digit_cam.py | 25 ++++++++-- python/rcs/envs/base.py | 3 +- python/rcs/envs/creators.py | 10 ++++ python/rcs/envs/utils.py | 4 +- python/rcsss/digit_cam/interface.py | 21 --------- 7 files changed, 59 insertions(+), 51 deletions(-) rename python/{rcsss => rcs}/digit_cam/__init__.py (100%) rename python/{rcsss => rcs}/digit_cam/digit_cam.py (59%) delete mode 100644 python/rcsss/digit_cam/interface.py diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py index 19cfb5bb..193081ff 100644 --- a/python/examples/env_cartesian_control_digit.py +++ b/python/examples/env_cartesian_control_digit.py @@ -1,23 +1,22 @@ import logging -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_gripper_cfg, +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_digit_cam_cfg + default_digit_cam_cfg, ) logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotInstance.SIMULATION +ROBOT_INSTANCE = RobotPlatform.SIMULATION """ @@ -27,44 +26,43 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcsss fr3 unlock +python -m rcs fr3 unlock or put it into guiding mode using: -python -m rcsss fr3 guiding-mode +python -m rcs fr3 guiding-mode When you are done you lock it again using: -python -m rcsss fr3 lock +python -m rcs fr3 lock or even shut it down using: -python -m rcsss fr3 shutdown +python -m rcs fr3 shutdown """ def main(): - resource_manger: FCI | DummyResourceManager - if ROBOT_INSTANCE == RobotInstance.HARDWARE: + 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 = DummyResourceManager() - + resource_manger = ContextManager() with resource_manger: - if ROBOT_INSTANCE == RobotInstance.HARDWARE: - env_rel = fr3_hw_env( + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSFR3EnvCreator()( ip=ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TQuart, + control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", - gripper_cfg=default_fr3_hw_gripper_cfg(), + gripper_cfg=default_fr3_hw_robot_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = fr3_sim_env( - control_mode=ControlMode.CARTESIAN_TQuart, + 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(), @@ -81,20 +79,19 @@ def main(): 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], "gripper": 0} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return env_rel.close() - if __name__ == "__main__": main() diff --git a/python/rcsss/digit_cam/__init__.py b/python/rcs/digit_cam/__init__.py similarity index 100% rename from python/rcsss/digit_cam/__init__.py rename to python/rcs/digit_cam/__init__.py diff --git a/python/rcsss/digit_cam/digit_cam.py b/python/rcs/digit_cam/digit_cam.py similarity index 59% rename from python/rcsss/digit_cam/digit_cam.py rename to python/rcs/digit_cam/digit_cam.py index 87efa107..a79ea678 100644 --- a/python/rcsss/digit_cam/digit_cam.py +++ b/python/rcs/digit_cam/digit_cam.py @@ -1,8 +1,27 @@ -from rcsss.camera.hw import BaseHardwareCameraSet, BaseCameraSetConfig, HWCameraSetConfig -from rcsss.camera.interface import Frame, DataFrame, CameraFrame -from rcsss.digit_cam.interface import DigitConfig +from rcs.camera.hw import BaseHardwareCameraSet, BaseCameraSetConfig, HWCameraSetConfig +from rcs.camera.interface import Frame, DataFrame, CameraFrame from digit_interface.digit import Digit + +class DigitConfig(HWCameraSetConfig): + """ + Configuration for the DIGIT device. + This class is used to define the settings for the DIGIT device. + """ + cameras: dict[str, str] ={ + "camera1": "D21182", + } + stream: dict = Digit.STREAMS["QVGA"] # QVGA resolution or VGA resolution + resolution_width: int = stream["resolution"]["width"] + resolution_height: int = stream["resolution"]["height"] + # for QVGA resolution, 60 fps is the default or 30 fps + # for VGA resolution, 30 fps is the default or 15 fps + frame_rate: int = stream["fps"]["30fps"] # 30 fps + + @property + def name_to_identifier(self): + return {key: serial for key, serial in self.cameras.items()} + class DigitCam(BaseHardwareCameraSet): """ This module provides an interface to interact with the DIGIT device. diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index bdc35ebb..34c0c5e9 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -7,6 +7,7 @@ import gymnasium as gym import numpy as np +import rcs from rcs import common from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( @@ -608,7 +609,7 @@ def check_depth(depth): if frameset.avg_timestamp is not None: info["frame_timestamp"] = frameset.avg_timestamp - if isinstance(self.camera_set, rcsss.digit_cam.digit_cam.DigitCam): #TODO reomve after debugging + if isinstance(self.camera_set, rcs.digit_cam.digit_cam.DigitCam): #TODO reomve after debugging pass return observation, info diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 38aa2784..4416b038 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -13,6 +13,7 @@ from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcs.envs.base import ( CameraSetWrapper, + DigitCameraSetWrapper, ControlMode, GripperWrapper, HandWrapper, @@ -40,6 +41,7 @@ get_urdf_path, ) from rcs.hand.tilburg_hand import TilburgHand +from rcs.digit_cam.digit_cam import DigitCam logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -154,6 +156,7 @@ def __call__( # type: ignore collision_guard: bool = False, gripper_cfg: rcs.sim.SimGripperConfig | None = None, camera_set_cfg: SimCameraSetConfig | None = None, + digit_set_cfg: rcs.digit_cam.digit_cam.DigitConfig | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, urdf_path: str | PathLike | None = None, @@ -198,6 +201,13 @@ def __call__( # type: ignore camera_set = SimCameraSet(simulation, camera_set_cfg) env = CameraSetWrapper(env, camera_set, include_depth=True) + if digit_set_cfg is not None: + digit_cam = DigitCam(digit_set_cfg) + digit_cam.start() + digit_cam.wait_for_frames() + logger.info("DigitCameraSet started") + env = DigitCameraSetWrapper(env, digit_cam) + if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): gripper = sim.SimGripper(simulation, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 807dba00..8154bbeb 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -11,6 +11,8 @@ from rcs.camera.interface import BaseCameraConfig from rcs.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcs.camera.sim import SimCameraConfig, SimCameraSetConfig +import rcs.digit_cam +import rcs.digit_cam.digit_cam from rcs.hand.tilburg_hand import THConfig logger = logging.getLogger(__name__) @@ -73,7 +75,7 @@ def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: return cfg def default_digit_cam_cfg(): - return rcsss.digit_cam.interface.DigitConfig() + return rcs.digit_cam.digit_cam.DigitConfig() def default_mujoco_cameraset_cfg() -> SimCameraSetConfig: cameras = { diff --git a/python/rcsss/digit_cam/interface.py b/python/rcsss/digit_cam/interface.py deleted file mode 100644 index c695be40..00000000 --- a/python/rcsss/digit_cam/interface.py +++ /dev/null @@ -1,21 +0,0 @@ -from rcsss.camera.hw import HWCameraSetConfig -from digit_interface.digit import Digit - -class DigitConfig(HWCameraSetConfig): - """ - Configuration for the DIGIT device. - This class is used to define the settings for the DIGIT device. - """ - cameras: dict[str, str] ={ - "camera1": "D21182", - } - stream: dict = Digit.STREAMS["QVGA"] # QVGA resolution or VGA resolution - resolution_width: int = stream["resolution"]["width"] - resolution_height: int = stream["resolution"]["height"] - # for QVGA resolution, 60 fps is the default or 30 fps - # for VGA resolution, 30 fps is the default or 15 fps - frame_rate: int = stream["fps"]["30fps"] # 30 fps - - @property - def name_to_identifier(self): - return {key: serial for key, serial in self.cameras.items()} \ No newline at end of file From 485911d272d5ca0b162883e96396204fb46ddc97 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 22 May 2025 17:03:32 +0200 Subject: [PATCH 03/10] refacror(examples envs digit_cam): pass pycheckformat, pylint, pytest --- python/examples/env_cartesian_control.py | 1 + .../examples/env_cartesian_control_digit.py | 8 +++-- python/rcs/digit_cam/digit_cam.py | 36 +++++++++++-------- python/rcs/envs/base.py | 22 ++++++------ python/rcs/envs/creators.py | 4 +-- python/rcs/envs/utils.py | 6 ++-- 6 files changed, 46 insertions(+), 31 deletions(-) diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index f8fee641..a544d7a9 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -92,5 +92,6 @@ def main(): logger.info("Truncated or terminated!") return + if __name__ == "__main__": main() diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py index 193081ff..267fdcb2 100644 --- a/python/examples/env_cartesian_control_digit.py +++ b/python/examples/env_cartesian_control_digit.py @@ -5,11 +5,12 @@ from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator from rcs.envs.utils import ( + default_digit_cam_cfg, + default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, - default_digit_cam_cfg, ) logger = logging.getLogger(__name__) @@ -56,7 +57,7 @@ def main(): control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", - gripper_cfg=default_fr3_hw_robot_cfg(), + gripper_cfg=default_fr3_hw_gripper_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) @@ -67,7 +68,7 @@ def main(): collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=default_mujoco_cameraset_cfg(), - digit_set_cfg= default_digit_cam_cfg(), + digit_set_cfg=default_digit_cam_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) @@ -93,5 +94,6 @@ def main(): return env_rel.close() + if __name__ == "__main__": main() diff --git a/python/rcs/digit_cam/digit_cam.py b/python/rcs/digit_cam/digit_cam.py index a79ea678..bdc50ff6 100644 --- a/python/rcs/digit_cam/digit_cam.py +++ b/python/rcs/digit_cam/digit_cam.py @@ -1,6 +1,7 @@ -from rcs.camera.hw import BaseHardwareCameraSet, BaseCameraSetConfig, HWCameraSetConfig -from rcs.camera.interface import Frame, DataFrame, CameraFrame from digit_interface.digit import Digit +from pydantic import Field +from rcs.camera.hw import BaseHardwareCameraSet, HWCameraSetConfig +from rcs.camera.interface import BaseCameraConfig, CameraFrame, DataFrame, Frame class DigitConfig(HWCameraSetConfig): @@ -8,10 +9,11 @@ class DigitConfig(HWCameraSetConfig): Configuration for the DIGIT device. This class is used to define the settings for the DIGIT device. """ - cameras: dict[str, str] ={ - "camera1": "D21182", - } - stream: dict = Digit.STREAMS["QVGA"] # QVGA resolution or VGA resolution + + cameras: dict[str, BaseCameraConfig] = Field( + default_factory=lambda: {"camera1": BaseCameraConfig(identifier="D21182")} + ) + stream: dict = Digit.STREAMS["QVGA"] # QVGA resolution or VGA resolution resolution_width: int = stream["resolution"]["width"] resolution_height: int = stream["resolution"]["height"] # for QVGA resolution, 60 fps is the default or 30 fps @@ -20,7 +22,8 @@ class DigitConfig(HWCameraSetConfig): @property def name_to_identifier(self): - return {key: serial for key, serial in self.cameras.items()} + return dict(self.cameras) + class DigitCam(BaseHardwareCameraSet): """ @@ -29,19 +32,19 @@ class DigitCam(BaseHardwareCameraSet): """ def __init__(self, cfg: DigitConfig): - self.config = cfg + self._cfg = cfg super().__init__() self._cameras: dict[str, Digit] = {} # Initialize the digit interface self.initalize(self.config) - def initalize(self, cfg: BaseCameraSetConfig): + def initalize(self, cfg: HWCameraSetConfig): """ Initialize the digit interface with the given configuration. :param cfg: Configuration for the DIGIT device. """ for name, serial in cfg.cameras.items(): - digit = Digit(serial, name) + digit = Digit(serial.identifier, name) digit.connect() self._cameras[name] = digit @@ -49,12 +52,17 @@ def _poll_frame(self, camera_name: str) -> Frame: """Polls the frame from the camera with the given name.""" digit = self._cameras[camera_name] frame = digit.get_frame() - color = DataFrame(data=frame) + color = DataFrame(data=frame) # rgb to bgr as expected by opencv - # color = DataFrame(data=frame[:, :, ::-1]) + # color = DataFrame(data=frame[:, :, ::-1]) cf = CameraFrame(color=color) return Frame(camera=cf) - def config(self) -> HWCameraSetConfig: - return self.config \ No newline at end of file + @property + def config(self) -> DigitConfig: + return self._cfg + + @config.setter + def config(self, cfg: DigitConfig) -> None: + self._cfg = cfg diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 34c0c5e9..b512f386 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -137,6 +137,7 @@ class CameraDictType(RCSpaceType): ], ] + class DigitCameraDictType(RCSpaceType): digit_frame: dict[ Annotated[str, "camera_names"], @@ -156,6 +157,7 @@ class DigitCameraDictType(RCSpaceType): ], ] + # joining works with inheritance but need to inherit from protocol again class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ... @@ -608,34 +610,34 @@ def check_depth(depth): info["camera_available"] = True if frameset.avg_timestamp is not None: info["frame_timestamp"] = frameset.avg_timestamp - - if isinstance(self.camera_set, rcs.digit_cam.digit_cam.DigitCam): #TODO reomve after debugging - pass + + if isinstance(self.camera_set, rcs.digit_cam.digit_cam.DigitCam): # TODO reomve after debugging + pass return observation, info def close(self): self.camera_set.close() super().close() + class DigitCameraSetWrapper(CameraSetWrapper): RGB_KEY = "rgb" DEPTH_KEY = "depth" def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False): super().__init__(env, camera_set, include_depth) - self.unwrapped: FR3Env + # self.unwrapped: FR3Env self.camera_set = camera_set self.include_depth = include_depth self.observation_space: gym.spaces.Dict # rgb is always included params: dict = { - "digit_frame": - { - "height": camera_set.config.resolution_height, - "width": camera_set.config.resolution_width, - } - } + "digit_frame": { + "height": camera_set.config.resolution_height, + "width": camera_set.config.resolution_width, + } + } if self.include_depth: # depth is optional diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 4416b038..f3ed089c 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -11,10 +11,11 @@ from rcs._core.sim import CameraType from rcs.camera.hw import BaseHardwareCameraSet from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcs.digit_cam.digit_cam import DigitCam from rcs.envs.base import ( CameraSetWrapper, - DigitCameraSetWrapper, ControlMode, + DigitCameraSetWrapper, GripperWrapper, HandWrapper, RelativeActionSpace, @@ -41,7 +42,6 @@ get_urdf_path, ) from rcs.hand.tilburg_hand import TilburgHand -from rcs.digit_cam.digit_cam import DigitCam logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 8154bbeb..b6a9eb8c 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -5,14 +5,14 @@ import mujoco as mj import numpy as np import rcs +import rcs.digit_cam +import rcs.digit_cam.digit_cam from rcs import sim from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType from rcs.camera.interface import BaseCameraConfig from rcs.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcs.camera.sim import SimCameraConfig, SimCameraSetConfig -import rcs.digit_cam -import rcs.digit_cam.digit_cam from rcs.hand.tilburg_hand import THConfig logger = logging.getLogger(__name__) @@ -74,9 +74,11 @@ def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg.add_id(idx) return cfg + def default_digit_cam_cfg(): return rcs.digit_cam.digit_cam.DigitConfig() + def default_mujoco_cameraset_cfg() -> SimCameraSetConfig: cameras = { "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), From ade1a2f58a46a4d2e43100532c028ab805456122 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 22 May 2025 17:09:28 +0200 Subject: [PATCH 04/10] feat(pyproject.toml): add digit-interface to pyproject.toml --- pyproject.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/pyproject.toml b/pyproject.toml index da64feac..b4107988 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -28,6 +28,7 @@ dependencies = ["websockets>=11.0", # NOTE: Same for pinocchio (pin) "pin==2.7.0", "tilburg-hand", + "digit-interface", ] readme = "README.md" maintainers = [ From 50a9d3848a4b5c5b6091637722c74778410c0686 Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 22 May 2025 18:15:36 +0200 Subject: [PATCH 05/10] fix(envs): - change digit_frame key in observation to digit_frames - remove rgb key from digit_frames --- python/rcs/envs/base.py | 57 +++++++++++++++++++---------------------- 1 file changed, 27 insertions(+), 30 deletions(-) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index b512f386..e12e27ac 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -139,10 +139,8 @@ class CameraDictType(RCSpaceType): class DigitCameraDictType(RCSpaceType): - digit_frame: dict[ + digit_frames: dict[ Annotated[str, "camera_names"], - dict[ - Annotated[str, "camera_type"], # "rgb" or "depth" Annotated[ np.ndarray, # needs to be filled with values downstream @@ -152,10 +150,10 @@ class DigitCameraDictType(RCSpaceType): shape=(height, width, color_dim), dtype=dtype, ), - "digit_frame", + "digit_frames", ], - ], - ] + ] + # joining works with inheritance but need to inherit from protocol again @@ -611,8 +609,6 @@ def check_depth(depth): if frameset.avg_timestamp is not None: info["frame_timestamp"] = frameset.avg_timestamp - if isinstance(self.camera_set, rcs.digit_cam.digit_cam.DigitCam): # TODO reomve after debugging - pass return observation, info def close(self): @@ -621,51 +617,52 @@ def close(self): class DigitCameraSetWrapper(CameraSetWrapper): - RGB_KEY = "rgb" - DEPTH_KEY = "depth" + """Wrapper for digit cameras.""" def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False): - super().__init__(env, camera_set, include_depth) + super().__init__(env, camera_set) # self.unwrapped: FR3Env self.camera_set = camera_set - self.include_depth = include_depth - self.observation_space: gym.spaces.Dict # rgb is always included params: dict = { - "digit_frame": { + "digit_frames": { "height": camera_set.config.resolution_height, "width": camera_set.config.resolution_width, } } - if self.include_depth: - # depth is optional - params.update( - { - f"/{name}/{self.DEPTH_KEY}/frame": { - "height": camera_set.config.resolution_height, - "width": camera_set.config.resolution_width, - "color_dim": 1, - "dtype": np.float32, - "low": 0.0, - "high": 1.0, - } - for name in camera_set.camera_names - } - ) self.observation_space.spaces.update( get_space( DigitCameraDictType, child_dict_keys_to_unfold={ "camera_names": camera_set.camera_names, - "camera_type": [self.RGB_KEY, self.DEPTH_KEY] if self.include_depth else [self.RGB_KEY], }, params=params, ).spaces ) self.camera_key = get_space_keys(DigitCameraDictType)[0] + def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + observation = copy.deepcopy(observation) + info = copy.deepcopy(info) + frameset = self.camera_set.get_latest_frames() + if frameset is None: + observation[self.camera_key] = {} + info["camera_available"] = False + return observation, info + + frame_dict: dict[str, np.ndarray] = { + camera_name: frame.camera.color.data + for camera_name, frame in frameset.frames.items() + } + observation[self.camera_key] = frame_dict + + info["camera_available"] = True + if frameset.avg_timestamp is not None: + info["frame_timestamp"] = frameset.avg_timestamp + + return observation, info class GripperWrapper(ActObsInfoWrapper): # TODO: sticky gripper, like in aloha From e20ff21a4e3db41b9f5207f5a2693389048dd88f Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 22 May 2025 18:18:30 +0200 Subject: [PATCH 06/10] refacror(envs): pass pycheckformat, pylint, pytest --- python/rcs/envs/base.py | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index e12e27ac..f38c0c90 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -7,7 +7,6 @@ import gymnasium as gym import numpy as np -import rcs from rcs import common from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( @@ -141,19 +140,18 @@ class CameraDictType(RCSpaceType): class DigitCameraDictType(RCSpaceType): digit_frames: dict[ Annotated[str, "camera_names"], - Annotated[ - np.ndarray, - # needs to be filled with values downstream - lambda height, width, color_dim=3, dtype=np.uint8, low=0, high=255: gym.spaces.Box( - low=low, - high=high, - shape=(height, width, color_dim), - dtype=dtype, - ), - "digit_frames", - ], - ] - + Annotated[ + np.ndarray, + # needs to be filled with values downstream + lambda height, width, color_dim=3, dtype=np.uint8, low=0, high=255: gym.spaces.Box( + low=low, + high=high, + shape=(height, width, color_dim), + dtype=dtype, + ), + "digit_frames", + ], + ] # joining works with inheritance but need to inherit from protocol again @@ -619,7 +617,7 @@ def close(self): class DigitCameraSetWrapper(CameraSetWrapper): """Wrapper for digit cameras.""" - def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False): + def __init__(self, env, camera_set: BaseCameraSet): super().__init__(env, camera_set) # self.unwrapped: FR3Env self.camera_set = camera_set @@ -653,8 +651,7 @@ def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str return observation, info frame_dict: dict[str, np.ndarray] = { - camera_name: frame.camera.color.data - for camera_name, frame in frameset.frames.items() + camera_name: frame.camera.color.data for camera_name, frame in frameset.frames.items() } observation[self.camera_key] = frame_dict @@ -664,6 +661,7 @@ def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str return observation, info + class GripperWrapper(ActObsInfoWrapper): # TODO: sticky gripper, like in aloha From 981d76edc4a3483d5a28fed98dc93dbf1c958c7e Mon Sep 17 00:00:00 2001 From: khaledmohamed00 Date: Thu, 22 May 2025 18:25:41 +0200 Subject: [PATCH 07/10] fix(examples envs): make digit_cam work with hardware instead of simulation --- python/examples/env_cartesian_control_digit.py | 2 +- python/rcs/envs/creators.py | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py index 267fdcb2..1ef4c2a4 100644 --- a/python/examples/env_cartesian_control_digit.py +++ b/python/examples/env_cartesian_control_digit.py @@ -58,6 +58,7 @@ def main(): robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", gripper_cfg=default_fr3_hw_gripper_cfg(), + digit_set_cfg=default_digit_cam_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) @@ -68,7 +69,6 @@ def main(): collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=default_mujoco_cameraset_cfg(), - digit_set_cfg=default_digit_cam_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index f3ed089c..0abe46db 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -60,6 +60,7 @@ def __call__( # type: ignore collision_guard: str | PathLike | None = None, gripper_cfg: rcs.hw.FHConfig | rcs.hand.tilburg_hand.THConfig | None = None, camera_set: BaseHardwareCameraSet | None = None, + digit_set_cfg: rcs.digit_cam.digit_cam.DigitConfig | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, urdf_path: str | PathLike | None = None, @@ -107,6 +108,13 @@ def __call__( # type: ignore logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) + if digit_set_cfg is not None: + digit_cam = DigitCam(digit_set_cfg) + digit_cam.start() + digit_cam.wait_for_frames() + logger.info("DigitCameraSet started") + env = DigitCameraSetWrapper(env, digit_cam) + if collision_guard is not None: assert urdf_path is not None env = CollisionGuard.env_from_xml_paths( @@ -156,7 +164,6 @@ def __call__( # type: ignore collision_guard: bool = False, gripper_cfg: rcs.sim.SimGripperConfig | None = None, camera_set_cfg: SimCameraSetConfig | None = None, - digit_set_cfg: rcs.digit_cam.digit_cam.DigitConfig | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, urdf_path: str | PathLike | None = None, @@ -201,13 +208,6 @@ def __call__( # type: ignore camera_set = SimCameraSet(simulation, camera_set_cfg) env = CameraSetWrapper(env, camera_set, include_depth=True) - if digit_set_cfg is not None: - digit_cam = DigitCam(digit_set_cfg) - digit_cam.start() - digit_cam.wait_for_frames() - logger.info("DigitCameraSet started") - env = DigitCameraSetWrapper(env, digit_cam) - if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): gripper = sim.SimGripper(simulation, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) From f50802e618d00314a91d48460d30e204c47a323a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 2 Jun 2025 16:16:21 +0200 Subject: [PATCH 08/10] chore: clean up digit code - factory produces digit class and not config - cleaned digit config and adapted to existing camera logic; moved example value to example script - removed set_config as it would need to reinitialize the cameras - cleaned digit import paths - added digit to exported modules - adapted info dict to represent digit explicitly --- .../examples/env_cartesian_control_digit.py | 4 +-- python/rcs/__init__.py | 2 +- python/rcs/digit_cam/digit_cam.py | 30 ++++++++----------- python/rcs/envs/base.py | 7 ++--- python/rcs/envs/creators.py | 11 ++++--- python/rcs/envs/utils.py | 17 ++++++++--- 6 files changed, 36 insertions(+), 35 deletions(-) diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py index 1ef4c2a4..73496909 100644 --- a/python/examples/env_cartesian_control_digit.py +++ b/python/examples/env_cartesian_control_digit.py @@ -5,7 +5,7 @@ from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator from rcs.envs.utils import ( - default_digit_cam_cfg, + default_digit, default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, @@ -58,7 +58,7 @@ def main(): robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", gripper_cfg=default_fr3_hw_gripper_cfg(), - digit_set_cfg=default_digit_cam_cfg(), + digit_cfg=default_digit({"digit_0": "D21182"}), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 9a2e42fb..9bf09940 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -20,7 +20,7 @@ } # make submodules available -__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand"] +__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand", "digit_cam"] # register gymnasium environments register( diff --git a/python/rcs/digit_cam/digit_cam.py b/python/rcs/digit_cam/digit_cam.py index bdc50ff6..267ad200 100644 --- a/python/rcs/digit_cam/digit_cam.py +++ b/python/rcs/digit_cam/digit_cam.py @@ -10,19 +10,18 @@ class DigitConfig(HWCameraSetConfig): This class is used to define the settings for the DIGIT device. """ - cameras: dict[str, BaseCameraConfig] = Field( - default_factory=lambda: {"camera1": BaseCameraConfig(identifier="D21182")} - ) - stream: dict = Digit.STREAMS["QVGA"] # QVGA resolution or VGA resolution - resolution_width: int = stream["resolution"]["width"] - resolution_height: int = stream["resolution"]["height"] - # for QVGA resolution, 60 fps is the default or 30 fps - # for VGA resolution, 30 fps is the default or 15 fps - frame_rate: int = stream["fps"]["30fps"] # 30 fps + cameras: dict[str, BaseCameraConfig] = Field(default={}) + stream_name: str = "QVGA" # options: "QVGA" (60 and 30 fps), "VGA" (30 and 15 fps) @property - def name_to_identifier(self): - return dict(self.cameras) + def resolution_width(self) -> int: + return Digit.STREAMS[self.stream_name]["resolution"]["width"] + + @property + def resolution_height(self) -> int: + return Digit.STREAMS[self.stream_name]["resolution"]["height"] + + class DigitCam(BaseHardwareCameraSet): @@ -35,7 +34,6 @@ def __init__(self, cfg: DigitConfig): self._cfg = cfg super().__init__() self._cameras: dict[str, Digit] = {} - # Initialize the digit interface self.initalize(self.config) def initalize(self, cfg: HWCameraSetConfig): @@ -43,8 +41,8 @@ def initalize(self, cfg: HWCameraSetConfig): Initialize the digit interface with the given configuration. :param cfg: Configuration for the DIGIT device. """ - for name, serial in cfg.cameras.items(): - digit = Digit(serial.identifier, name) + for name, serial in cfg.name_to_identifier.items(): + digit = Digit(serial, name) digit.connect() self._cameras[name] = digit @@ -62,7 +60,3 @@ def _poll_frame(self, camera_name: str) -> Frame: @property def config(self) -> DigitConfig: return self._cfg - - @config.setter - def config(self, cfg: DigitConfig) -> None: - self._cfg = cfg diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 2853bca7..7733ec41 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -656,7 +656,6 @@ def check_depth(depth): info["camera_available"] = True if frameset.avg_timestamp is not None: info["frame_timestamp"] = frameset.avg_timestamp - return observation, info def close(self): @@ -697,7 +696,7 @@ def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str frameset = self.camera_set.get_latest_frames() if frameset is None: observation[self.camera_key] = {} - info["camera_available"] = False + info["digit_available"] = False return observation, info frame_dict: dict[str, np.ndarray] = { @@ -705,9 +704,9 @@ def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str } observation[self.camera_key] = frame_dict - info["camera_available"] = True + info["digit_available"] = True if frameset.avg_timestamp is not None: - info["frame_timestamp"] = frameset.avg_timestamp + info["digit_timestamp"] = frameset.avg_timestamp return observation, info diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index a436b1c4..481ed551 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -61,7 +61,7 @@ def __call__( # type: ignore collision_guard: str | PathLike | None = None, gripper_cfg: rcs.hw.FHConfig | rcs.hand.tilburg_hand.THConfig | None = None, camera_set: BaseHardwareCameraSet | None = None, - digit_set_cfg: rcs.digit_cam.digit_cam.DigitConfig | None = None, + digit_set: DigitCam | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, urdf_path: str | PathLike | None = None, @@ -109,12 +109,11 @@ def __call__( # type: ignore logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) - if digit_set_cfg is not None: - digit_cam = DigitCam(digit_set_cfg) - digit_cam.start() - digit_cam.wait_for_frames() + if digit_set is not None: + digit_set.start() + digit_set.wait_for_frames() logger.info("DigitCameraSet started") - env = DigitCameraSetWrapper(env, digit_cam) + env = DigitCameraSetWrapper(env, digit_set) if collision_guard is not None: assert urdf_path is not None diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index b6a9eb8c..71f63f9e 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -5,9 +5,8 @@ import mujoco as mj import numpy as np import rcs -import rcs.digit_cam -import rcs.digit_cam.digit_cam from rcs import sim +from rcs.digit_cam import digit_cam from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType from rcs.camera.interface import BaseCameraConfig @@ -75,8 +74,18 @@ def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: return cfg -def default_digit_cam_cfg(): - return rcs.digit_cam.digit_cam.DigitConfig() +def default_digit(name2id: dict[str, str] | None) -> digit_cam.DigitCam | None: + if name2id is None: + return None + cameras = {name: BaseCameraConfig(identifier=id) for name, id in name2id.items()} + digit_cfg = digit_cam.DigitConfig( + cameras=cameras, + stream_name="QVGA", + frame_rate=30, + ) + return digit_cam.DigitCam(digit_cfg) + + def default_mujoco_cameraset_cfg() -> SimCameraSetConfig: From aea090ff2194bad1b92db56590a527fad5aca0bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 2 Jun 2025 16:19:43 +0200 Subject: [PATCH 09/10] refactor(digit): moved digit to camera folder --- python/rcs/__init__.py | 2 +- python/rcs/{digit_cam => camera}/digit_cam.py | 0 python/rcs/digit_cam/__init__.py | 0 python/rcs/envs/creators.py | 2 +- python/rcs/envs/utils.py | 8 ++++---- 5 files changed, 6 insertions(+), 6 deletions(-) rename python/rcs/{digit_cam => camera}/digit_cam.py (100%) delete mode 100644 python/rcs/digit_cam/__init__.py diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 9bf09940..9a2e42fb 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -20,7 +20,7 @@ } # make submodules available -__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand", "digit_cam"] +__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs", "hand"] # register gymnasium environments register( diff --git a/python/rcs/digit_cam/digit_cam.py b/python/rcs/camera/digit_cam.py similarity index 100% rename from python/rcs/digit_cam/digit_cam.py rename to python/rcs/camera/digit_cam.py diff --git a/python/rcs/digit_cam/__init__.py b/python/rcs/digit_cam/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 481ed551..e0686603 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -5,13 +5,13 @@ import gymnasium as gym import numpy as np import rcs +from rcs.camera.digit_cam import DigitCam import rcs.hand.tilburg_hand from gymnasium.envs.registration import EnvCreator from rcs import sim from rcs._core.sim import CameraType from rcs.camera.hw import BaseHardwareCameraSet from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig -from rcs.digit_cam.digit_cam import DigitCam from rcs.envs.base import ( CameraSetWrapper, ControlMode, diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 71f63f9e..e2035ae6 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -6,9 +6,9 @@ import numpy as np import rcs from rcs import sim -from rcs.digit_cam import digit_cam from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType +from rcs.camera.digit_cam import DigitCam, DigitConfig from rcs.camera.interface import BaseCameraConfig from rcs.camera.realsense import RealSenseCameraSet, RealSenseSetConfig from rcs.camera.sim import SimCameraConfig, SimCameraSetConfig @@ -74,16 +74,16 @@ def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: return cfg -def default_digit(name2id: dict[str, str] | None) -> digit_cam.DigitCam | None: +def default_digit(name2id: dict[str, str] | None) -> DigitCam | None: if name2id is None: return None cameras = {name: BaseCameraConfig(identifier=id) for name, id in name2id.items()} - digit_cfg = digit_cam.DigitConfig( + digit_cfg = DigitConfig( cameras=cameras, stream_name="QVGA", frame_rate=30, ) - return digit_cam.DigitCam(digit_cfg) + return DigitCam(digit_cfg) From 5e2367e8b4cb5a61de07dd83c76ca2b40ccc9b17 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 2 Jun 2025 19:35:25 +0200 Subject: [PATCH 10/10] fix(digit): remove property in digit config remove width and height properties in digit config and moved them to default factory --- python/examples/env_cartesian_control_digit.py | 2 +- python/rcs/camera/digit_cam.py | 14 +------------- python/rcs/envs/creators.py | 2 +- python/rcs/envs/utils.py | 12 +++++++----- 4 files changed, 10 insertions(+), 20 deletions(-) diff --git a/python/examples/env_cartesian_control_digit.py b/python/examples/env_cartesian_control_digit.py index 73496909..c9e5e3a3 100644 --- a/python/examples/env_cartesian_control_digit.py +++ b/python/examples/env_cartesian_control_digit.py @@ -58,7 +58,7 @@ def main(): robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", gripper_cfg=default_fr3_hw_gripper_cfg(), - digit_cfg=default_digit({"digit_0": "D21182"}), + digit_set=default_digit({"digit_0": "D21182"}), max_relative_movement=0.5, relative_to=RelativeTo.LAST_STEP, ) diff --git a/python/rcs/camera/digit_cam.py b/python/rcs/camera/digit_cam.py index 267ad200..38bcc0ef 100644 --- a/python/rcs/camera/digit_cam.py +++ b/python/rcs/camera/digit_cam.py @@ -1,7 +1,6 @@ from digit_interface.digit import Digit -from pydantic import Field from rcs.camera.hw import BaseHardwareCameraSet, HWCameraSetConfig -from rcs.camera.interface import BaseCameraConfig, CameraFrame, DataFrame, Frame +from rcs.camera.interface import CameraFrame, DataFrame, Frame class DigitConfig(HWCameraSetConfig): @@ -10,19 +9,8 @@ class DigitConfig(HWCameraSetConfig): This class is used to define the settings for the DIGIT device. """ - cameras: dict[str, BaseCameraConfig] = Field(default={}) stream_name: str = "QVGA" # options: "QVGA" (60 and 30 fps), "VGA" (30 and 15 fps) - @property - def resolution_width(self) -> int: - return Digit.STREAMS[self.stream_name]["resolution"]["width"] - - @property - def resolution_height(self) -> int: - return Digit.STREAMS[self.stream_name]["resolution"]["height"] - - - class DigitCam(BaseHardwareCameraSet): """ diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index e0686603..8aafc6eb 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -5,11 +5,11 @@ import gymnasium as gym import numpy as np import rcs -from rcs.camera.digit_cam import DigitCam import rcs.hand.tilburg_hand from gymnasium.envs.registration import EnvCreator from rcs import sim from rcs._core.sim import CameraType +from rcs.camera.digit_cam import DigitCam from rcs.camera.hw import BaseHardwareCameraSet from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcs.envs.base import ( diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index e2035ae6..3e9bb9b0 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -5,6 +5,7 @@ import mujoco as mj import numpy as np import rcs +from digit_interface import Digit from rcs import sim from rcs._core.hw import FR3Config, IKSolver from rcs._core.sim import CameraType @@ -74,19 +75,20 @@ def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: return cfg -def default_digit(name2id: dict[str, str] | None) -> DigitCam | None: +def default_digit(name2id: dict[str, str] | None, stream_name: str = "QVGA") -> DigitCam | None: if name2id is None: return None cameras = {name: BaseCameraConfig(identifier=id) for name, id in name2id.items()} + stream_dict = Digit.STREAMS[stream_name] digit_cfg = DigitConfig( cameras=cameras, - stream_name="QVGA", - frame_rate=30, + resolution_height=stream_dict["resolution"]["height"], + resolution_width=stream_dict["resolution"]["width"], + stream_name=stream_name, + frame_rate=stream_dict["fps"]["30fps"], ) return DigitCam(digit_cfg) - - def default_mujoco_cameraset_cfg() -> SimCameraSetConfig: cameras = {