语音播报

2025-08-12 11:52:35 +08:00
2 changed files with 4 additions and 349 deletions
--- a/lerobot/common/robot_devices/robots/realman_dual.py
+++ b/lerobot/common/robot_devices/robots/realman_dual.py
@@ -1,7 +1,7 @@
+
 """
    Teleoperation Realman with a PS5 controller and LLM interaction
 """
-
 import time
 import torch
 import numpy as np
@@ -11,7 +11,6 @@ from dataclasses import dataclass, field, replace
 from collections import deque
 import signal
 import sys
-
 from lerobot.common.robot_devices.teleop.realman_aloha_dual import HybridController
 from lerobot.common.robot_devices.motors.utils import get_motor_names, make_motors_buses_from_configs
 from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
@@ -27,7 +26,6 @@ from lerobot.common.robot_devices.robots.utils import (
    handle_llm_interaction_with_images
 )

-
 class RealmanDualRobot:
    """RealmanDual机器人控制类，支持双臂操作和LLM交互"""
    
@@ -53,7 +51,6 @@ class RealmanDualRobot:
        # 状态标志
        self._shutdown_flag = False
        self._llm_triggered = False
-
    def _initialize_teleop(self):
        """初始化遥操作控制器"""
        self.init_info = {
@@ -72,21 +69,18 @@ class RealmanDualRobot:
            self.teleop = HybridController(self.init_info)
        else:
            self.teleop = None
-
    def _initialize_state(self):
        """初始化状态管理"""
        self.joint_queue = deque(maxlen=2)
        self.last_endpose = self.arm.init_pose
        self.logs = {}
        self.is_connected = False
-
    def _initialize_keyboard_interface(self):
        """初始化键盘交互接口"""
        self.w_pressed = False  # W键触发LLM
        self.q_pressed = False  # Q键退出
        self.keyboard_listener = None
        self._start_keyboard_listener()
-
    def _start_keyboard_listener(self):
        """启动键盘监听器"""
        def on_key_press(key_char):
@@ -104,7 +98,6 @@ class RealmanDualRobot:
            print("键盘监听器启动成功 (W键:调用LLM, Q键:退出)")
        else:
            print("键盘监听器启动失败")
-
    def _read_robot_state(self) -> dict:
        """读取机器人状态"""
        before_read_t = time.perf_counter()
@@ -112,7 +105,6 @@ class RealmanDualRobot:
        state = deepcopy(self.arm.read())
        self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
        return state
-
    def _execute_action(self, action: dict, state: dict):
        """执行机器人动作"""
        before_write_t = time.perf_counter()
@@ -128,7 +120,6 @@ class RealmanDualRobot:
                self.arm.write(target_joint)
        
        self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-
    def _prepare_record_data(self) -> Tuple[Dict, Dict]:
        """准备记录数据 - 保持原有逻辑"""
        if len(self.joint_queue) < 2:
@@ -147,7 +138,6 @@ class RealmanDualRobot:
        }
        action_dict = {"action": action}
        return obs_dict, action_dict
-
    def _update_state_queue(self):
        """更新状态队列"""
        current_state = self.arm.read()['joint']
@@ -158,7 +148,6 @@ class RealmanDualRobot:
            elif "gripper" in data:
                current_state_lst.append((current_state[data]-500)/500)
        self.joint_queue.append(current_state_lst)
-
    def _capture_images(self) -> Dict[str, torch.Tensor]:
        """捕获摄像头图像"""
        images = {}
@@ -169,7 +158,6 @@ class RealmanDualRobot:
            self.logs[f"read_camera_{name}_dt_s"] = camera.logs["delta_timestamp_s"]
            self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
        return images
-
    def _handle_llm_interaction(self, obs_dict: Dict) -> bool:
        """处理LLM交互逻辑"""
        print("[W键已按下] 正在准备数据并调用LLM...")
@@ -188,7 +176,6 @@ class RealmanDualRobot:
        )
        
        return success
-
    def connect(self) -> None:
        """连接机器人和摄像头"""
        if self.is_connected:
@@ -209,7 +196,6 @@ class RealmanDualRobot:
        print("All connected")
        self.is_connected = True
        self.run_calibration()
-
    def disconnect(self) -> None:
        """断开连接"""
        if self._shutdown_flag:
@@ -251,7 +237,6 @@ class RealmanDualRobot:
            
        except Exception as e:
            print(f"断开连接时发生错误: {e}")
-
    def run_calibration(self):
        """运行标定"""
        if not self.is_connected:
