From 3d358112f56e3e5dd973761015ebf9c7612a9a33 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 13 Mar 2026 23:41:58 +0100 Subject: [PATCH 01/19] feat: initial version of the teleop interface --- python/rcs/envs/base.py | 3 + python/rcs/operator/__init__.py | 0 python/rcs/operator/franka.py | 116 ++++++++++++++++ python/rcs/operator/interface.py | 60 ++++++++ python/rcs/operator/pedals.py | 111 +++++++++++++++ python/rcs/operator/quest.py | 227 +++++++++++++++++++++++++++++++ 6 files changed, 517 insertions(+) create mode 100644 python/rcs/operator/__init__.py create mode 100644 python/rcs/operator/franka.py create mode 100644 python/rcs/operator/interface.py create mode 100644 python/rcs/operator/pedals.py create mode 100644 python/rcs/operator/quest.py diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 1518909d..e6239d85 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -167,6 +167,8 @@ class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ... CartOrJointContType: TypeAlias = TQuatDictType | JointsDictType | TRPYDictType LimitedCartOrJointContType: TypeAlias = LimitedTQuatRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType +class ArmWithGripper(CartOrJointContType, GripperDictType): ... + class ControlMode(Enum): JOINTS = auto() @@ -358,6 +360,7 @@ def close(self): class RelativeTo(Enum): LAST_STEP = auto() CONFIGURED_ORIGIN = auto() + NONE = auto() class RelativeActionSpace(gym.ActionWrapper): diff --git a/python/rcs/operator/__init__.py b/python/rcs/operator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/python/rcs/operator/franka.py b/python/rcs/operator/franka.py new file mode 100644 index 00000000..5c1a59b0 --- /dev/null +++ b/python/rcs/operator/franka.py @@ -0,0 +1,116 @@ +import logging +import threading +from time import sleep + +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, + GripperDictType, + LimitedTQuatRelDictType, + RelativeActionSpace, + RelativeTo, +) +from rcs.envs.creators import SimMultiEnvCreator +from rcs.envs.storage_wrapper import StorageWrapper +from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg +from rcs.utils import SimpleFrameRate +from rcs_fr3.creators import RCSFR3MultiEnvCreator +from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg +from rcs_realsense.utils import default_realsense +from rcs.operator.quest import QuestConfig, QuestOperator + +logger = logging.getLogger(__name__) + + + + +ROBOT2IP = { + # "left": "192.168.102.1", + "right": "192.168.101.1", +} + + +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE + +RECORD_FPS = 30 +# set camera dict to none disable cameras +# CAMERA_DICT = { +# "left_wrist": "230422272017", +# "right_wrist": "230422271040", +# "side": "243522070385", +# "bird_eye": "243522070364", +# } +CAMERA_DICT = None +MQ3_ADDR = "10.42.0.1" + +# DIGIT_DICT = { +# "digit_right_left": "D21182", +# "digit_right_right": "D21193" +# } +DIGIT_DICT = None + + +DATASET_PATH = "test_data_iris_dual_arm14" +INSTRUCTION = "build a tower with the blocks in front of you" +TELEOP = "quest" + +configs = {"quest": QuestConfig(robot_keys=ROBOT2IP.keys(), simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION, mq3_addr=MQ3_ADDR)} +operators = { + "quest": QuestOperator, +} + + +def main(): + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + + cams = [] + if CAMERA_DICT is not None: + cams.append(default_realsense(CAMERA_DICT)) + if DIGIT_DICT is not None: + cams.append(default_digit(DIGIT_DICT)) + + camera_set = HardwareCameraSet(cams) if cams else None + + env_rel = RCSFR3MultiEnvCreator()( + name2ip=ROBOT2IP, + camera_set=camera_set, + robot_cfg=default_fr3_hw_robot_cfg(async_control=True), + control_mode=operators[TELEOP].control_mode[0], + gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), + max_relative_movement=(0.5, np.deg2rad(90)), + relative_to=operators[TELEOP].control_mode[1], + ) + env_rel = StorageWrapper( + env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 + ) + else: + # FR3 + robot_cfg = default_sim_robot_cfg("fr3_empty_world") + + sim_cfg = SimConfig() + sim_cfg.async_control = True + env_rel = SimMultiEnvCreator()( + name2id=ROBOT2IP, + robot_cfg=robot_cfg, + control_mode=operators[TELEOP].control_mode[0], + gripper_cfg=default_sim_gripper_cfg(), + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=operators[TELEOP].control_mode[1], + sim_cfg=sim_cfg, + ) + sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore + sim.open_gui() + + env_rel.reset() + + with env_rel, operators[TELEOP](env_rel, configs[TELEOP]) as op: # type: ignore + op.environment_step_loop() + + +if __name__ == "__main__": + main() diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py new file mode 100644 index 00000000..a28e65e8 --- /dev/null +++ b/python/rcs/operator/interface.py @@ -0,0 +1,60 @@ +from abc import ABC +from dataclasses import dataclass +from enum import Enum, auto +import threading +from typing import Protocol +import numpy as np +import rcs +from rcs.envs.base import ArmWithGripper, ControlMode, GripperDictType, JointsDictType, RelativeTo, TQuatDictType, TRPYDictType +from rcs.utils import SimpleFrameRate +import gymnasium as gym + + +@dataclass(kw_only=True) +class BaseOperatorConfig: + env_frequency: int = 30 + +class BaseOperator(ABC, threading.Thread): + """Interface for a operator device""" + + control_mode: tuple[ControlMode, RelativeTo] + + def __init__(self, env: gym.Env, config: BaseOperatorConfig): + super().__init__() + self.config = config + self.env = env + self.reset_lock = threading.Lock() + + + def run(self): + # read out hardware, set states and process buttons + raise NotImplementedError() + + + # TODO: use action spaces, needs to return gripper and + # TODO: support multiple robots + def get_action() -> dict[str, ArmWithGripper]: + raise NotImplementedError() + + def stop(self): + self._exit_requested = True + self.join() + + def __enter__(self): + self.start() + return self + + def __exit__(self, *_): + self.stop() + + def environment_step_loop(self): + rate_limiter = SimpleFrameRate(self.env, "env loop") + while True: + if self._exit_requested: + break + with self.reset_lock: + actions = self.get_action() + self.env.step(actions) + rate_limiter() + + diff --git a/python/rcs/operator/pedals.py b/python/rcs/operator/pedals.py new file mode 100644 index 00000000..e1865f51 --- /dev/null +++ b/python/rcs/operator/pedals.py @@ -0,0 +1,111 @@ +import evdev +from evdev import ecodes +import threading +import time + +class FootPedal: + def __init__(self, device_name_substring="Foot Switch"): + """Initializes the foot pedal and starts the background reading thread.""" + self.device_path = self._find_device(device_name_substring) + + if not self.device_path: + raise FileNotFoundError(f"Could not find a device matching '{device_name_substring}'") + + self.device = evdev.InputDevice(self.device_path) + self.device.grab() # Prevent events from leaking into the OS/terminal + + # Dictionary to hold the current state of each key. + # True = Pressed/Held, False = Released + self._key_states = {} + self._lock = threading.Lock() + + # Start the background thread + self._running = True + self._thread = threading.Thread(target=self._read_events, daemon=True) + self._thread.start() + print(f"Connected to {self.device.name} at {self.device_path}") + + def _find_device(self, substring): + """Finds the device path for the foot pedal.""" + for path in evdev.list_devices(): + dev = evdev.InputDevice(path) + if substring.lower() in dev.name.lower(): + return path + return None + + def _read_events(self): + """Background loop that updates the state dictionary.""" + try: + for event in self.device.read_loop(): + if not self._running: + break + + if event.type == ecodes.EV_KEY: + key_event = evdev.categorize(event) + + with self._lock: + # keystate: 1 is DOWN, 2 is HOLD, 0 is UP + is_pressed = key_event.keystate in [1, 2] + + # Store state using the string name of the key (e.g., 'KEY_A') + # If a key resolves to a list (rare, but happens in evdev), take the first one + key_name = key_event.keycode + if isinstance(key_name, list): + key_name = key_name[0] + + self._key_states[key_name] = is_pressed + + except OSError: + pass # Device disconnected or closed + + def get_states(self): + """ + Returns a snapshot of the latest key states. + Example return: {'KEY_A': True, 'KEY_B': False, 'KEY_C': False} + """ + with self._lock: + # Return a copy to ensure thread safety + return self._key_states.copy() + + def get_key_state(self, key_name): + """Returns the state of a specific key, defaulting to False if never pressed.""" + with self._lock: + return self._key_states.get(key_name, False) + + def close(self): + """Cleans up the device and stops the thread.""" + self._running = False + try: + self.device.ungrab() + self.device.close() + except OSError: + pass + +# ========================================== +# Example Usage +# ========================================== +if __name__ == "__main__": + try: + # Initialize the pedal + pedal = FootPedal("Foot Switch") + + # Simulate a typical robotics control loop running at 10Hz + print("Starting control loop... Press Ctrl+C to exit.") + while True: + # Grab the latest states instantly without blocking + states = pedal.get_states() + + if states: + # Print only the keys that are currently pressed + pressed_keys = [key for key, is_pressed in states.items() if is_pressed] + print(f"Currently pressed: {pressed_keys}") + + # Your teleoperation logic goes here... + + time.sleep(0.1) # 10Hz loop + + except KeyboardInterrupt: + print("\nShutting down...") + finally: + if 'pedal' in locals(): + pedal.close() \ No newline at end of file diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py new file mode 100644 index 00000000..ce160441 --- /dev/null +++ b/python/rcs/operator/quest.py @@ -0,0 +1,227 @@ +from dataclasses import dataclass +import logging +import threading +from time import sleep + +import numpy as np +from rcs._core.common import Pose +from rcs.envs.base import ArmWithGripper, ControlMode, GripperDictType, RelativeActionSpace, RelativeTo, TQuatDictType +from rcs.operator.interface import BaseOperator, BaseOperatorConfig +from rcs.utils import SimpleFrameRate +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 + +logger = logging.getLogger(__name__) + +# 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 + +class FakeSimPublisher(SimPublisher): + def get_update(self): + return {} + +class FakeSimScene(SimScene): + def __init__(self): + super().__init__() + self.root = SimObject(name="root") + +@dataclass(kw_only=True) +class QuestConfig(BaseOperatorConfig): + robot_keys: list[str] + read_frame_rate: int = 30 + include_rotation: bool = True + simulation: bool = False + mq3_addr: str = "10.42.0.1" + + +class QuestOperator(BaseOperator): + + transform_to_robot = Pose() + + control_mode = (ControlMode.CARTESIAN_TQuat, RelativeTo.CONFIGURED_ORIGIN) + + def __init__(self, env, config: QuestConfig): + super().__init__(env, config) + self.config: QuestConfig + self._reader = MetaQuest3("RCSNode") + + self.resource_lock = threading.Lock() + + self.controller_names = self.config.robot_keys + self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} + self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} + self._start_btn = "A" + self._stop_btn = "B" + self._unsuccessful_btn = "Y" + + 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 self.config.robot_keys: + self.env.envs[robot].set_origin_to_current() + + self._step_env = False + self._set_frame = {key: Pose() for key in self.controller_names} + if self.config.simulation: + FakeSimPublisher(FakeSimScene(), self.config.mq3_addr) + # robot_cfg = default_sim_robot_cfg("fr3_empty_world") + # sim_cfg = SimConfig() + # sim_cfg.async_control = True + # 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) + else: + sim = self.env.unwrapped.envs[self.controller_names[0]].sim # type: ignore + MujocoPublisher(sim.model, sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) + + + + + def get_action(self) -> dict[str, ArmWithGripper]: + with self.resource_lock: + transforms = {} + for controller in self.controller_names: + transform = Pose( + translation=( + self._last_controller_pose[controller].translation() # type: ignore + - 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 = ( + set_axes.inverse() + * transform + * set_axes + ) + if not self.config.include_rotation: + transform = Pose(translation=transform.translation()) # identity rotation + transforms[controller] = TQuatDictType(tquat=np.concatenate([transform.translation(), transform.rotation_q()])) + transforms[controller].update( + GripperDictType(gripper=self._grp_pos[controller]) + ) + return transforms + + def run(self): + rate_limiter = SimpleFrameRate(self.config.read_frame_rate, "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 is None: + logger.warning("[Quest Reader] packets empty") + warning_raised = True + sleep(0.5) + continue + if input_data is None: + sleep(0.5) + continue + if warning_raised: + 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") + 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]): + print("reset successful pressed: resetting env") + with self.reset_lock: + # set successful + self.env.get_wrapper_attr("success")() + # sleep to allow to let the robot reach the goal + sleep(1) + # this might also move the robot to the home position + self.env.reset() + for controller in self.controller_names: + self._offset_pose[controller] = Pose() + self._last_controller_pose[controller] = Pose() + self._grp_pos[controller] = 1 + continue + + # reset unsuccessful + if input_data[self._unsuccessful_btn] and ( + self._prev_data is None or not self._prev_data[self._unsuccessful_btn] + ): + print("reset unsuccessful pressed: resetting env") + 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))) # type: ignore + * 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() # type: ignore + - 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() + 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 + + 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() \ No newline at end of file From 1119c0752241b0d56a867f8744e3187ce97eb3f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sun, 15 Mar 2026 13:38:21 +0100 Subject: [PATCH 02/19] refactor(operator): env independent operators --- python/rcs/operator/franka.py | 6 +- python/rcs/operator/interface.py | 122 ++++++++++++++++++++++++++++--- python/rcs/operator/quest.py | 76 +++++++------------ 3 files changed, 142 insertions(+), 62 deletions(-) diff --git a/python/rcs/operator/franka.py b/python/rcs/operator/franka.py index 5c1a59b0..6cd566f6 100644 --- a/python/rcs/operator/franka.py +++ b/python/rcs/operator/franka.py @@ -64,7 +64,7 @@ } -def main(): +def get_env(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: cams = [] @@ -105,9 +105,11 @@ def main(): ) sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore sim.open_gui() + return env_rel +def main(): + env_rel = get_env() env_rel.reset() - with env_rel, operators[TELEOP](env_rel, configs[TELEOP]) as op: # type: ignore op.environment_step_loop() diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index a28e65e8..f23af238 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -2,21 +2,31 @@ from dataclasses import dataclass from enum import Enum, auto import threading +import copy +from time import sleep from typing import Protocol import numpy as np -import rcs -from rcs.envs.base import ArmWithGripper, ControlMode, GripperDictType, JointsDictType, RelativeTo, TQuatDictType, TRPYDictType -from rcs.utils import SimpleFrameRate import gymnasium as gym +from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo +from rcs.utils import SimpleFrameRate + +@dataclass +class TeleopCommands: + """Semantic commands decoupled from specific hardware buttons.""" + record: bool = False + success: bool = False + failure: bool = False + reset_origin: bool = False @dataclass(kw_only=True) class BaseOperatorConfig: env_frequency: int = 30 class BaseOperator(ABC, threading.Thread): - """Interface for a operator device""" - + """Interface for an operator device""" + + # Define this as a class attribute so it can be accessed without instantiating control_mode: tuple[ControlMode, RelativeTo] def __init__(self, env: gym.Env, config: BaseOperatorConfig): @@ -24,16 +34,33 @@ def __init__(self, env: gym.Env, config: BaseOperatorConfig): self.config = config self.env = env self.reset_lock = threading.Lock() + self._exit_requested = False + + # State for semantic commands + self._commands = TeleopCommands() + self._cmd_lock = threading.Lock() + def consume_commands(self) -> TeleopCommands: + """Returns the current commands and resets them to False (edge-triggered).""" + with self._cmd_lock: + cmds = copy.copy(self._commands) + self._commands.record = False + self._commands.success = False + self._commands.failure = False + self._commands.reset_origin = False + return cmds + + def reset_operator_state(self): + """Hook for subclasses to reset their internal poses/offsets on env reset.""" + pass def run(self): - # read out hardware, set states and process buttons + """Read out hardware, set states and process buttons.""" raise NotImplementedError() - - # TODO: use action spaces, needs to return gripper and # TODO: support multiple robots - def get_action() -> dict[str, ArmWithGripper]: + def get_action(self) -> dict[str, ArmWithGripper]: + """Returns the action dictionary to step the environment.""" raise NotImplementedError() def stop(self): @@ -48,13 +75,86 @@ def __exit__(self, *_): self.stop() def environment_step_loop(self): - rate_limiter = SimpleFrameRate(self.env, "env loop") + rate_limiter = SimpleFrameRate(self.config.env_frequency, "env loop") while True: if self._exit_requested: break + + # 1. Process Meta-Commands + cmds = self.consume_commands() + + if cmds.record: + print("Command: Start Recording") + self.env.get_wrapper_attr("start_record")() + + if cmds.success: + print("Command: Success! Resetting env...") + with self.reset_lock: + self.env.get_wrapper_attr("success")() + sleep(1) # sleep to let the robot reach the goal + self.env.reset() + self.reset_operator_state() + + elif cmds.failure: + print("Command: Failure! Resetting env...") + with self.reset_lock: + self.env.reset() + self.reset_operator_state() + + # if cmds.reset_origin: + # print("Command: Resetting origin...") + # # env lock + # for robot in self.config.robot_keys: + # self.env.envs[robot].set_origin_to_current() + + + # 2. Step the Environment with self.reset_lock: actions = self.get_action() - self.env.step(actions) + if actions: # Only step if actions are provided + self.env.step(actions) + rate_limiter() +class CompositeOperator(BaseOperator): + def __init__(self, env, motion_operator: BaseOperator, command_operator: BaseOperator): + # We don't need a specific config for the composite itself, + # so we just pass a default one to the base class + super().__init__(env, BaseOperatorConfig()) + + self.motion_op = motion_operator + self.command_op = command_operator + + # Inherit the control mode from the motion operator (e.g., GELLO) + self.control_mode = self.motion_op.control_mode + + def start(self): + """Start the background threads for both hardware readers.""" + self.motion_op.start() + self.command_op.start() + + def stop(self): + """Stop both hardware readers.""" + self.motion_op.stop() + self.command_op.stop() + self._exit_requested = True + + def get_action(self): + """Fetch the physical movements from the motion operator (GELLO).""" + return self.motion_op.get_action() + + def consume_commands(self) -> TeleopCommands: + """Fetch the meta-commands (record/success/fail) from the command operator (Pedal).""" + # If both devices can send commands, you could logically OR them together here. + # But in this case, only the pedal sends commands. + return self.command_op.consume_commands() + + def reset_operator_state(self): + """Pass the reset hook down to the operators.""" + self.motion_op.reset_operator_state() + self.command_op.reset_operator_state() + + def run(self): + # The base class requires this, but the sub-operators handle their own run loops. + pass \ No newline at end of file diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index ce160441..abf412b1 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -71,6 +71,9 @@ def __init__(self, env, config: QuestConfig): self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} if self.config.simulation: + sim = self.env.unwrapped.envs[self.controller_names[0]].sim # type: ignore + MujocoPublisher(sim.model, sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) + else: FakeSimPublisher(FakeSimScene(), self.config.mq3_addr) # robot_cfg = default_sim_robot_cfg("fr3_empty_world") # sim_cfg = SimConfig() @@ -86,16 +89,18 @@ def __init__(self, env, config: QuestConfig): # 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: - sim = self.env.unwrapped.envs[self.controller_names[0]].sim # type: ignore - MujocoPublisher(sim.model, sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) - - + def reset_operator_state(self): + """Resets the hardware offsets when the environment resets.""" + with self.resource_lock: + for controller in self.controller_names: + self._offset_pose[controller] = Pose() + self._last_controller_pose[controller] = Pose() + self._grp_pos[controller] = 1 def get_action(self) -> dict[str, ArmWithGripper]: + transforms = {} with self.resource_lock: - transforms = {} for controller in self.controller_names: transform = Pose( translation=( @@ -125,48 +130,33 @@ def get_action(self) -> dict[str, ArmWithGripper]: def run(self): rate_limiter = SimpleFrameRate(self.config.read_frame_rate, "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 is None: - logger.warning("[Quest Reader] packets empty") - warning_raised = True - sleep(0.5) - continue + if input_data is None: + if not warning_raised: + logger.warning("[Quest Reader] packets empty") + warning_raised = True sleep(0.5) continue + if warning_raised: 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") - 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]): - print("reset successful pressed: resetting env") - with self.reset_lock: - # set successful - self.env.get_wrapper_attr("success")() - # sleep to allow to let the robot reach the goal - sleep(1) - # this might also move the robot to the home position - self.env.reset() - for controller in self.controller_names: - self._offset_pose[controller] = Pose() - self._last_controller_pose[controller] = Pose() - self._grp_pos[controller] = 1 - continue + # === Update Semantic Commands === + with self._cmd_lock: + if input_data[self._start_btn] and (self._prev_data is None or not self._prev_data[self._start_btn]): + self._commands.record = True - # 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") - self.env.reset() + if input_data[self._stop_btn] and (self._prev_data is None or not self._prev_data[self._stop_btn]): + self._commands.success = True + if input_data[self._unsuccessful_btn] and (self._prev_data is None or not self._prev_data[self._unsuccessful_btn]): + self._commands.failure = True + + # === Update Poses & Grippers === for controller in self.controller_names: last_controller_pose = Pose( translation=np.array(input_data[controller]["pos"]), @@ -190,18 +180,6 @@ def run(self): 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() # type: ignore - - 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() From b4ef47e48a2a706ae2a9f6349dad077448cdb314 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sun, 15 Mar 2026 14:55:01 +0100 Subject: [PATCH 03/19] refactor(operator): key translation and origin reset - refactored origin reset - proper operator interface - key translation --- python/rcs/operator/interface.py | 181 +++++++++++++------------------ python/rcs/operator/quest.py | 88 +++++++++------ 2 files changed, 130 insertions(+), 139 deletions(-) diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index f23af238..96bdb5da 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -1,160 +1,135 @@ from abc import ABC -from dataclasses import dataclass -from enum import Enum, auto -import threading import copy +from dataclasses import dataclass, field +import threading from time import sleep -from typing import Protocol -import numpy as np import gymnasium as gym from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo +from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate + @dataclass class TeleopCommands: """Semantic commands decoupled from specific hardware buttons.""" + record: bool = False success: bool = False failure: bool = False - reset_origin: bool = False + reset_origin_to_current: dict[str, bool] = field(default_factory=dict) + @dataclass(kw_only=True) class BaseOperatorConfig: - env_frequency: int = 30 + read_frequency: int = 30 + simulation: bool = True + class BaseOperator(ABC, threading.Thread): - """Interface for an operator device""" - - # Define this as a class attribute so it can be accessed without instantiating control_mode: tuple[ControlMode, RelativeTo] + controller_names: list[str] = field(default=["left", "right"]) - def __init__(self, env: gym.Env, config: BaseOperatorConfig): - super().__init__() + def __init__(self, config: BaseOperatorConfig, sim: Sim | None = None): self.config = config - self.env = env - self.reset_lock = threading.Lock() - self._exit_requested = False - - # State for semantic commands - self._commands = TeleopCommands() - self._cmd_lock = threading.Lock() + self.sim = sim def consume_commands(self) -> TeleopCommands: - """Returns the current commands and resets them to False (edge-triggered).""" - with self._cmd_lock: - cmds = copy.copy(self._commands) - self._commands.record = False - self._commands.success = False - self._commands.failure = False - self._commands.reset_origin = False - return cmds + """Returns the current commands and resets them (edge-triggered). Must be thread-safe.""" + raise NotImplementedError() def reset_operator_state(self): - """Hook for subclasses to reset their internal poses/offsets on env reset.""" - pass + """Hook for subclasses to reset their internal poses/offsets on env reset. Must be thread-safe.""" def run(self): """Read out hardware, set states and process buttons.""" raise NotImplementedError() - # TODO: support multiple robots - def get_action(self) -> dict[str, ArmWithGripper]: - """Returns the action dictionary to step the environment.""" + def consume_action(self) -> dict[str, ArmWithGripper]: + """Returns the action dictionary to step the environment. Must be thread-safe.""" raise NotImplementedError() + +class TeleopLoop: + """Interface for an operator device""" + + # Define this as a class attribute so it can be accessed without instantiating + control_mode: tuple[ControlMode, RelativeTo] + + def __init__( + self, + env: gym.Env, + operator: BaseOperator, + env_frequency: int = 30, + key_translation: dict[str, str] | None = None, + ): + super().__init__() + self.env = env + self.operator = operator + self._exit_requested = False + self.env_frequency = env_frequency + if key_translation is None: + # controller to robot translation + self.key_translation = {key: key for key in self.operator.controller_names} + else: + self.key_translation = key_translation + def stop(self): self._exit_requested = True - self.join() + self.operator.join() def __enter__(self): - self.start() + self.operator.start() return self def __exit__(self, *_): self.stop() + def _translate_keys(self, actions): + return {self.key_translation[key]: actions[key] for key in actions} + def environment_step_loop(self): - rate_limiter = SimpleFrameRate(self.config.env_frequency, "env loop") + rate_limiter = SimpleFrameRate(self.env_frequency, "env loop") while True: if self._exit_requested: break - + # 1. Process Meta-Commands - cmds = self.consume_commands() - + cmds = self.operator.consume_commands() + if cmds.record: print("Command: Start Recording") self.env.get_wrapper_attr("start_record")() - + if cmds.success: print("Command: Success! Resetting env...") - with self.reset_lock: - self.env.get_wrapper_attr("success")() - sleep(1) # sleep to let the robot reach the goal - self.env.reset() - self.reset_operator_state() - + self.env.get_wrapper_attr("success")() + sleep(1) # sleep to let the robot reach the goal + self.env.reset() + self.operator.reset_operator_state() + # consume new commands because of potential origin reset + continue + elif cmds.failure: print("Command: Failure! Resetting env...") - with self.reset_lock: - self.env.reset() - self.reset_operator_state() - - # if cmds.reset_origin: - # print("Command: Resetting origin...") - # # env lock - # for robot in self.config.robot_keys: - # self.env.envs[robot].set_origin_to_current() - + self.env.reset() + self.operator.reset_operator_state() + # consume new commands because of potential origin reset + continue + + for controller in cmds.reset_origin_to_current: + if cmds.reset_origin_to_current[controller]: + robot = self.key_translation[controller] + print(f"Command: Resetting origin for {robot}...") + assert ( + self.operator.control_mode[1] == RelativeTo.CONFIGURED_ORIGIN + and self.env.get_wrapper_attr("relative_to") == RelativeTo.CONFIGURED_ORIGIN + ), "both robot env and operator must be configured to relative_to.CONFIGURED_ORIGIN" + self.env.get_wrapper_attr("envs")[robot].set_origin_to_current() # 2. Step the Environment - with self.reset_lock: - actions = self.get_action() - if actions: # Only step if actions are provided - self.env.step(actions) - - rate_limiter() - - -class CompositeOperator(BaseOperator): - def __init__(self, env, motion_operator: BaseOperator, command_operator: BaseOperator): - # We don't need a specific config for the composite itself, - # so we just pass a default one to the base class - super().__init__(env, BaseOperatorConfig()) - - self.motion_op = motion_operator - self.command_op = command_operator - - # Inherit the control mode from the motion operator (e.g., GELLO) - self.control_mode = self.motion_op.control_mode + actions = self.operator.consume_action() + actions = self._translate_keys(actions) + self.env.step(actions) - def start(self): - """Start the background threads for both hardware readers.""" - self.motion_op.start() - self.command_op.start() - - def stop(self): - """Stop both hardware readers.""" - self.motion_op.stop() - self.command_op.stop() - self._exit_requested = True - - def get_action(self): - """Fetch the physical movements from the motion operator (GELLO).""" - return self.motion_op.get_action() - - def consume_commands(self) -> TeleopCommands: - """Fetch the meta-commands (record/success/fail) from the command operator (Pedal).""" - # If both devices can send commands, you could logically OR them together here. - # But in this case, only the pedal sends commands. - return self.command_op.consume_commands() - - def reset_operator_state(self): - """Pass the reset hook down to the operators.""" - self.motion_op.reset_operator_state() - self.command_op.reset_operator_state() - - def run(self): - # The base class requires this, but the sub-operators handle their own run loops. - pass \ No newline at end of file + rate_limiter() diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index abf412b1..74bb53ef 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -1,3 +1,4 @@ +import copy from dataclasses import dataclass import logging import threading @@ -6,7 +7,8 @@ import numpy as np from rcs._core.common import Pose from rcs.envs.base import ArmWithGripper, ControlMode, GripperDictType, RelativeActionSpace, RelativeTo, TQuatDictType -from rcs.operator.interface import BaseOperator, BaseOperatorConfig +from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands +from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate from simpub.core.simpub_server import SimPublisher from simpub.parser.simdata import SimObject, SimScene @@ -21,38 +23,37 @@ # install it on your quest with # adb install IRIS-Meta-Quest3.apk + class FakeSimPublisher(SimPublisher): def get_update(self): return {} + class FakeSimScene(SimScene): def __init__(self): super().__init__() self.root = SimObject(name="root") + @dataclass(kw_only=True) class QuestConfig(BaseOperatorConfig): - robot_keys: list[str] - read_frame_rate: int = 30 include_rotation: bool = True - simulation: bool = False mq3_addr: str = "10.42.0.1" class QuestOperator(BaseOperator): - transform_to_robot = Pose() - control_mode = (ControlMode.CARTESIAN_TQuat, RelativeTo.CONFIGURED_ORIGIN) + controller_names = ["left", "right"] - def __init__(self, env, config: QuestConfig): - super().__init__(env, config) + def __init__(self, config: QuestConfig, sim: Sim | None = None): + super().__init__(config, sim) self.config: QuestConfig self._reader = MetaQuest3("RCSNode") - self.resource_lock = threading.Lock() + self._resource_lock = threading.Lock() + self._cmd_lock = threading.Lock() - self.controller_names = self.config.robot_keys self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} self._start_btn = "A" @@ -65,14 +66,13 @@ def __init__(self, env, config: QuestConfig): 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 self.config.robot_keys: - self.env.envs[robot].set_origin_to_current() + self._commands = TeleopCommands() + self._reset_origin_to_current() self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} if self.config.simulation: - sim = self.env.unwrapped.envs[self.controller_names[0]].sim # type: ignore - MujocoPublisher(sim.model, sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) + MujocoPublisher(self.sim.model, self.sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) else: FakeSimPublisher(FakeSimScene(), self.config.mq3_addr) # robot_cfg = default_sim_robot_cfg("fr3_empty_world") @@ -90,17 +90,35 @@ def __init__(self, env, config: QuestConfig): # MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) # env_rel = DigitalTwin(env_rel, twin_env) - def reset_operator_state(self): - """Resets the hardware offsets when the environment resets.""" - with self.resource_lock: + def _reset_origin_to_current(self, controller: str | None = None): + with self._cmd_lock: + if controller is None: + self._commands.reset_origin_to_current = {key: True for key in self.controller_names} + else: + self._commands.reset_origin_to_current[controller] = True + + def _reset_state(self): + with self._resource_lock: for controller in self.controller_names: self._offset_pose[controller] = Pose() self._last_controller_pose[controller] = Pose() self._grp_pos[controller] = 1 + def consume_commands(self) -> TeleopCommands: + # must be threadsafe + with self._cmd_lock: + cmds = copy.copy(self._commands) + self._commands = TeleopCommands() + return cmds + + def reset_operator_state(self): + """Resets the hardware offsets when the environment resets.""" + self._reset_state() + self._reset_origin_to_current() + def get_action(self) -> dict[str, ArmWithGripper]: transforms = {} - with self.resource_lock: + with self._resource_lock: for controller in self.controller_names: transform = Pose( translation=( @@ -114,33 +132,29 @@ def get_action(self) -> dict[str, ArmWithGripper]: set_axes = Pose(quaternion=self._set_frame[controller].rotation_q()) - transform = ( - set_axes.inverse() - * transform - * set_axes - ) + transform = set_axes.inverse() * transform * set_axes if not self.config.include_rotation: transform = Pose(translation=transform.translation()) # identity rotation - transforms[controller] = TQuatDictType(tquat=np.concatenate([transform.translation(), transform.rotation_q()])) - transforms[controller].update( - GripperDictType(gripper=self._grp_pos[controller]) + transforms[controller] = TQuatDictType( + tquat=np.concatenate([transform.translation(), transform.rotation_q()]) ) + transforms[controller].update(GripperDictType(gripper=self._grp_pos[controller])) return transforms def run(self): - rate_limiter = SimpleFrameRate(self.config.read_frame_rate, "teleop readout") + rate_limiter = SimpleFrameRate(self.config.read_frequency, "teleop readout") warning_raised = False - + while not self._exit_requested: input_data = self._reader.get_controller_data() - + if input_data is None: if not warning_raised: logger.warning("[Quest Reader] packets empty") warning_raised = True sleep(0.5) continue - + if warning_raised: logger.warning("[Quest Reader] packets arriving again") warning_raised = False @@ -153,7 +167,9 @@ def run(self): if input_data[self._stop_btn] and (self._prev_data is None or not self._prev_data[self._stop_btn]): self._commands.success = True - if input_data[self._unsuccessful_btn] and (self._prev_data is None or not self._prev_data[self._unsuccessful_btn]): + if input_data[self._unsuccessful_btn] and ( + self._prev_data is None or not self._prev_data[self._unsuccessful_btn] + ): self._commands.failure = True # === Update Poses & Grippers === @@ -173,21 +189,21 @@ def run(self): ): # trigger just pressed (first data sample with button pressed) - with self.resource_lock: + 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]] ): - with self.resource_lock: + with self._resource_lock: self._last_controller_pose[controller] = Pose() self._offset_pose[controller] = Pose() - self.env.envs[controller].set_origin_to_current() + self._reset_origin_to_current(controller) elif input_data[controller][self._trg_btn[controller]]: # button is pressed - with self.resource_lock: + with self._resource_lock: self._last_controller_pose[controller] = last_controller_pose if input_data[controller][self._grp_btn[controller]] and ( @@ -202,4 +218,4 @@ def run(self): self._grp_pos[controller] = 1 self._prev_data = input_data - rate_limiter() \ No newline at end of file + rate_limiter() From 698fbbb90fb0a72dd833df157e70741a8f91455f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 16 Dec 2025 21:50:04 +0100 Subject: [PATCH 04/19] feat(scenes): iris scene integration --- assets/scenes/fr3_simple_pick_up/scene.xml | 2 +- examples/teleop/quest_iris_dual_arm.py | 54 ++++++++++++++++++---- python/rcs/__init__.py | 2 +- python/rcs/_core/sim.pyi | 4 ++ python/rcs/envs/creators.py | 17 ++++--- python/rcs/envs/sim.py | 28 +++++------ python/rcs/envs/utils.py | 6 ++- src/pybind/rcs.cpp | 12 +++++ 8 files changed, 92 insertions(+), 33 deletions(-) diff --git a/assets/scenes/fr3_simple_pick_up/scene.xml b/assets/scenes/fr3_simple_pick_up/scene.xml index 88b1f269..d077d3bb 100644 --- a/assets/scenes/fr3_simple_pick_up/scene.xml +++ b/assets/scenes/fr3_simple_pick_up/scene.xml @@ -28,7 +28,7 @@ - + diff --git a/examples/teleop/quest_iris_dual_arm.py b/examples/teleop/quest_iris_dual_arm.py index 6deeb5ae..f1beb455 100644 --- a/examples/teleop/quest_iris_dual_arm.py +++ b/examples/teleop/quest_iris_dual_arm.py @@ -3,8 +3,10 @@ from time import sleep import numpy as np +import rcs +from rcs._core import common from rcs._core.common import RPY, Pose, RobotPlatform -from rcs._core.sim import SimConfig +from rcs._core.sim import CameraType, SimCameraConfig, SimConfig from rcs.camera.hw import HardwareCameraSet from rcs.envs.base import ( ControlMode, @@ -19,7 +21,7 @@ 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_realsense.utils import default_realsense from simpub.core.simpub_server import SimPublisher from simpub.parser.simdata import SimObject, SimScene from simpub.sim.mj_publisher import MujocoPublisher @@ -39,10 +41,14 @@ # "left": "192.168.102.1", "right": "192.168.101.1", } +ROBOT2ID = { + "left": "0", + "right": "1", +} -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE +ROBOT_INSTANCE = RobotPlatform.SIMULATION +# ROBOT_INSTANCE = RobotPlatform.HARDWARE RECORD_FPS = 30 # set camera dict to none disable cameras # CAMERA_DICT = { @@ -87,7 +93,7 @@ def __init__(self, env: RelativeActionSpace): self._reset_lock = threading.Lock() self._env = env - self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ["right"] + self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID.keys() self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} self._start_btn = "A" @@ -100,7 +106,7 @@ def __init__(self, env: RelativeActionSpace): 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: + for robot in ROBOT2IP if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID: self._env.envs[robot].set_origin_to_current() self._step_env = False @@ -326,22 +332,50 @@ def main(): else: # FR3 - robot_cfg = default_sim_robot_cfg("fr3_empty_world") + rcs.scenes["rcs_icra_scene"] = rcs.Scene( + mjcf_scene="/home/tobi/coding/rcs_clones/prs/models/scenes/rcs_icra_scene/scene.xml", + mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, + robot_type=common.RobotType.FR3, + ) + rcs.scenes["pick"] = rcs.Scene( + mjcf_scene="/home/tobi/coding/rcs_clones/prs/assets/scenes/fr3_simple_pick_up/scene.xml", + mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, + robot_type=common.RobotType.FR3, + ) + + # robot_cfg = default_sim_robot_cfg("fr3_empty_world") + # robot_cfg = default_sim_robot_cfg("fr3_simple_pick_up") + robot_cfg = default_sim_robot_cfg("rcs_icra_scene") + # robot_cfg = default_sim_robot_cfg("pick") + + resolution = (256, 256) + cameras = { + cam: SimCameraConfig( + identifier=cam, + type=CameraType.fixed, + resolution_height=resolution[1], + resolution_width=resolution[0], + frame_rate=0, + ) + for cam in ["side", "wrist"] + } sim_cfg = SimConfig() sim_cfg.async_control = True env_rel = SimMultiEnvCreator()( - name2id=ROBOT2IP, + name2id=ROBOT2ID, robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, gripper_cfg=default_sim_gripper_cfg(), - # cameras=default_mujoco_cameraset_cfg(), + # cameras=cameras, max_relative_movement=0.5, relative_to=RelativeTo.CONFIGURED_ORIGIN, sim_cfg=sim_cfg, ) - sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore + # sim = env_rel.unwrapped.envs[ROBOT2ID.keys().__iter__().__next__()].sim # type: ignore + sim = env_rel.get_wrapper_attr("sim") sim.open_gui() + # MySimPublisher(MySimScene(), MQ3_ADDR) MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) env_rel.reset() diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index fdfee7bb..4131ea78 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -18,7 +18,7 @@ class Scene: """Scene configuration.""" - mjb: str + mjb: str | None = None """Path to the Mujoco binary scene file.""" mjcf_scene: str """Path to the Mujoco scene XML file.""" diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 26a187fc..c70d9209 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -136,6 +136,8 @@ class SimGripperConfig(rcs._core.common.GripperConfig): min_actuator_width: float min_joint_width: float seconds_between_callbacks: float + def __copy__(self) -> SimGripperConfig: ... + def __deepcopy__(self, arg0: dict) -> SimGripperConfig: ... def __init__(self) -> None: ... def add_id(self, id: str) -> None: ... @@ -169,6 +171,8 @@ class SimRobotConfig(rcs._core.common.RobotConfig): mjcf_scene_path: str seconds_between_callbacks: float trajectory_trace: bool + def __copy__(self) -> SimRobotConfig: ... + def __deepcopy__(self, arg0: dict) -> SimRobotConfig: ... def __init__(self) -> None: ... def add_id(self, id: str) -> None: ... diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 64bcce19..83857388 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -1,3 +1,4 @@ +import copy import logging import typing from functools import partial @@ -151,21 +152,24 @@ def __call__( # type: ignore simulation = sim.Sim(robot_cfg.mjcf_scene_path, sim_cfg) ik = rcs.common.Pin( robot_cfg.kinematic_model_path, - robot_cfg.attachment_site, + robot_cfg.attachment_site + "_0", 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.SimRobot] = {} - for key in name2id: - robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=robot_cfg) + for key, mid in name2id.items(): + cfg = copy.copy(robot_cfg) + cfg.add_id(mid) + robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=cfg) envs = {} - for key in name2id: + for key, mid 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) + cfg = copy.copy(gripper_cfg) + cfg.add_id(mid) + gripper = rcs.sim.SimGripper(simulation, cfg) env = GripperWrapper(env, gripper, binary=True) if max_relative_movement is not None: @@ -173,6 +177,7 @@ def __call__( # type: ignore envs[key] = env env = MultiRobotWrapper(envs) + env = RobotSimWrapper(env, simulation, sim_wrapper) if cameras is not None: camera_set = typing.cast( BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 589b49fd..dab96bdc 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -39,9 +39,9 @@ def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | Non if sim_wrapper is not None: env = sim_wrapper(env, simulation) super().__init__(env) - self.unwrapped: RobotEnv - assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." - self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) + # self.unwrapped: RobotEnv + # assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." + # self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) self.sim = simulation cfg = self.sim.get_config() self.frame_rate = SimpleFrameRate(1 / cfg.frequency, "RobotSimWrapper") @@ -56,18 +56,20 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo self.frame_rate() else: - self.sim_robot.clear_collision_flag() + # self.sim_robot.clear_collision_flag() self.sim.step_until_convergence() - state = self.sim_robot.get_state() - if "collision" not in info: - info["collision"] = state.collision - else: - info["collision"] = info["collision"] or state.collision - info["ik_success"] = state.ik_success + # state = self.sim_robot.get_state() + # if "collision" not in info: + # info["collision"] = state.collision + # else: + # info["collision"] = info["collision"] or state.collision + # info["ik_success"] = state.ik_success info["is_sim_converged"] = self.sim.is_converged() # truncate episode if collision - obs.update(self.unwrapped.get_obs()) - return obs, 0, False, info["collision"] or not state.ik_success, info + # obs.update(self.unwrapped.get_obs()) + # return obs, 0, False, info["collision"] or not state.ik_success, info + return obs, 0, False, False, info + def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None @@ -76,7 +78,7 @@ def reset( obs, info = super().reset(seed=seed, options=options) self.sim.step(1) # todo: an obs method that is recursive over wrappers would be needed - obs.update(self.unwrapped.get_obs()) + # obs.update(self.unwrapped.get_obs()) return obs, info diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 1d440098..58fa02ce 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -19,7 +19,7 @@ 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.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) - robot_cfg.add_id(idx) + # robot_cfg.add_id(idx) if rcs.scenes[scene].mjb is not None: robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb else: @@ -38,7 +38,9 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() - cfg.add_id(idx) + cfg.collision_geoms = [] + cfg.collision_geoms_fingers = [] + # cfg.add_id(idx) return cfg diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 6496ee1f..7a36ed70 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -472,6 +472,12 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("joints", &rcs::sim::SimRobotConfig::joints) .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators) .def_readwrite("base", &rcs::sim::SimRobotConfig::base) + .def("__copy__", [](const rcs::sim::SimRobotConfig &self) { + return rcs::sim::SimRobotConfig(self); + }) + .def("__deepcopy__", [](const rcs::sim::SimRobotConfig &self, py::dict) { + return rcs::sim::SimRobotConfig(self); + }) .def("add_id", &rcs::sim::SimRobotConfig::add_id, py::arg("id")); py::class_(sim, "SimRobotState") @@ -510,6 +516,12 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::SimGripperConfig::max_actuator_width) .def_readwrite("min_actuator_width", &rcs::sim::SimGripperConfig::min_actuator_width) + .def("__copy__", [](const rcs::sim::SimGripperConfig &self) { + return rcs::sim::SimGripperConfig(self); + }) + .def("__deepcopy__", [](const rcs::sim::SimGripperConfig &self, py::dict) { + return rcs::sim::SimGripperConfig(self); + }) .def("add_id", &rcs::sim::SimGripperConfig::add_id, py::arg("id")); py::class_( sim, "SimGripperState") From 4f6acc7fbbf8d77d1c76b15f016fc5a30076c093 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Mar 2026 20:45:20 +0100 Subject: [PATCH 05/19] fix(operator): test teleop --- python/rcs/envs/base.py | 2 +- python/rcs/operator/franka.py | 75 +++++++++++++++++++++++--------- python/rcs/operator/interface.py | 4 +- python/rcs/operator/quest.py | 6 +-- 4 files changed, 61 insertions(+), 26 deletions(-) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index e6239d85..17fecb37 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -167,7 +167,7 @@ class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ... CartOrJointContType: TypeAlias = TQuatDictType | JointsDictType | TRPYDictType LimitedCartOrJointContType: TypeAlias = LimitedTQuatRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType -class ArmWithGripper(CartOrJointContType, GripperDictType): ... +class ArmWithGripper(TQuatDictType, GripperDictType): ... class ControlMode(Enum): diff --git a/python/rcs/operator/franka.py b/python/rcs/operator/franka.py index 6cd566f6..83fd78e7 100644 --- a/python/rcs/operator/franka.py +++ b/python/rcs/operator/franka.py @@ -3,7 +3,9 @@ from time import sleep import numpy as np +import rcs from rcs._core.common import RPY, Pose, RobotPlatform +from rcs._core import common from rcs._core.sim import SimConfig from rcs.camera.hw import HardwareCameraSet from rcs.envs.base import ( @@ -21,20 +23,23 @@ from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg from rcs_realsense.utils import default_realsense from rcs.operator.quest import QuestConfig, QuestOperator +from rcs.operator.interface import TeleopLoop logger = logging.getLogger(__name__) - - ROBOT2IP = { # "left": "192.168.102.1", "right": "192.168.101.1", } +ROBOT2ID = { + "left": "0", + "right": "1", +} -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE +ROBOT_INSTANCE = RobotPlatform.SIMULATION +# ROBOT_INSTANCE = RobotPlatform.HARDWARE RECORD_FPS = 30 # set camera dict to none disable cameras @@ -45,7 +50,7 @@ # "bird_eye": "243522070364", # } CAMERA_DICT = None -MQ3_ADDR = "10.42.0.1" +MQ3_ADDR = "192.168.1.219" # DIGIT_DICT = { # "digit_right_left": "D21182", @@ -56,12 +61,9 @@ DATASET_PATH = "test_data_iris_dual_arm14" INSTRUCTION = "build a tower with the blocks in front of you" -TELEOP = "quest" -configs = {"quest": QuestConfig(robot_keys=ROBOT2IP.keys(), simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION, mq3_addr=MQ3_ADDR)} -operators = { - "quest": QuestOperator, -} + +config = QuestConfig(mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION) def get_env(): @@ -79,39 +81,70 @@ def get_env(): name2ip=ROBOT2IP, camera_set=camera_set, robot_cfg=default_fr3_hw_robot_cfg(async_control=True), - control_mode=operators[TELEOP].control_mode[0], + control_mode=QuestOperator.control_mode[0], gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), max_relative_movement=(0.5, np.deg2rad(90)), - relative_to=operators[TELEOP].control_mode[1], + relative_to=QuestOperator.control_mode[1], ) env_rel = StorageWrapper( env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 ) + operator = QuestOperator(config) else: # FR3 - robot_cfg = default_sim_robot_cfg("fr3_empty_world") + rcs.scenes["rcs_icra_scene"] = rcs.Scene( + mjcf_scene="/home/tobi/coding/rcs_clones/prs/models/scenes/rcs_icra_scene/scene.xml", + mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, + robot_type=common.RobotType.FR3, + ) + rcs.scenes["pick"] = rcs.Scene( + mjcf_scene="/home/tobi/coding/rcs_clones/prs/assets/scenes/fr3_simple_pick_up/scene.xml", + mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, + robot_type=common.RobotType.FR3, + ) + + # robot_cfg = default_sim_robot_cfg("fr3_empty_world") + # robot_cfg = default_sim_robot_cfg("fr3_simple_pick_up") + robot_cfg = default_sim_robot_cfg("rcs_icra_scene") + # robot_cfg = default_sim_robot_cfg("pick") + + # resolution = (256, 256) + # cameras = { + # cam: SimCameraConfig( + # identifier=cam, + # type=CameraType.fixed, + # resolution_height=resolution[1], + # resolution_width=resolution[0], + # frame_rate=0, + # ) + # for cam in ["side", "wrist"] + # } sim_cfg = SimConfig() sim_cfg.async_control = True env_rel = SimMultiEnvCreator()( - name2id=ROBOT2IP, + name2id=ROBOT2ID, robot_cfg=robot_cfg, - control_mode=operators[TELEOP].control_mode[0], + control_mode=QuestOperator.control_mode[0], gripper_cfg=default_sim_gripper_cfg(), # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, - relative_to=operators[TELEOP].control_mode[1], + relative_to=QuestOperator.control_mode[1], sim_cfg=sim_cfg, ) - sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore + # sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore + sim = env_rel.get_wrapper_attr("sim") + operator = QuestOperator(config, sim) sim.open_gui() - return env_rel + return env_rel, operator + def main(): - env_rel = get_env() + env_rel, operator = get_env() env_rel.reset() - with env_rel, operators[TELEOP](env_rel, configs[TELEOP]) as op: # type: ignore - op.environment_step_loop() + tele = TeleopLoop(env_rel, operator) + with env_rel, tele: # type: ignore + tele.environment_step_loop() if __name__ == "__main__": diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index 96bdb5da..7c1c0380 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -31,6 +31,7 @@ class BaseOperator(ABC, threading.Thread): controller_names: list[str] = field(default=["left", "right"]) def __init__(self, config: BaseOperatorConfig, sim: Sim | None = None): + threading.Thread.__init__(self) self.config = config self.sim = sim @@ -123,7 +124,8 @@ def environment_step_loop(self): print(f"Command: Resetting origin for {robot}...") assert ( self.operator.control_mode[1] == RelativeTo.CONFIGURED_ORIGIN - and self.env.get_wrapper_attr("relative_to") == RelativeTo.CONFIGURED_ORIGIN + # TODO the following is a dict and can thus not easily be used like this + # and self.env.get_wrapper_attr("relative_to") == RelativeTo.CONFIGURED_ORIGIN ), "both robot env and operator must be configured to relative_to.CONFIGURED_ORIGIN" self.env.get_wrapper_attr("envs")[robot].set_origin_to_current() diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 74bb53ef..f7859d82 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -5,7 +5,7 @@ from time import sleep import numpy as np -from rcs._core.common import Pose +from rcs._core.common import Pose, RPY from rcs.envs.base import ArmWithGripper, ControlMode, GripperDictType, RelativeActionSpace, RelativeTo, TQuatDictType from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands from rcs.sim.sim import Sim @@ -49,7 +49,6 @@ class QuestOperator(BaseOperator): def __init__(self, config: QuestConfig, sim: Sim | None = None): super().__init__(config, sim) self.config: QuestConfig - self._reader = MetaQuest3("RCSNode") self._resource_lock = threading.Lock() self._cmd_lock = threading.Lock() @@ -89,6 +88,7 @@ def __init__(self, config: QuestConfig, sim: Sim | None = None): # sim.open_gui() # MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) # env_rel = DigitalTwin(env_rel, twin_env) + self._reader = MetaQuest3("RCSNode") def _reset_origin_to_current(self, controller: str | None = None): with self._cmd_lock: @@ -116,7 +116,7 @@ def reset_operator_state(self): self._reset_state() self._reset_origin_to_current() - def get_action(self) -> dict[str, ArmWithGripper]: + def consume_action(self) -> dict[str, ArmWithGripper]: transforms = {} with self._resource_lock: for controller in self.controller_names: From 8ddf748c8bc99636280a792a82736d108929d49b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Mar 2026 21:39:53 +0100 Subject: [PATCH 06/19] feat: added close method to operator interface --- python/rcs/operator/interface.py | 4 ++++ python/rcs/operator/quest.py | 10 ++++++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index 7c1c0380..1f826a50 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -50,6 +50,9 @@ def consume_action(self) -> dict[str, ArmWithGripper]: """Returns the action dictionary to step the environment. Must be thread-safe.""" raise NotImplementedError() + def close(self): + pass + class TeleopLoop: """Interface for an operator device""" @@ -76,6 +79,7 @@ def __init__( self.key_translation = key_translation def stop(self): + self.operator.close() self._exit_requested = True self.operator.join() diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index f7859d82..4e0e5683 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -71,9 +71,9 @@ def __init__(self, config: QuestConfig, sim: Sim | None = None): self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} if self.config.simulation: - MujocoPublisher(self.sim.model, self.sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) + self._publisher = MujocoPublisher(self.sim.model, self.sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) else: - FakeSimPublisher(FakeSimScene(), self.config.mq3_addr) + self._publisher = FakeSimPublisher(FakeSimScene(), self.config.mq3_addr) # robot_cfg = default_sim_robot_cfg("fr3_empty_world") # sim_cfg = SimConfig() # sim_cfg.async_control = True @@ -141,6 +141,12 @@ def consume_action(self) -> dict[str, ArmWithGripper]: transforms[controller].update(GripperDictType(gripper=self._grp_pos[controller])) return transforms + def close(self): + self._reader.disconnect() + self._publisher.shutdown() + self._exit_requested = True + self.join() + def run(self): rate_limiter = SimpleFrameRate(self.config.read_frequency, "teleop readout") warning_raised = False From 227fd2fdbc3c924c7dbad484f6656e9e0da4ecce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Mar 2026 21:50:23 +0100 Subject: [PATCH 07/19] feat(quest): operator importable without install deps --- python/rcs/operator/quest.py | 33 ++++++++++++++++++++------------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 4e0e5683..9c4ca57d 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -10,10 +10,15 @@ from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate -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 + +try: + 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 + HAS_SIMPUB = True +except ImportError: + HAS_SIMPUB = False logger = logging.getLogger(__name__) @@ -23,16 +28,15 @@ # install it on your quest with # adb install IRIS-Meta-Quest3.apk +if HAS_SIMPUB: + class FakeSimPublisher(SimPublisher): + def get_update(self): + return {} -class FakeSimPublisher(SimPublisher): - def get_update(self): - return {} - - -class FakeSimScene(SimScene): - def __init__(self): - super().__init__() - self.root = SimObject(name="root") + class FakeSimScene(SimScene): + def __init__(self): + super().__init__() + self.root = SimObject(name="root") @dataclass(kw_only=True) @@ -48,6 +52,9 @@ class QuestOperator(BaseOperator): def __init__(self, config: QuestConfig, sim: Sim | None = None): super().__init__(config, sim) + if not HAS_SIMPUB: + raise ImportError("simpub is not installed. Please install it to use QuestOperator.") + self.config: QuestConfig self._resource_lock = threading.Lock() From b213b774cccdb785fe5e0cceac359be9b9d7b5d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Mar 2026 21:50:44 +0100 Subject: [PATCH 08/19] feat(operator): added gello operator --- python/rcs/operator/gello.py | 403 +++++++++++++++++++++++++++++++++++ 1 file changed, 403 insertions(+) create mode 100644 python/rcs/operator/gello.py diff --git a/python/rcs/operator/gello.py b/python/rcs/operator/gello.py new file mode 100644 index 00000000..0e61b5ac --- /dev/null +++ b/python/rcs/operator/gello.py @@ -0,0 +1,403 @@ +import copy +import logging +import threading +import time +from dataclasses import dataclass, field +from typing import Any, Dict, List, Optional, Sequence, Tuple, TypedDict, Iterator + +import numpy as np + +try: + from dynamixel_sdk.group_sync_read import GroupSyncRead + from dynamixel_sdk.group_sync_write import GroupSyncWrite + from dynamixel_sdk.packet_handler import PacketHandler + from dynamixel_sdk.port_handler import PortHandler + from dynamixel_sdk.robotis_def import COMM_SUCCESS + HAS_DYNAMIXEL_SDK = True +except ImportError: + HAS_DYNAMIXEL_SDK = False + +from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo, JointsDictType, GripperDictType +from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands +from rcs.sim.sim import Sim +from rcs.utils import SimpleFrameRate + +logger = logging.getLogger(__name__) + +# --- Dynamixel Driver Constants and Helpers --- + +XL330_CONTROL_TABLE = { + "model_number": {"addr": 0, "len": 2}, + "operating_mode": {"addr": 11, "len": 1}, + "torque_enable": {"addr": 64, "len": 1}, + "kp_d": {"addr": 80, "len": 2}, + "kp_i": {"addr": 82, "len": 2}, + "kp_p": {"addr": 84, "len": 2}, + "goal_current": {"addr": 102, "len": 2}, + "goal_position": {"addr": 116, "len": 4}, + "present_position": {"addr": 132, "len": 4}, +} + + +class DynamixelDriver: + """Simplified Dynamixel driver adapted for RCS.""" + + def __init__( + self, + ids: Sequence[int], + port: str = "/dev/ttyUSB0", + baudrate: int = 57600, + pulses_per_revolution: int = 4095, + ): + if not HAS_DYNAMIXEL_SDK: + raise ImportError("dynamixel_sdk is not installed. Please install it to use GelloOperator.") + + self._ids = ids + self._port = port + self._baudrate = baudrate + self._pulses_per_revolution = pulses_per_revolution + self._lock = threading.Lock() + self._buffered_joint_positions = None + + self._portHandler = PortHandler(self._port) + self._packetHandler = PacketHandler(2.0) + + self._groupSyncReadHandlers = {} + self._groupSyncWriteHandlers = {} + + for key, entry in XL330_CONTROL_TABLE.items(): + self._groupSyncReadHandlers[key] = GroupSyncRead( + self._portHandler, self._packetHandler, entry["addr"], entry["len"] + ) + for dxl_id in self._ids: + self._groupSyncReadHandlers[key].addParam(dxl_id) + + if key != "model_number" and key != "present_position": + self._groupSyncWriteHandlers[key] = GroupSyncWrite( + self._portHandler, self._packetHandler, entry["addr"], entry["len"] + ) + + if not self._portHandler.openPort(): + raise ConnectionError(f"Failed to open port {self._port}") + if not self._portHandler.setBaudRate(self._baudrate): + raise ConnectionError(f"Failed to set baudrate {self._baudrate}") + + self._stop_thread = threading.Event() + self._polling_thread = None + self._is_polling = False + + def write_value_by_name(self, name: str, values: Sequence[int | None]): + if len(values) != len(self._ids): + raise ValueError(f"The length of {name} must match the number of servos") + + handler = self._groupSyncWriteHandlers[name] + value_len = XL330_CONTROL_TABLE[name]["len"] + + with self._lock: + for dxl_id, value in zip(self._ids, values): + if value is None: + continue + param = [(value >> (8 * i)) & 0xFF for i in range(value_len)] + handler.addParam(dxl_id, param) + + comm_result = handler.txPacket() + if comm_result != COMM_SUCCESS: + handler.clearParam() + raise RuntimeError(f"Failed to syncwrite {name}: {self._packetHandler.getTxRxResult(comm_result)}") + handler.clearParam() + + def read_value_by_name(self, name: str) -> List[int]: + handler = self._groupSyncReadHandlers[name] + value_len = XL330_CONTROL_TABLE[name]["len"] + addr = XL330_CONTROL_TABLE[name]["addr"] + + with self._lock: + comm_result = handler.txRxPacket() + if comm_result != COMM_SUCCESS: + raise RuntimeError(f"Failed to sync read {name}: {self._packetHandler.getTxRxResult(comm_result)}") + + values = [] + for dxl_id in self._ids: + if handler.isAvailable(dxl_id, addr, value_len): + value = handler.getData(dxl_id, addr, value_len) + value = int(np.int32(np.uint32(value))) + values.append(value) + else: + raise RuntimeError(f"Failed to get {name} for ID {dxl_id}") + return values + + def start_joint_polling(self): + if self._is_polling: + return + self._stop_thread.clear() + self._polling_thread = threading.Thread(target=self._joint_polling_loop, daemon=True) + self._polling_thread.start() + self._is_polling = True + + def stop_joint_polling(self): + if not self._is_polling: + return + self._stop_thread.set() + if self._polling_thread: + self._polling_thread.join() + self._is_polling = False + + def _joint_polling_loop(self): + while not self._stop_thread.is_set(): + time.sleep(0.001) + try: + self._buffered_joint_positions = np.array(self.read_value_by_name("present_position"), dtype=int) + except RuntimeError as e: + logger.warning(f"Polling error: {e}") + + def get_joints(self) -> np.ndarray: + if self._is_polling: + while self._buffered_joint_positions is None: + time.sleep(0.01) + return self._pulses_to_rad(self._buffered_joint_positions.copy()) + return self._pulses_to_rad(np.array(self.read_value_by_name("present_position"), dtype=int)) + + def _pulses_to_rad(self, pulses) -> np.ndarray: + return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi + + def _rad_to_pulses(self, rad: float) -> int: + return int(rad / (2 * np.pi) * self._pulses_per_revolution) + + def close(self): + self.stop_joint_polling() + if self._portHandler: + self._portHandler.closePort() + + +# --- Gello Hardware Interface Logic --- + +class GelloHardwareParams(TypedDict): + com_port: str + num_arm_joints: int + joint_signs: List[int] + gripper: bool + gripper_range_rad: List[float] + assembly_offsets: List[float] + dynamixel_kp_p: List[int] + dynamixel_kp_i: List[int] + dynamixel_kp_d: List[int] + dynamixel_torque_enable: List[int] + dynamixel_goal_position: List[float] + + +@dataclass +class DynamixelControlConfig: + kp_p: List[int] = field(default_factory=list) + kp_i: List[int] = field(default_factory=list) + kp_d: List[int] = field(default_factory=list) + torque_enable: List[int] = field(default_factory=list) + goal_position: List[int] = field(default_factory=list) + goal_current: List[int] = field(default_factory=list) + operating_mode: List[int] = field(default_factory=list) + + _UPDATE_ORDER = [ + "operating_mode", + "goal_current", + "kp_p", + "kp_i", + "kp_d", + "torque_enable", + "goal_position", + ] + + def __iter__(self) -> Iterator[Tuple[str, List[int]]]: + for param_name in self._UPDATE_ORDER: + if hasattr(self, param_name): + yield param_name, getattr(self, param_name) + + def __getitem__(self, param_name: str) -> List[int]: + return getattr(self, param_name) + + def __setitem__(self, param_name: str, value: List[int]) -> None: + setattr(self, param_name, value) + + +class GelloHardware: + JOINT_POSITION_LIMITS = np.array( + [ + [-2.9007, 2.9007], + [-1.8361, 1.8361], + [-2.9007, 2.9007], + [-3.0770, -0.1169], + [-2.8763, 2.8763], + [0.4398, 4.6216], + [-3.0508, 3.0508], + ] + ) + MID_JOINT_POSITIONS = JOINT_POSITION_LIMITS.mean(axis=1) + OPERATING_MODE = 5 + CURRENT_LIMIT = 600 + + def __init__(self, config: GelloHardwareParams): + self._com_port = config["com_port"] + self._num_arm_joints = config["num_arm_joints"] + self._joint_signs = np.array(config["joint_signs"]) + self._gripper = config["gripper"] + self._num_total_joints = self._num_arm_joints + (1 if self._gripper else 0) + self._gripper_range_rad = config["gripper_range_rad"] + self._assembly_offsets = np.array(config["assembly_offsets"]) + + self._driver = DynamixelDriver( + ids=list(range(1, self._num_total_joints + 1)), + port=self._com_port, + ) + + self._initial_arm_joints_raw = self._driver.get_joints()[: self._num_arm_joints] + initial_arm_joints = self.normalize_joint_positions( + self._initial_arm_joints_raw, self._assembly_offsets, self._joint_signs + ) + self._prev_arm_joints_raw = self._initial_arm_joints_raw.copy() + self._prev_arm_joints = initial_arm_joints.copy() + + self._dynamixel_control_config = DynamixelControlConfig( + kp_p=config["dynamixel_kp_p"].copy(), + kp_i=config["dynamixel_kp_i"].copy(), + kp_d=config["dynamixel_kp_d"].copy(), + torque_enable=config["dynamixel_torque_enable"].copy(), + goal_position=self._goal_position_to_pulses(config["dynamixel_goal_position"]).copy(), + goal_current=[self.CURRENT_LIMIT] * self._num_total_joints, + operating_mode=[self.OPERATING_MODE] * self._num_total_joints, + ) + + self._initialize_parameters() + self._driver.start_joint_polling() + + @staticmethod + def normalize_joint_positions(raw, offsets, signs): + return (np.mod((raw - offsets) * signs - GelloHardware.MID_JOINT_POSITIONS, 2 * np.pi) + - np.pi + GelloHardware.MID_JOINT_POSITIONS) + + def _initialize_parameters(self): + for name, value in self._dynamixel_control_config: + self._driver.write_value_by_name(name, value) + time.sleep(0.1) + + def get_joint_and_gripper_positions(self) -> Tuple[np.ndarray, float]: + joints_raw = self._driver.get_joints() + arm_joints_raw = joints_raw[: self._num_arm_joints] + + arm_joints_delta = (arm_joints_raw - self._prev_arm_joints_raw) * self._joint_signs + arm_joints = self._prev_arm_joints + arm_joints_delta + self._prev_arm_joints = arm_joints.copy() + self._prev_arm_joints_raw = arm_joints_raw.copy() + + arm_joints_clipped = np.clip(arm_joints, self.JOINT_POSITION_LIMITS[:, 0], self.JOINT_POSITION_LIMITS[:, 1]) + + gripper_pos = 0.0 + if self._gripper: + raw_grp = joints_raw[-1] + gripper_pos = (raw_grp - self._gripper_range_rad[0]) / (self._gripper_range_rad[1] - self._gripper_range_rad[0]) + gripper_pos = max(0.0, min(1.0, gripper_pos)) + + return arm_joints_clipped, gripper_pos + + def _goal_position_to_pulses(self, goals): + arm_goals = np.array(goals[: self._num_arm_joints]) + initial_rotations = np.floor_divide(self._initial_arm_joints_raw - self._assembly_offsets - self.MID_JOINT_POSITIONS, 2 * np.pi) + arm_goals_raw = (initial_rotations * 2 * np.pi + arm_goals + self._assembly_offsets) * self._joint_signs + np.pi + goals_raw = np.append(arm_goals_raw, goals[-1]) if self._gripper else arm_goals_raw + return [self._driver._rad_to_pulses(rad) for rad in goals_raw] + + def close(self): + self._driver.write_value_by_name("torque_enable", [0] * self._num_total_joints) + self._driver.close() + + +# --- RCS Operator Implementation --- + +@dataclass(kw_only=True) +class GelloConfig(BaseOperatorConfig): + com_port: str = "/dev/ttyUSB0" + num_arm_joints: int = 7 + joint_signs: List[int] = field(default_factory=lambda: [1] * 7) + gripper: bool = True + gripper_range_rad: List[float] = field(default_factory=lambda: [0.0, 0.0]) # Needs calibration + assembly_offsets: List[float] = field(default_factory=lambda: [0.0] * 7) + dynamixel_kp_p: List[int] = field(default_factory=lambda: [800] * 8) + dynamixel_kp_i: List[int] = field(default_factory=lambda: [0] * 8) + dynamixel_kp_d: List[int] = field(default_factory=lambda: [0] * 8) + dynamixel_torque_enable: List[int] = field(default_factory=lambda: [0] * 8) + dynamixel_goal_position: List[float] = field(default_factory=lambda: [0.0] * 8) + + +class GelloOperator(BaseOperator): + control_mode = (ControlMode.JOINTS, RelativeTo.NONE) + controller_names = ["right"] + + def __init__(self, config: GelloConfig, sim: Sim | None = None): + super().__init__(config, sim) + self.config: GelloConfig + self._resource_lock = threading.Lock() + self._cmd_lock = threading.Lock() + + self._exit_requested = False + self._commands = TeleopCommands() + + self._hw = None + self._last_joints = None + self._last_gripper = 1.0 + + def consume_commands(self) -> TeleopCommands: + with self._cmd_lock: + cmds = copy.copy(self._commands) + self._commands = TeleopCommands() + return cmds + + def reset_operator_state(self): + # GELLO is absolute, but we might want to reset internal deltas if HW allows + pass + + def consume_action(self) -> Dict[str, Any]: + actions = {} + with self._resource_lock: + if self._last_joints is not None: + actions["right"] = { + "joints": self._last_joints.copy(), + "gripper": self._last_gripper + } + return actions + + def run(self): + hw_params: GelloHardwareParams = { + "com_port": self.config.com_port, + "num_arm_joints": self.config.num_arm_joints, + "joint_signs": self.config.joint_signs, + "gripper": self.config.gripper, + "gripper_range_rad": self.config.gripper_range_rad, + "assembly_offsets": self.config.assembly_offsets, + "dynamixel_kp_p": self.config.dynamixel_kp_p, + "dynamixel_kp_i": self.config.dynamixel_kp_i, + "dynamixel_kp_d": self.config.dynamixel_kp_d, + "dynamixel_torque_enable": self.config.dynamixel_torque_enable, + "dynamixel_goal_position": self.config.dynamixel_goal_position, + } + + try: + self._hw = GelloHardware(hw_params) + except Exception as e: + logger.error(f"Failed to initialize GELLO hardware: {e}") + return + + rate_limiter = SimpleFrameRate(self.config.read_frequency, "gello readout") + + while not self._exit_requested: + try: + joints, gripper = self._hw.get_joint_and_gripper_positions() + with self._resource_lock: + self._last_joints = joints + self._last_gripper = gripper + except Exception as e: + logger.warning(f"Error reading GELLO: {e}") + + rate_limiter() + + def close(self): + self._exit_requested = True + if self._hw: + self._hw.close() + self.join() From e5a0bd13cac776abc27355658b46cb11d97c9682 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Mar 2026 21:58:28 +0100 Subject: [PATCH 09/19] refactor(gello): generalize to support multiple --- python/rcs/operator/gello.py | 98 ++++++++++++++++++++++-------------- 1 file changed, 61 insertions(+), 37 deletions(-) diff --git a/python/rcs/operator/gello.py b/python/rcs/operator/gello.py index 0e61b5ac..a6b1d336 100644 --- a/python/rcs/operator/gello.py +++ b/python/rcs/operator/gello.py @@ -304,7 +304,10 @@ def _goal_position_to_pulses(self, goals): return [self._driver._rad_to_pulses(rad) for rad in goals_raw] def close(self): - self._driver.write_value_by_name("torque_enable", [0] * self._num_total_joints) + try: + self._driver.write_value_by_name("torque_enable", [0] * self._num_total_joints) + except: + pass self._driver.close() @@ -312,6 +315,7 @@ def close(self): @dataclass(kw_only=True) class GelloConfig(BaseOperatorConfig): + # Single arm defaults (used if 'arms' is empty) com_port: str = "/dev/ttyUSB0" num_arm_joints: int = 7 joint_signs: List[int] = field(default_factory=lambda: [1] * 7) @@ -324,10 +328,12 @@ class GelloConfig(BaseOperatorConfig): dynamixel_torque_enable: List[int] = field(default_factory=lambda: [0] * 8) dynamixel_goal_position: List[float] = field(default_factory=lambda: [0.0] * 8) + # Dictionary for multi-arm setups: {"left": {"com_port": "/dev/..."}, "right": {...}} + arms: Dict[str, Dict[str, Any]] = field(default_factory=dict) + class GelloOperator(BaseOperator): control_mode = (ControlMode.JOINTS, RelativeTo.NONE) - controller_names = ["right"] def __init__(self, config: GelloConfig, sim: Sim | None = None): super().__init__(config, sim) @@ -338,9 +344,14 @@ def __init__(self, config: GelloConfig, sim: Sim | None = None): self._exit_requested = False self._commands = TeleopCommands() - self._hw = None - self._last_joints = None - self._last_gripper = 1.0 + if self.config.arms: + self.controller_names = list(self.config.arms.keys()) + else: + self.controller_names = ["right"] + + self._last_joints = {name: None for name in self.controller_names} + self._last_gripper = {name: 1.0 for name in self.controller_names} + self._hws: Dict[str, GelloHardware] = {} def consume_commands(self) -> TeleopCommands: with self._cmd_lock: @@ -349,55 +360,68 @@ def consume_commands(self) -> TeleopCommands: return cmds def reset_operator_state(self): - # GELLO is absolute, but we might want to reset internal deltas if HW allows + # GELLO is absolute, no internal state to reset typically pass def consume_action(self) -> Dict[str, Any]: actions = {} with self._resource_lock: - if self._last_joints is not None: - actions["right"] = { - "joints": self._last_joints.copy(), - "gripper": self._last_gripper - } + for name in self.controller_names: + if self._last_joints[name] is not None: + actions[name] = { + "joints": self._last_joints[name].copy(), + "gripper": self._last_gripper[name] + } return actions def run(self): - hw_params: GelloHardwareParams = { - "com_port": self.config.com_port, - "num_arm_joints": self.config.num_arm_joints, - "joint_signs": self.config.joint_signs, - "gripper": self.config.gripper, - "gripper_range_rad": self.config.gripper_range_rad, - "assembly_offsets": self.config.assembly_offsets, - "dynamixel_kp_p": self.config.dynamixel_kp_p, - "dynamixel_kp_i": self.config.dynamixel_kp_i, - "dynamixel_kp_d": self.config.dynamixel_kp_d, - "dynamixel_torque_enable": self.config.dynamixel_torque_enable, - "dynamixel_goal_position": self.config.dynamixel_goal_position, - } + # Prepare hardware parameters for each arm + arms_to_init = self.config.arms if self.config.arms else {"right": {}} + + arm_configs = {} + for name, cfg in arms_to_init.items(): + # Merge with top-level defaults for any missing keys + arm_configs[name] = { + "com_port": cfg.get("com_port", self.config.com_port), + "num_arm_joints": cfg.get("num_arm_joints", self.config.num_arm_joints), + "joint_signs": cfg.get("joint_signs", self.config.joint_signs), + "gripper": cfg.get("gripper", self.config.gripper), + "gripper_range_rad": cfg.get("gripper_range_rad", self.config.gripper_range_rad), + "assembly_offsets": cfg.get("assembly_offsets", self.config.assembly_offsets), + "dynamixel_kp_p": cfg.get("dynamixel_kp_p", self.config.dynamixel_kp_p), + "dynamixel_kp_i": cfg.get("dynamixel_kp_i", self.config.dynamixel_kp_i), + "dynamixel_kp_d": cfg.get("dynamixel_kp_d", self.config.dynamixel_kp_d), + "dynamixel_torque_enable": cfg.get("dynamixel_torque_enable", self.config.dynamixel_torque_enable), + "dynamixel_goal_position": cfg.get("dynamixel_goal_position", self.config.dynamixel_goal_position), + } + + # Initialize all hardware instances + for name, params in arm_configs.items(): + try: + self._hws[name] = GelloHardware(params) + except Exception as e: + logger.error(f"Failed to initialize GELLO hardware for {name}: {e}") - try: - self._hw = GelloHardware(hw_params) - except Exception as e: - logger.error(f"Failed to initialize GELLO hardware: {e}") + if not self._hws: + logger.error("No GELLO hardware initialized. Exiting.") return rate_limiter = SimpleFrameRate(self.config.read_frequency, "gello readout") while not self._exit_requested: - try: - joints, gripper = self._hw.get_joint_and_gripper_positions() - with self._resource_lock: - self._last_joints = joints - self._last_gripper = gripper - except Exception as e: - logger.warning(f"Error reading GELLO: {e}") + for name, hw in self._hws.items(): + try: + joints, gripper = hw.get_joint_and_gripper_positions() + with self._resource_lock: + self._last_joints[name] = joints + self._last_gripper[name] = gripper + except Exception as e: + logger.warning(f"Error reading GELLO {name}: {e}") rate_limiter() def close(self): self._exit_requested = True - if self._hw: - self._hw.close() + for hw in self._hws.values(): + hw.close() self.join() From 0233a988ab288a1ee79210a2acae666b4a70b7cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 9 Apr 2026 13:42:00 +0200 Subject: [PATCH 10/19] feat: robot2world transform and gello operator sync - fixed relative api by adding None option - added robot2world transform offest in multirobot envs - feat: sync gello state to operator --- extensions/rcs_fr3/src/rcs_fr3/creators.py | 5 +- python/rcs/envs/base.py | 27 +++- python/rcs/envs/creators.py | 5 +- python/rcs/operator/franka.py | 64 +++++---- python/rcs/operator/gello.py | 152 +++++++++++---------- python/rcs/operator/interface.py | 105 ++++++++++++-- python/rcs/operator/quest.py | 31 +++-- 7 files changed, 256 insertions(+), 133 deletions(-) diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 0f61e2cc..488dbffa 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -121,7 +121,7 @@ def __call__( # type: ignore # sim_gui=True, # truncate_on_collision=False, # ) - if max_relative_movement is not None: + if relative_to != RelativeTo.NONE: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) return env @@ -137,6 +137,7 @@ def __call__( # type: ignore camera_set: HardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, + robot2world: dict[str, rcs.common.Pose] | None = None, ) -> gym.Env: ik = rcs.common.Pin( @@ -163,7 +164,7 @@ def __call__( # type: ignore env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) envs[key] = env - env = MultiRobotWrapper(envs) + env = MultiRobotWrapper(envs, robot2world) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 17fecb37..df9ce476 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -312,9 +312,26 @@ def close(self): class MultiRobotWrapper(gym.Env): """Wraps a dictionary of environments to allow for multi robot control.""" - def __init__(self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper]): + def __init__(self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper], robot2world: dict[str, common.Pose] | None = None): self.envs = envs self.unwrapped_multi = cast(dict[str, RobotEnv], {key: env.unwrapped for key, env in envs.items()}) + if robot2world is None: + self.robot2world = {} + else: + self.robot2world = robot2world + + def _translate_pose(self, key, dic, to_world=True): + r2w = self.robot2world.get(key, common.Pose()) + if not to_world: + r2w = r2w.inverse() + if "tquat" in dic: + p = r2w * common.Pose(translation=dic["tquat"][:3], quaternion=dic["tquat"][3:]) * r2w.inverse() + dic["tquat"] = np.concatenate([p.translation(), p.rotation_q()]) + if "xyzrpy" in dic: + p = r2w * common.Pose(translation=dic["xyzrpy"][:3], rpy_vector=dic["xyzrpy"][3:]) * r2w.inverse() + dic["xyzrpy"] = p.xyzrpy() + + return dic def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict[str, Any]]: # follows gym env by combinding a dict of envs into a single env @@ -324,7 +341,11 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo truncated = False info = {} for key, env in self.envs.items(): - obs[key], r, t, tr, info[key] = env.step(action[key]) + act = self._translate_pose(key, action[key], to_world=False) + ob, r, t, tr, info[key] = env.step(act) + obs[key] = self._translate_pose(key, ob, to_world=True) + # old + # obs[key], r, t, tr, info[key] = env.step(action[key]) reward += float(r) terminated = terminated or t truncated = truncated or tr @@ -737,7 +758,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: if self.binary: self.gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self.gripper.open() else: - self.gripper.set_normalized_width(next(gripper_action)) + self.gripper.set_normalized_width(gripper_action[0]) self._last_gripper_cmd = gripper_action del action[self.gripper_key] return action diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 83857388..071bf5b9 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -147,6 +147,7 @@ def __call__( # type: ignore max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, sim_wrapper: Type[SimWrapper] | None = None, + robot2world: dict[str, rcs.common.Pose] | None = None, ) -> gym.Env: simulation = sim.Sim(robot_cfg.mjcf_scene_path, sim_cfg) @@ -172,11 +173,11 @@ def __call__( # type: ignore gripper = rcs.sim.SimGripper(simulation, cfg) env = GripperWrapper(env, gripper, binary=True) - if max_relative_movement is not None: + if relative_to != RelativeTo.NONE: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) envs[key] = env - env = MultiRobotWrapper(envs) + env = MultiRobotWrapper(envs, robot2world) env = RobotSimWrapper(env, simulation, sim_wrapper) if cameras is not None: camera_set = typing.cast( diff --git a/python/rcs/operator/franka.py b/python/rcs/operator/franka.py index 83fd78e7..4234e99d 100644 --- a/python/rcs/operator/franka.py +++ b/python/rcs/operator/franka.py @@ -18,28 +18,30 @@ from rcs.envs.creators import SimMultiEnvCreator from rcs.envs.storage_wrapper import StorageWrapper from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg +from rcs.operator.quest import QuestConfig, QuestOperator 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.operator.quest import QuestConfig, QuestOperator +from rcs.operator.gello import GelloConfig, GelloOperator, GelloArmConfig from rcs.operator.interface import TeleopLoop +from simpub.sim.mj_publisher import MujocoPublisher logger = logging.getLogger(__name__) ROBOT2IP = { - # "left": "192.168.102.1", - "right": "192.168.101.1", + "right": "192.168.102.1", + "left": "192.168.101.1", } ROBOT2ID = { - "left": "0", - "right": "1", + "left": "1", + "right": "0", } -ROBOT_INSTANCE = RobotPlatform.SIMULATION -# ROBOT_INSTANCE = RobotPlatform.HARDWARE +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE RECORD_FPS = 30 # set camera dict to none disable cameras @@ -50,7 +52,7 @@ # "bird_eye": "243522070364", # } CAMERA_DICT = None -MQ3_ADDR = "192.168.1.219" +MQ3_ADDR = "10.42.0.1" # DIGIT_DICT = { # "digit_right_left": "D21182", @@ -62,8 +64,17 @@ DATASET_PATH = "test_data_iris_dual_arm14" INSTRUCTION = "build a tower with the blocks in front of you" +robot2world={"right": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[0.89360858, -0.17453293, 0.46425758]), + "left": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[-0.89360858, -0.17453293, -0.46425758])} config = QuestConfig(mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION) +# config = GelloConfig( +# arms={ +# "right": GelloArmConfig(com_port="/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_E505008B503059384C2E3120FF07332D-if00"), +# "left": GelloArmConfig(com_port="/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_ABA78B05503059384C2E3120FF062F26-if00"), +# }, +# simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION, +# ) def get_env(): @@ -81,32 +92,25 @@ def get_env(): name2ip=ROBOT2IP, camera_set=camera_set, robot_cfg=default_fr3_hw_robot_cfg(async_control=True), - control_mode=QuestOperator.control_mode[0], + control_mode=config.operator_class.control_mode[0], gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), - max_relative_movement=(0.5, np.deg2rad(90)), - relative_to=QuestOperator.control_mode[1], - ) - env_rel = StorageWrapper( - env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 + max_relative_movement = 0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90)), + relative_to=config.operator_class.control_mode[1], + robot2world=robot2world, ) - operator = QuestOperator(config) + # env_rel = StorageWrapper( + # env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 + # ) + operator = GelloOperator(config) if isinstance(config, GelloConfig) else QuestOperator(config) else: # FR3 - rcs.scenes["rcs_icra_scene"] = rcs.Scene( - mjcf_scene="/home/tobi/coding/rcs_clones/prs/models/scenes/rcs_icra_scene/scene.xml", - mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, - robot_type=common.RobotType.FR3, - ) - rcs.scenes["pick"] = rcs.Scene( - mjcf_scene="/home/tobi/coding/rcs_clones/prs/assets/scenes/fr3_simple_pick_up/scene.xml", + rcs.scenes["duo"] = rcs.Scene( + mjcf_scene="/ssd_data/juelg/rcs_modern/rcs_models/output/fr3_duo_flexible.xml", mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, robot_type=common.RobotType.FR3, ) - # robot_cfg = default_sim_robot_cfg("fr3_empty_world") - # robot_cfg = default_sim_robot_cfg("fr3_simple_pick_up") - robot_cfg = default_sim_robot_cfg("rcs_icra_scene") - # robot_cfg = default_sim_robot_cfg("pick") + robot_cfg = default_sim_robot_cfg("duo", idx="") # resolution = (256, 256) # cameras = { @@ -125,17 +129,19 @@ def get_env(): env_rel = SimMultiEnvCreator()( name2id=ROBOT2ID, robot_cfg=robot_cfg, - control_mode=QuestOperator.control_mode[0], + control_mode=GelloOperator.control_mode[0], gripper_cfg=default_sim_gripper_cfg(), # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, - relative_to=QuestOperator.control_mode[1], + relative_to=GelloOperator.control_mode[1], sim_cfg=sim_cfg, + robot2world=robot2world, ) # sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim # type: ignore sim = env_rel.get_wrapper_attr("sim") - operator = QuestOperator(config, sim) + operator = GelloOperator(config, sim) if isinstance(config, GelloConfig) else QuestOperator(config, sim) sim.open_gui() + MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) return env_rel, operator diff --git a/python/rcs/operator/gello.py b/python/rcs/operator/gello.py index a6b1d336..66a208f8 100644 --- a/python/rcs/operator/gello.py +++ b/python/rcs/operator/gello.py @@ -17,6 +17,12 @@ except ImportError: HAS_DYNAMIXEL_SDK = False +try: + from pynput import keyboard + HAS_PYNPUT = True +except ImportError: + HAS_PYNPUT = False + from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo, JointsDictType, GripperDictType from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands from rcs.sim.sim import Sim @@ -171,18 +177,32 @@ def close(self): # --- Gello Hardware Interface Logic --- -class GelloHardwareParams(TypedDict): - com_port: str - num_arm_joints: int - joint_signs: List[int] - gripper: bool - gripper_range_rad: List[float] - assembly_offsets: List[float] - dynamixel_kp_p: List[int] - dynamixel_kp_i: List[int] - dynamixel_kp_d: List[int] - dynamixel_torque_enable: List[int] - dynamixel_goal_position: List[float] +@dataclass +class GelloArmConfig: + com_port: str = "/dev/ttyUSB0" + num_arm_joints: int = 7 + joint_signs: List[int] = field(default_factory=lambda: [1, -1, 1, -1, 1, 1, 1]) + gripper: bool = True + gripper_range_rad: List[float] = field(default_factory=lambda: [2.23, 3.22]) + assembly_offsets: List[float] = field( + default_factory=lambda: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] + ) + dynamixel_kp_p: List[int] = field( + default_factory=lambda: [30, 60, 0, 30, 0, 0, 0, 50] + ) + dynamixel_kp_i: List[int] = field( + default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0] + ) + dynamixel_kp_d: List[int] = field( + default_factory=lambda: [250, 100, 80, 60, 30, 10, 5, 0] + ) + dynamixel_torque_enable: List[int] = field( + default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0] + ) + dynamixel_goal_position: List[float] = field( + default_factory=lambda: [0.0, 0.0, 0.0, -1.571, 0.0, 1.571, 0.0, 3.509] + ) + @dataclass @@ -233,14 +253,14 @@ class GelloHardware: OPERATING_MODE = 5 CURRENT_LIMIT = 600 - def __init__(self, config: GelloHardwareParams): - self._com_port = config["com_port"] - self._num_arm_joints = config["num_arm_joints"] - self._joint_signs = np.array(config["joint_signs"]) - self._gripper = config["gripper"] + def __init__(self, config: GelloArmConfig): + self._com_port = config.com_port + self._num_arm_joints = config.num_arm_joints + self._joint_signs = np.array(config.joint_signs) + self._gripper = config.gripper self._num_total_joints = self._num_arm_joints + (1 if self._gripper else 0) - self._gripper_range_rad = config["gripper_range_rad"] - self._assembly_offsets = np.array(config["assembly_offsets"]) + self._gripper_range_rad = config.gripper_range_rad + self._assembly_offsets = np.array(config.assembly_offsets) self._driver = DynamixelDriver( ids=list(range(1, self._num_total_joints + 1)), @@ -255,11 +275,11 @@ def __init__(self, config: GelloHardwareParams): self._prev_arm_joints = initial_arm_joints.copy() self._dynamixel_control_config = DynamixelControlConfig( - kp_p=config["dynamixel_kp_p"].copy(), - kp_i=config["dynamixel_kp_i"].copy(), - kp_d=config["dynamixel_kp_d"].copy(), - torque_enable=config["dynamixel_torque_enable"].copy(), - goal_position=self._goal_position_to_pulses(config["dynamixel_goal_position"]).copy(), + kp_p=config.dynamixel_kp_p.copy(), + kp_i=config.dynamixel_kp_i.copy(), + kp_d=config.dynamixel_kp_d.copy(), + torque_enable=config.dynamixel_torque_enable.copy(), + goal_position=self._goal_position_to_pulses(config.dynamixel_goal_position).copy(), goal_current=[self.CURRENT_LIMIT] * self._num_total_joints, operating_mode=[self.OPERATING_MODE] * self._num_total_joints, ) @@ -313,23 +333,7 @@ def close(self): # --- RCS Operator Implementation --- -@dataclass(kw_only=True) -class GelloConfig(BaseOperatorConfig): - # Single arm defaults (used if 'arms' is empty) - com_port: str = "/dev/ttyUSB0" - num_arm_joints: int = 7 - joint_signs: List[int] = field(default_factory=lambda: [1] * 7) - gripper: bool = True - gripper_range_rad: List[float] = field(default_factory=lambda: [0.0, 0.0]) # Needs calibration - assembly_offsets: List[float] = field(default_factory=lambda: [0.0] * 7) - dynamixel_kp_p: List[int] = field(default_factory=lambda: [800] * 8) - dynamixel_kp_i: List[int] = field(default_factory=lambda: [0] * 8) - dynamixel_kp_d: List[int] = field(default_factory=lambda: [0] * 8) - dynamixel_torque_enable: List[int] = field(default_factory=lambda: [0] * 8) - dynamixel_goal_position: List[float] = field(default_factory=lambda: [0.0] * 8) - # Dictionary for multi-arm setups: {"left": {"com_port": "/dev/..."}, "right": {...}} - arms: Dict[str, Dict[str, Any]] = field(default_factory=dict) class GelloOperator(BaseOperator): @@ -340,18 +344,33 @@ def __init__(self, config: GelloConfig, sim: Sim | None = None): self.config: GelloConfig self._resource_lock = threading.Lock() self._cmd_lock = threading.Lock() - + self._exit_requested = False self._commands = TeleopCommands() - - if self.config.arms: - self.controller_names = list(self.config.arms.keys()) - else: - self.controller_names = ["right"] + + self.controller_names = list(self.config.arms.keys()) self._last_joints = {name: None for name in self.controller_names} self._last_gripper = {name: 1.0 for name in self.controller_names} self._hws: Dict[str, GelloHardware] = {} + + if HAS_PYNPUT: + self._listener = keyboard.Listener(on_press=self._on_press) + self._listener.start() + else: + logger.warning("pynput not found. Keyboard triggers disabled.") + + def _on_press(self, key): + try: + if hasattr(key, "char"): + if key.char == "s": + with self._cmd_lock: + self._commands.sync_position = True + elif key.char == "r": + with self._cmd_lock: + self._commands.failure = True + except AttributeError: + pass def consume_commands(self) -> TeleopCommands: with self._cmd_lock: @@ -370,35 +389,15 @@ def consume_action(self) -> Dict[str, Any]: if self._last_joints[name] is not None: actions[name] = { "joints": self._last_joints[name].copy(), - "gripper": self._last_gripper[name] + "gripper": np.array([self._last_gripper[name]]), } return actions def run(self): - # Prepare hardware parameters for each arm - arms_to_init = self.config.arms if self.config.arms else {"right": {}} - - arm_configs = {} - for name, cfg in arms_to_init.items(): - # Merge with top-level defaults for any missing keys - arm_configs[name] = { - "com_port": cfg.get("com_port", self.config.com_port), - "num_arm_joints": cfg.get("num_arm_joints", self.config.num_arm_joints), - "joint_signs": cfg.get("joint_signs", self.config.joint_signs), - "gripper": cfg.get("gripper", self.config.gripper), - "gripper_range_rad": cfg.get("gripper_range_rad", self.config.gripper_range_rad), - "assembly_offsets": cfg.get("assembly_offsets", self.config.assembly_offsets), - "dynamixel_kp_p": cfg.get("dynamixel_kp_p", self.config.dynamixel_kp_p), - "dynamixel_kp_i": cfg.get("dynamixel_kp_i", self.config.dynamixel_kp_i), - "dynamixel_kp_d": cfg.get("dynamixel_kp_d", self.config.dynamixel_kp_d), - "dynamixel_torque_enable": cfg.get("dynamixel_torque_enable", self.config.dynamixel_torque_enable), - "dynamixel_goal_position": cfg.get("dynamixel_goal_position", self.config.dynamixel_goal_position), - } - # Initialize all hardware instances - for name, params in arm_configs.items(): + for name, arm_cfg in self.config.arms.items(): try: - self._hws[name] = GelloHardware(params) + self._hws[name] = GelloHardware(arm_cfg) except Exception as e: logger.error(f"Failed to initialize GELLO hardware for {name}: {e}") @@ -407,7 +406,7 @@ def run(self): return rate_limiter = SimpleFrameRate(self.config.read_frequency, "gello readout") - + while not self._exit_requested: for name, hw in self._hws.items(): try: @@ -417,11 +416,20 @@ def run(self): self._last_gripper[name] = gripper except Exception as e: logger.warning(f"Error reading GELLO {name}: {e}") - + rate_limiter() def close(self): self._exit_requested = True + if HAS_PYNPUT and hasattr(self, "_listener"): + self._listener.stop() for hw in self._hws.values(): hw.close() - self.join() + if self.is_alive() and threading.current_thread() != self: + self.join(timeout=1.0) + +@dataclass(kw_only=True) +class GelloConfig(BaseOperatorConfig): + operator_class = GelloOperator + # Dictionary for multi-arm setups: {"left": GelloArmConfig(...), "right": GelloArmConfig(...)} + arms: Dict[str, GelloArmConfig] = field(default_factory=lambda: {"right": GelloArmConfig()}) \ No newline at end of file diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index 1f826a50..965dd2a4 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -1,7 +1,9 @@ from abc import ABC import copy from dataclasses import dataclass, field +import logging import threading +import time from time import sleep import gymnasium as gym @@ -9,6 +11,8 @@ from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate +logger = logging.getLogger(__name__) + @dataclass class TeleopCommands: @@ -17,15 +21,10 @@ class TeleopCommands: record: bool = False success: bool = False failure: bool = False + sync_position: bool = False reset_origin_to_current: dict[str, bool] = field(default_factory=dict) -@dataclass(kw_only=True) -class BaseOperatorConfig: - read_frequency: int = 30 - simulation: bool = True - - class BaseOperator(ABC, threading.Thread): control_mode: tuple[ControlMode, RelativeTo] controller_names: list[str] = field(default=["left", "right"]) @@ -53,6 +52,11 @@ def consume_action(self) -> dict[str, ArmWithGripper]: def close(self): pass +@dataclass(kw_only=True) +class BaseOperatorConfig: + operator_class: BaseOperator + read_frequency: int = 30 + simulation: bool = True class TeleopLoop: """Interface for an operator device""" @@ -78,6 +82,9 @@ def __init__( else: self.key_translation = key_translation + # Absolute operators (RelativeTo.NONE) need an initial sync + self._synced = (self.operator.control_mode[1] != RelativeTo.NONE) + def stop(self): self.operator.close() self._exit_requested = True @@ -85,16 +92,32 @@ def stop(self): def __enter__(self): self.operator.start() + # sleep(2) return self def __exit__(self, *_): self.stop() def _translate_keys(self, actions): - return {self.key_translation[key]: actions[key] for key in actions} + translated = {self.key_translation[key]: actions[key] for key in actions} + # Fill in missing robots with "hold" actions from last observation + # This is necessary because absolute environments (like MultiRobotWrapper) + # require actions for all configured robots in every step. + for robot_name in self.env.get_wrapper_attr("envs").keys(): + if robot_name not in translated: + if robot_name in self._last_obs: + translated[robot_name] = { + "joints": self._last_obs[robot_name]["joints"].copy(), + "gripper": self._last_obs[robot_name].get("gripper", 1.0) + } + return translated def environment_step_loop(self): rate_limiter = SimpleFrameRate(self.env_frequency, "env loop") + + # 0. Initial Reset to get current positions for untracked robots + self._last_obs, _ = self.env.reset() + while True: if self._exit_requested: break @@ -110,18 +133,42 @@ def environment_step_loop(self): print("Command: Success! Resetting env...") self.env.get_wrapper_attr("success")() sleep(1) # sleep to let the robot reach the goal - self.env.reset() + self._last_obs, _ = self.env.reset() self.operator.reset_operator_state() + self._synced = (self.operator.control_mode[1] != RelativeTo.NONE) # consume new commands because of potential origin reset continue elif cmds.failure: print("Command: Failure! Resetting env...") - self.env.reset() + self._last_obs, _ = self.env.reset() self.operator.reset_operator_state() + self._synced = (self.operator.control_mode[1] != RelativeTo.NONE) # consume new commands because of potential origin reset continue + if cmds.sync_position: + self.sync_robot_to_operator() + self._synced = True + continue + + if not self._synced: + # Still waiting for sync, step the env with "hold" actions + if int(time.time()) % 5 == 0 and int(time.time() * self.env_frequency) % self.env_frequency == 0: + print("Waiting for sync... (Press 's' on GELLO/Keyboard to sync)") + + hold_actions = {} + for robot_name in self.env.get_wrapper_attr("envs").keys(): + if robot_name in self._last_obs and "joints" in self._last_obs[robot_name]: + hold_actions[robot_name] = { + "joints": self._last_obs[robot_name]["joints"].copy(), + "gripper": self._last_obs[robot_name].get("gripper", 1.0), + } + + self._last_obs, _, _, _, _ = self.env.step(hold_actions) + rate_limiter() + continue + for controller in cmds.reset_origin_to_current: if cmds.reset_origin_to_current[controller]: robot = self.key_translation[controller] @@ -136,6 +183,44 @@ def environment_step_loop(self): # 2. Step the Environment actions = self.operator.consume_action() actions = self._translate_keys(actions) - self.env.step(actions) + self._last_obs, _, _, _, _ = self.env.step(actions) rate_limiter() + + def sync_robot_to_operator(self, duration: float = 3.0): + print(f"Command: Syncing robot to operator (duration: {duration}s)...") + rate_limiter = SimpleFrameRate(self.env_frequency, "sync loop") + num_steps = int(duration * self.env_frequency) + + # 1. Capture the initial state for interpolation + start_obs = copy.deepcopy(self._last_obs) + + # 2. Interpolation Loop + for i in range(num_steps): + alpha = (i + 1) / num_steps + # Re-consume operator action to follow moving target! + target_actions = self._translate_keys(self.operator.consume_action()) + + interp_actions = {} + for robot_name, target in target_actions.items(): + try: + # Interpolate from FIXED start towards MOVING target + s_joints = start_obs[robot_name]["joints"] + t_joints = target["joints"] + interp_joints = s_joints + alpha * (t_joints - s_joints) + + s_gripper = start_obs[robot_name].get("gripper", 1.0) + t_gripper = target.get("gripper", 1.0) + interp_gripper = s_gripper + alpha * (t_gripper - s_gripper) + + interp_actions[robot_name] = { + "joints": interp_joints, + "gripper": interp_gripper, + } + except (KeyError, TypeError): + continue + + self._last_obs, _, _, _, _ = self.env.step(interp_actions) + rate_limiter() + + print("Sync Complete.") diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 9c4ca57d..69381894 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -39,12 +39,6 @@ def __init__(self): self.root = SimObject(name="root") -@dataclass(kw_only=True) -class QuestConfig(BaseOperatorConfig): - include_rotation: bool = True - mq3_addr: str = "10.42.0.1" - - class QuestOperator(BaseOperator): control_mode = (ControlMode.CARTESIAN_TQuat, RelativeTo.CONFIGURED_ORIGIN) @@ -77,9 +71,9 @@ def __init__(self, config: QuestConfig, sim: Sim | None = None): self._step_env = False self._set_frame = {key: Pose() for key in self.controller_names} - if self.config.simulation: - self._publisher = MujocoPublisher(self.sim.model, self.sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) - else: + # if self.config.simulation: + # self._publisher = MujocoPublisher(self.sim.model, self.sim.data, self.config.mq3_addr, visible_geoms_groups=list(range(1, 3))) + if not self.config.simulation: self._publisher = FakeSimPublisher(FakeSimScene(), self.config.mq3_addr) # robot_cfg = default_sim_robot_cfg("fr3_empty_world") # sim_cfg = SimConfig() @@ -145,7 +139,7 @@ def consume_action(self) -> dict[str, ArmWithGripper]: transforms[controller] = TQuatDictType( tquat=np.concatenate([transform.translation(), transform.rotation_q()]) ) - transforms[controller].update(GripperDictType(gripper=self._grp_pos[controller])) + transforms[controller].update(GripperDictType(gripper=np.array([self._grp_pos[controller]]))) return transforms def close(self): @@ -191,11 +185,11 @@ def run(self): 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))) # type: ignore - * last_controller_pose - ) + # if controller == "left": + # last_controller_pose = ( + # Pose(translation=np.array([0, 0, 0]), rpy=RPY(roll=0, pitch=0, yaw=np.deg2rad(180))) # type: ignore + # * 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]] @@ -232,3 +226,10 @@ def run(self): self._prev_data = input_data rate_limiter() + + +@dataclass(kw_only=True) +class QuestConfig(BaseOperatorConfig): + operator_class = QuestOperator + include_rotation: bool = True + mq3_addr: str = "10.42.0.1" \ No newline at end of file From 8af62c7598674d9d778b049d2373c627efb597b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sun, 15 Mar 2026 12:41:20 +0100 Subject: [PATCH 11/19] feat(franka): option to keep tcp offset in desk (#262) --- extensions/rcs_fr3/src/hw/Franka.cpp | 3 +++ extensions/rcs_fr3/src/hw/Franka.h | 1 + extensions/rcs_fr3/src/pybind/rcs.cpp | 2 ++ extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 1 + extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi | 1 + 5 files changed, 8 insertions(+) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 3283685b..d2b0b9e3 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -110,6 +110,9 @@ common::Pose Franka::get_cartesian_position() { x = common::Pose(this->curr_state.O_T_EE); this->interpolator_mutex.unlock(); } + if (!this->cfg.tcp_offset_configured_in_desk) { + return x * cfg.tcp_offset; + } return x; } diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 70f85ec7..789d18a4 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -43,6 +43,7 @@ struct FrankaConfig : common::RobotConfig { std::optional nominal_end_effector_frame = std::nullopt; std::optional world_to_robot = std::nullopt; bool async_control = false; + bool tcp_offset_configured_in_desk = true; }; struct FR3Config : FrankaConfig {}; diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 8c8de868..54b8171a 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -133,6 +133,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("nominal_end_effector_frame", &rcs::hw::FrankaConfig::nominal_end_effector_frame) .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot) + .def_readwrite("tcp_offset_configured_in_desk", + &rcs::hw::FrankaConfig::tcp_offset_configured_in_desk) .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control); py::class_(hw, "FR3Config") diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index f1d22f37..3c1c9082 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -100,6 +100,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): load_parameters: FrankaLoad | None nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float + tcp_offset_configured_in_desk: bool world_to_robot: rcs._core.common.Pose | None def __init__(self) -> None: ... diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index f1d22f37..3c1c9082 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -100,6 +100,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): load_parameters: FrankaLoad | None nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float + tcp_offset_configured_in_desk: bool world_to_robot: rcs._core.common.Pose | None def __init__(self) -> None: ... From 5c74e4205814e209cc43b78c3601c101ae7f28f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 17 Mar 2026 09:51:35 +0100 Subject: [PATCH 12/19] refactor(core): mjb in scene dict optional (#265) --- python/rcs/__init__.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 4131ea78..8c4efd3a 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -2,7 +2,7 @@ import os import site -from dataclasses import dataclass +from dataclasses import dataclass, field from gymnasium import register from rcs._core import __version__, common @@ -18,8 +18,6 @@ class Scene: """Scene configuration.""" - mjb: str | None = None - """Path to the Mujoco binary scene file.""" mjcf_scene: str """Path to the Mujoco scene XML file.""" mjcf_robot: str @@ -28,6 +26,10 @@ class Scene: """Path to the URDF robot file for IK, if available.""" robot_type: common.RobotType """Type of the robot in the scene.""" + mjb: str | None = None + """Path to the Mujoco binary scene file.""" + # TODO: add possibility to add robot config to the scene config (the field below is currently unused) + robot_config: dict[str, common.RobotConfig] = field(default_factory=dict) def get_scene_urdf(scene_name: str) -> str | None: From 5550cb2344ae0f472a1d647cb8b2bf751c66c5ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 18 Mar 2026 10:19:58 +0100 Subject: [PATCH 13/19] style(linting): fix linting in scene dict (#266) --- examples/fr3/fr3_direct_control.py | 3 ++- examples/so101/so101_env_cartesian_control.py | 3 ++- examples/so101/so101_env_joint_control.py | 3 ++- examples/ur5e/ur5e_env_cartesian_control.py | 3 ++- examples/ur5e/ur5e_env_joint_control.py | 3 ++- examples/xarm7/xarm7_env_cartesian_control.py | 3 ++- examples/xarm7/xarm7_env_joint_control.py | 3 ++- python/rcs/envs/utils.py | 9 +++++---- 8 files changed, 19 insertions(+), 11 deletions(-) diff --git a/examples/fr3/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py index 2ce8b7d5..ae3683f3 100644 --- a/examples/fr3/fr3_direct_control.py +++ b/examples/fr3/fr3_direct_control.py @@ -60,7 +60,8 @@ def main(): robot: rcs.common.Robot gripper: rcs.common.Gripper if ROBOT_INSTANCE == RobotPlatform.SIMULATION: - simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb) + scene = rcs.scenes["fr3_empty_world"] + simulation = sim.Sim(scene.mjb or scene.mjcf_scene) robot_cfg = sim.SimRobotConfig() robot_cfg.add_id("0") robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) diff --git a/examples/so101/so101_env_cartesian_control.py b/examples/so101/so101_env_cartesian_control.py index 19c9e4e7..051cc540 100644 --- a/examples/so101/so101_env_cartesian_control.py +++ b/examples/so101/so101_env_cartesian_control.py @@ -25,7 +25,8 @@ def main(): robot_cfg.robot_type = rcs.common.RobotType.SO101 robot_cfg.attachment_site = "gripper" robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb + scene = rcs.scenes["so101_empty_world"] + robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot gripper_cfg = sim.SimGripperConfig() diff --git a/examples/so101/so101_env_joint_control.py b/examples/so101/so101_env_joint_control.py index 2b3b1956..580fd5be 100644 --- a/examples/so101/so101_env_joint_control.py +++ b/examples/so101/so101_env_joint_control.py @@ -26,7 +26,8 @@ def main(): robot_cfg.robot_type = rcs.common.RobotType.SO101 robot_cfg.attachment_site = "gripper" robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb + scene = rcs.scenes["so101_empty_world"] + robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot gripper_cfg = sim.SimGripperConfig() diff --git a/examples/ur5e/ur5e_env_cartesian_control.py b/examples/ur5e/ur5e_env_cartesian_control.py index f0a3eefb..97cc9edb 100644 --- a/examples/ur5e/ur5e_env_cartesian_control.py +++ b/examples/ur5e/ur5e_env_cartesian_control.py @@ -44,7 +44,8 @@ def main(): robot_sim_cfg.robot_type = rcs.common.RobotType.UR5e robot_sim_cfg.attachment_site = "attachment_site" robot_sim_cfg.arm_collision_geoms = [] - robot_sim_cfg.mjcf_scene_path = rcs.scenes["ur5e_empty_world"].mjb + scene = rcs.scenes["ur5e_empty_world"] + robot_sim_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene robot_sim_cfg.kinematic_model_path = rcs.scenes["ur5e_empty_world"].mjcf_robot robot_sim_cfg.base = "base" robot_sim_cfg.tcp_offset = rcs.common.Pose() diff --git a/examples/ur5e/ur5e_env_joint_control.py b/examples/ur5e/ur5e_env_joint_control.py index 5177d380..7caee7e2 100644 --- a/examples/ur5e/ur5e_env_joint_control.py +++ b/examples/ur5e/ur5e_env_joint_control.py @@ -45,7 +45,8 @@ def main(): robot_sim_cfg.robot_type = rcs.common.RobotType.UR5e robot_sim_cfg.attachment_site = "attachment_site" robot_sim_cfg.arm_collision_geoms = [] - robot_sim_cfg.mjcf_scene_path = rcs.scenes["ur5e_empty_world"].mjb + scene = rcs.scenes["ur5e_empty_world"] + robot_sim_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene robot_sim_cfg.kinematic_model_path = rcs.scenes["ur5e_empty_world"].mjcf_robot robot_sim_cfg.base = "base" diff --git a/examples/xarm7/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py index 9bf3e50c..91cce45a 100644 --- a/examples/xarm7/xarm7_env_cartesian_control.py +++ b/examples/xarm7/xarm7_env_cartesian_control.py @@ -60,7 +60,8 @@ def main(): robot_cfg.robot_type = rcs.common.RobotType.XArm7 robot_cfg.attachment_site = "attachment_site" robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + scene = rcs.scenes["xarm7_empty_world"] + robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot env_rel = SimEnvCreator()( robot_cfg=robot_cfg, diff --git a/examples/xarm7/xarm7_env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py index 5b98abdd..9cfcc17b 100644 --- a/examples/xarm7/xarm7_env_joint_control.py +++ b/examples/xarm7/xarm7_env_joint_control.py @@ -60,7 +60,8 @@ def main(): robot_cfg.robot_type = rcs.common.RobotType.XArm7 robot_cfg.attachment_site = "attachment_site" robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + scene = rcs.scenes["xarm7_empty_world"] + robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot env_rel = SimEnvCreator()( robot_cfg=robot_cfg, diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 58fa02ce..3c904082 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -17,15 +17,16 @@ 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 + scene_cfg = rcs.scenes[scene] + robot_cfg.robot_type = scene_cfg.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 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 + robot_cfg.mjcf_scene_path = scene_cfg.mjcf_scene + robot_cfg.kinematic_model_path = scene_cfg.mjcf_robot + # robot_cfg.kinematic_model_path = scene_cfg.urdf return robot_cfg From 9e629b1b4874856b9db60ad8203d4d3b4a47d3b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 8 Apr 2026 10:34:24 +0200 Subject: [PATCH 14/19] fix(core): add constructors for grp config, state and robot config --- python/rcs/_core/common.pyi | 6 +++--- src/pybind/rcs.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 0e51534d..a120cd07 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -105,10 +105,10 @@ class Gripper: def shut(self) -> None: ... class GripperConfig: - pass + def __init__(self) -> None: ... class GripperState: - pass + def __init__(self) -> None: ... class Hand: def __init__(self) -> None: ... @@ -276,7 +276,7 @@ class RobotPlatform: def value(self) -> int: ... class RobotState: - pass + def __init__(self) -> None: ... class RobotType: """ diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 7a36ed70..36b52b47 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -372,9 +372,9 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("tcp_offset", &rcs::sim::SimRobotConfig::tcp_offset) .def_readwrite("robot_platform", &rcs::common::RobotConfig::robot_platform); - py::class_(common, "RobotState"); - py::class_(common, "GripperConfig"); - py::class_(common, "GripperState"); + py::class_(common, "RobotState").def(py::init<>()); + py::class_(common, "GripperConfig").def(py::init<>()); + py::class_(common, "GripperState").def(py::init<>()); py::enum_(common, "GraspType") .value("POWER_GRASP", rcs::common::GraspType::POWER_GRASP) .value("PRECISION_GRASP", rcs::common::GraspType::PRECISION_GRASP) From 48d719ae1833a27088f325e02f126a7e7ad65045 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 8 Apr 2026 10:34:55 +0200 Subject: [PATCH 15/19] feat(robotiq): async support and config - added config with force, speed and async option - added close method to close serial properly - rename class to reflect 2f85 type (hard coded 85mm width) --- extensions/rcs_robotiq2f85/pyproject.toml | 2 +- .../rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py | 57 ++++++++++++++++--- src/pybind/rcs.cpp | 6 +- 3 files changed, 55 insertions(+), 10 deletions(-) diff --git a/extensions/rcs_robotiq2f85/pyproject.toml b/extensions/rcs_robotiq2f85/pyproject.toml index 7e17d803..835dfe02 100644 --- a/extensions/rcs_robotiq2f85/pyproject.toml +++ b/extensions/rcs_robotiq2f85/pyproject.toml @@ -8,7 +8,7 @@ version = "0.6.3" description="RCS RobotiQ module" dependencies = [ "rcs>=0.6.3", - "2f85-python-driver @ git+https://github.com/PhilNad/2f85-python-driver.git", + "2f85-python-driver @ git+https://github.com/RobotControlStack/2f85-python-driver.git", ] readme = "README.md" maintainers = [ diff --git a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py index 0e256899..5e5ee766 100644 --- a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py +++ b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py @@ -1,10 +1,36 @@ -from rcs._core.common import Gripper -from Robotiq2F85Driver.Robotiq2F85Driver import Robotiq2F85Driver +from dataclasses import dataclass +from rcs._core.common import Gripper, GripperConfig, GripperState +from Robotiq2F85Driver.Robotiq2F85Driver import GripperStatus, Robotiq2F85Driver -class RobotiQGripper(Gripper): - def __init__(self, serial_number): + +@dataclass +class RobotiQ2F85GripperConfig(GripperConfig): + speed: float = 100 + """Speed in mm/s. Must be between 20 and 150 mm/s.""" + force: float = 50 + """Force in N. Must be between 20 and 235 N.""" + async_control: bool = True + """If True, gripper commands return immediately without waiting for the movement to complete. + A new command interrupts any ongoing movement.""" + + +@dataclass(kw_only=True) +class RobotiQ2F85GripperState(GripperState): + state: GripperStatus + + def __post_init__(self): + super().__init__() + + +class RobotiQ2F85Gripper(Gripper): + def __init__(self, serial_number: str, cfg: RobotiQ2F85GripperConfig): + """ + serial_number: + Get the serial number with `udevadm info -a -n /dev/ttyUSB0 | grep serial`, make sure you have read/write permissions to the port. + """ super().__init__() + self._cfg: RobotiQ2F85GripperConfig = cfg self.gripper = Robotiq2F85Driver(serial_number=serial_number) def get_normalized_width(self) -> float: @@ -15,7 +41,7 @@ def grasp(self) -> None: """ Close the gripper to grasp an object. """ - self.set_normalized_width(0.0) + self.set_normalized_width(0.0, force=self._cfg.force) def open(self) -> None: """ @@ -26,7 +52,7 @@ def open(self) -> None: def reset(self) -> None: self.gripper.reset() - def set_normalized_width(self, width: float, _: float = 0) -> None: + def set_normalized_width(self, width: float, force: float = 0) -> None: """ Set the gripper width to a normalized value between 0 and 1. """ @@ -34,10 +60,27 @@ def set_normalized_width(self, width: float, _: float = 0) -> None: msg = f"Width must be between 0 and 1, got {width}." raise ValueError(msg) abs_width = width * 85 - self.gripper.go_to(opening=float(abs_width), speed=150.0, force=30.0) + self.gripper.go_to( + opening=float(abs_width), + speed=self._cfg.speed, + force=force if force != 0 else self._cfg.force, + blocking_call=not self._cfg.async_control, + ) def shut(self) -> None: """ Close the gripper. """ self.set_normalized_width(0.0) + + def close(self) -> None: + self.gripper.client.serial.close() + + def get_config(self) -> GripperConfig: + return self._cfg + + def set_config(self, cfg: RobotiQ2F85GripperConfig) -> None: + self._cfg = cfg + + def get_state(self) -> GripperState: + return RobotiQ2F85GripperState(state=self.gripper.read_status()) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 36b52b47..6ae95fa9 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -373,8 +373,10 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("robot_platform", &rcs::common::RobotConfig::robot_platform); py::class_(common, "RobotState").def(py::init<>()); - py::class_(common, "GripperConfig").def(py::init<>()); - py::class_(common, "GripperState").def(py::init<>()); + py::class_(common, "GripperConfig") + .def(py::init<>()); + py::class_(common, "GripperState") + .def(py::init<>()); py::enum_(common, "GraspType") .value("POWER_GRASP", rcs::common::GraspType::POWER_GRASP) .value("PRECISION_GRASP", rcs::common::GraspType::PRECISION_GRASP) From ebb195117bf93c7ce3900ef82b73c8d5fef455a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 9 Apr 2026 16:01:40 +0200 Subject: [PATCH 16/19] docs(example): gello teleop and dependencies - moved teleop script to examples, replacing the quest teleop script - added gello to readme documentation - added gello dependencies - fixed simpub version dependency --- examples/teleop/README.md | 36 +- .../operator => examples/teleop}/franka.py | 0 examples/teleop/quest_iris_dual_arm.py | 388 ------------------ examples/teleop/requirements.txt | 4 +- 4 files changed, 26 insertions(+), 402 deletions(-) rename {python/rcs/operator => examples/teleop}/franka.py (100%) delete mode 100644 examples/teleop/quest_iris_dual_arm.py diff --git a/examples/teleop/README.md b/examples/teleop/README.md index d4a56f2c..4d6bd730 100644 --- a/examples/teleop/README.md +++ b/examples/teleop/README.md @@ -1,33 +1,35 @@ -# Teleoperation with Meta Quest +# Franka Teleoperation + +## Teleoperation with Meta Quest 3 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. +### How does it work? +In the script [`franka.py`](franka.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 +### 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 +Install the IRIS APK on your quest following [these instructions](https://github.com/intuitive-robots/IRIS-Meta-Quest3) use the apk released [here](https://github.com/RobotControlStack/IRIS-Meta-Quest3/releases/tag/rcsv1) to ensure compatiblity. +Finally, install [SimPub](https://github.com/intuitive-robots/SimPublisher) the IRIS python client by (make sure to install it via requirements file to have the correct version) ```shell pip install -r requirements.txt ``` -## Configuration +### Configuration -### Teleoperating in sim +#### Teleoperating in sim -1. go to [`quest_iris_dual_arm.py`](quest_iris_dual_arm.py) and set `ROBOT_INSTANCE = RobotPlatform.SIMULATION` +1. go to [`franka.py`](quest_iris_dual_arm.py) and set `ROBOT_INSTANCE = RobotPlatform.SIMULATION` -### Teleoperating a real robot +#### 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. +2. go to [`franka.py`](quest_iris_dual_arm.py), set `ROBOT_INSTANCE = RobotPlatform.HARDWARE` and set your IP addresses of your robots. Remove the left robot if you only have one. -## Running +### 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). @@ -39,5 +41,13 @@ Note that dual arm is only supported for a aloha like setup where the robot face 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. +8. start the teleoperation script [`franka.py`](quest_iris_dual_arm.py) and enjoy. + +## Teleoperation with Franka GELLO Duo +Teleoperate your Franka Duo using the [Franka GELLO Duo](https://franka.de/de-de/product-prototypes). +Install dependencies via +```shell +pip install -r requirements.txt +``` +and make sure the `GelloConfig` is commented in and the `QuestConfig` is commented out and adapt your USB IDs to it. \ No newline at end of file diff --git a/python/rcs/operator/franka.py b/examples/teleop/franka.py similarity index 100% rename from python/rcs/operator/franka.py rename to examples/teleop/franka.py diff --git a/examples/teleop/quest_iris_dual_arm.py b/examples/teleop/quest_iris_dual_arm.py deleted file mode 100644 index f1beb455..00000000 --- a/examples/teleop/quest_iris_dual_arm.py +++ /dev/null @@ -1,388 +0,0 @@ -import logging -import threading -from time import sleep - -import numpy as np -import rcs -from rcs._core import common -from rcs._core.common import RPY, Pose, RobotPlatform -from rcs._core.sim import CameraType, SimCameraConfig, 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.storage_wrapper import StorageWrapper -from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg -from rcs.utils import SimpleFrameRate -from rcs_fr3.creators import RCSFR3MultiEnvCreator -from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg -# from rcs_realsense.utils import default_realsense -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 - -logger = logging.getLogger(__name__) - -# 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 = { - # "left": "192.168.102.1", - "right": "192.168.101.1", -} -ROBOT2ID = { - "left": "0", - "right": "1", -} - - -ROBOT_INSTANCE = RobotPlatform.SIMULATION -# ROBOT_INSTANCE = RobotPlatform.HARDWARE -RECORD_FPS = 30 -# set camera dict to none disable cameras -# CAMERA_DICT = { -# "left_wrist": "230422272017", -# "right_wrist": "230422271040", -# "side": "243522070385", -# "bird_eye": "243522070364", -# } -CAMERA_DICT = None -MQ3_ADDR = "10.42.0.1" -# DIGIT_DICT = { -# "digit_right_left": "D21182", -# "digit_right_right": "D21193" -# } -DIGIT_DICT = None - -DATASET_PATH = "test_data_iris_dual_arm14" -INSTRUCTION = "build a tower with the blocks in front of you" - - -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() - - def __init__(self, env: RelativeActionSpace): - super().__init__() - self._reader = MetaQuest3("RCSNode") - - self._resource_lock = threading.Lock() - self._env_lock = threading.Lock() - self._reset_lock = threading.Lock() - self._env = env - - self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID.keys() - self._trg_btn = {"left": "index_trigger", "right": "index_trigger"} - self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"} - self._start_btn = "A" - self._stop_btn = "B" - self._unsuccessful_btn = "Y" - - 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 if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID: - 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) -> tuple[dict[str, Pose], dict[str, float]]: - with self._resource_lock: - transforms = {} - for controller in self.controller_names: - transform = Pose( - translation=( - self._last_controller_pose[controller].translation() # type: ignore - - 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 is None: - logger.warning("[Quest Reader] packets empty") - warning_raised = True - sleep(0.5) - continue - if input_data is None: - sleep(0.5) - continue - if warning_raised: - 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.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._reset_lock: - # set successful - self._env.get_wrapper_attr("success")() - # sleep to allow to let the robot reach the goal - sleep(1) - # this might also move the robot to the home position - self._env.reset() - for controller in self.controller_names: - self._offset_pose[controller] = Pose() - self._last_controller_pose[controller] = Pose() - self._grp_pos[controller] = 1 - continue - - # reset unsuccessful - if input_data[self._unsuccessful_btn] and ( - 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 controller == "left": - last_controller_pose = ( - Pose(translation=np.array([0, 0, 0]), rpy=RPY(roll=0, pitch=0, yaw=np.deg2rad(180))) # type: ignore - * 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() # type: ignore - - 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 - with self._reset_lock: - transforms, grippers = self.next_action() - actions = {} - for robot, transform in transforms.items(): - action = dict( - LimitedTQuatRelDictType(tquat=np.concatenate([transform.translation(), transform.rotation_q()])) # type: ignore - ) - - action.update(GripperDictType(gripper=grippers[robot])) - actions[robot] = action - - self._env.step(actions) - rate_limiter() - - -def main(): - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - - camera_set = HardwareCameraSet([default_realsense(CAMERA_DICT), default_digit(DIGIT_DICT)]) if CAMERA_DICT is not None else None # type: ignore - env_rel = RCSFR3MultiEnvCreator()( - name2ip=ROBOT2IP, - camera_set=camera_set, - 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, - ) - 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) - - # 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 = 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) - - else: - # FR3 - rcs.scenes["rcs_icra_scene"] = rcs.Scene( - mjcf_scene="/home/tobi/coding/rcs_clones/prs/models/scenes/rcs_icra_scene/scene.xml", - mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, - robot_type=common.RobotType.FR3, - ) - rcs.scenes["pick"] = rcs.Scene( - mjcf_scene="/home/tobi/coding/rcs_clones/prs/assets/scenes/fr3_simple_pick_up/scene.xml", - mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot, - robot_type=common.RobotType.FR3, - ) - - # robot_cfg = default_sim_robot_cfg("fr3_empty_world") - # robot_cfg = default_sim_robot_cfg("fr3_simple_pick_up") - robot_cfg = default_sim_robot_cfg("rcs_icra_scene") - # robot_cfg = default_sim_robot_cfg("pick") - - resolution = (256, 256) - cameras = { - cam: SimCameraConfig( - identifier=cam, - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=0, - ) - for cam in ["side", "wrist"] - } - - sim_cfg = SimConfig() - sim_cfg.async_control = True - env_rel = SimMultiEnvCreator()( - name2id=ROBOT2ID, - robot_cfg=robot_cfg, - control_mode=ControlMode.CARTESIAN_TQuat, - gripper_cfg=default_sim_gripper_cfg(), - # cameras=cameras, - max_relative_movement=0.5, - relative_to=RelativeTo.CONFIGURED_ORIGIN, - sim_cfg=sim_cfg, - ) - # sim = env_rel.unwrapped.envs[ROBOT2ID.keys().__iter__().__next__()].sim # type: ignore - sim = env_rel.get_wrapper_attr("sim") - sim.open_gui() - # MySimPublisher(MySimScene(), MQ3_ADDR) - 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: # type: ignore - action_server.environment_step_loop() - - -if __name__ == "__main__": - main() diff --git a/examples/teleop/requirements.txt b/examples/teleop/requirements.txt index db25a19a..b6deb24d 100644 --- a/examples/teleop/requirements.txt +++ b/examples/teleop/requirements.txt @@ -1 +1,3 @@ -simpub @ git+https://github.com/intuitive-robots/SimPublisher.git \ No newline at end of file +simpub @ git+https://github.com/intuitive-robots/SimPublisher.git@a0ebdbfba86f58aaa6360eb3b9da7b74a9c9b1dd +dynamixel_sdk +pynput \ No newline at end of file From a652087dbf7df544f8167247dd0fad61d151fd9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 9 Apr 2026 16:21:29 +0200 Subject: [PATCH 17/19] format: cpp and python --- examples/teleop/franka.py | 19 +++++---- python/rcs/envs/base.py | 5 ++- python/rcs/envs/sim.py | 5 +-- python/rcs/envs/utils.py | 4 +- python/rcs/operator/gello.py | 66 +++++++++++++++++--------------- python/rcs/operator/interface.py | 21 +++++----- python/rcs/operator/pedals.py | 45 ++++++++++++---------- python/rcs/operator/quest.py | 17 ++++++-- src/pybind/rcs.cpp | 28 ++++++++------ 9 files changed, 120 insertions(+), 90 deletions(-) diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py index 4234e99d..1157a06e 100644 --- a/examples/teleop/franka.py +++ b/examples/teleop/franka.py @@ -3,9 +3,8 @@ from time import sleep import numpy as np -import rcs -from rcs._core.common import RPY, Pose, RobotPlatform from rcs._core import common +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 ( @@ -18,15 +17,17 @@ from rcs.envs.creators import SimMultiEnvCreator from rcs.envs.storage_wrapper import StorageWrapper from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg +from rcs.operator.gello import GelloArmConfig, GelloConfig, GelloOperator +from rcs.operator.interface import TeleopLoop from rcs.operator.quest import QuestConfig, QuestOperator 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.operator.gello import GelloConfig, GelloOperator, GelloArmConfig -from rcs.operator.interface import TeleopLoop from simpub.sim.mj_publisher import MujocoPublisher +import rcs + logger = logging.getLogger(__name__) @@ -64,8 +65,10 @@ DATASET_PATH = "test_data_iris_dual_arm14" INSTRUCTION = "build a tower with the blocks in front of you" -robot2world={"right": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[0.89360858, -0.17453293, 0.46425758]), - "left": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[-0.89360858, -0.17453293, -0.46425758])} +robot2world = { + "right": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[0.89360858, -0.17453293, 0.46425758]), + "left": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[-0.89360858, -0.17453293, -0.46425758]), +} config = QuestConfig(mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION) # config = GelloConfig( @@ -94,7 +97,9 @@ def get_env(): robot_cfg=default_fr3_hw_robot_cfg(async_control=True), control_mode=config.operator_class.control_mode[0], gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True), - max_relative_movement = 0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90)), + max_relative_movement=( + 0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90)) + ), relative_to=config.operator_class.control_mode[1], robot2world=robot2world, ) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index df9ce476..4d712a0f 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -167,6 +167,7 @@ class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ... CartOrJointContType: TypeAlias = TQuatDictType | JointsDictType | TRPYDictType LimitedCartOrJointContType: TypeAlias = LimitedTQuatRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType + class ArmWithGripper(TQuatDictType, GripperDictType): ... @@ -312,7 +313,9 @@ def close(self): class MultiRobotWrapper(gym.Env): """Wraps a dictionary of environments to allow for multi robot control.""" - def __init__(self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper], robot2world: dict[str, common.Pose] | None = None): + def __init__( + self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper], robot2world: dict[str, common.Pose] | None = None + ): self.envs = envs self.unwrapped_multi = cast(dict[str, RobotEnv], {key: env.unwrapped for key, env in envs.items()}) if robot2world is None: diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index dab96bdc..feafcf06 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -60,9 +60,9 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo self.sim.step_until_convergence() # state = self.sim_robot.get_state() # if "collision" not in info: - # info["collision"] = state.collision + # info["collision"] = state.collision # else: - # info["collision"] = info["collision"] or state.collision + # info["collision"] = info["collision"] or state.collision # info["ik_success"] = state.ik_success info["is_sim_converged"] = self.sim.is_converged() # truncate episode if collision @@ -70,7 +70,6 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo # return obs, 0, False, info["collision"] or not state.ik_success, info return obs, 0, False, False, info - def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 3c904082..c6175a17 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -39,8 +39,8 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() - cfg.collision_geoms = [] - cfg.collision_geoms_fingers = [] + cfg.collision_geoms = [] + cfg.collision_geoms_fingers = [] # cfg.add_id(idx) return cfg diff --git a/python/rcs/operator/gello.py b/python/rcs/operator/gello.py index 66a208f8..27be47ee 100644 --- a/python/rcs/operator/gello.py +++ b/python/rcs/operator/gello.py @@ -3,7 +3,7 @@ import threading import time from dataclasses import dataclass, field -from typing import Any, Dict, List, Optional, Sequence, Tuple, TypedDict, Iterator +from typing import Any, Dict, Iterator, List, Optional, Sequence, Tuple, TypedDict import numpy as np @@ -13,17 +13,25 @@ from dynamixel_sdk.packet_handler import PacketHandler from dynamixel_sdk.port_handler import PortHandler from dynamixel_sdk.robotis_def import COMM_SUCCESS + HAS_DYNAMIXEL_SDK = True except ImportError: HAS_DYNAMIXEL_SDK = False try: from pynput import keyboard + HAS_PYNPUT = True except ImportError: HAS_PYNPUT = False -from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo, JointsDictType, GripperDictType +from rcs.envs.base import ( + ArmWithGripper, + ControlMode, + GripperDictType, + JointsDictType, + RelativeTo, +) from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate @@ -95,7 +103,7 @@ def __init__( def write_value_by_name(self, name: str, values: Sequence[int | None]): if len(values) != len(self._ids): raise ValueError(f"The length of {name} must match the number of servos") - + handler = self._groupSyncWriteHandlers[name] value_len = XL330_CONTROL_TABLE[name]["len"] @@ -105,7 +113,7 @@ def write_value_by_name(self, name: str, values: Sequence[int | None]): continue param = [(value >> (8 * i)) & 0xFF for i in range(value_len)] handler.addParam(dxl_id, param) - + comm_result = handler.txPacket() if comm_result != COMM_SUCCESS: handler.clearParam() @@ -121,7 +129,7 @@ def read_value_by_name(self, name: str) -> List[int]: comm_result = handler.txRxPacket() if comm_result != COMM_SUCCESS: raise RuntimeError(f"Failed to sync read {name}: {self._packetHandler.getTxRxResult(comm_result)}") - + values = [] for dxl_id in self._ids: if handler.isAvailable(dxl_id, addr, value_len): @@ -177,6 +185,7 @@ def close(self): # --- Gello Hardware Interface Logic --- + @dataclass class GelloArmConfig: com_port: str = "/dev/ttyUSB0" @@ -184,27 +193,16 @@ class GelloArmConfig: joint_signs: List[int] = field(default_factory=lambda: [1, -1, 1, -1, 1, 1, 1]) gripper: bool = True gripper_range_rad: List[float] = field(default_factory=lambda: [2.23, 3.22]) - assembly_offsets: List[float] = field( - default_factory=lambda: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] - ) - dynamixel_kp_p: List[int] = field( - default_factory=lambda: [30, 60, 0, 30, 0, 0, 0, 50] - ) - dynamixel_kp_i: List[int] = field( - default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0] - ) - dynamixel_kp_d: List[int] = field( - default_factory=lambda: [250, 100, 80, 60, 30, 10, 5, 0] - ) - dynamixel_torque_enable: List[int] = field( - default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0] - ) + assembly_offsets: List[float] = field(default_factory=lambda: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000]) + dynamixel_kp_p: List[int] = field(default_factory=lambda: [30, 60, 0, 30, 0, 0, 0, 50]) + dynamixel_kp_i: List[int] = field(default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0]) + dynamixel_kp_d: List[int] = field(default_factory=lambda: [250, 100, 80, 60, 30, 10, 5, 0]) + dynamixel_torque_enable: List[int] = field(default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0]) dynamixel_goal_position: List[float] = field( default_factory=lambda: [0.0, 0.0, 0.0, -1.571, 0.0, 1.571, 0.0, 3.509] ) - @dataclass class DynamixelControlConfig: kp_p: List[int] = field(default_factory=list) @@ -289,8 +287,11 @@ def __init__(self, config: GelloArmConfig): @staticmethod def normalize_joint_positions(raw, offsets, signs): - return (np.mod((raw - offsets) * signs - GelloHardware.MID_JOINT_POSITIONS, 2 * np.pi) - - np.pi + GelloHardware.MID_JOINT_POSITIONS) + return ( + np.mod((raw - offsets) * signs - GelloHardware.MID_JOINT_POSITIONS, 2 * np.pi) + - np.pi + + GelloHardware.MID_JOINT_POSITIONS + ) def _initialize_parameters(self): for name, value in self._dynamixel_control_config: @@ -300,25 +301,29 @@ def _initialize_parameters(self): def get_joint_and_gripper_positions(self) -> Tuple[np.ndarray, float]: joints_raw = self._driver.get_joints() arm_joints_raw = joints_raw[: self._num_arm_joints] - + arm_joints_delta = (arm_joints_raw - self._prev_arm_joints_raw) * self._joint_signs arm_joints = self._prev_arm_joints + arm_joints_delta self._prev_arm_joints = arm_joints.copy() self._prev_arm_joints_raw = arm_joints_raw.copy() arm_joints_clipped = np.clip(arm_joints, self.JOINT_POSITION_LIMITS[:, 0], self.JOINT_POSITION_LIMITS[:, 1]) - + gripper_pos = 0.0 if self._gripper: raw_grp = joints_raw[-1] - gripper_pos = (raw_grp - self._gripper_range_rad[0]) / (self._gripper_range_rad[1] - self._gripper_range_rad[0]) + gripper_pos = (raw_grp - self._gripper_range_rad[0]) / ( + self._gripper_range_rad[1] - self._gripper_range_rad[0] + ) gripper_pos = max(0.0, min(1.0, gripper_pos)) return arm_joints_clipped, gripper_pos def _goal_position_to_pulses(self, goals): arm_goals = np.array(goals[: self._num_arm_joints]) - initial_rotations = np.floor_divide(self._initial_arm_joints_raw - self._assembly_offsets - self.MID_JOINT_POSITIONS, 2 * np.pi) + initial_rotations = np.floor_divide( + self._initial_arm_joints_raw - self._assembly_offsets - self.MID_JOINT_POSITIONS, 2 * np.pi + ) arm_goals_raw = (initial_rotations * 2 * np.pi + arm_goals + self._assembly_offsets) * self._joint_signs + np.pi goals_raw = np.append(arm_goals_raw, goals[-1]) if self._gripper else arm_goals_raw return [self._driver._rad_to_pulses(rad) for rad in goals_raw] @@ -334,8 +339,6 @@ def close(self): # --- RCS Operator Implementation --- - - class GelloOperator(BaseOperator): control_mode = (ControlMode.JOINTS, RelativeTo.NONE) @@ -353,7 +356,7 @@ def __init__(self, config: GelloConfig, sim: Sim | None = None): self._last_joints = {name: None for name in self.controller_names} self._last_gripper = {name: 1.0 for name in self.controller_names} self._hws: Dict[str, GelloHardware] = {} - + if HAS_PYNPUT: self._listener = keyboard.Listener(on_press=self._on_press) self._listener.start() @@ -428,8 +431,9 @@ def close(self): if self.is_alive() and threading.current_thread() != self: self.join(timeout=1.0) + @dataclass(kw_only=True) class GelloConfig(BaseOperatorConfig): operator_class = GelloOperator # Dictionary for multi-arm setups: {"left": GelloArmConfig(...), "right": GelloArmConfig(...)} - arms: Dict[str, GelloArmConfig] = field(default_factory=lambda: {"right": GelloArmConfig()}) \ No newline at end of file + arms: Dict[str, GelloArmConfig] = field(default_factory=lambda: {"right": GelloArmConfig()}) diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index 965dd2a4..c7388d69 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -1,12 +1,12 @@ -from abc import ABC import copy -from dataclasses import dataclass, field import logging import threading import time +from abc import ABC +from dataclasses import dataclass, field from time import sleep -import gymnasium as gym +import gymnasium as gym from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate @@ -52,12 +52,14 @@ def consume_action(self) -> dict[str, ArmWithGripper]: def close(self): pass + @dataclass(kw_only=True) class BaseOperatorConfig: operator_class: BaseOperator read_frequency: int = 30 simulation: bool = True + class TeleopLoop: """Interface for an operator device""" @@ -83,7 +85,7 @@ def __init__( self.key_translation = key_translation # Absolute operators (RelativeTo.NONE) need an initial sync - self._synced = (self.operator.control_mode[1] != RelativeTo.NONE) + self._synced = self.operator.control_mode[1] != RelativeTo.NONE def stop(self): self.operator.close() @@ -108,13 +110,13 @@ def _translate_keys(self, actions): if robot_name in self._last_obs: translated[robot_name] = { "joints": self._last_obs[robot_name]["joints"].copy(), - "gripper": self._last_obs[robot_name].get("gripper", 1.0) + "gripper": self._last_obs[robot_name].get("gripper", 1.0), } return translated def environment_step_loop(self): rate_limiter = SimpleFrameRate(self.env_frequency, "env loop") - + # 0. Initial Reset to get current positions for untracked robots self._last_obs, _ = self.env.reset() @@ -135,7 +137,7 @@ def environment_step_loop(self): sleep(1) # sleep to let the robot reach the goal self._last_obs, _ = self.env.reset() self.operator.reset_operator_state() - self._synced = (self.operator.control_mode[1] != RelativeTo.NONE) + self._synced = self.operator.control_mode[1] != RelativeTo.NONE # consume new commands because of potential origin reset continue @@ -143,7 +145,7 @@ def environment_step_loop(self): print("Command: Failure! Resetting env...") self._last_obs, _ = self.env.reset() self.operator.reset_operator_state() - self._synced = (self.operator.control_mode[1] != RelativeTo.NONE) + self._synced = self.operator.control_mode[1] != RelativeTo.NONE # consume new commands because of potential origin reset continue @@ -174,7 +176,8 @@ def environment_step_loop(self): robot = self.key_translation[controller] print(f"Command: Resetting origin for {robot}...") assert ( - self.operator.control_mode[1] == RelativeTo.CONFIGURED_ORIGIN + self.operator.control_mode[1] + == RelativeTo.CONFIGURED_ORIGIN # TODO the following is a dict and can thus not easily be used like this # and self.env.get_wrapper_attr("relative_to") == RelativeTo.CONFIGURED_ORIGIN ), "both robot env and operator must be configured to relative_to.CONFIGURED_ORIGIN" diff --git a/python/rcs/operator/pedals.py b/python/rcs/operator/pedals.py index e1865f51..470f1150 100644 --- a/python/rcs/operator/pedals.py +++ b/python/rcs/operator/pedals.py @@ -1,24 +1,26 @@ -import evdev -from evdev import ecodes import threading import time +import evdev +from evdev import ecodes + + class FootPedal: def __init__(self, device_name_substring="Foot Switch"): """Initializes the foot pedal and starts the background reading thread.""" self.device_path = self._find_device(device_name_substring) - + if not self.device_path: raise FileNotFoundError(f"Could not find a device matching '{device_name_substring}'") - + self.device = evdev.InputDevice(self.device_path) self.device.grab() # Prevent events from leaking into the OS/terminal - - # Dictionary to hold the current state of each key. + + # Dictionary to hold the current state of each key. # True = Pressed/Held, False = Released self._key_states = {} self._lock = threading.Lock() - + # Start the background thread self._running = True self._thread = threading.Thread(target=self._read_events, daemon=True) @@ -39,24 +41,24 @@ def _read_events(self): for event in self.device.read_loop(): if not self._running: break - + if event.type == ecodes.EV_KEY: key_event = evdev.categorize(event) - + with self._lock: # keystate: 1 is DOWN, 2 is HOLD, 0 is UP is_pressed = key_event.keystate in [1, 2] - + # Store state using the string name of the key (e.g., 'KEY_A') # If a key resolves to a list (rare, but happens in evdev), take the first one key_name = key_event.keycode if isinstance(key_name, list): key_name = key_name[0] - + self._key_states[key_name] = is_pressed - + except OSError: - pass # Device disconnected or closed + pass # Device disconnected or closed def get_states(self): """ @@ -81,6 +83,7 @@ def close(self): except OSError: pass + # ========================================== # Example Usage # ========================================== @@ -88,24 +91,24 @@ def close(self): try: # Initialize the pedal pedal = FootPedal("Foot Switch") - + # Simulate a typical robotics control loop running at 10Hz print("Starting control loop... Press Ctrl+C to exit.") while True: # Grab the latest states instantly without blocking states = pedal.get_states() - + if states: # Print only the keys that are currently pressed pressed_keys = [key for key, is_pressed in states.items() if is_pressed] print(f"Currently pressed: {pressed_keys}") - + # Your teleoperation logic goes here... - - time.sleep(0.1) # 10Hz loop - + + time.sleep(0.1) # 10Hz loop + except KeyboardInterrupt: print("\nShutting down...") finally: - if 'pedal' in locals(): - pedal.close() \ No newline at end of file + if "pedal" in locals(): + pedal.close() diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 69381894..90d9c012 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -1,12 +1,19 @@ import copy -from dataclasses import dataclass import logging import threading +from dataclasses import dataclass from time import sleep import numpy as np -from rcs._core.common import Pose, RPY -from rcs.envs.base import ArmWithGripper, ControlMode, GripperDictType, RelativeActionSpace, RelativeTo, TQuatDictType +from rcs._core.common import RPY, Pose +from rcs.envs.base import ( + ArmWithGripper, + ControlMode, + GripperDictType, + RelativeActionSpace, + RelativeTo, + TQuatDictType, +) from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate @@ -16,6 +23,7 @@ from simpub.parser.simdata import SimObject, SimScene from simpub.sim.mj_publisher import MujocoPublisher from simpub.xr_device.meta_quest3 import MetaQuest3 + HAS_SIMPUB = True except ImportError: HAS_SIMPUB = False @@ -29,6 +37,7 @@ # adb install IRIS-Meta-Quest3.apk if HAS_SIMPUB: + class FakeSimPublisher(SimPublisher): def get_update(self): return {} @@ -232,4 +241,4 @@ def run(self): class QuestConfig(BaseOperatorConfig): operator_class = QuestOperator include_rotation: bool = True - mq3_addr: str = "10.42.0.1" \ No newline at end of file + mq3_addr: str = "10.42.0.1" diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 6ae95fa9..78726e2e 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -474,12 +474,14 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("joints", &rcs::sim::SimRobotConfig::joints) .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators) .def_readwrite("base", &rcs::sim::SimRobotConfig::base) - .def("__copy__", [](const rcs::sim::SimRobotConfig &self) { - return rcs::sim::SimRobotConfig(self); - }) - .def("__deepcopy__", [](const rcs::sim::SimRobotConfig &self, py::dict) { - return rcs::sim::SimRobotConfig(self); - }) + .def("__copy__", + [](const rcs::sim::SimRobotConfig& self) { + return rcs::sim::SimRobotConfig(self); + }) + .def("__deepcopy__", + [](const rcs::sim::SimRobotConfig& self, py::dict) { + return rcs::sim::SimRobotConfig(self); + }) .def("add_id", &rcs::sim::SimRobotConfig::add_id, py::arg("id")); py::class_(sim, "SimRobotState") @@ -518,12 +520,14 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::SimGripperConfig::max_actuator_width) .def_readwrite("min_actuator_width", &rcs::sim::SimGripperConfig::min_actuator_width) - .def("__copy__", [](const rcs::sim::SimGripperConfig &self) { - return rcs::sim::SimGripperConfig(self); - }) - .def("__deepcopy__", [](const rcs::sim::SimGripperConfig &self, py::dict) { - return rcs::sim::SimGripperConfig(self); - }) + .def("__copy__", + [](const rcs::sim::SimGripperConfig& self) { + return rcs::sim::SimGripperConfig(self); + }) + .def("__deepcopy__", + [](const rcs::sim::SimGripperConfig& self, py::dict) { + return rcs::sim::SimGripperConfig(self); + }) .def("add_id", &rcs::sim::SimGripperConfig::add_id, py::arg("id")); py::class_( sim, "SimGripperState") From 08805e69ba66fa5bf4e90b08b4b2aefd350b02f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 9 Apr 2026 17:12:22 +0200 Subject: [PATCH 18/19] lint: fixing linting errors --- examples/teleop/franka.py | 28 +++++++-------- python/rcs/envs/base.py | 5 +-- python/rcs/envs/creators.py | 6 ++-- python/rcs/envs/space_utils.py | 1 + python/rcs/envs/utils.py | 8 ++--- python/rcs/operator/gello.py | 59 ++++++++++++++++---------------- python/rcs/operator/interface.py | 23 ++++++------- python/rcs/operator/pedals.py | 22 ++++++------ python/rcs/operator/quest.py | 29 ++++++---------- python/tests/test_sim_envs.py | 22 ++++++------ 10 files changed, 98 insertions(+), 105 deletions(-) diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py index 1157a06e..042b60e9 100644 --- a/examples/teleop/franka.py +++ b/examples/teleop/franka.py @@ -1,26 +1,17 @@ import logging -import threading -from time import sleep +from typing import Any import numpy as np from rcs._core import common -from rcs._core.common import RPY, Pose, RobotPlatform +from rcs._core.common import 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.base import ControlMode from rcs.envs.creators import SimMultiEnvCreator -from rcs.envs.storage_wrapper import StorageWrapper from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg -from rcs.operator.gello import GelloArmConfig, GelloConfig, GelloOperator +from rcs.operator.gello import GelloConfig, GelloOperator from rcs.operator.interface import TeleopLoop from rcs.operator.quest import QuestConfig, QuestOperator -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 @@ -66,10 +57,15 @@ INSTRUCTION = "build a tower with the blocks in front of you" robot2world = { - "right": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[0.89360858, -0.17453293, 0.46425758]), - "left": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[-0.89360858, -0.17453293, -0.46425758]), + "right": rcs.common.Pose( + translation=np.array([0, 0, 0]), rpy_vector=np.array([0.89360858, -0.17453293, 0.46425758]) + ), + "left": rcs.common.Pose( + translation=np.array([0, 0, 0]), rpy_vector=np.array([-0.89360858, -0.17453293, -0.46425758]) + ), } +config: QuestConfig | GelloConfig config = QuestConfig(mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION) # config = GelloConfig( # arms={ @@ -83,7 +79,7 @@ def get_env(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - cams = [] + cams: list[Any] = [] if CAMERA_DICT is not None: cams.append(default_realsense(CAMERA_DICT)) if DIGIT_DICT is not None: diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 4d712a0f..9ae52a89 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -12,6 +12,7 @@ from rcs.envs.space_utils import ( ActObsInfoWrapper, RCSpaceType, + Vec1Type, Vec6Type, Vec7Type, Vec18Type, @@ -99,12 +100,12 @@ class LimitedJointsRelDictType(RCSpaceType): class GripperDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) - gripper: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] + gripper: Annotated[Vec1Type, gym.spaces.Box(low=0, high=1, shape=(1,), dtype=np.float32)] class HandBinDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) - gripper: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] + gripper: Annotated[Vec1Type, gym.spaces.Box(low=0, high=1, shape=(1,), dtype=np.float32)] class HandVecDictType(RCSpaceType): diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 071bf5b9..4a693bad 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -168,9 +168,9 @@ def __call__( # type: ignore for key, mid in name2id.items(): env: gym.Env = RobotEnv(robots[key], control_mode) if gripper_cfg is not None: - cfg = copy.copy(gripper_cfg) - cfg.add_id(mid) - gripper = rcs.sim.SimGripper(simulation, cfg) + gripper_cfg_copy = copy.copy(gripper_cfg) + gripper_cfg_copy.add_id(mid) + gripper = rcs.sim.SimGripper(simulation, gripper_cfg_copy) env = GripperWrapper(env, gripper, binary=True) if relative_to != RelativeTo.NONE: diff --git a/python/rcs/envs/space_utils.py b/python/rcs/envs/space_utils.py index 40c316d1..6f25fabe 100644 --- a/python/rcs/envs/space_utils.py +++ b/python/rcs/envs/space_utils.py @@ -17,6 +17,7 @@ M = TypeVar("M", bound=int) VecType: TypeAlias = np.ndarray[M, np.dtype[np.float64]] +Vec1Type: TypeAlias = np.ndarray[tuple[Literal[1]], np.dtype[np.float64]] Vec7Type: TypeAlias = np.ndarray[tuple[Literal[7]], np.dtype[np.float64]] Vec3Type: TypeAlias = np.ndarray[tuple[Literal[3]], np.dtype[np.float64]] Vec6Type: TypeAlias = np.ndarray[tuple[Literal[6]], np.dtype[np.float64]] diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index c6175a17..f375006b 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -20,9 +20,9 @@ def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim scene_cfg = rcs.scenes[scene] robot_cfg.robot_type = scene_cfg.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 + robot_cfg.add_id(idx) + if (mjb := rcs.scenes[scene].mjb) is not None: + robot_cfg.mjcf_scene_path = mjb else: robot_cfg.mjcf_scene_path = scene_cfg.mjcf_scene robot_cfg.kinematic_model_path = scene_cfg.mjcf_robot @@ -37,7 +37,7 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: return hand_cfg -def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: +def default_sim_gripper_cfg(_idx: str = "0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() cfg.collision_geoms = [] cfg.collision_geoms_fingers = [] diff --git a/python/rcs/operator/gello.py b/python/rcs/operator/gello.py index 27be47ee..418c736b 100644 --- a/python/rcs/operator/gello.py +++ b/python/rcs/operator/gello.py @@ -1,9 +1,10 @@ +import contextlib import copy import logging import threading import time from dataclasses import dataclass, field -from typing import Any, Dict, Iterator, List, Optional, Sequence, Tuple, TypedDict +from typing import Any, ClassVar, Dict, Iterator, List, Sequence, Tuple import numpy as np @@ -25,13 +26,7 @@ except ImportError: HAS_PYNPUT = False -from rcs.envs.base import ( - ArmWithGripper, - ControlMode, - GripperDictType, - JointsDictType, - RelativeTo, -) +from rcs.envs.base import ControlMode, RelativeTo from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate @@ -64,14 +59,15 @@ def __init__( pulses_per_revolution: int = 4095, ): if not HAS_DYNAMIXEL_SDK: - raise ImportError("dynamixel_sdk is not installed. Please install it to use GelloOperator.") + msg = "dynamixel_sdk is not installed. Please install it to use GelloOperator." + raise ImportError(msg) self._ids = ids self._port = port self._baudrate = baudrate self._pulses_per_revolution = pulses_per_revolution self._lock = threading.Lock() - self._buffered_joint_positions = None + self._buffered_joint_positions: np.ndarray | None = None self._portHandler = PortHandler(self._port) self._packetHandler = PacketHandler(2.0) @@ -86,29 +82,32 @@ def __init__( for dxl_id in self._ids: self._groupSyncReadHandlers[key].addParam(dxl_id) - if key != "model_number" and key != "present_position": + if key not in {"model_number", "present_position"}: self._groupSyncWriteHandlers[key] = GroupSyncWrite( self._portHandler, self._packetHandler, entry["addr"], entry["len"] ) if not self._portHandler.openPort(): - raise ConnectionError(f"Failed to open port {self._port}") + msg = f"Failed to open port {self._port}" + raise ConnectionError(msg) if not self._portHandler.setBaudRate(self._baudrate): - raise ConnectionError(f"Failed to set baudrate {self._baudrate}") + msg = f"Failed to set baudrate {self._baudrate}" + raise ConnectionError(msg) self._stop_thread = threading.Event() - self._polling_thread = None + self._polling_thread: threading.Thread | None = None self._is_polling = False def write_value_by_name(self, name: str, values: Sequence[int | None]): if len(values) != len(self._ids): - raise ValueError(f"The length of {name} must match the number of servos") + msg = f"The length of {name} must match the number of servos" + raise ValueError(msg) handler = self._groupSyncWriteHandlers[name] value_len = XL330_CONTROL_TABLE[name]["len"] with self._lock: - for dxl_id, value in zip(self._ids, values): + for dxl_id, value in zip(self._ids, values, strict=False): if value is None: continue param = [(value >> (8 * i)) & 0xFF for i in range(value_len)] @@ -117,7 +116,8 @@ def write_value_by_name(self, name: str, values: Sequence[int | None]): comm_result = handler.txPacket() if comm_result != COMM_SUCCESS: handler.clearParam() - raise RuntimeError(f"Failed to syncwrite {name}: {self._packetHandler.getTxRxResult(comm_result)}") + msg = f"Failed to syncwrite {name}: {self._packetHandler.getTxRxResult(comm_result)}" + raise RuntimeError(msg) handler.clearParam() def read_value_by_name(self, name: str) -> List[int]: @@ -128,7 +128,8 @@ def read_value_by_name(self, name: str) -> List[int]: with self._lock: comm_result = handler.txRxPacket() if comm_result != COMM_SUCCESS: - raise RuntimeError(f"Failed to sync read {name}: {self._packetHandler.getTxRxResult(comm_result)}") + msg = f"Failed to sync read {name}: {self._packetHandler.getTxRxResult(comm_result)}" + raise RuntimeError(msg) values = [] for dxl_id in self._ids: @@ -137,7 +138,8 @@ def read_value_by_name(self, name: str) -> List[int]: value = int(np.int32(np.uint32(value))) values.append(value) else: - raise RuntimeError(f"Failed to get {name} for ID {dxl_id}") + msg = f"Failed to get {name} for ID {dxl_id}" + raise RuntimeError(msg) return values def start_joint_polling(self): @@ -213,7 +215,7 @@ class DynamixelControlConfig: goal_current: List[int] = field(default_factory=list) operating_mode: List[int] = field(default_factory=list) - _UPDATE_ORDER = [ + _UPDATE_ORDER: ClassVar[list[str]] = [ "operating_mode", "goal_current", "kp_p", @@ -329,10 +331,8 @@ def _goal_position_to_pulses(self, goals): return [self._driver._rad_to_pulses(rad) for rad in goals_raw] def close(self): - try: + with contextlib.suppress(Exception): self._driver.write_value_by_name("torque_enable", [0] * self._num_total_joints) - except: - pass self._driver.close() @@ -342,7 +342,7 @@ def close(self): class GelloOperator(BaseOperator): control_mode = (ControlMode.JOINTS, RelativeTo.NONE) - def __init__(self, config: GelloConfig, sim: Sim | None = None): + def __init__(self, config: "GelloConfig", sim: Sim | None = None): super().__init__(config, sim) self.config: GelloConfig self._resource_lock = threading.Lock() @@ -353,7 +353,7 @@ def __init__(self, config: GelloConfig, sim: Sim | None = None): self.controller_names = list(self.config.arms.keys()) - self._last_joints = {name: None for name in self.controller_names} + self._last_joints: Dict[str, np.ndarray | None] = {name: None for name in self.controller_names} self._last_gripper = {name: 1.0 for name in self.controller_names} self._hws: Dict[str, GelloHardware] = {} @@ -386,12 +386,13 @@ def reset_operator_state(self): pass def consume_action(self) -> Dict[str, Any]: - actions = {} + actions: Dict[str, Any] = {} with self._resource_lock: for name in self.controller_names: - if self._last_joints[name] is not None: + joints = self._last_joints[name] + if joints is not None: actions[name] = { - "joints": self._last_joints[name].copy(), + "joints": joints.copy(), "gripper": np.array([self._last_gripper[name]]), } return actions @@ -434,6 +435,6 @@ def close(self): @dataclass(kw_only=True) class GelloConfig(BaseOperatorConfig): - operator_class = GelloOperator + operator_class: type[BaseOperator] = field(default=GelloOperator) # Dictionary for multi-arm setups: {"left": GelloArmConfig(...), "right": GelloArmConfig(...)} arms: Dict[str, GelloArmConfig] = field(default_factory=lambda: {"right": GelloArmConfig()}) diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index c7388d69..e967d60e 100644 --- a/python/rcs/operator/interface.py +++ b/python/rcs/operator/interface.py @@ -27,9 +27,9 @@ class TeleopCommands: class BaseOperator(ABC, threading.Thread): control_mode: tuple[ControlMode, RelativeTo] - controller_names: list[str] = field(default=["left", "right"]) + controller_names: list[str] - def __init__(self, config: BaseOperatorConfig, sim: Sim | None = None): + def __init__(self, config: "BaseOperatorConfig", sim: Sim | None = None): threading.Thread.__init__(self) self.config = config self.sim = sim @@ -55,7 +55,7 @@ def close(self): @dataclass(kw_only=True) class BaseOperatorConfig: - operator_class: BaseOperator + operator_class: type[BaseOperator] read_frequency: int = 30 simulation: bool = True @@ -105,13 +105,12 @@ def _translate_keys(self, actions): # Fill in missing robots with "hold" actions from last observation # This is necessary because absolute environments (like MultiRobotWrapper) # require actions for all configured robots in every step. - for robot_name in self.env.get_wrapper_attr("envs").keys(): - if robot_name not in translated: - if robot_name in self._last_obs: - translated[robot_name] = { - "joints": self._last_obs[robot_name]["joints"].copy(), - "gripper": self._last_obs[robot_name].get("gripper", 1.0), - } + for robot_name in self.env.get_wrapper_attr("envs"): + if robot_name not in translated and robot_name in self._last_obs: + translated[robot_name] = { + "joints": self._last_obs[robot_name]["joints"].copy(), + "gripper": self._last_obs[robot_name].get("gripper", 1.0), + } return translated def environment_step_loop(self): @@ -141,7 +140,7 @@ def environment_step_loop(self): # consume new commands because of potential origin reset continue - elif cmds.failure: + if cmds.failure: print("Command: Failure! Resetting env...") self._last_obs, _ = self.env.reset() self.operator.reset_operator_state() @@ -160,7 +159,7 @@ def environment_step_loop(self): print("Waiting for sync... (Press 's' on GELLO/Keyboard to sync)") hold_actions = {} - for robot_name in self.env.get_wrapper_attr("envs").keys(): + for robot_name in self.env.get_wrapper_attr("envs"): if robot_name in self._last_obs and "joints" in self._last_obs[robot_name]: hold_actions[robot_name] = { "joints": self._last_obs[robot_name]["joints"].copy(), diff --git a/python/rcs/operator/pedals.py b/python/rcs/operator/pedals.py index 470f1150..2d529a92 100644 --- a/python/rcs/operator/pedals.py +++ b/python/rcs/operator/pedals.py @@ -11,7 +11,8 @@ def __init__(self, device_name_substring="Foot Switch"): self.device_path = self._find_device(device_name_substring) if not self.device_path: - raise FileNotFoundError(f"Could not find a device matching '{device_name_substring}'") + msg = f"Could not find a device matching '{device_name_substring}'" + raise FileNotFoundError(msg) self.device = evdev.InputDevice(self.device_path) self.device.grab() # Prevent events from leaking into the OS/terminal @@ -45,17 +46,18 @@ def _read_events(self): if event.type == ecodes.EV_KEY: key_event = evdev.categorize(event) - with self._lock: - # keystate: 1 is DOWN, 2 is HOLD, 0 is UP - is_pressed = key_event.keystate in [1, 2] + if isinstance(key_event, evdev.KeyEvent): + with self._lock: + # keystate: 1 is DOWN, 2 is HOLD, 0 is UP + is_pressed = key_event.keystate in [1, 2] - # Store state using the string name of the key (e.g., 'KEY_A') - # If a key resolves to a list (rare, but happens in evdev), take the first one - key_name = key_event.keycode - if isinstance(key_name, list): - key_name = key_name[0] + # Store state using the string name of the key (e.g., 'KEY_A') + # If a key resolves to a list (rare, but happens in evdev), take the first one + key_name = key_event.keycode + if isinstance(key_name, list): + key_name = key_name[0] - self._key_states[key_name] = is_pressed + self._key_states[key_name] = is_pressed except OSError: pass # Device disconnected or closed diff --git a/python/rcs/operator/quest.py b/python/rcs/operator/quest.py index 90d9c012..bcde3f64 100644 --- a/python/rcs/operator/quest.py +++ b/python/rcs/operator/quest.py @@ -1,19 +1,12 @@ import copy import logging import threading -from dataclasses import dataclass +from dataclasses import dataclass, field from time import sleep import numpy as np -from rcs._core.common import RPY, Pose -from rcs.envs.base import ( - ArmWithGripper, - ControlMode, - GripperDictType, - RelativeActionSpace, - RelativeTo, - TQuatDictType, -) +from rcs._core.common import Pose +from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands from rcs.sim.sim import Sim from rcs.utils import SimpleFrameRate @@ -21,7 +14,6 @@ try: 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 HAS_SIMPUB = True @@ -51,12 +43,13 @@ def __init__(self): class QuestOperator(BaseOperator): control_mode = (ControlMode.CARTESIAN_TQuat, RelativeTo.CONFIGURED_ORIGIN) - controller_names = ["left", "right"] + controller_names: list[str] = ["left", "right"] # noqa: RUF012 - def __init__(self, config: QuestConfig, sim: Sim | None = None): + def __init__(self, config: "QuestConfig", sim: Sim | None = None): super().__init__(config, sim) if not HAS_SIMPUB: - raise ImportError("simpub is not installed. Please install it to use QuestOperator.") + msg = "simpub is not installed. Please install it to use QuestOperator." + raise ImportError(msg) self.config: QuestConfig @@ -145,10 +138,10 @@ def consume_action(self) -> dict[str, ArmWithGripper]: transform = set_axes.inverse() * transform * set_axes if not self.config.include_rotation: transform = Pose(translation=transform.translation()) # identity rotation - transforms[controller] = TQuatDictType( - tquat=np.concatenate([transform.translation(), transform.rotation_q()]) + transforms[controller] = ArmWithGripper( + tquat=np.concatenate([transform.translation(), transform.rotation_q()]), + gripper=np.array([self._grp_pos[controller]]), ) - transforms[controller].update(GripperDictType(gripper=np.array([self._grp_pos[controller]]))) return transforms def close(self): @@ -239,6 +232,6 @@ def run(self): @dataclass(kw_only=True) class QuestConfig(BaseOperatorConfig): - operator_class = QuestOperator + operator_class: type[BaseOperator] = field(default=QuestOperator) include_rotation: bool = True mq3_addr: str = "10.42.0.1" diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 364c1408..c997473d 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -98,7 +98,7 @@ def test_non_zero_action_trpy(self, cfg): initial_pose = rcs.common.Pose(translation=np.array(t), quaternion=np.array(q)) xyzrpy = np.concatenate([t, initial_pose.rotation_rpy().as_vector()], axis=0) non_zero_action = TRPYDictType(xyzrpy=np.array(xyzrpy)) - non_zero_action.update(GripperDictType(gripper=0)) + non_zero_action.update(GripperDictType(gripper=np.array([0.0]))) expected_obs = obs_initial.copy() expected_obs["tquat"][0] += x_pos_change obs, _, _, _, info = env.step(non_zero_action) @@ -113,7 +113,7 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg): obs_initial, _ = env.reset() # action to be performed zero_action = TRPYDictType(xyzrpy=np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)) # type: ignore - zero_action.update(GripperDictType(gripper=0)) + zero_action.update(GripperDictType(gripper=np.array([0.0]))) obs, _, _, _, info = env.step(zero_action) self.assert_no_pose_change(info, obs_initial, obs) @@ -127,7 +127,7 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg): # action to be performed x_pos_change = 0.2 non_zero_action = TRPYDictType(xyzrpy=np.array([x_pos_change, 0, 0, 0, 0, 0])) # type: ignore - non_zero_action.update(GripperDictType(gripper=0)) + non_zero_action.update(GripperDictType(gripper=np.array([0.0]))) expected_obs = obs_initial.copy() expected_obs["tquat"][0] += x_pos_change obs, _, _, _, info = env.step(non_zero_action) @@ -146,7 +146,7 @@ def test_collision_trpy(self, cfg, gripper_cfg): obs["xyzrpy"][0] = 0.4 obs["xyzrpy"][2] = -0.05 collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) - collision_action.update(GripperDictType(gripper=0)) + collision_action.update(GripperDictType(gripper=np.array([0.0]))) obs, _, _, _, info = env.step(collision_action) self.assert_collision(info) @@ -170,7 +170,7 @@ def test_collision_trpy(self, cfg, gripper_cfg): # obs["xyzrpy"][0] = 0.4 # obs["xyzrpy"][2] = -0.05 # collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) - # collision_action.update(GripperDictType(gripper=0)) + # collision_action.update(GripperDictType(gripper=np.array([0.0]))) # _, _, _, _, info = env.step(collision_action) # p2: np.ndarray = unwrapped.robot.get_joint_position() # self.assert_collision(info) @@ -245,7 +245,7 @@ def test_relative_zero_action_tquat(self, cfg, gripper_cfg): ) obs_initial, _ = env_rel.reset() zero_rel_action = TQuatDictType(tquat=np.array([0, 0, 0, 0, 0, 0, 1.0], dtype=np.float32)) # type: ignore - zero_rel_action.update(GripperDictType(gripper=0)) + zero_rel_action.update(GripperDictType(gripper=np.array([0.0]))) obs, _, _, _, info = env_rel.step(zero_rel_action) self.assert_no_pose_change(info, obs_initial, obs) @@ -266,7 +266,7 @@ def test_collision_tquat(self, cfg, gripper_cfg): obs["tquat"][0] = 0.4 obs["tquat"][2] = -0.05 collision_action = TQuatDictType(tquat=obs["tquat"]) - collision_action.update(GripperDictType(gripper=0)) + collision_action.update(GripperDictType(gripper=np.array([0.0]))) _, _, _, _, info = env.step(collision_action) self.assert_collision(info) @@ -290,7 +290,7 @@ def test_collision_tquat(self, cfg, gripper_cfg): # obs["tquat"][0] = 0.4 # obs["tquat"][2] = -0.05 # collision_action = TQuatDictType(tquat=obs["tquat"]) - # collision_action.update(GripperDictType(gripper=0)) + # collision_action.update(GripperDictType(gripper=np.array([0.0]))) # _, _, _, _, info = env.step(collision_action) # p2: np.ndarray = unwrapped.robot.get_joint_position() # self.assert_collision(info) @@ -355,7 +355,7 @@ def test_collision_joints(self, cfg, gripper_cfg): env.reset() # the below action is a test_case where there is an obvious collision regardless of the gripper action collision_act = JointsDictType(joints=np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)) - collision_act.update(GripperDictType(gripper=1)) + collision_act.update(GripperDictType(gripper=np.array([1.0]))) _, _, _, _, info = env.step(collision_act) self.assert_collision(info) @@ -377,7 +377,7 @@ def test_collision_joints(self, cfg, gripper_cfg): # p1: np.ndarray = unwrapped.robot.get_joint_position() # # the below action is a test_case where there is an obvious collision regardless of the gripper action # collision_act = JointsDictType(joints=np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)) - # collision_act.update(GripperDictType(gripper=1)) + # collision_act.update(GripperDictType(gripper=np.array([1.0]))) # _, _, _, _, info = env.step(collision_act) # p2: np.ndarray = unwrapped.robot.get_joint_position() @@ -393,6 +393,6 @@ def test_relative_zero_action_joints(self, cfg, gripper_cfg): env = SimEnvCreator()(ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, cameras=None, max_relative_movement=0.5) obs_initial, _ = env.reset() act = JointsDictType(joints=np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)) - act.update(GripperDictType(gripper=1)) + act.update(GripperDictType(gripper=np.array([1.0]))) obs, _, _, _, info = env.step(act) self.assert_no_pose_change(info, obs_initial, obs) From ac4d6a80a423220be59a7a820a45b47c6f0a58b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 9 Apr 2026 17:47:06 +0200 Subject: [PATCH 19/19] fix: previous sim wrapper and gripper id --- python/rcs/envs/sim.py | 28 ++++++++++++++-------------- python/rcs/envs/utils.py | 6 ++---- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index feafcf06..1f157d50 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -39,9 +39,9 @@ def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | Non if sim_wrapper is not None: env = sim_wrapper(env, simulation) super().__init__(env) - # self.unwrapped: RobotEnv - # assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." - # self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) + self.unwrapped: RobotEnv + assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." + self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) self.sim = simulation cfg = self.sim.get_config() self.frame_rate = SimpleFrameRate(1 / cfg.frequency, "RobotSimWrapper") @@ -56,19 +56,19 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo self.frame_rate() else: - # self.sim_robot.clear_collision_flag() + self.sim_robot.clear_collision_flag() self.sim.step_until_convergence() - # state = self.sim_robot.get_state() - # if "collision" not in info: - # info["collision"] = state.collision - # else: - # info["collision"] = info["collision"] or state.collision - # info["ik_success"] = state.ik_success + state = self.sim_robot.get_state() + if "collision" not in info: + info["collision"] = state.collision + else: + info["collision"] = info["collision"] or state.collision + info["ik_success"] = state.ik_success info["is_sim_converged"] = self.sim.is_converged() # truncate episode if collision - # obs.update(self.unwrapped.get_obs()) - # return obs, 0, False, info["collision"] or not state.ik_success, info - return obs, 0, False, False, info + obs.update(self.unwrapped.get_obs()) + return obs, 0, False, info["collision"] or not state.ik_success, info + # return obs, 0, False, False, info def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None @@ -77,7 +77,7 @@ def reset( obs, info = super().reset(seed=seed, options=options) self.sim.step(1) # todo: an obs method that is recursive over wrappers would be needed - # obs.update(self.unwrapped.get_obs()) + obs.update(self.unwrapped.get_obs()) return obs, info diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index f375006b..18b18639 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -37,11 +37,9 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: return hand_cfg -def default_sim_gripper_cfg(_idx: str = "0") -> sim.SimGripperConfig: +def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: cfg = sim.SimGripperConfig() - cfg.collision_geoms = [] - cfg.collision_geoms_fingers = [] - # cfg.add_id(idx) + cfg.add_id(idx) return cfg