diff --git a/examples/teleop/README.md b/examples/teleop/README.md index 86832541..c3876de5 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/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 a0ebdbfba86f58aaa6360eb3b9da7b74a9c9b1dd +git checkout af6560384f24520601798b47d900fee6fe27cf2d pip install -ve . ``` @@ -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/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/examples/teleop/quest_align_frame.py b/examples/teleop/quest_align_frame.py index ecf7e157..9cb5feb3 100644 --- a/examples/teleop/quest_align_frame.py +++ b/examples/teleop/quest_align_frame.py @@ -2,8 +2,8 @@ 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() - # print(data) + print(data) diff --git a/examples/teleop/requirements.txt b/examples/teleop/requirements.txt index 3707e128..67a3a808 100644 --- a/examples/teleop/requirements.txt +++ b/examples/teleop/requirements.txt @@ -1,6 +1,6 @@ -# 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 jupyter -mutplotlib \ No newline at end of file +matplotlib \ No newline at end of file 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 653a1ab0..a45343f4 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/__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/envs/base.py b/python/rcs/envs/base.py index 2c4a6a3c..8dc97d54 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( @@ -947,6 +951,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 diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py index c649326e..afbd9f95 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) @@ -281,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"] diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index 6df7c3fa..0242cebf 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,8 @@ 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 +282,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 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, 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 a9a82254..322b870a 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 @@ -12,8 +13,9 @@ 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.core.video_streamer import VideoStreamerManager + from simpub.parser.simdata import SimObject, SimScene, SimSceneConfig from simpub.xr_device.meta_quest3 import MetaQuest3 HAS_SIMPUB = True @@ -32,12 +34,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): @@ -73,10 +93,11 @@ def __init__(self, config: "QuestConfig", sim: Sim | None = None): self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} - # 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_stream_manager = None + 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 @@ -107,6 +128,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: @@ -155,6 +182,41 @@ def consume_action(self) -> dict[str, ArmWithGripper]: else transforms ) + def set_camera(self, observation: dict) -> None: + if not self.config.display_cameras: + return + 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] + 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 + 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() # self._publisher.shutdown() @@ -194,6 +256,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 +267,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() @@ -247,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