@@ -259,7 +244,6 @@ class RealmanDualRobot:
        self.arm.apply_calibration()
        if not self.inference_time:
            self.teleop.reset()
-
    def teleop_step(self, record_data=False) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
        """遥操作步骤 - 保持原有数据记录逻辑，添加LLM交互"""
        if not self.is_connected:
@@ -344,7 +328,6 @@ class RealmanDualRobot:
                return obs_dict, action_dict
            
            return None
-
    def send_action(self, action: torch.Tensor) -> torch.Tensor:
        """发送动作到机器人"""
        if not self.is_connected:
@@ -359,7 +342,6 @@ class RealmanDualRobot:
        self.arm.write(target_joints)
        
        return action
-
    def capture_observation(self) -> dict:
        """捕获当前观测"""
        if not self.is_connected:
@@ -388,11 +370,9 @@ class RealmanDualRobot:
        for name in self.cameras:
            obs_dict[f"observation.images.{name}"] = images[name]
        return obs_dict
-
    def teleop_safety_stop(self):
        """遥操作安全停止"""
        self.run_calibration()
-
    @property
    def camera_features(self) -> dict:
        """获取摄像头特征"""
@@ -405,7 +385,6 @@ class RealmanDualRobot:
                "info": None,
            }
        return cam_ft
-
    @property
    def motor_features(self) -> dict:
        """获取电机特征"""
@@ -423,15 +402,12 @@ class RealmanDualRobot:
                "names": state_names,
            },
        }
-
    @property
    def has_camera(self):
        return len(self.cameras) > 0
-
    @property
    def num_cameras(self):
        return len(self.cameras)
-
    def __del__(self):
        """析构函数"""
        try:
@@ -440,13 +416,11 @@ class RealmanDualRobot:
        except:
            pass

-
 def signal_handler(signum, frame):
    """信号处理器"""
    print("\n收到中断信号，正在安全退出...")
    sys.exit(0)

-
 if __name__ == '__main__':
    signal.signal(signal.SIGINT, signal_handler)
    robot = None
@@ -480,325 +454,3 @@ if __name__ == '__main__':
            except:
                pass
        print("程序已完全退出")
