From 4dcbb5856d72494dd70a5358a55d069bd12fe34d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 May 2025 09:44:11 +0200 Subject: [PATCH 1/5] feat(env): made reset optional --- python/rcs/envs/base.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 4b9bd8bb..c0fa8794 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -181,11 +181,12 @@ class RobotEnv(gym.Env): y """ - def __init__(self, robot: common.Robot, control_mode: ControlMode): + def __init__(self, robot: common.Robot, control_mode: ControlMode, home_on_reset: bool = False): self.robot = robot self._control_mode_overrides = [control_mode] self.action_space: gym.spaces.Dict self.observation_space: gym.spaces.Dict + self.home_on_reset = home_on_reset low, high = get_joint_limits(self.robot) if control_mode == ControlMode.JOINTS: self.action_space = get_space(JointsDictType, params={"joint_limits": {"low": low, "high": high}}) @@ -273,7 +274,8 @@ def reset( msg = "options not implemented yet" raise NotImplementedError(msg) self.robot.reset() - self.robot.move_home() + if self.home_on_reset: + self.robot.move_home() return self.get_obs(), {} def close(self): @@ -650,7 +652,7 @@ class GripperWrapper(ActObsInfoWrapper): BINARY_GRIPPER_CLOSED = 0 BINARY_GRIPPER_OPEN = 1 - def __init__(self, env, gripper: common.Gripper, binary: bool = True): + def __init__(self, env, gripper: common.Gripper, binary: bool = True, open_on_reset: bool = True): super().__init__(env) self.unwrapped: RobotEnv self.observation_space: gym.spaces.Dict @@ -661,9 +663,12 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True): self._gripper = gripper self.binary = binary self._last_gripper_cmd = None + self.open_on_reset = open_on_reset def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: - self._gripper.reset() + if self.open_on_reset: + # resetting opens the gripper + self._gripper.reset() self._last_gripper_cmd = None return super().reset(**kwargs) From bc2e78e12039b12afb7887070b199c7829804a1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 May 2025 09:45:32 +0200 Subject: [PATCH 2/5] fix(hw camera): recording doesnt block lock, clear buffer waits for frames --- python/rcs/camera/hw.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python/rcs/camera/hw.py b/python/rcs/camera/hw.py index 98388259..6aee0aa0 100644 --- a/python/rcs/camera/hw.py +++ b/python/rcs/camera/hw.py @@ -96,7 +96,6 @@ def start(self, warm_up: bool = True): def record_video(self, path: Path, str_id: str): if self.recording_ongoing(): return - self.clear_buffer() for camera in self.camera_names: self.writer[camera] = cv2.VideoWriter( str(path / f"episode_{str_id}_{camera}.mp4"), @@ -128,15 +127,15 @@ def polling_thread(self, warm_up: bool = True): self.warm_up() while self.running: frame_set = self.poll_frame_set() + # buffering with self._buffer_lock: self._buffer[self._next_ring_index] = frame_set - # copy the buffer to the record path - for camera_key, writer in self.writer.items(): - frameset = self._buffer[self._next_ring_index] - assert frameset is not None - writer.write(frameset.frames[camera_key].camera.color.data[:, :, ::-1]) self._next_ring_index = (self._next_ring_index + 1) % self.config.max_buffer_frames self._buffer_len = max(self._buffer_len + 1, self.config.max_buffer_frames) + # video recording + for camera_key, writer in self.writer.items(): + if frame_set is not None: + writer.write(frame_set.frames[camera_key].camera.color.data[:, :, ::-1]) self.rate(self.config.frame_rate) def poll_frame_set(self) -> FrameSet: @@ -155,6 +154,7 @@ def clear_buffer(self): self._buffer = [None for _ in range(self.config.max_buffer_frames)] self._next_ring_index = 0 self._buffer_len = 0 + self.wait_for_frames() @property @abstractmethod From 461482275d4b1e611a1f6c6b8290ca2d78dc3ea6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 May 2025 09:45:52 +0200 Subject: [PATCH 3/5] fix(cli): remove refactor left over --- python/rcs/cli/main.py | 47 ------------------------------------------ 1 file changed, 47 deletions(-) diff --git a/python/rcs/cli/main.py b/python/rcs/cli/main.py index 7247f9a2..95899dd1 100644 --- a/python/rcs/cli/main.py +++ b/python/rcs/cli/main.py @@ -9,7 +9,6 @@ import typer from rcs.camera.realsense import RealSenseCameraSet from rcs.control.fr3_desk import load_creds_fr3_desk -from rcs.control.record import PoseList from rcs.envs.creators import get_urdf_path logger = logging.getLogger(__name__) @@ -127,51 +126,5 @@ def shutdown( rcs.control.fr3_desk.shutdown(ip, user, pw) -@fr3_app.command() -def record( - ip_str: Annotated[str, typer.Argument(help="Name to IP dict. e.g. \"{'robot1': '192.168.100.1'}\"")], - urdf_path: Annotated[Optional[str], typer.Option(help="Path to the urdf file")] = None, - lpaths: Annotated[Optional[list[str]], typer.Option("--lpaths", help="Paths to load n recordings")] = None, - spath: Annotated[Optional[str], typer.Option("--spath", help="Paths to load n recordings")] = None, - buttons: Annotated[bool, typer.Option("-b", help="Use the robots buttons instead of the keyboard")] = False, -): - """Tool to record poses with multiple FR3 robots.""" - user, pw = load_creds_fr3_desk() - urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=False) # type: ignore - - name2ip: dict[str, str] = eval(ip_str) - - if lpaths is not None and len(lpaths) > 0: - with ExitStack() as stack: - for r_ip in name2ip.values(): - stack.enter_context(rcs.control.fr3_desk.Desk.fci(r_ip, username=user, password=pw, unlock=True)) - - p = PoseList.load(name2ip, lpaths, urdf_path=urdf_path) - input("Press any key to replay") - p.replay() - else: - with ExitStack() as stack: - gms = [ - rcs.control.fr3_desk.Desk.guiding_mode(r_ip, username=user, password=pw, unlock=True) - for r_ip in name2ip.values() - ] - for gm in gms: - stack.enter_context(gm) - p = PoseList(name2ip, urdf_path=urdf_path) - if not buttons: - p.record() - else: - p.start_button_recording() - for gm in gms: - gm.desk.listen(p.button_callback) - while p._button_recording: - pass - for gm in gms: - gm.desk.stop_listen() - - if spath is not None: - p.save(spath) - - def main(): main_app() From 0c60949ad79bd68a8e1b4401af1a46d612f3f40c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 May 2025 09:46:45 +0200 Subject: [PATCH 4/5] fix(franka hand): remove slow stopping --- src/hw/FrankaHand.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/hw/FrankaHand.cpp b/src/hw/FrankaHand.cpp index 0ef42013..b4868adb 100644 --- a/src/hw/FrankaHand.cpp +++ b/src/hw/FrankaHand.cpp @@ -126,7 +126,7 @@ void FrankaHand::grasp() { this->is_moving = false; return; } - this->m_stop(); + this->m_wait(); this->control_thread = std::thread([&]() { this->is_moving = true; try { @@ -149,7 +149,7 @@ void FrankaHand::open() { this->is_moving = false; return; } - this->m_stop(); + this->m_wait(); this->control_thread = std::thread([&]() { this->is_moving = true; try { @@ -174,7 +174,7 @@ void FrankaHand::shut() { this->is_moving = false; return; } - this->m_stop(); + this->m_wait(); this->control_thread = std::thread([&]() { this->is_moving = true; this->gripper.move(0, this->cfg.speed); From 9f8add9e0abce6ddbf0150e0f4a03cd1d295e96d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Fri, 23 May 2025 10:34:11 +0200 Subject: [PATCH 5/5] style: fix unused imports --- python/rcs/cli/main.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/python/rcs/cli/main.py b/python/rcs/cli/main.py index 95899dd1..425809b2 100644 --- a/python/rcs/cli/main.py +++ b/python/rcs/cli/main.py @@ -1,7 +1,6 @@ import logging -from contextlib import ExitStack from time import sleep -from typing import Annotated, Optional +from typing import Annotated import pyrealsense2 as rs import rcs @@ -9,7 +8,6 @@ import typer from rcs.camera.realsense import RealSenseCameraSet from rcs.control.fr3_desk import load_creds_fr3_desk -from rcs.envs.creators import get_urdf_path logger = logging.getLogger(__name__)