Fixes Camera and Gripper Issues

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

python/rcs/cli/main.py

@@ -1,16 +1,13 @@
 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
 import rcs.control.fr3_desk
 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 +124,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()

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)
 

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);