-# """
-#     Teleoperation Realman with a PS5 controller and 
-# """
-
-# import time
-# import torch
-# import numpy as np
-# import logging
-# from typing import Optional, Tuple, Dict
-# from dataclasses import dataclass, field, replace
-# from collections import deque
-# from lerobot.common.robot_devices.teleop.realman_aloha_dual import HybridController
-# from lerobot.common.robot_devices.motors.utils import get_motor_names, make_motors_buses_from_configs
-# from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
-# from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-# from lerobot.common.robot_devices.robots.configs import RealmanDualRobotConfig
-# from lerobot.common.robot_devices.robots.utils import ask_llm
-
-
-
-# class RealmanDualRobot:
-#     def __init__(self, config: RealmanDualRobotConfig | None = None, **kwargs):
-#         if config is None:
-#             config = RealmanDualRobotConfig()
-#         # Overwrite config arguments using kwargs
-#         self.config = replace(config, **kwargs)
-#         self.robot_type = self.config.type
-#         self.inference_time = self.config.inference_time # if it is inference time
-        
-#         # build cameras
-#         self.cameras = make_cameras_from_configs(self.config.cameras)
-#         # build realman motors
-#         self.piper_motors = make_motors_buses_from_configs(self.config.follower_arm)
-#         self.arm = self.piper_motors['main']
-        
-#         # 初始化遥操作
-#         self._initialize_teleop()
-#         # init state
-#         self._initialize_state()
-
-#     def _initialize_teleop(self):
-#         """初始化遥操作"""
-#         self.init_info = {
-#             'init_joint': self.arm.init_joint_position,
-#             'init_pose': self.arm.init_pose,
-#             'max_gripper': self.config.max_gripper,
-#             'min_gripper': self.config.min_gripper,
-#             'servo_config_file': self.config.servo_config_file,
-#             'end_control_info': {'left': self.config.left_end_control_guid , 'right': self.config.right_end_control_guid}
-#         }
-        
-#         if not self.inference_time:
-#             self.teleop = HybridController(self.init_info)
-#         else:
-#             self.teleop = None
-            
-#     def _initialize_state(self):
-#         """初始化状态"""
-#         self.joint_queue = deque(maxlen=2)
-#         self.last_endpose = self.arm.init_pose
-#         self.logs = {}
-#         self.is_connected = False
-
-#     def _read_robot_state(self) -> dict:
-#         """读取机器人状态"""
-#         before_read_t = time.perf_counter()
-#         from copy import deepcopy
-#         state = deepcopy(self.arm.read())
-#         self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-#         return state
-
-#     def _execute_action(self, action: dict, state: dict):
-#         """执行动作"""
-#         before_write_t = time.perf_counter()
-
-#         if action['control_mode'] == 'joint':
-#             # self.arm.write_action(action, state)
-#             pass
-#         else:
-#             if list(action['pose'].values()) != list(state['pose'].values()):
-#                 pose = list(action['pose'].values())
-#                 self.arm.write_endpose_canfd(pose)
-            
-#             elif list(action['joint'].values()) != list(state['joint'].values()):
-#                 target_joint = list(action['joint'].values())
-#                 self.arm.write(target_joint)
-        
-#         self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
-
-#     def _prepare_record_data(self) -> Tuple[Dict, Dict]:
-#         """准备记录数据"""
-#         if len(self.joint_queue) < 2:
-#             return {}, {}
-            
-#         state = torch.as_tensor(list(self.joint_queue[0]), dtype=torch.float32)
-#         action = torch.as_tensor(list(self.joint_queue[-1]), dtype=torch.float32)
-#         # 捕获图像
-#         images = self._capture_images()
-#         # 构建输出字典
-#         obs_dict = {
-#             "observation.state": state,
-#             **{f"observation.images.{name}": img for name, img in images.items()}
-#         }
-#         action_dict = {"action": action}
-#         return obs_dict, action_dict
-    
-#     def _update_state_queue(self):
-#         """更新状态队列"""
-#         current_state = self.arm.read()['joint']
-#         current_state_lst = []
-#         for data in current_state:
-#             if "joint" in data:
-#                 current_state_lst.append(current_state[data] / 180)
-#             elif "gripper" in data:
-#                 current_state_lst.append((current_state[data]-500)/500)
-#         self.joint_queue.append(current_state_lst)
-    
-#     def _capture_images(self) -> Dict[str, torch.Tensor]:
-#         """捕获图像"""
-#         images = {}
-#         for name, camera in self.cameras.items():
-#             before_camread_t = time.perf_counter()
-#             image = camera.async_read()
-#             images[name] = torch.from_numpy(image)
-            
-#             self.logs[f"read_camera_{name}_dt_s"] = camera.logs["delta_timestamp_s"]
-#             self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
-#         return images
-
-#     @property
-#     def camera_features(self) -> dict:
-#         cam_ft = {}
-#         for cam_key, cam in self.cameras.items():
-#             key = f"observation.images.{cam_key}"
-#             cam_ft[key] = {
-#                 "shape": (cam.height, cam.width, cam.channels),
-#                 "names": ["height", "width", "channels"],
-#                 "info": None,
-#             }
-#         return cam_ft
-
-    
-#     @property
-#     def motor_features(self) -> dict:
-#         action_names = get_motor_names(self.piper_motors)
-#         state_names = get_motor_names(self.piper_motors)
-#         return {
-#             "action": {
-#                 "dtype": "float32",
-#                 "shape": (len(action_names),),
-#                 "names": action_names,
-#             },
-#             "observation.state": {
-#                 "dtype": "float32",
-#                 "shape": (len(state_names),),
-#                 "names": state_names,
-#             },
-#         }
-    
-#     @property
-#     def has_camera(self):
-#         return len(self.cameras) > 0
-
-#     @property
-#     def num_cameras(self):
-#         return len(self.cameras)
-
-
-#     def connect(self) -> None:
-#         """Connect RealmanArm and cameras"""
-#         if self.is_connected:
-#             raise RobotDeviceAlreadyConnectedError(
-#                 "RealmanArm is already connected. Do not run `robot.connect()` twice."
-#             )
-        
-#         # connect RealmanArm
-#         self.arm.connect(enable=True)
-#         print("RealmanArm conneted")
-
-#         # connect cameras
-#         for name in self.cameras:
-#             self.cameras[name].connect()
-#             self.is_connected = self.is_connected and self.cameras[name].is_connected
-#             print(f"camera {name} conneted")
-        
-#         print("All connected")
-#         self.is_connected = True
-        
-#         self.run_calibration()
-
-
-#     def disconnect(self) -> None:
-#         """move to home position, disenable piper and cameras"""
-#         # move piper to home position, disable
-#         if not self.inference_time:
-#             self.teleop.stop()
-
-#         # disconnect piper
-#         self.arm.safe_disconnect()
-#         print("RealmanArm disable after 5 seconds")
-#         time.sleep(5)
-#         self.arm.connect(enable=False)
-
-#         # disconnect cameras
-#         if len(self.cameras) > 0:
-#             for cam in self.cameras.values():
-#                 cam.disconnect()
-
-#         self.is_connected = False
-
-
-#     def run_calibration(self):
-#         """move piper to the home position"""
-#         if not self.is_connected:
-#             raise ConnectionError()
-        
-#         self.arm.apply_calibration()
-#         if not self.inference_time:
-#             self.teleop.reset()
-
-#     def teleop_step(
-#         self, record_data=False
-#     ) -> None | tuple[dict[str, torch.Tensor], dict[str, torch.Tensor]]:
-#         if not self.is_connected:
-#             raise ConnectionError()
-
-#         if self.teleop is None and self.inference_time:
-#             self.teleop = HybridController(self.init_info)
-
-#         try:
-#             # 读取当前状态
-#             state = self._read_robot_state()
-#             # 获取动作
-#             action = self.teleop.get_action(state)
-#             self._execute_action(action, state)
-#             # 更新状态队列
-#             self._update_state_queue()
-#             time.sleep(0.019) # 50HZ
-
-#             if record_data:
-#                 data = self._prepare_record_data()
-#                 if data[0]:
-#                 #     # ask_llm("将超声仪左下角试管架上的试管移动到超声仪中", data[0])
-#                     pass
-#                 return data
-            
-#         except Exception as e:
-#             logging.error(f"遥操作步骤失败: {e}")
-#             return None
-
-
-#     def send_action(self, action: torch.Tensor) -> torch.Tensor:
-#         """Write the predicted actions from policy to the motors"""
-#         if not self.is_connected:
-#             raise RobotDeviceNotConnectedError(
-#                 "Piper is not connected. You need to run `robot.connect()`."
-#             )
-
-#         # send to motors, torch to list
-#         target_joints = action.tolist()
-#         len_joint = len(target_joints) - 1
-#         target_joints = [target_joints[i]*180 for i in range(len_joint)] + [target_joints[-1]]
-#         target_joints[-1] = int(target_joints[-1]*500 + 500)
-#         self.arm.write(target_joints)
-
-#         return action
-
-
-#     def capture_observation(self) -> dict:
-#         """capture current images and joint positions"""
-#         if not self.is_connected:
-#             raise RobotDeviceNotConnectedError(
-#                 "Piper is not connected. You need to run `robot.connect()`."
-#             )
-        
-#         # read current joint positions
-#         before_read_t = time.perf_counter()
-#         state = self.arm.read()  # 6 joints + 1 gripper
-#         self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
-
-#         state = torch.as_tensor(list(state.values()), dtype=torch.float32)
-
-#         # read images from cameras
-#         images = {}
-#         for name in self.cameras:
-#             before_camread_t = time.perf_counter()
-#             images[name] = self.cameras[name].async_read()
-#             images[name] = torch.from_numpy(images[name])
-#             self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs["delta_timestamp_s"]
-#             self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t
-
-#         # Populate output dictionnaries and format to pytorch
-#         obs_dict = {}
-#         obs_dict["observation.state"] = state
-#         for name in self.cameras:
-#             obs_dict[f"observation.images.{name}"] = images[name]
-#         return obs_dict
-    
-#     def teleop_safety_stop(self):
-#         """ move to home position after record one episode """
-#         self.run_calibration()
-
-    
-#     def __del__(self):
-#         if self.is_connected:
-#             self.disconnect()
-#             if not self.inference_time:
-#                 self.teleop.stop()
-
-
-# if __name__ == '__main__':
-#     robot = RealmanDualRobot()
-#     robot.connect()
-#     # robot.run_calibration()
-#     while True:
-#         robot.teleop_step(record_data=True)
-    
-#     # robot.capture_observation()
-#     # dummy_action = torch.Tensor([-0.40586111280653214, 0.5522833506266276, 0.4998166826036241, -0.3539944542778863, -0.524433347913954, 0.9064999898274739, 0.482])
-#     # robot.send_action(dummy_action)
-#     # robot.disconnect()
-#     # print('ok')
--- a/lerobot/common/utils/utils.py
+++ b/lerobot/common/utils/utils.py
@@ -177,8 +177,11 @@ def say(text, blocking=False):
    elif system == "Linux":
        # cmd = ["spd-say", text]
        cmd = ["edge-playback", "-t", text]
+
+######################################## remove #####################################
        # if blocking:
        #     cmd.append("--wait")
+######################################## remove #####################################

    elif system == "Windows":
        cmd = [