From 65976971beb4662d83e921eb378ab7b024836751 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 29 Apr 2026 11:37:51 +0200 Subject: [PATCH 01/15] chore(teleop): bump iris and adapt to continous buttons --- examples/teleop/README.md | 4 ++-- examples/teleop/requirements.txt | 2 +- python/rcs/operator/quest.py | 36 +++++++++++++++++--------------- 3 files changed, 22 insertions(+), 20 deletions(-) diff --git a/examples/teleop/README.md b/examples/teleop/README.md index 86832541..f400a81d 100644 --- a/examples/teleop/README.md +++ b/examples/teleop/README.md @@ -10,14 +10,14 @@ The buttons are used to start and stop data recording with the [`StorageWrapper` ### Installation [Install RCS](https://robotcontrolstack.org/getting_started/index.html) and the [FR3 extension](https://robotcontrolstack.org/extensions/rcs_fr3.html) (the script is writte for the FR3 as example but can be easily adapted for other robots). -Install the IRIS APK on your quest following [these instructions](https://github.com/intuitive-robots/IRIS-Meta-Quest3) use the apk released [here](https://github.com/RobotControlStack/IRIS-Meta-Quest3/releases/tag/rcsv1) to ensure compatiblity. +Install the IRIS APK on your quest following [these instructions](https://github.com/intuitive-robots/IRIS-Meta-Quest3) use the apk released [here](https://github.com/RobotControlStack/IRIS-Meta-Quest3/actions/runs/24963710474) to ensure compatibility. Finally, install [SimPub](https://github.com/intuitive-robots/SimPublisher) the IRIS python client by ```shell pip install -r requirements.txt git clone https://github.com/intuitive-robots/SimPublisher.git cd SimPublisher -git checkout a0ebdbfba86f58aaa6360eb3b9da7b74a9c9b1dd +git checkout 1397bb0946e2a9256c5024e5cd779be8ec7887ac pip install -ve . ``` diff --git a/examples/teleop/requirements.txt b/examples/teleop/requirements.txt index 3707e128..a6d980bb 100644 --- a/examples/teleop/requirements.txt +++ b/examples/teleop/requirements.txt @@ -3,4 +3,4 @@ dynamixel_sdk pynput scipy jupyter -mutplotlib \ No newline at end of file +matplotlib \ No newline at end of file diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index a9a82254..6ca973a3 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -107,6 +107,12 @@ def _reset_state(self): self._last_controller_pose[controller] = Pose() self._grp_pos[controller] = 1 + @staticmethod + def _normalize_axis(value: bool | float | int) -> float: + if isinstance(value, bool): + return float(value) + return float(np.clip(value, 0.0, 1.0)) + def consume_commands(self) -> TeleopCommands: # must be threadsafe with self._cmd_lock: @@ -194,6 +200,7 @@ def run(self): # === Update Poses & Grippers === for controller in self.controller_names: + prev_data = self._prev_data last_controller_pose = Pose( translation=np.array(input_data[controller]["pos"]), quaternion=np.array(input_data[controller]["rot"]), @@ -204,38 +211,33 @@ def run(self): # * last_controller_pose # ) - if input_data[controller][self._trg_btn[controller]] and ( - self._prev_data is None or not self._prev_data[controller][self._trg_btn[controller]] - ): + trigger_pressed = self._normalize_axis(input_data[controller][self._trg_btn[controller]]) > 0.5 + if prev_data is None: + prev_trigger_pressed = False + else: + prev_trigger_pressed = self._normalize_axis(prev_data[controller][self._trg_btn[controller]]) > 0.5 + + if trigger_pressed and not prev_trigger_pressed: # trigger just pressed (first data sample with button pressed) with self._resource_lock: self._offset_pose[controller] = last_controller_pose self._last_controller_pose[controller] = last_controller_pose - elif not input_data[controller][self._trg_btn[controller]] and ( - self._prev_data is None or self._prev_data[controller][self._trg_btn[controller]] - ): + elif not trigger_pressed and prev_trigger_pressed: with self._resource_lock: self._last_controller_pose[controller] = Pose() self._offset_pose[controller] = Pose() self._reset_origin_to_current(controller) - elif input_data[controller][self._trg_btn[controller]]: + elif trigger_pressed: # button is pressed with self._resource_lock: self._last_controller_pose[controller] = last_controller_pose - if input_data[controller][self._grp_btn[controller]] and ( - self._prev_data is None or not self._prev_data[controller][self._grp_btn[controller]] - ): - # just pressed - self._grp_pos[controller] = 0 - if not input_data[controller][self._grp_btn[controller]] and ( - self._prev_data is None or self._prev_data[controller][self._grp_btn[controller]] - ): - # just released - self._grp_pos[controller] = 1 + gripper_axis = self._normalize_axis(input_data[controller][self._grp_btn[controller]]) + # convert from IRIS to RCS gripper logic + self._grp_pos[controller] = 1.0 - gripper_axis self._prev_data = input_data rate_limiter() From 22707520fba6fee11c8baa904969196d234d3d88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 29 Apr 2026 11:38:32 +0200 Subject: [PATCH 02/15] chore(teleop:quest): switch gripper and trigger button --- python/rcs/operator/quest.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 6ca973a3..0bcaebc7 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -56,8 +56,8 @@ def __init__(self, config: "QuestConfig", sim: Sim | None = None): self._resource_lock = threading.Lock() self._cmd_lock = threading.Lock() - self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} - self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} + self._trg_btn = {"left": "hand_trigger", "right": "hand_trigger"} + self._grp_btn = {"left": "index_trigger", "right": "index_trigger"} self._start_btn = "A" self._stop_btn = "B" self._unsuccessful_btn = "Y" From f11d94d290e832687e55078dbfa3d7d7fb6cdb56 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 29 Apr 2026 13:58:17 +0200 Subject: [PATCH 03/15] fix(iris): empty scene teleop --- examples/teleop/quest_align_frame.py | 2 +- python/rcs/operator/quest.py | 30 ++++++++++++++++++++++------ 2 files changed, 25 insertions(+), 7 deletions(-) diff --git a/examples/teleop/quest_align_frame.py b/examples/teleop/quest_align_frame.py index ecf7e157..e9035308 100644 --- a/examples/teleop/quest_align_frame.py +++ b/examples/teleop/quest_align_frame.py @@ -2,7 +2,7 @@ from rcs.operator.quest import FakeSimPublisher, FakeSimScene from simpub.xr_device.meta_quest3 import MetaQuest3 -FakeSimPublisher(FakeSimScene(), MQ3_ADDR) +publisher = FakeSimPublisher(FakeSimScene(), MQ3_ADDR) reader = MetaQuest3("RCSNode") while True: data = reader.get_controller_data() diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 0bcaebc7..d30fe61f 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -12,8 +12,8 @@ from rcs.utils import SimpleFrameRate try: - from simpub.core.simpub_server import SimPublisher - from simpub.parser.simdata import SimObject, SimScene + from simpub.core.simpub_server import RigidObjectUpdateData, SimPublisher + from simpub.parser.simdata import SimObject, SimScene, SimSceneConfig from simpub.xr_device.meta_quest3 import MetaQuest3 HAS_SIMPUB = True @@ -32,12 +32,30 @@ class FakeSimPublisher(SimPublisher): def get_update(self): - return {} + return RigidObjectUpdateData(data={}) class FakeSimScene(SimScene): - def __init__(self): - super().__init__() - self.root = SimObject(name="root") + def __init__(self, name: str = "RCS"): + super().__init__( + SimSceneConfig( + name=name, + pos=[0.0, 0.0, 0.0], + rot=[0.0, 0.0, 0.0, 1.0], + scale=[1.0, 1.0, 1.0], + ) + ) + self.root.add_data( + SimObject( + name="root", + parent="root", + trans={ + "pos": [0.0, 0.0, 0.0], + "rot": [0.0, 0.0, 0.0, 1.0], + "scale": [1.0, 1.0, 1.0], + }, + visuals=[], + ) + ) class QuestOperator(BaseOperator): From 7c3cb8bd16065770c5d4405452a6bab47280e185 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 30 Apr 2026 10:26:26 +0200 Subject: [PATCH 04/15] feat: experimental camera stream iris --- pyproject.toml | 1 + python/rcs/__init__.py | 2 +- python/rcs/operator/interface.py | 10 ++++++++++ python/rcs/operator/quest.py | 26 ++++++++++++++++++++++++++ 4 files changed, 38 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index f35a1cb0..6a9213d3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -41,6 +41,7 @@ dependencies = [ "pin==3.7.0", "greenlet", "duckdb", + "pandas", ] readme = "README.md" maintainers = [{ name = "Tobias Jülg", email = "tobias.juelg@utn.de" }] diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index ba98dcd1..653a1ab0 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -127,7 +127,7 @@ class RobotMetaConfig: GRIPPER_OFFSETS: dict[common.GripperType, common.Pose] = { common.GripperType.FrankaHand: common.Pose(pose_matrix=common.FrankaHandTCPOffset()), - common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0.0, 0.0, 0.1628])), + common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0.1628, 0.0, 0.0])), } SCENE_PATHS: dict[str, str] = {"empty_world": "assets/scenes/empty_world/scene.xml"} diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index 37debc92..ee58f920 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -5,6 +5,7 @@ from abc import ABC from dataclasses import dataclass, field from time import sleep +from typing import Any import gymnasium as gym from rcs._core.common import RobotPlatform @@ -50,6 +51,9 @@ def consume_action(self) -> dict[str, ArmWithGripper]: """Returns the action dictionary to step the environment. Must be thread-safe.""" raise NotImplementedError() + def set_camera(self, observation: dict[str, Any]) -> None: + """Optional hook for operators that want to consume camera observations.""" + def close(self): pass @@ -123,6 +127,7 @@ def environment_step_loop(self): # 0. Initial Reset to get current positions for untracked robots self._last_obs, _ = self.env.reset() + self.operator.set_camera(self._last_obs) while True: if self._exit_requested: @@ -140,6 +145,7 @@ def environment_step_loop(self): self.env.get_wrapper_attr("success")() sleep(1) # sleep to let the robot reach the goal self._last_obs, _ = self.env.reset() + self.operator.set_camera(self._last_obs) self.operator.reset_operator_state() self._synced = self.operator.control_mode[1] != RelativeTo.NONE # consume new commands because of potential origin reset @@ -148,6 +154,7 @@ def environment_step_loop(self): if cmds.failure: print("Command: Failure! Resetting env...") self._last_obs, _ = self.env.reset() + self.operator.set_camera(self._last_obs) self.operator.reset_operator_state() self._synced = self.operator.control_mode[1] != RelativeTo.NONE # consume new commands because of potential origin reset @@ -172,6 +179,7 @@ def environment_step_loop(self): } self._last_obs, _, _, _, _ = self.env.step(hold_actions) + self.operator.set_camera(self._last_obs) rate_limiter() continue @@ -191,6 +199,7 @@ def environment_step_loop(self): actions = self.operator.consume_action() actions = self._translate_keys(actions) self._last_obs, _, _, _, _ = self.env.step(actions) + self.operator.set_camera(self._last_obs) rate_limiter() @@ -228,6 +237,7 @@ def sync_robot_to_operator(self, duration: float = 3.0): continue self._last_obs, _, _, _, _ = self.env.step(interp_actions) + self.operator.set_camera(self._last_obs) rate_limiter() print("Sync Complete.") diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index d30fe61f..bfe1a53f 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -13,6 +13,7 @@ try: from simpub.core.simpub_server import RigidObjectUpdateData, SimPublisher + from simpub.core.video_streamer import VideoStreamerManager from simpub.parser.simdata import SimObject, SimScene, SimSceneConfig from simpub.xr_device.meta_quest3 import MetaQuest3 @@ -91,6 +92,8 @@ def __init__(self, config: "QuestConfig", sim: Sim | None = None): self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} + self._video_stream_manager = None + self._video_streamers: dict[str, object] = {} # if self.config.simulation: # self._publisher = MujocoPublisher(self.sim.model, self.sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) if not self.config.simulation: @@ -179,6 +182,29 @@ def consume_action(self) -> dict[str, ArmWithGripper]: else transforms ) + def set_camera(self, observation: dict) -> None: + frames = observation.get("frames") + if not isinstance(frames, dict): + return + + if self._video_stream_manager is None: + self._video_stream_manager = VideoStreamerManager(self.config.mq3_addr) + + for camera_name, camera_data in frames.items(): + if not isinstance(camera_data, dict) or "rgb" not in camera_data: + continue + rgb = camera_data["rgb"] + if not isinstance(rgb, dict) or "data" not in rgb: + continue + frame = rgb["data"] + if camera_name not in self._video_streamers: + height, width = frame.shape[:2] + self._video_streamers[camera_name] = self._video_stream_manager.create_streamer( + f"{camera_name}_camera", width, height + ) + # self._video_streamers[camera_name].update_cv_image(np.ascontiguousarray(frame[:, :, ::-1])) + self._video_streamers[camera_name].update_cv_image(frame[:, :, ::-1]) + def close(self): self._reader.disconnect() # self._publisher.shutdown() From 5d6b4e73ae552e8857ca67a55d0d52a4b368d5d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 30 Apr 2026 11:54:59 +0200 Subject: [PATCH 05/15] feat(env): env info contains platform, robot and gripper type --- python/rcs/envs/base.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 49c85b24..cbf647eb 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -184,14 +184,17 @@ class ControlMode(Enum): class BaseEnv(gym.Env): PLATFORM: RobotPlatform + def _platform_info(self): + return {"platform": "simulation" if self.PLATFORM == RobotPlatform.SIMULATION else "hardware"} + def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: - return {}, 0, False, False, {} + return {}, 0, False, False, self._platform_info() def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: super().reset(seed=seed, options=options) - return {}, {} + return {}, self._platform_info() class HardwareEnv(BaseEnv): @@ -375,6 +378,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: def observation(self, observation: dict, info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: observation.update(self.get_robot_obs()) + observation.update({"robot_type": self.robot.get_config().robot_type.id}) return observation, info def reset( @@ -927,6 +931,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl ) else: observation[self.gripper_key] = [self.gripper.get_normalized_width()] + observation.update({"gripper_type": self.gripper.get_config().gripper_type.id}) return observation, info From 21582a6b4740b7be246da03840049ff72945c354 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 30 Apr 2026 11:55:30 +0200 Subject: [PATCH 06/15] fix(converter): robot type and helper string --- python/rcs/__main__.py | 2 +- python/rcs/lerobot_joint_converter.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/rcs/__main__.py b/python/rcs/__main__.py index f82ce406..384675ff 100644 --- a/python/rcs/__main__.py +++ b/python/rcs/__main__.py @@ -87,7 +87,7 @@ def lerobot_convert( output: Annotated[ Path, typer.Argument( - help="Output directory for the LeRobot dataset. Example: --output ./data_lerobot", + help="Output directory for the LeRobot dataset. Example: ./data_lerobot", ), ] = Path(DEFAULT_HF_DATA_DIR), dataset_paths: Annotated[ diff --git a/python/rcs/lerobot_joint_converter.py b/python/rcs/lerobot_joint_converter.py index a2819bf4..bdfe6e6e 100644 --- a/python/rcs/lerobot_joint_converter.py +++ b/python/rcs/lerobot_joint_converter.py @@ -123,7 +123,7 @@ def __init__( self.lrds = LeRobotDataset.create( repo_id=self.repo_id, - robot_type=self.robot_type, + robot_type=self.robot_type.id, root=self.root, fps=self.fps, use_videos=False, From cf3e5857f39661dfac0b40153da2d9df5b608476 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 30 Apr 2026 21:04:09 +0200 Subject: [PATCH 07/15] feat(duo): defined sane home positions --- python/rcs/__init__.py | 9 +++++++++ python/rcs/envs/configs.py | 3 ++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 653a1ab0..7b591e27 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -172,6 +172,14 @@ class RobotMetaConfig: ), } +HOME_POSITIONS = { + # calculated from + # "left": {"xyzrpy": [0.61, 0.21, 0.40, -np.pi, np.deg2rad(-20), 0], "gripper": [0]}, + # "right": {"xyzrpy": [0.61, -0.21, 0.40, -np.pi, np.deg2rad(-20), 0], "gripper": [0]}, + "FR3_DUO_LEFT": np.array([ 0.48797692, -0.57224476, -0.58536988, -2.57958827, 0.86400183, 2.0530809, -0.85965005]), + "FR3_DUO_RIGHT": np.array([-0.48797676, -0.57224472, 0.58536959, -2.57958788, -0.86400148, 2.05308196, 0.85965057]) +} + # Append RCS package prefix to all asset paths for path_dict in (SCENE_PATHS, OBJECT_PATHS, CAMERA_PATHS): for name, path in path_dict.items(): @@ -209,5 +217,6 @@ class RobotMetaConfig: "SCENE_PATHS", "OBJECT_PATHS", "CAMERA_PATHS", + "HOME_POSITIONS", "TASKS", ] diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py index c649326e..09b8d8ef 100644 --- a/python/rcs/envs/configs.py +++ b/python/rcs/envs/configs.py @@ -222,9 +222,10 @@ def config(self) -> SimEnvCreatorConfig: base="base", dof=rcs.ROBOTS[RobotType.FR3].dof, joint_limits=rcs.ROBOTS[RobotType.FR3].joint_limits, - q_home=rcs.ROBOTS[RobotType.FR3].q_home, + q_home=rcs.HOME_POSITIONS["FR3_DUO_LEFT"], ) robot_cfg_right = copy.deepcopy(robot_cfg) + robot_cfg_right.q_home = rcs.HOME_POSITIONS["FR3_DUO_RIGHT"] robot_cfgs: dict[str, SimRobotConfig] = {"left": robot_cfg, "right": robot_cfg_right} sim_cfg: SimConfig = SimConfig(async_control=False, realtime=True, frequency=1, max_convergence_steps=500) From de2c1a98e59b0aa6be297d89ffebc7468331ddfa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 09:03:31 +0200 Subject: [PATCH 08/15] fix(storage wrapper): copy obs before mutating --- python/rcs/envs/storage_wrapper.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index 6df7c3fa..f55e1149 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -1,3 +1,4 @@ +import copy import datetime import io import operator @@ -246,7 +247,9 @@ def step(self, action): msg = "Writer thread failed" raise RuntimeError(msg) from exc - obs, reward, terminated, truncated, info = self.env.step(action) + obs_original, reward, terminated, truncated, info = self.env.step(action) + obs = copy.deepcopy(obs_original) + if not self._pause: assert isinstance(obs, dict) @@ -280,7 +283,7 @@ def step(self, action): if len(self.buffer) == self.batch_size: self._flush() - return obs, reward, terminated, truncated, info + return obs_original, reward, terminated, truncated, info def set_instruction(self, instruction: str): self.instruction = instruction From d9375670c88b5edf5e5e5850df7542002bb7700c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 09:18:54 +0200 Subject: [PATCH 09/15] chore: switch gripper buttons quest, make stereo ready --- python/rcs/operator/quest.py | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index bfe1a53f..4d299844 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -75,8 +75,8 @@ def __init__(self, config: "QuestConfig", sim: Sim | None = None): self._resource_lock = threading.Lock() self._cmd_lock = threading.Lock() - self._trg_btn = {"left": "hand_trigger", "right": "hand_trigger"} - self._grp_btn = {"left": "index_trigger", "right": "index_trigger"} + self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} + self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} self._start_btn = "A" self._stop_btn = "B" self._unsuccessful_btn = "Y" @@ -199,11 +199,21 @@ def set_camera(self, observation: dict) -> None: frame = rgb["data"] if camera_name not in self._video_streamers: height, width = frame.shape[:2] + stream_topic = self._get_stream_topic_name(camera_name) self._video_streamers[camera_name] = self._video_stream_manager.create_streamer( - f"{camera_name}_camera", width, height + stream_topic, width, height ) - # self._video_streamers[camera_name].update_cv_image(np.ascontiguousarray(frame[:, :, ::-1])) - self._video_streamers[camera_name].update_cv_image(frame[:, :, ::-1]) + self._video_streamers[camera_name].update_cv_image( + frame[:, :, ::-1] + ) + + @staticmethod + def _get_stream_topic_name(camera_name: str) -> str: + if camera_name.endswith("_left"): + return f"{camera_name[:-len('_left')]}_camera_left" + if camera_name.endswith("_right"): + return f"{camera_name[:-len('_right')]}_camera_right" + return f"{camera_name}_camera" def close(self): self._reader.disconnect() From ccaa04a8971894dec6c663a4b17d881017619926 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 09:21:38 +0200 Subject: [PATCH 10/15] docs(teleop): update with latest checkout commits --- examples/teleop/README.md | 6 +++--- examples/teleop/requirements.txt | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/teleop/README.md b/examples/teleop/README.md index f400a81d..e7698892 100644 --- a/examples/teleop/README.md +++ b/examples/teleop/README.md @@ -10,14 +10,14 @@ The buttons are used to start and stop data recording with the [`StorageWrapper` ### Installation [Install RCS](https://robotcontrolstack.org/getting_started/index.html) and the [FR3 extension](https://robotcontrolstack.org/extensions/rcs_fr3.html) (the script is writte for the FR3 as example but can be easily adapted for other robots). -Install the IRIS APK on your quest following [these instructions](https://github.com/intuitive-robots/IRIS-Meta-Quest3) use the apk released [here](https://github.com/RobotControlStack/IRIS-Meta-Quest3/actions/runs/24963710474) to ensure compatibility. +Install the IRIS APK on your quest following [these instructions](https://github.com/intuitive-robots/IRIS-Meta-Quest3) use the apk released [here](https://github.com/RobotControlStack/IRIS-Meta-Quest3/actions/runs/25190284304) to ensure compatibility. Finally, install [SimPub](https://github.com/intuitive-robots/SimPublisher) the IRIS python client by ```shell pip install -r requirements.txt -git clone https://github.com/intuitive-robots/SimPublisher.git +git clone https://github.com/RobotControlStack/SimPublisher.git cd SimPublisher -git checkout 1397bb0946e2a9256c5024e5cd779be8ec7887ac +git checkout af6560384f24520601798b47d900fee6fe27cf2d pip install -ve . ``` diff --git a/examples/teleop/requirements.txt b/examples/teleop/requirements.txt index a6d980bb..67a3a808 100644 --- a/examples/teleop/requirements.txt +++ b/examples/teleop/requirements.txt @@ -1,4 +1,4 @@ -# simpub @ git+https://github.com/intuitive-robots/SimPublisher.git@a0ebdbfba86f58aaa6360eb3b9da7b74a9c9b1dd # doesnt work +# simpub @ git+https://github.com/intuitive-robots/SimPublisher.git@af6560384f24520601798b47d900fee6fe27cf2d # doesnt work dynamixel_sdk pynput scipy From c120ae8e68432d7680885956e28b0bb7b85eca9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 09:29:45 +0200 Subject: [PATCH 11/15] docs(teleop): improve docs accuracy --- examples/teleop/README.md | 11 ++++++----- examples/teleop/quest_align_frame.py | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/examples/teleop/README.md b/examples/teleop/README.md index e7698892..c3876de5 100644 --- a/examples/teleop/README.md +++ b/examples/teleop/README.md @@ -25,22 +25,23 @@ pip install -ve . #### Teleoperating in sim -1. go to [`franka.py`](quest_iris_dual_arm.py) and set `ROBOT_INSTANCE = RobotPlatform.SIMULATION` +1. go to [`franka.py`](franka.py) and set `ROBOT_INSTANCE = RobotPlatform.SIMULATION` #### Teleoperating a real robot Note that dual arm is only supported for a aloha like setup where the robot face each other (for more advanced setups you need to change the transformation between the robots yourself). 1. put your robots into FCI mode -2. go to [`franka.py`](quest_iris_dual_arm.py), set `ROBOT_INSTANCE = RobotPlatform.HARDWARE` and set your IP addresses of your robots. Remove the left robot if you only have one. +2. go to [`franka.py`](franka.py), set `ROBOT_INSTANCE = RobotPlatform.HARDWARE` and set your IP addresses of your robots. Remove the left robot if you only have one. ### Running 1. make sure your computer and quest is in the same subnetwork and they can ping each other. 2. start IRIS meta quest app on your quest (it should be located in the Library under "Unkown Sources" after installation) -3. run the [`quest_align_frame.py`](quest_align_frame.py) script once. Navigate to the link printed on the top likly [http://127.0.0.1:7000](http://127.0.0.1:7000). +3. run the [`quest_align_frame.py`](quest_align_frame.py) script once. Navigate to the link printed on the top likely [http://127.0.0.1:5000](http://127.0.0.1:5000). - click scan - your meta quest should show up - - click change name and type "RCSNode" and click ok - - the script should now print a bunch of numbers (the controller poses) + - click change name and type "RCSNode" and click ok (this needs to be done only once) + - restart both the app and the script + - the script should now print a bunch of numbers (the controller poses) which means the connection is working 4. put on your quest (dont remove it until done with teleop, otherwise the axis might change and you need to recalibrate), you should see a white ball with coordinate axis somewhere in your room (red is x, green is y and blue is z) 5. use the right controller to change the orientation of the coordinate axis to fit your right robot (for franka: x front, y left, z up) 6. click the "teleportation scene" button on the still open website diff --git a/examples/teleop/quest_align_frame.py b/examples/teleop/quest_align_frame.py index e9035308..9cb5feb3 100644 --- a/examples/teleop/quest_align_frame.py +++ b/examples/teleop/quest_align_frame.py @@ -6,4 +6,4 @@ reader = MetaQuest3("RCSNode") while True: data = reader.get_controller_data() - # print(data) + print(data) From e06c68c48d3d0a9e7932cbeed2e7cc7b5d538e97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 09:29:57 +0200 Subject: [PATCH 12/15] format: python formatting --- python/rcs/__init__.py | 4 ++-- python/rcs/envs/storage_wrapper.py | 1 - python/rcs/operator/quest.py | 4 +--- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 7b591e27..a45343f4 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -176,8 +176,8 @@ class RobotMetaConfig: # calculated from # "left": {"xyzrpy": [0.61, 0.21, 0.40, -np.pi, np.deg2rad(-20), 0], "gripper": [0]}, # "right": {"xyzrpy": [0.61, -0.21, 0.40, -np.pi, np.deg2rad(-20), 0], "gripper": [0]}, - "FR3_DUO_LEFT": np.array([ 0.48797692, -0.57224476, -0.58536988, -2.57958827, 0.86400183, 2.0530809, -0.85965005]), - "FR3_DUO_RIGHT": np.array([-0.48797676, -0.57224472, 0.58536959, -2.57958788, -0.86400148, 2.05308196, 0.85965057]) + "FR3_DUO_LEFT": np.array([0.48797692, -0.57224476, -0.58536988, -2.57958827, 0.86400183, 2.0530809, -0.85965005]), + "FR3_DUO_RIGHT": np.array([-0.48797676, -0.57224472, 0.58536959, -2.57958788, -0.86400148, 2.05308196, 0.85965057]), } # Append RCS package prefix to all asset paths diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index f55e1149..0242cebf 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -250,7 +250,6 @@ def step(self, action): obs_original, reward, terminated, truncated, info = self.env.step(action) obs = copy.deepcopy(obs_original) - if not self._pause: assert isinstance(obs, dict) if "frames" in obs and not obs["frames"]: diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 4d299844..7be590ad 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -203,9 +203,7 @@ def set_camera(self, observation: dict) -> None: self._video_streamers[camera_name] = self._video_stream_manager.create_streamer( stream_topic, width, height ) - self._video_streamers[camera_name].update_cv_image( - frame[:, :, ::-1] - ) + self._video_streamers[camera_name].update_cv_image(frame[:, :, ::-1]) @staticmethod def _get_stream_topic_name(camera_name: str) -> str: From 9000798fce2e4ae59f45fff941f3a5aa2eb3ef0f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 09:33:40 +0200 Subject: [PATCH 13/15] style: python linting --- python/rcs/operator/quest.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 7be590ad..5ed58d8b 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -3,6 +3,7 @@ import threading from dataclasses import dataclass, field from time import sleep +from typing import Any import numpy as np from rcs._core.common import Pose @@ -93,11 +94,10 @@ def __init__(self, config: "QuestConfig", sim: Sim | None = None): self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} self._video_stream_manager = None - self._video_streamers: dict[str, object] = {} - # if self.config.simulation: - # self._publisher = MujocoPublisher(self.sim.model, self.sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) + self._video_streamers: dict[str, Any] = {} if not self.config.simulation: self._publisher = FakeSimPublisher(FakeSimScene(), self.config.mq3_addr) + # Not working code for digital twin: # robot_cfg = default_sim_robot_cfg("fr3_empty_world") # sim_cfg = SimConfig() # sim_cfg.async_control = True @@ -200,9 +200,11 @@ def set_camera(self, observation: dict) -> None: if camera_name not in self._video_streamers: height, width = frame.shape[:2] stream_topic = self._get_stream_topic_name(camera_name) + assert self._video_stream_manager is not None self._video_streamers[camera_name] = self._video_stream_manager.create_streamer( stream_topic, width, height ) + assert self._video_streamers[camera_name] is not None self._video_streamers[camera_name].update_cv_image(frame[:, :, ::-1]) @staticmethod From 60c265eea8fe8c52103c4da3c93526d2328856a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 09:44:32 +0200 Subject: [PATCH 14/15] feat(quest): config option to display cameras in UI --- examples/teleop/franka.py | 5 ++++- python/rcs/operator/quest.py | 3 +++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py index ef073761..09824148 100644 --- a/examples/teleop/franka.py +++ b/examples/teleop/franka.py @@ -66,7 +66,10 @@ config: QuestConfig | GelloConfig config = QuestConfig( - mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION, switched_left_right=False + mq3_addr=MQ3_ADDR, + simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION, + switched_left_right=False, + display_cameras=True, ) # config = GelloConfig( # arms={ diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 5ed58d8b..322b870a 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -183,6 +183,8 @@ def consume_action(self) -> dict[str, ArmWithGripper]: ) def set_camera(self, observation: dict) -> None: + if not self.config.display_cameras: + return frames = observation.get("frames") if not isinstance(frames, dict): return @@ -303,3 +305,4 @@ class QuestConfig(BaseOperatorConfig): include_rotation: bool = True mq3_addr: str = "10.42.0.1" switched_left_right: bool = False + display_cameras: bool = True From c16566830df5b4fab7a77583e2970c11ba942f99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 1 May 2026 11:33:07 +0200 Subject: [PATCH 15/15] config(teleop): default to non binary gripper --- python/rcs/envs/configs.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py index 09b8d8ef..afbd9f95 100644 --- a/python/rcs/envs/configs.py +++ b/python/rcs/envs/configs.py @@ -282,7 +282,7 @@ def config(self) -> SimEnvCreatorConfig: "left": DEFAULT_TRANSFORMS["FR3_DUOMOUNT_LEFT_ROBOT"], "right": DEFAULT_TRANSFORMS["FR3_DUOMOUNT_RIGHT_ROBOT"], } - wrapper_cfg: WrapperConfig = WrapperConfig(binary_gripper=True, home_on_reset=True) + wrapper_cfg: WrapperConfig = WrapperConfig(binary_gripper=False, home_on_reset=True) headless = False add_gravcomp = True shared_base_frame_to_root_frame = DEFAULT_TRANSFORMS["FR3_DUOMOUNT_HEIGHT_OFFSET"]