diff --git a/src/lerobot/cameras/opencv/camera_opencv.py b/src/lerobot/cameras/opencv/camera_opencv.py index 1d7a1645d..1714e6e7f 100644 --- a/src/lerobot/cameras/opencv/camera_opencv.py +++ b/src/lerobot/cameras/opencv/camera_opencv.py @@ -112,7 +112,8 @@ class OpenCVCamera(Camera): self.config = config self.index_or_path = config.index_or_path - self.fps = config.fps + self.wanted_fps = config.fps + self.camera_fps = None self.color_mode = config.color_mode self.warmup_s = config.warmup_s @@ -200,10 +201,9 @@ class OpenCVCamera(Camera): if not self.is_connected: raise DeviceNotConnectedError(f"Cannot configure settings for {self} as it is not connected.") - if self.fps is None: - self.fps = self.videocapture.get(cv2.CAP_PROP_FPS) - else: - self._validate_fps() + # We don't set the FPS. We GET the actual (max) FPS from the camera. + self.camera_fps = self.videocapture.get(cv2.CAP_PROP_FPS) + logger.info(f"{self} is running at its default/max FPS: {self.camera_fps:.2f}") default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) default_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT))) @@ -316,19 +316,23 @@ class OpenCVCamera(Camera): if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") - start_time = time.perf_counter() + # Start the background capture thread if it's not running + if self.thread is None or not self.thread.is_alive(): + # Perform an initial blocking read to populate the first frame + ret, frame = self.videocapture.read() + if not ret or frame is None: + raise RuntimeError(f"{self} failed to read initial frame.") - ret, frame = self.videocapture.read() + self.latest_frame = self._postprocess_image(frame) + self._start_read_thread() - if not ret or frame is None: - raise RuntimeError(f"{self} read failed (status={ret}).") + with self.frame_lock: + frame = self.latest_frame - processed_frame = self._postprocess_image(frame, color_mode) + if frame is None: + raise RuntimeError(f"Internal error: Read thread started but no frame is available for {self}.") - read_duration_ms = (time.perf_counter() - start_time) * 1e3 - logger.debug(f"{self} read took: {read_duration_ms:.1f}ms") - - return processed_frame + return frame.copy() def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray: """ @@ -386,16 +390,23 @@ class OpenCVCamera(Camera): """ while not self.stop_event.is_set(): try: - color_image = self.read() + ret, frame = self.videocapture.read() + if not ret or frame is None: + logger.warning(f"Failed to read frame in background for {self}.") + time.sleep(0.01) + continue + + processed_frame = self._postprocess_image(frame) with self.frame_lock: - self.latest_frame = color_image + self.latest_frame = processed_frame + self.new_frame_event.set() - except DeviceNotConnectedError: - break except Exception as e: logger.warning(f"Error reading frame in background thread for {self}: {e}") + if not self.is_connected: + break def _start_read_thread(self) -> None: """Starts or restarts the background read thread if it's not running.""" diff --git a/src/lerobot/robots/so101_follower_torque/so101_follower_t.py b/src/lerobot/robots/so101_follower_torque/so101_follower_t.py index 1eb3a85ec..261bacde0 100644 --- a/src/lerobot/robots/so101_follower_torque/so101_follower_t.py +++ b/src/lerobot/robots/so101_follower_torque/so101_follower_t.py @@ -48,7 +48,7 @@ class SO101FollowerT(Robot): _CURRENT_STEP_A: float = 6.5e-3 # 6.5 mA per register LSB #http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0 _KT_NM_PER_AMP: float = 0.814 # Torque constant Kt [N·m/A] #https://www.feetechrc.com/811177.html - _MAX_CURRENT_A: float = 2.0 # Safe driver limit + _MAX_CURRENT_A: float = 4.0 # Safe driver limit # Position gains [Nm/rad] _KP_GAINS = { @@ -159,11 +159,11 @@ class SO101FollowerT(Robot): @property def _motors_ft(self) -> dict[str, type]: d: dict[str, type] = {} - for m in self.bus.motors: - d[f"{m}.pos"] = float - d[f"{m}.vel"] = float - d[f"{m}.acc"] = float - d[f"{m}.tau_meas"] = float + for motor in self.bus.motors: + d[f"{motor}.pos"] = float + d[f"{motor}.vel"] = float + # d[f"{motor}.acc"] = float + d[f"{motor}.effort"] = float return d @property @@ -178,7 +178,13 @@ class SO101FollowerT(Robot): @cached_property def action_features(self) -> dict[str, type]: - return {f"{m}.effort": int for m in self.bus.motors} + d: dict[str, type] = {} + for motor in self.bus.motors: + d[f"{motor}.pos"] = float + d[f"{motor}.vel"] = float + # d[f"{motor}.acc"] = float + d[f"{motor}.effort"] = float + return d @property def is_connected(self) -> bool: @@ -398,12 +404,8 @@ class SO101FollowerT(Robot): def configure(self) -> None: with self.bus.torque_disabled(): - self.bus.configure_motors() for motor in self.bus.motors: self.bus.write("Operating_Mode", motor, 2, num_retry=2) # Set to current mode - self.bus.write("Torque_Limit", motor, 1000, num_retry=2) # 100% - self.bus.write("Max_Torque_Limit", motor, 1000, num_retry=2) # 100% - self.bus.write("Overcurrent_Protection_Time", motor, 250, num_retry=2) def setup_motors(self) -> None: for motor in reversed(self.bus.motors): @@ -468,16 +470,19 @@ class SO101FollowerT(Robot): cur_raw = self.bus.sync_read("Present_Current", normalize=False, num_retry=5) tau_meas = self._current_to_torque_nm(cur_raw) + # Compute reaction torques using model-based approach + tau_reaction = self._compute_model_based_disturbance(pos_rad, vel_rad, acc_rad, tau_meas) + obs_dict = {} obs_dict |= {f"{m}.pos": pos_rad[m] for m in self.bus.motors} obs_dict |= {f"{m}.vel": vel_rad[m] for m in self.bus.motors} obs_dict |= {f"{m}.acc": acc_rad[m] for m in self.bus.motors} - obs_dict |= {f"{m}.tau_meas": tau_meas[m] for m in self.bus.motors} + obs_dict |= {f"{m}.effort": tau_reaction[m] for m in self.bus.motors} # Capture images from cameras for cam_key, cam in self.cameras.items(): start = time.perf_counter() - obs_dict[cam_key] = cam.async_read() + obs_dict[cam_key] = cam.read() dt_ms = (time.perf_counter() - start) * 1e3 logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms")