From c26b4eef61d9a306123d6cadfa07d553a408ff14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 3 Dec 2025 21:12:48 +0100 Subject: [PATCH 01/17] feat: teleop with meta quest both single and dual arm is supported --- scripts/quest.py | 281 ++++++++++++++++++++++++++++++++++++++ scripts/quest_dual_arm.py | 277 +++++++++++++++++++++++++++++++++++++ scripts/requirements.txt | 1 + 3 files changed, 559 insertions(+) create mode 100644 scripts/quest.py create mode 100644 scripts/quest_dual_arm.py create mode 100644 scripts/requirements.txt diff --git a/scripts/quest.py b/scripts/quest.py new file mode 100644 index 00000000..34991d33 --- /dev/null +++ b/scripts/quest.py @@ -0,0 +1,281 @@ +import logging +import threading +from time import sleep + +import numpy as np +import oculus_reader +from rcs._core.common import RPY, Pose, RobotPlatform +from rcs.camera.hw import HardwareCameraSet +from rcs.envs.base import ( + ControlMode, + GripperDictType, + LimitedTQuatRelDictType, + RelativeActionSpace, + RelativeTo, +) +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg +from rcs.utils import SimpleFrameRate +from rcs_fr3.creators import RCSFR3EnvCreator +from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg +from rcs_realsense.utils import default_realsense + +# from rcs_xarm7.creators import RCSXArm7EnvCreator + +logger = logging.getLogger(__name__) + +# in order to use usb connection install adb +# sudo apt install android-tools-adb +# and run the following forwarding +# adb reverse tcp:5037 tcp:5037 + + +INCLUDE_ROTATION = True +ROBOT_IP = "192.168.101.1" + +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE + +RECORD_FPS = 30 +# set camera dict to none disable cameras +# CAMERA_DICT = { +# "side_right": "244222071045", +# "bird_eye": "243522070364", +# "arro": "243522070385", +# } +CAMERA_DICT = None + + +class QuestReader(threading.Thread): + + transform_to_robot = Pose(RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) + + def __init__( + self, env: RelativeActionSpace, trg_btn="RTr", grp_btn="RG", controller="r", clb_btn="A", home_btn="B" + ): + super().__init__() + self._reader = oculus_reader.OculusReader() + + self._resource_lock = threading.Lock() + self._env_lock = threading.Lock() + self._env = env + self._trg_btn = trg_btn + self._grp_btn = grp_btn + self._home_btn = home_btn + self._clb_btn = clb_btn + self._controller = controller + self._grp_pos = 1 + self._prev_data = None + self._exit_requested = False + self._last_controller_pose = Pose() + self._offset_pose = Pose() + self._env.set_origin_to_current() + self._step_env = False + self._set_frame = Pose() + + def next_action(self) -> Pose: + with self._resource_lock: + transform = Pose( + translation=(self._last_controller_pose.translation() - self._offset_pose.translation()), + quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(), + ) + + set_axes = Pose(quaternion=self._set_frame.rotation_q()) + + transform = ( + QuestReader.transform_to_robot + * set_axes.inverse() + * transform + * set_axes + * QuestReader.transform_to_robot.inverse() + ) + if not INCLUDE_ROTATION: + transform = Pose(translation=transform.translation()) # identity rotation + return transform, self._grp_pos + + def run(self): + rate_limiter = SimpleFrameRate(90, "teleop readout") + warning_raised = False + while not self._exit_requested: + pos, buttons = self._reader.get_transformations_and_buttons() + if not warning_raised and len(pos) == 0: + logger.warning("[Quest Reader] packets empty") + warning_raised = True + sleep(0.5) + continue + elif len(pos) == 0: + sleep(0.5) + continue + elif warning_raised: + logger.warning("[Quest Reader] packets arriving again") + warning_raised = False + + data = {"pos": pos, "buttons": buttons} + + last_controller_pose = Pose( + pose_matrix=np.array(data["pos"][self._controller]), + ) + + if data["buttons"][self._clb_btn] and ( + self._prev_data is None or not self._prev_data["buttons"][self._clb_btn] + ): + print("clb button pressed") + with self._resource_lock: + self._set_frame = last_controller_pose + + if data["buttons"][self._trg_btn] and ( + self._prev_data is None or not self._prev_data["buttons"][self._trg_btn] + ): + # trigger just pressed (first data sample with button pressed) + + with self._resource_lock: + self._offset_pose = last_controller_pose + self._last_controller_pose = last_controller_pose + + elif not data["buttons"][self._trg_btn] and ( + self._prev_data is None or self._prev_data["buttons"][self._trg_btn] + ): + # released + transform = Pose( + translation=(self._last_controller_pose.translation() - self._offset_pose.translation()), + quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(), + ) + print(np.linalg.norm(transform.translation())) + print(np.rad2deg(transform.total_angle())) + with self._resource_lock: + self._last_controller_pose = Pose() + self._offset_pose = Pose() + with self._env_lock: + self._env.set_origin_to_current() + + elif data["buttons"][self._trg_btn]: + # button is pressed + with self._resource_lock: + self._last_controller_pose = last_controller_pose + else: + # trg button is not pressed + if data["buttons"][self._home_btn] and ( + self._prev_data is None or not self._prev_data["buttons"][self._home_btn] + ): + # home button just pressed + with self._env_lock: + self._env.unwrapped.robot.move_home() + self._env.set_origin_to_current() + + if data["buttons"][self._grp_btn] and ( + self._prev_data is None or not self._prev_data["buttons"][self._grp_btn] + ): + # just pressed + self._grp_pos = 0 + if not data["buttons"][self._grp_btn] and ( + self._prev_data is None or self._prev_data["buttons"][self._grp_btn] + ): + # just released + self._grp_pos = 1 + + self._prev_data = data + rate_limiter() + + def stop(self): + self._exit_requested = True + self.join() + + def __enter__(self): + self.start() + return self + + def __exit__(self, *_): + self.stop() + + def stop_env_loop(self): + self._step_env = False + + def environment_step_loop(self): + self._env.set_origin_to_current() + rate_limiter = SimpleFrameRate(RECORD_FPS, "env loop") + self._step_env = True + while self._step_env: + if self._exit_requested: + self._step_env = False + break + transform, gripper = self.next_action() + action = dict( + LimitedTQuatRelDictType(tquat=np.concatenate([transform.translation(), transform.rotation_q()])) + ) + + action.update(GripperDictType(gripper=gripper)) + + with self._env_lock: + self._env.step(action) + rate_limiter() + + +def main(): + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + # xarm7 + # env_rel = RCSXArm7EnvCreator()( + # control_mode=ControlMode.CARTESIAN_TQuat, + # ip=ROBOT_IP, + # relative_to=RelativeTo.CONFIGURED_ORIGIN, + # max_relative_movement=0.1, + # ) + camera_set = HardwareCameraSet([default_realsense(CAMERA_DICT)]) if CAMERA_DICT is not None else None + env_rel = RCSFR3EnvCreator()( + ip=ROBOT_IP, + camera_set=camera_set, + robot_cfg=default_fr3_hw_robot_cfg(async_control=True), + control_mode=ControlMode.CARTESIAN_TQuat, + gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), + max_relative_movement=(0.5, np.deg2rad(90)), + relative_to=RelativeTo.CONFIGURED_ORIGIN, + ) + else: + # FR3 + robot_cfg = default_sim_robot_cfg("fr3_empty_world") + # xarm7 + # robot_cfg = rcs.sim.SimRobotConfig() + # robot_cfg.actuators = [ + # "act1", + # "act2", + # "act3", + # "act4", + # "act5", + # "act6", + # "act7", + # ] + # robot_cfg.joints = [ + # "joint1", + # "joint2", + # "joint3", + # "joint4", + # "joint5", + # "joint6", + # "joint7", + # ] + # robot_cfg.base = "base" + # robot_cfg.robot_type = rcs.common.RobotType.XArm7 + # robot_cfg.attachment_site = "attachment_site" + # robot_cfg.arm_collision_geoms = [] + # robot_cfg.tcp_offset = rcs.common.Pose() + # robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + # robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + collision_guard=False, + gripper_cfg=default_sim_gripper_cfg(), + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.CONFIGURED_ORIGIN, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + env_rel.reset() + + with env_rel, QuestReader(env_rel) as action_server: + action_server.environment_step_loop() + + +if __name__ == "__main__": + main() diff --git a/scripts/quest_dual_arm.py b/scripts/quest_dual_arm.py new file mode 100644 index 00000000..0299e8b5 --- /dev/null +++ b/scripts/quest_dual_arm.py @@ -0,0 +1,277 @@ +import logging +import sys +import threading +from time import sleep + +import numpy as np +import oculus_reader +from rcs._core.common import RPY, Pose, RobotPlatform +from rcs.camera.hw import HardwareCameraSet +from rcs.envs.base import ( + ControlMode, + GripperDictType, + LimitedTQuatRelDictType, + RelativeActionSpace, + RelativeTo, +) +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import 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 +from rcs_realsense.utils import default_realsense +from rcs_toolbox.wrappers.recorder import StorageWrapperParquet + +# from rcs_xarm7.creators import RCSXArm7EnvCreator + +logger = logging.getLogger(__name__) + +# in order to use usb connection install adb +# sudo apt install android-tools-adb +# and run the following forwarding +# adb reverse tcp:5037 tcp:5037 + + +INCLUDE_ROTATION = True +ROBOT2IP = { + "l": "192.168.101.1", + "r": "192.168.102.1", +} + + +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE +RECORD_FPS = 30 +# set camera dict to none disable cameras +# CAMERA_DICT = { +# "side_right": "244222071045", +# "bird_eye": "243522070364", +# "arro": "243522070385", +# } +CAMERA_DICT = None + + +class QuestReader(threading.Thread): + + transform_to_robot = Pose(RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) + + def __init__(self, env: RelativeActionSpace): + super().__init__() + self._reader = oculus_reader.OculusReader() + + self._resource_lock = threading.Lock() + self._env_lock = threading.Lock() + self._env = env + + self.controller_names = ["l", "r"] + self._trg_btn = {"l": "LTr", "r": "RTr"} + self._grp_btn = {"l": "LG", "r": "RG"} + self._clb_btn = {"l": "X", "r": "A"} + self._home_btn = "B" + + self._prev_data = None + self._exit_requested = False + self._grp_pos = {key: 1.0 for key in self.controller_names} # start with opened gripper + self._last_controller_pose = {key: Pose() for key in self.controller_names} + self._offset_pose = {key: Pose() for key in self.controller_names} + + for robot in ROBOT2IP: + # self._env.unwrapped.get_wrapper_attr("set_origin_to_current")[robot]() + # self._env.unwrapped.set_origin_to_current[robot]() + # self._env.unwrapped.envs[robot].set_origin_to_current() + self._env.envs[robot].set_origin_to_current() + + self._step_env = False + self._set_frame = {key: Pose() for key in self.controller_names} + + def next_action(self) -> Pose: + with self._resource_lock: + transforms = {} + for controller in self.controller_names: + transform = Pose( + translation=( + self._last_controller_pose[controller].translation() + - self._offset_pose[controller].translation() + ), + quaternion=( + self._last_controller_pose[controller] * self._offset_pose[controller].inverse() + ).rotation_q(), + ) + + set_axes = Pose(quaternion=self._set_frame[controller].rotation_q()) + + transform = ( + QuestReader.transform_to_robot + * set_axes.inverse() + * transform + * set_axes + * QuestReader.transform_to_robot.inverse() + ) + if not INCLUDE_ROTATION: + transform = Pose(translation=transform.translation()) # identity rotation + transforms[controller] = transform + return transforms, {key: self._grp_pos[key] for key in self.controller_names} + + def run(self): + rate_limiter = SimpleFrameRate(90, "teleop readout") + warning_raised = False + while not self._exit_requested: + pos, buttons = self._reader.get_transformations_and_buttons() + if not warning_raised and len(pos) == 0: + logger.warning("[Quest Reader] packets empty") + warning_raised = True + sleep(0.5) + continue + elif len(pos) == 0: + sleep(0.5) + continue + elif warning_raised: + logger.warning("[Quest Reader] packets arriving again") + warning_raised = False + + data = {"pos": pos, "buttons": buttons} + + for controller in self.controller_names: + last_controller_pose = Pose( + pose_matrix=np.array(data["pos"][controller]), + ) + + if data["buttons"][self._clb_btn[controller]] and ( + self._prev_data is None or not self._prev_data["buttons"][self._clb_btn[controller]] + ): + print("clb button pressed") + with self._resource_lock: + self._set_frame[controller] = last_controller_pose + + if data["buttons"][self._trg_btn[controller]] and ( + self._prev_data is None or not self._prev_data["buttons"][self._trg_btn[controller]] + ): + # 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 data["buttons"][self._trg_btn[controller]] and ( + self._prev_data is None or self._prev_data["buttons"][self._trg_btn[controller]] + ): + # released + transform = Pose( + translation=( + self._last_controller_pose[controller].translation() + - self._offset_pose[controller].translation() + ), + quaternion=( + self._last_controller_pose[controller] * self._offset_pose[controller].inverse() + ).rotation_q(), + ) + print(np.linalg.norm(transform.translation())) + print(np.rad2deg(transform.total_angle())) + with self._resource_lock: + self._last_controller_pose[controller] = Pose() + self._offset_pose[controller] = Pose() + with self._env_lock: + self._env.envs[controller].set_origin_to_current() + + elif data["buttons"][self._trg_btn[controller]]: + # button is pressed + with self._resource_lock: + self._last_controller_pose[controller] = last_controller_pose + else: + # trg button is not pressed + # TODO: deactivated for now + # if data["buttons"][self._home_btn] and ( + # self._prev_data is None or not self._prev_data["buttons"][self._home_btn] + # ): + # # home button just pressed + # with self._env_lock: + # self._env.unwrapped.robot.move_home() + # self._env.set_origin_to_current() + pass + + if data["buttons"][self._grp_btn[controller]] and ( + self._prev_data is None or not self._prev_data["buttons"][self._grp_btn[controller]] + ): + # just pressed + self._grp_pos[controller] = 0 + if not data["buttons"][self._grp_btn[controller]] and ( + self._prev_data is None or self._prev_data["buttons"][self._grp_btn[controller]] + ): + # just released + self._grp_pos[controller] = 1 + + self._prev_data = data + rate_limiter() + + def stop(self): + self._exit_requested = True + self.join() + + def __enter__(self): + self.start() + return self + + def __exit__(self, *_): + self.stop() + + def stop_env_loop(self): + self._step_env = False + + def environment_step_loop(self): + rate_limiter = SimpleFrameRate(RECORD_FPS, "env loop") + self._step_env = True + while self._step_env: + 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()])) + ) + + 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 + env_rel = RCSFR3MultiEnvCreator()( + name2ip=ROBOT2IP, + camera_set=camera_set, + robot_cfg=default_fr3_hw_robot_cfg(async_control=True), + control_mode=ControlMode.CARTESIAN_TQuat, + gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), + max_relative_movement=(0.5, np.deg2rad(90)), + relative_to=RelativeTo.CONFIGURED_ORIGIN, + ) + else: + # FR3 + robot_cfg = default_sim_robot_cfg("fr3_empty_world") + + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + collision_guard=False, + gripper_cfg=default_sim_gripper_cfg(), + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.CONFIGURED_ORIGIN, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + env_rel.reset() + + with env_rel, QuestReader(env_rel) as action_server: + action_server.environment_step_loop() + + +if __name__ == "__main__": + main() diff --git a/scripts/requirements.txt b/scripts/requirements.txt new file mode 100644 index 00000000..8d15e8fa --- /dev/null +++ b/scripts/requirements.txt @@ -0,0 +1 @@ +oculus_reader @ git+https://github.com/rail-berkeley/oculus_reader.git@c52b4b6193879b6cf32a1db1bd5727652626f00d \ No newline at end of file From 456172d93eadba75a8e9c2979591791e436a6de5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 5 Dec 2025 17:19:27 +0100 Subject: [PATCH 02/17] feat: added iris teleop script --- scripts/quest_iris_dual_arm.py | 287 +++++++++++++++++++++++++++++++++ scripts/requirements.txt | 3 +- 2 files changed, 289 insertions(+), 1 deletion(-) create mode 100644 scripts/quest_iris_dual_arm.py diff --git a/scripts/quest_iris_dual_arm.py b/scripts/quest_iris_dual_arm.py new file mode 100644 index 00000000..b1d8e0df --- /dev/null +++ b/scripts/quest_iris_dual_arm.py @@ -0,0 +1,287 @@ +import logging +import threading +from time import sleep +# from simpub.core.node_manager import init_xr_node_manager +# from simpub.xr_device.meta_quest3 import MetaQuest3 + +from simpub.core.node_manager import init_xr_node_manager +from simpub.xr_device.meta_quest3 import MetaQuest3 + +import numpy as np +from rcs._core.common import RPY, Pose, RobotPlatform +from rcs.camera.hw import HardwareCameraSet +from rcs.envs.base import ( + ControlMode, + GripperDictType, + LimitedTQuatRelDictType, + RelativeActionSpace, + RelativeTo, +) +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import 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 +from rcs_realsense.utils import default_realsense +from rcs_toolbox.wrappers.recorder import StorageWrapperParquet + +# from rcs_xarm7.creators import RCSXArm7EnvCreator + +logger = logging.getLogger(__name__) + +# in order to use usb connection install adb +# sudo apt install android-tools-adb +# download the iris apk from the following repo release: https://github.com/intuitive-robots/IRIS-Meta-Quest3 +# install it on your quest with +# adb install IRIS-Meta-Quest3.apk + + +INCLUDE_ROTATION = True +ROBOT2IP = { + "left": "192.168.101.1", + "right": "192.168.102.1", +} + + +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE +RECORD_FPS = 30 +# set camera dict to none disable cameras +# CAMERA_DICT = { +# "side_right": "244222071045", +# "bird_eye": "243522070364", +# "arro": "243522070385", +# } +CAMERA_DICT = None +MQ3_ADDR = "192.168.0.134" + + + +class QuestReader(threading.Thread): + + transform_to_robot = Pose(RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) + + def __init__(self, env: RelativeActionSpace): + super().__init__() + # self._reader = oculus_reader.OculusReader() + + net_manager = init_xr_node_manager(MQ3_ADDR) + net_manager.start_discover_node_loop() + self._reader = MetaQuest3("RCSNode") + + self._resource_lock = threading.Lock() + self._env_lock = threading.Lock() + self._env = env + + self.controller_names = ["left", "right"] + self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} + self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} + self._clb_btn = {"left": "X", "right": "A"} + self._home_btn = "B" + + self._prev_data = None + self._exit_requested = False + self._grp_pos = {key: 1.0 for key in self.controller_names} # start with opened gripper + self._last_controller_pose = {key: Pose() for key in self.controller_names} + self._offset_pose = {key: Pose() for key in self.controller_names} + + for robot in ROBOT2IP: + # self._env.unwrapped.get_wrapper_attr("set_origin_to_current")[robot]() + # self._env.unwrapped.set_origin_to_current[robot]() + # self._env.unwrapped.envs[robot].set_origin_to_current() + self._env.envs[robot].set_origin_to_current() + + self._step_env = False + self._set_frame = {key: Pose() for key in self.controller_names} + + def next_action(self) -> Pose: + with self._resource_lock: + transforms = {} + for controller in self.controller_names: + transform = Pose( + translation=( + self._last_controller_pose[controller].translation() + - self._offset_pose[controller].translation() + ), + quaternion=( + self._last_controller_pose[controller] * self._offset_pose[controller].inverse() + ).rotation_q(), + ) + + set_axes = Pose(quaternion=self._set_frame[controller].rotation_q()) + + transform = ( + QuestReader.transform_to_robot + * set_axes.inverse() + * transform + * set_axes + * QuestReader.transform_to_robot.inverse() + ) + if not INCLUDE_ROTATION: + transform = Pose(translation=transform.translation()) # identity rotation + transforms[controller] = transform + return transforms, {key: self._grp_pos[key] for key in self.controller_names} + + def run(self): + rate_limiter = SimpleFrameRate(90, "teleop readout") + warning_raised = False + while not self._exit_requested: + # pos, buttons = self._reader.get_transformations_and_buttons() + input_data = self._reader.get_controller_data() + if not warning_raised and input_data: + logger.warning("[Quest Reader] packets empty") + warning_raised = True + sleep(0.5) + continue + elif input_data: + sleep(0.5) + continue + elif warning_raised: + logger.warning("[Quest Reader] packets arriving again") + warning_raised = False + + + for controller in self.controller_names: + last_controller_pose = Pose( + translation=np.array(input_data[controller]["pos"]), quaternion=np.array(input_data[controller]["rot"]) + ) + + # if input_data[self._clb_btn[controller]] and ( + # self._prev_data is None or not self._prev_data[self._clb_btn[controller]] + # ): + # print("clb button pressed") + # with self._resource_lock: + # self._set_frame[controller] = 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 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]] + ): + # released + transform = Pose( + translation=( + self._last_controller_pose[controller].translation() + - self._offset_pose[controller].translation() + ), + quaternion=( + self._last_controller_pose[controller] * self._offset_pose[controller].inverse() + ).rotation_q(), + ) + print(np.linalg.norm(transform.translation())) + print(np.rad2deg(transform.total_angle())) + with self._resource_lock: + self._last_controller_pose[controller] = Pose() + self._offset_pose[controller] = Pose() + with self._env_lock: + self._env.envs[controller].set_origin_to_current() + + elif input_data[controller][self._trg_btn[controller]]: + # button is pressed + with self._resource_lock: + self._last_controller_pose[controller] = last_controller_pose + else: + # trg button is not pressed + # TODO: deactivated for now + # if data["buttons"][self._home_btn] and ( + # self._prev_data is None or not self._prev_data["buttons"][self._home_btn] + # ): + # # home button just pressed + # with self._env_lock: + # self._env.unwrapped.robot.move_home() + # self._env.set_origin_to_current() + pass + + 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 + + self._prev_data = input_data + rate_limiter() + + def stop(self): + self._exit_requested = True + self.join() + + def __enter__(self): + self.start() + return self + + def __exit__(self, *_): + self.stop() + + def stop_env_loop(self): + self._step_env = False + + def environment_step_loop(self): + rate_limiter = SimpleFrameRate(RECORD_FPS, "env loop") + self._step_env = True + while self._step_env: + 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()])) + ) + + 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 + env_rel = RCSFR3MultiEnvCreator()( + name2ip=ROBOT2IP, + camera_set=camera_set, + robot_cfg=default_fr3_hw_robot_cfg(async_control=True), + control_mode=ControlMode.CARTESIAN_TQuat, + gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), + max_relative_movement=(0.5, np.deg2rad(90)), + relative_to=RelativeTo.CONFIGURED_ORIGIN, + ) + else: + # FR3 + robot_cfg = default_sim_robot_cfg("fr3_empty_world") + + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + collision_guard=False, + gripper_cfg=default_sim_gripper_cfg(), + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.CONFIGURED_ORIGIN, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + env_rel.reset() + + with env_rel, QuestReader(env_rel) as action_server: + action_server.environment_step_loop() + + +if __name__ == "__main__": + main() diff --git a/scripts/requirements.txt b/scripts/requirements.txt index 8d15e8fa..268e92e4 100644 --- a/scripts/requirements.txt +++ b/scripts/requirements.txt @@ -1 +1,2 @@ -oculus_reader @ git+https://github.com/rail-berkeley/oculus_reader.git@c52b4b6193879b6cf32a1db1bd5727652626f00d \ No newline at end of file +oculus_reader @ git+https://github.com/rail-berkeley/oculus_reader.git@c52b4b6193879b6cf32a1db1bd5727652626f00d +simpub @ git+https://github.com/intuitive-robots/SimPublisher.git \ No newline at end of file From ab89ba32a092b8957bfb416bb5efd0777208491b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 8 Dec 2025 15:20:41 +0100 Subject: [PATCH 03/17] fix(sim): allow scene path fallback --- python/rcs/envs/utils.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 6f1e4460..6af1f2bd 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -18,7 +18,10 @@ def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim robot_cfg = rcs.sim.SimRobotConfig() robot_cfg.robot_type = rcs.scenes[scene].robot_type robot_cfg.add_id(idx) - robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb + if rcs.scenes[scene].mjb is not None: + robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb + else: + robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjcf_scene robot_cfg.kinematic_model_path = rcs.scenes[scene].mjcf_robot # robot_cfg.kinematic_model_path = rcs.scenes[scene].urdf return robot_cfg From de6884905f957971d911a9ac7aa1205100af2d3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 8 Dec 2025 15:27:36 +0100 Subject: [PATCH 04/17] fix(teleop): multi robot env and iris --- scripts/quest_dual_arm.py | 13 +++++--- scripts/quest_iris_dual_arm.py | 58 ++++++++++++++++++++-------------- 2 files changed, 43 insertions(+), 28 deletions(-) diff --git a/scripts/quest_dual_arm.py b/scripts/quest_dual_arm.py index 0299e8b5..3d60178d 100644 --- a/scripts/quest_dual_arm.py +++ b/scripts/quest_dual_arm.py @@ -6,6 +6,7 @@ import numpy as np import oculus_reader from rcs._core.common import RPY, Pose, RobotPlatform +from rcs._core.sim import SimConfig from rcs.camera.hw import HardwareCameraSet from rcs.envs.base import ( ControlMode, @@ -14,13 +15,12 @@ RelativeActionSpace, RelativeTo, ) -from rcs.envs.creators import SimEnvCreator +from rcs.envs.creators import SimMultiEnvCreator from rcs.envs.utils import 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 from rcs_realsense.utils import default_realsense -from rcs_toolbox.wrappers.recorder import StorageWrapperParquet # from rcs_xarm7.creators import RCSXArm7EnvCreator @@ -256,16 +256,19 @@ def main(): # FR3 robot_cfg = default_sim_robot_cfg("fr3_empty_world") - env_rel = SimEnvCreator()( + sim_cfg = SimConfig() + sim_cfg.async_control = True + env_rel, sim = SimMultiEnvCreator()( + name2id=ROBOT2IP, robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, - collision_guard=False, gripper_cfg=default_sim_gripper_cfg(), # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.CONFIGURED_ORIGIN, + sim_cfg=sim_cfg, ) - env_rel.get_wrapper_attr("sim").open_gui() + sim.open_gui() env_rel.reset() diff --git a/scripts/quest_iris_dual_arm.py b/scripts/quest_iris_dual_arm.py index b1d8e0df..b527939c 100644 --- a/scripts/quest_iris_dual_arm.py +++ b/scripts/quest_iris_dual_arm.py @@ -1,14 +1,10 @@ import logging import threading from time import sleep -# from simpub.core.node_manager import init_xr_node_manager -# from simpub.xr_device.meta_quest3 import MetaQuest3 - -from simpub.core.node_manager import init_xr_node_manager -from simpub.xr_device.meta_quest3 import MetaQuest3 import numpy as np from rcs._core.common import RPY, Pose, RobotPlatform +from rcs._core.sim import SimConfig from rcs.camera.hw import HardwareCameraSet from rcs.envs.base import ( ControlMode, @@ -17,13 +13,16 @@ RelativeActionSpace, RelativeTo, ) -from rcs.envs.creators import SimEnvCreator +from rcs.envs.creators import SimMultiEnvCreator from rcs.envs.utils import 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 from rcs_realsense.utils import default_realsense -from rcs_toolbox.wrappers.recorder import StorageWrapperParquet +from simpub.core.simpub_server import SimPublisher +from simpub.parser.simdata import SimObject, SimScene +from simpub.sim.mj_publisher import MujocoPublisher +from simpub.xr_device.meta_quest3 import MetaQuest3 # from rcs_xarm7.creators import RCSXArm7EnvCreator @@ -39,7 +38,7 @@ INCLUDE_ROTATION = True ROBOT2IP = { "left": "192.168.101.1", - "right": "192.168.102.1", + # "right": "192.168.102.1", } @@ -53,27 +52,35 @@ # "arro": "243522070385", # } CAMERA_DICT = None -MQ3_ADDR = "192.168.0.134" +MQ3_ADDR = "10.228.9.83" + +class MySimPublisher(SimPublisher): + def get_update(self): + return {} + + +class MySimScene(SimScene): + def __init__(self): + super().__init__() + self.root = SimObject(name="root") class QuestReader(threading.Thread): - transform_to_robot = Pose(RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) + transform_to_robot = Pose() # RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) def __init__(self, env: RelativeActionSpace): super().__init__() # self._reader = oculus_reader.OculusReader() - net_manager = init_xr_node_manager(MQ3_ADDR) - net_manager.start_discover_node_loop() self._reader = MetaQuest3("RCSNode") self._resource_lock = threading.Lock() self._env_lock = threading.Lock() self._env = env - self.controller_names = ["left", "right"] + self.controller_names = ["left"] # , "right"] self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} self._clb_btn = {"left": "X", "right": "A"} @@ -128,30 +135,30 @@ def run(self): while not self._exit_requested: # pos, buttons = self._reader.get_transformations_and_buttons() input_data = self._reader.get_controller_data() - if not warning_raised and input_data: + if not warning_raised and input_data is None: logger.warning("[Quest Reader] packets empty") warning_raised = True sleep(0.5) continue - elif input_data: + elif input_data is None: sleep(0.5) continue elif warning_raised: logger.warning("[Quest Reader] packets arriving again") warning_raised = False - for controller in self.controller_names: last_controller_pose = Pose( - translation=np.array(input_data[controller]["pos"]), quaternion=np.array(input_data[controller]["rot"]) + translation=np.array(input_data[controller]["pos"]), + quaternion=np.array(input_data[controller]["rot"]), ) # if input_data[self._clb_btn[controller]] and ( # self._prev_data is None or not self._prev_data[self._clb_btn[controller]] # ): - # print("clb button pressed") - # with self._resource_lock: - # self._set_frame[controller] = last_controller_pose + # print("clb button pressed") + # with self._resource_lock: + # self._set_frame[controller] = 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]] @@ -262,20 +269,25 @@ def main(): max_relative_movement=(0.5, np.deg2rad(90)), relative_to=RelativeTo.CONFIGURED_ORIGIN, ) + MySimPublisher(MySimScene(), MQ3_ADDR) else: # FR3 robot_cfg = default_sim_robot_cfg("fr3_empty_world") - env_rel = SimEnvCreator()( + sim_cfg = SimConfig() + sim_cfg.async_control = True + env_rel, sim = SimMultiEnvCreator()( + name2id=ROBOT2IP, robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, - collision_guard=False, gripper_cfg=default_sim_gripper_cfg(), # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, relative_to=RelativeTo.CONFIGURED_ORIGIN, + sim_cfg=sim_cfg, ) - env_rel.get_wrapper_attr("sim").open_gui() + sim.open_gui() + MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) env_rel.reset() From 2ebe3e85b373e516b4a76f8c23d27cf2b1c8be20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 8 Dec 2025 16:24:13 +0100 Subject: [PATCH 05/17] feat: add multi robot sim wrapper --- python/rcs/envs/creators.py | 48 +++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index e336ae80..b0124ebb 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -14,6 +14,7 @@ ControlMode, GripperWrapper, HandWrapper, + MultiRobotWrapper, RelativeActionSpace, RelativeTo, RobotEnv, @@ -131,6 +132,53 @@ def __call__( # type: ignore return env +class SimMultiEnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + name2id: dict[str, str], + control_mode: ControlMode, + robot_cfg: rcs.sim.SimRobotConfig, + gripper_cfg: rcs.sim.SimGripperConfig | None = None, + sim_cfg: rcs.sim.SimConfig | None = None, + hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, + cameras: dict[str, SimCameraConfig] | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + sim_wrapper: Type[SimWrapper] | None = None, + ) -> gym.Env: + + simulation = sim.Sim(robot_cfg.mjcf_scene_path, sim_cfg) + ik = rcs.common.Pin( + robot_cfg.kinematic_model_path, + robot_cfg.attachment_site, + urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), + ) + # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) + + robots: dict[str, rcs.sim.SimRobotConfig] = {} + for key, ip in name2id.items(): + robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=robot_cfg) + + envs = {} + for key, ip in name2id.items(): + env: gym.Env = RobotEnv(robots[key], control_mode) + env = RobotSimWrapper(env, simulation, sim_wrapper) + if gripper_cfg is not None: + gripper = rcs.sim.SimGripper(simulation, gripper_cfg) + env = GripperWrapper(env, gripper, binary=True) + + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + envs[key] = env + + env = MultiRobotWrapper(envs) + if cameras is not None: + camera_set = typing.cast( + BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) + ) + env = CameraSetWrapper(env, camera_set, include_depth=True) + return env, simulation + class SimTaskEnvCreator(EnvCreator): def __call__( # type: ignore From c31210fdd9449cec0d777957eb4012f9f1b14bde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 10:32:24 +0100 Subject: [PATCH 06/17] feat: storage wrapper for parquet recording --- pyproject.toml | 2 + python/rcs/envs/storage_wrapper.py | 156 +++++++++++++++++++++++++++++ 2 files changed, 158 insertions(+) create mode 100644 python/rcs/envs/storage_wrapper.py diff --git a/pyproject.toml b/pyproject.toml index 81f4b465..b317ef2b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -28,6 +28,8 @@ dependencies = ["websockets>=11.0", "pyopengl>=3.1.9", "ompl>=1.7.0", "rpyc==6.0.2", + "pyarrow", + "simplejpeg", ] readme = "README.md" maintainers = [ diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py new file mode 100644 index 00000000..79f902b1 --- /dev/null +++ b/python/rcs/envs/storage_wrapper.py @@ -0,0 +1,156 @@ +import operator +from typing import Any, Optional +from uuid import uuid4 +from queue import Queue +import pyarrow.dataset as ds +import pyarrow as pa +import numpy as np +from itertools import chain +import simplejpeg +from concurrent.futures import ThreadPoolExecutor, wait +import gymnasium as gym + + +class StorageWrapper(gym.Wrapper): + QueueSentinel = None + + def __init__( + self, + env: gym.Env, + base_dir: str, + batch_size: int, + schema: Optional[pa.Schema] = None, + basename_template: Optional[str] = None, + max_rows_per_group: Optional[int] = None, + max_rows_per_file: Optional[int] = None, + ): + """ + Asynchronously log environment transitions to a Parquet + dataset on disk. + + Observation handling: + - Expects observations to be dictionaries. + - RGB camera frames are JPEG-encoded. + - Numpy arrays with ndim > 1 inside the observation dict are flattened + in-place, and their original shapes are stored alongside as + ``"_shape"``. Nested dicts are traversed recursively. + - Lists/tuples of arrays are not supported. + - ``close()`` must be called to flush the final batch. + + Parameters + ---------- + env : gym.Env + The environment to wrap. + base_dir : str + Output directory where the Parquet dataset will be written. + batch_size : int + Number of transitions to accumulate before flushing a RecordBatch + to the writer queue. + schema : Optional[pa.Schema], default=None + Optional Arrow schema to enforce for all batches. If None, the schema + is inferred from the first flushed batch and then reused. + basename_template : Optional[str], default=None + Template controlling Parquet file basenames. Passed through to + ``pyarrow.dataset.write_dataset``. + max_rows_per_group : Optional[int], default=None + Maximum row count per Parquet row group. Passed through to + ``pyarrow.dataset.write_dataset``. + max_rows_per_file : Optional[int], default=None + Maximum row count per Parquet file. Passed through to + ``pyarrow.dataset.write_dataset``. + """ + + super().__init__(env) + self.base_dir = base_dir + self.batch_size = batch_size + self.schema = schema + self.basename_template = basename_template + self.max_rows_per_group = max_rows_per_group + self.max_rows_per_file = max_rows_per_file + self.buffer = [] + self.step_cnt = 0 + self.thread_pool = ThreadPoolExecutor() + self.queue = Queue(maxsize=2) + self.uuid = uuid4() + self._writer_future = self.thread_pool.submit(self._writer_worker) + + def _generator_from_queue(self): + while (batch := self.queue.get()) is not self.QueueSentinel: + yield batch + + def _writer_worker(self): + gen = self._generator_from_queue() + first = next(gen) + ds.write_dataset( + data=chain([first], gen), + base_dir=self.base_dir, + format="parquet", + schema=self.schema, + existing_data_behavior="overwrite_or_ignore", + basename_template=self.basename_template, + max_rows_per_group=self.max_rows_per_group, + max_rows_per_file=self.max_rows_per_file, + partitioning=ds.partitioning( + schema=pa.schema(fields=[pa.field("uuid", pa.binary(36))]), + flavor="filename", + ), + ) + + def _flush(self): + batch = pa.RecordBatch.from_pylist(self.buffer, schema=self.schema) + if self.schema is None: + self.schema = batch.schema + self.queue.put(batch) + self.buffer.clear() + + def _flatten_arrays(self, d: dict[str, Any]): + # NOTE: list / tuples of arrays not supported + updates = {} + for k, v in d.items(): + if isinstance(v, dict): + self._flatten_arrays(v) + elif isinstance(v, np.ndarray) and len(v.shape) > 1: + d[k] = v.flatten() + updates[f"{k}_shape"] = v.shape + d.update(updates) + + def _encode_images(self, obs: dict[str, Any]): + _ = [ + *self.thread_pool.map( + lambda cam: operator.setitem( + obs["frames"][cam]["rgb"], + "data", + simplejpeg.encode_jpeg(np.ascontiguousarray(obs["frames"][cam]["rgb"]["data"])), + ), + obs["frames"], + ) + ] + + def step(self, action): + # NOTE: expects the observation to be a dictionary + if self._writer_future.done(): + exc = self._writer_future.exception() + assert exc is not None + msg = "Writer thread failed" + raise RuntimeError(msg) from exc + obs, reward, terminated, truncated, info = self.env.step(action) + assert isinstance(obs, dict) + self._encode_images(obs) + self._flatten_arrays(obs) + self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes}) + self.step_cnt += 1 + if len(self.buffer) == self.batch_size: + self._flush() + return obs, reward, terminated, truncated, info + + def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = None): + if len(self.buffer) > 0: + self._flush() + obs, info = self.env.reset() + self.step_cnt = 0 + self.uuid = uuid4() + return obs, info + + def close(self): + self.queue.put(self.QueueSentinel) + wait([self._writer_future]) From f21012aadaaabd61b8c94d2e964c01d6301c4eb5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 10:44:24 +0100 Subject: [PATCH 07/17] feat: storage wrapper added recording logic --- python/rcs/envs/storage_wrapper.py | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index 79f902b1..dffbbd48 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -69,6 +69,7 @@ def __init__( self.max_rows_per_file = max_rows_per_file self.buffer = [] self.step_cnt = 0 + self._pause = True self.thread_pool = ThreadPoolExecutor() self.queue = Queue(maxsize=2) self.uuid = uuid4() @@ -134,23 +135,35 @@ def step(self, action): msg = "Writer thread failed" raise RuntimeError(msg) from exc obs, reward, terminated, truncated, info = self.env.step(action) - assert isinstance(obs, dict) - self._encode_images(obs) - self._flatten_arrays(obs) - self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes}) - self.step_cnt += 1 - if len(self.buffer) == self.batch_size: - self._flush() + if not self._pause: + assert isinstance(obs, dict) + self._encode_images(obs) + self._flatten_arrays(obs) + self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes}) + self.step_cnt += 1 + if len(self.buffer) == self.batch_size: + self._flush() return obs, reward, terminated, truncated, info + def stop_record(self): + self._pause = True + if len(self.buffer) > 0: + self._flush() + + def start_record(self): + self._pause = False + def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = None): if len(self.buffer) > 0: self._flush() + self._pause = True obs, info = self.env.reset() self.step_cnt = 0 self.uuid = uuid4() return obs, info def close(self): + if len(self.buffer) > 0: + self._flush() self.queue.put(self.QueueSentinel) wait([self._writer_future]) From 6fd4400d97e8358700cddfa9d5cf8b350dfadd6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 11:18:52 +0100 Subject: [PATCH 08/17] feat(recorder): success attribute --- python/rcs/envs/storage_wrapper.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index dffbbd48..ada6616a 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -20,6 +20,7 @@ def __init__( base_dir: str, batch_size: int, schema: Optional[pa.Schema] = None, + start_record: bool = False, basename_template: Optional[str] = None, max_rows_per_group: Optional[int] = None, max_rows_per_file: Optional[int] = None, @@ -70,6 +71,7 @@ def __init__( self.buffer = [] self.step_cnt = 0 self._pause = True + self._success = start_record self.thread_pool = ThreadPoolExecutor() self.queue = Queue(maxsize=2) self.uuid = uuid4() @@ -139,12 +141,17 @@ def step(self, action): assert isinstance(obs, dict) self._encode_images(obs) self._flatten_arrays(obs) - self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes}) + if "success" in info and info["success"]: + self.success() + self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes, "success": self._success}) self.step_cnt += 1 if len(self.buffer) == self.batch_size: self._flush() return obs, reward, terminated, truncated, info + def success(self): + self._success = True + def stop_record(self): self._pause = True if len(self.buffer) > 0: @@ -157,6 +164,7 @@ def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = Non if len(self.buffer) > 0: self._flush() self._pause = True + self._success = False obs, info = self.env.reset() self.step_cnt = 0 self.uuid = uuid4() From e7a28a78683be7b33ad945446e1414c60da46690 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 11:20:46 +0100 Subject: [PATCH 09/17] feat(recorder): add delayed action --- python/rcs/envs/storage_wrapper.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index ada6616a..ac568c4b 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -72,6 +72,7 @@ def __init__( self.step_cnt = 0 self._pause = True self._success = start_record + self._prev_action = None self.thread_pool = ThreadPoolExecutor() self.queue = Queue(maxsize=2) self.uuid = uuid4() @@ -143,7 +144,8 @@ def step(self, action): self._flatten_arrays(obs) if "success" in info and info["success"]: self.success() - self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes, "success": self._success}) + self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes, "success": self._success, "action": self._prev_action}) + self._prev_action = action self.step_cnt += 1 if len(self.buffer) == self.batch_size: self._flush() @@ -165,6 +167,7 @@ def reset(self, *, seed: int | None = None, options: dict[str, Any] | None = Non self._flush() self._pause = True self._success = False + self._prev_action = None obs, info = self.env.reset() self.step_cnt = 0 self.uuid = uuid4() From 152a6d27f2896484c0c076afe238290cbaf3c336 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 11:32:33 +0100 Subject: [PATCH 10/17] feat: digital twin wrapper --- python/rcs/envs/sim.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 74eb2c78..d9e733cd 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -449,3 +449,19 @@ def step(self, action: dict[str, Any]): # normalize reward /= 5 # type: ignore return obs, reward, success, truncated, info + +class DigitalTwin(gym.Wrapper): + + def __init__(self, env, twin_env): + super().__init__(env) + self.twin_env = twin_env + assert self.twin_env.unwrapped.get_control_mode() == ControlMode.JOINTS + + + def step(self, action): + obs, reward, terminated, truncated, info = super().step(action) + + twin_obs, _, _, _, _ = self.twin_env.step(obs) + info["twin_obs"] = twin_obs + return obs, reward, terminated, truncated, info + From cafec31a28d2eb91bda25130a2b438e989c9d9fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 11:33:05 +0100 Subject: [PATCH 11/17] feat(teleop): recording logic and digital twin --- scripts/quest_iris_dual_arm.py | 63 +++++++++++++++++++++++++++++----- 1 file changed, 55 insertions(+), 8 deletions(-) diff --git a/scripts/quest_iris_dual_arm.py b/scripts/quest_iris_dual_arm.py index b527939c..94a5e185 100644 --- a/scripts/quest_iris_dual_arm.py +++ b/scripts/quest_iris_dual_arm.py @@ -14,6 +14,8 @@ RelativeTo, ) from rcs.envs.creators import SimMultiEnvCreator +from rcs.envs.sim import DigitalTwin +from rcs.envs.storage_wrapper import StorageWrapper from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg from rcs.utils import SimpleFrameRate from rcs_fr3.creators import RCSFR3MultiEnvCreator @@ -54,6 +56,8 @@ CAMERA_DICT = None MQ3_ADDR = "10.228.9.83" +DATASET_PATH = "test_data_iris_dual_arm" + class MySimPublisher(SimPublisher): def get_update(self): @@ -83,8 +87,10 @@ def __init__(self, env: RelativeActionSpace): self.controller_names = ["left"] # , "right"] self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} - self._clb_btn = {"left": "X", "right": "A"} - self._home_btn = "B" + self._start_btn = "X" + self._stop_btn = "Y" + self._home_btn = "A" + self._unsuccessful_btn = "B" self._prev_data = None self._exit_requested = False @@ -147,18 +153,39 @@ def run(self): logger.warning("[Quest Reader] packets arriving again") warning_raised = False + # start recording + if input_data[self._start_btn] and ( + self._prev_data is None or not self._prev_data[self._start_btn] + ): + print("start button pressed") + with self._env_lock: + self._env.unwrapped.get_wrapper_attr("start_record")() + + 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: + # set successful + self._env.unwrapped.get_wrapper_attr("success")() + # this might also move the robot to the home position + self._env.reset() + + # reset unsuccessful + if input_data[self._unsuccessful_btn] and ( + self._prev_data is None or not self._prev_data[self._unsuccessful_btn] + ): + print("reset unsuccessful pressed: resetting env") + with self._env_lock: + self._env.reset() + + for controller in self.controller_names: last_controller_pose = Pose( translation=np.array(input_data[controller]["pos"]), quaternion=np.array(input_data[controller]["rot"]), ) - # if input_data[self._clb_btn[controller]] and ( - # self._prev_data is None or not self._prev_data[self._clb_btn[controller]] - # ): - # print("clb button pressed") - # with self._resource_lock: - # self._set_frame[controller] = 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]] @@ -269,7 +296,27 @@ def main(): max_relative_movement=(0.5, np.deg2rad(90)), relative_to=RelativeTo.CONFIGURED_ORIGIN, ) + env_rel = StorageWrapper(env_rel, DATASET_PATH, 32, max_rows_per_group=100, max_rows_per_file=1000) MySimPublisher(MySimScene(), MQ3_ADDR) + + # robot_cfg = default_sim_robot_cfg("fr3_empty_world") + # sim_cfg = SimConfig() + # sim_cfg.async_control = True + # twin_env, sim = SimMultiEnvCreator()( + # name2id=ROBOT2IP, + # robot_cfg=robot_cfg, + # control_mode=ControlMode.CARTESIAN_TQuat, + # gripper_cfg=default_sim_gripper_cfg(), + # # cameras=default_mujoco_cameraset_cfg(), + # max_relative_movement=0.5, + # relative_to=RelativeTo.CONFIGURED_ORIGIN, + # sim_cfg=sim_cfg, + # ) + # sim.open_gui() + # MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) + # env_rel = DigitalTwin(env_rel, twin_env) + + else: # FR3 robot_cfg = default_sim_robot_cfg("fr3_empty_world") From 67948b643a8e53ddf531e01f90a0789a065fc7ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 12:35:18 +0100 Subject: [PATCH 12/17] fix(recorder): uuid partioning, added instruction - fix filename partioning after uuid - feat: added instruction - fix: frame key safeguard --- python/rcs/envs/storage_wrapper.py | 11 +++++++---- scripts/quest_iris_dual_arm.py | 22 +++++++++++----------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index ac568c4b..8e9060a2 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -18,7 +18,8 @@ def __init__( self, env: gym.Env, base_dir: str, - batch_size: int, + instruction: str, + batch_size: int = 32, schema: Optional[pa.Schema] = None, start_record: bool = False, basename_template: Optional[str] = None, @@ -71,6 +72,7 @@ def __init__( self.buffer = [] self.step_cnt = 0 self._pause = True + self.instruction = instruction self._success = start_record self._prev_action = None self.thread_pool = ThreadPoolExecutor() @@ -95,7 +97,7 @@ def _writer_worker(self): max_rows_per_group=self.max_rows_per_group, max_rows_per_file=self.max_rows_per_file, partitioning=ds.partitioning( - schema=pa.schema(fields=[pa.field("uuid", pa.binary(36))]), + schema=pa.schema(fields=[pa.field("uuid", pa.string())]), flavor="filename", ), ) @@ -140,11 +142,12 @@ def step(self, action): obs, reward, terminated, truncated, info = self.env.step(action) if not self._pause: assert isinstance(obs, dict) - self._encode_images(obs) + if "frames" in obs: + self._encode_images(obs) self._flatten_arrays(obs) if "success" in info and info["success"]: self.success() - self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.bytes, "success": self._success, "action": self._prev_action}) + self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.hex, "success": self._success, "action": self._prev_action, "instruction": self.instruction}) self._prev_action = action self.step_cnt += 1 if len(self.buffer) == self.batch_size: diff --git a/scripts/quest_iris_dual_arm.py b/scripts/quest_iris_dual_arm.py index 94a5e185..f562272f 100644 --- a/scripts/quest_iris_dual_arm.py +++ b/scripts/quest_iris_dual_arm.py @@ -39,8 +39,8 @@ INCLUDE_ROTATION = True ROBOT2IP = { - "left": "192.168.101.1", - # "right": "192.168.102.1", + # "left": "192.168.102.1", + "right": "192.168.102.1", } @@ -54,9 +54,10 @@ # "arro": "243522070385", # } CAMERA_DICT = None -MQ3_ADDR = "10.228.9.83" +MQ3_ADDR = "10.42.0.1" DATASET_PATH = "test_data_iris_dual_arm" +INSTRUCTION = "build a tower with the blocks in front of you" class MySimPublisher(SimPublisher): @@ -84,13 +85,12 @@ def __init__(self, env: RelativeActionSpace): self._env_lock = threading.Lock() self._env = env - self.controller_names = ["left"] # , "right"] + self.controller_names = ["right"] # , "right"] self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} - self._start_btn = "X" - self._stop_btn = "Y" - self._home_btn = "A" - self._unsuccessful_btn = "B" + self._start_btn = "A" + self._stop_btn = "B" + self._unsuccessful_btn = "Y" self._prev_data = None self._exit_requested = False @@ -159,7 +159,7 @@ def run(self): ): print("start button pressed") with self._env_lock: - self._env.unwrapped.get_wrapper_attr("start_record")() + self._env.get_wrapper_attr("start_record")() if input_data[self._stop_btn] and ( self._prev_data is None or not self._prev_data[self._stop_btn] @@ -167,7 +167,7 @@ def run(self): print("reset successful pressed: resetting env") with self._env_lock: # set successful - self._env.unwrapped.get_wrapper_attr("success")() + self._env.get_wrapper_attr("success")() # this might also move the robot to the home position self._env.reset() @@ -296,7 +296,7 @@ def main(): max_relative_movement=(0.5, np.deg2rad(90)), relative_to=RelativeTo.CONFIGURED_ORIGIN, ) - env_rel = StorageWrapper(env_rel, DATASET_PATH, 32, max_rows_per_group=100, max_rows_per_file=1000) + env_rel = StorageWrapper(env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000) MySimPublisher(MySimScene(), MQ3_ADDR) # robot_cfg = default_sim_robot_cfg("fr3_empty_world") From 1eeaaf182690135b8e8e20b51d59c7d8098b53b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 9 Dec 2025 19:22:32 +0100 Subject: [PATCH 13/17] fix(fr3): resetting behavior --- extensions/rcs_fr3/src/hw/Franka.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 1b6972a0..2014ccaf 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -174,6 +174,7 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { if (this->running_controller == Controller::none) { this->controller_time = 0.0; this->get_joint_position(); + this->joint_interpolator = common::LinearJointPositionTrajInterpolator(); } else if (this->running_controller != Controller::jsc) { // runtime error throw std::runtime_error( @@ -208,7 +209,7 @@ void Franka::osc_set_cartesian_position( if (this->running_controller == Controller::none) { this->controller_time = 0.0; - this->get_cartesian_position(); + this->traj_interpolator = common::LinearPoseTrajInterpolator(); } else if (this->running_controller != Controller::osc) { throw std::runtime_error( "Controller type must but osc but is " + @@ -586,6 +587,7 @@ void Franka::zero_torque_controller() { void Franka::move_home() { // sync + this->stop_control_thread(); FrankaMotionGenerator motion_generator( this->cfg.speed_factor, common::robots_meta_config.at(this->cfg.robot_type).q_home); From 2a254dc640b7fa2653184fe20ed7c47cf9b36288 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 11 Dec 2025 19:00:35 +0100 Subject: [PATCH 14/17] feat: adds digital twin and quest frame aligner --- python/rcs/envs/sim.py | 1 - python/rcs/envs/utils.py | 2 ++ scripts/quest_align_frame.py | 9 ++++++++ scripts/quest_iris_dual_arm.py | 39 ++++++++++++++++------------------ 4 files changed, 29 insertions(+), 22 deletions(-) create mode 100644 scripts/quest_align_frame.py diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index d9e733cd..389b0e89 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -455,7 +455,6 @@ class DigitalTwin(gym.Wrapper): def __init__(self, env, twin_env): super().__init__(env) self.twin_env = twin_env - assert self.twin_env.unwrapped.get_control_mode() == ControlMode.JOINTS def step(self, action): diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 6af1f2bd..1d440098 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -2,6 +2,7 @@ from os import PathLike from digit_interface import Digit +from rcs._core import common from rcs._core.common import BaseCameraConfig from rcs._core.sim import CameraType, SimCameraConfig from rcs.camera.digit_cam import DigitCam @@ -17,6 +18,7 @@ def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: robot_cfg = rcs.sim.SimRobotConfig() robot_cfg.robot_type = rcs.scenes[scene].robot_type + robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) robot_cfg.add_id(idx) if rcs.scenes[scene].mjb is not None: robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb diff --git a/scripts/quest_align_frame.py b/scripts/quest_align_frame.py new file mode 100644 index 00000000..f04f6006 --- /dev/null +++ b/scripts/quest_align_frame.py @@ -0,0 +1,9 @@ +from simpub.xr_device.meta_quest3 import MetaQuest3 +from quest_iris_dual_arm import MySimPublisher, MySimScene, MQ3_ADDR + + +MySimPublisher(MySimScene(), MQ3_ADDR) +reader = MetaQuest3("RCSNode") +while True: + data = reader.get_controller_data() + print(data) \ No newline at end of file diff --git a/scripts/quest_iris_dual_arm.py b/scripts/quest_iris_dual_arm.py index f562272f..e6a63cd7 100644 --- a/scripts/quest_iris_dual_arm.py +++ b/scripts/quest_iris_dual_arm.py @@ -39,8 +39,8 @@ INCLUDE_ROTATION = True ROBOT2IP = { - # "left": "192.168.102.1", - "right": "192.168.102.1", + "right": "192.168.101.1", + "left": "192.168.102.1", } @@ -85,7 +85,7 @@ def __init__(self, env: RelativeActionSpace): self._env_lock = threading.Lock() self._env = env - self.controller_names = ["right"] # , "right"] + self.controller_names = ["left", "right"] self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} self._start_btn = "A" @@ -297,24 +297,21 @@ def main(): relative_to=RelativeTo.CONFIGURED_ORIGIN, ) env_rel = StorageWrapper(env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000) - MySimPublisher(MySimScene(), MQ3_ADDR) - - # robot_cfg = default_sim_robot_cfg("fr3_empty_world") - # sim_cfg = SimConfig() - # sim_cfg.async_control = True - # twin_env, sim = SimMultiEnvCreator()( - # name2id=ROBOT2IP, - # robot_cfg=robot_cfg, - # control_mode=ControlMode.CARTESIAN_TQuat, - # gripper_cfg=default_sim_gripper_cfg(), - # # cameras=default_mujoco_cameraset_cfg(), - # max_relative_movement=0.5, - # relative_to=RelativeTo.CONFIGURED_ORIGIN, - # sim_cfg=sim_cfg, - # ) - # sim.open_gui() - # MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) - # env_rel = DigitalTwin(env_rel, twin_env) + # MySimPublisher(MySimScene(), MQ3_ADDR) + + robot_cfg = default_sim_robot_cfg("fr3_empty_world") + sim_cfg = SimConfig() + sim_cfg.async_control = True + twin_env, sim = SimMultiEnvCreator()( + name2id=ROBOT2IP, + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + gripper_cfg=default_sim_gripper_cfg(), + sim_cfg=sim_cfg, + ) + sim.open_gui() + MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) + env_rel = DigitalTwin(env_rel, twin_env) else: From cfbcda34e2aa869de58f545843954308a72a5917 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 12 Dec 2025 12:32:22 +0100 Subject: [PATCH 15/17] chore: move scripts into example and add readme --- examples/teleop/README.md | 17 +++++++++++++++++ {scripts => examples/teleop}/quest.py | 0 .../teleop}/quest_align_frame.py | 0 {scripts => examples/teleop}/quest_dual_arm.py | 0 .../teleop}/quest_iris_dual_arm.py | 0 {scripts => examples/teleop}/requirements.txt | 0 6 files changed, 17 insertions(+) create mode 100644 examples/teleop/README.md rename {scripts => examples/teleop}/quest.py (100%) rename {scripts => examples/teleop}/quest_align_frame.py (100%) rename {scripts => examples/teleop}/quest_dual_arm.py (100%) rename {scripts => examples/teleop}/quest_iris_dual_arm.py (100%) rename {scripts => examples/teleop}/requirements.txt (100%) diff --git a/examples/teleop/README.md b/examples/teleop/README.md new file mode 100644 index 00000000..c0b61d07 --- /dev/null +++ b/examples/teleop/README.md @@ -0,0 +1,17 @@ +# Teleoperation with Meta Quest +Teleoperate your robot (optinally dual arm) with the Meta Quest + +## How does it work? +In the script [`quest_iris_dual_arm.py`](quest_iris_dual_arm.py) we use the [IRIS platform](https://intuitive-robots.github.io/iris-project-page/index.html) to get controller poses from the meta quest. +With the relative space wrapper and the relative to configured origin setting theses poses are then apply to the robot in a delta fashion whenever the trigger button is pressed. +The buttons are used to start and stop data recording with the [`StorageWrapper`](robot-control-stack/python/rcs/envs/storage_wrapper.py). + +## 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). +Finally, install [SimPub](https://github.com/intuitive-robots/SimPublisher) the IRIS python client by + +```shell +pip install -r requirements.txt +``` + diff --git a/scripts/quest.py b/examples/teleop/quest.py similarity index 100% rename from scripts/quest.py rename to examples/teleop/quest.py diff --git a/scripts/quest_align_frame.py b/examples/teleop/quest_align_frame.py similarity index 100% rename from scripts/quest_align_frame.py rename to examples/teleop/quest_align_frame.py diff --git a/scripts/quest_dual_arm.py b/examples/teleop/quest_dual_arm.py similarity index 100% rename from scripts/quest_dual_arm.py rename to examples/teleop/quest_dual_arm.py diff --git a/scripts/quest_iris_dual_arm.py b/examples/teleop/quest_iris_dual_arm.py similarity index 100% rename from scripts/quest_iris_dual_arm.py rename to examples/teleop/quest_iris_dual_arm.py diff --git a/scripts/requirements.txt b/examples/teleop/requirements.txt similarity index 100% rename from scripts/requirements.txt rename to examples/teleop/requirements.txt From a714f2f4bdef2a21a0fbb4c2ec4cd2fc61b5ed85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 12 Dec 2025 12:54:20 +0100 Subject: [PATCH 16/17] chore: improved readme and removed unused scripts --- examples/teleop/README.md | 26 +++ examples/teleop/quest.py | 281 ------------------------- examples/teleop/quest_align_frame.py | 5 +- examples/teleop/quest_dual_arm.py | 280 ------------------------ examples/teleop/quest_iris_dual_arm.py | 66 +++--- examples/teleop/requirements.txt | 1 - python/rcs/envs/creators.py | 1 + python/rcs/envs/sim.py | 3 +- python/rcs/envs/storage_wrapper.py | 25 ++- 9 files changed, 80 insertions(+), 608 deletions(-) delete mode 100644 examples/teleop/quest.py delete mode 100644 examples/teleop/quest_dual_arm.py diff --git a/examples/teleop/README.md b/examples/teleop/README.md index c0b61d07..d4a56f2c 100644 --- a/examples/teleop/README.md +++ b/examples/teleop/README.md @@ -15,3 +15,29 @@ Finally, install [SimPub](https://github.com/intuitive-robots/SimPublisher) the pip install -r requirements.txt ``` +## Configuration + +### Teleoperating in sim + +1. go to [`quest_iris_dual_arm.py`](quest_iris_dual_arm.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 [`quest_iris_dual_arm.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. + + +## 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). + - 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) +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 +7. cancel the script +8. start the teleoperation script [`quest_iris_dual_arm.py`](quest_iris_dual_arm.py) and enjoy. + diff --git a/examples/teleop/quest.py b/examples/teleop/quest.py deleted file mode 100644 index 34991d33..00000000 --- a/examples/teleop/quest.py +++ /dev/null @@ -1,281 +0,0 @@ -import logging -import threading -from time import sleep - -import numpy as np -import oculus_reader -from rcs._core.common import RPY, Pose, RobotPlatform -from rcs.camera.hw import HardwareCameraSet -from rcs.envs.base import ( - ControlMode, - GripperDictType, - LimitedTQuatRelDictType, - RelativeActionSpace, - RelativeTo, -) -from rcs.envs.creators import SimEnvCreator -from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg -from rcs.utils import SimpleFrameRate -from rcs_fr3.creators import RCSFR3EnvCreator -from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg -from rcs_realsense.utils import default_realsense - -# from rcs_xarm7.creators import RCSXArm7EnvCreator - -logger = logging.getLogger(__name__) - -# in order to use usb connection install adb -# sudo apt install android-tools-adb -# and run the following forwarding -# adb reverse tcp:5037 tcp:5037 - - -INCLUDE_ROTATION = True -ROBOT_IP = "192.168.101.1" - -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE - -RECORD_FPS = 30 -# set camera dict to none disable cameras -# CAMERA_DICT = { -# "side_right": "244222071045", -# "bird_eye": "243522070364", -# "arro": "243522070385", -# } -CAMERA_DICT = None - - -class QuestReader(threading.Thread): - - transform_to_robot = Pose(RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) - - def __init__( - self, env: RelativeActionSpace, trg_btn="RTr", grp_btn="RG", controller="r", clb_btn="A", home_btn="B" - ): - super().__init__() - self._reader = oculus_reader.OculusReader() - - self._resource_lock = threading.Lock() - self._env_lock = threading.Lock() - self._env = env - self._trg_btn = trg_btn - self._grp_btn = grp_btn - self._home_btn = home_btn - self._clb_btn = clb_btn - self._controller = controller - self._grp_pos = 1 - self._prev_data = None - self._exit_requested = False - self._last_controller_pose = Pose() - self._offset_pose = Pose() - self._env.set_origin_to_current() - self._step_env = False - self._set_frame = Pose() - - def next_action(self) -> Pose: - with self._resource_lock: - transform = Pose( - translation=(self._last_controller_pose.translation() - self._offset_pose.translation()), - quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(), - ) - - set_axes = Pose(quaternion=self._set_frame.rotation_q()) - - transform = ( - QuestReader.transform_to_robot - * set_axes.inverse() - * transform - * set_axes - * QuestReader.transform_to_robot.inverse() - ) - if not INCLUDE_ROTATION: - transform = Pose(translation=transform.translation()) # identity rotation - return transform, self._grp_pos - - def run(self): - rate_limiter = SimpleFrameRate(90, "teleop readout") - warning_raised = False - while not self._exit_requested: - pos, buttons = self._reader.get_transformations_and_buttons() - if not warning_raised and len(pos) == 0: - logger.warning("[Quest Reader] packets empty") - warning_raised = True - sleep(0.5) - continue - elif len(pos) == 0: - sleep(0.5) - continue - elif warning_raised: - logger.warning("[Quest Reader] packets arriving again") - warning_raised = False - - data = {"pos": pos, "buttons": buttons} - - last_controller_pose = Pose( - pose_matrix=np.array(data["pos"][self._controller]), - ) - - if data["buttons"][self._clb_btn] and ( - self._prev_data is None or not self._prev_data["buttons"][self._clb_btn] - ): - print("clb button pressed") - with self._resource_lock: - self._set_frame = last_controller_pose - - if data["buttons"][self._trg_btn] and ( - self._prev_data is None or not self._prev_data["buttons"][self._trg_btn] - ): - # trigger just pressed (first data sample with button pressed) - - with self._resource_lock: - self._offset_pose = last_controller_pose - self._last_controller_pose = last_controller_pose - - elif not data["buttons"][self._trg_btn] and ( - self._prev_data is None or self._prev_data["buttons"][self._trg_btn] - ): - # released - transform = Pose( - translation=(self._last_controller_pose.translation() - self._offset_pose.translation()), - quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(), - ) - print(np.linalg.norm(transform.translation())) - print(np.rad2deg(transform.total_angle())) - with self._resource_lock: - self._last_controller_pose = Pose() - self._offset_pose = Pose() - with self._env_lock: - self._env.set_origin_to_current() - - elif data["buttons"][self._trg_btn]: - # button is pressed - with self._resource_lock: - self._last_controller_pose = last_controller_pose - else: - # trg button is not pressed - if data["buttons"][self._home_btn] and ( - self._prev_data is None or not self._prev_data["buttons"][self._home_btn] - ): - # home button just pressed - with self._env_lock: - self._env.unwrapped.robot.move_home() - self._env.set_origin_to_current() - - if data["buttons"][self._grp_btn] and ( - self._prev_data is None or not self._prev_data["buttons"][self._grp_btn] - ): - # just pressed - self._grp_pos = 0 - if not data["buttons"][self._grp_btn] and ( - self._prev_data is None or self._prev_data["buttons"][self._grp_btn] - ): - # just released - self._grp_pos = 1 - - self._prev_data = data - rate_limiter() - - def stop(self): - self._exit_requested = True - self.join() - - def __enter__(self): - self.start() - return self - - def __exit__(self, *_): - self.stop() - - def stop_env_loop(self): - self._step_env = False - - def environment_step_loop(self): - self._env.set_origin_to_current() - rate_limiter = SimpleFrameRate(RECORD_FPS, "env loop") - self._step_env = True - while self._step_env: - if self._exit_requested: - self._step_env = False - break - transform, gripper = self.next_action() - action = dict( - LimitedTQuatRelDictType(tquat=np.concatenate([transform.translation(), transform.rotation_q()])) - ) - - action.update(GripperDictType(gripper=gripper)) - - with self._env_lock: - self._env.step(action) - rate_limiter() - - -def main(): - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - # xarm7 - # env_rel = RCSXArm7EnvCreator()( - # control_mode=ControlMode.CARTESIAN_TQuat, - # ip=ROBOT_IP, - # relative_to=RelativeTo.CONFIGURED_ORIGIN, - # max_relative_movement=0.1, - # ) - camera_set = HardwareCameraSet([default_realsense(CAMERA_DICT)]) if CAMERA_DICT is not None else None - env_rel = RCSFR3EnvCreator()( - ip=ROBOT_IP, - camera_set=camera_set, - robot_cfg=default_fr3_hw_robot_cfg(async_control=True), - control_mode=ControlMode.CARTESIAN_TQuat, - gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), - max_relative_movement=(0.5, np.deg2rad(90)), - relative_to=RelativeTo.CONFIGURED_ORIGIN, - ) - else: - # FR3 - robot_cfg = default_sim_robot_cfg("fr3_empty_world") - # xarm7 - # robot_cfg = rcs.sim.SimRobotConfig() - # robot_cfg.actuators = [ - # "act1", - # "act2", - # "act3", - # "act4", - # "act5", - # "act6", - # "act7", - # ] - # robot_cfg.joints = [ - # "joint1", - # "joint2", - # "joint3", - # "joint4", - # "joint5", - # "joint6", - # "joint7", - # ] - # robot_cfg.base = "base" - # robot_cfg.robot_type = rcs.common.RobotType.XArm7 - # robot_cfg.attachment_site = "attachment_site" - # robot_cfg.arm_collision_geoms = [] - # robot_cfg.tcp_offset = rcs.common.Pose() - # robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - # robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.CARTESIAN_TQuat, - collision_guard=False, - gripper_cfg=default_sim_gripper_cfg(), - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.CONFIGURED_ORIGIN, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - env_rel.reset() - - with env_rel, QuestReader(env_rel) as action_server: - action_server.environment_step_loop() - - -if __name__ == "__main__": - main() diff --git a/examples/teleop/quest_align_frame.py b/examples/teleop/quest_align_frame.py index f04f6006..9a4bda17 100644 --- a/examples/teleop/quest_align_frame.py +++ b/examples/teleop/quest_align_frame.py @@ -1,9 +1,8 @@ +from quest_iris_dual_arm import MQ3_ADDR, MySimPublisher, MySimScene from simpub.xr_device.meta_quest3 import MetaQuest3 -from quest_iris_dual_arm import MySimPublisher, MySimScene, MQ3_ADDR - MySimPublisher(MySimScene(), MQ3_ADDR) reader = MetaQuest3("RCSNode") while True: data = reader.get_controller_data() - print(data) \ No newline at end of file + print(data) diff --git a/examples/teleop/quest_dual_arm.py b/examples/teleop/quest_dual_arm.py deleted file mode 100644 index 3d60178d..00000000 --- a/examples/teleop/quest_dual_arm.py +++ /dev/null @@ -1,280 +0,0 @@ -import logging -import sys -import threading -from time import sleep - -import numpy as np -import oculus_reader -from rcs._core.common import RPY, Pose, RobotPlatform -from rcs._core.sim import SimConfig -from rcs.camera.hw import HardwareCameraSet -from rcs.envs.base import ( - ControlMode, - GripperDictType, - LimitedTQuatRelDictType, - RelativeActionSpace, - RelativeTo, -) -from rcs.envs.creators import SimMultiEnvCreator -from rcs.envs.utils import 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 -from rcs_realsense.utils import default_realsense - -# from rcs_xarm7.creators import RCSXArm7EnvCreator - -logger = logging.getLogger(__name__) - -# in order to use usb connection install adb -# sudo apt install android-tools-adb -# and run the following forwarding -# adb reverse tcp:5037 tcp:5037 - - -INCLUDE_ROTATION = True -ROBOT2IP = { - "l": "192.168.101.1", - "r": "192.168.102.1", -} - - -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE -RECORD_FPS = 30 -# set camera dict to none disable cameras -# CAMERA_DICT = { -# "side_right": "244222071045", -# "bird_eye": "243522070364", -# "arro": "243522070385", -# } -CAMERA_DICT = None - - -class QuestReader(threading.Thread): - - transform_to_robot = Pose(RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) - - def __init__(self, env: RelativeActionSpace): - super().__init__() - self._reader = oculus_reader.OculusReader() - - self._resource_lock = threading.Lock() - self._env_lock = threading.Lock() - self._env = env - - self.controller_names = ["l", "r"] - self._trg_btn = {"l": "LTr", "r": "RTr"} - self._grp_btn = {"l": "LG", "r": "RG"} - self._clb_btn = {"l": "X", "r": "A"} - self._home_btn = "B" - - self._prev_data = None - self._exit_requested = False - self._grp_pos = {key: 1.0 for key in self.controller_names} # start with opened gripper - self._last_controller_pose = {key: Pose() for key in self.controller_names} - self._offset_pose = {key: Pose() for key in self.controller_names} - - for robot in ROBOT2IP: - # self._env.unwrapped.get_wrapper_attr("set_origin_to_current")[robot]() - # self._env.unwrapped.set_origin_to_current[robot]() - # self._env.unwrapped.envs[robot].set_origin_to_current() - self._env.envs[robot].set_origin_to_current() - - self._step_env = False - self._set_frame = {key: Pose() for key in self.controller_names} - - def next_action(self) -> Pose: - with self._resource_lock: - transforms = {} - for controller in self.controller_names: - transform = Pose( - translation=( - self._last_controller_pose[controller].translation() - - self._offset_pose[controller].translation() - ), - quaternion=( - self._last_controller_pose[controller] * self._offset_pose[controller].inverse() - ).rotation_q(), - ) - - set_axes = Pose(quaternion=self._set_frame[controller].rotation_q()) - - transform = ( - QuestReader.transform_to_robot - * set_axes.inverse() - * transform - * set_axes - * QuestReader.transform_to_robot.inverse() - ) - if not INCLUDE_ROTATION: - transform = Pose(translation=transform.translation()) # identity rotation - transforms[controller] = transform - return transforms, {key: self._grp_pos[key] for key in self.controller_names} - - def run(self): - rate_limiter = SimpleFrameRate(90, "teleop readout") - warning_raised = False - while not self._exit_requested: - pos, buttons = self._reader.get_transformations_and_buttons() - if not warning_raised and len(pos) == 0: - logger.warning("[Quest Reader] packets empty") - warning_raised = True - sleep(0.5) - continue - elif len(pos) == 0: - sleep(0.5) - continue - elif warning_raised: - logger.warning("[Quest Reader] packets arriving again") - warning_raised = False - - data = {"pos": pos, "buttons": buttons} - - for controller in self.controller_names: - last_controller_pose = Pose( - pose_matrix=np.array(data["pos"][controller]), - ) - - if data["buttons"][self._clb_btn[controller]] and ( - self._prev_data is None or not self._prev_data["buttons"][self._clb_btn[controller]] - ): - print("clb button pressed") - with self._resource_lock: - self._set_frame[controller] = last_controller_pose - - if data["buttons"][self._trg_btn[controller]] and ( - self._prev_data is None or not self._prev_data["buttons"][self._trg_btn[controller]] - ): - # 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 data["buttons"][self._trg_btn[controller]] and ( - self._prev_data is None or self._prev_data["buttons"][self._trg_btn[controller]] - ): - # released - transform = Pose( - translation=( - self._last_controller_pose[controller].translation() - - self._offset_pose[controller].translation() - ), - quaternion=( - self._last_controller_pose[controller] * self._offset_pose[controller].inverse() - ).rotation_q(), - ) - print(np.linalg.norm(transform.translation())) - print(np.rad2deg(transform.total_angle())) - with self._resource_lock: - self._last_controller_pose[controller] = Pose() - self._offset_pose[controller] = Pose() - with self._env_lock: - self._env.envs[controller].set_origin_to_current() - - elif data["buttons"][self._trg_btn[controller]]: - # button is pressed - with self._resource_lock: - self._last_controller_pose[controller] = last_controller_pose - else: - # trg button is not pressed - # TODO: deactivated for now - # if data["buttons"][self._home_btn] and ( - # self._prev_data is None or not self._prev_data["buttons"][self._home_btn] - # ): - # # home button just pressed - # with self._env_lock: - # self._env.unwrapped.robot.move_home() - # self._env.set_origin_to_current() - pass - - if data["buttons"][self._grp_btn[controller]] and ( - self._prev_data is None or not self._prev_data["buttons"][self._grp_btn[controller]] - ): - # just pressed - self._grp_pos[controller] = 0 - if not data["buttons"][self._grp_btn[controller]] and ( - self._prev_data is None or self._prev_data["buttons"][self._grp_btn[controller]] - ): - # just released - self._grp_pos[controller] = 1 - - self._prev_data = data - rate_limiter() - - def stop(self): - self._exit_requested = True - self.join() - - def __enter__(self): - self.start() - return self - - def __exit__(self, *_): - self.stop() - - def stop_env_loop(self): - self._step_env = False - - def environment_step_loop(self): - rate_limiter = SimpleFrameRate(RECORD_FPS, "env loop") - self._step_env = True - while self._step_env: - 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()])) - ) - - 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 - env_rel = RCSFR3MultiEnvCreator()( - name2ip=ROBOT2IP, - camera_set=camera_set, - robot_cfg=default_fr3_hw_robot_cfg(async_control=True), - control_mode=ControlMode.CARTESIAN_TQuat, - gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), - max_relative_movement=(0.5, np.deg2rad(90)), - relative_to=RelativeTo.CONFIGURED_ORIGIN, - ) - else: - # FR3 - robot_cfg = default_sim_robot_cfg("fr3_empty_world") - - sim_cfg = SimConfig() - sim_cfg.async_control = True - env_rel, sim = SimMultiEnvCreator()( - name2id=ROBOT2IP, - robot_cfg=robot_cfg, - control_mode=ControlMode.CARTESIAN_TQuat, - gripper_cfg=default_sim_gripper_cfg(), - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.CONFIGURED_ORIGIN, - sim_cfg=sim_cfg, - ) - sim.open_gui() - - env_rel.reset() - - with env_rel, QuestReader(env_rel) as action_server: - action_server.environment_step_loop() - - -if __name__ == "__main__": - main() diff --git a/examples/teleop/quest_iris_dual_arm.py b/examples/teleop/quest_iris_dual_arm.py index e6a63cd7..571b6dd3 100644 --- a/examples/teleop/quest_iris_dual_arm.py +++ b/examples/teleop/quest_iris_dual_arm.py @@ -30,17 +30,17 @@ logger = logging.getLogger(__name__) -# in order to use usb connection install adb -# sudo apt install android-tools-adb # download the iris apk from the following repo release: https://github.com/intuitive-robots/IRIS-Meta-Quest3 +# in order to use usb connection install adb install adb +# sudo apt install android-tools-adb # install it on your quest with # adb install IRIS-Meta-Quest3.apk INCLUDE_ROTATION = True ROBOT2IP = { - "right": "192.168.101.1", "left": "192.168.102.1", + "right": "192.168.101.1", } @@ -73,19 +73,17 @@ def __init__(self): class QuestReader(threading.Thread): - transform_to_robot = Pose() # RPY(roll=np.deg2rad(90), yaw=np.deg2rad(-90))) + transform_to_robot = Pose() def __init__(self, env: RelativeActionSpace): super().__init__() - # self._reader = oculus_reader.OculusReader() - self._reader = MetaQuest3("RCSNode") self._resource_lock = threading.Lock() self._env_lock = threading.Lock() self._env = env - self.controller_names = ["left", "right"] + self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ["right"] self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} self._start_btn = "A" @@ -99,9 +97,6 @@ def __init__(self, env: RelativeActionSpace): self._offset_pose = {key: Pose() for key in self.controller_names} for robot in ROBOT2IP: - # self._env.unwrapped.get_wrapper_attr("set_origin_to_current")[robot]() - # self._env.unwrapped.set_origin_to_current[robot]() - # self._env.unwrapped.envs[robot].set_origin_to_current() self._env.envs[robot].set_origin_to_current() self._step_env = False @@ -154,16 +149,12 @@ def run(self): warning_raised = False # start recording - if input_data[self._start_btn] and ( - self._prev_data is None or not self._prev_data[self._start_btn] - ): + if input_data[self._start_btn] and (self._prev_data is None or not self._prev_data[self._start_btn]): print("start button pressed") with self._env_lock: self._env.get_wrapper_attr("start_record")() - if input_data[self._stop_btn] and ( - self._prev_data is None or not self._prev_data[self._stop_btn] - ): + 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: # set successful @@ -179,13 +170,16 @@ def run(self): with self._env_lock: self._env.reset() - for controller in self.controller_names: last_controller_pose = Pose( translation=np.array(input_data[controller]["pos"]), quaternion=np.array(input_data[controller]["rot"]), ) - + if controller == "left": + last_controller_pose = ( + Pose(translation=np.array([0, 0, 0]), rpy=RPY(roll=0, pitch=0, yaw=np.deg2rad(180))) + * 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]] @@ -296,23 +290,27 @@ def main(): max_relative_movement=(0.5, np.deg2rad(90)), relative_to=RelativeTo.CONFIGURED_ORIGIN, ) - env_rel = StorageWrapper(env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000) - # MySimPublisher(MySimScene(), MQ3_ADDR) - - robot_cfg = default_sim_robot_cfg("fr3_empty_world") - sim_cfg = SimConfig() - sim_cfg.async_control = True - twin_env, sim = SimMultiEnvCreator()( - name2id=ROBOT2IP, - robot_cfg=robot_cfg, - control_mode=ControlMode.JOINTS, - gripper_cfg=default_sim_gripper_cfg(), - sim_cfg=sim_cfg, + env_rel = StorageWrapper( + env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 ) - sim.open_gui() - MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) - env_rel = DigitalTwin(env_rel, twin_env) - + MySimPublisher(MySimScene(), MQ3_ADDR) + + # DIGITAL TWIN: comment out the line above and uncomment the lines below to use a digital twin + # Note: only supported for one arm at the moment + + # robot_cfg = default_sim_robot_cfg("fr3_empty_world") + # sim_cfg = SimConfig() + # sim_cfg.async_control = True + # twin_env, sim = SimMultiEnvCreator()( + # name2id=ROBOT2IP, + # robot_cfg=robot_cfg, + # control_mode=ControlMode.JOINTS, + # gripper_cfg=default_sim_gripper_cfg(), + # sim_cfg=sim_cfg, + # ) + # sim.open_gui() + # MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) + # env_rel = DigitalTwin(env_rel, twin_env) else: # FR3 diff --git a/examples/teleop/requirements.txt b/examples/teleop/requirements.txt index 268e92e4..db25a19a 100644 --- a/examples/teleop/requirements.txt +++ b/examples/teleop/requirements.txt @@ -1,2 +1 @@ -oculus_reader @ git+https://github.com/rail-berkeley/oculus_reader.git@c52b4b6193879b6cf32a1db1bd5727652626f00d simpub @ git+https://github.com/intuitive-robots/SimPublisher.git \ No newline at end of file diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index b0124ebb..a47b340c 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -132,6 +132,7 @@ def __call__( # type: ignore return env + class SimMultiEnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore self, diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 389b0e89..3b1cd523 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -450,17 +450,16 @@ def step(self, action: dict[str, Any]): reward /= 5 # type: ignore return obs, reward, success, truncated, info + class DigitalTwin(gym.Wrapper): def __init__(self, env, twin_env): super().__init__(env) self.twin_env = twin_env - def step(self, action): obs, reward, terminated, truncated, info = super().step(action) twin_obs, _, _, _, _ = self.twin_env.step(obs) info["twin_obs"] = twin_obs return obs, reward, terminated, truncated, info - diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index 8e9060a2..5652a9c1 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -1,14 +1,15 @@ import operator +from concurrent.futures import ThreadPoolExecutor, wait +from itertools import chain +from queue import Queue from typing import Any, Optional from uuid import uuid4 -from queue import Queue -import pyarrow.dataset as ds -import pyarrow as pa + +import gymnasium as gym import numpy as np -from itertools import chain +import pyarrow as pa +import pyarrow.dataset as ds import simplejpeg -from concurrent.futures import ThreadPoolExecutor, wait -import gymnasium as gym class StorageWrapper(gym.Wrapper): @@ -147,7 +148,17 @@ def step(self, action): self._flatten_arrays(obs) if "success" in info and info["success"]: self.success() - self.buffer.append({"obs": obs, "reward": reward, "step": self.step_cnt, "uuid": self.uuid.hex, "success": self._success, "action": self._prev_action, "instruction": self.instruction}) + self.buffer.append( + { + "obs": obs, + "reward": reward, + "step": self.step_cnt, + "uuid": self.uuid.hex, + "success": self._success, + "action": self._prev_action, + "instruction": self.instruction, + } + ) self._prev_action = action self.step_cnt += 1 if len(self.buffer) == self.batch_size: From b523eea9787ed3ae67f1799405659d8500dac603 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 12 Dec 2025 16:03:20 +0100 Subject: [PATCH 17/17] style: code linting and formatting --- examples/teleop/quest_iris_dual_arm.py | 17 +++++++++-------- python/rcs/envs/creators.py | 8 ++++---- python/rcs/envs/storage_wrapper.py | 6 +++--- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/examples/teleop/quest_iris_dual_arm.py b/examples/teleop/quest_iris_dual_arm.py index 571b6dd3..a6746a39 100644 --- a/examples/teleop/quest_iris_dual_arm.py +++ b/examples/teleop/quest_iris_dual_arm.py @@ -14,7 +14,6 @@ RelativeTo, ) from rcs.envs.creators import SimMultiEnvCreator -from rcs.envs.sim import DigitalTwin from rcs.envs.storage_wrapper import StorageWrapper from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg from rcs.utils import SimpleFrameRate @@ -102,7 +101,7 @@ def __init__(self, env: RelativeActionSpace): self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} - def next_action(self) -> Pose: + def next_action(self) -> tuple[dict[str, Pose], dict[str, float]]: with self._resource_lock: transforms = {} for controller in self.controller_names: @@ -141,10 +140,10 @@ def run(self): warning_raised = True sleep(0.5) continue - elif input_data is None: + if input_data is None: sleep(0.5) continue - elif warning_raised: + if warning_raised: logger.warning("[Quest Reader] packets arriving again") warning_raised = False @@ -280,7 +279,7 @@ def environment_step_loop(self): def main(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - camera_set = HardwareCameraSet([default_realsense(CAMERA_DICT)]) if CAMERA_DICT is not None else None + camera_set = HardwareCameraSet([default_realsense(CAMERA_DICT)]) if CAMERA_DICT is not None else None # type: ignore env_rel = RCSFR3MultiEnvCreator()( name2ip=ROBOT2IP, camera_set=camera_set, @@ -301,13 +300,14 @@ def main(): # robot_cfg = default_sim_robot_cfg("fr3_empty_world") # sim_cfg = SimConfig() # sim_cfg.async_control = True - # twin_env, sim = SimMultiEnvCreator()( + # twin_env = SimMultiEnvCreator()( # name2id=ROBOT2IP, # robot_cfg=robot_cfg, # control_mode=ControlMode.JOINTS, # gripper_cfg=default_sim_gripper_cfg(), # sim_cfg=sim_cfg, # ) + # sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # sim.open_gui() # MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) # env_rel = DigitalTwin(env_rel, twin_env) @@ -318,7 +318,7 @@ def main(): sim_cfg = SimConfig() sim_cfg.async_control = True - env_rel, sim = SimMultiEnvCreator()( + env_rel = SimMultiEnvCreator()( name2id=ROBOT2IP, robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, @@ -328,12 +328,13 @@ def main(): relative_to=RelativeTo.CONFIGURED_ORIGIN, sim_cfg=sim_cfg, ) + sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore sim.open_gui() MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) env_rel.reset() - with env_rel, QuestReader(env_rel) as action_server: + with env_rel, QuestReader(env_rel) as action_server: # type: ignore action_server.environment_step_loop() diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index a47b340c..64bcce19 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -156,12 +156,12 @@ def __call__( # type: ignore ) # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) - robots: dict[str, rcs.sim.SimRobotConfig] = {} - for key, ip in name2id.items(): + robots: dict[str, rcs.sim.SimRobot] = {} + for key in name2id: robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=robot_cfg) envs = {} - for key, ip in name2id.items(): + for key in name2id: env: gym.Env = RobotEnv(robots[key], control_mode) env = RobotSimWrapper(env, simulation, sim_wrapper) if gripper_cfg is not None: @@ -178,7 +178,7 @@ def __call__( # type: ignore BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) ) env = CameraSetWrapper(env, camera_set, include_depth=True) - return env, simulation + return env class SimTaskEnvCreator(EnvCreator): diff --git a/python/rcs/envs/storage_wrapper.py b/python/rcs/envs/storage_wrapper.py index 5652a9c1..2ef0847b 100644 --- a/python/rcs/envs/storage_wrapper.py +++ b/python/rcs/envs/storage_wrapper.py @@ -70,14 +70,14 @@ def __init__( self.basename_template = basename_template self.max_rows_per_group = max_rows_per_group self.max_rows_per_file = max_rows_per_file - self.buffer = [] + self.buffer: list[dict[str, Any]] = [] self.step_cnt = 0 self._pause = True self.instruction = instruction self._success = start_record self._prev_action = None self.thread_pool = ThreadPoolExecutor() - self.queue = Queue(maxsize=2) + self.queue: Queue[pa.Table | pa.RecordBatch] = Queue(maxsize=2) self.uuid = uuid4() self._writer_future = self.thread_pool.submit(self._writer_worker) @@ -146,7 +146,7 @@ def step(self, action): if "frames" in obs: self._encode_images(obs) self._flatten_arrays(obs) - if "success" in info and info["success"]: + if info.get("success"): self.success() self.buffer.append( {