chore: adapt latest iris version

examples/teleop/README.md

@@ -10,37 +10,38 @@ 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 .
 ```
 
 ### Configuration
 
 #### 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

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={

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)

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
+matplotlib

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" }]

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",
 ]

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[

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
 

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"]

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

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,

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.")