diff --git a/README.md b/README.md index 6623165..e40fc9e 100644 --- a/README.md +++ b/README.md @@ -84,6 +84,7 @@ Predefined configs are provided under `config/`: - `robot.teach_sensitivity` — teaching sensitivity, valid range 1–5 - `robot.manual_gripper_speed` — gripper velocity in normalized position per second (default `0.5`) - `robot.observe_joint_vel` — record joint velocities in observations (`false` by default) +- `robot.enable_logs` — enable optional per-cycle timing/diagnostic logs (`false` by default) - `robot.cameras.camera` — Intel RealSense camera (`serial_number_or_name`, resolution, fps) - `dataset.root` / `dataset.repo_id` / `single_task` / `fps` / `episode_time_s` / `reset_time_s` / `num_episodes` — dataset settings @@ -134,6 +135,7 @@ records the loop period, GELLO read, safety guard, ServoJ, and complete ```bash uv run uf-robot-teleop \ --config_path config/gello/xarm7_gello_record_config.yaml \ + --robot.enable_logs=true \ --fps 60 \ --guard_latency_experiment=true \ --experiment_duration_s 60 diff --git a/README_ZH.md b/README_ZH.md index 7b13d99..61ba00e 100644 --- a/README_ZH.md +++ b/README_ZH.md @@ -84,6 +84,7 @@ ls /dev/serial/by-id/ - `robot.teach_sensitivity` — 示教灵敏度，有效范围 1–5 - `robot.manual_gripper_speed` — 夹爪速度（每秒归一化位置变化，默认 `0.5`） - `robot.observe_joint_vel` — 是否在观测中记录关节速度（默认 `false`） +- `robot.enable_logs` — 是否启用每帧耗时和诊断日志（默认 `false`） - `robot.cameras.camera` — Intel RealSense 相机配置（`serial_number_or_name`、分辨率、fps） - `dataset.root` / `dataset.repo_id` / `single_task` / `fps` / `episode_time_s` / `reset_time_s` / `num_episodes` — 数据集配置 @@ -124,6 +125,7 @@ GELLO 读取、安全检测、ServoJ 和完整 `send_action` 耗时： ```bash uv run uf-robot-teleop \ --config_path config/gello/xarm7_gello_record_config.yaml \ + --robot.enable_logs=true \ --fps 60 \ --guard_latency_experiment=true \ --experiment_duration_s 60 diff --git a/config/gello/xarm5_gello_record_config.yaml b/config/gello/xarm5_gello_record_config.yaml index d5e17ed..d16f971 100644 --- a/config/gello/xarm5_gello_record_config.yaml +++ b/config/gello/xarm5_gello_record_config.yaml @@ -5,6 +5,7 @@ robot: control_space: "joint" robot_ip: "192.168.1.245" gripper_type: 1 + enable_logs: false # make sure to edit with your correct configurations! teleop: diff --git a/config/gello/xarm6_gello_record_config.yaml b/config/gello/xarm6_gello_record_config.yaml index 17dfc8c..1792fe6 100644 --- a/config/gello/xarm6_gello_record_config.yaml +++ b/config/gello/xarm6_gello_record_config.yaml @@ -5,6 +5,7 @@ robot: control_space: "joint" robot_ip: "192.168.1.245" gripper_type: 1 + enable_logs: false # make sure to edit with your correct configurations! teleop: diff --git a/config/gello/xarm7_gello_cartesian_record_config.yaml b/config/gello/xarm7_gello_cartesian_record_config.yaml index a106105..12d9c1a 100644 --- a/config/gello/xarm7_gello_cartesian_record_config.yaml +++ b/config/gello/xarm7_gello_cartesian_record_config.yaml @@ -5,6 +5,7 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.245" gripper_type: 1 + enable_logs: false max_linear_velocity: 200 min_tcp_z_mm: 50 gripper_error_log_path: "logs/xarm7_gripper_errors.log" diff --git a/config/gello/xarm7_gello_record_config.yaml b/config/gello/xarm7_gello_record_config.yaml index a3cf4ae..c5bc9c7 100644 --- a/config/gello/xarm7_gello_record_config.yaml +++ b/config/gello/xarm7_gello_record_config.yaml @@ -5,6 +5,7 @@ robot: control_space: "joint" robot_ip: "192.168.1.245" gripper_type: 1 + enable_logs: false # Up to 60 Hz goal updates; the first command after idle is immediate. # Keeping the gripper below its 5000 maximum speed avoids C19 in testing. gripper_command_interval_s: 0.0166667 diff --git a/config/manual_mode/xarm7_manual_record_config.yaml b/config/manual_mode/xarm7_manual_record_config.yaml index 0ebf89c..fa18c96 100644 --- a/config/manual_mode/xarm7_manual_record_config.yaml +++ b/config/manual_mode/xarm7_manual_record_config.yaml @@ -7,6 +7,7 @@ robot: robot_ip: "192.168.1.245" # Gripper type: 1 is the xArm gripper. gripper_type: 1 + enable_logs: false manual_mode: true # Normalized gripper position change per second while holding C/O. manual_gripper_speed: 0.5 diff --git a/config/pika/xarm7_pika_record_config.yaml b/config/pika/xarm7_pika_record_config.yaml index 380ae29..51b648a 100644 --- a/config/pika/xarm7_pika_record_config.yaml +++ b/config/pika/xarm7_pika_record_config.yaml @@ -5,6 +5,7 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.85" gripper_type: 10 + enable_logs: false max_linear_velocity: 200 cameras: fisheye: diff --git a/config/spacemouse/xarm7_spacemouse_record_config.yaml b/config/spacemouse/xarm7_spacemouse_record_config.yaml index 0519ac9..d1fae3d 100644 --- a/config/spacemouse/xarm7_spacemouse_record_config.yaml +++ b/config/spacemouse/xarm7_spacemouse_record_config.yaml @@ -5,6 +5,7 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.127" gripper_type: 0 + enable_logs: false cameras: overhead: diff --git a/config/umi/dual_xarm6_umi_record_config.yaml b/config/umi/dual_xarm6_umi_record_config.yaml index 26987ac..18f2943 100644 --- a/config/umi/dual_xarm6_umi_record_config.yaml +++ b/config/umi/dual_xarm6_umi_record_config.yaml @@ -12,6 +12,7 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.29" gripper_type: 2 + enable_logs: false max_linear_velocity: 200 cameras: fisheye: @@ -28,6 +29,7 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.83" gripper_type: 2 + enable_logs: false max_linear_velocity: 200 cameras: fisheye: diff --git a/config/umi/xarm6_umi_record_config.yaml b/config/umi/xarm6_umi_record_config.yaml index 68796d5..afc0a93 100644 --- a/config/umi/xarm6_umi_record_config.yaml +++ b/config/umi/xarm6_umi_record_config.yaml @@ -5,6 +5,7 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.83" gripper_type: 2 + enable_logs: false max_linear_velocity: 250 cameras: fisheye: diff --git a/docs/gello_guard_latency_experiment_20260817.md b/docs/gello_guard_latency_experiment_20260817.md index 9cd167e..db0c283 100644 --- a/docs/gello_guard_latency_experiment_20260817.md +++ b/docs/gello_guard_latency_experiment_20260817.md @@ -55,6 +55,7 @@ GELLO get_action ```bash uv run uf-robot-teleop \ --config_path config/gello/xarm7_gello_record_config.yaml \ + --robot.enable_logs=true \ --fps 60 \ --experiment_1_baseline=true \ --experiment_duration_s 60 diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py index 31b4697..4990357 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py +++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py @@ -107,6 +107,7 @@ class UFRobot(Robot, Thread): self._is_connected = False self._is_calibrated =True + self.enable_logs = bool(getattr(config, "enable_logs", False)) self.logs = {} self._cmd_cnt = 0 @@ -539,7 +540,8 @@ class UFRobot(Robot, Thread): local = self._local_kinematics.tcp_position(joints) error_mm = float(np.linalg.norm(local - reported)) limit = float(self.config.local_kinematics_max_error_mm) - self.logs["local_kinematics_error_mm"] = error_mm + if getattr(self, "enable_logs", True): + self.logs["local_kinematics_error_mm"] = error_mm if math.isfinite(error_mm) and error_mm <= limit: self._local_model_fault_count = 0 return True @@ -820,9 +822,10 @@ class UFRobot(Robot, Thread): def get_observation(self) -> dict[str, np.ndarray]: obs_dict = {} self._log_controller_error_if_changed("get_observation") + logs_enabled = bool(getattr(self, "enable_logs", True)) - # Read Stretch state - before_read_t = time.perf_counter() + # Read robot state + before_read_t = time.perf_counter() if logs_enabled else None if self._control_space == "joint": code, states = self.real_arm.get_joint_states(is_radian=True, num=3) pos_list = states[0].copy() @@ -867,12 +870,13 @@ class UFRobot(Robot, Thread): self.real_arm.robotiq_get_status(number_of_registers=3) grippos = self.real_arm.robotiq_status['gPO'] # 0..255 grippos_norm = self._gripper_param.get_gripper_norm(grippos) # 0=open, 1=closed - self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t + if logs_enabled: + self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t obs_dict[f"{self.prefix}gripper.pos"] = grippos_norm # Capture images from cameras for cam_key, cam in self.cameras.items(): - before_camread_t = time.perf_counter() + before_camread_t = time.perf_counter() if logs_enabled else None frame = cam.async_read() shape = frame.shape if (self.camera_height > 0 and self.camera_height != shape[0]) or (self.camera_width > 0 and self.camera_width != shape[1]): @@ -881,7 +885,10 @@ class UFRobot(Robot, Thread): import cv2 frame = cv2.resize(frame, (camera_height, camera_width), interpolation=cv2.INTER_AREA) obs_dict[f"{self.prefix}{cam_key}"] = frame - self.logs[f"async_read_camera_{cam_key}_dt_s"] = time.perf_counter() - before_camread_t + if logs_enabled: + self.logs[f"async_read_camera_{cam_key}_dt_s"] = ( + time.perf_counter() - before_camread_t + ) return obs_dict @@ -896,6 +903,7 @@ class UFRobot(Robot, Thread): return self.get_observation() if not self._rt_report_normal: raise ConnectionError("RT Report for target robot NOT READY!") + logs_enabled = bool(getattr(self, "enable_logs", True)) with self._update_lock: positions = self.rt_actual_joint_pos.copy() velocities = self.rt_actual_joint_speed.copy() @@ -919,9 +927,9 @@ class UFRobot(Robot, Thread): obs_dict[f"{self.prefix}gripper.pos"] = float(gripper) for camera_key, camera in self.cameras.items(): - before_camera_t = time.perf_counter() + before_camera_t = time.perf_counter() if logs_enabled else None frame = camera.async_read() - after_camera_t = time.perf_counter() + after_camera_t = time.perf_counter() if logs_enabled else None shape = frame.shape if ( self.camera_height > 0 @@ -935,20 +943,23 @@ class UFRobot(Robot, Thread): height = self.camera_height if self.camera_height != 0 else shape[0] frame = cv2.resize(frame, (width, height), interpolation=cv2.INTER_AREA) obs_dict[f"{self.prefix}{camera_key}"] = frame - self.logs[f"async_read_camera_{camera_key}_dt_s"] = ( - after_camera_t - before_camera_t - ) - if not hasattr(self, "_last_realtime_camera_timings"): - self._last_realtime_camera_timings = {} - self._last_realtime_camera_timings[camera_key] = ( - before_camera_t, - after_camera_t, - ) - self._last_realtime_observation_end_monotonic_s = time.perf_counter() + if logs_enabled: + self.logs[f"async_read_camera_{camera_key}_dt_s"] = ( + after_camera_t - before_camera_t + ) + if not hasattr(self, "_last_realtime_camera_timings"): + self._last_realtime_camera_timings = {} + self._last_realtime_camera_timings[camera_key] = ( + before_camera_t, + after_camera_t, + ) + if logs_enabled: + self._last_realtime_observation_end_monotonic_s = time.perf_counter() return obs_dict def _send_gripper_action(self, gripper_norm: float) -> None: gripper_norm = min(max(float(gripper_norm), 0.0), 1.0) + logs_enabled = bool(getattr(self, "enable_logs", True)) if ( self._last_gripper_command is not None and abs(gripper_norm - self._last_gripper_command) @@ -964,7 +975,7 @@ class UFRobot(Robot, Thread): if now - self._last_gripper_command_attempt_s < self.config.gripper_command_interval_s: return self._last_gripper_command_attempt_s = now - command_start_s = now + command_start_s = now if logs_enabled else None if self._gripper_type == GripperType.xArmGripper: grippos = self._gripper_param.get_grippos(gripper_norm) @@ -1005,15 +1016,23 @@ class UFRobot(Robot, Thread): result = self.real_arm.getset_tgpio_modbus_data(modbus_datas) code = result[0] if isinstance(result, (tuple, list)) else result - command_dt_ms = (time.perf_counter() - command_start_s) * 1000 + command_dt_ms = ( + (time.perf_counter() - command_start_s) * 1000 + if logs_enabled + else None + ) if code not in (None, 0): + detail = f"target={gripper_norm:.6f}, pulse={grippos}" + if command_dt_ms is not None: + detail += f", dt_ms={command_dt_ms:.3f}" self._log_gripper_error( "send_gripper_action", code, - f"target={gripper_norm:.6f}, pulse={grippos}, dt_ms={command_dt_ms:.3f}", + detail, ) return - self._log_gripper_command(gripper_norm, grippos, command_dt_ms) + if logs_enabled: + self._log_gripper_command(gripper_norm, grippos, command_dt_ms) self._last_gripper_command = gripper_norm def _log_gripper_command(self, target: float, pulse: int, dt_ms: float) -> None: @@ -1139,10 +1158,8 @@ class UFRobot(Robot, Thread): if self.config.no_action: return action - before_write_t = time.perf_counter() - self.logs["safety_guard_dt_s"] = 0.0 - self.logs["servo_j_dt_s"] = 0.0 - self.logs["safety_guard_path"] = "not_run" + logs_enabled = bool(getattr(self, "enable_logs", True)) + before_write_t = time.perf_counter() if logs_enabled else None safe_action = dict(action) if self._control_space == "joint": # first sync with gello or other control device SLOWLY! @@ -1152,10 +1169,11 @@ class UFRobot(Robot, Thread): cmd_list = [0]*(self._dof) for i in range(self._dof): cmd_list[i] = action[f"{self.prefix}J{i+1}.pos"] - guard_start_t = time.perf_counter() + guard_start_t = time.perf_counter() if logs_enabled else None safe_cmd = self._guard_joint_target(cmd_list) - self.logs["safety_guard_dt_s"] = time.perf_counter() - guard_start_t - self.logs["safety_guard_path"] = self._last_guard_path + if logs_enabled: + self.logs["safety_guard_dt_s"] = time.perf_counter() - guard_start_t + self.logs["safety_guard_path"] = self._last_guard_path if safe_cmd is None: # Do not send an unverified arm target. Gripper handling below # remains independent and can continue safely. @@ -1174,11 +1192,12 @@ class UFRobot(Robot, Thread): code = self.real_arm.set_state(0) self._check_motion_code("set_state(0)", code) time.sleep(0.1) - servo_j_start_t = time.perf_counter() + servo_j_start_t = time.perf_counter() if logs_enabled else None code = self.real_arm.set_servo_angle_j( safe_cmd[:self._dof].tolist(), speed=jnt_spd, is_radian=True ) - self.logs["servo_j_dt_s"] = time.perf_counter() - servo_j_start_t + if logs_enabled: + self.logs["servo_j_dt_s"] = time.perf_counter() - servo_j_start_t self._check_motion_code("set_servo_angle_j", code) elif safe_cmd is not None: # The legacy mode-6 path uses the absolute move_joint API. @@ -1222,7 +1241,8 @@ class UFRobot(Robot, Thread): if self._gripper_type > GripperType.NoGripper: self._send_gripper_action(safe_action[f"{self.prefix}gripper.pos"]) - self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t + if logs_enabled: + self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t return safe_action def print_logs(self) -> None: diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py index 626b86b..c15758f 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py +++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py @@ -20,6 +20,7 @@ class UFRobotConfig(RobotConfig): gripper_command_threshold: float = 0.01 # normalized change required before sending a new command gripper_command_interval_s: float = 0.1 # minimum interval between tool RS485 goals gripper_error_log_path: str | None = "logs/xarm_gripper_errors.log" + enable_logs: bool = False # optional per-cycle timing and diagnostic logs observe_joint_vel: bool = False # only effective in joint control mode manual_mode: bool = False # xArm joint teaching mode; records state and optional gripper actions manual_gripper_speed: float = 0.5 # normalized gripper position per second in manual mode diff --git a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py index df741c1..936d3d4 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py +++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py @@ -102,6 +102,18 @@ def _manual_gripper_action_key(action_features): return next((key for key in action_features if key.endswith("gripper.pos")), None) +def _diagnostic_logs_enabled(robot) -> bool: + """Return whether optional per-cycle diagnostics are enabled for a robot.""" + config = getattr(robot, "config", None) + if config is not None and hasattr(config, "enable_logs"): + return bool(config.enable_logs) + + child_robots = getattr(robot, "robots", None) + if child_robots: + return any(_diagnostic_logs_enabled(child) for child in child_robots.values()) + return False + + def _manual_action_from_observation(observation, action_features, gripper_target=None): """Keep only robot action fields when mirroring manual-mode state.""" action = {key: value for key, value in observation.items() if key in action_features} @@ -353,6 +365,7 @@ def record_loop( manual_gripper_action_key = _manual_gripper_action_key(robot.action_features) realtime_controller = None + diagnostic_logs_enabled = _diagnostic_logs_enabled(robot) sync_log_file = None sync_log_writer = None sync_frame_index = 0 @@ -371,27 +384,29 @@ def record_loop( initial_observation=last_robot_cmd, ) realtime_controller.start() - sync_log_dir = Path("logs") - sync_log_dir.mkdir(parents=True, exist_ok=True) - sync_log_path = sync_log_dir / ( - f"gello_record_sync_{time.strftime('%Y%m%d_%H%M%S')}_{time.time_ns() % 1_000_000:06d}.csv" - ) - sync_log_file = sync_log_path.open("w", newline="", buffering=1) - sync_log_writer = csv.DictWriter( - sync_log_file, - fieldnames=[ - "frame", - "state_sample_s", - "action_sent_s", - "action_age_ms", - "observation_end_s", - "state_to_observation_end_ms", - "camera_timings", - "frame_loop_ms", - ], - ) - sync_log_writer.writeheader() - logging.info("Realtime dataset synchronization log: %s", sync_log_path) + if diagnostic_logs_enabled: + sync_log_dir = Path("logs") + sync_log_dir.mkdir(parents=True, exist_ok=True) + sync_log_path = sync_log_dir / ( + f"gello_record_sync_{time.strftime('%Y%m%d_%H%M%S')}_" + f"{time.time_ns() % 1_000_000:06d}.csv" + ) + sync_log_file = sync_log_path.open("w", newline="", buffering=1) + sync_log_writer = csv.DictWriter( + sync_log_file, + fieldnames=[ + "frame", + "state_sample_s", + "action_sent_s", + "action_age_ms", + "observation_end_s", + "state_to_observation_end_ms", + "camera_timings", + "frame_loop_ms", + ], + ) + sync_log_writer.writeheader() + logging.info("Realtime dataset synchronization log: %s", sync_log_path) timestamp = 0 start_episode_t = time.perf_counter() @@ -405,13 +420,16 @@ def record_loop( # Get robot observation if realtime_controller is not None: obs = robot.get_realtime_observation() - observation_monotonic_s = getattr( - robot, "_last_realtime_observation_monotonic_s", time.perf_counter() - ) + observation_monotonic_s = getattr(robot, "_last_realtime_observation_monotonic_s", None) + if observation_monotonic_s is None: + observation_monotonic_s = time.perf_counter() realtime_controller.update_observation(obs) - matched_action, matched_action_sent_s = realtime_controller.action_sample_at( - observation_monotonic_s - ) + if diagnostic_logs_enabled: + matched_action, matched_action_sent_s = realtime_controller.action_sample_at( + observation_monotonic_s + ) + else: + matched_action = realtime_controller.action_at(observation_monotonic_s) else: obs = robot.get_observation() diff --git a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py index f6439bf..dcc0f5a 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py +++ b/src/lerobot_robot_ufactory/scripts/uf_robot_teleop.py @@ -162,6 +162,10 @@ def teleop_loop(cfg: TeleopConfig): if cfg.guard_latency_experiment: _validate_guard_latency_config(cfg) + if hasattr(cfg.robot, "enable_logs") and not cfg.robot.enable_logs: + raise ValueError( + "Guard latency experiment requires robot.enable_logs=true" + ) logging.warning( "Guard latency experiment enabled: measuring GELLO, safety guard, ServoJ, " "and total send latency for %.1f active seconds", diff --git a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py index 19bf514..21510e5 100644 --- a/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py +++ b/src/lerobot_robot_ufactory/teleoperators/gello_teleop/gello_teleop.py @@ -1,6 +1,5 @@ #!/usr/bin/env python import logging -import time import numpy as np from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from ..base_teleop import UFBaseTeleop @@ -170,11 +169,8 @@ class GelloTeleop(UFBaseTeleop): def get_action(self) -> dict[str, np.ndarray]: if not self._teleop_enabled: raise RuntimeError("Gello teleop is disabled") - start = time.perf_counter() fake_obs = dict({"joint_state": np.array([0.0]*(self.dof+1))}) # for agent.act() argument, actually no use action_array = self.gello_agent.act(fake_obs) # current gello joint pos as np.ndarray - dt_ms = (time.perf_counter() - start) * 1e3 - logger.debug(f"{self} read action: {dt_ms:.1f}ms") action = {} for i in range(self.dof):