Merge pull request #251 from RobotControlStack/juelg/teleop-fixes

fix: teleop example

Author: juelg

examples/teleop/quest_iris_dual_arm.py

@@ -15,7 +15,7 @@
 )
 from rcs.envs.creators import SimMultiEnvCreator
 from rcs.envs.storage_wrapper import StorageWrapper
-from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg
+from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg
 from rcs.utils import SimpleFrameRate
 from rcs_fr3.creators import RCSFR3MultiEnvCreator
 from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
@@ -25,8 +25,6 @@
 from simpub.sim.mj_publisher import MujocoPublisher
 from simpub.xr_device.meta_quest3 import MetaQuest3
 
-# from rcs_xarm7.creators import RCSXArm7EnvCreator
-
 logger = logging.getLogger(__name__)
 
 # download the iris apk from the following repo release: https://github.com/intuitive-robots/IRIS-Meta-Quest3
@@ -38,7 +36,7 @@
 
 INCLUDE_ROTATION = True
 ROBOT2IP = {
-    "left": "192.168.102.1",
+    # "left": "192.168.102.1",
     "right": "192.168.101.1",
 }
 
@@ -48,14 +46,20 @@
 RECORD_FPS = 30
 # set camera dict to none disable cameras
 # CAMERA_DICT = {
-#     "side_right": "244222071045",
+#     "left_wrist": "230422272017",
+#     "right_wrist": "230422271040",
+#     "side": "243522070385",
 #     "bird_eye": "243522070364",
-#     "arro": "243522070385",
 # }
 CAMERA_DICT = None
 MQ3_ADDR = "10.42.0.1"
+# DIGIT_DICT = {
+#     "digit_right_left": "D21182",
+#     "digit_right_right": "D21193"
+# }
+DIGIT_DICT = None
 
-DATASET_PATH = "test_data_iris_dual_arm"
+DATASET_PATH = "test_data_iris_dual_arm14"
 INSTRUCTION = "build a tower with the blocks in front of you"
 
 
@@ -80,6 +84,7 @@ def __init__(self, env: RelativeActionSpace):
 
         self._resource_lock = threading.Lock()
         self._env_lock = threading.Lock()
+        self._reset_lock = threading.Lock()
         self._env = env
 
         self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ["right"]
@@ -155,11 +160,18 @@ def run(self):
 
             if input_data[self._stop_btn] and (self._prev_data is None or not self._prev_data[self._stop_btn]):
                 print("reset successful pressed: resetting env")
-                with self._env_lock:
+                with self._reset_lock:
                     # set successful
                     self._env.get_wrapper_attr("success")()
+                    # sleep to allow to let the robot reach the goal
+                    sleep(1)
                     # this might also move the robot to the home position
                     self._env.reset()
+                    for controller in self.controller_names:
+                        self._offset_pose[controller] = Pose()
+                        self._last_controller_pose[controller] = Pose()
+                        self._grp_pos[controller] = 1
+                    continue
 
             # reset unsuccessful
             if input_data[self._unsuccessful_btn] and (
@@ -261,25 +273,25 @@ def environment_step_loop(self):
             if self._exit_requested:
                 self._step_env = False
                 break
-            transforms, grippers = self.next_action()
-            actions = {}
-            for robot, transform in transforms.items():
-                action = dict(
-                    LimitedTQuatRelDictType(tquat=np.concatenate([transform.translation(), transform.rotation_q()]))  # type: ignore
-                )
+            with self._reset_lock:
+                transforms, grippers = self.next_action()
+                actions = {}
+                for robot, transform in transforms.items():
+                    action = dict(
+                        LimitedTQuatRelDictType(tquat=np.concatenate([transform.translation(), transform.rotation_q()]))  # type: ignore
+                    )
 
-                action.update(GripperDictType(gripper=grippers[robot]))
-                actions[robot] = action
+                    action.update(GripperDictType(gripper=grippers[robot]))
+                    actions[robot] = action
 
-            with self._env_lock:
                 self._env.step(actions)
             rate_limiter()
 
 
 def main():
     if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
 
-        camera_set = HardwareCameraSet([default_realsense(CAMERA_DICT)]) if CAMERA_DICT is not None else None  # type: ignore
+        camera_set = HardwareCameraSet([default_realsense(CAMERA_DICT), default_digit(DIGIT_DICT)]) if CAMERA_DICT is not None else None  # type: ignore
         env_rel = RCSFR3MultiEnvCreator()(
             name2ip=ROBOT2IP,
             camera_set=camera_set,

extensions/rcs_fr3/src/rcs_fr3/envs.py

@@ -40,6 +40,7 @@ def _rs2dict(self, state: hw.RobotState):
             self._robot_state_keys = [
                 attr for attr in dir(state) if not attr.startswith("__") and not callable(getattr(state, attr))
             ]
+            self._robot_state_keys.remove("robot_mode")
         return {key: getattr(state, key) for key in self._robot_state_keys}
 
     def reset(

pyproject.toml

@@ -51,7 +51,7 @@ dev = [
     "types-requests~=2.31",
     # temporarily use fixed version until PR #275 is merged
     # pybind11-stubgen==2.5.5
-    "pybind11-stubgen @ git+https://github.com/juelg/pybind11-stubgen@fix/class-sorting",
+    "pybind11-stubgen @ git+https://github.com/juelg/pybind11-stubgen@7f6afa59cfb8a485d3e53311be62214fefd96d75",
     "pytest==8.1.1",
     "commitizen~=3.28.0",
     "clang",

python/rcs/__main__.py

@@ -0,0 +1,36 @@
+from pathlib import Path
+from typing import Annotated
+
+import typer
+from rcs.envs.storage_wrapper import StorageWrapper
+
+app = typer.Typer()
+
+
+@app.command()
+def consolidate(
+    path: Annotated[
+        Path,
+        typer.Argument(
+            exists=True,
+            file_okay=False,
+            dir_okay=True,
+            help="The root directory of the parquet dataset to consolidate.",
+        ),
+    ]
+):
+    """
+    Consolidates a fragmented Parquet dataset into larger files.
+
+    This is useful if the recording process crashed or was interrupted,
+    leaving many small files behind.
+    """
+    typer.echo(f"Starting consolidation for: {path}")
+
+    StorageWrapper.consolidate(str(path), schema=None)
+
+    typer.echo("Done.")
+
+
+if __name__ == "__main__":
+    app()